Add bare-metal TDK IMU ESKF fusion

soc/software/fusion/README.md

@@ -69,6 +69,435 @@ ESKF'nin anlamlı olması için GPS ve ham/decimate edilmiş IMU örnekleri ayn
 firmware çalışmasında ortak donanım zaman damgasıyla kaydedilmelidir; 500 ms
 ortalamalar hızlı dinamiği korumaz.
 
+## Bare-metal C ESKF ve Linux doğrulaması (2026-07-13)
+
+> **Doğrulama durumu:** Bu ESKF/TDK entegrasyonu henüz real hardware üzerinde
+> çalıştırılmadı. Kart test sırasında bağlı değildi. Aşağıdaki hareket,
+> stationary, accuracy, outlier-rejection ve performans sonuçları Linux/GCC
+> ortamında üretim bare-metal C kaynaklarıyla yapılan sentetik testlere aittir.
+> Bunlar algoritma/regression doğrulamasıdır; gerçek sensör eksenleri, SPI/INT1,
+> IRQ/sample kaybı, 50 MHz Rocket deadline'ı veya gerçek saha GPS doğruluğu için
+> donanım kanıtı değildir.
+
+Python modelinden bağımsız olarak aynı algoritma artık `gps_system` içinde
+freestanding C ile çalışır. Algoritma katmanı Linux testinde de kartta da aynı
+kaynaklardan derlenir; Linux için ayrı veya sadeleştirilmiş bir ESKF kopyası
+yoktur.
+
+Temel parçalar:
+
+- `gps_system/src/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
+  durum makinesi, filtrelenmiş jerk ve ZUPT izni;
+- `gps_system/src/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
+  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
+  bağlayan üst koordinatör.
+
+### ESKF state ve matematik sınırı
+
+Nominal state aşağıdaki bileşenlerden oluşur:
+
+```text
+x = [p_ENU(3), v_ENU(3), q_body_to_ENU(4), b_accel(3), b_gyro(3)]
+```
+
+Covariance içinde quaternion'ın dört elemanı doğrudan tutulmaz. Sağ-taraflı
+küçük açı hatası kullanılarak hata state'i 15 boyutta kalır:
+
+```text
+delta_x = [delta_p(3), delta_v(3), delta_theta(3),
+           delta_b_accel(3), delta_b_gyro(3)]
+```
+
+IMU propagation'da ölçülen specific force ve angular rate'ten bias çıkarılır.
+Quaternion body frame'den ENU frame'e dönüşümü temsil eder. ENU lineer ivmesi
+`R(q) * (accel - b_accel) - [0, 0, g]` ile, quaternion ise gyro exponential
+map ile ilerletilir. Büyük `dt` değerleri en fazla 20 ms'lik alt adımlara
+bölünür. Covariance birinci dereceden error-state transition ve IMU/bias
+process noise ile ilerletilir; her işlemden sonra simetrikleştirilir ve
+diagonal tabanı uygulanır.
+
+GPS konum residual'ı `r = z_ENU - p_ENU`, innovation covariance'i
+`S = P_pp + R_GPS` olarak hesaplanır. `NIS = r^T S^-1 r` değeri 3 DOF için
+`16.266236` (%99.9) kapısını aşarsa ölçüm state'e dokunmadan reddedilir.
+Kabul edilen update Joseph covariance formunu, quaternion error injection'ı
+ve attitude reset Jacobian'ını kullanır.
+
+Default ESKF process/başlangıç değerleri:
+
+| Parametre | Default |
+|---|---:|
+| Accel noise density | 0.03 m/s2/sqrt(Hz), her eksen |
+| Gyro noise density | 0.003 rad/s/sqrt(Hz), her eksen |
+| Accel bias random walk | 0.002 m/s2/sqrt(s) |
+| Gyro bias random walk | 0.002 deg/s/sqrt(s) |
+| İlk yatay/düşey konum sigma | 10 m / 20 m |
+| İlk hız sigma | 1 m/s |
+| İlk roll-pitch/yaw sigma | 5 deg / 30 deg |
+| Gravity | 9.80665 m/s2 |
+| Maksimum propagation alt adımı | 20 ms |
+| Konum NIS kapısı | 16.266236, 3 DOF %99.9 |
+
+### Asenkron veri akışı ve timestamp
+
+IMU ve GPS aynı local monotonic CPU/CLINT tick alanında timestamp edilir.
+Filtre `dt` hesabında GPS week/TOW kullanmaz. GPS ölçümü WLS'nin bittiği anda
+değil, ölçüm epoch interrupt'ında alınan local tick ile filtreye girer. WLS
+geç geldiyse engine son GPS checkpoint'ine dönüp IMU geçmişini gerçek ölçüm
+anına kadar oynatır, GPS update'i uygular ve bugüne kadar tekrar ilerler.
+
+Akışın ayrıntısı:
+
+1. TDK INT1 ISR'a girildiğinde `BSP_CLINT_MTIME` hemen okunur.
+2. ISR sensörün 14-byte register burst'ünü alır ve yalnız raw integer değerleri
+   256-elemanlı SPSC kuyruğa koyar; ISR içinde floating-point ESKF hesabı yoktur.
+3. Foreground raw accel/gyro/temp değerlerini SI birimlerine çevirir ve aynı
+   local timestamp ile fusion engine'e verir.
+4. Her uydu nav-solve event'i yakalandığında local tick event kaydına eklenir.
+   Coherent WLS epoch'u için geçerli uydu timestamp'lerinin medyanı seçilir.
+5. WLS daha sonra tamamlansa bile GPS mesajı bu ölçüm-epoch timestamp'ini taşır.
+6. Foreground önce bekleyen IMU'ları, sonra GPS kuyruğunu tüketir. Gecikmiş GPS
+   son checkpoint'ten kendi timestamp'ine kadar IMU replay edilerek uygulanır;
+   kalan IMU geçmişi tekrar oynatılarak current state yeniden oluşturulur.
+
+IMU history kapasitesi 2048 örnektir; 500 Hz'de yaklaşık 4.096 s fixed-lag
+alanı sağlar. History dolarsa en eski IMU sessizce zaman aralığı kaybettirilerek
+atılmaz: checkpoint önce o örnekle ilerletilir, sonra ring buffer'dan çıkarılır.
+Gelecek timestamp'li GPS için sekiz-elemanlı sıralı pending queue bulunur;
+checkpoint'ten eski ölçüm stale olarak reddedilir.
+
+GPS week/TOW uydu zamanı çözümü ve kanal coherence kontrolü için gereklidir,
+ancak ESKF `dt` kaynağı değildir. Bu ayrım WLS computation latency'sinin IMU
+zaman eksenini bozmasını önler.
+
+### Başlangıç kalibrasyonu, hareket kararı ve zero update
+
+Engine açılışta doğrudan RUNNING olmaz. Default olarak en az 2 s ve en az 50
+quiet IMU örneği ister. Tek örnekte gravity norm hatası 0.30 m/s2'yi veya gyro
+normu 2 deg/s'yi aşarsa pencere sıfırlanır. Pencere sonunda her eksende accel
+standard deviation 0.12 m/s2, gyro standard deviation 0.15 deg/s altında
+olmalıdır. Gravity ortalaması roll/pitch'i, gyro ortalaması ilk gyro bias'ını
+verir. Yaw gravity ile gözlenemediği için başlangıçta sıfır referanslı kalır.
+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.
+
+Motion-state default eşikleri:
+
+| Karar parametresi | Değer |
+|---|---:|
+| Stationary lineer ivme üst sınırı | 0.15 m/s2 |
+| Stationary gyro üst sınırı | 1 deg/s |
+| Güçlü hareket accel eşiği | 0.40 m/s2 |
+| Güçlü hareket gyro eşiği | 3 deg/s |
+| Güçlü hareket jerk eşiği | 4 m/s3 |
+| Jerk düşük-geçiren zaman sabiti | 100 ms |
+| Stationary hız kapısı | 0.40 m/s |
+| Moving hız kapısı | 0.50 m/s |
+| 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.
+
+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
+kararı iki doğrulama ister. Bu metrik GPS position update NIS'inden ayrıdır;
+biri hareket-state veto'su, diğeri ölçüm kabul/reject kapısıdır.
+
+GPS adapter ilk valid çözümü ENU origin yapar. WLS covariance yatayda en az
+0.50 m, düşeyde en az 1.00 m sigma ile sınırlandırılır; korelasyonlar clamp
+edilir ve covariance positive-definite değilse ESKF'ye ulaşmadan reddedilir.
+
+### WLS çıktısı ESKF'ye nasıl besleniyor?
+
+WLS çıktısı ESKF'ye gerçekten beslenmektedir. ESKF yalnız IMU ile serbest INS
+çalıştırmıyor; her valid GPS epoch'unda WLS konumu ve WLS measurement
+covariance'i ile position update yapıyor. Tam veri yolu şöyledir:
+
+```text
+Uydu pseudorange ölçümleri
+        |
+        v
+GPS WLS: konum + ENU covariance + residual/chi-square + validity
+        |
+        v
+gps_fusion_publisher: ortak local CPU timestamp'li GPS mesajı
+        |
+        v
+GPS adapter: LLH -> local ENU, covariance sanitize/floor
+        |
+        v
+ESKF: GPS position innovation + NIS + Kalman update
+```
+
+WLS--ESKF sınırındaki alanlar:
+
+| WLS çıktısı | ESKF tarafındaki mevcut kullanım |
+|---|---|
+| Latitude/longitude/height | ENU position measurement'a çevrilip update'te kullanılır |
+| 3x3 ENU covariance | GPS measurement covariance `R_GPS` olarak kullanılır |
+| `solution_valid` | False ise GPS update yapılmaz |
+| Measurement local timestamp | GPS update fixed-lag replay ile doğru epoch'ta uygulanır |
+| `chi_square_ratio` | Mesajda taşınır ve source-quality üretilir; Kalman gain'i şu an sürekli ölçeklemez |
+| `hAcc/vAcc` | Mesajda taşınır; ESKF ayrı scalar olarak değil tam 3x3 covariance'i kullanır |
+| Residual RMS | Mesajda taşınır fakat ESKF update denkleminde şu an doğrudan kullanılmaz |
+| Uydu başına post-fit residual | ESKF'ye tek tek gönderilmez; WLS integrity/FDE katmanında kalır |
+
+Burada iki farklı residual karıştırılmamalıdır:
+
+1. **WLS post-fit pseudorange residual'ı:** Her uydu için ölçülen pseudorange
+   ile çözülen geometri/clock modelinin farkıdır. WLS bu residual'lardan
+   `sum(weight_i * residual_i^2)`, DOF ve chi-square üretir. Chi-square fail
+   olursa `solution_valid=false` olur ve ölçüm ESKF tarafından kullanılmaz.
+2. **ESKF GPS--INS innovation residual'ı:** WLS'nin ürettiği ENU konum ile
+   ESKF'nin IMU propagation sonucu tahmin ettiği ENU konumun farkıdır:
+
+   ```text
+   r_ESKF = p_WLS_ENU - p_INS_ENU
+   S = P_ESKF_position + R_GPS_WLS
+   NIS = r_ESKF^T * S^-1 * r_ESKF
+   ```
+
+   ESKF bu NIS değeri 3 DOF %99.9 kapısını aşarsa upstream WLS valid olsa bile
+   GPS ölçümünü ikinci kez reddeder.
+
+Dolayısıyla iki covariance birbirinden farklıdır:
+
+- `P_ESKF`, IMU/INS error-state covariance'idir. IMU noise, gyro/accel bias
+  random walk, attitude, geçen `dt` ve önceki update'lerle ESKF içinde
+  propagation edilir.
+- `R_GPS`, WLS position measurement covariance'idir. Şu anda
+  `(A^T * W_absolute * A)^-1` ile uydu geometrisi ve absolute measurement
+  weight'lerinden hesaplanıp ENU'ya çevrilir.
+
+WLS post-fit residual büyüklüğü mevcut kodda `R_GPS`'yi sürekli/adaptif olarak
+şişirmez. Residual'ın mevcut doğrudan etkisi hard validity/chi-square kapısıdır;
+accepted bir ölçümün Kalman ağırlığı esas olarak `P_ESKF` ve WLS'den gelen
+`R_GPS` ile belirlenir. İleride absolute uydu sigma modeli gerçek loglarla
+kalibre edildikten sonra reduced chi-square tabanlı, sınırlandırılmış bir
+covariance inflation eklenebilir. Bunun için threshold'a bölünmüş mevcut
+`chi_square_ratio` yerine istatistiksel ölçek olarak
+`weighted_residual_sum / DOF` kullanılmalıdır.
+
+Hareket kararıyla ilgili iki koruma sentetik test sonucunda eklendi:
+
+- 200--500 Hz ham örnek farkı doğrudan jerk sayılmıyor; 100 ms zaman sabitli
+  düşük-geçiren ivmeden jerk üretiliyor. Böylece stationary IMU noise yanlış
+  hareket kararı üretmiyor, 1 m/s2 ani ivme yine ilk örnekte algılanıyor.
+- Position-only GPS'in tek/iki gürültülü noktadan çıkardığı `still` kararı,
+  yüksek ESKF hızını ezip ZUPT başlatamıyor. GPS `moving` ek veto olarak
+  kullanılıyor; stationary hız kapısı 0.40 m/s, moving kapısı 0.50 m/s ile
+  histerezisli. İleride GPS Doppler velocity gelirse daha güçlü ve hızlı bir
+  duruş kanıtı olarak ayrıca eklenebilir.
+
+Linux/GCC testlerini çalıştırma:
+
+```sh
+cd soc/software/fusion/tests
+make clean
+make test
+make sanitize
+make profile
+```
+
+`fusion_synthetic_test.c`, üretim kaynaklarıyla şu durumları birlikte sınar:
+
+- bias ve noise içeren stationary başlangıç kalibrasyonu;
+- 100/200/500 Hz ve düzensiz local-tick IMU aralıkları;
+- ani ivmelenme, sabit hız, ani fren, 90 derece yaw ve tekrar hareket;
+- 0.8--1.2 s değişken GPS periodu ve 180--350 ms teslim gecikmesi;
+- fixed-lag replay, covariance simetrisi/PSD ve uzun IMU geçmişi;
+- iki adet yaklaşık 200 m sınıfı multipath/outlier ve NIS rejection;
+- sekiz ayrı deterministic noise/timing seed'i;
+- 30 s boyunca 500 Hz stationary IMU ve gecikmiş 1 Hz GPS.
+
+### Sentetik hareketli senaryo
+
+Hareketli senaryo 200 Hz IMU ile 60 s sürer. Ground-truth profil şöyledir:
+
+| Zaman | Ground truth hareket |
+|---|---|
+| 0--5 s | Stationary; ilk 2 s calibration alanı |
+| 5--7 s | East yönünde +1.0 m/s2 ani ivmelenme |
+| 7--10 s | East yönünde 2.0 m/s sabit hız |
+| 10--12 s | East yönünde -1.0 m/s2 fren, tam duruş |
+| 12--15 s | Stationary |
+| 15--17 s | Yerinde +45 deg/s yaw, toplam 90 deg dönüş |
+| 17--19 s | North yönünde +0.8 m/s2 ivmelenme |
+| 19--22 s | North yönünde 1.6 m/s sabit hız |
+| 22--24 s | North yönünde -0.8 m/s2 fren, tam duruş |
+| 24--60 s | Stationary |
+
+Truth son konumu `[east=10 m, north=8 m, up=0 m]` ve son hızı sıfırdır.
+Body-frame specific force, truth yaw ile ENU ivmesinin ters rotasyonundan
+üretilir; dolayısıyla 90 derece dönüşten sonraki North ivmesi body X eksenine
+doğru düşer ve attitude propagation da test edilir.
+
+Sentetik sensör modeli:
+
+| Girdi | Bias / noise |
+|---|---|
+| Accel bias | `[+0.030, -0.025, +0.060]` m/s2 |
+| Gyro bias | `[+0.0010, -0.0007, +0.0009]` rad/s |
+| Accel sample noise | Gaussian, 0.025 m/s2 sigma |
+| Gyro sample noise | Gaussian, 0.0008 rad/s sigma |
+| GPS yatay/düşey noise | 1.2 m / 2.0 m sigma |
+| GPS measurement periodu | 0.8--1.2 s deterministic-random |
+| WLS/teslim gecikmesi | 180--350 ms |
+| GPS outlier | İki epoch'ta `[+150, -120, +80]` m; yaklaşık 208 m 3B |
+
+Outlier GPS mesajları source-valid bırakılır; böylece sadece upstream validity
+değil ESKF'nin kendi NIS kapısı gerçekten sınanır. Her GPS farklı gecikmeyle
+geldiği için current state'e doğrudan uygulanamaz ve fixed-lag replay yolu her
+update'te çalışır. Sekiz seed IMU noise, GPS noise ve GPS period/delay dizisini
+değiştirir; aynı seed her çalıştırmada aynı sonucu vererek regression testini
+deterministic tutar.
+
+### Sentetik stationary senaryo
+
+İkinci uçtan uca senaryo 30 s boyunca 500 Hz IMU ve 1 Hz GPS kullanır. GPS
+250 ms gecikmeli gelir; yatay/düşey sigma 0.8/1.5 m'dir. Accel bias
+`[+0.020, -0.015, +0.050]` m/s2, gyro bias
+`[+0.0008, -0.0005, +0.0006]` rad/s; sample noise sırasıyla 0.020 m/s2 ve
+0.0006 rad/s'dir. Testin amacı yalnız position error değil, yüksek IMU rate'te
+ham sample-to-sample noise'un jerk üzerinden sahte `MOVING` üretmediğini ve
+20 Hz ZUPT'nin rate'ten bağımsız kaldığını göstermektir.
+
+### Unit-level ve invariant kontrolleri
+
+`eskf_motion_test.c` aşağıdaki kontrolleri ayrıca tek tek yapar:
+
+- düz ve tilt edilmiş stationary gravity propagation;
+- normal GPS NIS accept ve büyük outlier reject; reject sonrası state'in
+  bit seviyesine yakın değişmemesi;
+- ani hızlanma/fren, sabit hız sırasında stationary veto ve quiet dwell;
+- stationary 500 Hz IMU noise altında false-motion olmaması;
+- 100/200/500 Hz ve 3--17 ms düzensiz local-tick `dt` dizileri;
+- 300 ms gecikmiş GPS replay sonucunun on-time filtre ile position, velocity,
+  quaternion, bias, 15x15 covariance ve motion state'te `1e-11` toleransta
+  aynı olması;
+- future GPS pending, checkpoint'ten eski GPS stale reject;
+- GPS kesintisinde 2048-history rollover ve checkpoint'in zaman kaybetmeden
+  ilerlemesi;
+- LLH--ECEF--ENU round-trip, covariance floor/PD kontrolü;
+- IMU/GPS SPSC queue FIFO sırası ve overflow sayaçları.
+
+Uçtan uca acceptance limitleri test kodunun içinde sabittir: hareketli yatay
+RMSE `<2.0 m`, düşey RMSE `<2.5 m`, son yatay/düşey hata `<1.5/2.0 m`, son
+hız `<0.05 m/s`, constant-velocity false-stationary örneği sıfır, bütün
+outlier'lar reject ve covariance PSD olmalıdır. Stationary 500 Hz koşusunda
+son yatay/düşey hata `<1/1 m`, son hız `<0.02 m/s` ve false-moving örneği
+sıfır olmalıdır. Bunlar gerçek dünya accuracy garantisi değil, regression
+fail sınırlarıdır.
+
+Son Linux/GCC `-O2` sonuçları:
+
+| Sentetik koşu | Sonuç |
+|---|---:|
+| 60 s hareketli yatay/düşey RMSE | 0.925 m / 0.427 m |
+| 60 s son yatay/düşey hata | 0.306 m / 0.110 m |
+| Moving algılama | 5.005 s, hareket başlangıcından 1 IMU örneği |
+| Son duruşu stationary tanıma | 27.505 s, gerçek duruş 24.000 s |
+| GPS accept/reject | 56 / 2 |
+| Enjekte edilen/reddedilen outlier | 2 / 2 |
+| Reddedilen GPS'in state sıçraması | 0.0 m |
+| Sekiz seed en kötü yatay RMSE | 0.959 m |
+| 30 s / 500 Hz stationary yanlış-moving örneği | 0 |
+| 30 s / 500 Hz son hız | 0.0002 m/s |
+
+Baseline hareketli koşuda 58 delayed GPS update için 14,508 IMU örneği replay
+edildi; 56 GPS kabul, iki sentetik outlier reject edildi. Toplam 749 rate-limitli
+zero update uygulandı. Stationary 500 Hz koşusunda 27 GPS kabul edildi, 560
+zero update uygulandı. Final covariance Cholesky tabanlı PSD kontrolünü geçti
+ve maksimum symmetry error test çözünürlüğünde sıfırdı.
+
+### Sentetik testin ortaya çıkardığı iki hata
+
+İlk sürümde jerk doğrudan iki ham accel örneğinin farkının `dt`'ye bölünmesiyle
+üretiliyordu. 500 Hz'de küçük sample noise bile 4 m/s3 eşiğini sık geçebildiği
+için stationary kartın motion state'i chatter edebilirdi. Jerk için 100 ms
+low-pass state eklenince 10 s/500 Hz detector testi ve 30 s/500 Hz uçtan uca
+test sıfır false-moving ile geçti; 1 m/s2 step yine ilk örnekte moving yaptı.
+
+İkinci ara sürüm, iki position-only GPS örneğinin `STILL` kararına ESKF hızını
+ezme yetkisi veriyordu. 1.2 m GPS sigma ile 2 m/s sabit hareketin tek-epoch
+displacement'i her zaman istatistiksel olarak güçlü değildir. Bazı seed'lerde
+yanlış stationary kararı ZUPT'yi gerçek hareket sırasında açtı; sekiz-seed
+testinde yatay RMSE 480 m ve son hata 1 km üzerini görerek regression'ı
+yakaladı. Kalıcı düzeltmede GPS-still yüksek ESKF hızını artık geçersiz
+kılamaz; GPS-moving yalnız ek veto olur. Düzeltme sonrası sekiz seed'in en kötü
+yatay RMSE'si 0.959 m, false-stationary sayısı sıfır oldu.
+
+Bu sonuç GPS Doppler velocity'nin neden değerli olduğunu da gösterir. Position
+farkından stationary/moving ayırımı düşük hızda ve kısa periodda zayıftır.
+İleride GPS modülü güvenilir Doppler velocity ve covariance üretirse ayrı bir
+velocity measurement update ve daha hızlı stop confirmation eklenmelidir.
+
+ASan ve UBSan hata vermedi. Sekiz adet 60 s hareketli koşu ile 30 s/500 Hz
+stationary koşunun tamamı masaüstünde yaklaşık 0.70 s user CPU süresinde
+tamamlandı. `gprof`te sürenin yaklaşık %73'ü covariance dahil ESKF
+propagation, %18'i measurement/ZUPT update içindeydi. Bu yüzdeler darboğazı
+gösterir; masaüstü süresi 50 MHz Rocket için doğrudan cycle tahmini değildir.
+Profil koşusunda fixed-lag replay dahil 111,009 `fusion_engine_push_imu`,
+239,075 covariance propagation step ve 30,352 selected measurement/ZUPT
+update çağrısı oluştu. Host maksimum RSS yaklaşık 2.6 MB idi; bu değer glibc,
+sanitizer/profiler ve process overhead içerdiğinden bare-metal BSS ile aynı
+metrik değildir.
+
+Genesys2/RISC-V bare-metal build'i de geçti:
+
+```sh
+make FPGA_BOARD=genesys2 fpga-elf-flash-image APP=gps_system \
+  GPS_DEBUG=1 GPS_DEBUG_LEVEL=3 \
+  TDK_IMU_ODR_HZ=500 GPS_NAV_SOLVE_TIMER_TICKS=50
+```
+
+Son ELF boyutu text/data/bss olarak `103840 / 2944 / 204816` bayt, flash
+payload `106784` bayt ve CRC32 `0x16dbc828` oldu. Kart bu test sırasında bağlı
+olmadığı için SPI interrupt hızı, gerçek Rocket cycle yükü ve UART telemetry
+henüz donanım üzerinde ölçülmedi.
+
+Build yeni fusion kaynaklarında compiler warning üretmedi. Linker'ın RAM
+imajındaki mevcut `LOAD segment with RWX permissions` uyarısı devam ediyor;
+bu ESKF değişikliğine özel değildir fakat ileride linker-script güvenlik/segment
+ayrımı çalışmasında ele alınmalıdır.
+
+Kart tekrar bağlandığında sentetik testin yerine geçmeyen şu donanım
+doğrulamaları yapılmalıdır:
+
+1. ICM-42688-P `WHO_AM_I=0x47`, gerçek accel/gyro eksen yönleri ve kart mounting
+   dönüşümü doğrulanmalı.
+2. 100/200/500 Hz seçeneklerinde INT1 IRQ sayısı, sample sayısı, spurious IRQ,
+   estimated missed sample ve 256-raw-queue overflow sayaçları izlenmeli.
+3. `navigation_fusion_service()` ve tek `eskf_propagate_step()` için Rocket
+   `rdcycle` ortalama/maksimum değerleri ölçülmeli; 50 MHz'deki gerçek CPU payı
+   bu cycle ölçümünden çıkarılmalı.
+4. En az 2 s fiziksel stationary başlangıçta calibration reset sayısı, accel/
+   gyro varyansı ve bulunan bias'lar sıcaklıkla birlikte loglanmalı.
+5. Elle hareket testinde stationary -> moving çıkış süresi, sabit hızda ZUPT'nin
+   kapalı kaldığı ve durduktan sonra candidate -> stationary geçişi doğrulanmalı.
+6. GPS WLS ölçüm timestamp'i ile IMU local tick'lerinin aynı CLINT domaininde
+   monoton kaldığı; replay/stale/pending/history-overflow sayaçlarının beklenen
+   değerlerde olduğu kontrol edilmeli.
+7. Fused LLH/ENU, velocity, motion state, NIS, GPS accept/reject ve covariance
+   sigma değerleri UART'a kaydedilip fiziksel ground truth veya surveyed nokta
+   ile karşılaştırılmalı.
+
+Sentetik test; SPI elektrik bağlantısını, interrupt polarity/clear davranışını,
+sensor scale/mounting hatasını, gerçek multipath dağılımını, sıcaklıkla bias
+driftini veya 50 MHz timing deadline'ını kanıtlamaz. Buradaki metre sonuçları
+gerçek ürün accuracy iddiası değil; bilinen truth ve covariance ile algoritma
+tutarlılığı/regression sonucudur.
+
 ## Standalone GPS WLS bütünlük çalışması (2026-07-12)
 
 Bu bölüm, standalone GPS konumundaki 20--30 m ve daha büyük sıçramaları

