Advanced Mobile Robotics
Probabilistic Robotics:
SLAM/FastSLAM
Dr. Jizhong Xiao
Department of Electrical Engineering
CUNY City College
jxiao@ccny.cuny.edu
City College of New York
1
Probabilistic Robotics
SLAM/FastSLAM
City College of New York
SLAM Problem : Chicken or Egg
Fundamental problems for localization and mapping
 The task of SLAM is to build a map while estimating the
pose of the robot relative to this map.
 Without a map, robot cannot localize itself
 Without knowing its location, robot cannot build a map
 Which needed to be done first? Localization or mapping?
City College of New York
3
SLAM
Simultaneous Localization and Mapping
A robot is exploring an unknown, static environment.
Given:
– The robot’s controls (U1:t)
– Observations of nearby features (Z1:t)
Estimate:
– Map of features (m)
– Pose / Path of the robot (xt)
City College of New York
4
Why is SLAM a hard problem?
Uncertanties
• Error
in pose
• Error in observation
• Error in mapping
• Error accumulated
City College of New York
5
Why is SLAM a hard problem?
SLAM: robot path and map are both unknown!
Robot path error correlates errors in the map
City College of New York
6
Why is SLAM a hard problem?
Robot pose
uncertainty
• In the real world, the mapping between observations
and landmarks is unknown
• Picking wrong data associations can have
catastrophic consequences
• Pose error correlates data associations
City College of New York
7
Data Association Problem
• A data association is an assignment of observations
to landmarks
• In general there are more than
(n observations, m landmarks) possible associations
• Also called “assignment problem”
City College of New York
8
SLAM:
Simultaneous Localization and Mapping
• Full SLAM:
Estimates entire path and map!
p( x1:t , m | z1:t , u1:t )
Estimates
Given
Entire pose (x1:t) and map (m)
Previous knowledge (Z1:t-1, U1:t-1)
Current measurement (Zt, Ut)
City College of New York
9
Graphical Model of Full SLAM:
p( x1:t , m | z1:t , u1:t )
City College of New York
10
SLAM:
Simultaneous Localization and Mapping
• Online SLAM:
p( xt , m | z1:t , u1:t )
Estimates
Given
Most recent pose (xt) and map (m)
Previous knowledge (Z1:t-1, U1:t-1)
Current measurement (Zt, Ut)
Estimates most recent pose and map!
City College of New York
11
Graphical Model of Online SLAM:
p( xt , m | z1:t , u1:t )      p( x1:t , m | z1:t , u1:t ) dx1 dx2 ...dxt 1
Integrations typically done one at a time
City College of New York
12
SLAM with Extended Kalman Filter
• Pre-requisites
– Maps are feature-based (landmarks)
small number (< 1000)
– Assumption - Gaussian Noise
– Process only positive sightings
No landmark = negative
Landmark = positive
City College of New York
13
EKF-SLAM with known correspondences
Correspondence
Data association problem
Landmarks can’t be
uniquely identified
Correspondence variable
(Cit)
True identity of
observed feature
between feature (fit) and real landmark

f t  rt
i
i

i
t
S

i T
t
City College of New York
14
EKF-SLAM with known correspondences
Signature
Numerical value (average color)
Characterize type of landmark
(integer)
Multidimensional vector
(height and color)
City College of New York
15
EKF-SLAM with known correspondences
Similar development to EKF localization
Diff  robot pose + coordinates of all landmarks
Combined state vector
 xt 
yt     x
 m
y 
m1, x
p( yt | z1:t , u1:t )
m1, y
S1 ... mN , x
(3N + 3)
mN , y
Online posterior
City College of New York
SN 
T
16
Mean
Motion update
Covariance
Test for new landmarks
Initialization
of elements
Expected measurement
Iteration through
measurements
Filter is updated
City College of New York
17
FastSLAM
Jizhong Xiao
Department of Electrical Engineering
The City College of New York
City College of New York
18
Particle Filters



Represent belief by random samples
Estimation of non-Gaussian, nonlinear processes
Sampling Importance Resampling (SIR) principle
 Draw the new generation of particles
 Assign an importance weight to each particle
 Resampling

Typical application scenarios are
tracking, localization, mapping …
City College of New York
19
Localization vs. SLAM



A particle filter can be used to solve both problems
Monte Carlo Localization: state space < x, y, >
SLAM: state space < x, y, , map>
 for landmark maps = < l1, l2, …, lm>
 for grid maps = < c11, c12, …, c1n, c21, …, cnm>

