M8-T1 — Linearization and the Extended Kalman Filter

Learning objectives

After this lesson, you should be able to:

• Linearise a motion and a measurement model with Jacobians, and state the EKF prediction and correction.
• Derive the EKF from the first-order Taylor expansion and the propagation of a Gaussian through it.
• Compute the Jacobians of a unicycle motion model and of a range-bearing measurement.
• Judge when linearisation is accurate and handle angles correctly in innovations.

The intuition

The left panel shows the problem in its simplest form. A robot is about to move 5 m with a heading that is uncertain by 0.5 rad (29°). Where will it end up? The true distribution of its position, shown by the dots, is a curved banana: the robot ends up on an arc of radius 5 m, and the points at the ends of the arc are closer in x than the points in the middle. The linearised belief is a straight line, the tangent at the mean: it has no curvature, so it puts the mean at x = 5 m while the true mean is 4.42 m, and it spreads y by 2.5 m where the truth spreads 2.2 m.

The Kalman filter is exact for linear models. The EKF applies it to the tangent: at each step it evaluates the Jacobian $F$ of the motion function and $H$ of the measurement function at the current estimate, and uses them where the linear filter used its constant matrices. The mean is propagated through the true nonlinear function, because that is cheap and exact; the covariance is propagated through the Jacobian, because that is the only way to keep it Gaussian.

The right panel shows an EKF at work: a robot circling a landmark at (4, 3), with a range-bearing sensor with noise of 10 cm and 0.05 rad. The estimate follows the true path closely, and the ellipses are small while the robot is near the landmark and grow as it moves away (a bearing error at a distance is a large position error). Over 100 runs the position error rises from 0.29 m at step 5 to 1.04 m at step 40, and the mean NEES, expected to be 3, rises from 2.95 to 5.4 at step 40: the EKF becomes overconfident as the linearisation errors accumulate, a typical behaviour.

The EKF is the Kalman filter applied to a straight-line approximation re-made at every step. It is accurate when the uncertainty is small relative to the curvature of the models, and overconfident when it is not.

Definitions

Motion function $g$

$\mathbf{x}_t = g(\mathbf{x}_{t-1}, \mathbf{u}_t) + \mathbf{w}_t$, a nonlinear function plus Gaussian noise $\mathbf{w}_t\sim\mathcal{N}(0,Q)$.

Measurement function $h$

$\mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v}_t$, with $\mathbf{v}_t\sim\mathcal{N}(0,R)$.

Jacobians

$G_t = \left.\dfrac{\partial g}{\partial\mathbf{x}}\right|_{\mathbf{x}_{t-1},\mathbf{u}_t}$ and $H_t = \left.\dfrac{\partial h}{\partial\mathbf{x}}\right|_{\bar{\mathbf{x}}_t}$: matrices of partial derivatives, evaluated at the current estimate.

Angle residual

An innovation or error component that is an angle must be wrapped to $[-\pi,\pi)$ after subtraction.

Derivation: the first-order Taylor expansion

The idea is to expand each function to first order around the current estimate, where the Kalman filter's linear assumptions hold, and apply the linear results.

1. $$g(\mathbf{x}) \approx g(\boldsymbol\mu) + G\,(\mathbf{x} - \boldsymbol\mu), \qquad G = \left.\frac{\partial g}{\partial\mathbf{x}}\right|_{\boldsymbol\mu}$$

   The first-order Taylor expansion around the mean of the belief. The error is quadratic in $\mathbf{x}-\boldsymbol\mu$.

2. $$\mathbb{E}[g(\mathbf{x})]\approx g(\boldsymbol\mu),\qquad \mathrm{Cov}(g(\mathbf{x}))\approx G\Sigma G^\top$$

   A linear function of a Gaussian: the mean is the function of the mean and the covariance is $G\Sigma G^\top$ (propagation lesson of module 4). Adding the independent noise gives $+Q$.

3. $$h(\mathbf{x}) \approx h(\bar{\boldsymbol\mu}) + H\,(\mathbf{x}-\bar{\boldsymbol\mu})$$

   The same for the measurement function, around the predicted mean, which is the best estimate available before seeing the reading.

