M8-T3 — The Unscented Transform and the UKF

Learning objectives

After this lesson, you should be able to:

• Construct the sigma points and weights of the unscented transform and state the moments they match.
• Compare the mean and covariance of a nonlinear transform from the Jacobian, the unscented transform and Monte Carlo.
• Write the UKF prediction and correction using the transform.
• State what the UKF improves, what it does not, and the cost of handling angles.

The intuition

A Gaussian in two dimensions is a cloud. Pushing the whole cloud through a function is expensive and the result is no longer Gaussian. The EKF replaces the function by a straight line, so the cloud stays Gaussian but the function is wrong. The unscented transform keeps the true function and replaces the cloud by five points: the mean and four points one scaled standard deviation away along each principal axis. Their sample mean and covariance are exactly those of the Gaussian. They go through the real function; the mean and covariance of the five outputs are the answer.

The left panel is the standard example. A reading of 10 m with a bearing uncertainty of 0.5 rad is converted to Cartesian coordinates. The result is a banana wrapped around the origin. The Jacobian puts the mean at x = 10 and a variance of 0.25 in x; but 50 000 samples give a mean of 8.83 (the curvature pulls it in) and a variance of 2.64. The unscented transform with five points gets a mean of 8.826 and a variance of 3.0, much closer, and its y variance, 19.3, is nearer the sample 19.7 than the Jacobian's 25.

The right panel is a reminder not to oversell it. For a single correction with a wide prior, both filters are overconfident (NEES far above 2): the true posterior is a banana, and no Gaussian describes it. The UKF is less overconfident (NEES 14.8 against 23.3 at a prior of ±1 m; 66 against 117 at ±1.5 m), but its error is not smaller, and at ±1.5 m it is larger (1.58 m against 1.39 m).

The unscented transform is a better Gaussian approximation than the Jacobian, and needs no derivatives. It is not a way to represent non-Gaussian beliefs. When the belief is a banana, use particles.

Definitions

Sigma points $\mathcal{X}_i$

$2n+1$ points that have the mean and covariance of the Gaussian: the mean, and the mean ± the columns of a matrix square root of $(n+\lambda)\Sigma$.

Scaling $\lambda = \alpha^2(n+\kappa) - n$

Sets how far from the mean the points lie. The original choice $\alpha = 1, \kappa = 3 - n$ is optimal for Gaussians; $\alpha < 1$ with $\beta = 2$ keeps the points closer.

Weights $w^{(m)}_i, w^{(c)}_i$

For the mean and covariance. $w_0^{(m)} = \lambda/(n+\lambda)$ and $w_i = 1/(2(n+\lambda))$ for the others.

Unscented transform

Map the points through $g$: $\mathcal{Y}_i = g(\mathcal{X}_i)$, and compute their weighted mean and covariance.

Derivation: why the sigma points are right

The claim is that the weighted points reproduce the mean and covariance of the Gaussian exactly, and that the transformed points capture the mean and covariance of a nonlinear function to second order, one order better than the Jacobian.

1. $$\sum_i w_i^{(m)}\mathcal{X}_i = \boldsymbol\mu$$

   The points come in symmetric pairs $\boldsymbol\mu\pm\mathbf{c}_i$ with equal weights, so the offsets cancel, and the weights sum to 1.

2. $$\sum_i w_i^{(c)}(\mathcal{X}_i - \boldsymbol\mu)(\mathcal{X}_i - \boldsymbol\mu)^\top = 2\cdot\frac{1}{2(n+\lambda)}\sum_i \mathbf{c}_i\mathbf{c}_i^\top = \frac{1}{n+\lambda}(n+\lambda)\Sigma = \Sigma$$

   Each pair contributes twice the outer product of the column. With $L = \sqrt{(n+\lambda)\Sigma}$ the sum of outer products of the columns is $LL^\top = (n+\lambda)\Sigma$.

3. $$g(\boldsymbol\mu + \boldsymbol\delta) = g + G\boldsymbol\delta + \tfrac12\,\boldsymbol\delta^\top\nabla^2g\,\boldsymbol\delta + \cdots$$

   The Taylor series of the function around the mean. The odd terms cancel in the symmetric pairs.

4. $$\mathbb{E}[g] \approx g(\boldsymbol\mu) + \tfrac12\operatorname{tr}\bigl(\nabla^2 g;\Sigma\bigr)$$

   The true mean of a function of a Gaussian, to second order. The Jacobian method keeps only the first term; the sigma points, whose second moments match $\Sigma$, also reproduce the second term (the weighted average of the pairs gives exactly this curvature term).

The numbers in the radar example

For $n=2$ with $\kappa = 1$ (so $\lambda=1$) the points are the mean and $\mu\pm\sqrt{3}\,\sigma$ along each axis: (10, 0), (10.87, 0), (10, 0.87), (9.13, 0), (10, −0.87), with weights 1/3 for the centre and 1/6 for each of the others. The points at a bearing of ±0.87 rad map to (6.4, ±7.6), far from the Jacobian's tangent, and they pull the mean of x towards 8.83.