Disadvantage: The number of particles needed to
represent a posterior grows exponentially with
the dimension of the state space!
City College of New York
20
Some considerations of particle filter
•
•
•
•
•
•
Scale exponentially with dimension
Large variables involved to mapping
Convergence of particles
Choice of Re-sampling
Conditionally independent mapping error
And more….
City College of New York
21
Dependencies

Is there a dependency between the dimensions of
the state space?

If so, can we use the dependency to solve the
problem more efficiently?

In the SLAM context
 The map depends on the poses of the robot.
 We know how to build a map given the position
of the sensor is known.
City College of New York
22
Mapping using Landmarks
l1
Landmark 1
z1
observations
Robot poses
controls
x1
x0
u0
z3
x2
x3
...
u1
u1
z2
Landmark 2
xt
ut-1
zt
l2
Knowledge of the robot’s true path renders
landmark positions conditionally independent
City College of New York
23
FastSLAM Principle
Features are conditionally
independent given the robot path
Figure 13.3 The SLAM problem depicted as Bayes network graph. The
robot moves from pose xt-1 to pose xt+2, driven by a sequence of controls.
At each pose xt , it observes a nearby feature in the map m={m1, m2, m3}.
This graphical network illustrates that the pose variables “separate” the
individual features in the map from each other. If the poses are known,
there remains no other path involving variables whose value is not known,
between any two features in the map. This lack of a path renders the
posterior of any two features in the map conditionally independent (given
the poses).
City College of New York
24
Factored Posterior (Landmarks)
poses
map
observations & movements
SLAM posterior
Robot path posterior landmark positions
Does this help to solve the problem?
(Factorization first introduced by Murphy in 1999)
City College of New York
25
Factored Posterior
Robot path posterior
(localization problem)
Conditionally
independent
landmark positions
City College of New York
26
Rao-Blackwellization
 This factorization is also called Rao-Blackwellization
 Given that the second term can be computed
efficiently, particle filtering becomes possible!
City College of New York
27
Fast SLAM

Rao-Blackwellized particle filtering based on landmarks
[Montemerlo et al., 2002]


Each landmark is represented by a Extended Kalman Filter
Each particle therefore has to maintain M EKFs
x, y, 
Landmark 1 Landmark 2 … Landmark M
Particle
#2
x, y, 
Landmark 1 Landmark 2 … Landmark M
Particle
N
x, y, 
Landmark 1 Landmark 2 … Landmark M
…
Particle
#1
City College of New York
28
FastSLAM Algorithm setup
City College of New York
29
Basic Step of FastSLAM
City College of New York
30
Rao-Blackwellization
Landmarks
Grid-based
poses
map
map
observations movements
City College of New York
31
Factored SLAM Posterior
The mathematic expression of full SLAM posterior:
p( y1:t | z1:t , c1:t , u1:t ) 
p( x1:t | z1:t , c1:t , u1:t ) 
p(m | z1:t , x1:t , c1:t )
After a long derivation (on page 442), it becomes
p( y1:t | z1:t , c1:t , u1:t ) 
p( x1:t | z1:t , c1:t , u1:t ) 
N
 p(m
n
| z1:t , x1:t , c1:t )
n 1
City College of New York
32
Advantage of factorization
• FastSLAM estimates the path posterior using
particle filter, estimate map feature using EKF
•
•
•
•
Because of factorization:
Maintain MN+1 filters (M – particles, N features)
All filters are low-dimensional
Separate EFK for each map features which
makes update more efficient than EKF-SLAM
• Each particle possess its own sets of EKF’s
• Easy for data association
City College of New York
33
FastSLAM 1.0 Algorithm:
N-th feature location (mean, variance)
relative to the k-th particle
Robot path estimation
K-th Particle is denoted as:
Yt  xt , 1,t ,  1,t ..., N ,t ,  N ,t ,
[k ]
[k ]
[k ]
[k ]
[k ]
[k ]
Step 1: Extend the path posterior by sampling new poses
xt[ k ] ~ p( xt | xt[k1] , ut )
Uses control ut to sample new robot pose xt
Added into a temporary set of particle, along with the path of
previous poses x1[:kt]1
Step 2: Updating the observed feature estimate
Step 3: Resembling
City College of New York
34
FastSLAM 1.0 Algorithm:
Step 1: Extend the path posterior by sampling new poses
Step 2: Updating the observed feature estimate
For n≠ct do not update
For n=ct
p(mct | x1:t , z1:t , c1:t ) p( zt | xt , mct , ct ) p(mct | z1:t 1, x1:t 1, , c1:t 1 )
Perceptual model, Gaussian
Previous belief, Gaussian
Using EKF, approximate the measurement model g by Taylor expansion
g (mct , xt[ k ] )  g(c[tk,]t 1 , xt[k ] )  g ( xt[k ] , c[tk,]t 1 )(mct  c[tk,]t 1 )
 zˆt[k ]  Gt[k ] (mc  c[k,]t 1 )
