Part X: Sensor Fusion, World Models, and Spatial AI
Chapter 49: Probabilistic and Bayesian Fusion

Uncertainty propagation

"Every function I evaluate is a rumor mill: feed it a whisper of error and it hands back a shout, or swallows it whole. My only job is to predict which."

A Prudent AI Agent

Prerequisites

This section assumes the multivariate Gaussian, the covariance matrix, and the idea of an uncertainty ellipse from the primer in Chapter 4, plus the covariance-as-currency view established in Section 49.1. It uses the Jacobian and first-order Taylor expansion (Appendix A) and connects to the unscented and particle machinery of Chapter 10. No filter loop is needed here; we study a single transformation, not a time recursion.

The Big Picture

Fusion, tracking, and mapping are built from functions: a raw range and bearing become an \((x, y)\) point, a quaternion and a lever arm become a world coordinate, a set of pixel disparities becomes a depth. Each sensor's reading carries a covariance, but the quantity you actually fuse or report is the output of one of these functions. Uncertainty propagation is the discipline of pushing the input covariance through the function so the output carries an honest covariance too. Skip it and every downstream Bayesian rule from Section 49.1 is fed a lie: gates reject good detections, fusion weights the wrong sensor, and a system that looks confident is quietly wrong. Get it right and uncertainty stays calibrated all the way from the transducer to the decision.

The problem: a distribution goes in, a distribution must come out

The what is precise. You have a random input \(x\) with mean \(\mu_x\) and covariance \(\Sigma_x\), and a function \(y = f(x)\) (a coordinate change, a sensor model, a triangulation). You want the mean \(\mu_y\) and covariance \(\Sigma_y\) of the output. If \(f\) is linear, \(y = A x + b\), the answer is exact and famous: \(\mu_y = A\mu_x + b\) and \(\Sigma_y = A \Sigma_x A^\top\). The covariance is squeezed and rotated by \(A\), and nothing is approximated.

The why it is hard is that almost every interesting sensor function is nonlinear. A radar reports polar \((r, \theta)\); you need Cartesian \((x, y) = (r\cos\theta, r\sin\theta)\). A nonlinear \(f\) does two things a linear one never does. It bends a Gaussian into a non-Gaussian, banana-shaped cloud, so no mean-and-covariance pair fully describes the output. And it biases the mean: \(\mathbb{E}[f(x)] \neq f(\mathbb{E}[x])\) in general, so evaluating the function at the input mean gives the wrong output mean. Uncertainty propagation is the set of methods that approximate \((\mu_y, \Sigma_y)\) well enough to keep the covariance honest, trading accuracy against cost.

First-order propagation: the Jacobian sandwich

The workhorse method linearizes \(f\) around the input mean with a first-order Taylor expansion, \(f(x) \approx f(\mu_x) + J\,(x - \mu_x)\), where \(J = \left.\partial f / \partial x\right|_{\mu_x}\) is the Jacobian. Treating that local linear map exactly as the \(A\) above gives the delta method, the single most-used formula in all of sensor fusion:

\[ \mu_y \approx f(\mu_x), \qquad \Sigma_y \approx J\,\Sigma_x\,J^\top. \]

The how to read the sandwich \(J \Sigma_x J^\top\): the Jacobian tells you how sensitive each output is to each input, and the sandwich squeezes the input ellipse through that local sensitivity. A direction the function stretches (large Jacobian entries) inflates the output variance; a direction it compresses shrinks it. This is exactly the covariance step buried inside the Extended Kalman Filter of Chapter 9, met here in isolation. The when it works: when \(f\) is nearly linear across the width of the input ellipse, meaning the uncertainty is small compared with the function's curvature. The when it fails: large input uncertainty, sharp curvature, or a near-singular Jacobian. It silently drops the mean bias entirely (it assumes \(\mu_y = f(\mu_x)\)), so a range-heavy radar conversion at long distance or a low-parallax triangulation will report a covariance that is both mis-shaped and centered in the wrong place.

Key Insight

