Sensor Fusion and Object Tracking

The Kalman filter, introduced in Chapter 12, is the linear-Gaussian instance of the Bayesian filter and a workhorse for sensor fusion. We assume linear dynamics:

and a linear measurement model:

with a Gaussian belief $\text{bel}({𝒙}_t)\sim 𝒩({𝝁}_t,{\mathrm{\Sigma }}_t)$. Here $Q_t$ is the process-noise covariance and $R_t$ is the measurement-noise covariance, which may encode multiple sensors.

When fusing estimates from separate filters^††margin: For example, in track-to-track fusion, decentralized networks, or when reusing map priors. , cross-covariances are often unknown. Naively fusing them as if they are independent produces overconfident posteriors. A principled, conservative alternative is Covariance Intersection (CI), which combines Gaussian estimates without requiring cross-covariances. Given $({𝝁}_1,{\mathrm{\Sigma }}_1)$ and $({𝝁}_2,{\mathrm{\Sigma }}_2)$, CI defines:

with $\omega \in [0,1]$ chosen to minimize, for example, $\mathrm{tr}({\mathrm{\Sigma }}_{\text{CI}})$ or $\log det{\mathrm{\Sigma }}_{\text{CI}}$. CI guarantees consistency irrespective of the true, unknown correlation structure and will reappear in our discussion of distributed fusion in Section 15.4.

In summary, the Bayesian lens turns the problem of sensor fusion into a precise algebra over beliefs and likelihoods. Linear-Gaussian assumptions yield closed-form Kalman filters with simple stacked updates and an additive information form, mild nonlinearities are handled by EKF/UKF variants, and practical wrinkles such as biases, asynchrony, and unknown correlations are addressed with state augmentation, smoothing, and conservative fusion. We build on this foundation in the next sections.

Before any fusion can succeed, measurements must be made commensurate with per-sensor intrinsics^††margin: Such as a camera’s focal length and distortion, an IMU’s scale factors and axis misalignment, or a radar’s range/Doppler scaling. and extrinsics^††margin: The rigid transform from each sensor frame to the body frame and, where needed, from the body to a global frame. . Calibration is not a one-time event: temperature changes, mechanical wear, and firmware updates can shift sensor parameters over time. A filter that appears increasingly overconfident often points to registration that has drifted out of spec.

A useful way to understand the effect of small miscalibration is to linearize it. If a measurement depends on the transform from body $B$ to sensor $S$, denoted ${}_{}{}^{S}T_B^{}$, through some function $h$, then around a nominal ${}_{}{}^S\overline{T}_B^{}$ we can write:

where $𝜼$ is a small pose perturbation, $J_T$ is its sensitivity, and $𝜹$ is sensor noise. This first-order view suggests two complementary strategies:

• •

  Treat extrinsics as known but uncertain: inflate the measurement covariance by $J_T{\mathrm{\Sigma }}_T{J}_T^{\top }$, where ${\mathrm{\Sigma }}_T$ encodes extrinsic uncertainty. This preserves consistency without changing the state.

• •

  Augment the state with a minimal parameterization of the extrinsic and assign it a slow random-walk prior. The filter then learns small misalignments online rather than forcing other states to absorb them.

Sensors almost never report in lockstep time. They are clocked by different oscillators, traverse different processing pipelines, and communicate through different buses. The filter, however, should apply each measurement at the time it was taken. Accurate measurement timestamps are therefore critical; arrival times are, at best, indirect clues.

Hardware triggers or disciplined clocks, such as those using Network Time Protocol (NTP) or Precision Time Protocol (PTP), reduce relative drift. Additionally, known fixed latencies, such as a camera’s ISP delay, should be subtracted so that time stamps reflect exposure times, not the end of the processing chain. When an update is due at an intermediate time $\tau$ between filter steps, the state should be interpolated to $\tau$ before applying the measurement. For inertial navigation, IMU data can be preintegrated over $(t_i,t_j]$ to produce a relative-motion pseudo-measurement consistent with $SE(3)$ geometry, removing the need to resample at the IMU rate.

