test(fusion): guard the deadlock and calibration regressions, add noise tool

The existing suite passed both before and after the fusion fixes, so it
never covered either failure. Two tests close that gap; both were checked
against the pre-fix sources and fail there.

calibration_offset_test reproduces the bench part's +28 mg zero-g offset
and requires startup to complete, while still refusing a sustained 3 m/s2
acceleration. It also asserts the blind spot rather than leaving it
implicit: horizontal acceleration adds to gravity in quadrature, so
1.5 m/s2 moves the magnitude by only 0.11 m/s2 and is not separable from
offset by any accelerometer gate. Separating it needs GNSS velocity.

long_outage_trajectory_test drives pedestrian, gentle-car (0-100 km/h in
30 s), slow-city and isolated-mount profiles across 1 and 5 minute
outages, using the production rest supervisor rather than a hand-set
flag. Only the isolated-mount slow-city case reproduces the field bug:
GNSS motion evidence compares displacement against covariance, so 4 m/s
under 6 m sigma scores a displacement NIS of 0.22 against a 9.21 gate and
reads as stationary, whereas highway speed scores 42.9 and is correctly
called moving. Pre-fix that case produces 608 zero-velocity updates while
driving, matching the 609 in the board logs.

Drift is judged against the filter's own covariance rather than a fixed
distance. Five minutes unaided legitimately reaches kilometres on this
hardware; what must not happen is the error outrunning the covariance,
which is divergence rather than honest coasting.

tdk_noise_test characterises the sensor at rest through the production
driver, so ODR, full scale, UI filter and LSB scaling match the flight
build, and reports the norms fusion_motion_update() actually gates on.
It also carries an INT1-to-PLIC continuity test that drives the pin to a
static level and reads the PLIC pending bit directly, separating a broken
line from a source that never fires.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>

soc/software/fusion/tests/Makefile

@@ -10,7 +10,7 @@ IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c
 
 .PHONY: all test gps-system-test scenario-logs sanitize profile benchmark clean
 
-all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test
+all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test calibration_offset_test long_outage_trajectory_test
 
 standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(GPS_EPOCH_SOURCE)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
@@ -42,7 +42,13 @@ platform_scenario_matrix_test: platform_scenario_matrix_test.c $(ESKF_SOURCES)
 gps_failure_scenario_test: gps_failure_scenario_test.c $(ESKF_SOURCES)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
 
-test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test
+calibration_offset_test: calibration_offset_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+long_outage_trajectory_test: long_outage_trajectory_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test calibration_offset_test long_outage_trajectory_test
 	./standalone_gps_integrity_test
 	./eskf_motion_test
 	./fusion_rest_detector_test
@@ -53,6 +59,8 @@ test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test f
 	./gps_reacquisition_test
 	./platform_scenario_matrix_test
 	./gps_failure_scenario_test
+	./calibration_offset_test
+	./long_outage_trajectory_test
 
 gps-system-test: standalone_gps_integrity_test
 	./standalone_gps_integrity_test
@@ -146,4 +154,4 @@ benchmark: standalone_gps_integrity_test
 	./standalone_gps_integrity_test --bench-double 100000
 
 clean:
-	rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san gmon.out
+	rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san calibration_offset_test calibration_offset_test_san long_outage_trajectory_test long_outage_trajectory_test_san gmon.out

soc/software/fusion/tests/calibration_offset_test.c

