onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
bdce4a3046b8045d52e92a6b1b32f2e1ff7c6b83
gps/soc/software/fusion/README.md
T

51 KiB
Raw Blame History

GPS / IMU ESKF fusion

Bu klasör Genesys2 kartından UART ile alınan GPS WLS konumlarını ve PMOD SPI üzerindeki TDK ICM-4268x veya ST LSM6DSOX ölçümlerini Python'da işler. Filtre 15 hata durumlu bir ESKF'dir: ENU konum/hız, quaternion tutum, ivmeölçer biası ve jiroskop biası. Kartın durağan olduğu deneyde zero-velocity update (ZUPT) uygulanır ve GPS innovation değerlerine aykırı-ölçüm kapısı konur.

Güncel durum ve devralma notu (2026-07-16)

Bu bölüm yeni bir geliştirme oturumunun nereden devam edeceğini tek yerde özetler. Aşağıdaki Python model ve bare-metal C uygulaması aynı problemi çözer, fakat aynı program değildir:

Asenkron IMU/GPS düzeltmesi ve sentetik oran matrisi

Önceki prototipte 52 Hz ST akışından yalnız yaklaşık 250 ms'de bir ham örnek seçiliyordu. Bu inertial downsampling değildir: kısa gyro hareketi iki seçilen örneğin arasında kalınca değişen gravity vektörü görülüyor, dönüş görülmüyor ve yanlış yatay ivme entegre ediliyordu. Linux C hızlı-eğim testinde bu yol 8.169 m tepe yatay hata ve 1.554 m/s kalıcı hız üretti.

Bu seçim katmanı kaldırıldı. Artık her raw sample kendi timestamp'iyle işlenir; ardışık accel/gyro örnekleri midpoint mechanization ile entegre edilir. GPS, IMU'dan bağımsız bir position observation'dır. WLS yalnız geçerli olduğunda ölçüm epoch'unda uygulanır; epoch iki IMU sample'ı arasındaysa input lineer interpolate edilir, update sonrası kalan IMU geçmişi current-time horizon'a replay edilir. Ana döngü IMU kuyruğunu WLS'den önce ve sonra tamamen boşaltır.

async_rate_matrix_test.c aynı üretim C kaynaklarına 52 Hz timestamp'li IMU, 100/200/1000 ms GPS, 73/310 ms delivery delay, farklı eksenlerde ivmelenme ve fren, yaw dönüşü ve 10--18 s GPS outage verir. Sonuçlar sırasıyla 0.172/0.180/ 0.389 m yatay RMSE ve outage sırasında 0.634/0.671/0.822 m tepe hatadır; üçünde de GPS recovery kabul edilmiş, false ZUPT ve NIS reject sıfırdır. Tam GPS'siz koşu da kendine konum feed etmeden yerel dead-reckoning yapar: global origin false kalır, kovaryans büyür ve 25 s sonunda 3.674 m drift oluşur. Bu drift gizlenmez; mutlak konumun IMU-only durumda gözlenememesinin beklenen sonucudur.

gps_reacquisition_test.c, canlı UART'ta görülen kötü ilk GPS fix'i senaryosunu ENU'da 80/40/-170 m başlangıç ofsetiyle tekrarlar. Hard NIS kapısı tek başına sonraki doğru fix'leri sürekli reddettiği için fusion engine zaman tabanlı bir position reacquisition uygular: son başarılı position fusion üzerinden yedi saniye timeout geçtikten ve en az üç rejected fix kendi içinde devamlı olduktan sonra yalnız position state resetlenir. Hız, attitude ve IMU bias'ları korunur. Test 100/200/1000 ms GPS periyotlarında sıfır son hata üretir; ayrı 150/-120/+80 m tek-fix testi aykırı ölçümün reddedildiğini ve state reset oluşturmadığını doğrular.

Testin --trace seçeneği UART'a benzer IMU ts=..., GPS ts=... arrival=... satırları üretir. Standart regression komutu:

make -C soc/software/fusion/tests test

platform_scenario_matrix_test.c ayrıca stationary/tilt/vibration, yaya, trafik ışığında duran otomobil ve ani dönüşlü/yüksek ivmeli uçak profillerini; 200/500/1000 ms GNSS ile 19/20/200/300 ms IMU'nun 48 kombinasyonunda çalıştırır. GNSS dört uydu kullanır, ilk bölümde 5--10 m, sonra 3--5 m sınıfı yatay gürültü, 3 saniye outage ve 2 saniye tutarlı büyük hata içerir. Kısa hata yedi saniyelik recovery timeout'unu açamaz. 200/300 ms IMU yüksek dinamikteki doğruluk kaybıyla birlikte açıkça raporlanır; test bunu daha sık IMU varmış gibi gizlemez.

Offline model:
UART GPS + IMU logları -> Python parser -> fixed/adaptive ESKF -> CSV/grafik/harita

Ürün yolu:
GPS WLS ---------> fusion_bus --\
                                  -> fusion_engine -> 15-state ESKF -> fused LLH/ENU
TDK veya ST IMU -> imu_sensor ----/

Python tarafındaki giriş noktaları:

  • serial_capture.py: UART akışını zaman damgalı CSV olarak toplar;
  • fusion_eskf.py: GPS ve TDK/ST IMU kayıtlarını offline ESKF'de oynatır;
  • compare_eskf_models.py: sürekli stationary kısıtlı model ile hareketi algılayıp ZUPT'yi açıp kapatan adaptif modeli aynı veri üzerinde karşılaştırır;
  • map_player.py: ham GPS ve düzeltilmiş ESKF noktalarını Linux masaüstü penceresinde OpenStreetMap üzerinde zamana göre oynatır;
  • compare_firmware_logs.py ve synthetic_firmware_ab.py: standalone GPS firmware A/B ve sentetik WLS bütünlük analizlerini üretir.

Bare-metal ürün mimarisi sürücüden bağımsızdır. imu_sensor aktif TDK veya ST backend'ini seçer, imu_calibration SI birimine ve ortak eksen takımına dönüştürür, fusion_engine ise sensör markasını bilmeden motion detector ve ESKF'yi çalıştırır. IMU ve GPS asenkrondur; ikisi de aynı monotonik CLINT/CPU tick alanında ölçüm anında damgalanır. GPS WLS geç tamamlanırsa fixed-lag IMU history replay ile update gerçek ölçüm epoch'unda uygulanır.

Doğrulama durumu:

  • Linux/GCC sentetik hareket, stationary, ani ivmelenme/fren, dönüş, ZUPT, GPS NIS rejection, gecikmiş GPS replay, covariance invariantları ve ASan/UBSan testleri geçti. Ayrıntılı sonuçlar aşağıdadır.
  • STEVAL-MKI217V1 üzerindeki LSM6DSOX'un SPI Mode-0 okuması, 60 saniyelik stationary ham veri kaydı ve fiziksel INT1 yolu gerçek Genesys2 kartında doğrulandı. 2026-07-16 canlı tekrarında 4084 interrupt'ın tamamı sample'a dönüştü; read-fail, queue overflow, unexpected IRQ ve trap sayıları sıfırdı. Ölçülen 105.789 Hz hız yapılandırılan 104 Hz ODR ile uyumluydu. Bu, tek başına bütünleşik fusion doğrulaması değildir.
  • PMOD asynchronous input -> 3FF synchronizer -> PLIC device 11 -> C ISR yolu ASIC/Questa simülasyonunda geçti.
  • Genesys2 fiziksel sensör interrupt girişi son tasarımda JB1/V29, GPS PPS çıkışı JC10/AK28'dir. Eski JC9/AK30 dokümantasyonu geçersizdir. STEVAL INT1 -> JB1 -> 3FF synchronizer -> PLIC 11 -> C ISR yolu gerçek kartta kesintisiz sample akışıyla doğrulandı.
  • GPS + ST/TDK + ESKF'nin aynı gerçek firmware koşusunda hareketli platform testi, Rocket rdcycle yük ölçümü, mounting dönüşümü ve sample-loss testi henüz tamamlanmadı. Sentetik metre sonuçları gerçek saha accuracy iddiası değildir.