On a typical platform, the IMU runs at hundreds of Hertz, cameras at a few tens, lidar somewhere in between, and GNSS in single digits. The Kalman family naturally accommodates this: we predict forward to the next measurement time, and we update whenever a packet arrives.

In software, it is convenient to maintain a priority queue keyed by measurement time. The filter repeatedly advances (predicts) to the earliest timestamp, next applies^††margin: Stacking multiple measurements with the same stamp into a single block so that the innovation covariance $S_t$ reflects their joint effect. all measurements with that timestamp, and then repeats. This simple discipline—“predict to the stamp, then stack at the stamp”—prevents subtle double counting and preserves the meaning of NIS tests used later.

Even with careful time stamping, networks reorder and delay packets. A classical example is GNSS delivered over a congested channel, where a position measurement computed at time $k-\mathrm{\ell }$ may arrive at time $k$. Applying the measurement at the head of the filter timeline warps the uncertainty and can cause visible jumps in the estimate. The remedy is to keep a short history and re-solve the portion of the problem that lies within that window. A high level procedure for fixed-lag Rauch–Tung–Striebel (RTS)²²22. Rauch, Herbert E, Tung, F, Striebel, Charlotte T. “Maximum likelihood estimates of linear dynamic systems.” AIAA journal 3(8), 1445–1450, 1965. smoothing for out-of-sequence measurements is:

1. 1.

   Fix a lag $L$ and maintain a buffer of estimates ${\{{𝝁}_{j|j},{\mathrm{\Sigma }}_{j|j}\}}_{j=k-L}^k$ together with the dynamics $(A,Q)$. Suppose a measurement $({𝒛}_{k-\mathrm{\ell }},C,R)$ with $0\le \mathrm{\ell }\le L$ arrives.

2. 2.

   Run an RTS backward pass from $k$ back to $k-\mathrm{\ell }$ to compute smoothed estimates $\{{𝝁}_{j|k},{\mathrm{\Sigma }}_{j|k}\}$ (and, if needed, cross-covariances).

3. 3.

   Insert the update at $k-\mathrm{\ell }$ using ${𝝁}_{k-\mathrm{\ell }|k}$ as the prior.

4. 4.

   Re-propagate forward from $k-\mathrm{\ell }$ to $k$, re-applying any later measurements in the buffer.

5. 5.

   Commit the corrected states.

The lag $L$ should exceed typical delay jitter. If a packet falls outside the window, we can either discard it or assimilate it approximately with an inflated, time-shifted update.

Fusion is not only about more data, it is about honest information. Measurement streams may be correlated because they share process noise^††margin: For example, the sensors are mounted on the same body. , because they reuse the same map or prior, or because one stream already incorporates measurements from the other. If cross-covariances are known, they can be modeled explicitly. Often they are not, and fusing as if streams were independent yields overconfident covariances: NIS and NEES statistics will then fail ${\chi }^2$ checks.

Several responses are available:

• •

  When possible, exchange innovations (residuals and their covariances) rather than full posteriors, since innovations are closer to independent across nodes.

• •

  When that is insufficient, conservative fusion methods such as Covariance Intersection (Section 15.2.2) combine Gaussians without any knowledge of cross-covariances and guarantee consistency at the price of modest optimality.

• •

  In all cases, monitor empirical NIS/NEES against theoretical quantiles and treat persistent deviations as feedback that some correlation, timing error, or noise model has been overlooked.

Even a well-registered, well-timed system must protect itself against bad data. Kalman filters already provide a convenient diagnostic: the innovation $\widetilde{𝒛}$ and its covariance $S$ define the NIS ${\widetilde{𝒛}}^{\top }{S}^{-1}\widetilde{𝒛}$, which under a correct model follows a ${\chi }^2$ distribution of known dimension. Setting a gate at a chosen significance level rejects gross outliers, and tracking moving averages of the NIS per sensor exposes slow degradation.

When a sensor begins to misbehave, there are two complementary reactions:

