Robust and adaptive filtering

"My filter trusted every measurement equally, so the day a satellite reflected off a glass tower, my careful centimeter-accurate position leapt forty meters into a river. I now assume that some of what I am told is a lie, and I have never drowned since."

A Skeptical AI Agent

The innovation is your lie detector

What makes robustness possible is that a filter always has a second opinion. Before it sees measurement \(\mathbf{z}_k\), it has already predicted what that measurement should be, \(\hat{\mathbf{z}}_{k}\), from the propagated state. The disagreement is the innovation (or residual):

where \(\mathbf{S}_k\) is the innovation covariance, the filter's own estimate of how large \(\boldsymbol{\nu}_k\) should be if everything is behaving. Why this is the load-bearing quantity: if the model is correct, the normalized innovation squared (NIS),

follows a chi-square distribution with degrees of freedom equal to the measurement dimension. That gives a calibrated, unit-free alarm. A three-sigma gate rejects a scalar measurement whenever \(\epsilon_k > 9\); for a two-dimensional measurement the 99th-percentile gate sits near \(\epsilon_k > 9.2\). How you use it splits into two questions this section answers in turn: what to do with a single measurement that fails the test (robustness), and what to do when the test fails too often, because your covariances themselves are wrong (adaptation and consistency).

Robustness: gating, Huber, and heavy tails

The bluntest tool is the chi-square gate: compute \(\epsilon_k\), and if it exceeds a threshold, discard the measurement entirely and skip the update (the predict step still runs, so the state coasts on the motion model). This is standard in target tracking and GNSS. Gating is cheap and effective for rare, gross outliers, but it is brittle at the boundary: a measurement at \(\epsilon_k = 8.9\) is fully trusted and one at \(9.1\) is fully ignored, and a run of borderline-bad data can starve the filter of any correction at all.

A gentler approach reshapes the update itself. The Kalman update is the solution to a weighted least-squares problem, and least squares is exactly what outliers exploit because the squared residual grows without bound. Replacing the quadratic loss with a Huber loss, quadratic for small normalized residuals and linear beyond a knee at \(c\) (typically \(c \approx 1.345\) standard deviations), caps the influence any one measurement can have. Concretely, you scale that measurement's effective information by a weight

so good measurements pass through untouched while a wild one is pulled back toward unit leverage rather than thrown away. This degrades gracefully where the hard gate snaps. The distributional view of the same idea replaces the Gaussian measurement model with a heavy-tailed Student-t, whose fat tails assign non-trivial probability to large residuals; a t-distributed filter automatically down-weights outliers as a consequence of its likelihood, and reduces to the ordinary Kalman filter as the degrees of freedom grow. The particle filter of Section 10.3 gets robustness almost for free here: you simply use a heavy-tailed likelihood when computing weights.

Adaptation: re-learning Q and R online

Gating assumes your covariances are right and only the data is occasionally wrong. The opposite failure is a filter whose \(\mathbf{Q}\) and \(\mathbf{R}\) were correct in the lab and are wrong in the field: a warm IMU, a fouled pressure port, a maneuvering target that your constant-velocity \(\mathbf{Q}\) never anticipated. Adaptive filtering estimates the noise statistics from the data stream itself. The most practical family is innovation-based or covariance-matching adaptation. Over a sliding window of length \(N\) you form the empirical innovation covariance

and compare it to what the filter predicted, \(\mathbf{S}_k\). If \(\hat{\mathbf{C}}_k\) is persistently larger, the world is noisier than assumed, and you inflate \(\mathbf{R}\) (measurement noise) or \(\mathbf{Q}\) (process noise) to match; if it is smaller, you can tighten them. A common lightweight rule inflates process noise directly from the measured excess, \(\hat{\mathbf{Q}}_k = \mathbf{K}_k \hat{\mathbf{C}}_k \mathbf{K}_k^{\top}\), using the Kalman gain \(\mathbf{K}_k\). When you reach for the heavier machinery: full maximum-likelihood or expectation-maximization estimation of the noise matrices is more principled and is the right tool offline (calibrating a sensor from a recorded log), but online you almost always prefer the cheap windowed estimator because it reacts in real time and never stalls a hard deadline.

