Localisation

Docs index · Fault modes · Design notes

Does the robot still know where it is when the sensors lie to you?

The stack is an ordinary one — robot_localization’s EKF with navsat_transform in front of it — wired the real way, with nothing stubbed. What is unusual is what sits beside it: a shim that breaks the sensors on cue, and a scorer that grades the estimate against ground truth rather than against itself.

1. The pipeline

flowchart LR
    sim[simulator<br/>O3DE or headless] -->|/gnss/fix<br/>/imu/data<br/>/wheel/odom| inj[fault_injector]
    inj -->|.../faulted| local[EKF local<br/>owns odom -> base_link]
    inj -->|.../faulted| ekf[navsat_transform<br/>+ EKF global<br/>owns map -> odom]
    ekf -->|/odometry/filtered| scorer
    sim -->|/ground_truth/odom| scorer
    runner[scenario_runner] -->|/cmd_vel| sim
    runner -->|set_fault| inj
    scorer --> report[(JSON report)]

The injector is a shim between the sensors and the filter. Nothing in the simulator or the filter knows it exists, which is what lets the same scenarios run against O3DE and against the headless model.

Two filters, which is what robot_localization is built for. The local one fuses everything except GNSS and publishes odom → base_link, so that frame stays continuous and never jumps. The global one adds GNSS through navsat_transform and publishes map → odom; it is the one the scorer reads. Both run in 2D mode at 30 Hz.

The local filter’s sensor set is derived from the profile rather than written out separately, so the two cannot drift apart — and so that naive stays a control case. A local filter that fused a gyro naive does not would put a heading into odom → base_link that the profile is supposed to prove it lacks.

2. The three profiles

ros2 launch kraken_localisation localisation.launch.py profile:=robust. Everything else about the stack is identical between them, which is what makes the comparison a measurement rather than a claim.

profile wheel forward speed lateral constraint GNSS IMU yaw rate ground speed
naive ✓ ✓ ✓    
robust ✓ ✓ ✓ ✓  
radar   ✓ ✓ ✓ ✓
crosscheck cross-checked ✓ ✓ ✓ ✓

naive → robust is one measurement, the yaw rate. Without it heading is observable only through GNSS, so losing the fix leaves the estimate rotating at whatever rate it last held. Absolute IMU yaw is deliberately not fused: plenty of vehicles carry a gyro and no usable magnetometer, and assuming a heading reference would hide the problem rather than solve it.

radar removes one and adds one, and the pairing is the point: under slip the encoders are confidently wrong, and an EKF blends by inverse variance, so a lying measurement has to be removed rather than outvoted. See design.

crosscheck keeps both, which works only because slip_monitor stops the wheels claiming a precision they do not have: it rates them against the radar and widens their covariance by how far the two disagree. On the same drive it matches radar — 0.318 ± 0.139 m against 0.267 ± 0.185 m — so removing a sensor was never the necessary part. It also publishes the slip ratio on slip, and widens rather than narrows when nothing is watching the ground at all.

On O3DE the camera is the reference

The radar only exists in the headless simulator. O3DE has no ground-speed sensor of any kind, so there the job falls to the camera, the only other thing aboard that measures motion against the world rather than against a wheel.

ros2 launch kraken_camera visual_odometry.launch.py namespace:=kraken1 runs three nodes: stereo_camera_info to make the simulator’s two cameras describable as one pair, rtabmap_odom’s stereo_odometry to turn that pair into motion, and visual_ground_speed to reduce motion to a forward speed. It publishes ground_speed — the same topic the radar publishes — so nothing downstream needs to know the difference: the fault channels, slip_monitor and both the radar and crosscheck profiles are already wired to that name.

Two corrections stand between rtabmap’s odometry and being usable as that reference.