Cost and angles

The UKF runs the true function $2n+1$ times per step, which is cheap for small n and replaces the derivation and testing of Jacobians. The price is care with angles: averaging angles by summation is wrong (the mean of 179° and −179° is 180°, not 0°). The weighted mean of an angular component must be computed from sines and cosines, and the residuals wrapped, as the program of this lesson does for the state and the bearing.

The unscented transform is accurate to second order in the mean and to the first in the covariance of an arbitrary function, with no derivatives. Its limits: the result is still summarised by a single Gaussian.

A worked example

Two experiments with the library.

1. The radar conversion. A range of 10 m with 0.5 m of noise and a bearing of 0 with 0.5 rad of noise, converted to (x, y). 50 000 samples give mean x = 8.827 and variances (2.64, 19.7). The Jacobian gives 10.0 and (0.25, 25.0). The five sigma points give 8.826 and (3.01, 19.3).
2. One correction, three priors. A position with a prior of standard deviation s around (3, 0) is corrected by a range-bearing reading from a sensor at the origin (noises 0.1 m and 0.05 rad), 2 000 trials each. At s = 0.5 both are nearly consistent (NEES 2.92 for the EKF, 2.43 for the UKF; 2 is expected) and the errors are equal (0.21 m). At s = 1.0 the NEES is 23.3 and 14.8 and the rms error 0.63 and 0.66 m. At s = 1.5 the NEES is 117 and 66 and the error 1.39 and 1.58 m.
3. How to read this. The UKF's covariance is more honest than the EKF's in every case, but a Gaussian posterior cannot describe a banana, so both are overconfident, and a more honest covariance does not translate into a smaller error. For a posterior that is close to Gaussian (small s, or a more informative sensor) they are the same.

import math
from robotprob import (Rng, sigma_points, unscented_transform, numerical_jacobian, propagate_nonlinear, ekf_update, ukf_update,
                       mean, nees)

# 1. a polar-to-Cartesian conversion: a radar-like reading r = 10 m, bearing 0, with a large bearing error
polar = lambda p: [p[0] * math.cos(p[1]), p[0] * math.sin(p[1])]
mu, P = [10.0, 0.0], [[0.25, 0.0], [0.0, 0.5 ** 2]]
rng = Rng(7)
S = [polar([mu[0] + rng.gauss(0, 0.5), mu[1] + rng.gauss(0, 0.5)]) for _ in range(50000)]
mc_m = [mean([s[0] for s in S]), mean([s[1] for s in S])]
mc_v = [mean([(s[0] - mc_m[0]) ** 2 for s in S]), mean([(s[1] - mc_m[1]) ** 2 for s in S])]
lin_m, lin_P = propagate_nonlinear(polar, mu, P)
pts, wm, wc = sigma_points(mu, P, alpha=1.0, beta=0.0, kappa=1.0)
ut_m, ut_P = unscented_transform([polar(p) for p in pts], wm, wc)
print('sigma points (r, bearing):', [[round(a, 2) for a in p] for p in pts])
print('weights:', [round(w, 3) for w in wm])
print('method          mean x   mean y   var x   var y')
print('monte carlo    ', str(round(mc_m[0], 3)).rjust(7), str(round(mc_m[1], 3)).rjust(8), str(round(mc_v[0], 3)).rjust(7), str(round(mc_v[1], 3)).rjust(7))
print('jacobian (EKF) ', str(round(lin_m[0], 3)).rjust(7), str(round(lin_m[1], 3)).rjust(8), str(round(lin_P[0][0], 3)).rjust(7), str(round(lin_P[1][1], 3)).rjust(7))
print('unscented      ', str(round(ut_m[0], 3)).rjust(7), str(round(ut_m[1], 3)).rjust(8), str(round(ut_P[0][0], 3)).rjust(7), str(round(ut_P[1][1], 3)).rjust(7))

# 2. one correction of a position (x, y) known to N((3, 0), 1.5^2 I) by a range-bearing reading from a sensor at the origin
from robotprob import sample_mvn
sr, sb = 0.1, 0.05
hh = lambda p: [math.hypot(p[0], p[1]), math.atan2(p[1], p[0])]
def HH(p):
    q = p[0] ** 2 + p[1] ** 2; r = math.sqrt(q)
    return [[p[0] / r, p[1] / r], [-p[1] / q, p[0] / q]]