Git devralma durumu: aktif branch tdk_ekf'dir. Çalışma ağacında GPS/nav-timer kaynak değişiklikleri ile çok sayıda üretilmiş bitstream, object, simülasyon ve log dosyası bulunabilir. Yeni oturum önce git status, git diff ve git log --oneline --decorate -5 ile tekrar kontrol etmeli; üretilmiş dosyaları ayırmadan git add -A kullanmamalıdır.

Sonraki ana adımlar:

  1. gps_system içinde ST veya TDK seçerek GPS + IMU + ESKF UART telemetry'sini gerçek karttan toplamak; fused LLH/ENU, hız, motion state, NIS, GPS accept/ reject, IRQ/sample-loss ve queue sayaçlarını kaydetmek.
  2. Stationary -> ani hareket -> sabit hız -> fren -> tekrar stationary testini yapmak; hareket sırasında ZUPT'nin kapalı kaldığını doğrulamak.
  3. PPS için aşağıdaki lock teşhisini tamamlamak.

Prototip montaj notu: STEVAL jumper kablolarıyla serbest durduğu için başlangıç sensor yönü sabit kabul edilmemelidir. Her açılışta en az iki saniye stationary gravity/bias başlangıcı yapılmalı; roll/pitch gravity'den bulunmalı ve başlangıç yaw'ı ayrı bir mutlak gözlem yoksa keyfi referans olarak kalmalıdır. Sensör bir fusion koşusu başladıktan sonra gövdeye göre yön değiştirirse bunu yalnız accel/gyro ile araç hareketinden güvenilir biçimde ayırmak mümkün değildir; hareketli test boyunca sensör geçici de olsa rijitlenmeli veya filtre yeniden başlatılmalıdır.

PPS çıkmıyor: lock problemi ve teşhis

Gözlenen belirti GPS konumunun UART'ta gelmesi fakat JC10'da PPS pulse görülmemesidir. Bu henüz kesinleşmiş bir RTL hatası değildir. Mevcut PPS tasarımı lock oluşmadan fiziksel çıkışı bilinçli olarak LOW tutar; dolayısıyla konum çözülmesi tek başına PPS üretildiğini kanıtlamaz.

İlk kabul edilen WLS/PPS commit'i yalnız coarse alignment yapar. Ardından faz hatası en fazla sekiz adet 125 MHz tracking-clock tick'i, yani 64 ns olan beş ardışık PI update gerekir. Lock kazanıldıktan sonra 256 ns üzerindeki yeni bir faz hatası lock'u düşürür. WLS receiver-clock bias/pseudorange jitter'ı veya epoch capture belirsizliği beş ardışık ölçümde 64 ns koşulunu bozarsa konum gelmeye devam ederken PPS pini sürekli LOW kalabilir. Bu şu an en güçlü olasılıktır; register ölçmeden eşik değiştirilmemelidir.

Firmware ancak WLS başarılı, solution_valid=true, PPS referans satırı FDE ile dışlanmamış ve referans kanalın capture sequence'i sıfır değilse PPS mailbox'a commit yapar. Bunlardan biri sağlanmazsa PPS daha coarse alignment'a bile ulaşmaz.

Genesys2 post-route checkpoint kontrolünde gps_pps portunun JC10/AK28, LVCMOS33 olarak yerleştiği, sabit 0/1'e bağlanmadığı ve PPS core pps_out_p_reg/Q tarafından sürüldüğü doğrulandı. Canlı ayrım için çalışan kartta GDB ile şu register'lar okunmalıdır:

x/wx 0x1002a07c
x/wx 0x1002a000
x/wx 0x1002a004
x/wx 0x1002a048
x/wx 0x1002a074
Register Beklenen/yorum
VERSION @ 0x1002a07c 0x00020100; farklıysa ELF/bitstream eşleşmiyor
CONTROL @ 0x1002a000 Enable + track açıkken alt bitler genellikle 0x3
STATUS @ 0x1002a004 0x1: commit/alignment yok; alt bitler 0x7: aligned fakat lock değil; bit 11 set ve alt bitler 0xf: locked/valid
ACTIVE_SEQUENCE @ 0x1002a048 0: firmware hiç geçerli PPS commit'i yapmamış; artıyorsa WLS referansları PPS'ye ulaşıyor
PULSE_COUNT @ 0x1002a074 Artıyorsa RTL pulse üretiyor; sorun JC10/prob/GND tarafındadır

Sağlıklı ve sticky hata biti olmayan tipik lock durumu STATUS=0x0000080f'tir. ACTIVE_SEQUENCE artarken STATUS yaklaşık 0x7 kalıyorsa problem pin değil, lock acquisition/faz jitter'ıdır. STATUS lock gösteriyor ve PULSE_COUNT artıyorsa JC10 sinyali JC11 GND referansıyla osiloskop veya logic analyzer'da ölçülmelidir; pulse saniyede bir yaklaşık 100 ms HIGH'dır.

Çalıştırma

UART kaydı:

python3 serial_capture.py --duration 60 --output logs/gps_60s.csv
python3 serial_capture.py --duration 75 --stop-text 'Complete:' \
  --output logs/imu_60s.csv

Mevcut logları işleme:

python3 fusion_eskf.py

Sabit-kısıtlı ve hareket-kabiliyetli modelleri aynı kayıt üzerinde karşılaştırma:

python3 compare_eskf_models.py \
  --gps logs/gps_live_5min_20260713.csv \
  --imu logs/imu_live_5min_20260713.csv \
  --output output/model_compare_5min

Sabit model her IMU penceresinde zero-velocity ve zero-gyro ölçümü uygular. Hareket-kabiliyetli model bunları sürekli uygulamaz; ivme normu ve gyro normundan üç ardışık durağan pencere algıladığında etkinleştirir. PMOD testinin UART çıktısı 500 ms pencere ortalaması olduğundan bu deney durağan gürültü ve model davranışı karşılaştırmasıdır. Gerçek hareketli kullanım için ham veya yüksek hızlı IMU örnekleri ile GPS ölçümleri aynı CPU sayacıyla firmware içinde zaman damgalanmalıdır.

Linux masaüstü OpenStreetMap oynatıcısını açma:

python3 map_player.py

5 dakikalık sabit/adaptif model karşılaştırmasını doğrudan açma:

python3 map_player.py \
  --comparison output/model_compare_5min_20260713/model_comparison.csv \
  --model adaptive

--model fixed aynı kaydı sürekli-stationary modelle gösterir.

Pencerede oynat/duraklat, sample sürgüsü, 0.5–4x hız, zoom/pan, ham GPS ve ESKF izleri bulunur. Tile'lar output/osm_tile_cache/ altında cache edilir; ilk açılışta harita tabanı için internet bağlantısı gerekir.

Deney sınırı

İstenen akışta GPS ve IMU UART logları peş peşe toplandı. Kart durağan olduğu için iki 0–60 s ekseni offline modellemede üst üste bindirilmiştir. Bu sonuç durağan bias/noise ve konum kararlılığı deneyidir. Hareketli bir sistemde aynı ESKF'nin anlamlı olması için GPS ve ham/decimate edilmiş IMU örnekleri aynı firmware çalışmasında ortak donanım zaman damgasıyla kaydedilmelidir; 500 ms ortalamalar hızlı dinamiği korumaz.

Bare-metal C ESKF ve Linux doğrulaması (2026-07-13)