@@ -0,0 +1,180 @@
+/*
+ * Startup calibration must complete on a sensor whose zero-g offset is within
+ * datasheet spec, and must still refuse to calibrate while the platform is
+ * accelerating.
+ *
+ * The bench ICM-42688-P on this project measures a stationary accelerometer
+ * magnitude of 10.0847 m/s^2 against a 9.80665 m/s^2 reference: a +28 mg
+ * zero-g offset, comfortably in spec.  When one tolerance served both as the
+ * sensor-offset allowance and as the motion gate, that offset consumed 93% of
+ * it and left 0.022 m/s^2 of motion headroom, so ordinary sensor noise reset
+ * the accumulator and the engine never left FUSION_ENGINE_CALIBRATING.  Field
+ * logs show exactly that: calibration_samples oscillating 0,3,0,4,2,1,0,7...
+ * and never reaching calibration_min_samples.
+ *
+ * Widening a single combined tolerance would have restored startup at the cost
+ * of the motion gate, so the offset case and the acceleration case are both
+ * asserted here.
+ */
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "adapters/imu_fusion_profile.h"
+#include "fusion/fusion_engine.h"
+#include "imu/imu_sensor.h"
+
+#define TIMEBASE_HZ 1000000u
+#define IMU_HZ 50u
+#define DURATION_S 8u
+#define GRAVITY_MPS2 9.80665
+
+/* Measured on the bench unit: see soc/software/tdk_noise_test. */
+#define MEASURED_Z_OFFSET_MPS2 0.278
+#define MEASURED_ACCEL_NOISE_MPS2 0.0028
+#define MEASURED_GYRO_NOISE_RPS 0.00016
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+static uint32_t random_state = 12345u;
+
+static double uniform_random(void)
+{
+	random_state = 1664525u * random_state + 1013904223u;
+	return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) / 16777216.0;
+}
+
+static double gaussian_random(void)
+{
+	double first = fmax(uniform_random(), 1.0e-12);
+	double second = uniform_random();
+	return sqrt(-2.0 * log(first)) * cos(2.0 * 3.14159265358979323846 * second);
+}
+
+/*
+ * Runs a stationary (or uniformly accelerating) calibration window and reports
+ * whether the engine reached the running state.
+ */
+static bool run_calibration(double z_offset_mps2,
+	                        double horizontal_acceleration_mps2,
+	                        imu_sensor_backend_t backend,
+	                        unsigned int* samples_out)
+{
+	fusion_imu_quality_profile_t profile;
+	fusion_engine_config_t config;
+	fusion_engine_t engine;
+
+	fusion_engine_default_config(&config, TIMEBASE_HZ);
+	if(!imu_fusion_quality_profile_for_backend(backend, &profile))
+	{
+		return false;
+	}
+	if(!fusion_engine_apply_imu_quality(&config, &profile))
+	{
+		return false;
+	}
+	if(!fusion_engine_init(&engine, &config))
+	{
+		return false;
+	}
+
+	random_state = 12345u;
+	unsigned int total = DURATION_S * IMU_HZ;
+	for(unsigned int i = 0u ; i < total ; i++)
+	{
+		fusion_imu_measurement_t imu;
+		memset(&imu, 0, sizeof(imu));
+		imu.timestamp_ticks =
+			(uint64_t)(i + 1u) * (uint64_t)(TIMEBASE_HZ / IMU_HZ);
+		imu.accel_mps2[0] = horizontal_acceleration_mps2 +
+			MEASURED_ACCEL_NOISE_MPS2 * gaussian_random();
+		imu.accel_mps2[1] = MEASURED_ACCEL_NOISE_MPS2 * gaussian_random();
+		imu.accel_mps2[2] = GRAVITY_MPS2 + z_offset_mps2 +
+			MEASURED_ACCEL_NOISE_MPS2 * gaussian_random();
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			imu.gyro_rps[axis] = MEASURED_GYRO_NOISE_RPS * gaussian_random();
+		}
+		imu.temperature_c = 25.0;
+		imu.sequence = i;
+		imu.vehicle_at_rest = true;
+		(void)fusion_engine_push_imu(&engine, &imu);
+	}
+
+	fusion_runtime_t runtime;
+	bool running = fusion_engine_get_runtime(&engine, &runtime);
+	if(samples_out != NULL)
+	{
+		*samples_out = engine.calibration.sample_count;
+	}
+	return running;
+}
+
+int main(void)
+{
+	unsigned int samples = 0u;
+
+	/* The regression: an in-spec zero-g offset must not block startup. */
+	CHECK(run_calibration(MEASURED_Z_OFFSET_MPS2, 0.0,
+	                      IMU_SENSOR_TDK_ICM42688, &samples),
+	      "TDK calibration must complete with the measured +28 mg zero-g offset");
+	printf("bench offset  z=%.3f m/s^2 accel_norm=%.4f -> %s\n",
+	       MEASURED_Z_OFFSET_MPS2, GRAVITY_MPS2 + MEASURED_Z_OFFSET_MPS2,
+	       failures == 0 ? "calibrated" : "STUCK");
+
+	/* A part right at the datasheet limit must still start. */
+	CHECK(run_calibration(0.392, 0.0, IMU_SENSOR_TDK_ICM42688, NULL),
+	      "TDK calibration must complete at the +-40 mg datasheet offset limit");
+
+	/* The same must hold for the ST backend. */
+	CHECK(run_calibration(MEASURED_Z_OFFSET_MPS2, 0.0,
+	                      IMU_SENSOR_ST_LSM6DSOX, NULL),
+	      "ST calibration must complete with an in-spec zero-g offset");
+
+	/*
+	 * The motion gate must survive the offset allowance.  Calibrating under
+	 * power would align attitude to a tilted specific-force vector, so a
+	 * sustained acceleration large enough to be visible in the magnitude has
+	 * to be rejected on both an ideal and an offset part.
+	 */
+	CHECK(!run_calibration(0.0, 3.0, IMU_SENSOR_TDK_ICM42688, NULL),
+	      "calibration must refuse a 3.0 m/s^2 sustained acceleration");
+	CHECK(!run_calibration(MEASURED_Z_OFFSET_MPS2, 3.0,
+	                       IMU_SENSOR_TDK_ICM42688, NULL),
+	      "calibration must refuse 3.0 m/s^2 on an offset part too");
+
+	/*
+	 * Documented blind spot, asserted so it stays a known quantity rather than
+	 * an assumption.  Horizontal acceleration adds to gravity in quadrature,
+	 * so 1.5 m/s^2 moves the magnitude by only 0.11 m/s^2 -- below any usable
+	 * offset allowance, and below the original 0.30 tolerance as well.  This
+	 * is not a regression; separating it needs GNSS velocity, not a tighter
+	 * accelerometer gate.
+	 */
+	CHECK(run_calibration(0.0, 1.5, IMU_SENSOR_TDK_ICM42688, NULL),
+	      "1.5 m/s^2 is expected to remain indistinguishable from offset");
+
+	/* A grossly wrong magnitude still indicates a misconfigured range. */
+	CHECK(!run_calibration(5.0, 0.0, IMU_SENSOR_TDK_ICM42688, NULL),
+	      "calibration must refuse an implausible accelerometer magnitude");
+
+	if(failures != 0)
+	{
+		fprintf(stderr, "FAIL: %d calibration offset check(s) failed\n",
+		        failures);
+		return 1;
+	}
+	printf("PASS: in-spec zero-g offset calibrates; sustained acceleration and "
+	       "implausible magnitude are still refused.\n");
+	return 0;
+}

soc/software/fusion/tests/long_outage_trajectory_test.c

