M9-T4 — Covariance Propagation

Learning objectives

After this lesson, you should be able to:

• Derive Σ_y = AΣ_xAᵀ for a linear map and verify it numerically.
• Propagate covariance through a nonlinear function by linearising with the Jacobian.
• Add covariances of independent contributions and write the Kalman prediction P⁻ = FPFᵀ + Q.
• Recognise when linearisation fails (large noise, strong curvature) and what to do about it.

Introduction and motivation

A LiDAR gives range and bearing with small independent errors. A mapping module needs the point in x and y, and with it an error ellipse that grows with range: at 20 m a 1° bearing error is 35 cm sideways. That ellipse comes from propagating the sensor covariance through the polar-to-Cartesian conversion. The same operation, with a different function, predicts the robot's uncertainty after driving a metre, composes uncertain poses, and converts joint noise to hand noise through the arm Jacobian.

The intuition

Imagine a cloud of possible values. Push every point through the map A: the cloud is stretched, rotated and sheared into a new ellipse. Its spread in any direction can be computed from the old spread, which gives AΣAᵀ. For a nonlinear function, zoom in on the cloud: if it is small compared with the curvature of the function, the function looks linear there, with matrix J, and the same formula applies. If the cloud is large, the function bends it into a banana and the simple rule is only an approximation.

Geometric and physical interpretation

Geometric view. The image of the covariance ellipse under A is the new covariance ellipse (Module 6: a matrix maps the unit circle to an ellipse). The new axes come from the SVD of A Lc where Σ = LcLcᵀ. Physical view. Uncertainty in the heading of a robot is turned by the distance travelled into lateral position uncertainty; the Jacobian of the motion model contains the distance as a coefficient, which is why heading errors dominate long-term drift.

The mathematics

x ~ (μ, Σ_x)

an uncertain vector with mean μ and covariance Σ_x.

A

a deterministic matrix, y = A x + b.

f, J

a nonlinear function and its Jacobian J = ∂f/∂x at the mean.

F, Q

the state-transition matrix and the process-noise covariance in a filter.

Linear maps

y = A x + b   ⇒   E[y] = Aμ + b,    Σy = A Σx Aᵀ

What it means. The mean passes through the map; the covariance is sandwiched between A and its transpose. The offset b does not affect the covariance. This is exact for any distribution, and gives the full Gaussian when x is Gaussian.

Nonlinear functions: linearise

y = f(x)   ⇒   E[y] ≈ f(μ),    Σy ≈ J Σx Jᵀ,    J = ∂f/∂x |_μ

What it means. Replace f by its tangent at the mean (Module 8). Good when the noise is small compared with the curvature of f; the result is a Gaussian approximation of what may be a non-Gaussian output.

Sums of independent errors and the filter prediction

z = x + y (independent)  ⇒  Σz = Σx + Σy        x⁺ = F x + w:  P⁻ = F P Fᵀ + Q

What it means. Independent errors add in covariance. The Kalman prediction is a linear map of the old covariance plus the process-noise covariance.

Linear: Σ_y = AΣAᵀ. Nonlinear: Σ_y ≈ JΣJᵀ with J evaluated at the mean. Independent sums: covariances add. Prediction: P⁻ = FPFᵀ + Q.

Step-by-step derivation: AΣAᵀ, the Jacobian form and the failure of linearisation