soc/software/fusion/tests/Makefile

@@ -4,16 +4,46 @@ 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
 
-.PHONY: all test benchmark clean
+.PHONY: all test sanitize profile benchmark clean
 
-all: standalone_gps_integrity_test
+all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test
 
 standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
 	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
 
-test: standalone_gps_integrity_test
+eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES)
+	$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
+
+test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test
 	./standalone_gps_integrity_test
+	./eskf_motion_test
+	./fusion_synthetic_test
+
+sanitize:
+	$(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) \
+		-o eskf_motion_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./eskf_motion_test_san
+	$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I../../gps_system/header fusion_synthetic_test.c $(ESKF_SOURCES) \
+		-o fusion_synthetic_test_san $(LDLIBS)
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./fusion_synthetic_test_san
+
+profile:
+	$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \
+		-I../../gps_system/header fusion_synthetic_test.c $(ESKF_SOURCES) \
+		-o fusion_synthetic_test_gprof $(LDLIBS)
+	./fusion_synthetic_test_gprof
+	gprof ./fusion_synthetic_test_gprof gmon.out
 
 benchmark: standalone_gps_integrity_test
 	./standalone_gps_integrity_test --bench-clean 100000
@@ -21,4 +51,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 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_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace gmon.out

soc/software/fusion/tests/eskf_motion_test.c

@@ -0,0 +1,724 @@
+#include <math.h>
+#include <stdbool.h>
+#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"
+
+#define TEST_PI 3.14159265358979323846264338327950288
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+static bool nearly_equal(double first, double second, double tolerance)
+{
+	return fabs(first - second) <= tolerance;
+}
+
+static double test_norm3(const double value[3])
+{
+	return sqrt(value[0] * value[0] + value[1] * value[1] +
+	            value[2] * value[2]);
+}
+
+static uint32_t noise_state = 0x9e3779b9u;
+
+static double uniform_noise(void)
+{
+	noise_state ^= noise_state << 13;
+	noise_state ^= noise_state >> 17;
+	noise_state ^= noise_state << 5;
+	return ((double)noise_state + 0.5) / 4294967296.0;
+}
+
+static double gaussian_noise(void)
+{
+	double first = fmax(uniform_noise(), 1.0e-12);
+	double second = uniform_noise();
+	return sqrt(-2.0 * log(first)) * cos(2.0 * TEST_PI * second);
+}
+
+static void identity_filter(eskf_t* filter, eskf_config_t* config)
+{
+	eskf_default_config(config);
+	double position[3] = {0.0, 0.0, 0.0};
+	double quaternion[4] = {1.0, 0.0, 0.0, 0.0};
+	double accel_bias[3] = {0.0, 0.0, 0.0};
+	double gyro_bias[3] = {0.0, 0.0, 0.0};
+	CHECK(eskf_initialize(filter, config, position, quaternion, accel_bias, gyro_bias),
+	      "ESKF identity initialization");
+}
+
+static void test_stationary_propagation(void)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	identity_filter(&filter, &config);
+	double accel[3] = {0.0, 0.0, config.gravity_mps2};
+	double gyro[3] = {0.0, 0.0, 0.0};
+	for(unsigned int i = 0 ; i < 500u ; i++)
+	{
+		CHECK(eskf_propagate(&filter, &config, accel, gyro, 0.01),
+		      "stationary propagation succeeds");
+	}
+	CHECK(fabs(eskf_speed_mps(&filter)) < 1.0e-9,
+	      "stationary propagation keeps zero velocity");
+	CHECK(fabs(filter.position_m[2]) < 1.0e-9,
+	      "stationary propagation keeps zero height");
+	CHECK(eskf_state_is_finite(&filter), "stationary state remains finite");
+}
+
+static void test_gravity_alignment(void)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	eskf_default_config(&config);
+	double measured_gravity[3] = {0.35, -0.22, 9.79793};
+	double nav_up[3] = {0.0, 0.0, 1.0};
+	double quaternion[4];
+	CHECK(eskf_quaternion_align_vectors(measured_gravity, nav_up, quaternion),
+	      "gravity alignment quaternion");
+	double position[3] = {0.0, 0.0, 0.0};
+	double accel_bias[3] = {0.0, 0.0, 0.0};
+	double gyro_bias[3] = {0.0, 0.0, 0.0};
+	double gravity_norm = sqrt(measured_gravity[0] * measured_gravity[0] +
+	                           measured_gravity[1] * measured_gravity[1] +
+	                           measured_gravity[2] * measured_gravity[2]);
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		measured_gravity[i] *= config.gravity_mps2 / gravity_norm;
+	}
+	CHECK(eskf_initialize(&filter, &config, position, quaternion,
+	                      accel_bias, gyro_bias),
+	      "tilted ESKF initialization");
+	double gyro[3] = {0.0, 0.0, 0.0};
+	CHECK(eskf_propagate(&filter, &config, measured_gravity, gyro, 0.01),
+	      "tilted stationary propagation");
+	CHECK(sqrt(filter.last_linear_accel_nav_mps2[0] *
+	           filter.last_linear_accel_nav_mps2[0] +
+	           filter.last_linear_accel_nav_mps2[1] *
+	           filter.last_linear_accel_nav_mps2[1] +
+	           filter.last_linear_accel_nav_mps2[2] *
+	           filter.last_linear_accel_nav_mps2[2]) < 1.0e-8,
+	      "gravity alignment removes gravity from linear acceleration");
+}
+
+static void test_position_nis_gate(void)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	identity_filter(&filter, &config);
+	double covariance[9] = {
+		4.0, 0.2, 0.0,
+		0.2, 4.0, 0.0,
+		0.0, 0.0, 9.0
+	};
+	double normal_position[3] = {1.0, -0.5, 0.2};
+	double nis = NAN;
+	CHECK(eskf_position_update(&filter, &config, normal_position,
+	                           covariance, &nis) == ESKF_UPDATE_ACCEPTED,
+	      "normal GPS position accepted");
+	CHECK(isfinite(nis) && (nis < config.position_nis_gate),
+	      "accepted GPS has finite in-gate NIS");
+	double before[3];
+	memcpy(before, filter.position_m, sizeof(before));
+	double outlier[3] = {1000.0, -800.0, 500.0};
+	CHECK(eskf_position_update(&filter, &config, outlier,
+	                           covariance, &nis) == ESKF_UPDATE_REJECTED_NIS,
+	      "large GPS outlier rejected by NIS");
+	CHECK(nearly_equal(before[0], filter.position_m[0], 1.0e-12) &&
+	      nearly_equal(before[1], filter.position_m[1], 1.0e-12) &&
+	      nearly_equal(before[2], filter.position_m[2], 1.0e-12),
+	      "NIS rejection leaves nominal position unchanged");
+	CHECK(filter.position_update_accepted_count == 1u &&
+	      filter.position_update_rejected_count == 1u,
+	      "GPS accept/reject counters");
+}
+
+static double profile_acceleration(double time_s)
+{
+	if((time_s >= 2.0) && (time_s < 4.0))
+	{
+		return 2.0;
+	}
+	if((time_s >= 6.0) && (time_s < 8.0))
+	{
+		return -2.0;
+	}
+	return 0.0;
+}
+
+static void test_motion_transitions(void)
+{
+	eskf_t filter;
+	eskf_config_t eskf_config;
+	identity_filter(&filter, &eskf_config);
+	fusion_motion_config_t motion_config;
+	fusion_motion_detector_t detector;
+	fusion_motion_default_config(&motion_config);
+	fusion_motion_init(&detector, true);
+
+	const double dt_s = 0.01;
+	double first_moving_time = NAN;
+	double return_stationary_time = NAN;
+	bool stationary_during_constant_velocity = false;
+	bool deceleration_seen = false;
+	for(unsigned int sample = 1u ; sample <= 1100u ; sample++)
+	{
+		double time_s = (double)sample * dt_s;
+		double acceleration = profile_acceleration(time_s - 0.5 * dt_s);
+		double accel[3] = {acceleration, 0.0, eskf_config.gravity_mps2};
+		double gyro[3] = {0.0, 0.0, 0.0};
+		CHECK(eskf_propagate(&filter, &eskf_config, accel, gyro, dt_s),
+		      "motion profile propagation");
+		fusion_motion_state_t state = fusion_motion_update(
+			&detector, &motion_config,
+			filter.last_linear_accel_nav_mps2,
+			filter.last_unbiased_gyro_rps,
+			eskf_speed_mps(&filter),
+			FUSION_GNSS_MOTION_UNKNOWN,
+			dt_s);
+		if(isnan(first_moving_time) && (state == FUSION_MOTION_MOVING))
+		{
+			first_moving_time = time_s;
+		}
+		if((time_s >= 4.0) && (time_s < 6.0) &&
+		   (state == FUSION_MOTION_STATIONARY))
+		{
+			stationary_during_constant_velocity = true;
+		}
+		if((time_s >= 6.0) && (time_s < 8.0) &&
+		   (detector.last_linear_accel_norm_mps2 > 1.5))
+		{
+			deceleration_seen = true;
+		}
+		if((time_s > 8.0) && isnan(return_stationary_time) &&
+		   (state == FUSION_MOTION_STATIONARY))
+		{
+			return_stationary_time = time_s;
+		}
+		if(fusion_motion_allows_zero_update(&detector) &&
+		   (fmod(time_s, 0.05) < 0.5 * dt_s))
+		{
+			CHECK(eskf_zero_velocity_update(&filter, 0.03),
+			      "stationary ZUPT succeeds");
+		}
+	}
+
+	CHECK(isfinite(first_moving_time) && (first_moving_time <= 2.02),
+	      "sudden acceleration exits stationary within one IMU interval");
+	CHECK(!stationary_during_constant_velocity,
+	      "constant velocity is not mistaken for stationary");
+	CHECK(deceleration_seen, "sudden deceleration appears as dynamic evidence");
+	CHECK(isfinite(return_stationary_time) && (return_stationary_time >= 9.0) &&
+	      (return_stationary_time <= 9.25),
+	      "stationary re-entry waits for jerk settling and quiet dwell");
+	CHECK(fabs(filter.velocity_mps[0]) < 0.03,
+	      "accelerate/decelerate profile returns close to zero speed");
+}
+
+static double run_rate_profile(unsigned int imu_hz, double gps_period_s)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	identity_filter(&filter, &config);
+	double dt_s = 1.0 / (double)imu_hz;
+	double next_gps_s = gps_period_s;
+	double truth_position = 0.0;
+	double truth_velocity = 0.0;
+	double gps_covariance[9] = {
+		0.25, 0.0, 0.0,
+		0.0, 0.25, 0.0,
+		0.0, 0.0, 1.0
+	};
+	for(unsigned int sample = 0 ; sample < 6u * imu_hz ; sample++)
+	{
+		double time_s = (double)sample * dt_s;
+		double acceleration = 0.0;
+		if((time_s >= 1.0) && (time_s < 2.0))
+		{
+			acceleration = 1.0;
+		}
+		else if((time_s >= 3.0) && (time_s < 4.0))
+		{
+			acceleration = -1.0;
+		}
+		truth_position += truth_velocity * dt_s + 0.5 * acceleration * dt_s * dt_s;
+		truth_velocity += acceleration * dt_s;
+		double accel[3] = {acceleration, 0.0, config.gravity_mps2};
+		double gyro[3] = {0.0, 0.0, 0.0};
+		CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s),
+		      "variable-rate propagation");
+		double end_time_s = time_s + dt_s;
+		if((end_time_s + 1.0e-9) >= next_gps_s)
+		{
+			double gps_position[3] = {truth_position, 0.0, 0.0};
+			CHECK(eskf_position_update(&filter, &config, gps_position,
+			                           gps_covariance, NULL) == ESKF_UPDATE_ACCEPTED,
+			      "variable-rate asynchronous GPS update");
+			next_gps_s += gps_period_s;
+		}
+	}
+	CHECK(fabs(truth_position - 2.0) < 1.0e-9,
+	      "synthetic rate profile truth distance");
+	return filter.position_m[0];
+}
+
+static void test_variable_rates(void)
+{
+	double position_100hz = run_rate_profile(100u, 1.0);
+	double position_200hz = run_rate_profile(200u, 0.5);
+	double position_500hz = run_rate_profile(500u, 1.0);
+	CHECK(fabs(position_100hz - 2.0) < 0.02,
+	      "100 Hz IMU / 1 Hz GPS result");
+	CHECK(fabs(position_200hz - 2.0) < 0.02,
+	      "200 Hz IMU / 2 Hz GPS result");
+	CHECK(fabs(position_500hz - 2.0) < 0.02,
+	      "500 Hz IMU / 1 Hz GPS result");
+	CHECK(fabs(position_100hz - position_200hz) < 0.02 &&
+	      fabs(position_200hz - position_500hz) < 0.02,
+	      "filter result is insensitive to configured asynchronous rates");
+}
+
+static void test_irregular_imu_timestamps(void)
+{
+	eskf_t filter;
+	eskf_config_t config;
+	identity_filter(&filter, &config);
+	const double intervals_s[] = {0.003, 0.011, 0.004, 0.017, 0.006, 0.009};
+	double elapsed_s = 0.0;
+	double truth_position[3] = {0.0, 0.0, 0.0};
+	double truth_velocity[3] = {0.0, 0.0, 0.0};
+	const double nav_acceleration[3] = {0.20, -0.10, 0.0};
+	unsigned int interval_index = 0u;
+	while(elapsed_s < 5.0)
+	{
+		double dt_s = intervals_s[interval_index %
+			(sizeof(intervals_s) / sizeof(intervals_s[0]))];
+		if((elapsed_s + dt_s) > 5.0)
+		{
+			dt_s = 5.0 - elapsed_s;
+		}
+		double accel[3] = {
+			nav_acceleration[0], nav_acceleration[1], config.gravity_mps2
+		};
+		double gyro[3] = {0.0, 0.0, 0.0};
+		CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s),
+		      "irregular timestamp propagation succeeds");
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			truth_position[axis] += truth_velocity[axis] * dt_s +
+			                        0.5 * nav_acceleration[axis] * dt_s * dt_s;
+			truth_velocity[axis] += nav_acceleration[axis] * dt_s;
+		}
+		elapsed_s += dt_s;
+		interval_index++;
+	}
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		CHECK(fabs(filter.position_m[axis] - truth_position[axis]) < 1.0e-10,
+		      "irregular local-tick dt preserves position integration");
+		CHECK(fabs(filter.velocity_mps[axis] - truth_velocity[axis]) < 1.0e-10,
+		      "irregular local-tick dt preserves velocity integration");
+	}
+}
+
+static void test_motion_vetoes(void)
+{
+	fusion_motion_config_t config;
+	fusion_motion_detector_t detector;
+	fusion_motion_default_config(&config);
+	fusion_motion_init(&detector, false);
+	double zero[3] = {0.0, 0.0, 0.0};
+	for(unsigned int i = 0 ; i < 200u ; i++)
+	{
+		(void)fusion_motion_update(&detector, &config, zero, zero, 3.0,
+		                           FUSION_GNSS_MOTION_UNKNOWN, 0.01);
+	}
+	CHECK(detector.state == FUSION_MOTION_MOVING,
+	      "nonzero ESKF speed vetoes stationary at constant velocity");
+	for(unsigned int i = 0 ; i < 200u ; i++)
+	{
+		(void)fusion_motion_update(&detector, &config, zero, zero, 3.0,
+		                           FUSION_GNSS_MOTION_STILL, 0.01);
+	}
+	CHECK(detector.state == FUSION_MOTION_MOVING,
+	      "position-only GNSS still evidence cannot override high ESKF speed");
+	for(unsigned int i = 0 ; i < 200u ; i++)
+	{
+		(void)fusion_motion_update(&detector, &config, zero, zero, 0.0,
+		                           FUSION_GNSS_MOTION_MOVING, 0.01);
+	}
+	CHECK(detector.state == FUSION_MOTION_MOVING,
+	      "GNSS motion evidence vetoes stationary");
+	for(unsigned int i = 0 ; i < 120u ; i++)
+	{
+		(void)fusion_motion_update(&detector, &config, zero, zero, 0.0,
+		                           FUSION_GNSS_MOTION_STILL, 0.01);
+	}
+	CHECK(detector.state == FUSION_MOTION_STATIONARY,
+	      "quiet low-speed state enters stationary after dwell");
+
+	fusion_motion_init(&detector, true);
+	double weak_accel[3] = {0.25, 0.0, 0.0};
+	for(unsigned int i = 0 ; i < config.weak_motion_samples - 1u ; i++)
+	{
+		(void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0,
+		                           FUSION_GNSS_MOTION_UNKNOWN, 0.01);
+	}
+	CHECK(detector.state == FUSION_MOTION_STATIONARY,
+	      "short weak disturbance does not chatter state");
+	(void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0,
+	                           FUSION_GNSS_MOTION_UNKNOWN, 0.01);
+	CHECK(detector.state == FUSION_MOTION_MOVING,
+	      "persistent weak motion exits stationary");
+}
+
+static void test_stationary_noise_and_impulse_detection(void)
+{
+	fusion_motion_config_t config;
+	fusion_motion_detector_t detector;
+	fusion_motion_default_config(&config);
+	fusion_motion_init(&detector, true);
+	noise_state = 0x12345678u;
+	const double dt_s = 1.0 / 500.0;
+	unsigned int nonstationary_samples = 0u;
+	for(unsigned int sample = 0u ; sample < 5000u ; sample++)
+	{
+		double accel[3];
+		double gyro[3];
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			accel[axis] = 0.025 * gaussian_noise();
+			gyro[axis] = 0.0008 * gaussian_noise();
+		}
+		fusion_motion_state_t state = fusion_motion_update(
+			&detector, &config, accel, gyro, 0.0,
+			FUSION_GNSS_MOTION_STILL, dt_s);
+		if(state != FUSION_MOTION_STATIONARY)
+		{
+			nonstationary_samples++;
+		}
+	}
+	CHECK(nonstationary_samples == 0u,
+	      "500 Hz stationary IMU noise does not cause false motion");
+
+	double sudden_accel[3] = {1.0, 0.0, 0.0};
+	double zero_gyro[3] = {0.0, 0.0, 0.0};
+	CHECK(fusion_motion_update(&detector, &config, sudden_accel, zero_gyro,
+	                           0.0, FUSION_GNSS_MOTION_STILL, dt_s) ==
+	      FUSION_MOTION_MOVING,
+	      "filtered jerk still detects a sudden acceleration in one 500 Hz sample");
+}
+
+static void initialize_engine_for_replay(fusion_engine_t* engine)
+{
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, 1000u);
+	config.calibration_duration_s = 0.10;
+	config.calibration_min_samples = 10u;
+	CHECK(fusion_engine_init(engine, &config), "fusion engine initialization");
+
+	fusion_position_measurement_t origin = {0};
+	origin.timestamp_ticks = 0u;
+	origin.source_valid = true;
+	origin.source_quality = 1.0;
+	origin.covariance_enu_m2[0] = 0.25;
+	origin.covariance_enu_m2[4] = 0.25;
+	origin.covariance_enu_m2[8] = 1.0;
+	CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
+	      "initial position retained during calibration");
+
+	for(uint64_t tick = 0u ; tick <= 100u ; tick += 10u)
+	{
+		fusion_imu_measurement_t imu = {0};
+		imu.timestamp_ticks = tick;
+		imu.accel_mps2[2] = config.eskf.gravity_mps2;
+		imu.temperature_c = 25.0;
+		imu.sequence = (uint32_t)(tick / 10u);
+		CHECK(fusion_engine_push_imu(engine, &imu),
+		      "stationary calibration IMU accepted");
+	}
+	CHECK(engine->state == FUSION_ENGINE_RUNNING,
+	      "fusion engine leaves calibration after quiet duration");
+}
+
+static double replay_profile_accel(uint64_t tick)
+{
+	double time_s = (double)tick / 1000.0;
+	if((time_s > 1.0) && (time_s <= 2.0))
+	{
+		return 1.0;
+	}
+	if((time_s > 2.0) && (time_s <= 3.0))
+	{
+		return -1.0;
+	}
+	return 0.0;
+}
+
+static void push_replay_imu(fusion_engine_t* engine, uint64_t tick)
+{
+	fusion_imu_measurement_t imu = {0};
+	imu.timestamp_ticks = tick;
+	imu.accel_mps2[0] = replay_profile_accel(tick);
+	imu.accel_mps2[2] = engine->config.eskf.gravity_mps2;
+	imu.temperature_c = 25.0;
+	imu.sequence = (uint32_t)(tick / 10u);
+	CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted");
+}
+
+static fusion_position_measurement_t replay_position(uint64_t tick, double east_m)
+{
+	fusion_position_measurement_t position = {0};
+	position.timestamp_ticks = tick;
+	position.position_enu_m[0] = east_m;
+	position.covariance_enu_m2[0] = 0.25;
+	position.covariance_enu_m2[4] = 0.25;
+	position.covariance_enu_m2[8] = 1.0;
+	position.source_quality = 1.0;
+	position.source_valid = true;
+	return position;
+}
+
+static void check_runtime_close(const fusion_runtime_t* first,
+	                            const fusion_runtime_t* second,
+	                            double tolerance)
+{
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		CHECK(fabs(first->filter.position_m[i] - second->filter.position_m[i]) < tolerance,
+		      "delayed replay position matches on-time update");
+		CHECK(fabs(first->filter.velocity_mps[i] - second->filter.velocity_mps[i]) < tolerance,
+		      "delayed replay velocity matches on-time update");
+		CHECK(fabs(first->filter.accel_bias_mps2[i] - second->filter.accel_bias_mps2[i]) < tolerance,
+		      "delayed replay accel bias matches on-time update");
+		CHECK(fabs(first->filter.gyro_bias_rps[i] - second->filter.gyro_bias_rps[i]) < tolerance,
+		      "delayed replay gyro bias matches on-time update");
+	}
+	for(unsigned int i = 0 ; i < 4u ; i++)
+	{
+		CHECK(fabs(first->filter.body_to_nav_quaternion[i] -
+		           second->filter.body_to_nav_quaternion[i]) < tolerance,
+		      "delayed replay attitude matches on-time update");
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			CHECK(fabs(first->filter.covariance[row][col] -
+			           second->filter.covariance[row][col]) < tolerance,
+			      "delayed replay covariance matches on-time update");
+		}
+	}
+	CHECK(first->motion.state == second->motion.state,
+	      "delayed replay motion state matches on-time update");
+	CHECK(first->filter_timestamp_ticks == second->filter_timestamp_ticks,
+	      "delayed replay timestamp matches on-time update");
+}
+
+static void test_delayed_position_replay(void)
+{
+	static fusion_engine_t on_time;
+	static fusion_engine_t delayed;
+	initialize_engine_for_replay(&on_time);
+	initialize_engine_for_replay(&delayed);
+	fusion_position_measurement_t gps_at_1500 = replay_position(1500u, 0.125);
+	for(uint64_t tick = 110u ; tick <= 3200u ; tick += 10u)
+	{
+		push_replay_imu(&on_time, tick);
+		push_replay_imu(&delayed, tick);
+		if(tick == 1500u)
+		{
+			eskf_update_result_t result = ESKF_UPDATE_INVALID;
+			CHECK(fusion_engine_push_position(&on_time, &gps_at_1500,
+			                                  &result, NULL),
+			      "on-time position update processed");
+			CHECK(result == ESKF_UPDATE_ACCEPTED,
+			      "on-time position update accepted");
+		}
+		if(tick == 1800u)
+		{
+			eskf_update_result_t result = ESKF_UPDATE_INVALID;
+			CHECK(fusion_engine_push_position(&delayed, &gps_at_1500,
+			                                  &result, NULL),
+			      "300 ms delayed position update replayed");
+			CHECK(result == ESKF_UPDATE_ACCEPTED,
+			      "delayed position update accepted at measurement epoch");
+			check_runtime_close(&on_time.current, &delayed.current, 1.0e-11);
+		}
+	}
+	check_runtime_close(&on_time.current, &delayed.current, 1.0e-11);
+	CHECK(delayed.diagnostics.replay_count == 1u,
+	      "one delayed position caused one fixed-lag replay");
+	CHECK(delayed.diagnostics.replayed_imu_count > 0u,
+	      "delayed position replayed stored IMU samples");
+
+	fusion_position_measurement_t future = replay_position(3500u, 1.0);
+	CHECK(fusion_engine_push_position(&delayed, &future, NULL, NULL),
+	      "future-dated position queued without changing filter time");
+	CHECK(delayed.pending_position_count == 1u,
+	      "future position is pending until IMU reaches its timestamp");
+	for(uint64_t tick = 3210u ; tick <= 3500u ; tick += 10u)
+	{
+		push_replay_imu(&delayed, tick);
+	}
+	CHECK(delayed.pending_position_count == 0u,
+	      "pending position consumed at matching IMU time");
+	CHECK(delayed.diagnostics.position_accepted == 2u,
+	      "on-time and future position updates accepted");
+
+	fusion_position_measurement_t stale = replay_position(1400u, 0.1);
+	CHECK(!fusion_engine_push_position(&delayed, &stale, NULL, NULL),
+	      "position older than last checkpoint rejected");
+	CHECK(delayed.diagnostics.position_stale_rejected == 1u,
+	      "stale position rejection is diagnosed");
+}
+
+static fusion_gps_measurement_t adapter_gps(double latitude_deg,
+	                                        double longitude_deg,
+	                                        double height_m)
+{
+	fusion_gps_measurement_t gps = {0};
+	gps.latitude_deg = latitude_deg;
+	gps.longitude_deg = longitude_deg;
+	gps.height_m = height_m;
+	gps.covariance_enu_m2[0][0] = 0.04;
+	gps.covariance_enu_m2[1][1] = 0.09;
+	gps.covariance_enu_m2[2][2] = 0.25;
+	gps.chi_square_ratio = 0.5;
+	gps.solution_valid = true;
+	return gps;
+}
+
+static void test_gps_adapter(void)
+{
+	fusion_gps_adapter_config_t config;
+	fusion_gps_adapter_t adapter;
+	fusion_gps_adapter_default_config(&config);
+	CHECK(fusion_gps_adapter_init(&adapter, &config), "GPS adapter initialization");
+	fusion_gps_measurement_t origin = adapter_gps(41.015137, 28.979530, 100.0);
+	fusion_position_measurement_t local;
+	CHECK(fusion_gps_adapter_convert(&adapter, &origin, &local),
+	      "GPS origin converts to local frame");
+	CHECK(test_norm3(local.position_enu_m) < 1.0e-8,
+	      "first valid GPS establishes zero ENU origin");
+	CHECK(nearly_equal(local.covariance_enu_m2[0], 0.25, 1.0e-12) &&
+	      nearly_equal(local.covariance_enu_m2[4], 0.25, 1.0e-12) &&
+	      nearly_equal(local.covariance_enu_m2[8], 1.0, 1.0e-12),
+	      "GPS covariance floors prevent overconfident ESKF updates");
+
+	fusion_gps_measurement_t moved = adapter_gps(41.015147, 28.979550, 103.0);
+	moved.timestamp_ticks = 1000u;
+	CHECK(fusion_gps_adapter_convert(&adapter, &moved, &local),
+	      "second GPS converts to ENU");
+	CHECK(local.position_enu_m[0] > 1.0 && local.position_enu_m[1] > 0.5,
+	      "latitude/longitude change has expected ENU direction");
+	double latitude;
+	double longitude;
+	double height;
+	CHECK(fusion_gps_adapter_local_to_geodetic(&adapter, local.position_enu_m,
+	                                           &latitude, &longitude, &height),
+	      "fused ENU converts back to geodetic telemetry");
+	CHECK(fabs(latitude - moved.latitude_deg) < 1.0e-8 &&
+	      fabs(longitude - moved.longitude_deg) < 1.0e-8 &&
+	      fabs(height - moved.height_m) < 1.0e-3,
+	      "ENU/geodetic round trip is accurate");
+
+	fusion_gps_measurement_t invalid = moved;
+	invalid.covariance_enu_m2[0][0] = -1.0;
+	CHECK(!fusion_gps_adapter_convert(&adapter, &invalid, &local),
+	      "invalid GPS covariance is rejected before ESKF");
+}
+
+static void test_measurement_bus(void)
+{
+	static fusion_bus_t bus;
+	fusion_bus_init(&bus);
+	for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++)
+	{
+		fusion_imu_measurement_t imu = {0};
+		imu.sequence = sequence;
+		imu.timestamp_ticks = sequence;
+		CHECK(fusion_bus_publish_imu(&bus, &imu), "IMU bus enqueue");
+	}
+	fusion_imu_measurement_t overflow = {0};
+	CHECK(!fusion_bus_publish_imu(&bus, &overflow),
+	      "full IMU bus rejects newest sample without overwriting history");
+	CHECK(bus.imu_overflow_count == 1u, "IMU bus overflow counter");
+	for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++)
+	{
+		fusion_imu_measurement_t imu;
+		CHECK(fusion_bus_take_imu(&bus, &imu), "IMU bus dequeue");
+		CHECK(imu.sequence == sequence, "IMU bus preserves FIFO ordering");
+	}
+	CHECK(!fusion_bus_take_imu(&bus, &overflow), "empty IMU bus reports no sample");
+
+	for(unsigned int sequence = 0u ; sequence < FUSION_GPS_QUEUE_CAPACITY - 1u ; sequence++)
+	{
+		fusion_gps_measurement_t gps = {0};
+		gps.timestamp_ticks = sequence;
+		CHECK(fusion_bus_publish_gps(&bus, &gps), "GPS bus enqueue");
+	}
+	fusion_gps_measurement_t gps_overflow = {0};
+	CHECK(!fusion_bus_publish_gps(&bus, &gps_overflow), "GPS bus overflow detected");
+	CHECK(bus.gps_overflow_count == 1u, "GPS bus overflow counter");
+}
+
+static void test_history_rolls_checkpoint_forward(void)
+{
+	static fusion_engine_t engine;
+	initialize_engine_for_replay(&engine);
+	uint64_t tick = 110u;
+	for(unsigned int i = 0u ; i < FUSION_IMU_HISTORY_CAPACITY + 64u ; i++, tick += 10u)
+	{
+		push_replay_imu(&engine, tick);
+	}
+	CHECK(engine.state == FUSION_ENGINE_RUNNING,
+	      "long GPS outage does not fault fusion engine");
+	CHECK(engine.diagnostics.imu_history_overflow == 64u,
+	      "fixed-lag history reports bounded rollover");
+	CHECK(engine.diagnostics.checkpoint_advance_count == 64u,
+	      "checkpoint advances with each expired IMU sample");
+	CHECK(engine.checkpoint.filter_timestamp_ticks > 100u,
+	      "checkpoint no longer precedes retained history");
+	fusion_position_measurement_t recovery = replay_position(tick - 10u, 0.0);
+	eskf_update_result_t result = ESKF_UPDATE_INVALID;
+	CHECK(fusion_engine_push_position(&engine, &recovery, &result, NULL),
+	      "GPS recovery after history rollover is processed");
+	CHECK(result == ESKF_UPDATE_ACCEPTED,
+	      "GPS recovery after long outage passes consistent NIS");
+}
+
+int main(void)
+{
+	test_stationary_propagation();
+	test_gravity_alignment();
+	test_position_nis_gate();
+	test_motion_transitions();
+	test_variable_rates();
+	test_irregular_imu_timestamps();
+	test_motion_vetoes();
+	test_stationary_noise_and_impulse_detection();
+	test_delayed_position_replay();
+	test_gps_adapter();
+	test_measurement_bus();
+	test_history_rolls_checkpoint_forward();
+	if(failures != 0)
+	{
+		fprintf(stderr, "%d ESKF/motion test(s) failed\n", failures);
+		return 1;
+	}
+	printf("ESKF/motion tests passed (gravity, NIS, replay, noise, 100/200/500 Hz).\n");
+	return 0;
+}

