[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