"My lidar returned a perfect, confident map of a world that had not existed for eight hundred milliseconds. Nobody thought to ask it whether it was still awake."
A Chastened AI Agent
Prerequisites
This section builds directly on the fused proprioceptive-exteroceptive estimator of Section 57.4 and the body-state and exteroceptive channels of Sections 57.1 and 57.2. The core detection math is the innovation (residual) sequence of the Kalman family from Chapter 9; the change-point machinery comes from Chapter 12; and the calibrated-confidence tools appear in Chapter 18. If a robot must remain safe while degraded, read this alongside the functional-safety framing of Chapter 68.
The Big Picture
Every prior section in this chapter assumed the sensors were telling the truth. They will not always. A camera saturates in a tunnel exit, an IMU develops a thermal bias, a wheel spins on ice and lies about odometry, a ROS node crashes and republishes its last stale message forever. The dangerous case is not the sensor that goes silent, which is easy to notice; it is the sensor that keeps producing confident, well-formatted, wrong numbers. A robot that fuses those numbers blindly will drive its beautifully calibrated filter straight into a wall. Failure detection is the discipline of noticing that a stream has stopped being trustworthy, and recovery is the discipline of continuing to operate safely once you know. This is what separates a demo that works on a good day from a system you can deploy.
A taxonomy of perception failures
Before you can detect a failure you must name the kinds. Robot perception faults fall into a few families, and each demands a different detector. Silent faults are the merciful ones: a sensor stops publishing, a driver returns an error, a message goes missing. A timeout catches these. Stale-data faults are subtler: the node is alive, the topic still ticks, but the payload is a frozen copy of an old reading (a common failure of a hung camera driver that keeps republishing its last frame). Here the bytes look healthy, so you must check the timestamp and the content, not merely the arrival. Out-of-bounds faults produce physically impossible values: a range of 0 metres, an acceleration of 400 g, a quaternion that is not unit-norm. Drift and bias faults are the hardest: the values remain plausible and self-consistent but slowly diverge from reality, like an IMU gyro whose bias grows with temperature. No single-sample check will ever catch drift; only comparison against an independent source will.
The reason this taxonomy matters is that detection cost rises steeply down the list. Timeouts are free. Range checks are cheap. Catching a biased-but-plausible stream requires redundancy or a model of what the reading should be, which is exactly where the fused estimator of Section 57.4 earns its keep: fusion does not merely improve the estimate, it manufactures the cross-checks that make drift detectable.
Key Insight
Detection is impossible without redundancy. If a quantity is measured exactly once, by one sensor, with no model predicting it, then a wrong-but-plausible value is indistinguishable from a right one, by definition. Every failure detector in this section works by exploiting some form of redundancy: two sensors that should agree, a motion model that predicts the next reading, or a physical constraint the data must obey. Design the redundancy in deliberately; do not hope to bolt a detector on afterward to a stack that has none.
Detecting failures with the innovation sequence
The single most useful detector for a fused robot comes for free from the estimator you already run. A Kalman-family filter, at each step, predicts the next measurement and compares it to what arrived. That difference is the innovation \(\nu_k = z_k - H\hat{x}_{k|k-1}\), and the filter also computes its expected covariance \(S_k = HP_{k|k-1}H^\top + R\). If the sensor is behaving, the innovation should be zero-mean with covariance \(S_k\). If it is faulting, the innovation blows up. You quantify this with the normalized innovation squared, a chi-square statistic:
$$ d_k^2 = \nu_k^\top S_k^{-1} \nu_k \sim \chi^2_m $$
where \(m\) is the measurement dimension. Under normal operation \(d_k^2\) follows a chi-square distribution with \(m\) degrees of freedom, so you pick a threshold \(\gamma\) at (say) the 99.7th percentile and reject any measurement whose \(d_k^2 > \gamma\) as an outlier. This is a gating test, and it is the same statistic used for data association in tracking (Section 57.3) and for change detection in Chapter 12. The beauty is that it is scale-aware: it does not ask "is this reading far from prediction in metres" but "is it far relative to how uncertain I expected to be", which is exactly the right question.
A single-sample gate catches spikes but not slow drift, whose per-sample innovation stays inside the gate while the bias accumulates. For that you accumulate evidence over time with a cumulative sum (CUSUM) of the normalized innovations, the sequential-detection tool of Chapter 12: a small persistent bias that no single sample would flag drives the running sum past a threshold in a few seconds. Pairing a per-sample chi-square gate with a windowed CUSUM covers both the spike and the creep.
import numpy as np
class InnovationMonitor:
"""Chi-square gate + CUSUM on a Kalman filter's innovation stream."""
def __init__(self, gate=11.8, cusum_h=25.0, drift=0.5):
self.gate = gate # chi-square 99.7% threshold, m=2 -> ~11.8
self.cusum_h = cusum_h # CUSUM alarm level for slow drift
self.drift = drift # slack: ignore fluctuations below this
self.g = 0.0 # running CUSUM statistic
def check(self, nu, S):
d2 = float(nu.T @ np.linalg.solve(S, nu)) # normalized innovation squared
spike = d2 > self.gate # reject this sample?
self.g = max(0.0, self.g + d2 - self.gate * self.drift)
drift = self.g > self.cusum_h # persistent bias alarm
if drift:
self.g = 0.0 # latch + reset after alarm
return {"nis": d2, "reject_sample": spike, "drift_alarm": drift}reject_sample gates single-step outliers (a lidar glint, a GNSS multipath jump); drift_alarm integrates small biases the gate lets through until they are statistically undeniable. Both use only \(\nu\) and \(S\), which every Kalman update already computes, so the monitor adds microseconds per step.Listing 57.5 is deliberately estimator-agnostic: feed it the innovation and covariance from the EKF of Section 57.4, or from any of the fusion filters in Chapter 49, and it flags both abrupt and creeping faults. When the estimator is a neural network rather than a Kalman filter, the analogous signal is a calibrated predictive uncertainty or a conformal nonconformity score (Chapter 18): the model's own "I am surprised" measurement plays the role of \(d_k^2\).
In Practice: an autonomous shuttle leaving a tunnel
A low-speed campus shuttle fuses GNSS, wheel odometry, an IMU, and a lidar map-matcher. Emerging from an underpass, GNSS reacquires satellites but reports a multipath-corrupted fix that jumps the vehicle three metres sideways. The raw fix looks healthy: valid fix flag, tight reported accuracy, correct checksum. The innovation monitor is not fooled: the lateral jump produces \(d_k^2 = 40\) against a gate of 11.8, so the fusion node rejects that GNSS update and coasts on odometry, IMU, and lidar map-matching, which still agree with each other. Six seconds later GNSS settles, its innovations fall back inside the gate, and the node silently readmits it. No passenger felt a thing. Had the stack fused the bad fix, the shuttle would have lurched toward the curb and triggered a hard safety stop, the exact false-alarm-versus-safety tradeoff every deployment must tune.
Recovery: degrade, do not die
Detection without a plan is just a better-documented crash. Recovery is a ladder of responses, ordered from least to most disruptive, and a robust system climbs only as far as it must. The first rung is reject and coast: drop the faulted measurement and let the remaining sensors carry the estimate, exactly as the shuttle did. This works precisely because fusion is redundant; losing one of four inputs widens the covariance but does not break the filter. The second rung is reconfigure: formally mark the sensor unhealthy, inflate or remove its noise contribution, and switch the estimator into a degraded mode (for instance, IMU-plus-odometry dead reckoning, the technique of Chapter 24, when exteroception is lost). The third rung is reinitialize: if the estimate itself has diverged (covariance exploded, innovations chi-square failing on every channel at once), the fused state is unrecoverable and must be reset from a fresh absolute fix rather than nursed. The final rung is the safe stop: when no consistent state can be recovered and the remaining sensors cannot support the current maneuver, the only correct action is to bring the robot to a controlled halt in its last-known-clear space and hand off to a supervisor.
The controller that walks this ladder is a small finite-state machine with hysteresis: it should demote a sensor quickly on strong evidence but readmit it only after a sustained clean stretch, so that a flickering fault does not whipsaw the robot between modes. Two design rules make it trustworthy. First, degraded modes must be tested as first-class citizens, not afterthoughts; a fallback path that has never run in simulation will fail the one time it fires. Second, the health state must be observable: publish it on a diagnostics topic so an operator, a black-box recorder, and the higher-level planner all know the robot is running on three sensors instead of four. A silent degradation is a latent double fault waiting to happen.
The Right Tool
Hand-rolling fleet-wide health monitoring means writing per-node heartbeat timers, staleness checks, threshold aggregators, and an alerting fan-out: easily 200 or more lines of bespoke plumbing, plus the bugs in your own timeout logic. In ROS 2 the diagnostic_updater and diagnostic_aggregator stack does it declaratively:
from diagnostic_updater import Updater, FunctionDiagnosticTask
from diagnostic_msgs.msg import DiagnosticStatus
def imu_health(stat):
if monitor.drift_alarm:
stat.summary(DiagnosticStatus.ERROR, "IMU innovation drift")
else:
stat.summary(DiagnosticStatus.OK, "IMU nominal")
return stat
updater = Updater(node) # ~6 lines replaces ~200
updater.add(FunctionDiagnosticTask("imu", imu_health))
# staleness, heartbeat, and severity roll-up are handled by the framework/diagnostics topic it publishes is what operators, recorders, and planners subscribe to.The framework handles the plumbing, but the health predicate, the actual judgement of "is this sensor lying", is yours to write. It cannot be outsourced, because only you know the physics your sensor must obey.
Research Frontier
Classical residual monitoring assumes you have a model that predicts each measurement. As of 2026 the active frontier is learned failure detection for the perception modules that have no clean analytic residual: monocular depth networks, BEV occupancy heads, and end-to-end policies. Approaches include introspective perception (a model trained to predict its own error), conformal risk control that gives distribution-free guarantees on detection rate (Chapter 18), and runtime monitors that flag out-of-distribution inputs before the downstream network acts on them (Chapter 66). The open problem is coverage guarantees: an automotive safety case under ISO 21448 (Safety of the Intended Functionality) needs a bound on the probability that a perception fault goes undetected, and learned monitors do not yet supply one that certifiers accept. This is where perception failure detection meets functional safety in Chapter 68.
Exercise
Take the EKF you built in Section 57.4 and inject three faults into its logged run: (a) a single 5-metre GNSS spike on one timestep, (b) a stuck lidar-odometry channel that republishes the same pose for two seconds, and (c) a gyro bias that ramps linearly to 0.02 rad/s over ten seconds. Wire in the monitor of Listing 57.5. For each fault, report which detector fired (per-sample gate, CUSUM, or neither), how many samples elapsed before the alarm, and what recovery rung the fault warrants. Then answer: which fault does a pure timeout miss entirely, and why?
Self-Check
1. Why is a sensor that publishes confident, well-formed, wrong values more dangerous than one that goes silent, and which detector catches each?
2. The normalized innovation squared \(d_k^2 = \nu_k^\top S_k^{-1}\nu_k\) is compared to a chi-square threshold rather than to a fixed distance in metres. Why is the covariance normalization essential?
3. A per-sample chi-square gate passes every individual reading from a slowly biasing gyro, yet the estimate drifts. What second detector catches this, and what does it accumulate?
What's Next
In Section 57.6, we confront the clock: a failure detector that fires 300 milliseconds too late is itself a failure, so we turn to latency budgets, hard real-time scheduling, and the deadline guarantees that decide whether a correct answer arrives in time to matter.