soc/software/fusion/tests/fusion_synthetic_test.c

@@ -0,0 +1,648 @@
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "fusion_engine.h"
+
+#define SYNTH_PI 3.14159265358979323846264338327950288
+#define SYNTH_TIMEBASE_HZ 1000000u
+#define SYNTH_IMU_HZ 200u
+#define SYNTH_DURATION_S 60u
+#define SYNTH_GPS_EVENT_CAPACITY 96u
+
+typedef struct {
+	uint32_t state;
+} synth_rng_t;
+
+typedef struct {
+	double position_m[3];
+	double velocity_mps[3];
+	double yaw_rad;
+} synth_truth_t;
+
+typedef struct {
+	fusion_position_measurement_t measurement;
+	uint64_t delivery_ticks;
+	bool injected_outlier;
+} synth_gps_event_t;
+
+typedef struct {
+	double horizontal_rmse_m;
+	double vertical_rmse_m;
+	double final_horizontal_error_m;
+	double final_vertical_error_m;
+	double final_speed_mps;
+	double max_rejected_state_jump_m;
+	double covariance_symmetry_error;
+	uint64_t accepted_gps;
+	uint64_t rejected_gps;
+	uint64_t replay_count;
+	uint64_t replayed_imu_count;
+	uint64_t zero_updates;
+	unsigned int injected_outliers;
+	unsigned int rejected_injected_outliers;
+	unsigned int false_stationary_samples;
+	double moving_detection_time_s;
+	double stationary_reentry_time_s;
+	bool covariance_positive_semidefinite;
+	bool state_finite;
+} synth_metrics_t;
+
+typedef struct {
+	double final_horizontal_error_m;
+	double final_vertical_error_m;
+	double final_speed_mps;
+	uint64_t accepted_gps;
+	uint64_t zero_updates;
+	unsigned int nonstationary_samples;
+	bool covariance_positive_semidefinite;
+	bool state_finite;
+} stationary_metrics_t;
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double norm2(double first, double second)
+{
+	return sqrt(first * first + second * second);
+}
+
+static double norm3(const double value[3])
+{
+	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
+}
+
+static uint32_t rng_next(synth_rng_t* rng)
+{
+	uint32_t value = rng->state;
+	value ^= value << 13;
+	value ^= value >> 17;
+	value ^= value << 5;
+	rng->state = value;
+	return value;
+}
+
+static double rng_uniform(synth_rng_t* rng)
+{
+	return ((double)rng_next(rng) + 0.5) / 4294967296.0;
+}
+
+static double rng_gaussian(synth_rng_t* rng)
+{
+	double first = fmax(rng_uniform(rng), 1.0e-12);
+	double second = rng_uniform(rng);
+	return sqrt(-2.0 * log(first)) * cos(2.0 * SYNTH_PI * second);
+}
+
+static void trajectory_input(double time_s,
+	                         double acceleration_nav_mps2[3],
+	                         double* yaw_rate_rps)
+{
+	memset(acceleration_nav_mps2, 0, 3u * sizeof(double));
+	*yaw_rate_rps = 0.0;
+	if((time_s >= 5.0) && (time_s < 7.0))
+	{
+		acceleration_nav_mps2[0] = 1.0;
+	}
+	else if((time_s >= 10.0) && (time_s < 12.0))
+	{
+		acceleration_nav_mps2[0] = -1.0;
+	}
+	else if((time_s >= 15.0) && (time_s < 17.0))
+	{
+		*yaw_rate_rps = 0.25 * SYNTH_PI;
+	}
+	else if((time_s >= 17.0) && (time_s < 19.0))
+	{
+		acceleration_nav_mps2[1] = 0.8;
+	}
+	else if((time_s >= 22.0) && (time_s < 24.0))
+	{
+		acceleration_nav_mps2[1] = -0.8;
+	}
+}
+
+static void truth_step(synth_truth_t* truth,
+	                   double dt_s,
+	                   const double acceleration_nav_mps2[3],
+	                   double yaw_rate_rps)
+{
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
+		                            0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s;
+		truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s;
+	}
+	truth->yaw_rad += yaw_rate_rps * dt_s;
+}
+
+static fusion_position_measurement_t make_position(
+	uint64_t timestamp_ticks,
+	const synth_truth_t* truth,
+	synth_rng_t* gps_rng,
+	bool outlier)
+{
+	fusion_position_measurement_t position;
+	memset(&position, 0, sizeof(position));
+	position.timestamp_ticks = timestamp_ticks;
+	position.position_enu_m[0] = truth->position_m[0] + 1.2 * rng_gaussian(gps_rng);
+	position.position_enu_m[1] = truth->position_m[1] + 1.2 * rng_gaussian(gps_rng);
+	position.position_enu_m[2] = truth->position_m[2] + 2.0 * rng_gaussian(gps_rng);
+	if(outlier)
+	{
+		position.position_enu_m[0] += 150.0;
+		position.position_enu_m[1] -= 120.0;
+		position.position_enu_m[2] += 80.0;
+	}
+	position.covariance_enu_m2[0] = square(1.2);
+	position.covariance_enu_m2[4] = square(1.2);
+	position.covariance_enu_m2[8] = square(2.0);
+	position.source_quality = outlier ? 0.7 : 1.0;
+	position.source_valid = true;
+	return position;
+}
+
+static bool covariance_is_psd(const eskf_t* filter,
+	                          double* symmetry_error_out)
+{
+	double lower[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double max_symmetry_error = 0.0;
+	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			double error = fabs(filter->covariance[row][col] -
+			                    filter->covariance[col][row]);
+			if(error > max_symmetry_error)
+			{
+				max_symmetry_error = error;
+			}
+		}
+	}
+	if(symmetry_error_out != NULL)
+	{
+		*symmetry_error_out = max_symmetry_error;
+	}
+
+	/* A tiny diagonal jitter makes the semidefinite check numerically robust. */
+	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0u ; col <= row ; col++)
+		{
+			double value = filter->covariance[row][col];
+			if(row == col)
+			{
+				value += 1.0e-10;
+			}
+			for(unsigned int k = 0u ; k < col ; k++)
+			{
+				value -= lower[row][k] * lower[col][k];
+			}
+			if(row == col)
+			{
+				if(!isfinite(value) || (value <= 0.0))
+				{
+					return false;
+				}
+				lower[row][col] = sqrt(value);
+			}
+			else
+			{
+				lower[row][col] = value / lower[col][col];
+			}
+		}
+	}
+	return true;
+}
+
+static void initialize_synthetic_engine(fusion_engine_t* engine)
+{
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
+	config.calibration_duration_s = 2.0;
+	config.calibration_min_samples = SYNTH_IMU_HZ;
+	CHECK(fusion_engine_init(engine, &config), "synthetic fusion engine initializes");
+
+	fusion_position_measurement_t origin;
+	memset(&origin, 0, sizeof(origin));
+	origin.timestamp_ticks = 0u;
+	origin.covariance_enu_m2[0] = square(1.2);
+	origin.covariance_enu_m2[4] = square(1.2);
+	origin.covariance_enu_m2[8] = square(2.0);
+	origin.source_quality = 1.0;
+	origin.source_valid = true;
+	CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
+	      "initial GPS is retained during calibration");
+}
+
+static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed)
+{
+	static fusion_engine_t engine;
+	initialize_synthetic_engine(&engine);
+	synth_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0};
+	synth_rng_t imu_rng = {0x13579bdfu ^ (0x9e3779b9u * scenario_seed)};
+	synth_rng_t gps_rng = {0x2468ace1u ^ (0x85ebca6bu * scenario_seed)};
+	synth_rng_t timing_rng = {0xa5a5c3c3u ^ (0xc2b2ae35u * scenario_seed)};
+	const double accel_bias[3] = {0.030, -0.025, 0.060};
+	const double gyro_bias[3] = {0.0010, -0.0007, 0.0009};
+	const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / SYNTH_IMU_HZ;
+	const double dt_s = 1.0 / (double)SYNTH_IMU_HZ;
+	const uint64_t final_tick = (uint64_t)SYNTH_DURATION_S * SYNTH_TIMEBASE_HZ;
+	uint64_t next_gps_tick = 3000000u;
+	unsigned int gps_sequence = 0u;
+	synth_gps_event_t events[SYNTH_GPS_EVENT_CAPACITY];
+	unsigned int event_head = 0u;
+	unsigned int event_count = 0u;
+	double horizontal_error_sum = 0.0;
+	double vertical_error_sum = 0.0;
+	uint64_t error_sample_count = 0u;
+	synth_metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+	metrics.moving_detection_time_s = NAN;
+	metrics.stationary_reentry_time_s = NAN;
+#ifdef SYNTH_TRACE
+	fusion_motion_state_t traced_state = FUSION_MOTION_BOOTSTRAP;
+#endif
+
+	for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
+	{
+		double acceleration_nav[3] = {0.0, 0.0, 0.0};
+		double yaw_rate = 0.0;
+		if(tick != 0u)
+		{
+			double midpoint_s = ((double)tick - 0.5 * (double)imu_period_ticks) /
+			                    (double)SYNTH_TIMEBASE_HZ;
+			trajectory_input(midpoint_s, acceleration_nav, &yaw_rate);
+			truth_step(&truth, dt_s, acceleration_nav, yaw_rate);
+		}
+
+		double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s;
+		double cosine = cos(midpoint_yaw);
+		double sine = sin(midpoint_yaw);
+		double specific_force_nav[3] = {
+			acceleration_nav[0], acceleration_nav[1], engine.config.eskf.gravity_mps2
+		};
+		fusion_imu_measurement_t imu;
+		memset(&imu, 0, sizeof(imu));
+		imu.timestamp_ticks = tick;
+		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] = yaw_rate;
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			imu.accel_mps2[axis] += accel_bias[axis] + 0.025 * rng_gaussian(&imu_rng);
+			imu.gyro_rps[axis] += gyro_bias[axis] + 0.0008 * rng_gaussian(&imu_rng);
+		}
+		imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0);
+		imu.sequence = (uint32_t)(tick / imu_period_ticks);
+		CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted");
+		if(engine.state == FUSION_ENGINE_FAULT)
+		{
+			break;
+		}
+
+		if((tick >= next_gps_tick) &&
+		   ((event_head + event_count) < SYNTH_GPS_EVENT_CAPACITY))
+		{
+			bool outlier = (gps_sequence == 8u) || (gps_sequence == 27u);
+			unsigned int index = event_head + event_count;
+			events[index].measurement = make_position(tick, &truth, &gps_rng, outlier);
+			double delay_s = 0.18 + 0.17 * rng_uniform(&timing_rng);
+			events[index].delivery_ticks = tick +
+				(uint64_t)(delay_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
+			events[index].injected_outlier = outlier;
+			event_count++;
+			if(outlier)
+			{
+				metrics.injected_outliers++;
+			}
+			gps_sequence++;
+			double next_period_s = 0.80 + 0.40 * rng_uniform(&timing_rng);
+			next_gps_tick = tick +
+				(uint64_t)(next_period_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
+		}
+
+		while((event_count != 0u) && (events[event_head].delivery_ticks <= tick))
+		{
+			double before[3];
+			memcpy(before, engine.current.filter.position_m, sizeof(before));
+			eskf_update_result_t result = ESKF_UPDATE_INVALID;
+			double nis = NAN;
+			CHECK(fusion_engine_push_position(&engine, &events[event_head].measurement,
+			                                  &result, &nis),
+			      "delayed asynchronous GPS is processed");
+			if(events[event_head].injected_outlier &&
+			   (result == ESKF_UPDATE_REJECTED_NIS))
+			{
+				metrics.rejected_injected_outliers++;
+				double state_jump = norm2(
+					engine.current.filter.position_m[0] - before[0],
+					engine.current.filter.position_m[1] - before[1]);
+				if(state_jump > metrics.max_rejected_state_jump_m)
+				{
+					metrics.max_rejected_state_jump_m = state_jump;
+				}
+			}
+			event_head++;
+			event_count--;
+		}
+
+		if(engine.state == FUSION_ENGINE_RUNNING)
+		{
+			double time_s = (double)tick / (double)SYNTH_TIMEBASE_HZ;
+			fusion_motion_state_t state = engine.current.motion.state;
+#ifdef SYNTH_TRACE
+			if(state != traced_state)
+			{
+				printf("TRACE t=%.3f state=%s speed=%.3f gnss=%u confirm=%u accel=%.3f jerk=%.3f\n",
+				       time_s, fusion_motion_state_name(state),
+				       eskf_speed_mps(&engine.current.filter),
+				       (unsigned int)engine.current.gnss_evidence,
+				       engine.current.gnss_moving_count,
+				       engine.current.motion.last_linear_accel_norm_mps2,
+				       engine.current.motion.last_jerk_norm_mps3);
+				traced_state = state;
+			}
+#endif
+			if(isnan(metrics.moving_detection_time_s) && (time_s >= 5.0) &&
+			   (state == FUSION_MOTION_MOVING))
+			{
+				metrics.moving_detection_time_s = time_s;
+			}
+			if((time_s >= 7.25) && (time_s <= 9.75) &&
+			   (state == FUSION_MOTION_STATIONARY))
+			{
+				metrics.false_stationary_samples++;
+			}
+			if((time_s >= 19.25) && (time_s <= 21.75) &&
+			   (state == FUSION_MOTION_STATIONARY))
+			{
+				metrics.false_stationary_samples++;
+			}
+			if(isnan(metrics.stationary_reentry_time_s) && (time_s >= 24.0) &&
+			   (state == FUSION_MOTION_STATIONARY))
+			{
+				metrics.stationary_reentry_time_s = time_s;
+			}
+			if(time_s >= 3.0)
+			{
+				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 up_error = engine.current.filter.position_m[2] - truth.position_m[2];
+				horizontal_error_sum += square(east_error) + square(north_error);
+				vertical_error_sum += square(up_error);
+				error_sample_count++;
+			}
+		}
+	}
+
+	metrics.accepted_gps = engine.diagnostics.position_accepted;
+	metrics.rejected_gps = engine.diagnostics.position_nis_rejected;
+	metrics.replay_count = engine.diagnostics.replay_count;
+	metrics.replayed_imu_count = engine.diagnostics.replayed_imu_count;
+	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
+	metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
+	metrics.covariance_positive_semidefinite = covariance_is_psd(
+		&engine.current.filter, &metrics.covariance_symmetry_error);
+	if(error_sample_count != 0u)
+	{
+		metrics.horizontal_rmse_m = sqrt(horizontal_error_sum / (double)error_sample_count);
+		metrics.vertical_rmse_m = sqrt(vertical_error_sum / (double)error_sample_count);
+	}
+	metrics.final_horizontal_error_m = norm2(
+		engine.current.filter.position_m[0] - truth.position_m[0],
+		engine.current.filter.position_m[1] - truth.position_m[1]);
+	metrics.final_vertical_error_m = fabs(
+		engine.current.filter.position_m[2] - truth.position_m[2]);
+	metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps);
+	return metrics;
+}
+
+static void check_dynamic_metrics(const synth_metrics_t* metrics)
+{
+	CHECK(metrics->state_finite, "60 s synthetic state remains finite");
+	CHECK(metrics->covariance_positive_semidefinite,
+	      "60 s covariance remains positive semidefinite");
+	CHECK(metrics->covariance_symmetry_error < 1.0e-11,
+	      "60 s covariance remains symmetric");
+	CHECK(isfinite(metrics->moving_detection_time_s) &&
+	      (metrics->moving_detection_time_s <= 5.010),
+	      "sudden acceleration is detected within two 200 Hz samples");
+	CHECK(metrics->false_stationary_samples == 0u,
+	      "constant-velocity legs never enable stationary updates");
+	CHECK(isfinite(metrics->stationary_reentry_time_s) &&
+	      (metrics->stationary_reentry_time_s >= 25.0) &&
+	      (metrics->stationary_reentry_time_s <= 28.0),
+	      "final stop enters stationary after hysteresis dwell");
+	CHECK(metrics->injected_outliers == 2u,
+	      "two synthetic multipath-like GPS outliers were injected");
+	CHECK(metrics->rejected_injected_outliers == metrics->injected_outliers,
+	      "NIS rejects every injected GPS outlier");
+	CHECK(metrics->max_rejected_state_jump_m < 1.0e-8,
+	      "rejected delayed GPS does not perturb the current state");
+	CHECK(metrics->accepted_gps >= 45u,
+	      "most asynchronous GPS measurements are accepted");
+	CHECK(metrics->rejected_gps >= metrics->injected_outliers,
+	      "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->horizontal_rmse_m < 2.0,
+	      "bias/noise/delay trajectory horizontal RMSE is bounded");
+	CHECK(metrics->vertical_rmse_m < 2.5,
+	      "bias/noise/delay trajectory vertical RMSE is bounded");
+	CHECK(metrics->final_horizontal_error_m < 1.5,
+	      "final stationary horizontal error is bounded");
+	CHECK(metrics->final_vertical_error_m < 2.0,
+	      "final stationary vertical error is bounded");
+	CHECK(metrics->final_speed_mps < 0.05,
+	      "final stationary ZUPT speed is near zero");
+}
+
+static stationary_metrics_t run_stationary_500hz_scenario(void)
+{
+	static fusion_engine_t engine;
+	fusion_engine_config_t config;
+	fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
+	config.calibration_duration_s = 2.0;
+	config.calibration_min_samples = 500u;
+	CHECK(fusion_engine_init(&engine, &config), "500 Hz stationary engine initializes");
+
+	fusion_position_measurement_t origin;
+	memset(&origin, 0, sizeof(origin));
+	origin.covariance_enu_m2[0] = square(0.8);
+	origin.covariance_enu_m2[4] = square(0.8);
+	origin.covariance_enu_m2[8] = square(1.5);
+	origin.source_quality = 1.0;
+	origin.source_valid = true;
+	CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL),
+	      "500 Hz stationary origin accepted");
+
+	const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / 500u;
+	const uint64_t final_tick = 30000000u;
+	uint64_t next_gps_tick = 3000000u;
+	uint64_t delivery_tick = 0u;
+	bool gps_pending = false;
+	fusion_position_measurement_t pending_gps;
+	synth_rng_t imu_rng = {0x10203040u};
+	synth_rng_t gps_rng = {0x55667788u};
+	const double accel_bias[3] = {0.020, -0.015, 0.050};
+	const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
+	stationary_metrics_t metrics;
+	memset(&metrics, 0, sizeof(metrics));
+
+	for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
+	{
+		fusion_imu_measurement_t imu;
+		memset(&imu, 0, sizeof(imu));
+		imu.timestamp_ticks = tick;
+		imu.accel_mps2[2] = config.eskf.gravity_mps2;
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			imu.accel_mps2[axis] += accel_bias[axis] + 0.020 * rng_gaussian(&imu_rng);
+			imu.gyro_rps[axis] = gyro_bias[axis] + 0.0006 * rng_gaussian(&imu_rng);
+		}
+		imu.temperature_c = 24.0;
+		imu.sequence = (uint32_t)(tick / imu_period_ticks);
+		CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted");
+
+		if((tick >= next_gps_tick) && !gps_pending)
+		{
+			synth_truth_t stationary_truth = {{0.0, 0.0, 0.0},
+			                                    {0.0, 0.0, 0.0}, 0.0};
+			pending_gps = make_position(tick, &stationary_truth, &gps_rng, false);
+			pending_gps.position_enu_m[0] *= 0.8 / 1.2;
+			pending_gps.position_enu_m[1] *= 0.8 / 1.2;
+			pending_gps.position_enu_m[2] *= 1.5 / 2.0;
+			pending_gps.covariance_enu_m2[0] = square(0.8);
+			pending_gps.covariance_enu_m2[4] = square(0.8);
+			pending_gps.covariance_enu_m2[8] = square(1.5);
+			delivery_tick = tick + 250000u;
+			gps_pending = true;
+			next_gps_tick += 1000000u;
+		}
+		if(gps_pending && (tick >= delivery_tick))
+		{
+			eskf_update_result_t result = ESKF_UPDATE_INVALID;
+			CHECK(fusion_engine_push_position(&engine, &pending_gps, &result, NULL),
+			      "500 Hz delayed stationary GPS processed");
+			CHECK(result == ESKF_UPDATE_ACCEPTED,
+			      "500 Hz stationary GPS passes NIS");
+			gps_pending = false;
+		}
+		if((tick >= 3000000u) && (engine.state == FUSION_ENGINE_RUNNING) &&
+		   (engine.current.motion.state != FUSION_MOTION_STATIONARY))
+		{
+			metrics.nonstationary_samples++;
+		}
+	}
+
+	metrics.accepted_gps = engine.diagnostics.position_accepted;
+	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
+	metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
+	metrics.covariance_positive_semidefinite = covariance_is_psd(
+		&engine.current.filter, NULL);
+	metrics.final_horizontal_error_m = norm2(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 = norm3(engine.current.filter.velocity_mps);
+	return metrics;
+}
+
+static void check_stationary_metrics(const stationary_metrics_t* metrics)
+{
+	CHECK(metrics->state_finite, "30 s / 500 Hz stationary state remains finite");
+	CHECK(metrics->covariance_positive_semidefinite,
+	      "30 s / 500 Hz stationary covariance remains PSD");
+	CHECK(metrics->nonstationary_samples == 0u,
+	      "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->final_horizontal_error_m < 1.0,
+	      "30 s / 500 Hz stationary horizontal error is bounded");
+	CHECK(metrics->final_vertical_error_m < 1.0,
+	      "30 s / 500 Hz stationary vertical error is bounded");
+	CHECK(metrics->final_speed_mps < 0.02,
+	      "30 s / 500 Hz stationary speed remains near zero");
+}
+
+int main(void)
+{
+	synth_metrics_t metrics = run_dynamic_scenario(0u);
+	check_dynamic_metrics(&metrics);
+	double worst_horizontal_rmse_m = metrics.horizontal_rmse_m;
+	double worst_final_horizontal_error_m = metrics.final_horizontal_error_m;
+	double latest_stationary_reentry_s = metrics.stationary_reentry_time_s;
+	for(uint32_t scenario = 1u ; scenario < 8u ; scenario++)
+	{
+		synth_metrics_t trial = run_dynamic_scenario(scenario);
+		check_dynamic_metrics(&trial);
+		worst_horizontal_rmse_m = fmax(worst_horizontal_rmse_m,
+		                                trial.horizontal_rmse_m);
+		worst_final_horizontal_error_m = fmax(worst_final_horizontal_error_m,
+		                                       trial.final_horizontal_error_m);
+		latest_stationary_reentry_s = fmax(latest_stationary_reentry_s,
+		                                      trial.stationary_reentry_time_s);
+	}
+	stationary_metrics_t stationary = run_stationary_500hz_scenario();
+	check_stationary_metrics(&stationary);
+	printf("Synthetic 60 s asynchronous ESKF scenario\n");
+	printf("  position RMSE     horizontal=%6.3f m vertical=%6.3f m\n",
+	       metrics.horizontal_rmse_m, metrics.vertical_rmse_m);
+	printf("  final error       horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
+	       metrics.final_horizontal_error_m, metrics.final_vertical_error_m,
+	       metrics.final_speed_mps);
+	printf("  motion timing     moving=%6.3f s stationary-again=%6.3f s false-still=%u\n",
+	       metrics.moving_detection_time_s, metrics.stationary_reentry_time_s,
+	       metrics.false_stationary_samples);
+	printf("  GPS/replay        accepted=%llu rejected=%llu replay=%llu replayed-imu=%llu\n",
+	       (unsigned long long)metrics.accepted_gps,
+	       (unsigned long long)metrics.rejected_gps,
+	       (unsigned long long)metrics.replay_count,
+	       (unsigned long long)metrics.replayed_imu_count);
+	printf("  outlier isolation injected=%u rejected=%u max-state-jump=%.3e m\n",
+	       metrics.injected_outliers, metrics.rejected_injected_outliers,
+	       metrics.max_rejected_state_jump_m);
+	printf("  covariance        PSD=%d symmetry-error=%.3e zero-updates=%llu\n",
+	       metrics.covariance_positive_semidefinite ? 1 : 0,
+	       metrics.covariance_symmetry_error,
+	       (unsigned long long)metrics.zero_updates);
+	printf("  8-seed robustness worst-h-rmse=%6.3f m worst-final-h=%6.3f m latest-stop=%6.3f s\n",
+	       worst_horizontal_rmse_m, worst_final_horizontal_error_m,
+	       latest_stationary_reentry_s);
+	printf("Synthetic 30 s stationary / 500 Hz scenario\n");
+	printf("  final error       horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
+	       stationary.final_horizontal_error_m,
+	       stationary.final_vertical_error_m,
+	       stationary.final_speed_mps);
+	printf("  decisions         false-moving-samples=%u GPS-accepted=%llu zero-updates=%llu\n",
+	       stationary.nonstationary_samples,
+	       (unsigned long long)stationary.accepted_gps,
+	       (unsigned long long)stationary.zero_updates);
+	if(failures != 0)
+	{
+		fprintf(stderr, "%d synthetic fusion check(s) failed\n", failures);
+		return 1;
+	}
+	printf("PASS: production bare-metal ESKF sources passed the Linux/GCC scenario.\n");
+	return 0;
+}

