1. SLAM: Simultaneous Localization and Mapping: Part I Chang Young Kim These slides are based on: Probabilistic Robotics, S. Thrun, W. Burgard, D. Fox, MIT Press, 2005 and Zane Goodwin’s Slide from the previous class Many images are also taken from Probabilistic Robotics. http://www.probabilistic-robotics.com

2. Overview Introduction Basics: Bayes filters SLAM  Formulation  EKF-SLAM  FastSLAM

3. Motivation Exit

4. What is SLAM? Given:  The robot’s controls  Observations of nearby features Estimate:  Map of features  Path of the robot A robot is exploring an unknown, static environment.

5. Why is SLAM a hard problem? SLAM: robot path and map are both unknown RobotLandmark Estimated True

6. SLAM Applications Indoors Space Undersea Underground

7. Overview Introduction Basics: Bayes filters SLAM  Formulation  EKF-SLAM  FastSLAM

8. Robot State (or pose): x t =[ x, y, θ]  Position and heading  x 1:t = {x 1, …, x t } Robot Controls: u t  Robot motion and manipulation  u 1:t = {u 1,..., u t } Sensor Measurements: z t  Range scans, images, etc.  z 1:t = {z 1,..., z t } Landmark or Map:  Landmarks or Map Terminology

9. Observation model: or  The probability of a measurement z t given that the robot is at position x t and map m. Motion Model:  The posterior probability that action u t carries the robot from x t-1 to x t.

10. Terminology Belief:  Posterior probability  Conditioned on available data  Prediction:  Estimate before measurement data  b e l ( x t ) b e l ( x t )

11. Prediction Update Bayes Filter

12. Overview Introduction Basics:Bayes filters SLAM  Formulation  EKF-SLAM  FastSLAM

13. SLAM No Map Available and No Pose Info... Landmark 1 observations Robot poses controls x1x1 x2x2 xtxt u1u1 u t-1 m2m2 m1m1 z1z1 z2z2 x3x3 u1u1 z3z3 ztzt Landmark 2 x0x0 u0u0

14. SLAM algorithm Prediction Update

15. The observation model makes the dependence between observations and robot locations. The joint posterior cannot be partitioned because there is dependence between observations and robot locations. SLAM

16. Correlations between landmark estimates increase monotonically as more and more observations are made. Thus, this dependency is solved by two different methods:  Data association using a correlation matrix (EKF-SLAM).  Rao-Blackwelized Particles Filter (FastSLAM) SLAM

17. Extended Kalman Filter approximates the posterior as a Gaussian Mean (state vector) contains robot location and map points Covariance Matrix estimates uncertainties and relationships between each element in state vector EKF-SLAM

18. EKF : Non-linear Function

19. EKF : Linearization

20. 20 EKF Algorithm 1. Algorithm EKF(  t-1,  t-1, u t, z t ): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return  t,  t

21. EKF-SLAM Map Correlation matrix

22. EKF-SLAM Map Correlation matrix

23. EKF-SLAM Map Correlation matrix

24. EKF-SLAM Drawbacks  EKF-SLAM employs linearized models of nonlinear motion and observation models and so inherits many caveats.  Computational effort is demand because computation grows quadratically with the number of landmarks. Better Solution : FastSLAM using a particle filter  The particle filter represents nonlinear process model and non- Gaussian pose distribution for the robot pose estimation  Rao-Blackwellized method reduces computation (*FastSLAM still linearizes the observation model i.e., EKF) FastSLAM

25. However, X t is trajectory rather than the single pose x t. The trajectory X t is represented by particles X t (i) Represented by factorizing called Rao-Blackwellized Filter. Solution  by a particle filter  by EKF. FastSLAM

26.  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 Particle Filters Weighted samples After resampling

27. draw x i t  1 from Bel (x t  1 ) draw x i t from p ( x t | x i t  1,u t  1 ) Importance factor for x i t : Particle Filter Algorithm

28. Particle Filters Importance Sampling Robot Motion

29. 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 Landmark 1Landmark 2Landmark M … x, y,  Landmark 1Landmark 2Landmark M … x, y,  Particle #1 Landmark 1Landmark 2Landmark M … x, y,  Particle #2 Particle N …

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

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

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

33. Issues will be addressed in Part II Computation Convergence Data Association

34. Summary SLAM Is Hard SLAM problem and the essential methods for solving it. Key implementations and demonstrations of the methods.