Doğrulama durumu: Bu ESKF/TDK entegrasyonu henüz real hardware üzerinde çalıştırılmadı. Kart test sırasında bağlı değildi. Aşağıdaki hareket, stationary, accuracy, outlier-rejection ve performans sonuçları Linux/GCC ortamında üretim bare-metal C kaynaklarıyla yapılan sentetik testlere aittir. Bunlar algoritma/regression doğrulamasıdır; gerçek sensör eksenleri, SPI/INT1, IRQ/sample kaybı, 50 MHz Rocket deadline'ı veya gerçek saha GPS doğruluğu için donanım kanıtı değildir.

Python modelinden bağımsız olarak aynı algoritma artık gps_system içinde freestanding C ile çalışır. Algoritma katmanı Linux testinde de kartta da aynı kaynaklardan derlenir; Linux için ayrı veya sadeleştirilmiş bir ESKF kopyası yoktur.

Temel parçalar:

  • gps_system/src/fusion/eskf.c: ENU konum/hız, body-to-ENU quaternion, accel bias, gyro bias ve 15x15 covariance içeren 15-hata-durumlu ESKF;
  • gps_system/src/fusion/fusion_motion.c: bootstrap/stationary/moving/candidate durum makinesi, filtrelenmiş jerk ve ZUPT izni;
  • gps_system/src/fusion/fusion_engine.c: başlangıç kalibrasyonu, asenkron IMU/GPS, NIS kapısı ve gecikmiş GPS için fixed-lag IMU replay;
  • gps_system/src/fusion/fusion_gps_adapter.c: WGS-84 LLH/ECEF/ENU dönüşümü ve covariance tabanı;
  • gps_system/src/drivers/imu/tdk_icm42688.c: ICM-42688-P SPI/INT1 sürücüsü;
  • gps_system/src/fusion/navigation_fusion.c: GPS ve IMU kuyruklarını algoritmaya bağlayan üst koordinatör.

ESKF state ve matematik sınırı

Nominal state aşağıdaki bileşenlerden oluşur:

x = [p_ENU(3), v_ENU(3), q_body_to_ENU(4), b_accel(3), b_gyro(3)]

Covariance içinde quaternion'ın dört elemanı doğrudan tutulmaz. Sağ-taraflı küçük açı hatası kullanılarak hata state'i 15 boyutta kalır:

delta_x = [delta_p(3), delta_v(3), delta_theta(3),
           delta_b_accel(3), delta_b_gyro(3)]

IMU propagation'da ölçülen specific force ve angular rate'ten bias çıkarılır. Quaternion body frame'den ENU frame'e dönüşümü temsil eder. ENU lineer ivmesi R(q) * (accel - b_accel) - [0, 0, g] ile, quaternion ise gyro exponential map ile ilerletilir. Büyük dt değerleri en fazla 20 ms'lik alt adımlara bölünür. Covariance birinci dereceden error-state transition ve IMU/bias process noise ile ilerletilir; her işlemden sonra simetrikleştirilir ve diagonal tabanı uygulanır.

GPS konum residual'ı r = z_ENU - p_ENU, innovation covariance'i S = P_pp + R_GPS olarak hesaplanır. NIS = r^T S^-1 r değeri 3 DOF için 16.266236 (%99.9) kapısını aşarsa ölçüm state'e dokunmadan reddedilir. Kabul edilen update Joseph covariance formunu, quaternion error injection'ı ve attitude reset Jacobian'ını kullanır.

Default ESKF process/başlangıç değerleri:

Parametre Default
Accel noise density 0.03 m/s2/sqrt(Hz), her eksen
Gyro noise density 0.003 rad/s/sqrt(Hz), her eksen
Accel bias random walk 0.002 m/s2/sqrt(s)
Gyro bias random walk 0.002 deg/s/sqrt(s)
İlk yatay/düşey konum sigma 10 m / 20 m
İlk hız sigma 1 m/s
İlk roll-pitch/yaw sigma 5 deg / 30 deg
Gravity 9.80665 m/s2
Maksimum propagation alt adımı 20 ms
Konum NIS kapısı 16.266236, 3 DOF %99.9

Innovation rejection, recovery ve katman siniri (2026-07-16)

Standart ESKF position update'i tek basina buyuk ve kalici bir baslangic ofsetini duzeltemez. GPS--INS residual'i NIS kapisini astiginda olcum state'e hic uygulanmaz; state GPS'e yaklasmadigi icin sonraki dogru GPS fix'leri de ayni nedenle sonsuza kadar reddedilebilir. NIS yalniz iki bilgi kaynaginin tutarsiz oldugunu soyler. GPS'in mi hatali, yoksa ataletsel cozumun mu yanlis ankrajlandigini matematiksel update tek basina belirleyemez.

Bu nedenle sorumluluk iki katmana ayrilir:

  • eskf.c innovation, innovation covariance, NIS gate, kabul edilmis Kalman correction ve covariance bakimindan dogru position-reset primitive'ini icerir. Rejected olcumu gizlice correction'a sokmaz ve NIS'i bypass etmez.
  • fusion_engine.c sensor gecmisi ve zaman bilgisine sahip supervisor'dur. Ardisik rejected GNSS fix'lerinin kendi aralarinda zamansal olarak tutarli olup olmadigini izler ve gerekli recovery/reset kararini burada verir.

Mevcut yalın recovery, son başarılı position fusion'dan sonra yedi saniyelik timeout dolmuşsa ve en az üç rejected position fix'i kendi arasında zamansal tutarlıysa yalnız position state ve position covariance'ini GNSS'e yeniden ankrajlar. Position cross-covariance'lari gecersiz korelasyon tasimamasi icin sifirlanir; velocity, attitude ve IMU bias state'leri korunur. Hard NIS gate aktif kalir ve tek aykiri olcum reset uretemez. Bu bir innovation-bypass workaround'u degil, kontrollu state recovery mekanizmasidir.

PX4 EKF2 de ayni temel ayrimi yapar. Generic fuseHorizontalPosition() yalniz innovation testinden gecen olcumu fuse eder. GNSS'e baslama, quality check, time_last_fuse timeout'u, state reset, fusion stop ve sensor-fault kararlari ayri GNSS control akisindadir. Otomatik modda baska yatay konum kaynagi yoksa uzun fusion timeout'undan sonra GNSS'e reset yapilabilir; alternatif aiding varsa veya dead-reckoning politikasinda GNSS durdurulabilir/fault kabul edilebilir. Kaynaklar:

Mevcut recovery PX4'teki aiding yoneticisinin sade bare-metal karsiligidir; nihai mimari hedefi asagidaki acik GNSS aiding durum makinesidir:

DISABLED -> STARTING -> FUSING -> FAULTED/RECOVERING

Karar yalniz rejected-candidate suresine degil son basarili position fusion zamanina baglanmalidir. Uydu sayisi, finite covariance, DOP, WLS integrity ve alternatif aiding kaynaklari birlikte degerlendirilmelidir. Tam dort uyduda WLS residual redundancy/chi-square DOF bulunmadigi icin temporal consistency daha onemlidir. Ileride Doppler velocity ayri innovation, fusion timeout ve velocity-reset politikasi ile fuse edilmelidir. Yatay position ve height da barometre gibi ek kaynaklar geldiginde birbirinden bagimsiz yonetilmelidir.

Baslangic GNSS'i icin mutlak bir dogruluk referansi yoksa tutarli fakat yanlis bir GNSS kumesini otomatik olarak gercek konumdan ayirmak temel olarak mumkun degildir. Kisa bir stable-fix/startup-quality penceresi riski azaltir, fakat spoofing veya tutarli multipath'e karsi tek basina kanit degildir.

Asenkron veri akışı ve timestamp

IMU ve GPS aynı local monotonic CPU/CLINT tick alanında timestamp edilir. Filtre dt hesabında GPS week/TOW kullanmaz. GPS ölçümü WLS'nin bittiği anda değil, ölçüm epoch interrupt'ında alınan local tick ile filtreye girer. WLS geç geldiyse engine son GPS checkpoint'ine dönüp IMU geçmişini gerçek ölçüm anına kadar oynatır, GPS update'i uygular ve bugüne kadar tekrar ilerler.