soc/software/gps_system/Makefile

@@ -7,6 +7,14 @@ BSP_UART_CLOCK_HZ ?= 50000000
 
 APP_SRCS := \
 	gps_test.c \
+	src/tdk_icm42688.c \
+	src/eskf.c \
+	src/fusion_motion.c \
+	src/fusion_engine.c \
+	src/fusion_bus.c \
+	src/fusion_gps_adapter.c \
+	src/navigation_fusion.c \
+	src/gps_fusion_publisher.c \
 	src/gps_ip.c \
 	src/gps_interrupt_handlers.c \
 	src/gps_time_anchor.c \
@@ -15,9 +23,16 @@ APP_SRCS := \
 	src/NavEquations.c \
 	src/matrixLib.c
 
+BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
+
+TDK_IMU_ODR_HZ ?= 500
+GPS_NAV_SOLVE_TIMER_TICKS ?= 50
+
 include ../common/app.mk
 
 CFLAGS += -Iheader
+CFLAGS += -DTDK_IMU_ODR_HZ=$(TDK_IMU_ODR_HZ) \
+	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
 LDLIBS += -lm
 
 ifeq ($(GPS_DEBUG),1)

soc/software/gps_system/gps_test.c

@@ -5,21 +5,54 @@
 #include <float.h>
 #include "matrixLib.h"
 #include "gps_macros.h"
+#include "navigation_fusion.h"
+#include "tdk_icm42688.h"
 
 int main()
 {
-		
+	tdk_icm42688_config_t imu_config;
-	gps_startup_initialization(15);
+	tdk_icm42688_default_config(&imu_config);
+	bool imu_initialized = tdk_icm42688_init(&imu_config);
+	bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ);
+
+	gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS);
+	bool imu_running = imu_initialized && fusion_initialized && tdk_icm42688_start();
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+	        "init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n",
+	        imu_initialized ? 1u : 0u, imu_running ? 1u : 0u,
+	        imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ);
 
 	acq_start_loop();
+	uint64_t last_fusion_status_tick = 0u;
 
 	while(1)
 	{
 		asm volatile("wfi"); 
 		gps_background_task();
 		calculate_position();
-		// TODO: implement calculate position nav solve
+		if(fusion_initialized)
-		//calculate_position_nav_solve();
+		{
+			(void)navigation_fusion_service();
+			uint64_t now = gps_get_systick();
+			if((now - last_fusion_status_tick) >= GPS_SYSTICK_HZ)
+			{
+				navigation_fusion_output_t output;
+				if(navigation_fusion_get_output(&output))
+				{
+					GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+					        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
+					        "speed=%.3lf nis=%.3lf gps=%lu/%lu sigma=%.2lf,%.2lf,%.2lf\r\n",
+					        fusion_motion_state_name(output.motion_state),
+					        output.latitude_deg, output.longitude_deg, output.height_m,
+					        output.speed_mps, output.last_position_nis,
+					        (unsigned long)output.gps_accepted,
+					        (unsigned long)output.gps_nis_rejected,
+					        output.position_std_m[0], output.position_std_m[1],
+					        output.position_std_m[2]);
+				}
+				last_fusion_status_tick = now;
+			}
+		}
 	}
 
 	return 0;

soc/software/gps_system/header/eskf.h

@@ -0,0 +1,85 @@
+#ifndef ESKF_H
+#define ESKF_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define ESKF_VECTOR_DIM 3u
+#define ESKF_QUATERNION_DIM 4u
+#define ESKF_ERROR_STATE_DIM 15u
+
+typedef enum {
+	ESKF_UPDATE_INVALID = 0,
+	ESKF_UPDATE_ACCEPTED,
+	ESKF_UPDATE_REJECTED_NIS
+} eskf_update_result_t;
+
+typedef struct {
+	double accel_noise_density[ESKF_VECTOR_DIM];
+	double gyro_noise_density[ESKF_VECTOR_DIM];
+	double accel_bias_random_walk[ESKF_VECTOR_DIM];
+	double gyro_bias_random_walk[ESKF_VECTOR_DIM];
+	double initial_position_std_m[ESKF_VECTOR_DIM];
+	double initial_velocity_std_mps[ESKF_VECTOR_DIM];
+	double initial_attitude_std_rad[ESKF_VECTOR_DIM];
+	double initial_accel_bias_std_mps2[ESKF_VECTOR_DIM];
+	double initial_gyro_bias_std_rps[ESKF_VECTOR_DIM];
+	double gravity_mps2;
+	double max_propagation_step_s;
+	double position_nis_gate;
+	double covariance_diagonal_floor;
+} eskf_config_t;
+
+typedef struct {
+	double position_m[ESKF_VECTOR_DIM];
+	double velocity_mps[ESKF_VECTOR_DIM];
+	double body_to_nav_quaternion[ESKF_QUATERNION_DIM];
+	double accel_bias_mps2[ESKF_VECTOR_DIM];
+	double gyro_bias_rps[ESKF_VECTOR_DIM];
+	double covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM];
+	double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM];
+	double last_unbiased_gyro_rps[ESKF_VECTOR_DIM];
+	double last_position_nis;
+	uint64_t propagation_count;
+	uint64_t position_update_accepted_count;
+	uint64_t position_update_rejected_count;
+	bool initialized;
+} eskf_t;
+
+void eskf_default_config(eskf_config_t* config);
+
+bool eskf_quaternion_align_vectors(const double source[ESKF_VECTOR_DIM],
+	                               const double target[ESKF_VECTOR_DIM],
+	                               double quaternion_out[ESKF_QUATERNION_DIM]);
+
+bool eskf_initialize(eskf_t* filter,
+	                 const eskf_config_t* config,
+	                 const double initial_position_m[ESKF_VECTOR_DIM],
+	                 const double body_to_nav_quaternion[ESKF_QUATERNION_DIM],
+	                 const double accel_bias_mps2[ESKF_VECTOR_DIM],
+	                 const double gyro_bias_rps[ESKF_VECTOR_DIM]);
+
+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[ESKF_VECTOR_DIM],
+	                const double gyro_rps[ESKF_VECTOR_DIM],
+	                double dt_s);
+
+eskf_update_result_t eskf_position_update(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double position_m[ESKF_VECTOR_DIM],
+	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
+	double* nis_out);
+
+bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);
+
+bool eskf_zero_angular_rate_update(
+	eskf_t* filter,
+	const double measured_gyro_rps[ESKF_VECTOR_DIM],
+	const double measurement_std_rps[ESKF_VECTOR_DIM]);
+
+double eskf_speed_mps(const eskf_t* filter);
+bool eskf_state_is_finite(const eskf_t* filter);
+
+#endif

soc/software/gps_system/header/fusion_bus.h

@@ -0,0 +1,42 @@
+#ifndef FUSION_BUS_H
+#define FUSION_BUS_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_measurement.h"
+
+#ifndef FUSION_IMU_QUEUE_CAPACITY
+#define FUSION_IMU_QUEUE_CAPACITY 256u
+#endif
+
+#ifndef FUSION_GPS_QUEUE_CAPACITY
+#define FUSION_GPS_QUEUE_CAPACITY 8u
+#endif
+
+typedef struct {
+	fusion_imu_measurement_t imu_queue[FUSION_IMU_QUEUE_CAPACITY];
+	fusion_gps_measurement_t gps_queue[FUSION_GPS_QUEUE_CAPACITY];
+	volatile unsigned int imu_head;
+	volatile unsigned int imu_tail;
+	volatile unsigned int gps_head;
+	volatile unsigned int gps_tail;
+	volatile uint64_t imu_overflow_count;
+	volatile uint64_t gps_overflow_count;
+} fusion_bus_t;
+
+void fusion_bus_init(fusion_bus_t* bus);
+
+/* Single producer (TDK ISR), single consumer (foreground). */
+bool fusion_bus_publish_imu(fusion_bus_t* bus,
+	                        const fusion_imu_measurement_t* measurement);
+bool fusion_bus_take_imu(fusion_bus_t* bus,
+	                     fusion_imu_measurement_t* measurement_out);
+
+/* Single producer (GPS WLS foreground), single fusion consumer. */
+bool fusion_bus_publish_gps(fusion_bus_t* bus,
+	                        const fusion_gps_measurement_t* measurement);
+bool fusion_bus_take_gps(fusion_bus_t* bus,
+	                     fusion_gps_measurement_t* measurement_out);
+
+#endif

soc/software/gps_system/header/fusion_engine.h

@@ -0,0 +1,123 @@
+#ifndef FUSION_ENGINE_H
+#define FUSION_ENGINE_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "eskf.h"
+#include "fusion_measurement.h"
+#include "fusion_motion.h"
+
+#ifndef FUSION_IMU_HISTORY_CAPACITY
+#define FUSION_IMU_HISTORY_CAPACITY 2048u
+#endif
+
+#ifndef FUSION_POSITION_PENDING_CAPACITY
+#define FUSION_POSITION_PENDING_CAPACITY 8u
+#endif
+
+typedef enum {
+	FUSION_ENGINE_CALIBRATING = 0,
+	FUSION_ENGINE_RUNNING,
+	FUSION_ENGINE_FAULT
+} fusion_engine_state_t;
+
+typedef struct {
+	eskf_config_t eskf;
+	fusion_motion_config_t motion;
+	uint64_t timebase_hz;
+	double calibration_duration_s;
+	unsigned int calibration_min_samples;
+	double calibration_accel_norm_tolerance_mps2;
+	double calibration_gyro_limit_rps;
+	double calibration_accel_std_limit_mps2;
+	double calibration_gyro_std_limit_rps;
+	double zero_update_period_s;
+	double zero_velocity_std_mps;
+	double zero_gyro_std_rps[3];
+	double gnss_motion_speed_threshold_mps;
+	double gnss_displacement_nis_gate;
+	unsigned int gnss_moving_confirmations;
+	unsigned int gnss_still_confirmations;
+} fusion_engine_config_t;
+
+typedef struct {
+	eskf_t filter;
+	fusion_motion_detector_t motion;
+	fusion_imu_measurement_t held_imu;
+	fusion_gnss_motion_evidence_t gnss_evidence;
+	double last_accepted_position_m[3];
+	double last_accepted_position_covariance_m2[9];
+	uint64_t filter_timestamp_ticks;
+	uint64_t last_zero_update_ticks;
+	uint64_t last_accepted_position_ticks;
+	unsigned int gnss_moving_count;
+	unsigned int gnss_still_count;
+	bool have_held_imu;
+	bool have_accepted_position;
+} fusion_runtime_t;
+
+typedef struct {
+	uint64_t imu_received;
+	uint64_t imu_rejected;
+	uint64_t imu_history_overflow;
+	uint64_t checkpoint_advance_count;
+	uint64_t calibration_reset_count;
+	uint64_t position_received;
+	uint64_t position_source_rejected;
+	uint64_t position_nis_rejected;
+	uint64_t position_stale_rejected;
+	uint64_t position_pending_overflow;
+	uint64_t position_accepted;
+	uint64_t replay_count;
+	uint64_t replayed_imu_count;
+	uint64_t zero_velocity_update_count;
+} fusion_engine_diagnostics_t;
+
+typedef struct {
+	uint64_t start_ticks;
+	uint64_t last_ticks;
+	double accel_mean[3];
+	double gyro_mean[3];
+	double accel_m2[3];
+	double gyro_m2[3];
+	unsigned int sample_count;
+	bool have_start;
+} fusion_calibration_accumulator_t;
+
+typedef struct {
+	fusion_engine_config_t config;
+	fusion_engine_state_t state;
+	fusion_runtime_t current;
+	fusion_runtime_t checkpoint;
+	fusion_calibration_accumulator_t calibration;
+	fusion_imu_measurement_t imu_history[FUSION_IMU_HISTORY_CAPACITY];
+	fusion_position_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY];
+	fusion_position_measurement_t initial_position;
+	fusion_engine_diagnostics_t diagnostics;
+	unsigned int imu_history_head;
+	unsigned int imu_history_count;
+	unsigned int pending_position_count;
+	bool checkpoint_valid;
+	bool have_initial_position;
+} fusion_engine_t;
+
+void fusion_engine_default_config(fusion_engine_config_t* config,
+	                              uint64_t timebase_hz);
+bool fusion_engine_init(fusion_engine_t* engine,
+	                    const fusion_engine_config_t* config);
+
+bool fusion_engine_push_imu(fusion_engine_t* engine,
+	                        const fusion_imu_measurement_t* measurement);
+bool fusion_engine_push_position(
+	fusion_engine_t* engine,
+	const fusion_position_measurement_t* measurement,
+	eskf_update_result_t* update_result_out,
+	double* nis_out);
+
+bool fusion_engine_get_runtime(const fusion_engine_t* engine,
+	                           fusion_runtime_t* runtime_out);
+const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
+	const fusion_engine_t* engine);
+
+#endif

soc/software/gps_system/header/fusion_gps_adapter.h

@@ -0,0 +1,37 @@
+#ifndef FUSION_GPS_ADAPTER_H
+#define FUSION_GPS_ADAPTER_H
+
+#include <stdbool.h>
+
+#include "fusion_measurement.h"
+
+typedef struct {
+	double minimum_horizontal_std_m;
+	double minimum_vertical_std_m;
+	double maximum_position_std_m;
+} fusion_gps_adapter_config_t;
+
+typedef struct {
+	double origin_latitude_deg;
+	double origin_longitude_deg;
+	double origin_height_m;
+	double origin_ecef_m[3];
+	double ecef_to_enu[3][3];
+	fusion_gps_adapter_config_t config;
+	bool origin_valid;
+} fusion_gps_adapter_t;
+
+void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config);
+bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
+	                         const fusion_gps_adapter_config_t* config);
+bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
+	                            const fusion_gps_measurement_t* gps,
+	                            fusion_position_measurement_t* position_out);
+bool fusion_gps_adapter_local_to_geodetic(
+	const fusion_gps_adapter_t* adapter,
+	const double position_enu_m[3],
+	double* latitude_deg_out,
+	double* longitude_deg_out,
+	double* height_m_out);
+
+#endif

soc/software/gps_system/header/fusion_measurement.h

@@ -0,0 +1,43 @@
+#ifndef FUSION_MEASUREMENT_H
+#define FUSION_MEASUREMENT_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/*
+ * Hardware- and solver-neutral messages shared at the GPS/ESKF boundary.
+ * timestamp_ticks always belongs to the common local monotonic timebase; GPS
+ * week/TOW is metadata and must not drive filter dt.
+ */
+typedef struct {
+	uint64_t timestamp_ticks;
+	double latitude_deg;
+	double longitude_deg;
+	double height_m;
+	double covariance_enu_m2[3][3];
+	double hacc_m;
+	double vacc_m;
+	double residual_rms_m;
+	double chi_square_ratio;
+	unsigned int used_satellites;
+	bool solution_valid;
+} fusion_gps_measurement_t;
+
+typedef struct {
+	uint64_t timestamp_ticks;
+	double accel_mps2[3];
+	double gyro_rps[3];
+	double temperature_c;
+	uint32_t sequence;
+} fusion_imu_measurement_t;
+
+/* Generic local-navigation measurement consumed by the ESKF coordinator. */
+typedef struct {
+	uint64_t timestamp_ticks;
+	double position_enu_m[3];
+	double covariance_enu_m2[9];
+	double source_quality;
+	bool source_valid;
+} fusion_position_measurement_t;
+
+#endif

soc/software/gps_system/header/fusion_motion.h

