Unscented Kalman filter and sigma points

"The extended filter asked the function for its slope and believed the answer. I prefer to send a small delegation through the function and see who comes back."

A Prudent AI Agent

This section assumes the linear Kalman recursion and the meaning of the state mean and covariance from Chapter 9, the linearization idea and Jacobian notation from Section 10.1, and comfort with Gaussians, covariance, and matrix square roots from Chapter 4. Like the EKF, the UKF still assumes the belief is a single Gaussian; when that assumption itself breaks, you reach for the particle filter of Section 10.3.

The idea: approximate the distribution, not the function

Here is the intuition that names the method. It is easier to approximate a probability distribution than an arbitrary nonlinear function. If you want to know what a Gaussian looks like after passing through some curved map \(g\), you do not need to know \(g\) everywhere; you need to know it at a handful of representative places. The unscented transform chooses those places deterministically so that they carry the mean and covariance of the input exactly, propagates them through the real \(g\), and reads the output mean and covariance back off the transformed set. The claim, which the derivation makes precise, is that this captures the true posterior mean and covariance to second order in the Taylor expansion for any nonlinearity, and to third order when the input is Gaussian and the weights are tuned. The EKF, by contrast, is accurate only to first order. You get better statistics for the same Gaussian-in, Gaussian-out contract, and you never touch a derivative.

Choosing the sigma points

Let the state have dimension \(n\), with mean \(\hat{x}\) and covariance \(P\). The unscented transform draws \(2n+1\) sigma points: one at the mean, and a symmetric pair along each of the \(n\) principal axes of the covariance. The spread along axis \(i\) is set by a column of the matrix square root \(\sqrt{(n+\lambda)P}\), usually the lower-triangular Cholesky factor, so the cloud has exactly the covariance \(P\) scaled by \(n+\lambda\):

for \(i = 1, \dots, n\). Each point carries two weights, one for reconstructing the mean (\(W_i^{m}\)) and one for the covariance (\(W_i^{c}\)):

The composite scaling parameter is \(\lambda = \alpha^2(n+\kappa) - n\). The three knobs each have a job. \(\alpha\) (small, e.g. \(10^{-3}\)) controls how tightly the sigma points hug the mean, keeping them inside the region where the function is well-behaved and suppressing higher-order sampling error. \(\beta\) folds in prior knowledge of the distribution's shape; \(\beta = 2\) is optimal for a Gaussian and shows up only in the covariance weight of the center point. \(\kappa\) is a secondary spread control, usually \(0\) or \(3-n\). These are the Merwe scaled-sigma-point defaults, and for most sensor work you set \(\alpha, \beta, \kappa\) once and forget them.

The unscented transform in code

The whole method is short enough to write once and trust. The function below generates the sigma points and weights, and a second function pushes them through any map and refits a Gaussian. We test it on the classic pain point for linearization: converting a noisy range-and-bearing reading (polar) into a Cartesian position, a map curved enough that evaluating it only at the mean is measurably biased.

import numpy as np
rng = np.random.default_rng(0)

def sigma_points(mean, cov, alpha=1e-3, beta=2.0, kappa=0.0):
    n = mean.size
    lam = alpha**2 * (n + kappa) - n
    S = np.linalg.cholesky((n + lam) * cov)      # scaled matrix square root
    pts = np.vstack([mean, mean + S.T, mean - S.T])   # 2n+1 sigma points
    wm = np.full(2 * n + 1, 1.0 / (2 * (n + lam)))
    wc = wm.copy()
    wm[0] = lam / (n + lam)
    wc[0] = wm[0] + (1 - alpha**2 + beta)
    return pts, wm, wc

def unscented_transform(pts, wm, wc, f):
    y = np.array([f(p) for p in pts])            # push each point through real f
    mean = wm @ y
    d = y - mean
    cov = (wc[:, None] * d).T @ d
    return mean, cov

def polar_to_xy(s):                              # nonlinear sensor->world map
    r, th = s
    return np.array([r * np.cos(th), r * np.sin(th)])

mean = np.array([30.0, 0.9])                     # range 30 m, bearing 0.9 rad
cov  = np.diag([1.0**2, 0.35**2])                # range / bearing noise
pts, wm, wc = sigma_points(mean, cov)
ut_mean, _ = unscented_transform(pts, wm, wc, polar_to_xy)

samples = rng.multivariate_normal(mean, cov, 400_000)
mc_mean = np.array([polar_to_xy(s) for s in samples]).mean(0)   # truth
lin_mean = polar_to_xy(mean)                     # EKF-style: f(mean) only

print("Monte-Carlo mean :", mc_mean.round(3))
print("unscented  mean :", ut_mean.round(3))
print("linearized mean :", lin_mean.round(3))

Run it and the unscented mean tracks the 400,000-sample Monte-Carlo truth to a few centimeters, while the linearized f(mean) is off by a visible margin. That gap is exactly the second-order term the EKF drops and the UKF keeps, purchased with nine function evaluations here and zero calculus.

Wiring the transform into the filter

The full UKF is just the recursion of Chapter 9 with every "propagate the Gaussian" step replaced by an unscented transform. Predict: draw sigma points from \((\hat{x}_{t-1}, P_{t-1})\), push them through the motion model \(f\), and refit to get the predicted mean and covariance, then add the process noise \(Q\). Correct: draw fresh sigma points from the predicted Gaussian, push them through the measurement model \(h\) to get predicted measurements, and compute three things from the propagated sets: the innovation covariance \(S = P_{zz} + R\), the state-measurement cross-covariance \(P_{xz}\), and from them the Kalman gain \(K = P_{xz} S^{-1}\). The mean and covariance update, \(\hat{x} = \hat{x}^- + K(z - \hat{z})\) and \(P = P^- - K S K^{\top}\), are algebraically identical to the linear filter. The only structural change from the EKF is that Jacobians \(F\) and \(H\) are gone; the cross-covariance \(P_{xz}\), which the EKF forms as \(P^- H^{\top}\), now falls straight out of the sigma-point spread.

When to choose it, and what it still cannot do

Reach for the UKF when the models are meaningfully nonlinear over the width of your uncertainty, when analytic Jacobians are painful, error-prone, or expensive to derive (attitude and orientation estimation in Chapter 24 is a canonical case, where quaternion kinematics make Jacobians treacherous), or when you simply want a derivative-free drop-in that is more robust than the EKF at essentially the same cost. The compute is \(O(n^3)\) per step from the Cholesky factorization, the same order as the EKF, with a small constant factor from the \(2n+1\) function evaluations; for the low-dimensional states typical of sensor tracking this difference is negligible. The hard limit is inherited, not introduced: the UKF still represents the belief as one Gaussian. If the true posterior is multi-modal, if you are tracking through a data-association ambiguity or a bimodal likelihood, no choice of sigma points saves you, because the output is refit to a single mean and covariance by construction. That wall is where Section 10.3 begins.

What's Next

In Section 10.3, we cross the wall the UKF cannot climb. When the belief is multi-modal or the noise is anything but Gaussian, we stop refitting a single Gaussian and instead represent the posterior with a cloud of many weighted samples, the particles, propagated by the same predict-correct logic but weighted by importance sampling. The sigma points you just met were a deterministic handful chosen to match two moments; the particle filter trades that elegance for a stochastic swarm that can chase any shape the posterior takes.