Section 10.1: Extended Kalman filter

"The Extended Kalman filter's entire career rests on one hopeful assumption: that the world, viewed closely enough, is a straight line. It usually is. The interesting failures are when it is not."
A Pragmatic AI Agent

Prerequisites

This section assumes the linear Kalman filter and its predict-correct structure from Chapter 9, including what the process noise $\mathbf{Q}$ and measurement noise $\mathbf{R}$ represent. It also assumes the Gaussian, covariance propagation, and first-order Taylor expansion, all developed in Chapter 4 and Appendix A. If the Jacobian of a vector-valued function is unfamiliar, review the multivariable-calculus notes in Appendix A before continuing.

The big picture

The linear Kalman filter of Chapter 9 is exact, but it earns that exactness by assuming both the dynamics and the measurement are linear in the state. Real sensors rarely oblige. A radar reports range and bearing, which are square roots and arctangents of position. A GNSS receiver relates pseudorange to position through a nonlinear distance. A robot's heading rotates its velocity through sines and cosines. Feed any of these to a linear filter and the Gaussian machinery collapses, because a Gaussian pushed through a nonlinear function is no longer Gaussian. The Extended Kalman filter (EKF) is the oldest and still the most deployed answer: linearize the nonlinear functions around the current estimate with a first-order Taylor expansion, then run the ordinary Kalman equations on that local linear approximation. This section explains what gets linearized and how, why the Jacobian replaces the constant matrices $\mathbf{F}$ and $\mathbf{H}$, when the approximation is trustworthy, and the specific ways it silently diverges so you can catch it before it drives a system off the road.

Why a linear filter cannot survive a nonlinear world

Keep the state as $\mathbf{x}$ and the measurement as $\mathbf{z}$, following Chapter 9. The generalization is to let both the dynamics and the measurement be arbitrary nonlinear functions,

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

with process noise $\mathbf{w}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{Q})$ and measurement noise $\mathbf{v}_k \sim \mathcal{N}(\mathbf{0}, \mathbf{R})$. The linear filter is the special case $f(\mathbf{x}) = \mathbf{F}\mathbf{x}$ and $h(\mathbf{x}) = \mathbf{H}\mathbf{x}$. The trouble is that the Kalman filter tracks a state estimate and a covariance, that is, a Gaussian, and it only stays Gaussian under linear maps. Push a Gaussian through $h(\mathbf{x}) = \sqrt{x^2 + y^2}$ and the result is skewed and heavy on one side; no mean-and-covariance pair describes it faithfully. So we cannot propagate the true posterior in closed form. We can only approximate it, and the EKF's approximation is the cheapest one that exists: pretend each nonlinear function is linear over the small region where the state probably lives.

Linearize with the Jacobian

A differentiable function looks linear if you zoom in far enough. Expand $f$ and $h$ to first order around the current best estimate and the correction terms are the Jacobians, the matrices of partial derivatives

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

These play exactly the roles that the constant $\mathbf{F}$ and $\mathbf{H}$ played in the linear filter, with one crucial difference: they are recomputed at every step, evaluated at the latest estimate. The state itself is still propagated through the true nonlinear functions; only the covariance is propagated through the linearized ones. That split is the whole idea of the EKF. Writing it out, the predict step becomes

$$ \hat{\mathbf{x}}_k^- = f(\hat{\mathbf{x}}_{k-1}, \mathbf{u}_k), \qquad \mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^\top + \mathbf{Q}, $$

and the correct step, using the innovation $\boldsymbol{\nu}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_k^-)$, becomes

$$ \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^\top \left(\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^\top + \mathbf{R}\right)^{-1}, \quad \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \boldsymbol{\nu}_k, \quad \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)\mathbf{P}_k^-. $$

Key insight

The EKF propagates the mean through the exact nonlinear function but propagates the uncertainty through a linear approximation of it. Every EKF error traces back to that mismatch. The estimate is honest about where the state is; the covariance is only as honest as the linearization is accurate over the current uncertainty ellipse. When the function curves sharply relative to the width of that ellipse, the reported covariance becomes a fiction, and because the filter never checks its own Taylor expansion, it reports that fiction with total confidence.

A worked example: tracking with range and bearing

