SLAM: Simultaneous Localization and Mapping: Part IOverviewMotivationWhat is SLAM?Why is SLAM a hard problem?SLAM ApplicationsSlide 7TerminologySlide 9Slide 10Bayes FilterSlide 12SLAMSLAM algorithmSlide 15Slide 16EKF-SLAMEKF : Non-linear FunctionEKF : LinearizationEKF AlgorithmSlide 21Slide 22Slide 23FastSLAMSlide 25Particle FiltersParticle Filter AlgorithmSlide 28Slide 29FastSLAM – Action UpdateFastSLAM – Sensor UpdateSlide 32Issues will be addressed in Part IISummarySLAM: Simultaneous Localization and Mapping: Part IChang Young KimThese 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 fromProbabilistic Robotics.http://www.probabilistic-robotics.comOverviewIntroductionBasics: Bayes filtersSLAMFormulationEKF-SLAMFastSLAMMotivationExit< Adapted from "American Maze" by Dale Wilkins >What is SLAM?Given:The robot’s controlsObservations of nearby featuresEstimate:Map of featuresPath of the robotA robot is exploring an unknown, static environment.Why is SLAM a hard problem?SLAM: robot path and map are both unknown Robot LandmarkEstimatedTrueSLAM ApplicationsIndoorsSpaceUnderseaUndergroundOverviewIntroductionBasics: Bayes filtersSLAMFormulationEKF-SLAMFastSLAM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 MapTerminology lll or mmmnn},...,{},...,{11iil or mTerminology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. ( | )t tP z x),|(1 tttuxxP( | , )t tP z x mTerminologyBelief:Posterior probabilityConditioned on available data Prediction:Estimate before measurement data ( ) ( | , )t t t tBel x p x z u=bel(xt)bel(xt)1( ) ( | , )t t t tBel x p x z u-=PredictionUpdateBayes Filter111)(),|()(ttttttdxxbelxuxpxbel)()|()(ttttxbelxzpxbelOverviewIntroductionBasics:Bayes filtersSLAMFormulationEKF-SLAMFastSLAMSLAMNo Map Available and No Pose Info),|,(:1:1:1 tttuzmxp. . .Landmark 1observationsRobot posescontrolsx1x2xtu1 ut-1m2m1z1z2x3u1z3ztLandmark 2x0u0SLAM algorithmPredictionUpdate1 1 1( , ) ( | , ) ( , )t t t t t tbel x m p x u x bel x m dx- - -=�( , ) ( | , ) ( , )t t t tbel x m p z x m bel x mh=1: 1: 1:( , | , ) ( , )t t t tp x m z u bel x m=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( , | ) ( | ) ( | )t t t t tp x m z P x z P m z�( | , )t tp z x m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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 vectorEKF-SLAM1{ , ,..., }t t Mx m mm =1: 1: 1:( , | , ) ~ ( , )t t t t tp x m z u N m S[ ]Tt t tmmS =EKF : Non-linear FunctionEKF : Linearization20EKF Algorithm 1. Algorithm EKF( t-1, t-1, ut, zt):2. Prediction:3. 4. 5. Correction:6. 7. 8. 9. Return t, t tttttuBA  1tTttttRAA  11)(tTtttTtttQCCCK)(ttttttCzKttttCKI  )(),(1tttugtTttttRGG  11)(tTtttTtttQHHHK))((ttttthzKttttHKI  )(EKF-SLAMMap Correlation matrixEKF-SLAMMap Correlation matrixEKF-SLAMMap Correlation matrix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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.FastSLAM( , | ) ( | ) ( | , )t t t t t tp X m z P X z P m X z=( , | ) ( | ) ( | )t t t t tp x m z P x z P m z�( | , )t tP m X z( | )t tP X z( ) ( )( | , ) ( | , )Mi it t j t tjP m X z P m X z=�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 FiltersWeighted samplesAfter resamplingdraw xit1 from Bel(xt1)draw xit from p(xt | xit1,ut1)Importance factor for xit:)|()(),|()(),|()|(ondistributi proposalondistributitarget 111111ttttttttttttitxzpxBeluxxpxBeluxxpxzpw1111)(),|()|()(ttttttttdxxBeluxxpxzpxBelParticle Filter AlgorithmParticle Filters)|()()()|()()|()(xzpxBelxBelxzpwxBelxzpxBelImportance Sampling'd)'()'|()(,xxBelxuxpxBelRobot MotionFastSLAM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 EKFsLandmark 1 Landmark 2 Landmark M…x, y, Landmark 1 Landmark 2 Landmark M…x, y, Particle#1Landmark 1 Landmark 2 Landmark M…x, y, Particle#2ParticleN…FastSLAM – Action UpdateParticle #1Particle #2Particle #3Landmark #1FilterLandmark #2FilterFastSLAM – Sensor UpdateParticle #1Particle #2Particle #3Landmark #1FilterLandmark #2FilterFastSLAM – Sensor UpdateParticle #1Particle