Step 2 Re-observation

Parent: Basic EKF for SLAM

Source: SLAM for Dummies

Second step in the three-step EKF — overview

• In this step we update the robot position that we got in [step 1]](studienarbeit/step-1-odometry-update-prediction-step.md)

  • Compensate for errors due to odometry pos_est (odometry-based) - pos_actual (LM-based) = Innovation, (based on the LM that the robot can see)

  • Use this to update robot position

• Update the uncertainty of each observed LM to reflect recent changes e.g.
  • very low uncertainty of a LM.
  • Re-observing the LM from the same position with low uncertainty will increase the LM certainty (s. loop closure )
  • (variance of LM w.r.t. current robot position)
• This step is run for each re-observed landmark

1. Calculate range and bearing to the landmark (range and bearing in current measurements) using the measurement model This can be compared to the range and bearing obtained from data association (denoted as z)

2. Calculate the  Jacobian H  of the measurement model

3. Update the error matrix R to reflect range and bearing in current measurements

4. Compute Kalman gain

   1. 
   2. term in bracket is S (innovation covariance)

5. Compute new state vector (range and bearing) using Kalman gain

   1. 
   2. this updates robot pose along with landmark positions
   3. z - h = v (displacement in range and bearing)