@@ -0,0 +1,63 @@
+#ifndef FUSION_MOTION_H
+#define FUSION_MOTION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef enum {
+	FUSION_MOTION_BOOTSTRAP = 0,
+	FUSION_MOTION_STATIONARY,
+	FUSION_MOTION_MOVING,
+	FUSION_MOTION_STATIONARY_CANDIDATE
+} fusion_motion_state_t;
+
+typedef enum {
+	FUSION_GNSS_MOTION_UNKNOWN = 0,
+	FUSION_GNSS_MOTION_STILL,
+	FUSION_GNSS_MOTION_MOVING
+} fusion_gnss_motion_evidence_t;
+
+typedef struct {
+	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;
+	double jerk_filter_time_constant_s;
+	double stationary_speed_limit_mps;
+	double moving_speed_threshold_mps;
+	double stationary_dwell_s;
+	unsigned int weak_motion_samples;
+} fusion_motion_config_t;
+
+typedef struct {
+	fusion_motion_state_t state;
+	double quiet_time_s;
+	double filtered_linear_accel_mps2[3];
+	double previous_filtered_linear_accel_mps2[3];
+	double last_linear_accel_norm_mps2;
+	double last_gyro_norm_rps;
+	double last_jerk_norm_mps3;
+	double last_speed_mps;
+	unsigned int weak_motion_count;
+	uint64_t transition_count;
+	bool have_previous_accel;
+} fusion_motion_detector_t;
+
+void fusion_motion_default_config(fusion_motion_config_t* config);
+void fusion_motion_init(fusion_motion_detector_t* detector,
+	                    bool bootstrap_stationary);
+
+fusion_motion_state_t fusion_motion_update(
+	fusion_motion_detector_t* detector,
+	const fusion_motion_config_t* config,
+	const double linear_accel_nav_mps2[3],
+	const double unbiased_gyro_rps[3],
+	double speed_mps,
+	fusion_gnss_motion_evidence_t gnss_evidence,
+	double dt_s);
+
+bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector);
+const char* fusion_motion_state_name(fusion_motion_state_t state);
+
+#endif

soc/software/gps_system/header/gps_fusion_publisher.h

@@ -0,0 +1,12 @@
+#ifndef GPS_FUSION_PUBLISHER_H
+#define GPS_FUSION_PUBLISHER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "NavEquations.h"
+
+bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
+	                             uint64_t measurement_timestamp_ticks);
+
+#endif

soc/software/gps_system/header/gps_ip_defines.h

@@ -46,7 +46,9 @@
 			#define GPS_EVENT_QUEUE_SIZE        64u
 			#define GPS_SYSTICK_HZ              20u
 			#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
+			#ifndef GPS_NAV_SOLVE_TIMER_TICKS
 			#define GPS_NAV_SOLVE_TIMER_TICKS   50u
+			#endif
 			#define GPS_NAV_SOLVE_PERIOD_MS     (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
 			/* Keep integrity statistics active, but do not let leave-one-out FDE
 			 * replace the full-WLS position until the real measurement-noise model
@@ -62,7 +64,7 @@
 			#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
 			#define GPS_HOW_ANCHOR_MAX_AGE_SEC         12.0
 
-			_Static_assert(GPS_NAV_SOLVE_PERIOD_MS == 1000u, "NAV solve period must be 1 second");
+			_Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero");
 			_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
 			               "GPS_NAV_ENABLE_FDE must be zero or one");
 

soc/software/gps_system/header/gps_macros.h

@@ -30,6 +30,7 @@
 		#define GPS_LOG_CAT_SCHED "SCHED"
 		#define GPS_LOG_CAT_MEM   "MEM"
 		#define GPS_LOG_CAT_HWREG "HWREG"
+		#define GPS_LOG_CAT_FUSION "FUSION"
 	
 		// Set the current verbosity level
 		#ifdef DEBUG

soc/software/gps_system/header/linked_list.h

@@ -17,6 +17,7 @@ typedef struct gps_ephem_ll {
 	volatile uint32_t gps_nav_solve_consumed_sequence;
 	volatile uint32_t gps_last_nav_solve_abs_sample;
 	volatile double gps_last_nav_solve_corrected_abs_sample;
+	volatile uint64_t gps_last_nav_solve_local_ticks;
 	volatile uint64_t gps_last_nav_solve_pps_counter;
 	volatile uint32_t gps_last_pps_capture_sequence;
 	volatile uint64_t gps_last_measurement_tick;

soc/software/gps_system/header/navigation_fusion.h

@@ -0,0 +1,33 @@
+#ifndef NAVIGATION_FUSION_H
+#define NAVIGATION_FUSION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_engine.h"
+#include "fusion_measurement.h"
+#include "fusion_motion.h"
+
+typedef struct {
+	uint64_t timestamp_ticks;
+	double position_enu_m[3];
+	double velocity_enu_mps[3];
+	double latitude_deg;
+	double longitude_deg;
+	double height_m;
+	double position_std_m[3];
+	double speed_mps;
+	double last_position_nis;
+	fusion_motion_state_t motion_state;
+	uint64_t gps_accepted;
+	uint64_t gps_nis_rejected;
+	bool valid;
+} navigation_fusion_output_t;
+
+bool navigation_fusion_init(uint64_t local_timebase_hz);
+bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement);
+unsigned int navigation_fusion_service(void);
+bool navigation_fusion_get_output(navigation_fusion_output_t* output);
+const fusion_engine_t* navigation_fusion_engine(void);
+
+#endif

soc/software/gps_system/header/tdk_icm42688.h

@@ -0,0 +1,75 @@
+#ifndef TDK_ICM42688_H
+#define TDK_ICM42688_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion_measurement.h"
+
+#ifndef TDK_IMU_RAW_QUEUE_CAPACITY
+#define TDK_IMU_RAW_QUEUE_CAPACITY 256u
+#endif
+
+#ifndef TDK_IMU_ODR_HZ
+#define TDK_IMU_ODR_HZ 500u
+#endif
+
+typedef enum {
+	TDK_ACCEL_FS_2G = 2,
+	TDK_ACCEL_FS_4G = 4,
+	TDK_ACCEL_FS_8G = 8,
+	TDK_ACCEL_FS_16G = 16
+} tdk_accel_full_scale_t;
+
+typedef enum {
+	TDK_GYRO_FS_15D625 = 15625,
+	TDK_GYRO_FS_31D25 = 31250,
+	TDK_GYRO_FS_62D5 = 62500,
+	TDK_GYRO_FS_125 = 125000,
+	TDK_GYRO_FS_250 = 250000,
+	TDK_GYRO_FS_500 = 500000,
+	TDK_GYRO_FS_1000 = 1000000,
+	TDK_GYRO_FS_2000 = 2000000
+} tdk_gyro_full_scale_mdps_t;
+
+typedef struct {
+	uint32_t odr_hz;
+	tdk_accel_full_scale_t accel_full_scale_g;
+	tdk_gyro_full_scale_mdps_t gyro_full_scale_mdps;
+	uint8_t ui_filter_config;
+} tdk_icm42688_config_t;
+
+typedef struct {
+	uint64_t timestamp_ticks;
+	int16_t temperature_raw;
+	int16_t accel_raw[3];
+	int16_t gyro_raw[3];
+	uint32_t sequence;
+} tdk_icm42688_raw_sample_t;
+
+typedef struct {
+	uint64_t irq_count;
+	uint64_t sample_count;
+	uint64_t invalid_sample_count;
+	uint64_t spurious_irq_count;
+	uint64_t queue_overflow_count;
+	uint64_t estimated_missed_sample_count;
+	uint64_t last_timestamp_ticks;
+	uint8_t who_am_i;
+	bool initialized;
+	bool running;
+} tdk_icm42688_diagnostics_t;
+
+void tdk_icm42688_default_config(tdk_icm42688_config_t* config);
+bool tdk_icm42688_init(const tdk_icm42688_config_t* config);
+bool tdk_icm42688_start(void);
+void tdk_icm42688_stop(void);
+
+/* Called only by the common PLIC ISR for BSP_PMOD_IRQ. */
+void tdk_icm42688_irq_handler(void);
+
+/* Foreground conversion; no floating-point work is performed in the ISR. */
+bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out);
+void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out);
+
+#endif

soc/software/gps_system/src/eskf.c

@@ -0,0 +1,809 @@
+#include "eskf.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define ESKF_POSITION_INDEX 0u
+#define ESKF_VELOCITY_INDEX 3u
+#define ESKF_ATTITUDE_INDEX 6u
+#define ESKF_ACCEL_BIAS_INDEX 9u
+#define ESKF_GYRO_BIAS_INDEX 12u
+
+#define ESKF_PI 3.14159265358979323846264338327950288
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double vector_norm3(const double value[3])
+{
+	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
+}
+
+static bool vector_is_finite(const double* value, size_t count)
+{
+	if(value == NULL)
+	{
+		return false;
+	}
+	for(size_t i = 0 ; i < count ; i++)
+	{
+		if(!isfinite(value[i]))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool quaternion_normalize(double quaternion[4])
+{
+	double norm = sqrt(square(quaternion[0]) + square(quaternion[1]) +
+	                   square(quaternion[2]) + square(quaternion[3]));
+	if(!(norm > DBL_EPSILON) || !isfinite(norm))
+	{
+		return false;
+	}
+	for(unsigned int i = 0 ; i < 4u ; i++)
+	{
+		quaternion[i] /= norm;
+	}
+	return true;
+}
+
+static void quaternion_multiply(const double first[4], const double second[4],
+	                            double output[4])
+{
+	output[0] = first[0] * second[0] - first[1] * second[1] -
+	            first[2] * second[2] - first[3] * second[3];
+	output[1] = first[0] * second[1] + first[1] * second[0] +
+	            first[2] * second[3] - first[3] * second[2];
+	output[2] = first[0] * second[2] - first[1] * second[3] +
+	            first[2] * second[0] + first[3] * second[1];
+	output[3] = first[0] * second[3] + first[1] * second[2] -
+	            first[2] * second[1] + first[3] * second[0];
+}
+
+static void rotation_vector_quaternion(const double rotation_vector[3],
+	                                   double quaternion[4])
+{
+	double angle = vector_norm3(rotation_vector);
+	if(angle < 1.0e-10)
+	{
+		quaternion[0] = 1.0;
+		quaternion[1] = 0.5 * rotation_vector[0];
+		quaternion[2] = 0.5 * rotation_vector[1];
+		quaternion[3] = 0.5 * rotation_vector[2];
+		(void)quaternion_normalize(quaternion);
+		return;
+	}
+
+	double scale = sin(0.5 * angle) / angle;
+	quaternion[0] = cos(0.5 * angle);
+	quaternion[1] = scale * rotation_vector[0];
+	quaternion[2] = scale * rotation_vector[1];
+	quaternion[3] = scale * rotation_vector[2];
+}
+
+static void quaternion_rotation_matrix(const double quaternion[4],
+	                                   double rotation[3][3])
+{
+	const double w = quaternion[0];
+	const double x = quaternion[1];
+	const double y = quaternion[2];
+	const double z = quaternion[3];
+
+	rotation[0][0] = 1.0 - 2.0 * (y * y + z * z);
+	rotation[0][1] = 2.0 * (x * y - w * z);
+	rotation[0][2] = 2.0 * (x * z + w * y);
+	rotation[1][0] = 2.0 * (x * y + w * z);
+	rotation[1][1] = 1.0 - 2.0 * (x * x + z * z);
+	rotation[1][2] = 2.0 * (y * z - w * x);
+	rotation[2][0] = 2.0 * (x * z - w * y);
+	rotation[2][1] = 2.0 * (y * z + w * x);
+	rotation[2][2] = 1.0 - 2.0 * (x * x + y * y);
+}
+
+static void skew_matrix(const double vector[3], double skew[3][3])
+{
+	skew[0][0] = 0.0;
+	skew[0][1] = -vector[2];
+	skew[0][2] = vector[1];
+	skew[1][0] = vector[2];
+	skew[1][1] = 0.0;
+	skew[1][2] = -vector[0];
+	skew[2][0] = -vector[1];
+	skew[2][1] = vector[0];
+	skew[2][2] = 0.0;
+}
+
+static bool inverse_symmetric3(const double input[9], double inverse[9])
+{
+	double lower[3][3] = {{0.0}};
+	double scale = fmax(fabs(input[0]), fmax(fabs(input[4]), fabs(input[8])));
+	if(!(scale > 0.0) || !isfinite(scale))
+	{
+		return false;
+	}
+
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col <= row ; col++)
+		{
+			double value = 0.5 * (input[row * 3u + col] + input[col * 3u + row]);
+			for(unsigned int k = 0 ; k < col ; k++)
+			{
+				value -= lower[row][k] * lower[col][k];
+			}
+			if(row == col)
+			{
+				if(value <= scale * DBL_EPSILON * 128.0 || !isfinite(value))
+				{
+					return false;
+				}
+				lower[row][col] = sqrt(value);
+			}
+			else
+			{
+				lower[row][col] = value / lower[col][col];
+			}
+		}
+	}
+
+	for(unsigned int column = 0 ; column < 3u ; column++)
+	{
+		double forward[3] = {0.0, 0.0, 0.0};
+		double solution[3] = {0.0, 0.0, 0.0};
+		for(unsigned int row = 0 ; row < 3u ; row++)
+		{
+			double rhs = (row == column) ? 1.0 : 0.0;
+			for(unsigned int k = 0 ; k < row ; k++)
+			{
+				rhs -= lower[row][k] * forward[k];
+			}
+			forward[row] = rhs / lower[row][row];
+		}
+		for(int row = 2 ; row >= 0 ; row--)
+		{
+			double rhs = forward[row];
+			for(unsigned int k = (unsigned int)row + 1u ; k < 3u ; k++)
+			{
+				rhs -= lower[k][row] * solution[k];
+			}
+			solution[row] = rhs / lower[row][row];
+		}
+		for(unsigned int row = 0 ; row < 3u ; row++)
+		{
+			inverse[row * 3u + column] = solution[row];
+		}
+	}
+	return vector_is_finite(inverse, 9u);
+}
+
+static void covariance_symmetrize_and_floor(eskf_t* filter, double floor_value)
+{
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = row + 1u ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			double symmetric = 0.5 *
+				(filter->covariance[row][col] + filter->covariance[col][row]);
+			filter->covariance[row][col] = symmetric;
+			filter->covariance[col][row] = symmetric;
+		}
+		if(filter->covariance[row][row] < floor_value)
+		{
+			filter->covariance[row][row] = floor_value;
+		}
+	}
+}
+
+void eskf_default_config(eskf_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	memset(config, 0, sizeof(*config));
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		config->accel_noise_density[i] = 0.03;
+		config->gyro_noise_density[i] = 0.003;
+		config->accel_bias_random_walk[i] = 0.002;
+		config->gyro_bias_random_walk[i] = 0.002 * ESKF_PI / 180.0;
+		config->initial_position_std_m[i] = (i < 2u) ? 10.0 : 20.0;
+		config->initial_velocity_std_mps[i] = 1.0;
+		config->initial_attitude_std_rad[i] =
+			((i < 2u) ? 5.0 : 30.0) * ESKF_PI / 180.0;
+		config->initial_accel_bias_std_mps2[i] = 0.1;
+		config->initial_gyro_bias_std_rps[i] = 0.05 * ESKF_PI / 180.0;
+	}
+	config->gravity_mps2 = 9.80665;
+	config->max_propagation_step_s = 0.02;
+	config->position_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */
+	config->covariance_diagonal_floor = 1.0e-12;
+}
+
+bool eskf_quaternion_align_vectors(const double source[3],
+	                               const double target[3],
+	                               double quaternion_out[4])
+{
+	if(!vector_is_finite(source, 3u) || !vector_is_finite(target, 3u) ||
+	   (quaternion_out == NULL))
+	{
+		return false;
+	}
+	double source_norm = vector_norm3(source);
+	double target_norm = vector_norm3(target);
+	if(!(source_norm > DBL_EPSILON) || !(target_norm > DBL_EPSILON))
+	{
+		return false;
+	}
+	double first[3];
+	double second[3];
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		first[i] = source[i] / source_norm;
+		second[i] = target[i] / target_norm;
+	}
+	double dot = first[0] * second[0] + first[1] * second[1] + first[2] * second[2];
+	if(dot < -0.999999)
+	{
+		double axis[3] = {0.0, first[2], -first[1]};
+		if(vector_norm3(axis) < 1.0e-6)
+		{
+			axis[0] = -first[2];
+			axis[1] = 0.0;
+			axis[2] = first[0];
+		}
+		double axis_norm = vector_norm3(axis);
+		quaternion_out[0] = 0.0;
+		for(unsigned int i = 0 ; i < 3u ; i++)
+		{
+			quaternion_out[i + 1u] = axis[i] / axis_norm;
+		}
+		return true;
+	}
+
+	quaternion_out[0] = 1.0 + dot;
+	quaternion_out[1] = first[1] * second[2] - first[2] * second[1];
+	quaternion_out[2] = first[2] * second[0] - first[0] * second[2];
+	quaternion_out[3] = first[0] * second[1] - first[1] * second[0];
+	return quaternion_normalize(quaternion_out);
+}
+
+bool eskf_initialize(eskf_t* filter,
+	                 const eskf_config_t* config,
+	                 const double initial_position_m[3],
+	                 const double body_to_nav_quaternion[4],
+	                 const double accel_bias_mps2[3],
+	                 const double gyro_bias_rps[3])
+{
+	if((filter == NULL) || (config == NULL) ||
+	   !vector_is_finite(initial_position_m, 3u) ||
+	   !vector_is_finite(body_to_nav_quaternion, 4u) ||
+	   !vector_is_finite(accel_bias_mps2, 3u) ||
+	   !vector_is_finite(gyro_bias_rps, 3u) ||
+	   !(config->gravity_mps2 > 0.0))
+	{
+		return false;
+	}
+
+	memset(filter, 0, sizeof(*filter));
+	memcpy(filter->position_m, initial_position_m, sizeof(filter->position_m));
+	memcpy(filter->body_to_nav_quaternion, body_to_nav_quaternion,
+	       sizeof(filter->body_to_nav_quaternion));
+	memcpy(filter->accel_bias_mps2, accel_bias_mps2,
+	       sizeof(filter->accel_bias_mps2));
+	memcpy(filter->gyro_bias_rps, gyro_bias_rps, sizeof(filter->gyro_bias_rps));
+	if(!quaternion_normalize(filter->body_to_nav_quaternion))
+	{
+		return false;
+	}
+
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		filter->covariance[ESKF_POSITION_INDEX + i][ESKF_POSITION_INDEX + i] =
+			square(config->initial_position_std_m[i]);
+		filter->covariance[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] =
+			square(config->initial_velocity_std_mps[i]);
+		filter->covariance[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] =
+			square(config->initial_attitude_std_rad[i]);
+		filter->covariance[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] =
+			square(config->initial_accel_bias_std_mps2[i]);
+		filter->covariance[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] =
+			square(config->initial_gyro_bias_std_rps[i]);
+	}
+	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
+	filter->last_position_nis = NAN;
+	filter->initialized = true;
+	return true;
+}
+
+static bool eskf_propagate_step(eskf_t* filter,
+	                            const eskf_config_t* config,
+	                            const double accel_mps2[3],
+	                            const double gyro_rps[3],
+	                            double dt_s)
+{
+	double specific_force[3];
+	double angular_rate[3];
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		specific_force[i] = accel_mps2[i] - filter->accel_bias_mps2[i];
+		angular_rate[i] = gyro_rps[i] - filter->gyro_bias_rps[i];
+		filter->last_unbiased_gyro_rps[i] = angular_rate[i];
+	}
+
+	double rotation[3][3];
+	quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
+	double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			linear_accel[row] += rotation[row][col] * specific_force[col];
+		}
+		filter->last_linear_accel_nav_mps2[row] = linear_accel[row];
+		filter->position_m[row] += filter->velocity_mps[row] * dt_s +
+		                           0.5 * linear_accel[row] * dt_s * dt_s;
+		filter->velocity_mps[row] += linear_accel[row] * dt_s;
+	}
+
+	double rotation_vector[3] = {
+		angular_rate[0] * dt_s,
+		angular_rate[1] * dt_s,
+		angular_rate[2] * dt_s
+	};
+	double delta_quaternion[4];
+	double updated_quaternion[4];
+	rotation_vector_quaternion(rotation_vector, delta_quaternion);
+	quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
+	                    updated_quaternion);
+	memcpy(filter->body_to_nav_quaternion, updated_quaternion,
+	       sizeof(updated_quaternion));
+	if(!quaternion_normalize(filter->body_to_nav_quaternion))
+	{
+		return false;
+	}
+
+	double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
+		system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0;
+	}
+	double force_skew[3][3];
+	double rate_skew[3][3];
+	skew_matrix(specific_force, force_skew);
+	skew_matrix(angular_rate, rate_skew);
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			double rotated_skew = 0.0;
+			for(unsigned int k = 0 ; k < 3u ; k++)
+			{
+				rotated_skew += rotation[row][k] * force_skew[k][col];
+			}
+			system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
+				-rotated_skew;
+			system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
+				-rotation[row][col];
+			system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
+				-rate_skew[row][col];
+		}
+	}
+
+	double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			transition[row][col] = system[row][col] * dt_s;
+		}
+		transition[row][row] += 1.0;
+	}
+
+	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				intermediate[row][col] +=
+					transition[row][k] * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				propagated[row][col] += intermediate[row][k] * transition[col][k];
+			}
+		}
+	}
+
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			double accel_noise = 0.0;
+			for(unsigned int axis = 0 ; axis < 3u ; axis++)
+			{
+				accel_noise += rotation[row][axis] *
+				               square(config->accel_noise_density[axis]) *
+				               rotation[col][axis];
+			}
+			propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] +=
+				accel_noise * dt_s;
+		}
+		propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] +=
+			square(config->gyro_noise_density[row]) * dt_s;
+		propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] +=
+			square(config->accel_bias_random_walk[row]) * dt_s;
+		propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] +=
+			square(config->gyro_bias_random_walk[row]) * dt_s;
+	}
+	memcpy(filter->covariance, propagated, sizeof(propagated));
+	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
+	filter->propagation_count++;
+	return eskf_state_is_finite(filter);
+}
+
+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[3],
+	                const double gyro_rps[3],
+	                double dt_s)
+{
+	if((filter == NULL) || !filter->initialized || (config == NULL) ||
+	   !vector_is_finite(accel_mps2, 3u) || !vector_is_finite(gyro_rps, 3u) ||
+	   !isfinite(dt_s) || (dt_s < 0.0) ||
+	   !(config->max_propagation_step_s > 0.0))
+	{
+		return false;
+	}
+	if(dt_s == 0.0)
+	{
+		return true;
+	}
+	unsigned int steps = (unsigned int)ceil(dt_s / config->max_propagation_step_s);
+	if((steps == 0u) || (steps > 10000u))
+	{
+		return false;
+	}
+	double step_s = dt_s / (double)steps;
+	for(unsigned int step = 0 ; step < steps ; step++)
+	{
+		if(!eskf_propagate_step(filter, config, accel_mps2, gyro_rps, step_s))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
+{
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		filter->position_m[i] += error[ESKF_POSITION_INDEX + i];
+		filter->velocity_mps[i] += error[ESKF_VELOCITY_INDEX + i];
+		filter->accel_bias_mps2[i] += error[ESKF_ACCEL_BIAS_INDEX + i];
+		filter->gyro_bias_rps[i] += error[ESKF_GYRO_BIAS_INDEX + i];
+	}
+	double attitude_error[3] = {
+		error[ESKF_ATTITUDE_INDEX],
+		error[ESKF_ATTITUDE_INDEX + 1u],
+		error[ESKF_ATTITUDE_INDEX + 2u]
+	};
+	double delta_quaternion[4];
+	double updated_quaternion[4];
+	rotation_vector_quaternion(attitude_error, delta_quaternion);
+	quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
+	                    updated_quaternion);
+	memcpy(filter->body_to_nav_quaternion, updated_quaternion,
+	       sizeof(updated_quaternion));
+	if(!quaternion_normalize(filter->body_to_nav_quaternion))
+	{
+		return false;
+	}
+
+	/* Right attitude-error reset after q <- q (x) Exp(delta_theta). */
+	double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
+	{
+		reset[i][i] = 1.0;
+	}
+	double attitude_skew[3][3];
+	skew_matrix(attitude_error, attitude_skew);
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -=
+				0.5 * attitude_skew[row][col];
+		}
+	}
+	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
+			}
+		}
+	}
+	memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance));
+	return true;
+}
+
+static bool eskf_selected_update(eskf_t* filter,
+	                             unsigned int selected_state_index,
+	                             const double residual[3],
+	                             const double measurement_covariance[9],
+	                             bool gate_enabled,
+	                             double gate,
+	                             double* nis_out)
+{
+	if((selected_state_index + 3u) > ESKF_ERROR_STATE_DIM ||
+	   !vector_is_finite(residual, 3u) ||
+	   !vector_is_finite(measurement_covariance, 9u))
+	{
+		return false;
+	}
+	double covariance_inverse[9];
+	if(!inverse_symmetric3(measurement_covariance, covariance_inverse))
+	{
+		return false;
+	}
+	double innovation[9];
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			innovation[row * 3u + col] =
+				filter->covariance[selected_state_index + row][selected_state_index + col] +
+				measurement_covariance[row * 3u + col];
+		}
+	}
+	double innovation_inverse[9];
+	if(!inverse_symmetric3(innovation, innovation_inverse))
+	{
+		return false;
+	}
+	double nis = 0.0;
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			nis += residual[row] * innovation_inverse[row * 3u + col] * residual[col];
+		}
+	}
+	if(nis_out != NULL)
+	{
+		*nis_out = nis;
+	}
+	if(!isfinite(nis) || (gate_enabled && (nis > gate)))
+	{
+		return false;
+	}
+
+	double gain[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			for(unsigned int k = 0 ; k < 3u ; k++)
+			{
+				gain[row][col] +=
+					filter->covariance[row][selected_state_index + k] *
+					innovation_inverse[k * 3u + col];
+			}
+		}
+	}
+	double error[ESKF_ERROR_STATE_DIM] = {0.0};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			error[row] += gain[row][col] * residual[col];
+		}
+	}
+
+	double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		joseph_left[row][row] = 1.0;
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			joseph_left[row][selected_state_index + col] -= gain[row][col];
+		}
+	}
+	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				intermediate[row][col] +=
+					joseph_left[row][k] * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+		{
+			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			{
+				updated_covariance[row][col] +=
+					intermediate[row][k] * joseph_left[col][k];
+			}
+			for(unsigned int first = 0 ; first < 3u ; first++)
+			{
+				for(unsigned int second = 0 ; second < 3u ; second++)
+				{
+					updated_covariance[row][col] += gain[row][first] *
+						measurement_covariance[first * 3u + second] * gain[col][second];
+				}
+			}
+		}
+	}
+	memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance));
+	if(!eskf_inject_error(filter, error))
+	{
+		return false;
+	}
+	covariance_symmetrize_and_floor(filter, 1.0e-12);
+	return eskf_state_is_finite(filter);
+}
+
+eskf_update_result_t eskf_position_update(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double position_m[3],
+	const double covariance_m2[9],
+	double* nis_out)
+{
+	if(nis_out != NULL)
+	{
+		*nis_out = NAN;
+	}
+	if((filter == NULL) || !filter->initialized || (config == NULL) ||
+	   !vector_is_finite(position_m, 3u) || !vector_is_finite(covariance_m2, 9u))
+	{
+		return ESKF_UPDATE_INVALID;
+	}
+	double residual[3];
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		residual[i] = position_m[i] - filter->position_m[i];
+	}
+	double nis = NAN;
+	if(!eskf_selected_update(filter, ESKF_POSITION_INDEX, residual, covariance_m2,
+	                         true, config->position_nis_gate, &nis))
+	{
+		filter->last_position_nis = nis;
+		if(isfinite(nis) && (nis > config->position_nis_gate))
+		{
+			filter->position_update_rejected_count++;
+			if(nis_out != NULL)
+			{
+				*nis_out = nis;
+			}
+			return ESKF_UPDATE_REJECTED_NIS;
+		}
+		return ESKF_UPDATE_INVALID;
+	}
+	filter->last_position_nis = nis;
+	filter->position_update_accepted_count++;
+	if(nis_out != NULL)
+	{
+		*nis_out = nis;
+	}
+	return ESKF_UPDATE_ACCEPTED;
+}
+
+bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !(velocity_std_mps > 0.0) || !isfinite(velocity_std_mps))
+	{
+		return false;
+	}
+	double residual[3] = {
+		-filter->velocity_mps[0],
+		-filter->velocity_mps[1],
+		-filter->velocity_mps[2]
+	};
+	double covariance[9] = {0.0};
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		covariance[i * 3u + i] = square(velocity_std_mps);
+	}
+	return eskf_selected_update(filter, ESKF_VELOCITY_INDEX, residual, covariance,
+	                            false, 0.0, NULL);
+}
+
+bool eskf_zero_angular_rate_update(
+	eskf_t* filter,
+	const double measured_gyro_rps[3],
+	const double measurement_std_rps[3])
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !vector_is_finite(measured_gyro_rps, 3u) ||
+	   !vector_is_finite(measurement_std_rps, 3u))
+	{
+		return false;
+	}
+	double residual[3];
+	double covariance[9] = {0.0};
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		if(!(measurement_std_rps[i] > 0.0))
+		{
+			return false;
+		}
+		residual[i] = measured_gyro_rps[i] - filter->gyro_bias_rps[i];
+		covariance[i * 3u + i] = square(measurement_std_rps[i]);
+	}
+	return eskf_selected_update(filter, ESKF_GYRO_BIAS_INDEX, residual, covariance,
+	                            false, 0.0, NULL);
+}
+
+double eskf_speed_mps(const eskf_t* filter)
+{
+	if((filter == NULL) || !filter->initialized)
+	{
+		return NAN;
+	}
+	return vector_norm3(filter->velocity_mps);
+}
+
+bool eskf_state_is_finite(const eskf_t* filter)
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !vector_is_finite(filter->position_m, 3u) ||
+	   !vector_is_finite(filter->velocity_mps, 3u) ||
+	   !vector_is_finite(filter->body_to_nav_quaternion, 4u) ||
+	   !vector_is_finite(filter->accel_bias_mps2, 3u) ||
+	   !vector_is_finite(filter->gyro_bias_rps, 3u) ||
+	   !vector_is_finite(&filter->covariance[0][0],
+	                     ESKF_ERROR_STATE_DIM * ESKF_ERROR_STATE_DIM))
+	{
+		return false;
+	}
+	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
+	{
+		if(filter->covariance[i][i] < 0.0)
+		{
+			return false;
+		}
+	}
+	return true;
+}