New mean
t
t
Kalman Gain
K
[k ]
t

[k ]
ct ,t 1
[k ]
t
G (G
[ k ]T
t

[k ]
ct ,t 1
Gt[ k ]  Rt ) 1
c[k,]t  c[k,]t 1  Kt[k ] ( zt  zˆt[k ] )
t

[k ]
ct ,t
t
 ( I  K t[ k ]Gt[ k ] ) [ckt ,]t 1
City College of New York
T
New covariance
35
FastSLAM 1.0 Algorithm:
Step 1: Extend the path posterior by sampling new poses
Step 2: Updating the observed feature estimate
Step 3: Resembling
•
Necessity: particles in the temporary set are not distributed
according to the desired posterior, (i.e., poses xt driven by most
recent control ut, without paying attention to measurement zt).
•
Importance factor = Target distribution / Proposal distribution
[k ]
t
w
[k ]
1:t 1
1:t
[k ]
1:t 1
1:t 1
p( x | u , z1:t , c1:t )

p( x | u , z1:t 1 , c1:t 1 )
City College of New York
36
Re-sampling by Importance Sampler
Samples cannot be drawn
conveniently from the
target distribution f.
Instead, the importance
sampler draws samples
from the proposal
distribution g, which has a
simpler form.
Target distribution, takes into account
of measurement zt, along with the
correspondence ct
wt[ k ]
p( xt[ k ] | u1:t , z1:t , c1:t )

p( xt[ k ] | u1:t , z1:t 1 , c1:t 1 )
Proposal distribution of the path
particles in the temporary set
A sample of f is obtained by
attaching the weight f/g to
each sample x.
City College of New York
37
Re-sampling by Importance Sampler
Importance factor
p( xt[ k ] | z1:t , c1:t , u1:t )
wt[ k ]
p( xt[ k ] | u1:t , z1:t , c1:t )

p( xt[ k ] | u1:t , z1:t 1 , c1:t 1 )
 p( z t xt[ k ] , z1:t 1 , u1:t , c1:t ) p( xt[ k ] z1:t 1 , u1:t , c1:t )
 p ( z t xt[ k ] , z1:t 1 , c1:t ) p ( xt[ k ] z1:t 1 , u1:t , c1:t 1 )
Observed feature
wt[ k ]  p ( z t xt[ k ] , z1:t 1 , c1:t )
   p( z t mct , xt[ k ] , z1:t 1 , c1:t ) p(mct xt[ k ] , z1:t 1 , c1:t )dmct
   p( z t mct , xt[ k ] , ct ) p(mct x1[:kt ] , z1:t 1 , c1:t 1 )dmct
Measurement model, Gaussian
[k ]
t
w
1

[k ] 2
t
  2Q
~ N (  c[ tk,]t 1 ,  [ckt ,]t 1 )

 With covariance:
 1
exp ( z t  zˆt[ k ] )Qt[ k ]1 ( z t  zˆt[ k ] ) 
 2
City College of New York
[k ]
t
Q
G
[ k ]T
t

[k ]
n ,t 1
Gt[ k ]  Rt
38
FastSLAM 1.0 Algorithm
with known
correspondence
• See table 13.1
of text book on
page 450
City College of New York
39
Weighting particle to Approximate Target
Distribution
City College of New York
40
Resampling mismatch
FastSLAM 1.0 problem: samples poses based on control ut only, but the
accuracy of control is low. Proposal generates a large spectrum of samples,
but only a small subset has high likelihood . After re-sampling, only particles
within the ellipsoid “survive” with reasonably high likelihood
City College of New York
41
FastSLAM 2.0 Algorithm:
FastSLAM 2.0 avoids the problem by sampling poses based on
the measurement Zt in addition to control ut
Step 1: Extend the path posterior by sampling new poses
[k ]
t
x
[k ]
t 1
~ p( xt | x , ut , z1:t , c1:t )
Bayes

p( zt xt , x1[:kt]1 , u1:t , z1:t 1 , c1:t ) p( xt x1[:kt]1 , u1:t , z1:t 1 , c1:t )
p( zt x1[:kt]1 ,u1:t , z1:t 1 , c1:t )
See math on page 452~453
Step 2: Updating the observed feature estimate
Step 3: Re-sampling
City College of New York
42
FastSLAM 2.0 Algorithm:
FastSLAM 2.0 avoids the problem by sampling poses based on
the measurement Zt in addition to control ut
1. Extend the path posterior by sampling new poses
[k ]
t
x
[k ]
t 1
~ p( xt | x , ut , z1:t , c1:t )
2. Updating the observed feature estimate
p(mct | x , z1:t , c1:t ) 
  p( zt | xt[k ] , mct , ct ) p(mct | z1:t 1, x1[:kt]1, c1:t 1 )