Consider a sensor at the origin tracking a target with planar state $\mathbf{x} = [p_x, v_x, p_y, v_y]^\top$. The motion is linear (constant velocity), so $f$ uses a constant $\mathbf{F}$, but the sensor reports range $r = \sqrt{p_x^2 + p_y^2}$ and bearing $\theta = \operatorname{atan2}(p_y, p_x)$, which are thoroughly nonlinear. The measurement Jacobian $\mathbf{H}_k = \partial h / \partial \mathbf{x}$ is where the geometry enters: the range row grows as $p_x/r$ and $p_y/r$, and the bearing row scales as $1/r$, which already warns us that bearing information degrades far from the sensor. This range-bearing model is the backbone of radar tracking (Chapter 44) and of the landmark measurements in Chapter 52.

import numpy as np

def h(x):                        # nonlinear measurement: range, bearing
    px, py = x[0], x[2]
    r = np.hypot(px, py)
    return np.array([r, np.arctan2(py, px)])

def H_jacobian(x):               # d h / d x, evaluated at the current estimate
    px, py = x[0], x[2]
    r2 = px**2 + py**2
    r = np.sqrt(r2)
    return np.array([[ px/r,  0.0,  py/r,  0.0],      # d range / d state
                     [-py/r2, 0.0,  px/r2, 0.0]])     # d bearing / d state

def ekf_update(x, P, z, R):
    Hk = H_jacobian(x)
    nu = z - h(x)                                     # innovation
    nu[1] = (nu[1] + np.pi) % (2*np.pi) - np.pi       # wrap bearing to [-pi, pi]
    S = Hk @ P @ Hk.T + R
    K = P @ Hk.T @ np.linalg.inv(S)
    x = x + K @ nu
    P = (np.eye(len(x)) - K @ Hk) @ P
    return x, P

One EKF measurement update for range-bearing tracking. The Jacobian H_jacobian is recomputed at the current estimate x every call, which is the only structural change from the linear update in Chapter 9. The bearing-wrap line is not optional decoration: an innovation of $+359^\circ$ must be read as $-1^\circ$, and forgetting the wrap is the single most common EKF bug in angular measurements.

Notice what the code makes concrete. The innovation $\boldsymbol{\nu}$ uses the true nonlinear $h$, while $\mathbf{S}$ and $\mathbf{K}$ use the Jacobian $\mathbf{H}_k$. And the bearing residual must be wrapped into $[-\pi, \pi]$, because angles live on a circle and naive subtraction produces a spurious near-$2\pi$ innovation that yanks the estimate violently. This wrap-around trap recurs everywhere orientation is estimated, which is why Chapter 24 abandons raw angle subtraction for quaternion error states.

In practice: an automotive radar that lost a cut-in car

An automotive perception team ran an EKF per tracked vehicle, fusing radar range-bearing returns into constant-velocity state estimates for adaptive cruise control. On highway following it tracked flawlessly. The failure showed up when a car cut in sharply from an adjacent lane at close range. At three meters the bearing Jacobian scales as $1/r$, so a tiny lateral motion produced a large bearing change that the linearization, anchored at the previous frame's estimate, badly under-predicted. The innovation blew past what $\mathbf{S}$ said was plausible, the gate rejected the return as an outlier, and the track coasted straight through the cut-in on stale velocity for several frames. The linear algebra was flawless; the Taylor expansion simply was not valid over a maneuver that large at that range. The fix combined a fatter $\mathbf{Q}$ to keep the covariance from collapsing, a re-linearization within the update (the iterated EKF), and ultimately the sigma-point method of the next section for the close-range regime.

When the EKF works, and the three ways it breaks

When it works. The EKF is trustworthy when the nonlinearity is mild over the region the state occupies, meaning the function is nearly straight across roughly one standard deviation of uncertainty. That covers a large fraction of real systems: GNSS positioning (Chapter 25), attitude and heading estimation, and most well-conditioned tracking. Its cost is modest, a couple of Jacobian evaluations per step, so it runs comfortably on a microcontroller, which is why it has flown on spacecraft since Apollo and still dominates embedded sensor fusion.