1. Mean. By linearity of expectation E[Ax + b] = AE[x] + b = Aμ + b.
2. Covariance. The deviation of y is y − E[y] = A(x − μ). Hence Σ_y = E[A(x − μ)(x − μ)ᵀAᵀ] = A E[(x − μ)(x − μ)ᵀ] Aᵀ = AΣ_xAᵀ, since A is constant.
3. Symmetric PSD preserved. vᵀ(AΣAᵀ)v = (Aᵀv)ᵀΣ(Aᵀv) ≥ 0, so the output covariance is again symmetric positive semi-definite; it is positive definite if Σ is and A has full row rank.
4. Nonlinear case. Expand f(x) ≈ f(μ) + J(x − μ) (first-order Taylor, Module 8). The deviation of y is then J(x − μ) and the linear result gives Σ_y ≈ JΣ_xJᵀ with E[y] ≈ f(μ).
5. Polar to Cartesian. For x = r cos β, y = r sin β, J = [[cos β, −r sin β], [sin β, r cos β]]. With independent errors Σ_p = diag(σ_r², σ_β²), Σ_xy = J Σ_p Jᵀ. Because J is a rotation times diag(1, r), the ellipse has axes along and across the line of sight with semi-axes σ_r and r σ_β.
6. Failure of the first-order rule. The second-order Taylor term gives a mean shift of about ½ tr(H Σ) (H the Hessian of f). For the polar example E[x] = r E[cos β] = r e^(−σ_β²/2) ≈ r(1 − σ_β²/2), so the mean of x sits below r by rσ_β²/2; the cloud is a banana, curved along an arc of radius r.
7. Rule of thumb. Linearisation is safe if r σ_β² ≪ σ_r and similar curvature-versus-noise tests hold: the error of the mean must be small compared with the standard deviations.
8. Prediction step. The motion model x⁺ = F x + w, with w ~ (0, Q) independent of x, gives Σ_{x⁺} = FΣFᵀ + Q. Applying it repeatedly makes the covariance grow whenever no measurement shrinks it, the dead-reckoning drift.

Alternatives when linearisation fails. The unscented transform pushes a small set of deterministically chosen sigma points through f and re-estimates the mean and covariance from the outputs, capturing second-order effects without Jacobians. Monte Carlo sampling pushes many random samples through f and is the gold standard for verification.

Correlated inputs. If the two inputs of a sum are correlated, Σ_z = Σ_x + Σ_y + Σ_xy + Σ_yx. Pose composition in SLAM involves correlated terms; ignoring them is a typical source of over-confidence.

A worked example: a shear, a LiDAR point and dead reckoning

• Linear map. Σ_x = diag(1, 0.25) and the shear A = [[1, 1], [0, 1]] give Σ_y = AΣAᵀ = [[1.25, 0.25], [0.25, 0.25]]. The shear creates a positive covariance 0.25 from independent inputs; the correlation is 0.25/√(1.25·0.25) = 0.447.
• Polar to Cartesian. A return at range 4 m, bearing 0°, with σ_r = 0.1 m and σ_β = 0.05 rad (2.9°): J = [[1, 0], [0, 4]], Σ = diag(0.01, 0.04): along-track σ = 0.1 m, across-track σ = 0.2 m.
• At bearing 30°. J = [[0.866, −2.0], [0.5, 3.464]]; Σ_xy = JΣ_pJᵀ = [[0.0175, −0.0130], [−0.0130, 0.0325]], whose eigenvalues are 0.01 and 0.04 (rotation of the same ellipse), with a negative correlation of −0.0130/√(0.0175·0.0325) = −0.545 in map axes.
• Sum rule. Two independent error sources with covariances diag(0.04, 0.09) and diag(0.01, 0.04) give total diag(0.05, 0.13).
• Prediction. Position and velocity with F = [[1, 1], [0, 1]] (one-second steps), P₀ = diag(0.01, 0.04), Q = diag(0, 0.01): P₁ = [[0.05, 0.04], [0.04, 0.05]]. After 10 steps without measurements P₁₀[0,0] = 6.86, a position standard deviation of 2.6 m from an initial 0.1 m: dead reckoning drift.
• Where linearisation fails. With the same range but a large bearing noise σ_β = 0.35 rad (20°): the true mean of x is 4 e^(−0.35²/2) = 3.76, a bias of 0.24 m, larger than the range σ of 0.1 m; the straight ellipse centred at (4, 0) misses the banana.

Robotics application

