The unscented Kalman filter and comparing Gaussian filters

The extended Kalman filter approximates a nonlinear function by its tangent, which requires derivatives and ignores curvature. The unscented Kalman filter, UKF, introduced by Simon Julier and Jeffrey Uhlmann in the 1990s, takes a different view: it is easier to approximate a probability distribution than an arbitrary nonlinear function. Instead of linearizing, the UKF picks a small, deterministic set of sample points, called sigma points, that capture the mean and covariance of the belief, passes each one through the true nonlinear function, and recomputes a mean and covariance from the transformed points. No Jacobians are needed, and the result captures second-order effects that the EKF misses. This lesson explains the unscented transform, builds the UKF from it, and then compares the EKF, UKF and the particle filters of the next lesson so that you can choose sensibly among them.

The unscented transform

For an n-dimensional Gaussian with mean μ and covariance Σ, the standard unscented transform uses 2n + 1 sigma points: the mean itself, and pairs placed symmetrically on either side along the columns of a matrix square root of Σ:

The matrix square root is usually the Cholesky factor, and \( (\cdot)_i \) denotes its i-th column. The scaling parameter is \( \lambda = \alpha^2(n + \kappa) - n \), where α (typically between 0.001 and 1) controls how far the points spread from the mean and κ (often 0) is a secondary tuning parameter. Each point carries a weight for computing the mean and one for the covariance:

with β = 2 optimal for Gaussian beliefs. Each sigma point is passed through the nonlinear function, \( \mathcal{Y}_i = g(\mathcal{X}_i) \), and the output mean and covariance are the weighted sample statistics,

These sigma points are not random samples; they are chosen so that their weighted mean and covariance equal μ and Σ exactly. For any nonlinear function, the transformed mean is correct to second order in the Taylor expansion (third order for Gaussian inputs), where the EKF is correct only to first order. The cost is 2n + 1 function evaluations, about the same work as computing a Jacobian numerically.

The UKF algorithm

The UKF uses the unscented transform for both steps of the filter. In prediction, it generates sigma points from \( (\boldsymbol{\mu}_{t-1}, \boldsymbol{\Sigma}_{t-1}) \), passes each through the motion model, and computes \( \bar{\boldsymbol{\mu}}_t \) and \( \bar{\boldsymbol{\Sigma}}_t \) from the results, adding the process noise Q to the covariance. In the update, it regenerates sigma points from the prediction (or reuses the propagated ones), passes them through the measurement model to get predicted measurements \( \mathcal{Z}_i \), and computes the predicted measurement mean \( \hat{\mathbf{z}} \), the innovation covariance S and the cross-covariance between state and measurement:

The gain is \( \mathbf{K} = \mathbf{C}\mathbf{S}^{-1} \), and the update is \( \boldsymbol{\mu} = \bar{\boldsymbol{\mu}} + \mathbf{K}(\mathbf{z} - \hat{\mathbf{z}}) \), \( \boldsymbol{\Sigma} = \bar{\boldsymbol{\Sigma}} - \mathbf{K}\mathbf{S}\mathbf{K}^{\mathsf{T}} \). For a linear system the UKF reproduces the Kalman filter exactly, since the cross-covariance C equals \( \bar{\boldsymbol{\Sigma}}\mathbf{H}^{\mathsf{T}} \). When the noise is not additive, as with noise on odometry controls, the state can be augmented with the noise variables so that sigma points also sample the noise, at the cost of a larger n.

Angles need care. Averaging sigma points with headings of 179° and −179° naively gives 0°, the opposite direction. UKF implementations for robot poses must compute angular means with circular averaging, \( \operatorname{atan2}(\sum w_i\sin\theta_i, \sum w_i\cos\theta_i) \), and wrap every angular difference in the covariance sums. Forgetting this is the most common UKF bug in mobile robotics.

