1. Probabilistic Robotics
SLAM and FastSLAM

2. The SLAM Problem SLAM stands for simultaneous localization and mapping
originally developed by Hugh Durrant-Whyte and John J. Leonard
“Mobile robot localization by tracking geometric beacons”
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
Errors correlate!

3. SLAM Applications
Indoors
Space
Undersea
Underground

4. Typical Robot Equipment
Mobile robot
Typically want error under
2 cm per meter moved
2° per 45° degrees turned

5. Typical Robot Equipment
Range finding sensors
Sonar
10 mm accuracy
4 m range
SICK laser
80 m range
IR range finder
1.5 m range
Vision
Increasingly viable

6. Outline of typical SLAM Process

7. Representations Grid maps or scans Landmark-based
[Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…]
Landmark-based
[Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…

8. The SLAM Problem Given: Estimate: The robot’s controls
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

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

10. Structure of the Landmark-based SLAM-Problem

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

21. Detecting Landmarks Look for spikes in range data
Red dots show table legs
Fit geometric primitives
RANSAC line fitting algorithm
Don’t want to fit to all data
Pick a random subset
Fit a primitive
Classify remaining points as outlier or not
Keep if enough points classify well
Keep a full occupancy map
Each cell is a landmark

22. (E)KF SLAM
Represent robot state and landmark position all in one big state
( rx, ry, l1x, l1y, l2x, l2y,…)
Apply Kalman filter to state update and observations
Uncertainty represented in large covariance matrix

23. Kalman Filter and Linearity
For many applications, the time update and measurement equations are NOT linear
The KF is not directly applicable
Linearize around the non-linearities
Use of the KF is extended to more realistic situations

24. The Extended Kalman Filter (EKF)
The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm
The EKF allows for estimation of non-linear processes or measurement relationships
Kalman Filter
Extended Kalman Filter

25. Linearity Assumption Revisited

26. Non-linear Function

27. EKF Linearization (1)

28. EKF Linearization (2)

29. EKF Linearization (3)

30. (E)KF-SLAM Map with N landmarks:(3+2N)-dimensional Gaussian
Can handle hundreds of dimensions

31. Map Correlation matrix
EKF-SLAM
Map Correlation matrix

32. Map Correlation matrix
EKF-SLAM
Map Correlation matrix

33. Map Correlation matrix
EKF-SLAM
Map Correlation matrix

34. EKF-SLAM Can be quadratic in number of landmarks
Non-linearities a problem
Has led to other approaches

35. 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 if the position of the sensor is known.

36. 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
Landmark 1
Landmark 2
Landmark M …
x, y, 

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

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

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

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

41. Improved Proposal
The proposal adapts to the structure of the environment

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

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

44. Selective Re-sampling
Re-sampling is dangerous, since important samples might get lost (particle depletion problem)
Key question: When should we re-sample?

45. Typical Evolution of particle number
visiting new areas
closing the first loop
visiting known areas
second loop closure

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

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