M4-T4 — Propagating Uncertainty Through Transformations

Learning objectives

After this lesson, you should be able to:

• Propagate a mean and a covariance through a linear map, and show that the result is Aμ + b and AΣAᵀ.
• Propagate through a nonlinear map by linearization with a Jacobian, and compute the Jacobian of a polar-to-Cartesian conversion.
• Derive the first-order covariance and the second-order bias of a nonlinear function, and say when linearization is unreliable.
• Check a propagation by Monte Carlo simulation, and apply it to a stereo camera’s depth error.

The intuition

An uncertain quantity is a cloud of possible values. A function takes every point of the cloud to a new point, so the output is a new cloud. For some functions the new cloud is easy to describe.

• Rotate or shift the coordinates. The cloud keeps its shape. A shift moves its centre and leaves the spread alone. A rotation turns the whole cloud, so its ellipse turns with it: the 5 cm by 1 cm ellipse along the corridor becomes the same ellipse along the 45° diagonal, with the covariance matrix [[13, 12], [12, 13]] of the earlier lessons.
• Scale. Stretching a quantity by 100 (metres to centimetres) stretches its standard deviation by 100 and its variance by 10 000.
• Add two independent quantities. The variances add (the previous lessons).

All three are linear maps, and the rule for all of them is the same: the new mean is the map applied to the old mean, and the new covariance is $A\,\Sigma\,A^{\top}$, where $A$ is the matrix of the map.

The trouble starts when the map is not linear. A range and a bearing become x and y through a cosine and a sine; stereo depth is the reciprocal of a disparity; a robot's new pose depends on its heading through trigonometry. A curved function bends the cloud. If the cloud is small compared with the curve, the function looks straight over it and a linear rule is accurate. If the cloud is large, it is not.

In the left panel the bearing noise (20°) is large, so the point cloud follows a circular arc of radius 10 m, and the average x is 9.41 m, not 10. Linearizing at the mean treats the arc as its tangent line: the predicted spread in x is 2 cm, the real one 81 cm. For a bearing noise of 1° the same conversion is almost exactly linear and the rule works to three digits. The question to ask of any nonlinear conversion is therefore: how large is the uncertainty compared with the scale on which the function curves?

A linear map gives an exact answer: the new covariance is AΣAᵀ. A nonlinear map is replaced by its tangent (a Jacobian) at the mean. That is accurate when the cloud is small compared with the curvature, and misleading when it is not.

Definitions

Linear map

$\mathbf{y} = A\mathbf{x} + \mathbf{b}$, with a matrix $A$ and a vector $\mathbf{b}$. Rotations, scalings, shifts, projections and sums are all linear.

Nonlinear map

$\mathbf{y} = g(\mathbf{x})$: a polar-to-Cartesian conversion, a trigonometric motion model, a reciprocal.

Jacobian $J$

The matrix of first derivatives of $g$: entry $(i, j)$ is $\partial g_i / \partial x_j$, evaluated at a point (usually the mean). It is the best linear approximation to $g$ near that point.

Linearization

Replacing $g$ by its tangent $g(\boldsymbol\mu) + J(\mathbf{x} - \boldsymbol\mu)$ near the mean.

Derivation: the covariance rule, and what linearization leaves out

The linear rule

1. $$\boldsymbol\mu_y = \mathbb{E}[A\mathbf{x} + \mathbf{b}] = A\,\mathbb{E}[\mathbf{x}] + \mathbf{b} = A\boldsymbol\mu_x + \mathbf{b}$$

   Linearity of expectation (the variance lesson).

2. $$\mathbf{y} - \boldsymbol\mu_y = A(\mathbf{x} - \boldsymbol\mu_x)$$

   The shift $\mathbf{b}$ cancels: it moves the centre and does not affect the spread.

3. $$\Sigma_y = \mathbb{E}\bigl[A(\mathbf{x} - \boldsymbol\mu_x)(\mathbf{x} - \boldsymbol\mu_x)^{\top}A^{\top}\bigr] = A\,\Sigma_x\,A^{\top}$$

   The definition of the covariance matrix, with $A$ pulled out of the expectation on both sides ($(A\mathbf{v})^{\top} = \mathbf{v}^{\top}A^{\top}$).

