1. Probabilistic Robotics
SLAM
Based on slides from the book's website

2. Given: The SLAM Problem
A robot is exploring an unknown, static environment.
Given:
The robot’s control signals.
Observations of nearby features.
Estimate:
Map of features.
Path (sequence of poses) of the robot.

3. Structure of the Landmark-based SLAM Problem

4. Mapping with Raw Odometry

5. SLAM Applications
Indoors
Space
Undersea
Underground

6. 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;…

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

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

9. SLAM: Simultaneous Localization and Mapping
Full SLAM:
Online SLAM:
Integrations typically done one at a time.
Aspects:
Continuous: Robot pose, object locations.
Discrete: feature correspondence i.e. relationship with previously seen objects.
Estimate entire path and map!
Estimate most recent pose and map!

10. Graphical Model of Online SLAM

11. Graphical Model of Full SLAM:

12. Techniques for Consistent Maps
Scan matching: online.
EKF SLAM: online and incremental.
Graph-SLAM: offline with stored information!
Sparse Extended Information Filters (SEIFs): online with stored knowledge 
Fast-SLAM: Rao-Blackwellized Particle Filters 

13. Scan Matching
Maximize the likelihood of the ith pose and map relative to the (i-1)th pose.
Calculate the map according to “mapping with known poses” based on the poses and observations.
robot motion
current measurement
map constructed so far
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.

14. Scan Matching Example

15. (E)Kalman Filter Algorithm
Algorithm (E)Kalman_filter( mt-1, St-1, ut, zt):
Prediction:
Correction:
Return mt, St

16. (E)KF-SLAM Map with N landmarks:(3+2N)-dimensional Gaussian.
Can handle hundreds of dimensions.
Approximately known initial pose.

17. Classical Solution – The EKF
Approximate the SLAM posterior with a high-dimensional Gaussian [Smith & Cheeseman, 1986].
Single hypothesis data association!

18. Known/Unknown Correspondences
Easier to solve 
Features associated with signatures – visual landmarks.
3N + 3 dimensions.
Unknown correspondence:
Harder to solve 
Features not associated with unique signatures – range information.

19. EKF-SLAM (Known correspondences)
Map Correlation matrix

20. EKF-SLAM (Known correspondences)
Map Correlation matrix

21. EKF-SLAM (Known correspondences)
Map Correlation matrix

22. Properties of KF-SLAM (Linear Case)
[Dissanayake et al., 2001]
Theorem:
The determinant of any sub-matrix of the map covariance matrix decreases monotonically as successive observations are made.
In the limit the landmark estimates become fully correlated!

23. Some Observations…
Over time, the x-y coordinate estimates become fully correlated!
Implications:
Absolute map coordinates relative to coordinate system defined by initial robot pose is approximately known.
Map coordinates relative to robot pose known with certainty asymptotically.
Local accuracy much better than global accuracy.

24. Victoria Park Data Set
[courtesy by E. Nebot]

25. Victoria Park Data Set Vehicle
[courtesy by E. Nebot]

26. SLAM
[courtesy by E. Nebot]

27. Map and Trajectory
Landmarks
Covariance
[courtesy by E. Nebot]

28. Landmark Covariance
[courtesy by E. Nebot]

29. Estimated Trajectory
[courtesy by E. Nebot]

30. EKF SLAM Application (MIT B21)
[courtesy by John Leonard]

31. EKF SLAM Application (MIT B21)
odometry
estimated trajectory
[courtesy by John Leonard]

32. Unknown Correspondences
Algorithm similar to the case of known correspondences.
Incremental Maximum Likelihood (ML) estimation of correspondences.
Strategy:
Propose new landmark and correspondence.
Accept if Mahalanobis distance to previous landmarks more than a threshold.

33. EKF-SLAM Summary
Have been applied successfully in real-world environments.
Convergence results for the linear case.
Can diverge if nonlinearities are large!
Quadratic in the number of landmarks: O(n2). Not suitable for more than a few 1000 landmarks.
Additional features:
Map management required to overcome errors due to Gaussian assumption and spurious landmark creation.
Landmark existence probabilities: landmarks on “probation” 
Numerical instability for large sparse matrices.
Approximations reduce the computational complexity.

34. Approximations for SLAM
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]