soc/software/gps_system/src/fusion_bus.c

@@ -0,0 +1,102 @@
+#include "fusion_bus.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#if (FUSION_IMU_QUEUE_CAPACITY < 2u)
+#error "FUSION_IMU_QUEUE_CAPACITY must be at least 2"
+#endif
+
+#if (FUSION_GPS_QUEUE_CAPACITY < 2u)
+#error "FUSION_GPS_QUEUE_CAPACITY must be at least 2"
+#endif
+
+static unsigned int next_index(unsigned int index, unsigned int capacity)
+{
+	index++;
+	return (index == capacity) ? 0u : index;
+}
+
+void fusion_bus_init(fusion_bus_t* bus)
+{
+	if(bus != NULL)
+	{
+		memset(bus, 0, sizeof(*bus));
+	}
+}
+
+bool fusion_bus_publish_imu(fusion_bus_t* bus,
+	                        const fusion_imu_measurement_t* measurement)
+{
+	if((bus == NULL) || (measurement == NULL))
+	{
+		return false;
+	}
+	unsigned int head = __atomic_load_n(&bus->imu_head, __ATOMIC_RELAXED);
+	unsigned int next = next_index(head, FUSION_IMU_QUEUE_CAPACITY);
+	if(next == __atomic_load_n(&bus->imu_tail, __ATOMIC_ACQUIRE))
+	{
+		__atomic_fetch_add(&bus->imu_overflow_count, 1u, __ATOMIC_RELAXED);
+		return false;
+	}
+	bus->imu_queue[head] = *measurement;
+	__atomic_store_n(&bus->imu_head, next, __ATOMIC_RELEASE);
+	return true;
+}
+
+bool fusion_bus_take_imu(fusion_bus_t* bus,
+	                     fusion_imu_measurement_t* measurement_out)
+{
+	if((bus == NULL) || (measurement_out == NULL))
+	{
+		return false;
+	}
+	unsigned int tail = __atomic_load_n(&bus->imu_tail, __ATOMIC_RELAXED);
+	if(tail == __atomic_load_n(&bus->imu_head, __ATOMIC_ACQUIRE))
+	{
+		return false;
+	}
+	*measurement_out = bus->imu_queue[tail];
+	__atomic_store_n(&bus->imu_tail,
+	                 next_index(tail, FUSION_IMU_QUEUE_CAPACITY),
+	                 __ATOMIC_RELEASE);
+	return true;
+}
+
+bool fusion_bus_publish_gps(fusion_bus_t* bus,
+	                        const fusion_gps_measurement_t* measurement)
+{
+	if((bus == NULL) || (measurement == NULL))
+	{
+		return false;
+	}
+	unsigned int head = __atomic_load_n(&bus->gps_head, __ATOMIC_RELAXED);
+	unsigned int next = next_index(head, FUSION_GPS_QUEUE_CAPACITY);
+	if(next == __atomic_load_n(&bus->gps_tail, __ATOMIC_ACQUIRE))
+	{
+		__atomic_fetch_add(&bus->gps_overflow_count, 1u, __ATOMIC_RELAXED);
+		return false;
+	}
+	bus->gps_queue[head] = *measurement;
+	__atomic_store_n(&bus->gps_head, next, __ATOMIC_RELEASE);
+	return true;
+}
+
+bool fusion_bus_take_gps(fusion_bus_t* bus,
+	                     fusion_gps_measurement_t* measurement_out)
+{
+	if((bus == NULL) || (measurement_out == NULL))
+	{
+		return false;
+	}
+	unsigned int tail = __atomic_load_n(&bus->gps_tail, __ATOMIC_RELAXED);
+	if(tail == __atomic_load_n(&bus->gps_head, __ATOMIC_ACQUIRE))
+	{
+		return false;
+	}
+	*measurement_out = bus->gps_queue[tail];
+	__atomic_store_n(&bus->gps_tail,
+	                 next_index(tail, FUSION_GPS_QUEUE_CAPACITY),
+	                 __ATOMIC_RELEASE);
+	return true;
+}

soc/software/gps_system/src/fusion_engine.c

@@ -0,0 +1,730 @@
+#include "fusion_engine.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define FUSION_PI 3.14159265358979323846264338327950288
+
+static double square(double value)
+{
+	return value * value;
+}
+
+static double norm3(const double value[3])
+{
+	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
+}
+
+static bool finite_values(const double* values, unsigned int count)
+{
+	if(values == NULL)
+	{
+		return false;
+	}
+	for(unsigned int i = 0 ; i < count ; i++)
+	{
+		if(!isfinite(values[i]))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static double ticks_to_seconds(const fusion_engine_t* engine, uint64_t ticks)
+{
+	return (double)ticks / (double)engine->config.timebase_hz;
+}
+
+static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds)
+{
+	double ticks = seconds * (double)engine->config.timebase_hz;
+	if(ticks <= 1.0)
+	{
+		return 1u;
+	}
+	return (uint64_t)(ticks + 0.5);
+}
+
+void fusion_engine_default_config(fusion_engine_config_t* config,
+	                              uint64_t timebase_hz)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	memset(config, 0, sizeof(*config));
+	eskf_default_config(&config->eskf);
+	fusion_motion_default_config(&config->motion);
+	config->timebase_hz = timebase_hz;
+	config->calibration_duration_s = 2.0;
+	config->calibration_min_samples = 50u;
+	config->calibration_accel_norm_tolerance_mps2 = 0.30;
+	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;
+	config->zero_update_period_s = 0.05;
+	config->zero_velocity_std_mps = 0.03;
+	for(unsigned int i = 0 ; i < 3u ; i++)
+	{
+		config->zero_gyro_std_rps[i] = 0.01 * FUSION_PI / 180.0;
+	}
+	config->gnss_motion_speed_threshold_mps = 0.50;
+	config->gnss_displacement_nis_gate = 9.21034037197618; /* 2 DOF, 99%. */
+	config->gnss_moving_confirmations = 2u;
+	config->gnss_still_confirmations = 2u;
+}
+
+static bool config_valid(const fusion_engine_config_t* config)
+{
+	return (config != NULL) && (config->timebase_hz != 0u) &&
+	       isfinite(config->calibration_duration_s) &&
+	       (config->calibration_duration_s > 0.0) &&
+	       (config->calibration_min_samples >= 2u) &&
+	       isfinite(config->calibration_accel_norm_tolerance_mps2) &&
+	       (config->calibration_accel_norm_tolerance_mps2 > 0.0) &&
+	       isfinite(config->calibration_gyro_limit_rps) &&
+	       (config->calibration_gyro_limit_rps > 0.0) &&
+	       isfinite(config->zero_update_period_s) &&
+	       (config->zero_update_period_s > 0.0) &&
+	       isfinite(config->zero_velocity_std_mps) &&
+	       (config->zero_velocity_std_mps > 0.0) &&
+	       finite_values(config->zero_gyro_std_rps, 3u) &&
+	       (config->gnss_moving_confirmations != 0u) &&
+	       (config->gnss_still_confirmations != 0u);
+}
+
+bool fusion_engine_init(fusion_engine_t* engine,
+	                    const fusion_engine_config_t* config)
+{
+	if((engine == NULL) || !config_valid(config))
+	{
+		return false;
+	}
+	memset(engine, 0, sizeof(*engine));
+	engine->config = *config;
+	engine->state = FUSION_ENGINE_CALIBRATING;
+	return true;
+}
+
+static bool imu_valid(const fusion_imu_measurement_t* measurement)
+{
+	return (measurement != NULL) &&
+	       finite_values(measurement->accel_mps2, 3u) &&
+	       finite_values(measurement->gyro_rps, 3u) &&
+	       isfinite(measurement->temperature_c);
+}
+
+static bool position_valid(const fusion_position_measurement_t* measurement)
+{
+	return (measurement != NULL) &&
+	       finite_values(measurement->position_enu_m, 3u) &&
+	       finite_values(measurement->covariance_enu_m2, 9u) &&
+	       isfinite(measurement->source_quality);
+}
+
+static void calibration_reset(fusion_engine_t* engine)
+{
+	memset(&engine->calibration, 0, sizeof(engine->calibration));
+	engine->diagnostics.calibration_reset_count++;
+}
+
+static void calibration_add(fusion_calibration_accumulator_t* calibration,
+	                        const fusion_imu_measurement_t* measurement)
+{
+	calibration->sample_count++;
+	double count = (double)calibration->sample_count;
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double accel_delta = measurement->accel_mps2[axis] - calibration->accel_mean[axis];
+		calibration->accel_mean[axis] += accel_delta / count;
+		calibration->accel_m2[axis] +=
+			accel_delta * (measurement->accel_mps2[axis] - calibration->accel_mean[axis]);
+		double gyro_delta = measurement->gyro_rps[axis] - calibration->gyro_mean[axis];
+		calibration->gyro_mean[axis] += gyro_delta / count;
+		calibration->gyro_m2[axis] +=
+			gyro_delta * (measurement->gyro_rps[axis] - calibration->gyro_mean[axis]);
+	}
+}
+
+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);
+	return (accel_error <= engine->config.calibration_accel_norm_tolerance_mps2) &&
+	       (norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps);
+}
+
+static bool calibration_statistics_are_quiet(const fusion_engine_t* engine)
+{
+	const fusion_calibration_accumulator_t* calibration = &engine->calibration;
+	if(calibration->sample_count < 2u)
+	{
+		return false;
+	}
+	double denominator = (double)(calibration->sample_count - 1u);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double accel_std = sqrt(fmax(0.0, calibration->accel_m2[axis] / denominator));
+		double gyro_std = sqrt(fmax(0.0, calibration->gyro_m2[axis] / denominator));
+		if((accel_std > engine->config.calibration_accel_std_limit_mps2) ||
+		   (gyro_std > engine->config.calibration_gyro_std_limit_rps))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static void history_clear(fusion_engine_t* engine)
+{
+	engine->imu_history_head = 0u;
+	engine->imu_history_count = 0u;
+}
+
+static const fusion_imu_measurement_t* history_at(const fusion_engine_t* engine,
+	                                              unsigned int offset)
+{
+	unsigned int index = (engine->imu_history_head + offset) % FUSION_IMU_HISTORY_CAPACITY;
+	return &engine->imu_history[index];
+}
+
+static bool runtime_apply_imu(fusion_engine_t* engine,
+	                          fusion_runtime_t* runtime,
+	                          const fusion_imu_measurement_t* measurement,
+	                          bool count_zero_updates);
+
+static void history_push(fusion_engine_t* engine,
+	                     const fusion_imu_measurement_t* measurement)
+{
+	if(engine->imu_history_count == FUSION_IMU_HISTORY_CAPACITY)
+	{
+		const fusion_imu_measurement_t oldest = *history_at(engine, 0u);
+		if(engine->checkpoint_valid &&
+		   (oldest.timestamp_ticks > engine->checkpoint.filter_timestamp_ticks))
+		{
+			if(!runtime_apply_imu(engine, &engine->checkpoint, &oldest, false))
+			{
+				engine->state = FUSION_ENGINE_FAULT;
+				return;
+			}
+			engine->diagnostics.checkpoint_advance_count++;
+		}
+		engine->imu_history_head =
+			(engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY;
+		engine->imu_history_count--;
+		engine->diagnostics.imu_history_overflow++;
+	}
+	unsigned int index = (engine->imu_history_head + engine->imu_history_count) %
+	                     FUSION_IMU_HISTORY_CAPACITY;
+	engine->imu_history[index] = *measurement;
+	engine->imu_history_count++;
+}
+
+static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_ticks)
+{
+	while((engine->imu_history_count != 0u) &&
+	      (history_at(engine, 0u)->timestamp_ticks <= timestamp_ticks))
+	{
+		engine->imu_history_head =
+			(engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY;
+		engine->imu_history_count--;
+	}
+}
+
+static bool runtime_propagate_to(fusion_engine_t* engine,
+	                             fusion_runtime_t* runtime,
+	                             uint64_t timestamp_ticks,
+	                             const fusion_imu_measurement_t* input)
+{
+	if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized ||
+	   (timestamp_ticks < runtime->filter_timestamp_ticks))
+	{
+		return false;
+	}
+	if(timestamp_ticks == runtime->filter_timestamp_ticks)
+	{
+		return true;
+	}
+	double dt_s = ticks_to_seconds(engine,
+	                              timestamp_ticks - runtime->filter_timestamp_ticks);
+	if(!eskf_propagate(&runtime->filter, &engine->config.eskf,
+	                   input->accel_mps2, input->gyro_rps, dt_s))
+	{
+		return false;
+	}
+	runtime->filter_timestamp_ticks = timestamp_ticks;
+	return true;
+}
+
+static bool runtime_apply_imu(fusion_engine_t* engine,
+	                          fusion_runtime_t* runtime,
+	                          const fusion_imu_measurement_t* measurement,
+	                          bool count_zero_updates)
+{
+	if(!runtime->have_held_imu)
+	{
+		runtime->held_imu = *measurement;
+		runtime->have_held_imu = true;
+		runtime->filter_timestamp_ticks = measurement->timestamp_ticks;
+		return true;
+	}
+	if((measurement->timestamp_ticks <= runtime->held_imu.timestamp_ticks) ||
+	   (measurement->timestamp_ticks < runtime->filter_timestamp_ticks))
+	{
+		return false;
+	}
+	double motion_dt_s = ticks_to_seconds(
+		engine, measurement->timestamp_ticks - runtime->held_imu.timestamp_ticks);
+	if(!runtime_propagate_to(engine, runtime, measurement->timestamp_ticks, measurement))
+	{
+		return false;
+	}
+	(void)fusion_motion_update(
+		&runtime->motion, &engine->config.motion,
+		runtime->filter.last_linear_accel_nav_mps2,
+		runtime->filter.last_unbiased_gyro_rps,
+		eskf_speed_mps(&runtime->filter), runtime->gnss_evidence, motion_dt_s);
+	bool inertial_motion =
+		(runtime->motion.last_linear_accel_norm_mps2 >=
+		 engine->config.motion.moving_accel_threshold_mps2) ||
+		(runtime->motion.last_gyro_norm_rps >=
+		 engine->config.motion.moving_gyro_threshold_rps) ||
+		(runtime->motion.last_jerk_norm_mps3 >=
+		 engine->config.motion.moving_jerk_threshold_mps3);
+	if(inertial_motion &&
+	   (runtime->gnss_evidence == FUSION_GNSS_MOTION_STILL))
+	{
+		/* A new IMU motion onset makes an older GNSS-still decision stale. */
+		runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
+		runtime->gnss_moving_count = 0u;
+		runtime->gnss_still_count = 0u;
+	}
+	runtime->held_imu = *measurement;
+
+	uint64_t zero_period_ticks =
+		seconds_to_ticks(engine, engine->config.zero_update_period_s);
+	if(fusion_motion_allows_zero_update(&runtime->motion) &&
+	   ((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >=
+	    zero_period_ticks))
+	{
+		if(!eskf_zero_velocity_update(&runtime->filter,
+		                              engine->config.zero_velocity_std_mps) ||
+		   !eskf_zero_angular_rate_update(&runtime->filter,
+		                                  measurement->gyro_rps,
+		                                  engine->config.zero_gyro_std_rps))
+		{
+			return false;
+		}
+		runtime->last_zero_update_ticks = measurement->timestamp_ticks;
+		if(count_zero_updates)
+		{
+			engine->diagnostics.zero_velocity_update_count++;
+		}
+	}
+	return true;
+}
+
+static bool finish_calibration(fusion_engine_t* engine,
+	                           const fusion_imu_measurement_t* last_measurement)
+{
+	if(!calibration_statistics_are_quiet(engine))
+	{
+		calibration_reset(engine);
+		return false;
+	}
+	const fusion_calibration_accumulator_t* calibration = &engine->calibration;
+	double gravity_norm = norm3(calibration->accel_mean);
+	if(!(gravity_norm > DBL_EPSILON))
+	{
+		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))
+	{
+		engine->state = FUSION_ENGINE_FAULT;
+		return false;
+	}
+	double accel_bias[3];
+	double gyro_bias[3];
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double expected_gravity = calibration->accel_mean[axis] *
+		                          engine->config.eskf.gravity_mps2 / gravity_norm;
+		accel_bias[axis] = calibration->accel_mean[axis] - expected_gravity;
+		gyro_bias[axis] = calibration->gyro_mean[axis];
+	}
+	double initial_position[3] = {0.0, 0.0, 0.0};
+	if(engine->have_initial_position)
+	{
+		memcpy(initial_position, engine->initial_position.position_enu_m,
+		       sizeof(initial_position));
+	}
+	if(!eskf_initialize(&engine->current.filter, &engine->config.eskf,
+	                    initial_position, quaternion, accel_bias, gyro_bias))
+	{
+		engine->state = FUSION_ENGINE_FAULT;
+		return false;
+	}
+	fusion_motion_init(&engine->current.motion, true);
+	engine->current.held_imu = *last_measurement;
+	engine->current.have_held_imu = true;
+	engine->current.filter_timestamp_ticks = last_measurement->timestamp_ticks;
+	engine->current.last_zero_update_ticks = last_measurement->timestamp_ticks;
+	engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
+	if(engine->have_initial_position)
+	{
+		engine->current.have_accepted_position = true;
+		engine->current.last_accepted_position_ticks =
+			engine->initial_position.timestamp_ticks;
+		memcpy(engine->current.last_accepted_position_m,
+		       engine->initial_position.position_enu_m,
+		       sizeof(engine->current.last_accepted_position_m));
+		memcpy(engine->current.last_accepted_position_covariance_m2,
+		       engine->initial_position.covariance_enu_m2,
+		       sizeof(engine->current.last_accepted_position_covariance_m2));
+	}
+	engine->checkpoint = engine->current;
+	engine->checkpoint_valid = true;
+	history_clear(engine);
+	engine->state = FUSION_ENGINE_RUNNING;
+	return true;
+}
+
+static bool process_calibration_imu(fusion_engine_t* engine,
+	                                const fusion_imu_measurement_t* measurement)
+{
+	if(engine->calibration.have_start &&
+	   (measurement->timestamp_ticks <= engine->calibration.last_ticks))
+	{
+		return false;
+	}
+	if(!calibration_sample_is_quiet(engine, measurement))
+	{
+		calibration_reset(engine);
+		return true;
+	}
+	if(!engine->calibration.have_start)
+	{
+		engine->calibration.start_ticks = measurement->timestamp_ticks;
+		engine->calibration.have_start = true;
+	}
+	engine->calibration.last_ticks = measurement->timestamp_ticks;
+	calibration_add(&engine->calibration, measurement);
+	double duration_s = ticks_to_seconds(
+		engine, engine->calibration.last_ticks - engine->calibration.start_ticks);
+	if((engine->calibration.sample_count >= engine->config.calibration_min_samples) &&
+	   (duration_s >= engine->config.calibration_duration_s))
+	{
+		(void)finish_calibration(engine, measurement);
+	}
+	return true;
+}
+
+static fusion_gnss_motion_evidence_t position_motion_evidence(
+	const fusion_engine_t* engine,
+	const fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement)
+{
+	if(!runtime->have_accepted_position ||
+	   (measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	double dt_s = ticks_to_seconds(
+		engine, measurement->timestamp_ticks - runtime->last_accepted_position_ticks);
+	if(!(dt_s > 0.0))
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0];
+	double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1];
+	double s00 = measurement->covariance_enu_m2[0] +
+	             runtime->last_accepted_position_covariance_m2[0];
+	double s01 = measurement->covariance_enu_m2[1] +
+	             runtime->last_accepted_position_covariance_m2[1];
+	double s10 = measurement->covariance_enu_m2[3] +
+	             runtime->last_accepted_position_covariance_m2[3];
+	double s11 = measurement->covariance_enu_m2[4] +
+	             runtime->last_accepted_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) ||
+	   !isfinite(determinant))
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	double displacement_nis =
+		(delta_e * delta_e * s11 - 2.0 * delta_e * delta_n * off_diagonal +
+		 delta_n * delta_n * s00) / determinant;
+	double speed_mps = sqrt(delta_e * delta_e + delta_n * delta_n) / dt_s;
+	if((speed_mps >= engine->config.gnss_motion_speed_threshold_mps) &&
+	   (displacement_nis >= engine->config.gnss_displacement_nis_gate))
+	{
+		return FUSION_GNSS_MOTION_MOVING;
+	}
+	return FUSION_GNSS_MOTION_STILL;
+}
+
+static eskf_update_result_t runtime_apply_position(
+	fusion_engine_t* engine,
+	fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement,
+	double* nis_out)
+{
+	eskf_update_result_t result = eskf_position_update(
+		&runtime->filter, &engine->config.eskf,
+		measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
+	if(result == ESKF_UPDATE_ACCEPTED)
+	{
+		fusion_gnss_motion_evidence_t raw_evidence =
+			position_motion_evidence(engine, runtime, measurement);
+		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;
+			}
+		}
+		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;
+}
+
+static bool process_position_now(fusion_engine_t* engine,
+	                             const fusion_position_measurement_t* measurement,
+	                             eskf_update_result_t* result_out,
+	                             double* nis_out)
+{
+	if(!engine->checkpoint_valid ||
+	   (measurement->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks))
+	{
+		engine->diagnostics.position_stale_rejected++;
+		return false;
+	}
+	fusion_runtime_t revised = engine->checkpoint;
+	uint64_t replayed = 0u;
+	for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
+	{
+		const fusion_imu_measurement_t* imu = history_at(engine, i);
+		if(imu->timestamp_ticks > measurement->timestamp_ticks)
+		{
+			break;
+		}
+		if(!runtime_apply_imu(engine, &revised, imu, false))
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+		replayed++;
+	}
+	if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
+	{
+		if(!revised.have_held_imu ||
+		   !runtime_propagate_to(engine, &revised,
+		                         measurement->timestamp_ticks, &revised.held_imu))
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+	}
+	double nis = NAN;
+	eskf_update_result_t result = runtime_apply_position(engine, &revised,
+	                                                    measurement, &nis);
+	if(result == ESKF_UPDATE_INVALID)
+	{
+		return false;
+	}
+	if(result == ESKF_UPDATE_ACCEPTED)
+	{
+		engine->diagnostics.position_accepted++;
+	}
+	else
+	{
+		engine->diagnostics.position_nis_rejected++;
+	}
+	engine->checkpoint = revised;
+	engine->checkpoint_valid = true;
+	history_drop_through(engine, measurement->timestamp_ticks);
+	for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
+	{
+		if(!runtime_apply_imu(engine, &revised, history_at(engine, i), false))
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+		replayed++;
+	}
+	engine->current = revised;
+	engine->diagnostics.replay_count++;
+	engine->diagnostics.replayed_imu_count += replayed;
+	if(result_out != NULL)
+	{
+		*result_out = result;
+	}
+	if(nis_out != NULL)
+	{
+		*nis_out = nis;
+	}
+	return true;
+}
+
+static bool pending_insert(fusion_engine_t* engine,
+	                       const fusion_position_measurement_t* measurement)
+{
+	if(engine->pending_position_count >= FUSION_POSITION_PENDING_CAPACITY)
+	{
+		engine->diagnostics.position_pending_overflow++;
+		return false;
+	}
+	unsigned int index = engine->pending_position_count;
+	while((index != 0u) &&
+	      (engine->pending_positions[index - 1u].timestamp_ticks >
+	       measurement->timestamp_ticks))
+	{
+		engine->pending_positions[index] = engine->pending_positions[index - 1u];
+		index--;
+	}
+	engine->pending_positions[index] = *measurement;
+	engine->pending_position_count++;
+	return true;
+}
+
+static void process_ready_pending_positions(fusion_engine_t* engine)
+{
+	while((engine->pending_position_count != 0u) &&
+	      (engine->pending_positions[0].timestamp_ticks <=
+	       engine->current.filter_timestamp_ticks))
+	{
+		fusion_position_measurement_t measurement = engine->pending_positions[0];
+		for(unsigned int i = 1u ; i < engine->pending_position_count ; i++)
+		{
+			engine->pending_positions[i - 1u] = engine->pending_positions[i];
+		}
+		engine->pending_position_count--;
+		(void)process_position_now(engine, &measurement, NULL, NULL);
+		if(engine->state == FUSION_ENGINE_FAULT)
+		{
+			return;
+		}
+	}
+}
+
+bool fusion_engine_push_imu(fusion_engine_t* engine,
+	                        const fusion_imu_measurement_t* measurement)
+{
+	if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
+	   !imu_valid(measurement))
+	{
+		if(engine != NULL)
+		{
+			engine->diagnostics.imu_rejected++;
+		}
+		return false;
+	}
+	engine->diagnostics.imu_received++;
+	if(engine->state == FUSION_ENGINE_CALIBRATING)
+	{
+		bool ok = process_calibration_imu(engine, measurement);
+		if(!ok)
+		{
+			engine->diagnostics.imu_rejected++;
+		}
+		return ok;
+	}
+	if(!runtime_apply_imu(engine, &engine->current, measurement, true))
+	{
+		engine->diagnostics.imu_rejected++;
+		return false;
+	}
+	history_push(engine, measurement);
+	if(engine->state == FUSION_ENGINE_FAULT)
+	{
+		return false;
+	}
+	process_ready_pending_positions(engine);
+	return engine->state == FUSION_ENGINE_RUNNING;
+}
+
+bool fusion_engine_push_position(
+	fusion_engine_t* engine,
+	const fusion_position_measurement_t* measurement,
+	eskf_update_result_t* update_result_out,
+	double* nis_out)
+{
+	if(update_result_out != NULL)
+	{
+		*update_result_out = ESKF_UPDATE_INVALID;
+	}
+	if(nis_out != NULL)
+	{
+		*nis_out = NAN;
+	}
+	if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
+	   !position_valid(measurement))
+	{
+		return false;
+	}
+	engine->diagnostics.position_received++;
+	if(!measurement->source_valid)
+	{
+		engine->diagnostics.position_source_rejected++;
+		return false;
+	}
+	if(engine->state == FUSION_ENGINE_CALIBRATING)
+	{
+		engine->initial_position = *measurement;
+		engine->have_initial_position = true;
+		return true;
+	}
+	if(measurement->timestamp_ticks > engine->current.filter_timestamp_ticks)
+	{
+		return pending_insert(engine, measurement);
+	}
+	return process_position_now(engine, measurement, update_result_out, nis_out);
+}
+
+bool fusion_engine_get_runtime(const fusion_engine_t* engine,
+	                           fusion_runtime_t* runtime_out)
+{
+	if((engine == NULL) || (runtime_out == NULL) ||
+	   (engine->state != FUSION_ENGINE_RUNNING))
+	{
+		return false;
+	}
+	*runtime_out = engine->current;
+	return true;
+}
+
+const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
+	const fusion_engine_t* engine)
+{
+	return (engine != NULL) ? &engine->diagnostics : NULL;
+}

