Step-by-step Calculation of Covariance Matrices in Ekf-slam

Extended Kalman Filter (EKF) Simultaneous Localization and Mapping (SLAM) is a technique used in robotics to build a map of an unknown environment while simultaneously determining the robot’s position within it. A key component of EKF-SLAM is the covariance matrix, which represents the uncertainty in the robot’s estimated state and the map features. This article provides a step-by-step process for calculating covariance matrices within EKF-SLAM.

Initialization of Covariance Matrix

The process begins with initializing the covariance matrix, typically denoted as P. This matrix combines the uncertainties of the robot’s pose and the map features. The initial covariance reflects the initial confidence in the robot’s starting position and the known features.

Prediction Step

During the prediction phase, the robot’s motion model is used to estimate the new state. The covariance matrix is updated using the Jacobian of the motion model, denoted as F, and the process noise covariance, Q. The update follows:

P_pred = F P F^T + Q

Update Step with Measurements

When new sensor measurements are received, the covariance matrix is updated to incorporate this information. The measurement model’s Jacobian, H, and the measurement noise covariance, R, are used to compute the Kalman gain, K:

K = P_pred H^T (H P_pred H^T + R)^-1

The covariance matrix is then updated as:

P_upd = (I – K H) P_pred

Incorporating Map Features

The covariance matrix expands to include map features, increasing in size as new features are added. Each update adjusts the uncertainty associated with both the robot’s pose and the features, maintaining a consistent estimate of the overall uncertainty in the map and localization.