[k ]
t
3. Re-sampling
[k ]
t
w
[k ]
1:t 1
1:t
[k ]
1:t 1
1:t 1
p( x | u , z1:t , c1:t )

p( x | u , z1:t 1 , c1:t 1 )
City College of New York
43
FastSLAM Algorithm
• Particle filter approach to the SLAM problem
• Maintain a set of particles
• Particles contain a sampled robot path and a map
• The features of the map are represented by own local Gaussian
• Map is created as a set of separate Gaussians
Map features are conditionally independent
given the path
Factoring out the path (1 per particle)
Map feature become independent
Eliminates the need to maintain
correlation among them
City College of New York
44
FastSLAM Algorithm
• Updating in FastSLAM
Sample new pose
update the observed features
• Update can be performed online
• Solves both online and offline SLAM problem
• Instances
Feature-based maps
Grid-based algorithm
City College of New York
45
Features of Fast-SLAM
• Each particle can rely on its own, local data
association decisions
• More robust data association decision on perparticle basis.
• Solves both full SLAM & on-line SLAM problems.
• FastSLAM 1.0 – less efficient in generating
samples
• FastSLAM 2.0 – more efficient by improved
proposal distribution, at the cost of mathematic
complexity.
City College of New York
46
Unknown Data Association
Correspondence: maximizing the likelihood of
the measurement zt
[k ]
t
c
 arg max p( zt | x , u1:t 1, z1:t 1, ct , c1:t 1 )
[k ]
1:t
City College of New York
47
FastSLAM – Action Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3
City College of New York
48
FastSLAM – Sensor Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3
City College of New York
49
FastSLAM – Sensor Update
Particle #1
Weight = 0.8
Particle #2
Weight = 0.4
Particle #3
Weight = 0.1
City College of New York
50
FastSLAM - Video
City College of New York
51
FastSLAM Complexity
• Update robot particles based
on control ut-1
• Incorporate observation zt into
Kalman filters
• Resample particle set
N = Number of particles
M = Number of map features
City College of New York
O(N)
Constant time per particle
O(N•log(M))
Log time per particle
O(N•log(M))
Log time per particle
O(N•log(M))
Log time per particle
52
Data Association Problem
• Which observation belongs to which landmark?
• A robust SLAM must consider possible data
associations
• Potential data associations depend also on the pose
of the robot
City College of New York
53
Multi-Hypothesis Data Association
• Data association is done
on a per-particle basis
• Robot pose error is
factored out of data
association decisions
City College of New York
54
Per-Particle Data Association
Was the observation
generated by the red
or the blue landmark?
P(observation|red) = 0.3
P(observation|blue) = 0.7
• Two options for per-particle data association
– Pick the most probable match
– Pick an random association weighted by the observation likelihoods
• If the probability is too low, generate a new landmark
City College of New York
55
Results – Data Association
City College of New York
56
Results – Victoria Park
• 4 km traverse
• < 5 m RMS
position error
• 100 particles
Blue = GPS
Yellow = Fast SLAM
Dataset courtesy of University
of Sydney
City College of New York
57
Results – Victoria Park
Dataset courtesy of University of Sydney
City College of New York
58
Grid-based SLAM

Can we solve the SLAM problem if no pre-defined
landmarks are available?

Can we use the ideas of Fast SLAM to build grid
maps?

As with landmarks, the map depends on the poses
of the robot during data acquisition

If the poses are known, grid-based mapping is easy
(“mapping with known poses”)
City College of New York
59
Rao-Blackwellization
poses
map
observations & movements
City College of New York
60
Rao-Blackwellization
poses
map
observations & movements
SLAM posterior
Robot path posterior
Mapping with known poses
City College of New York
61
Rao-Blackwellization
This is localization, use MCL
Use the pose estimate
from the MCL part and apply
mapping with known poses
City College of New York
62
A Graphical Model of Rao-Blackwellized
Mapping
u0
x0
u1
ut-1
x1
x2
z1
z2
...
xt
m
City College of New York
zt
63
Rao-Blackwellized Mapping
 Each particle represents a possible trajectory of
the robot
 Each particle
 maintains its own map and
 updates it upon “mapping with known poses”
 Each particle survives with a probability
