Sensor Fusion

Fusion is the same predict-update cycle as a single-sensor filter, just with one update per incoming measurement stream: predict the state forward, then apply an update for whichever sensor's reading just arrived, each with its own $R_i$.

Asynchronous rates (IMU at 200 Hz, GPS at 1 Hz, camera at 30 Hz) fall out naturally — you only update with a sensor when its data lands.

A sensor with small $R_i$ pulls the estimate hard toward its reading; large $R_i$ lets the prediction dominate. If a sensor's true noise is worse than its stated $R_i$, the filter over-trusts it and the estimate becomes overconfident and eventually wrong — tuning $Q$ and $R$ correctly matters as much as the mean equations.

Before accepting a measurement, check its Mahalanobis distance against the predicted state:

$d^2 = (z - h(x))^\top S^{-1} (z - h(x))$

where $S$ is the innovation covariance. Compare $d^2$ against a chi-squared threshold for the measurement dimension; reject or down-weight measurements that fail the test instead of letting a spurious reading corrupt the fused estimate.

Between GPS updates, the filter keeps predicting from IMU/odometry alone — accurate short-term but drifting, since gyro and accelerometer biases integrate into unbounded position error over time.

Each new GPS fix (or loop closure, or map match) resets that accumulated drift; fusion is precisely what makes the combination outperform either sensor run alone.

Every sensor measures in its own frame at its own clock. Fusion requires a known extrinsic transform between sensor frames (and the body frame) and either hardware timestamp sync or interpolation/buffering to align measurements to a common time base — get either wrong and the fused estimate is consistently biased, not just noisy.