How it breaks. Three failure modes recur. First, strong curvature: when the function bends sharply across the uncertainty ellipse, the first-order term discards the curvature that actually matters, and the propagated covariance is wrong. Second, overconfidence and divergence: because the linearized $\mathbf{P}$ systematically ignores the extra spread that nonlinearity injects, the covariance shrinks too fast, the gain goes toward zero, the filter stops listening to measurements, and the estimate drifts away from a truth it now considers implausible. This is the same going-deaf pathology from Chapter 9's tuning discussion, but here the mistuning is structural rather than a bad $\mathbf{Q}$. Third, bad Jacobians: a hand-derived analytic Jacobian with a sign error or a wrong partial will not crash; it will quietly bias the filter, which is why numerically checking every Jacobian against a finite-difference reference is standard practice.

The right tool: EKF without the boilerplate

Hand-writing the predict-correct loop, the covariance bookkeeping, the Joseph-form update for numerical stability, and the innovation gating adds up to roughly fifty lines you can get subtly wrong. The filterpy library's ExtendedKalmanFilter reduces the per-step logic to about ten, and you supply only the two things that are genuinely yours: the measurement function and its Jacobian.

from filterpy.kalman import ExtendedKalmanFilter
import numpy as np

ekf = ExtendedKalmanFilter(dim_x=4, dim_z=2)
ekf.F = F_const_velocity(dt)          # linear dynamics
ekf.Q, ekf.R = Q, R                   # noise from your tuning
ekf.x, ekf.P = x0, P0

for z in measurements:
    ekf.predict()
    ekf.update(z, HJacobian=H_jacobian, Hx=h,
               residual=wrap_bearing_residual)   # handles the update, gain, covariance

The same range-bearing EKF using filterpy.kalman.ExtendedKalmanFilter. You pass the Jacobian function and the nonlinear measurement function directly to update; the library owns the gain, the covariance update, and (via the residual hook) the angle wrapping. Fifty lines of careful matrix code become about ten, and you still verify your H_jacobian against finite differences yourself.

Research frontier: learned and invariant extensions

The EKF is sixty years old and still evolving. The invariant EKF (InEKF), now standard in legged-robot and visual-inertial state estimation, exploits the geometric symmetry of the state (rotations live on the Lie group $SO(3)$) so that the linearization error is independent of the estimate, giving provable convergence where the naive EKF diverges. Separately, differentiable Kalman filters embed the EKF equations as a layer inside a neural network, learning the noise models $\mathbf{Q}$ and $\mathbf{R}$, or even the dynamics, end to end from data; the KalmanNet architecture is the widely cited example, and it connects the classical machinery here to the neural state-space models of Chapter 16. Both keep the EKF's recursive, interpretable skeleton while repairing its two chronic weaknesses: linearization error and hand-tuned noise.

Exercise

Implement the range-bearing EKF above for a target that moves in a straight line past the sensor, passing within one meter of the origin at closest approach.

Log the average normalized innovation squared (NIS, from Chapter 9) over the whole run, then again restricted to the twenty steps around closest approach. Explain why the two numbers differ.
Remove the bearing-wrap line and re-run. Describe exactly what the estimate does the first time the true bearing crosses $\pm\pi$.
Replace your analytic H_jacobian with a finite-difference Jacobian and confirm they agree to five decimal places. Then inject a sign error into one partial and report how the tracking degrades without any crash.

Hint

At closest approach $r$ is smallest, so the $1/r$ and $1/r^2$ terms in the bearing Jacobian are largest and the linearization is worst exactly where the geometry changes fastest. That is where a valid-on-average NIS goes bad, and it is the concrete motivation for the sigma-point filter you meet next.

Self-check

The EKF pushes the state mean through $f$ and $h$ but the covariance through their Jacobians. Why this asymmetry, and which of the two is the approximation?
Give a concrete geometric condition under which the EKF's linearization is trustworthy, stated in terms of the function and the uncertainty ellipse.
A colleague reports that their EKF's covariance keeps shrinking while the estimate visibly drifts from ground truth. Name the failure mode and one structural fix that does not touch the code's linear algebra.

What's Next

In Section 10.2, we confront the EKF's core weakness head-on. Instead of differentiating the nonlinear function, the unscented Kalman filter samples it: a small, deterministic set of sigma points is pushed through the true $f$ and $h$, and the transformed points reconstruct the mean and covariance directly. No Jacobians, no discarded curvature, and derivative-free accuracy that holds where the EKF's Taylor expansion falls apart.