1. Playing hide and seek:
probabilistic localization and SLAM applied to marine
robotic middleware
Tomas Van Oyen
email: tomas.vanoyen@ugent.be
October 11, 2016
Abstract
The application of a probabilistic localization and simultanous localization and
mapping (SLAM) algorithm to a marine robotic middleware code (MOOS) is pre-
sented. The localization algorithm describes the positioning by means of the Monte
Carlo Localization algorithm and exploits only range measurements from known bea-
cons. The measurement model of the localization algorithm takes into account (i)
unexpected objects (short), (ii) measurement noise, (iii) unexplained measurements
(random) and (iv) measurement failure. The algorithm is able to accurately estimate
the position (∆ ∼ 0.1 m) of the vehicle and recover from the application of unknown
drift and time windows with less measurements. The applied SLAM is based on the
FastSLAM methodology where the positioning of the autonomous vehicle is deter-
mined by a particle filter while the positioning of beacons is estimated by applying an
extended Kalman filter. The results show that the algorithm is quite capable of esti-
mating the vehicle position and mapping features in an unknown environment, apart
from an initial offset.
1 Introduction
Localization and mapping are central
themes for autonomous vehicles. Here, we
discuss a probabilistic approach to these
problems. The main advantage of prob-
abilistic robotics is the ability to seem-
lessly accommodate for uncertainty; which
is inevitably present in the physical world.
In particular, sensors are limited in their
description of the physical reality while
also motion models cannot fully describe
the kinematics of a robot. Probabilistic
robotics tackles this problem by explic-
itly describing the uncertainty by means
of probability theory. In addition to the
ability to account for uncertainty in mo-
tion and measurement models, a proba-
bilistic approach also allows to account for
uncertainty when making control choices,
enabling this approach to outperform tra-
ditional approaches in real-world applica-
tions, see [3].
In the following, we present the appli-
cation of a probabilistic localization al-
gorithm and simultanous localization and
mapping algorithm to a marine robotic
middleware code (MOOS). Background
reading of MOOS can be found at [1, 2].
To this end, an autonomous vehicle is set
1

2. Figure 1: Illustration of the survey route followed by the autonomous vehicle in the
localization problem (top left picture), and when the SLAM problem is considered (bot-
tom left picture). The yellow circles denote the beacons, while the red sign represents
the vehicle. The white line shows the survey route followed by the vehicle.
to survey a certain route within the har-
bour of Ostend, as illustrated in figure 1.
In the next section, the motion model
is described, after which the measurement
model for the localization and SLAM prob-
lem is specified. Results are presented in
Section 5 while the final section provides
conclusions.
2 Motion model
In order to describe the movement of the
vehicle, a local orthogonal coordinate sys-
tem is considered with the ˆx− axis pointing
East and the ˆy− axis pointing North. The
evolution in time of the position x = (x, y)
of the vehicle in the MOOS simulator is
computed by
x(t + ∆T) = x(t) + v∗∆T. (1)
In (1), ∆T describes the time period be-
tween two positioning updates, while v∗ =
(v∗
x, v∗
y) estimates the velocity of the vehicle
and is computed as
v∗ =
(vprop + v(t))
2
. (2)
Here, vprop is a function of the desired
thrust γ and rudder ω; i.e. vprop = F(γ, ω).
2

3. (a) (b)
Figure 2: Evolution in time positioning capability ∆ (left) and the uncertainty (right)
of the particle filter when the range measurements with the beacons are not taken into
account.
The latter are considered input in the fol-
lowing, and are given by MOOS after con-
sidering a multi-objective optimization in
order to execute the programmed survey
mission. Furthermore, the velocity compo-
nents of the vehicle (v∗
x, v∗
y) are given by
v∗
x = v∗
sin(θ∗
), v∗
y = v∗
cos(θ∗
), (3)
where θ∗ denotes the heading of the vehicle:
θ∗
=
(θprop + θ(t))
2
, (4)
and θprop is a function of the desired thrust
and rudder: θprop = G(γ, ω). Finally, in
order to model the motion error, Gaussian
noise (Gaus(σ)) is added to both vprop and
θprop:
vprop = F(γ, ω) + Gaus(σ2
v), (5)
θprop = G(γ, ω) + Gaus(σ2
θ), (6)
with
σ2
v = α1F2
+ α2ω2
, and σ2
θ = α3γ2
+ α4ω2
.
3 Localization problem
For the localizaiton problem, we consider
the Monte Carlo Localization (MCL) algo-
rithm, and thus estimate the robot (posi-
tion) state by M particles:
Xt := x
[1]
t , x
[2]
t , ..., x
[M]
t . (7)
The MCL-algorithm (pseudo-code) is given
in detail in [3, pag. 98].
When moving forward in time (i.e. from
t → t + ∆t), the algorithm determines an
importance weight (w[m]) of each particle
m and then resamples with replacement
particles from Xt considering the probabil-
ity of drawing a particle proportional to
w[m].
In turn, the importance weight is evalu-
ated by means of a measurement model.
In the localization problem, we consider
only range measurements r from known
beacon locations. Moreover, the measure-
ment model takes into account (i) unex-
pected objects (short), (ii) measurement
noise, (iii) unexplained measurements (ran-
dom) and (iv) measurement failure. In par-
3