Its twist covariance is overconfident, which is the one thing this stack cannot tolerate. Measured against ground truth on a simulator verified to hold real time and hold exactly one robot, sigma runs from 0.62 to 1.16 of the error actually made, over nine runs. Sigma is multiplied by 2 — the worst of those rather than the average, because an average leaves the filter overconfident in whichever conditions turn out worse than average. Its pose covariance is the opposite, 11 to 19 times conservative; the two are filled independently and neither predicts the other.

The spread matters more than the middle. Repeating one case moves this figure further than changing the case does — straight running gave 0.90, 1.16 and 1.14 on three runs, while turning gave 0.79, 0.62 and 0.96 — so a factor fitted to a single run of each case is fitted mostly to noise. One was, and read 1.6 from a worst case of 0.64 that three runs later turned out to be 0.62.

Those figures belong to the 0.12 m baseline. The 0.5 m pair needed a factor of 3, so narrowing improved the honesty of the covariance about as much as it improved the accuracy of the estimate. Change the baseline and this has to be measured again.

Frames it cannot solve are dropped rather than scaled. rtabmap reports them honestly, with covariance 9999 and a pose that has not moved. A zero speed published during a turn is not a cautious reading but a wrong one, and slip_monitor would read it as the wheels disagreeing with the ground by the whole speed of the robot. Publishing nothing is what “I do not know” looks like on a topic, and slip_monitor already widens the wheels when its reference goes stale.

It works, turns included, on a 0.12 m baseline. Straight it follows the row to within 0.4% over 17 m; through turns of 80°, 111° and 137° it holds to under 0.7% and solves every frame of all five cases measured.

That is a recent correction rather than a standing property. On the 0.5 m baseline this stack shipped with, visual odometry did not degrade on turns, it stopped: past roughly 46° of heading it failed to solve between a third and two thirds of its frames and reported centimetres where the robot had moved metres. The cause was near range, not turn rate — the slowest turn measured is one of the failures and a faster one is perfect — and narrowing the pair fixed it. See simulation for the measurement.

The dropping still matters, because it is what stands between that regression returning and slip_monitor reading a zero as the wheels disagreeing with the ground by the whole speed of the robot.

The lateral constraint is in every profile. A differential-drive base cannot translate sideways, but nothing in the filter knew that, and the estimate happily ramped to 0.98 m/s of sideways motion on a robot whose true lateral velocity is zero. It is a kinematic constraint rather than a sensor, so it belongs in all three, and it stays true under slip because slip is forward.

The naive profile is kept, and asserted to stay broken, as the control case.

3. The result

One EKF configuration, one extra fused measurement, same 40-second drive with the GNSS fix killed at t=15 s. Ten runs per profile, seeds 0–9:

profile fuses worst position error worst heading error
naive wheel speed + GNSS 21.8 ± 0.9 m 179.9 ± 0.04°
robust + IMU yaw rate 0.36 ± 0.25 m 1.9 ± 1.2°

Both look identical while the fix is healthy. That is the entire point: the bug is invisible until the day it matters.

4. The robustness suite

Ten scenario files, each a YAML description of a drive and the faults injected during it. Adding a failure mode is adding a file; no Python required.

file fault asserts
total_gnss_dropout fix lost entirely stays localised
gnss_dropout_naive fix lost, no gyro fused diverges (control case)
gnss_degraded RTK → single point, 2 m noise position bounded, heading recovers
gnss_spoof fix walks off at 0.2 m/s, covariance still tight follows the spoof (known gap)
imu_dropout gyro dies, GNSS healthy position bounded, heading lags then recovers
wheel_slip encoders report 2× the real motion stays localised
terrain_dropout fix lost and the ground is slippery drifts 9.6 m while heading stays under 3°
terrain_dropout_radar the same, with the radar replacing wheel speed recovers to 0.27 m
terrain_dropout_crosscheck the same, with both fused and the wheels cross-checked recovers to 0.32 m
nav_baseline none, one Nav2 goal 12.65 m out arrives, and is really there
nav_terrain_dropout the same goal, fix lost, ground slippery believes it arrived, is 5.7 m short
nav_terrain_dropout_radar the same again, with a ground-speed radar arrives late, and is really there

