Design notes

Why a shim instead of simulator support

Fault injection lives in a node between the sensors and the filter, not inside the simulator. That decision buys three things:

The cost is that faults are applied to messages, not to physics. wheel_slip corrupts the encoder reading; it does not actually make the tyres lose grip. To a filter fed only that message the two look much alike, which is why the shim is enough for most of this suite. Terrain is the deliberate exception, and it lives in the simulator for the reason given below.

Why a headless simulator at all

O3DE is the better demo and a worse test harness: it needs a GPU, an authored level, and a long build. The suite would then run rarely, and a robustness suite that runs rarely is decoration.

kraken_sim is a unicycle model with about a hundred lines of physics in it. It owns /clock, so scenarios run at a configurable multiple of wall speed and produce the same numbers on every machine.

The time factor is load-bearing. At 5× the EKF could not keep up with its own input, and the same scenario returned 0.46 m one run and 1.46 m the next. At 3× it keeps up, but nothing gates a simulator step on the consumers having finished the previous one, so the interleaving still moves between runs. Holding the seed fixed and repeating total_gnss_dropout eight times gives 0.136–0.405 m (mean 0.183, sd 0.092). The values are quantised rather than noisy — runs repeat exactly — so this is piecewise determinism across scheduling interleavings.

Varying the seed is the larger effect, because after GNSS loss the robot is dead reckoning and each seed is a different random walk. Bounds are therefore set from sweep distributions rather than single runs, and each scenario file records the range observed. If you raise real_time_factor, re-measure.

Why errors are relative to the fault instant

navsat_transform anchors the map frame on a datum; the simulator anchors its world frame on its own origin. Those never line up perfectly, and the residual offset is a constant, not a localisation failure. Comparing the estimate and the ground truth in the frame of the pose pair captured when the fault was injected cancels it, leaving only the drift accumulated since.

Why path length and rotation are checked

A robot that never moved has excellent localisation error. So does one wedged against a wall, or one whose cmd_vel never arrived. Every scenario therefore asserts a minimum ground-truth path length and a minimum accumulated rotation, so that a run has to have actually happened before its error numbers are allowed to mean anything.

Both are path totals rather than net displacement, because the trajectory loops back on itself and net displacement would be near zero.

Why worst-case and final error

Losing a sensor mid-turn produces a transient the filter then recovers from. For some faults that transient is acceptable and a permanent offset is not, so scenarios choose:

imu_dropout is the clearest example: heading lags by ~30° during the turn and comes back under 15°, while position never exceeds half a metre. Bounding the peak there would just be a slow way of asserting that turning exists.

Why lateral velocity is fused

A differential-drive base cannot translate sideways, but nothing in the filter knew that. odom0_config fused forward speed only and imu0_config yaw rate only, so once GNSS stopped, lateral velocity was unobservable. Measured, the estimate ramped to 0.98 m/s of sideways motion on a robot whose true lateral velocity is exactly zero, and integrated it straight into cross-track error.

Heading error stayed under 2° throughout, which is why every metric kept reporting a healthy filter. Position error that grows while heading error does not is a velocity-state problem, not a tuning problem.

The simulator now publishes that zero with a covariance and both profiles fuse it. It is a kinematic constraint rather than a sensor, so it belongs in both, and the two configs still differ by exactly one measurement. It moved the robust profile from 2.57 m mean (max 5.13) to 0.359 m (max 0.904) across seeds. Most of what earlier notes here blamed on scheduling jitter was this.

Why the naive profile is kept

ekf_naive.yaml and ekf_robust.yaml differ by one fused measurement, the IMU yaw rate. Without it, heading is only observable through GNSS, so losing the fix leaves the estimate rotating at whatever yaw rate it last held.

Keeping the broken configuration, and asserting that it stays broken, is what makes the comparison a measurement rather than a claim. If someone changes the stack so the naive profile stops diverging, gnss_dropout_naive goes red and tells them the demo no longer demonstrates anything.

Why Nav2 runs without a map

