FusionCore 0.3.2: closing the outlier-gate hole and surviving clock rewinds

[manankharwar]

FusionCore 0.3.2 is out. This one is about robustness, not new capability. Three things that could take the filter down or quietly corrupt its estimate are now fixed, and the reasoning behind each is worth writing down because they are the kind of failure that only shows up on real hardware.

The outlier gate had a hole under sustained spikes

The chi-squared gate rejects a GPS outlier on the first fix, and that has always worked for a single bad reading. But a persistent multipath spike (a reflection off a building that lasts several seconds) is not a single reading, it is a stream of consistently wrong fixes. What happened next was the bug: after gnss.coast_n consecutive rejections the filter entered coast mode and inflated its position process noise, because coast mode is designed for re-acquiring GPS after a real dropout. That inflation widened the gate until it eventually admitted the very spike it had been rejecting. On an 8 second, 60 metre spike the estimate lunged to about 62 metres of error after roughly 5 seconds.

The fix is to separate the two situations that were being conflated. Coast-mode inflation is correct after a GPS gap (you have no better information, so you widen and wait). It is wrong when GPS is continuously present and continuously being rejected (you have a persistent outlier, not a dropout). Rejection-triggered coast now only fires when the rejection streak began after an actual GPS gap, controlled by the new gnss.coast_min_gap_s (default 1.0 s, set to 0 for the old behaviour). On the deterministic repro the peak error drops from 62 m to 1.9 m, and post-outage re-acquisition still works. In the outdoor Gazebo demo it takes FusionCore from 18.15 m RMSE to 2.76 m, below robot_localization's 18.6 m.

A backward time-jump used to crash the whole process

If a sensor timestamp ever went backwards (a clock rewind, a replayed bag looping, a WSL2 clock glitch), the filter computed a negative dt, integrated a negative time step, drove the covariance non-positive-definite, and the UKF threw and aborted the process. That is the worst possible failure mode: the localization node dying mid-run.

Now predict_to detects the backward jump and re-syncs its clock instead of integrating a negative step, and the UKF floors its covariance eigenvalues rather than throwing. It survives an NCLT run that used to SIGABRT within about two minutes. A localization filter should never be the thing that crashes because a timestamp arrived out of order.

Two new gates you can opt into

gnss.max_speed is a physical-plausibility gate: it rejects a fix farther from the predicted position than the robot could physically have moved or drifted since the last accepted fix. It is a per-platform kinematic spec, like wheel radius, not something you tune per run. Off by default.

Adaptive magnetic-disturbance rejection (magnetometer.field_strength) rejects a magnetometer reading whose corrected magnitude deviates from the local Earth field, which is the signature of a nearby motor or steel structure, even when its heading direction would pass the chi-squared gate. This is what makes an absolute-heading source trustworthy enough to lean on through a long GPS blackout on real hardware.

A note on the benchmark numbers

The documentation had drifted ahead of reality in a few places, so this release includes an accuracy pass. The published NCLT numbers are a snapshot that predates a controlled full-suite re-run on current main, and one sequence (2013-04-05) has regressed from 12.1 m to about 19.4 m (still a 93% win over robot_localization, but not the number the docs claimed). The pages now say so plainly, and this release adds regression tracking (check_benchmark_regression.py) so a tuning change that silently worsens another sequence gets caught before it ships rather than weeks later.

Full changelog: https://github.com/manankharwar/fusioncore/blob/main/CHANGELOG.md
Release: https://github.com/manankharwar/fusioncore/releases/tag/v0.3.2

As always: if you are running FusionCore on real hardware, I want to hear what breaks. That is the feedback that drives releases like this one.