Know where you are: IMU, RTK GPS and two Kalman filters

Phase 3 gave the mower a gyro and centimetre-level GPS, and fused them with its wheels. The fusion worked. Getting there meant catching a sensor configured in the wrong units, a ground truth that spikes, and a filter that started 5,700 km from the robot.

The sensors, contract first

Modelled on the H1 hardware in the parts list: a BNO085-class IMU at 100 Hz (gyro σ 0.002 rad/s) and a u-blox ZED-F9P with RTK corrections at 5 Hz (σ 1.5 cm). The antenna sits on a mast 0.19 m forward and 0.30 m up, because RTK reports the antenna, not the robot. analysis/sensor_report.py checks each sensor against its configuration, the same way the battery report did in Phase 1.

Finding 1: "1.5 cm" of noise was 1.7 km

The first GNSS report said the fixes were 1,513 m off. Gazebo's NavSat sensor applies <noise> to latitude and longitude in degrees, so 0.015 meant 0.015°. And a degree isn't a fixed distance: at 40°N one degree of latitude is 111.0 km but one of longitude is 85.4 km, so no single number in degrees can mean "1.5 cm" in both directions.

Fix: Gazebo publishes a perfect fix on /gps/fix_raw, and a small gnss_noise node adds noise in metres, sets the RTK status and covariance, and (later) simulates outages. Result: σ 1.49 cm, bias 0.10 cm.

The lever arm matters: comparing the antenna's fixes with the robot's centre instead of the antenna gives an RMS error of 19.2 cm, 13 times the sensor noise. Every real build needs that offset measured and in the URDF.

Finding 2: the ground truth spikes

The gyro "residual" was 12 rad/s. The gyro was fine. Gazebo's ground-truth odometry reported a yaw rate of −628 rad/s for one sample, as the heading wrapped past ±180°. We difference the true rate from the true poses ourselves now. Then the residual was still 15× too big, but only during turns: a 100 Hz IMU sees the robot wobble as it pivots, and 50 Hz truth can't. So the report checks noise where truth is precise enough (σ 0.0022 vs 0.002 configured), plus the check that matters for navigation: the gyro integrated over the whole drive drifts 0.30° in 367° of turning.

Finding 3: the filter started 5,700 km away

robot_localization runs two EKFs: local (wheels + gyro, smooth but drifting) and global (+ RTK, no long-term drift). On the first run the global estimate began at 5,726 km error. navsat_transform published one fix before it had applied the datum, and that single jump wrecked the filter's heading for 25 seconds (143° wrong). The fix is a Mahalanobis gate: reject any GNSS position more than 5σ from the current estimate. Real RTK needs the same gate for multipath jumps.

We also found that Gazebo's wheel odometry reports zero covariance, which tells a filter "I'm perfect". A wheel_odom node now republishes it with honest uncertainty, and with knobs for the most common real-world bug: a mis-measured wheel radius and track width.

The experiment

Run	Wheels only	+ gyro (local EKF)	+ RTK (global EKF)
Perfect calibration	0.6 cm RMS	1.1 cm	2.1 cm (that's RTK's own noise)
Radius +3%, track +5%	20.6 cm (max 29.5)	7.7 cm (max 12.4)	2.1 cm (max 5.9)
…plus a 20 s RTK outage	20.6 cm	7.7 cm	3.8 cm (max 9.1)

What it shows: - The gyro fixes heading (4.5° → 0.2° RMS). Heading errors are what turn into big position errors. - RTK fixes position, and it doesn't care how badly the wheels are calibrated. - During an outage, the global filter drifts like the local one, then snaps back within about a second of RTK returning. In Phase 4 that becomes a rule: if RTK is lost for more than a few seconds, stop the blade and wait. - In a perfect sim, wheels alone (0.6 cm) beat RTK (2.1 cm). A simulator that's too kind hides the reason a sensor exists. That's why the knobs exist. - The local filter's error shrinks near the end only because errors partly cancel around a closed square. That's luck, not accuracy, and on a mowing pattern it won't happen.

Bonus bug

An empty OUTAGES= became gnss_outages:= on the command line (the shell drops empty quotes) and launch refused to start. Every default run would have broken, and the experiment that was meant to run without outages caught it.

Next: the yard boundary as a GPS polygon, then Phase 4, mowing.