Akışın ayrıntısı:

  1. TDK INT1 ISR'a girildiğinde BSP_CLINT_MTIME hemen okunur.
  2. ISR sensörün 14-byte register burst'ünü alır ve yalnız raw integer değerleri 256-elemanlı SPSC kuyruğa koyar; ISR içinde floating-point ESKF hesabı yoktur.
  3. Foreground raw accel/gyro/temp değerlerini SI birimlerine çevirir ve aynı local timestamp ile fusion engine'e verir.
  4. Her uydu nav-solve event'i yakalandığında local tick event kaydına eklenir. Coherent WLS epoch'u için geçerli uydu timestamp'lerinin medyanı seçilir.
  5. WLS daha sonra tamamlansa bile GPS mesajı bu ölçüm-epoch timestamp'ini taşır.
  6. Foreground önce bekleyen IMU'ları, sonra GPS kuyruğunu tüketir. Gecikmiş GPS son checkpoint'ten kendi timestamp'ine kadar IMU replay edilerek uygulanır; kalan IMU geçmişi tekrar oynatılarak current state yeniden oluşturulur.

IMU history kapasitesi 2048 örnektir; 500 Hz'de yaklaşık 4.096 s fixed-lag alanı sağlar. History dolarsa en eski IMU sessizce zaman aralığı kaybettirilerek atılmaz: checkpoint önce o örnekle ilerletilir, sonra ring buffer'dan çıkarılır. Gelecek timestamp'li GPS için sekiz-elemanlı sıralı pending queue bulunur; checkpoint'ten eski ölçüm stale olarak reddedilir.

GPS week/TOW uydu zamanı çözümü ve kanal coherence kontrolü için gereklidir, ancak ESKF dt kaynağı değildir. Bu ayrım WLS computation latency'sinin IMU zaman eksenini bozmasını önler.

Başlangıç kalibrasyonu, hareket kararı ve zero update

Engine açılışta doğrudan RUNNING olmaz. Default olarak en az 2 s ve en az 50 quiet IMU örneği ister. Tek örnekte gravity norm hatası 0.30 m/s2'yi veya gyro normu 2 deg/s'yi aşarsa pencere sıfırlanır. Pencere sonunda her eksende accel standard deviation 0.12 m/s2, gyro standard deviation 0.15 deg/s altında olmalıdır. Gravity ortalaması roll/pitch'i, gyro ortalaması ilk gyro bias'ını verir. Yaw gravity ile gözlenemediği için başlangıçta sıfır referanslı kalır. Benzer şekilde stationary IMU ile yatay accel bias ve küçük tilt tamamen ayrılamaz; sonraki GPS/kinematik update'ler bu error-state'leri düzeltir.

IMU motion-state eşikleri tanılama için tutulur, fakat ZUPT yetkisini tek başına vermez. PX4 ayrımına uygun olarak fusion_rest_detector titreşim metriklerini izler ve dışarıdan gelen platform_landed kapısıyla birleşir. Prototipin unarmed bench modu landed=true başlar; gerçek araç kontrolü bu değeri kendi state'inden güncellemelidir. Sessiz IMU, landed=false iken sabit hızlı hareketi stationary ilan edemez.

Motion-state default eşikleri:

Karar parametresi Değer
Stationary lineer ivme üst sınırı 0.15 m/s2
Stationary gyro üst sınırı 1 deg/s
Güçlü hareket accel eşiği 0.40 m/s2
Güçlü hareket gyro eşiği 3 deg/s
Güçlü hareket jerk eşiği 4 m/s3
Jerk düşük-geçiren zaman sabiti 100 ms
Stationary hız kapısı 0.40 m/s
Moving hız kapısı 0.50 m/s
Stationary'e dönüş quiet dwell 1.0 s
Zayıf hareket doğrulaması 3 ardışık IMU örneği

Güçlü accel/gyro/jerk veya yüksek ESKF hızı motion tanılamasını tek örnekte moving'e çıkarabilir. ZUPT ve zero-gyro ise yalnız ardışık iki timestamp'li örnekte dış supervisor vehicle_at_rest=true verdiğinde 200 ms periyotla uygulanır. ZUPT velocity sigma 0.20 m/s'dir. Zero-gyro aynı 200 ms penceredeki delta-angle toplamından ortalama açısal hız üretir; tek bir örneği bias diye kabul etmez. Bu politika PX4 EKF2 ZeroVelocityUpdate/ZeroGyroUpdate ve land detector katman ayrımının bare-metal karşılığıdır.

Kaynaklar:

GPS motion evidence, ardışık kabul edilmiş ENU konumları arasındaki yatay displacement için 2 DOF 9.21034 (%99) NIS ve en az 0.50 m/s hız ister. Moving kararı iki doğrulama ister. Bu metrik GPS position update NIS'inden ayrıdır; biri hareket-state veto'su, diğeri ölçüm kabul/reject kapısıdır.

GPS adapter ilk valid çözümü ENU origin yapar. WLS covariance yatayda en az 0.50 m, düşeyde en az 1.00 m sigma ile sınırlandırılır; korelasyonlar clamp edilir ve covariance positive-definite değilse ESKF'ye ulaşmadan reddedilir.

WLS çıktısı ESKF'ye nasıl besleniyor?

WLS çıktısı ESKF'ye gerçekten beslenmektedir. ESKF yalnız IMU ile serbest INS çalıştırmıyor; her valid GPS epoch'unda WLS konumu ve WLS measurement covariance'i ile position update yapıyor. Tam veri yolu şöyledir:

Uydu pseudorange ölçümleri
        |
        v
GPS WLS: konum + ENU covariance + residual/chi-square + validity
        |
        v
gps_fusion_publisher: ortak local CPU timestamp'li GPS mesajı
        |
        v
GPS adapter: LLH -> local ENU, covariance sanitize/floor
        |
        v
ESKF: GPS position innovation + NIS + Kalman update

WLS--ESKF sınırındaki alanlar:

WLS çıktısı ESKF tarafındaki mevcut kullanım
Latitude/longitude/height ENU position measurement'a çevrilip update'te kullanılır
3x3 ENU covariance GPS measurement covariance R_GPS olarak kullanılır
solution_valid False ise GPS update yapılmaz
Measurement local timestamp GPS update fixed-lag replay ile doğru epoch'ta uygulanır
chi_square_ratio Mesajda taşınır ve source-quality üretilir; Kalman gain'i şu an sürekli ölçeklemez
hAcc/vAcc Mesajda taşınır; ESKF ayrı scalar olarak değil tam 3x3 covariance'i kullanır
Residual RMS Mesajda taşınır fakat ESKF update denkleminde şu an doğrudan kullanılmaz
Uydu başına post-fit residual ESKF'ye tek tek gönderilmez; WLS integrity/FDE katmanında kalır

Burada iki farklı residual karıştırılmamalıdır:

  1. WLS post-fit pseudorange residual'ı: Her uydu için ölçülen pseudorange ile çözülen geometri/clock modelinin farkıdır. WLS bu residual'lardan sum(weight_i * residual_i^2), DOF ve chi-square üretir. Chi-square fail olursa solution_valid=false olur ve ölçüm ESKF tarafından kullanılmaz.

  2. ESKF GPS--INS innovation residual'ı: WLS'nin ürettiği ENU konum ile ESKF'nin IMU propagation sonucu tahmin ettiği ENU konumun farkıdır:

    r_ESKF = p_WLS_ENU - p_INS_ENU
S = P_ESKF_position + R_GPS_WLS
NIS = r_ESKF^T * S^-1 * r_ESKF

    ESKF bu NIS değeri 3 DOF %99.9 kapısını aşarsa upstream WLS valid olsa bile GPS ölçümünü ikinci kez reddeder.