kraken_nav wires Smac Hybrid-A* to a curvature-following controller on top of the same filter output the scenarios measure. Obstacles come from one source: the lidar the O3DE Kraken carries, read by an ObstacleLayer on both costmaps. The headless kinematic model has no ranging sensor, so against it that layer never receives an observation, the costmaps stay empty and what the scenarios exercise is the interface and the control loop rather than obstacle avoidance.

There is no map server and no static layer either way. Both costmaps are rolling windows, so the world is remembered only as far back as the window reaches, and the only prior the machine is given is the geofence — a costmap filter rather than a layer, because it is a boundary set for it and not something it senses. Structure inside a row is measured rather than mapped: kraken_orchard fits a line to the trunks either side out of the same cloud and publishes the corridor between them, which needs no estimate of where the robot is in the world.

It is wired to the flat headless model first, so the plumbing is verified independently of terrain. minimum_turning_radius is a smoothness choice rather than a kinematic limit — a unicycle can spin in place — and only becomes a real constraint once the trailer is modelled, which is why the planner is Hybrid-A* and not Navfn.

Why terrain is in the simulator, not the shim

Every other fault here is a lie told to the filter. Terrain is not: the wheels turn at the commanded rate, the encoders report that honestly, and the ground declines to convert it into motion. No shim between the sensors and the filter can produce that, because the robot’s true position has to change.

It is also the only fault in the suite with no cross-check available. A spoofed fix disagrees with the wheels. A dead gyro stops saying anything. Slip produces a consistent, plausible and entirely honest set of messages that happen to describe a robot which is somewhere else.

terrain_dropout measures the consequence against an otherwise identical flat run: 9.57 m of position error instead of 0.36 m, with heading error inside the same bound, because heading comes from the gyro and the gyro is not wrong. The spread across seeds falls from sd 0.25 to sd 0.022 — flat dead reckoning is a random walk over the noise draws, slip is a bias.

The model is traction only: a scalar in (0, 1] per rectangular patch, scaling forward and yaw rate together. There is no slope, so there is no downhill drift and the nonholonomic constraint the filter now fuses stays true. Adding height would break that constraint, which is the more interesting experiment and the reason traction lives in its own module rather than three lines in the step function.

Why the goal error is scored twice

The scorer reports goal_error_estimated and goal_error_true separately. The first is the distance from the goal to where the filter thinks the robot is; the second is the distance to where it actually is. On good ground the two agree and the pair looks redundant. That is the point of measuring both.

Nav2 decides it has arrived by comparing the goal against the transform tree, which is fed by the filter. It has no other source of truth, so a goal check can only ever confirm that the estimate arrived. Anything that biases the estimate biases the arrival test by exactly the same amount, and the error cancels out of every number the robot can compute about itself.

Terrain slip is such a bias. The wheels turn at the commanded rate and the encoders report that rate honestly, so with no fix to correct it the estimate advances at the commanded speed while the robot advances at 45% of it. Over the 12.65 m goal used by nav_baseline and nav_terrain_dropout, across 8 seeds:

  flat, healthy fix slippery, no fix slippery, radar
goal_error_estimated 0.583 +/- 0.029 0.587 +/- 0.010 0.77 +/- 0.07
goal_error_true 0.617 +/- 0.029 5.658 +/- 0.026 0.75 +/- 0.08
path_length 12.17 7.13 12.05
elapsed_time_s 19.0 21.6 34.3
worst_cross_track_error 0.58 +/- 0.15 0.59 +/- 0.23 0.40 +/- 0.13
recovery_count 0/8 0/8 0/8
Nav2 reported success 8/8 8/8 7/8

These are the headless kinematic sim, which scales commanded velocity by a traction factor. O3DE gives real friction and load transfer instead, so the mechanism carries over but these magnitudes will not.

The absolute goal errors are set by the controller’s 0.6 m xy_goal_tolerance, not by the filter. The goal checker stops the machine as soon as it is inside that radius, so ~0.6 m is the floor for every column and no bound tighter than the tolerance can be met. Read the columns against each other, not against zero.

