Integrate asynchronous GPS IMU fusion

soc/software/fusion/README.md

@@ -12,6 +12,55 @@ Bu bölüm yeni bir geliştirme oturumunun nereden devam edeceğini tek yerde
 özetler. Aşağıdaki Python model ve bare-metal C uygulaması aynı problemi çözer,
 fakat aynı program değildir:
 
+### Asenkron IMU/GPS düzeltmesi ve sentetik oran matrisi
+
+Önceki prototipte 52 Hz ST akışından yalnız yaklaşık 250 ms'de bir ham örnek
+seçiliyordu. Bu inertial downsampling değildir: kısa gyro hareketi iki seçilen
+örneğin arasında kalınca değişen gravity vektörü görülüyor, dönüş görülmüyor ve
+yanlış yatay ivme entegre ediliyordu. Linux C hızlı-eğim testinde bu yol 8.169 m
+tepe yatay hata ve 1.554 m/s kalıcı hız üretti.
+
+Bu seçim katmanı kaldırıldı. Artık her raw sample kendi timestamp'iyle işlenir;
+ardışık accel/gyro örnekleri midpoint mechanization ile entegre edilir. GPS,
+IMU'dan bağımsız bir position observation'dır. WLS yalnız geçerli olduğunda
+ölçüm epoch'unda uygulanır; epoch iki IMU sample'ı arasındaysa input lineer
+interpolate edilir, update sonrası kalan IMU geçmişi current-time horizon'a
+replay edilir. Ana döngü IMU kuyruğunu WLS'den önce ve sonra tamamen boşaltır.
+
+`async_rate_matrix_test.c` aynı üretim C kaynaklarına 52 Hz timestamp'li IMU,
+100/200/1000 ms GPS, 73/310 ms delivery delay, farklı eksenlerde ivmelenme ve
+fren, yaw dönüşü ve 10--18 s GPS outage verir. Sonuçlar sırasıyla 0.172/0.180/
+0.389 m yatay RMSE ve outage sırasında 0.634/0.671/0.822 m tepe hatadır; üçünde
+de GPS recovery kabul edilmiş, false ZUPT ve NIS reject sıfırdır. Tam GPS'siz
+koşu da kendine konum feed etmeden yerel dead-reckoning yapar: global origin
+false kalır, kovaryans büyür ve 25 s sonunda 3.674 m drift oluşur. Bu drift
+gizlenmez; mutlak konumun IMU-only durumda gözlenememesinin beklenen sonucudur.
+
+`gps_reacquisition_test.c`, canlı UART'ta görülen kötü ilk GPS fix'i senaryosunu
+ENU'da 80/40/-170 m başlangıç ofsetiyle tekrarlar. Hard NIS kapısı tek başına
+sonraki doğru fix'leri sürekli reddettiği için fusion engine zaman tabanlı bir
+position reacquisition uygular: son başarılı position fusion üzerinden yedi
+saniye timeout geçtikten ve en az üç rejected fix kendi içinde devamlı olduktan
+sonra yalnız position state resetlenir. Hız, attitude ve IMU bias'ları korunur.
+Test 100/200/1000 ms GPS periyotlarında sıfır son hata üretir; ayrı
+150/-120/+80 m tek-fix testi aykırı ölçümün reddedildiğini ve state reset
+oluşturmadığını doğrular.
+
+Testin `--trace` seçeneği UART'a benzer `IMU ts=...`, `GPS ts=... arrival=...`
+satırları üretir. Standart regression komutu:
+
+```sh
+make -C soc/software/fusion/tests test
+```
+
+`platform_scenario_matrix_test.c` ayrıca stationary/tilt/vibration, yaya,
+trafik ışığında duran otomobil ve ani dönüşlü/yüksek ivmeli uçak profillerini;
+200/500/1000 ms GNSS ile 19/20/200/300 ms IMU'nun 48 kombinasyonunda çalıştırır.
+GNSS dört uydu kullanır, ilk bölümde 5--10 m, sonra 3--5 m sınıfı yatay gürültü,
+3 saniye outage ve 2 saniye tutarlı büyük hata içerir. Kısa hata yedi saniyelik
+recovery timeout'unu açamaz. 200/300 ms IMU yüksek dinamikteki doğruluk kaybıyla
+birlikte açıkça raporlanır; test bunu daha sık IMU varmış gibi gizlemez.
+
 ```text
 Offline model:
 UART GPS + IMU logları -> Python parser -> fixed/adaptive ESKF -> CSV/grafik/harita
@@ -213,16 +262,16 @@ yoktur.
 
 Temel parçalar:
 
-- `gps_system/src/eskf.c`: ENU konum/hız, body-to-ENU quaternion, accel bias,
+- `gps_system/src/fusion/eskf.c`: ENU konum/hız, body-to-ENU quaternion, accel bias,
   gyro bias ve 15x15 covariance içeren 15-hata-durumlu ESKF;
-- `gps_system/src/fusion_motion.c`: bootstrap/stationary/moving/candidate
+- `gps_system/src/fusion/fusion_motion.c`: bootstrap/stationary/moving/candidate
   durum makinesi, filtrelenmiş jerk ve ZUPT izni;
-- `gps_system/src/fusion_engine.c`: başlangıç kalibrasyonu, asenkron IMU/GPS,
+- `gps_system/src/fusion/fusion_engine.c`: başlangıç kalibrasyonu, asenkron IMU/GPS,
   NIS kapısı ve gecikmiş GPS için fixed-lag IMU replay;
-- `gps_system/src/fusion_gps_adapter.c`: WGS-84 LLH/ECEF/ENU dönüşümü ve
+- `gps_system/src/fusion/fusion_gps_adapter.c`: WGS-84 LLH/ECEF/ENU dönüşümü ve
   covariance tabanı;
-- `gps_system/src/tdk_icm42688.c`: ICM-42688-P SPI/INT1 sürücüsü;
-- `gps_system/src/navigation_fusion.c`: GPS ve IMU kuyruklarını algoritmaya
+- `gps_system/src/drivers/imu/tdk_icm42688.c`: ICM-42688-P SPI/INT1 sürücüsü;
+- `gps_system/src/fusion/navigation_fusion.c`: GPS ve IMU kuyruklarını algoritmaya
   bağlayan üst koordinatör.
 
 ### ESKF state ve matematik sınırı
@@ -270,6 +319,64 @@ Default ESKF process/başlangıç değerleri:
 | Maksimum propagation alt adımı | 20 ms |
 | Konum NIS kapısı | 16.266236, 3 DOF %99.9 |
 
+### Innovation rejection, recovery ve katman siniri (2026-07-16)
+
+Standart ESKF position update'i tek basina buyuk ve kalici bir baslangic
+ofsetini duzeltemez. GPS--INS residual'i NIS kapisini astiginda olcum state'e
+hic uygulanmaz; state GPS'e yaklasmadigi icin sonraki dogru GPS fix'leri de
+ayni nedenle sonsuza kadar reddedilebilir. NIS yalniz iki bilgi kaynaginin
+tutarsiz oldugunu soyler. GPS'in mi hatali, yoksa ataletsel cozumun mu yanlis
+ankrajlandigini matematiksel update tek basina belirleyemez.
+
+Bu nedenle sorumluluk iki katmana ayrilir:
+
+- `eskf.c` innovation, innovation covariance, NIS gate, kabul edilmis Kalman
+  correction ve covariance bakimindan dogru position-reset primitive'ini
+  icerir. Rejected olcumu gizlice correction'a sokmaz ve NIS'i bypass etmez.
+- `fusion_engine.c` sensor gecmisi ve zaman bilgisine sahip supervisor'dur.
+  Ardisik rejected GNSS fix'lerinin kendi aralarinda zamansal olarak tutarli
+  olup olmadigini izler ve gerekli recovery/reset kararini burada verir.
+
+Mevcut yalın recovery, son başarılı position fusion'dan sonra yedi saniyelik
+timeout dolmuşsa ve en az üç rejected position fix'i kendi arasında zamansal
+tutarlıysa yalnız position state ve position covariance'ini GNSS'e yeniden
+ankrajlar. Position cross-covariance'lari gecersiz korelasyon tasimamasi icin
+sifirlanir; velocity, attitude ve IMU bias state'leri korunur. Hard NIS gate
+aktif kalir ve tek aykiri olcum reset uretemez. Bu bir innovation-bypass
+workaround'u degil, kontrollu state recovery mekanizmasidir.
+
+PX4 EKF2 de ayni temel ayrimi yapar. Generic `fuseHorizontalPosition()` yalniz
+innovation testinden gecen olcumu fuse eder. GNSS'e baslama, quality check,
+`time_last_fuse` timeout'u, state reset, fusion stop ve sensor-fault kararlari
+ayri GNSS control akisindadir. Otomatik modda baska yatay konum kaynagi yoksa
+uzun fusion timeout'undan sonra GNSS'e reset yapilabilir; alternatif aiding
+varsa veya dead-reckoning politikasinda GNSS durdurulabilir/fault kabul
+edilebilir. Kaynaklar:
+
+- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp>
+- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/position_fusion.cpp>
+- <https://docs.px4.io/main/en/advanced_config/tuning_the_ecl_ekf>
+
+Mevcut recovery PX4'teki aiding yoneticisinin sade bare-metal karsiligidir;
+nihai mimari hedefi asagidaki acik GNSS aiding durum makinesidir:
+
+```text
+DISABLED -> STARTING -> FUSING -> FAULTED/RECOVERING
+```
+
+Karar yalniz rejected-candidate suresine degil son basarili position fusion
+zamanina baglanmalidir. Uydu sayisi, finite covariance, DOP, WLS integrity ve
+alternatif aiding kaynaklari birlikte degerlendirilmelidir. Tam dort uyduda
+WLS residual redundancy/chi-square DOF bulunmadigi icin temporal consistency
+daha onemlidir. Ileride Doppler velocity ayri innovation, fusion timeout ve
+velocity-reset politikasi ile fuse edilmelidir. Yatay position ve height da
+barometre gibi ek kaynaklar geldiginde birbirinden bagimsiz yonetilmelidir.
+
+Baslangic GNSS'i icin mutlak bir dogruluk referansi yoksa tutarli fakat yanlis
+bir GNSS kumesini otomatik olarak gercek konumdan ayirmak temel olarak mumkun
+degildir. Kisa bir stable-fix/startup-quality penceresi riski azaltir, fakat
+spoofing veya tutarli multipath'e karsi tek basina kanit degildir.
+
 ### Asenkron veri akışı ve timestamp
 
 IMU ve GPS aynı local monotonic CPU/CLINT tick alanında timestamp edilir.
@@ -313,6 +420,13 @@ verir. Yaw gravity ile gözlenemediği için başlangıçta sıfır referanslı
 Benzer şekilde stationary IMU ile yatay accel bias ve küçük tilt tamamen
 ayrılamaz; sonraki GPS/kinematik update'ler bu error-state'leri düzeltir.
 
+IMU motion-state eşikleri tanılama için tutulur, fakat ZUPT yetkisini tek başına
+vermez. PX4 ayrımına uygun olarak `fusion_rest_detector` titreşim metriklerini
+izler ve dışarıdan gelen `platform_landed` kapısıyla birleşir. Prototipin unarmed
+bench modu landed=true başlar; gerçek araç kontrolü bu değeri kendi state'inden
+güncellemelidir. Sessiz IMU, landed=false iken sabit hızlı hareketi stationary
+ilan edemez.
+
 Motion-state default eşikleri:
 
 | Karar parametresi | Değer |
@@ -328,12 +442,20 @@ Motion-state default eşikleri:
 | Stationary'e dönüş quiet dwell | 1.0 s |
 | Zayıf hareket doğrulaması | 3 ardışık IMU örneği |
 
-Güçlü accel/gyro/jerk veya yüksek ESKF hızı stationary state'ten tek örnekte
-çıkabilir. Geri dönüş daha konservatiftir: quiet IMU, düşük ESKF hızı, aktif
-GNSS-moving veto olmaması ve 1 s dwell birlikte gerekir. ZUPT ile zero-gyro
-update yalnız gerçek `STATIONARY` state'inde, her IMU örneğinde değil 20 Hz'de
-uygulanır. ZUPT velocity sigma 0.03 m/s, zero-gyro sigma eksen başına
-0.01 deg/s'dir.
+Güçlü accel/gyro/jerk veya yüksek ESKF hızı motion tanılamasını tek örnekte
+moving'e çıkarabilir. ZUPT ve zero-gyro ise yalnız ardışık iki timestamp'li
+örnekte dış supervisor `vehicle_at_rest=true` verdiğinde 200 ms periyotla
+uygulanır. ZUPT velocity sigma 0.20 m/s'dir. Zero-gyro aynı 200 ms penceredeki
+delta-angle toplamından ortalama açısal hız üretir; tek bir örneği bias diye
+kabul etmez. Bu politika PX4 EKF2 `ZeroVelocityUpdate`/`ZeroGyroUpdate` ve land
+detector katman ayrımının bare-metal karşılığıdır.
+
+Kaynaklar:
+
+- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp>
+- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp>
+- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/land_detector/LandDetector.cpp>
+- <https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/sensors/vehicle_imu/VehicleIMU.cpp>
 
 GPS motion evidence, ardışık kabul edilmiş ENU konumları arasındaki yatay
 displacement için 2 DOF `9.21034` (%99) NIS ve en az 0.50 m/s hız ister. Moving
@@ -836,7 +958,7 @@ azınlık TOW'unu çoğunluğa kopyalayan geçici recovery yolu da kaldırıldı
    veya problemli kanal reject edilir. TOW değeri sessizce başka kanalın
    değeriyle değiştirilmez.
 
-Üretim kodu `header/gps_time_anchor.h` ve `src/gps_time_anchor.c` içindedir.
+Üretim kodu `header/gps/gps_time_anchor.h` ve `src/gps/gps_time_anchor.c` içindedir.
 `parse_subframe()` artık TOW state'ini değiştirmez; yalnız HOW TOW değerini
 dışarı verir. `gps_time_from_sample_anchor()` her nav-solve için GPS zamanını
 son HOW ve iki receiver-sample latch'i arasındaki farktan baştan üretir.

soc/software/fusion/tests/Makefile

@@ -3,13 +3,13 @@ CFLAGS ?= -std=c11 -O2 -Wall -Wextra -Wpedantic
 CFLAGS += -I../../gps_system/header
 LDLIBS ?= -lm
 
-GPS_TIME_SOURCE := ../../gps_system/src/gps_time_anchor.c
-ESKF_SOURCES := ../../gps_system/src/eskf.c ../../gps_system/src/fusion_motion.c ../../gps_system/src/fusion_engine.c ../../gps_system/src/fusion_gps_adapter.c ../../gps_system/src/fusion_bus.c
-IMU_PROFILE_SOURCE := ../../gps_system/src/imu_sensor_profile.c
+GPS_TIME_SOURCE := ../../gps_system/src/gps/gps_time_anchor.c
+ESKF_SOURCES := ../../gps_system/src/fusion/eskf.c ../../gps_system/src/fusion/fusion_motion.c ../../gps_system/src/fusion/fusion_rest_detector.c ../../gps_system/src/fusion/fusion_engine.c ../../gps_system/src/fusion/fusion_gps_adapter.c ../../gps_system/src/fusion/fusion_bus.c
+IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c
 
-.PHONY: all test sanitize profile benchmark clean
+.PHONY: all test scenario-logs sanitize profile benchmark clean
 
-all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_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
 
 standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
@@ -17,19 +17,58 @@ standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE
 eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
 
+fusion_rest_detector_test: fusion_rest_detector_test.c ../../gps_system/src/fusion/fusion_rest_detector.c
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
 fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
 
 imu_quality_synthetic_test: imu_quality_synthetic_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
 
-test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test
+async_tilt_recovery_test: async_tilt_recovery_test.c $(ESKF_SOURCES)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+async_rate_matrix_test: async_rate_matrix_test.c $(ESKF_SOURCES)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+gps_reacquisition_test: gps_reacquisition_test.c $(ESKF_SOURCES)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+platform_scenario_matrix_test: platform_scenario_matrix_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
 	./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
+
+scenario-logs: platform_scenario_matrix_test
+	mkdir -p output/scenarios
+	@for scenario in stationary pedestrian car aircraft; do \
+		for gps in 200 500 1000; do \
+			for imu in 19 20 200 300; do \
+				./platform_scenario_matrix_test --log $$scenario $$gps $$imu \
+					output/scenarios/$${scenario}_gps$${gps}_imu$${imu}.log >/dev/null || exit $$?; \
+			done; \
+		done; \
+	done
+	@echo "Wrote 48 replay logs under output/scenarios/"
 
 sanitize:
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header fusion_rest_detector_test.c \
+		../../gps_system/src/fusion/fusion_rest_detector.c \
+		-o fusion_rest_detector_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./fusion_rest_detector_test_san
 	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
 		-fsanitize=address,undefined -fno-omit-frame-pointer \
 		-I../../gps_system/header eskf_motion_test.c $(ESKF_SOURCES) \
@@ -48,6 +87,30 @@ sanitize:
 		$(IMU_PROFILE_SOURCE) -o imu_quality_synthetic_test_san $(LDLIBS)
 	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
 		UBSAN_OPTIONS=halt_on_error=1 ./imu_quality_synthetic_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header async_tilt_recovery_test.c $(ESKF_SOURCES) \
+		-o async_tilt_recovery_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./async_tilt_recovery_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header async_rate_matrix_test.c $(ESKF_SOURCES) \
+		-o async_rate_matrix_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./async_rate_matrix_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header gps_reacquisition_test.c $(ESKF_SOURCES) \
+		-o gps_reacquisition_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./gps_reacquisition_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header platform_scenario_matrix_test.c $(ESKF_SOURCES) \
+		-o platform_scenario_matrix_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./platform_scenario_matrix_test_san
 
 profile:
 	$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \
@@ -62,4 +125,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_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_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 gmon.out

soc/software/fusion/tests/async_rate_matrix_test.c

@@ -0,0 +1,416 @@
+#include "fusion/fusion_engine.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#define TIMEBASE_HZ 1000000u
+#define IMU_HZ 52u
+#define DURATION_S 25.0
+#define PI 3.14159265358979323846264338327950288
+#define GPS_EVENT_CAPACITY 300u
+
+typedef struct {
+	double position_m[3];
+	double velocity_mps[3];
+	double yaw_rad;
+	double accel_nav_mps2[3];
+	double yaw_rate_rps;
+} truth_t;
+
+typedef struct {
+	fusion_position_measurement_t measurement;
+	uint64_t arrival_ticks;
+} gps_event_t;
+
+typedef struct {
+	unsigned int gps_period_ms;
+	double horizontal_rmse_m;
+	double peak_outage_error_m;
+	double final_horizontal_error_m;
+	double final_vertical_error_m;
+	double final_speed_mps;
+	double outage_sigma_growth_m;
+	double final_horizontal_sigma_m;
+	double maximum_position_step_m;
+	uint64_t gps_accepted;
+	uint64_t gps_rejected;
+	uint64_t replay_count;
+	uint64_t imu_received;
+	unsigned int false_stationary_samples;
+	bool moving_detected;
+	bool recovery_gps_accepted;
+	bool local_state_finite;
+	bool covariance_finite_positive;
+	bool global_origin_available;
+} metrics_t;
+
+static bool trace_enabled;
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static void command_at(double time_s, double accel[3], double* yaw_rate)
+{
+	memset(accel, 0, 3u * sizeof(double));
+	*yaw_rate = 0.0;
+	if((time_s >= 3.0) && (time_s < 5.0))
+	{
+		accel[0] = 1.0;
+	}
+	else if((time_s >= 8.0) && (time_s < 10.0))
+	{
+		accel[0] = -1.0;
+	}
+	else if((time_s >= 12.0) && (time_s < 14.0))
+	{
+		accel[1] = 0.5;
+		*yaw_rate = PI / 4.0;
+	}
+	else if((time_s >= 18.0) && (time_s < 20.0))
+	{
+		accel[1] = -0.5;
+	}
+}
+
+static truth_t truth_at(double time_s)
+{
+	static const double boundaries[] = {
+		0.0, 3.0, 5.0, 8.0, 10.0, 12.0, 14.0, 18.0, 20.0, DURATION_S
+	};
+	truth_t truth;
+	memset(&truth, 0, sizeof(truth));
+	for(unsigned int segment = 0u;
+	    segment + 1u < sizeof(boundaries) / sizeof(boundaries[0]); segment++)
+	{
+		double start = boundaries[segment];
+		if(time_s <= start)
+		{
+			break;
+		}
+		double end = fmin(time_s, boundaries[segment + 1u]);
+		double dt = end - start;
+		double accel[3];
+		double yaw_rate;
+		command_at(start + 0.5 * dt, accel, &yaw_rate);
+		for(unsigned int axis = 0u; axis < 3u; axis++)
+		{
+			truth.position_m[axis] += truth.velocity_mps[axis] * dt +
+			                           0.5 * accel[axis] * dt * dt;
+			truth.velocity_mps[axis] += accel[axis] * dt;
+		}
+		truth.yaw_rad += yaw_rate * dt;
+		if(end >= time_s)
+		{
+			break;
+		}
+	}
+	command_at(time_s, truth.accel_nav_mps2, &truth.yaw_rate_rps);
+	return truth;
+}
+
+static fusion_imu_measurement_t make_imu(uint64_t ticks, double gravity)
+{
+	double time_s = (double)ticks / (double)TIMEBASE_HZ;
+	truth_t truth = truth_at(time_s);
+	double cosine = cos(truth.yaw_rad);
+	double sine = sin(truth.yaw_rad);
+	double specific_force_nav[3] = {
+		truth.accel_nav_mps2[0], truth.accel_nav_mps2[1],
+		truth.accel_nav_mps2[2] + gravity
+	};
+	fusion_imu_measurement_t imu;
+	memset(&imu, 0, sizeof(imu));
+	imu.timestamp_ticks = ticks;
+	imu.accel_mps2[0] = cosine * specific_force_nav[0] +
+	                    sine * specific_force_nav[1];
+	imu.accel_mps2[1] = -sine * specific_force_nav[0] +
+	                     cosine * specific_force_nav[1];
+	imu.accel_mps2[2] = specific_force_nav[2];
+	imu.gyro_rps[2] = truth.yaw_rate_rps;
+	/* Constant offsets are learned during stationary startup.  Small
+	 * deterministic broadband components keep the test reproducible. */
+	static const double accel_bias[3] = {0.030, -0.020, 0.050};
+	static const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
+	for(unsigned int axis = 0u; axis < 3u; axis++)
+	{
+		double phase = time_s * (11.0 + 2.0 * axis) + (double)axis;
+		imu.accel_mps2[axis] += accel_bias[axis] + 0.008 * sin(phase);
+		imu.gyro_rps[axis] += gyro_bias[axis] + 0.00025 * cos(1.7 * phase);
+	}
+	imu.temperature_c = 25.0 + 0.4 * sin(0.1 * time_s);
+	imu.sequence = (uint32_t)((ticks * IMU_HZ + TIMEBASE_HZ / 2u) /
+	                          TIMEBASE_HZ);
+	imu.vehicle_at_rest =
+		(hypot(truth.velocity_mps[0], truth.velocity_mps[1]) < 0.05) &&
+		(hypot(truth.accel_nav_mps2[0], truth.accel_nav_mps2[1]) < 0.05) &&
+		(fabs(truth.yaw_rate_rps) < 0.02);
+	return imu;
+}
+
+static fusion_position_measurement_t make_gps(uint64_t ticks)
+{
+	double time_s = (double)ticks / (double)TIMEBASE_HZ;
+	truth_t truth = truth_at(time_s);
+	fusion_position_measurement_t gps;
+	memset(&gps, 0, sizeof(gps));
+	gps.timestamp_ticks = ticks;
+	gps.position_enu_m[0] = truth.position_m[0] + 0.45 * sin(7.1 * time_s);
+	gps.position_enu_m[1] = truth.position_m[1] + 0.45 * cos(5.3 * time_s);
+	gps.position_enu_m[2] = truth.position_m[2] + 0.70 * sin(3.7 * time_s);
+	gps.covariance_enu_m2[0] = square(1.2);
+	gps.covariance_enu_m2[4] = square(1.2);
+	gps.covariance_enu_m2[8] = square(2.0);
+	gps.source_quality = 1.0;
+	gps.source_valid = true;
+	return gps;
+}
+
+static bool covariance_diagonal_valid(const eskf_t* filter)
+{
+	for(unsigned int index = 0u; index < ESKF_ERROR_STATE_DIM; index++)
+	{
+		if(!isfinite(filter->covariance[index][index]) ||
+		   !(filter->covariance[index][index] > 0.0))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static metrics_t run_scenario(unsigned int gps_period_ms)
+{
+	static fusion_engine_t engine;
+	memset(&engine, 0, sizeof(engine));
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, TIMEBASE_HZ);
+	config.calibration_min_samples = 100u;
+	metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	metrics.gps_period_ms = gps_period_ms;
+	if(!fusion_engine_init(&engine, &config))
+	{
+		return metrics;
+	}
+	if(gps_period_ms != 0u)
+	{
+		fusion_position_measurement_t origin = make_gps(0u);
+		origin.position_enu_m[0] = 0.0;
+		origin.position_enu_m[1] = 0.0;
+		origin.position_enu_m[2] = 0.0;
+		(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
+	}
+
+	gps_event_t events[GPS_EVENT_CAPACITY];
+	unsigned int event_head = 0u;
+	unsigned int event_count = 0u;
+	uint64_t next_gps_ticks = 3037000u;
+	uint64_t gps_period_ticks = (uint64_t)gps_period_ms * 1000u;
+	double error_square_sum = 0.0;
+	uint64_t error_samples = 0u;
+	double sigma_at_outage_start = NAN;
+	double sigma_before_recovery = NAN;
+	uint64_t accepted_before_recovery = 0u;
+	double previous_position[3] = {0.0, 0.0, 0.0};
+	bool have_previous_position = false;
+	uint64_t sample_count = (uint64_t)(DURATION_S * (double)IMU_HZ);
+
+	for(uint64_t sample = 0u; sample <= sample_count; sample++)
+	{
+		uint64_t ticks = (sample * TIMEBASE_HZ + IMU_HZ / 2u) / IMU_HZ;
+		double time_s = (double)ticks / (double)TIMEBASE_HZ;
+		fusion_imu_measurement_t imu = make_imu(ticks, config.eskf.gravity_mps2);
+		if(gps_period_ms == 0u)
+		{
+			/* Preserve the pure unaided observability case: no GNSS and no
+			 * external platform-rest constraint. */
+			imu.vehicle_at_rest = false;
+		}
+		if(trace_enabled)
+		{
+			printf("IMU ts=%llu seq=%u accel=%.6f,%.6f,%.6f gyro=%.6f,%.6f,%.6f\n",
+			       (unsigned long long)imu.timestamp_ticks, imu.sequence,
+			       imu.accel_mps2[0], imu.accel_mps2[1], imu.accel_mps2[2],
+			       imu.gyro_rps[0], imu.gyro_rps[1], imu.gyro_rps[2]);
+		}
+		if(!fusion_engine_push_imu(&engine, &imu))
+		{
+			break;
+		}
+
+		if((gps_period_ms != 0u) && (ticks >= next_gps_ticks))
+		{
+			double gps_time_s = (double)next_gps_ticks / (double)TIMEBASE_HZ;
+			if(!((gps_time_s >= 10.0) && (gps_time_s < 18.0)) &&
+			   (event_count < GPS_EVENT_CAPACITY))
+			{
+				events[event_count].measurement = make_gps(next_gps_ticks);
+				events[event_count].arrival_ticks = next_gps_ticks +
+					((gps_period_ms == 1000u) ? 310000u : 73000u);
+				event_count++;
+			}
+			next_gps_ticks += gps_period_ticks;
+		}
+
+		while((event_head < event_count) &&
+		      (events[event_head].arrival_ticks <= ticks))
+		{
+			eskf_update_result_t result = ESKF_UPDATE_INVALID;
+			double nis = NAN;
+			(void)fusion_engine_push_position(
+				&engine, &events[event_head].measurement, &result, &nis);
+			if(trace_enabled)
+			{
+				printf("GPS ts=%llu arrival=%llu result=%u nis=%.6f pos=%.3f,%.3f,%.3f\n",
+				       (unsigned long long)events[event_head].measurement.timestamp_ticks,
+				       (unsigned long long)ticks, (unsigned int)result, nis,
+				       events[event_head].measurement.position_enu_m[0],
+				       events[event_head].measurement.position_enu_m[1],
+				       events[event_head].measurement.position_enu_m[2]);
+			}
+			event_head++;
+		}
+
+		if(engine.state == FUSION_ENGINE_RUNNING)
+		{
+			truth_t truth = truth_at(time_s);
+			double east_error = engine.current.filter.position_m[0] - truth.position_m[0];
+			double north_error = engine.current.filter.position_m[1] - truth.position_m[1];
+			double horizontal_error = hypot(east_error, north_error);
+			if(have_previous_position)
+			{
+				double position_step = hypot(
+					engine.current.filter.position_m[0] - previous_position[0],
+					engine.current.filter.position_m[1] - previous_position[1]);
+				metrics.maximum_position_step_m =
+					fmax(metrics.maximum_position_step_m, position_step);
+			}
+			memcpy(previous_position, engine.current.filter.position_m,
+			       sizeof(previous_position));
+			have_previous_position = true;
+			if(time_s >= 2.5)
+			{
+				error_square_sum += square(horizontal_error);
+				error_samples++;
+			}
+			if((time_s >= 10.0) && (time_s < 18.0))
+			{
+				metrics.peak_outage_error_m =
+					fmax(metrics.peak_outage_error_m, horizontal_error);
+			}
+			if((hypot(truth.velocity_mps[0], truth.velocity_mps[1]) > 0.2) &&
+			   (engine.current.motion.state == FUSION_MOTION_STATIONARY))
+			{
+				metrics.false_stationary_samples++;
+			}
+			if(engine.current.motion.state == FUSION_MOTION_MOVING)
+			{
+				metrics.moving_detected = true;
+			}
+			if(isnan(sigma_at_outage_start) && (time_s >= 10.0))
+			{
+				sigma_at_outage_start = sqrt(engine.current.filter.covariance[0][0]);
+				accepted_before_recovery = engine.diagnostics.position_accepted;
+			}
+			if(isnan(sigma_before_recovery) && (time_s >= 18.0))
+			{
+				sigma_before_recovery = sqrt(engine.current.filter.covariance[0][0]);
+			}
+		}
+	}
+
+	truth_t final_truth = truth_at(DURATION_S);
+	metrics.horizontal_rmse_m = (error_samples != 0u)
+		? sqrt(error_square_sum / (double)error_samples) : INFINITY;
+	metrics.final_horizontal_error_m = hypot(
+		engine.current.filter.position_m[0] - final_truth.position_m[0],
+		engine.current.filter.position_m[1] - final_truth.position_m[1]);
+	metrics.final_vertical_error_m =
+		fabs(engine.current.filter.position_m[2] - final_truth.position_m[2]);
+	metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter);
+	metrics.outage_sigma_growth_m =
+		(isfinite(sigma_at_outage_start) && isfinite(sigma_before_recovery))
+		? sigma_before_recovery - sigma_at_outage_start : 0.0;
+	metrics.final_horizontal_sigma_m = sqrt(
+		engine.current.filter.covariance[0][0] +
+		engine.current.filter.covariance[1][1]);
+	metrics.gps_accepted = engine.diagnostics.position_accepted;
+	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
+	metrics.replay_count = engine.diagnostics.replay_count;
+	metrics.imu_received = engine.diagnostics.imu_received;
+	metrics.recovery_gps_accepted = (gps_period_ms == 0u) ||
+		(engine.diagnostics.position_accepted > accepted_before_recovery);
+	metrics.local_state_finite = (engine.state == FUSION_ENGINE_RUNNING) &&
+		eskf_state_is_finite(&engine.current.filter);
+	metrics.covariance_finite_positive =
+		covariance_diagonal_valid(&engine.current.filter);
+	metrics.global_origin_available = engine.have_initial_position;
+	return metrics;
+}
+
+static void print_metrics(const metrics_t* metrics)
+{
+	printf("GPS=%4u ms IMU=%llu GPS-ok/rej=%llu/%llu replay=%llu "
+	       "rmse-h=%.3f m outage-peak=%.3f m sigma-growth=%.3f m "
+	       "final=%.3f/%.3f m sigma-h=%.3f m step-max=%.3f m "
+	       "speed=%.4f m/s false-ZUPT=%u origin=%u\n",
+	       metrics->gps_period_ms,
+	       (unsigned long long)metrics->imu_received,
+	       (unsigned long long)metrics->gps_accepted,
+	       (unsigned long long)metrics->gps_rejected,
+	       (unsigned long long)metrics->replay_count,
+	       metrics->horizontal_rmse_m, metrics->peak_outage_error_m,
+	       metrics->outage_sigma_growth_m,
+	       metrics->final_horizontal_error_m, metrics->final_vertical_error_m,
+	       metrics->final_horizontal_sigma_m, metrics->maximum_position_step_m,
+	       metrics->final_speed_mps, metrics->false_stationary_samples,
+	       metrics->global_origin_available ? 1u : 0u);
+}
+
+int main(int argc, char** argv)
+{
+	trace_enabled = (argc == 2) && (strcmp(argv[1], "--trace") == 0);
+	const unsigned int periods_ms[] = {100u, 200u, 1000u, 0u};
+	bool passed = true;
+	printf("Timestamped 52 Hz IMU, asynchronous GPS, 8 s outage (10..18 s)\n");
+	for(unsigned int index = 0u;
+	    index < sizeof(periods_ms) / sizeof(periods_ms[0]); index++)
+	{
+		metrics_t metrics = run_scenario(periods_ms[index]);
+		print_metrics(&metrics);
+		bool common_ok = metrics.local_state_finite &&
+		                 metrics.covariance_finite_positive &&
+		                 metrics.moving_detected &&
+		                 (metrics.false_stationary_samples == 0u) &&
+		                 (metrics.imu_received == 1301u) &&
+		                 (metrics.maximum_position_step_m < 1.0);
+		bool gps_ok = (periods_ms[index] == 0u)
+			? (!metrics.global_origin_available && (metrics.gps_accepted == 0u) &&
+			   (metrics.peak_outage_error_m < 1.5) &&
+			   (metrics.final_horizontal_error_m < 5.0) &&
+			   (metrics.final_horizontal_error_m <
+			    3.0 * metrics.final_horizontal_sigma_m) &&
+			   (metrics.outage_sigma_growth_m > 0.0))
+			: (metrics.global_origin_available && metrics.recovery_gps_accepted &&
+			   (metrics.gps_rejected == 0u) && (metrics.replay_count > 0u) &&
+			   (metrics.peak_outage_error_m < 2.0) &&
+			   (metrics.outage_sigma_growth_m > 0.0) &&
+			   (metrics.final_speed_mps < 0.08) &&
+			   (metrics.final_horizontal_error_m < 1.5) &&
+			   (metrics.final_vertical_error_m < 1.5));
+		passed = passed && common_ok && gps_ok;
+	}
+	if(!passed)
+	{
+		fprintf(stderr, "FAIL: asynchronous rate/dead-reckoning matrix.\n");
+		return 1;
+	}
+	printf("PASS: all rates use the same IMU prediction path; GPS loss remains "
+	       "local dead reckoning and recovery is an external update.\n");
+	return 0;
+}

soc/software/fusion/tests/async_tilt_recovery_test.c

@@ -0,0 +1,315 @@
+#include "fusion/fusion_engine.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#define TEST_TIMEBASE_HZ 1000000u
+#define TEST_IMU_HZ 52u
+#define TEST_DURATION_S 30u
+#define TEST_PI 3.14159265358979323846264338327950288
+#define GPS_PERIOD_TICKS 200000u
+#define GPS_DELAY_TICKS 73000u
+#define MAX_GPS_EVENTS 160u
+
+typedef struct {
+	fusion_position_measurement_t measurement;
+	uint64_t arrival_ticks;
+} gps_event_t;
+
+typedef struct {
+	const char* name;
+	double peak_horizontal_error_m;
+	double peak_vertical_error_m;
+	double final_horizontal_error_m;
+	double final_vertical_error_m;
+	double final_speed_mps;
+	double final_roll_error_deg;
+	uint64_t imu_pushed;
+	uint64_t gps_accepted;
+	uint64_t gps_rejected;
+	uint64_t zero_updates;
+	fusion_engine_state_t engine_state;
+	fusion_motion_state_t motion_state;
+} scenario_metrics_t;
+
+static double radians(double degrees)
+{
+	return degrees * TEST_PI / 180.0;
+}
+
+static double degrees(double radians_value)
+{
+	return radians_value * 180.0 / TEST_PI;
+}
+
+/* The IMU rotates about its own X axis without translating.  Calibration is
+ * complete before the manoeuvre.  This is the smallest synthetic version of
+ * the live hand-tilt failure: the true position and the GNSS antenna remain at
+ * the origin while body-frame gravity changes with roll. */
+static void truth_roll(uint64_t ticks,
+	                   double final_roll_rad,
+	                   double* roll_rad,
+	                   double* roll_rate_rps)
+{
+	double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
+	double roll = 0.0;
+	double rate = 0.0;
+	const double maximum_roll = radians(20.0);
+	/* A short hand motion is intentionally placed between the 250 ms selected
+	 * samples.  Full-rate IMU observes the angular rate; sample dropping can
+	 * observe only the changed gravity vector and completely miss the gyro. */
+	if((time_s >= 16.05) && (time_s < 16.15))
+	{
+		roll = maximum_roll * (time_s - 16.05) / 0.10;
+		rate = maximum_roll / 0.10;
+	}
+	else if((time_s >= 16.15) && (time_s < 17.05))
+	{
+		roll = maximum_roll;
+	}
+	else if((time_s >= 17.05) && (time_s < 17.15))
+	{
+		double fraction = (time_s - 17.05) / 0.10;
+		roll = maximum_roll + fraction * (final_roll_rad - maximum_roll);
+		rate = (final_roll_rad - maximum_roll) / 0.10;
+	}
+	else if(time_s >= 17.15)
+	{
+		roll = final_roll_rad;
+	}
+	*roll_rad = roll;
+	*roll_rate_rps = rate;
+}
+
+static fusion_imu_measurement_t make_imu(uint64_t ticks,
+	                                     double gravity_mps2,
+	                                     double final_roll_rad,
+	                                     double gyro_scale)
+{
+	double roll;
+	double roll_rate;
+	truth_roll(ticks, final_roll_rad, &roll, &roll_rate);
+	fusion_imu_measurement_t imu;
+	memset(&imu, 0, sizeof(imu));
+	imu.timestamp_ticks = ticks;
+	/* R_nb is a positive X rotation.  A stationary accelerometer measures
+	 * R_nb^T [0, 0, g], hence [0, g sin(phi), g cos(phi)] in body axes. */
+	imu.accel_mps2[1] = gravity_mps2 * sin(roll);
+	imu.accel_mps2[2] = gravity_mps2 * cos(roll);
+	imu.gyro_rps[0] = gyro_scale * roll_rate;
+	imu.temperature_c = 25.0;
+	imu.sequence = (uint32_t)((ticks * TEST_IMU_HZ + TEST_TIMEBASE_HZ / 2u) /
+	                          TEST_TIMEBASE_HZ);
+	double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
+	imu.vehicle_at_rest = (time_s < 16.05) || (time_s >= 18.15);
+	return imu;
+}
+
+static fusion_position_measurement_t make_gps(uint64_t ticks)
+{
+	fusion_position_measurement_t gps;
+	memset(&gps, 0, sizeof(gps));
+	gps.timestamp_ticks = ticks;
+	gps.covariance_enu_m2[0] = 4.0;
+	gps.covariance_enu_m2[4] = 4.0;
+	gps.covariance_enu_m2[8] = 9.0;
+	gps.source_quality = 100.0;
+	gps.source_valid = true;
+	return gps;
+}
+
+static unsigned int build_gps_events(gps_event_t events[MAX_GPS_EVENTS])
+{
+	unsigned int count = 0u;
+	/* Deliberately offset GNSS epochs from both the 52 Hz IMU samples and
+	 * integer seconds. */
+	for(uint64_t ticks = 123457u;
+	    ticks <= (uint64_t)TEST_DURATION_S * TEST_TIMEBASE_HZ;
+	    ticks += GPS_PERIOD_TICKS)
+	{
+		events[count].measurement = make_gps(ticks);
+		events[count].arrival_ticks = ticks + GPS_DELAY_TICKS;
+		count++;
+	}
+	return count;
+}
+
+static double quaternion_roll(const double quaternion[4])
+{
+	double w = quaternion[0];
+	double x = quaternion[1];
+	double y = quaternion[2];
+	double z = quaternion[3];
+	return atan2(2.0 * (w * x + y * z),
+	             1.0 - 2.0 * (x * x + y * y));
+}
+
+static scenario_metrics_t run_scenario(const char* name,
+	                                   bool emulate_250_ms_sample_drop,
+	                                   bool provide_gps,
+	                                   double final_roll_deg,
+	                                   double gyro_scale)
+{
+	static fusion_engine_t engine;
+	memset(&engine, 0, sizeof(engine));
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, TEST_TIMEBASE_HZ);
+	if(!fusion_engine_init(&engine, &config))
+	{
+		return (scenario_metrics_t){.name = name, .engine_state = FUSION_ENGINE_FAULT};
+	}
+
+	/* Give calibration an absolute origin, just as the product does when GNSS
+	 * becomes available before initial tilt/bias calibration completes. */
+	if(provide_gps)
+	{
+		fusion_position_measurement_t origin = make_gps(1u);
+		(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
+	}
+
+	gps_event_t events[MAX_GPS_EVENTS];
+	unsigned int event_count = provide_gps ? build_gps_events(events) : 0u;
+	unsigned int event_head = 0u;
+	uint64_t last_forwarded_ticks = 0u;
+	bool have_forwarded = false;
+	scenario_metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	metrics.name = name;
+
+	const uint64_t sample_count =
+		(uint64_t)TEST_DURATION_S * TEST_IMU_HZ;
+	for(uint64_t sample = 0u ; sample <= sample_count ; sample++)
+	{
+		uint64_t ticks = (sample * TEST_TIMEBASE_HZ + TEST_IMU_HZ / 2u) /
+		                 TEST_IMU_HZ;
+		fusion_imu_measurement_t imu = make_imu(
+			ticks, config.eskf.gravity_mps2, radians(final_roll_deg), gyro_scale);
+		bool forward = true;
+		if(emulate_250_ms_sample_drop && have_forwarded &&
+		   ((ticks - last_forwarded_ticks) < 250000u))
+		{
+			forward = false;
+		}
+		if(forward)
+		{
+			if(!fusion_engine_push_imu(&engine, &imu))
+			{
+				break;
+			}
+			last_forwarded_ticks = ticks;
+			have_forwarded = true;
+			metrics.imu_pushed++;
+		}
+
+		while((event_head < event_count) &&
+		      (events[event_head].arrival_ticks <= ticks))
+		{
+			(void)fusion_engine_push_position(
+				&engine, &events[event_head].measurement, NULL, NULL);
+			event_head++;
+		}
+
+		if(engine.state == FUSION_ENGINE_RUNNING)
+		{
+			double horizontal = hypot(engine.current.filter.position_m[0],
+			                          engine.current.filter.position_m[1]);
+			metrics.peak_horizontal_error_m =
+				fmax(metrics.peak_horizontal_error_m, horizontal);
+			metrics.peak_vertical_error_m =
+				fmax(metrics.peak_vertical_error_m,
+				     fabs(engine.current.filter.position_m[2]));
+		}
+	}
+
+	metrics.engine_state = engine.state;
+	if(engine.state == FUSION_ENGINE_RUNNING)
+	{
+		metrics.final_horizontal_error_m =
+			hypot(engine.current.filter.position_m[0],
+			      engine.current.filter.position_m[1]);
+		metrics.final_vertical_error_m = fabs(engine.current.filter.position_m[2]);
+		metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter);
+		metrics.final_roll_error_deg =
+			fabs(degrees(quaternion_roll(engine.current.filter.body_to_nav_quaternion)) -
+			     final_roll_deg);
+		metrics.motion_state = engine.current.motion.state;
+	}
+	metrics.gps_accepted = engine.diagnostics.position_accepted;
+	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
+	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
+	return metrics;
+}
+
+static void print_metrics(const scenario_metrics_t* metrics)
+{
+	printf("%-20s imu=%4llu state=%u motion=%-20s peak-h=%9.3f m peak-v=%9.3f m "
+	       "final-h=%9.3f m final-v=%9.3f m speed=%8.3f m/s "
+	       "roll=%7.3f deg gps=%llu/%llu zupt=%llu\n",
+	       metrics->name,
+	       (unsigned long long)metrics->imu_pushed,
+	       (unsigned int)metrics->engine_state,
+	       fusion_motion_state_name(metrics->motion_state),
+	       metrics->peak_horizontal_error_m,
+	       metrics->peak_vertical_error_m,
+	       metrics->final_horizontal_error_m,
+	       metrics->final_vertical_error_m,
+	       metrics->final_speed_mps,
+	       metrics->final_roll_error_deg,
+	       (unsigned long long)metrics->gps_accepted,
+	       (unsigned long long)metrics->gps_rejected,
+	       (unsigned long long)metrics->zero_updates);
+}
+
+int main(void)
+{
+	scenario_metrics_t full_rate = run_scenario(
+		"52 Hz full IMU", false, true, 0.0, 1.0);
+	scenario_metrics_t imu_only = run_scenario(
+		"52 Hz, no GNSS", false, false, 0.0, 1.0);
+	scenario_metrics_t residual = run_scenario(
+		"1 deg residual tilt", false, true, 10.0, 0.9);
+	scenario_metrics_t sparse = run_scenario(
+		"250 ms sample-drop", true, true, 0.0, 1.0);
+	printf("Asynchronous stationary tilt: 52 Hz IMU, 5 Hz GNSS, 73 ms GNSS delay\n");
+	print_metrics(&full_rate);
+	print_metrics(&imu_only);
+	print_metrics(&residual);
+	print_metrics(&sparse);
+
+	bool full_rate_ok = (full_rate.engine_state == FUSION_ENGINE_RUNNING) &&
+	                    (full_rate.peak_horizontal_error_m < 1.0) &&
+	                    (full_rate.peak_vertical_error_m < 1.0) &&
+	                    (full_rate.final_speed_mps < 0.05);
+	bool imu_only_ok = (imu_only.engine_state == FUSION_ENGINE_RUNNING) &&
+	                   (imu_only.gps_accepted == 0u) &&
+	                   (imu_only.peak_horizontal_error_m < 1.0) &&
+	                   (imu_only.peak_vertical_error_m < 1.0) &&
+	                   (imu_only.final_speed_mps < 0.05);
+	bool residual_recovered =
+		(residual.engine_state == FUSION_ENGINE_RUNNING) &&
+		(residual.motion_state == FUSION_MOTION_STATIONARY) &&
+		(residual.final_speed_mps < 0.05) &&
+		(residual.final_horizontal_error_m < 1.0) &&
+		(residual.final_roll_error_deg < 0.5);
+	bool sparse_aliasing_detected_and_recovered =
+		(sparse.engine_state == FUSION_ENGINE_RUNNING) &&
+		(sparse.peak_horizontal_error_m > 3.0) &&
+		(sparse.final_horizontal_error_m < 1.0) &&
+		(sparse.final_speed_mps < 0.10) &&
+		(sparse.motion_state == FUSION_MOTION_STATIONARY);
+	if(!full_rate_ok || !imu_only_ok || !residual_recovered ||
+	   !sparse_aliasing_detected_and_recovered)
+	{
+		fprintf(stderr,
+		        "FAIL: full-rate propagation and GNSS-aided rest recovery must "
+		        "remain bounded; sparse sampling must expose a large transient but "
+		        "recover after an external at-rest decision.\n");
+		return 1;
+	}
+	printf("PASS: full-rate and residual-tilt cases remained bounded; sparse "
+	       "sampling exposed the transient and external at-rest recovered it.\n");
+	return 0;
+}

soc/software/fusion/tests/eskf_motion_test.c

@@ -3,11 +3,11 @@
 #include <stdio.h>
 #include <string.h>
 
-#include "eskf.h"
-#include "fusion_engine.h"
-#include "fusion_bus.h"
-#include "fusion_gps_adapter.h"
-#include "fusion_motion.h"
+#include "fusion/eskf.h"
+#include "fusion/fusion_engine.h"
+#include "fusion/fusion_bus.h"
+#include "fusion/fusion_gps_adapter.h"
+#include "fusion/fusion_motion.h"
 
 #define TEST_PI 3.14159265358979323846264338327950288
 
@@ -78,6 +78,47 @@ static void test_stationary_propagation(void)
 	CHECK(eskf_state_is_finite(&filter), "stationary state remains finite");
 }
 
+static void test_zero_update_primitives(void)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	identity_filter(&filter, &config);
+	filter.velocity_mps[0] = 1.2;
+	filter.velocity_mps[1] = -0.8;
+	filter.velocity_mps[2] = 0.3;
+	CHECK(eskf_zero_velocity_update(&filter, 0.03),
+	      "direct ZUPT measurement update succeeds");
+	CHECK(eskf_speed_mps(&filter) < 0.005,
+	      "direct ZUPT drives a stale velocity estimate close to zero");
+
+	double stationary_gyro[3] = {0.0010, -0.0006, 0.0003};
+	double gyro_std[3] = {0.0002, 0.0002, 0.0002};
+	for(unsigned int update = 0u ; update < 20u ; update++)
+	{
+		CHECK(eskf_zero_angular_rate_update(
+			&filter, stationary_gyro, gyro_std),
+		      "direct zero-angular-rate update succeeds");
+	}
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		CHECK(fabs(filter.gyro_bias_rps[axis] - stationary_gyro[axis]) < 1.0e-5,
+		      "zero-angular-rate update converges gyro bias to stationary reading");
+	}
+	CHECK(eskf_state_is_finite(&filter),
+	      "ZUPT and zero-angular-rate updates preserve finite ESKF state");
+	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		CHECK(filter.covariance[row][row] > 0.0,
+		      "zero updates preserve positive covariance diagonal");
+		for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			CHECK(fabs(filter.covariance[row][col] -
+			           filter.covariance[col][row]) < 1.0e-12,
+			      "zero updates preserve covariance symmetry");
+		}
+	}
+}
+
 static void test_gravity_alignment(void)
 {
 	eskf_t filter;
@@ -446,6 +487,7 @@ static void initialize_engine_for_replay(fusion_engine_t* engine)
 		imu.accel_mps2[2] = config.eskf.gravity_mps2;
 		imu.temperature_c = 25.0;
 		imu.sequence = (uint32_t)(tick / 10u);
+		imu.vehicle_at_rest = true;
 		CHECK(fusion_engine_push_imu(engine, &imu),
 		      "stationary calibration IMU accepted");
 	}
@@ -475,6 +517,7 @@ static void push_replay_imu(fusion_engine_t* engine, uint64_t tick)
 	imu.accel_mps2[2] = engine->config.eskf.gravity_mps2;
 	imu.temperature_c = 25.0;
 	imu.sequence = (uint32_t)(tick / 10u);
+	imu.vehicle_at_rest = fabs(imu.accel_mps2[0]) < 0.1;
 	CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted");
 }
 
@@ -703,6 +746,7 @@ static void test_history_rolls_checkpoint_forward(void)
 int main(void)
 {
 	test_stationary_propagation();
+	test_zero_update_primitives();
 	test_gravity_alignment();
 	test_position_nis_gate();
 	test_motion_transitions();

soc/software/fusion/tests/fusion_rest_detector_test.c

@@ -0,0 +1,131 @@
+#include "fusion/fusion_rest_detector.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+static imu_sample_t sample_at(uint64_t ticks, uint64_t timebase_hz,
+	                          double gyro_x_rps, unsigned int sequence)
+{
+	imu_sample_t sample;
+	memset(&sample, 0, sizeof(sample));
+	sample.timestamp_ticks = ticks;
+	double time_s = (double)ticks / (double)timebase_hz;
+	sample.accel_mps2[0] = 0.004 * sin(11.0 * time_s);
+	sample.accel_mps2[1] = 0.004 * cos(13.0 * time_s);
+	sample.accel_mps2[2] = 9.80665 + 0.004 * sin(17.0 * time_s);
+	sample.gyro_rps[0] = gyro_x_rps;
+	sample.gyro_rps[1] = 0.0002 * sin(7.0 * time_s);
+	sample.gyro_rps[2] = 0.0002 * cos(9.0 * time_s);
+	sample.temperature_c = 25.0;
+	sample.sequence = sequence;
+	return sample;
+}
+
+static void test_rate(unsigned int rate_hz)
+{
+	const uint64_t timebase_hz = 1000000u;
+	fusion_rest_detector_config_t config;
+	fusion_rest_detector_t detector;
+	fusion_rest_detector_default_config(&config);
+	CHECK(fusion_rest_detector_init(
+		&detector, &config, timebase_hz, true),
+	      "rest detector initializes in unarmed/landed mode");
+
+	bool initial_rest_seen = false;
+	bool impulse_cleared_rest = false;
+	double reentry_time_s = NAN;
+	uint64_t sample_count = 6u * rate_hz;
+	for(uint64_t index = 0u ; index <= sample_count ; index++)
+	{
+		uint64_t ticks = (index * timebase_hz + rate_hz / 2u) / rate_hz;
+		double time_s = (double)ticks / (double)timebase_hz;
+		double gyro_x = ((time_s >= 2.0) &&
+		                 (time_s < 2.0 + 2.0 / (double)rate_hz)) ? 4.0 : 0.0;
+		imu_sample_t sample = sample_at(
+			ticks, timebase_hz, gyro_x, (unsigned int)index);
+		CHECK(fusion_rest_detector_update(&detector, &sample),
+		      "monotonic IMU sample updates rest detector");
+		if((time_s >= 1.0) && (time_s < 2.0) && detector.at_rest)
+		{
+			initial_rest_seen = true;
+		}
+		if((time_s >= 2.0) && (time_s < 2.2) && !detector.at_rest)
+		{
+			impulse_cleared_rest = true;
+		}
+		if((time_s > 2.0) && !isfinite(reentry_time_s) && detector.at_rest)
+		{
+			reentry_time_s = time_s;
+		}
+	}
+	CHECK(initial_rest_seen, "quiet landed platform reaches at-rest after dwell");
+	CHECK(impulse_cleared_rest, "high-frequency gyro impulse clears at-rest");
+	CHECK(isfinite(reentry_time_s) && (reentry_time_s >= 3.0) &&
+	      (reentry_time_s <= 5.5),
+	      "rest returns only after vibration decay and one-second dwell");
+	printf("rest-detector %3u Hz: reentry=%.3f s accel-vibe=%.6f gyro-vibe=%.6f\n",
+	       rate_hz, reentry_time_s,
+	       detector.accel_vibration_metric_mps2,
+	       detector.gyro_vibration_metric_rps);
+}
+
+static void test_platform_gate_and_timestamps(void)
+{
+	const uint64_t timebase_hz = 1000000u;
+	fusion_rest_detector_config_t config;
+	fusion_rest_detector_t detector;
+	fusion_rest_detector_default_config(&config);
+	CHECK(fusion_rest_detector_init(
+		&detector, &config, timebase_hz, false),
+	      "rest detector initializes in mobile/not-landed mode");
+	for(unsigned int index = 0u ; index <= 150u ; index++)
+	{
+		imu_sample_t sample = sample_at(
+			(uint64_t)index * 10000u, timebase_hz, 0.0, index);
+		CHECK(fusion_rest_detector_update(&detector, &sample),
+		      "mobile quiet samples remain valid");
+	}
+	CHECK(!detector.at_rest,
+	      "quiet IMU cannot authorize ZUPT while platform is not landed");
+
+	imu_sample_t duplicate = sample_at(1500000u, timebase_hz, 0.0, 151u);
+	CHECK(!fusion_rest_detector_update(&detector, &duplicate),
+	      "duplicate timestamp is rejected");
+	fusion_rest_detector_set_landed(&detector, true, 1500000u);
+	for(unsigned int index = 151u ; index <= 260u ; index++)
+	{
+		imu_sample_t sample = sample_at(
+			(uint64_t)index * 10000u, timebase_hz, 0.0, index);
+		CHECK(fusion_rest_detector_update(&detector, &sample),
+		      "landed quiet samples update rest detector");
+	}
+	CHECK(detector.at_rest,
+	      "external landed fact plus quiet dwell authorizes at-rest");
+}
+
+int main(void)
+{
+	test_rate(52u);
+	test_rate(104u);
+	test_rate(208u);
+	test_platform_gate_and_timestamps();
+	if(failures != 0)
+	{
+		fprintf(stderr, "%d rest-detector test(s) failed\n", failures);
+		return 1;
+	}
+	printf("PX4-style platform rest detector tests passed.\n");
+	return 0;
+}

soc/software/fusion/tests/fusion_synthetic_test.c

@@ -4,7 +4,7 @@
 #include <stdio.h>
 #include <string.h>
 
-#include "fusion_engine.h"
+#include "fusion/fusion_engine.h"
 
 #define SYNTH_PI 3.14159265358979323846264338327950288
 #define SYNTH_TIMEBASE_HZ 1000000u
@@ -311,6 +311,9 @@ static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed)
 		}
 		imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0);
 		imu.sequence = (uint32_t)(tick / imu_period_ticks);
+		imu.vehicle_at_rest =
+			(norm3(truth.velocity_mps) < 0.05) &&
+			(norm3(acceleration_nav) < 0.05) && (fabs(yaw_rate) < 0.02);
 		CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted");
 		if(engine.state == FUSION_ENGINE_FAULT)
 		{
@@ -462,8 +465,8 @@ static void check_dynamic_metrics(const synth_metrics_t* metrics)
 	      "GPS reject diagnostics include injected outliers");
 	CHECK(metrics->replay_count >= 50u && metrics->replayed_imu_count > 1000u,
 	      "delayed GPS exercises fixed-lag replay extensively");
-	CHECK(metrics->zero_updates > 300u,
-	      "stationary periods apply rate-limited ZUPT updates");
+	CHECK(metrics->zero_updates > 150u,
+	      "stationary periods apply PX4-rate 5 Hz ZUPT updates");
 	CHECK(metrics->horizontal_rmse_m < 2.0,
 	      "bias/noise/delay trajectory horizontal RMSE is bounded");
 	CHECK(metrics->vertical_rmse_m < 2.5,
@@ -521,6 +524,7 @@ static stationary_metrics_t run_stationary_500hz_scenario(void)
 		}
 		imu.temperature_c = 24.0;
 		imu.sequence = (uint32_t)(tick / imu_period_ticks);
+		imu.vehicle_at_rest = true;
 		CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted");
 
 		if((tick >= next_gps_tick) && !gps_pending)
@@ -575,8 +579,8 @@ static void check_stationary_metrics(const stationary_metrics_t* metrics)
 	      "30 s / 500 Hz noise never creates a false moving state");
 	CHECK(metrics->accepted_gps >= 26u,
 	      "30 s / 500 Hz delayed stationary GPS remains accepted");
-	CHECK(metrics->zero_updates >= 500u,
-	      "30 s / 500 Hz stationary run keeps rate-limited ZUPT active");
+	CHECK(metrics->zero_updates >= 120u,
+	      "30 s / 500 Hz stationary run keeps PX4-rate ZUPT active");
 	CHECK(metrics->final_horizontal_error_m < 1.0,
 	      "30 s / 500 Hz stationary horizontal error is bounded");
 	CHECK(metrics->final_vertical_error_m < 1.0,

soc/software/fusion/tests/gps_reacquisition_test.c

@@ -0,0 +1,178 @@
+#include "fusion/fusion_engine.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#define TEST_TIMEBASE_HZ 1000000u
+#define TEST_IMU_HZ 52u
+#define TEST_GPS_PERIOD_TICKS 200000u
+
+static fusion_imu_measurement_t stationary_imu(uint64_t timestamp_ticks,
+	                                            double gravity_mps2)
+{
+	fusion_imu_measurement_t imu;
+	memset(&imu, 0, sizeof(imu));
+	imu.timestamp_ticks = timestamp_ticks;
+	imu.accel_mps2[2] = gravity_mps2;
+	imu.temperature_c = 25.0;
+	imu.vehicle_at_rest = true;
+	return imu;
+}
+
+static fusion_position_measurement_t gps_position(
+	uint64_t timestamp_ticks,
+	double east_m,
+	double north_m,
+	double up_m)
+{
+	fusion_position_measurement_t gps;
+	memset(&gps, 0, sizeof(gps));
+	gps.timestamp_ticks = timestamp_ticks;
+	gps.position_enu_m[0] = east_m;
+	gps.position_enu_m[1] = north_m;
+	gps.position_enu_m[2] = up_m;
+	gps.covariance_enu_m2[0] = 4.0;
+	gps.covariance_enu_m2[4] = 4.0;
+	gps.covariance_enu_m2[8] = 16.0;
+	gps.source_quality = 100.0;
+	gps.source_valid = true;
+	return gps;
+}
+
+static bool calibrate_at_origin(fusion_engine_t* engine,
+	                            fusion_engine_config_t* config)
+{
+	fusion_engine_default_config(config, TEST_TIMEBASE_HZ);
+	if(!fusion_engine_init(engine, config))
+	{
+		return false;
+	}
+	fusion_position_measurement_t origin = gps_position(1u, 0.0, 0.0, 0.0);
+	if(!fusion_engine_push_position(engine, &origin, NULL, NULL))
+	{
+		return false;
+	}
+	for(uint64_t sample = 0u ; sample <= 3u * TEST_IMU_HZ ; sample++)
+	{
+		uint64_t ticks = sample * TEST_TIMEBASE_HZ / TEST_IMU_HZ;
+		fusion_imu_measurement_t imu = stationary_imu(ticks, config->eskf.gravity_mps2);
+		if(!fusion_engine_push_imu(engine, &imu))
+		{
+			return false;
+		}
+	}
+	return engine->state == FUSION_ENGINE_RUNNING;
+}
+
+static int startup_offset_recovers(uint64_t gps_period_ticks)
+{
+	static fusion_engine_t engine;
+	fusion_engine_config_t config;
+	if(!calibrate_at_origin(&engine, &config))
+	{
+		fprintf(stderr, "FAIL: calibration did not complete\n");
+		return 1;
+	}
+
+	/* Models a poor first LLH origin followed by the stable, correct fixes seen
+	 * on UART.  ENU remains relative to the first origin, so the correct fixes
+	 * legitimately appear far from the initialized ESKF position. */
+	const double expected[3] = {80.0, 40.0, -170.0};
+	uint64_t next_gps_ticks = 3200000u;
+	for(uint64_t sample = 3u * TEST_IMU_HZ + 1u ;
+	    sample <= 12u * TEST_IMU_HZ ; sample++)
+	{
+		uint64_t ticks = sample * TEST_TIMEBASE_HZ / TEST_IMU_HZ;
+		fusion_imu_measurement_t imu = stationary_imu(ticks, config.eskf.gravity_mps2);
+		if(!fusion_engine_push_imu(&engine, &imu))
+		{
+			fprintf(stderr, "FAIL: IMU propagation failed\n");
+			return 1;
+		}
+		while(next_gps_ticks <= ticks)
+		{
+			fusion_position_measurement_t gps = gps_position(
+				next_gps_ticks, expected[0], expected[1], expected[2]);
+			(void)fusion_engine_push_position(&engine, &gps, NULL, NULL);
+			next_gps_ticks += gps_period_ticks;
+		}
+	}
+
+	double error = sqrt(
+		pow(engine.current.filter.position_m[0] - expected[0], 2.0) +
+		pow(engine.current.filter.position_m[1] - expected[1], 2.0) +
+		pow(engine.current.filter.position_m[2] - expected[2], 2.0));
+	printf("startup offset GPS=%4llu ms: error=%.3f m accepted=%llu "
+	       "rejected=%llu recovery=%llu\n",
+	       (unsigned long long)(gps_period_ticks / 1000u), error,
+	       (unsigned long long)engine.diagnostics.position_accepted,
+	       (unsigned long long)engine.diagnostics.position_nis_rejected,
+	       (unsigned long long)engine.diagnostics.position_recovery_count);
+	if((error > 5.0) || (engine.diagnostics.position_recovery_count != 1u))
+	{
+		fprintf(stderr, "FAIL: consistent GPS did not recover the wrong startup position\n");
+		return 1;
+	}
+	return 0;
+}
+
+static int isolated_outlier_does_not_reset(void)
+{
+	static fusion_engine_t engine;
+	fusion_engine_config_t config;
+	if(!calibrate_at_origin(&engine, &config))
+	{
+		fprintf(stderr, "FAIL: outlier scenario calibration did not complete\n");
+		return 1;
+	}
+
+	for(uint64_t gps_ticks = 3200000u ; gps_ticks <= 7000000u ;
+	    gps_ticks += TEST_GPS_PERIOD_TICKS)
+	{
+		uint64_t imu_ticks = gps_ticks + 10000u;
+		fusion_imu_measurement_t imu = stationary_imu(
+			imu_ticks, config.eskf.gravity_mps2);
+		if(!fusion_engine_push_imu(&engine, &imu))
+		{
+			fprintf(stderr, "FAIL: outlier scenario IMU propagation failed\n");
+			return 1;
+		}
+		bool outlier = (gps_ticks == 5000000u);
+		fusion_position_measurement_t gps = gps_position(
+			gps_ticks, outlier ? 150.0 : 0.0, outlier ? -120.0 : 0.0,
+			outlier ? 80.0 : 0.0);
+		(void)fusion_engine_push_position(&engine, &gps, NULL, NULL);
+	}
+
+	double distance = sqrt(
+		pow(engine.current.filter.position_m[0], 2.0) +
+		pow(engine.current.filter.position_m[1], 2.0) +
+		pow(engine.current.filter.position_m[2], 2.0));
+	printf("single outlier: error=%.3f m rejected=%llu recovery=%llu\n",
+	       distance,
+	       (unsigned long long)engine.diagnostics.position_nis_rejected,
+	       (unsigned long long)engine.diagnostics.position_recovery_count);
+	if((distance > 1.0) || (engine.diagnostics.position_nis_rejected == 0u) ||
+	   (engine.diagnostics.position_recovery_count != 0u))
+	{
+		fprintf(stderr, "FAIL: isolated GPS outlier moved/reset the ESKF\n");
+		return 1;
+	}
+	return 0;
+}
+
+int main(void)
+{
+	int failures = startup_offset_recovers(100000u);
+	failures += startup_offset_recovers(200000u);
+	failures += startup_offset_recovers(1000000u);
+	failures += isolated_outlier_does_not_reset();
+	if(failures == 0)
+	{
+		puts("GPS reacquisition tests passed");
+	}
+	return failures == 0 ? 0 : 1;
+}

soc/software/fusion/tests/imu_quality_synthetic_test.c

@@ -4,8 +4,9 @@
 #include <stdio.h>
 #include <string.h>
 
-#include "fusion_engine.h"
-#include "imu_sensor.h"
+#include "fusion/fusion_engine.h"
+#include "adapters/imu_fusion_profile.h"
+#include "imu/imu_sensor.h"
 
 #define QUALITY_PI 3.14159265358979323846264338327950288
 #define QUALITY_TIMEBASE_HZ 1000000u
@@ -158,7 +159,7 @@ static quality_metrics_t run_profile(imu_sensor_backend_t backend,
 {
 	static fusion_engine_t engine;
 	fusion_imu_quality_profile_t profile;
-	CHECK(imu_sensor_quality_profile_for_backend(backend, &profile),
+	CHECK(imu_fusion_quality_profile_for_backend(backend, &profile),
 	      "sensor fusion quality profile exists");
 	fusion_engine_config_t config;
 	fusion_engine_default_config(&config, QUALITY_TIMEBASE_HZ);
@@ -255,6 +256,8 @@ static quality_metrics_t run_profile(imu_sensor_backend_t backend,
 		}
 		imu.temperature_c = 25.0 + 0.05 * time_s;
 		imu.sequence = (uint32_t)(tick / imu_period_ticks);
+		imu.vehicle_at_rest = !truth_is_dynamic(
+			&truth, acceleration_nav, yaw_rate);
 		CHECK(fusion_engine_push_imu(&engine, &imu),
 		      "quality-profile IMU is accepted");
 
@@ -353,8 +356,8 @@ static void check_metrics(const quality_metrics_t* metrics,
 	      (metrics->final_stationary_reentry_s <=
 	       (noisier_st_profile ? 29.5 : 25.0)),
 	      "stationary re-entry waits for motion hysteresis and then recovers");
-	CHECK(metrics->zero_updates > (noisier_st_profile ? 100u : 150u),
-	      "stationary intervals apply rate-limited zero updates");
+	CHECK(metrics->zero_updates > 90u,
+	      "stationary intervals apply PX4-rate zero updates");
 	CHECK(metrics->accepted_gps >= 15u,
 	      "GPS updates remain usable with the sensor profile");
 	CHECK(metrics->horizontal_rmse_m < (noisier_st_profile ? 5.0 : 2.0),
@@ -371,10 +374,10 @@ int main(void)
 {
 	fusion_imu_quality_profile_t tdk_profile;
 	fusion_imu_quality_profile_t st_profile;
-	CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_TDK_ICM42688,
+	CHECK(imu_fusion_quality_profile_for_backend(IMU_SENSOR_TDK_ICM42688,
 	                                             &tdk_profile),
 	      "TDK profile is valid");
-	CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_ST_LSM6DSOX,
+	CHECK(imu_fusion_quality_profile_for_backend(IMU_SENSOR_ST_LSM6DSOX,
 	                                             &st_profile),
 	      "ST profile is valid");
 	CHECK(tdk_profile.nominal_gyro_noise_density_rps_sqrt_hz[0] <

soc/software/fusion/tests/platform_scenario_matrix_test.c

@@ -0,0 +1,551 @@
+#include "fusion/fusion_engine.h"
+
+#include <errno.h>
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define TIMEBASE_HZ 1000000u
+#define DURATION_S 50.0
+#define PI 3.14159265358979323846264338327950288
+
+typedef enum {
+	SCENARIO_STATIONARY = 0,
+	SCENARIO_PEDESTRIAN,
+	SCENARIO_CAR,
+	SCENARIO_AIRCRAFT,
+	SCENARIO_COUNT
+} scenario_kind_t;
+
+typedef struct {
+	double position_m[3];
+	double velocity_mps[3];
+	double yaw_rad;
+} truth_t;
+
+typedef struct {
+	double forward_accel_mps2;
+	double vertical_accel_mps2;
+	double yaw_rate_rps;
+} command_t;
+
+typedef struct {
+	uint32_t state;
+} rng_t;
+
+typedef struct {
+	double horizontal_rmse_m;
+	double final_horizontal_error_m;
+	double maximum_horizontal_error_m;
+	double maximum_burst_state_step_m;
+	uint64_t gps_accepted;
+	uint64_t gps_rejected;
+	uint64_t position_recoveries;
+	uint64_t zero_updates;
+	uint64_t zero_updates_after_startup;
+	bool finite;
+	bool recovered_after_burst;
+} metrics_t;
+
+static const char* scenario_name(scenario_kind_t scenario)
+{
+	static const char* names[] = {"stationary", "pedestrian", "car", "aircraft"};
+	return names[(unsigned int)scenario];
+}
+
+static bool parse_scenario(const char* text, scenario_kind_t* scenario_out)
+{
+	for(unsigned int index = 0u; index < SCENARIO_COUNT; index++)
+	{
+		if(strcmp(text, scenario_name((scenario_kind_t)index)) == 0)
+		{
+			*scenario_out = (scenario_kind_t)index;
+			return true;
+		}
+	}
+	return false;
+}
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double rng_uniform(rng_t* rng)
+{
+	rng->state = 1664525u * rng->state + 1013904223u;
+	return ((double)(rng->state >> 8u) + 0.5) / 16777216.0;
+}
+
+static double rng_normal(rng_t* rng)
+{
+	double u1 = fmax(rng_uniform(rng), 1.0e-12);
+	double u2 = rng_uniform(rng);
+	return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);
+}
+
+static command_t command_at(scenario_kind_t scenario, double time_s)
+{
+	command_t command = {0.0, 0.0, 0.0};
+	if(scenario == SCENARIO_PEDESTRIAN)
+	{
+		if((time_s >= 4.0) && (time_s < 6.0)) command.forward_accel_mps2 = 0.75;
+		else if((time_s >= 14.0) && (time_s < 16.0)) command.forward_accel_mps2 = -0.75;
+		else if((time_s >= 21.0) && (time_s < 23.0)) command.forward_accel_mps2 = 0.60;
+		else if((time_s >= 31.0) && (time_s < 33.0)) command.forward_accel_mps2 = -0.60;
+		if((time_s >= 24.0) && (time_s < 29.0)) command.yaw_rate_rps = 0.10;
+	}
+	else if(scenario == SCENARIO_CAR)
+	{
+		if((time_s >= 4.0) && (time_s < 9.0)) command.forward_accel_mps2 = 4.0;
+		else if((time_s >= 15.0) && (time_s < 18.0)) command.forward_accel_mps2 = -20.0 / 3.0;
+		else if((time_s >= 25.0) && (time_s < 29.0)) command.forward_accel_mps2 = 5.0;
+		else if((time_s >= 39.0) && (time_s < 42.0)) command.forward_accel_mps2 = -20.0 / 3.0;
+		if((time_s >= 31.0) && (time_s < 36.0)) command.yaw_rate_rps = 0.12;
+	}
+	else if(scenario == SCENARIO_AIRCRAFT)
+	{
+		if((time_s >= 4.0) && (time_s < 14.0)) command.forward_accel_mps2 = 8.0;
+		else if((time_s >= 26.0) && (time_s < 29.0)) command.forward_accel_mps2 = 4.0;
+		else if((time_s >= 42.0) && (time_s < 47.0)) command.forward_accel_mps2 = -6.0;
+		if((time_s >= 6.0) && (time_s < 10.0)) command.vertical_accel_mps2 = 2.0;
+		else if((time_s >= 18.0) && (time_s < 22.0)) command.vertical_accel_mps2 = -2.0;
+		if((time_s >= 20.0) && (time_s < 25.0)) command.yaw_rate_rps = 0.20;
+		else if((time_s >= 34.0) && (time_s < 36.0)) command.yaw_rate_rps = -0.35;
+	}
+	return command;
+}
+
+static void attitude_at(scenario_kind_t scenario, double time_s,
+	                    const truth_t* truth, double euler_rad[3],
+	                    double body_rate_rps[3])
+{
+	memset(euler_rad, 0, 3u * sizeof(double));
+	memset(body_rate_rps, 0, 3u * sizeof(double));
+	euler_rad[2] = truth->yaw_rad;
+	command_t command = command_at(scenario, time_s);
+	body_rate_rps[2] = command.yaw_rate_rps;
+	if(scenario != SCENARIO_STATIONARY)
+	{
+		return;
+	}
+	const double roll_max = 20.0 * PI / 180.0;
+	const double pitch_max = 12.0 * PI / 180.0;
+	if((time_s >= 10.0) && (time_s < 11.0))
+	{
+		euler_rad[0] = roll_max * (time_s - 10.0);
+		body_rate_rps[0] = roll_max;
+	}
+	else if((time_s >= 11.0) && (time_s < 13.0)) euler_rad[0] = roll_max;
+	else if((time_s >= 13.0) && (time_s < 14.0))
+	{
+		euler_rad[0] = roll_max * (14.0 - time_s);
+		body_rate_rps[0] = -roll_max;
+	}
+	if((time_s >= 18.0) && (time_s < 19.0))
+	{
+		euler_rad[1] = pitch_max * (time_s - 18.0);
+		body_rate_rps[1] = pitch_max;
+	}
+	else if((time_s >= 19.0) && (time_s < 21.0)) euler_rad[1] = pitch_max;
+	else if((time_s >= 21.0) && (time_s < 22.0))
+	{
+		euler_rad[1] = pitch_max * (22.0 - time_s);
+		body_rate_rps[1] = -pitch_max;
+	}
+}
+
+static void rotation_body_to_nav(const double euler[3], double rotation[3][3])
+{
+	double cr = cos(euler[0]);
+	double sr = sin(euler[0]);
+	double cp = cos(euler[1]);
+	double sp = sin(euler[1]);
+	double cy = cos(euler[2]);
+	double sy = sin(euler[2]);
+	rotation[0][0] = cy * cp;
+	rotation[0][1] = cy * sp * sr - sy * cr;
+	rotation[0][2] = cy * sp * cr + sy * sr;
+	rotation[1][0] = sy * cp;
+	rotation[1][1] = sy * sp * sr + cy * cr;
+	rotation[1][2] = sy * sp * cr - cy * sr;
+	rotation[2][0] = -sp;
+	rotation[2][1] = cp * sr;
+	rotation[2][2] = cp * cr;
+}
+
+static void propagate_truth(truth_t* truth, scenario_kind_t scenario,
+	                        double start_s, double dt_s)
+{
+	command_t command = command_at(scenario, start_s + 0.5 * dt_s);
+	double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
+	double yaw_mid = truth->yaw_rad + 0.5 * command.yaw_rate_rps * dt_s;
+	double acceleration[3] = {
+		command.forward_accel_mps2 * cos(yaw_mid) - speed * command.yaw_rate_rps * sin(yaw_mid),
+		command.forward_accel_mps2 * sin(yaw_mid) + speed * command.yaw_rate_rps * cos(yaw_mid),
+		command.vertical_accel_mps2
+	};
+	for(unsigned int axis = 0u; axis < 3u; axis++)
+	{
+		truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
+		                            0.5 * acceleration[axis] * dt_s * dt_s;
+		truth->velocity_mps[axis] += acceleration[axis] * dt_s;
+	}
+	truth->yaw_rad += command.yaw_rate_rps * dt_s;
+}
+
+static bool externally_at_rest(scenario_kind_t scenario, double time_s)
+{
+	if(time_s < 3.5) return true;
+	if(scenario != SCENARIO_STATIONARY) return false;
+	/* An external landed/rest supervisor owns this decision.  Attitude changes
+	 * and the vibration interval revoke the constraint; quiet IMU alone never
+	 * invents rest for the moving scenarios. */
+	return !(((time_s >= 9.8) && (time_s < 15.0)) ||
+	         ((time_s >= 17.8) && (time_s < 23.0)) ||
+	         ((time_s >= 27.0) && (time_s < 30.0)));
+}
+
+static fusion_imu_measurement_t make_imu(scenario_kind_t scenario,
+	                                    const truth_t* truth, double time_s,
+	                                    uint64_t ticks, double gravity,
+	                                    unsigned int sequence, rng_t* rng)
+{
+	command_t command = command_at(scenario, time_s);
+	double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
+	double acceleration_nav[3] = {
+		command.forward_accel_mps2 * cos(truth->yaw_rad) -
+			speed * command.yaw_rate_rps * sin(truth->yaw_rad),
+		command.forward_accel_mps2 * sin(truth->yaw_rad) +
+			speed * command.yaw_rate_rps * cos(truth->yaw_rad),
+		command.vertical_accel_mps2 + gravity
+	};
+	double euler[3];
+	double body_rate[3];
+	double rotation[3][3];
+	attitude_at(scenario, time_s, truth, euler, body_rate);
+	rotation_body_to_nav(euler, rotation);
+	fusion_imu_measurement_t imu;
+	memset(&imu, 0, sizeof(imu));
+	imu.timestamp_ticks = ticks;
+	for(unsigned int body = 0u; body < 3u; body++)
+	{
+		for(unsigned int nav = 0u; nav < 3u; nav++)
+		{
+			imu.accel_mps2[body] += rotation[nav][body] * acceleration_nav[nav];
+		}
+		static const double accel_bias[3] = {0.035, -0.025, 0.055};
+		static const double gyro_bias[3] = {0.0008, -0.0006, 0.0007};
+		imu.accel_mps2[body] += accel_bias[body] + 0.012 * rng_normal(rng);
+		imu.gyro_rps[body] = body_rate[body] + gyro_bias[body] +
+		                     0.00035 * rng_normal(rng);
+	}
+	if((scenario == SCENARIO_STATIONARY) &&
+	   (time_s >= 27.0) && (time_s < 29.0))
+	{
+		imu.accel_mps2[0] += 0.80 * sin(2.0 * PI * 7.0 * time_s);
+		imu.accel_mps2[1] += 0.55 * sin(2.0 * PI * 11.0 * time_s);
+		imu.gyro_rps[2] += 0.035 * sin(2.0 * PI * 5.0 * time_s);
+	}
+	imu.temperature_c = 25.0 + 0.5 * sin(0.08 * time_s);
+	imu.sequence = sequence;
+	imu.vehicle_at_rest = externally_at_rest(scenario, time_s);
+	return imu;
+}
+
+static fusion_position_measurement_t make_gps(const truth_t* truth,
+	                                          double time_s, uint64_t ticks,
+	                                          rng_t* rng, double gps_position[3])
+{
+	double horizontal_std = time_s < 8.0 ? 8.0 : 4.0;
+	double vertical_std = time_s < 8.0 ? 12.0 : 7.0;
+	for(unsigned int axis = 0u; axis < 3u; axis++)
+	{
+		double std = axis < 2u ? horizontal_std : vertical_std;
+		gps_position[axis] = truth->position_m[axis] + std * rng_normal(rng);
+	}
+	if((time_s >= 32.0) && (time_s < 34.0))
+	{
+		gps_position[0] += 130.0;
+		gps_position[1] -= 90.0;
+		gps_position[2] += 55.0;
+	}
+	fusion_position_measurement_t gps;
+	memset(&gps, 0, sizeof(gps));
+	gps.timestamp_ticks = ticks;
+	memcpy(gps.position_enu_m, gps_position, sizeof(gps.position_enu_m));
+	gps.covariance_enu_m2[0] = square(horizontal_std);
+	gps.covariance_enu_m2[4] = square(horizontal_std);
+	gps.covariance_enu_m2[8] = square(vertical_std);
+	/* Exactly four satellites have zero residual degrees of freedom.  This is
+	 * therefore a valid WLS output with covariance but no redundant FDE test. */
+	gps.source_quality = 100.0;
+	gps.source_valid = true;
+	return gps;
+}
+
+static void local_to_geodetic(const double position[3], double* latitude_deg,
+	                          double* longitude_deg, double* height_m)
+{
+	const double origin_latitude_deg = 40.78925034214207;
+	const double origin_longitude_deg = 29.46277530841728;
+	const double origin_height_m = 100.0;
+	const double latitude_rad = origin_latitude_deg * PI / 180.0;
+	*latitude_deg = origin_latitude_deg + position[1] / 6378137.0 * 180.0 / PI;
+	*longitude_deg = origin_longitude_deg +
+		position[0] / (6378137.0 * cos(latitude_rad)) * 180.0 / PI;
+	*height_m = origin_height_m + position[2];
+}
+
+static void write_location(FILE* log_file, const char* source,
+	                       scenario_kind_t scenario, double time_s,
+	                       const double position[3], unsigned int gps_ms,
+	                       unsigned int imu_ms)
+{
+	if(log_file == NULL) return;
+	double latitude;
+	double longitude;
+	double height;
+	local_to_geodetic(position, &latitude, &longitude, &height);
+	fprintf(log_file,
+	        "[LOCATION] source=%s scenario=%s t=%.3f lat=%.12f lon=%.12f "
+	        "height=%.3f gdop=3.2 pdop=2.6 hdop=2.0 vdop=1.7 tdop=1.8 "
+	        "tracked=4 used=4 gps_ms=%u imu_ms=%u\n",
+	        source, scenario_name(scenario), time_s, latitude, longitude,
+	        height, gps_ms, imu_ms);
+}
+
+static metrics_t run_case(scenario_kind_t scenario, unsigned int gps_ms,
+	                      unsigned int imu_ms, FILE* log_file)
+{
+	static fusion_engine_t engine;
+	memset(&engine, 0, sizeof(engine));
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, TIMEBASE_HZ);
+	config.calibration_duration_s = 2.0;
+	/* Calibration is time-based for asynchronous sensors; retain enough samples
+	 * to estimate variance without silently assuming a particular ODR. */
+	config.calibration_min_samples = 2u * 1000u / imu_ms + 1u;
+	metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	if(!fusion_engine_init(&engine, &config)) return metrics;
+
+	truth_t truth;
+	memset(&truth, 0, sizeof(truth));
+	rng_t imu_rng = {0x7a31f20du ^ ((uint32_t)scenario << 24u) ^ imu_ms};
+	rng_t gps_rng = {0xb41d93c7u ^ ((uint32_t)scenario << 20u) ^ gps_ms};
+	uint64_t imu_period_ticks = (uint64_t)imu_ms * 1000u;
+	uint64_t gps_period_ticks = (uint64_t)gps_ms * 1000u;
+	uint64_t next_imu_ticks = 0u;
+	uint64_t next_gps_ticks = 0u;
+	uint64_t next_plot_ticks = 0u;
+	uint64_t end_ticks = (uint64_t)(DURATION_S * TIMEBASE_HZ);
+	uint64_t last_ticks = 0u;
+	unsigned int imu_sequence = 0u;
+	double error_squared_sum = 0.0;
+	uint64_t error_count = 0u;
+	uint64_t accepted_at_burst_end = 0u;
+	uint64_t zero_updates_at_startup_end = 0u;
+	double previous_horizontal[2] = {0.0, 0.0};
+	bool have_previous = false;
+
+	if(log_file != NULL)
+	{
+		fprintf(log_file, "# ESKF synthetic replay; blue=GPS red=IMU-corrected green=truth\n");
+		fprintf(log_file, "# scenario=%s gps_ms=%u imu_ms=%u satellites=4\n",
+		        scenario_name(scenario), gps_ms, imu_ms);
+	}
+	while(true)
+	{
+		uint64_t ticks = next_imu_ticks;
+		if(next_gps_ticks < ticks) ticks = next_gps_ticks;
+		if(next_plot_ticks < ticks) ticks = next_plot_ticks;
+		if(ticks > end_ticks) break;
+		double start_s = (double)last_ticks / TIMEBASE_HZ;
+		double dt_s = (double)(ticks - last_ticks) / TIMEBASE_HZ;
+		propagate_truth(&truth, scenario, start_s, dt_s);
+		last_ticks = ticks;
+		double time_s = (double)ticks / TIMEBASE_HZ;
+
+		if(ticks == next_gps_ticks)
+		{
+			/* A 3 s dropout exercises asynchronous dead reckoning. */
+			if(!((time_s >= 25.0) && (time_s < 28.0)))
+			{
+				double gps_position[3];
+				fusion_position_measurement_t gps = make_gps(
+					&truth, time_s, ticks, &gps_rng, gps_position);
+				eskf_update_result_t result = ESKF_UPDATE_INVALID;
+				(void)fusion_engine_push_position(&engine, &gps, &result, NULL);
+				write_location(log_file, "gps", scenario, time_s, gps_position,
+				               gps_ms, imu_ms);
+			}
+			next_gps_ticks += gps_period_ticks;
+		}
+
+		if(ticks == next_imu_ticks)
+		{
+			fusion_imu_measurement_t imu = make_imu(
+				scenario, &truth, time_s, ticks, config.eskf.gravity_mps2,
+				imu_sequence++, &imu_rng);
+			if(!fusion_engine_push_imu(&engine, &imu)) break;
+			next_imu_ticks += imu_period_ticks;
+		}
+
+		if(engine.state == FUSION_ENGINE_RUNNING)
+		{
+			if((time_s >= 4.0) && (zero_updates_at_startup_end == 0u))
+				zero_updates_at_startup_end =
+					engine.diagnostics.zero_velocity_update_count;
+			double horizontal_error = hypot(
+				engine.current.filter.position_m[0] - truth.position_m[0],
+				engine.current.filter.position_m[1] - truth.position_m[1]);
+			metrics.maximum_horizontal_error_m = fmax(
+				metrics.maximum_horizontal_error_m, horizontal_error);
+			if(time_s >= 8.0)
+			{
+				error_squared_sum += square(horizontal_error);
+				error_count++;
+			}
+			if(have_previous && (time_s >= 32.0) && (time_s < 34.2))
+			{
+				double step = hypot(
+					engine.current.filter.position_m[0] - previous_horizontal[0],
+					engine.current.filter.position_m[1] - previous_horizontal[1]);
+				metrics.maximum_burst_state_step_m = fmax(
+					metrics.maximum_burst_state_step_m, step);
+			}
+			previous_horizontal[0] = engine.current.filter.position_m[0];
+			previous_horizontal[1] = engine.current.filter.position_m[1];
+			have_previous = true;
+			if((time_s >= 34.0) && (accepted_at_burst_end == 0u))
+				accepted_at_burst_end = engine.diagnostics.position_accepted;
+		}
+
+		if(ticks == next_plot_ticks)
+		{
+			write_location(log_file, "trajectory", scenario, time_s,
+			               truth.position_m, gps_ms, imu_ms);
+			if(engine.state == FUSION_ENGINE_RUNNING)
+				write_location(log_file, "imu_corrected", scenario, time_s,
+				               engine.current.filter.position_m, gps_ms, imu_ms);
+			next_plot_ticks += 200000u;
+		}
+	}
+
+	double final_error = hypot(
+		engine.current.filter.position_m[0] - truth.position_m[0],
+		engine.current.filter.position_m[1] - truth.position_m[1]);
+	metrics.horizontal_rmse_m = error_count > 0u
+		? sqrt(error_squared_sum / (double)error_count) : INFINITY;
+	metrics.final_horizontal_error_m = final_error;
+	metrics.gps_accepted = engine.diagnostics.position_accepted;
+	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
+	metrics.position_recoveries = engine.diagnostics.position_recovery_count;
+	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
+	metrics.zero_updates_after_startup = metrics.zero_updates >= zero_updates_at_startup_end
+		? metrics.zero_updates - zero_updates_at_startup_end : metrics.zero_updates;
+	metrics.recovered_after_burst = engine.diagnostics.position_accepted >
+		accepted_at_burst_end;
+	metrics.finite = (engine.state == FUSION_ENGINE_RUNNING) &&
+		eskf_state_is_finite(&engine.current.filter) && isfinite(final_error);
+	return metrics;
+}
+
+static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
+	                      unsigned int imu_ms, const metrics_t* metrics)
+{
+	printf("%-10s GPS=%4ums IMU=%3ums rmse=%7.2fm final=%7.2fm max=%7.2fm "
+	       "burst-step=%6.2fm gps=%3llu/%2llu recovery=%llu zupt=%llu(+%llu) finite=%u\n",
+	       scenario_name(scenario), gps_ms, imu_ms, metrics->horizontal_rmse_m,
+	       metrics->final_horizontal_error_m, metrics->maximum_horizontal_error_m,
+	       metrics->maximum_burst_state_step_m,
+	       (unsigned long long)metrics->gps_accepted,
+	       (unsigned long long)metrics->gps_rejected,
+	       (unsigned long long)metrics->position_recoveries,
+	       (unsigned long long)metrics->zero_updates,
+	       (unsigned long long)metrics->zero_updates_after_startup,
+	       metrics->finite ? 1u : 0u);
+}
+
+static int run_matrix(void)
+{
+	static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
+	static const unsigned int imu_periods_ms[] = {19u, 20u, 200u, 300u};
+	bool passed = true;
+	printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
+	for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
+	{
+		for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
+		{
+			for(unsigned int imu_index = 0u; imu_index < 4u; imu_index++)
+			{
+				metrics_t metrics = run_case((scenario_kind_t)scenario,
+					gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
+				print_metrics((scenario_kind_t)scenario, gps_periods_ms[gps_index],
+				              imu_periods_ms[imu_index], &metrics);
+				bool common_ok = metrics.finite && metrics.recovered_after_burst &&
+					(metrics.position_recoveries == 0u) &&
+					(metrics.maximum_burst_state_step_m < 40.0);
+				bool rest_ok = (scenario == SCENARIO_STATIONARY)
+					? (metrics.zero_updates_after_startup > 0u)
+					: (metrics.zero_updates_after_startup == 0u);
+				bool production_rate_ok = (imu_periods_ms[imu_index] > 20u) ||
+					(metrics.final_horizontal_error_m < 25.0);
+				passed = passed && common_ok && rest_ok && production_rate_ok;
+			}
+		}
+	}
+	if(!passed)
+	{
+		fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
+		return 1;
+	}
+	printf("PASS: 19/20 ms production rates stayed bounded; 200/300 ms coarse IMU "
+	       "rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
+	return 0;
+}
+
+int main(int argc, char** argv)
+{
+	if(argc == 1) return run_matrix();
+	if((argc == 6) && (strcmp(argv[1], "--log") == 0))
+	{
+		scenario_kind_t scenario;
+		if(!parse_scenario(argv[2], &scenario))
+		{
+			fprintf(stderr, "unknown scenario: %s\n", argv[2]);
+			return 2;
+		}
+		unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
+		unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
+		if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
+		   !((imu_ms == 19u) || (imu_ms == 20u) ||
+		      (imu_ms == 200u) || (imu_ms == 300u)))
+		{
+			fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/200/300 ms\n");
+			return 2;
+		}
+		FILE* log_file = fopen(argv[5], "w");
+		if(log_file == NULL)
+		{
+			fprintf(stderr, "cannot create %s: %s\n", argv[5], strerror(errno));
+			return 2;
+		}
+		metrics_t metrics = run_case(scenario, gps_ms, imu_ms, log_file);
+		if(fclose(log_file) != 0)
+		{
+			fprintf(stderr, "cannot close %s\n", argv[5]);
+			return 2;
+		}
+		print_metrics(scenario, gps_ms, imu_ms, &metrics);
+		printf("wrote replay log: %s\n", argv[5]);
+		return metrics.finite ? 0 : 1;
+	}
+	fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
+	                "200|500|1000 19|20|200|300 output.log]\n", argv[0]);
+	return 2;
+}

soc/software/fusion/tests/standalone_gps_integrity_test.c

@@ -9,7 +9,7 @@
 #include <string.h>
 #include <time.h>
 
-#include "gps_time_anchor.h"
+#include "gps/gps_time_anchor.h"
 
 #define MAX_SATELLITES 9
 #define STATE_DIM 4