This is the first build log of the open project I described in the charter: a system that knows when it can't trust its own sensors, and fails safe instead of failing. Before any of that, though, I need something less glamorous and more important. I need an estimate of where the vehicle is that I trust when nothing is wrong. You cannot tell that a sensor is lying until you have a baseline that is honest when it isn't. So milestone zero is exactly that baseline: fuse the sensors, and prove the result tracks reality.
You can't tell that a sensor is lying
until you trust the estimate when it isn't.
01 What the system actually has to do
An automated vehicle never directly knows where it is. It works it out from sensors, and every sensor is imperfect and partial. In the simulation I use three, each standing for a real one: a GPS that gives a jittery absolute position, an IMU that gives how fast the vehicle is turning and accelerating, and wheel speed from the odometry. None of them alone is good enough to drive on. GPS jitters and can jump; the IMU drifts; the wheels slip. The job is to combine them into a single running estimate of the vehicle's state (its position, heading, and speed) that is better than any one sensor.
02 The architecture, and where the real work is
The fusion is a standard Extended Kalman Filter. It runs a simple loop: predict where the vehicle will be a moment later using the IMU, then correct that guess when a GPS fix or a wheel-speed reading arrives, nudging it toward the reading by an amount that depends on how much it trusts the guess versus the reading. Prediction on its own drifts, like navigating with your eyes closed; the corrections keep it honest.
The filter is the commodity part. The engineering that matters is around it: which sensor feeds prediction and which feed correction, the interface contract at each of those boundaries (units, rates, coordinate frames, and what the estimator must do when a reading is late or missing), and the discipline of keeping the true trajectory beside the estimate the whole time. Because this is a simulation, I always know the real answer, so I can measure how good the estimate is rather than assert it. That is the difference between a testbed you can trust and a demo that looks nice.
03 The result
On a clean drive with no faults, the fused estimate tracks ground truth to within about 0.3 m in position, 0.03 rad in heading, and 0.09 m/s in speed. The run is one command, and the numbers are measured against the true trajectory, not eyeballed. That is the whole point of the milestone: an honest, reproducible baseline.
Two honest limits. First, it is a simulation: it proves the reasoning and the method, not real-world performance, and where the model stops being realistic is stated in the repo, not hidden. Second, and this is the interesting part, nothing here notices a sensor lying. Every reading is trusted. If the GPS jumped, this filter would faithfully follow it off the road. That is the next milestone, on purpose.
The unglamorous foundation is the one that makes everything after it meaningful. A fault check is only as trustworthy as the estimate it sits on, so the first job is an estimate that tracks the truth when nothing is wrong, measured honestly, not asserted. Next: make the same system notice when a sensor starts lying.