M4-T5 — Conditioning and Fusing Gaussians

Learning objectives

After this lesson, you should be able to:

• Fuse two independent Gaussian estimates of one scalar, in the precision-weighted and the gain form, and explain why the variance never increases.
• Derive the fusion by completing the square, and fuse vector estimates in information form.
• Condition a joint Gaussian on one of its components, and derive the conditional mean and covariance.
• Explain why measuring one quantity improves the estimate of a correlated one, and by how much.

The intuition

Rover's odometry says it is 12.40 m down the corridor, uncertain by 0.20 m. Its lidar scan matcher, comparing the scan with the map, says 12.15 m, uncertain by 0.10 m. Both are Gaussian and independent. What should Rover believe?

Common sense gives the answer: somewhere between the two, closer to the more reliable one. The mathematics makes it exact. The lidar is twice as precise, which makes it four times as informative (information is the inverse of the variance), so it gets four times the weight: the fused estimate is 12.20 m, four-fifths of the way from the odometry to the lidar. And it is better than either: its standard deviation is 0.089 m, below the lidar's 0.10 m.

Two properties in the left panel hold in general. First, precisions add: with the precision defined as $1/\sigma^2$, the fused precision is the sum of the two, 25 + 100 = 125 per m², so the variance is 1/125 = 0.008 m². Information from independent sources accumulates. Second, even a poor sensor never hurts: fusing the 0.20 m odometry with a hopeless sensor of 2 m only trims the standard deviation from 0.200 to 0.199. It carries almost no information and so gets almost no weight.

The right-hand panel shows a subtler effect. Rover's state is a position and a velocity, whose errors after a prediction step are correlated (the tilted ellipse). Now it measures position only, and finds it 0.4 m ahead of the prediction. The position estimate moves, of course. But the velocity estimate also moves (to +0.12 m/s) and its uncertainty falls, from 0.40 to 0.36 m/s, though velocity was never measured. Because the errors were correlated, learning one error told us something about the other. Using this is how a filter that measures only positions can estimate velocities, and how a camera that sees one landmark can correct a heading.

Fusion combines two estimates of the same quantity. Conditioning uses a measurement of one quantity to update another that is correlated with it. In a Gaussian both are exact, and both are the Kalman update.

Definitions

Precision

$1/\sigma^2$, the inverse of the variance: how much information an estimate carries. For vectors, the information matrix $\Sigma^{-1}$.

Fusion

Combining two independent estimates of the same quantity. The posterior of Bayes’ rule with one estimate as the prior and the other as the likelihood.

Gain $K$

The fraction of the difference between the two estimates that is added to the first one. It equals the first variance divided by the sum of the two.

Joint Gaussian

A Gaussian over a vector that is made of several parts, for example the state $\mathbf{x}$ and a measurement $\mathbf{z}$: mean $(\boldsymbol\mu_x, \boldsymbol\mu_z)$ and covariance blocks $\Sigma_{xx}, \Sigma_{xz}, \Sigma_{zz}$.

Conditional Gaussian

The distribution of $\mathbf{x}$ once $\mathbf{z}$ is known. It is again Gaussian.

Derivation: completing the square, and conditioning by decorrelation

Fusing two Gaussians

Bayes' rule says the posterior is proportional to the prior times the likelihood. If the first estimate is the prior $\mathcal{N}(\mu_1, \sigma_1^2)$ and the second is a measurement with $z = \mu_2$ and noise variance $\sigma_2^2$, then as a function of $x$ the likelihood is $\mathcal{N}(x; \mu_2, \sigma_2^2)$. So the fused density is the product of the two densities.

1. $$p(x) \;\propto\; \exp\!\Bigl(-\dfrac{(x - \mu_1)^2}{2\sigma_1^2} - \dfrac{(x - \mu_2)^2}{2\sigma_2^2}\Bigr)$$

   Multiply the two Gaussians: the exponents add.

2. $$-\tfrac12\Bigl[x^2\Bigl(\dfrac{1}{\sigma_1^2} + \dfrac{1}{\sigma_2^2}\Bigr) - 2x\Bigl(\dfrac{\mu_1}{\sigma_1^2} + \dfrac{\mu_2}{\sigma_2^2}\Bigr)\Bigr] + \text{const}$$

   Expand both squares and collect the terms in $x^2$, $x$ and constants. Everything that does not depend on $x$ goes into the constant, which normalization will absorb.

3. $$\dfrac{1}{\sigma^2} = \dfrac{1}{\sigma_1^2} + \dfrac{1}{\sigma_2^2}, \qquad \mu = \sigma^2\Bigl(\dfrac{\mu_1}{\sigma_1^2} + \dfrac{\mu_2}{\sigma_2^2}\Bigr)$$

   This is the exponent $-\tfrac{(x-\mu)^2}{2\sigma^2}$ with its square completed: the coefficient of $x^2$ is $1/\sigma^2$ and the coefficient of $x$ is $\mu/\sigma^2$. The product of two Gaussians is, up to a constant, a Gaussian.

