Probabilistic Robotics

FastSLAM
 The SLAM Problem

 SLAM stands for simultaneous localization and

mapping
 The task of building a map while estimating
the pose of the robot relative to this map

 Why is SLAM hard?

Chicken and egg problem:
a map is needed to localize the robot and
a pose estimate is needed to build a map

2
The SLAM Problem
A robot moving though an unknown, static environment

Given:
 The robot’s
controls
 Observations of
nearby features
Estimate:
 Map of features
 Path of the
robot 3
Why is SLAM a hard problem?

SLAM: robot path and map are both unknown!

Robot path error correlates errors in the map 4

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
5
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”

6
 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, …

7
 Localization vs. SLAM
 A particle filter can be used to solve both problems

 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>

 Problem: The number of particles needed to

represent a posterior grows exponentially with
the dimension of the state space!
8
 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?

9
 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.

10
 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 11
 Factored Posterior (Landmarks)
poses map observations & movements

Factorization first introduced by Murphy in 1999 12

 Mapping using Landmarks

Landmark 1 l1

observations z1 z3

Robot poses x x1 x2 x3 ... xt

0

controls u0 u1 u ut-1
1
z2 zt
Landmark 2 l2

Knowledge of the robot’s true path renders

landmark positions conditionally independent 13
Factored Posterior

Robot path posterior

(localization problem) Conditionally
independent
landmark positions
14
 Rao-Blackwellization

 This factorization is also called Rao-Blackwellization

 Given that the second term can be computed
efficiently, particle filtering becomes possible!

15
 FastSLAM
 Rao-Blackwellized particle filtering based on
landmarks [Montemerlo et al., 2002]
 Each landmark is represented by a 2x2
Extended Kalman Filter (EKF)
 Each particle therefore has to maintain M EKFs

Particle
#1
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
16
 FastSLAM – Action Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter

Particle #2

Particle #3

17
 FastSLAM – Sensor Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter

Particle #2

Particle #3

18
 FastSLAM – Sensor Update

Particle #1 Weight = 0.8

Particle #2 Weight = 0.4

Particle #3 Weight = 0.1

19
FastSLAM - Video

20
FastSLAM Complexity

O(N)
 Update robot particles Constant time per particle
based on control ut-1

 Incorporate observation zt O(N•log(M))

into Kalman filters Log time per particle

 Resample particle set O(N•log(M))

Log time per particle

N = Number of particles
O(N•log(M))
M = Number of map features Log time per particle
21
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

22
 Multi-Hypothesis Data Association

 Data association is
done on a per-particle
basis

 Robot pose error is

factored out of data
association decisions

23
 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 24
 Results – Victoria Park

 4 km traverse
 < 5 m RMS
position error
 100 particles

Blue = GPS
Yellow = FastSLAM
Dataset courtesy of University of Sydney 25
Results – Victoria Park

Dataset courtesy of University of Sydney 26

Results – Data Association

27
Results – Accuracy

28
 Grid-based SLAM

 Can we solve the SLAM problem if no pre-defined

landmarks are available?
 Can we use the ideas of FastSLAM 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”)

29
Mapping using Raw Odometry

30
 Mapping with Known Poses

 Mapping with known poses using laser range data

31
 Rao-Blackwellization
poses map observations & movements

Factorization first introduced by Murphy in 1999 32

 Rao-Blackwellization
poses map observations & movements

SLAM posterior

Robot path posterior

Mapping with known poses

Factorization first introduced by Murphy in 1999 33

 Rao-Blackwellization

This is localization, use MCL

Use the pose estimate

from the MCL part and apply
mapping with known poses
34
A Graphical Model of Rao-
Blackwellized Mapping

u0 u1 ut-1

x0 x1 x2 ... xt

z1 z2 zt
35
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

36
 Particle Filter Example

3 particles

map of particle 1 map of particle 3

37
map of particle 2
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:
Improve the pose estimate before applying
the particle filter

38
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

39
Motion Model for Scan Matching

Raw Odometry
Scan Matching

40
Mapping using Scan Matching

41
 FastSLAM 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 FastSLAM

 Fewer particles are needed, since the

error in the input in smaller

[Haehnel et al., 2003] 42

Graphical Model for Mapping
with Improved Odometry
u0 ... uk-1 uk ... u2k-1 un·k ... u(n+1)·k-1
z1 ... z k-1 zk+1...z2k-1 ... z n·k+1... z(n+1)·k-1

u'1 u'2 ... u'n

x0 xk x2k ... x n·k

zk z2k ... zn·k

43
FastSLAM with Scan-Matching

44
FastSLAM with Scan-Matching

Loop Closure
45
 FastSLAM with Scan-Matching

46

Map: Intel Research Lab Seattle

Comparison to Standard FastSLAM
 Same model for observations
 Odometry instead of scan matching as input
 Number of particles varying from 500 to 2.000
 Typical result:

47
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.

48
Improved Proposal
 The proposal adapts to the structure
of the environment

49
Selective Re-sampling
 Re-sampling is dangerous, since
important samples might get lost
(particle depletion problem)

 In case of suboptimal proposal

distributions re-sampling is
necessary to achieve convergence.

 Key question: When should we re-

sample?
50
 Number of Effective Particles

 Empirical measure of how well the goal distribution

is approximated by samples
drawn from the proposal
 neff describes “the variance of the particle weights”

 neff is maximal for equal weights. In this case, the

distribution is close to the proposal

51
Resampling with Neff

 Only re-sample when neff drops below

a given threshold (n/2)

 See [Doucet, ’98; Arulampalam, ’01]

52
 Typical Evolution of neff
visiting new
areas closing the
first loop

visiting
known areas

second loop closure53

Intel Lab
 15 particles
 four times faster
than real-time
P4, 2.8GHz
 5cm resolution
during scan
matching
 1cm resolution in
final map

54
Intel Lab
 15 particles
 Compared to
FastSLAM with
Scan-Matching,
the particles are
propagated
closer to the true
distribution

55
Outdoor Campus Map
 30 particles
 250x250m2
 1.75
1.088km
miles
(odometry)
 20cm resolution
during scan
matching
 30cm resolution
in final map

56
Outdoor Campus Map - Video

57
MIT Killian Court

 The “infinite-corridor-dataset” at MIT

58
MIT Killian Court

59
MIT Killian Court - Video

60
Conclusion
 The ideas of FastSLAM 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 FastSLAM can
handle larger environments than naïve
implementations in “real time” since they need
one order of magnitude fewer samples
61
 More Details on FastSLAM
 M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A
factored solution to simultaneous localization and mapping, AAAI02

 D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient

FastSLAM algorithm for generating maps of large-scale cyclic
environments from raw laser range measurements, IROS03

 M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An

Improved particle filtering algorithm for simultaneous localization
and mapping that provably converges. IJCAI-2003

 G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based

slam with rao-blackwellized particle filters by adaptive proposals
and selective resampling, ICRA05

 A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous

localization and mapping without predetermined landmarks,
IJCAI03

62