Integrate asynchronous GPS IMU fusion

2026-07-16 23:49:43 +03:00
parent 38a96dd88f
commit 7cc3eb2a54
80 changed files with 3901 additions and 553 deletions
@@ -1,5 +1,12 @@
 TARGET := gps_system

+# Keep the standalone app build observable on the board.  The board and parent
+# software Makefiles already pass these values, but a direct `make` in this
+# directory previously compiled every GPS_LOG call out and produced a silent
+# UART image.
+GPS_DEBUG ?= 1
+GPS_DEBUG_LEVEL ?= 4
+
 # The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz.
 # Keep the requested 115200 baud physical rather than producing 57600 baud
 # with the common 100 MHz fallback divider.
@@ -9,55 +16,59 @@ BSP_UART_CLOCK_HZ ?= 50000000
 # Accepted values: auto, tdk, st (stm is an alias for st).
 IMU_SENSOR ?= auto
 ifeq ($(IMU_SENSOR),auto)
-IMU_DRIVER_SRCS := src/tdk_icm42688.c src/st_lsm6dsox.c
+IMU_DRIVER_SRCS := src/drivers/imu/tdk_icm42688.c src/drivers/imu/st_lsm6dsox.c
 IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_AUTO=1
 else ifeq ($(IMU_SENSOR),tdk)
-IMU_DRIVER_SRCS := src/tdk_icm42688.c
+IMU_DRIVER_SRCS := src/drivers/imu/tdk_icm42688.c
 IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_TDK=1
 else ifeq ($(IMU_SENSOR),st)
-IMU_DRIVER_SRCS := src/st_lsm6dsox.c
+IMU_DRIVER_SRCS := src/drivers/imu/st_lsm6dsox.c
 IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1
 else ifeq ($(IMU_SENSOR),stm)
-IMU_DRIVER_SRCS := src/st_lsm6dsox.c
+IMU_DRIVER_SRCS := src/drivers/imu/st_lsm6dsox.c
 IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1
 else
 $(error IMU_SENSOR must be auto, tdk, st or stm)
 endif

-# Zero means use the selected backend's default. The legacy TDK variable is
-# accepted so existing build commands remain valid.
+# A non-zero value is a sensor-independent target.  The common layer accepts
+# only a hardware ODR within 5%; thus 200 selects 200 Hz on TDK and the real
+# 208 Hz setting on LSM6DSOX.  Zero keeps the backend default.
 ifdef TDK_IMU_ODR_HZ
 IMU_ODR_HZ ?= $(TDK_IMU_ODR_HZ)
 else
-IMU_ODR_HZ ?= 0
+IMU_ODR_HZ ?= 50
 endif
 IMU_ACCEL_FS_G ?= 0
 IMU_GYRO_FS_DPS ?= 0
+GPS_ESKF_ENABLE ?= 1

 APP_SRCS := \
-	gps_test.c \
-	src/imu_calibration.c \
-	src/imu_sensor.c \
-	src/imu_sensor_profile.c \
+	gps_system.c \
+	src/gps/gps_receiver.c \
+	src/imu/imu_calibration.c \
+	src/imu/imu_sensor.c \
+	src/adapters/imu_fusion_profile.c \
+	src/adapters/gps_system_uart.c \
 	$(IMU_DRIVER_SRCS) \
-	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 \
-	src/gps_pps.c \
-	src/linked_list.c \
-	src/NavEquations.c \
-	src/matrixLib.c
+	src/fusion/eskf.c \
+	src/fusion/fusion_motion.c \
+	src/fusion/fusion_rest_detector.c \
+	src/fusion/fusion_engine.c \
+	src/fusion/fusion_gps_adapter.c \
+	src/fusion/navigation_fusion.c \
+	src/adapters/gps_fusion_publisher.c \
+	src/gps/gps_ip.c \
+	src/gps/gps_interrupt_handlers.c \
+	src/gps/gps_time_anchor.c \
+	src/gps/gps_pps.c \
+	src/gps/linked_list.c \
+	src/gps/NavEquations.c \
+	src/gps/matrixLib.c

 BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c

-GPS_NAV_SOLVE_TIMER_TICKS ?= 50
+GPS_NAV_SOLVE_TIMER_TICKS ?= 10

 include ../common/app.mk

@@ -66,6 +77,7 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
 	-DIMU_ODR_HZ=$(IMU_ODR_HZ) \
 	-DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \
 	-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
+	-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
 	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
 LDLIBS += -lm

@@ -2,25 +2,87 @@

 Bu notlar GPS bare-metal yaziliminin VCU118 Rocket SoC portu ve interrupt debug sureci icin yazildi.

+## Uygulama akisi ve dosya sinirlari
+
+Firmware giris noktasi [gps_system.c](gps_system.c)'dir. `main()` surucu veya
+ESKF matematigi icermez; fakat sistemlerin nasil baglandigini acikca gosterir.
+ESKF enable/disable, GPS WLS sonucu, timestamp'li IMU girdisi, GPS'in fusion
+olcumune donusumu ve ham/duzeltilmis lat-lon bu dosyada gorunur.
+Main'in GPS/IMU/fusion arasinda tasidigi mesajlar ve yasam-dongusu bayraklari
+`header/gps_system.h` icindeki `gps_system_state_t` tipindedir; uygulama veri
+tipleri giris noktasi C dosyasinda tekrar tanimlanmaz.
+
+```text
+gps_system.c
+    -> initialize GPS, IMU and optional ESKF
+    -> wfi
+    -> timestamped IMU queue -> navigation_fusion prediction
+    -> GPS events -> coherent WLS -> gps_nav_solution_t
+    -> valid GPS -> fusion_gps_measurement_t -> navigation_fusion update
+    -> navigation_fusion_output_t -> corrected lat/lon publish
+```
+
+Kaynak ve public header agaci ayni modul sinirlarini izler:
+
+```text
+header/ + src/
+  gps/          acquisition, tracking, nav epoch, WLS ve GPS interrupt state
+  imu/          sensor secimi, kalibrasyon ve ortak imu_sample_t yonetimi
+  drivers/imu/  yalniz cihaza ozel SPI/register/INT1 kodu
+  fusion/       ESKF, motion/ZUPT, NIS ve delayed-measurement replay
+  adapters/     GPS->fusion ve sensor-profile->fusion donusumleri
+```
+
+UART konum/status formatlamasi `adapters/gps_system_uart` icindedir; bu sayede
+`gps_system.c` telemetry format string'leriyle sismez. Buna karsilik GPS WLS
+sonucunun fusion olcumune cevrilip ESKF'ye teslim edilmesi bilincli olarak
+`main()` icinde gorunur kalir.
+
+GPS kaynaklari IMU veya ESKF header'i include etmez; IMU suruculeri de
+GPS/fusion header'i include etmez. Ortak trap/PLIC dispatcher harici bir IRQ
+numarasi ile callback kabul eder. `BSP_PMOD_IRQ -> imu_sensor_irq_handler`
+baglantisini GPS icinde hard-code etmek yerine `main`, GPS init'inden sonra ve
+IMU start'indan once kaydeder. GPS ve IMU girdileri yalniz `gps_system.c`
+uzerinden `navigation_fusion` API'sine verilir; tip donusumleri `adapters/`
+sinirinda kalir.
+
+ESKF'nin public girisi `fusion/navigation_fusion.h`, sensor yonetiminin public
+girisi `imu/imu_sensor.h`, GPS alicisinin public girisi `gps/gps_receiver.h`dir.
+Yeni sensor eklemek icin yeni bir `drivers/imu` backend'i ve sensor profil
+adapter'i eklenir; GPS veya ESKF cekirdegi degistirilmez.
+
+ISR icinde WLS veya ESKF calismaz. IMU ISR'i ornegi olcum aninda timestamp'ler
+ve raw kuyruga koyar. GPS ISR'leri acquisition/tracking olaylarini kaydeder.
+`wfi` sonrasi foreground bu kuyruklari tuketir. WLS sirasinda yeni IMU
+interrupt'lari gelebilecegi icin IMU kuyrugu WLS'den hem once hem sonra drain
+edilir; GPS update'i WLS bitis zamaninda degil capture epoch'unda uygulanir.
+
 ## Generic IMU Mimarisi (TDK ve ST)

 GPS/ESKF artik dogrudan bir sensor surucusune bagli degildir. Veri yolu:

 ```text
 TDK ICM42688 -----\
-                   -> imu_sensor -> imu_calibration -> fusion_imu_measurement -> motion detector + ESKF
-ST LSM6DSOX ------/
+                   -> imu_sensor -> imu_calibration -> imu_sample_t
+ST LSM6DSOX ------/                                      |
+                                                         v
+GPS receiver -> WLS -> gps_nav_solution_t -> GPS adapter -> navigation_fusion
+                                                          |
+                                                          v
+                                           motion/ZUPT + ESKF -> fused LLH
 ```

- `src/tdk_icm42688.c`: TDK ICM42688'e ozel SPI, register, interrupt ve
+- `src/drivers/imu/tdk_icm42688.c`: TDK ICM42688'e ozel SPI, register, interrupt ve
  ham-veri donusumu.
- `src/st_lsm6dsox.c`: STEVAL-MKI217V1 uzerindeki ST LSM6DSOX'e ozel SPI,
+- `src/drivers/imu/st_lsm6dsox.c`: STEVAL-MKI217V1 uzerindeki ST LSM6DSOX'e ozel SPI,
  register, INT1 data-ready interrupt ve ham-veri donusumu.
- `src/imu_sensor.c`: ortak init/start/stop/IRQ/service/measurement arayuzu.
- `src/imu_calibration.c`: markadan bagimsiz bias, 3x3 scale/cross-axis/mounting
+- `src/imu/imu_sensor.c`: ortak init/start/stop/IRQ/service/measurement arayuzu.
+- `src/imu/imu_calibration.c`: markadan bagimsiz bias, 3x3 scale/cross-axis/mounting
  donusumu ve opsiyonel sicaklik katsayisi.
- `fusion_measurement.h`, motion detector ve ESKF yalniz SI birimindeki ortak
-  mesaji gorur; aktif sensorun markasini bilmez.
+- `header/imu/imu_sample.h`: driver ve IMU yonetimi ile fusion arasindaki
+  sensor-markasindan bagimsiz SI mesaj sozlesmesi.
+- `src/adapters/imu_fusion_profile.c`: aktif sensorun karakteristigini ESKF
+  process-noise/kalibrasyon esiklerine cevirir.

 Firmware isletim sistemi kullanmaz. Yeni IMU katmani `malloc` cagirmadan sensor
 kuyruklarini ve butun durumunu statik ayirir. Newlib ve formatli UART destegi
@@ -47,6 +109,17 @@ edilerek 512 KiB sinirinda dogrulanmistir.

 ### Bare-metal sensor secimi

+ESKF/fusion varsayilan olarak aciktir. GPS-only firmware icin ana uygulama
+akisini degistirmeden kapatilabilir:
+
+```sh
+make -C soc/software/gps_system GPS_ESKF_ENABLE=0
+```
+
+Bu durumda GPS acquisition/tracking/WLS ve ham lat-lon yayini calismaya devam
+eder; IMU stream'i ve `navigation_fusion` baslatilmaz. UART startup satirinda
+`eskf_enabled` ve gercek init sonucu olan `eskf_ready` ayri alanlardir.
+
 Otomatik WHO_AM_I algilama varsayilandir:

 ```sh
@@ -61,7 +134,8 @@ make -C soc/software/gps_system IMU_SENSOR=tdk \
  IMU_ODR_HZ=500 IMU_ACCEL_FS_G=4 IMU_GYRO_FS_DPS=250

 make -C soc/software/gps_system IMU_SENSOR=st \
-  IMU_ODR_HZ=104 IMU_ACCEL_FS_G=2 IMU_GYRO_FS_DPS=250
+  IMU_ODR_HZ=50 GPS_NAV_SOLVE_TIMER_TICKS=10 \
+  IMU_ACCEL_FS_G=2 IMU_GYRO_FS_DPS=250
 ```

 `IMU_SENSOR=stm`, `st` seciminin alias'idir. `IMU_ODR_HZ=0`,
@@ -70,6 +144,33 @@ secer. Bu ozellikle `auto` icin onemlidir; TDK ve ST'nin ortak bir ODR degeri
 yoktur. Desteklenen ODR'ler TDK icin 25/50/100/200/500/1000 Hz, ST icin
 52/104/208 Hz'dir.

+`IMU_ODR_HZ` fiziksel data-ready hizini secer; ST icin `50` istegi gercek 52 Hz
+ayarina eslenir. Her IMU ornegi kendi monotonik CLINT timestamp'i ile ESKF
+tahminine girer. 19/20 ms gibi sabit bir `dt` varsayilmaz ve CPU yuku icin ham
+ornek atlanmaz. Ileride ESKF orani dusurulecekse PX4'teki gibi delta-angle ve
+delta-velocity biriktiren, coning/sculling bilgisini koruyan bir downsampler
+kullanilmalidir.
+
+GPS orani bundan bagimsizdir. `GPS_NAV_SOLVE_TIMER_TICKS=5/10/50` sirasiyla
+100/200/1000 ms nav-solve periyodu verir. WLS sonucu gec gelse bile olcum kendi
+capture timestamp'inde fixed-lag replay ile uygulanir; yalniz
+`solution_valid=true` olan GPS cozumu fusion'a girer. GPS yokken IMU ESKF'nin
+process-model tahminini surdurur, fakat filtre kendine sahte konum olcumu
+vermez. Global origin yoksa LLH cikisi valid degildir; origin daha once
+kurulduysa sonuc yardimsiz dead-reckoning'dir ve kovaryansi zamanla buyur.
+
+NIS kapisi tek bir aykiri GPS'in ESKF'yi suruklemesini engeller. Ancak acilista
+ilk GPS fix'i kotu olup sonraki tutarli fix'ler bu ilk noktadan onlarca metre
+uzakta kalirsa, yalniz hard NIS rejection kullanmak filtreyi yanlis baslangica
+kilitler. Fusion engine bu nedenle ardisik reddedilen position fix'lerinin
+zamansal surekliligini izler. Son başarılı fusion'dan sonra yedi saniye timeout
+dolmuşsa ve en az uc fix birbirinin devamı
+ise yalniz ESKF konumu ve konum kovaryansi son olcume yeniden ankrajlanir;
+attitude, hiz ve IMU bias state'leri korunur, gecersiz hale gelen position
+cross-covariance'lari sifirlanir. Tek aykiri fix bu yolu tetiklemez. Bu davranis
+100/200/1000 ms GPS oranlarinda `gps_reacquisition_test.c` ile dogrulanir ve
+level-2 fusion status satirinda `recoveries=` sayaci ile gozlenir.
+
 TDK ve ST veri-hazir interrupt'i ile calisir ve ana dongu `wfi` kullanabilir.
 Genesys2 son pin planinda ortak aktif sensor interrupt girişi `JB1/V29`
 uzerinden wrapper'daki 3FF synchronizer'a, oradan PLIC device 11'e gider. ST
