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;
+}

soc/software/gps_system/header/adapters/gps_system_uart.h

@@ -17,6 +17,11 @@ void gps_system_uart_publish_location(
 void gps_system_uart_publish_fused_location(
 	const navigation_fusion_output_t* output);
 
+/* Raw Doppler velocity beside the ESKF velocity state, for tuning the
+ * velocity covariance floors and the zero-velocity veto threshold. */
+void gps_system_uart_publish_velocity(
+	const navigation_fusion_output_t* output);
+
 void gps_system_uart_publish_startup(
 	bool fusion_enabled,
 	bool fusion_initialized,

soc/software/gps_system/header/fusion/fusion_engine.h

@@ -30,6 +30,7 @@ typedef struct {
 	double calibration_duration_s;
 	unsigned int calibration_min_samples;
 	double calibration_accel_norm_tolerance_mps2;
+	double calibration_accel_offset_allowance_mps2;
 	double calibration_gyro_limit_rps;
 	double calibration_accel_std_limit_mps2;
 	double calibration_gyro_std_limit_rps;
@@ -44,6 +45,7 @@ typedef struct {
 	double position_recovery_timeout_s;
 	double position_recovery_max_speed_mps;
 	double position_recovery_sigma_multiplier;
+	double gnss_velocity_motion_timeout_s;
 	unsigned int position_recovery_min_samples;
 } fusion_engine_config_t;
 
@@ -54,16 +56,33 @@ typedef struct {
 	fusion_gnss_motion_evidence_t gnss_evidence;
 	double last_accepted_position_m[3];
 	double last_accepted_position_covariance_m2[9];
+	/*
+	 * Displacement baseline for GNSS motion evidence.  Held separately from
+	 * the accepted-fix baseline because the evidence must keep updating while
+	 * the innovation gate is rejecting fixes -- that is precisely when it is
+	 * the only signal able to release the zero-velocity update.
+	 */
+	double last_evidence_position_m[3];
+	double last_evidence_position_covariance_m2[9];
+	double last_gnss_speed_mps;
 	double zero_gyro_integral_rad[3];
 	double zero_gyro_duration_s;
 	uint64_t filter_timestamp_ticks;
 	uint64_t last_zero_update_ticks;
 	uint64_t last_accepted_position_ticks;
+	uint64_t last_evidence_position_ticks;
+	uint64_t last_gnss_velocity_ticks;
 	uint64_t last_position_fuse_ticks;
 	unsigned int gnss_moving_count;
 	unsigned int gnss_still_count;
 	bool have_held_imu;
 	bool have_accepted_position;
+	bool have_evidence_position;
+	bool have_gnss_velocity;
+	/* Latched once GNSS Doppler has shown the platform moving, and cleared
+	 * only by a later fix showing it slow.  A timeout cannot clear it: an
+	 * unaided platform gives no evidence that it has stopped. */
+	bool gnss_confirmed_moving;
 	bool vehicle_at_rest;
 } fusion_runtime_t;
 
@@ -96,6 +115,11 @@ typedef struct {
 	double gyro_mean[3];
 	double accel_m2[3];
 	double gyro_m2[3];
+	/* Running statistics of the accelerometer magnitude.  Comparing a sample
+	 * against the window's own mean cancels any fixed sensor offset, which
+	 * leaves the whole norm tolerance available to detect real acceleration. */
+	double norm_mean;
+	double norm_m2;
 	unsigned int sample_count;
 	bool have_start;
 } fusion_calibration_accumulator_t;

soc/software/gps_system/header/fusion/fusion_imu_quality.h

@@ -22,7 +22,29 @@ typedef struct {
 	double gyro_bias_random_walk_rps_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT];
 	double calibration_accel_std_limit_mps2;
 	double calibration_gyro_std_limit_rps;
+	/*
+	 * How far the stationary accelerometer magnitude may sit from gravity
+	 * purely because of the part's zero-g offset and sensitivity tolerance.
+	 * This is a fixed per-unit property, not motion headroom:
+	 * finish_calibration() estimates it as accel bias and hands it to the
+	 * ESKF, so admitting it here costs nothing.  Sized from the datasheet
+	 * zero-g offset; a unit near this limit is worth characterizing.
+	 */
+	double calibration_accel_offset_allowance_mps2;
 	double zero_gyro_measurement_std_rps[FUSION_IMU_QUALITY_AXIS_COUNT];
+	/*
+	 * Rest-detector thresholds.  These separate genuine platform motion from
+	 * the sensor's own residual noise, so they scale with the part rather than
+	 * with the vehicle: a quieter IMU sits below a fixed quiescence limit more
+	 * of the time and therefore declares rest more readily.  The speed and
+	 * dwell thresholds stay in fusion_motion_default_config() because they
+	 * describe the platform, not the sensor.
+	 */
+	double stationary_accel_limit_mps2;
+	double stationary_gyro_limit_rps;
+	double moving_accel_threshold_mps2;
+	double moving_gyro_threshold_rps;
+	double moving_jerk_threshold_mps3;
 } fusion_imu_quality_profile_t;
 
 void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile);

soc/software/gps_system/header/fusion/navigation_fusion.h

@@ -13,7 +13,20 @@ typedef struct {
 	uint64_t timestamp_ticks;
 	uint64_t last_accepted_gps_ticks;
 	double position_enu_m[3];
+	/* ESKF velocity state.  An IMU cannot measure speed -- this is the
+	 * integrated result, corrected by GNSS, and is the only meaningful
+	 * "inertial speed" the system has. */
 	double velocity_enu_mps[3];
+	/*
+	 * Last GNSS Doppler velocity in the same ENU frame, held so telemetry can
+	 * difference it against the state above.  Characterising the receiver's
+	 * velocity noise needs the raw observation, not the filtered estimate.
+	 */
+	double gps_velocity_enu_mps[3];
+	double gps_speed_mps;
+	double gps_velocity_residual_rms_mps;
+	uint64_t gps_velocity_sample_ticks;
+	bool gps_velocity_valid;
 	double latitude_deg;
 	double longitude_deg;
 	double height_m;

soc/software/gps_system/src/adapters/gps_system_uart.c

@@ -64,6 +64,50 @@ void gps_system_uart_publish_fused_location(
 	        output->height_m, (unsigned long)age_ms);
 }
 
+/*
+ * Velocity telemetry for tuning.
+ *
+ * An IMU does not measure speed; it measures specific force.  eskf_* below is
+ * the integrated velocity state after GNSS correction, which is the only
+ * inertial speed the system has, and gps_* is the raw Doppler observation that
+ * corrected it.  Logging both in one line, in one frame, at one instant is
+ * what makes them differenceable -- reading them from separate records at
+ * different rates does not.
+ *
+ * The difference is the velocity innovation.  With the platform held still it
+ * is the receiver's Doppler noise, which is what sizes the velocity covariance
+ * floors and the zero-velocity veto threshold; in motion it is the residual
+ * the filter is correcting.
+ */
+void gps_system_uart_publish_velocity(const navigation_fusion_output_t* output)
+{
+	if((output == NULL) || !output->local_valid || !output->gps_velocity_valid)
+	{
+		return;
+	}
+	double innovation[3];
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		innovation[axis] = output->gps_velocity_enu_mps[axis] -
+		                   output->velocity_enu_mps[axis];
+	}
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
+	        "vel gps_e=%.3lf gps_n=%.3lf gps_u=%.3lf gps_speed=%.3lf "
+	        "gps_rms=%.3lf eskf_e=%.3lf eskf_n=%.3lf eskf_u=%.3lf "
+	        "eskf_speed=%.3lf inn_e=%.3lf inn_n=%.3lf inn_u=%.3lf "
+	        "vel_nis=%.3lf accepted=%lu rejected=%lu at_rest=%u\r\n",
+	        output->gps_velocity_enu_mps[0], output->gps_velocity_enu_mps[1],
+	        output->gps_velocity_enu_mps[2], output->gps_speed_mps,
+	        output->gps_velocity_residual_rms_mps,
+	        output->velocity_enu_mps[0], output->velocity_enu_mps[1],
+	        output->velocity_enu_mps[2], output->speed_mps,
+	        innovation[0], innovation[1], innovation[2],
+	        output->last_velocity_nis,
+	        (unsigned long)output->gps_velocity_accepted,
+	        (unsigned long)output->gps_velocity_nis_rejected,
+	        output->vehicle_at_rest ? 1u : 0u);
+}
+
 void gps_system_uart_publish_startup(
 	bool fusion_enabled,
 	bool fusion_initialized,
@@ -139,6 +183,7 @@ void gps_system_uart_publish_periodic_fusion_status(
 		        (unsigned long)output->imu_processed_count,
 		        (unsigned long)imu.queue_overflow_count,
 		        (unsigned long)imu.estimated_missed_sample_count);
+		gps_system_uart_publish_velocity(output);
 		return;
 	}
 

soc/software/gps_system/src/adapters/imu_fusion_profile.c