Dolayısıyla iki covariance birbirinden farklıdır:

  • P_ESKF, IMU/INS error-state covariance'idir. IMU noise, gyro/accel bias random walk, attitude, geçen dt ve önceki update'lerle ESKF içinde propagation edilir.
  • R_GPS, WLS position measurement covariance'idir. Şu anda (A^T * W_absolute * A)^-1 ile uydu geometrisi ve absolute measurement weight'lerinden hesaplanıp ENU'ya çevrilir.

WLS post-fit residual büyüklüğü mevcut kodda R_GPS'yi sürekli/adaptif olarak şişirmez. Residual'ın mevcut doğrudan etkisi hard validity/chi-square kapısıdır; accepted bir ölçümün Kalman ağırlığı esas olarak P_ESKF ve WLS'den gelen R_GPS ile belirlenir. İleride absolute uydu sigma modeli gerçek loglarla kalibre edildikten sonra reduced chi-square tabanlı, sınırlandırılmış bir covariance inflation eklenebilir. Bunun için threshold'a bölünmüş mevcut chi_square_ratio yerine istatistiksel ölçek olarak weighted_residual_sum / DOF kullanılmalıdır.

Hareket kararıyla ilgili iki koruma sentetik test sonucunda eklendi:

  • 200--500 Hz ham örnek farkı doğrudan jerk sayılmıyor; 100 ms zaman sabitli düşük-geçiren ivmeden jerk üretiliyor. Böylece stationary IMU noise yanlış hareket kararı üretmiyor, 1 m/s2 ani ivme yine ilk örnekte algılanıyor.
  • Position-only GPS'in tek/iki gürültülü noktadan çıkardığı still kararı, yüksek ESKF hızını ezip ZUPT başlatamıyor. GPS moving ek veto olarak kullanılıyor; stationary hız kapısı 0.40 m/s, moving kapısı 0.50 m/s ile histerezisli. İleride GPS Doppler velocity gelirse daha güçlü ve hızlı bir duruş kanıtı olarak ayrıca eklenebilir.

Linux/GCC testlerini çalıştırma:

cd soc/software/fusion/tests
make clean
make test
make sanitize
make profile

fusion_synthetic_test.c, üretim kaynaklarıyla şu durumları birlikte sınar:

  • bias ve noise içeren stationary başlangıç kalibrasyonu;
  • 100/200/500 Hz ve düzensiz local-tick IMU aralıkları;
  • ani ivmelenme, sabit hız, ani fren, 90 derece yaw ve tekrar hareket;
  • 0.8--1.2 s değişken GPS periodu ve 180--350 ms teslim gecikmesi;
  • fixed-lag replay, covariance simetrisi/PSD ve uzun IMU geçmişi;
  • iki adet yaklaşık 200 m sınıfı multipath/outlier ve NIS rejection;
  • sekiz ayrı deterministic noise/timing seed'i;
  • 30 s boyunca 500 Hz stationary IMU ve gecikmiş 1 Hz GPS.

Sentetik hareketli senaryo

Hareketli senaryo 200 Hz IMU ile 60 s sürer. Ground-truth profil şöyledir:

Zaman Ground truth hareket
0--5 s Stationary; ilk 2 s calibration alanı
5--7 s East yönünde +1.0 m/s2 ani ivmelenme
7--10 s East yönünde 2.0 m/s sabit hız
10--12 s East yönünde -1.0 m/s2 fren, tam duruş
12--15 s Stationary
15--17 s Yerinde +45 deg/s yaw, toplam 90 deg dönüş
17--19 s North yönünde +0.8 m/s2 ivmelenme
19--22 s North yönünde 1.6 m/s sabit hız
22--24 s North yönünde -0.8 m/s2 fren, tam duruş
24--60 s Stationary

Truth son konumu [east=10 m, north=8 m, up=0 m] ve son hızı sıfırdır. Body-frame specific force, truth yaw ile ENU ivmesinin ters rotasyonundan üretilir; dolayısıyla 90 derece dönüşten sonraki North ivmesi body X eksenine doğru düşer ve attitude propagation da test edilir.

Sentetik sensör modeli:

Girdi Bias / noise
Accel bias [+0.030, -0.025, +0.060] m/s2
Gyro bias [+0.0010, -0.0007, +0.0009] rad/s
Accel sample noise Gaussian, 0.025 m/s2 sigma
Gyro sample noise Gaussian, 0.0008 rad/s sigma
GPS yatay/düşey noise 1.2 m / 2.0 m sigma
GPS measurement periodu 0.8--1.2 s deterministic-random
WLS/teslim gecikmesi 180--350 ms
GPS outlier İki epoch'ta [+150, -120, +80] m; yaklaşık 208 m 3B

Outlier GPS mesajları source-valid bırakılır; böylece sadece upstream validity değil ESKF'nin kendi NIS kapısı gerçekten sınanır. Her GPS farklı gecikmeyle geldiği için current state'e doğrudan uygulanamaz ve fixed-lag replay yolu her update'te çalışır. Sekiz seed IMU noise, GPS noise ve GPS period/delay dizisini değiştirir; aynı seed her çalıştırmada aynı sonucu vererek regression testini deterministic tutar.

Sentetik stationary senaryo

İkinci uçtan uca senaryo 30 s boyunca 500 Hz IMU ve 1 Hz GPS kullanır. GPS 250 ms gecikmeli gelir; yatay/düşey sigma 0.8/1.5 m'dir. Accel bias [+0.020, -0.015, +0.050] m/s2, gyro bias [+0.0008, -0.0005, +0.0006] rad/s; sample noise sırasıyla 0.020 m/s2 ve 0.0006 rad/s'dir. Testin amacı yalnız position error değil, yüksek IMU rate'te ham sample-to-sample noise'un jerk üzerinden sahte MOVING üretmediğini ve 20 Hz ZUPT'nin rate'ten bağımsız kaldığını göstermektir.

Unit-level ve invariant kontrolleri

eskf_motion_test.c aşağıdaki kontrolleri ayrıca tek tek yapar:

  • düz ve tilt edilmiş stationary gravity propagation;
  • normal GPS NIS accept ve büyük outlier reject; reject sonrası state'in bit seviyesine yakın değişmemesi;
  • ani hızlanma/fren, sabit hız sırasında stationary veto ve quiet dwell;
  • stationary 500 Hz IMU noise altında false-motion olmaması;
  • 100/200/500 Hz ve 3--17 ms düzensiz local-tick dt dizileri;
  • 300 ms gecikmiş GPS replay sonucunun on-time filtre ile position, velocity, quaternion, bias, 15x15 covariance ve motion state'te 1e-11 toleransta aynı olması;
  • future GPS pending, checkpoint'ten eski GPS stale reject;
  • GPS kesintisinde 2048-history rollover ve checkpoint'in zaman kaybetmeden ilerlemesi;
  • LLH--ECEF--ENU round-trip, covariance floor/PD kontrolü;
  • IMU/GPS SPSC queue FIFO sırası ve overflow sayaçları.

Uçtan uca acceptance limitleri test kodunun içinde sabittir: hareketli yatay RMSE <2.0 m, düşey RMSE <2.5 m, son yatay/düşey hata <1.5/2.0 m, son hız <0.05 m/s, constant-velocity false-stationary örneği sıfır, bütün outlier'lar reject ve covariance PSD olmalıdır. Stationary 500 Hz koşusunda son yatay/düşey hata <1/1 m, son hız <0.02 m/s ve false-moving örneği sıfır olmalıdır. Bunlar gerçek dünya accuracy garantisi değil, regression fail sınırlarıdır.

Son Linux/GCC -O2 sonuçları:

Sentetik koşu Sonuç
60 s hareketli yatay/düşey RMSE 0.925 m / 0.427 m
60 s son yatay/düşey hata 0.306 m / 0.110 m
Moving algılama 5.005 s, hareket başlangıcından 1 IMU örneği
Son duruşu stationary tanıma 27.505 s, gerçek duruş 24.000 s
GPS accept/reject 56 / 2
Enjekte edilen/reddedilen outlier 2 / 2
Reddedilen GPS'in state sıçraması 0.0 m
Sekiz seed en kötü yatay RMSE 0.959 m
30 s / 500 Hz stationary yanlış-moving örneği 0
30 s / 500 Hz son hız 0.0002 m/s