4. $$\sigma^2 = \dfrac{\sigma_1^2\,\sigma_2^2}{\sigma_1^2 + \sigma_2^2} = \sigma_1^2\cdot\dfrac{\sigma_2^2}{\sigma_1^2 + \sigma_2^2} \;\le\; \sigma_1^2$$

   The last factor is at most 1, so the fused variance is no larger than either input (and likewise no larger than $\sigma_2^2$). Adding independent information cannot increase uncertainty.

5. $$\mu = \dfrac{\sigma_2^2\,\mu_1 + \sigma_1^2\,\mu_2}{\sigma_1^2 + \sigma_2^2} = \mu_1 + K(\mu_2 - \mu_1), \qquad K = \dfrac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}$$

   Rewrite the mean in the gain form. If the second estimate is perfect ($\sigma_2 \to 0$) then $K \to 1$ and $\mu \to \mu_2$; if it is useless ($\sigma_2 \to \infty$) then $K \to 0$ and the first estimate is kept.

Conditioning a joint Gaussian, in one dimension

Let $x$ and $z$ be jointly Gaussian with variances $\sigma_x^2, \sigma_z^2$, covariance $\sigma_{xz}$ and means $\mu_x, \mu_z$. Split $x$ into the part that can be predicted from $z$ and the rest.

1. $$e = (x - \mu_x) - \beta\,(z - \mu_z)$$

   Define the residual left after predicting $x$ from $z$ with a coefficient $\beta$ to be chosen.

2. $$\mathrm{Cov}(e, z) = \sigma_{xz} - \beta\,\sigma_z^2 = 0 \;\;\Rightarrow\;\; \beta = \dfrac{\sigma_{xz}}{\sigma_z^2}$$

   Choose $\beta$ so that the residual is uncorrelated with $z$.

3. $$e \text{ and } z \text{ are jointly Gaussian and uncorrelated} \;\Rightarrow\; e \text{ and } z \text{ are independent}$$

   For jointly Gaussian variables, zero correlation means independence (the factorization of the joint density). This is a special property of the Gaussian.

4. $$x = \mu_x + \beta\,(z - \mu_z) + e \;\;\Rightarrow\;\; x \mid z \;\sim\; \mathcal{N}\bigl(\mu_x + \beta(z - \mu_z),\; \mathrm{Var}(e)\bigr)$$

   Once $z$ is known, the first two terms are constants, and $e$ is unaffected by $z$ because it is independent of it.

5. $$\mathrm{Var}(e) = \sigma_x^2 - 2\beta\sigma_{xz} + \beta^2\sigma_z^2 = \sigma_x^2 - \dfrac{\sigma_{xz}^2}{\sigma_z^2} = \sigma_x^2\,(1 - \rho^2)$$

   Expand the variance of the residual. The conditional variance is the original variance reduced by the factor $1 - \rho^2$, where $\rho$ is the correlation between $x$ and $z$.

The matrix version, in the definitions above, follows from the same argument with $\beta$ replaced by the matrix $K = \Sigma_{xz}\Sigma_{zz}^{-1}$.

Fusion is conditioning

Let a measurement be $z = x + v$ with noise variance $\sigma_2^2$, and the prior on $x$ have variance $\sigma_1^2$. Then $\sigma_z^2 = \sigma_1^2 + \sigma_2^2$ and $\sigma_{xz} = \sigma_1^2$, so $\beta = \sigma_1^2/(\sigma_1^2 + \sigma_2^2) = K$. The conditioning formula gives exactly the fusion formula: the two views are the same.

A measurement is informative about a quantity to the extent that it is correlated with it. The variance falls by the factor 1 − ρ²: a measurement with ρ = 0.9 removes 81% of the variance, one with ρ = 0.1 removes 1%.

A worked example

Three calculations, one program.