Take the middle column first. The believed error is the same as the control’s to within a hundredth of a metre. The true error is 9x worse. Every signal available to the robot – the goal check, the filter covariance, the encoders, the heading, which stays under 1 degree – says the run went as well as the control run did. This is the second failure in this project that is invisible in heading and visible only in position; the first was the unobservable lateral velocity above, and both were found by scoring against ground truth rather than against the estimate.

The cross-track row is what says where the failure is. It is flat across all three columns, so the slipping robot is not wandering: it drives the line it was given and simply does not get far enough along it. The failure is entirely along-track, which is why goal_error_true catches it and nothing lateral does.

The spread is the other half of it. goal_error_true varies by 0.026 m across seeds, less than the flat case’s 0.029 m, so the miss is not noise that a longer run would average out. It is a systematic bias, reproducible to the centimetre, which is what makes it dangerous rather than merely inaccurate.

Nav2 is not wrong to report success, and it is worth being precise about that. It was asked to drive the estimate to a pose and it did. The failure is that nothing in the stack is positioned to notice the estimate and the robot have parted company, because the only instrument that could notice is the fix, and the scenario removed it.

nav_terrain_dropout asserts on navigation_succeeded, which it did not always do. Nav2 used to return SUCCEEDED in only 5 of 8 runs, thrashing near the goal until the progress checker fired, and asserting either outcome would have made the test flaky. That variation turned out to be the controller being told it was a differential drive: given the robot’s real kinematics it settles, and now declares success in all 8 runs while 5.5 m from the goal. The finding survived the fix and got sharper, because a controller that reliably believes it arrived is worse news than one that sometimes gives up.

Why arriving is not the whole score

Goal error is an endpoint measurement, and an endpoint measurement says nothing about the journey. A controller that crosses the aisle twice on the way and a controller that holds the centre line produce the same number, and in an orchard those are not the same run: the aisle is 3.5 m between trunk lines and the canopy closes it to about 3 m, so lateral excursions are what hit trees. Three metrics cover the gap.

Cross-track error is the distance from ground truth to the path the planner laid down. The subtlety is which path. Nav2 replans continuously, and it plans from wherever the robot currently is, so the live plan is drawn through any wandering and measuring against it always reads near zero. The scorer therefore latches the first plan published for each goal and holds it, re-latching only when the plan’s endpoint moves — a new goal, not a replan of the current one.

Measuring truth against a plan expressed in the map frame conflates two things: how well the controller tracked, and how far the estimate had drifted. That is deliberate and it is the same choice goal_error_true makes. The question this metric answers is whether the machine physically went where the route said, not whether it thought it did.

Recovery count and time come from the behaviour server’s action status topics — backup, drive_on_heading, wait. Each invocation is the navigator admitting it is stuck, which is a cost that does not show up anywhere in goal error: a run that reaches the goal after backing up four times is a different result from one that drove straight through. Costmap clearing is a service call and leaves no trace on any topic, so it is not counted; the number is a floor on how much recovering happened, not a total.

Elapsed time is measured from the reference mark rather than from the start of the navigation goal, so open-loop scenarios get a duration too. Its most useful form is a lower bound: nav_terrain_dropout_radar asserts that the run takes at least 25 s, because a run that crosses slippery ground as fast as the baseline crossed flat ground did not actually encounter the terrain, and no goal error would notice that the scenario had stopped testing anything.

Upper time bounds are kept loose on purpose. These scenarios run at real_time_factor: 1.0, so the controller gets whatever compute is left over, and the same scenario measured 20 s on an idle machine and 69 s with the other nine running beside it. The bounds guard the phase timeout; the sweep tool, which runs one scenario at a time, is where durations are compared.

Why there is a ground-speed radar

The third column is what fixes it. A Doppler ground-speed radar bounces a beam off the ground and reads the returned shift, so it measures how fast the vehicle is travelling over the surface rather than how fast its wheels are turning. It is not exotic: agricultural machinery has carried these for decades, precisely because wheel slip in a field is the normal case rather than the fault case.

It is the only sensor in the stack that observes the quantity the bias lives in. GNSS would also catch it, but the scenario has removed GNSS, and that is the realistic case under canopy or in a headland shadow. The gyro sees rotation and is already correct. The encoders are, by construction, the thing being checked.