4. $$\mathbf{z} \approx h(\bar{\boldsymbol\mu}) + H(\mathbf{x}-\bar{\boldsymbol\mu}) + \mathbf{v}$$

   The measurement model is now linear in x, with a constant offset $h(\bar{\boldsymbol\mu}) - H\bar{\boldsymbol\mu}$.

5. $$\boldsymbol\mu = \bar{\boldsymbol\mu} + K\bigl(\mathbf{z} - h(\bar{\boldsymbol\mu})\bigr)$$

   Apply the linear Kalman correction (conditioning of a joint Gaussian). The constant offset makes the predicted reading $h(\bar{\boldsymbol\mu})$, so the innovation uses the nonlinear function, not the linearised one.

The Jacobian of the unicycle

For $x\leftarrow x + v\Delta t\cos\theta$ the derivatives are $\partial/\partial x = 1$, $\partial/\partial y = 0$, $\partial/\partial\theta = -v\Delta t\sin\theta$, which gives the first row of $G$; the second row follows the same way. The heading enters the position through $\sin\theta$ and $\cos\theta$: a heading error becomes a position error proportional to the distance travelled, which is why the position covariance after a turn is stretched perpendicular to the direction of travel.

The Jacobian of the range-bearing function

With $\rho = \sqrt{\Delta x^2+\Delta y^2}$ and $\phi = \operatorname{atan2}(\Delta y,\Delta x) - \theta$, using $\partial\rho/\partial x = -\Delta x/\rho$ and $\partial\operatorname{atan2}(\Delta y,\Delta x)/\partial x = \Delta y/\rho^2$, one gets the matrix $H$ above. Note the last column: a bearing changes one for one with the heading, while the range does not depend on it at all.

When it works

The error of the first-order approximation is governed by the second derivative times the spread. For $x = d\cos\theta$ with $\theta\sim\mathcal{N}(0,s^2)$ the true mean is $d\,e^{-s^2/2}$, against the linearised $d$: a relative error of $s^2/2$. For $s = 0.2$ this is 2%; for $s = 1$ it is 39% (the table below).

The EKF is as good as the first-order approximation is accurate over the width of the belief. Keep the uncertainty small (good sensors, frequent updates) or move to a method that does not linearise (the unscented filter or particles).

A worked example

Two experiments. First, the accuracy of the linearisation: push a heading $\mathcal{N}(0,s^2)$ through $(5\cos\theta, 5\sin\theta)$, with 20 000 samples. Second, an EKF for the unicycle of the figure ($v = 1$ m/s, $\omega = 0.2$ rad/s, $\Delta t = 0.5$ s, a landmark at (4, 3)), 40 steps from a poor initial estimate.

1. Linearisation. For a heading standard deviation of 0.05 rad, the linearised mean x = 5.0 and the true mean 4.99 are the same, and the linearised y spread 0.25 matches the true 0.249. At 0.5 rad, the true mean is 4.42 (linearised 5.0) and the spread 2.21 against 2.5. At 1.0 rad the true mean is 3.05 against 5.0, and the spread 3.3 against 5.0: the linearisation is far off.
2. The filter. It starts 0.42 m off with a standard deviation of 0.5 m. After the first step the estimate is (0.93, −0.21) against the truth (0.51, 0.00) with a standard deviation of 0.32 m: the readings have not yet pinned it down. By step 10 the standard deviations are 9 cm and 6 cm, while the robot is near the landmark.
3. Far from the landmark. At step 40 the robot is at (−4.0, 7.0), 8.9 m away, and the standard deviations have grown to 0.40 and 0.88 m. The estimate is 0.2 m from the truth in x.
4. Consistency. Over 100 runs the root-mean-square position error is 0.29, 0.43 and 1.04 m at steps 5, 20 and 40. The mean NEES is 2.95, 3.29 and 5.4 (expected 3): consistent at first, overconfident later.

import math
from robotprob import Rng, ekf_predict, ekf_update, numerical_jacobian, mean, nees

