Simultaneous localization and mapping (SLAM)

via particle ltering

Lecture Notes
Fabien Campillo
5/5/05
In many applications, mobile robot must acquire a representation of its environ-
ment and determine its location in this environment. This means that a model for
the environment must be proposed and that the robot must be able to identify this
model, i.e. build a map of an unknown environment, while simultaneously localiz-
ing itself within it. The so called Simultaneous Localization and Mapping or SLAM
problem is a formulation of this task, and has been the subject of a considerable
amount of robotics research in the last decades (see [10, 11, 4] and [14] for a review).
According to Thrun [14], since the 90s, robot mapping is dominated by proba-
bilistic techniques. Before particle approximation we mention occupancy grid map-
ping [8, 7] and EKF approaches [1]. Particle approximation appears a few years ago
with people like Sebastian Thrun, Dieter Fox, Wolfram Burgard and coworkers,
see [3, 5, 6, 12, 15, 16, 17, 13, 14, 17].
FastSLAM is the rst, and more advanced, application for particle ltering to
SLAM problem. Our goal is to present this method.
1 The problem
Consider a mobile robot evolving in an unknown but static environment. It responds
to some controls and collect observations. The aim of simultaneous localization and
mapping (SLAM) is to recover the map of the environment and the path oh the
robot in this map.
Each time index t corresponds to a pose of a robot (see Fig. 1) and
s
t
is the state of the robot (e.g. (x, y)position and bearing),
z
t
is the observation.
The map features N landmark locations =
1:N
(R
2
)
N
,

is the (x, y)position

