"You gave me five sensors and one truth. Do not be surprised that I insist on adding their information rather than averaging their opinions."
A Careful AI Agent
Prerequisites
This section assumes the single-sensor Kalman filter, its predict-and-update loop, and the meaning of the state covariance \(P\), all developed in Chapter 9. It builds directly on the covariance-weighted fusion rule of Section 49.1, and it leans on Gaussian conditioning from the estimation primer in Chapter 4. Here we take the filter you already know and ask a single new question: what changes when many sensors, with different models and different rates, all report on one state?
The Big Picture
A Kalman filter maintains one belief about one hidden state, a Gaussian summarized by a mean \(\hat{x}\) and a covariance \(P\). Multi-sensor fusion is not a new algorithm layered on top; it is the same filter with more than one measurement model feeding the same update step. The prediction stage does not change at all, because the physics of the state evolves regardless of who is watching. The update stage simply runs once per sensor, each with its own observation matrix \(H_i\) and its own noise covariance \(R_i\). The deep result of this section is that the answer does not depend on the order in which you fold the sensors in, and that in the right coordinates fusion becomes plain addition. Understand that, and a GPS-plus-IMU-plus-wheel-odometry stack stops looking like a systems-integration nightmare and starts looking like arithmetic.
One state, many measurement models
Keep the linear-Gaussian state-space model from Chapter 9: the state evolves as \(x_k = F x_{k-1} + w_k\) with process noise \(w_k \sim \mathcal{N}(0, Q)\). The only structural change for multiple sensors lives in the measurement equation. Instead of one observation, sensor \(i\) reports \(z_k^{(i)} = H_i x_k + v_k^{(i)}\) with \(v_k^{(i)} \sim \mathcal{N}(0, R_i)\). Each sensor gets its own \(H_i\), because each sensor sees a different function of the state. A GPS receiver observes position, so its \(H\) picks out the position components. A wheel encoder or Doppler radar observes speed, so its \(H\) picks out velocity. A magnetometer observes heading. The state they all report on is the same; the linear map from that state to what each device actually measures is not.
The why of a shared state is the whole point of fusion: the position estimate improves when a velocity sensor updates the filter, because \(F\) couples velocity to future position, so tightening velocity tightens tomorrow's position. A pile of independent per-sensor filters can never do this, since none of them holds the joint belief that lets one sensor's evidence flow into another's quantity. That coupling is exactly the complementary-sensor argument from Chapter 48, now made quantitative.
Two ways to fold in many sensors: stacked and sequential
Given several sensors that all report at the same instant \(k\), you have two mathematically equivalent options. The stacked (batch) update concatenates every measurement into one tall vector \(z = [z^{(1)}; \dots; z^{(m)}]\), stacks the models into \(H = [H_1; \dots; H_m]\), and forms a block-diagonal noise \(R = \mathrm{diag}(R_1, \dots, R_m)\). Then you run the ordinary Kalman update once with these big matrices. The block-diagonal \(R\) encodes a real assumption: that the sensors' noises are mutually independent given the state. When that holds (usually because the devices are physically separate), stacking is exact.
The sequential update instead processes the sensors one at a time. Start from the predicted belief, apply sensor 1's update to get an intermediate posterior, then treat that as the prior for sensor 2, and so on. Because each update uses only \(H_i\) and \(R_i\) of a single device, the matrix you invert is the size of one sensor's output, not the whole stack. For \(m\) scalar sensors this replaces one \(m \times m\) inversion with \(m\) scalar divisions, which is both cheaper and numerically kinder. Crucially, when the noises are independent the sequential result is identical to the stacked result, to floating-point rounding. Sensor order does not matter.
Key Insight
Order-independence is not a lucky coincidence; it is what makes fusion trustworthy. Each Kalman update is a Bayesian conditioning step, and conditioning on independent pieces of evidence is commutative: \(p(x \mid z_1, z_2) = p(x \mid z_2, z_1)\). So you may ingest a GPS fix now and an IMU-derived pseudo-measurement a millisecond later, or the reverse, and land on the same posterior. This is the license that lets a real system accept sensors asynchronously, whenever each one happens to produce a sample, without maintaining a rigid combined frame. If ever the order does change your answer, that is a red flag that your noises are correlated and your block-diagonal \(R\) is a lie.
The information form, where fusion becomes addition
Sequential updating hints at something cleaner, and the information (inverse-covariance) filter delivers it. Represent the belief not by \((\hat{x}, P)\) but by the information matrix \(Y = P^{-1}\) and information vector \(y = P^{-1}\hat{x}\). In these coordinates the measurement update for sensor \(i\) is simply
\[ Y^{+} = Y^{-} + H_i^\top R_i^{-1} H_i, \qquad y^{+} = y^{-} + H_i^\top R_i^{-1} z^{(i)}. \]Fusing \(m\) independent sensors is therefore nothing but summing their \(m\) information contributions onto the prior:
\[ Y^{+} = Y^{-} + \sum_{i=1}^{m} H_i^\top R_i^{-1} H_i, \qquad y^{+} = y^{-} + \sum_{i=1}^{m} H_i^\top R_i^{-1} z^{(i)}. \]The why this matters is threefold. First, each term \(H_i^\top R_i^{-1} H_i\) is the Fisher information a sensor contributes, so a precise sensor (small \(R_i\)) adds a lot and a noisy one adds a little, automatically, with no hand-tuned weights. This is the covariance-weighting principle of Section 49.1 in its most transparent dress. Second, summation is trivially order-independent and trivially parallel, which is why decentralized and multi-robot fusion systems live in information space: each node computes its own contribution and the fusion center just adds them. Third, a sensor that measures nothing about part of the state contributes zero information there, so the form degrades gracefully when a sensor drops out, you simply stop adding its term.
In the field: a delivery robot fusing GPS and wheel odometry
A sidewalk delivery robot tracks its planar state \(x = [p, v]^\top\) (position and velocity along a path segment). A GPS receiver reports position at 5 Hz with a standard deviation of about 2.5 m, and wheel odometry reports speed at 50 Hz with a standard deviation of about 0.1 m/s. These have different rates, different units, and different observation matrices: \(H_{\text{gps}} = [1, 0]\) and \(H_{\text{odo}} = [0, 1]\). The team's first instinct was to average the two into a position estimate, which is nonsense, since one of them does not measure position at all. The correct design runs a single filter whose predict step advances at the fast 50 Hz odometry clock and whose update step fires whichever measurement just arrived: an odometry sample tightens velocity ten times more often than GPS arrives, and because \(F\) couples velocity into position, the position covariance stays small between the sparse, noisy GPS fixes. When the robot rolls under a bridge and GPS drops for eight seconds, the filter keeps dead-reckoning on odometry alone (the mechanism of Chapter 24), its position covariance visibly inflating until the next fix snaps it back. No sensor was averaged; each simply added the information it had.
Asynchronous sensors and out-of-sequence measurements
Real sensors do not agree on a clock. They sample at different rates, and packets arrive late over a bus or a network. The predict-then-update structure handles the first problem naturally: predict the state forward to the timestamp of whatever measurement just arrived, apply that one sensor's update, and wait for the next. Because prediction is just propagating the Gaussian through \(F\) and adding \(Q\), you can advance to any timestamp, so different rates need no special machinery beyond honest timestamps, which is precisely why the synchronization discipline of Chapter 3 is load-bearing here: a measurement folded in at the wrong time is worse than one dropped.
The harder case is an out-of-sequence measurement, a sample whose true timestamp is older than the filter's current time, common with GNSS fixes that take a second to compute (the positioning pipelines of Chapter 25). You cannot simply apply it now, because the state has already moved past its epoch. The principled fix is to keep a short buffer of recent states, roll back to the measurement's epoch, apply the update there, and re-propagate forward, or to use a dedicated out-of-sequence update that corrects the current state without a full replay. Ignoring the staleness and applying the fix at the current time injects a lag-shaped bias that no amount of tuning removes.
Listing 49.2.1 implements the whole idea in a few lines: a shared predict step and a single generic update that any sensor calls with its own \(H\), \(R\), and \(z\). The same update serves GPS and odometry, which is the code-level proof that multi-sensor fusion is one filter with many measurement models.
import numpy as np
class MultiSensorKF:
def __init__(self, x, P, F, Q):
self.x, self.P, self.F, self.Q = x, P, F, Q
def predict(self, dt):
F = self.F(dt) # F may depend on the elapsed time
self.x = F @ self.x
self.P = F @ self.P @ F.T + self.Q(dt)
def update(self, z, H, R): # ONE update, any sensor supplies H, R, z
y = z - H @ self.x # innovation
S = H @ self.P @ H.T + R # innovation covariance
K = self.P @ H.T @ np.linalg.inv(S) # Kalman gain
self.x = self.x + K @ y
self.P = (np.eye(len(self.x)) - K @ H) @ self.P
return y, S # returned for consistency checks (Sec. 49.7)
# state = [position, velocity]; constant-velocity model
F = lambda dt: np.array([[1, dt], [0, 1]])
Q = lambda dt: np.array([[0.05*dt**3/3, 0.05*dt**2/2],
[0.05*dt**2/2, 0.05*dt]])
kf = MultiSensorKF(x=np.array([0., 1.]), P=np.eye(2)*10.0, F=F, Q=Q)
H_gps, R_gps = np.array([[1., 0.]]), np.array([[2.5**2]]) # measures position
H_odo, R_odo = np.array([[0., 1.]]), np.array([[0.1**2]]) # measures velocity
# a stream of timestamped, asynchronous measurements
stream = [(0.02, "odo", 1.03), (0.04, "odo", 0.98),
(0.20, "gps", 0.31), (0.22, "odo", 1.06)]
t_prev = 0.0
for t, kind, val in stream:
kf.predict(dt=t - t_prev); t_prev = t
if kind == "gps": kf.update(np.array([val]), H_gps, R_gps)
else: kf.update(np.array([val]), H_odo, R_odo)
print(f"t={t:.2f} {kind}: pos={kf.x[0]:.3f} vel={kf.x[1]:.3f} "
f"sigma_p={kf.P[0,0]**0.5:.3f}")update handles GPS (a position model) and odometry (a velocity model) with no per-sensor code; the predict step advances the belief to each measurement's timestamp before folding it in. Running it shows the position sigma shrinking after the GPS fix even though most samples are velocity-only, because the constant-velocity \(F\) couples the two.The Right Tool
Hand-rolling the matrix algebra above is fine for two sensors, but the bookkeeping (Joseph-form covariance updates for numerical stability, per-sensor \(R\) registration, out-of-sequence handling) grows fast. The filterpy library reduces the core loop to a handful of lines and keeps the covariance symmetric and positive-definite for you:
from filterpy.kalman import KalmanFilter
kf = KalmanFilter(dim_x=2, dim_z=1)
kf.F = np.array([[1., 0.02], [0., 1.]]); kf.Q = Q(0.02)
kf.x = np.array([0., 1.]); kf.P *= 10.
# fuse two sensors by calling update() with each sensor's H and R
kf.predict()
kf.update(z=0.31, H=np.array([[1., 0.]]), R=np.array([[2.5**2]])) # GPS
kf.update(z=1.06, H=np.array([[0., 1.]]), R=np.array([[0.1**2]])) # odometryfilterpy, cutting roughly 30 lines of gain-and-covariance algebra to about 6 and supplying stabilized (Joseph-form) updates. The library removes the arithmetic; deciding each sensor's \(H\) and \(R\), the modeling work of this section, is still yours.As Listing 49.2.2 shows, the library owns the numerics but not the physics: choosing what each sensor observes and how much to trust it is the design decision the tool cannot make for you.
Research Frontier
The linear multi-sensor filter assumes you already know each \(R_i\), yet real sensor noise drifts with temperature, motion, and multipath. Adaptive and learned-noise filters estimate \(R_i\) online, and differentiable Kalman filters (KalmanNet and related recurrent designs) train a network to supply the gain or the noise when \(F\) and the true \(R\) are unknown, blending the filter's structure with data-driven flexibility. For strongly nonlinear \(H_i\) (bearing-only or range-only sensors), the extended and unscented variants and the particle methods of Chapter 10 replace the linear update while keeping this section's multi-sensor bookkeeping unchanged.
Exercise
Extend Listing 49.2.1 with a third sensor: a noisy accelerometer that measures the derivative of velocity, so you must grow the state to \([p, v, a]\) and give the accelerometer \(H = [0, 0, 1]\). Then verify the order-independence claim empirically: at a single timestamp, feed the three sensors in all six orders and confirm the posterior mean and covariance agree to floating-point precision. Finally, deliberately correlate two sensors' noise (share a bias) and show that the six orderings now disagree, demonstrating why the block-diagonal \(R\) assumption is the thing that buys you commutativity.
Self-Check
1. A velocity-only sensor updates the filter, yet the position estimate improves. Which matrix is responsible for propagating velocity evidence into position, and at which step does it act?
2. In information form, fusing sensors is addition of \(H_i^\top R_i^{-1} H_i\) terms. Why does a noisier sensor (larger \(R_i\)) automatically contribute less, without any manual weighting?
3. What assumption makes the sequential update give exactly the same posterior as the stacked update, and what observable symptom appears in your filter when that assumption is violated?
What's Next
This section fused sensors that report on a low-dimensional state like position and velocity. But many sensing problems, a robot mapping a room, a car understanding free space, are about where things are in the world, a state far too large to carry as one Gaussian. In Section 49.3, we meet occupancy grids: a way to fuse many range measurements into a spatial map of what is occupied and what is free, trading the single-Gaussian belief for a grid of per-cell probabilities updated in log-odds.