• •

  Innovation-based adaptive estimation: inflate that sensor’s measurement covariance $R$ according to a feedback law derived from recent NIS so that the filter automatically down-weights it.

• •

  Structural response: if the issue resembles a bias or scale error more than random noise, augment the state with the offending parameter and let the filter learn it.

In systems with redundancy^††margin: Two or more sensors nominally measuring the same quantity. , it is natural to maintain a per-sensor health score and to prefer the healthier stream while keeping the others alive for fault detection and graceful recovery.

A few numerical practices are particularly useful. First is the use of square-root filters, which propagate a Cholesky factor of $\mathrm{\Sigma }$ to be more stable than propagating $\mathrm{\Sigma }$ directly, and help prevent negative-definite covariances due to round-off. Second, when updating the covariance we can use the Joseph stabilized form:

which preserves positive semidefiniteness in finite precision. On the monitoring side, plotting NIS/NEES histograms with ${\chi }^2$ overlays, tracking the determinant and condition number of $\mathrm{\Sigma }$, and logging gate hit-rates and stamp-minus-arrival statistics provide early warnings for timing and calibration regressions.

Centralized designs are conceptually simplest: raw measurements or lightly processed features are transported to a single estimator that maintains the posterior over the state. Mathematically, this is just the stacked update in Section 15.2.1, where at time $t$ we form:

compute the innovation ${\widetilde{𝒛}}_t$ and its covariance $S_t$, and apply a single Kalman update. Centralized fusion is statistically efficient and gives the clearest path to proper gating, bias handling, and OOSM smoothing (the referenced section).

Its limitations are operational rather than mathematical. Communicating raw sensor streams consumes bandwidth, creates central bottlenecks and single points of failure, and can be awkward when sensors live on different platforms, such as across vehicles and infrastructure. A common compromise is to centralize only within a platform or robot, fusing camera/lidar/radar/IMU locally and exposing higher-level artifacts—detections, tracks, occupancy—to the outside world.

In a decentralized system, each subsystem or platform runs its own filter and publishes summarized beliefs—typically a state estimate with covariance and, possibly, a timestamp and a health score. A fusion node then combines these “tracks”²³23. Zhang, Shuo, Bar-Shalom, Yaakov, Watson, Gregory. “Tracking with multisensor out-of-sequence measurements with residual biases.” In 2010 13th International Conference on Information Fusion, 1–8, 2010.. The main question is how to fuse them without breaking consistency.

If the contributing tracks are conditionally independent given the true state^††margin: For example, if they use disjoint raw measurements. , fusion is straightforward in information form. If a local node transforms a common prior $(Y^{-},{𝐲}^{-})$ into $(Y^{+},{𝐲}^{+})$, it can transmit its increment:

and the fusion node simply adds the increments from all sources:

In the linear-Gaussian case, these increments equal $C^{\top }{R}^{-1}C$ and $C^{\top }{R}^{-1}𝒛$ for that node’s local measurements. This “common-prior + increment” view keeps the algebra exact, but it requires that all nodes agree on (or communicate) the prior against which those increments were formed.

When two tracks are not independent—because they share process noise, maps, or each other’s measurements—their cross-covariance is generally unknown. Fusing as if independent is then dangerous (Section 15.3.5). A safe default is Covariance Intersection (Section 15.2.2), which guarantees consistency at the cost of some conservatism²⁴24. Julier, Simon J, Uhlmann, Jeffrey K. “A non-divergent estimation algorithm in the presence of unknown correlations.” In Proceedings of the 1997 American Control Conference (Cat. No. 97CH36041), 2369–2373, 1997.. If cross-covariances are known^††margin: For example, in a carefully engineered multi-radar system with a common process model. , we can compute the optimal best linear unbiased estimator (BLUE) weights using the joint covariance of the two track estimates. In practice, the engineering overhead to maintain those cross-terms often outweighs the marginal gain over CI.