@@ -79,12 +180,14 @@ sicaklik alanlarini okuyup statik SPSC kuyruğa koyar. `imu_sensor_service()`
 interrupt backend'lerde no-op'tur.

 Standalone LSM6DSOX SPI okuma, stationary log ve `INT1 -> JB1 -> 3FF -> PLIC
-11 -> C ISR` fiziksel yolu kartta dogrulanmistir. 2026-07-16 canli sayac
-snapshot'inda 4084 interrupt'in 4084'u sample'a donusmus; read-fail, queue
-overflow, unexpected IRQ ve trap sayilari sifir kalmistir. Buna karsilik GPS +
-ST + ESKF'nin ayni urun firmware kosusundaki end-to-end testi henuz tamamlanmis
-sayilmamalidir. Yuksek ODR ve uzun WLS sirasinda sample kaybini daha da azaltmak
-gerekirse sonraki gelistirme sensor FIFO + sensor timestamp kullanmaktir;
+11 -> C ISR` fiziksel yolu kartta dogrulanmistir. 2026-07-16 GPS + ST + ESKF
+end-to-end stationary kosusunda 6531 interrupt/sample, 467 ESKF aktarimi ve
+322 kabul edilen GPS update'i goruldu; read-fail, queue overflow, missed sample
+ve GPS NIS reject sayilari sifir kaldi. Ayni logun yerlesmis son bolumunde 100
+ham GPS fix'inin yatay RMS sacilimi 13.62 m, son 20 fused durumun yatay RMS
+sacilimi 0.39 m oldu. Bu degerler survey referansina gore mutlak dogruluk degil,
+sabit anten etrafindaki repeatability olcumudur. Yuksek ODR gerekirse sonraki
+gelistirme sensor FIFO + sensor timestamp veya daha yuksek CPU butcesidir;
 ortak IMU arayuzu ve ESKF bunun icin degismeyecektir.

 Sensor ODR'si ile GPS nav-solve periyodu birbirinden bagimsizdir. Her IMU ve GPS
@@ -93,6 +196,30 @@ ESKF olaylari timestamp sirasinda isler. TDK ve ST ISR timestamp'i data-ready
 kenarini yakindan temsil eder. Daha kesin ve WLS/interrupt latency'sinden
 bagimsiz yuksek-ODR zamanlama gerekirse sensor FIFO timestamp'i kullanilmalidir.

+### Level-1 konum UART protokolu
+
+Her gecerli GPS navigation epoch'unda level 1'de iki konum satiri art arda
+yayinlanir. Ilk satir WLS GPS konumu, ikinci satir o anda mevcut IMU+ESKF
+duzeltilmis konumdur:
+
+```text
+[LOCATION] source=gps lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=...
+[LOCATION] source=imu_corrected lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=...
+```
+
+`tracked`, donanimda calisan tracking kanal sayisini; `used`, ilgili WLS
+cozumunde kullanilan uydu sayisini belirtir. DOP alanlari ayni GPS epoch'unun
+geometrisidir ve iki satirda ayni tasinir. ESKF henuz kalibrasyon/origin
+asamasindaysa yalniz GPS satiri yayinlanir; ESKF valid olduktan sonra her GPS
+konumu geldiginde iki satir da yayinlanir. Eski `[NAV] position` ve periyodik
+`[FUSION] lat/lon` ayrintilari daha yuksek debug seviyesindedir; level 1
+parser'inin ayni konumu birden fazla kez gormesi engellenir.
+
+Desktop `location_plotter`, `source=gps` serisini mavi ve
+`source=imu_corrected` serisini kirmizi cizer. Iki kaynak arasinda zigzag yol
+cizmez; corrected veri varsa CEP/RMS analizi yalniz corrected seriyle yapilir.
+Linux sentetik replay loglarindaki `source=trajectory` gerçek izi yeşil çizer.
+
 ### Kalibrasyon ve sicaklik politikasi

 Uc farkli duzeltme birbirinden ayridir:
@@ -142,7 +269,7 @@ Generated DTS/RTL'e gore GPS IP adresleri:
 | TRCK7 | `0x10028000` | `9` |
 | TRCK8 | `0x10029000` | `10` |

-Bu degerler `soc/software/bsp/include/bsp/soc.h` icinde BSP define olarak tutuluyor. GPS yazilimi `header/gps_ip_defines.h` icinde bu BSP define'larini kullaniyor.
+Bu degerler `soc/software/bsp/include/bsp/soc.h` icinde BSP define olarak tutuluyor. GPS yazilimi `header/gps/gps_ip_defines.h` icinde bu BSP define'larini kullaniyor.

 PLIC base:

@@ -357,14 +484,14 @@ Bu bolum continuous/background acquisition ve parametrik acquisition engine dest

 ### Degisen Dosyalar

- `header/gps_ip_defines.h`
- `header/gps_macros.h`
- `header/gps_interrupt_handlers.h`
- `header/linked_list.h`
- `src/gps_ip.c`
- `src/gps_interrupt_handlers.c`
- `src/linked_list.c`
- `gps_test.c`
+- `header/gps/gps_ip_defines.h`
+- `header/gps/gps_macros.h`
+- `header/gps/gps_interrupt_handlers.h`
+- `header/gps/linked_list.h`
+- `src/gps/gps_ip.c`
+- `src/gps/gps_interrupt_handlers.c`
+- `src/gps/linked_list.c`
+- `gps_system.c` (onceki `gps_test.c` giris noktasi)

 ### Parametric Acquisition Engine Destegi

@@ -526,8 +653,9 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
 - Subframe 1 `toc` alani parity bitleri disarida birakilarak 16 bit okunur ve `2^4` ile olceklenir.
 - Subframe 4 page 18 henuz alinmadiysa iyonosfer duzeltmesi uygulanmaz. Page 18 hazir oldugunda her navigation solve tek bir `ionoutc` kopyasi kullanir.
 - Klobuchar gecikmesi WLS'nin receiver ECEF, azimut ve elevasyon hesabindan uretilir. Ilk unseeded iteration iyonosfersizdir; yaklasik receiver konumu olustuktan sonraki iteration'larda gecikme gozlenen pseudorange'dan cikarilir.
- Bundan sonra periyodik pozisyon hesaplamasi nav-solve interrupt'lari ile tetiklenir. RTL `subframe_ready` sonrasinda COUNT durumuna girer ve `NAV_SOLVE_TIMER=N` icin N adet navigation `bit_valid` darbesi sayar. Her navigation biti 20 ms oldugu icin periyot `N * 20 ms`'dir; mevcut `gps_startup_initialization(10)` ayari 200 ms'de bir ABS_SAMPLE latch ve nav-solve interrupt'i uretir.
- Bu siralama onemlidir: RTL `nav_solve_timer` sayaci `NAV_SOLVE_EN` ve `subframe_ready` ile baslar. `NAV_SOLVE_EN` nav-bit asamasinda acik tutulur ki timer GPS bit/subframe fazina otursun; subframe 1..5 hazir olana kadar yalniz nav-solve interrupt/flag kapali kalir.
+- Bundan sonra periyodik pozisyon hesaplamasi nav-solve interrupt'lari ile tetiklenir. RTL `subframe_ready` sonrasinda COUNT durumuna girer ve `NAV_SOLVE_TIMER=N` icin N adet navigation `bit_valid` darbesi sayar. Her navigation biti 20 ms oldugu icin periyot `N * 20 ms`'dir; firmware `GPS_NAV_SOLVE_TIMER_TICKS` build ayarini `gps_startup_initialization()` fonksiyonuna verir.
+- Bu siralama onemlidir: RTL `nav_solve_timer` sayaci `NAV_SOLVE_EN` ve ilk gecerli `subframe_ready` ile baslar. `NAV_SOLVE_EN` nav-bit asamasinda acik tutulur; subframe 1..5 hazir olana kadar yalniz nav-solve interrupt/flag kapali kalir. Timer COUNT durumuna girdikten sonra sonraki subframe darbeleri sayaci sifirlamaz veya yeniden fazlamaz.
+- Tracking kanallari farkli zamanlarda lock olabilir ve ilk gecerli subframe'lerini 6 saniyenin farkli katlarinda gorebilir. Timer bu nedenle ilk subframe'in HOW alanindaki 17-bit Z-count degerini kullanarak `Z_COUNT * 300 mod NAV_SOLVE_TIMER` baslangic fazini hesaplar. Hesap ALIGN durumunda 26 PCLK cevriminde ard arda modulo ile yapilir; 20 ms'lik ilk navigation bitinden cok once tamamlanir. RTL ayni GPS haftasi icinde keyfi pozitif `N` degerlerini hizalayabilir; firmware ise hafta rollover'i oncesinden aktif kalan kanallarla rollover sonrasinda katilan kanallarin da ayni fazda kalmasi icin `30240000 mod N == 0` kosulunu compile time'da zorunlu tutar. Ornegin `N=40`, `N=100`, `N=125`, `N=150`, `N=200` ve `N=300` gecerlidir; kosulu saglamayan build acik bir `#error` ile durur.
 - Initial position sonrasinda subframe'lerini tamamlayan yeni tracking kanallarinda da nav bits acik tutularak nav-solve otomatik etkinlestirilir.
 - Coherence kontrolu sadece software systick ile yapilmaz; periodic snapshot icin tracking node'da saklanan nav-solve interrupt sonrasi raw `ABS_SAMPLE` kullanilir. `GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS=25` oldugu icin 4 satellite measurement'i 25 ms'den fazla ayriksa navigation solve calistirilmaz. Bu pencere interrupt skew'den cok uydu path delay farkini kapsamak icindir: 20 ms yaklasik 6000 km, 25 ms yaklasik 7500 km pseudorange farkina denk gelir. 25 ms, dusuk-elevation uydu geometrisi ve tracking jitter icin pay birakir ama 50 ms kadar gevsek degildir.
 - Partial coherent grup sayisi mevcut `ready channels` sayisina ulasmadiysa firmware nav-solve sequence'lerini consume etmez. Coherent snapshot tum ready kanallari kapsasa bile sequence ancak navigation solve basarili olduktan sonra, snapshot aninda kaydedilen sequence degerine kadar consume edilir. Basarisiz solve ayni epoch'u tekrar deneyebilir; daha yeni sequence yanlislikla tuketilmez.
@@ -538,6 +666,7 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
 - Tracking nav-solve interrupt'lari ayni GPS epoch'unda birbirine cok yakin gelebilir. External interrupt handler bu yuzden PLIC'te bekleyen farkli GPS source ID'lerini tek ISR girisinde bounded olarak drain eder. Ayni source ID tekrar claim edilirse handler durur; bu durum level interrupt clear'in henuz donanima yansimadigini gosterir ve `PLIC repeated source while draining` WARN sayaci ile raporlanir.
 - Normal nav-bit interrupt'larinda mevcut tracking node tekrar insert edilmeye calisilmaz. Bu nedenle `requested node already exists` UART spam'i normal akista gorulmemelidir; per-subframe `nav bits` logu `TRACE` seviyesine alinmistir.
 - Nav solve mode'da `NAV_SOLVE_TIMER`, subframe fazindan sonra kac adet 20 ms navigation biti sayilacagini; dolayisiyla ABS_SAMPLE latch ve interrupt epoch'unu belirler. Her islenen event'te TOW `NAV_SOLVE_TIMER * 20 ms` artirilir ve GPS hafta sinirinda normalize edilir. Raw `ABS_SAMPLE` TOW artisini belirlemez; yalniz ayni nav-solve epoch'undan gelen kanallari secmek ve pseudorange olcumunu olusturmak icin kullanilir. Pseudorange hesabina giden `gps_ephems.abs_sample` receiver sample'in corrected double halidir.
+- `hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_tb.sv`, RTL'nin ayni hafta icindeki genel fazlama davranisini bir kanalin digerinden tam bir subframe gec basladigi `N=40`, `N=37`, `N=512` ve `N=1` durumlariyla kontrol eder. Firmware build kurali bunlardan GPS haftasindaki `30240000` navigation biti tam bolmeyen degerleri sistem konfigurasyonu olarak reddeder.
 - Tracking wrapper'daki `REM_CODE_PHASE_REG` donanimda IEEE-754 double degildir. RTL'de code NCO phase accumulator 46 bittir; software'e expose edilen kisim `rem_code_ph[45:20]` yani 26-bit integer residue'dur ve APB `PRDATA[25:0]` alanina zero-extend edilerek verilir. Bu yuzden firmware bu register'i `uint32_t` okur; `uint64_t` veya `double*` olarak MMIO okumak iki 32-bit APB read uretir ve register map ile uyumlu degildir.
 - Navigation icin kullanilan corrected receiver sample formulu:

@@ -578,6 +707,11 @@ weight   = 1 / variance
 - Ephemeris alanlarinin mevcut isimleri ile ICD anlamlari su sekildedir: `omgdot` gercekte `Omega0`, `omg0` gercekte argument of perigee `omega`, `omgkdot` gercekte `OmegaDot` degeridir. `satpos()` bunlari dogru matematiksel yerde kullanir; konu su an hesap hatasi degil, ileride yapilacak kontrollu isim duzeltmesidir.
 - `POSTPONE_TOW=0.160` donanim/pipeline zaman kalibrasyonunun bir parcasidir; kaynagi dogrulanmadan kaldirilmamalidir.
 - Tum ready kanallarin coherent nav-solve interrupt'inin beklenmesi bilincli davranistir. Donanimin her aktif tracking kanali icin ilgili interrupt'i uretecegi kabul edilir.
+- Dort uydu 3D konum + receiver clock bias icin minimum ve yeterlidir. Bu durumda
+  `dof=N-4=0` oldugu icin WLS konumu publish edilir fakat residual chi-square
+  integrity bayragi false kalir. Bes veya daha fazla uyduda publish icin mevcut
+  chi-square gecis kosulu uygulanir. Boylece `solution_valid` ile
+  `integrity_available/pass` ayni kavram olarak kullanilmaz.
 - Broadcast SV health filtresi uygulanir. Sonraki integrity adimi ephemeris age/fit interval ve post-fit residual metriklerini eklemektir. En az bes satellite olmadan tek hatali olcumu residual ile guvenilir sekilde ayirmak mumkun degildir.

 ### Debug Mesajlari