Baseline hareketli koşuda 58 delayed GPS update için 14,508 IMU örneği replay edildi; 56 GPS kabul, iki sentetik outlier reject edildi. Toplam 749 rate-limitli zero update uygulandı. Stationary 500 Hz koşusunda 27 GPS kabul edildi, 560 zero update uygulandı. Final covariance Cholesky tabanlı PSD kontrolünü geçti ve maksimum symmetry error test çözünürlüğünde sıfırdı.

Sentetik testin ortaya çıkardığı iki hata

İlk sürümde jerk doğrudan iki ham accel örneğinin farkının dt'ye bölünmesiyle üretiliyordu. 500 Hz'de küçük sample noise bile 4 m/s3 eşiğini sık geçebildiği için stationary kartın motion state'i chatter edebilirdi. Jerk için 100 ms low-pass state eklenince 10 s/500 Hz detector testi ve 30 s/500 Hz uçtan uca test sıfır false-moving ile geçti; 1 m/s2 step yine ilk örnekte moving yaptı.

İkinci ara sürüm, iki position-only GPS örneğinin STILL kararına ESKF hızını ezme yetkisi veriyordu. 1.2 m GPS sigma ile 2 m/s sabit hareketin tek-epoch displacement'i her zaman istatistiksel olarak güçlü değildir. Bazı seed'lerde yanlış stationary kararı ZUPT'yi gerçek hareket sırasında açtı; sekiz-seed testinde yatay RMSE 480 m ve son hata 1 km üzerini görerek regression'ı yakaladı. Kalıcı düzeltmede GPS-still yüksek ESKF hızını artık geçersiz kılamaz; GPS-moving yalnız ek veto olur. Düzeltme sonrası sekiz seed'in en kötü yatay RMSE'si 0.959 m, false-stationary sayısı sıfır oldu.

Bu sonuç GPS Doppler velocity'nin neden değerli olduğunu da gösterir. Position farkından stationary/moving ayırımı düşük hızda ve kısa periodda zayıftır. İleride GPS modülü güvenilir Doppler velocity ve covariance üretirse ayrı bir velocity measurement update ve daha hızlı stop confirmation eklenmelidir.

ASan ve UBSan hata vermedi. Sekiz adet 60 s hareketli koşu ile 30 s/500 Hz stationary koşunun tamamı masaüstünde yaklaşık 0.70 s user CPU süresinde tamamlandı. gprofte sürenin yaklaşık %73'ü covariance dahil ESKF propagation, %18'i measurement/ZUPT update içindeydi. Bu yüzdeler darboğazı gösterir; masaüstü süresi 50 MHz Rocket için doğrudan cycle tahmini değildir. Profil koşusunda fixed-lag replay dahil 111,009 fusion_engine_push_imu, 239,075 covariance propagation step ve 30,352 selected measurement/ZUPT update çağrısı oluştu. Host maksimum RSS yaklaşık 2.6 MB idi; bu değer glibc, sanitizer/profiler ve process overhead içerdiğinden bare-metal BSS ile aynı metrik değildir.

Genesys2/RISC-V bare-metal build'i de geçti:

make FPGA_BOARD=genesys2 fpga-elf-flash-image APP=gps_system \
  GPS_DEBUG=1 GPS_DEBUG_LEVEL=3 \
  TDK_IMU_ODR_HZ=500 GPS_NAV_SOLVE_TIMER_TICKS=50

Son ELF boyutu text/data/bss olarak 103840 / 2944 / 204816 bayt, flash payload 106784 bayt ve CRC32 0x16dbc828 oldu. Kart bu test sırasında bağlı olmadığı için SPI interrupt hızı, gerçek Rocket cycle yükü ve UART telemetry henüz donanım üzerinde ölçülmedi.

Build yeni fusion kaynaklarında compiler warning üretmedi. Linker'ın RAM imajındaki mevcut LOAD segment with RWX permissions uyarısı devam ediyor; bu ESKF değişikliğine özel değildir fakat ileride linker-script güvenlik/segment ayrımı çalışmasında ele alınmalıdır.

Kart tekrar bağlandığında sentetik testin yerine geçmeyen şu donanım doğrulamaları yapılmalıdır:

  1. ICM-42688-P WHO_AM_I=0x47, gerçek accel/gyro eksen yönleri ve kart mounting dönüşümü doğrulanmalı.
  2. 100/200/500 Hz seçeneklerinde INT1 IRQ sayısı, sample sayısı, spurious IRQ, estimated missed sample ve 256-raw-queue overflow sayaçları izlenmeli.
  3. navigation_fusion_service() ve tek eskf_propagate_step() için Rocket rdcycle ortalama/maksimum değerleri ölçülmeli; 50 MHz'deki gerçek CPU payı bu cycle ölçümünden çıkarılmalı.
  4. En az 2 s fiziksel stationary başlangıçta calibration reset sayısı, accel/ gyro varyansı ve bulunan bias'lar sıcaklıkla birlikte loglanmalı.
  5. Elle hareket testinde stationary -> moving çıkış süresi, sabit hızda ZUPT'nin kapalı kaldığı ve durduktan sonra candidate -> stationary geçişi doğrulanmalı.
  6. GPS WLS ölçüm timestamp'i ile IMU local tick'lerinin aynı CLINT domaininde monoton kaldığı; replay/stale/pending/history-overflow sayaçlarının beklenen değerlerde olduğu kontrol edilmeli.
  7. Fused LLH/ENU, velocity, motion state, NIS, GPS accept/reject ve covariance sigma değerleri UART'a kaydedilip fiziksel ground truth veya surveyed nokta ile karşılaştırılmalı.

Sentetik test; SPI elektrik bağlantısını, interrupt polarity/clear davranışını, sensor scale/mounting hatasını, gerçek multipath dağılımını, sıcaklıkla bias driftini veya 50 MHz timing deadline'ını kanıtlamaz. Buradaki metre sonuçları gerçek ürün accuracy iddiası değil; bilinen truth ve covariance ile algoritma tutarlılığı/regression sonucudur.

Standalone GPS WLS bütünlük çalışması (2026-07-12)

Bu bölüm, standalone GPS konumundaki 20--30 m ve daha büyük sıçramaları araştırmak için yapılan firmware karşılaştırmasını kaydeder. Karşılaştırılan eski kaynak commit'i 07ddf29 (first draft) idi. Yeni kaynakta residual, chi-square, covariance, validity, leave-one-out FDE, çalışma süresi ve ISR tanıları bulunuyordu.

Karşılaştırma yöntemi

GDB ile çalışan DTIM imajının üzerine başka bir ELF yazıldığında Rocket I-cache'in eski firmware satırlarını yürütmeye devam ettiği gözlendi. Örneğin 0x80002f58 adresi eski ELF'te gps_pps_init, yeni ELF'te ise gps_process_isr_events içindeydi; access-fault anındaki return address yeni ELF anlamına uyuyordu. fence.i stub'ı çalıştırılması da bu kart/akışta güvenilir bir hot-load sağlamadı.

Bu nedenle aşağıdaki A/B ölçümlerinde GDB hot-load kullanılmadı:

  1. İlgili ELF bootloader formatındaki application flash image'a çevrildi.
  2. Image 0x01800000 application flash bölgesine yazıldı.
  3. FPGA SRAM aynı bitstream ile yeniden programlandı.
  4. Bootloader ELF'i flash'tan DTIM'e kopyaladı; ilk I-cache dolumu ile RAM imajının aynı firmware olması sağlandı.
  5. İlk konum çözümü acquisition süresinden sonra beklendi.
  6. Kart ve anten sabitken 300 saniyelik UART kaydı alındı.