# 1. how good is the linearisation? push N(heading 0, std s) through x = d*cos(theta), y = d*sin(theta)
d = 5.0
f = lambda th: [d * math.cos(th[0]), d * math.sin(th[0])]
print('heading std   linearised mean (x, y)    true mean (x, y)    linearised std y   true std y')
for s in (0.05, 0.2, 0.5, 1.0):
    J = numerical_jacobian(f, [0.0])
    rng = Rng(1)
    pts = [f([rng.gauss(0.0, s)]) for _ in range(20000)]
    mx, my = mean([p[0] for p in pts]), mean([p[1] for p in pts])
    sy = math.sqrt(mean([(p[1] - my) ** 2 for p in pts]))
    print(str(s).rjust(11), '   ', (round(f([0.0])[0], 2), round(f([0.0])[1], 2)), '            ', (round(mx, 2), round(my, 2)), '        ', round(abs(J[1][0]) * s, 3), '           ', round(sy, 3))

# 2. an EKF for a unicycle with range-bearing readings of one known landmark
dt, v, w = 0.5, 1.0, 0.2
sv, sw = 0.05, 0.03
sr, sb = 0.1, 0.05
lm = (4.0, 3.0)
wrap = lambda a: math.atan2(math.sin(a), math.cos(a))
def f(x, u):
    return [x[0] + u[0] * dt * math.cos(x[2]), x[1] + u[0] * dt * math.sin(x[2]), wrap(x[2] + u[1] * dt)]
def F(x, u):                                     # Jacobian of the motion function
    return [[1, 0, -u[0] * dt * math.sin(x[2])], [0, 1, u[0] * dt * math.cos(x[2])], [0, 0, 1]]
def h(x):                                        # expected range and bearing of the landmark
    dx, dy = lm[0] - x[0], lm[1] - x[1]
    return [math.hypot(dx, dy), wrap(math.atan2(dy, dx) - x[2])]
def H(x):                                        # Jacobian of the measurement function
    dx, dy = lm[0] - x[0], lm[1] - x[1]; q = dx * dx + dy * dy; r = math.sqrt(q)
    return [[-dx / r, -dy / r, 0.0], [dy / q, -dx / q, -1.0]]
Q = [[(sv * dt) ** 2, 0, 0], [0, (sv * dt) ** 2, 0], [0, 0, (sw * dt) ** 2]]
R = [[sr ** 2, 0], [0, sb ** 2]]
res = lambda z, zp: [z[0] - zp[0], wrap(z[1] - zp[1])]   # the bearing residual is wrapped
def run(seed, steps=40):
    rng = Rng(seed)
    x_true = [0.0, 0.0, 0.0]
    x, P = [0.3, -0.3, 0.2], [[0.25, 0, 0], [0, 0.25, 0], [0, 0, 0.1]]
    out = []
    for t in range(1, steps + 1):
        vn, wn = v + rng.gauss(0, sv), w + rng.gauss(0, sw)
        x_true = f(x_true, (vn, wn))
        zt = h(x_true)
        z = [zt[0] + rng.gauss(0, sr), wrap(zt[1] + rng.gauss(0, sb))]
        x, P = ekf_predict(x, P, f, F, Q, (v, w))
        x, P, y, S, K = ekf_update(x, P, z, h, H, R, res)
        e = [x[0] - x_true[0], x[1] - x_true[1], wrap(x[2] - x_true[2])]
        out.append((t, x_true, x, P, e))
    return out
o = run(4)
print('step  true (x, y, th)          estimate (x, y, th)      std (x, y, th)')
for t, xt, x, P, e in o:
    if t in (1, 5, 10, 20, 40):
        print(str(t).rjust(4), [round(a, 2) for a in xt], [round(a, 2) for a in x], [round(math.sqrt(P[i][i]), 3) for i in range(3)])
E = []; N = []
for seed in range(1, 101):
    r_ = run(seed)
    E.append([math.hypot(e[0], e[1]) for *_, e in r_])
    N.append([nees(e, P) for _, _, _, P, e in r_])
print('100 runs: rms position error at step 5, 20, 40:', [round(math.sqrt(mean([a[i] ** 2 for a in E])), 3) for i in (4, 19, 39)])
print('mean NEES at step 5, 20, 40 (3 expected):', [round(mean([a[i] for a in N]), 2) for i in (4, 19, 39)])

heading std   linearised mean (x, y)    true mean (x, y)    linearised std y   true std y
       0.05     (5.0, 0.0)              (4.99, -0.0)          0.25             0.249
        0.2     (5.0, 0.0)              (4.9, -0.01)          1.0             0.977
        0.5     (5.0, 0.0)              (4.42, -0.02)          2.5             2.208
        1.0     (5.0, 0.0)              (3.05, -0.02)          5.0             3.275