The Jacobian sandwich \(\Sigma_y = J \Sigma_x J^\top\) is the same computation whether \(f\) is a coordinate change, a camera projection, or a stack of neural-network layers; only \(J\) changes. But it captures only the first derivative. Its two blind spots are structural, not numerical: it assumes the output mean is \(f(\mu_x)\) (ignoring second-order bias) and it forces the output back into a Gaussian (ignoring the banana). When either bite matters, no amount of numerical care in computing \(J\) will save you; you need a method that samples the function, not one that differentiates it once.

When linearization lies: the unscented transform

The fix is to stop differentiating and start probing. The unscented transform chooses a small, deterministic set of sigma points that exactly reproduce the input mean and covariance, pushes each one through the true nonlinear \(f\), and recovers the output mean and covariance from the weighted spread of the transformed points. For an \(n\)-dimensional input it uses \(2n + 1\) points placed at the mean and along the axes of the input ellipse at a tuned distance. Because the points ride through the real function rather than a tangent line, the recovered mean captures second-order bias and the recovered covariance reflects how the function actually bends the cloud, all without a single derivative.

The why this is the default for nonlinear estimation: it is derivative-free (no Jacobian to derive, code, or debug for a messy sensor model), it is accurate to second order for any nonlinearity and to third order for Gaussian inputs, and it costs only a handful of function evaluations. The when: use it whenever the delta method's linearity assumption is shaky but a Gaussian output is still acceptable, which covers most radar, GNSS, and inertial conversions. Its own limit is that it still summarizes the output as a mean and covariance, so a genuinely multi-modal or heavy-tailed output (a data-association ambiguity, a range fold) needs the sampling density of a particle representation from Chapter 10.

Monte Carlo: the honest, expensive ground truth

When you need the full output shape, or simply a trustworthy reference to check the two approximations above, draw many samples from the input distribution, push every sample through \(f\), and read off whatever statistics you like from the transformed cloud. Monte Carlo propagation makes no linearity or Gaussian assumption; its error shrinks as \(1/\sqrt{N}\), so it converges slowly but surely to the truth for any \(f\). The when: use it as the gold standard in offline validation, when the output is multi-modal, or when the function is a non-differentiable black box (a ray-caster, a learned model without clean gradients). The trade is cost: thousands of evaluations where the delta method needed one and the unscented transform needed a handful, which is why real-time filters lean on the cheaper two and reserve Monte Carlo for the bench. Listing 49.3 runs all three on a radar polar-to-Cartesian conversion and exposes exactly where linearization drifts.

import numpy as np

# Radar returns polar (range, bearing) with a large bearing uncertainty.
mu   = np.array([50.0, np.deg2rad(30.0)])              # r = 50 m, theta = 30 deg
Sx   = np.diag([0.5**2, np.deg2rad(8.0)**2])           # sigma_r = 0.5 m, sigma_theta = 8 deg
f    = lambda r, t: np.array([r*np.cos(t), r*np.sin(t)])  # polar -> Cartesian

def jacobian(r, t):
    return np.array([[np.cos(t), -r*np.sin(t)],
                     [np.sin(t),  r*np.cos(t)]])

# (1) Delta method: mean = f(mu), covariance = J Sx J^T
J    = jacobian(*mu)
mu_lin, S_lin = f(*mu), J @ Sx @ J.T

# (2) Unscented transform: 2n+1 sigma points (n = 2), symmetric set
n, kappa = 2, 1.0
L = np.linalg.cholesky((n + kappa) * Sx)
pts = np.column_stack([mu, *(mu + L.T), *(mu - L.T)])            # 5 sigma points
w   = np.full(2*n + 1, 1.0 / (2*(n + kappa))); w[0] = kappa/(n + kappa)
Y   = np.array([f(r, t) for r, t in pts.T])                     # push each through f
mu_ut = w @ Y
S_ut  = (w * (Y - mu_ut).T) @ (Y - mu_ut)

# (3) Monte Carlo reference
rng = np.random.default_rng(0)
X   = rng.multivariate_normal(mu, Sx, size=200_000)
Ym  = np.array([f(r, t) for r, t in X])
mu_mc, S_mc = Ym.mean(0), np.cov(Ym.T)

