Introduction to SLAM Simultaneous Localization And Mapping

1 Introduction to SLAMS imultaneous LocalizationAnd Mapping Paul RobertsonCognitive RoboticsWed Feb 9th, 2005 Outline Introduction Localization SLAM KalmanFilter Example Large SLAM Scaling to large maps2 Introduction3 ( Localization ) Robot needs to estimate its location with respects to objects in its environment (Map provided). ( Mapping ) Robot need to map the positions of objects that it encounters in its environment (Robot position known) (SLAM) Robot simultaneously maps objects that it encounters and determines its position (as well as the position of the objects) using noisy Movie4 Localization Tracking Bounded uncertainty Can flip into kidnapping problem Global Localization Initially huge uncertainties Degenerates to tracking Kidnapping Problem Unexpected global localizationTrackingGlobalLocalizationKi dnapped5 Representing RobotsPosition of robot is represented as a triple consisting of its xt, ytcomponents and its heading t.

2 Xt=(xt, yt, t)TXt+1=Xt+ (ut tcos t, ut tsin t, ut t)Txtxt+1 txi, yiut t6 KalmanFilter LocalizationRobotLandmark7 KalmanFilter LocalizationRobotLandmark8 KalmanFilter LocalizationRobotLandmark9 KalmanFilter LocalizationRobotLandmark10 Basic SLAM Localize using a KalmanFilter (EKF) Consider all landmarks as well as the robot position as part of the posterior. Use a single state vector to store estimates of robot position and feature positions. Closing the loop allows estimates to be improved by correctly propagating all the coupling between estimates which arise in map Feature ACBA12 Drive ForwardCBA13 Initialize C14 CBAI nitialize B15 ACBD rive Back16 ACBRe-measure A17 ACBRe-measure B18 ACBB asic EKF framework for SLAM x = State Vector, P= ..Covariance Matrix (Square, Symmetric)x and P grow as features are added to the map!

3 19 SLAM robot initial position as the root of the world coordinate space or start with some pre-existing features in the map with high uncertainty of the robot : When the robot moves, motion model provides new estimates of its new position and also the uncertainty of its location positional uncertainty always : (a) Add new features to map (b) re-measure previously added steps 2 and 3 as appropriate. 20 The State all state variables need to be requires memory of the previous is quadratic on the number of state state estimate with a new current state from the previous state estimate and the time the observation from the the difference between the predicted observation and the actual the estimate of the current stateVector KalmanFiltergaincorrectionG(k)CTAY(k)+-+ +X(k)estimatedelayAX(k-1)measurement parameterACX(k-1)Y(k)system parametercurrent prediction23G(k) = AP(k|k-1)CT[CP(k|k-1)CT+R(k)]-1 (Predictor gain)P(k+1|k) = [A-G(k)C]P(k|k-1)AT+Q(k) (Prediction mean square error)24 Matrix newMeasurement(Matrix measurement, double ts){Matrix matP1=null; // variances of components of predictionMatrix cvecE=null.}

4 // residual errorMatrix cvecY=null; // estimated inputdouble timeDelta=ts-pts; // Establish system matrix for time duration since the last ( (timeDelta));matAt= ();// Predict the new state at the given time stepcvecX= (cvecX);// Measurement prediction-estimate of inputcvecY= (cvecX);cvecE= (cvecY);// Calculate Kalmangain matKmatP1= (matP).mul(matAt).add(matQ);matK= (matCt).mul( (matP1).mul (matCt).add(matR).invert());// update the error covariance matrixmatP= ( (matC).mul(matP1));cvecX= ( (cvecE)); // Correctpts=ts;return (cvecX); // return estimated values}Simple ExampleRobot with estimated position (Px, Py) and estimated velocity (Vx, Vy). Four state variables ( Px, Py, Vx, Vy)TRelationships between the state variables:Px+= tVxPy+= tVyObservations.

5 (Px, Py) Position variance ( p) = variance ( v) = Equation26Px(k)Py(k)Vx(k)Vy(k)Px(k+1)Py( k +1)Vx(k +1)Vy(k +1)=1 0 T 00 1 0 T0 0 1 00 0 0 100 v v+ X(k+1) A X(k) w(k)Observations27 Observation Matrix Y(k) =Measurement Covariance Matrix = p 00 pPxPyPx(k)Py(k)=1 0 0 00 1 0 0Px(k)Py(k)Vx(k)Vy(k)+ p pY(k) C X(k) v(k)System Noise0 0 0 00 0 0 00 0 v 00 0 0 vSystem Noise Covariance Matrix Q(k) =28 Prediction Matrices p 0 p 00 p 0 p0 p p v 00 p0 p vInitial Prediction Covariance Matrix (P) =State Equations (A) =1 0 T 00 1 0 T0 0 1 00 0 0 12930 Run KalmanFilter Demo Program31 Problems with the distribution (Gaussian) often be expensive with large number of state linear transition model system equations must be specifiable as a multiplication of the state equation.

6 The Extended KalmanFilter (EKF) attempts to overcome this Reading List for KalmanFilterRussell&Norvig Artificial Intelligence a modern approach second edition (pp551-559).33 Large SLAMB asic SLAM is quadratic on the number of features and the number of features can be very large. Intuitively we want the cost of an additional piece of information to be look at one approach that addresses this issue by dividing the map up into overlapping sub Consistent, Convergent, and Constant-Time SLAM IJCAI 033435 Location Vector The mapped space is divided up into overlapping sub maps with shared features in the overlapping sub (i, j) = (x, y, )T(Location vector)An entity is a location + a unique Map is a collection of entities described with respect to a local coordinate frameEach map has a root entity i and a map location vector T(G,m)3637 Complete Algorithm Description on Blackboard38