SLAM: Simultaneous Localization and Mapping: Part II BY TIM BAILEY AND HUGH DURRANT-WHYTEOverviewWhat is SLAM?TerminologySlide 5SLAM algorithmEKF State Space ModelEKF-SLAMSlide 9EKF-SLAM : ComplexityState AugmentationSlide 12Partitioned UpdatesSlide 14SparsificationSlide 16Slide 17Data Association ProblemBatch GatingSIFTMulti-Hypothesis Data AssociationPer-Particle Data AssociationFuture WoksSLAM: Simultaneous Localization and Mapping: Part IIBY TIM BAILEY AND HUGH DURRANT-WHYTEPresented by Chang Young KimThese slides are based on:Probabilistic Robotics, S. Thrun, W. Burgard, D. Fox, MIT Press, 2005 Many images are also taken fromProbabilistic Robotics.http://www.probabilistic-robotics.comOverviewReviewSLAMReducing complexityState AugmentationPartitioned UpdatesSparsificationData associationBatch GatingSIFTMulti-HypothesisFuture worksWhat 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.TerminologyRobot State (or pose): Position and heading Robot Controls:Robot motion and manipulation Sensor Measurements:Range scans, images, etc. Landmark or Map: Landmarks or Map1{ ,..., }nm m m =ztutxt=(x;y;µ)x1:t=fx1;x2; : : : ;xtgu1:t=fu1;u2;: : :;utgz1:t=fz1;z2;: : :;ztim }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. ( | )t tP z x),|(1 tttuxxP( | , )t tP z x mSLAM 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=7EKF State Space ModelPredictionUpdate whereMaintaining values: Bel(xt,m) and its covariance matrix Pt. Map with N landmarks:(3+2N)-dimensional Gaussian.81 21 21 21 1 1 1 1 2 12 2 2 1 2 2 21 222221222( , ) ,NNNNNN N N N N Nx xy x xl xl xlxy y y yl yl ylx y l l ltxl yl l l l l l lxl yl l l l l l lNxl yl l l l l l lxyBel x m mmmqqq q q q q qqqqs s s s s ss s s s s ss s s s s sqs s s s s ss s s s s ss s s s s s�� ��� �� �� �� �=� �� �� �� �� �� ��LLLLLMM M M M M O ML��� �� �� �� �� �� �� �� �� ��EKF-SLAMOverviewReviewSLAMReducing complexityState AugmentationPartitioned UpdatesSparsificationData associationBatch GatingSIFTMulti-HypothesisFuture worksComplexity O(N3) with N landmarks due to the covariance matrix and matrix multiplication of Jacobian.Can handle hundreds of dimensions?  It can be reduced by approximation methods: State Augmentation for the prediction stagePartitioned Updates for the update stageSparsification using an information form101 21 21 21 1 1 1 1 2 12 2 2 1 2 2 21 222221222( , ) ,NNNNNN N N N N Nx xy x xl xl xlxy y y yl yl ylx y l l ltxl yl l l l l l lxl yl l l l l l lNxl yl l l l l l lxyBel x m mmmqqq q q q q qqqqs s s s s ss s s s s ss s s s s sqs s s s s ss s s s s ss s s s s s�� ��� �� �� �� �=� �� �� �� �� �� ��LLLLLMM M M M M O ML��� �� �� �� �� �� �� �� �� ��EKF-SLAM : Complexity1 1 1( , ) ( | , ) ( , )t t t t t tbel x m p x u x bel x m dx- - -=�111 21 21 21 1 1 1 1 2 12 2 2 1 2 2 21 222221222( , ) ,NNNNNN N N N N Nx xy x xl xl xlxy y y yl yl ylx y l l ltxl yl l l l l l lxl yl l l l l l lNxl yl l l l l l lxyBel x m mmmqqq q q q q qqqqs s s s s ss s s s s ss s s s s sqs s s s s ss s s s s ss s s s s s�� ��� �� �� �� �=� �� �� �� �� �� ��LLLLLMM M M M M O ML��� �� �� �� �� �� �� �� �� ��State AugmentationPrediction :Solution : State Augmentation • Separating the state into an augmented states• Update only affected matrixesStaticState AugmentationCovariance predictionCovariance predictionState AugmentationStaticO(N3)O(N)131 21 21 21 1 1 1 1 2 12 2 2 1 2 2 21 222221222( , ) ,NNNNNN N N N N Nx xy x xl xl xlxy y y yl yl ylx y l l ltxl yl l l l l l lxl yl l l l l l lNxl yl l l l l l lxyBel x m mmmqqq q q q q qqqqs s s s s ss s s s s ss s s s s sqs s s s s ss s s s s ss s s s s s�� ��� �� �� �� �=� �� �� �� �� �� ��LLLLLMM M M M M O ML��� �� �� �� �� �� �� �� �� ��Partitioned UpdatesUpdate :Solution : Partitioned Update with local submap.• Confines the map to a small local region.• Only Updates the small local region.• Updates the whole map only at a much lower frequency( , ) ( | , ) ( , )t t t tbel x m p z x m bel x mh=Partitioned Updates( , )L t LBel x m1 21 21 21 1 1 1 1 2 12 2 2 1 2 2 21 222221222,NNNNNN N N N N Nx xy x xl xl xlxy y y yl yl ylx y l l lxl yl l l l l l lxl yl l l l l l lNxl yl l l l l l lxymmmqqq q q q q qqqqs s s s s ss s s s s ss s s s s sqs s s s s ss s s s s ss s s s s s� �� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��LLLLLMM M M M M O ML�����������1 21 21 21 1 1 1 1 2 12 2 2 1 2 2 21 222221222,NNNNNN N N N N Nx xy x xl xl xlxy y y yl yl ylx y l l lxl yl l l l l l lxl yl l l l l l lNxl yl l l l l l lxymmmqqq q q q q qqqqs s s s s ss s s s s ss s s s s sqs s s s s ss s s s s ss s s s s s� �� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��� ��LLLLLMM M M M M O ML�����������Local State :Global State:( , )G t GBel x mPeriodically registersUpdated by Local SLAM• State Bel(xt ,m) and covariance matrix Pt are Gaussian probability density which, •implicitly describes the two central moments of Gaussian• Using Moment or Information Form •Sparsification Pt Yt• Many of none diagonal components are very close to 0  they can be set to zero.SparsificationMoment Form1( , ), ,where and ( , )t …