1. Advanced Mobile Robotics
Probabilistic Robotics:
SLAM/FastSLAM
Dr. Jizhong Xiao
Department of Electrical Engineering
CUNY City College

2. Probabilistic Robotics
SLAM/FastSLAM

3. 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?

4. 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)

5. Why is SLAM a hard problem?
Uncertanties
Error in pose
Error in observation
Error in mapping
Error accumulated

6. Why is SLAM a hard problem?
SLAM: robot path and map are both unknown!
Robot path error correlates errors in the map

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

8. 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”

9. SLAM: Simultaneous Localization and Mapping
Full SLAM:
Estimates Entire pose (x1:t) and map (m)
Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)
Estimates entire path and map!

10. Graphical Model of Full SLAM:

11. SLAM: Simultaneous Localization and Mapping
Online SLAM:
Estimates Most recent pose (xt) and map (m)
Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)
Estimates most recent pose and map!

12. Graphical Model of Online SLAM:
Integrations typically done one at a time

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

14. EKF-SLAM with known correspondences
Correspondence Data association problem
Landmarks can’t be uniquely identified
True identity of observed feature
Correspondence variable (Cit)
between feature (fit) and real landmark

15. EKF-SLAM with known correspondences
Signature Numerical value (average color)
Characterize type of landmark (integer)
Multidimensional vector
(height and color)

16. EKF-SLAM with known correspondences
Similar development to EKF localization
Diff  robot pose + coordinates of all landmarks
Combined state vector
(3N + 3)
Online posterior

17. Iteration through measurements
Mean
Motion update
Covariance
Test for new landmarks
Initialization of elements
Expected measurement
Filter is updated
Iteration through measurements

18. FastSLAM Jizhong Xiao Department of Electrical Engineering
The City College of New York

19. 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 …

20. 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!

21. 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….

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

23. Mapping using Landmarks
. . .
Landmark 1
observations
Robot poses
controls x1 x2 xt u1
ut-1 l2 l1 z1 z2 x3 z3 zt
Landmark 2 x0 u0
Knowledge of the robot’s true path renders landmark positions conditionally independent

24. 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).

25. 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)

26. Factored Posterior Robot path posterior (localization problem)
Conditionally independent landmark positions

27. Rao-Blackwellization
This factorization is also called Rao-Blackwellization
Given that the second term can be computed efficiently, particle filtering becomes possible!

28. 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
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 …

29. FastSLAM Algorithm setup

30. Basic Step of FastSLAM

31. Rao-Blackwellization
Landmarks Grid-based
map
poses
map
observations movements

32. Factored SLAM Posterior
The mathematic expression of full SLAM posterior:
After a long derivation (on page 442), it becomes

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

34. 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:
Step 1: Extend the path posterior by sampling new poses
Step 2: Updating the observed feature estimate
Step 3: Resembling
Uses control ut to sample new robot pose xt
Added into a temporary set of particle, along with the path of previous poses

35. 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
Perceptual model, Gaussian
Previous belief, Gaussian
Using EKF, approximate the measurement model g by Taylor expansion
New mean
Kalman Gain
New covariance

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

37. Re-sampling by Importance Sampler
Target distribution, takes into account of measurement zt, along with the correspondence ct
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.
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.

38. Re-sampling by Importance Sampler
Importance factor
Observed feature
Measurement model, Gaussian
With covariance:

39. FastSLAM 1.0 Algorithm with known correspondence
See table 13.1 of text book on page 450

40. Weighting particle to Approximate Target Distribution

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

42. 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
Step 2: Updating the observed feature estimate
Step 3: Re-sampling
See math on page 452~453

43. FastSLAM 2.0 Algorithm:
FastSLAM 2.0 avoids the problem by sampling poses based on the measurement Zt in addition to control ut
Extend the path posterior by sampling new poses
Updating the observed feature estimate
Re-sampling

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

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

46. Features of Fast-SLAM
Each particle can rely on its own, local data association decisions
More robust data association decision on per-particle 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.

47. Unknown Data Association
Correspondence: maximizing the likelihood of the measurement zt

48. FastSLAM – Action Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3

49. FastSLAM – Sensor Update
Landmark #1
Filter
Particle #1
Landmark #2
Filter
Particle #2
Particle #3

50. FastSLAM – Sensor Update
Particle #1
Weight = 0.8
Weight = 0.4
Weight = 0.1
Particle #2
Particle #3

51. FastSLAM - Video

52. FastSLAM Complexity O(N) O(N•log(M)) O(N•log(M))
Constant time per particle
Update robot particles based on control ut-1
Incorporate observation zt into Kalman filters
Resample particle set
O(N•log(M))
Log time per particle
O(N•log(M))
Log time per particle
N = Number of particles
M = Number of map features

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

54. Multi-Hypothesis Data Association
Data association is done on a per-particle basis
Robot pose error is factored out of data association decisions

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

56. Results – Data Association

57. Results – Victoria Park
4 km traverse
< 5 m RMS position error
100 particles
Blue = GPS
Yellow = Fast SLAM
Dataset courtesy of University of Sydney

58. Results – Victoria Park
Dataset courtesy of University of Sydney

59. 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”)

60. Rao-Blackwellization
poses
map
observations & movements

61. Rao-Blackwellization
poses
map
observations & movements
SLAM posterior
Robot path posterior
Mapping with known poses

62. Rao-Blackwellization
This is localization, use MCL
Use the pose estimate from the MCL part and apply mapping with known poses

63. A Graphical Model of Rao-Blackwellized Mappingx z u 2
... t 1
t-1

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

65. Mapping with Known Poses
Mapping with known poses using laser range data

66. Particle Filter Example
3 particles
map of particle 3
map of particle 1
map of particle 2

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

68. Pose Correction Using Scan Matching
Maximize the likelihood of the i-th pose and map relative to the (i-1)-th pose and map
map constructed so far
robot motion
current measurement
To compute consistent maps, we apply a recursive scheme.
At each point in time we compute the most likely position of the robot, given the map constructed so far.
Based on the position hat l-t, we then extend the map an incorporate the scan obtained at time t.

69. Motion Model for Scan Matching
Raw Odometry
Scan Matching

70. Mapping using Scan Matching

71. FastSLAM with Scan-Matching

72. Fast SLAM with Scan-Matching
Loop Closure

73. Fast SLAM with Scan-Matching
Map: Intel Research Lab Seattle

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

75. Intel Lab
15 particles
Compared to Fast SLAM with Scan-Matching, the particles are propagated closer to the true distribution

76. Outdoor Campus Map 30 particles 250x250m2 1.75 km (odometry)
20cm resolution during scan matching
30cm resolution in final map

77. 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]

78. 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:

79. Graphical Model for Mapping with Improved Odometryz k x 1 u' u
k-1
...
k+1
2k-1 2k 2 n
n·k
(n+1)·k-1
n·k+1

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

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

82. 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]

83. Thank You

84. 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:
Extend the path posterior by sampling new poses
Updating the observed feature estimate
Resembling – difference of target and proposal distribution. – importance factor = Target distri. / Proposal distri.
Added into a temporary set of particle, along with the path of previous poses
For n≠ct do not update
Represented by a Gaussian
For n=ct
Perceptual model