0.3.6: three GPS gates that were rejecting good data

[manankharwar]

0.3.6 is out. It fixes four GNSS bugs, and three of them turned out to be the same mistake made in three different places, so it seemed worth writing up properly instead of just pointing at the changelog.

The mistake

FusionCore has several gates that decide whether a GPS fix is good enough to fuse. Each of them compared a distance against a threshold in metres. That works if you know what your receiver's noise looks like. It fails quietly if you don't, because a rejected fix does not produce an error. It simply is not used, the filter dead-reckons instead, and you see a bad trajectory and reasonably conclude the filter is bad.

Here is the first one, and it is the worst. sensor_msgs/NavSatFix has no DOP fields, so FusionCore derived fix quality from position_covariance by taking the square root, which gives you metres. That value was then compared against gnss.max_hdop and gnss.max_vdop, whose defaults were 4.0 and 6.0. Everybody reads those names as the dimensionless geometry factor where 4 is a fairly relaxed limit. What they actually meant was "reject anything worse than 4 m horizontal or 6 m vertical".

I measured a u-blox NEO-M9N over a 500 fix outdoor run: 3.6 to 6.0 m horizontal, 14.4 to 24.0 m vertical. That is a healthy receiver behaving normally. At the shipped defaults, all 500 fixes were rejected. On every run, for anyone on a consumer receiver, since the defaults were introduced.

The second was gnss.max_speed, a gate that rejects a fix implying motion the robot could not physically have made. The bound was max_speed * gap + margin, again pure metres. At 1 Hz with a 2.0 m/s limit and a 5 m margin that comes to 7 m, and the receiver's own noise was around 6 m, so the gate was sitting inside the noise distribution. It rejected 157 of 500 fixes on a rover whose actual top speed is 0.6 m/s, so no honest motion was ever involved. Loop closure against a chalk mark went from 2.62 m to 7.27 m.

The third is a variation. GPS track heading derives yaw from the displacement bearing, which is course over ground. On a curved path that differs from where the robot is actually pointing, and that is a bias rather than noise, so it pulls the estimate no matter how honest its covariance is. It was fusing even on robots that already had a magnetometer, which is a much better heading source. A user reported that his Nav2 path was a straight line without GPS and a zig-zag with it, which is exactly what that looks like.

Why none of this showed up in testing

This is the part I found more interesting than the bugs.

There are 126 unit tests, 12 NCLT benchmark sequences, and a Gazebo demo. None of them could have caught any of it, because all of them run on GPS metadata that is cleaner than what a real receiver reports. The NCLT player, for instance, invents its own covariance, and the value it happens to use passed the old vertical gate with about a metre to spare. Had whoever wrote that line picked a slightly larger number, every benchmark run would have silently become dead reckoning and the results would still have looked like plausible numbers.

So these were only findable on hardware, and that is where they were found: twice on my own test rover, once by a user with an RTK setup. Two independent sources hitting the same defect is what made it obvious the problem was systemic rather than a quirk of one robot.

There is a related lesson I keep relearning. Every time I had to loosen a default on my own rover to make GPS work at all, I treated it as a rover quirk. It was not. It was the shipped default being wrong for everybody, and I had that evidence twice before I acted on it.

What changed

GnssFix now carries explicit sigma_xy and sigma_z in metres, and the quality gate reads those against gnss.max_sigma_xy (25.0) and gnss.max_sigma_z (50.0). max_hdop and max_vdop keep their original meaning and only apply when a fix has no covariance at all, which in practice means gps_msgs/GPSFix with receiver-native DOP.

The jump gate now adds gnss.max_speed_sigma_k multiples of the receiver's reported sigma to its bound. With the gate on, the same bag now gives 0 rejections out of 500 and a 2.62 m closure, identical to having it switched off, while a 700 m spike is still rejected. The noise term scales with the receiver's sigma and deliberately not with the filter's own covariance, because chi-squared is already the covariance-scaled test and this gate exists to catch what chi-squared misses after a blackout.

Track heading no longer fuses when a stronger absolute heading source is active, and its min_speed and max_yaw_rate parameters now gate the fusion instead of only the validation flag, which is what they were documented as doing all along. Robots with no absolute heading source are unaffected and there is a test pinning that.

Fourth fix, smaller but annoying: /fusion/debug/gnss_status was reporting NOT_PROCESSED for jump-gate rejections, which is the value that means "this was never evaluated". Fixes were disappearing with no stated cause, including in my own field monitoring.

What I am not claiming

The published NCLT numbers have not been re-run and I have marked the tracked baseline as superseded, because every entry in it was measured with the old jump gate. I tried to get a clean re-run for this release and could not: on my machine the benchmark harness gives results that swing by a large factor depending on playback rate and CPU load, which means it is not currently a reliable instrument. That is a separate problem and I would rather say so than publish a number I do not trust.

What I do trust here is narrower. Every fix is covered by unit tests that fail if you revert them, and the gate behaviour is verified against 500 real GPS fixes from a bag I can replay deterministically. Accepted fixes are fused exactly as before, so nothing about the filter maths changed.

If you are upgrading

You probably do not need to change anything. If you had raised gnss.max_hdop or gnss.max_vdop to get your GPS working, you can leave those values in place, they simply will not be what is gating any more. If you set gnss.track_heading_enabled: false to stop heading jitter, try removing it and see whether the default behaves now.

If your GPS still is not being used, ros2 topic echo /fusion/debug/gnss_status will tell you which gate is rejecting it, and the reason string is honest now.