4. $$\Sigma_x = R\Lambda R^{\top},\; A = Q \text{ orthogonal} \;\Rightarrow\; \Sigma_y = (QR)\,\Lambda\,(QR)^{\top}$$

   A rotation $Q$ keeps the eigenvalues $\Lambda$ and rotates the eigenvectors: the ellipse turns and its axis lengths do not change, and $\det\Sigma_y = \det\Sigma_x$.

The nonlinear rule, to first order

1. $$g(\mathbf{x}) \approx g(\boldsymbol\mu) + J\,(\mathbf{x} - \boldsymbol\mu)$$

   Taylor expansion to first order about the mean. It is the tangent plane of $g$ at $\boldsymbol\mu$.

2. $$\mathbb{E}[g(\mathbf{x})] \approx g(\boldsymbol\mu) + J\,\mathbb{E}[\mathbf{x} - \boldsymbol\mu] = g(\boldsymbol\mu)$$

   The expectation of the linear term is zero.

3. $$\mathrm{Cov}\bigl(g(\mathbf{x})\bigr) \approx J\,\Sigma\,J^{\top}$$

   The linear rule, applied to the tangent plane.

What the first-order rule leaves out

Keep the next term of the expansion in one dimension, with $x \sim \mathcal{N}(\mu, \sigma^2)$:

1. $$g(x) \approx g(\mu) + g'(\mu)(x - \mu) + \tfrac{1}{2}\,g''(\mu)(x - \mu)^2$$

   Second-order Taylor expansion.

2. $$\mathbb{E}[g(x)] \approx g(\mu) + \tfrac{1}{2}\,g''(\mu)\,\sigma^2$$

   Take expectations: the linear term vanishes, and $\mathbb{E}[(x-\mu)^2] = \sigma^2$. The mean of the output is shifted by half the curvature times the variance: this is a bias that linearization ignores.

3. $$g = \cos: \;\; \mathbb{E}[\cos\theta] \approx 1 - \tfrac{1}{2}\sigma_\theta^2 \quad (\text{exactly } e^{-\sigma_\theta^2/2})$$

   For the polar conversion at bearing zero, $g'' = -\cos 0 = -1$. With $\sigma_\theta = 0.349$ (20°) the mean of $x$ is $10\,e^{-0.061} = 9.41$ m.

4. $$\text{the linearization is reliable when } \;|g''|\,\sigma^2 \ll |g'|\,\sigma \;\;\Longleftrightarrow\;\; \sigma \ll \left|\dfrac{g'}{g''}\right|$$

   Compare the curvature term with the slope term. Here $g'/g''$ is the distance over which the function turns away from its tangent: the uncertainty must be small compared with it.

For a linear map the covariance rule is exact. For a nonlinear one it is the rule for the tangent line, accurate when the standard deviation is small compared with the scale on which the function curves, and unreliable otherwise.

A worked example

Three propagations, checked against Monte Carlo (100 000 samples for each).

1. A rotation. Take the 5 cm by 1 cm error ellipse along the corridor, $\Sigma = \mathrm{diag}(25, 1)$, and rotate by 45°. $A\Sigma A^{\top}$ gives $\begin{pmatrix} 13 & 12 \\ 12 & 13 \end{pmatrix}$, the matrix of the earlier lessons. The eigenvalues are still 25 and 1.
2. Polar to Cartesian, small bearing noise. Range 10 m ± 2 cm and bearing 0° ± 1°. The Jacobian at the mean is $\begin{pmatrix} 1 & 0 \\ 0 & 10 \end{pmatrix}$, so $\sigma_x = 0.02$ m and $\sigma_y = 10 \times 0.01745 = 0.175$ m. Monte Carlo: 0.020 and 0.174, and a mean x of 9.999. Linearization is excellent.
3. Polar to Cartesian, large bearing noise. Same range, bearing noise 20° (0.349 rad). Linearization: $\sigma_x = 0.02$ m, $\sigma_y = 3.49$ m, mean x = 10. Monte Carlo: mean x = 9.408, $\sigma_x = 0.812$ m, $\sigma_y = 3.29$ m. The linearized spread in x is 40 times too small, and the mean is off by 59 cm. The cloud is a curved band, not an ellipse.
4. Stereo depth. A camera with focal length 700 pixels and a baseline of 12 cm measures disparity with a standard deviation of 0.25 pixel. At 2 m, $\sigma_Z = Z^2\sigma_d/(fB) = 4 \times 0.25 / 84 = 0.0119$ m; at 10 m, $100 \times 0.25/84 = 0.298$ m. Monte Carlo: 0.0119 and 0.2984. The depth error is 25 times larger at five times the distance. Here linearization works because the disparity error (0.25 px) is tiny compared with the disparity itself (8.4 px at 10 m).

