SLAM: Simultaneous Localization and Mapping: Part I

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 < Adapted from "American Maze" by Dale Wilkins>

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

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

6. Indoors Undersea Underground Space SLAM Applications

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

8. Terminology • Robot State (or pose):xt =[ x, y, θ] • Position and heading • x1:t = {x1, …, xt} • Robot Controls:ut • Robot motion and manipulation • u1:t = {u1,..., ut} • Sensor Measurements:zt • Range scans, images, etc. • z1:t = {z1,..., zt} • Landmark or Map: • Landmarks or Map

9. Terminology • Observation model: or • The probability of a measurement zt given that the robot is at position xt and map m. • Motion Model: • The posterior probability that action ut carries the robot from xt-1 to xt.

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

11. Bayes Filter • Prediction • Update

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

13. m1 Landmark 1 z1 z3 observations . . . x1 x2 x3 xt x0 Robot poses u1 ut-1 u1 u0 controls z2 zt m2 Landmark 2 SLAM • No Map Available and No Pose Info

14. SLAM algorithm • Prediction • Update

15. SLAM • 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.

16. SLAM • 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)

17. EKF-SLAM • 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

18. EKF : Non-linear Function

19. EKF : Linearization

20. EKF Algorithm Algorithm EKF( mt-1,St-1, ut, zt): Prediction: Correction: Returnmt,St 20

21. EKF-SLAM Map Correlation matrix

22. EKF-SLAM Map Correlation matrix

23. EKF-SLAM Map Correlation matrix

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

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

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

27. draw xit-1from Bel(xt-1) draw xitfrom p(xt | xit-1,ut-1) Importance factor for xit: Particle Filter Algorithm

28. Particle Filters Importance Sampling Robot Motion

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

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

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

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

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.