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