Four assert failure on purpose. gnss_spoof documents a real gap — the filter has no innovation gate, so a confident lie is a trusted lie. Better red and visible than assumed and absent.

What each injector mode does to which channel is in fault modes. Thresholds live in the scenario files, each annotated with the distribution actually measured over a sweep.

A goal is only sent once the stack can take one. Waiting for the first score message says localisation is up and says nothing about navigation, and the nav_* scenarios cut GNSS milliseconds before the goal goes out: if navsat_transform has not fixed a datum by then, map → odom never appears, the global costmap times out waiting for it, and Nav2 rejects the goal outright in about 16 ms. That reads as a navigation failure and is a race in the harness — roughly one run in eight, and every run under load. The runner now waits on the map → base_link transform itself before the first navigate_to, rather than on a proxy for it.

5. How a run is scored

Four decisions do most of the work, and each is argued in design:

Do not quote a single run. The simulator steps on a wall-clock timer while the filter, injector and scorer consume its output over DDS, so the interleaving moves between runs. Use ros2 run kraken_scenarios sweep <scenario> -n 10 and read the order of magnitude.

Ground truth has to be in the frame it claims

The headless simulator stamps ground truth in the world frame. O3DE does not: it measures from wherever the robot was spawned, facing whatever way it was facing, and stamps that <ns>/odom — which is also the name of the map-aligned frame the filter publishes. Two different frames, one frame_id.

map->odom therefore carried the spawn translation but no rotation, and RViz drew the truth arrow ninety degrees from the machine it described. Ground truth itself was never wrong — driven straight, its course and its quaternion agree to 0.3° and its twist is properly body-framed — only the frame it was published in.

ground_truth_map republishes it in map, so O3DE agrees with the convention the headless sim already followed. The translation comes from tf, because where row one lands in map is decided by the georeference rather than by the level; the rotation is a parameter, because the published one is the identity that causes the problem.

Trap. An identity map->odom means “not yet”, not “the origin”. The filter publishes one until GNSS converges, and latching it pins ground truth to the map origin — 63 m from the robot, in a run that otherwise looks healthy.

Scenario scoring was unaffected throughout, because it compares reference-relative poses and a constant offset cancels. What was affected was every by-eye reading of RViz.

6. Where localisation and navigation meet

nav_terrain_dropout is the scenario that joins the two halves of this repo. Every sensor is healthy and every message is honest — the wheels really are turning at the commanded rate, the encoders really do report that, the ground simply declines to convert it into motion. There is no disagreement for a filter to detect.

  flat, healthy fix slippery, no fix slippery, radar
distance to goal, believed 0.583 m 0.587 m 0.77 m
distance to goal, true 0.617 m 5.658 m 0.75 m
ground actually covered 12.17 m 7.13 m 12.05 m
time to goal 19.0 s 21.6 s 34.3 s
worst cross-track error 0.58 m 0.59 m 0.40 m
recovery behaviours used 0/8 0/8 0/8
Nav2 reported success 8/8 8/8 7/8

Eight seeds each. The absolute goal errors are set by the controller’s 0.6 m xy_goal_tolerance — the goal checker halts the machine as soon as it is inside that radius — so read the columns against each other rather than against zero.

The robot’s own account of the run is unchanged while it sits 5.7 m from where it was sent, and it reported SUCCEEDED in every run. Cross-track is flat across all three columns, which locates the failure: the slipping robot is not wandering, it drives the right line and stops short. The radar recovers it — and takes 34 s instead of 19, because the ground really is slippery and no sensor changes that. Arriving late is what success looks like here; arriving on time is the lie.

The radar column is 7/8 rather than 8/8 because roughly one seed in eight never leaves the start: the goal returns in under a second having covered 0.00 m. It reproduced across two independent sweeps and is listed in known gaps.

7. Stopping when the pose is not trustworthy