step  true (x, y, th)          estimate (x, y, th)      std (x, y, th)
   1 [0.51, 0.0, 0.1] [0.93, -0.21, 0.19] [0.318, 0.317, 0.105]
   5 [2.41, 0.51, 0.52] [2.72, 0.35, 0.6] [0.254, 0.133, 0.098]
  10 [4.18, 2.08, 1.04] [4.28, 2.15, 1.16] [0.086, 0.06, 0.099]
  20 [4.31, 6.87, 2.16] [4.11, 6.91, 2.19] [0.409, 0.048, 0.106]
  40 [-4.02, 6.96, -2.01] [-4.22, 6.73, -2.03] [0.399, 0.875, 0.109]
100 runs: rms position error at step 5, 20, 40: [0.291, 0.429, 1.035]
mean NEES at step 5, 20, 40 (3 expected): [2.95, 3.29, 5.4]

In robotics

The workhorse of robot localisation and navigation

Most deployed vehicle state estimators are EKFs: GPS/IMU fusion in drones and cars, wheel-odometry with a laser, visual-inertial odometry. The UAV course derives an EKF for attitude and position in its estimation lessons, and the Navigation course uses the same Gaussian machinery in localisation.

Jacobians: by hand, by tool or by differences

Jacobians are the main source of EKF bugs. Derive them by hand for small models, and always check them against a finite-difference Jacobian (numerical_jacobian) in a unit test: a wrong sign in H makes the filter diverge in a way that looks like a tuning problem. Automatic differentiation tools remove the work for large models.

Angles

The bearing residual must be wrapped, as must the heading in the state. Without wrapping, a bearing of 3.1 rad against a prediction of −3.1 rad looks like a 6.2 rad error instead of 0.08 rad, and the filter throws the estimate far away. Module 12 treats angles properly; here the rule is: wrap the angle components of every residual and of the state.

Practical safeguards

• Initialise with a reasonably accurate estimate. Linearising around a poor one can lock the filter into a wrong solution.
• Inflate Q a little to absorb the linearisation error (the over-confidence in the example).
• Gate readings with the Mahalanobis distance (module 4); an outlier is amplified by a wrong linearisation point.
• Use the Joseph form for P.

Iterated and other variants

The iterated EKF re-linearises the measurement at the updated estimate and repeats the correction, which helps with strong nonlinearity in h. The unscented filter, two lessons from here, avoids Jacobians altogether.

Common misconceptions

Key takeaways

1. The EKF linearises g and h at the current estimate: the mean uses the nonlinear functions, the covariance and the gain use the Jacobians G and H.
2. The Jacobian of the unicycle turns heading uncertainty into position uncertainty proportional to the distance travelled; that of range-bearing links the bearing one for one to the heading.
3. The approximation error is second order in the width of the belief: about s²/2 for a heading of spread s.
4. Wrap angle residuals and check Jacobians numerically.
5. The EKF becomes overconfident as linearisation error accumulates. Inflate Q, gate outliers, initialise well, or use a method that does not linearise.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 3.3 (the extended Kalman filter). Paid book.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , chapter 4 (nonlinear non-Gaussian estimation) covers the EKF and its limits. Free to read as the author’s PDF; the book is copyrighted.
• C. Stachniss, “Kalman Filter & EKF” (video lecture) , covers the EKF in its second half. Free to watch.
• R. Labbe, Kalman and Bayesian Filters in Python (online book) , the chapter on the extended Kalman filter. Free to read; licence not checked.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

The EKF has the straightforward version of the idea. The next lesson uses it for the standard problem of mobile robotics: localising from several landmarks with unknown data association, and shows how gating handles the associations.

Related content

Nothing below is required to finish this lesson.

• Lab
  M8-L1: EKF versus UKF

  The EKF reports a tight covariance while Rover is a metre away. Push the same uncertain pose through a range-bearing conversion with a Jacobian and with sigma points, and compare both with the true spread.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M3-T5: Extended Kalman Filter
• SLAM M3-T7: EKF-SLAM
• SLAM M3-L5: Implement a Simplified EKF-SLAM