import math
from robotprob import Rng, sample_mvn, propagate_linear, propagate_nonlinear, cov_matrix, mean_vector, mean, std

c = s = math.sqrt(0.5)                                  # 1. a linear map: rotate a 5 cm by 1 cm cloud by 45 degrees
R = [[c, -s], [s, c]]
m2, S2 = propagate_linear([0.0, 0.0], [[25.0, 0.0], [0.0, 1.0]], R)
print('rotated covariance =', [[round(v, 2) for v in row] for row in S2])

def to_xy(p):                                           # 2. polar to Cartesian: range, bearing -> x, y
    return [p[0] * math.cos(p[1]), p[0] * math.sin(p[1])]
rng = Rng(7)
for sigma_theta_deg in (1.0, 20.0):
    sig = math.radians(sigma_theta_deg)
    cov = [[0.02 ** 2, 0.0], [0.0, sig ** 2]]
    mean_lin, cov_lin = propagate_nonlinear(to_xy, [10.0, 0.0], cov)            # Jacobian from finite differences
    pts = [to_xy(sample_mvn(rng, [10.0, 0.0], cov)) for _ in range(100000)]     # the reference: sample and convert
    mean_mc, cov_mc = mean_vector(pts), cov_matrix(pts)
    print(sigma_theta_deg, 'deg bearing noise:')
    print('   linearized:  mean x', round(mean_lin[0], 3), ' std x', round(math.sqrt(cov_lin[0][0]), 3), ' std y', round(math.sqrt(cov_lin[1][1]), 3))
    print('   Monte Carlo: mean x', round(mean_mc[0], 3), ' std x', round(math.sqrt(cov_mc[0][0]), 3), ' std y', round(math.sqrt(cov_mc[1][1]), 3))

f_px, base = 700.0, 0.12                                # 3. stereo depth from disparity
for z in (2.0, 10.0):
    d = f_px * base / z
    sigma_z = z * z * 0.25 / (f_px * base)
    zs = [f_px * base / rng.gauss(d, 0.25) for _ in range(100000)]
    print('stereo at', z, 'm: linear sigma', round(sigma_z, 4), ' Monte Carlo std', round(std(zs), 4), ' mean', round(mean(zs), 3))

rotated covariance = [[13.0, 12.0], [12.0, 13.0]]
1.0 deg bearing noise:
   linearized:  mean x 10.0  std x 0.02  std y 0.175
   Monte Carlo: mean x 9.999  std x 0.02  std y 0.174
20.0 deg bearing noise:
   linearized:  mean x 10.0  std x 0.02  std y 3.491
   Monte Carlo: mean x 9.408  std x 0.812  std y 3.29
stereo at 2.0 m: linear sigma 0.0119  Monte Carlo std 0.0119  mean 2.0
stereo at 10.0 m: linear sigma 0.2976  Monte Carlo std 0.2984  mean 10.008

In robotics

From sensor frame to world frame

A lidar point is measured in the sensor frame. To place it in the map it is rotated and shifted by the robot pose, and its covariance is rotated with it: $R\,\Sigma_p\,R^{\top}$ plus the contribution of the robot's own pose uncertainty, which grows with the distance of the point because a heading error swings far points through a longer arc. Scan matching and landmark SLAM both weight points by exactly these matrices. The SLAM course shows the transformation of poses in Odometry and Motion Models.

