CS 4610/5335 Mapping and SLAM
Robert Platt Northeastern University 2/24/20
Material adapted from:
1. Lawson Wong, CS 4610/5335
2. Peter Corke, Robotics, Vision and Control
3. Sebastian Thrun, Wolfram Burgard, & Dieter Fox,
Probabilistic Robotics
4. Marc Toussaint, U. Stuttgart Robotics Course
Process dynamics:
2-D Mobile Robot Model
2-D Mobile Robot Model
Extended Kalman Filter: Prediction Step
Predicted mean Predicted covariance
Extended Kalman Filter: Prediction Step
For our motion model:
Extended Kalman Filter: Update Step
Innovation
Extended Kalman Filter: Update Step
Innovation Kalman gain
Extended Kalman Filter: Update Step
Innovation Kalman gain
Posterior mean Posterior cov.
Extended Kalman Filter: Update Step
For our measurement model:
Extended Kalman Filter
Extended Kalman Filter
Mapping using the EKF
How do we use the EKF to estimate landmark positions? State:
Positions of each of the M landmarks (base frame)
Mapping using the EKF
Landmark positions are now our states (assume known vehicle pose):
Highlevelidea: 1. Landmarkpositionsdonotmove 2a.If new landmark detected, expand the state 2b.Else, update the appropriate landmark
EKF Mapping: Prediction Step
EKF Mapping: Update Step (new landmark)
Given a measurement (r, ¦Â), where is the new landmark?
EKF Mapping: Update Step (new landmark)
Given a measurement (r, ¦Â), where is the new landmark? Expand the state with ¡°insertion¡± operator:
This is our new posterior mean
EKF Mapping: Update Step (new landmark)
Expand the state with ¡°insertion¡± operator:
This is our new posterior mean Covariance update:
EKF Mapping: Update Step (existing landmark)
Assumption: We know which landmark we observed: Landmark i with pose pi
EKF Mapping: Update Step (existing landmark)
Assumption: We know which landmark we observed: Landmark i with position pi = ( xi , yi )
Jacobian of (r, ¦Â) with respect to landmark position ( xi , yi ):
EKF Mapping: Update Step (existing landmark)
Known xv Unknown pi
For our measurement model:
Mapping using the EKF
Landmark positions are now our states (assume known vehicle pose):
High level idea:
1. Landmarkpositionsdonotmove Nothing to do in prediction step!
2a.If new landmark detected, expand the state State mean, covariance grow by two dimensions Covariance update requires ¡°insertion¡± Jacobian
2b.Else, update the appropriate landmark
Very similar to previous EKF update step Jacobian is now 2 * (2M) matrix instead of 2 * 3
Mapping using the EKF
SLAM using the EKF
Vehicle/landmark covariance
Vehicle covariance
Estimate both robot position and landmark positions:
robot position
Landmark positions
Landmark covariance
SLAM using the EKF
SLAM using the EKF
Landmark positions and vehicle pose are now our states:
High level idea: Combine localization with known map and mapping with known vehicle pose
SLAM using the EKF
Landmark positions and vehicle pose are now our states:
High level idea: Combine localization with known map and mapping with known vehicle pose
1. Writedownjointmotionmodel,deriveJacobian
2a.If new landmark detected, expand the state ¡°Insertion¡± Jacobian is slightly different now Derive this from the joint ¡°insertion¡± operator
2b.Else, update the appropriate landmark Measurement Jacobian also slightly different now Derive this from the joint measurement model
Recall: Mapping using the EKF
New: SLAM using the EKF (update step)
SLAM using the EKF
SLAM using the EKF
SLAM using the EKF
SLAM using the EKF
Landmark covariance drops significantly as soon as ¡°loop closure¡± occurs.
SLAM using the EKF
SLAM using the EKF
Another perspective on SLAM (GraphSLAM)
SLAM + Particle Filtering: FastSLAM
SLAM + Particle Filtering: FastSLAM
SLAM + Particle Filtering: FastSLAM
SLAM + Particle Filtering: FastSLAM
SLAM + Particle Filtering: FastSLAM