soc/software/gps_system/src/fusion_gps_adapter.c

@@ -0,0 +1,254 @@
+#include "fusion_gps_adapter.h"
+
+#include <float.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define WGS84_SEMI_MAJOR_M 6378137.0
+#define WGS84_FLATTENING (1.0 / 298.257223563)
+#define FUSION_GPS_PI 3.14159265358979323846264338327950288
+
+static double radians(double degrees)
+{
+	return degrees * (FUSION_GPS_PI / 180.0);
+}
+
+static double degrees(double radians_value)
+{
+	return radians_value * (180.0 / FUSION_GPS_PI);
+}
+
+static bool llh_valid(double latitude_deg, double longitude_deg, double height_m)
+{
+	return isfinite(latitude_deg) && isfinite(longitude_deg) && isfinite(height_m) &&
+	       (latitude_deg >= -90.0) && (latitude_deg <= 90.0) &&
+	       (longitude_deg >= -180.0) && (longitude_deg <= 180.0) &&
+	       (height_m >= -10000.0) && (height_m <= 100000.0);
+}
+
+static void geodetic_to_ecef(double latitude_deg, double longitude_deg,
+	                         double height_m, double ecef_m[3])
+{
+	double latitude = radians(latitude_deg);
+	double longitude = radians(longitude_deg);
+	double flattening = WGS84_FLATTENING;
+	double eccentricity_squared = flattening * (2.0 - flattening);
+	double sin_latitude = sin(latitude);
+	double cos_latitude = cos(latitude);
+	double prime_vertical = WGS84_SEMI_MAJOR_M /
+		                    sqrt(1.0 - eccentricity_squared *
+		                         sin_latitude * sin_latitude);
+	ecef_m[0] = (prime_vertical + height_m) * cos_latitude * cos(longitude);
+	ecef_m[1] = (prime_vertical + height_m) * cos_latitude * sin(longitude);
+	ecef_m[2] = (prime_vertical * (1.0 - eccentricity_squared) + height_m) *
+	            sin_latitude;
+}
+
+static void configure_origin(fusion_gps_adapter_t* adapter,
+	                         const fusion_gps_measurement_t* gps)
+{
+	adapter->origin_latitude_deg = gps->latitude_deg;
+	adapter->origin_longitude_deg = gps->longitude_deg;
+	adapter->origin_height_m = gps->height_m;
+	geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m,
+	                 adapter->origin_ecef_m);
+	double latitude = radians(gps->latitude_deg);
+	double longitude = radians(gps->longitude_deg);
+	double sin_latitude = sin(latitude);
+	double cos_latitude = cos(latitude);
+	double sin_longitude = sin(longitude);
+	double cos_longitude = cos(longitude);
+	adapter->ecef_to_enu[0][0] = -sin_longitude;
+	adapter->ecef_to_enu[0][1] = cos_longitude;
+	adapter->ecef_to_enu[0][2] = 0.0;
+	adapter->ecef_to_enu[1][0] = -sin_latitude * cos_longitude;
+	adapter->ecef_to_enu[1][1] = -sin_latitude * sin_longitude;
+	adapter->ecef_to_enu[1][2] = cos_latitude;
+	adapter->ecef_to_enu[2][0] = cos_latitude * cos_longitude;
+	adapter->ecef_to_enu[2][1] = cos_latitude * sin_longitude;
+	adapter->ecef_to_enu[2][2] = sin_latitude;
+	adapter->origin_valid = true;
+}
+
+void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->minimum_horizontal_std_m = 0.50;
+	config->minimum_vertical_std_m = 1.00;
+	config->maximum_position_std_m = 1000.0;
+}
+
+bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
+	                         const fusion_gps_adapter_config_t* config)
+{
+	if((adapter == NULL) || (config == NULL) ||
+	   !(config->minimum_horizontal_std_m > 0.0) ||
+	   !(config->minimum_vertical_std_m > 0.0) ||
+	   !(config->maximum_position_std_m >= config->minimum_vertical_std_m))
+	{
+		return false;
+	}
+	memset(adapter, 0, sizeof(*adapter));
+	adapter->config = *config;
+	return true;
+}
+
+static bool sanitize_covariance(const fusion_gps_adapter_t* adapter,
+	                            const double input[3][3], double output[9])
+{
+	double variance_floor[3] = {
+		adapter->config.minimum_horizontal_std_m *
+			adapter->config.minimum_horizontal_std_m,
+		adapter->config.minimum_horizontal_std_m *
+			adapter->config.minimum_horizontal_std_m,
+		adapter->config.minimum_vertical_std_m *
+			adapter->config.minimum_vertical_std_m
+	};
+	double variance_ceiling = adapter->config.maximum_position_std_m *
+	                          adapter->config.maximum_position_std_m;
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		double variance = input[axis][axis];
+		if(!isfinite(variance) || (variance <= 0.0))
+		{
+			return false;
+		}
+		output[axis * 3u + axis] = fmin(fmax(variance, variance_floor[axis]),
+		                                      variance_ceiling);
+	}
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = row + 1u ; col < 3u ; col++)
+		{
+			double covariance = 0.5 * (input[row][col] + input[col][row]);
+			if(!isfinite(covariance))
+			{
+				return false;
+			}
+			double limit = 0.95 * sqrt(output[row * 3u + row] *
+			                           output[col * 3u + col]);
+			covariance = fmin(fmax(covariance, -limit), limit);
+			output[row * 3u + col] = covariance;
+			output[col * 3u + row] = covariance;
+		}
+	}
+	/* Sylvester criterion: the sanitized symmetric covariance must be PD. */
+	double minor2 = output[0] * output[4] - output[1] * output[3];
+	double determinant =
+		output[0] * (output[4] * output[8] - output[5] * output[7]) -
+		output[1] * (output[3] * output[8] - output[5] * output[6]) +
+		output[2] * (output[3] * output[7] - output[4] * output[6]);
+	return (output[0] > 0.0) && (minor2 > DBL_EPSILON) &&
+	       (determinant > DBL_EPSILON) && isfinite(determinant);
+}
+
+bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
+	                            const fusion_gps_measurement_t* gps,
+	                            fusion_position_measurement_t* position_out)
+{
+	if((adapter == NULL) || (gps == NULL) || (position_out == NULL) ||
+	   !llh_valid(gps->latitude_deg, gps->longitude_deg, gps->height_m))
+	{
+		return false;
+	}
+	memset(position_out, 0, sizeof(*position_out));
+	position_out->timestamp_ticks = gps->timestamp_ticks;
+	position_out->source_valid = gps->solution_valid;
+	position_out->source_quality =
+		(isfinite(gps->chi_square_ratio) && (gps->chi_square_ratio >= 0.0))
+		? 100.0 / (1.0 + gps->chi_square_ratio) : 0.0;
+	if(!sanitize_covariance(adapter, gps->covariance_enu_m2,
+	                        position_out->covariance_enu_m2))
+	{
+		return false;
+	}
+	if(!adapter->origin_valid)
+	{
+		if(!gps->solution_valid)
+		{
+			return false;
+		}
+		configure_origin(adapter, gps);
+	}
+	double ecef_m[3];
+	double delta_ecef_m[3];
+	geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m, ecef_m);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		delta_ecef_m[axis] = ecef_m[axis] - adapter->origin_ecef_m[axis];
+	}
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0 ; col < 3u ; col++)
+		{
+			position_out->position_enu_m[row] +=
+				adapter->ecef_to_enu[row][col] * delta_ecef_m[col];
+		}
+	}
+	return isfinite(position_out->position_enu_m[0]) &&
+	       isfinite(position_out->position_enu_m[1]) &&
+	       isfinite(position_out->position_enu_m[2]);
+}
+
+bool fusion_gps_adapter_local_to_geodetic(
+	const fusion_gps_adapter_t* adapter,
+	const double position_enu_m[3],
+	double* latitude_deg_out,
+	double* longitude_deg_out,
+	double* height_m_out)
+{
+	if((adapter == NULL) || !adapter->origin_valid || (position_enu_m == NULL) ||
+	   (latitude_deg_out == NULL) || (longitude_deg_out == NULL) ||
+	   (height_m_out == NULL))
+	{
+		return false;
+	}
+	double ecef_m[3];
+	for(unsigned int row = 0 ; row < 3u ; row++)
+	{
+		ecef_m[row] = adapter->origin_ecef_m[row];
+		for(unsigned int axis = 0 ; axis < 3u ; axis++)
+		{
+			/* R^-1 = R^T for the orthonormal ECEF-to-ENU transform. */
+			ecef_m[row] += adapter->ecef_to_enu[axis][row] * position_enu_m[axis];
+		}
+	}
+	double flattening = WGS84_FLATTENING;
+	double eccentricity_squared = flattening * (2.0 - flattening);
+	double longitude = atan2(ecef_m[1], ecef_m[0]);
+	double horizontal = hypot(ecef_m[0], ecef_m[1]);
+	double latitude = atan2(ecef_m[2], horizontal * (1.0 - eccentricity_squared));
+	double height = 0.0;
+	for(unsigned int iteration = 0 ; iteration < 12u ; iteration++)
+	{
+		double sin_latitude = sin(latitude);
+		double prime_vertical = WGS84_SEMI_MAJOR_M /
+			sqrt(1.0 - eccentricity_squared * sin_latitude * sin_latitude);
+		double cos_latitude = cos(latitude);
+		if(fabs(cos_latitude) < 1.0e-12)
+		{
+			height = fabs(ecef_m[2]) - prime_vertical * (1.0 - eccentricity_squared);
+		}
+		else
+		{
+			height = horizontal / cos_latitude - prime_vertical;
+		}
+		double next = atan2(ecef_m[2],
+			horizontal * (1.0 - eccentricity_squared *
+			prime_vertical / (prime_vertical + height)));
+		if(fabs(next - latitude) < 1.0e-13)
+		{
+			latitude = next;
+			break;
+		}
+		latitude = next;
+	}
+	*latitude_deg_out = degrees(latitude);
+	*longitude_deg_out = degrees(longitude);
+	*height_m_out = height;
+	return llh_valid(*latitude_deg_out, *longitude_deg_out, *height_m_out);
+}

soc/software/gps_system/src/fusion_motion.c

@@ -0,0 +1,217 @@
+#include "fusion_motion.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define FUSION_MOTION_PI 3.14159265358979323846264338327950288
+
+static double norm3(const double vector[3])
+{
+	return sqrt(vector[0] * vector[0] + vector[1] * vector[1] +
+	            vector[2] * vector[2]);
+}
+
+static bool finite3(const double vector[3])
+{
+	return (vector != NULL) && isfinite(vector[0]) &&
+	       isfinite(vector[1]) && isfinite(vector[2]);
+}
+
+static void transition_to(fusion_motion_detector_t* detector,
+	                      fusion_motion_state_t state)
+{
+	if(detector->state != state)
+	{
+		detector->state = state;
+		detector->transition_count++;
+	}
+	if(state != FUSION_MOTION_STATIONARY_CANDIDATE)
+	{
+		detector->quiet_time_s = 0.0;
+	}
+	if(state != FUSION_MOTION_STATIONARY)
+	{
+		detector->weak_motion_count = 0u;
+	}
+}
+
+void fusion_motion_default_config(fusion_motion_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->stationary_accel_limit_mps2 = 0.15;
+	config->stationary_gyro_limit_rps = 1.0 * FUSION_MOTION_PI / 180.0;
+	config->moving_accel_threshold_mps2 = 0.40;
+	config->moving_gyro_threshold_rps = 3.0 * FUSION_MOTION_PI / 180.0;
+	config->moving_jerk_threshold_mps3 = 4.0;
+	config->jerk_filter_time_constant_s = 0.10;
+	config->stationary_speed_limit_mps = 0.40;
+	config->moving_speed_threshold_mps = 0.50;
+	config->stationary_dwell_s = 1.0;
+	config->weak_motion_samples = 3u;
+}
+
+void fusion_motion_init(fusion_motion_detector_t* detector,
+	                    bool bootstrap_stationary)
+{
+	if(detector == NULL)
+	{
+		return;
+	}
+	memset(detector, 0, sizeof(*detector));
+	detector->state = bootstrap_stationary
+	                ? FUSION_MOTION_STATIONARY
+	                : FUSION_MOTION_BOOTSTRAP;
+}
+
+fusion_motion_state_t fusion_motion_update(
+	fusion_motion_detector_t* detector,
+	const fusion_motion_config_t* config,
+	const double linear_accel_nav_mps2[3],
+	const double unbiased_gyro_rps[3],
+	double speed_mps,
+	fusion_gnss_motion_evidence_t gnss_evidence,
+	double dt_s)
+{
+	if((detector == NULL) || (config == NULL) ||
+	   !finite3(linear_accel_nav_mps2) || !finite3(unbiased_gyro_rps) ||
+	   !isfinite(speed_mps) || (speed_mps < 0.0) ||
+	   !isfinite(dt_s) || (dt_s <= 0.0))
+	{
+		return (detector != NULL) ? detector->state : FUSION_MOTION_BOOTSTRAP;
+	}
+
+	double accel_norm = norm3(linear_accel_nav_mps2);
+	double gyro_norm = norm3(unbiased_gyro_rps);
+	double jerk_norm = 0.0;
+	if(!detector->have_previous_accel)
+	{
+		memcpy(detector->filtered_linear_accel_mps2, linear_accel_nav_mps2,
+		       sizeof(detector->filtered_linear_accel_mps2));
+		memcpy(detector->previous_filtered_linear_accel_mps2,
+		       detector->filtered_linear_accel_mps2,
+		       sizeof(detector->previous_filtered_linear_accel_mps2));
+		detector->have_previous_accel = true;
+	}
+	else
+	{
+		double time_constant_s = fmax(0.0, config->jerk_filter_time_constant_s);
+		double alpha = (time_constant_s > 0.0)
+		             ? dt_s / (time_constant_s + dt_s)
+		             : 1.0;
+		double delta[3];
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			detector->previous_filtered_linear_accel_mps2[axis] =
+				detector->filtered_linear_accel_mps2[axis];
+			detector->filtered_linear_accel_mps2[axis] += alpha *
+				(linear_accel_nav_mps2[axis] -
+				 detector->filtered_linear_accel_mps2[axis]);
+			delta[axis] = detector->filtered_linear_accel_mps2[axis] -
+			              detector->previous_filtered_linear_accel_mps2[axis];
+		}
+		jerk_norm = norm3(delta) / dt_s;
+	}
+	detector->last_linear_accel_norm_mps2 = accel_norm;
+	detector->last_gyro_norm_rps = gyro_norm;
+	detector->last_jerk_norm_mps3 = jerk_norm;
+	detector->last_speed_mps = speed_mps;
+
+	bool gnss_moving = gnss_evidence == FUSION_GNSS_MOTION_MOVING;
+	bool strong_motion = gnss_moving ||
+	                     (accel_norm >= config->moving_accel_threshold_mps2) ||
+	                     (gyro_norm >= config->moving_gyro_threshold_rps) ||
+	                     (detector->have_previous_accel &&
+	                      (jerk_norm >= config->moving_jerk_threshold_mps3)) ||
+	                     (speed_mps >= config->moving_speed_threshold_mps);
+	bool quiet = (accel_norm <= config->stationary_accel_limit_mps2) &&
+	             (gyro_norm <= config->stationary_gyro_limit_rps);
+	bool stationary_candidate = quiet && !gnss_moving &&
+	                            (speed_mps <= config->stationary_speed_limit_mps);
+	bool weak_motion = !quiet;
+
+	switch(detector->state)
+	{
+		case FUSION_MOTION_STATIONARY:
+			if(strong_motion)
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			else if(weak_motion)
+			{
+				detector->weak_motion_count++;
+				if(detector->weak_motion_count >= config->weak_motion_samples)
+				{
+					transition_to(detector, FUSION_MOTION_MOVING);
+				}
+			}
+			else
+			{
+				detector->weak_motion_count = 0u;
+			}
+			break;
+
+		case FUSION_MOTION_BOOTSTRAP:
+		case FUSION_MOTION_MOVING:
+			if(strong_motion)
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			else if(stationary_candidate)
+			{
+				transition_to(detector, FUSION_MOTION_STATIONARY_CANDIDATE);
+				detector->quiet_time_s = dt_s;
+			}
+			else
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			break;
+
+		case FUSION_MOTION_STATIONARY_CANDIDATE:
+			if(strong_motion || !stationary_candidate)
+			{
+				transition_to(detector, FUSION_MOTION_MOVING);
+			}
+			else
+			{
+				detector->quiet_time_s += dt_s;
+				if(detector->quiet_time_s >= config->stationary_dwell_s)
+				{
+					transition_to(detector, FUSION_MOTION_STATIONARY);
+				}
+			}
+			break;
+
+		default:
+			transition_to(detector, FUSION_MOTION_BOOTSTRAP);
+			break;
+	}
+	return detector->state;
+}
+
+bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector)
+{
+	return (detector != NULL) &&
+	       (detector->state == FUSION_MOTION_STATIONARY);
+}
+
+const char* fusion_motion_state_name(fusion_motion_state_t state)
+{
+	switch(state)
+	{
+		case FUSION_MOTION_BOOTSTRAP:
+			return "bootstrap";
+		case FUSION_MOTION_STATIONARY:
+			return "stationary";
+		case FUSION_MOTION_MOVING:
+			return "moving";
+		case FUSION_MOTION_STATIONARY_CANDIDATE:
+			return "stationary_candidate";
+		default:
+			return "unknown";
+	}
+}

soc/software/gps_system/src/gps_fusion_publisher.c