print("mean  lin/ut/mc:", mu_lin.round(3), mu_ut.round(3), mu_mc.round(3))
print("std-y lin/ut/mc:", round(np.sqrt(S_lin[1,1]),3),
      round(np.sqrt(S_ut[1,1]),3), round(np.sqrt(S_mc[1,1]),3))
Listing 49.3. Propagating radar polar uncertainty into Cartesian by three methods. With an 8-degree bearing sigma the arc is wide: the delta method places the mean at \(f(\mu)\) and misses the inward bias that the unscented and Monte Carlo means both recover, and it mis-sizes the cross-range spread. The unscented result tracks the 200k-sample reference at the cost of five function evaluations.

As Listing 49.3 makes concrete, the three methods agree when curvature is gentle and diverge exactly when it is not, which is the whole reason a fusion engineer keeps all three in reach.

In Practice: an automotive radar tracker gates the wrong lane

A highway radar reports targets in polar coordinates and the tracker fuses them in Cartesian. Early in development the team used the delta-method conversion for speed. At 120 m range with a realistic 6-degree bearing sigma, the true cross-range uncertainty spans nearly a full lane, but the linearized covariance both under-sized that spread and centered it slightly outside the arc. The Mahalanobis gate from Section 49.1 then rejected valid returns from an adjacent vehicle and, worse, occasionally associated a return to the wrong lane's track. Swapping the conversion to an unscented transform (five function evaluations per detection, a negligible cost against the radar's own processing) restored an honest banana-shaped covariance, the gate stopped clipping, and the phantom lane-change events disappeared. The physics had not changed; only the propagation had become truthful. This same conversion sits at the front of the radar pipeline in Chapter 44.

The Right Tool

Hand-rolling the unscented transform means a Cholesky factor, sigma-point weights that are easy to get subtly wrong, and a weighted outer-product sum, roughly twenty lines that a sign error in the weights will quietly corrupt. A filtering library ships a vetted, numerically guarded implementation:

from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints
import numpy as np

sp    = MerweScaledSigmaPoints(n=2, alpha=1e-3, beta=2.0, kappa=0.0)
sigmas = sp.sigma_points(mu, Sx)                 # from Listing 49.3
Y      = np.array([f(r, t) for r, t in sigmas]) # push points through f
mu_ut, S_ut = unscented_transform(Y, sp.Wm, sp.Wc)   # mean + covariance, tuned weights
Listing 49.4. The same unscented propagation in four lines with filterpy, versus roughly twenty by hand. The library owns the sigma-point placement, the Merwe scaling parameters, and the mean/covariance recombination; you own only the function f and the choice of whether a Gaussian output is even adequate. It will not warn you when the true output is bimodal and you should have used samples instead.

As Listing 49.4 shows, the library removes the fiddly linear algebra but not the modeling judgment: choosing among delta, unscented, and Monte Carlo remains yours, and it is the choice that keeps calibration honest, a theme that returns in Chapter 18.

Exercise

Take the radar setup of Listing 49.3 but sweep the bearing sigma from 1 degree to 20 degrees at fixed range 50 m. For each value, compute the output mean by all three methods and record the gap between the delta-method mean and the Monte Carlo mean along the cross-range axis. At what bearing uncertainty does the linearization bias exceed 0.5 m? Plot the three cross-range standard deviations on one axis and write two sentences: one on where the unscented and Monte Carlo curves separate, and one on what that separation implies for a gate width.

Self-Check

1. Why does the delta method get the output mean wrong for a strongly curved \(f\), and which term in the Taylor expansion is it dropping?

2. The unscented transform uses \(2n+1\) points and is derivative-free. State one advantage over the delta method and one situation where both fail and you must sample instead.

3. You propagate a small covariance through a nearly linear function. Which method should you reach for first, and why is spending 200k samples wasteful here?

What's Next

In Section 49.7, we turn a propagated covariance into a test. Once uncertainty is honestly carried through every transform, the innovation (the gap between what a sensor reports and what the fused belief predicted) can be normalized by its own covariance and checked against a statistical threshold. That is the foundation of consistency checks and fault detection: catching a lying sensor, a diverging filter, or a broken model before the rest of the system trusts it.