Truly distributed systems have no fusion center. Instead, peers exchange messages over a communication graph and attempt to agree on the posterior. The information form makes this natural: at each time step, node $i$ computes its local increment $(\mathrm{\Delta }{Y}_i,\mathrm{\Delta }{𝐲}_i)$ from its own measurements, then the network mixes these contributions so that everyone converges to the same sum.

One simple version is average consensus on information increments using a symmetric, doubly stochastic mixing matrix $W=[w_{ij}]$ that respects the communication graph. Assume all nodes start from the same prior $(Y^{-},{𝐲}^{-})$. Each node initializes its local consensus state from its own increment:

Then perform $r$ rounds of neighbor averaging:

When the graph is connected and $W$ is well chosen, ${\widetilde{\mathrm{\Delta }Y}}_i^{(s)}$ and ${\widetilde{\mathrm{\Delta }𝐲}}_i^{(s)}$ converge to the network-wide averages of the increments. An exact centralized posterior is then recovered by multiplying by the number of nodes $n$:

In directed or time-varying graphs, push-sum or diffusion variants play the same role. In practice, a small number of rounds^††margin: Even $r=1$ or $2$. often captures most of the benefit.

Correlation again requires care. If nodes are connected through shared process models or re-used features, repeated neighbor mixing can re-inject the same evidence multiple times. Two mitigations are common. First, we can exchange innovations rather than full posteriors, since these tend to be closer to independent across nodes. Second, we can replace consensus averaging with CI-consensus: combining neighbors’ information conservatively when independence is doubtful so each node’s covariance remains an upper bound on its true error.

Architectures are inseparable from communication budgets. A useful mental model is to choose a bandwidth tier:

• •

  Raw or ROI data^††margin: For example, point clouds or image crops. : maximal accuracy and flexibility, but highest bandwidth, strict latency requirements, and potential privacy concerns.

• •

  Features or bird’s-eye-view (BEV) grids: a strong trade-off in modern stacks; semantics are preserved, bandwidth is moderate, and time alignment can be handled at the feature level.

• •

  Decisions (detections, tracks, occupancy): minimal bandwidth and simplest to distribute; best suited to decentralized and distributed fusion, but with the least flexibility for correcting upstream errors.

Two additional practicalities:

• •

  Messages should carry uncertainty (covariances or credible intervals), not just point estimates, to enable principled fusion at the receiver.

• •

  Timing matters as much as content: include measurement timestamps, not just send times, so that receivers can place information correctly on their own timelines (the referenced section).

We model each target $k$ at time $t$ with a state vector ${𝒙}_t^k$ and a measurement model that connects states to observed quantities. A common starting point for ground vehicles is a constant-velocity (CV) model in the plane:

with white-acceleration process noise:

For targets that turn, a coordinated-turn (CT) model augments the state with a turn rate $\omega$ and typically uses an EKF or UKF update. A constant-acceleration (CA) model can be more appropriate for pedestrians or stop-and-go traffic. The choice of model trades fidelity for simplicity; a good practice is to begin with CV and only move to CT or CA if innovation statistics suggest a benefit.

Measurements take many forms. A radar may report range $r$, bearing $\varphi$, and sometimes radial velocity $\dot{r}$:

Camera detections may provide image-space bounding boxes and, if depth is available, 3D centroids in the ego frame. Lidar clusters similarly yield centroids and sometimes yaw estimates. We fold these into linear or linearized measurement models and apply the Kalman-family update from Section 15.2.1 for each track.

Before tackling the combinatorics of association, we narrow the search space with gating. Given a predicted measurement $\overline{𝒛}$ for a track and its innovation covariance $S$, a detection $𝒛$ is deemed compatible if the (squared) Mahalanobis distance:

lies below a threshold from the ${\chi }^2$ distribution at a chosen confidence^††margin: For example, $\alpha =0.99$ for a conservative, wide gate. . This is exactly the NIS concept from Section 15.3; under the model, $m^2$ follows ${\chi }_d^2$ for measurement dimension $d$.

Gating dramatically reduces the clutter presented to the assignment solver: it retains almost all true matches while excluding most spurious ones.