@@ -44,8 +44,50 @@ bool imu_fusion_quality_profile_for_backend(
 			profile_out->calibration_accel_std_limit_mps2 = 0.09;
 			profile_out->calibration_gyro_std_limit_rps =
 				0.10 * PROFILE_DEGREES_TO_RADIANS;
+			/*
+			 * ICM-42688-P zero-g offset is specified at +-40 mg (0.392 m/s^2).
+			 * The bench unit measured 0.278 m/s^2 (+28 mg): in spec, but it
+			 * consumed 93% of the old combined 0.30 tolerance and left the
+			 * motion gate no usable headroom, so startup calibration stalled.
+			 *
+			 * 0.42 is deliberately only just above the datasheet limit.  This
+			 * allowance also sets how much sustained acceleration calibration
+			 * cannot distinguish from offset, so widening it further would
+			 * trade startup robustness for a worse initial attitude.
+			 */
+			profile_out->calibration_accel_offset_allowance_mps2 = 0.42;
 			set_all(profile_out->zero_gyro_measurement_std_rps,
 			        0.02 * PROFILE_DEGREES_TO_RADIANS);
+			/*
+			 * Rest-detector limits from a stationary bench capture of this
+			 * board (soc/software/tdk_noise_test, 3000 samples at the
+			 * production 50 Hz / +-4 g / +-250 dps / UI filter 0x77), reported
+			 * as the norms fusion_motion_update() actually gates on:
+			 *
+			 *   accel_norm  p99.9 = 0.014 m/s^2   max = 0.0154 m/s^2
+			 *   gyro_norm   p99.9 = 0.040 deg/s   max = 0.0417 deg/s
+			 *   jerk_norm   p99.9 = 0.10 m/s^3    max = 0.0915 m/s^3
+			 *
+			 * Gyro and jerk sit far above their noise floors here because they
+			 * are measured directly: 6x p99.9 still leaves room for gyro bias
+			 * drift between ESKF updates, and a static attitude error cancels
+			 * in the jerk derivative.  Jerk is therefore the most sensitive
+			 * motion discriminator available and is tightened hardest.
+			 *
+			 * The accel limit deliberately keeps far more headroom than its
+			 * 0.014 m/s^2 noise floor would suggest.  linear_accel_nav carries
+			 * the attitude error times gravity, so a 0.1 deg tilt error alone
+			 * contributes 0.017 m/s^2 -- more than the entire sensor noise
+			 * budget.  Sizing this limit to the noise floor would break rest
+			 * detection whenever attitude estimation is slightly off.
+			 */
+			profile_out->stationary_accel_limit_mps2 = 0.1125;
+			profile_out->stationary_gyro_limit_rps =
+				0.25 * PROFILE_DEGREES_TO_RADIANS;
+			profile_out->moving_accel_threshold_mps2 = 0.30;
+			profile_out->moving_gyro_threshold_rps =
+				0.75 * PROFILE_DEGREES_TO_RADIANS;
+			profile_out->moving_jerk_threshold_mps3 = 0.60;
 			break;
 
 		case IMU_SENSOR_ST_LSM6DSOX:
@@ -63,8 +105,19 @@ bool imu_fusion_quality_profile_for_backend(
 			profile_out->calibration_accel_std_limit_mps2 = 0.12;
 			profile_out->calibration_gyro_std_limit_rps =
 				0.20 * PROFILE_DEGREES_TO_RADIANS;
+			/* LSM6DSOX zero-g offset is specified at +-40 mg over temperature. */
+			profile_out->calibration_accel_offset_allowance_mps2 = 0.42;
 			set_all(profile_out->zero_gyro_measurement_std_rps,
 			        0.04 * PROFILE_DEGREES_TO_RADIANS);
+			/* Unchanged from the historical fusion_motion defaults; these are
+			 * the field-tuned values this platform was validated against. */
+			profile_out->stationary_accel_limit_mps2 = 0.15;
+			profile_out->stationary_gyro_limit_rps =
+				1.00 * PROFILE_DEGREES_TO_RADIANS;
+			profile_out->moving_accel_threshold_mps2 = 0.40;
+			profile_out->moving_gyro_threshold_rps =
+				3.00 * PROFILE_DEGREES_TO_RADIANS;
+			profile_out->moving_jerk_threshold_mps3 = 4.0;
 			break;
 
 		default:

soc/software/gps_system/src/drivers/imu/tdk_icm42688.c

@@ -30,6 +30,8 @@
 #define ICM42688_WHO_AM_I       0x47u
 #define ICM_PWR_LOW_NOISE       0x0fu
 #define ICM_INT1_LATCHED_HIGH_PUSH_PULL 0x07u
+/* DEVICE_CONFIG bit 0: self-clearing soft reset. */
+#define ICM_SOFT_RESET          0x01u
 #define ICM_UI_DRDY_INT1_EN     0x08u
 
 #define TDK_GRAVITY_MPS2 9.80665
@@ -224,6 +226,33 @@ bool tdk_icm42688_init(const tdk_icm42688_config_t* config)
 	bsp_pmod_spi_set_clock_mode(BSP_PMOD_SPI_CLOCK_MODE0);
 	bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u);
 	delay_us(1000u);
+
+	/*
+	 * Recover a device left mid-transfer before probing it.
+	 *
+	 * Halting the core with a debugger, or resetting it while a transaction is
+	 * in flight, leaves the sensor's SPI shift register part-way through a
+	 * word.  Every later access is then misaligned, WHO_AM_I reads back 0x00,
+	 * and initialisation fails until the board is power cycled -- the
+	 * `sensor=none who=0x00` case seen in field logs.  Toggling chip select
+	 * around idle bytes returns the device to command state, after which a
+	 * soft reset restores the register file to defaults so this function
+	 * starts from the same place regardless of what ran before it.
+	 */
+	for(unsigned int attempt = 0u ; attempt < 4u ; attempt++)
+	{
+		bsp_pmod_spi_select();
+		(void)bsp_pmod_spi_xfer8(ICM_SPI_DUMMY);
+		bsp_pmod_spi_deselect();
+		delay_us(50u);
+	}
+	write_register(ICM_DEVICE_CONFIG, ICM_SOFT_RESET);
+	/* Datasheet requires 1 ms before any register access after a soft reset. */
+	delay_ms(2u);
+	/* The reset asserts RESET_DONE; clear it so it cannot be mistaken for a
+	 * data-ready later. */
+	(void)read_register(ICM_INT_STATUS);
+
 	diagnostics.who_am_i = read_register(ICM_WHO_AM_I);
 	if((diagnostics.who_am_i != ICM42688_WHO_AM_I) ||
 	   (read_register(ICM_DEVICE_CONFIG) != 0x00u))

soc/software/gps_system/src/fusion/fusion_engine.c

@@ -71,6 +71,16 @@ void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile)
 	}
 	profile->calibration_accel_std_limit_mps2 = 0.12;
 	profile->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
+	/* About 46 mg: covers a typical consumer MEMS zero-g offset plus its
+	 * sensitivity tolerance without assuming a specific part. */
+	profile->calibration_accel_offset_allowance_mps2 = 0.42;
+	/* Matches fusion_motion_default_config() so an unprofiled backend keeps
+	 * the historical rest-detector behaviour. */
+	profile->stationary_accel_limit_mps2 = 0.15;
+	profile->stationary_gyro_limit_rps = 1.0 * FUSION_PI / 180.0;
+	profile->moving_accel_threshold_mps2 = 0.40;
+	profile->moving_gyro_threshold_rps = 3.0 * FUSION_PI / 180.0;
+	profile->moving_jerk_threshold_mps3 = 4.0;
 }
 
 bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile)
@@ -79,10 +89,38 @@ bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile)
 	   !isfinite(profile->calibration_accel_std_limit_mps2) ||
 	   !(profile->calibration_accel_std_limit_mps2 > 0.0) ||
 	   !isfinite(profile->calibration_gyro_std_limit_rps) ||
-	   !(profile->calibration_gyro_std_limit_rps > 0.0))
+	   !(profile->calibration_gyro_std_limit_rps > 0.0) ||
+	   !isfinite(profile->calibration_accel_offset_allowance_mps2) ||
+	   !(profile->calibration_accel_offset_allowance_mps2 > 0.0))
 	{
 		return false;
 	}