R = [[sr ** 2, 0.0], [0.0, sb ** 2]]
res = lambda a, b: [a[0] - b[0], math.atan2(math.sin(a[1] - b[1]), math.cos(a[1] - b[1]))]
print('prior std   filter   rms error   mean NEES (2 expected)')
for s0 in (0.5, 1.0, 1.5):
    P0 = [[s0 ** 2, 0.0], [0.0, s0 ** 2]]
    for kind in ('ekf', 'ukf'):
        rng = Rng(11)
        E, N = [], []
        for _ in range(2000):
            xt = sample_mvn(rng, [3.0, 0.0], P0)
            z = [hh(xt)[0] + rng.gauss(0, sr), hh(xt)[1] + rng.gauss(0, sb)]
            if kind == 'ekf':
                x, P, y, S_, K = ekf_update([3.0, 0.0], P0, z, hh, HH, R, res)
            else:
                x, P, y, S_, K = ukf_update([3.0, 0.0], P0, z, hh, R, alpha=1.0, beta=0.0, kappa=1.0, z_residual_fn=res)
            e = [x[0] - xt[0], x[1] - xt[1]]
            E.append(e[0] ** 2 + e[1] ** 2); N.append(nees(e, P))
        print(str(s0).rjust(9), kind.rjust(8), str(round(math.sqrt(mean(E)), 3)).rjust(10), str(round(mean(N), 2)).rjust(10))

sigma points (r, bearing): [[10.0, 0.0], [10.87, 0.0], [10.0, 0.87], [9.13, 0.0], [10.0, -0.87]]
weights: [0.333, 0.167, 0.167, 0.167, 0.167]
method          mean x   mean y   var x   var y
monte carlo       8.827    0.013   2.639  19.692
jacobian (EKF)     10.0      0.0    0.25    25.0
unscented         8.826      0.0   3.006  19.343
prior std   filter   rms error   mean NEES (2 expected)
      0.5      ekf       0.21       2.92
      0.5      ukf      0.209       2.43
      1.0      ekf      0.625      23.27
      1.0      ukf      0.657      14.77
      1.5      ekf      1.392     117.06
      1.5      ukf      1.582       66.4

In robotics

When to prefer the UKF

• The Jacobians are hard to derive, expensive, or unavailable (a black-box model, a simulation as the motion function).
• The nonlinearity is strong relative to the uncertainty (bearing-only tracking, large attitude errors) but the belief stays roughly unimodal.
• You want less overconfidence without tuning an inflated Q.

When the EKF is the better choice

When the models are nearly linear over the belief (as in most navigation filters with frequent corrections), the two filters give the same answer and the EKF is cheaper and simpler to debug. The UKF is no cure for a bad model or a bad initial estimate. The UAV course discusses the choice in its state estimation lessons.

Tuning

The parameters $\alpha, \beta, \kappa$ change the spread of the points. For the original transform ($\alpha=1, \beta=0, \kappa = 3 - n$) or the commonly used $\alpha = 10^{-3}\ldots 0.5, \beta=2, \kappa=0$, the results differ little for mild nonlinearity. A very small $\alpha$ makes the points almost coincide with the mean, so the transform approaches the Jacobian; a large one reaches into regions where the function behaves differently. The weights can be negative for some choices, so the covariance can lose positive definiteness: monitor it.

Related methods

Other deterministic-sampling filters (the cubature Kalman filter, Gauss–Hermite filters) choose the points differently; the central difference filter fits a polynomial instead of a Jacobian. Particle filters, next module, drop the Gaussian assumption altogether.

Common misconceptions

Key takeaways

1. Sigma points are 2n+1 deterministic points with the exact mean and covariance of the Gaussian. Push them through the true function and recompute the moments.
2. The result is accurate to second order in the mean, which the Jacobian is not: 8.826 against the true 8.827 in the radar example (Jacobian: 10.0).
3. The UKF correction uses the cross-covariance Pxz and the innovation covariance S from the transformed points. No derivatives are needed.
4. Angles must be averaged with sines and cosines and their residuals wrapped.
5. The UKF is still a single-Gaussian filter: less overconfident than the EKF in strong nonlinearity, but not able to represent a banana or two modes.

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. J. Julier and J. K. Uhlmann, “Unscented Filtering and Nonlinear Estimation”, Proceedings of the IEEE 92 (3), 2004. Paid journal article.
• E. A. Wan and R. van der Merwe, “The Unscented Kalman Filter for Nonlinear Estimation”, IEEE Adaptive Systems for Signal Processing, Communications, and Control Symposium, 2000. Paid conference paper.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 3.4 (the unscented Kalman filter). Paid book.
• R. Labbe, Kalman and Bayesian Filters in Python (online book) , the chapter on the unscented Kalman filter has an intuitive treatment of sigma points. 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

Both Gaussian filters fail the banana. The last lesson of the module looks at the situations in which a single Gaussian is simply the wrong representation (multimodality, ambiguity, kidnapping), setting up the particle filter of the next module.

Related content

Nothing below is required to finish this lesson.

• Before
  M8-T2: EKF Localization with Landmarks

  Write the Jacobians of the unicycle motion model and the range-bearing measurement, and gate a measurement with the Mahalanobis distance.

• 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