@@ -0,0 +1,816 @@
+/*
+ * Pedestrian and gentle-vehicle trajectories across 1 and 5 minute GNSS
+ * outages.
+ *
+ * What this is actually checking
+ * -----------------------------
+ * Five minutes of inertial-only navigation on a consumer MEMS part drifts by
+ * hundreds of metres to kilometres.  That is physics, not a defect: a residual
+ * gyro bias of only 1.6e-5 rad/s -- what a 2 s static calibration leaves
+ * behind at this sensor's measured noise -- tilts the gravity projection by
+ * 0.27 deg over 300 s and integrates to roughly 2 km.  Asserting a small
+ * outage error would therefore only be asserting that the synthetic IMU is
+ * unrealistically good.
+ *
+ * The properties worth defending are the ones that separate honest dead
+ * reckoning from a filter that has come apart:
+ *
+ *   1. every published state stays finite, with no step discontinuities
+ *   2. drift accumulates smoothly rather than diverging super-quadratically
+ *   3. GNSS re-acquisition pulls the solution back within seconds
+ *   4. constant-velocity cruise is never mistaken for standing still
+ *
+ * Property 4 is the field failure this suite exists for.  A cruising vehicle
+ * produces almost no specific force and almost no angular rate, so a rest
+ * detector fed only inertial data declares rest.  These cases therefore assert
+ * vehicle_at_rest during constant-velocity cruise on purpose, reproducing that
+ * mistake, and require the filter to keep tracking anyway on the strength of
+ * GNSS Doppler velocity.
+ *
+ * Property 3 is the regression guard for the innovation-gate lockout: once the
+ * filter had drifted far enough for every fix to fail the NIS gate, nothing
+ * could re-anchor it and the reported position stayed on the dead-reckoned
+ * track indefinitely.
+ *
+ * IMU noise and bias below are the values measured on this project's bench
+ * ICM-42688-P; see soc/software/tdk_noise_test.
+ */
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "adapters/imu_fusion_profile.h"
+#include "fusion/fusion_engine.h"
+#include "fusion/fusion_rest_detector.h"
+#include "imu/imu_sensor.h"
+
+#define TIMEBASE_HZ 1000000u
+#define IMU_HZ 50u
+#define GPS_PERIOD_MS 200u
+#define PI 3.14159265358979323846
+
+/* Bench-measured stationary noise (soc/software/tdk_noise_test). */
+#define ACCEL_NOISE_MPS2 0.0028
+#define GYRO_NOISE_RPS 0.00016
+/* Residual bias left after a short static calibration. */
+#define ACCEL_BIAS_MPS2 0.010
+#define GYRO_BIAS_RPS 0.00004
+
+/*
+ * Broadband platform vibration.  A purely kinematic trajectory has none, which
+ * would flatter the rest detector.  These are set at the per-sample sigma the
+ * TDK profile's process noise already assumes at 50 Hz (0.125 m/s^2 and
+ * 0.011 rad/s), so the synthetic stream stays consistent with what the filter
+ * expects; injecting more would make the covariance check fail for modelling
+ * reasons rather than for filter behaviour.
+ */
+#define ROAD_VIBRATION_MPS2 0.125
+#define ROAD_VIBRATION_RPS 0.011
+
+#define GPS_HORIZONTAL_STD_M 3.0
+#define GPS_VERTICAL_STD_M 5.0
+#define GPS_VELOCITY_STD_MPS 0.3
+
+/* Timeline shared by every case. */
+#define STATIONARY_UNTIL_S 10.0
+#define ACCEL_START_S 10.0
+#define OUTAGE_START_S 90.0
+#define RECOVERY_OBSERVE_S 60.0
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+typedef enum {
+	PROFILE_PEDESTRIAN = 0,
+	PROFILE_GENTLE_CAR,
+	/*
+	 * The field failure's operating point.  GNSS motion evidence compares the
+	 * displacement between fixes against their combined covariance, so slow
+	 * travel under mediocre accuracy reads as stationary: 4 m/s over the 1 s
+	 * window is 4 m of displacement against 6 m per-fix sigma, a displacement
+	 * NIS of 0.22 versus a 9.21 gate.  Highway speed with good accuracy scores
+	 * 42.9 and is correctly called moving, which is why fast cruise alone does
+	 * not exercise the zero-velocity lockout.
+	 */
+	PROFILE_SLOW_CITY,
+	/*
+	 * The field configuration.  Board logs show 609 zero-velocity updates while
+	 * the vehicle was driving at 3-5 m/s, which only happens if the mounting
+	 * isolates the sensor from road vibration well enough for the rest
+	 * supervisor's vibration metric to stay under its limit.  Suppressing
+	 * vibration here reproduces that: the supervisor genuinely declares rest
+	 * while moving, and the engine must survive being told so.
+	 */
+	PROFILE_ISOLATED_CITY,
+	/*
+	 * Repeated hard course changes while unaided.  Answers whether the path
+	 * shape survives dead reckoning, separately from how far the whole path
+	 * drifts: heading comes from integrating a directly measured angular rate,
+	 * whereas position comes from double-integrating a specific force that
+	 * carries the attitude error times gravity.  The two degrade at very
+	 * different rates, so the turns are placed inside the outage window.
+	 */
+	PROFILE_URBAN_TURNS
+} profile_kind_t;
+
+#define PROFILE_COUNT 5u
+
+typedef struct {
+	double position_enu_m[3];
+	double velocity_enu_mps[3];
+	double acceleration_enu_mps2[3];
+	double yaw_rad;
+	double yaw_rate_rps;
+	double speed_mps;
+	bool constant_velocity;
+} truth_t;
+
+static uint32_t random_state;
+
+static double uniform_random(void)
+{
+	random_state = 1664525u * random_state + 1013904223u;
+	return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) / 16777216.0;
+}
+
+static double gaussian_random(void)
+{
+	double first = fmax(uniform_random(), 1.0e-12);
+	double second = uniform_random();
+	return sqrt(-2.0 * log(first)) * cos(2.0 * PI * second);
+}
+
+static const char* profile_name(profile_kind_t profile)
+{
+	switch(profile)
+	{
+		case PROFILE_PEDESTRIAN: return "pedestrian";
+		case PROFILE_GENTLE_CAR: return "gentle-car";
+		case PROFILE_SLOW_CITY: return "slow-city";
+		case PROFILE_ISOLATED_CITY: return "isolated-city";
+		default: return "urban-turns";
+	}
+}
+
+/* Slow urban travel is where the receiver reports its weakest geometry. */
+static double profile_gps_horizontal_std_m(profile_kind_t profile)
+{
+	return ((profile == PROFILE_SLOW_CITY) ||
+	        (profile == PROFILE_ISOLATED_CITY) ||
+	        (profile == PROFILE_URBAN_TURNS)) ? 6.0 : GPS_HORIZONTAL_STD_M;
+}
+
+/*
+ * Longitudinal speed and yaw rate.
+ *
+ * The vehicle case is the requested 0 to 100 km/h in 30 s, a constant
+ * 0.926 m/s^2, followed by open-road cruise.  Cruise is where specific force
+ * returns to gravity alone and the platform becomes inertially
+ * indistinguishable from parked.
+ */
+static void profile_command(profile_kind_t profile, double time_s,
+	                        double* speed_mps, double* yaw_rate_rps,
+	                        double* acceleration_mps2,
+	                        bool* constant_velocity)
+{
+	*yaw_rate_rps = 0.0;
+	*acceleration_mps2 = 0.0;
+	*constant_velocity = false;
+
+	if(time_s < STATIONARY_UNTIL_S)
+	{
+		*speed_mps = 0.0;
+		*constant_velocity = true;
+		return;
+	}
+
+	if(profile == PROFILE_PEDESTRIAN)
+	{
+		/* Ramp to a 1.4 m/s walk, then hold it with slow course changes. */
+		double elapsed = time_s - ACCEL_START_S;
+		if(elapsed < 3.0)
+		{
+			*speed_mps = 1.4 * (elapsed / 3.0);
+			*acceleration_mps2 = 1.4 / 3.0;
+			return;
+		}
+		*speed_mps = 1.4;
+		*constant_velocity = true;
+		/* A gentle serpentine keeps the gyro genuinely excited. */
+		*yaw_rate_rps = 0.05 * sin(2.0 * PI * (time_s - 13.0) / 40.0);
+		return;
+	}
+
+	if(profile == PROFILE_URBAN_TURNS)
+	{
+		double elapsed = time_s - ACCEL_START_S;
+		if(elapsed < 8.0)
+		{
+			*acceleration_mps2 = 4.0 / 8.0;
+			*speed_mps = *acceleration_mps2 * elapsed;
+			return;
+		}
+		*speed_mps = 4.0;
+		/* A 90 degree turn every 12 s, each swept over 3 s at 30 deg/s --
+		 * an ordinary city block, and a large signal next to the 0.0092 deg/s
+		 * gyro noise measured on this part. */
+		double cycle = fmod(time_s - 18.0, 12.0);
+		if((cycle >= 0.0) && (cycle < 3.0))
+		{
+			*yaw_rate_rps = (PI / 2.0) / 3.0;
+		}
+		else
+		{
+			*constant_velocity = true;
+		}
+		return;
+	}
+
+	if((profile == PROFILE_SLOW_CITY) || (profile == PROFILE_ISOLATED_CITY))
+	{
+		/* Gentle pull-away to a 4 m/s urban cruise. */
+		double elapsed = time_s - ACCEL_START_S;
+		if(elapsed < 8.0)
+		{
+			*acceleration_mps2 = 4.0 / 8.0;
+			*speed_mps = *acceleration_mps2 * elapsed;
+			return;
+		}
+		*speed_mps = 4.0;
+		*constant_velocity = true;
+		*yaw_rate_rps = 0.010 * sin(2.0 * PI * (time_s - 18.0) / 90.0);
+		return;
+	}
+
+	/* 0 -> 100 km/h (27.78 m/s) in 30 s, then constant-velocity cruise. */
+	double target_mps = 100.0 / 3.6;
+	double ramp_s = 30.0;
+	double elapsed = time_s - ACCEL_START_S;
+	if(elapsed < ramp_s)
+	{
+		*acceleration_mps2 = target_mps / ramp_s;
+		*speed_mps = *acceleration_mps2 * elapsed;
+		return;
+	}
+	*speed_mps = target_mps;
+	*constant_velocity = true;
+	/* A long highway curve; small enough that lateral acceleration stays
+	 * well under the rest detector's thresholds. */
+	*yaw_rate_rps = 0.004 * sin(2.0 * PI * (time_s - 40.0) / 120.0);
+}
+
+static void propagate_truth(truth_t* truth, profile_kind_t profile,
+	                        double start_s, double dt_s)
+{
+	if(dt_s <= 0.0)
+	{
+		return;
+	}
+	double speed_mps;
+	double yaw_rate_rps;
+	double acceleration_mps2;
+	bool constant_velocity;
+	profile_command(profile, start_s + 0.5 * dt_s, &speed_mps, &yaw_rate_rps,
+	                &acceleration_mps2, &constant_velocity);
+
+	truth->yaw_rad += yaw_rate_rps * dt_s;
+	truth->yaw_rate_rps = yaw_rate_rps;
+	truth->speed_mps = speed_mps;
+	truth->constant_velocity = constant_velocity;
+
+	double forward_e = cos(truth->yaw_rad);
+	double forward_n = sin(truth->yaw_rad);
+	double previous_velocity[3];
+	memcpy(previous_velocity, truth->velocity_enu_mps,
+	       sizeof(previous_velocity));
+
+	truth->velocity_enu_mps[0] = speed_mps * forward_e;
+	truth->velocity_enu_mps[1] = speed_mps * forward_n;
+	truth->velocity_enu_mps[2] = 0.0;
+
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		truth->acceleration_enu_mps2[axis] =
+			(truth->velocity_enu_mps[axis] - previous_velocity[axis]) / dt_s;
+		truth->position_enu_m[axis] += 0.5 * dt_s *
+			(previous_velocity[axis] + truth->velocity_enu_mps[axis]);
+	}
+	(void)acceleration_mps2;
+}
+
+/*
+ * Body-frame specific force and angular rate for a level platform.
+ *
+ * The accelerometer senses a - g_vec, so with ENU navigation and g_vec
+ * pointing down the vertical channel carries +g.  Yaw rotates the horizontal
+ * components into the body frame.
+ */
+static fusion_imu_measurement_t make_imu(
+	profile_kind_t profile, const truth_t* truth, double time_s,
+	uint64_t ticks, double gravity_mps2, unsigned int sequence)
+{
+	fusion_imu_measurement_t imu;
+	memset(&imu, 0, sizeof(imu));
+	imu.timestamp_ticks = ticks;
+
+	double cos_yaw = cos(truth->yaw_rad);
+	double sin_yaw = sin(truth->yaw_rad);
+	double east = truth->acceleration_enu_mps2[0];
+	double north = truth->acceleration_enu_mps2[1];
+
+	imu.accel_mps2[0] = east * cos_yaw + north * sin_yaw;
+	imu.accel_mps2[1] = -east * sin_yaw + north * cos_yaw;
+	imu.accel_mps2[2] = gravity_mps2;
+	imu.gyro_rps[0] = 0.0;
+	imu.gyro_rps[1] = 0.0;
+	imu.gyro_rps[2] = truth->yaw_rate_rps;
+
+	if((profile == PROFILE_PEDESTRIAN) && (truth->speed_mps > 0.1))
+	{
+		/* Step cadence near 1.9 Hz: strong vertical bob plus lateral sway.
+		 * This is the signature that keeps a walker out of the rest state. */
+		double phase = 2.0 * PI * 1.9 * time_s;
+		imu.accel_mps2[2] += 1.30 * sin(phase);
+		imu.accel_mps2[1] += 0.55 * sin(0.5 * phase);
+		imu.accel_mps2[0] += 0.30 * sin(phase + 0.7);
+		imu.gyro_rps[0] += 0.12 * sin(0.5 * phase);
+		imu.gyro_rps[1] += 0.09 * cos(phase);
+	}
+
+	bool rolling = truth->speed_mps > 0.5;
+	double mount = (profile == PROFILE_ISOLATED_CITY) ? 0.05 : 1.0;
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		imu.accel_mps2[axis] +=
+			ACCEL_BIAS_MPS2 + ACCEL_NOISE_MPS2 * gaussian_random();
+		imu.gyro_rps[axis] +=
+			GYRO_BIAS_RPS + GYRO_NOISE_RPS * gaussian_random();
+		if(rolling)
+		{
+			imu.accel_mps2[axis] +=
+				mount * ROAD_VIBRATION_MPS2 * gaussian_random();
+			imu.gyro_rps[axis] += mount * ROAD_VIBRATION_RPS * gaussian_random();
+		}
+	}
+	imu.temperature_c = 25.0;
+	imu.sequence = sequence;
+
+	/* vehicle_at_rest is filled in by the caller from the real supervisor. */
+	return imu;
+}
+
+static fusion_navigation_measurement_t make_navigation(
+	const truth_t* truth, uint64_t ticks, double horizontal_std_m)
+{
+	fusion_navigation_measurement_t navigation;
+	memset(&navigation, 0, sizeof(navigation));
+	navigation.position.timestamp_ticks = ticks;
+	navigation.position.position_enu_m[0] =
+		truth->position_enu_m[0] + horizontal_std_m * gaussian_random();
+	navigation.position.position_enu_m[1] =
+		truth->position_enu_m[1] + horizontal_std_m * gaussian_random();
+	navigation.position.position_enu_m[2] =
+		truth->position_enu_m[2] + GPS_VERTICAL_STD_M * gaussian_random();
+	navigation.position.covariance_enu_m2[0] = horizontal_std_m * horizontal_std_m;
+	navigation.position.covariance_enu_m2[4] = horizontal_std_m * horizontal_std_m;
+	navigation.position.covariance_enu_m2[8] =
+		GPS_VERTICAL_STD_M * GPS_VERTICAL_STD_M;
+	navigation.position.source_quality = 85.0;
+	navigation.position.source_valid = true;
+
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		navigation.velocity_enu_mps[axis] = truth->velocity_enu_mps[axis] +
+			GPS_VELOCITY_STD_MPS * gaussian_random();
+	}
+	navigation.velocity_covariance_enu_mps2[0] =
+		GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS;
+	navigation.velocity_covariance_enu_mps2[4] =
+		GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS;
+	navigation.velocity_covariance_enu_mps2[8] =
+		4.0 * GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS;
+	navigation.velocity_valid = true;
+	return navigation;
+}
+
+typedef struct {
+	double outage_max_error_m;
+	double error_at_outage_end_m;
+	double error_after_recovery_m;
+	double max_state_step_m;
+	double sigma_at_outage_end_m;
+	double heading_error_deg;
+	double truth_path_length_m;
+	double drift_sigma_ratio;
+	double max_cruise_speed_error_mps;
+	double recovery_seconds;
+	uint64_t zero_updates_during_cruise;
+	uint64_t false_rest_samples;
+	uint64_t gps_accepted;
+	uint64_t gps_rejected;
+	uint64_t recoveries;
+	bool finite;
+	bool recovered;
+	bool reached_running;
+} metrics_t;
+
+static metrics_t run_case(profile_kind_t profile, double outage_s)
+{
+	static fusion_engine_t engine;
+	metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	metrics.finite = true;
+
+	fusion_engine_config_t config;
+	fusion_imu_quality_profile_t quality;
+	fusion_engine_default_config(&config, TIMEBASE_HZ);
+	if(!imu_fusion_quality_profile_for_backend(
+		IMU_SENSOR_TDK_ICM42688, &quality) ||
+	   !fusion_engine_apply_imu_quality(&config, &quality))
+	{
+		return metrics;
+	}
+	memset(&engine, 0, sizeof(engine));
+	if(!fusion_engine_init(&engine, &config))
+	{
+		return metrics;
+	}
+
+	random_state = 0x51ed270bu ^ ((uint32_t)profile << 16) ^
+	               (uint32_t)(outage_s);
+	/* The same supervisor navigation_fusion.c feeds the engine, so whatever it
+	 * concludes about a smoothly cruising vehicle is what the engine sees. */
+	fusion_rest_detector_t rest_detector;
+	fusion_rest_detector_config_t rest_config;
+	fusion_rest_detector_default_config(&rest_config);
+	if(!fusion_rest_detector_init(&rest_detector, &rest_config, TIMEBASE_HZ, true))
+	{
+		return metrics;
+	}
+
+	truth_t truth;
+	memset(&truth, 0, sizeof(truth));
+
+	double outage_end_s = OUTAGE_START_S + outage_s;
+	double duration_s = outage_end_s + RECOVERY_OBSERVE_S;
+	uint64_t imu_period_ticks = TIMEBASE_HZ / IMU_HZ;
+	uint64_t gps_period_ticks = (uint64_t)GPS_PERIOD_MS * 1000u;
+	uint64_t end_ticks = (uint64_t)(duration_s * (double)TIMEBASE_HZ);
+	uint64_t next_imu_ticks = imu_period_ticks;
+	uint64_t next_gps_ticks = gps_period_ticks;
+	uint64_t last_ticks = 0u;
+	unsigned int sequence = 0u;
+
+	double previous_state[3] = {0.0, 0.0, 0.0};
+	bool have_previous_state = false;
+	double recovery_started_s = -1.0;
+	uint64_t zero_updates_at_cruise_start = 0u;
+	bool cruise_started = false;
+
+	while(true)
+	{
+		uint64_t ticks = (next_imu_ticks < next_gps_ticks)
+		               ? next_imu_ticks : next_gps_ticks;
+		if(ticks > end_ticks)
+		{
+			break;
+		}
+		double start_s = (double)last_ticks / (double)TIMEBASE_HZ;
+		double dt_s = (double)(ticks - last_ticks) / (double)TIMEBASE_HZ;
+		propagate_truth(&truth, profile, start_s, dt_s);
+		last_ticks = ticks;
+		double time_s = (double)ticks / (double)TIMEBASE_HZ;
+		bool in_outage = (time_s >= OUTAGE_START_S) && (time_s < outage_end_s);
+
+		if(ticks == next_gps_ticks)
+		{
+			if(!in_outage)
+			{
+				fusion_navigation_measurement_t navigation =
+					make_navigation(&truth, ticks,
+						profile_gps_horizontal_std_m(profile));
+				(void)fusion_engine_push_navigation(
+					&engine, &navigation, NULL, NULL, NULL, NULL);
+			}
+			next_gps_ticks += gps_period_ticks;
+		}
+
+		if(ticks == next_imu_ticks)
+		{
+			fusion_imu_measurement_t imu = make_imu(
+				profile, &truth, time_s, ticks, config.eskf.gravity_mps2,
+				sequence++);
+			imu_sample_t sample;
+			memset(&sample, 0, sizeof(sample));
+			sample.timestamp_ticks = imu.timestamp_ticks;
+			memcpy(sample.accel_mps2, imu.accel_mps2, sizeof(sample.accel_mps2));
+			memcpy(sample.gyro_rps, imu.gyro_rps, sizeof(sample.gyro_rps));
+			sample.temperature_c = imu.temperature_c;
+			sample.sequence = imu.sequence;
+			(void)fusion_rest_detector_update(&rest_detector, &sample);
+			imu.vehicle_at_rest = rest_detector.at_rest;
+			if(rest_detector.at_rest && (truth.speed_mps > 1.0))
+			{
+				metrics.false_rest_samples++;
+			}
+			(void)fusion_engine_push_imu(&engine, &imu);
+			next_imu_ticks += imu_period_ticks;
+		}
+
+		if(engine.state != FUSION_ENGINE_RUNNING)
+		{
+			continue;
+		}
+		metrics.reached_running = true;
+
+		fusion_runtime_t runtime;
+		if(!fusion_engine_get_runtime(&engine, &runtime))
+		{
+			continue;
+		}
+		const double* state = runtime.filter.position_m;
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			if(!isfinite(state[axis]))
+			{
+				metrics.finite = false;
+			}
+		}
+		if(!metrics.finite)
+		{
+			break;
+		}
+
+		double error = hypot(state[0] - truth.position_enu_m[0],
+		                     state[1] - truth.position_enu_m[1]);
+		if(have_previous_state)
+		{
+			double step = hypot(state[0] - previous_state[0],
+			                    state[1] - previous_state[1]);
+			/* Re-anchoring after an outage is a deliberate discontinuity;
+			 * only the free-running interval must stay smooth. */
+			if(in_outage && (step > metrics.max_state_step_m))
+			{
+				metrics.max_state_step_m = step;
+			}
+		}
+		memcpy(previous_state, state, sizeof(previous_state));
+		have_previous_state = true;
+
+		if(in_outage)
+		{
+			metrics.truth_path_length_m += truth.speed_mps * dt_s;
+			if(error > metrics.outage_max_error_m)
+			{
+				metrics.outage_max_error_m = error;
+			}
+			metrics.error_at_outage_end_m = error;
+			/*
+			 * Horizontal one-sigma from the filter's own error covariance
+			 * (position occupies error states 0..2).  Judging drift against
+			 * this rather than against a fixed distance is what distinguishes
+			 * honest dead reckoning from divergence: a filter that knows it is
+			 * uncertain is behaving; one whose error outruns its covariance is
+			 * not.  The absolute drift is dominated by whatever vibration and
+			 * bias the platform actually has, so no fixed metre bound
+			 * generalises across profiles.
+			 */
+			const double* q = runtime.filter.body_to_nav_quaternion;
+			double yaw = atan2(2.0 * (q[0] * q[3] + q[1] * q[2]),
+			                   1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3]));
+			double heading_error = yaw - truth.yaw_rad;
+			while(heading_error > PI) { heading_error -= 2.0 * PI; }
+			while(heading_error < -PI) { heading_error += 2.0 * PI; }
+			metrics.heading_error_deg = fabs(heading_error) * 180.0 / PI;
+			double sigma = sqrt(fmax(0.0, runtime.filter.covariance[0][0]) +
+			                    fmax(0.0, runtime.filter.covariance[1][1]));
+			metrics.sigma_at_outage_end_m = sigma;
+			metrics.drift_sigma_ratio = (sigma > 1.0e-6) ? (error / sigma) : 0.0;
+		}
+		else if(time_s >= outage_end_s)
+		{
+			if(recovery_started_s < 0.0)
+			{
+				recovery_started_s = time_s;
+			}
+			if(!metrics.recovered && (error <= 15.0))
+			{
+				metrics.recovered = true;
+				metrics.recovery_seconds = time_s - recovery_started_s;
+			}
+			metrics.error_after_recovery_m = error;
+		}
+
+		/* Cruise: constant velocity with GNSS present, the regime that used to
+		 * collapse into a false zero-velocity update.  The zero-update counter
+		 * is sampled across this window only; zero-velocity updates during the
+		 * genuinely stationary opening phase are correct and must not be
+		 * charged against it. */
+		if(!(truth.constant_velocity && (truth.speed_mps > 1.0) && !in_outage))
+		{
+			cruise_started = false;
+		}
+		/* Skip the re-convergence transient right after GNSS returns; that
+		 * interval is judged by the recovery checks instead. */
+		if(truth.constant_velocity && (truth.speed_mps > 1.0) && !in_outage &&
+		   ((time_s < OUTAGE_START_S) || (time_s > outage_end_s + 10.0)))
+		{
+			double speed_error =
+				fabs(eskf_speed_mps(&runtime.filter) - truth.speed_mps);
+			if(speed_error > metrics.max_cruise_speed_error_mps)
+			{
+				metrics.max_cruise_speed_error_mps = speed_error;
+			}
+			uint64_t zero_updates =
+				fusion_engine_diagnostics(&engine)->zero_velocity_update_count;
+			if(cruise_started)
+			{
+				metrics.zero_updates_during_cruise +=
+					zero_updates - zero_updates_at_cruise_start;
+			}
+			cruise_started = true;
+			zero_updates_at_cruise_start = zero_updates;
+		}
+	}
+
+	const fusion_engine_diagnostics_t* diagnostics =
+		fusion_engine_diagnostics(&engine);
+	metrics.gps_accepted = diagnostics->position_accepted;
+	metrics.gps_rejected = diagnostics->position_nis_rejected;
+	metrics.recoveries = diagnostics->position_recovery_count;
+	return metrics;
+}
+
+static void report(profile_kind_t profile, double outage_s,
+	               const metrics_t* metrics)
+{
+	printf("%-11s outage=%3.0fs  drift_max=%8.1fm drift_end=%8.1fm  "
+	       "recovered=%s in %4.1fs (err=%6.2fm)  cruise_speed_err=%5.2fm/s  "
+	       "sigma_end=%8.1fm err/sigma=%4.2f hdg_err=%5.2fdeg path=%7.0fm "
+	       "zupt=%lu gps=%lu/%lu recov=%lu step_max=%.2fm false_rest=%lu "
+	       "finite=%d\n",
+	       profile_name(profile), outage_s,
+	       metrics->outage_max_error_m, metrics->error_at_outage_end_m,
+	       metrics->recovered ? "yes" : "NO ", metrics->recovery_seconds,
+	       metrics->error_after_recovery_m,
+	       metrics->max_cruise_speed_error_mps,
+	       metrics->sigma_at_outage_end_m,
+	       metrics->drift_sigma_ratio,
+	       metrics->heading_error_deg,
+	       metrics->truth_path_length_m,
+	       (unsigned long)metrics->zero_updates_during_cruise,
+	       (unsigned long)metrics->gps_accepted,
+	       (unsigned long)metrics->gps_rejected,
+	       (unsigned long)metrics->recoveries,
+	       metrics->max_state_step_m,
+	       (unsigned long)metrics->false_rest_samples,
+	       metrics->finite ? 1 : 0);
+}
+
+int main(void)
+{
+	const profile_kind_t profiles[] = {
+		PROFILE_PEDESTRIAN, PROFILE_GENTLE_CAR, PROFILE_SLOW_CITY,
+		PROFILE_ISOLATED_CITY, PROFILE_URBAN_TURNS};
+	const double outages_s[] = {60.0, 300.0};
+
+	printf("Pedestrian and gentle-vehicle trajectories across GNSS outages\n");
+	printf("IMU 50 Hz, GNSS 5 Hz, bench-measured ICM-42688-P noise\n");
+
+	for(unsigned int p = 0u ; p < PROFILE_COUNT ; p++)
+	{
+		for(unsigned int o = 0u ; o < 2u ; o++)
+		{
+			profile_kind_t profile = profiles[p];
+			double outage_s = outages_s[o];
+			metrics_t metrics = run_case(profile, outage_s);
+			report(profile, outage_s, &metrics);
+
+			char message[192];
+
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: engine must finish calibration",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.reached_running, message);
+
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: every published state must stay finite",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.finite, message);
+
+			/*
+			 * Dead reckoning must stay smooth.  A step larger than a few
+			 * metres between consecutive IMU samples means the filter jumped,
+			 * not that it drifted.
+			 */
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: no position discontinuity while free-running",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.max_state_step_m < 5.0, message);
+
+			/*
+			 * Drift must stay consistent with the filter's own uncertainty.
+			 * Five minutes of inertial-only travel legitimately reaches
+			 * kilometres on this hardware; what must never happen is the error
+			 * outrunning the covariance, which is the signature of divergence
+			 * rather than of honest coasting.
+			 */
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: drift must stay within 5 sigma of the filter's "
+			         "own covariance", profile_name(profile), outage_s);
+			CHECK(metrics.drift_sigma_ratio < 5.0, message);
+
+			/* The covariance must actually grow while unaided, otherwise the
+			 * filter would stay overconfident and reject the returning fixes --
+			 * the innovation-gate lockout seen in the field. */
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: covariance must grow while unaided",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.sigma_at_outage_end_m > 10.0, message);
+
+			/*
+			 * Heading is integrated from a directly measured rate, so course
+			 * changes must survive an unaided minute far better than position
+			 * does.  This is what makes the dead-reckoned path the right shape
+			 * even once it has drifted a long way from the truth.
+			 */
+			if(outage_s <= 60.0)
+			{
+				/*
+				 * A walker is the hard case: gait swings the sensor through
+				 * large angles at frequencies a 50 Hz strapdown integration
+				 * resolves poorly, so heading degrades several times faster
+				 * than on a vehicle.  Pedestrian navigation is better served
+				 * by step detection than by strapdown integration, and this
+				 * looser bound records that rather than hiding it.
+				 */
+				double heading_limit_deg =
+					(profile == PROFILE_PEDESTRIAN) ? 10.0 : 5.0;
+				snprintf(message, sizeof(message),
+				         "%s/%.0fs: heading must survive the outage",
+				         profile_name(profile), outage_s);
+				CHECK(metrics.heading_error_deg < heading_limit_deg, message);
+			}
+
+			/* The lockout regression: GNSS must be able to re-anchor. */
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: must re-converge after GNSS returns",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.recovered, message);
+
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: re-convergence must take under 30 s",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.recovery_seconds < 30.0, message);
+
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: must hold position after re-converging",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.error_after_recovery_m < 25.0, message);
+
+			/*
+			 * The false-rest regression.  With GNSS present, a filter that
+			 * accepted the bogus at-rest flag would pin speed near zero while
+			 * the vehicle cruised at 27.8 m/s.
+			 */
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: cruise speed must track truth, not collapse",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.max_cruise_speed_error_mps < 3.0, message);
+
+			snprintf(message, sizeof(message),
+			         "%s/%.0fs: no zero-velocity update during aided cruise",
+			         profile_name(profile), outage_s);
+			CHECK(metrics.zero_updates_during_cruise == 0u, message);
+
+			if(profile == PROFILE_ISOLATED_CITY)
+			{
+				/* Precondition: without a genuinely fooled supervisor this
+				 * case would prove nothing, so assert the trap is armed. */
+				snprintf(message, sizeof(message),
+				         "%s/%.0fs: rest supervisor must actually be fooled "
+				         "for this case to mean anything",
+				         profile_name(profile), outage_s);
+				CHECK(metrics.false_rest_samples > 100u, message);
+			}
+		}
+	}
+
+	if(failures != 0)
+	{
+		fprintf(stderr, "FAIL: %d trajectory check(s) failed\n", failures);
+		return 1;
+	}
+	printf("PASS: pedestrian and gentle-vehicle trajectories stayed bounded "
+	       "through 1 and 5 minute outages and re-converged on GNSS return.\n");
+	return 0;
+}