+	{
+		const double motion[] = {
+			profile->stationary_accel_limit_mps2,
+			profile->stationary_gyro_limit_rps,
+			profile->moving_accel_threshold_mps2,
+			profile->moving_gyro_threshold_rps,
+			profile->moving_jerk_threshold_mps3
+		};
+		for(unsigned int value = 0u ;
+		    value < sizeof(motion) / sizeof(motion[0]) ; value++)
+		{
+			if(!isfinite(motion[value]) || !(motion[value] > 0.0))
+			{
+				return false;
+			}
+		}
+		/* A quiescence limit at or above the motion threshold would let the
+		 * detector call the same sample both stationary and moving. */
+		if(!(profile->stationary_accel_limit_mps2 <
+		     profile->moving_accel_threshold_mps2) ||
+		   !(profile->stationary_gyro_limit_rps <
+		     profile->moving_gyro_threshold_rps))
+		{
+			return false;
+		}
+	}
 	for(unsigned int axis = 0u ; axis < 3u ; axis++)
 	{
 		const double values[] = {
@@ -120,6 +158,9 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
 	config->calibration_duration_s = 2.0;
 	config->calibration_min_samples = 50u;
 	config->calibration_accel_norm_tolerance_mps2 = 0.30;
+	/* Fixed sensor offset, kept separate from the motion tolerance above so
+	 * that a healthy part's zero-g offset cannot consume the motion budget. */
+	config->calibration_accel_offset_allowance_mps2 = 0.42;
 	config->calibration_gyro_limit_rps = 2.0 * FUSION_PI / 180.0;
 	config->calibration_accel_std_limit_mps2 = 0.12;
 	config->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
@@ -143,6 +184,15 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
 	config->position_recovery_max_speed_mps = 100.0;
 	config->position_recovery_sigma_multiplier = 5.0;
 	config->position_recovery_min_samples = 3u;
+	/*
+	 * How long an accepted GNSS Doppler velocity keeps vetoing zero-velocity
+	 * updates.  Doppler velocity is observed independently of the position
+	 * state, so it stays trustworthy while the position innovation gate is
+	 * rejecting fixes.  At a 200 ms solve period this tolerates several missed
+	 * epochs yet still lapses quickly enough that a genuine stop is not held
+	 * out of ZUPT for long.
+	 */
+	config->gnss_velocity_motion_timeout_s = 1.5;
 }
 
 bool fusion_engine_apply_imu_quality(
@@ -170,6 +220,18 @@ bool fusion_engine_apply_imu_quality(
 		profile->calibration_accel_std_limit_mps2;
 	config->calibration_gyro_std_limit_rps =
 		profile->calibration_gyro_std_limit_rps;
+	config->calibration_accel_offset_allowance_mps2 =
+		profile->calibration_accel_offset_allowance_mps2;
+	config->motion.stationary_accel_limit_mps2 =
+		profile->stationary_accel_limit_mps2;
+	config->motion.stationary_gyro_limit_rps =
+		profile->stationary_gyro_limit_rps;
+	config->motion.moving_accel_threshold_mps2 =
+		profile->moving_accel_threshold_mps2;
+	config->motion.moving_gyro_threshold_rps =
+		profile->moving_gyro_threshold_rps;
+	config->motion.moving_jerk_threshold_mps3 =
+		profile->moving_jerk_threshold_mps3;
 	return true;
 }
 
@@ -185,6 +247,8 @@ static bool config_valid(const fusion_engine_config_t* config)
 	       (config->calibration_min_samples >= 2u) &&
 	       isfinite(config->calibration_accel_norm_tolerance_mps2) &&
 	       (config->calibration_accel_norm_tolerance_mps2 > 0.0) &&
+	       isfinite(config->calibration_accel_offset_allowance_mps2) &&
+	       (config->calibration_accel_offset_allowance_mps2 > 0.0) &&
 	       isfinite(config->calibration_gyro_limit_rps) &&
 	       (config->calibration_gyro_limit_rps > 0.0) &&
 	       isfinite(config->zero_update_period_s) &&
@@ -202,6 +266,8 @@ static bool config_valid(const fusion_engine_config_t* config)
 	       (config->position_recovery_max_speed_mps > 0.0) &&
 	       isfinite(config->position_recovery_sigma_multiplier) &&
 	       (config->position_recovery_sigma_multiplier > 0.0) &&
+	       isfinite(config->gnss_velocity_motion_timeout_s) &&
+	       (config->gnss_velocity_motion_timeout_s > 0.0) &&
 	       (config->position_recovery_min_samples >= 2u);
 }
 
@@ -257,6 +323,10 @@ static void calibration_add(fusion_calibration_accumulator_t* calibration,
 {
 	calibration->sample_count++;
 	double count = (double)calibration->sample_count;
+	double norm = norm3(measurement->accel_mps2);
+	double norm_delta = norm - calibration->norm_mean;
+	calibration->norm_mean += norm_delta / count;
+	calibration->norm_m2 += norm_delta * (norm - calibration->norm_mean);
 	for(unsigned int axis = 0 ; axis < 3u ; axis++)
 	{
 		double accel_delta = measurement->accel_mps2[axis] - calibration->accel_mean[axis];
@@ -273,9 +343,33 @@ static void calibration_add(fusion_calibration_accumulator_t* calibration,
 static bool calibration_sample_is_quiet(const fusion_engine_t* engine,
 	                                    const fusion_imu_measurement_t* measurement)
 {
-	double accel_error = fabs(norm3(measurement->accel_mps2) - engine->config.eskf.gravity_mps2);
+	double norm = norm3(measurement->accel_mps2);
-	return (accel_error <= engine->config.calibration_accel_norm_tolerance_mps2) &&
+	/*
-	       (norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps);
+	 * Plausibility.  A healthy part's zero-g offset shifts this magnitude
+	 * permanently, so the allowance for it is added to -- not taken out of --
+	 * the motion tolerance.  What remains rejects a misconfigured full-scale
+	 * range, a dead axis or a mounting fault.
+	 */
+	if(fabs(norm - engine->config.eskf.gravity_mps2) >
+	   (engine->config.calibration_accel_offset_allowance_mps2 +
+	    engine->config.calibration_accel_norm_tolerance_mps2))
+	{
+		return false;
+	}
+	/*
+	 * Motion.  Measuring against the window's own mean magnitude rather than
+	 * against gravity makes this test independent of the fixed offset, so the
+	 * full tolerance detects acceleration instead of being spent on a
+	 * per-unit constant.  A slow ramp that tracks the mean still fails the
+	 * per-axis standard-deviation gate before calibration completes.
+	 */
+	if((engine->calibration.sample_count != 0u) &&
+	   (fabs(norm - engine->calibration.norm_mean) >
+	    engine->config.calibration_accel_norm_tolerance_mps2))
+	{
+		return false;
+	}
+	return norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps;
 }
 
 static bool calibration_statistics_are_quiet(const fusion_engine_t* engine)
@@ -317,11 +411,51 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 	                          const fusion_imu_measurement_t* measurement,
 	                          bool count_zero_updates);
 
+/* Age of the most recent accepted GNSS Doppler velocity, or a negative value
+ * when there is none to age. */
+static double gnss_velocity_age_s(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime)
+{
+	if(!runtime->have_gnss_velocity ||
+	   (runtime->filter_timestamp_ticks < runtime->last_gnss_velocity_ticks))
+	{
+		return -1.0;
+	}
+	return ticks_to_seconds(
+		engine,
+		runtime->filter_timestamp_ticks - runtime->last_gnss_velocity_ticks);
+}
+
+static bool gnss_velocity_is_fresh(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime)
+{
+	double age_s = gnss_velocity_age_s(engine, runtime);
+	return (age_s >= 0.0) &&
+	       (age_s <= engine->config.gnss_velocity_motion_timeout_s);
+}
+
 static bool runtime_allows_zero_velocity_update(
 	const fusion_engine_t* engine,
 	const fusion_runtime_t* runtime)
 {
-	(void)engine;
+	/*
+	 * A cruising vehicle produces almost no specific force and almost no
+	 * angular rate, so inertial data alone cannot distinguish constant
+	 * velocity from rest.  Doppler velocity can, and it survives a position
+	 * innovation-gate lockout because it is observed independently of the
+	 * position state.  Without this veto a false ZUPT pins filter velocity to
+	 * zero, which keeps eskf_speed_mps() below the rest-detector speed limit
+	 * and latches the mistake.
+	 */
+	if(runtime->gnss_confirmed_moving ||
+	   (gnss_velocity_is_fresh(engine, runtime) &&
+	    (runtime->last_gnss_speed_mps >=
+	     engine->config.gnss_motion_speed_threshold_mps)))
+	{
+		return false;
+	}
 	return runtime->vehicle_at_rest &&
 	       (runtime->gnss_evidence != FUSION_GNSS_MOTION_MOVING);
 }
@@ -339,7 +473,21 @@ static bool runtime_allows_position_recovery(
 	const fusion_engine_t* engine,
 	const fusion_runtime_t* runtime)
 {
-	return runtime_allows_zero_gyro_update(engine, runtime);
+	/* Stationary re-anchoring: recovers a bad startup position. */
+	if(runtime_allows_zero_gyro_update(engine, runtime))
+	{
+		return true;
+	}
+	/*
+	 * Moving re-anchoring.  Gating recovery on being at rest made the escape
+	 * hatch unreachable in exactly the case that needs it, because divergence
+	 * large enough to lock the innovation gate happens while driving.  A
+	 * mobile reset still needs aiding that is not derived from the locked-out
+	 * position state: accepted Doppler velocity supplies it, and
+	 * position_recovery_is_continuous() separately establishes that the
+	 * candidate GNSS track is self-consistent before any reset occurs.
+	 */
+	return gnss_velocity_is_fresh(engine, runtime);
 }
 
 static void history_push(fusion_engine_t* engine,
@@ -564,6 +712,25 @@ static bool finish_calibration(fusion_engine_t* engine,
 		calibration_reset(engine);
 		return false;
 	}
+	/*
+	 * A zero-g offset is constant, so it shows up in the window mean and
+	 * nowhere else; a sustained acceleration does too.  Bounding the mean is
+	 * therefore the only test that separates a tolerable sensor offset from
+	 * an intolerable one, and it is what rejects calibrating under power.
+	 *
+	 * The two cases are not fully separable: any allowance A accepts sustained
+	 * accelerations whose magnitude shift stays under A.  Horizontal
+	 * acceleration adds in quadrature, so A = 0.42 m/s^2 still admits roughly
+	 * 2.9 m/s^2 of it -- the same blind spot the original tolerance had, since
+	 * 1.5 m/s^2 shifts this magnitude by only 0.11 m/s^2.  A part whose offset
+	 * approaches A needs stored per-unit calibration rather than a wider gate.
+	 */
+	if(fabs(gravity_norm - engine->config.eskf.gravity_mps2) >
+	   engine->config.calibration_accel_offset_allowance_mps2)
+	{
+		calibration_reset(engine);
+		return false;
+	}
 	double nav_up[3] = {0.0, 0.0, 1.0};
 	double quaternion[4];
 	if(!eskf_quaternion_align_vectors(calibration->accel_mean, nav_up, quaternion))
@@ -611,6 +778,15 @@ static bool finish_calibration(fusion_engine_t* engine,
 		memcpy(engine->current.last_accepted_position_covariance_m2,
 		       engine->initial_position.covariance_enu_m2,
 		       sizeof(engine->current.last_accepted_position_covariance_m2));
+		engine->current.have_evidence_position = true;
+		engine->current.last_evidence_position_ticks =
+			engine->initial_position.timestamp_ticks;
+		memcpy(engine->current.last_evidence_position_m,
+		       engine->initial_position.position_enu_m,
+		       sizeof(engine->current.last_evidence_position_m));
+		memcpy(engine->current.last_evidence_position_covariance_m2,
+		       engine->initial_position.covariance_enu_m2,
+		       sizeof(engine->current.last_evidence_position_covariance_m2));
 	}
 	engine->checkpoint = engine->current;
 	engine->checkpoint_valid = true;
@@ -659,13 +835,13 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
 	{
 		*window_complete_out = false;
 	}
-	if(!runtime->have_accepted_position ||
+	if(!runtime->have_evidence_position ||
-	   (measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
+	   (measurement->timestamp_ticks <= runtime->last_evidence_position_ticks))
 	{
 		return FUSION_GNSS_MOTION_UNKNOWN;
 	}
 	double dt_s = ticks_to_seconds(
-		engine, measurement->timestamp_ticks - runtime->last_accepted_position_ticks);
+		engine, measurement->timestamp_ticks - runtime->last_evidence_position_ticks);
 	if(!(dt_s > 0.0))
 	{
 		return FUSION_GNSS_MOTION_UNKNOWN;
@@ -678,16 +854,16 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
 	{
 		*window_complete_out = true;
 	}
-	double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0];
+	double delta_e = measurement->position_enu_m[0] - runtime->last_evidence_position_m[0];
-	double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1];
+	double delta_n = measurement->position_enu_m[1] - runtime->last_evidence_position_m[1];
 	double s00 = measurement->covariance_enu_m2[0] +
-	             runtime->last_accepted_position_covariance_m2[0];
+	             runtime->last_evidence_position_covariance_m2[0];
 	double s01 = measurement->covariance_enu_m2[1] +
-	             runtime->last_accepted_position_covariance_m2[1];
+	             runtime->last_evidence_position_covariance_m2[1];
 	double s10 = measurement->covariance_enu_m2[3] +
-	             runtime->last_accepted_position_covariance_m2[3];
+	             runtime->last_evidence_position_covariance_m2[3];
 	double s11 = measurement->covariance_enu_m2[4] +
-	             runtime->last_accepted_position_covariance_m2[4];
+	             runtime->last_evidence_position_covariance_m2[4];
 	double off_diagonal = 0.5 * (s01 + s10);
 	double determinant = s00 * s11 - off_diagonal * off_diagonal;
 	if(!(s00 > 0.0) || !(s11 > 0.0) || !(determinant > DBL_EPSILON) ||
@@ -819,6 +995,15 @@ static void runtime_record_recovered_position(
 	memcpy(runtime->last_accepted_position_covariance_m2,
 	       measurement->covariance_enu_m2,
 	       sizeof(runtime->last_accepted_position_covariance_m2));
+	/* The re-anchor discards the old position track, so the displacement
+	 * baseline must restart here too rather than measure across the jump. */
+	runtime->have_evidence_position = true;
+	runtime->last_evidence_position_ticks = measurement->timestamp_ticks;
+	memcpy(runtime->last_evidence_position_m, measurement->position_enu_m,
+	       sizeof(runtime->last_evidence_position_m));
+	memcpy(runtime->last_evidence_position_covariance_m2,
+	       measurement->covariance_enu_m2,
+	       sizeof(runtime->last_evidence_position_covariance_m2));
 	runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
 	runtime->gnss_moving_count = 0u;
 	runtime->gnss_still_count = 0u;
@@ -833,50 +1018,66 @@ static eskf_update_result_t runtime_apply_position(
 	eskf_update_result_t result = eskf_position_update(
 		&runtime->filter, &engine->config.eskf,
 		measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
+
+	/*
+	 * Motion evidence is a displacement test between raw GNSS fixes and does
+	 * not depend on the filter state, so it is derived from every fix rather
+	 * than only from accepted ones.  Updating it solely on acceptance froze it
+	 * for as long as the innovation gate rejected fixes, which deadlocked the
+	 * one signal that could veto a false ZUPT or authorise a re-anchor.
+	 */
+	bool motion_window_complete = false;
+	fusion_gnss_motion_evidence_t raw_evidence =
+		position_motion_evidence(engine, runtime, measurement,
+		                         &motion_window_complete);
+	if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
+	{
+		runtime->gnss_still_count = 0u;
+		if(runtime->gnss_moving_count < UINT32_MAX)
+		{
+			runtime->gnss_moving_count++;
+		}
+		if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations)
+		{
+			runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING;
+		}
+	}
+	else if(raw_evidence == FUSION_GNSS_MOTION_STILL)
+	{
+		runtime->gnss_moving_count = 0u;
+		if(runtime->gnss_still_count < UINT32_MAX)
+		{
+			runtime->gnss_still_count++;
+		}
+		if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations)
+		{
+			runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
+		}
+	}
+	/* Keep a long-enough displacement baseline.  At a 200 ms GNSS period,
+	 * replacing the reference on every fix would classify statistically
+	 * inconclusive 10--20 cm steps as stationary evidence. */
+	if(!runtime->have_evidence_position || motion_window_complete)
+	{
+		runtime->have_evidence_position = true;
+		runtime->last_evidence_position_ticks = measurement->timestamp_ticks;
+		memcpy(runtime->last_evidence_position_m, measurement->position_enu_m,
+		       sizeof(runtime->last_evidence_position_m));
+		memcpy(runtime->last_evidence_position_covariance_m2,
+		       measurement->covariance_enu_m2,
+		       sizeof(runtime->last_evidence_position_covariance_m2));
+	}
+
 	if(result == ESKF_UPDATE_ACCEPTED)
 	{
 		runtime->last_position_fuse_ticks = measurement->timestamp_ticks;
-		bool motion_window_complete = false;
+		runtime->have_accepted_position = true;
-		fusion_gnss_motion_evidence_t raw_evidence =
+		runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
-			position_motion_evidence(engine, runtime, measurement,
+		memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
-			                         &motion_window_complete);
+		       sizeof(runtime->last_accepted_position_m));
-		if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
+		memcpy(runtime->last_accepted_position_covariance_m2,
-		{
+		       measurement->covariance_enu_m2,
-			runtime->gnss_still_count = 0u;
+		       sizeof(runtime->last_accepted_position_covariance_m2));
-			if(runtime->gnss_moving_count < UINT32_MAX)
-			{
-				runtime->gnss_moving_count++;
-			}
-			if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations)
-			{
-				runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING;
-			}
-		}
-		else if(raw_evidence == FUSION_GNSS_MOTION_STILL)
-		{
-			runtime->gnss_moving_count = 0u;
-			if(runtime->gnss_still_count < UINT32_MAX)
-			{
-				runtime->gnss_still_count++;
-			}
-			if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations)
-			{
-				runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
-			}
-		}
-		/* Keep a long-enough displacement baseline.  At a 200 ms GNSS period,
-		 * replacing the reference on every fix would classify statistically
-		 * inconclusive 10--20 cm steps as stationary evidence. */
-		if(!runtime->have_accepted_position || motion_window_complete)
-		{
-			runtime->have_accepted_position = true;
-			runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
-			memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
-			       sizeof(runtime->last_accepted_position_m));
-			memcpy(runtime->last_accepted_position_covariance_m2,
-			       measurement->covariance_enu_m2,
-			       sizeof(runtime->last_accepted_position_covariance_m2));
-		}
 	}
 	return result;
 }
@@ -972,6 +1173,41 @@ static bool process_navigation_now(
 	eskf_update_result_t velocity_result = ESKF_UPDATE_INVALID;
 	if(measurement->velocity_valid)
 	{
+		/*
+		 * Taken from the measurement rather than from the update outcome.
+		 * Doppler speed is observed independently of the filter, so a filter
+		 * that has drifted far enough to reject it is precisely the case this
+		 * signal exists to break.  Gating it on acceptance rebuilt the same
+		 * circular dependency that deriving motion evidence from accepted
+		 * fixes did: a false zero-velocity update pins filter velocity near
+		 * zero, the resulting innovation then fails the NIS gate, and the one
+		 * observation able to veto that update is never recorded.
+		 *
+		 * Recorded before the IMU replay below so the zero-velocity and
+		 * position-recovery gates see this epoch's speed.  Trusting a
+		 * spurious high speed only suppresses a zero-velocity update, which
+		 * forgoes a drift correction; trusting a spurious low one re-opens the
+		 * lockout, so the asymmetry is deliberate.
+		 */
+		if(finite_values(measurement->velocity_enu_mps, 3u))
+		{
+			revised.last_gnss_speed_mps = norm3(measurement->velocity_enu_mps);
+			revised.last_gnss_velocity_ticks = position->timestamp_ticks;
+			revised.have_gnss_velocity = true;
+			/*
+			 * Latched rather than aged out.  Losing GNSS produces no evidence
+			 * that the platform stopped, and a mount that isolates road
+			 * vibration leaves the rest supervisor asserting rest throughout.
+			 * Letting the veto lapse on a timer therefore resumed zero-velocity
+			 * updates mid-journey, which clamps the position covariance to
+			 * metres while the true error grows to kilometres -- the
+			 * overconfidence that makes returning fixes fail the innovation
+			 * gate.  Only a later fix showing low speed clears this.
+			 */
+			revised.gnss_confirmed_moving =
+				revised.last_gnss_speed_mps >=
+				engine->config.gnss_motion_speed_threshold_mps;
+		}
 		velocity_result = eskf_velocity_update(
 			&revised.filter, &engine->config.eskf,
 			measurement->velocity_enu_mps,

soc/software/gps_system/src/fusion/navigation_fusion.c

@@ -12,6 +12,13 @@ static fusion_engine_t fusion_engine;
 static fusion_rest_detector_t rest_detector;
 static bool fusion_initialized;
 static uint64_t imu_processed_count;
+/* Last Doppler velocity handed to the filter, in ENU.  Kept so telemetry can
+ * report the raw observation alongside the state it corrected. */
+static double last_gps_velocity_enu_mps[3];
+static double last_gps_speed_mps;
+static double last_gps_velocity_rms_mps;
+static uint64_t last_gps_velocity_ticks;
+static bool last_gps_velocity_valid;
 
 bool navigation_fusion_init(
 	uint64_t local_timebase_hz,
@@ -96,6 +103,15 @@ bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement)
 	{
 		return false;
 	}
+	last_gps_velocity_valid = navigation.velocity_valid;
+	if(navigation.velocity_valid)
+	{
+		memcpy(last_gps_velocity_enu_mps, navigation.velocity_enu_mps,
+		       sizeof(last_gps_velocity_enu_mps));
+		last_gps_speed_mps = measurement->speed_mps;
+		last_gps_velocity_rms_mps = measurement->velocity_residual_rms_mps;
+		last_gps_velocity_ticks = measurement->timestamp_ticks;
+	}
 	return fusion_engine_push_navigation(
 		&fusion_engine, &navigation, NULL, NULL, NULL, NULL);
 }
@@ -141,6 +157,12 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
 			sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
 	}
 	output->speed_mps = eskf_speed_mps(&runtime->filter);
+	memcpy(output->gps_velocity_enu_mps, last_gps_velocity_enu_mps,
+	       sizeof(output->gps_velocity_enu_mps));
+	output->gps_speed_mps = last_gps_speed_mps;
+	output->gps_velocity_residual_rms_mps = last_gps_velocity_rms_mps;
+	output->gps_velocity_sample_ticks = last_gps_velocity_ticks;
+	output->gps_velocity_valid = last_gps_velocity_valid;
 	output->linear_acceleration_norm_mps2 =
 		runtime->motion.last_linear_accel_norm_mps2;
 	output->last_position_nis = runtime->filter.last_position_nis;

soc/software/tdk_noise_test/Makefile

@@ -0,0 +1,39 @@
+TARGET := tdk_noise_test
+
+GPS_SYSTEM_DIR ?= ../gps_system
+
+# The production driver is compiled in directly so the sensor is configured
+# exactly as the flight build configures it.
+vpath %.c $(GPS_SYSTEM_DIR)/src/drivers/imu
+
+APP_SRCS := \
+	main.c \
+	tdk_icm42688.c
+
+BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c
+
+# Genesys2 fabric/UART and CLINT defaults, matching the gps_system app.
+BSP_UART_CLOCK_HZ ?= 50000000
+BSP_UART_BAUDRATE ?= 460800
+BSP_CLINT_TIMEBASE_HZ ?= 500000
+
+# Mirrors gps_system IMU_ODR_HZ; override together with it.
+NOISE_ODR_HZ ?= 50
+NOISE_DISCARD_SAMPLES ?= 100
+NOISE_BIAS_SAMPLES ?= 1000
+NOISE_MEASURE_SAMPLES ?= 3000
+# Set to 1 to refuse the polled fallback, so a broken INT1 route fails loudly.
+NOISE_REQUIRE_IRQ ?= 0
+# Set to 1 to run only the INT1 -> PLIC continuity test and exit.
+NOISE_PIN_TEST ?= 0
+
+include ../common/app.mk
+
+CFLAGS += -I$(GPS_SYSTEM_DIR)/header
+CFLAGS += -DNOISE_ODR_HZ=$(NOISE_ODR_HZ)u \
+	-DNOISE_DISCARD_SAMPLES=$(NOISE_DISCARD_SAMPLES)u \
+	-DNOISE_BIAS_SAMPLES=$(NOISE_BIAS_SAMPLES)u \
+	-DNOISE_MEASURE_SAMPLES=$(NOISE_MEASURE_SAMPLES)u \
+	-DNOISE_REQUIRE_IRQ=$(NOISE_REQUIRE_IRQ) \
+	-DNOISE_PIN_TEST=$(NOISE_PIN_TEST)
+LDLIBS += -lm

soc/software/tdk_noise_test/main.c

@@ -0,0 +1,805 @@
+/*
+ * Stationary noise characterization for the TDK ICM-42688-P.
+ *
+ * The board must stay still for the whole run.
+ *
+ * This app drives the production tdk_icm42688 driver, so ODR, full-scale
+ * ranges, UI filter configuration and LSB scaling are identical to the flight
+ * build.  Measuring through a hand-rolled register setup would characterize a
+ * different bandwidth and produce thresholds that do not transfer.
+ *
+ * It reports exactly the three quantities fusion_motion_update() compares
+ * against its thresholds:
+ *
+ *   accel_norm = || accel - mean(accel) ||   linear acceleration at rest, i.e.
+ *                                            what remains once the ESKF has
+ *                                            absorbed gravity and accel bias
+ *   gyro_norm  = || gyro  - mean(gyro)  ||   bias-removed angular rate
+ *   jerk_norm  = || d/dt IIR(accel_res) ||   same 0.10 s time constant the
+ *                                            detector uses
+ *
+ * A stationary limit must sit above the observed quiescent distribution or the
+ * platform is never recognised as still; a motion threshold must sit above it
+ * by enough margin that noise alone cannot trip it.  Both therefore follow
+ * from the numbers printed here rather than from datasheet noise densities,
+ * which describe the bare part and not this board, mounting or bandwidth.
+ *
+ * All values print as integers in micro-units to keep the report independent
+ * of newlib float formatting: um/s^2, urad/s, um/s^3.
+ */
+
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "bsp/irq.h"
+#include "bsp/mmio.h"
+#include "bsp/plic.h"
+#include "bsp/pmod_spi.h"
+#include "bsp/soc.h"
+#include "bsp/uart.h"
+
+#include "drivers/imu/tdk_icm42688.h"
+
+/* Mirrors soc/software/gps_system/Makefile IMU_ODR_HZ. */
+#ifndef NOISE_ODR_HZ
+#define NOISE_ODR_HZ 50u
+#endif
+/* Discards sensor and UI-filter settling after start. */
+#ifndef NOISE_DISCARD_SAMPLES
+#define NOISE_DISCARD_SAMPLES 100u
+#endif
+#ifndef NOISE_BIAS_SAMPLES
+#define NOISE_BIAS_SAMPLES 1000u
+#endif
+#ifndef NOISE_MEASURE_SAMPLES
+#define NOISE_MEASURE_SAMPLES 3000u
+#endif
+
+/* Matches fusion_motion_default_config()->jerk_filter_time_constant_s. */
+#define JERK_TIME_CONSTANT_S 0.10
+
+#define HISTOGRAM_BINS 400u
+/* Full-scale of each histogram; samples above it still count in the last bin
+ * and the exact maximum is tracked separately, so an overflow is visible. */
+#define ACCEL_HISTOGRAM_FULL_SCALE 0.40   /* m/s^2  -> 1.0 mm/s^2 per bin */
+#define GYRO_HISTOGRAM_FULL_SCALE 0.040   /* rad/s  -> 100 urad/s per bin */
+#define JERK_HISTOGRAM_FULL_SCALE 8.0     /* m/s^3  -> 20 mm/s^3 per bin */
+
+typedef struct {
+	const char* name;
+	const char* unit;
+	double full_scale;
+	double sum;
+	double sum_squares;
+	double maximum;
+	uint32_t count;
+	uint32_t overflow;
+	uint32_t bins[HISTOGRAM_BINS];
+} statistic_t;
+
+static volatile uint64_t unexpected_irq_count;
+static volatile uint64_t unexpected_trap_cause;
+
+/*
+ * Must carry interrupt("machine"): without it GCC ends the function with ret
+ * instead of mret, so mstatus.MIE -- cleared by hardware on trap entry -- is
+ * never restored and exactly one interrupt is ever taken.  aligned(4) is
+ * required because mtvec holds the address.
+ */
+void tdk_noise_trap_handler(void)
+	__attribute__((interrupt("machine"), aligned(4)));
+
+void tdk_noise_trap_handler(void)
+{
+	asm volatile(
+		".option push\n"
+		".option norelax\n"
+		"la gp, __global_pointer$\n"
+		".option pop\n"
+		: : : "memory");
+
+	const uint64_t mcause = bsp_read_mcause();
+	if(((mcause >> 63) == 0u) ||
+	   ((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL))
+	{
+		unexpected_trap_cause = mcause;
+		return;
+	}
+	const uint32_t irq_id = bsp_plic_claim();
+	if(irq_id == 0u)
+	{
+		__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
+		return;
+	}
+	if(irq_id == BSP_PMOD_IRQ)
+	{
+		tdk_icm42688_irq_handler();
+	}
+	else
+	{
+		__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
+	}
+	bsp_plic_complete(irq_id);
+}
+
+static void statistic_init(statistic_t* statistic, const char* name,
+	                       const char* unit, double full_scale)
+{
+	memset(statistic, 0, sizeof(*statistic));
+	statistic->name = name;
+	statistic->unit = unit;
+	statistic->full_scale = full_scale;
+}
+
+static void statistic_add(statistic_t* statistic, double value)
+{
+	statistic->sum += value;
+	statistic->sum_squares += value * value;
+	if(value > statistic->maximum)
+	{
+		statistic->maximum = value;
+	}
+	statistic->count++;
+	double scaled = value / statistic->full_scale * (double)HISTOGRAM_BINS;
+	if(!(scaled >= 0.0))
+	{
+		scaled = 0.0;
+	}
+	unsigned int bin = (unsigned int)scaled;
+	if(bin >= HISTOGRAM_BINS)
+	{
+		bin = HISTOGRAM_BINS - 1u;
+		statistic->overflow++;
+	}
+	statistic->bins[bin]++;
+}
+
+/* Upper edge of the bin holding the requested quantile. */
+static double statistic_percentile(const statistic_t* statistic,
+	                               double fraction)
+{
+	if(statistic->count == 0u)
+	{
+		return 0.0;
+	}
+	uint32_t target = (uint32_t)(fraction * (double)statistic->count);
+	uint32_t cumulative = 0u;
+	for(unsigned int bin = 0u ; bin < HISTOGRAM_BINS ; bin++)
+	{
+		cumulative += statistic->bins[bin];
+		if(cumulative >= target)
+		{
+			return (double)(bin + 1u) * statistic->full_scale /
+			       (double)HISTOGRAM_BINS;
+		}
+	}
+	return statistic->full_scale;
+}
+
+static long micro(double value)
+{
+	double scaled = value * 1.0e6;
+	if(scaled > 2.0e9)
+	{
+		scaled = 2.0e9;
+	}
+	if(scaled < -2.0e9)
+	{
+		scaled = -2.0e9;
+	}
+	return (long)scaled;
+}
+
+static void statistic_report(const statistic_t* statistic)
+{
+	double mean = (statistic->count != 0u)
+	            ? statistic->sum / (double)statistic->count : 0.0;
+	double variance = (statistic->count > 1u)
+	                ? (statistic->sum_squares / (double)statistic->count) -
+	                  (mean * mean)
+	                : 0.0;
+	if(variance < 0.0)
+	{
+		variance = 0.0;
+	}
+	printf("STAT,%s,unit=%s,n=%lu,mean=%ld,std=%ld,"
+	       "p50=%ld,p95=%ld,p99=%ld,p999=%ld,max=%ld,overflow=%lu\n",
+	       statistic->name, statistic->unit,
+	       (unsigned long)statistic->count,
+	       micro(mean), micro(sqrt(variance)),
+	       micro(statistic_percentile(statistic, 0.50)),
+	       micro(statistic_percentile(statistic, 0.95)),
+	       micro(statistic_percentile(statistic, 0.99)),
+	       micro(statistic_percentile(statistic, 0.999)),
+	       micro(statistic->maximum),
+	       (unsigned long)statistic->overflow);
+}
+
+/*
+ * Polled fallback.
+ *
+ * The data-ready INT1 route is not delivering on this bitstream (the driver
+ * reports a single spurious IRQ and then silence), so the interrupt path
+ * cannot feed the characterization.  Polling INT_STATUS reads the identical
+ * registers the ISR would read, at the identical ODR, full scale and UI
+ * filter, so the noise statistics are unaffected; only the delivery mechanism
+ * differs.  Sample timestamps still come from CLINT.
+ */
+#define ICM_INT_STATUS_REG 0x2du
+#define ICM_TEMP_DATA1_REG 0x1du
+#define ICM_SPI_READ_BIT 0x80u
+#define ICM_SPI_DUMMY_BYTE 0xffu
+#define ICM_DATA_READY_BIT 0x08u
+
+static bool polled_mode;
+
+static int16_t big_endian_i16(const uint8_t* bytes)
+{
+	return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
+}
+
+static bool poll_for_sample(imu_sample_t* sample, uint64_t deadline)
+{
+	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
+	{
+		uint8_t status = bsp_pmod_spi_read_reg8(
+			ICM_INT_STATUS_REG | ICM_SPI_READ_BIT, ICM_SPI_DUMMY_BYTE);
+		if((status & ICM_DATA_READY_BIT) == 0u)
+		{
+			continue;
+		}
+		uint64_t timestamp = bsp_read64(BSP_CLINT_MTIME);
+		uint8_t bytes[14];
+		bsp_pmod_spi_read_regs(ICM_TEMP_DATA1_REG | ICM_SPI_READ_BIT,
+		                       ICM_SPI_DUMMY_BYTE, bytes, sizeof(bytes));
+		/* Same scaling the driver applies: full scale over a 15-bit half range. */
+		double accel_scale = 4.0 * 9.80665 / 32768.0;
+		double gyro_scale = 250.0 * 0.017453292519943295769236907684886 / 32768.0;
+		sample->timestamp_ticks = timestamp;
+		sample->temperature_c = (double)big_endian_i16(&bytes[0]) / 132.48 + 25.0;
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			sample->accel_mps2[axis] =
+				(double)big_endian_i16(&bytes[2u + 2u * axis]) * accel_scale;
+			sample->gyro_rps[axis] =
+				(double)big_endian_i16(&bytes[8u + 2u * axis]) * gyro_scale;
+		}
+		return true;
+	}
+	return false;
+}
+
+/*
+ * Why the data-ready interrupt stopped after the first one.
+ *
+ * Three causes produce the same silence and are told apart by whether the
+ * sensor is still generating a data-ready condition at all:
+ *
+ *   sensor stopped   PWR_MGMT0 or the ODR fields no longer hold their
+ *                    configured values, and DRDY never sets
+ *   routing lost     DRDY sets in INT_STATUS but INT_SOURCE0 no longer routes
+ *                    it to the pin
+ *   latch stuck      DRDY sets and routing is intact, so the pin is asserted
+ *                    but the PLIC is not seeing a fresh edge/level
+ */
+/*
+ * Continuity test for the sensor INT1 -> FPGA pmod_irq -> PLIC path.
+ *
+ * Everything measured so far depends on the sensor raising a data-ready edge,
+ * which conflates "the line is broken" with "the source never fired".  This
+ * separates them by driving INT1 to a static level the sensor holds
+ * regardless of data-ready, and reading the PLIC pending bit directly.
+ *
+ * INT_CONFIG bit 0 selects INT1 polarity.  Clearing it makes the pin active
+ * low, so its idle state becomes high and it stays there.  If pmod_irq is
+ * really wired to this pin, the PLIC gateway then reports pending forever; if
+ * the input is floating, its pulldown keeps it clear.  The core's external
+ * interrupts stay masked throughout, so a permanently asserted line cannot
+ * storm the ISR -- the pending bit is polled, never taken.
+ */
+#define PLIC_PENDING_WORD0 (BSP_PLIC_BASE + 0x1000u)
+#define ICM_INT_CONFIG_REG 0x14u
+#define ICM_INT_SOURCE0_REG 0x65u
+#define ICM_INT1_ACTIVE_HIGH_PP_LATCHED 0x07u
+#define ICM_INT1_ACTIVE_LOW_PP_LATCHED 0x06u
+#define ICM_UI_DRDY_INT1 0x08u
+
+static uint32_t sample_pending_percent(uint64_t window_ticks)
+{
+	/* The gateway latches; claiming clears it so what follows reflects the
+	 * line's present level rather than a stale assertion. */
+	uint32_t claimed = bsp_plic_claim();
+	if(claimed != 0u)
+	{
+		bsp_plic_complete(claimed);
+	}
+	uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) + window_ticks;
+	uint32_t samples = 0u;
+	uint32_t asserted = 0u;
+	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
+	{
+		if((bsp_read32(PLIC_PENDING_WORD0) & (1u << BSP_PMOD_IRQ)) != 0u)
+		{
+			asserted++;
+			uint32_t id = bsp_plic_claim();
+			if(id != 0u)
+			{
+				bsp_plic_complete(id);
+			}
+		}
+		samples++;
+	}
+	return (samples != 0u) ? ((asserted * 100u) / samples) : 0u;
+}
+
+static void run_pin_continuity_test(void)
+{
+	uint64_t window = BSP_CLINT_TIMEBASE_HZ / 5u;   /* 200 ms */
+
+	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+	bsp_plic_enable(BSP_PMOD_IRQ);
+
+	/* Baseline: nothing routed to the pin, so it should rest inactive-low. */
+	bsp_pmod_spi_write_reg8(ICM_INT_SOURCE0_REG, 0x00u);
+	bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
+	                        ICM_INT1_ACTIVE_HIGH_PP_LATCHED);
+	uint32_t idle_low = sample_pending_percent(window);
+
+	/* Forced high: polarity inverted, so the idle state is asserted. */
+	bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
+	                        ICM_INT1_ACTIVE_LOW_PP_LATCHED);
+	uint32_t forced_high = sample_pending_percent(window);
+
+	/* Restore and let data-ready drive the pin normally. */
+	bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
+	                        ICM_INT1_ACTIVE_HIGH_PP_LATCHED);
+	bsp_pmod_spi_write_reg8(ICM_INT_SOURCE0_REG, ICM_UI_DRDY_INT1);
+	uint32_t data_ready = sample_pending_percent(window);
+
+	printf("PIN_TEST,idle_low=%lu%%,forced_high=%lu%%,data_ready=%lu%%\n",
+	       (unsigned long)idle_low, (unsigned long)forced_high,
+	       (unsigned long)data_ready);
+	printf("PIN_TEST,int_config=0x%02x,int_source0=0x%02x\n",
+	       bsp_pmod_spi_read_reg8(ICM_INT_CONFIG_REG | ICM_SPI_READ_BIT,
+	                              ICM_SPI_DUMMY_BYTE),
+	       bsp_pmod_spi_read_reg8(ICM_INT_SOURCE0_REG | ICM_SPI_READ_BIT,
+	                              ICM_SPI_DUMMY_BYTE));
+
+	if(forced_high >= 90u)
+	{
+		printf("VERDICT,INT1 reaches PLIC %lu. The pin is wired correctly; "
+		       "any remaining silence is the source not re-arming, not the "
+		       "wire.\n", (unsigned long)BSP_PMOD_IRQ);
+	}
+	else if(forced_high <= 10u)
+	{
+		printf("VERDICT,INT1 does NOT reach PLIC %lu. Holding the sensor pin "
+		       "statically asserted produced no pending bit, so the FPGA input "
+		       "(package pin AK30) is not connected to the sensor INT1 pin. "
+		       "Confirm which physical PMOD position AK30 is in Digilent's "
+		       "Genesys2 master XDC -- the comment in genesys2_pmod_irq.xdc is "
+		       "not authoritative.\n", (unsigned long)BSP_PMOD_IRQ);
+	}
+	else
+	{
+		printf("VERDICT,intermittent. A marginal or noisy connection.\n");
+	}
+	bsp_plic_disable(BSP_PMOD_IRQ);
+}
+
+/* MEIE (bit 11) tells us whether the hart is actually accepting the PLIC's
+ * external interrupt, which a pending bit alone does not. */
+static uint64_t bsp_read_mie(void)
+{
+	uint64_t value;
+	asm volatile("csrr %0, mie" : "=r"(value));
+	return value;
+}
+
+static void delay_ticks(uint64_t ticks)
+{
+	uint64_t start = bsp_read64(BSP_CLINT_MTIME);
+	while((bsp_read64(BSP_CLINT_MTIME) - start) < ticks)
+	{
+	}
+}
+
+static void dump_sensor_state(void)
+{
+	static const struct { uint8_t address; const char* name; } registers[] = {
+		{0x14u, "INT_CONFIG"},
+		{0x2du, "INT_STATUS"},
+		{0x4eu, "PWR_MGMT0"},
+		{0x4fu, "GYRO_CONFIG0"},
+		{0x50u, "ACCEL_CONFIG0"},
+		{0x63u, "INT_CONFIG0"},
+		{0x64u, "INT_CONFIG1"},
+		{0x65u, "INT_SOURCE0"},
+		{0x75u, "WHO_AM_I"}
+	};
+	for(unsigned int i = 0u ;
+	    i < sizeof(registers) / sizeof(registers[0]) ; i++)
+	{
+		printf("REG,%s,0x%02x=0x%02x\n", registers[i].name,
+		       registers[i].address,
+		       bsp_pmod_spi_read_reg8(registers[i].address | ICM_SPI_READ_BIT,
+		                              ICM_SPI_DUMMY_BYTE));
+	}
+
+	/*
+	 * Sample INT_STATUS over several ODR periods.  Reading it also clears it,
+	 * so a healthy sensor sets DATA_RDY again on the very next sample and this
+	 * count lands near the number of periods observed.
+	 */
+	uint64_t window_ticks = (BSP_CLINT_TIMEBASE_HZ * 10u) / NOISE_ODR_HZ;
+	uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) + window_ticks;
+	uint32_t polls = 0u;
+	uint32_t drdy_seen = 0u;
+	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
+	{
+		uint8_t status = bsp_pmod_spi_read_reg8(
+			ICM_INT_STATUS_REG | ICM_SPI_READ_BIT, ICM_SPI_DUMMY_BYTE);
+		polls++;
+		if((status & ICM_DATA_READY_BIT) != 0u)
+		{
+			drdy_seen++;
+		}
+	}
+	printf("DRDY_POLL,window_periods=10,polls=%lu,data_ready=%lu\n",
+	       (unsigned long)polls, (unsigned long)drdy_seen);
+}
+
+static bool wait_for_sample(imu_sample_t* sample)
+{
+	/* The bound covers roughly ten ODR periods. */
+	uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) +
+	                    (BSP_CLINT_TIMEBASE_HZ * 10u) / NOISE_ODR_HZ +
+	                    BSP_CLINT_TIMEBASE_HZ / 10u;
+	if(polled_mode)
+	{
+		return poll_for_sample(sample, deadline);
+	}
+	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
+	{
+		if(tdk_icm42688_take_measurement(sample))
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+static double norm3(const double vector[3])
+{
+	return sqrt(vector[0] * vector[0] + vector[1] * vector[1] +
+	            vector[2] * vector[2]);
+}
+
+int main(void)
+{
+	printf("\nTDK ICM-42688-P stationary noise characterization\n");
+	printf("KEEP THE BOARD COMPLETELY STILL FOR THE ENTIRE RUN\n");
+
+	tdk_icm42688_config_t config;
+	tdk_icm42688_default_config(&config);
+	config.odr_hz = NOISE_ODR_HZ;
+
+	bsp_plic_disable_all();
+	bsp_plic_set_threshold(0u);
+	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+	/* Recover the PMOD LevelGateway if a previous GDB-loaded image stopped
+	 * between claim and complete. */
+	bsp_plic_complete(BSP_PMOD_IRQ);
+	bsp_set_mtvec(tdk_noise_trap_handler);
+
+	if(!tdk_icm42688_init(&config))
+	{
+		tdk_icm42688_diagnostics_t diagnostics;
+		tdk_icm42688_get_diagnostics(&diagnostics);
+		printf("INIT FAIL: who_am_i=0x%02x (expected 0x47)\n",
+		       diagnostics.who_am_i);
+		bsp_uart_wait_tx_idle();
+		return 2;
+	}
+
+#if NOISE_PIN_TEST
+	/* Runs with the core's external interrupts still masked, so a statically
+	 * asserted line is polled rather than taken. */
+	printf("CONFIG,odr_hz=%lu,accel_fs_g=%d,gyro_fs_mdps=%d,ui_filter=0x%02x\n",
+	       (unsigned long)config.odr_hz, (int)config.accel_full_scale_g,
+	       (int)config.gyro_full_scale_mdps, config.ui_filter_config);
+	run_pin_continuity_test();
+	tdk_icm42688_stop();
+	bsp_uart_wait_tx_idle();
+	return 0;
+#endif
+
+	bsp_enable_machine_external_interrupts();
+
+	/*
+	 * Separate a stale PLIC pending bit from a real interrupt.
+	 *
+	 * tdk_icm42688_init() leaves INT_SOURCE0 clear, so the sensor is not
+	 * driving INT1 yet.  Enabling the PLIC route now and counting what arrives
+	 * measures everything that is not the sensor: a pending bit left latched
+	 * by a previous GDB-loaded image, or a floating input picking up noise.
+	 * Without this baseline a single stale interrupt looks exactly like a
+	 * working data-ready line, which is how one sample was mistaken for a
+	 * healthy route.
+	 */
+	bsp_plic_enable(BSP_PMOD_IRQ);
+	delay_ticks(BSP_CLINT_TIMEBASE_HZ / 2u);
+	tdk_icm42688_diagnostics_t baseline;
+	tdk_icm42688_get_diagnostics(&baseline);
+
+	if(!tdk_icm42688_start())
+	{
+		printf("START FAIL\n");
+		bsp_uart_wait_tx_idle();
+		return 3;
+	}
+	/*
+	 * Sample the PLIC pending bit while the sensor is armed and the core's
+	 * external interrupts are unmasked.  This separates the two remaining
+	 * layers: pending asserting without the trap count moving means the
+	 * gateway sees the line but delivery to the hart is broken, whereas
+	 * pending never asserting means the gateway is not reacting to a pin the
+	 * continuity test already proved is toggling.
+	 */
+	uint64_t armed_deadline = bsp_read64(BSP_CLINT_MTIME) + BSP_CLINT_TIMEBASE_HZ;
+	uint32_t pending_samples = 0u;
+	uint32_t pending_asserted = 0u;
+	while(bsp_read64(BSP_CLINT_MTIME) < armed_deadline)
+	{
+		if((bsp_read32(PLIC_PENDING_WORD0) & (1u << BSP_PMOD_IRQ)) != 0u)
+		{
+			pending_asserted++;
+		}
+		pending_samples++;
+	}
+	tdk_icm42688_diagnostics_t armed;
+	tdk_icm42688_get_diagnostics(&armed);
+	printf("PLIC_PENDING,armed_samples=%lu,asserted=%lu,percent=%lu,"
+	       "mie_meie=%lu,threshold_ok=1\n",
+	       (unsigned long)pending_samples, (unsigned long)pending_asserted,
+	       (unsigned long)((pending_samples != 0u)
+	                       ? (pending_asserted * 100u) / pending_samples : 0u),
+	       (unsigned long)((bsp_read_mie() >> 11) & 1u));
+	printf("IRQ_PROBE,quiet_500ms_irq=%lu,armed_1s_irq=%lu,armed_samples=%lu,"
+	       "spurious=%lu,expected_hz=%lu\n",
+	       (unsigned long)baseline.irq_count,
+	       (unsigned long)(armed.irq_count - baseline.irq_count),
+	       (unsigned long)armed.sample_count,
+	       (unsigned long)armed.spurious_irq_count,
+	       (unsigned long)NOISE_ODR_HZ);
+
+	printf("CONFIG,odr_hz=%lu,accel_fs_g=%d,gyro_fs_mdps=%d,"
+	       "ui_filter=0x%02x,tick_hz=%lu\n",
+	       (unsigned long)config.odr_hz,
+	       (int)config.accel_full_scale_g,
+	       (int)config.gyro_full_scale_mdps,
+	       config.ui_filter_config,
+	       (unsigned long)BSP_CLINT_TIMEBASE_HZ);
+	printf("PLAN,discard=%lu,bias=%lu,measure=%lu,seconds=%lu\n",
+	       (unsigned long)NOISE_DISCARD_SAMPLES,
+	       (unsigned long)NOISE_BIAS_SAMPLES,
+	       (unsigned long)NOISE_MEASURE_SAMPLES,
+	       (unsigned long)((NOISE_DISCARD_SAMPLES + NOISE_BIAS_SAMPLES +
+	                        NOISE_MEASURE_SAMPLES) / NOISE_ODR_HZ));
+
+	imu_sample_t sample;
+	uint32_t dropouts = 0u;
+
+	/*
+	 * Decide on the sustained rate measured above, not on a single sample.
+	 * A lone interrupt is exactly what a stale pending bit produces, and
+	 * whether it happens to carry DATA_RDY is chance -- so one sample was
+	 * previously enough to report a healthy route that did not exist.
+	 */
+	{
+		uint64_t observed_hz = armed.sample_count;
+		uint64_t stale_hz = baseline.irq_count;
+		if(observed_hz < (NOISE_ODR_HZ / 2u))
+		{
+#if NOISE_REQUIRE_IRQ
+			printf("DELIVERY,mode=FAILED,reason=rate_too_low,"
+			       "observed_hz=%lu,expected_hz=%lu,stale_irq=%lu\n",
+			       (unsigned long)observed_hz, (unsigned long)NOISE_ODR_HZ,
+			       (unsigned long)stale_hz);
+			if((stale_hz != 0u) && (observed_hz == 0u))
+			{
+				printf("VERDICT,the only interrupts arrived before the sensor "
+				       "was driving INT1, so they came from a stale PLIC "
+				       "pending bit or a floating input -- not from the "
+				       "sensor. INT1 is still not reaching PLIC %lu.\n",
+				       (unsigned long)BSP_PMOD_IRQ);
+			}
+			else if(observed_hz == 0u)
+			{
+				printf("VERDICT,no interrupts at all. INT1 is not reaching "
+				       "PLIC %lu; check the pin named in "
+				       "hardware/fpga/genesys2/constraints/"
+				       "genesys2_pmod_irq.xdc.\n",
+				       (unsigned long)BSP_PMOD_IRQ);
+			}
+			else
+			{
+				printf("VERDICT,the route works but the source is not "
+				       "re-arming at the configured rate.\n");
+			}
+			dump_sensor_state();
+			tdk_icm42688_stop();
+			bsp_uart_wait_tx_idle();
+			return 5;
+#else
+			polled_mode = true;
+			printf("DELIVERY,mode=polled,reason=rate_too_low,"
+			       "observed_hz=%lu,expected_hz=%lu,stale_irq=%lu\n",
+			       (unsigned long)observed_hz, (unsigned long)NOISE_ODR_HZ,
+			       (unsigned long)stale_hz);
+			/* Drop only the PLIC route.  INT_SOURCE0 stays enabled so the
+			 * DATA_RDY condition keeps setting INT_STATUS for polling, and
+			 * no ISR competes to read (and clear) that register. */
+			bsp_plic_disable(BSP_PMOD_IRQ);
+#endif
+		}
+		else
+		{
+			printf("DELIVERY,mode=interrupt,rate_hz=%lu,stale_irq=%lu\n",
+			       (unsigned long)observed_hz, (unsigned long)stale_hz);
+		}
+	}
+
+	for(uint32_t i = 0u ; i < NOISE_DISCARD_SAMPLES ; i++)
+	{
+		if(!wait_for_sample(&sample))
+		{
+			dropouts++;
+		}
+	}
+
+	/* Pass 1: the stationary mean is gravity plus accel bias, and gyro bias.
+	 * Removing it reproduces the residual the ESKF sees once calibrated. */
+	double accel_sum[3] = {0.0, 0.0, 0.0};
+	double gyro_sum[3] = {0.0, 0.0, 0.0};
+	double accel_sum_squares[3] = {0.0, 0.0, 0.0};
+	double gyro_sum_squares[3] = {0.0, 0.0, 0.0};
+	uint32_t bias_count = 0u;
+	for(uint32_t i = 0u ; i < NOISE_BIAS_SAMPLES ; i++)
+	{
+		if(!wait_for_sample(&sample))
+		{
+			dropouts++;
+			continue;
+		}
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			accel_sum[axis] += sample.accel_mps2[axis];
+			gyro_sum[axis] += sample.gyro_rps[axis];
+			accel_sum_squares[axis] +=
+				sample.accel_mps2[axis] * sample.accel_mps2[axis];
+			gyro_sum_squares[axis] +=
+				sample.gyro_rps[axis] * sample.gyro_rps[axis];
+		}
+		bias_count++;
+	}
+	if(bias_count == 0u)
+	{
+		printf("NO SAMPLES: check PMOD wiring and INT1 routing\n");
+		bsp_uart_wait_tx_idle();
+		return 4;
+	}
+
+	double accel_mean[3];
+	double gyro_mean[3];
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		accel_mean[axis] = accel_sum[axis] / (double)bias_count;
+		gyro_mean[axis] = gyro_sum[axis] / (double)bias_count;
+		double accel_variance =
+			(accel_sum_squares[axis] / (double)bias_count) -
+			(accel_mean[axis] * accel_mean[axis]);
+		double gyro_variance =
+			(gyro_sum_squares[axis] / (double)bias_count) -
+			(gyro_mean[axis] * gyro_mean[axis]);
+		printf("AXIS,%u,accel_mean=%ld,accel_std=%ld,"
+		       "gyro_mean=%ld,gyro_std=%ld\n",
+		       axis, micro(accel_mean[axis]),
+		       micro(sqrt(accel_variance > 0.0 ? accel_variance : 0.0)),
+		       micro(gyro_mean[axis]),
+		       micro(sqrt(gyro_variance > 0.0 ? gyro_variance : 0.0)));
+	}
+	printf("GRAVITY,measured=%ld,expected=9806650\n", micro(norm3(accel_mean)));
+
+	/* Pass 2: the detector inputs themselves. */
+	statistic_t accel_statistic;
+	statistic_t gyro_statistic;
+	statistic_t jerk_statistic;
+	statistic_init(&accel_statistic, "accel_norm", "um/s2",
+	               ACCEL_HISTOGRAM_FULL_SCALE);
+	statistic_init(&gyro_statistic, "gyro_norm", "urad/s",
+	               GYRO_HISTOGRAM_FULL_SCALE);
+	statistic_init(&jerk_statistic, "jerk_norm", "um/s3",
+	               JERK_HISTOGRAM_FULL_SCALE);
+
+	double filtered[3] = {0.0, 0.0, 0.0};
+	bool have_filtered = false;
+	uint64_t previous_ticks = 0u;
+
+	for(uint32_t i = 0u ; i < NOISE_MEASURE_SAMPLES ; i++)
+	{
+		if(!wait_for_sample(&sample))
+		{
+			dropouts++;
+			continue;
+		}
+		double residual_accel[3];
+		double residual_gyro[3];
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			residual_accel[axis] = sample.accel_mps2[axis] - accel_mean[axis];
+			residual_gyro[axis] = sample.gyro_rps[axis] - gyro_mean[axis];
+		}
+		statistic_add(&accel_statistic, norm3(residual_accel));
+		statistic_add(&gyro_statistic, norm3(residual_gyro));
+
+		if(previous_ticks != 0u)
+		{
+			double dt_s = (double)(sample.timestamp_ticks - previous_ticks) /
+			              (double)BSP_CLINT_TIMEBASE_HZ;
+			if(dt_s > 0.0)
+			{
+				double alpha = dt_s / (JERK_TIME_CONSTANT_S + dt_s);
+				double delta[3];
+				for(unsigned int axis = 0u ; axis < 3u ; axis++)
+				{
+					double previous = filtered[axis];
+					filtered[axis] += alpha * (residual_accel[axis] - previous);
+					delta[axis] = filtered[axis] - previous;
+				}
+				if(have_filtered)
+				{
+					statistic_add(&jerk_statistic, norm3(delta) / dt_s);
+				}
+				have_filtered = true;
+			}
+		}
+		else
+		{
+			memcpy(filtered, residual_accel, sizeof(filtered));
+		}
+		previous_ticks = sample.timestamp_ticks;
+
+		if(((i + 1u) % (NOISE_ODR_HZ * 10u)) == 0u)
+		{
+			printf("PROGRESS,%lu/%lu\n", (unsigned long)(i + 1u),
+			       (unsigned long)NOISE_MEASURE_SAMPLES);
+		}
+	}
+
+	statistic_report(&accel_statistic);
+	statistic_report(&gyro_statistic);
+	statistic_report(&jerk_statistic);
+
+	tdk_icm42688_diagnostics_t diagnostics;
+	tdk_icm42688_get_diagnostics(&diagnostics);
+	printf("HEALTH,irq=%lu,samples=%lu,invalid=%lu,spurious=%lu,"
+	       "overflow=%lu,missed=%lu,dropouts=%lu,unexpected_irq=%lu,"
+	       "trap_cause=%lu\n",
+	       (unsigned long)diagnostics.irq_count,
+	       (unsigned long)diagnostics.sample_count,
+	       (unsigned long)diagnostics.invalid_sample_count,
+	       (unsigned long)diagnostics.spurious_irq_count,
+	       (unsigned long)diagnostics.queue_overflow_count,
+	       (unsigned long)diagnostics.estimated_missed_sample_count,
+	       (unsigned long)dropouts,
+	       (unsigned long)unexpected_irq_count,
+	       (unsigned long)unexpected_trap_cause);
+	printf("DONE\n");
+
+	tdk_icm42688_stop();
+	bsp_uart_wait_tx_idle();
+	return 0;
+}