With it, true error falls from 5.494 m to 0.297 m and, more to the point, belief and truth agree again: 0.357 against 0.297, a gap of 0.06 m where before it was 5.26 m. The robot’s account of itself can be believed, which is a different and stronger property than the account being accurate. The true error also lands on the control’s 0.292 m, so the radar does not merely improve the slippery case, it recovers the flat one.

The run takes 64 s where the control takes 23 s. That is the honest outcome: the ground really is slippery and no sensor changes that, so covering 12.6 m takes longer. Arriving late is what success looks like here. Arriving in 23 s is what the version without the radar reports, and it is a lie.

An earlier version of this document claimed the radar run was the most reproducible of the three, and that fixing the estimate had stabilised the controller without touching it. Re-measuring after the controller was given the robot’s real kinematics reversed that: the unaided slippery case is now the tightest of the three, at 0.26 s and 0.05 m. The thrashing was never the estimate’s doing. It was the controller of the day, MPPI, planning for a robot that could pivot.

Why the radar replaces the wheels rather than joining them

The obvious change is to fuse the radar alongside the existing wheel velocity. It does not work, and the reason is worth stating because it generalises.

An EKF blends disagreeing measurements by inverse variance, so the sensor claiming to be more precise wins. The wheels report 0.01 m/s of noise and the radar 0.02, which weights the blend 4:1 towards the wheels and recovers about a fifth of a 0.44 m/s disagreement. Tightening the radar to 0.005 m/s still only reaches 0.45 m/s against a true 0.36.

The problem is not that the radar is too quiet. A covariance is a claim about accuracy, and under slip the encoders’ claim is false: they report 0.01 m/s of noise while being 0.44 m/s wrong. No quantity of correct evidence outvotes a confident lie, because confidence is exactly what the filter weighs by. So ekf_radar.yaml removes wheel forward speed rather than supplementing it. The wheels keep the nonholonomic constraint, which stays true under slip, because slip is forward and the constraint is sideways.

This is the one profile that is not a single measurement from its neighbour. It removes one and adds one, and the pairing is the point.

Why removing it turned out not to be necessary

The argument above tries tightening the honest sensor, finds it cannot rescue the blend, and concludes the lying measurement has to go. Only the first half of that follows. The other direction was never tried, and the same arithmetic that lets the smallest sigma dominate lets it stop dominating — with the wheels at 0.5 m/s against the radar’s 0.02:

\[\frac{0.8/0.5^2 + 0.36/0.02^2}{1/0.5^2 + 1/0.02^2} = 0.361\]

against a true 0.36. Inflating the liar works where tightening the honest one does not.

Which leaves the question of where a 0.5 m/s comes from, since an encoder cannot know it is slipping. It comes from the only local evidence there is: how far the wheels disagree with something that watches the ground. slip_monitor measures that, republishes wheel odometry with a covariance set by it, and publishes the disagreement as a slip ratio — a number a fruit picker could act on, and one this document previously listed as computed nowhere.

Same drive, same terrain, three profiles, eight seeds each:

profile speed comes from worst position error
robust wheels only, claiming 0.01 m/s while 0.44 m/s wrong 9.566 ± 0.036 m
radar wheels removed, radar instead 0.267 ± 0.185 m
crosscheck both, wheels widened by their disagreement 0.318 ± 0.139 m

The last two are the same answer to within their spread. A sensor does not have to be removed for lying; it has to stop lying. That generalises where deletion does not: a third opinion on ground speed joins by being cross-checked like the others, rather than by a decision about which of the existing two to throw away. That is what the camera does on O3DE, where there is no radar to remove anything in favour of — see localisation.

It also removes the case for innovation gating here. A gate asks only which measurement disagrees with the filter’s current belief, and that belief is dragged by whichever sensor is loudest — under slip, the 50 Hz wheels insisting they are precise. It would reject the honest sensor. Fix the covariances and the filter needs no referee.