of the
th
landmark. As we will see, the number N of landmarks could evolve in
time (and will be denoted N
t
).
We will suppose that each observation provides informations about the location
of exactly one landmark, the correspondence landmark/observation is denoted n
t
(i.e. at time t, landmark
n
t
is observed).
The corresponding dependency diagram is described in Fig. 2. This model is
dened through the following conditional p.d.f.s
p(s
t+1
[s
t
) (motion model)
p(z
t
[, s
t
, n
t
) (perceptual model) .
1
1
s
s
2
s
3
s
t
landmarks
robot poses
robot
Figure 1: Along successive poses s
t
a robot acquires measurements form the nearest
landmark located at
n
t
. The problem is to determine simultaneously and recursively
the locations =
1:N
of these landmarks and the trajectory s
1:t
of the robot.
s
t 1
s
t
s
t+1
z
t 1
z
t
z
t+1
n
t 1
n
t
n
t+1

Figure 2: Dependency diagram for the SLAM problem: s

t
is the state of the robot
at time t, z
t
is the observation of the feature n
t
of the map .
The measurement model adopted here is
z
t
= g(
n
t
, s
t
) + v
t
(1)
where v
t
is a white Gaussian noise ^(0, R
t
).
What do we want ? The rst goal of SLAM is to recover the best estimate of
the robot pose s
t
and of the map given the set of noisy observations z
1:t
through
the SLAM posterior law
p(s
t
, [z
1:t
) . (2)
If the set of associations n
1:t
is given, the SLAM posterior is just p(s
t
, [z
1:t
, n
1:t
).
2
2 Bayesian lter for a given data association
We suppose that n
1:t
is given, then we can write the optimal Bayesian lter for
posterior law p(s
t
, [z
1:t
, n
1:t
). Indeed
p(s
t
, [z
1:t
, n
1:t
) p(z
t
[s
t
, , n
t
)

s
t1
p(s
t
[s
t1
) p(s
t1
, [z
1:t1
, n
1:t1
) ds
t1
. (3)
Proof of (3) Bayes rule gives
p(s
t
, [z
1:t
, n
1:t
) p(z
t
[s
t
, , z
1:t1
, n
1:t
) p(s
t
, [z
1:t1
, n
1:t
) .
By hypothesis, p(z
t
[s
t
, , z
1:t1
, n
1:t
) = p(z
t
[s
t
, , n
t
) so that
p(s
t
, [z
1:t
, n
1:t
) p(z
t
[s
t
, , n
t
) p(s
t
, [z
1:t1
, n
1:t
) .
and
p(s
t
, [z
1:t
, n
1:t
)
p(z
t
[s
t
, , n
t
)

s
t1
p(s
t
, [s
t1
, z
1:t1
, n
1:t
) p(s
t1
[z
1:t1
, n
1:t
) ds
t1
= p(z
t
[s
t
, , n
t
)

s
t1
p(s
t
[, s
t1
, z
1:t1
, n
1:t
) p([s
t1
, z
1:t1
, n
1:t
)
p(s
t1
[z
1:t1
, n
1:t
) ds
t1
= p(z
t
[s
t
, , n
t
)

s
t1
p(s
t
[s
t1
) p([s
t1
, z
1:t1
, n
1:t
)
p(s
t1
[z
1:t1
, n
1:t
) ds
t1
= p(z
t
[s
t
, , n
t
)

s
t1
p(s
t
[s
t1
) p(s
t1
, [z
1:t1
, n
1:t
) ds
t1
this leads to (3). 2
3 FastSLAM 1.0 with known association
FastSLAM exploits conditional independences that are a consequence of the sparse
structure of the SLAM problem, so that we can factorized the posterior law as a
product of low dimensional estimation problems.
3.1 FastSLAM posterior
Instead of (2), we consider the following posterior law
p(s
1:t
, [z
1:t
, n
1:t
) . (4)
We make the following hypothesis
Given s
1:t
, the landmark positions
1

N
are independent. (5)
Then the FastSLAM posterior (4) could be factorized as
p(s
1:t
, [z
1:t
, n
1:t
) = p(s
1:t
[z
1:t
, n
1:t
)
. .. .
robot path posterior

n=1
p(
n
[s
1:t
, z
1:t
, n
1:t
)
. .. .
landmark posteriors
(6)
3
Proof of (6) Note that
p(s
1:t
, [z
1:t
, n
1:t
) = p(s
1:t
[z
1:t
, n
1:t
) p([s
1:t
, z
1:t
, n
1:t
)
thus, to derive (6) it suces to prove
p(
1:N
[s
1:t
, z
1:t
, n
1:t
) =
N

n=1
p(
n
[s
1:t
, z
1:t
, n
1:t
) . (7)
We establish (7) by induction.
(i ) From Bayes formula
p(
n
t
[s
1:t
, z
1:t
, n
1:t
) =
p(z
t
[
n
t
, s
1:t
, z
1:t1
, n
1:t
)
p(z
t
[s
1:t
, z
1:t1
, n
1:t
)
p(
n
t
[s
1:t
, z
1:t1
, n
1:t
)
z
t
depend only on the current state of the robot and the landmark being
observed, also the current data association n
t
and pose s
t
have no eect on

n
t
without z
t
, hence
p(
n
t
[s
1:t
, z
1:t
, n
1:t
) =
p(z
t
[
n
t
, s
t
, n
t
)
p(z
t
[s
1:t
, z
1:t1
, n
1:t
)
p(
n
t
[s
1:t1
, z
1:t1
, n
1:t1
) . (8)
(ii ) Consider now the probability p(
n=n
t
[s
1:t
, z
1:t
, n
1:t
) of any landmark that is
not observed conditioned on the data. This is simple because the landmark
posterior will not change if the landmark is not observed. Thus
p(
n=n
t
[s
1:t
, z
1:t
, n
1:t
) = p(
n=n
t
[s
1:t1
, z
1:t1
, n
1:t1
) (9)
Suppose that (7) is true for t 1. As seen previously
p([s
1:t
, z
1:t
, n
1:t
) =
p(z
t
[, s
1:t
, z
1:t1
, n
1:t
)
p(z
t
[s
1:t
, z
1:t1
, n
1:t
)
p([s
1:t
, z
1:t1
, n
1:t
) .
Again, z
t
only depends on (, s
t
, n
t
), and does not depends on (s
t
, n
t
) without the
current observation, hence
p([s
1:t
, z
1:t
, n
1:t
) =
p(z
t
[
n
t
, s
t
, n
t
)
p(z
t
[s
1:t
, z
1:t1
, n
1:t
)
p([s
1:t1
, z
1:t1
, n
1:t1
) .
Using (7) for t 1 gives
p([s
1:t
, z
1:t
, n
1:t
) =
p(z
t
[
n
t
, s
t
, n
t
)
p(z
t
[s
1:t
, z
1:t1
, n
1:t
)
N

n=1
p(
n
[s
1:t1
, z
1:t1
, n
1:t1
) .
which leads, using (8) and (9), to
p([s
1:t
, z
1:t
, n
1:t
) = p(
n
t
[s
1:t
, z
1:t
, n
1:t
)

n=n
t
p(
n
[s
1:t
, z
1:t
, n
1:t
)
which is (7). 2
4
3.2 Particle approximation
Posterior (4) will be approximated by a RaoBlackwellized particle lter: namely a
particle lter for the robot path posterior and an EKF for the landmark posterior
p(s
1:t
, [z
1:t
, n
1:t
) = p(s
1:t
[z
1:t
, n
1:t
)
. .. .

1
M

M
m=1

s
m
1:t
(s
1:t
)

n=1
p(
n
[s
1:t
, z
1:t
, n
1:t
)
. .. .
N(
n
;
n,m
t
,
n,m
t
)

1
M
M

m=1

s
m
1:t
(s
1:t
)
N

n=1
^(
n
;
n,m
t
,
n,m
t
)

.
Hence, at time t, the lter is dened by the set
{
t
def
= s
m
1:t
, (
n,m
t
,
n,m
t
)
n=1:N
t

m=1:M
.
Note that the number N of landmarks could increase in time and could also depend
on the particle index m (why ?). Instead of N we can consider N
m
t
...
The lter iteration {
t1
{
t
is
Step 1: sampling a new pose. For each particle, a new robot pose
is drawn and s
m
1:t
= [s
m
1:t1
, s
m
t
].
Step 2: update map estimate. The new observation corresponds to
one landmark, the EKFs corresponding to this landmark is up-
dates.
Step 3: resampling. Each particle is given an importance weight and
a new set of particles {
t
is drawn according to these weights.
Each step is now detailed:
Step 1: Sampling a new pose
Sample
s
m
t
p(s
t
[s
m
1:t1
) for m = 1 : M (10a)
then, if s
m
1:t1
p(s
1:t1
[z
1:t1
, n
1:t1
) then
s
m
1:t
= [s
m
1:t1
, s
m
t
] p(s
1:t
[z
1:t1
, n
1:t1
) (10b)
which is the proposal distribution (prediction distribution). This operation does not
depend on the size of the map (it is a constanttime operation for each particle).
Step 2: Updating the map
As already seen, each landmark posterior p(
n
[s
1:t
, z
1:t
, n
1:t
), for n = 1 : N, will be
given through an EKF, that is
p(
n
[s
1:t
, z
1:t
, n
1:t
) ^(
n,m
t
,
n,m
t
) .
At time t, we get a fresh observation z
t
associated with the landmark n
t
, then
the corresponding landmark (n = n
t
) estimate is updated,
other landmark (n = n
t
) estimates are left unchanged
5
We expand the posterior distribution corresponding to the observed landmark
n
t
using Bayes rule
p(
n
t
[s
1:t
, z
1:t
, n
1:t
) p(z
t
[
n
t
, s
1:t
, z
1:t1
, n
1:t
) p(
n
t
[s
1:t
, z
1:t1
, n
1:t
) . (11)
Note z
t
depends only on (
n
t
, s
t
, n
t
), and
n
t
is not aected by (s
t
, n
t
) without z
t
.
Thus, previous expression reads
p(
n
t
[s
1:t
, z
1:t
, n
1:t
) p(z
t
[
n
t
, s
t
, n
t
) p(
n
t
[s
1:t1
, z
1:t1
, n
1:t1
) . (12)
For n = n
t
, we leave the landmark posterior unchanged
p(
n=n
t
[s
1:t
, z
1:t
, n
1:t
) = p(
n=n
t
[s
1:t1
, z
1:t1
, n
1:t1
) . (13)
FastSLAM implements the update equation (12) using an EKF. The nonlinear
measurement model
g(
n
t
, s
t
)
at s
t
= s
m
t
is approximated using a rstorder Taylor expansion
g(
n
t
, s
m
t
) z
m
t
+G
m
t
(
n
t

n
t
,m
t1
) with

z
m
t
def
= g(
n
t
,m
t1
, s
m
t
) ,
G
m
t
def
= g

(s, )[
s=s
m
t
,=
n
t
,m
t1
,
This approximation is applied to the rst term of the RHS of (12)
p(z
t
[
n
t
, s
m
t
, n
t
) ^

z
t
; z
m
t
+G
m
t
(
n
t

n
t
,m
t
), R
t

and the second term is

p(
n
t
[s
m
1:t1
, z
1:t1
, n
1:t1
) ^

n
t
;
n
t
,m
t
,
n
t
,m
t

.
The mean
n
t
,m
t
and covariance matrix
n
t
,m
t
are given by the following EKF iter-
ation
z
m
t
= g(
n
t
,m
t1
, s
m
t
)
G
m
t
= g

(s, )[
s=s
m
t
,=
n
t
,m
t1
Q
m
t
= G
m
t

n
t
,m
t1
[G
m
t
]

+R
t
K
m
t
=
n
t
,m
t1
[G
m
t
]

[Q
m
t
]
1

n
t
,m
t
=
n
t
,m
t1
+K
m
t
(z
t
z
m
t
)

n
t
,m
t
= [I K
m
t
G
m
t
]
n
t
,m
t1
Step 3: Resampling
Previously, the particles were drawn from the motion model p(s
1:t
[z
1:t1
, n
1:t1
) (the
proposal distribution) and therefore do not match the desired posterior p(s
1:t
[z
1:t
, n
1:t
)
(the target distribution). This dierence is corrected through a process called im-
portance sampling.
Dene the importance weights
w
m
t
def
=
target distribution
proposal distribution
=
p(s
m
1:t
[z
1:t
, n
1:t
)
p(s
m
1:t
[z
1:t1
, n
1:t1
)
. (14)
6
With standard arguments
w
m
t

p(z
t
[s
m
1:t
, z
1:t1
, n
1:t
) p(s
m
1:t
[z
1:t1
, n
1:t
)
p(s
m
1:t
[z
1:t1
, n
1:t1
)
=
p(z
t
[s
m
1:t
, z
1:t1
, n
1:t
) p(s
m
1:t
[z
1:t1
, n
1:t1
)
p(s
m
1:t
[z
1:t1
, n
1:t1
)
= p(z
t
[s
m
1:t
, z
1:t1
, n
1:t
)
which implies that
w
m
t

n
t
p(z
t
[
n
t
, s
m
1:t
, z
1:t1
, n
1:t
) p(
n
t
[s
m
1:t
, z
1:t1
, n
1:t
) d
n
t
.
The expression under the integration sign is the r.h.s. of (11), the same kind of
approximation leads to
w
m
t

n
t
^

z
t
; z
m
t
+G
m
t
(
n
t

n
t
,m
t
), R
t

n
t
;
n
t
,m
t
,
n
t
,m
t

d
n
t
that is (
1
)
w
m
t
[2Q
m
t
[

1
2
exp

1
2
[z
t
z
m
t
]

[Q
m
t
]
1
[]

where d is the dimension of the observation vector.

A new set of samples {
t
is drawn (with replacement) from{
t1
with probabilities
in proportion to the weights w
m
t
.
Note that along this iteration we just need the most recent pose s
m
t1
.
4 FastSLAM 1.0 with unknown association
In real SLAM applications, n
1:t
is unknown. If the uncertainty in landmark positions
is low relatively to the average distance between landmarks, simple heuristics are
eective. The most common approach is the maximum likelihood:
n
t
def
= Arg max
n
t
p(z
t
[n
t
, s
1:t
, z
1:t1
, n
1:t1
) . (15)
Another possibility, not developed here and called Monte Carlo association, is to
sample
n
t
p(z
t
[n
t
, s
1:t
, z
1:t1
, n
1:t1
) .
In (15), when the likelihood is below some xed threshold the observation is asso-
ciated to a new landmark.
4.1 Per particle data association
We propose a per particle implementation of (15). At each time t, each particle
m possesses its own map represented by it own number N
m
t
of features. Given the
past data association n
m
1:t1
, the last association n
m
t
is computed through
n
m
t
def
= Arg max
n
t
p(z
t
[n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
) . (16)
1
In case of Gaussian densities, we use the notation
[z
t
z
m
t
]

[Q
m
t
]
1
[] = [z
t
z
m
t
]

[Q
m
t
]
1
[z
t
z
m
t
]
7
Remember that the map corresponding to this map is

(
m,n
t
,
m,n
t
)

n=1:N
m
t
associated with the state s
m
t
and the association n
m
t
.
With the argument proposed in Step 3, we get
p(z
t
[n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
) =
=

n
t
p(z
t
[
n
t
, n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
)
. .. .
N(z
t
;g(
n
t
,s
m
t
),R
t
)
p(
n
t
[n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
)
. .. .
N(
n
t
;
n
t
,m
t
,
n
t
,m
t
)
d
n
t
still, because g(
n
t
, s
m
t
) is nonlinear, we cannot compute this expression explicitly.
So we linearize g(
n
t
, s
m
t
) and get:
p(z
t
[n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
) [2Q
m
t
[

1
2
exp

1
2
[z
t
g(
n
t
,m
t1
, s
m
t
)
. .. .
z
m
t
]

[Q
m
t
]
1
[]

4.2 Adding new features

If the likelihood (16) is too poor, i.e. p(z
t
[n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
) p for all n
t
, then
we conclude that the observed feature does not correspond to the previously known
features and its added to the map. In particle approximation, in (16), if
max
n
t
p(z
t
[n
t
, s
m
1:t
, z
1:t1
, n
m
1:t1
) p (17)
then a new feature is added N
m
t
= N
m
t1
+ 1. Its associated likelihood is p and
n
t
= N
m
t
.
Initializing the newly added EKF can be tricky. In many SLAM problem the
function g is invertible, for example when we observe both range and bearing. In
this case, one single measurement z
t
produces a non degenerate estimate of the
feature location:

n,m
t
= g
1
(z
t
, s
m
t
) ,

n,m
t
=

G
m
t
R
1
t
(G
m
t
)

1
,
where z = g(, s) iif = g
1
(z, s).
4.3 Feature elimination
How to deal with features introduced erroneously onto the map ? We consider a
mechanism for eliminating features that are not supported by sucient evidence
(see Thrun [14, 9]). So we have to keep track of the probabilities of the actual
existence of individual features in the map. Let
i
n,m
=

1 if feature n exists for particle m,

0 if feature n does not exist for particle m.
(18)
At time t, the feature n
t
is observed, i.e. is in the perceptual range of the robot
(see Fig. 3). Then, for each particle m,
(i ) we reinforce the evidence for the existence of
n
t
,m
,
8
perceptual range
s
s
2
s
t
t
n
robot
1
Figure 3: At time t, the feature n
t
is in the perceptual range of the robot the
probability of existence of this feature is reinforced.
(ii ) we decrease the evidence for the existence of
n,m
for n = n
t
if

n,m
t
falls into the robots perceptual range.
The associated posterior probability is
p(i
n,m
[ n
m
1:t
, s
m
1:t
, z
1:t
)
which is estimated by a binary Bayes lter. In the odds notation
p(i
n,m
[ n
m
1:t
, s
m
1:t
, z
1:t
)
1 p(i
n,m
[ n
m
1:t
, s
m
1:t
, z
1:t
)
=
=
p(i
n,m
[ n
m
t
, s
m
t
, z
t
)
1 p(i
n,m
[ n
m
t
, s
m
t
, z
t
)

p(i
n,m
)
1 p(i
n,m
)

p(i
n,m
[ n
m
1:t1
, s
m
1:t1
, z
1:t1
)
1 p(i
n,m
[ n
m
1:t1
, s
m
1:t1
, z
1:t1
)
We represent this posterior in its logodds form
r
n,m
t
def
= log
p(i
n,m
[ n
m
1:t
, s
m
1:t
, z
1:t
)
1 p(i
n,m
[ n
m
1:t
, s
m
1:t
, z
1:t
)
The usual choice concerning the prior p(i
n,m
) is 0.5 so that its logodds is 0. Hence
r
n,m
t
= r
n,m
t1
+ log
p(i
n,m
[ n
m
t
, s
m
t
, z
t
)
1 p(i
n,m
[ n
m
t
, s
m
t
, z
t
)
The recurrence starts when the features is introduced in the map. Let

+
def
= log
p(i
m
n
t
[ n
m
t
, s
m
t
, z
t
)
1 p(i
m
n
t
[ n
m
t
, s
m
t
, z
t
)
> 0

def
= log
p(i
m
n=n
t
[ n
m
t
, s
m
t
, z
t
)
1 p(i
m
n=n
t
[ n
m
t
, s
m
t
, z
t
)
< 0
In practice a much more simple procedure is proposed (and implemented in
FastSLAM): every time the landmark is observed, a constant value is added to the
logodds; every time a landmark is observed when it should have been, a constant
value is subtracted.
Features are terminated when their logodds of existence falls beyond a certain
bound.
9
1: for m = 1 : M do
2: draw s
m
t
p(s
t
[s
m
t1
) {sample new pose}
3: for n = 1 : N
m
t1
do
4: z
n,m
t
g(
n,m
t1
, s
m
t
)
5: G
n,m
t
g

(
n,m
t1
, s
m
t
)
6: Q
n,m
t
[G
n,m
t
]

n,m
t
G
n,m
t
+R
t
7: p
n,m
t
[2Q
n,m
t
[
1/2
exp
1
2
(z
t
z
m
t
)

[Q
n,m
t
]
1
()
8: end for
9: p
N
m
t1
+1,m
t
p {extra weight for hypothetic new feature}
10: n
m
t
Arg maxp
n,m
t
; n = 1 : N
m
t1
+ 1 {most probable feature}
11: N
m
t
N
m
t1
n
m
t
{new number of features in map}
12: for n = 1 : N
m
t
do
13: if n = N
m
t1
+ 1 then
14:
n,m
t
g
1
(z
t
, s
m
t
) {add a new feature}
15:
n,m
t
[G
n,m
t
]
1
R
t
[G
n,m
t
]
1
16: r
n,m
t

+
{initialize feature likelihood}
17: else if n = n
m
t
then
18: K
n,m
t1
GQ
1
{update observed feature}
19:
n,m
t

n,m
t1
+K (z
t
z
n,m
t
)
20:
n,m
t
(I K G

)
n,m
t1
21: r
n,m
t
r
n,m
t1
+
+
{increase feature likelihood}
22: else
23:
n,m
t

n,m
t1
{all other features}
24:
n,m
t

n,m
t1
25: if
n,m
t1
inside perceptual range of s
m
t
then
26: r
n,m
t
r
n,m
t1
+

{decrease feature likelihood}

27: if r
n,m
t
< 0 then
28: discard feature n
29: end if
30: end if
31: end if
32: end for
33: {
m
aux

s
m
t
, N
m
t
, (
n,m
t
,
n,m
t
, r
n,m
t
)
n=1:N
m
t

34: w
m
t
p
n
m
t
,m
t
{importance weights}
35: end for
36: {
t
resample({
1:M
aux
, w
1:M
t
)
37: return {
t
Algorithm 1: FastSLAM 1.0 iteration {
t1
{
t
. Instead of the max-
imum likelihood instruction 10, we can use a sampling procedure n
m
t

p
n,m
t
; n = 1 : N
m
t1
+ 1
10
5 FastSLAM 2.0: a new proposal distribution
5.1 Proposal distribution
Instead of the proposal distribution p(s
1:t
[z
1:t1
, n
1:t1
) used in the previous sam-
pling step (10), we may want to sample from
s
m
t
p(s
t
[s
m
1:t1
, z
1:t
, n
1:t
) (19)
the question is how. The idea is to use an EKF approximation. In FastSLAM 1.0
only the part was approximated through an EKF, now we must linearize in
both variables s and . This is why we need a motion model, namely
s
t
= h
t
(s
t1
) + w
t
(20)
where w
t
^(0, P
t
).
We have
p(s
t
[s
m
1:t1
, z
1:t
, n
1:t
)
=
m
t
p(z
t
[s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) p(s
t
[s
m
1:t1
, z
1:t1
, n
1:t
)
=
m
t
p(z
t
[s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) p(s
t
[s
m
1:t1
)
=
m
t

n
t
p(z
t
[
n
t
, s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) p(
n
t
[s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) d
n
t
p(s
t
[s
m
1:t1
)
=
m
t

n
t
p(z
t
[
n
t
, s
t
)
. .. .
N(z
t
;g(
n
t
,s
t
),R
t
)
p(
n
t
[s
m
1:t1
, z
1:t1
, n
1:t1
)
. .. .
N(
n
t
;
n
t
,m
t1
,
n
t
,m
t1
)
d
n
t
p(s
t
[s
m
1:t1
)
. .. .
N(s
t
;h
t
(s
m
t1
),P
t
)
. (21)
To be able to sample from the previous distribution we will have to linearize the
function g(, s) in both and s variables
g(
n
t
, s
t
) z
m
t
+G
1,m
t
(
n
t

n
t
,m
t1
) +G
2,m
t
(s
t
s
m
t
)
where
z
m
t
def
= g(
n
t
,m
t1
, s
m
t
)
s
m
t
def
= h
t
(s
m
t1
)
G
1,m
t
def
= g

(, s)[
=
n
t
,m
t1
,s=s
m
t
G
2,m
t
def
= g

s
(, s)[
=
n
t
,m
t1
,s=s
m
t
Under this approximation, the desired sampling distribution (19) is Gaussian with
the following parameters

m
s
t
def
=

(G
2,m
t
)

(Q
m
t
)
1
G
2,m
t
+P
1
t

m
s
t
def
=
m
s
t
(G
2,m
t
)

(Q
m
t
)
1
(z
t
z
m
t
) +s
m
t
where
Q
m
t
def
= G
1,m
t

m
t
(G
1,m
t
)

(Q
m
t
)
1
+R
t
So ^(z
t
; g(
n
t
, s
t
), R
t
) ^(z
t
; z
m
t
+ G
2,m
t
(s
t
s
m
t
), Q
m
t
) and we get a closed form
for the integral representation in (21). Also ^(s
t
; h
t
(s
m
t1
), P
t
) ^(s
t
; s
m
t
, P
t
) so
that
p(s
t
[s
m
1:t1
, z
1:t
, n
1:t
) exp

1
2
[z
t
z
m
t
G
2,m
t
(s
t
s
m
t
)]

[Q
m
t
]
1
[]+[s
t
s
m
t
]

P
1
t
[]

11
which is Gaussian. Hence
p(s
t
[s
m
1:t1
, z
1:t
, n
1:t
) ^(s
t
;
m
s
t
,
m
s
t
)
with

m
s
t
def
=

[G
2,m
t
]

[Q
m
t
]
1
G
2,m
t
+P
1
t

1
,

m
s
t
def
=
m
s
t
[G
2,m
t
]

[Q
m
t
]
1
[z
t
z
m
t
] +s
m
t
.
5.2 Updating landmark estimates
This updating uses the observation z
t
and the sampled poses s
m
t
. At time t 1,
the estimates for the m
th
particle are (
n,m
t1
,
n,m
t1
)
n=1:N
m
t1
. If n = n
t
(i.e. feature
n is not observed) these estimates remains unchanged. If n = n
t
(i.e. feature n is
observed), then
p(
n
t
[s
m
1:t
, z
1:t
, n
1:t
) p(z
t
[
n
t
, s
m
t
, n
t
)
. .. .
N(z
t
;g(
n
t
,s
m
t
),R
t
)
p(
n
t
[s
m
1:t1
, z
1:t1
, n
1:t1
)
. .. .
N(
n
t
;
n
t
,m
t1
,
n
t
,m
t1
)
.
Again, the nonlinearity of
n
t
g(
n
t
, s
m
t
) causes this posterior to be nonGaussian.
We linearize
g(
n
t
, s
m
t
) z
m
t
+G
1,m
t
[
n
t

n
t
,m
t1
]
which leads to
p(
n
t
[s
m
1:t
, z
1:t
, n
1:t
) ^(
n
t
,
n
t
,m
t
,
n
t
,m
t
)
with
K
m
t
def
=
n
t
,m
t1
[G
1,m
t
]

[Q
m
t
]
1
,

n
t
,m
t
def
=
n
t
,m
t1
+K
m
t
[z
t
z
m
t
] ,

n
t
,m
t
def
= (I K
m
t
G
1,m
t
)
n
t
,m
t1
.
5.3 Importance factors
Now
w
m
t
=
p(s
m
1:t
[z
1:t
, n
1:t
)
p(s
m
1:t1
[z
1:t1
, n
1:t1
) p(s
m
t
[s
m
1:t1
, z
1:t
, n
1:t
)
=
p(s
m
1:t1
[z
1:t
, n
1:t
) p(s
m
t
[s
m
1:t1
, z
1:t
, n
1:t
)
p(s
m
1:t1
[z
1:t1
, n
1:t1
) p(s
m
t
[s
m
1:t1
, z
1:t
, n
1:t
)
=
p(s
m
1:t1
[z
1:t
, n
1:t
)
p(s
m
1:t1
[z
1:t1
, n
1:t1
)

p(z
t
[s
m
1:t1
, z
1:t1
, n
1:t
) p(s
m
1:t1
[z
1:t1
, n
1:t
)
p(s
m
1:t1
[z
1:t1
, n
1:t1
)
= p(z
t
[s
m
1:t1
, z
1:t1
, n
1:t
)
because p(s
m
1:t1
[z
1:t1
, n
1:t
) = p(s
m
1:t1
[z
1:t1
, n
1:t1
). Then
w
m
t

s
t
p(z
t
[s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) p(s
t
[s
m
1:t1
, z
1:t1
, n
1:t
)
. .. .
p(s
t
|s
m
t
1)
ds
t
=

s
t

n
t
p(z
t
[
n
t
, s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) p(
n
t
[s
t
, s
m
1:t1
, z
1:t1
, n
1:t
) d
n
t

p(s
t
[s
m
t
1) ds
t
=

s
t

n
t
p(z
t
[
n
t
, s
t
)
. .. .
N(z
t
;g(
n
t
,s
t
),R
t
)
p(
n
t
[s
m
1:t1
, z
1:t1
, n
1:t1
)
. .. .
N(
n
t
;
n
t
,m
t1
,
n
t
,m
t1
)
d
n
t

p(s
t
[s
m
t
1)
. .. .
N(s
t
;s
m
t1
,P
t
)
ds
t
12
Same problem, same argument: a linearization leads to
w
m
t
= [2 L
m
t
[
1/2
exp

1
2
[z
t
z
m
t
]

[L
m
t
]
1
[]

with
L
m
t
def
= G
2,m
t
P
t
[G
2,m
t
]

+G
1,m
t

n
t
,m
t1
[G
1,m
t
]

+R
t
.
6 Misc.
6.1 Dealing with multiple observation
If at time t, many features are in the perceptual range of the robot we have to deal
with many observation at each timestep. Its a piece of cake for FastSLAM 1.0: we
just have to treat the measurements sequentially. Its more dicult for FastSLAM
2.0: we need an EKF update for each observation
6.2 People tracking
The map is given (and fairly accurate), it says if one point (x, y) is free or occupied.
This map will remain unchanged. Its usually implemented as an evidence grid.
The N people to be tracked are located in [
n
t
]
n=1:N
(the number
N
could
evolved in time.
At each pose particle s
m
t
one associates a particle representation
i,m,n
t

i=1:M
p
of M
p
particles for the n
th
person.
People motion model: These people move like independent Brownian particles,
i.e. p(
t
[
t1
) = ^(
t
;
t1
,
2
I).
References
[1] Nicolas Ayache and Olivier Faugeras . Maintaining representations of the
environment of a mobile robot. IEEE Trans. Robot. Automation, 5(6):804
819, 1989.
[2] Gamini Dissanayake, Paul Newman, Steven Clark, Hugh F. Durrant-Whyte,
and Michael Csorba. A solution to the simultaneous localisation and map build-
ing (SLAM) problem. IEEE Trans. on Robotics and Automation, 17(3):229
241, 2001.
[3] Dieter Fox, Sebastian Thrun, Wolfram Burgard, and Frank Dellaert. Improving
regularized particle lters. In Arnaud Doucet, Nando de Freitas, and Neil J.
Gordon, editors, Sequential Monte Carlo Methods in Practice, Statistics for
Engineering and Information Science, chapter 19, pages 401428. Springer
Verlag, New York, 2001.
[4] John J. Leonard and Hugh F. Durrant-Whyte. Mobile robot localization by
tracking geometric beacons. IEEE Trans. Robotics and Automation, 7(3):376
382, 1991.
[5] Michael Montemerlo. FastSLAM: A Factored Solution to the Simultaneous Lo-
calization and Mapping Problem with Unknown Data Association. PhD thesis,
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, July 2003.
13
[6] Michael Montemerlo and Sebastian Thrun. FastSLAM: A Factored Solution to
the Simultaneous and Localization Mapping Problem. SpringerVerlag, 2005.
To appear.
[7] Hans P. Moravec. Sensor fusion in certainty grids for mobile robots. AI Mag.,
9(2):6174, 1988.
[8] Hans P. Moravec and Alberto E. Elfes. High resolution maps from wide angle
sonar. In Proceedings of the 1985 IEEE International Conference on Robotics
and Automation, pages 116 121, March 1985.
[9] Jose Nieto, Juan Guivant, Eduardo Nebot, and Sebastian Thrun. Real time
data association for fastslam. In IEEE ICRA, Taiwan, 2003.
[10] Randall C. Smith and Peter Cheeseman. On the representation and estimation
of spatial uncertainty. International Journal of Robotics Research, 5(4):5668,
1986.
[11] Randall C. Smith, Mathew Self, and Peter Cheeseman. Estimating uncertain
spatial relationships in robotics. In I.J. Cox and G.T. Wilfong, editors, Au-
tonomous Robot Vehicles, volume 8, pages 167193, 1990.
[12] Sebastian Thrun. Learning metric-topological maps for indoor mobile robot
navigation. Articial Intelligence, 99(1):2171, 1998.
[13] Sebastian Thrun. Probabilistic robotics. Commun. ACM, 45(3):5257, 2002.
[14] Sebastian Thrun. Robotic mapping: A survey. In G. Lakemeyer and B. Nebel,
editors, Exploring Articial Intelligence in the New Millenium. Morgan Kauf-
mann, 2002.
[15] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. A probabilistic approach
to concurrent mapping and localization for mobile robots. Machine Learning,
31:2953, 1998. also appeared in Autonomous Robots 5, 253271 (joint issue).
[16] Sebastian Thrun, Dieter Fox, Wolfram Burgard, and Franck Dellaert. Robust
Monte Carlo localization for mobile robots. Articial Intelligence, 128(1-2):99
141, 2001.
[17] Sebastian Thrun, Michael Montemerlo, Daphne Koller, Beb Wegbreit, Juan
Nieto, and Eduardo Nebot. FastSLAM: An ecient solution to the simultane-
ous localization and mapping problem with unknown data association. Journal
of Machine Learning Research, 2004. To appear.
14