proportional to the likelihood of the observations
relative to its own map
City College of New York
64
Mapping with Known Poses

Mapping with known poses using laser range data
City College of New York
65
Particle Filter Example
3 particles
map of particle 3
map of particle 1
map of particle 2
City College of New York
66
Problem
 Each map is quite big in case of grid maps
 Since each particle maintains its own map
 Therefore, one needs to keep the number
of particles small
 Solution:
Compute better proposal distributions!
 Idea 1:
Improve the pose estimate before
applying the particle filter
City College of New York
67
Pose Correction Using Scan Matching
Maximize the likelihood of the i-th pose and
map relative to the (i-1)-th pose and map
xˆt  arg max p( zt | xt , mˆ t 1 )  p( xt | ut 1 , xˆt 1 )
xt
current measurement
robot motion
map constructed so far
City College of New York
68
Motion Model for Scan Matching
Raw Odometry
Scan Matching
City College of New York
69
Mapping using Scan Matching
City College of New York
70
FastSLAM with Scan-Matching
City College of New York
71
Fast SLAM with Scan-Matching
Loop Closure
City College of New York
72
Map: Intel Research Lab Seattle
Fast SLAM with Scan-Matching
City College of New York
73
Intel Lab
 15 particles
 four times faster
than real-time
P4, 2.8GHz
 5cm resolution
during scan
matching
 1cm resolution in
final map
City College of New York
74
Intel Lab
 15 particles
 Compared to
Fast SLAM with
Scan-Matching,
the particles are
propagated
closer to the
true distribution
City College of New York
75
Outdoor Campus Map
 30 particles
 250x250m2
 1.75 km
(odometry)
 20cm resolution
during scan
matching
 30cm resolution
in final map
City College of New York
76
Fast SLAM with Improved Odometry
 Scan-matching provides a locally
consistent pose correction
 Pre-correct short odometry sequences
using scan-matching and use them as
input to Fast SLAM
 Fewer particles are needed, since the
error in the input is smaller
[Haehnel et al., 2003]
City College of New York
77
Comparison to Standard Fast SLAM
 Same model for observations
 Odometry instead of scan matching as input
 Number of particles varying from 500 to 2,000
 Typical result:
City College of New York
78
Graphical Model for Mapping with
Improved Odometry
u0 ... uk-1
z1 ... z k-1
x0
uk ... u2k-1
zk+1...z2k-1
...
un·k ... u(n+1)·k-1
z n·k+1... z(n+1)·k-1
u'1
u'2
...
u'n
xk
x2k
...
x n·k
zk
z2k
...
zn·k
m
City College of New York
79
Further Improvements
• Improved proposals will lead to more
accurate maps
• They can be achieved by adapting the proposal
distribution according to the most recent
observations
• Flexible re-sampling steps can further improve
the accuracy.
City College of New York
80
Conclusion
• The ideas of Fast SLAM can also be applied in the
context of grid maps
• Utilizing accurate sensor observation leads to good
proposals and highly efficient filters
• It is similar to scan-matching on a per-particle base
• The number of necessary particles and
re-sampling steps can seriously be reduced
• Improved versions of grid-based Fast SLAM can handle
larger environments than naïve implementations in “real
time” since they need one order of magnitude fewer
samples
City College of New York
81
Approximations for SLAM Problem
• Local submaps
[Leonard et al.99, Bosse et al. 02, Newman et al. 03]
• Sparse links (correlations)
[Lu & Milios 97, Guivant & Nebot 01]
• Sparse extended information filters
[Frese et al. 01, Thrun et al. 02]
• Thin junction tree filters
[Paskin 03]
• Rao-Blackwellisation (FastSLAM)
[Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]
City College of New York
82
Thank You
83
City College of New York
FastSLAM 1.0 Algorithm:
N-th feature location (mean, variance)
relative to the k-th particle
Robot path estimation
K-th Particle is denoted as:
Yt  xt , 1,t ,  1,t ..., N ,t ,  N ,t ,
[k ]
[k ]
[k ]
[k ]
[k ]
[k ]
1. Extend the path posterior by sampling new poses
[k ]
t
x
[k ]
t 1
~ p( xt | x , ut )
Added into a temporary set of particle,
along with the path of previous poses
2. Updating the observed feature estimate For n≠ct do not update
For n=ct
Represented by a Gaussian
p(mct | x1:t , z1:t , c1:t ) 
p( zt | xt , mct , ct ) p(mct | z1:t 1, x1:t 1, , c1:t 1 )
Perceptual model
3. Resembling
– difference of target and proposal distribution.
– importance factor = Target distri. / Proposal distri.
City College of New York
84