automatom, thanks for coming back to this thread too. Let me go through all of this properly.
- On reproducibility first
Fair point and you’re right to ask. The FusionCore config used for the NCLT run is committed here: github.com/manankharwar/fusioncore_datasets/config/nclt_fusioncore.yaml
The bag file is from the public NCLT dataset at robots.engin.umich.edu/nclt: specifically the 2012-01-08 sequence shown in that trajectory plot. The raw sensor data, the ground truth RTK, and the evaluation scripts are all in the datasets repo. I should have linked all of that in the original post and I didn’t. Fixing that now.
- On the GPS covariance claim
You’re right, same correction you made in the other thread. I read the navsat_transform source after you flagged it and found the covariance being read and rotated correctly. The claim that RL ignores GPS-reported covariance is wrong. I’ve updated the README and docs to remove it.
- On bias estimation and REP-105: this is the part I want to engage with properly
You said bias estimation doesn’t solve the heading drift problem, it just slows it. That’s accurate and I should have been more careful about how I framed it. Let me explain what FusionCore actually does here because I think the nuance matters.
For a robot running odom only with no GPS, you’re completely right. Bias estimation on a 6-axis IMU with no absolute heading reference means your yaw error grows more slowly but it still grows. FusionCore in that configuration is publishing an odom frame estimate and nothing about it is REP-105 compliant for map-level accuracy. The filter knows this too: the heading_validated_ flag only gets set true when there’s an actual external heading source: a dual antenna receiver, a 9-axis AHRS with magnetometer, or… here’s the tricky one… GPS position updates over distance.
When GPS is active and the robot is moving, sequential position fixes give the filter weak heading observability. If you’re at point A one second and point B the next, the vector A->B tells you something about which direction you’re facing. It’s not an absolute heading measurement and it degrades if you’re moving slowly or turning, but it does constrain heading drift in a way that pure gyro integration doesn’t. FusionCore uses this to set heading_validated_ after 5 meters of straight-line travel at low yaw rate. Is that a true absolute heading reference? No. Does it meaningfully reduce drift over a 600-second outdoor run compared to no GPS? Yes, and the trajectory shows it.
The honest answer to your REP-105 point is: FusionCore is fully compliant when you have a real heading source (dual antenna or magnetometer), and it produces a usable but not strictly compliant map-frame estimate when GPS position is your only heading signal. The code tries to be transparent about this… there’s a warning in the diagnostics when heading hasn’t been independently validated. But I should say this more clearly in the documentation rather than letting it be buried in a config flag.
On the UKF numerical instability
“The UKF in RL does have numerical instability, definitely. I just haven’t had the cycles to look into it.”
Thank you for confirming this. I was careful not to claim this was a configuration problem because I genuinely didn’t know if it was, and I didn’t want to be wrong twice. The data shows NaN at t=31 seconds on the 2012-01-08 sequence. I can reproduce it consistently. The same data, same timestamp, same result across multiple runs.
My working hypothesis is that it’s a gating issue. FusionCore runs a chi-squared test on every sensor update. When a GPS fix is a statistical outlier relative to the current state, it gets dropped before it touches the filter. RL’s navsat_transform uses the covariance to weight the update but doesn’t reject it. A bad fix with wrong covariance enters the filter with low weight, nudges the state slightly, the next fix is now a larger innovation relative to the corrupted state, it also gets accepted, and eventually the UKF’s sigma point computation or covariance update hits something that isn’t positive definite. That’s where NaN comes from.
If you ever do get the cycles to look into it, I’d genuinely be curious whether adding a Mahalanobis gate to the GPS update in navsat_transform fixes it. My guess is it would.
