The Extended Kalman Filter

The EKF is the recursive estimator at the heart of nearly every navigation system. It maintains a Gaussian belief over the state — a mean $\hat{\mathbf{x}}$ and covariance $P$ — propagating it through a (possibly nonlinear) motion model and correcting it with (possibly nonlinear) measurements, each weighted by its uncertainty.

Symbols used on this page are defined in the Nomenclature → estimation & filtering.

The model

A nonlinear discrete-time system with process noise $\mathbf{w}\sim\mathcal N(0,Q)$ and measurement noise $\mathbf{v}\sim\mathcal N(0,R)$:

$$ \mathbf{x}_k = f(\mathbf{x}_{k-1}) + \mathbf{w}_k,\qquad \mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k $$

The EKF's trick: linearize $f$ and $h$ about the current estimate using their Jacobians

$$ F=\left.\frac{\partial f}{\partial \mathbf{x}}\right|_{\hat{\mathbf x}},\qquad H=\left.\frac{\partial h}{\partial \mathbf{x}}\right|_{\hat{\mathbf x}} $$

so the covariance can be propagated with the standard linear-Gaussian equations.

Predict & update

Predict (time update) $$\hat{\mathbf x}^- = f(\hat{\mathbf x})$$ $$P^- = F P F^\top + Q$$

Push the estimate forward; uncertainty grows by the process noise.

Update (measurement update) $$\mathbf y = \mathbf z - h(\hat{\mathbf x}^-)\quad(\text{innovation})$$ $$S = H P^- H^\top + R,\quad K = P^- H^\top S^{-1}$$ $$\hat{\mathbf x} = \hat{\mathbf x}^- + K\mathbf y,\quad P=(I-KH)P^-$$

Fold in the measurement; uncertainty shrinks. $K$ balances trust in model vs sensor.

Reading the Kalman gain. $K=P^-H^\top S^{-1}$ is large when the state is uncertain ($P^-$ big) or the measurement is precise ($R$ small) — then the filter leans on the measurement. When the sensor is noisy ($R$ big), $K\to 0$ and the filter trusts its prediction.

A nonlinear example: range & bearing

A classic EKF problem: a sensor at the origin measures nonlinear range $r=\sqrt{x^2+y^2}$ and bearing $\theta=\operatorname{atan2}(y,x)$ to a moving target. The measurement Jacobian is

$$ H=\begin{bmatrix}\dfrac{x}{r} & \dfrac{y}{r} & 0 & 0\\[6pt] -\dfrac{y}{r^2} & \dfrac{x}{r^2} & 0 & 0\end{bmatrix} $$

The filter runs a constant-velocity prediction and corrects with each range/bearing return. Raise the noise or drop the measurement rate and watch the amber covariance ellipse swell during gaps — the filter honestly reports that it is less sure:

When the EKF struggles

Strong nonlinearity — a single linearization point is a poor approximation; the Unscented KF (sigma points) or a particle filter does better.
Bad initialization — linearizing about a wrong estimate can diverge.
Wrong noise models — too-small $Q$ makes the filter overconfident and reject good data (the failure the tracker guards against with gating).

Related: IMU supplies the prediction; GNSS and star trackers supply the updates. The SimpleTrack tracker is a linear KF special case.