[SOLVED] CS CS 4610/5335 Mapping and SLAM

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