Range and bearing to position

A radar, a sonar or a camera-plus-detector reports range and bearing. In Cartesian coordinates the uncertainty is a sideways-stretched ellipse whose width grows with the range. The Jacobian above, with $r\,\sigma_\theta$ as the cross-range standard deviation, explains why a distant object is easy to detect and hard to localize sideways. For large bearing noise or short range the banana shape means a Gaussian in x and y is a poor description; the unscented transform of module 8 handles it better.

Stereo and depth sensors

Depth from disparity has an error that grows with the square of the range, so an obstacle at 10 m is located 25 times less accurately than one at 2 m. Time-of-flight cameras have the opposite behaviour (range error roughly constant) and lidars an intermediate one. The choice of depth sensor is largely a choice of how error grows with range; the Systems Engineering guide to cameras discusses it for real products.

Pose uncertainty through a motion

A robot's new pose is a nonlinear function of its old pose and the commanded motion, because the heading enters through sines and cosines. The covariance after the move is $F\,\Sigma\,F^{\top} + G\,Q\,G^{\top}$, with $F$ the Jacobian with respect to the old pose and $G$ that with respect to the motion noise: the prediction step of the extended Kalman filter. M5-T2: Odometry and Velocity Motion Models derives the Jacobians for the odometry model, and M8-T1: Linearization and the Extended Kalman Filter the filter.

Always keep a Monte Carlo check

When a new conversion enters a pipeline, sample the input distribution, push 10 000 samples through the true function, and compare the sample covariance with the linearized one. If they disagree, either the uncertainty is too large for a first-order treatment or the Jacobian has a bug. It takes ten lines of code and is how the library of this course is tested.

Common misconceptions

Key takeaways

1. For y = Ax + b the mean becomes Aμ + b and the covariance AΣAᵀ. This is exact, and covers rotations, scalings, shifts, sums and frame changes.
2. A rotation turns the error ellipse and leaves its axis lengths, eigenvalues and determinant unchanged.
3. For a nonlinear y = g(x), linearize at the mean: μy ≈ g(μ) and Σy ≈ JΣJᵀ with J the Jacobian. This is the heart of the extended Kalman filter.
4. Linearization ignores curvature. It misses a bias of about ½ g″σ² in the mean and can badly underestimate the spread; trust it only when σ is small compared with |g′/g″|.
5. Stereo depth error grows as Z²: depth is a reciprocal, and the error in a reciprocal grows quickly. Check any propagation with a Monte Carlo run.

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.

• T. D. Barfoot, State Estimation for Robotics, 2nd edition, Cambridge University Press, 2024 , chapter “Primer on Probability Theory” (transformation of random variables, linearization). Free to read as the author’s PDF; the book is copyrighted.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 3.3 (the extended Kalman filter, linearization and its limits). Paid book.
• C. Stachniss, “Kalman Filter & EKF” (video lecture) , the EKF linearization. Free to watch.
• J. Tsitsiklis and P. Jaillet, MIT RES.6-012 Introduction to Probability , lecture 11 (derived distributions). Free videos and notes, licence CC BY-NC-SA 4.0.

Knowledge check

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

What’s next

We can now push a Gaussian through a function. The last lesson of this module goes the other way and asks how a Gaussian belief changes when new information arrives: conditioning on a measurement, and fusing two estimates of the same thing.

Related content

Nothing below is required to finish this lesson.

• Before
  M4-T3: The Multivariate Gaussian and Covariance Ellipses

  Write the multivariate Gaussian density, turn a covariance matrix into an ellipse with its eigenvectors, and use the Mahalanobis distance.

• Lab
  M4-L1: Robot Uncertainty Visualizer

  The position estimate comes with a covariance that nobody on the team can read. Compute it from samples, draw its ellipse, and push it through a rotation and a range-bearing conversion.

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

• UAV M3-T2: Gaussian distributions and covariance
• SLAM M2-L3: Understand Pose Uncertainty
• Navigation M7-T3: Navigation Under Pose Uncertainty