Consistency: the diagnostic that ties it together

You cannot tune robustness or adaptation blind, so you need a scoring rule for whether a filter's uncertainty is honest. A filter is consistent when its actual errors match its self-reported covariance: the estimate is unbiased and the true error lands inside the \(\pm\)one-sigma band about as often as a Gaussian says it should. The operational test is the averaged NIS (from measurements, needs no ground truth) and, when you have truth in a controlled log, the normalized estimation error squared (NEES). Averaged over a window, either statistic should sit inside a chi-square confidence interval around its degrees of freedom. Above the band means overconfident (the classic cause of divergence and a symptom that \(\mathbf{Q}\) is too small); below means conservative and wasteful. This links directly to the calibration and coverage machinery of Chapter 18: a filter that fails its NIS test is an uncalibrated probabilistic model, and every downstream fusion step in Chapter 49 that trusts its covariance will inherit the error.

import numpy as np
from scipy.stats import chi2

def robust_adaptive_update(x_pred, P_pred, z, H, R, gate_p=0.997, win=None):
    """One EKF-style measurement update with a chi-square gate,
    Huber down-weighting, and windowed R adaptation."""
    nu = z - H @ x_pred                      # innovation
    S  = H @ P_pred @ H.T + R                # innovation covariance
    nis = float(nu.T @ np.linalg.solve(S, nu))
    dof = z.shape[0]

    # 1. Robust gate: reject gross outliers outright.
    if nis > chi2.ppf(gate_p, dof):
        return x_pred, P_pred, nis, "rejected"

    # 2. Huber weight: soften merely-noisy measurements (c = 1.345 sigma).
    c = 1.345 * np.sqrt(dof)
    w = min(1.0, c / np.sqrt(max(nis, 1e-9)))
    R_eff = R / w                            # down-weight = inflate R

    # 3. Standard update with the effective noise.
    S_eff = H @ P_pred @ H.T + R_eff
    K = P_pred @ H.T @ np.linalg.inv(S_eff)
    x_upd = x_pred + K @ nu
    P_upd = (np.eye(len(x_pred)) - K @ H) @ P_pred

    # 4. Adapt R only from this accepted innovation (windowed outside).
    if win is not None:
        win.append(np.outer(nu, nu))         # feed the covariance matcher
    return x_upd, P_upd, nis, "accepted"

The snippet above shows why the three ideas belong in one update: gating, Huber weighting, and adaptation are all just different transforms of the same innovation, applied in a deliberate order.

Switching models: when the dynamics change, not just the noise

Sometimes the mismatch is not a bad measurement or a wrong noise level but a wrong model: an aircraft that was cruising straight begins a hard turn, so your constant-velocity motion model is now structurally wrong for a few seconds. Inflating \(\mathbf{Q}\) is a crude patch. The principled answer is the Interacting Multiple Model (IMM) estimator, which runs a bank of filters (constant velocity, constant turn, constant acceleration), each with its own model, and maintains a probability that each model is currently active. A Markov transition matrix governs how those probabilities shift, and the fused estimate is a probability-weighted mix. IMM is the workhorse of air-traffic and automotive radar tracking precisely because it adapts the structure of the filter, not merely its scalars, and it hands you the model probabilities as an interpretable bonus (a rising "turn" probability is an early maneuver detector). It connects naturally to the change-detection methods of this part's Chapter 12, where the same regime shift is treated as a change point to be flagged rather than a mode to be tracked.

What's Next

In Section 10.7, we step back from individual tricks and ask the design question that governs this whole chapter: given a specific sensing problem, its nonlinearity, its dimensionality, its compute budget, and its failure modes, which estimator do you actually reach for? We turn the EKF, UKF, particle filter, Rao-Blackwellization, and the robust-adaptive machinery of this section into a decision guide.