import numpy as np
rng = np.random.default_rng(2)
# a range-bearing sensor reports r and phi; convert to x, y (a classic nonlinear test)
mu = np.array([1.0, np.radians(90)]); Sig = np.diag([0.02**2, np.radians(15)**2])
f = lambda p: np.array([p[0]*np.cos(p[1]), p[0]*np.sin(p[1])])
# Monte Carlo truth
P = rng.multivariate_normal(mu, Sig, 200000); Y = np.c_[P[:, 0]*np.cos(P[:, 1]), P[:, 0]*np.sin(P[:, 1])]
# linearization
r, ph = mu; J = np.array([[np.cos(ph), -r*np.sin(ph)], [np.sin(ph), r*np.cos(ph)]])
lin_mu, lin_S = f(mu), J @ Sig @ J.T
# unscented transform
n, alpha, beta, kappa = 2, 1.0, 2.0, 1.0
lam = alpha**2*(n + kappa) - n; L = np.linalg.cholesky((n + lam)*Sig)
X = [mu] + [mu + L[:, i] for i in range(n)] + [mu - L[:, i] for i in range(n)]
wm = np.r_[lam/(n + lam), np.full(2*n, 1/(2*(n + lam)))]; wc = wm.copy(); wc[0] += 1 - alpha**2 + beta
Yu = np.array([f(x) for x in X]); ut_mu = wm @ Yu
ut_S = sum(w*np.outer(y - ut_mu, y - ut_mu) for w, y in zip(wc, Yu))
for name, m, S in [("Monte Carlo", Y.mean(0), np.cov(Y.T)), ("linearized", lin_mu, lin_S), ("unscented", ut_mu, ut_S)]:
    print(f"{name:12s} mean y = {m[1]:.4f}   var x = {S[0, 0]:.4f}   var y = {S[1, 1]:.5f}")

Monte Carlo  mean y = 0.9664   var x = 0.0641   var y = 0.00259
linearized   mean y = 1.0000   var x = 0.0685   var y = 0.00040
unscented    mean y = 0.9663   var x = 0.0640   var y = 0.00494

With 15° of bearing uncertainty, the true mean y is noticeably less than 1 m, because points spread along an arc pull the average inward. The Monte Carlo mean is 0.966 m. Linearization puts the mean at exactly 1 m and reports a variance in y of 0.0004, more than six times too small. The unscented transform matches the Monte Carlo mean to three decimal places using just five function evaluations; its variance in y is about twice the true value, an error on the cautious side, which a filter tolerates far better than the linearized overconfidence. Both get the variance in x, the direction across the bearing, about right. This is the same effect as the banana of lesson 3, and it explains why UKFs often stay consistent in situations where EKFs become overconfident.

Tuning the UKF's parameters matters less than tuning its noise models, but a few rules help. With α near 1 and κ = 3 − n, the sigma points sit about \( \sqrt{3} \) standard deviations from the mean, which suits most robotics problems. Very small α keeps the points close to the mean, which avoids evaluating the model in regions where it is invalid but makes the center weight large and negative, and can produce a covariance that is not positive definite; square-root implementations of the UKF, which propagate a Cholesky factor directly, avoid this numerical problem.

Choosing among Gaussian and sample-based filters

Three families of filters cover almost all robot state estimation. The table compares them on the properties that usually decide the choice.

A practical guide follows from the table. If the system is close to linear over the uncertainty, or Jacobians are available and the uncertainty stays small, the EKF is efficient, well understood and supported by mature libraries. If nonlinearity is significant, Jacobians are tedious or error-prone, or the EKF proves inconsistent, the UKF usually does better at modest extra cost. If the belief can be multimodal, the robot must localize from an unknown start, or measurements include frequent ambiguity, a particle filter is needed, provided the state is low-dimensional enough, which in practice means up to about three to six dimensions. Many real systems combine them: a particle filter for global 2-D localization feeding an EKF or UKF that fuses inertial and wheel data at high rate.