Sapmalar surveyed bir koordinata göre değil, her koşunun kendi medyan konumuna göre hesaplandı. Bu nedenle aşağıdaki değerler mutlak doğruluktan çok durağan konum dağılımını/hassasiyetini gösterir. Farklı koşulardaki uydu sayısı ve DOP ayrıca belirtilmiştir.

Kullanılan imajlar ve loglar

  • Exact eski ELF: artifacts/firmware_ab/gps_old.elf
  • FDE açık yeni ELF: artifacts/firmware_ab/gps_new.elf
  • FDE kapalı yeni ELF: artifacts/firmware_ab/gps_new_fde_off.elf
  • Eski log: logs/ab_live_old_5min.csv
  • FDE açık yeni log: logs/ab_live_new_5min.csv
  • FDE kapalı yeni log: logs/ab_live_new_fde_off_5min.csv
  • Uydu/FDE gruplu sonuç: output/firmware_ab_live_5min_by_satellites.csv
  • FDE kapalı karşılaştırma: output/firmware_ab_live_fde_off_5min_by_satellites.csv
  • Analiz programı: compare_firmware_logs.py

FDE kapalı ELF SHA-256:

23e2d5be11ab0bf23213c5253b4213277f11e5785d90484c9d624b4fcae944dc

FDE kapalı 5 dakika log SHA-256:

c2815c0cf3e7f76dd0a19db9526421355d6699c4bc1edc6407fde496e09bafc8

Canlı A/B sonuçları

Firmware/çözüm Kullanılabilir uydu Örnek Yatay ort. Yatay P95 Yatay maks. Düşey mutlak ort. 3B P95 3B maks.
Eski firmware 8 485 6.01 m 11.70 m 20.79 m 9.60 m 25.05 m 38.97 m
Yeni, FDE açık, tümü çoğunlukla 7 300 9.71 m 30.34 m 44.90 m 16.43 m 78.95 m 128.96 m
Yeni, FDE açık ama FDE seçilmeyen epoch 7 231 6.84 m 13.41 m 17.33 m 10.85 m 28.03 m 37.80 m
Yeni, FDE tarafından 7'den 6'ya düşürülen epoch 7 -> 6 68 19.22 m 37.09 m 44.90 m 34.52 m 108.30 m 128.96 m
Yeni, FDE compile-time kapalı 7 300 8.64 m 16.74 m 22.56 m 14.34 m 40.90 m 88.73 m

DOP medyanları aynı değildi:

Koşu HDOP VDOP
Eski, 8 uydu 1.222 1.991
Yeni, FDE açık 1.339 2.102
Yeni, FDE kapalı 1.642 2.852

FDE kapalı koşu daha kötü geometriye rağmen FDE açık toplam sonuçtan daha kararlıydı. Yatay P95 30.34 mden 16.74 mye, yatay maksimum 44.90 mden 22.56 mye ve 3B P95 78.95 mden 40.90 mye düştü.

FDE teşhisi ve alınan karar

FDE açık koşuda 300 epoch'un 68'inde (%22.7) leave-one-out FDE uygulandı. 99% chi-square kapısının nominal false-alarm oranı yaklaşık %1 olmalıydı. Çıkarılan uydu tek bir SVID değildi: SVID 13, 19, 20, 21, 6 ve 29 farklı epoch'larda seçildi. Bu davranış tek ve sürekli arızalı bir uydudan çok, measurement sigma/covariance modelinin gerçek tracking gürültüsü ve multipath'i küçümsemesiyle uyumludur.

FDE açık koşuda exclusion içeren ardışık konum çiftlerinin 3B step ortalaması 47.40 m, FDE içermeyen çiftlerde 18.45 m idi. Bazı kötü FDE seçimleri integrity-valid olarak işaretlendi. Altı uydu için FDE daha önce sentetik testte ayrıca engellenmişti; çünkü 6 -> 5 leave-one-out çözümü yalnız bir residual serbestlik derecesi bırakıp 123 m hatalı bir çözümü geçerli gösterebilmişti.

Ürün firmware'inde otomatik FDE seçimi şu anda compile-time kapalıdır:

#define GPS_NAV_ENABLE_FDE 0u

Full-WLS konumu üretilmeye devam eder. Post-fit residual, chi-square, DOF, threshold, covariance, hAcc/vAcc ve validity hesapları/logları korunur. Chi-square fail eden çözüm konum olarak görülebilir fakat valid=0 olur; PPS ve sonraki WLS seed'i invalid çözümle güncellenmez.

FDE kapalı koşuda:

  • 300/300 epoch'ta fde=0 ve fde_trials=0;
  • 235/300 epoch integrity-valid (%78.3);
  • ISR overflow, PLIC repeat, drain-limit, coalesced event, satellite loss ve period overrun sayaçları sıfır;
  • maksimum foreground süresi 288.74 ms;
  • maksimum WLS süresi 28.11 ms;
  • 1 saniyelik/50 milyon cycle bütçe hiç aşılmadı.

FDE ancak C/N0, tracking variance, URA, elevation ve multipath bileşenlerinden kalibre edilmiş absolute measurement sigma üretildikten; normalized residual, solution separation, exclusion uniqueness ve zaman hysteresis'i eklendikten sonra yeniden açılmalıdır.

Eski firmware'deki kalıcı 0.2 s TOW faz hatası

Ölçüm kanıtı

Eski firmware acquisition logunun ilk çözümü 5 uyduyla yapıldı ve bu ilk epoch'ta bütün TOW değerleri 64620.200 s idi. SVID 6 daha sonra tracking kanalı 7'ye bağlandı. Sekiz uydulu 5 dakika kaydında:

  • 485 epoch'un 485'i TOW-incoherent idi;
  • çoğunluk TOW ile SVID 6 arasındaki fark her epoch'ta tam -0.200 s idi;
  • ilk epoch: çoğunluk 64634.200, SVID 6 64634.000;
  • son epoch: çoğunluk 64933.400, SVID 6 64933.200;
  • sonraki subframe/HOW alımları farkı düzeltmedi.

Dolayısıyla hata geçici değildi. SVID 6 çözümde kaldığı sürece uydu konumu 0.2 saniye yanlış epoch için hesaplandı. Bunun alıcı WLS konumuna etkisi uydu geometrisine bağlıdır; iki firmware medyanı arasındaki 3B fark bu nedenle mutlak doğruluk ölçümü olarak yorumlanamaz.

Yeni 1 saniyelik koşuların her ikisinde de 300/300 epoch TOW-coherent idi; tow_aligned=0 ve tow_rejected=0 kaldı. Bu, eski hatanın her bootta zorunlu olmadığını fakat kanal geç katıldığında kalıcı olabildiğini gösterir.

Kök mekanizma

Mevcut yazılım her tracking kanalı için ayrı TOW state'i yürütüyor:

  1. Kanal önce LNAV HOW alanından TOW alıyor.
  2. İlk nav-solve interrupt'ında TOW ownership HOW kaynağından NAV_SOLVE kaynağına geçiriliyor.
  3. Bundan sonra kanal TOW'u recursively tow += event_count * NAV_SOLVE_TIMER * 20 ms ile artırılıyor.
  4. Ownership geçtikten sonra daha sonraki HOW değerleri TOW'u yeniden anchor etmiyor.

Kanalın enable/ilk interrupt sınırında bir tick off-by-one oluşursa bütün sonraki artışlar doğru miktarda fakat yanlış başlangıçtan yapılır; sabit faz hatası bu yüzden kalıcı olur. Log, SVID 6'nın geç katıldığını ve hatanın tam bir eski nav periodu olduğunu kanıtlıyor. İlk yanlış tick'in enable/flag sıralamasından mı yoksa aynı olayda HOW ve nav-solve işlenmesinden mi geldiği geçiş anı iki UART dosyası arasındaki kısa boşlukta kaldığı için logdan tek başına kesinleştirilemiyor. Ancak recursive kanal-bağımsız TOW mimarisinin hatayı kendi kendine düzeltemediği kesindir.