SLAM and LiDAR. Each measurement carries a covariance obtained by propagating range and bearing noise (and pose noise) to Cartesian coordinates; scan-matching weights residuals with it; see LiDAR point clouds for SLAM, EKF-SLAM and pose graphs.

UAVs. The EKF prediction P⁻ = FPFᵀ + Q with the linearised strapdown model; the error-state filter propagates the covariance of small attitude, velocity and position errors; see the extended Kalman filter, the error-state Kalman filter and IMU preintegration.

Manipulators. Joint-encoder noise maps to hand noise by Σ_p = J Σ_q Jᵀ (Module 8): near a singularity the Jacobian is nearly rank-deficient and the hand ellipse becomes long along the weak direction in reverse (the inverse map amplifies).

Mobile robots. Odometry-based motion models add heading-dependent lateral uncertainty; see robot motion models for navigation. IMU and GPS fusion relies on the same propagation to align time steps.

Engineering implications and limitations

Practical rules

• Evaluate J at the mean (current estimate). A Jacobian at the wrong point gives a wrong covariance; update it every step.
• Mind the units and the order. The sandwich is A Σ Aᵀ, not Aᵀ Σ A; dimensions must match (m × n)(n × n)(n × m).
• Check the output is symmetric PSD. Rounding can break symmetry; symmetrise (Σ + Σᵀ)/2.
• Verify with Monte Carlo. Sample, push through f, compute the sample covariance and compare with JΣJᵀ; this exposes both Jacobian bugs and linearisation failures.
• Use the unscented transform or sampling for strong nonlinearity. Typical triggers: bearing noise above 10°, ranges comparable to the position uncertainty, angle wrap near ±π.
• Do not treat correlated terms as independent. Compose poses with the correct cross-covariances (Jacobians with respect to both inputs), or the result is over-confident.

Limits

First-order propagation gives only the leading behaviour. It ignores bias created by curvature and can badly misrepresent heavy nonlinearities, multi-modal outputs and angles near their wrap-around.

Common misconceptions

Engineering takeaways

1. For y = Ax + b, mean Aμ + b and covariance AΣAᵀ; the output is symmetric PSD.
2. For nonlinear f, Σ_y ≈ JΣJᵀ with J evaluated at the mean: the Jacobian of Module 8 is the local matrix A.
3. Independent errors add covariances; the Kalman prediction is P⁻ = FPFᵀ + Q.
4. Linearisation fails when noise is large relative to curvature (a banana with a biased mean); use sigma points or sampling and check with Monte Carlo.
5. Evaluate J at the current estimate, keep units and order straight, and symmetrise the result.

References and further reading

Primary papers and authoritative documentation.

• Barfoot, State Estimation for Robotics (author-hosted first edition, free to read): chapter 2.2.4 and chapter 4, nonlinear transformation of Gaussians and the Kalman filter
• Mathematics for Machine Learning: chapter 6.5, sums, products and linear transformations of Gaussians
• UCSD ECE276A: Sensing and Estimation in Robotics: covariance propagation through motion and measurement models
• Solà, Deray and Atchuthan, A micro Lie theory for state estimation in robotics: A micro Lie theory: uncertainty on manifolds and its propagation

Knowledge check

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

What’s next

To decide whether a measurement is plausible or which landmark it belongs to, we compare it with the prediction in units of its uncertainty. That is the Mahalanobis distance of the next lesson.

Related content

Nothing below is required to finish this lesson.

• Before
  M9-T3: Multivariate Gaussian Distributions

  Write the multivariate Gaussian density and interpret its mean, covariance and level sets.

• Before
  M2-T3: Matrix-Vector Multiplication

  Compute A x as a combination of the columns of A and as rows dotted with x, and use it to transform robot measurements.

• Lab
  M9-L1: Robot Localization with Uncertainty

  Localise a robot with range measurements: update a position estimate and its covariance, and find why an overconfident filter fails.