After gating, we must decide which detection updates which track. The simplest approach is Global Nearest Neighbor (GNN): we define a cost matrix $C_{ij}$ between track $i$ and detection $j$ (often using Mahalanobis distance), and solve the 2-D assignment problem with the Hungarian algorithm. Algorithm 2 summarizes a standard gated GNN pipeline with gating and occlusion handling. Unassigned tracks receive missed-detection events; unassigned detections may seed new tracks.

GNN is fast and effective in relatively unambiguous scenes, but it commits to a single hypothesis even when multiple associations are plausible. Two families of methods hedge this ambiguity.

Bookkeeping is a critical function for tracking modules. Good track management balances eagerness to explain new detections with skepticism that avoids proliferating false tracks.

When the number of targets varies and clutter is heavy, it is natural to treat the set of objects as the fundamental random variable. RFS filters propagate distributions over sets rather than over a fixed list of tracks.

Modern trackers often follow a tracking-by-detection paradigm, where a perception module (classical or learned) produces frame-wise detections with uncertainties, a motion model predicts track states, and an association layer links detections to tracks. In camera-centric systems, learned appearance embeddings significantly improve identity stability. In lidar/radar-centric systems, motion and geometry dominate, and appearance plays a smaller role.

Two themes recur:

1. 1.

   Uncertainty matters. Downstream gating and data association work much better when the detector exports calibrated confidence and geometric covariance (or a proxy such as a covariance in BEV cells). Calibrate these heads before fusion, for example with temperature scaling for classification and reliability diagrams for regression.

2. 2.

   Robustness to missing modalities. In adverse weather or partial failures, radar or thermal imaging may carry the burden. Design the tracker to operate with a subset of sensors by inflating the covariance of the missing modality, adjusting initiation thresholds, and tuning $T_{\text{miss}}$ accordingly.

Tracking quality is multi-faceted: we care about detection quality, geometric accuracy, and identity preservation. Three widely used metrics capture different aspects:

Classical fusion, as in Section 15.2.1, stacked raw measurements and operated directly on likelihoods. This remains the gold standard when bandwidth is ample and calibration is impeccable. Modern stacks, however, often benefit from two additional levels that better reflect computational and communication realities.

The BEV idea is conceptually simple but rich in practice. We represent the local scene as a grid aligned with the ground plane and ego pose, and let each modality contribute to that grid in the way most natural for it. The payoff is a shared canvas on which geometry, semantics, and motion can be reasoned about jointly.

Learned modules speak in logits, heatmaps, and BEV tensors; filters expect pseudo-measurements with covariances. An adaptor translates between these languages so that the Bayesian machinery from earlier sections can operate on learned outputs without any sleight of hand.

At its simplest, an adaptor takes a detection $(\widehat{𝒑},{\mathrm{\Sigma }}_{\text{net}},s)$—a position, a covariance proxy, and a confidence—and turns it into a measurement $𝒛$ with covariance $R$ suitable for a KF/EKF update. Algorithm 3 shows a simple per-detection adaptor. The crucial step is calibration: mapping ${\mathrm{\Sigma }}_{\text{net}}$ to a covariance $R$ whose empirical NIS statistics match ${\chi }^2$ quantiles on held-out data. Once calibrated, learned detections and classical sensors inhabit the same probabilistic currency.

A single vehicle’s field of view is limited by its own geometry and occluders such as other vehicles and buildings. By contrast, an intersection camera may “see around corners,” and neighboring vehicles can reveal what an ego vehicle cannot. Cooperative perception asks three questions: what should we send, when should we send it, and how should the receiver fuse it while maintaining honest uncertainty?

Filters and networks are complementary rather than competing tools. Three hybrid patterns recur in modern systems.

Neural modules are powerful, but their confidence estimates are often miscalibrated, and fusion depends on honest uncertainty to avoid gates admitting outliers and associations overcommitting. Three classes of tools help keep confidence in line with reality: calibration, deep ensembles (or evidential models), and conformal prediction.

Two additional practical issues round out the learning-era picture.