4. ticular, we consider p(z|x, m) = cshpsh +
chitphit + cfapfa + cunipuni:
(i) psh = λ exp (−λl) ,
(ii) phit =
1
√
2πσ2
exp −0.5
(l − r)2
σ2
,
(iii) if (l < lmax) :
puni = 1/(πl2
max),
else:
puni = 0,
(iv) if (l = lmax) :
pfa = 1/(2πlmax),
else:
pfa = 0,
where λ and σ represent the drop-off rate
of psh and the variance of phit, respectively.
Moreover, l = x2 + y2 and lmax describes
a maximum sensor range, while the coef-
ficients csh, chit, cfa, cuni allow to vary the
proportionality of each probability density
and to normalize the distribution.
The finally obtained probability density
p(z|x, m) is then used to provide each par-
ticle an importance weight, and a new state
is determined by sampling from the particle
set Xt with replacement, relating the prob-
ability of sampling a particle to the weight
of this particle.
4 Simultanous Localization and
Mapping (SLAM)
The FastSLAM algorithm is applied in or-
der to simultaneously describe the location
of the vehicle and map the unknown en-
vironment. Hence, we describe the state
(i.e. the robot position, and the locations
of the beacons) by considering M (Monte
Figure 3: Evolution in time positioning ca-
pability ∆ of the particle filter when the
range measurements with the beacons are
not taken into account and an unknown
time-varying drift is applied.
Carlo) particles, and each particle Y m com-
prises both information on the vehicle and
location of all the beacons. In particular,
the location of each beacon k is estimated
by each particle m by means of a Gaussian
probability distribution characterized by a
mean µ[m],k and a covariance Σ[m],k:
Y m
t = xm
t , µ
[m],1
t , Σ
[m],1
t , ..., µ
[m],k
t , Σ
[m],k
t .
Hence, an extended Kalman filter is consid-
ered for the beacon locations while a parti-
cle filter is applied to estimate the vehicle
location.
In the FastSLAM algorithm we consider
both range r and angle α measurements be-
tween the beacons vehicle.
The FastSLAM-algorithm (pseudo-code)
is given in detail in [3, pag. 450], and in
appendix ?? some details of the current ap-
plication of the algorithm are described.
4

5. (a) (b)
(c) (d)
(e) (f)
Figure 4: Evolution in time positioning capability ∆ (left) and the uncertainty (right)
of the particle filter taking into account range measurements without application of
unknown drift.
5

6. (a) (b)
(c) (d)
(e) (f)
Figure 5: Green (red) dots illustrate the real (estimated) vehicle path. The locations
of the beacons are given by the yellow triangles while the other dots plot the mean of
beacon location estimate of each particle. 6