@@ -0,0 +1,30 @@
+#include "gps_fusion_publisher.h"
+
+#include "navigation_fusion.h"
+
+#include <stddef.h>
+#include <string.h>
+
+bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
+	                             uint64_t measurement_timestamp_ticks)
+{
+	if((solution == NULL) || (measurement_timestamp_ticks == 0u))
+	{
+		return false;
+	}
+	fusion_gps_measurement_t measurement;
+	memset(&measurement, 0, sizeof(measurement));
+	measurement.timestamp_ticks = measurement_timestamp_ticks;
+	measurement.latitude_deg = solution->latitude_deg;
+	measurement.longitude_deg = solution->longitude_deg;
+	measurement.height_m = solution->height_m;
+	memcpy(measurement.covariance_enu_m2, solution->covariance_enu_m2,
+	       sizeof(measurement.covariance_enu_m2));
+	measurement.hacc_m = solution->hacc_m;
+	measurement.vacc_m = solution->vacc_m;
+	measurement.residual_rms_m = solution->residual_rms_m;
+	measurement.chi_square_ratio = solution->chi_square_ratio;
+	measurement.used_satellites = solution->used_satellite_count;
+	measurement.solution_valid = solution->solution_valid;
+	return navigation_fusion_publish_gps(&measurement);
+}

soc/software/gps_system/src/gps_interrupt_handlers.c

@@ -3,6 +3,7 @@
 #include "Navigation.h"
 #include "gps_pps.h"
 #include "gps_time_anchor.h"
+#include "tdk_icm42688.h"
 #include <bsp/irq.h>
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -25,6 +26,7 @@ typedef struct {
 	double corrected_abs_sample;
 	uint32_t nav_solve_timer;
 	uint32_t nav_solve_event_count;
+	uint64_t local_timestamp_ticks;
 	uint64_t pps_counter;
 	uint32_t pps_capture_sequence;
 	bool pps_capture_valid;
@@ -393,6 +395,12 @@ void gps_interrupt_init(void)
 
 void gps_dispatch_irq(unsigned int irq_id)
 {
+	if(irq_id == BSP_PMOD_IRQ)
+	{
+		tdk_icm42688_irq_handler();
+		return;
+	}
+
 	unsigned char acq_engine_id;
 	if(gps_irq_to_acq_engine(irq_id, &acq_engine_id))
 	{
@@ -633,6 +641,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
 		return;
 	}
 
+	uint64_t local_timestamp_ticks = bsp_read64(BSP_CLINT_MTIME);
 	uint32_t status_reg = trck_ip->STATUS_REG;
 	if((status_reg & (1u << GPS_TRCK_INTR_PEND_POS)) == 0u)
 	{
@@ -653,6 +662,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
 	event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase);
 	event.nav_solve_timer = trck_ip->NAV_SOLVE_TIMER;
 	event.nav_solve_event_count = (status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) ? 1u : 0u;
+	event.local_timestamp_ticks = local_timestamp_ticks;
 	if(event.nav_solve_event_count != 0u)
 	{
 		event.pps_capture_valid = gps_pps_read_capture(trck_ip_no,
@@ -1032,6 +1042,7 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 		gps_nav_solve_period_pending = true;
 		node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
 		node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
+		node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
 		if(event->pps_capture_valid)
 		{
 			node->gps_last_nav_solve_pps_counter = event->pps_counter;

soc/software/gps_system/src/gps_ip.c

@@ -9,6 +9,7 @@
 #include "linked_list.h"
 #include "gps_interrupt_handlers.h"
 #include "gps_pps.h"
+#include "gps_fusion_publisher.h"
 #include "defines.h"
 #include <bsp/irq.h>
 
@@ -766,6 +767,7 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS
 	                                         uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
 	                                         uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
 	                                         uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
+	                                         uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
 	                                         unsigned int satellite_count)
 {
 	for(unsigned int i = 0 ; i < satellite_count ; i++)
@@ -774,9 +776,34 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS
 		snapshot_sequences[i] = (node != NULL) ? node->gps_nav_solve_sequence : 0u;
 		snapshot_pps_counters[i] = (node != NULL) ? node->gps_last_nav_solve_pps_counter : 0u;
 		snapshot_pps_sequences[i] = (node != NULL) ? node->gps_last_pps_capture_sequence : 0u;
+		snapshot_local_timestamps[i] =
+			(node != NULL) ? node->gps_last_nav_solve_local_ticks : 0u;
 	}
 }
 
+static uint64_t gps_nav_median_local_timestamp(
+	const uint64_t timestamps[GPS_TRCK_IP_COUNT], unsigned int count)
+{
+	uint64_t sorted[GPS_TRCK_IP_COUNT];
+	unsigned int valid_count = 0u;
+	for(unsigned int i = 0 ; (i < count) && (i < GPS_TRCK_IP_COUNT) ; i++)
+	{
+		if(timestamps[i] == 0u)
+		{
+			continue;
+		}
+		unsigned int insert = valid_count;
+		while((insert != 0u) && (sorted[insert - 1u] > timestamps[i]))
+		{
+			sorted[insert] = sorted[insert - 1u];
+			insert--;
+		}
+		sorted[insert] = timestamps[i];
+		valid_count++;
+	}
+	return (valid_count != 0u) ? sorted[valid_count / 2u] : 0u;
+}
+
 static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
 	                                         const uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
 	                                         unsigned int satellite_count)
@@ -1069,6 +1096,7 @@ void calculate_position()
 	uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
 	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
+	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
 	unsigned int satellite_count = 0;
 	bool run_nav = false;
 	bool initial_run = false;
@@ -1115,6 +1143,7 @@ void calculate_position()
 			{
 				gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
 				                               snapshot_pps_counters, snapshot_pps_sequences,
+				                               snapshot_local_timestamps,
 				                               satellite_count);
 			}
 		}
@@ -1157,6 +1186,14 @@ void calculate_position()
 		uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
 		if(nav_ok)
 		{
+			uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
+				snapshot_local_timestamps, satellite_count);
+			if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp))
+			{
+				GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+				        "fusion GPS publish skipped timestamp=%lu\r\n",
+				        (unsigned long)measurement_timestamp);
+			}
 			uint64_t consume_irq_state = bsp_irq_save();
 			gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
 			bsp_irq_restore(consume_irq_state);

soc/software/gps_system/src/linked_list.c

@@ -70,6 +70,7 @@ bool insert_element(volatile gps_ephem_ll_t** node,
 	new_node->gps_nav_solve_consumed_sequence = 0u;
 	new_node->gps_last_nav_solve_abs_sample = 0u;
 	new_node->gps_last_nav_solve_corrected_abs_sample = 0.0;
+	new_node->gps_last_nav_solve_local_ticks = 0u;
 	new_node->gps_last_nav_solve_pps_counter = 0u;
 	new_node->gps_last_pps_capture_sequence = 0u;
 	new_node->gps_last_measurement_tick = 0u;

soc/software/gps_system/src/navigation_fusion.c

@@ -0,0 +1,108 @@
+#include "navigation_fusion.h"
+
+#include "fusion_bus.h"
+#include "fusion_gps_adapter.h"
+#include "tdk_icm42688.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static fusion_bus_t measurement_bus;
+static fusion_gps_adapter_t gps_adapter;
+static fusion_engine_t fusion_engine;
+static bool fusion_initialized;
+
+bool navigation_fusion_init(uint64_t local_timebase_hz)
+{
+	fusion_engine_config_t engine_config;
+	fusion_gps_adapter_config_t adapter_config;
+	if(local_timebase_hz == 0u)
+	{
+		return false;
+	}
+	fusion_bus_init(&measurement_bus);
+	fusion_engine_default_config(&engine_config, local_timebase_hz);
+	fusion_gps_adapter_default_config(&adapter_config);
+	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
+	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config))
+	{
+		return false;
+	}
+	fusion_initialized = true;
+	return true;
+}
+
+bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement)
+{
+	return fusion_initialized &&
+	       fusion_bus_publish_gps(&measurement_bus, measurement);
+}
+
+unsigned int navigation_fusion_service(void)
+{
+	if(!fusion_initialized)
+	{
+		return 0u;
+	}
+	unsigned int processed = 0u;
+	fusion_imu_measurement_t imu;
+	while(tdk_icm42688_take_measurement(&imu))
+	{
+		(void)fusion_engine_push_imu(&fusion_engine, &imu);
+		processed++;
+	}
+
+	/* GPS is intentionally consumed after all currently queued IMU samples.
+	 * A delayed WLS result is applied at its own timestamp by fixed-lag replay. */
+	fusion_gps_measurement_t gps;
+	while(fusion_bus_take_gps(&measurement_bus, &gps))
+	{
+		fusion_position_measurement_t position;
+		if(fusion_gps_adapter_convert(&gps_adapter, &gps, &position))
+		{
+			(void)fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
+		}
+		processed++;
+	}
+	return processed;
+}
+
+bool navigation_fusion_get_output(navigation_fusion_output_t* output)
+{
+	if((output == NULL) || !fusion_initialized ||
+	   (fusion_engine.state != FUSION_ENGINE_RUNNING))
+	{
+		return false;
+	}
+	memset(output, 0, sizeof(*output));
+	const fusion_runtime_t* runtime = &fusion_engine.current;
+	output->timestamp_ticks = runtime->filter_timestamp_ticks;
+	memcpy(output->position_enu_m, runtime->filter.position_m,
+	       sizeof(output->position_enu_m));
+	memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
+	       sizeof(output->velocity_enu_mps));
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		output->position_std_m[axis] =
+			sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
+	}
+	output->speed_mps = eskf_speed_mps(&runtime->filter);
+	output->last_position_nis = runtime->filter.last_position_nis;
+	output->motion_state = runtime->motion.state;
+	output->gps_accepted = fusion_engine.diagnostics.position_accepted;
+	output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
+	if(!fusion_gps_adapter_local_to_geodetic(
+		&gps_adapter, runtime->filter.position_m,
+		&output->latitude_deg, &output->longitude_deg, &output->height_m))
+	{
+		return false;
+	}
+	output->valid = true;
+	return true;
+}
+
+const fusion_engine_t* navigation_fusion_engine(void)
+{
+	return fusion_initialized ? &fusion_engine : NULL;
+}

soc/software/gps_system/src/tdk_icm42688.c

@@ -0,0 +1,393 @@
+#include "tdk_icm42688.h"
+
+#include <bsp/mmio.h>
+#include <bsp/plic.h>
+#include <bsp/pmod_spi.h>
+#include <bsp/soc.h>
+#include <limits.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define ICM_DEVICE_CONFIG       0x11u
+#define ICM_INT_CONFIG          0x14u
+#define ICM_TEMP_DATA1          0x1du
+#define ICM_INT_STATUS          0x2du
+#define ICM_PWR_MGMT0           0x4eu
+#define ICM_GYRO_CONFIG0        0x4fu
+#define ICM_ACCEL_CONFIG0       0x50u
+#define ICM_GYRO_CONFIG1        0x51u
+#define ICM_GYRO_ACCEL_CONFIG0  0x52u
+#define ICM_ACCEL_CONFIG1       0x53u
+#define ICM_INT_CONFIG0         0x63u
+#define ICM_INT_CONFIG1         0x64u
+#define ICM_INT_SOURCE0         0x65u
+#define ICM_WHO_AM_I            0x75u
+
+#define ICM_SPI_READ            0x80u
+#define ICM_SPI_DUMMY           0xffu
+#define ICM_DATA_READY          0x08u
+#define ICM42688_WHO_AM_I       0x47u
+#define ICM_PWR_LOW_NOISE       0x0fu
+#define ICM_INT1_LATCHED_HIGH_PUSH_PULL 0x07u
+#define ICM_UI_DRDY_INT1_EN     0x08u
+
+#define TDK_GRAVITY_MPS2 9.80665
+#define TDK_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
+
+#if (TDK_IMU_RAW_QUEUE_CAPACITY < 2u)
+#error "TDK_IMU_RAW_QUEUE_CAPACITY must be at least 2"
+#endif
+
+static tdk_icm42688_config_t active_config;
+static tdk_icm42688_raw_sample_t sample_queue[TDK_IMU_RAW_QUEUE_CAPACITY];
+static volatile unsigned int sample_head;
+static volatile unsigned int sample_tail;
+static volatile tdk_icm42688_diagnostics_t diagnostics;
+
+static uint64_t time_now(void)
+{
+	return bsp_read64(BSP_CLINT_MTIME);
+}
+
+static void delay_us(uint32_t microseconds)
+{
+	uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * microseconds + 999999u) /
+	                 1000000u;
+	uint64_t start = time_now();
+	while((time_now() - start) < ticks)
+	{
+	}
+}
+
+static void delay_ms(uint32_t milliseconds)
+{
+	uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * milliseconds + 999u) /
+	                 1000u;
+	uint64_t start = time_now();
+	while((time_now() - start) < ticks)
+	{
+	}
+}
+
+static void write_register(uint8_t reg, uint8_t value)
+{
+	bsp_pmod_spi_write_reg8(reg & (uint8_t)~ICM_SPI_READ, value);
+}
+
+static uint8_t read_register(uint8_t reg)
+{
+	return bsp_pmod_spi_read_reg8(reg | ICM_SPI_READ, ICM_SPI_DUMMY);
+}
+
+static int16_t big_endian_i16(const uint8_t* bytes)
+{
+	return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
+}
+
+static bool odr_register_value(uint32_t odr_hz, uint8_t* register_value)
+{
+	if(register_value == NULL)
+	{
+		return false;
+	}
+	switch(odr_hz)
+	{
+		case 1000u: *register_value = 0x06u; return true;
+		case 500u:  *register_value = 0x0fu; return true;
+		case 200u:  *register_value = 0x07u; return true;
+		case 100u:  *register_value = 0x08u; return true;
+		case 50u:   *register_value = 0x09u; return true;
+		case 25u:   *register_value = 0x0au; return true;
+		default: return false;
+	}
+}
+
+static bool accel_full_scale_bits(tdk_accel_full_scale_t full_scale,
+	                              uint8_t* bits)
+{
+	if(bits == NULL)
+	{
+		return false;
+	}
+	switch(full_scale)
+	{
+		case TDK_ACCEL_FS_16G: *bits = 0u; return true;
+		case TDK_ACCEL_FS_8G:  *bits = 1u; return true;
+		case TDK_ACCEL_FS_4G:  *bits = 2u; return true;
+		case TDK_ACCEL_FS_2G:  *bits = 3u; return true;
+		default: return false;
+	}
+}
+
+static bool gyro_full_scale_bits(tdk_gyro_full_scale_mdps_t full_scale,
+	                             uint8_t* bits)
+{
+	if(bits == NULL)
+	{
+		return false;
+	}
+	switch(full_scale)
+	{
+		case TDK_GYRO_FS_2000:   *bits = 0u; return true;
+		case TDK_GYRO_FS_1000:   *bits = 1u; return true;
+		case TDK_GYRO_FS_500:    *bits = 2u; return true;
+		case TDK_GYRO_FS_250:    *bits = 3u; return true;
+		case TDK_GYRO_FS_125:    *bits = 4u; return true;
+		case TDK_GYRO_FS_62D5:   *bits = 5u; return true;
+		case TDK_GYRO_FS_31D25:  *bits = 6u; return true;
+		case TDK_GYRO_FS_15D625: *bits = 7u; return true;
+		default: return false;
+	}
+}
+
+static unsigned int next_queue_index(unsigned int index)
+{
+	index++;
+	return (index == TDK_IMU_RAW_QUEUE_CAPACITY) ? 0u : index;
+}
+
+static bool raw_sample_valid(const tdk_icm42688_raw_sample_t* sample)
+{
+	if((sample == NULL) || (sample->temperature_raw == INT16_MIN))
+	{
+		return false;
+	}
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		if((sample->accel_raw[axis] == INT16_MIN) ||
+		   (sample->gyro_raw[axis] == INT16_MIN))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+static bool queue_push_isr(const tdk_icm42688_raw_sample_t* sample)
+{
+	unsigned int head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
+	unsigned int next = next_queue_index(head);
+	if(next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE))
+	{
+		__atomic_fetch_add(&diagnostics.queue_overflow_count, 1u, __ATOMIC_RELAXED);
+		return false;
+	}
+	sample_queue[head] = *sample;
+	__atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
+	return true;
+}
+
+static bool queue_pop(tdk_icm42688_raw_sample_t* sample)
+{
+	unsigned int tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
+	if(tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE))
+	{
+		return false;
+	}
+	*sample = sample_queue[tail];
+	__atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
+	return true;
+}
+
+void tdk_icm42688_default_config(tdk_icm42688_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	config->odr_hz = TDK_IMU_ODR_HZ;
+	/* Motion-capable defaults; stationary characterization may select narrower ranges. */
+	config->accel_full_scale_g = TDK_ACCEL_FS_4G;
+	config->gyro_full_scale_mdps = TDK_GYRO_FS_250;
+	config->ui_filter_config = 0x77u;
+}
+
+bool tdk_icm42688_init(const tdk_icm42688_config_t* config)
+{
+	uint8_t odr_bits;
+	uint8_t accel_bits;
+	uint8_t gyro_bits;
+	if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) ||
+	   !accel_full_scale_bits(config->accel_full_scale_g, &accel_bits) ||
+	   !gyro_full_scale_bits(config->gyro_full_scale_mdps, &gyro_bits))
+	{
+		return false;
+	}
+	memset((void*)&diagnostics, 0, sizeof(diagnostics));
+	sample_head = 0u;
+	sample_tail = 0u;
+	active_config = *config;
+
+	bsp_pmod_spi_init();
+	bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
+	bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u);
+	delay_us(1000u);
+	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))
+	{
+		return false;
+	}
+
+	write_register(ICM_INT_SOURCE0, 0x00u);
+	write_register(ICM_PWR_MGMT0, 0x00u);
+	delay_ms(1u);
+	write_register(ICM_GYRO_CONFIG0, (uint8_t)((gyro_bits << 5) | odr_bits));
+	write_register(ICM_ACCEL_CONFIG0, (uint8_t)((accel_bits << 5) | odr_bits));
+	write_register(ICM_GYRO_CONFIG1,
+	               (uint8_t)((read_register(ICM_GYRO_CONFIG1) & 0x10u) | 0xcau));
+	write_register(ICM_ACCEL_CONFIG1,
+	               (uint8_t)((read_register(ICM_ACCEL_CONFIG1) & 0xe1u) | 0x14u));
+	write_register(ICM_GYRO_ACCEL_CONFIG0, config->ui_filter_config);
+
+	/* Active-high, push-pull, latched INT1. Status read clears data-ready. */
+	write_register(ICM_INT_CONFIG, ICM_INT1_LATCHED_HIGH_PUSH_PULL);
+	write_register(ICM_INT_CONFIG0, 0x00u);
+	/* Datasheet requires clearing the reset default bit 4 for pin operation. */
+	write_register(ICM_INT_CONFIG1, 0x00u);
+	write_register(ICM_PWR_MGMT0, ICM_PWR_LOW_NOISE);
+	delay_ms(50u);
+
+	if((read_register(ICM_PWR_MGMT0) != ICM_PWR_LOW_NOISE) ||
+	   (read_register(ICM_GYRO_CONFIG0) != (uint8_t)((gyro_bits << 5) | odr_bits)) ||
+	   (read_register(ICM_ACCEL_CONFIG0) != (uint8_t)((accel_bits << 5) | odr_bits)) ||
+	   (read_register(ICM_GYRO_ACCEL_CONFIG0) != config->ui_filter_config) ||
+	   (read_register(ICM_INT_CONFIG) != ICM_INT1_LATCHED_HIGH_PUSH_PULL) ||
+	   (read_register(ICM_INT_CONFIG1) != 0x00u))
+	{
+		write_register(ICM_PWR_MGMT0, 0x00u);
+		return false;
+	}
+	(void)read_register(ICM_INT_STATUS);
+	diagnostics.initialized = true;
+	return true;
+}
+
+bool tdk_icm42688_start(void)
+{
+	if(!diagnostics.initialized || diagnostics.running)
+	{
+		return diagnostics.running;
+	}
+	(void)read_register(ICM_INT_STATUS);
+	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
+	write_register(ICM_INT_SOURCE0, ICM_UI_DRDY_INT1_EN);
+	if(read_register(ICM_INT_SOURCE0) != ICM_UI_DRDY_INT1_EN)
+	{
+		bsp_plic_disable(BSP_PMOD_IRQ);
+		return false;
+	}
+	(void)read_register(ICM_INT_STATUS);
+	diagnostics.running = true;
+	bsp_plic_enable(BSP_PMOD_IRQ);
+	return true;
+}
+
+void tdk_icm42688_stop(void)
+{
+	bsp_plic_disable(BSP_PMOD_IRQ);
+	write_register(ICM_INT_SOURCE0, 0x00u);
+	(void)read_register(ICM_INT_STATUS);
+	diagnostics.running = false;
+}
+
+void tdk_icm42688_irq_handler(void)
+{
+	uint64_t timestamp = time_now();
+	__atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED);
+	uint8_t status = read_register(ICM_INT_STATUS);
+	if(!diagnostics.running || ((status & ICM_DATA_READY) == 0u))
+	{
+		__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u, __ATOMIC_RELAXED);
+		return;
+	}
+	uint8_t bytes[14];
+	bsp_pmod_spi_read_regs(ICM_TEMP_DATA1 | ICM_SPI_READ, ICM_SPI_DUMMY,
+	                       bytes, sizeof(bytes));
+	tdk_icm42688_raw_sample_t sample;
+	sample.timestamp_ticks = timestamp;
+	sample.temperature_raw = big_endian_i16(&bytes[0]);
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		sample.accel_raw[axis] = big_endian_i16(&bytes[2u + 2u * axis]);
+		sample.gyro_raw[axis] = big_endian_i16(&bytes[8u + 2u * axis]);
+	}
+	sample.sequence = (uint32_t)(__atomic_load_n(&diagnostics.sample_count,
+	                                            __ATOMIC_RELAXED) + 1u);
+	if(!raw_sample_valid(&sample))
+	{
+		__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, __ATOMIC_RELAXED);
+		return;
+	}
+	uint64_t previous_timestamp =
+		__atomic_exchange_n(&diagnostics.last_timestamp_ticks, timestamp,
+		                    __ATOMIC_RELAXED);
+	if(previous_timestamp != 0u)
+	{
+		uint64_t expected_ticks = BSP_CLINT_TIMEBASE_HZ / active_config.odr_hz;
+		uint64_t elapsed = timestamp - previous_timestamp;
+		if((expected_ticks != 0u) && (elapsed > expected_ticks + expected_ticks / 2u))
+		{
+			uint64_t periods = (elapsed + expected_ticks / 2u) / expected_ticks;
+			if(periods > 1u)
+			{
+				__atomic_fetch_add(&diagnostics.estimated_missed_sample_count,
+				                   periods - 1u, __ATOMIC_RELAXED);
+			}
+		}
+	}
+	(void)queue_push_isr(&sample);
+	__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
+}
+
+bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out)
+{
+	if(measurement_out == NULL)
+	{
+		return false;
+	}
+	tdk_icm42688_raw_sample_t raw;
+	if(!queue_pop(&raw))
+	{
+		return false;
+	}
+	double accel_scale = (double)active_config.accel_full_scale_g *
+	                     TDK_GRAVITY_MPS2 / 32768.0;
+	double gyro_scale = ((double)active_config.gyro_full_scale_mdps / 1000.0) *
+	                    TDK_DEGREES_TO_RADIANS / 32768.0;
+	measurement_out->timestamp_ticks = raw.timestamp_ticks;
+	for(unsigned int axis = 0 ; axis < 3u ; axis++)
+	{
+		measurement_out->accel_mps2[axis] = (double)raw.accel_raw[axis] * accel_scale;
+		measurement_out->gyro_rps[axis] = (double)raw.gyro_raw[axis] * gyro_scale;
+	}
+	measurement_out->temperature_c = 25.0 + (double)raw.temperature_raw / 132.48;
+	measurement_out->sequence = raw.sequence;
+	return true;
+}
+
+void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out)
+{
+	if(diagnostics_out == NULL)
+	{
+		return;
+	}
+	diagnostics_out->irq_count =
+		__atomic_load_n(&diagnostics.irq_count, __ATOMIC_RELAXED);
+	diagnostics_out->sample_count =
+		__atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED);
+	diagnostics_out->invalid_sample_count =
+		__atomic_load_n(&diagnostics.invalid_sample_count, __ATOMIC_RELAXED);
+	diagnostics_out->spurious_irq_count =
+		__atomic_load_n(&diagnostics.spurious_irq_count, __ATOMIC_RELAXED);
+	diagnostics_out->queue_overflow_count =
+		__atomic_load_n(&diagnostics.queue_overflow_count, __ATOMIC_RELAXED);
+	diagnostics_out->estimated_missed_sample_count =
+		__atomic_load_n(&diagnostics.estimated_missed_sample_count, __ATOMIC_RELAXED);
+	diagnostics_out->last_timestamp_ticks =
+		__atomic_load_n(&diagnostics.last_timestamp_ticks, __ATOMIC_RELAXED);
+	diagnostics_out->who_am_i = diagnostics.who_am_i;
+	diagnostics_out->initialized = diagnostics.initialized;
+	diagnostics_out->running = diagnostics.running;
+}