IS-GPS-200N'e göre HOW içindeki TOW sayacı bir sonraki subframe'in başlangıç zamanını ifade eder. Kodun (HOW >> 13) * 6 dönüşümü bu 6 saniyelik değeri üretir. Kalıcı düzeltme, bu absolute yayın zamanını yalnız ilk başlangıç için değil sürekli time anchor olarak kullanmalıdır. Referans: https://archive.gps.gov/technical/icwg/IS-GPS-200N.pdf, bölüm 3.3.4 ve 20.3.3.2.

Uygulanan kalıcı TOW mimarisi

2026-07-12'de kanal başına recursive TOW artışı kaldırıldı ve aşağıdaki HOW/receiver-sample anchor modeli ürün firmware'ine uygulandı. Snapshot'ta azınlık TOW'unu çoğunluğa kopyalayan geçici recovery yolu da kaldırıldı:

  1. Her geçerli LNAV subframe sonunda kanal için şu anchor saklanır: how_tow, how_week, how_abs_sample_raw, düzeltilmiş how_abs_sample ve anchor-valid bilgisi.

  2. HOW TOW bir sonraki subframe başlangıcını gösterdiği ve nav-bits event'i alınan subframe'in sonuna karşılık geldiği için bu ikisi aynı zaman sınırında anchor edilir. RTL incelemesinde bit_gen.sv içindeki absolute_sample_o değerinin geçerli 20 ms bit sınırında üretildiği ve tracking_wrapper.sv içindeki load_sigs yolunun geçerli/pariteli subframe'de ABS_SAMPLE ile REM_CODE_PHASE'ı birlikte latch ettiği doğrulandı.

  3. Her nav-solve ölçümünün TOW'u önceki nav-solve TOW'una ekleme yapılarak değil, son HOW anchor'ından tekrar hesaplanır:

    delta_samples = signed_wrap(nav_abs_sample - how_abs_sample)
measurement_tow = normalize(how_tow + delta_samples / 16_368_000)

    REM_CODE_PHASE düzeltmesi iki uçta da hesaba katılmalıdır. 32-bit ABS_SAMPLE farkı signed/wrapped alınmalıdır; anchor 6 saniyede bir yenilendiği için wrap belirsizliği oluşmaz.

  4. Aynı interrupt'ta hem nav-bits hem nav-solve flag varsa önce HOW anchor güncellenir; nav-solve için delta_samples=0 olur. Eski koddaki koşulsuz +period off-by-one riski ortadan kalkar.

  5. Anchor yaşı, subframe/parity geçerliliği ve finite/range kontrolleri zorunlu yapıldı. Geçerli anchor olmayan veya 12 saniyeden eski anchor'a sahip kanalın nav-solve ölçümü kabul edilmez.

  6. gps_tow_source ownership state'i ve gps_advance_tow_from_nav_timer() kaldırılır. Interrupt count yalnız event kaybı tanısı için tutulur, absolute GPS zamanının kaynağı olmaz.

  7. Coherent snapshot kontrolü son savunma olarak kalır; fark görülürse epoch veya problemli kanal reject edilir. TOW değeri sessizce başka kanalın değeriyle değiştirilmez.

Üretim kodu header/gps/gps_time_anchor.h ve src/gps/gps_time_anchor.c içindedir. parse_subframe() artık TOW state'ini değiştirmez; yalnız HOW TOW değerini dışarı verir. gps_time_from_sample_anchor() her nav-solve için GPS zamanını son HOW ve iki receiver-sample latch'i arasındaki farktan baştan üretir.

Uygulanan birim testler:

  • HOW ile aynı anda gelen ilk nav-solve (delta=0);
  • 200 ms ve 1 s sonraki nav-solve;
  • geç enable edilen kanal;
  • bir veya birkaç coalesced/missed interrupt;
  • 32-bit ABS_SAMPLE wrap;
  • GPS week rollover;
  • stale HOW anchor;
  • aynı coherent epoch'ta bütün kanalların TOW farkının tolerans içinde olması.

Host testleri normal optimizasyonla ve AddressSanitizer/UndefinedBehaviorSanitizer ile geçti. 50 MHz Rocket ürün build'i de başarılı oldu. Yeni anchor fonksiyonları ELF'te toplam yaklaşık 264 bayt text alanı kullanıyor; 1 Hz'de kanal başına bir kez çalıştığı için WLS yanında CPU etkisi ihmal edilebilir düzeydedir.

Flash-boot donanım doğrulaması

GDB hot-load kullanılmadan yeni ELF application flash bölgesine yazıldı ve FPGA yeniden programlanarak bootloader üzerinden temiz DTIM/I-cache başlangıcı yapıldı. Kullanılan dosyalar:

  • artifacts/firmware_ab/gps_new_how_anchor_fde_off.elf
  • artifacts/firmware_ab/gps_new_how_anchor_fde_off.flash.bin
  • logs/how_anchor_acquisition.csv
  • logs/how_anchor_fde_off_5min.csv
  • output/firmware_ab_how_anchor_fde_off_5min_by_satellites.csv

ELF SHA-256:

fb1de03aafacf46d95bde375f3f744635472e9893a9aff6f3c3db9a16444c5ce

5 dakika log SHA-256:

82b08bd35d4452f92db6af609b2a03db2ff7b8f78443414ca7a0238c3cb603ed

Acquisition sırasında bir kanal diğerlerinden altı nav-solve epoch daha geç katıldı (seq=8, diğer kanallar seq=14). Buna rağmen ortak ölçüm epoch'unda bütün kanallar 67640.000 s TOW üretti; eski yazılımdaki -0.200 s kalıcı faz farkı yeniden oluşmadı.

5 dakikalık sabit anten ölçümü:

Metrik Sonuç
NAV çözümü 300
Kanal zaman ölçümü 2100
TOW yayılımı P95 19 us
TOW yayılımı maksimum 33 us
1 ms üstü TOW epoch'u 0
tow_mismatch / tow_rejected 0 / 0
ISR overflow / PLIC repeat / drain limit 0 / 0 / 0
Coalesced / lost / period overrun 0 / 0 / 0
Integrity-valid 270/300 (%90.0)
FDE uygulanması 0/300
Maksimum foreground / WLS 290.13 ms / 28.34 ms
HDOP / VDOP medyan 1.511 / 2.407
Yatay ortalama / P95 / maksimum 8.34 m / 16.36 m / 24.93 m
3B P95 32.64 m

FDE kapalı önceki yeni-firmware koşusu yatay ortalama/P95 olarak 8.64/16.74 m, HOW-anchor koşusu 8.34/16.36 m verdi. Dolayısıyla FDE kapatılınca full-WLS dağılımı eski firmware'e yakın seviyeye döndü. Eski firmware 8 uydu ve daha iyi geometriyle 6.01/11.70 m, yeni koşu çoğunlukla 7 uyduyla 8.34/16.36 m üretti. Bu farkın tamamı algoritma farkı sayılamaz; koşuların uydu sayıları/DOP'ları farklıdır ve surveyed ground truth yoktur. FDE açık yeni koşunun 9.71/30.34 m toplam sonucu ise belirgin daha kötüydü.

Not: Eski logdaki hata 0.2 ms değil, 0.2 s = 200 ms idi. Yeni ölçümdeki en büyük kanallar arası TOW farkı 33 us, yani 0.033 ms oldu.

Son savunma

Anchor düzeltmesine rağmen TOW farkı görülürse epoch invalid/reject edilir. SVID'e özel +0.2 s eklenmez ve azınlık TOW'u çoğunluğa kopyalanmaz; böylece başka kanal veya başka nav periodundaki gerçek zaman hatası gizlenmez.

Reference in New Issue View Git Blame Copy Permalink