1. Fusing two position estimates. Odometry $12.40 \pm 0.20$ m, scan matching $12.15 \pm 0.10$ m. Precisions 25 and 100, sum 125: $\sigma^2 = 0.008$, $\sigma = 0.0894$ m. Mean $0.008 \times (12.40 \times 25 + 12.15 \times 100) = 12.20$ m. In gain form: $K = 0.04/0.05 = 0.8$, $\mu = 12.40 + 0.8(12.15 - 12.40) = 12.20$ and $\sigma^2 = 0.2 \times 0.04 = 0.008$. The two forms agree.
2. A hopeless second sensor. With a second standard deviation of 2.0 m, $K = 0.04/4.04 = 0.0099$ and the fused standard deviation is 0.1990 m: no gain.
3. Fusing in two dimensions. One estimate has covariance diag(4, 1) and the other diag(1, 4): each is well known in one direction and poor in the other. In information form, diag(0.25, 1) + diag(1, 0.25) = diag(1.25, 1.25), so the fused covariance is diag(0.8, 0.8): better than the best of each in both directions.
4. Conditioning. The state is (position, velocity) with covariance $\begin{pmatrix} 0.25 & 0.10 \\ 0.10 & 0.16 \end{pmatrix}$ (standard deviations 0.5 m and 0.4 m/s, correlation 0.5), and a position measurement $z = x + v$ has noise variance 0.09. Then $\Sigma_{zz} = 0.34$ and $\Sigma_{xz} = (0.25, 0.10)^{\top}$, giving the gain $K = (0.735, 0.294)^{\top}$.
5. The update. The measured position is 0.4 m above the prediction. The mean moves by $K \times 0.4 = (0.294, 0.118)$: the position moves by 0.29 m and the velocity by +0.12 m/s. The covariance becomes $\begin{pmatrix} 0.0662 & 0.0265 \\ 0.0265 & 0.1306 \end{pmatrix}$: the position standard deviation falls from 0.50 to 0.26 m and the velocity's from 0.40 to 0.36 m/s, with no velocity measurement at all.
6. Conditioning by sampling. Draw 300 000 samples of (position, velocity, measurement) from the joint Gaussian and keep those whose measurement is within 0.02 of 0.4. Of those, 6 492 remain: mean position 0.296, mean velocity 0.114, standard deviation of the velocity 0.369, against the exact 0.294, 0.118 and 0.361. Conditioning really is “restrict, then renormalize”.

import math
from robotprob import fuse, fuse_mv, condition, Rng, cholesky, mean, std

mu, var = fuse(12.40, 0.20 ** 2, 12.15, 0.10 ** 2)               # 1. fuse two scalar estimates
print('fused: mean', round(mu, 3), ' std', round(math.sqrt(var), 4), ' (the better sensor alone: 0.1)')
k = 0.04 / (0.04 + 0.01)
print('gain form: K =', round(k, 2), ' mean', round(12.40 + k * (12.15 - 12.40), 3), ' var', round((1 - k) * 0.04, 4))
mu2, var2 = fuse(12.40, 0.2 ** 2, 12.15, 2.0 ** 2)
print('with a very noisy second sensor (std 2.0): std', round(math.sqrt(var2), 4), ' (from 0.2)')

m1, P1 = [0.0, 0.0], [[4.0, 0.0], [0.0, 1.0]]                    # 2. fuse vectors: each estimate is good in one direction
m2, P2 = [0.0, 0.0], [[1.0, 0.0], [0.0, 4.0]]
mf, Pf = fuse_mv(m1, P1, m2, P2)
print('two perpendicular estimates fuse to covariance', [[round(v, 3) for v in row] for row in Pf])

mean_x = [0.0, 0.0, 0.0]                                         # 3. position, velocity, and a measurement of position
cov = [[0.25, 0.10, 0.25], [0.10, 0.16, 0.10], [0.25, 0.10, 0.34]]
rest, mc, cc = condition(mean_x, cov, [2], [0.4])                # condition on z = 0.4
print('posterior mean', [round(v, 4) for v in mc], ' posterior std', [round(math.sqrt(cc[i][i]), 3) for i in range(2)], ' (prior 0.5, 0.4)')
print('posterior covariance', [[round(v, 4) for v in row] for row in cc])

L = cholesky(cov)                                                # check by sampling: keep the samples whose measurement is near 0.4
rng = Rng(5)
kept = []
for _ in range(300000):
    w = [rng.gauss(), rng.gauss(), rng.gauss()]
    x = L[0][0] * w[0]
    v = L[1][0] * w[0] + L[1][1] * w[1]
    z = L[2][0] * w[0] + L[2][1] * w[1] + L[2][2] * w[2]
    if abs(z - 0.4) < 0.02:
        kept.append((x, v))
print('sampling check:', len(kept), 'samples kept; mean position', round(mean([p[0] for p in kept]), 3), ' mean velocity', round(mean([p[1] for p in kept]), 3), ' std velocity', round(std([p[1] for p in kept]), 3))

fused: mean 12.2  std 0.0894  (the better sensor alone: 0.1)
gain form: K = 0.8  mean 12.2  var 0.008
with a very noisy second sensor (std 2.0): std 0.199  (from 0.2)
two perpendicular estimates fuse to covariance [[0.8, 0.0], [0.0, 0.8]]
posterior mean [0.2941, 0.1176]  posterior std [0.257, 0.361]  (prior 0.5, 0.4)
posterior covariance [[0.0662, 0.0265], [0.0265, 0.1306]]
sampling check: 6492 samples kept; mean position 0.296  mean velocity 0.114  std velocity 0.369

