In the last build log I got the fusion working: GPS, IMU, and wheel speed combined into one estimate that tracks the truth on a clean drive. But that filter has a dangerous property. It trusts every reading. So this milestone is about the failure that trust invites, and how to catch it. I inject a GPS fault, a sudden jump of several metres that then holds, the kind of thing reflections off buildings or a spoofing signal cause in the real world, and I make the system notice.
The filter already knows what it expects to see.
The surprise is a free lie detector.
01 The problem: a trusting filter follows the lie
When the GPS jumps by eight metres and stays there, the fusion filter does exactly what it was built to do: it pulls the estimate toward the reading. Over a few seconds the estimate walks off the true path and follows the spoof. Nothing looks broken. The vehicle would keep acting, confidently, on a position that is wrong by the width of a lane. That is the shape of the danger in autonomy: not a crash with an error message, but a confident decision built on an input that quietly went bad.
02 The idea: measure the surprise
Here is the part I like, because it is nearly free. The filter, before it folds in any reading, already has an expectation of what that reading should be, and a sense of how far off it could reasonably be by chance. The gap between the actual reading and the expectation is called the innovation. Normally it is small. When the GPS jumps, it is enormous relative to what chance allows.
You can turn that into a single number, the normalized innovation squared, that says how surprising a reading is given the uncertainty. Under a healthy sensor it stays low and follows a known statistical pattern, so you can set a threshold that a good sensor almost never crosses. Cross it, and the reading is, in effect, claiming something that cannot be true.
03 Detect, then gate
Detection on its own is not enough; you have to act. So when a reading is flagged as too surprising, the estimator simply does not fold it in. The lying GPS is gated out and the filter carries on with the IMU and the wheels until the GPS behaves again. The check and the gate live behind a clean interface, deliberately, so a more capable detector can drop in later without touching the rest of the system.
04 The result
On an eight-metre GPS jump lasting eight seconds, the check catches it at the very first bad reading, with zero false alarms across the rest of the run. With the check in place, position error stays at 0.42 m. Without it, the estimate follows the spoof and the error blows up to 5.59 m. Same drive, same fault, one small integrity layer between a wrong answer and a right one.
The code, the tests, and a plain-language explainer of every term are public and Apache-2.0, built on standard methods (an Extended Kalman filter, and the innovation / NIS consistency test) cited in the repo: github.com/andrewmichelis/av-integrity.
This catches a jump, which is a loud fault. A slow, small drift is harder, because step to step it is not very surprising; that is future work. And while the GPS is gated out, the estimate is dead-reckoning on the IMU and wheels, so it drifts a little; over a long enough outage that drift matters. Naming these limits is part of the point of building in the open.
The estimator already computes what it expects each sensor to say. Turning that expectation into a trust decision costs almost nothing and is the core of integrity monitoring: a reading far more surprising than the model allows is probably a lie, so flag it and keep it out. Next: a fault taxonomy across every sensor, and a safe-state layer for when trust runs out.