Extended Kalman Filter (EKF)

The Extended Kalman Filter is the workhorse of recursive state estimation with nonlinear models. It maintains a Gaussian belief $\mathcal{N}(\hat{\mathbf{x}}, P)$ over the state and alternates two steps — predict (propagate the belief through a motion model) and update (correct it with a measurement) — which is exactly the recursive Bayes filter with all densities approximated as Gaussians and all models linearized at the current estimate.

Setup

The system is described by a nonlinear motion model and a nonlinear observation model:

\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k, \qquad \mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, Q_k)

\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k, \qquad \mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, R_k)

where $\mathbf{x}$ is the state (e.g., camera pose, velocity, landmark positions), $\mathbf{u}$ a control or IMU input, $\mathbf{z}$ a measurement (e.g., pixel coordinates of a tracked feature), and $Q$ , $R$ the process and measurement noise covariances. The EKF linearizes $f$ and $h$ with their Jacobians evaluated at the current estimate:

F_k = \left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k-1|k-1}}, \qquad H_k = \left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf{x}}_{k|k-1}}

Predict

\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k)

P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k

The mean is pushed through the full nonlinear model; only the covariance uses the linearization. Uncertainty grows in this step (dead-reckoning drift).

Update

\mathbf{y}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1}) \qquad \text{(innovation)}

S_k = H_k P_{k|k-1} H_k^T + R_k \qquad \text{(innovation covariance)}

K_k = P_{k|k-1} H_k^T S_k^{-1} \qquad \text{(Kalman gain)}

\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k \mathbf{y}_k, \qquad P_{k|k} = (I - K_k H_k)\, P_{k|k-1}

The gain $K_k$ weighs the measurement against the prediction: confident predictions (small $P$ ) or noisy sensors (large $R$ ) yield small corrections, and vice versa. The innovation covariance $S_k$ also supports gating — a Mahalanobis test $\mathbf{y}^T S^{-1} \mathbf{y} < \chi^2$ threshold rejects outlier measurements before they corrupt the state.

EKF-SLAM

In EKF-SLAM (the formulation behind MonoSLAM), the state stacks the camera/robot pose and all landmark positions,

\mathbf{x} = \begin{bmatrix} \mathbf{x}_{\text{robot}}^T & \mathbf{m}_1^T & \cdots & \mathbf{m}_n^T \end{bmatrix}^T,

and $P$ stores the full joint covariance including pose-landmark and landmark-landmark correlations. Those cross-correlations are what makes loop closure work in a filter: correcting the pose also drags every correlated landmark. The costs are structural:

Quadratic scaling: $P$ has $O(n^2)$ entries and each update touches all of them, capping real-time EKF-SLAM at small maps (on the order of tens to a hundred landmarks).
Linearization error: Jacobians are evaluated at the current estimate once, and errors are baked permanently into $P$ — unlike optimization approaches, the filter cannot re-linearize the past. This causes the well-known inconsistency (overconfidence) of EKF-SLAM.
Rotation handling: naive parameterizations of orientation misbehave; practical filters use the error-state (indirect) EKF, where the filter estimates a small error around a nominal state and quaternion kinematics handle attitude.

These limitations are why modern visual SLAM moved to keyframe-based nonlinear optimization (“why filter?” debate, Strasdat et al.), while filtering survives where its constant-time recursive form shines: visual-inertial odometry (MSCKF, ROVIO, error-state EKFs) and sensor fusion of odometry with GPS/RADAR/wheel encoders.

Why it matters for SLAM

The EKF is the historical foundation of SLAM (EKF-SLAM was the solution for a decade) and is still the backbone of production VIO (MSCKF derivatives run on many AR headsets and drones). Even in optimization-centric pipelines, EKF concepts are everywhere: prediction/update structure, innovation gating for outlier rejection, covariance propagation for uncertainty, and marginalization (a fixed-lag smoother’s marginalization step is algebraically a Kalman update). Understanding when the EKF’s single-linearization assumption breaks is the key to understanding why bundle adjustment wins on accuracy and why MSCKF-style filters delay linearization.