7. 5 Results
5.1 Localization: dead reckoning
Figure 2 illustrates the evolution in time
of the positioning capability (left) and un-
certainty (right) when simulating the evo-
lution in time of the location of the vehicle
without consideration of the measurements
(i.e. dead reckoning). In particular, ∆
equals (¯x − xr)2 + (¯y − yr)2, where (¯x, ¯y)
denotes the location given by the particle
filter by averaging over all the particles,
and (xr, yr) describes the real vehicle loca-
tion. In particular, hereinafter we approxi-
mate the location of the vehicle by averag-
ing the location over all particles (instead
of estimating it by kerned density extrac-
tion).
In figure 2b the vertical axes describes
the maximum amplitude of the eigenvalues
of the covariance matrix, providing thus an
indication on the uncertainty. Figure 2 il-
lustrates that in the case of dead reckoning,
the uncertainty in the positioning increases
strongly while also ∆ becomes larger in
time.
Figure 3 plots ∆ in time when periodi-
cally an external (unknown) variable drift
is applied to the vehicle. The figures il-
lustrates that in that case, localization
based on the motion model only deterio-
rates quickly.
5.2 Localization: MCL algorithm
Figure 4 describes the evolution in time of
∆ (left) and the uncertainty of the loca-
tion estimation (right) taking into account
range measurements from several beacons.
The top figure shows these quantities while
the vehicle is surveying the route shown in
the top left picture of figure 1. The re-
sult shows that the algorithm can quite ac-
curately describe the location of the vehi-
cle, and is able to recover position after a
time window where only few range mea-
surements are available [time = 350 - 420
s].
The middle figures show that the algo-
rithm is also able to reasonably describe
the positioning of the vehicle, even in the
presence of an unknown variable periodiek
drift. Nevertheless, the figure illustrates
the importance of the frequent incoming
range measurements as it is apparant that
in the time window with very few measure-
ments the localization estimate differs quite
strongly with the real vehicle position.
Finally, the bottom figures plot the
same variables in time as the middle fig-
ure, however, in this experiment the ran-
domness coefficients in the motion model
(α1, α2, α3, α4) are increased. The results
presented in these figures illustrate that
increasing the randomness in the motion
model can be benificial to diminish the in-
fluence of unknown externally applied drift,
however increases the average deviation of
the location estimate w.r.t. the real vehicle
position. Also, in this case, the importance
of frequent measurement updates is appar-
ent by the increase in ∆ in the time window
with less frequent measurement input [time
= 350 - 420 s].
5.3 SLAM
Figure 5 presents the results of the Fast-
SLAM algorithm applied to the survey
route as illustrated in figure 1 bottom
left. The considered route concerns a short
mowing pattern along which 4 beacons are
set. As discussed above and in Appendix,
each beacon provides range and angle in-
formation to the vehicle to which gaussian
noise is set (proportional to the range be-
tween beacon and vehicle).
7

8. In figure 5 the green dots describe the
real vehicle track while the yellow triangles
illustrate the locations of the beacons. The
red dots present the track of the vehicle
estimated by the algorithm (as discussed
above, an average of the location of each
particle is taken) while the other scatter
plots show the mean of the beacon location
estimated by each particle.
The figure shows that the algorithm is
quite capable of descibing both the vehicle
path and the beacon locations apart from
an initial random offset which mimics that
the robot has no absolute infomation on the
environment (e.g. does not know the origin
location).
6 Conclusions
The application of a probabilistic localiza-
tion algorithm and a SLAM algorithm to a
marine robotic middleware (MOOS) is pre-
sented. In the localization algorithm the
robot position (state) is represented by M
particles and the Monte Carlo Particle fil-
ter is considered to update the state es-
timate. The results show that the algo-
rithm is able to accurately estimate the ve-
hicle location with small uncertainty, even
when an unknown periodic drift is applied
provided that frequent range measurements
are available.
The FastSLAM algorithm is considered
to simultaneously estimate the vehicle po-
sition and estimate the locations of beacons
in an unknown environment. Results indi-
cate that the algorithm is able to solve this
problem swiftly apart from an initial ran-
dom offset, which cannot be resolved by the
algorithm.
Further work concerns the evaluation
of how the FastSLAM algorithm can be
blended with a localization algorithm in or-
der to use initially unmapped beacons to
improve the localization of the robot.
Ackowledgments
Prof. Alain Sarlette is gratefully acknowl-
egded for his enthusiasm and patience.
References
[1] M. R. Benjamin. Moos-ivp autonomy
tools users manual. release 13.5. Techni-
cal report, Department Mechanical En-
gineering. Computer Science and Arti-
ficial Intelligence Laboratory, 2013.
[2] M. R. Benjamin, H. Schmidt, P. New-
man, and J.J. Leonard. An overview
of moos-ivp and a users guide to the
ivp helm. release 13.5. Technical report,
Department Mechanical Engineering.
Computer Science and Artificial Intel-
ligence Laboratory, 2013.
[3] S. Trun, W. Burgard, and D. Fox.
Probabilistic Robotics. The MIT Press,
Cambridge Massachusetts, 2006.
8