What it costs. The cross-check is only as good as its reference, and that is now a stated assumption rather than an accident: comparing wheels against radar and widening the wheels encodes a decision that the radar is right. It measures speed over the ground directly rather than inferring it, which is the justification, but it is a judgement and not a proof. And when the reference fails there is no evidence either way — so the wheels widen rather than narrow, and hold the last disagreement they were shown rather than forgetting it. The ground does not start gripping because a sensor died.

Why a sensor is checked to be fused at all

The filter has no obligation to tell you it is ignoring something. robot_localization drops a measurement it cannot transform into the base frame, and reports that at RF_DEBUG — invisible at any normal log level. The filter goes on publishing at its configured rate, with a covariance that looks reasonable, and the only symptom is that one state quietly stops being observable.

That is what a transport bug did here: base_link → imu_link never reached the EKF, every angular-velocity measurement was discarded, and because ekf_robust.yaml takes yaw rate from the IMU alone, heading stopped being observable. robust produced naive’s numbers — 19.27 ± 6.43 m against 0.39 ± 0.24 m — and failed its own bound in nine runs out of ten. Nothing in the logs said why. It was found by attaching a debugger-grade trace to the filter and counting: 6717 measurements enqueued from the wheels, 6717 refused from the IMU.

The lesson is not about that bug, which is fixed. It is that the suite could not tell the difference between a filter that fused a sensor and one that threw it away, and neither could any amount of reading the numbers: they were plausible, they were stable, and they were describing a different stack than the one configured. Every scenario now asserts that each configured input both arrives and resolves. It is the cheapest possible check and it is unconditional, because there is no scenario in which discarding a configured sensor is the intended behaviour.

Why there are two filters

Until recently there was one, and the odom → base_link transform came straight out of the fault injector — raw wheel odometry, faults and all, with no gyro in it. Everything working in the odom frame inherited that, Nav2’s local costmap above all: the planner was avoiding obstacles positioned by dead reckoning.

The split is the arrangement robot_localization is designed for. The local filter fuses everything except GNSS, so the odom frame stays continuous and never jumps; the global filter adds GNSS and absorbs the jumps into map → odom, which is where REP-105 says they belong.

Measured across five scenarios at eight seeds each, before and after, it changes nothing it should not — nav_terrain_dropout, the closed-loop case that leans hardest on the odom frame, agrees to 8 mm (5.6567 ± 0.0219 m against 5.6492 ± 0.0212 m). That is the point: it is a correctness fix for the frame, not a tuning change, and a tuning change is exactly what it must not look like.

It is also where a visual odometry estimate goes. VO drifts, so it cannot be fused as an absolute pose against GNSS; the odom frame is the one place a drifting, continuous estimate is the correct thing to have. What is fused today is narrower than that — forward speed only, because the wheels lie about $v_x$ and nothing else — but the frame is the reason it can be fused at all.

Why the navigation scenarios run in real time

The localisation scenarios run at real_time_factor: 3.0. The navigation ones run at 1.0, and the difference is not a preference.

Nav2’s controller runs in wall time while the simulator advances sim time, so running the clock three times fast gives it a third of the compute per simulated second. Open-loop scenarios do not notice, because nothing in them reacts to the result. A closed loop does. It was measured under MPPI, which optimises by sampling rollouts and so felt it hardest — on flat ground it survived, because tracking a plan that works needs little search; under slip, where every rollout mispredicts, it did not, and the scenario became a measurement of the host’s spare CPU. The controller has since been replaced by ArcTracker, which is cheap, but the clock stays at 1.0 for closed-loop runs: the coupling is the point, not the particular controller.

This was found by accident. Adding the radar added two publishers, which was enough extra load to change nav_terrain_dropout from a tight 5.32 +/- 0.13 m to 3.95 +/- 2.29 m. The sensor could not have caused that, since the profile under test does not fuse it; the load could, and did. In real time the scenario returns to 5.33 +/- 0.08 m. The lesson is that a fast clock is free only for open-loop measurements.

The radar’s noise is also drawn from its own RNG stream, seeded separately, so that adding a sensor leaves every other sensor’s noise sequence untouched and the existing scenarios’ measured thresholds still mean what they meant. That held: total_gnss_dropout reproduces bit-for-bit, with a path-length spread of exactly zero across seeds.

Known gaps