In robotics

This is the Kalman filter

Read the conditioning formula again with $\mathbf{x}$ the robot's state and $\mathbf{z} = H\mathbf{x} + \mathbf{v}$ a measurement. Then $\Sigma_{xz} = \Sigma H^{\top}$ and $\Sigma_{zz} = H\Sigma H^{\top} + R$, and the update becomes $\boldsymbol\mu \leftarrow \boldsymbol\mu + K(\mathbf{z} - H\boldsymbol\mu)$, $\Sigma \leftarrow (I - KH)\Sigma$ with $K = \Sigma H^{\top}(H\Sigma H^{\top} + R)^{-1}$: the Kalman update. Module 7 derives the whole filter from this lesson and the previous one: the prediction step is the propagation of the last lesson, and the update step is this one. The UAV course derives it from a different starting point in Kalman Filter.

Sensor fusion with different strengths

A GPS fix is good to a few metres in all directions, wheel odometry is excellent over a few metres and poor over a kilometre, a compass is good in heading and says nothing about position. Fusing them in information form combines each one's strengths: every direction ends up as well known as the best sensor allows. The precision-weighted view makes it easy to see which sensor dominates in each direction. The Perception course shows a simple case in Fuse Simulated Observations with Uncertainty.

Unobserved states from correlated ones

A filter never measures velocity, bias or scale directly in many systems. They are estimated because the dynamics make their errors correlated with measured quantities: a position error grows with a velocity error, an accelerometer bias shows up in velocity. The correlation is built by the prediction step and used by the update, just as in the right-hand panel above. A state that has no correlation with any measurement (a state that is unobservable) is never improved however long the filter runs; module 7 returns to this.

Marginals and conditionals in SLAM

Marginalizing a joint Gaussian just picks the corresponding rows and columns (marginal in the library). Conditioning uses the formula above. Both appear when a SLAM system drops old poses (marginalization) or fixes some variables at known values (conditioning), see M11-T3: Factor Graphs.

Where it breaks

Everything above assumes independent Gaussian errors and linear relations. Two estimates that share an error (they were derived from the same data) are not independent, and fusing them as if they were is the double counting of M3-T3: Independence and Conditional Independence. For a nonlinear measurement the formulas hold only for its tangent (M4-T4: Propagating Uncertainty Through Transformations), which is the extended Kalman filter.

Common misconceptions

Key takeaways

1. Fusing two independent Gaussian estimates adds their precisions (1/σ²) and takes the precision-weighted mean: μ = μ₁ + K(μ₂ − μ₁), K = σ₁²/(σ₁² + σ₂²), σ² = (1 − K)σ₁².
2. The fused variance is never larger than either input. A poor sensor is not harmful but nearly useless; a mis-stated one is harmful.
3. The product of two Gaussian densities is a Gaussian (complete the square): this is why a Gaussian prior and a Gaussian likelihood give a Gaussian posterior.
4. Conditioning a joint Gaussian on z gives mean μx + ΣxzΣzz⁻¹(z − μz) and covariance Σxx − ΣxzΣzz⁻¹Σzx. For scalars the variance shrinks by 1 − ρ².
5. Fusion is a special case of conditioning, and conditioning with z = Hx + v is the Kalman update. A measurement improves every quantity correlated with it.

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 , chapters “Primer on Probability Theory” (joint, marginal and conditional Gaussians) and “Linear-Gaussian Estimation”. Free to read as the author’s PDF; the book is copyrighted.
• S. Särkkä and L. Svensson, Bayesian Filtering and Smoothing, 2nd edition, Cambridge University Press, 2023 , chapter 2 (Bayesian inference, with the Gaussian conditioning lemma). Free to read for personal use; no redistribution or derivative works.
• C. Stachniss, “Kalman Filter & EKF” (video lecture) , the derivation from conditioning. Free to watch.
• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, section 3.2 (the Kalman filter, with the Gaussian product). Paid book.
• K. P. Murphy, Probabilistic Machine Learning: An Introduction, MIT Press, 2022 , chapter 3 (Gaussian marginals and conditionals). Free to read as the draft PDF, licence CC BY-NC-ND.

Knowledge check

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

What’s next

Module 4 is complete: you can now describe, transform, combine and condition uncertainty. Module 5 builds the two probability models every robot filter needs: how its pose changes when it moves, and what its sensors read from a given pose.

Related content

Nothing below is required to finish this lesson.

• Before
  M4-T4: Propagating Uncertainty Through Transformations

  Propagate a mean and covariance through a linear map, and through a nonlinear map with a Jacobian, and say when the linear result is wrong.

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