Everything above measures how wrong the filter gets. None of it stops the machine, and a robot that keeps driving on a pose it has lost is the failure that matters: nav_terrain_dropout above reports SUCCEEDED while sitting 5.7 m from where it was sent.

safety_monitor sits in the command path rather than beside it. Nav2’s controller and behaviour servers, and the scenario runner, all publish to cmd_vel_raw; the monitor is what turns that into cmd_vel. So it cannot be bypassed, and it cannot fail quietly — if the process dies nothing republishes cmd_vel, and the machine times out to a stop. A monitor that only published an opinion would go silent exactly when it mattered, and silence is indistinguishable from health.

When a bound is breached it stops first and deactivates second: a zero command goes out immediately and is repeated at 10 Hz, and only then is controller_server asked for a lifecycle DEACTIVATE. The stop latches.

It judges three things, and two of them are worth less than they look.

What each check is worth

Are the filter’s inputs usable? This one is solid, and it is why the node exists. robot_localization discards a measurement it cannot transform into the base frame and reports that at RF_DEBUG only, so a filter can drop 100% of an input all run while publishing at its configured rate with a plausible covariance. That is not hypothetical — a transport bug kept base_link->imu_link off /tf_static and silently demoted robust to naive in nine runs out of ten.

Is the position covariance bounded? Only good for gross loss. Measured across the suite at seed 0, claimed 1-sigma against actual error:

scenario claims actually is  
nav_baseline 0.077 m 0.078 m agrees
wheel_slip 0.080 m 0.502 m agrees
gnss_spoof 0.084 m 8.090 m blind
gnss_degraded 0.599 m 1.708 m agrees
terrain_dropout_radar 7.374 m 0.241 m false alarm
total_gnss_dropout 8.212 m 0.173 m false alarm
terrain_dropout 16.334 m 9.628 m agrees
gnss_dropout_naive 249.8 m 23.215 m agrees

Two things follow. A spoof is invisible to it: a false fix makes the filter more confident while taking it 8 m off course, so this check is no defence at all against gnss_spoof — the crosscheck profile is. And there is no threshold below about 9 m that separates a lost filter from a merely pessimistic one, because terrain_dropout_radar claims 7.4 m while being 0.24 m out. The bound therefore defaults to 12 m: a backstop against the filter falling over, not a guarantee of staying in the row. Tightening it means calibrating the covariance first, and process_noise_covariance is currently unset, so robot_localization’s defaults apply.

Do the wheels still agree with the ground? Healthy runs measure a slip ratio of 0.16, a scenario built around slip measures 0.55, and a stationary machine reads about 1.0 because the ratio is a division by nearly nothing. The bound is 0.8, and the startup reading is what the settling tolerance is for.

Yaw covariance is not used. It reads about 15.7° at t=0 and 91° at t=50 in every run regardless of health, because no absolute heading is ever fused — it is a clock, not a health signal.

Why the suite mostly runs with it off

Every other scenario asserts a min_path_length: they exist to measure how far localisation degrades while still driving, so a safety stop would fail the very runs it is meant to protect. They pass max_position_stddev: 0.0, which means no bound. The bringup default is the production value, because a safety function that has to be switched on is one that will be found switched off.

safety_stop is the scenario that turns it on, and it is the only evidence the stop works. Naive profile, GNSS lost entirely: the machine covers 15.5–16.3 m over four runs against 32.1 m when left to drive on, and its worst position error is 7.5–11.4 m instead of 23.3 m.

The settling tolerance and the stop heartbeat are both counted on the node clock, not the wall. The first version used wall timers, and under a simulator at 3x that made a ten-check tolerance thirty seconds long in the world the machine was driving through — the stop fired as the run ended, and the machine covered the full 32 m. The heartbeat has the same problem in slower motion: the simulator ages commands in sim nanoseconds, so a wall-clock heartbeat gets weaker against that timeout as the real time factor rises.

8. Known gaps

The full list, with the reasoning behind each, is in design.