M3-T11: Multi-sensor fusion architectures

Learning objectives

After this lesson, you should be able to:

• Compare centralised, federated and factor-graph fusion.
• Explain double counting when combining correlated estimates.
• Apply covariance intersection to fuse estimates with unknown correlation.
• Choose an architecture for a design with fault-isolation requirements.

Conceptual explanation

Centralised fusion feeds every raw measurement to one filter: optimal and consistent when modelled correctly, but one fault can contaminate everything and complexity grows. Federated (decentralised) fusion runs local filters per sensor group and combines their outputs; isolation is easy, but local estimates share the same IMU and process noise, so they are correlated. Factor-graph smoothing optimises over a window of states with factors from every sensor, handling asynchrony and delayed data well at higher compute.

Engineering intuition

Two navigation filters both using the same IMU have errors that are correlated through the IMU noise. Averaging their outputs as if independent halves the variance in the books but not in reality: the combined covariance is too small, the filter is overconfident. Covariance intersection (CI) gives a fused estimate that is consistent for any correlation, at the price of being more conservative than the optimal fusion when the estimates really are independent.

Mathematical formulation

Naive fusion of two independent estimates $(x_{1},\,P_{1})$ and $(x_{2},\,P_{2})$ in information form:

Covariance intersection for unknown correlation, with weight $\omega \in [0,\,1]$ chosen to minimise, for example, the trace or determinant of $P$:

CI is guaranteed consistent ($P$ ≥ true error covariance) whatever the cross-correlation. When one estimate carries the other’s information already (a VIO built on the same IMU as the autopilot’s EKF), CI is the safe combination; for independent estimates it is conservative.

$\omega$

weight of the first estimate (scalar), chosen by a one-dimensional search.

$P_{i}$

covariance of estimate $i$.

Architecture and data flow

In the reference architecture the IMU-driven fusion estimator on the companion combines VIO, LIO and GNSS (centralised or factor-graph), and its output enters PX4’s EKF2 as external odometry: EKF2 then holds a second estimator using the same IMU, a federated structure. Treat the external odometry covariance conservatively (inflate it) because EKF2’s own IMU propagation is correlated with it.

Real-world UAV applications

• Designing redundant navigation for fault isolation.
• Combining VIO from the companion with the autopilot’s estimate.
• Multi-vehicle cooperative localisation.
• Choosing between EKF and a factor graph for a research estimator.

Implementation example

This runs as shown: two estimates with correlated errors (shared process noise) are fused naively and with covariance intersection; the naive fusion is overconfident (NEES too high) and CI is consistent (NEES at most the expected value).

import numpy as np
rng = np.random.default_rng(0)
N = 20000
shared = rng.standard_normal((N, 2)) * 1.0                       # error component common to both estimates (e.g. from a shared IMU)
e1 = shared + 0.5 * rng.standard_normal((N, 2)); e2 = shared + 0.7 * rng.standard_normal((N, 2))
P1 = np.eye(2) * (1.0 + 0.25); P2 = np.eye(2) * (1.0 + 0.49)     # each estimate's true error covariance (shared + own)
true = np.zeros(2); x1, x2 = true + e1, true + e2

def fuse_naive(x1, x2, P1, P2):
    I1, I2 = np.linalg.inv(P1), np.linalg.inv(P2); P = np.linalg.inv(I1 + I2)
    return (P @ (I1 @ x1.T + I2 @ x2.T)).T, P
def fuse_ci(x1, x2, P1, P2):
    I1, I2 = np.linalg.inv(P1), np.linalg.inv(P2)
    w = min(np.linspace(0, 1, 101), key=lambda w: np.trace(np.linalg.inv(w * I1 + (1 - w) * I2)))   # minimise the trace of the fused covariance
    P = np.linalg.inv(w * I1 + (1 - w) * I2); return (P @ (w * I1 @ x1.T + (1 - w) * I2 @ x2.T)).T, P

def mean_nees(x, P): e = x - true; return np.mean(np.einsum('ij,jk,ik->i', e, np.linalg.inv(P), e))
xn, Pn = fuse_naive(x1, x2, P1, P2); xc, Pc = fuse_ci(x1, x2, P1, P2)
print(f"mean NEES (expected <= 2): naive {mean_nees(xn, Pn):.2f}, covariance intersection {mean_nees(xc, Pc):.2f}; fused std: naive {np.sqrt(Pn[0, 0]):.2f}, CI {np.sqrt(Pc[0, 0]):.2f}")
assert mean_nees(xn, Pn) > 2.5 and mean_nees(xc, Pc) <= 2.05                  # naive: overconfident; CI: consistent (conservative)
assert np.sqrt(Pc[0, 0]) > np.sqrt(Pn[0, 0])                                  # CI admits the shared error that the naive fusion hides

Common engineering mistakes

Failure modes

Debugging approach

1. Run the Monte Carlo NEES test on the fused output (lesson M3-T15).
2. Check whether any two estimators share an input (IMU, clock, map).
3. Inject a sensor fault and see which architecture isolates it.
4. Log the covariance before and after each fusion step.

Practical exercises

1. Show that CI never claims a covariance smaller than the best individual estimate allows when the correlation is worst-case.
2. Compare CI with the optimal correlated fusion when the cross-covariance is known.
3. Decide the architecture for a vehicle with two independent VIO systems.
4. Explain how external odometry into EKF2 is a federated structure.

Interview questions

Write an answer in your own words, then reveal the model answer. Nothing is sent anywhere.

References and further reading

Primary papers, official documentation and standards. Each claim above that is specific to a tool or version points here.

• S. Julier, J. Uhlmann, “A non-divergent estimation algorithm in the presence of unknown correlations”, ACC 1997
• $Y$. Bar-Shalom, $X$. $R$. Li, $T$. Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley, 2001, chapter 8.
• $F$. Dellaert, $M$. Kaess, “Factor graphs for robot perception”, Foundations and Trends in Robotics 6(1–2), 2017.

What’s next