@@ -688,20 +822,20 @@ Eger ACQ engine'ler tracking bolgesinden once arka arkaya yerlestiriliyorsa `BSP
 #define BSP_GPS_TRCK0_IRQ         3UL
 ```

-ACQ tarafinda normal durumda `src/gps_ip.c` icinde manuel pointer eklemek gerekmez. Startup'ta `gps_init_acq_tables()` su helper'lari kullanarak tabloyu doldurur:
+ACQ tarafinda normal durumda `src/gps/gps_ip.c` icinde manuel pointer eklemek gerekmez. Startup'ta `gps_init_acq_tables()` su helper'lari kullanarak tabloyu doldurur:

 ```c
 GPS_ACQ_IP_BASE_ADDR(index)
 GPS_ACQ_INTR_LINE(index)
 ```

-ACQ engine sayisi firmware tarafinda tek noktadan `header/gps_ip_defines.h` icindeki `ACQ_ENGINE_COUNT` ile kontrol edilir. Varsayilan olarak `BSP_GPS_ACQ_COUNT` kullanilir. Donanim/BSP dogru count veriyorsa ekstra override gerekmez.
+ACQ engine sayisi firmware tarafinda tek noktadan `header/gps/gps_ip_defines.h` icindeki `ACQ_ENGINE_COUNT` ile kontrol edilir. Varsayilan olarak `BSP_GPS_ACQ_COUNT` kullanilir. Donanim/BSP dogru count veriyorsa ekstra override gerekmez.

 Eger ACQ address veya IRQ offset'leri uniform degilse, mevcut `base + index * stride` modeli yeterli degildir. Bu durumda:

 - `soc.h` icinde her ACQ icin explicit base/IRQ define'i eklenmeli, ornek `BSP_GPS_ACQ0_BASE`, `BSP_GPS_ACQ1_BASE`.
- `header/gps_ip_defines.h` icindeki `GPS_ACQ_IP_BASE_ADDR(index)` ve `GPS_ACQ_INTR_LINE(index)` helper'lari explicit tablo veya switch ile guncellenmeli.
- `src/gps_ip.c` icindeki `gps_init_acq_tables()` explicit base/IRQ tablolarini kullanacak sekilde guncellenmeli.
+- `header/gps/gps_ip_defines.h` icindeki `GPS_ACQ_IP_BASE_ADDR(index)` ve `GPS_ACQ_INTR_LINE(index)` helper'lari explicit tablo veya switch ile guncellenmeli.
+- `src/gps/gps_ip.c` icindeki `gps_init_acq_tables()` explicit base/IRQ tablolarini kullanacak sekilde guncellenmeli.

 ACQ register layout'u degisirse sadece count/base degistirmek yetmez. Su alanlar da XLSX/RTL'e gore kontrol edilmelidir:

@@ -726,14 +860,14 @@ Uniform tracking channel address/IRQ stride varsa `soc.h` icinde su define'lar g

 Tracking tarafinda mevcut firmware bazi yerlerde hala manuel channel entry'leri kullaniyor. `BSP_GPS_TRCK_COUNT` arttirilinca su dosyalarda yeni kanal icin entry eklenmelidir:

- `header/gps_ip_defines.h`
+- `header/gps/gps_ip_defines.h`
  - `TRCK9_IP_BASE_ADDR`, `TRCK10_IP_BASE_ADDR`, ...
  - `TRCK9_INTR_LINE`, `TRCK10_INTR_LINE`, ...
- `src/gps_ip.c`
+- `src/gps/gps_ip.c`
  - `trck9_ip`, `trck10_ip`, ...
  - `trck_ips[]` array'ine yeni pointer'lar,
  - `trck_ips_intr_lines[]` array'ine yeni IRQ line'lar.
- `src/gps_interrupt_handlers.c`
+- `src/gps/gps_interrupt_handlers.c`
  - dosya sonundaki `DEFINE_TRCK_INTR_HANDLER(9)`, `DEFINE_TRCK_INTR_HANDLER(10)`, ... wrapper'lari.

 `GPS_TRCK_IP_COUNT`, `BSP_GPS_TRCK_COUNT` uzerinden gelir ve loop/array boyutlari bu count'u kullanir. Ancak yukaridaki manuel array initializer'lar ve handler wrapper'lari yeni count ile uyumlu olmak zorundadir.
@@ -741,8 +875,8 @@ Tracking tarafinda mevcut firmware bazi yerlerde hala manuel channel entry'leri
 Eger TRCK address veya IRQ offset'leri uniform degilse, mevcut `BSP_GPS_TRCK0_BASE + index * BSP_GPS_TRCK_STRIDE` ve `BSP_GPS_TRCK0_IRQ + index * BSP_GPS_TRCK_IRQ_STRIDE` modeli yeterli degildir. Bu durumda:

 - BSP'ye her tracking channel icin explicit base/IRQ define'i eklenmeli.
- `header/gps_ip_defines.h` icindeki `TRCKn_IP_BASE_ADDR` ve `TRCKn_INTR_LINE` define'lari explicit degerlerden gelmeli.
- `src/gps_ip.c` icindeki `trck_ips[]` ve `trck_ips_intr_lines[]` array'leri bu explicit define'lari kullanmali.
+- `header/gps/gps_ip_defines.h` icindeki `TRCKn_IP_BASE_ADDR` ve `TRCKn_INTR_LINE` define'lari explicit degerlerden gelmeli.
+- `src/gps/gps_ip.c` icindeki `trck_ips[]` ve `trck_ips_intr_lines[]` array'leri bu explicit define'lari kullanmali.

 TRCK register layout'u degisirse su alanlar XLSX/RTL'e gore tekrar kontrol edilmelidir:

@@ -0,0 +1,135 @@
+#include <bsp/soc.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "adapters/gps_fusion_publisher.h"
+#include "adapters/gps_system_uart.h"
+#include "adapters/imu_fusion_profile.h"
+#include "fusion/navigation_fusion.h"
+#include "gps/gps_receiver.h"
+#include "gps_system.h"
+#include "imu/imu_sensor.h"
+
+#ifndef GPS_ESKF_ENABLE
+#define GPS_ESKF_ENABLE 1
+#endif
+
+#if (GPS_ESKF_ENABLE != 0) && (GPS_ESKF_ENABLE != 1)
+#error "GPS_ESKF_ENABLE must be 0 or 1"
+#endif
+
+static void initialize_sensor_fusion(gps_system_state_t* system)
+{
+	system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
+	if(!system->fusion_enabled)
+	{
+		return;
+	}
+
+	imu_sensor_config_t imu_config;
+	imu_sensor_default_config(&imu_config);
+	imu_config.odr_hz = IMU_ODR_HZ;
+	imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
+	imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
+	system->imu_initialized = imu_sensor_init(&imu_config);
+
+	fusion_imu_quality_profile_t imu_quality;
+	bool quality_available = system->imu_initialized &&
+		imu_fusion_quality_profile_for_backend(
+			imu_sensor_active_backend(), &imu_quality);
+	system->fusion_initialized = quality_available &&
+		navigation_fusion_init(
+			BSP_CLINT_TIMEBASE_HZ, &imu_quality,
+			true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
+}
+
+static unsigned int feed_pending_imu_to_eskf(gps_system_state_t* system)
+{
+	if(!system->fusion_initialized)
+	{
+		return 0u;
+	}
+
+	unsigned int processed = 0u;
+	imu_sensor_service();
+	while(imu_sensor_take_measurement(&system->imu_sample))
+	{
+		(void)navigation_fusion_push_imu(&system->imu_sample);
+		processed++;
+	}
+	return processed;
+}
+
+int main(void)
+{
+	gps_system_state_t system = {0};
+
+	/* GPS and IMU are initialized independently.  The application owns their
+	 * interrupt connection and decides whether the ESKF path is enabled. */
+	initialize_sensor_fusion(&system);
+	gps_receiver_initialize(GPS_NAV_SOLVE_TIMER_TICKS);
+	bool imu_irq_attached = system.fusion_initialized &&
+		gps_receiver_attach_external_irq(BSP_PMOD_IRQ,
+		                                 imu_sensor_irq_handler);
+	system.imu_running = imu_irq_attached && system.fusion_initialized &&
+	                     imu_sensor_start();
+	gps_receiver_start_acquisition();
+	gps_system_uart_publish_startup(
+		system.fusion_enabled, system.fusion_initialized,
+		system.imu_initialized, system.imu_running);
+
+	for(;;)
+	{
+		/* GPS, IMU data-ready and the maintenance timer wake this loop. */
+		asm volatile("wfi");
+
+		/* IMU prediction runs at the sensor's own timestamped rate. */
+		(void)feed_pending_imu_to_eskf(&system);
+
+		/* GPS remains an independent receiver: events first, then coherent WLS. */
+		gps_receiver_process_pending_events();
+		system.gps_epoch_ready = gps_receiver_solve_ready_epoch(
+			&system.gps_solution, &system.gps_measurement_ticks);
+
+		/* WLS may take long enough for more IMU samples to arrive. */
+		(void)feed_pending_imu_to_eskf(&system);
+
+		if(system.gps_epoch_ready && system.gps_solution.solution_valid)
+		{
+			/* Raw WLS latitude/longitude remains visible even with ESKF disabled. */
+			gps_system_uart_publish_location("gps",
+			                 system.gps_solution.latitude_deg,
+			                 system.gps_solution.longitude_deg,
+			                 system.gps_solution.height_m,
+			                 &system.gps_solution);
+
+			/* This is the only GPS -> sensor-fusion hand-off in the application. */
+			bool gps_measurement_ready = system.fusion_initialized &&
+				gps_fusion_build_measurement(
+					&system.gps_solution,
+					system.gps_measurement_ticks,
+					&system.gps_for_fusion);
+			if(gps_measurement_ready)
+			{
+				/* Delayed GPS update is applied at its capture timestamp; newer
+				 * IMU history is replayed to produce the current fused state. */
+				(void)navigation_fusion_push_gps(&system.gps_for_fusion);
+				if(navigation_fusion_get_output(&system.fused_solution) &&
+				   system.fused_solution.global_valid)
+				{
+					gps_system_uart_publish_location("imu_corrected",
+					                 system.fused_solution.latitude_deg,
+					                 system.fused_solution.longitude_deg,
+					                 system.fused_solution.height_m,
+					                 &system.gps_solution);
+				}
+			}
+		}
+
+		if(system.fusion_initialized)
+		{
+			gps_system_uart_publish_periodic_fusion_status(
+				&system.fused_solution, &system.last_status_tick);
+		}
+	}
+}
@@ -1,72 +0,0 @@
-#include <stdio.h>
-#include "gps_ip_defines.h"
-#include "linked_list.h"
-#include <stddef.h>
-#include <float.h>
-#include "matrixLib.h"
-#include "gps_macros.h"
-#include "imu_sensor.h"
-#include "navigation_fusion.h"
-
-int main()
-{
-	imu_sensor_config_t imu_config;
-	imu_sensor_default_config(&imu_config);
-	imu_config.odr_hz = IMU_ODR_HZ;
-	imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
-	imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
-	bool imu_initialized = imu_sensor_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 && imu_sensor_start();
-	(void)imu_running;
-	imu_sensor_diagnostics_t imu_diagnostics;
-	imu_sensor_get_diagnostics(&imu_diagnostics);
-	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
-	        "init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu "
-	        "poll=%u timebase_hz=%lu\r\n",
-	        imu_initialized ? 1u : 0u,
-	        imu_sensor_backend_name(imu_diagnostics.backend),
-	        (unsigned int)imu_diagnostics.who_am_i,
-	        imu_running ? 1u : 0u,
-	        (unsigned long)imu_diagnostics.configured_odr_hz,
-	        imu_diagnostics.foreground_polling ? 1u : 0u,
-	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);
-
-	acq_start_loop();
-	uint64_t last_fusion_status_tick = 0u;
-
-	while(1)
-	{
-		/* GPS and either IMU backend wake the shared machine interrupt path. */
-		asm volatile("wfi");
-		gps_background_task();
-		calculate_position();
-		if(fusion_initialized)
-		{
-			(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;
-}
@@ -0,0 +1,17 @@
+#ifndef GPS_FUSION_PUBLISHER_H
+#define GPS_FUSION_PUBLISHER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "gps/NavEquations.h"
+#include "fusion/fusion_measurement.h"
+
+/* Thin boundary adapter used by main.  GPS and the ESKF do not call each
+ * other; main converts the completed WLS output to the shared measurement. */
+bool gps_fusion_build_measurement(
+	const gps_nav_solution_t* solution,
+	uint64_t measurement_timestamp_ticks,
+	fusion_gps_measurement_t* measurement_out);
+
+#endif
@@ -0,0 +1,27 @@
+#ifndef GPS_SYSTEM_UART_H
+#define GPS_SYSTEM_UART_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion/navigation_fusion.h"
+#include "gps/NavEquations.h"
+
+void gps_system_uart_publish_location(
+	const char* source,
+	double latitude_deg,
+	double longitude_deg,
+	double height_m,
+	const gps_nav_solution_t* gps);
+
+void gps_system_uart_publish_startup(
+	bool fusion_enabled,
+	bool fusion_initialized,
+	bool imu_initialized,
+	bool imu_running);
+
+void gps_system_uart_publish_periodic_fusion_status(
+	navigation_fusion_output_t* output,
+	uint64_t* last_status_tick);
+
+#endif
@@ -0,0 +1,14 @@
+#ifndef IMU_FUSION_PROFILE_H
+#define IMU_FUSION_PROFILE_H
+
+#include <stdbool.h>
+
+#include "fusion/fusion_imu_quality.h"
+#include "imu/imu_sensor.h"
+
+/* Explicit adapter from a selected sensor backend to ESKF tuning values. */
+bool imu_fusion_quality_profile_for_backend(
+	imu_sensor_backend_t backend,
+	fusion_imu_quality_profile_t* profile_out);
+
+#endif
@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <stdint.h>

-#include "fusion_measurement.h"
+#include "imu/imu_sample.h"

 #ifndef ST_IMU_RAW_QUEUE_CAPACITY
 #define ST_IMU_RAW_QUEUE_CAPACITY 256u
@@ -46,7 +46,7 @@ void st_lsm6dsox_irq_handler(void);
 /* Retained as a no-op so every backend has the same service interface. */
 void st_lsm6dsox_service(void);
 bool st_lsm6dsox_take_measurement(
-	fusion_imu_measurement_t* measurement_out);
+	imu_sample_t* measurement_out);
 void st_lsm6dsox_get_diagnostics(
 	st_lsm6dsox_diagnostics_t* diagnostics_out);

@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <stdint.h>

-#include "fusion_measurement.h"
+#include "imu/imu_sample.h"

 #ifndef TDK_IMU_RAW_QUEUE_CAPACITY
 #define TDK_IMU_RAW_QUEUE_CAPACITY 256u
@@ -69,7 +69,7 @@ void tdk_icm42688_stop(void);
 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);
+bool tdk_icm42688_take_measurement(imu_sample_t* measurement_out);
 void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out);

 #endif
@@ -65,6 +65,18 @@ bool eskf_propagate(eskf_t* filter,
 	                const double gyro_rps[ESKF_VECTOR_DIM],
 	                double dt_s);

+/* Midpoint inertial integration over two timestamped raw samples.  The
+ * constant-input eskf_propagate() entry point remains available for callers
+ * that already provide an interval-integrated/held measurement. */
+bool eskf_propagate_interval(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double start_accel_mps2[ESKF_VECTOR_DIM],
+	const double start_gyro_rps[ESKF_VECTOR_DIM],
+	const double end_accel_mps2[ESKF_VECTOR_DIM],
+	const double end_gyro_rps[ESKF_VECTOR_DIM],
+	double dt_s);
+
 eskf_update_result_t eskf_position_update(
 	eskf_t* filter,
 	const eskf_config_t* config,
@@ -72,6 +84,15 @@ eskf_update_result_t eskf_position_update(
 	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
 	double* nis_out);

+/* Re-anchor position after a higher-level fusion timeout has established that
+ * repeated, valid position observations are mutually coherent.  Velocity,
+ * attitude and IMU biases are preserved; position cross-covariances are
+ * cleared because they are no longer valid after the state discontinuity. */
+bool eskf_reset_position(
+	eskf_t* filter,
+	const double position_m[ESKF_VECTOR_DIM],
+	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM]);
+
 bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);

 bool eskf_zero_angular_rate_update(
@@ -4,7 +4,7 @@
 #include <stdbool.h>
 #include <stdint.h>

-#include "fusion_measurement.h"
+#include "fusion/fusion_measurement.h"

 #ifndef FUSION_IMU_QUEUE_CAPACITY
 #define FUSION_IMU_QUEUE_CAPACITY 256u
@@ -4,10 +4,10 @@
 #include <stdbool.h>
 #include <stdint.h>

-#include "eskf.h"
-#include "fusion_imu_quality.h"
-#include "fusion_measurement.h"
-#include "fusion_motion.h"
+#include "fusion/eskf.h"
+#include "fusion/fusion_imu_quality.h"
+#include "fusion/fusion_measurement.h"
+#include "fusion/fusion_motion.h"

 #ifndef FUSION_IMU_HISTORY_CAPACITY
 #define FUSION_IMU_HISTORY_CAPACITY 2048u
@@ -38,8 +38,13 @@ typedef struct {
 	double zero_gyro_std_rps[3];
 	double gnss_motion_speed_threshold_mps;
 	double gnss_displacement_nis_gate;
+	double gnss_motion_window_s;
 	unsigned int gnss_moving_confirmations;
 	unsigned int gnss_still_confirmations;
+	double position_recovery_timeout_s;
+	double position_recovery_max_speed_mps;
+	double position_recovery_sigma_multiplier;
+	unsigned int position_recovery_min_samples;
 } fusion_engine_config_t;

 typedef struct {
@@ -49,13 +54,17 @@ typedef struct {
 	fusion_gnss_motion_evidence_t gnss_evidence;
 	double last_accepted_position_m[3];
 	double last_accepted_position_covariance_m2[9];
+	double zero_gyro_integral_rad[3];
+	double zero_gyro_duration_s;
 	uint64_t filter_timestamp_ticks;
 	uint64_t last_zero_update_ticks;
 	uint64_t last_accepted_position_ticks;
+	uint64_t last_position_fuse_ticks;
 	unsigned int gnss_moving_count;
 	unsigned int gnss_still_count;
 	bool have_held_imu;
 	bool have_accepted_position;
+	bool vehicle_at_rest;
 } fusion_runtime_t;

 typedef struct {
@@ -70,9 +79,11 @@ typedef struct {
 	uint64_t position_stale_rejected;
 	uint64_t position_pending_overflow;
 	uint64_t position_accepted;
+	uint64_t position_recovery_count;
 	uint64_t replay_count;
 	uint64_t replayed_imu_count;
 	uint64_t zero_velocity_update_count;
+	uint64_t zero_gyro_update_count;
 } fusion_engine_diagnostics_t;

 typedef struct {
@@ -95,12 +106,16 @@ typedef struct {
 	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_position_measurement_t position_recovery_candidate;
 	fusion_engine_diagnostics_t diagnostics;
+	uint64_t position_recovery_start_ticks;
 	unsigned int imu_history_head;
 	unsigned int imu_history_count;
 	unsigned int pending_position_count;
+	unsigned int position_recovery_consistent_count;
 	bool checkpoint_valid;
 	bool have_initial_position;
+	bool position_recovery_active;
 } fusion_engine_t;

 void fusion_engine_default_config(fusion_engine_config_t* config,
@@ -3,7 +3,7 @@

 #include <stdbool.h>

-#include "fusion_measurement.h"
+#include "fusion/fusion_measurement.h"

 typedef struct {
 	double minimum_horizontal_std_m;
@@ -4,6 +4,8 @@
 #include <stdbool.h>
 #include <stdint.h>

+#include "imu/imu_sample.h"
+
 /*
 * Hardware- and solver-neutral messages shared at the GPS/ESKF boundary.
 * timestamp_ticks always belongs to the common local monotonic timebase; GPS
@@ -23,12 +25,15 @@ typedef struct {
 	bool solution_valid;
 } fusion_gps_measurement_t;

+/* Timestamped IMU plus the independently supervised platform state consumed
+ * by fusion. Drivers publish imu_sample_t and do not decide `vehicle_at_rest`. */
 typedef struct {
 	uint64_t timestamp_ticks;
 	double accel_mps2[3];
 	double gyro_rps[3];
 	double temperature_c;
 	uint32_t sequence;
+	bool vehicle_at_rest;
 } fusion_imu_measurement_t;

 /* Generic local-navigation measurement consumed by the ESKF coordinator. */
@@ -0,0 +1,50 @@
+#ifndef FUSION_REST_DETECTOR_H
+#define FUSION_REST_DETECTOR_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "imu/imu_sample.h"
+
+/*
+ * PX4-style platform rest supervisor.  The ESKF consumes the resulting
+ * at-rest flag; it does not infer that flag from its own velocity state.
+ * `landed` is an external platform fact (unarmed bench prototype, wheel/leg
+ * contact, land detector, and so on), not a GNSS/ESKF estimate.
+ */
+typedef struct {
+	double vibration_filter_time_constant_s;
+	double accel_vibration_limit_mps2;
+	double gyro_vibration_limit_rps;
+	double rest_dwell_s;
+} fusion_rest_detector_config_t;
+
+typedef struct {
+	fusion_rest_detector_config_t config;
+	double previous_accel_mps2[3];
+	double previous_gyro_rps[3];
+	double accel_vibration_metric_mps2;
+	double gyro_vibration_metric_rps;
+	uint64_t timebase_hz;
+	uint64_t last_sample_ticks;
+	uint64_t last_motion_ticks;
+	bool have_previous_sample;
+	bool landed;
+	bool at_rest;
+} fusion_rest_detector_t;
+
+void fusion_rest_detector_default_config(fusion_rest_detector_config_t* config);
+bool fusion_rest_detector_init(
+	fusion_rest_detector_t* detector,
+	const fusion_rest_detector_config_t* config,
+	uint64_t timebase_hz,
+	bool landed);
+void fusion_rest_detector_set_landed(
+	fusion_rest_detector_t* detector,
+	bool landed,
+	uint64_t timestamp_ticks);
+bool fusion_rest_detector_update(
+	fusion_rest_detector_t* detector,
+	const imu_sample_t* sample);
+
+#endif
@@ -0,0 +1,64 @@
+#ifndef NAVIGATION_FUSION_H
+#define NAVIGATION_FUSION_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion/fusion_engine.h"
+#include "fusion/fusion_measurement.h"
+#include "fusion/fusion_motion.h"
+#include "imu/imu_sample.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;
+	uint64_t gps_position_recoveries;
+	uint64_t zero_update_count;
+	uint64_t replay_count;
+	uint64_t stale_gps_count;
+	uint64_t imu_processed_count;
+	unsigned int calibration_sample_count;
+	fusion_engine_state_t engine_state;
+	bool local_valid;
+	bool global_valid;
+	bool vehicle_at_rest;
+	/* Compatibility alias for UART publishers: valid means global LLH valid. */
+	bool valid;
+} navigation_fusion_output_t;
+
+/* Initialize the sensor-independent 15-state ESKF and GPS-to-ENU adapter. */
+bool navigation_fusion_init(
+	uint64_t local_timebase_hz,
+	const fusion_imu_quality_profile_t* imu_quality,
+	bool platform_landed);
+
+/* PX4-style external platform fact. For this bench prototype `true` means
+ * unarmed/on-ground; a mobile integration must drive it from its own
+ * arm/contact/land supervisor. */
+void navigation_fusion_set_platform_landed(
+	bool platform_landed,
+	uint64_t timestamp_ticks);
+
+/* Prediction input: every timestamped IMU sample is integrated; it is not an
+ * ESKF observation and is never rate-selected in this layer. */
+bool navigation_fusion_push_imu(const imu_sample_t* measurement);
+
+/* Observation input: convert a valid LLH fix to local ENU, apply it at the
+ * captured GPS epoch, then replay newer IMU history to current time. */
+bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);
+
+/* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
+bool navigation_fusion_get_output(navigation_fusion_output_t* output);
+const char* navigation_fusion_state_name(fusion_engine_state_t state);
+
+#endif
@@ -1,11 +1,11 @@
-#ifndef NAVEQUATIONS_H 
+#ifndef NAVEQUATIONS_H
 	#define NAVEQUATIONS_H

 		#include "gps.h"
-		#include "gps_ip_defines.h"
-		#include "matrixLib.h"
+		#include "gps/gps_ip_defines.h"
+		#include "gps/matrixLib.h"
 		
-		typedef struct {
+		typedef struct gps_nav_solution {
 			double ecef_m[3];
 			double clock_bias_m;
 			double latitude_deg;
@@ -39,6 +39,7 @@
 			double quality_score;
 			unsigned int available_satellite_count;
 			unsigned int used_satellite_count;
+			unsigned int tracked_satellite_count;
 			unsigned int degrees_of_freedom;
 			uint32_t used_row_mask;
 			uint32_t excluded_row_mask;
@@ -558,4 +558,3 @@
 	

 #endif
-
@@ -4,7 +4,7 @@
 #define GPS_H

 	//#include "plutogpssim.h"
-	#include "gps_ip_defines.h"
+	#include "gps/gps_ip_defines.h"
 	
 	//#define GM_EARTH 3.986005e14
 	//#define OMEGA_EARTH 7.2921151467e-5
@@ -1,7 +1,7 @@
 #ifndef GPS_INT_HAND
 	#define GPS_INT_HAND
 
-	#include "gps_ip_defines.h"
+	#include "gps/gps_ip_defines.h"

 	typedef struct {
 		uint32_t event_overflow_total;
@@ -16,9 +16,14 @@
 		uint64_t last_lost_tick;
 	} gps_runtime_diagnostics_t;

+	typedef void (*gps_external_irq_handler_t)(void);
+
 	bool assign_tracking_from_result(const gps_acq_result_t* result);
 	void acq_intr_handler(unsigned char engine_id);
 	void gps_dispatch_irq(unsigned int irq_id);
+	bool gps_interrupt_attach_external_source(
+		unsigned int irq_id,
+		gps_external_irq_handler_t handler);
 	void gps_irq_handler(void) GPS_IRQ_HANDLER_ATTR;
 	void gps_process_isr_events(void);
 	void gps_acq_scheduler_run(void);
@@ -1,4 +1,4 @@
-#ifndef GPS_IP_DEFINES 
+#ifndef GPS_IP_DEFINES
 	#define GPS_IP_DEFINES

 	#include <stdbool.h>
@@ -6,8 +6,8 @@
 	#include <float.h>
 	#include <stdint.h>
 	#include <bsp/soc.h>
-	#include "matrixLib.h"
-	#include "gps_macros.h"
+	#include "gps/matrixLib.h"
+	#include "gps/gps_macros.h"

 	#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))

@@ -49,6 +49,7 @@
 			#ifndef GPS_NAV_SOLVE_TIMER_TICKS
 			#define GPS_NAV_SOLVE_TIMER_TICKS   50u
 			#endif
+			#define GPS_NAV_BITS_PER_WEEK       30240000u
 			#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
@@ -64,7 +65,11 @@
 			#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
 			#define GPS_HOW_ANCHOR_MAX_AGE_SEC         12.0

-			_Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero");
+			#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
+			#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
+			#elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
+			#error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week"
+			#endif
 			_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
 			               "GPS_NAV_ENABLE_FDE must be zero or one");

@@ -460,10 +465,14 @@
 	// parse functions. Generic parse function.
 		unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
 		                             unsigned int* how_tow_sec_out);
+	struct gps_nav_solution;
 #ifdef EKF_ENABLE
-void calculate_position(Ekf* g_ekf);
+	bool calculate_position(Ekf* g_ekf,
+	                        struct gps_nav_solution* solution_out,
+	                        uint64_t* measurement_timestamp_ticks_out);
 #else
-void calculate_position();
+	bool calculate_position(struct gps_nav_solution* solution_out,
+	                        uint64_t* measurement_timestamp_ticks_out);
 #endif
 	void gps_startup_initialization(unsigned int nsp_tick);
 	void gps_interrupt_init(void);
@@ -5,8 +5,9 @@
 	
 	/*
 	 * Verbosity is cumulative:
-	 *   1: navigation result (position and DOP)
-	 *   2: tracked channel/PRN summary
+	 *   1: navigation result (position and all DOP values) plus tracked
+	 *      satellite count/PRN summary
+	 *   2: reserved for additional status detail
 	 *   3: state transitions, warnings/errors and integrity summary
 	 *   4: performance, NAV satellite and low-level diagnostic detail
 	 *
@@ -14,7 +15,7 @@
 	 * levels. GPS_DEBUG_LEVEL should normally be in the range 1..4.
 	 */
 		#define DEBUG_STATUS   1
-		#define DEBUG_CHANNELS 2
+		#define DEBUG_CHANNELS DEBUG_STATUS
 		#define DEBUG_EVENTS   3
 		#define DEBUG_TRACE    4

@@ -31,6 +32,7 @@
 		#define GPS_LOG_CAT_MEM   "MEM"
 		#define GPS_LOG_CAT_HWREG "HWREG"
 		#define GPS_LOG_CAT_FUSION "FUSION"
+		#define GPS_LOG_CAT_LOCATION "LOCATION"
 	
 		// Set the current verbosity level
 		#ifdef DEBUG
@@ -0,0 +1,41 @@
+#ifndef GPS_RECEIVER_H
+#define GPS_RECEIVER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+struct gps_nav_solution;
+
+/*
+ * High-level boundary around the legacy GPS acquisition/tracking subsystem.
+ *
+ * initialize(): resets GPS hardware/software state and installs the machine
+ * interrupt handler used by the receiver.
+ *
+ * start_acquisition(): schedules the initial PRN acquisition work.
+ *
+ * process_pending_events(): foreground half of GPS interrupt processing.  It
+ * consumes ISR events, handles channel loss/reacquisition and timer work.
+ *
+ * solve_ready_epoch(): when at least four coherent channels report the same
+ * nav-solve epoch, snapshots them and runs WLS.  The returned timestamp is the
+ * measurement epoch, not the later time at which WLS completed.
+ */
+void gps_receiver_initialize(unsigned int nav_solve_timer_ticks);
+typedef void (*gps_receiver_external_irq_handler_t)(void);
+/* Attach one application-owned PLIC source to the installed trap dispatcher.
+ * The source driver remains responsible for its priority and enable state. */
+bool gps_receiver_attach_external_irq(
+	unsigned int irq_id,
+	gps_receiver_external_irq_handler_t handler);
+void gps_receiver_start_acquisition(void);
+void gps_receiver_process_pending_events(void);
+bool gps_receiver_solve_ready_epoch(
+	struct gps_nav_solution* solution_out,
+	uint64_t* measurement_timestamp_ticks_out);
+
+/* Slow status clock maintained by the GPS machine-timer interrupt. */
+uint64_t gps_receiver_status_ticks(void);
+uint64_t gps_receiver_status_ticks_per_second(void);
+
+#endif
@@ -1,7 +1,7 @@
 #ifndef LINKED_LIST_H_INCLUDED
 #define LINKED_LIST_H_INCLUDED

-#include "gps_ip_defines.h"
+#include "gps/gps_ip_defines.h"

 typedef struct gps_ephem_ll {
 	volatile gps_ephemeris_t* data_p;
@@ -1,12 +0,0 @@
-#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
@@ -0,0 +1,28 @@
+#ifndef GPS_SYSTEM_APP_H
+#define GPS_SYSTEM_APP_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "fusion/fusion_measurement.h"
+#include "fusion/navigation_fusion.h"
+#include "gps/NavEquations.h"
+#include "imu/imu_sample.h"
+
+/* Application-owned messages and lifecycle state at the GPS/IMU/fusion
+ * boundary.  Solver, driver and ESKF private state remain in their modules. */
+typedef struct {
+	gps_nav_solution_t gps_solution;
+	fusion_gps_measurement_t gps_for_fusion;
+	imu_sample_t imu_sample;
+	navigation_fusion_output_t fused_solution;
+	uint64_t gps_measurement_ticks;
+	uint64_t last_status_tick;
+	bool gps_epoch_ready;
+	bool fusion_enabled;
+	bool imu_initialized;
+	bool fusion_initialized;
+	bool imu_running;
+} gps_system_state_t;
+
+#endif
@@ -3,7 +3,7 @@

 #include <stdbool.h>

-#include "fusion_measurement.h"
+#include "imu/imu_sample.h"

 /*
 * Sensor-independent calibration in the IMU body frame.
@@ -28,6 +28,6 @@ typedef struct {
 void imu_calibration_default(imu_calibration_t* calibration);
 bool imu_calibration_is_valid(const imu_calibration_t* calibration);
 bool imu_calibration_apply(const imu_calibration_t* calibration,
-	                       fusion_imu_measurement_t* measurement);
+	                       imu_sample_t* measurement);

 #endif
@@ -0,0 +1,18 @@
+#ifndef IMU_SAMPLE_H
+#define IMU_SAMPLE_H
+
+#include <stdint.h>
+
+/*
+ * Sensor-independent, calibrated inertial sample in SI units.  Drivers and
+ * IMU management publish this type without depending on GPS or ESKF headers.
+ */
+typedef struct {
+	uint64_t timestamp_ticks;
+	double accel_mps2[3];
+	double gyro_rps[3];
+	double temperature_c;
+	uint32_t sequence;
+} imu_sample_t;
+
+#endif
@@ -4,9 +4,8 @@
 #include <stdbool.h>
 #include <stdint.h>

-#include "fusion_measurement.h"
-#include "fusion_imu_quality.h"
-#include "imu_calibration.h"
+#include "imu/imu_calibration.h"
+#include "imu/imu_sample.h"

 typedef enum {
 	IMU_SENSOR_NONE = 0,
@@ -16,7 +15,8 @@ typedef enum {
 } imu_sensor_backend_t;

 typedef struct {
-	/* Zero selects the active backend's safe default. */
+	/* Zero selects the active backend's safe default.  A non-zero target may
+	 * be quantized to a nearby hardware ODR; diagnostics reports both values. */
 	uint32_t odr_hz;
 	unsigned int accel_full_scale_g;
 	unsigned int gyro_full_scale_dps;
@@ -32,6 +32,7 @@ typedef struct {
 	uint64_t queue_overflow_count;
 	uint64_t estimated_missed_sample_count;
 	uint64_t last_timestamp_ticks;
+	uint32_t requested_odr_hz;
 	uint32_t configured_odr_hz;
 	uint8_t who_am_i;
 	imu_sensor_backend_t backend;
@@ -41,25 +42,27 @@ typedef struct {
 } imu_sensor_diagnostics_t;

 void imu_sensor_default_config(imu_sensor_config_t* config);
+
+/* Probe the compiled backend(s), select one, validate the requested ODR/range
+ * and prepare calibration.  This does not enable data-ready interrupts. */
 bool imu_sensor_init(const imu_sensor_config_t* config);
+
+/* Configure streaming and enable BSP_PMOD_IRQ.  The application must attach
+ * imu_sensor_irq_handler to the active machine-interrupt router first. */
 bool imu_sensor_start(void);
 void imu_sensor_stop(void);

-/* Interrupt backend hook. Safe no-op for a foreground-polled backend. */
+/* PLIC/ISR half: read and timestamp one raw sample, then enqueue it. */
 void imu_sensor_irq_handler(void);

-/* Foreground acquisition hook. Safe no-op for an interrupt backend. */
+/* Foreground half. Safe no-op for the current interrupt-driven backends. */
 void imu_sensor_service(void);
 bool imu_sensor_requires_foreground_service(void);

-bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out);
+/* Pop the oldest sample, convert to SI/common axes and apply the selected
+ * backend's calibration.  No averaging or motion decision occurs here. */
+bool imu_sensor_take_measurement(imu_sample_t* measurement_out);
 void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out);
 imu_sensor_backend_t imu_sensor_active_backend(void);
 const char* imu_sensor_backend_name(imu_sensor_backend_t backend);
-bool imu_sensor_quality_profile_for_backend(
-	imu_sensor_backend_t backend,
-	fusion_imu_quality_profile_t* profile_out);
-bool imu_sensor_active_quality_profile(
-	fusion_imu_quality_profile_t* profile_out);
-
 #endif
@@ -1,33 +0,0 @@
-#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
@@ -0,0 +1,30 @@
+#include "adapters/gps_fusion_publisher.h"
+
+#include <stddef.h>
+#include <string.h>
+
+bool gps_fusion_build_measurement(
+	const gps_nav_solution_t* solution,
+	uint64_t measurement_timestamp_ticks,
+	fusion_gps_measurement_t* measurement_out)
+{
+	if((solution == NULL) || (measurement_timestamp_ticks == 0u) ||
+	   (measurement_out == NULL))
+	{
+		return false;
+	}
+	memset(measurement_out, 0, sizeof(*measurement_out));
+	measurement_out->timestamp_ticks = measurement_timestamp_ticks;
+	measurement_out->latitude_deg = solution->latitude_deg;
+	measurement_out->longitude_deg = solution->longitude_deg;
+	measurement_out->height_m = solution->height_m;
+	memcpy(measurement_out->covariance_enu_m2, solution->covariance_enu_m2,
+	       sizeof(measurement_out->covariance_enu_m2));
+	measurement_out->hacc_m = solution->hacc_m;
+	measurement_out->vacc_m = solution->vacc_m;
+	measurement_out->residual_rms_m = solution->residual_rms_m;
+	measurement_out->chi_square_ratio = solution->chi_square_ratio;
+	measurement_out->used_satellites = solution->used_satellite_count;
+	measurement_out->solution_valid = solution->solution_valid;
+	return true;
+}
@@ -0,0 +1,112 @@
+#include "adapters/gps_system_uart.h"
+
+#include <bsp/soc.h>
+#include <stddef.h>
+
+#include "gps/gps_macros.h"
+#include "gps/gps_receiver.h"
+#include "imu/imu_sensor.h"
+
+void gps_system_uart_publish_location(
+	const char* source,
+	double latitude_deg,
+	double longitude_deg,
+	double height_m,
+	const gps_nav_solution_t* gps)
+{
+	if((source == NULL) || (gps == NULL))
+	{
+		return;
+	}
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
+	        "source=%s lat=%.20lf lon=%.20lf height=%.3lf "
+	        "gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
+	        "tracked=%u used=%u\r\n",
+	        source, latitude_deg, longitude_deg, height_m,
+	        gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
+	        gps->tracked_satellite_count, gps->used_satellite_count);
+}
+
+void gps_system_uart_publish_startup(
+	bool fusion_enabled,
+	bool fusion_initialized,
+	bool imu_initialized,
+	bool imu_running)
+{
+	imu_sensor_diagnostics_t imu;
+	imu_sensor_get_diagnostics(&imu);
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
+	        "init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
+	        "running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n",
+	        fusion_enabled ? 1u : 0u,
+	        fusion_initialized ? 1u : 0u,
+	        imu_initialized ? 1u : 0u,
+	        imu_sensor_backend_name(imu.backend),
+	        (unsigned int)imu.who_am_i,
+	        imu_running ? 1u : 0u,
+	        (unsigned long)imu.requested_odr_hz,
+	        (unsigned long)imu.configured_odr_hz,
+	        GPS_NAV_SOLVE_PERIOD_MS,
+	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);
+}
+
+void gps_system_uart_publish_periodic_fusion_status(
+	navigation_fusion_output_t* output,
+	uint64_t* last_status_tick)
+{
+	if((output == NULL) || (last_status_tick == NULL))
+	{
+		return;
+	}
+	uint64_t now = gps_receiver_status_ticks();
+	if((now - *last_status_tick) < gps_receiver_status_ticks_per_second())
+	{
+		return;
+	}
+	*last_status_tick = now;
+
+	imu_sensor_diagnostics_t imu;
+	imu_sensor_get_diagnostics(&imu);
+	if(!navigation_fusion_get_output(output))
+	{
+		return;
+	}
+
+	if(output->global_valid)
+	{
+		GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+		        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
+		        "speed=%.3lf nis=%.3lf gps=%lu/%lu recoveries=%lu "
+		        "zupt=%lu replay=%lu "
+		        "sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu "
+		        "overflow=%lu missed=%lu\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,
+		        (unsigned long)output->gps_position_recoveries,
+		        (unsigned long)output->zero_update_count,
+		        (unsigned long)output->replay_count,
+		        output->position_std_m[0], output->position_std_m[1],
+		        output->position_std_m[2],
+		        (unsigned long)imu.sample_count,
+		        (unsigned long)output->imu_processed_count,
+		        (unsigned long)imu.queue_overflow_count,
+		        (unsigned long)imu.estimated_missed_sample_count);
+		return;
+	}
+
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
+	        "state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
+	        "imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
+	        navigation_fusion_state_name(output->engine_state),
+	        output->calibration_sample_count,
+	        (unsigned long)output->gps_accepted,
+	        (unsigned long)output->gps_nis_rejected,
+	        (unsigned long)output->gps_position_recoveries,
+	        (unsigned long)imu.sample_count,
+	        (unsigned long)output->imu_processed_count,
+	        (unsigned long)imu.queue_overflow_count,
+	        (unsigned long)imu.estimated_missed_sample_count);
+}
@@ -1,4 +1,4 @@
-#include "imu_sensor.h"
+#include "adapters/imu_fusion_profile.h"

 #include <stddef.h>

@@ -13,7 +13,7 @@ static void set_all(double values[3], double value)
 	}
 }

-bool imu_sensor_quality_profile_for_backend(
+bool imu_fusion_quality_profile_for_backend(
 	imu_sensor_backend_t backend,
 	fusion_imu_quality_profile_t* profile_out)
 {
@@ -1,4 +1,4 @@
-#include "st_lsm6dsox.h"
+#include "drivers/imu/st_lsm6dsox.h"

 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -437,7 +437,7 @@ void st_lsm6dsox_service(void)
 }

 bool st_lsm6dsox_take_measurement(
-	fusion_imu_measurement_t* measurement_out)
+	imu_sample_t* measurement_out)
 {
 	if(measurement_out == NULL)
 	{
@@ -1,4 +1,4 @@
-#include "tdk_icm42688.h"
+#include "drivers/imu/tdk_icm42688.h"

 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -342,7 +342,7 @@ void tdk_icm42688_irq_handler(void)
 	__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
 }

-bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out)
+bool tdk_icm42688_take_measurement(imu_sample_t* measurement_out)
 {
 	if(measurement_out == NULL)
 	{
@@ -1,4 +1,4 @@
-#include "eskf.h"
+#include "fusion/eskf.h"

 #include <float.h>
 #include <math.h>
@@ -325,21 +325,41 @@ bool eskf_initialize(eskf_t* filter,

 static bool eskf_propagate_step(eskf_t* filter,
 	                            const eskf_config_t* config,
-	                            const double accel_mps2[3],
-	                            const double gyro_rps[3],
+	                            const double start_accel_mps2[3],
+	                            const double start_gyro_rps[3],
+	                            const double end_accel_mps2[3],
+	                            const double end_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];
+		specific_force[i] =
+			0.5 * (start_accel_mps2[i] + end_accel_mps2[i]) -
+			filter->accel_bias_mps2[i];
+		angular_rate[i] =
+			0.5 * (start_gyro_rps[i] + end_gyro_rps[i]) -
+			filter->gyro_bias_rps[i];
 		filter->last_unbiased_gyro_rps[i] = angular_rate[i];
 	}

+	double half_rotation_vector[3] = {
+		0.5 * angular_rate[0] * dt_s,
+		0.5 * angular_rate[1] * dt_s,
+		0.5 * angular_rate[2] * dt_s
+	};
+	double half_delta_quaternion[4];
+	double midpoint_quaternion[4];
+	rotation_vector_quaternion(half_rotation_vector, half_delta_quaternion);
+	quaternion_multiply(filter->body_to_nav_quaternion, half_delta_quaternion,
+	                    midpoint_quaternion);
+	if(!quaternion_normalize(midpoint_quaternion))
+	{
+		return false;
+	}
 	double rotation[3][3];
-	quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
+	quaternion_rotation_matrix(midpoint_quaternion, rotation);
 	double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
 	for(unsigned int row = 0 ; row < 3u ; row++)
 	{
@@ -412,22 +432,36 @@ static bool eskf_propagate_step(eskf_t* filter,
 	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++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double coefficient = transition[row][k];
+			if(coefficient == 0.0)
+			{
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 			{
 				intermediate[row][col] +=
-					transition[row][k] * filter->covariance[k][col];
+					coefficient * 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++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double value = intermediate[row][k];
+			if(value == 0.0)
 			{
-				propagated[row][col] += intermediate[row][k] * transition[col][k];
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				double coefficient = transition[col][k];
+				if(coefficient != 0.0)
+				{
+					propagated[row][col] += value * coefficient;
+				}
 			}
 		}
 	}
@@ -459,14 +493,20 @@ static bool eskf_propagate_step(eskf_t* filter,
 	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)
+bool eskf_propagate_interval(
+	eskf_t* filter,
+	const eskf_config_t* config,
+	const double start_accel_mps2[3],
+	const double start_gyro_rps[3],
+	const double end_accel_mps2[3],
+	const double end_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) ||
+	   !vector_is_finite(start_accel_mps2, 3u) ||
+	   !vector_is_finite(start_gyro_rps, 3u) ||
+	   !vector_is_finite(end_accel_mps2, 3u) ||
+	   !vector_is_finite(end_gyro_rps, 3u) ||
 	   !isfinite(dt_s) || (dt_s < 0.0) ||
 	   !(config->max_propagation_step_s > 0.0))
 	{
@@ -484,7 +524,28 @@ bool eskf_propagate(eskf_t* filter,
 	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))
+		double start_fraction = (double)step / (double)steps;
+		double end_fraction = (double)(step + 1u) / (double)steps;
+		double step_start_accel[3];
+		double step_start_gyro[3];
+		double step_end_accel[3];
+		double step_end_gyro[3];
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			double accel_delta = end_accel_mps2[axis] - start_accel_mps2[axis];
+			double gyro_delta = end_gyro_rps[axis] - start_gyro_rps[axis];
+			step_start_accel[axis] =
+				start_accel_mps2[axis] + start_fraction * accel_delta;
+			step_end_accel[axis] =
+				start_accel_mps2[axis] + end_fraction * accel_delta;
+			step_start_gyro[axis] =
+				start_gyro_rps[axis] + start_fraction * gyro_delta;
+			step_end_gyro[axis] =
+				start_gyro_rps[axis] + end_fraction * gyro_delta;
+		}
+		if(!eskf_propagate_step(filter, config,
+		                        step_start_accel, step_start_gyro,
+		                        step_end_accel, step_end_gyro, step_s))
 		{
 			return false;
 		}
@@ -492,6 +553,17 @@ bool eskf_propagate(eskf_t* filter,
 	return true;
 }

+bool eskf_propagate(eskf_t* filter,
+	                const eskf_config_t* config,
+	                const double accel_mps2[3],
+	                const double gyro_rps[3],
+	                double dt_s)
+{
+	return eskf_propagate_interval(filter, config,
+	                               accel_mps2, gyro_rps,
+	                               accel_mps2, gyro_rps, dt_s);
+}
+
 static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
 {
 	for(unsigned int i = 0 ; i < 3u ; i++)
@@ -538,21 +610,35 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
 	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++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double coefficient = reset[row][k];
+			if(coefficient == 0.0)
 			{
-				intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				intermediate[row][col] += coefficient * 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++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double value = intermediate[row][k];
+			if(value == 0.0)
 			{
-				reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				double coefficient = reset[col][k];
+				if(coefficient != 0.0)
+				{
+					reset_covariance[row][col] += value * coefficient;
+				}
 			}
 		}
 	}
@@ -646,12 +732,48 @@ static bool eskf_selected_update(eskf_t* filter,
 	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++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			double coefficient = joseph_left[row][k];
+			if(coefficient == 0.0)
+			{
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 			{
 				intermediate[row][col] +=
-					joseph_left[row][k] * filter->covariance[k][col];
+					coefficient * filter->covariance[k][col];
+			}
+		}
+	}
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+		{
+			double value = intermediate[row][k];
+			if(value == 0.0)
+			{
+				continue;
+			}
+			for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
+			{
+				double coefficient = joseph_left[col][k];
+				if(coefficient != 0.0)
+				{
+					updated_covariance[row][col] += value * coefficient;
+				}
+			}
+		}
+	}
+	double gain_times_covariance[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
+	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
+	{
+		for(unsigned int second = 0 ; second < 3u ; second++)
+		{
+			for(unsigned int first = 0 ; first < 3u ; first++)
+			{
+				gain_times_covariance[row][second] += gain[row][first] *
+					measurement_covariance[first * 3u + second];
 			}
 		}
 	}
@@ -659,18 +781,10 @@ static bool eskf_selected_update(eskf_t* filter,
 	{
 		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
 		{
-			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
+			for(unsigned int second = 0 ; second < 3u ; second++)
 			{
 				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];
-				}
+					gain_times_covariance[row][second] * gain[col][second];
 			}
 		}
 	}
@@ -729,6 +843,50 @@ eskf_update_result_t eskf_position_update(
 	return ESKF_UPDATE_ACCEPTED;
 }

+bool eskf_reset_position(
+	eskf_t* filter,
+	const double position_m[3],
+	const double covariance_m2[9])
+{
+	if((filter == NULL) || !filter->initialized ||
+	   !vector_is_finite(position_m, 3u) ||
+	   !vector_is_finite(covariance_m2, 9u))
+	{
+		return false;
+	}
+
+	/* inverse_symmetric3() also rejects singular/non-positive-definite input. */
+	double unused_inverse[9];
+	if(!inverse_symmetric3(covariance_m2, unused_inverse))
+	{
+		return false;
+	}
+
+	memcpy(filter->position_m, position_m, 3u * sizeof(double));
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		unsigned int state = ESKF_POSITION_INDEX + axis;
+		for(unsigned int other = 0u ; other < ESKF_ERROR_STATE_DIM ; other++)
+		{
+			filter->covariance[state][other] = 0.0;
+			filter->covariance[other][state] = 0.0;
+		}
+	}
+	for(unsigned int row = 0u ; row < 3u ; row++)
+	{
+		for(unsigned int col = 0u ; col < 3u ; col++)
+		{
+			filter->covariance[ESKF_POSITION_INDEX + row]
+			                  [ESKF_POSITION_INDEX + col] =
+				covariance_m2[row * 3u + col];
+		}
+	}
+	covariance_symmetrize_and_floor(filter, 1.0e-12);
+	filter->last_position_nis = NAN;
+	filter->position_update_accepted_count++;
+	return eskf_state_is_finite(filter);
+}
+
 bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
 {
 	if((filter == NULL) || !filter->initialized ||
@@ -1,4 +1,4 @@
-#include "fusion_bus.h"
+#include "fusion/fusion_bus.h"

 #include <stddef.h>
 #include <string.h>
@@ -1,4 +1,4 @@
-#include "fusion_engine.h"
+#include "fusion/fusion_engine.h"

 #include <float.h>
 #include <math.h>
@@ -123,16 +123,26 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
 	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;
+	/* PX4 EKF2 fuses aligned zero velocity at 5 Hz with 0.2 m/s sigma. */
+	config->zero_update_period_s = 0.20;
+	config->zero_velocity_std_mps = 0.20;
 	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_motion_window_s = 1.0;
 	config->gnss_moving_confirmations = 2u;
 	config->gnss_still_confirmations = 2u;
+	/* A hard innovation gate protects against isolated GNSS outliers, but it
+	 * must not permanently lock the filter to a bad startup position.  After a
+	 * full second of mutually continuous rejected fixes, re-anchor position
+	 * while preserving inertial velocity, attitude and bias states. */
+	config->position_recovery_timeout_s = 7.0;
+	config->position_recovery_max_speed_mps = 100.0;
+	config->position_recovery_sigma_multiplier = 5.0;
+	config->position_recovery_min_samples = 3u;
 }

 bool fusion_engine_apply_imu_quality(
@@ -178,8 +188,17 @@ static bool config_valid(const fusion_engine_config_t* config)
 	       isfinite(config->zero_velocity_std_mps) &&
 	       (config->zero_velocity_std_mps > 0.0) &&
 	       finite_values(config->zero_gyro_std_rps, 3u) &&
+	       isfinite(config->gnss_motion_window_s) &&
+	       (config->gnss_motion_window_s > 0.0) &&
 	       (config->gnss_moving_confirmations != 0u) &&
-	       (config->gnss_still_confirmations != 0u);
+	       (config->gnss_still_confirmations != 0u) &&
+	       isfinite(config->position_recovery_timeout_s) &&
+	       (config->position_recovery_timeout_s > 0.0) &&
+	       isfinite(config->position_recovery_max_speed_mps) &&
+	       (config->position_recovery_max_speed_mps > 0.0) &&
+	       isfinite(config->position_recovery_sigma_multiplier) &&
+	       (config->position_recovery_sigma_multiplier > 0.0) &&
+	       (config->position_recovery_min_samples >= 2u);
 }

 bool fusion_engine_init(fusion_engine_t* engine,
@@ -322,26 +341,32 @@ static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_tic

 static bool runtime_propagate_to(fusion_engine_t* engine,
 	                             fusion_runtime_t* runtime,
-	                             uint64_t timestamp_ticks,
-	                             const fusion_imu_measurement_t* input)
+	                             const fusion_imu_measurement_t* end_input)
 {
-	if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized ||
-	   (timestamp_ticks < runtime->filter_timestamp_ticks))
+	if((runtime == NULL) || (end_input == NULL) ||
+	   !runtime->filter.initialized || !runtime->have_held_imu ||
+	   (runtime->filter_timestamp_ticks != runtime->held_imu.timestamp_ticks) ||
+	   (end_input->timestamp_ticks < runtime->filter_timestamp_ticks))
 	{
 		return false;
 	}
-	if(timestamp_ticks == runtime->filter_timestamp_ticks)
+	if(end_input->timestamp_ticks == runtime->filter_timestamp_ticks)
 	{
+		runtime->held_imu = *end_input;
 		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))
+	                              end_input->timestamp_ticks -
+	                              runtime->filter_timestamp_ticks);
+	if(!eskf_propagate_interval(
+		&runtime->filter, &engine->config.eskf,
+		runtime->held_imu.accel_mps2, runtime->held_imu.gyro_rps,
+		end_input->accel_mps2, end_input->gyro_rps, dt_s))
 	{
 		return false;
 	}
-	runtime->filter_timestamp_ticks = timestamp_ticks;
+	runtime->filter_timestamp_ticks = end_input->timestamp_ticks;
+	runtime->held_imu = *end_input;
 	return true;
 }

@@ -364,7 +389,8 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 	}
 	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))
+	fusion_imu_measurement_t start_measurement = runtime->held_imu;
+	if(!runtime_propagate_to(engine, runtime, measurement))
 	{
 		return false;
 	}
@@ -388,31 +414,82 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
 		runtime->gnss_moving_count = 0u;
 		runtime->gnss_still_count = 0u;
 	}
-	runtime->held_imu = *measurement;
-
+	runtime->vehicle_at_rest = measurement->vehicle_at_rest;
+	bool at_rest_continuing = measurement->vehicle_at_rest &&
+	                          start_measurement.vehicle_at_rest;
+	if(measurement->vehicle_at_rest)
+	{
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			double mean_gyro = 0.5 *
+				(start_measurement.gyro_rps[axis] + measurement->gyro_rps[axis]);
+			runtime->zero_gyro_integral_rad[axis] += mean_gyro * motion_dt_s;
+		}
+		runtime->zero_gyro_duration_s += motion_dt_s;
+	}
+	else
+	{
+		memset(runtime->zero_gyro_integral_rad, 0,
+		       sizeof(runtime->zero_gyro_integral_rad));
+		runtime->zero_gyro_duration_s = 0.0;
+	}
 	uint64_t zero_period_ticks =
 		seconds_to_ticks(engine, engine->config.zero_update_period_s);
-	if(fusion_motion_allows_zero_update(&runtime->motion) &&
+	if(at_rest_continuing &&
 	   ((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))
+		double mean_gyro_rps[3];
+		if(!(runtime->zero_gyro_duration_s > 0.0))
 		{
 			return false;
 		}
+		for(unsigned int axis = 0u ; axis < 3u ; axis++)
+		{
+			mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
+			                      runtime->zero_gyro_duration_s;
+		}
+		if(!eskf_zero_velocity_update(
+			&runtime->filter, engine->config.zero_velocity_std_mps) ||
+		   !eskf_zero_angular_rate_update(
+			&runtime->filter, mean_gyro_rps, engine->config.zero_gyro_std_rps))
+		{
+			return false;
+		}
+		memset(runtime->zero_gyro_integral_rad, 0,
+		       sizeof(runtime->zero_gyro_integral_rad));
+		runtime->zero_gyro_duration_s = 0.0;
 		runtime->last_zero_update_ticks = measurement->timestamp_ticks;
 		if(count_zero_updates)
 		{
 			engine->diagnostics.zero_velocity_update_count++;
+			engine->diagnostics.zero_gyro_update_count++;
 		}
 	}
 	return true;
 }

+static fusion_imu_measurement_t interpolate_imu(
+	const fusion_imu_measurement_t* start,
+	const fusion_imu_measurement_t* end,
+	uint64_t timestamp_ticks)
+{
+	fusion_imu_measurement_t interpolated = *start;
+	interpolated.timestamp_ticks = timestamp_ticks;
+	double fraction = (double)(timestamp_ticks - start->timestamp_ticks) /
+	                  (double)(end->timestamp_ticks - start->timestamp_ticks);
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		interpolated.accel_mps2[axis] = start->accel_mps2[axis] + fraction *
+			(end->accel_mps2[axis] - start->accel_mps2[axis]);
+		interpolated.gyro_rps[axis] = start->gyro_rps[axis] + fraction *
+			(end->gyro_rps[axis] - start->gyro_rps[axis]);
+	}
+	interpolated.temperature_c = start->temperature_c + fraction *
+		(end->temperature_c - start->temperature_c);
+	return interpolated;
+}
+
 static bool finish_calibration(fusion_engine_t* engine,
 	                           const fusion_imu_measurement_t* last_measurement)
 {
@@ -461,6 +538,8 @@ static bool finish_calibration(fusion_engine_t* engine,
 	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.last_position_fuse_ticks = last_measurement->timestamp_ticks;
+	engine->current.vehicle_at_rest = last_measurement->vehicle_at_rest;
 	engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
 	if(engine->have_initial_position)
 	{
@@ -514,8 +593,13 @@ static bool process_calibration_imu(fusion_engine_t* engine,
 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)
+	const fusion_position_measurement_t* measurement,
+	bool* window_complete_out)
 {
+	if(window_complete_out != NULL)
+	{
+		*window_complete_out = false;
+	}
 	if(!runtime->have_accepted_position ||
 	   (measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
 	{
@@ -527,6 +611,14 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
 	{
 		return FUSION_GNSS_MOTION_UNKNOWN;
 	}
+	if(dt_s < engine->config.gnss_motion_window_s)
+	{
+		return FUSION_GNSS_MOTION_UNKNOWN;
+	}
+	if(window_complete_out != NULL)
+	{
+		*window_complete_out = true;
+	}
 	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] +
@@ -556,6 +648,114 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
 	return FUSION_GNSS_MOTION_STILL;
 }

+static void position_recovery_clear(fusion_engine_t* engine)
+{
+	engine->position_recovery_active = false;
+	engine->position_recovery_start_ticks = 0u;
+	engine->position_recovery_consistent_count = 0u;
+	memset(&engine->position_recovery_candidate, 0,
+	       sizeof(engine->position_recovery_candidate));
+}
+
+static void position_recovery_begin(
+	fusion_engine_t* engine,
+	const fusion_position_measurement_t* measurement)
+{
+	engine->position_recovery_candidate = *measurement;
+	engine->position_recovery_start_ticks = measurement->timestamp_ticks;
+	engine->position_recovery_consistent_count = 1u;
+	engine->position_recovery_active = true;
+}
+
+static bool position_recovery_is_continuous(
+	const fusion_engine_t* engine,
+	const fusion_position_measurement_t* previous,
+	const fusion_position_measurement_t* current)
+{
+	if(current->timestamp_ticks <= previous->timestamp_ticks)
+	{
+		return false;
+	}
+	double dt_s = ticks_to_seconds(
+		engine, current->timestamp_ticks - previous->timestamp_ticks);
+	double displacement_squared = 0.0;
+	double combined_variance = 0.0;
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		double delta = current->position_enu_m[axis] -
+		               previous->position_enu_m[axis];
+		displacement_squared += delta * delta;
+		double variance = current->covariance_enu_m2[axis * 3u + axis] +
+		                  previous->covariance_enu_m2[axis * 3u + axis];
+		if(!(variance > 0.0) || !isfinite(variance))
+		{
+			return false;
+		}
+		combined_variance += variance;
+	}
+	double allowed_displacement =
+		engine->config.position_recovery_max_speed_mps * dt_s +
+		engine->config.position_recovery_sigma_multiplier *
+			sqrt(combined_variance);
+	return isfinite(displacement_squared) &&
+	       (displacement_squared <= square(allowed_displacement));
+}
+
+static bool position_recovery_ready(
+	fusion_engine_t* engine,
+	const fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement)
+{
+	if(!engine->position_recovery_active)
+	{
+		position_recovery_begin(engine, measurement);
+		return false;
+	}
+	if(!position_recovery_is_continuous(
+		engine, &engine->position_recovery_candidate, measurement))
+	{
+		/* A discontinuity is an isolated outlier or a new candidate track; it
+		 * must earn a complete recovery interval of its own. */
+		position_recovery_begin(engine, measurement);
+		return false;
+	}
+	engine->position_recovery_candidate = *measurement;
+	if(engine->position_recovery_consistent_count < UINT32_MAX)
+	{
+		engine->position_recovery_consistent_count++;
+	}
+	if(engine->position_recovery_consistent_count <
+	   engine->config.position_recovery_min_samples)
+	{
+		return false;
+	}
+	if(measurement->timestamp_ticks < runtime->last_position_fuse_ticks)
+	{
+		return false;
+	}
+	uint64_t elapsed_ticks = measurement->timestamp_ticks -
+	                         runtime->last_position_fuse_ticks;
+	return elapsed_ticks >= seconds_to_ticks(
+		engine, engine->config.position_recovery_timeout_s);
+}
+
+static void runtime_record_recovered_position(
+	fusion_runtime_t* runtime,
+	const fusion_position_measurement_t* measurement)
+{
+	runtime->have_accepted_position = true;
+	runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
+	runtime->last_position_fuse_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));
+	runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
+	runtime->gnss_moving_count = 0u;
+	runtime->gnss_still_count = 0u;
+}
+
 static eskf_update_result_t runtime_apply_position(
 	fusion_engine_t* engine,
 	fusion_runtime_t* runtime,
@@ -567,8 +767,11 @@ static eskf_update_result_t runtime_apply_position(
 		measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
 	if(result == ESKF_UPDATE_ACCEPTED)
 	{
+		runtime->last_position_fuse_ticks = measurement->timestamp_ticks;
+		bool motion_window_complete = false;
 		fusion_gnss_motion_evidence_t raw_evidence =
-			position_motion_evidence(engine, runtime, measurement);
+			position_motion_evidence(engine, runtime, measurement,
+			                         &motion_window_complete);
 		if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
 		{
 			runtime->gnss_still_count = 0u;
@@ -593,13 +796,19 @@ static eskf_update_result_t runtime_apply_position(
 				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));
+		/* Keep a long-enough displacement baseline.  At a 200 ms GNSS period,
+		 * replacing the reference on every fix would classify statistically
+		 * inconclusive 10--20 cm steps as stationary evidence. */
+		if(!runtime->have_accepted_position || motion_window_complete)
+		{
+			runtime->have_accepted_position = true;
+			runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
+			memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
+			       sizeof(runtime->last_accepted_position_m));
+			memcpy(runtime->last_accepted_position_covariance_m2,
+			       measurement->covariance_enu_m2,
+			       sizeof(runtime->last_accepted_position_covariance_m2));
+		}
 	}
 	return result;
 }
@@ -617,11 +826,13 @@ static bool process_position_now(fusion_engine_t* engine,
 	}
 	fusion_runtime_t revised = engine->checkpoint;
 	uint64_t replayed = 0u;
+	const fusion_imu_measurement_t* next_imu = NULL;
 	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)
 		{
+			next_imu = imu;
 			break;
 		}
 		if(!runtime_apply_imu(engine, &revised, imu, false))
@@ -633,9 +844,20 @@ static bool process_position_now(fusion_engine_t* engine,
 	}
 	if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
 	{
-		if(!revised.have_held_imu ||
-		   !runtime_propagate_to(engine, &revised,
-		                         measurement->timestamp_ticks, &revised.held_imu))
+		if(!revised.have_held_imu)
+		{
+			engine->state = FUSION_ENGINE_FAULT;
+			return false;
+		}
+		fusion_imu_measurement_t endpoint = revised.held_imu;
+		endpoint.timestamp_ticks = measurement->timestamp_ticks;
+		if((next_imu != NULL) &&
+		   (next_imu->timestamp_ticks > revised.held_imu.timestamp_ticks))
+		{
+			endpoint = interpolate_imu(&revised.held_imu, next_imu,
+			                           measurement->timestamp_ticks);
+		}
+		if(!runtime_propagate_to(engine, &revised, &endpoint))
 		{
 			engine->state = FUSION_ENGINE_FAULT;
 			return false;
@@ -649,6 +871,23 @@ static bool process_position_now(fusion_engine_t* engine,
 		return false;
 	}
 	if(result == ESKF_UPDATE_ACCEPTED)
+	{
+		position_recovery_clear(engine);
+	}
+	else if((result == ESKF_UPDATE_REJECTED_NIS) &&
+	        position_recovery_ready(engine, &revised, measurement) &&
+	        eskf_reset_position(&revised.filter, measurement->position_enu_m,
+	                            measurement->covariance_enu_m2))
+	{
+		/* This is a deliberate position-state reset, not a Kalman update with
+		 * a disabled gate.  The independent IMU-derived attitude and bias state
+		 * survive, while invalid position cross-covariances are discarded. */
+		runtime_record_recovered_position(&revised, measurement);
+		engine->diagnostics.position_recovery_count++;
+		position_recovery_clear(engine);
+		result = ESKF_UPDATE_ACCEPTED;
+	}
+	if(result == ESKF_UPDATE_ACCEPTED)
 	{
 		engine->diagnostics.position_accepted++;
 	}
@@ -1,4 +1,4 @@
-#include "fusion_gps_adapter.h"
+#include "fusion/fusion_gps_adapter.h"

 #include <float.h>
 #include <math.h>
@@ -1,4 +1,4 @@
-#include "fusion_motion.h"
+#include "fusion/fusion_motion.h"

 #include <math.h>
 #include <stddef.h>
@@ -0,0 +1,144 @@
+#include "fusion/fusion_rest_detector.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static double difference_norm3(const double current[3], const double previous[3])
+{
+	double sum = 0.0;
+	for(unsigned int axis = 0u ; axis < 3u ; axis++)
+	{
+		double difference = current[axis] - previous[axis];
+		sum += difference * difference;
+	}
+	return sqrt(sum);
+}
+
+void fusion_rest_detector_default_config(fusion_rest_detector_config_t* config)
+{
+	if(config == NULL)
+	{
+		return;
+	}
+	/* Same high-frequency vibration metric and limits used by PX4's current
+	 * VehicleIMU/LandDetector path. */
+	/* PX4's 0.99/0.01 filter at a 250 Hz reference rate, represented as a
+	 * physical time constant so 52/104/208 Hz sensors behave consistently. */
+	config->vibration_filter_time_constant_s =
+		-0.004 / log(0.99);
+	config->accel_vibration_limit_mps2 = 1.2;
+	config->gyro_vibration_limit_rps = 0.02;
+	config->rest_dwell_s = 1.0;
+}
+
+static bool config_valid(const fusion_rest_detector_config_t* config)
+{
+	return (config != NULL) &&
+	       isfinite(config->vibration_filter_time_constant_s) &&
+	       (config->vibration_filter_time_constant_s > 0.0) &&
+	       isfinite(config->accel_vibration_limit_mps2) &&
+	       (config->accel_vibration_limit_mps2 > 0.0) &&
+	       isfinite(config->gyro_vibration_limit_rps) &&
+	       (config->gyro_vibration_limit_rps > 0.0) &&
+	       isfinite(config->rest_dwell_s) && (config->rest_dwell_s > 0.0);
+}
+
+bool fusion_rest_detector_init(
+	fusion_rest_detector_t* detector,
+	const fusion_rest_detector_config_t* config,
+	uint64_t timebase_hz,
+	bool landed)
+{
+	if((detector == NULL) || !config_valid(config) || (timebase_hz == 0u))
+	{
+		return false;
+	}
+	memset(detector, 0, sizeof(*detector));
+	detector->config = *config;
+	detector->timebase_hz = timebase_hz;
+	detector->landed = landed;
+	return true;
+}
+
+void fusion_rest_detector_set_landed(
+	fusion_rest_detector_t* detector,
+	bool landed,
+	uint64_t timestamp_ticks)
+{
+	if(detector == NULL)
+	{
+		return;
+	}
+	detector->landed = landed;
+	if(!landed)
+	{
+		detector->at_rest = false;
+		detector->last_motion_ticks = timestamp_ticks;
+	}
+}
+
+bool fusion_rest_detector_update(
+	fusion_rest_detector_t* detector,
+	const imu_sample_t* sample)
+{
+	if((detector == NULL) || (sample == NULL) ||
+	   !isfinite(sample->accel_mps2[0]) ||
+	   !isfinite(sample->accel_mps2[1]) ||
+	   !isfinite(sample->accel_mps2[2]) ||
+	   !isfinite(sample->gyro_rps[0]) ||
+	   !isfinite(sample->gyro_rps[1]) ||
+	   !isfinite(sample->gyro_rps[2]) ||
+	   (detector->have_previous_sample &&
+	    (sample->timestamp_ticks <= detector->last_sample_ticks)))
+	{
+		return false;
+	}
+
+	if(!detector->have_previous_sample)
+	{
+		memcpy(detector->previous_accel_mps2, sample->accel_mps2,
+		       sizeof(detector->previous_accel_mps2));
+		memcpy(detector->previous_gyro_rps, sample->gyro_rps,
+		       sizeof(detector->previous_gyro_rps));
+		detector->last_sample_ticks = sample->timestamp_ticks;
+		detector->last_motion_ticks = sample->timestamp_ticks;
+		detector->have_previous_sample = true;
+		detector->at_rest = false;
+		return true;
+	}
+
+	double accel_difference = difference_norm3(
+		sample->accel_mps2, detector->previous_accel_mps2);
+	double gyro_difference = difference_norm3(
+		sample->gyro_rps, detector->previous_gyro_rps);
+	double dt_s = (double)(sample->timestamp_ticks - detector->last_sample_ticks) /
+	              (double)detector->timebase_hz;
+	double alpha = 1.0 - exp(
+		-dt_s / detector->config.vibration_filter_time_constant_s);
+	detector->accel_vibration_metric_mps2 =
+		(1.0 - alpha) * detector->accel_vibration_metric_mps2 +
+		alpha * accel_difference;
+	detector->gyro_vibration_metric_rps =
+		(1.0 - alpha) * detector->gyro_vibration_metric_rps +
+		alpha * gyro_difference;
+	memcpy(detector->previous_accel_mps2, sample->accel_mps2,
+	       sizeof(detector->previous_accel_mps2));
+	memcpy(detector->previous_gyro_rps, sample->gyro_rps,
+	       sizeof(detector->previous_gyro_rps));
+	detector->last_sample_ticks = sample->timestamp_ticks;
+
+	if((detector->accel_vibration_metric_mps2 >
+	    detector->config.accel_vibration_limit_mps2) ||
+	   (detector->gyro_vibration_metric_rps >
+	    detector->config.gyro_vibration_limit_rps))
+	{
+		detector->last_motion_ticks = sample->timestamp_ticks;
+	}
+
+	uint64_t dwell_ticks = (uint64_t)(
+		detector->config.rest_dwell_s * (double)detector->timebase_hz + 0.5);
+	detector->at_rest = detector->landed &&
+		((sample->timestamp_ticks - detector->last_motion_ticks) >= dwell_ticks);
+	return true;
+}
@@ -0,0 +1,162 @@
+#include "fusion/navigation_fusion.h"
+
+#include "fusion/fusion_gps_adapter.h"
+#include "fusion/fusion_rest_detector.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+static fusion_gps_adapter_t gps_adapter;
+static fusion_engine_t fusion_engine;
+static fusion_rest_detector_t rest_detector;
+static bool fusion_initialized;
+static uint64_t imu_processed_count;
+
+bool navigation_fusion_init(
+	uint64_t local_timebase_hz,
+	const fusion_imu_quality_profile_t* imu_quality,
+	bool platform_landed)
+{
+	fusion_engine_config_t engine_config;
+	fusion_gps_adapter_config_t adapter_config;
+	fusion_rest_detector_config_t rest_config;
+	if((local_timebase_hz == 0u) ||
+	   !fusion_imu_quality_is_valid(imu_quality))
+	{
+		return false;
+	}
+	fusion_engine_default_config(&engine_config, local_timebase_hz);
+	if(!fusion_engine_apply_imu_quality(&engine_config, imu_quality))
+	{
+		return false;
+	}
+	fusion_gps_adapter_default_config(&adapter_config);
+	fusion_rest_detector_default_config(&rest_config);
+	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
+	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config) ||
+	   !fusion_rest_detector_init(
+		&rest_detector, &rest_config, local_timebase_hz, platform_landed))
+	{
+		return false;
+	}
+	imu_processed_count = 0u;
+	fusion_initialized = true;
+	return true;
+}
+
+void navigation_fusion_set_platform_landed(
+	bool platform_landed,
+	uint64_t timestamp_ticks)
+{
+	if(fusion_initialized)
+	{
+		fusion_rest_detector_set_landed(
+			&rest_detector, platform_landed, timestamp_ticks);
+	}
+}
+
+bool navigation_fusion_push_imu(const imu_sample_t* measurement)
+{
+	if(!fusion_initialized || (measurement == NULL))
+	{
+		return false;
+	}
+	if(!fusion_rest_detector_update(&rest_detector, measurement))
+	{
+		return false;
+	}
+	fusion_imu_measurement_t fusion_measurement;
+	memset(&fusion_measurement, 0, sizeof(fusion_measurement));
+	fusion_measurement.timestamp_ticks = measurement->timestamp_ticks;
+	memcpy(fusion_measurement.accel_mps2, measurement->accel_mps2,
+	       sizeof(fusion_measurement.accel_mps2));
+	memcpy(fusion_measurement.gyro_rps, measurement->gyro_rps,
+	       sizeof(fusion_measurement.gyro_rps));
+	fusion_measurement.temperature_c = measurement->temperature_c;
+	fusion_measurement.sequence = measurement->sequence;
+	fusion_measurement.vehicle_at_rest = rest_detector.at_rest;
+	if(!fusion_engine_push_imu(&fusion_engine, &fusion_measurement))
+	{
+		return false;
+	}
+	imu_processed_count++;
+	return true;
+}
+
+bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement)
+{
+	if(!fusion_initialized)
+	{
+		return false;
+	}
+	fusion_position_measurement_t position;
+	if(!fusion_gps_adapter_convert(&gps_adapter, measurement, &position))
+	{
+		return false;
+	}
+	return fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
+}
+
+bool navigation_fusion_get_output(navigation_fusion_output_t* output)
+{
+	if((output == NULL) || !fusion_initialized)
+	{
+		return false;
+	}
+	memset(output, 0, sizeof(*output));
+	output->engine_state = fusion_engine.state;
+	output->calibration_sample_count = fusion_engine.calibration.sample_count;
+	output->gps_accepted = fusion_engine.diagnostics.position_accepted;
+	output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
+	output->gps_position_recoveries =
+		fusion_engine.diagnostics.position_recovery_count;
+	output->zero_update_count =
+		fusion_engine.diagnostics.zero_velocity_update_count;
+	output->replay_count = fusion_engine.diagnostics.replay_count;
+	output->stale_gps_count =
+		fusion_engine.diagnostics.position_stale_rejected;
+	output->imu_processed_count = imu_processed_count;
+	if(fusion_engine.state != FUSION_ENGINE_RUNNING)
+	{
+		return true;
+	}
+	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->vehicle_at_rest = runtime->vehicle_at_rest;
+	output->local_valid = true;
+	if(!fusion_gps_adapter_local_to_geodetic(
+		&gps_adapter, runtime->filter.position_m,
+		&output->latitude_deg, &output->longitude_deg, &output->height_m))
+	{
+		/* IMU propagation is still a valid local dead-reckoning state.  Without
+		 * an externally established origin it must not be labelled as LLH. */
+		return true;
+	}
+	output->global_valid = true;
+	output->valid = true;
+	return true;
+}
+
+const char* navigation_fusion_state_name(fusion_engine_state_t state)
+{
+	switch(state)
+	{
+		case FUSION_ENGINE_CALIBRATING: return "calibrating";
+		case FUSION_ENGINE_RUNNING: return "running";
+		case FUSION_ENGINE_FAULT: return "fault";
+		default: return "unknown";
+	}
+}
@@ -1,11 +1,11 @@
 #include <stdlib.h>
-#include "NavEquations.h"
-#include "Navigation.h"
-#include "gps_ip_defines.h"
-#include "gps.h"
+#include "gps/NavEquations.h"
+#include "gps/Navigation.h"
+#include "gps/gps_ip_defines.h"
+#include "gps/gps.h"
 #include <math.h>
 #include <string.h>
-#include "defines.h"
+#include "gps/defines.h"


 /* Function:  double computeRange(range_t* rho, ephem_t eph, ionoutc_t* ionoutc, gpstime_t g, double xyz[], t_temp_pos* tempPos)
@@ -1199,7 +1199,13 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
 	solution_out->residual_test_valid = residual_test_valid;
 	solution_out->chi_square_pass = chi_square_pass;
 	solution_out->fde_applied = false;
-	solution_out->solution_valid = chi_square_pass;
+	/* Four observations exactly determine x/y/z plus receiver clock bias.  With
+	 * zero residual DOF no chi-square integrity statement is possible, but the
+	 * finite WLS solution and geometry covariance are still a valid minimum fix. */
+	solution_out->solution_valid = solution_out->candidate_valid &&
+		(((solution_out->used_satellite_count == 4u) &&
+		  (solution_out->degrees_of_freedom == 0u)) ||
+		 (solution_out->residual_test_valid && solution_out->chi_square_pass));
 	solution_out->iteration_count = iteration_count;
 	solution_out->candidate_cycles = gps_read_cycle_counter() - candidate_start_cycles;

@@ -1386,8 +1392,10 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 	                             ? (gps_read_cycle_counter() - fde_start_cycles) : 0u;
 	selected_solution.total_wls_cycles = gps_read_cycle_counter() - total_wls_start_cycles;
 	selected_solution.solution_valid = selected_solution.candidate_valid &&
-	                                   selected_solution.residual_test_valid &&
-	                                   selected_solution.chi_square_pass;
+		(((selected_solution.used_satellite_count == 4u) &&
+		  (selected_solution.degrees_of_freedom == 0u)) ||
+		 (selected_solution.residual_test_valid &&
+		  selected_solution.chi_square_pass));
 	if(selected_solution.solution_valid)
 	{
 		gps_wls_last_solution[0] = selected_solution.ecef_m[0];
@@ -1398,12 +1406,19 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 	}
 	*solution_out = selected_solution;

-	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
-	        "GDOP=%.10lf\r\n", solution_out->gdop);
-	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
-	        "position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
-	        solution_out->latitude_deg, solution_out->longitude_deg,
-	        solution_out->height_m);
+	/* Print only publishable fixes.  A four-satellite minimum fix is publishable,
+	 * although its separate integrity flag remains false because residual DOF=0. */
+	if(solution_out->solution_valid)
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "DOP gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf\r\n",
+		        solution_out->gdop, solution_out->pdop, solution_out->hdop,
+		        solution_out->vdop, solution_out->tdop);
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
+		        solution_out->latitude_deg, solution_out->longitude_deg,
+		        solution_out->height_m);
+	}
 	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
 	        "NAVSOL valid=%u integrity=%u fde=%u sats=%u/%u excluded=0x%03x "
 	        "residual_rms=%.3lf max_residual=%.3lf weighted_rms=%.3lf "
@@ -1,9 +1,8 @@
-#include "linked_list.h"
-#include "gps_interrupt_handlers.h"
-#include "Navigation.h"
-#include "gps_pps.h"
-#include "gps_time_anchor.h"
-#include "imu_sensor.h"
+#include "gps/linked_list.h"
+#include "gps/gps_interrupt_handlers.h"
+#include "gps/Navigation.h"
+#include "gps/gps_pps.h"
+#include "gps/gps_time_anchor.h"
 #include <bsp/irq.h>
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -74,6 +73,8 @@ static volatile bool gps_timer_maintenance_pending = true;
 static volatile bool gps_nav_solve_period_pending = false;
 static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
 static unsigned int gps_acq_round_robin_prn = 0;
+static unsigned int gps_external_irq_id = 0u;
+static gps_external_irq_handler_t gps_external_irq_handler = NULL;

 #define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)

@@ -295,6 +296,8 @@ static void gps_runtime_state_init(void)
 	gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
 	gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
 	gps_acq_round_robin_prn = 0;
+	gps_external_irq_id = 0u;
+	gps_external_irq_handler = NULL;

 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
 	{
@@ -325,6 +328,22 @@ static void gps_runtime_state_init(void)

 }

+bool gps_interrupt_attach_external_source(
+	unsigned int irq_id,
+	gps_external_irq_handler_t handler)
+{
+	if((irq_id == 0u) || (handler == NULL))
+	{
+		return false;
+	}
+
+	uint64_t irq_state = bsp_irq_save();
+	gps_external_irq_id = irq_id;
+	gps_external_irq_handler = handler;
+	bsp_irq_restore(irq_state);
+	return true;
+}
+
 static void gps_dispatch_tracking(unsigned char trck_ip_no)
 {
 	trck_intr_handler(
@@ -395,9 +414,10 @@ void gps_interrupt_init(void)

 void gps_dispatch_irq(unsigned int irq_id)
 {
-	if(irq_id == BSP_PMOD_IRQ)
+	if((irq_id == gps_external_irq_id) &&
+	   (gps_external_irq_handler != NULL))
 	{
-		imu_sensor_irq_handler();
+		gps_external_irq_handler();
 		return;
 	}

@@ -3,14 +3,13 @@
 #include <stdlib.h>
 #include <math.h>
 #include <string.h>
-#include "NavEquations.h"
-#include "Navigation.h"
-#include "gps_ip_defines.h"
-#include "linked_list.h"
-#include "gps_interrupt_handlers.h"
-#include "gps_pps.h"
-#include "gps_fusion_publisher.h"
-#include "defines.h"
+#include "gps/NavEquations.h"
+#include "gps/Navigation.h"
+#include "gps/gps_ip_defines.h"
+#include "gps/linked_list.h"
+#include "gps/gps_interrupt_handlers.h"
+#include "gps/gps_pps.h"
+#include "gps/defines.h"
 #include <bsp/irq.h>

 #define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES ((uint32_t)(SAMPLES_PER_CODE * (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
@@ -1086,11 +1085,19 @@ static void gps_log_nav_jump_diagnostic(const gps_nav_solution_t* solution,
 #endif

 #ifdef EKF_ENABLE
-void calculate_position(Ekf* g_ekf)
+bool calculate_position(Ekf* g_ekf, gps_nav_solution_t* solution_out,
+	                    uint64_t* measurement_timestamp_ticks_out)
 #else
-void calculate_position()
+bool calculate_position(gps_nav_solution_t* solution_out,
+	                    uint64_t* measurement_timestamp_ticks_out)
 #endif
 {
+	if((solution_out == NULL) || (measurement_timestamp_ticks_out == NULL))
+	{
+		return false;
+	}
+	*measurement_timestamp_ticks_out = 0u;
+	bool result_ready = false;
 	gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT];
 	unsigned char snapshot_ips[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
@@ -1186,13 +1193,14 @@ void calculate_position()
 		uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
 		if(nav_ok)
 		{
+			nav_solution.tracked_satellite_count = busy_count;
 			uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
 				snapshot_local_timestamps, satellite_count);
-			if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp))
+			if(measurement_timestamp != 0u)
 			{
-				GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
-				        "fusion GPS publish skipped timestamp=%lu\r\n",
-				        (unsigned long)measurement_timestamp);
+				*solution_out = nav_solution;
+				*measurement_timestamp_ticks_out = measurement_timestamp;
+				result_ready = true;
 			}
 			uint64_t consume_irq_state = bsp_irq_save();
 			gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
@@ -1264,6 +1272,7 @@ void calculate_position()
 			}
 		}
 	}
+	return result_ready;
 }


@@ -1,4 +1,4 @@
-#include "gps_pps.h"
+#include "gps/gps_pps.h"

 #include <bsp/soc.h>
 #include <math.h>
@@ -0,0 +1,44 @@
+#include "gps/gps_receiver.h"
+
+#include "gps/NavEquations.h"
+#include "gps/gps_interrupt_handlers.h"
+#include "gps/gps_ip_defines.h"
+
+void gps_receiver_initialize(unsigned int nav_solve_timer_ticks)
+{
+	gps_startup_initialization(nav_solve_timer_ticks);
+}
+
+bool gps_receiver_attach_external_irq(
+	unsigned int irq_id,
+	gps_receiver_external_irq_handler_t handler)
+{
+	return gps_interrupt_attach_external_source(irq_id, handler);
+}
+
+void gps_receiver_start_acquisition(void)
+{
+	acq_start_loop();
+}
+
+void gps_receiver_process_pending_events(void)
+{
+	gps_background_task();
+}
+
+bool gps_receiver_solve_ready_epoch(
+	gps_nav_solution_t* solution_out,
+	uint64_t* measurement_timestamp_ticks_out)
+{
+	return calculate_position(solution_out, measurement_timestamp_ticks_out);
+}
+
+uint64_t gps_receiver_status_ticks(void)
+{
+	return gps_get_systick();
+}
+
+uint64_t gps_receiver_status_ticks_per_second(void)
+{
+	return GPS_SYSTICK_HZ;
+}
@@ -1,4 +1,4 @@
-#include "gps_time_anchor.h"
+#include "gps/gps_time_anchor.h"

 #include <math.h>
 #include <stddef.h>
@@ -1,4 +1,4 @@
-#include "linked_list.h"
+#include "gps/linked_list.h"

 #include <stdlib.h>

@@ -5,8 +5,8 @@
 * (c) Copyright by ANKASYS
 **/

-#include "matrixLib.h"
-#include "gps_macros.h"
+#include "gps/matrixLib.h"
+#include "gps/gps_macros.h"
 #include <float.h>
 #include <math.h>

@@ -1,30 +0,0 @@
-#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);
-}
@@ -1,4 +1,4 @@
-#include "imu_calibration.h"
+#include "imu/imu_calibration.h"

 #include <math.h>
 #include <stddef.h>
@@ -60,7 +60,7 @@ bool imu_calibration_is_valid(const imu_calibration_t* calibration)
 }

 bool imu_calibration_apply(const imu_calibration_t* calibration,
-	                       fusion_imu_measurement_t* measurement)
+	                       imu_sample_t* measurement)
 {
 	if(!imu_calibration_is_valid(calibration) || (measurement == NULL) ||
 	   !vector_is_finite(measurement->accel_mps2) ||
@@ -1,14 +1,14 @@
-#include "imu_sensor.h"
+#include "imu/imu_sensor.h"

 #include <stddef.h>
 #include <string.h>

 #if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
-#include "tdk_icm42688.h"
+#include "drivers/imu/tdk_icm42688.h"
 #endif

 #if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
-#include "st_lsm6dsox.h"
+#include "drivers/imu/st_lsm6dsox.h"
 #endif

 #if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \
@@ -18,9 +18,47 @@

 static imu_sensor_backend_t active_backend;
 static imu_calibration_t active_calibration;
+static uint32_t requested_odr_hz;
 static uint32_t active_odr_hz;
 static uint64_t calibration_rejected_count;

+#define IMU_ODR_MAX_ERROR_PERCENT 5u
+
+static bool nearest_supported_odr(uint32_t requested,
+	                              const uint32_t* supported,
+	                              unsigned int count,
+	                              uint32_t* selected)
+{
+	if((supported == NULL) || (count == 0u) || (selected == NULL))
+	{
+		return false;
+	}
+	if(requested == 0u)
+	{
+		*selected = 0u;
+		return true;
+	}
+	uint32_t best = supported[0];
+	uint32_t best_error = (best > requested) ? best - requested : requested - best;
+	for(unsigned int i = 1u ; i < count ; i++)
+	{
+		uint32_t error = (supported[i] > requested)
+		               ? supported[i] - requested : requested - supported[i];
+		if(error < best_error)
+		{
+			best = supported[i];
+			best_error = error;
+		}
+	}
+	if(((uint64_t)best_error * 100u) >
+	   ((uint64_t)requested * IMU_ODR_MAX_ERROR_PERCENT))
+	{
+		return false;
+	}
+	*selected = best;
+	return true;
+}
+
 void imu_sensor_default_config(imu_sensor_config_t* config)
 {
 	if(config == NULL)
@@ -72,6 +110,9 @@ static bool tdk_gyro_range(unsigned int full_scale_dps,

 static bool try_tdk(const imu_sensor_config_t* config)
 {
+	static const uint32_t supported_odr_hz[] = {
+		25u, 50u, 100u, 200u, 500u, 1000u
+	};
 	if(!imu_calibration_is_valid(
 		&config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]))
 	{
@@ -79,9 +120,17 @@ static bool try_tdk(const imu_sensor_config_t* config)
 	}
 	tdk_icm42688_config_t backend_config;
 	tdk_icm42688_default_config(&backend_config);
-	if(config->odr_hz != 0u)
+	uint32_t selected_odr_hz;
+	if(!nearest_supported_odr(config->odr_hz, supported_odr_hz,
+	                          sizeof(supported_odr_hz) /
+	                          sizeof(supported_odr_hz[0]),
+	                          &selected_odr_hz))
 	{
-		backend_config.odr_hz = config->odr_hz;
+		return false;
+	}
+	if(selected_odr_hz != 0u)
+	{
+		backend_config.odr_hz = selected_odr_hz;
 	}
 	if((config->accel_full_scale_g != 0u) &&
 	   !tdk_accel_range(config->accel_full_scale_g,
@@ -110,6 +159,7 @@ static bool try_tdk(const imu_sensor_config_t* config)
 #if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
 static bool try_st(const imu_sensor_config_t* config)
 {
+	static const uint32_t supported_odr_hz[] = {52u, 104u, 208u};
 	if(!imu_calibration_is_valid(
 		&config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]))
 	{
@@ -117,9 +167,17 @@ static bool try_st(const imu_sensor_config_t* config)
 	}
 	st_lsm6dsox_config_t backend_config;
 	st_lsm6dsox_default_config(&backend_config);
-	if(config->odr_hz != 0u)
+	uint32_t selected_odr_hz;
+	if(!nearest_supported_odr(config->odr_hz, supported_odr_hz,
+	                          sizeof(supported_odr_hz) /
+	                          sizeof(supported_odr_hz[0]),
+	                          &selected_odr_hz))
 	{
-		backend_config.odr_hz = config->odr_hz;
+		return false;
+	}
+	if(selected_odr_hz != 0u)
+	{
+		backend_config.odr_hz = selected_odr_hz;
 	}
 	if(config->accel_full_scale_g != 0u)
 	{
@@ -144,12 +202,14 @@ static bool try_st(const imu_sensor_config_t* config)
 bool imu_sensor_init(const imu_sensor_config_t* config)
 {
 	active_backend = IMU_SENSOR_NONE;
+	requested_odr_hz = 0u;
 	active_odr_hz = 0u;
 	calibration_rejected_count = 0u;
 	if(config == NULL)
 	{
 		return false;
 	}
+	requested_odr_hz = config->odr_hz;

 #if defined(IMU_SENSOR_BUILD_TDK)
 	return try_tdk(config);
@@ -232,7 +292,7 @@ bool imu_sensor_requires_foreground_service(void)
 	return false;
 }

-bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out)
+bool imu_sensor_take_measurement(imu_sample_t* measurement_out)
 {
 	if(measurement_out == NULL)
 	{
@@ -276,6 +336,7 @@ void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out)
 	}
 	memset(diagnostics_out, 0, sizeof(*diagnostics_out));
 	diagnostics_out->backend = active_backend;
+	diagnostics_out->requested_odr_hz = requested_odr_hz;
 	diagnostics_out->configured_odr_hz = active_odr_hz;
 	diagnostics_out->foreground_polling =
 		imu_sensor_requires_foreground_service();
@@ -333,12 +394,6 @@ imu_sensor_backend_t imu_sensor_active_backend(void)
 	return active_backend;
 }

-bool imu_sensor_active_quality_profile(
-	fusion_imu_quality_profile_t* profile_out)
-{
-	return imu_sensor_quality_profile_for_backend(active_backend, profile_out);
-}
-
 const char* imu_sensor_backend_name(imu_sensor_backend_t backend)
 {
 	switch(backend)
@@ -1,115 +0,0 @@
-#include "navigation_fusion.h"
-
-#include "fusion_bus.h"
-#include "fusion_gps_adapter.h"
-#include "imu_sensor.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_imu_quality_profile_t imu_profile;
-	if(imu_sensor_active_quality_profile(&imu_profile) &&
-	   !fusion_engine_apply_imu_quality(&engine_config, &imu_profile))
-	{
-		return false;
-	}
-	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;
-	imu_sensor_service();
-	fusion_imu_measurement_t imu;
-	while(imu_sensor_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;
-}
@@ -8,7 +8,7 @@ TARGET := test_imu_calibration

 all: $(TARGET)

-$(TARGET): test_imu_calibration.c $(ROOT)/src/imu_calibration.c
+$(TARGET): test_imu_calibration.c $(ROOT)/src/imu/imu_calibration.c
 	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@

 run: $(TARGET)
@@ -1,4 +1,4 @@
-#include "imu_calibration.h"
+#include "imu/imu_calibration.h"

 #include <assert.h>
 #include <math.h>
@@ -14,12 +14,12 @@ static void test_identity_is_noop(void)
 {
 	imu_calibration_t calibration;
 	imu_calibration_default(&calibration);
-	fusion_imu_measurement_t measurement = {
+	imu_sample_t measurement = {
 		.accel_mps2 = {1.0, -2.0, 9.7},
 		.gyro_rps = {0.01, -0.02, 0.03},
 		.temperature_c = 41.0
 	};
-	fusion_imu_measurement_t expected = measurement;
+	imu_sample_t expected = measurement;
 	assert(imu_calibration_apply(&calibration, &measurement));
 	assert(memcmp(measurement.accel_mps2, expected.accel_mps2,
 	              sizeof(measurement.accel_mps2)) == 0);
@@ -46,7 +46,7 @@ static void test_bias_transform_and_temperature(void)
 	calibration.reference_temperature_c = 25.0;
 	calibration.temperature_compensation_enabled = true;

-	fusion_imu_measurement_t measurement = {
+	imu_sample_t measurement = {
 		.accel_mps2 = {3.0, 5.0, 7.0},
 		.gyro_rps = {0.4, 0.5, 0.6},
 		.temperature_c = 35.0