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ı
|
||||
|
||||
@@ -4,16 +4,46 @@ CFLAGS += -I../../gps_system/header
|
||||
LDLIBS ?= -lm
|
||||
|
||||
GPS_TIME_SOURCE := ../../gps_system/src/gps_time_anchor.c
|
||||
ESKF_SOURCES := ../../gps_system/src/eskf.c ../../gps_system/src/fusion_motion.c ../../gps_system/src/fusion_engine.c ../../gps_system/src/fusion_gps_adapter.c ../../gps_system/src/fusion_bus.c
|
||||
|
||||
.PHONY: all test benchmark clean
|
||||
.PHONY: all test sanitize profile benchmark clean
|
||||
|
||||
all: standalone_gps_integrity_test
|
||||
all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test
|
||||
|
||||
standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
test: standalone_gps_integrity_test
|
||||
eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES)
|
||||
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
|
||||
|
||||
test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test
|
||||
./standalone_gps_integrity_test
|
||||
./eskf_motion_test
|
||||
./fusion_synthetic_test
|
||||
|
||||
sanitize:
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header eskf_motion_test.c $(ESKF_SOURCES) \
|
||||
-o eskf_motion_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./eskf_motion_test_san
|
||||
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-I../../gps_system/header fusion_synthetic_test.c $(ESKF_SOURCES) \
|
||||
-o fusion_synthetic_test_san $(LDLIBS)
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./fusion_synthetic_test_san
|
||||
|
||||
profile:
|
||||
$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \
|
||||
-I../../gps_system/header fusion_synthetic_test.c $(ESKF_SOURCES) \
|
||||
-o fusion_synthetic_test_gprof $(LDLIBS)
|
||||
./fusion_synthetic_test_gprof
|
||||
gprof ./fusion_synthetic_test_gprof gmon.out
|
||||
|
||||
benchmark: standalone_gps_integrity_test
|
||||
./standalone_gps_integrity_test --bench-clean 100000
|
||||
@@ -21,4 +51,4 @@ benchmark: standalone_gps_integrity_test
|
||||
./standalone_gps_integrity_test --bench-double 100000
|
||||
|
||||
clean:
|
||||
rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof gmon.out
|
||||
rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace gmon.out
|
||||
|
||||
@@ -0,0 +1,724 @@
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "eskf.h"
|
||||
#include "fusion_engine.h"
|
||||
#include "fusion_bus.h"
|
||||
#include "fusion_gps_adapter.h"
|
||||
#include "fusion_motion.h"
|
||||
|
||||
#define TEST_PI 3.14159265358979323846264338327950288
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
static bool nearly_equal(double first, double second, double tolerance)
|
||||
{
|
||||
return fabs(first - second) <= tolerance;
|
||||
}
|
||||
|
||||
static double test_norm3(const double value[3])
|
||||
{
|
||||
return sqrt(value[0] * value[0] + value[1] * value[1] +
|
||||
value[2] * value[2]);
|
||||
}
|
||||
|
||||
static uint32_t noise_state = 0x9e3779b9u;
|
||||
|
||||
static double uniform_noise(void)
|
||||
{
|
||||
noise_state ^= noise_state << 13;
|
||||
noise_state ^= noise_state >> 17;
|
||||
noise_state ^= noise_state << 5;
|
||||
return ((double)noise_state + 0.5) / 4294967296.0;
|
||||
}
|
||||
|
||||
static double gaussian_noise(void)
|
||||
{
|
||||
double first = fmax(uniform_noise(), 1.0e-12);
|
||||
double second = uniform_noise();
|
||||
return sqrt(-2.0 * log(first)) * cos(2.0 * TEST_PI * second);
|
||||
}
|
||||
|
||||
static void identity_filter(eskf_t* filter, eskf_config_t* config)
|
||||
{
|
||||
eskf_default_config(config);
|
||||
double position[3] = {0.0, 0.0, 0.0};
|
||||
double quaternion[4] = {1.0, 0.0, 0.0, 0.0};
|
||||
double accel_bias[3] = {0.0, 0.0, 0.0};
|
||||
double gyro_bias[3] = {0.0, 0.0, 0.0};
|
||||
CHECK(eskf_initialize(filter, config, position, quaternion, accel_bias, gyro_bias),
|
||||
"ESKF identity initialization");
|
||||
}
|
||||
|
||||
static void test_stationary_propagation(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t config;
|
||||
identity_filter(&filter, &config);
|
||||
double accel[3] = {0.0, 0.0, config.gravity_mps2};
|
||||
double gyro[3] = {0.0, 0.0, 0.0};
|
||||
for(unsigned int i = 0 ; i < 500u ; i++)
|
||||
{
|
||||
CHECK(eskf_propagate(&filter, &config, accel, gyro, 0.01),
|
||||
"stationary propagation succeeds");
|
||||
}
|
||||
CHECK(fabs(eskf_speed_mps(&filter)) < 1.0e-9,
|
||||
"stationary propagation keeps zero velocity");
|
||||
CHECK(fabs(filter.position_m[2]) < 1.0e-9,
|
||||
"stationary propagation keeps zero height");
|
||||
CHECK(eskf_state_is_finite(&filter), "stationary state remains finite");
|
||||
}
|
||||
|
||||
static void test_gravity_alignment(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t config;
|
||||
eskf_default_config(&config);
|
||||
double measured_gravity[3] = {0.35, -0.22, 9.79793};
|
||||
double nav_up[3] = {0.0, 0.0, 1.0};
|
||||
double quaternion[4];
|
||||
CHECK(eskf_quaternion_align_vectors(measured_gravity, nav_up, quaternion),
|
||||
"gravity alignment quaternion");
|
||||
double position[3] = {0.0, 0.0, 0.0};
|
||||
double accel_bias[3] = {0.0, 0.0, 0.0};
|
||||
double gyro_bias[3] = {0.0, 0.0, 0.0};
|
||||
double gravity_norm = sqrt(measured_gravity[0] * measured_gravity[0] +
|
||||
measured_gravity[1] * measured_gravity[1] +
|
||||
measured_gravity[2] * measured_gravity[2]);
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
measured_gravity[i] *= config.gravity_mps2 / gravity_norm;
|
||||
}
|
||||
CHECK(eskf_initialize(&filter, &config, position, quaternion,
|
||||
accel_bias, gyro_bias),
|
||||
"tilted ESKF initialization");
|
||||
double gyro[3] = {0.0, 0.0, 0.0};
|
||||
CHECK(eskf_propagate(&filter, &config, measured_gravity, gyro, 0.01),
|
||||
"tilted stationary propagation");
|
||||
CHECK(sqrt(filter.last_linear_accel_nav_mps2[0] *
|
||||
filter.last_linear_accel_nav_mps2[0] +
|
||||
filter.last_linear_accel_nav_mps2[1] *
|
||||
filter.last_linear_accel_nav_mps2[1] +
|
||||
filter.last_linear_accel_nav_mps2[2] *
|
||||
filter.last_linear_accel_nav_mps2[2]) < 1.0e-8,
|
||||
"gravity alignment removes gravity from linear acceleration");
|
||||
}
|
||||
|
||||
static void test_position_nis_gate(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t config;
|
||||
identity_filter(&filter, &config);
|
||||
double covariance[9] = {
|
||||
4.0, 0.2, 0.0,
|
||||
0.2, 4.0, 0.0,
|
||||
0.0, 0.0, 9.0
|
||||
};
|
||||
double normal_position[3] = {1.0, -0.5, 0.2};
|
||||
double nis = NAN;
|
||||
CHECK(eskf_position_update(&filter, &config, normal_position,
|
||||
covariance, &nis) == ESKF_UPDATE_ACCEPTED,
|
||||
"normal GPS position accepted");
|
||||
CHECK(isfinite(nis) && (nis < config.position_nis_gate),
|
||||
"accepted GPS has finite in-gate NIS");
|
||||
double before[3];
|
||||
memcpy(before, filter.position_m, sizeof(before));
|
||||
double outlier[3] = {1000.0, -800.0, 500.0};
|
||||
CHECK(eskf_position_update(&filter, &config, outlier,
|
||||
covariance, &nis) == ESKF_UPDATE_REJECTED_NIS,
|
||||
"large GPS outlier rejected by NIS");
|
||||
CHECK(nearly_equal(before[0], filter.position_m[0], 1.0e-12) &&
|
||||
nearly_equal(before[1], filter.position_m[1], 1.0e-12) &&
|
||||
nearly_equal(before[2], filter.position_m[2], 1.0e-12),
|
||||
"NIS rejection leaves nominal position unchanged");
|
||||
CHECK(filter.position_update_accepted_count == 1u &&
|
||||
filter.position_update_rejected_count == 1u,
|
||||
"GPS accept/reject counters");
|
||||
}
|
||||
|
||||
static double profile_acceleration(double time_s)
|
||||
{
|
||||
if((time_s >= 2.0) && (time_s < 4.0))
|
||||
{
|
||||
return 2.0;
|
||||
}
|
||||
if((time_s >= 6.0) && (time_s < 8.0))
|
||||
{
|
||||
return -2.0;
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
static void test_motion_transitions(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t eskf_config;
|
||||
identity_filter(&filter, &eskf_config);
|
||||
fusion_motion_config_t motion_config;
|
||||
fusion_motion_detector_t detector;
|
||||
fusion_motion_default_config(&motion_config);
|
||||
fusion_motion_init(&detector, true);
|
||||
|
||||
const double dt_s = 0.01;
|
||||
double first_moving_time = NAN;
|
||||
double return_stationary_time = NAN;
|
||||
bool stationary_during_constant_velocity = false;
|
||||
bool deceleration_seen = false;
|
||||
for(unsigned int sample = 1u ; sample <= 1100u ; sample++)
|
||||
{
|
||||
double time_s = (double)sample * dt_s;
|
||||
double acceleration = profile_acceleration(time_s - 0.5 * dt_s);
|
||||
double accel[3] = {acceleration, 0.0, eskf_config.gravity_mps2};
|
||||
double gyro[3] = {0.0, 0.0, 0.0};
|
||||
CHECK(eskf_propagate(&filter, &eskf_config, accel, gyro, dt_s),
|
||||
"motion profile propagation");
|
||||
fusion_motion_state_t state = fusion_motion_update(
|
||||
&detector, &motion_config,
|
||||
filter.last_linear_accel_nav_mps2,
|
||||
filter.last_unbiased_gyro_rps,
|
||||
eskf_speed_mps(&filter),
|
||||
FUSION_GNSS_MOTION_UNKNOWN,
|
||||
dt_s);
|
||||
if(isnan(first_moving_time) && (state == FUSION_MOTION_MOVING))
|
||||
{
|
||||
first_moving_time = time_s;
|
||||
}
|
||||
if((time_s >= 4.0) && (time_s < 6.0) &&
|
||||
(state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
stationary_during_constant_velocity = true;
|
||||
}
|
||||
if((time_s >= 6.0) && (time_s < 8.0) &&
|
||||
(detector.last_linear_accel_norm_mps2 > 1.5))
|
||||
{
|
||||
deceleration_seen = true;
|
||||
}
|
||||
if((time_s > 8.0) && isnan(return_stationary_time) &&
|
||||
(state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
return_stationary_time = time_s;
|
||||
}
|
||||
if(fusion_motion_allows_zero_update(&detector) &&
|
||||
(fmod(time_s, 0.05) < 0.5 * dt_s))
|
||||
{
|
||||
CHECK(eskf_zero_velocity_update(&filter, 0.03),
|
||||
"stationary ZUPT succeeds");
|
||||
}
|
||||
}
|
||||
|
||||
CHECK(isfinite(first_moving_time) && (first_moving_time <= 2.02),
|
||||
"sudden acceleration exits stationary within one IMU interval");
|
||||
CHECK(!stationary_during_constant_velocity,
|
||||
"constant velocity is not mistaken for stationary");
|
||||
CHECK(deceleration_seen, "sudden deceleration appears as dynamic evidence");
|
||||
CHECK(isfinite(return_stationary_time) && (return_stationary_time >= 9.0) &&
|
||||
(return_stationary_time <= 9.25),
|
||||
"stationary re-entry waits for jerk settling and quiet dwell");
|
||||
CHECK(fabs(filter.velocity_mps[0]) < 0.03,
|
||||
"accelerate/decelerate profile returns close to zero speed");
|
||||
}
|
||||
|
||||
static double run_rate_profile(unsigned int imu_hz, double gps_period_s)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t config;
|
||||
identity_filter(&filter, &config);
|
||||
double dt_s = 1.0 / (double)imu_hz;
|
||||
double next_gps_s = gps_period_s;
|
||||
double truth_position = 0.0;
|
||||
double truth_velocity = 0.0;
|
||||
double gps_covariance[9] = {
|
||||
0.25, 0.0, 0.0,
|
||||
0.0, 0.25, 0.0,
|
||||
0.0, 0.0, 1.0
|
||||
};
|
||||
for(unsigned int sample = 0 ; sample < 6u * imu_hz ; sample++)
|
||||
{
|
||||
double time_s = (double)sample * dt_s;
|
||||
double acceleration = 0.0;
|
||||
if((time_s >= 1.0) && (time_s < 2.0))
|
||||
{
|
||||
acceleration = 1.0;
|
||||
}
|
||||
else if((time_s >= 3.0) && (time_s < 4.0))
|
||||
{
|
||||
acceleration = -1.0;
|
||||
}
|
||||
truth_position += truth_velocity * dt_s + 0.5 * acceleration * dt_s * dt_s;
|
||||
truth_velocity += acceleration * dt_s;
|
||||
double accel[3] = {acceleration, 0.0, config.gravity_mps2};
|
||||
double gyro[3] = {0.0, 0.0, 0.0};
|
||||
CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s),
|
||||
"variable-rate propagation");
|
||||
double end_time_s = time_s + dt_s;
|
||||
if((end_time_s + 1.0e-9) >= next_gps_s)
|
||||
{
|
||||
double gps_position[3] = {truth_position, 0.0, 0.0};
|
||||
CHECK(eskf_position_update(&filter, &config, gps_position,
|
||||
gps_covariance, NULL) == ESKF_UPDATE_ACCEPTED,
|
||||
"variable-rate asynchronous GPS update");
|
||||
next_gps_s += gps_period_s;
|
||||
}
|
||||
}
|
||||
CHECK(fabs(truth_position - 2.0) < 1.0e-9,
|
||||
"synthetic rate profile truth distance");
|
||||
return filter.position_m[0];
|
||||
}
|
||||
|
||||
static void test_variable_rates(void)
|
||||
{
|
||||
double position_100hz = run_rate_profile(100u, 1.0);
|
||||
double position_200hz = run_rate_profile(200u, 0.5);
|
||||
double position_500hz = run_rate_profile(500u, 1.0);
|
||||
CHECK(fabs(position_100hz - 2.0) < 0.02,
|
||||
"100 Hz IMU / 1 Hz GPS result");
|
||||
CHECK(fabs(position_200hz - 2.0) < 0.02,
|
||||
"200 Hz IMU / 2 Hz GPS result");
|
||||
CHECK(fabs(position_500hz - 2.0) < 0.02,
|
||||
"500 Hz IMU / 1 Hz GPS result");
|
||||
CHECK(fabs(position_100hz - position_200hz) < 0.02 &&
|
||||
fabs(position_200hz - position_500hz) < 0.02,
|
||||
"filter result is insensitive to configured asynchronous rates");
|
||||
}
|
||||
|
||||
static void test_irregular_imu_timestamps(void)
|
||||
{
|
||||
eskf_t filter;
|
||||
eskf_config_t config;
|
||||
identity_filter(&filter, &config);
|
||||
const double intervals_s[] = {0.003, 0.011, 0.004, 0.017, 0.006, 0.009};
|
||||
double elapsed_s = 0.0;
|
||||
double truth_position[3] = {0.0, 0.0, 0.0};
|
||||
double truth_velocity[3] = {0.0, 0.0, 0.0};
|
||||
const double nav_acceleration[3] = {0.20, -0.10, 0.0};
|
||||
unsigned int interval_index = 0u;
|
||||
while(elapsed_s < 5.0)
|
||||
{
|
||||
double dt_s = intervals_s[interval_index %
|
||||
(sizeof(intervals_s) / sizeof(intervals_s[0]))];
|
||||
if((elapsed_s + dt_s) > 5.0)
|
||||
{
|
||||
dt_s = 5.0 - elapsed_s;
|
||||
}
|
||||
double accel[3] = {
|
||||
nav_acceleration[0], nav_acceleration[1], config.gravity_mps2
|
||||
};
|
||||
double gyro[3] = {0.0, 0.0, 0.0};
|
||||
CHECK(eskf_propagate(&filter, &config, accel, gyro, dt_s),
|
||||
"irregular timestamp propagation succeeds");
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
truth_position[axis] += truth_velocity[axis] * dt_s +
|
||||
0.5 * nav_acceleration[axis] * dt_s * dt_s;
|
||||
truth_velocity[axis] += nav_acceleration[axis] * dt_s;
|
||||
}
|
||||
elapsed_s += dt_s;
|
||||
interval_index++;
|
||||
}
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
CHECK(fabs(filter.position_m[axis] - truth_position[axis]) < 1.0e-10,
|
||||
"irregular local-tick dt preserves position integration");
|
||||
CHECK(fabs(filter.velocity_mps[axis] - truth_velocity[axis]) < 1.0e-10,
|
||||
"irregular local-tick dt preserves velocity integration");
|
||||
}
|
||||
}
|
||||
|
||||
static void test_motion_vetoes(void)
|
||||
{
|
||||
fusion_motion_config_t config;
|
||||
fusion_motion_detector_t detector;
|
||||
fusion_motion_default_config(&config);
|
||||
fusion_motion_init(&detector, false);
|
||||
double zero[3] = {0.0, 0.0, 0.0};
|
||||
for(unsigned int i = 0 ; i < 200u ; i++)
|
||||
{
|
||||
(void)fusion_motion_update(&detector, &config, zero, zero, 3.0,
|
||||
FUSION_GNSS_MOTION_UNKNOWN, 0.01);
|
||||
}
|
||||
CHECK(detector.state == FUSION_MOTION_MOVING,
|
||||
"nonzero ESKF speed vetoes stationary at constant velocity");
|
||||
for(unsigned int i = 0 ; i < 200u ; i++)
|
||||
{
|
||||
(void)fusion_motion_update(&detector, &config, zero, zero, 3.0,
|
||||
FUSION_GNSS_MOTION_STILL, 0.01);
|
||||
}
|
||||
CHECK(detector.state == FUSION_MOTION_MOVING,
|
||||
"position-only GNSS still evidence cannot override high ESKF speed");
|
||||
for(unsigned int i = 0 ; i < 200u ; i++)
|
||||
{
|
||||
(void)fusion_motion_update(&detector, &config, zero, zero, 0.0,
|
||||
FUSION_GNSS_MOTION_MOVING, 0.01);
|
||||
}
|
||||
CHECK(detector.state == FUSION_MOTION_MOVING,
|
||||
"GNSS motion evidence vetoes stationary");
|
||||
for(unsigned int i = 0 ; i < 120u ; i++)
|
||||
{
|
||||
(void)fusion_motion_update(&detector, &config, zero, zero, 0.0,
|
||||
FUSION_GNSS_MOTION_STILL, 0.01);
|
||||
}
|
||||
CHECK(detector.state == FUSION_MOTION_STATIONARY,
|
||||
"quiet low-speed state enters stationary after dwell");
|
||||
|
||||
fusion_motion_init(&detector, true);
|
||||
double weak_accel[3] = {0.25, 0.0, 0.0};
|
||||
for(unsigned int i = 0 ; i < config.weak_motion_samples - 1u ; i++)
|
||||
{
|
||||
(void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0,
|
||||
FUSION_GNSS_MOTION_UNKNOWN, 0.01);
|
||||
}
|
||||
CHECK(detector.state == FUSION_MOTION_STATIONARY,
|
||||
"short weak disturbance does not chatter state");
|
||||
(void)fusion_motion_update(&detector, &config, weak_accel, zero, 0.0,
|
||||
FUSION_GNSS_MOTION_UNKNOWN, 0.01);
|
||||
CHECK(detector.state == FUSION_MOTION_MOVING,
|
||||
"persistent weak motion exits stationary");
|
||||
}
|
||||
|
||||
static void test_stationary_noise_and_impulse_detection(void)
|
||||
{
|
||||
fusion_motion_config_t config;
|
||||
fusion_motion_detector_t detector;
|
||||
fusion_motion_default_config(&config);
|
||||
fusion_motion_init(&detector, true);
|
||||
noise_state = 0x12345678u;
|
||||
const double dt_s = 1.0 / 500.0;
|
||||
unsigned int nonstationary_samples = 0u;
|
||||
for(unsigned int sample = 0u ; sample < 5000u ; sample++)
|
||||
{
|
||||
double accel[3];
|
||||
double gyro[3];
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
accel[axis] = 0.025 * gaussian_noise();
|
||||
gyro[axis] = 0.0008 * gaussian_noise();
|
||||
}
|
||||
fusion_motion_state_t state = fusion_motion_update(
|
||||
&detector, &config, accel, gyro, 0.0,
|
||||
FUSION_GNSS_MOTION_STILL, dt_s);
|
||||
if(state != FUSION_MOTION_STATIONARY)
|
||||
{
|
||||
nonstationary_samples++;
|
||||
}
|
||||
}
|
||||
CHECK(nonstationary_samples == 0u,
|
||||
"500 Hz stationary IMU noise does not cause false motion");
|
||||
|
||||
double sudden_accel[3] = {1.0, 0.0, 0.0};
|
||||
double zero_gyro[3] = {0.0, 0.0, 0.0};
|
||||
CHECK(fusion_motion_update(&detector, &config, sudden_accel, zero_gyro,
|
||||
0.0, FUSION_GNSS_MOTION_STILL, dt_s) ==
|
||||
FUSION_MOTION_MOVING,
|
||||
"filtered jerk still detects a sudden acceleration in one 500 Hz sample");
|
||||
}
|
||||
|
||||
static void initialize_engine_for_replay(fusion_engine_t* engine)
|
||||
{
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, 1000u);
|
||||
config.calibration_duration_s = 0.10;
|
||||
config.calibration_min_samples = 10u;
|
||||
CHECK(fusion_engine_init(engine, &config), "fusion engine initialization");
|
||||
|
||||
fusion_position_measurement_t origin = {0};
|
||||
origin.timestamp_ticks = 0u;
|
||||
origin.source_valid = true;
|
||||
origin.source_quality = 1.0;
|
||||
origin.covariance_enu_m2[0] = 0.25;
|
||||
origin.covariance_enu_m2[4] = 0.25;
|
||||
origin.covariance_enu_m2[8] = 1.0;
|
||||
CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
|
||||
"initial position retained during calibration");
|
||||
|
||||
for(uint64_t tick = 0u ; tick <= 100u ; tick += 10u)
|
||||
{
|
||||
fusion_imu_measurement_t imu = {0};
|
||||
imu.timestamp_ticks = tick;
|
||||
imu.accel_mps2[2] = config.eskf.gravity_mps2;
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = (uint32_t)(tick / 10u);
|
||||
CHECK(fusion_engine_push_imu(engine, &imu),
|
||||
"stationary calibration IMU accepted");
|
||||
}
|
||||
CHECK(engine->state == FUSION_ENGINE_RUNNING,
|
||||
"fusion engine leaves calibration after quiet duration");
|
||||
}
|
||||
|
||||
static double replay_profile_accel(uint64_t tick)
|
||||
{
|
||||
double time_s = (double)tick / 1000.0;
|
||||
if((time_s > 1.0) && (time_s <= 2.0))
|
||||
{
|
||||
return 1.0;
|
||||
}
|
||||
if((time_s > 2.0) && (time_s <= 3.0))
|
||||
{
|
||||
return -1.0;
|
||||
}
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
static void push_replay_imu(fusion_engine_t* engine, uint64_t tick)
|
||||
{
|
||||
fusion_imu_measurement_t imu = {0};
|
||||
imu.timestamp_ticks = tick;
|
||||
imu.accel_mps2[0] = replay_profile_accel(tick);
|
||||
imu.accel_mps2[2] = engine->config.eskf.gravity_mps2;
|
||||
imu.temperature_c = 25.0;
|
||||
imu.sequence = (uint32_t)(tick / 10u);
|
||||
CHECK(fusion_engine_push_imu(engine, &imu), "replay profile IMU accepted");
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t replay_position(uint64_t tick, double east_m)
|
||||
{
|
||||
fusion_position_measurement_t position = {0};
|
||||
position.timestamp_ticks = tick;
|
||||
position.position_enu_m[0] = east_m;
|
||||
position.covariance_enu_m2[0] = 0.25;
|
||||
position.covariance_enu_m2[4] = 0.25;
|
||||
position.covariance_enu_m2[8] = 1.0;
|
||||
position.source_quality = 1.0;
|
||||
position.source_valid = true;
|
||||
return position;
|
||||
}
|
||||
|
||||
static void check_runtime_close(const fusion_runtime_t* first,
|
||||
const fusion_runtime_t* second,
|
||||
double tolerance)
|
||||
{
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
CHECK(fabs(first->filter.position_m[i] - second->filter.position_m[i]) < tolerance,
|
||||
"delayed replay position matches on-time update");
|
||||
CHECK(fabs(first->filter.velocity_mps[i] - second->filter.velocity_mps[i]) < tolerance,
|
||||
"delayed replay velocity matches on-time update");
|
||||
CHECK(fabs(first->filter.accel_bias_mps2[i] - second->filter.accel_bias_mps2[i]) < tolerance,
|
||||
"delayed replay accel bias matches on-time update");
|
||||
CHECK(fabs(first->filter.gyro_bias_rps[i] - second->filter.gyro_bias_rps[i]) < tolerance,
|
||||
"delayed replay gyro bias matches on-time update");
|
||||
}
|
||||
for(unsigned int i = 0 ; i < 4u ; i++)
|
||||
{
|
||||
CHECK(fabs(first->filter.body_to_nav_quaternion[i] -
|
||||
second->filter.body_to_nav_quaternion[i]) < tolerance,
|
||||
"delayed replay attitude matches on-time update");
|
||||
}
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
CHECK(fabs(first->filter.covariance[row][col] -
|
||||
second->filter.covariance[row][col]) < tolerance,
|
||||
"delayed replay covariance matches on-time update");
|
||||
}
|
||||
}
|
||||
CHECK(first->motion.state == second->motion.state,
|
||||
"delayed replay motion state matches on-time update");
|
||||
CHECK(first->filter_timestamp_ticks == second->filter_timestamp_ticks,
|
||||
"delayed replay timestamp matches on-time update");
|
||||
}
|
||||
|
||||
static void test_delayed_position_replay(void)
|
||||
{
|
||||
static fusion_engine_t on_time;
|
||||
static fusion_engine_t delayed;
|
||||
initialize_engine_for_replay(&on_time);
|
||||
initialize_engine_for_replay(&delayed);
|
||||
fusion_position_measurement_t gps_at_1500 = replay_position(1500u, 0.125);
|
||||
for(uint64_t tick = 110u ; tick <= 3200u ; tick += 10u)
|
||||
{
|
||||
push_replay_imu(&on_time, tick);
|
||||
push_replay_imu(&delayed, tick);
|
||||
if(tick == 1500u)
|
||||
{
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
CHECK(fusion_engine_push_position(&on_time, &gps_at_1500,
|
||||
&result, NULL),
|
||||
"on-time position update processed");
|
||||
CHECK(result == ESKF_UPDATE_ACCEPTED,
|
||||
"on-time position update accepted");
|
||||
}
|
||||
if(tick == 1800u)
|
||||
{
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
CHECK(fusion_engine_push_position(&delayed, &gps_at_1500,
|
||||
&result, NULL),
|
||||
"300 ms delayed position update replayed");
|
||||
CHECK(result == ESKF_UPDATE_ACCEPTED,
|
||||
"delayed position update accepted at measurement epoch");
|
||||
check_runtime_close(&on_time.current, &delayed.current, 1.0e-11);
|
||||
}
|
||||
}
|
||||
check_runtime_close(&on_time.current, &delayed.current, 1.0e-11);
|
||||
CHECK(delayed.diagnostics.replay_count == 1u,
|
||||
"one delayed position caused one fixed-lag replay");
|
||||
CHECK(delayed.diagnostics.replayed_imu_count > 0u,
|
||||
"delayed position replayed stored IMU samples");
|
||||
|
||||
fusion_position_measurement_t future = replay_position(3500u, 1.0);
|
||||
CHECK(fusion_engine_push_position(&delayed, &future, NULL, NULL),
|
||||
"future-dated position queued without changing filter time");
|
||||
CHECK(delayed.pending_position_count == 1u,
|
||||
"future position is pending until IMU reaches its timestamp");
|
||||
for(uint64_t tick = 3210u ; tick <= 3500u ; tick += 10u)
|
||||
{
|
||||
push_replay_imu(&delayed, tick);
|
||||
}
|
||||
CHECK(delayed.pending_position_count == 0u,
|
||||
"pending position consumed at matching IMU time");
|
||||
CHECK(delayed.diagnostics.position_accepted == 2u,
|
||||
"on-time and future position updates accepted");
|
||||
|
||||
fusion_position_measurement_t stale = replay_position(1400u, 0.1);
|
||||
CHECK(!fusion_engine_push_position(&delayed, &stale, NULL, NULL),
|
||||
"position older than last checkpoint rejected");
|
||||
CHECK(delayed.diagnostics.position_stale_rejected == 1u,
|
||||
"stale position rejection is diagnosed");
|
||||
}
|
||||
|
||||
static fusion_gps_measurement_t adapter_gps(double latitude_deg,
|
||||
double longitude_deg,
|
||||
double height_m)
|
||||
{
|
||||
fusion_gps_measurement_t gps = {0};
|
||||
gps.latitude_deg = latitude_deg;
|
||||
gps.longitude_deg = longitude_deg;
|
||||
gps.height_m = height_m;
|
||||
gps.covariance_enu_m2[0][0] = 0.04;
|
||||
gps.covariance_enu_m2[1][1] = 0.09;
|
||||
gps.covariance_enu_m2[2][2] = 0.25;
|
||||
gps.chi_square_ratio = 0.5;
|
||||
gps.solution_valid = true;
|
||||
return gps;
|
||||
}
|
||||
|
||||
static void test_gps_adapter(void)
|
||||
{
|
||||
fusion_gps_adapter_config_t config;
|
||||
fusion_gps_adapter_t adapter;
|
||||
fusion_gps_adapter_default_config(&config);
|
||||
CHECK(fusion_gps_adapter_init(&adapter, &config), "GPS adapter initialization");
|
||||
fusion_gps_measurement_t origin = adapter_gps(41.015137, 28.979530, 100.0);
|
||||
fusion_position_measurement_t local;
|
||||
CHECK(fusion_gps_adapter_convert(&adapter, &origin, &local),
|
||||
"GPS origin converts to local frame");
|
||||
CHECK(test_norm3(local.position_enu_m) < 1.0e-8,
|
||||
"first valid GPS establishes zero ENU origin");
|
||||
CHECK(nearly_equal(local.covariance_enu_m2[0], 0.25, 1.0e-12) &&
|
||||
nearly_equal(local.covariance_enu_m2[4], 0.25, 1.0e-12) &&
|
||||
nearly_equal(local.covariance_enu_m2[8], 1.0, 1.0e-12),
|
||||
"GPS covariance floors prevent overconfident ESKF updates");
|
||||
|
||||
fusion_gps_measurement_t moved = adapter_gps(41.015147, 28.979550, 103.0);
|
||||
moved.timestamp_ticks = 1000u;
|
||||
CHECK(fusion_gps_adapter_convert(&adapter, &moved, &local),
|
||||
"second GPS converts to ENU");
|
||||
CHECK(local.position_enu_m[0] > 1.0 && local.position_enu_m[1] > 0.5,
|
||||
"latitude/longitude change has expected ENU direction");
|
||||
double latitude;
|
||||
double longitude;
|
||||
double height;
|
||||
CHECK(fusion_gps_adapter_local_to_geodetic(&adapter, local.position_enu_m,
|
||||
&latitude, &longitude, &height),
|
||||
"fused ENU converts back to geodetic telemetry");
|
||||
CHECK(fabs(latitude - moved.latitude_deg) < 1.0e-8 &&
|
||||
fabs(longitude - moved.longitude_deg) < 1.0e-8 &&
|
||||
fabs(height - moved.height_m) < 1.0e-3,
|
||||
"ENU/geodetic round trip is accurate");
|
||||
|
||||
fusion_gps_measurement_t invalid = moved;
|
||||
invalid.covariance_enu_m2[0][0] = -1.0;
|
||||
CHECK(!fusion_gps_adapter_convert(&adapter, &invalid, &local),
|
||||
"invalid GPS covariance is rejected before ESKF");
|
||||
}
|
||||
|
||||
static void test_measurement_bus(void)
|
||||
{
|
||||
static fusion_bus_t bus;
|
||||
fusion_bus_init(&bus);
|
||||
for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++)
|
||||
{
|
||||
fusion_imu_measurement_t imu = {0};
|
||||
imu.sequence = sequence;
|
||||
imu.timestamp_ticks = sequence;
|
||||
CHECK(fusion_bus_publish_imu(&bus, &imu), "IMU bus enqueue");
|
||||
}
|
||||
fusion_imu_measurement_t overflow = {0};
|
||||
CHECK(!fusion_bus_publish_imu(&bus, &overflow),
|
||||
"full IMU bus rejects newest sample without overwriting history");
|
||||
CHECK(bus.imu_overflow_count == 1u, "IMU bus overflow counter");
|
||||
for(unsigned int sequence = 0u ; sequence < FUSION_IMU_QUEUE_CAPACITY - 1u ; sequence++)
|
||||
{
|
||||
fusion_imu_measurement_t imu;
|
||||
CHECK(fusion_bus_take_imu(&bus, &imu), "IMU bus dequeue");
|
||||
CHECK(imu.sequence == sequence, "IMU bus preserves FIFO ordering");
|
||||
}
|
||||
CHECK(!fusion_bus_take_imu(&bus, &overflow), "empty IMU bus reports no sample");
|
||||
|
||||
for(unsigned int sequence = 0u ; sequence < FUSION_GPS_QUEUE_CAPACITY - 1u ; sequence++)
|
||||
{
|
||||
fusion_gps_measurement_t gps = {0};
|
||||
gps.timestamp_ticks = sequence;
|
||||
CHECK(fusion_bus_publish_gps(&bus, &gps), "GPS bus enqueue");
|
||||
}
|
||||
fusion_gps_measurement_t gps_overflow = {0};
|
||||
CHECK(!fusion_bus_publish_gps(&bus, &gps_overflow), "GPS bus overflow detected");
|
||||
CHECK(bus.gps_overflow_count == 1u, "GPS bus overflow counter");
|
||||
}
|
||||
|
||||
static void test_history_rolls_checkpoint_forward(void)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
initialize_engine_for_replay(&engine);
|
||||
uint64_t tick = 110u;
|
||||
for(unsigned int i = 0u ; i < FUSION_IMU_HISTORY_CAPACITY + 64u ; i++, tick += 10u)
|
||||
{
|
||||
push_replay_imu(&engine, tick);
|
||||
}
|
||||
CHECK(engine.state == FUSION_ENGINE_RUNNING,
|
||||
"long GPS outage does not fault fusion engine");
|
||||
CHECK(engine.diagnostics.imu_history_overflow == 64u,
|
||||
"fixed-lag history reports bounded rollover");
|
||||
CHECK(engine.diagnostics.checkpoint_advance_count == 64u,
|
||||
"checkpoint advances with each expired IMU sample");
|
||||
CHECK(engine.checkpoint.filter_timestamp_ticks > 100u,
|
||||
"checkpoint no longer precedes retained history");
|
||||
fusion_position_measurement_t recovery = replay_position(tick - 10u, 0.0);
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
CHECK(fusion_engine_push_position(&engine, &recovery, &result, NULL),
|
||||
"GPS recovery after history rollover is processed");
|
||||
CHECK(result == ESKF_UPDATE_ACCEPTED,
|
||||
"GPS recovery after long outage passes consistent NIS");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_stationary_propagation();
|
||||
test_gravity_alignment();
|
||||
test_position_nis_gate();
|
||||
test_motion_transitions();
|
||||
test_variable_rates();
|
||||
test_irregular_imu_timestamps();
|
||||
test_motion_vetoes();
|
||||
test_stationary_noise_and_impulse_detection();
|
||||
test_delayed_position_replay();
|
||||
test_gps_adapter();
|
||||
test_measurement_bus();
|
||||
test_history_rolls_checkpoint_forward();
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "%d ESKF/motion test(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("ESKF/motion tests passed (gravity, NIS, replay, noise, 100/200/500 Hz).\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,648 @@
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "fusion_engine.h"
|
||||
|
||||
#define SYNTH_PI 3.14159265358979323846264338327950288
|
||||
#define SYNTH_TIMEBASE_HZ 1000000u
|
||||
#define SYNTH_IMU_HZ 200u
|
||||
#define SYNTH_DURATION_S 60u
|
||||
#define SYNTH_GPS_EVENT_CAPACITY 96u
|
||||
|
||||
typedef struct {
|
||||
uint32_t state;
|
||||
} synth_rng_t;
|
||||
|
||||
typedef struct {
|
||||
double position_m[3];
|
||||
double velocity_mps[3];
|
||||
double yaw_rad;
|
||||
} synth_truth_t;
|
||||
|
||||
typedef struct {
|
||||
fusion_position_measurement_t measurement;
|
||||
uint64_t delivery_ticks;
|
||||
bool injected_outlier;
|
||||
} synth_gps_event_t;
|
||||
|
||||
typedef struct {
|
||||
double horizontal_rmse_m;
|
||||
double vertical_rmse_m;
|
||||
double final_horizontal_error_m;
|
||||
double final_vertical_error_m;
|
||||
double final_speed_mps;
|
||||
double max_rejected_state_jump_m;
|
||||
double covariance_symmetry_error;
|
||||
uint64_t accepted_gps;
|
||||
uint64_t rejected_gps;
|
||||
uint64_t replay_count;
|
||||
uint64_t replayed_imu_count;
|
||||
uint64_t zero_updates;
|
||||
unsigned int injected_outliers;
|
||||
unsigned int rejected_injected_outliers;
|
||||
unsigned int false_stationary_samples;
|
||||
double moving_detection_time_s;
|
||||
double stationary_reentry_time_s;
|
||||
bool covariance_positive_semidefinite;
|
||||
bool state_finite;
|
||||
} synth_metrics_t;
|
||||
|
||||
typedef struct {
|
||||
double final_horizontal_error_m;
|
||||
double final_vertical_error_m;
|
||||
double final_speed_mps;
|
||||
uint64_t accepted_gps;
|
||||
uint64_t zero_updates;
|
||||
unsigned int nonstationary_samples;
|
||||
bool covariance_positive_semidefinite;
|
||||
bool state_finite;
|
||||
} stationary_metrics_t;
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
static double square(double value)
|
||||
{
|
||||
return value * value;
|
||||
}
|
||||
|
||||
static double norm2(double first, double second)
|
||||
{
|
||||
return sqrt(first * first + second * second);
|
||||
}
|
||||
|
||||
static double norm3(const double value[3])
|
||||
{
|
||||
return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
|
||||
}
|
||||
|
||||
static uint32_t rng_next(synth_rng_t* rng)
|
||||
{
|
||||
uint32_t value = rng->state;
|
||||
value ^= value << 13;
|
||||
value ^= value >> 17;
|
||||
value ^= value << 5;
|
||||
rng->state = value;
|
||||
return value;
|
||||
}
|
||||
|
||||
static double rng_uniform(synth_rng_t* rng)
|
||||
{
|
||||
return ((double)rng_next(rng) + 0.5) / 4294967296.0;
|
||||
}
|
||||
|
||||
static double rng_gaussian(synth_rng_t* rng)
|
||||
{
|
||||
double first = fmax(rng_uniform(rng), 1.0e-12);
|
||||
double second = rng_uniform(rng);
|
||||
return sqrt(-2.0 * log(first)) * cos(2.0 * SYNTH_PI * second);
|
||||
}
|
||||
|
||||
static void trajectory_input(double time_s,
|
||||
double acceleration_nav_mps2[3],
|
||||
double* yaw_rate_rps)
|
||||
{
|
||||
memset(acceleration_nav_mps2, 0, 3u * sizeof(double));
|
||||
*yaw_rate_rps = 0.0;
|
||||
if((time_s >= 5.0) && (time_s < 7.0))
|
||||
{
|
||||
acceleration_nav_mps2[0] = 1.0;
|
||||
}
|
||||
else if((time_s >= 10.0) && (time_s < 12.0))
|
||||
{
|
||||
acceleration_nav_mps2[0] = -1.0;
|
||||
}
|
||||
else if((time_s >= 15.0) && (time_s < 17.0))
|
||||
{
|
||||
*yaw_rate_rps = 0.25 * SYNTH_PI;
|
||||
}
|
||||
else if((time_s >= 17.0) && (time_s < 19.0))
|
||||
{
|
||||
acceleration_nav_mps2[1] = 0.8;
|
||||
}
|
||||
else if((time_s >= 22.0) && (time_s < 24.0))
|
||||
{
|
||||
acceleration_nav_mps2[1] = -0.8;
|
||||
}
|
||||
}
|
||||
|
||||
static void truth_step(synth_truth_t* truth,
|
||||
double dt_s,
|
||||
const double acceleration_nav_mps2[3],
|
||||
double yaw_rate_rps)
|
||||
{
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
|
||||
0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s;
|
||||
truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s;
|
||||
}
|
||||
truth->yaw_rad += yaw_rate_rps * dt_s;
|
||||
}
|
||||
|
||||
static fusion_position_measurement_t make_position(
|
||||
uint64_t timestamp_ticks,
|
||||
const synth_truth_t* truth,
|
||||
synth_rng_t* gps_rng,
|
||||
bool outlier)
|
||||
{
|
||||
fusion_position_measurement_t position;
|
||||
memset(&position, 0, sizeof(position));
|
||||
position.timestamp_ticks = timestamp_ticks;
|
||||
position.position_enu_m[0] = truth->position_m[0] + 1.2 * rng_gaussian(gps_rng);
|
||||
position.position_enu_m[1] = truth->position_m[1] + 1.2 * rng_gaussian(gps_rng);
|
||||
position.position_enu_m[2] = truth->position_m[2] + 2.0 * rng_gaussian(gps_rng);
|
||||
if(outlier)
|
||||
{
|
||||
position.position_enu_m[0] += 150.0;
|
||||
position.position_enu_m[1] -= 120.0;
|
||||
position.position_enu_m[2] += 80.0;
|
||||
}
|
||||
position.covariance_enu_m2[0] = square(1.2);
|
||||
position.covariance_enu_m2[4] = square(1.2);
|
||||
position.covariance_enu_m2[8] = square(2.0);
|
||||
position.source_quality = outlier ? 0.7 : 1.0;
|
||||
position.source_valid = true;
|
||||
return position;
|
||||
}
|
||||
|
||||
static bool covariance_is_psd(const eskf_t* filter,
|
||||
double* symmetry_error_out)
|
||||
{
|
||||
double lower[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
double max_symmetry_error = 0.0;
|
||||
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
double error = fabs(filter->covariance[row][col] -
|
||||
filter->covariance[col][row]);
|
||||
if(error > max_symmetry_error)
|
||||
{
|
||||
max_symmetry_error = error;
|
||||
}
|
||||
}
|
||||
}
|
||||
if(symmetry_error_out != NULL)
|
||||
{
|
||||
*symmetry_error_out = max_symmetry_error;
|
||||
}
|
||||
|
||||
/* A tiny diagonal jitter makes the semidefinite check numerically robust. */
|
||||
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0u ; col <= row ; col++)
|
||||
{
|
||||
double value = filter->covariance[row][col];
|
||||
if(row == col)
|
||||
{
|
||||
value += 1.0e-10;
|
||||
}
|
||||
for(unsigned int k = 0u ; k < col ; k++)
|
||||
{
|
||||
value -= lower[row][k] * lower[col][k];
|
||||
}
|
||||
if(row == col)
|
||||
{
|
||||
if(!isfinite(value) || (value <= 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
lower[row][col] = sqrt(value);
|
||||
}
|
||||
else
|
||||
{
|
||||
lower[row][col] = value / lower[col][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void initialize_synthetic_engine(fusion_engine_t* engine)
|
||||
{
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
|
||||
config.calibration_duration_s = 2.0;
|
||||
config.calibration_min_samples = SYNTH_IMU_HZ;
|
||||
CHECK(fusion_engine_init(engine, &config), "synthetic fusion engine initializes");
|
||||
|
||||
fusion_position_measurement_t origin;
|
||||
memset(&origin, 0, sizeof(origin));
|
||||
origin.timestamp_ticks = 0u;
|
||||
origin.covariance_enu_m2[0] = square(1.2);
|
||||
origin.covariance_enu_m2[4] = square(1.2);
|
||||
origin.covariance_enu_m2[8] = square(2.0);
|
||||
origin.source_quality = 1.0;
|
||||
origin.source_valid = true;
|
||||
CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
|
||||
"initial GPS is retained during calibration");
|
||||
}
|
||||
|
||||
static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
initialize_synthetic_engine(&engine);
|
||||
synth_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0};
|
||||
synth_rng_t imu_rng = {0x13579bdfu ^ (0x9e3779b9u * scenario_seed)};
|
||||
synth_rng_t gps_rng = {0x2468ace1u ^ (0x85ebca6bu * scenario_seed)};
|
||||
synth_rng_t timing_rng = {0xa5a5c3c3u ^ (0xc2b2ae35u * scenario_seed)};
|
||||
const double accel_bias[3] = {0.030, -0.025, 0.060};
|
||||
const double gyro_bias[3] = {0.0010, -0.0007, 0.0009};
|
||||
const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / SYNTH_IMU_HZ;
|
||||
const double dt_s = 1.0 / (double)SYNTH_IMU_HZ;
|
||||
const uint64_t final_tick = (uint64_t)SYNTH_DURATION_S * SYNTH_TIMEBASE_HZ;
|
||||
uint64_t next_gps_tick = 3000000u;
|
||||
unsigned int gps_sequence = 0u;
|
||||
synth_gps_event_t events[SYNTH_GPS_EVENT_CAPACITY];
|
||||
unsigned int event_head = 0u;
|
||||
unsigned int event_count = 0u;
|
||||
double horizontal_error_sum = 0.0;
|
||||
double vertical_error_sum = 0.0;
|
||||
uint64_t error_sample_count = 0u;
|
||||
synth_metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
metrics.moving_detection_time_s = NAN;
|
||||
metrics.stationary_reentry_time_s = NAN;
|
||||
#ifdef SYNTH_TRACE
|
||||
fusion_motion_state_t traced_state = FUSION_MOTION_BOOTSTRAP;
|
||||
#endif
|
||||
|
||||
for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
|
||||
{
|
||||
double acceleration_nav[3] = {0.0, 0.0, 0.0};
|
||||
double yaw_rate = 0.0;
|
||||
if(tick != 0u)
|
||||
{
|
||||
double midpoint_s = ((double)tick - 0.5 * (double)imu_period_ticks) /
|
||||
(double)SYNTH_TIMEBASE_HZ;
|
||||
trajectory_input(midpoint_s, acceleration_nav, &yaw_rate);
|
||||
truth_step(&truth, dt_s, acceleration_nav, yaw_rate);
|
||||
}
|
||||
|
||||
double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s;
|
||||
double cosine = cos(midpoint_yaw);
|
||||
double sine = sin(midpoint_yaw);
|
||||
double specific_force_nav[3] = {
|
||||
acceleration_nav[0], acceleration_nav[1], engine.config.eskf.gravity_mps2
|
||||
};
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = tick;
|
||||
imu.accel_mps2[0] = cosine * specific_force_nav[0] +
|
||||
sine * specific_force_nav[1];
|
||||
imu.accel_mps2[1] = -sine * specific_force_nav[0] +
|
||||
cosine * specific_force_nav[1];
|
||||
imu.accel_mps2[2] = specific_force_nav[2];
|
||||
imu.gyro_rps[2] = yaw_rate;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
imu.accel_mps2[axis] += accel_bias[axis] + 0.025 * rng_gaussian(&imu_rng);
|
||||
imu.gyro_rps[axis] += gyro_bias[axis] + 0.0008 * rng_gaussian(&imu_rng);
|
||||
}
|
||||
imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0);
|
||||
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
||||
CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted");
|
||||
if(engine.state == FUSION_ENGINE_FAULT)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
if((tick >= next_gps_tick) &&
|
||||
((event_head + event_count) < SYNTH_GPS_EVENT_CAPACITY))
|
||||
{
|
||||
bool outlier = (gps_sequence == 8u) || (gps_sequence == 27u);
|
||||
unsigned int index = event_head + event_count;
|
||||
events[index].measurement = make_position(tick, &truth, &gps_rng, outlier);
|
||||
double delay_s = 0.18 + 0.17 * rng_uniform(&timing_rng);
|
||||
events[index].delivery_ticks = tick +
|
||||
(uint64_t)(delay_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
|
||||
events[index].injected_outlier = outlier;
|
||||
event_count++;
|
||||
if(outlier)
|
||||
{
|
||||
metrics.injected_outliers++;
|
||||
}
|
||||
gps_sequence++;
|
||||
double next_period_s = 0.80 + 0.40 * rng_uniform(&timing_rng);
|
||||
next_gps_tick = tick +
|
||||
(uint64_t)(next_period_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
|
||||
}
|
||||
|
||||
while((event_count != 0u) && (events[event_head].delivery_ticks <= tick))
|
||||
{
|
||||
double before[3];
|
||||
memcpy(before, engine.current.filter.position_m, sizeof(before));
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
double nis = NAN;
|
||||
CHECK(fusion_engine_push_position(&engine, &events[event_head].measurement,
|
||||
&result, &nis),
|
||||
"delayed asynchronous GPS is processed");
|
||||
if(events[event_head].injected_outlier &&
|
||||
(result == ESKF_UPDATE_REJECTED_NIS))
|
||||
{
|
||||
metrics.rejected_injected_outliers++;
|
||||
double state_jump = norm2(
|
||||
engine.current.filter.position_m[0] - before[0],
|
||||
engine.current.filter.position_m[1] - before[1]);
|
||||
if(state_jump > metrics.max_rejected_state_jump_m)
|
||||
{
|
||||
metrics.max_rejected_state_jump_m = state_jump;
|
||||
}
|
||||
}
|
||||
event_head++;
|
||||
event_count--;
|
||||
}
|
||||
|
||||
if(engine.state == FUSION_ENGINE_RUNNING)
|
||||
{
|
||||
double time_s = (double)tick / (double)SYNTH_TIMEBASE_HZ;
|
||||
fusion_motion_state_t state = engine.current.motion.state;
|
||||
#ifdef SYNTH_TRACE
|
||||
if(state != traced_state)
|
||||
{
|
||||
printf("TRACE t=%.3f state=%s speed=%.3f gnss=%u confirm=%u accel=%.3f jerk=%.3f\n",
|
||||
time_s, fusion_motion_state_name(state),
|
||||
eskf_speed_mps(&engine.current.filter),
|
||||
(unsigned int)engine.current.gnss_evidence,
|
||||
engine.current.gnss_moving_count,
|
||||
engine.current.motion.last_linear_accel_norm_mps2,
|
||||
engine.current.motion.last_jerk_norm_mps3);
|
||||
traced_state = state;
|
||||
}
|
||||
#endif
|
||||
if(isnan(metrics.moving_detection_time_s) && (time_s >= 5.0) &&
|
||||
(state == FUSION_MOTION_MOVING))
|
||||
{
|
||||
metrics.moving_detection_time_s = time_s;
|
||||
}
|
||||
if((time_s >= 7.25) && (time_s <= 9.75) &&
|
||||
(state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
metrics.false_stationary_samples++;
|
||||
}
|
||||
if((time_s >= 19.25) && (time_s <= 21.75) &&
|
||||
(state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
metrics.false_stationary_samples++;
|
||||
}
|
||||
if(isnan(metrics.stationary_reentry_time_s) && (time_s >= 24.0) &&
|
||||
(state == FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
metrics.stationary_reentry_time_s = time_s;
|
||||
}
|
||||
if(time_s >= 3.0)
|
||||
{
|
||||
double east_error = engine.current.filter.position_m[0] - truth.position_m[0];
|
||||
double north_error = engine.current.filter.position_m[1] - truth.position_m[1];
|
||||
double up_error = engine.current.filter.position_m[2] - truth.position_m[2];
|
||||
horizontal_error_sum += square(east_error) + square(north_error);
|
||||
vertical_error_sum += square(up_error);
|
||||
error_sample_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
metrics.accepted_gps = engine.diagnostics.position_accepted;
|
||||
metrics.rejected_gps = engine.diagnostics.position_nis_rejected;
|
||||
metrics.replay_count = engine.diagnostics.replay_count;
|
||||
metrics.replayed_imu_count = engine.diagnostics.replayed_imu_count;
|
||||
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
||||
metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
|
||||
metrics.covariance_positive_semidefinite = covariance_is_psd(
|
||||
&engine.current.filter, &metrics.covariance_symmetry_error);
|
||||
if(error_sample_count != 0u)
|
||||
{
|
||||
metrics.horizontal_rmse_m = sqrt(horizontal_error_sum / (double)error_sample_count);
|
||||
metrics.vertical_rmse_m = sqrt(vertical_error_sum / (double)error_sample_count);
|
||||
}
|
||||
metrics.final_horizontal_error_m = norm2(
|
||||
engine.current.filter.position_m[0] - truth.position_m[0],
|
||||
engine.current.filter.position_m[1] - truth.position_m[1]);
|
||||
metrics.final_vertical_error_m = fabs(
|
||||
engine.current.filter.position_m[2] - truth.position_m[2]);
|
||||
metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps);
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void check_dynamic_metrics(const synth_metrics_t* metrics)
|
||||
{
|
||||
CHECK(metrics->state_finite, "60 s synthetic state remains finite");
|
||||
CHECK(metrics->covariance_positive_semidefinite,
|
||||
"60 s covariance remains positive semidefinite");
|
||||
CHECK(metrics->covariance_symmetry_error < 1.0e-11,
|
||||
"60 s covariance remains symmetric");
|
||||
CHECK(isfinite(metrics->moving_detection_time_s) &&
|
||||
(metrics->moving_detection_time_s <= 5.010),
|
||||
"sudden acceleration is detected within two 200 Hz samples");
|
||||
CHECK(metrics->false_stationary_samples == 0u,
|
||||
"constant-velocity legs never enable stationary updates");
|
||||
CHECK(isfinite(metrics->stationary_reentry_time_s) &&
|
||||
(metrics->stationary_reentry_time_s >= 25.0) &&
|
||||
(metrics->stationary_reentry_time_s <= 28.0),
|
||||
"final stop enters stationary after hysteresis dwell");
|
||||
CHECK(metrics->injected_outliers == 2u,
|
||||
"two synthetic multipath-like GPS outliers were injected");
|
||||
CHECK(metrics->rejected_injected_outliers == metrics->injected_outliers,
|
||||
"NIS rejects every injected GPS outlier");
|
||||
CHECK(metrics->max_rejected_state_jump_m < 1.0e-8,
|
||||
"rejected delayed GPS does not perturb the current state");
|
||||
CHECK(metrics->accepted_gps >= 45u,
|
||||
"most asynchronous GPS measurements are accepted");
|
||||
CHECK(metrics->rejected_gps >= metrics->injected_outliers,
|
||||
"GPS reject diagnostics include injected outliers");
|
||||
CHECK(metrics->replay_count >= 50u && metrics->replayed_imu_count > 1000u,
|
||||
"delayed GPS exercises fixed-lag replay extensively");
|
||||
CHECK(metrics->zero_updates > 300u,
|
||||
"stationary periods apply rate-limited ZUPT updates");
|
||||
CHECK(metrics->horizontal_rmse_m < 2.0,
|
||||
"bias/noise/delay trajectory horizontal RMSE is bounded");
|
||||
CHECK(metrics->vertical_rmse_m < 2.5,
|
||||
"bias/noise/delay trajectory vertical RMSE is bounded");
|
||||
CHECK(metrics->final_horizontal_error_m < 1.5,
|
||||
"final stationary horizontal error is bounded");
|
||||
CHECK(metrics->final_vertical_error_m < 2.0,
|
||||
"final stationary vertical error is bounded");
|
||||
CHECK(metrics->final_speed_mps < 0.05,
|
||||
"final stationary ZUPT speed is near zero");
|
||||
}
|
||||
|
||||
static stationary_metrics_t run_stationary_500hz_scenario(void)
|
||||
{
|
||||
static fusion_engine_t engine;
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
|
||||
config.calibration_duration_s = 2.0;
|
||||
config.calibration_min_samples = 500u;
|
||||
CHECK(fusion_engine_init(&engine, &config), "500 Hz stationary engine initializes");
|
||||
|
||||
fusion_position_measurement_t origin;
|
||||
memset(&origin, 0, sizeof(origin));
|
||||
origin.covariance_enu_m2[0] = square(0.8);
|
||||
origin.covariance_enu_m2[4] = square(0.8);
|
||||
origin.covariance_enu_m2[8] = square(1.5);
|
||||
origin.source_quality = 1.0;
|
||||
origin.source_valid = true;
|
||||
CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL),
|
||||
"500 Hz stationary origin accepted");
|
||||
|
||||
const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / 500u;
|
||||
const uint64_t final_tick = 30000000u;
|
||||
uint64_t next_gps_tick = 3000000u;
|
||||
uint64_t delivery_tick = 0u;
|
||||
bool gps_pending = false;
|
||||
fusion_position_measurement_t pending_gps;
|
||||
synth_rng_t imu_rng = {0x10203040u};
|
||||
synth_rng_t gps_rng = {0x55667788u};
|
||||
const double accel_bias[3] = {0.020, -0.015, 0.050};
|
||||
const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
|
||||
stationary_metrics_t metrics;
|
||||
memset(&metrics, 0, sizeof(metrics));
|
||||
|
||||
for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
|
||||
{
|
||||
fusion_imu_measurement_t imu;
|
||||
memset(&imu, 0, sizeof(imu));
|
||||
imu.timestamp_ticks = tick;
|
||||
imu.accel_mps2[2] = config.eskf.gravity_mps2;
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
imu.accel_mps2[axis] += accel_bias[axis] + 0.020 * rng_gaussian(&imu_rng);
|
||||
imu.gyro_rps[axis] = gyro_bias[axis] + 0.0006 * rng_gaussian(&imu_rng);
|
||||
}
|
||||
imu.temperature_c = 24.0;
|
||||
imu.sequence = (uint32_t)(tick / imu_period_ticks);
|
||||
CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted");
|
||||
|
||||
if((tick >= next_gps_tick) && !gps_pending)
|
||||
{
|
||||
synth_truth_t stationary_truth = {{0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0}, 0.0};
|
||||
pending_gps = make_position(tick, &stationary_truth, &gps_rng, false);
|
||||
pending_gps.position_enu_m[0] *= 0.8 / 1.2;
|
||||
pending_gps.position_enu_m[1] *= 0.8 / 1.2;
|
||||
pending_gps.position_enu_m[2] *= 1.5 / 2.0;
|
||||
pending_gps.covariance_enu_m2[0] = square(0.8);
|
||||
pending_gps.covariance_enu_m2[4] = square(0.8);
|
||||
pending_gps.covariance_enu_m2[8] = square(1.5);
|
||||
delivery_tick = tick + 250000u;
|
||||
gps_pending = true;
|
||||
next_gps_tick += 1000000u;
|
||||
}
|
||||
if(gps_pending && (tick >= delivery_tick))
|
||||
{
|
||||
eskf_update_result_t result = ESKF_UPDATE_INVALID;
|
||||
CHECK(fusion_engine_push_position(&engine, &pending_gps, &result, NULL),
|
||||
"500 Hz delayed stationary GPS processed");
|
||||
CHECK(result == ESKF_UPDATE_ACCEPTED,
|
||||
"500 Hz stationary GPS passes NIS");
|
||||
gps_pending = false;
|
||||
}
|
||||
if((tick >= 3000000u) && (engine.state == FUSION_ENGINE_RUNNING) &&
|
||||
(engine.current.motion.state != FUSION_MOTION_STATIONARY))
|
||||
{
|
||||
metrics.nonstationary_samples++;
|
||||
}
|
||||
}
|
||||
|
||||
metrics.accepted_gps = engine.diagnostics.position_accepted;
|
||||
metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
|
||||
metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
|
||||
metrics.covariance_positive_semidefinite = covariance_is_psd(
|
||||
&engine.current.filter, NULL);
|
||||
metrics.final_horizontal_error_m = norm2(engine.current.filter.position_m[0],
|
||||
engine.current.filter.position_m[1]);
|
||||
metrics.final_vertical_error_m = fabs(engine.current.filter.position_m[2]);
|
||||
metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps);
|
||||
return metrics;
|
||||
}
|
||||
|
||||
static void check_stationary_metrics(const stationary_metrics_t* metrics)
|
||||
{
|
||||
CHECK(metrics->state_finite, "30 s / 500 Hz stationary state remains finite");
|
||||
CHECK(metrics->covariance_positive_semidefinite,
|
||||
"30 s / 500 Hz stationary covariance remains PSD");
|
||||
CHECK(metrics->nonstationary_samples == 0u,
|
||||
"30 s / 500 Hz noise never creates a false moving state");
|
||||
CHECK(metrics->accepted_gps >= 26u,
|
||||
"30 s / 500 Hz delayed stationary GPS remains accepted");
|
||||
CHECK(metrics->zero_updates >= 500u,
|
||||
"30 s / 500 Hz stationary run keeps rate-limited ZUPT active");
|
||||
CHECK(metrics->final_horizontal_error_m < 1.0,
|
||||
"30 s / 500 Hz stationary horizontal error is bounded");
|
||||
CHECK(metrics->final_vertical_error_m < 1.0,
|
||||
"30 s / 500 Hz stationary vertical error is bounded");
|
||||
CHECK(metrics->final_speed_mps < 0.02,
|
||||
"30 s / 500 Hz stationary speed remains near zero");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
synth_metrics_t metrics = run_dynamic_scenario(0u);
|
||||
check_dynamic_metrics(&metrics);
|
||||
double worst_horizontal_rmse_m = metrics.horizontal_rmse_m;
|
||||
double worst_final_horizontal_error_m = metrics.final_horizontal_error_m;
|
||||
double latest_stationary_reentry_s = metrics.stationary_reentry_time_s;
|
||||
for(uint32_t scenario = 1u ; scenario < 8u ; scenario++)
|
||||
{
|
||||
synth_metrics_t trial = run_dynamic_scenario(scenario);
|
||||
check_dynamic_metrics(&trial);
|
||||
worst_horizontal_rmse_m = fmax(worst_horizontal_rmse_m,
|
||||
trial.horizontal_rmse_m);
|
||||
worst_final_horizontal_error_m = fmax(worst_final_horizontal_error_m,
|
||||
trial.final_horizontal_error_m);
|
||||
latest_stationary_reentry_s = fmax(latest_stationary_reentry_s,
|
||||
trial.stationary_reentry_time_s);
|
||||
}
|
||||
stationary_metrics_t stationary = run_stationary_500hz_scenario();
|
||||
check_stationary_metrics(&stationary);
|
||||
printf("Synthetic 60 s asynchronous ESKF scenario\n");
|
||||
printf(" position RMSE horizontal=%6.3f m vertical=%6.3f m\n",
|
||||
metrics.horizontal_rmse_m, metrics.vertical_rmse_m);
|
||||
printf(" final error horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
|
||||
metrics.final_horizontal_error_m, metrics.final_vertical_error_m,
|
||||
metrics.final_speed_mps);
|
||||
printf(" motion timing moving=%6.3f s stationary-again=%6.3f s false-still=%u\n",
|
||||
metrics.moving_detection_time_s, metrics.stationary_reentry_time_s,
|
||||
metrics.false_stationary_samples);
|
||||
printf(" GPS/replay accepted=%llu rejected=%llu replay=%llu replayed-imu=%llu\n",
|
||||
(unsigned long long)metrics.accepted_gps,
|
||||
(unsigned long long)metrics.rejected_gps,
|
||||
(unsigned long long)metrics.replay_count,
|
||||
(unsigned long long)metrics.replayed_imu_count);
|
||||
printf(" outlier isolation injected=%u rejected=%u max-state-jump=%.3e m\n",
|
||||
metrics.injected_outliers, metrics.rejected_injected_outliers,
|
||||
metrics.max_rejected_state_jump_m);
|
||||
printf(" covariance PSD=%d symmetry-error=%.3e zero-updates=%llu\n",
|
||||
metrics.covariance_positive_semidefinite ? 1 : 0,
|
||||
metrics.covariance_symmetry_error,
|
||||
(unsigned long long)metrics.zero_updates);
|
||||
printf(" 8-seed robustness worst-h-rmse=%6.3f m worst-final-h=%6.3f m latest-stop=%6.3f s\n",
|
||||
worst_horizontal_rmse_m, worst_final_horizontal_error_m,
|
||||
latest_stationary_reentry_s);
|
||||
printf("Synthetic 30 s stationary / 500 Hz scenario\n");
|
||||
printf(" final error horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
|
||||
stationary.final_horizontal_error_m,
|
||||
stationary.final_vertical_error_m,
|
||||
stationary.final_speed_mps);
|
||||
printf(" decisions false-moving-samples=%u GPS-accepted=%llu zero-updates=%llu\n",
|
||||
stationary.nonstationary_samples,
|
||||
(unsigned long long)stationary.accepted_gps,
|
||||
(unsigned long long)stationary.zero_updates);
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "%d synthetic fusion check(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("PASS: production bare-metal ESKF sources passed the Linux/GCC scenario.\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -7,6 +7,14 @@ BSP_UART_CLOCK_HZ ?= 50000000
|
||||
|
||||
APP_SRCS := \
|
||||
gps_test.c \
|
||||
src/tdk_icm42688.c \
|
||||
src/eskf.c \
|
||||
src/fusion_motion.c \
|
||||
src/fusion_engine.c \
|
||||
src/fusion_bus.c \
|
||||
src/fusion_gps_adapter.c \
|
||||
src/navigation_fusion.c \
|
||||
src/gps_fusion_publisher.c \
|
||||
src/gps_ip.c \
|
||||
src/gps_interrupt_handlers.c \
|
||||
src/gps_time_anchor.c \
|
||||
@@ -15,9 +23,16 @@ APP_SRCS := \
|
||||
src/NavEquations.c \
|
||||
src/matrixLib.c
|
||||
|
||||
BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c
|
||||
|
||||
TDK_IMU_ODR_HZ ?= 500
|
||||
GPS_NAV_SOLVE_TIMER_TICKS ?= 50
|
||||
|
||||
include ../common/app.mk
|
||||
|
||||
CFLAGS += -Iheader
|
||||
CFLAGS += -DTDK_IMU_ODR_HZ=$(TDK_IMU_ODR_HZ) \
|
||||
-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
|
||||
LDLIBS += -lm
|
||||
|
||||
ifeq ($(GPS_DEBUG),1)
|
||||
|
||||
@@ -5,21 +5,54 @@
|
||||
#include <float.h>
|
||||
#include "matrixLib.h"
|
||||
#include "gps_macros.h"
|
||||
#include "navigation_fusion.h"
|
||||
#include "tdk_icm42688.h"
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
gps_startup_initialization(15);
|
||||
tdk_icm42688_config_t imu_config;
|
||||
tdk_icm42688_default_config(&imu_config);
|
||||
bool imu_initialized = tdk_icm42688_init(&imu_config);
|
||||
bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ);
|
||||
|
||||
gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS);
|
||||
bool imu_running = imu_initialized && fusion_initialized && tdk_icm42688_start();
|
||||
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
|
||||
"init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n",
|
||||
imu_initialized ? 1u : 0u, imu_running ? 1u : 0u,
|
||||
imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ);
|
||||
|
||||
acq_start_loop();
|
||||
uint64_t last_fusion_status_tick = 0u;
|
||||
|
||||
while(1)
|
||||
{
|
||||
asm volatile("wfi");
|
||||
gps_background_task();
|
||||
calculate_position();
|
||||
// TODO: implement calculate position nav solve
|
||||
//calculate_position_nav_solve();
|
||||
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,85 @@
|
||||
#ifndef ESKF_H
|
||||
#define ESKF_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define ESKF_VECTOR_DIM 3u
|
||||
#define ESKF_QUATERNION_DIM 4u
|
||||
#define ESKF_ERROR_STATE_DIM 15u
|
||||
|
||||
typedef enum {
|
||||
ESKF_UPDATE_INVALID = 0,
|
||||
ESKF_UPDATE_ACCEPTED,
|
||||
ESKF_UPDATE_REJECTED_NIS
|
||||
} eskf_update_result_t;
|
||||
|
||||
typedef struct {
|
||||
double accel_noise_density[ESKF_VECTOR_DIM];
|
||||
double gyro_noise_density[ESKF_VECTOR_DIM];
|
||||
double accel_bias_random_walk[ESKF_VECTOR_DIM];
|
||||
double gyro_bias_random_walk[ESKF_VECTOR_DIM];
|
||||
double initial_position_std_m[ESKF_VECTOR_DIM];
|
||||
double initial_velocity_std_mps[ESKF_VECTOR_DIM];
|
||||
double initial_attitude_std_rad[ESKF_VECTOR_DIM];
|
||||
double initial_accel_bias_std_mps2[ESKF_VECTOR_DIM];
|
||||
double initial_gyro_bias_std_rps[ESKF_VECTOR_DIM];
|
||||
double gravity_mps2;
|
||||
double max_propagation_step_s;
|
||||
double position_nis_gate;
|
||||
double covariance_diagonal_floor;
|
||||
} eskf_config_t;
|
||||
|
||||
typedef struct {
|
||||
double position_m[ESKF_VECTOR_DIM];
|
||||
double velocity_mps[ESKF_VECTOR_DIM];
|
||||
double body_to_nav_quaternion[ESKF_QUATERNION_DIM];
|
||||
double accel_bias_mps2[ESKF_VECTOR_DIM];
|
||||
double gyro_bias_rps[ESKF_VECTOR_DIM];
|
||||
double covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM];
|
||||
double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM];
|
||||
double last_unbiased_gyro_rps[ESKF_VECTOR_DIM];
|
||||
double last_position_nis;
|
||||
uint64_t propagation_count;
|
||||
uint64_t position_update_accepted_count;
|
||||
uint64_t position_update_rejected_count;
|
||||
bool initialized;
|
||||
} eskf_t;
|
||||
|
||||
void eskf_default_config(eskf_config_t* config);
|
||||
|
||||
bool eskf_quaternion_align_vectors(const double source[ESKF_VECTOR_DIM],
|
||||
const double target[ESKF_VECTOR_DIM],
|
||||
double quaternion_out[ESKF_QUATERNION_DIM]);
|
||||
|
||||
bool eskf_initialize(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double initial_position_m[ESKF_VECTOR_DIM],
|
||||
const double body_to_nav_quaternion[ESKF_QUATERNION_DIM],
|
||||
const double accel_bias_mps2[ESKF_VECTOR_DIM],
|
||||
const double gyro_bias_rps[ESKF_VECTOR_DIM]);
|
||||
|
||||
bool eskf_propagate(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double accel_mps2[ESKF_VECTOR_DIM],
|
||||
const double gyro_rps[ESKF_VECTOR_DIM],
|
||||
double dt_s);
|
||||
|
||||
eskf_update_result_t eskf_position_update(
|
||||
eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double position_m[ESKF_VECTOR_DIM],
|
||||
const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
|
||||
double* nis_out);
|
||||
|
||||
bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);
|
||||
|
||||
bool eskf_zero_angular_rate_update(
|
||||
eskf_t* filter,
|
||||
const double measured_gyro_rps[ESKF_VECTOR_DIM],
|
||||
const double measurement_std_rps[ESKF_VECTOR_DIM]);
|
||||
|
||||
double eskf_speed_mps(const eskf_t* filter);
|
||||
bool eskf_state_is_finite(const eskf_t* filter);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,42 @@
|
||||
#ifndef FUSION_BUS_H
|
||||
#define FUSION_BUS_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
|
||||
#ifndef FUSION_IMU_QUEUE_CAPACITY
|
||||
#define FUSION_IMU_QUEUE_CAPACITY 256u
|
||||
#endif
|
||||
|
||||
#ifndef FUSION_GPS_QUEUE_CAPACITY
|
||||
#define FUSION_GPS_QUEUE_CAPACITY 8u
|
||||
#endif
|
||||
|
||||
typedef struct {
|
||||
fusion_imu_measurement_t imu_queue[FUSION_IMU_QUEUE_CAPACITY];
|
||||
fusion_gps_measurement_t gps_queue[FUSION_GPS_QUEUE_CAPACITY];
|
||||
volatile unsigned int imu_head;
|
||||
volatile unsigned int imu_tail;
|
||||
volatile unsigned int gps_head;
|
||||
volatile unsigned int gps_tail;
|
||||
volatile uint64_t imu_overflow_count;
|
||||
volatile uint64_t gps_overflow_count;
|
||||
} fusion_bus_t;
|
||||
|
||||
void fusion_bus_init(fusion_bus_t* bus);
|
||||
|
||||
/* Single producer (TDK ISR), single consumer (foreground). */
|
||||
bool fusion_bus_publish_imu(fusion_bus_t* bus,
|
||||
const fusion_imu_measurement_t* measurement);
|
||||
bool fusion_bus_take_imu(fusion_bus_t* bus,
|
||||
fusion_imu_measurement_t* measurement_out);
|
||||
|
||||
/* Single producer (GPS WLS foreground), single fusion consumer. */
|
||||
bool fusion_bus_publish_gps(fusion_bus_t* bus,
|
||||
const fusion_gps_measurement_t* measurement);
|
||||
bool fusion_bus_take_gps(fusion_bus_t* bus,
|
||||
fusion_gps_measurement_t* measurement_out);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,123 @@
|
||||
#ifndef FUSION_ENGINE_H
|
||||
#define FUSION_ENGINE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "eskf.h"
|
||||
#include "fusion_measurement.h"
|
||||
#include "fusion_motion.h"
|
||||
|
||||
#ifndef FUSION_IMU_HISTORY_CAPACITY
|
||||
#define FUSION_IMU_HISTORY_CAPACITY 2048u
|
||||
#endif
|
||||
|
||||
#ifndef FUSION_POSITION_PENDING_CAPACITY
|
||||
#define FUSION_POSITION_PENDING_CAPACITY 8u
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
FUSION_ENGINE_CALIBRATING = 0,
|
||||
FUSION_ENGINE_RUNNING,
|
||||
FUSION_ENGINE_FAULT
|
||||
} fusion_engine_state_t;
|
||||
|
||||
typedef struct {
|
||||
eskf_config_t eskf;
|
||||
fusion_motion_config_t motion;
|
||||
uint64_t timebase_hz;
|
||||
double calibration_duration_s;
|
||||
unsigned int calibration_min_samples;
|
||||
double calibration_accel_norm_tolerance_mps2;
|
||||
double calibration_gyro_limit_rps;
|
||||
double calibration_accel_std_limit_mps2;
|
||||
double calibration_gyro_std_limit_rps;
|
||||
double zero_update_period_s;
|
||||
double zero_velocity_std_mps;
|
||||
double zero_gyro_std_rps[3];
|
||||
double gnss_motion_speed_threshold_mps;
|
||||
double gnss_displacement_nis_gate;
|
||||
unsigned int gnss_moving_confirmations;
|
||||
unsigned int gnss_still_confirmations;
|
||||
} fusion_engine_config_t;
|
||||
|
||||
typedef struct {
|
||||
eskf_t filter;
|
||||
fusion_motion_detector_t motion;
|
||||
fusion_imu_measurement_t held_imu;
|
||||
fusion_gnss_motion_evidence_t gnss_evidence;
|
||||
double last_accepted_position_m[3];
|
||||
double last_accepted_position_covariance_m2[9];
|
||||
uint64_t filter_timestamp_ticks;
|
||||
uint64_t last_zero_update_ticks;
|
||||
uint64_t last_accepted_position_ticks;
|
||||
unsigned int gnss_moving_count;
|
||||
unsigned int gnss_still_count;
|
||||
bool have_held_imu;
|
||||
bool have_accepted_position;
|
||||
} fusion_runtime_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t imu_received;
|
||||
uint64_t imu_rejected;
|
||||
uint64_t imu_history_overflow;
|
||||
uint64_t checkpoint_advance_count;
|
||||
uint64_t calibration_reset_count;
|
||||
uint64_t position_received;
|
||||
uint64_t position_source_rejected;
|
||||
uint64_t position_nis_rejected;
|
||||
uint64_t position_stale_rejected;
|
||||
uint64_t position_pending_overflow;
|
||||
uint64_t position_accepted;
|
||||
uint64_t replay_count;
|
||||
uint64_t replayed_imu_count;
|
||||
uint64_t zero_velocity_update_count;
|
||||
} fusion_engine_diagnostics_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t start_ticks;
|
||||
uint64_t last_ticks;
|
||||
double accel_mean[3];
|
||||
double gyro_mean[3];
|
||||
double accel_m2[3];
|
||||
double gyro_m2[3];
|
||||
unsigned int sample_count;
|
||||
bool have_start;
|
||||
} fusion_calibration_accumulator_t;
|
||||
|
||||
typedef struct {
|
||||
fusion_engine_config_t config;
|
||||
fusion_engine_state_t state;
|
||||
fusion_runtime_t current;
|
||||
fusion_runtime_t checkpoint;
|
||||
fusion_calibration_accumulator_t calibration;
|
||||
fusion_imu_measurement_t imu_history[FUSION_IMU_HISTORY_CAPACITY];
|
||||
fusion_position_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY];
|
||||
fusion_position_measurement_t initial_position;
|
||||
fusion_engine_diagnostics_t diagnostics;
|
||||
unsigned int imu_history_head;
|
||||
unsigned int imu_history_count;
|
||||
unsigned int pending_position_count;
|
||||
bool checkpoint_valid;
|
||||
bool have_initial_position;
|
||||
} fusion_engine_t;
|
||||
|
||||
void fusion_engine_default_config(fusion_engine_config_t* config,
|
||||
uint64_t timebase_hz);
|
||||
bool fusion_engine_init(fusion_engine_t* engine,
|
||||
const fusion_engine_config_t* config);
|
||||
|
||||
bool fusion_engine_push_imu(fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* measurement);
|
||||
bool fusion_engine_push_position(
|
||||
fusion_engine_t* engine,
|
||||
const fusion_position_measurement_t* measurement,
|
||||
eskf_update_result_t* update_result_out,
|
||||
double* nis_out);
|
||||
|
||||
bool fusion_engine_get_runtime(const fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime_out);
|
||||
const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
|
||||
const fusion_engine_t* engine);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,37 @@
|
||||
#ifndef FUSION_GPS_ADAPTER_H
|
||||
#define FUSION_GPS_ADAPTER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
|
||||
typedef struct {
|
||||
double minimum_horizontal_std_m;
|
||||
double minimum_vertical_std_m;
|
||||
double maximum_position_std_m;
|
||||
} fusion_gps_adapter_config_t;
|
||||
|
||||
typedef struct {
|
||||
double origin_latitude_deg;
|
||||
double origin_longitude_deg;
|
||||
double origin_height_m;
|
||||
double origin_ecef_m[3];
|
||||
double ecef_to_enu[3][3];
|
||||
fusion_gps_adapter_config_t config;
|
||||
bool origin_valid;
|
||||
} fusion_gps_adapter_t;
|
||||
|
||||
void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config);
|
||||
bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
|
||||
const fusion_gps_adapter_config_t* config);
|
||||
bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
|
||||
const fusion_gps_measurement_t* gps,
|
||||
fusion_position_measurement_t* position_out);
|
||||
bool fusion_gps_adapter_local_to_geodetic(
|
||||
const fusion_gps_adapter_t* adapter,
|
||||
const double position_enu_m[3],
|
||||
double* latitude_deg_out,
|
||||
double* longitude_deg_out,
|
||||
double* height_m_out);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,43 @@
|
||||
#ifndef FUSION_MEASUREMENT_H
|
||||
#define FUSION_MEASUREMENT_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* Hardware- and solver-neutral messages shared at the GPS/ESKF boundary.
|
||||
* timestamp_ticks always belongs to the common local monotonic timebase; GPS
|
||||
* week/TOW is metadata and must not drive filter dt.
|
||||
*/
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
double latitude_deg;
|
||||
double longitude_deg;
|
||||
double height_m;
|
||||
double covariance_enu_m2[3][3];
|
||||
double hacc_m;
|
||||
double vacc_m;
|
||||
double residual_rms_m;
|
||||
double chi_square_ratio;
|
||||
unsigned int used_satellites;
|
||||
bool solution_valid;
|
||||
} fusion_gps_measurement_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
double accel_mps2[3];
|
||||
double gyro_rps[3];
|
||||
double temperature_c;
|
||||
uint32_t sequence;
|
||||
} fusion_imu_measurement_t;
|
||||
|
||||
/* Generic local-navigation measurement consumed by the ESKF coordinator. */
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
double position_enu_m[3];
|
||||
double covariance_enu_m2[9];
|
||||
double source_quality;
|
||||
bool source_valid;
|
||||
} fusion_position_measurement_t;
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,63 @@
|
||||
#ifndef FUSION_MOTION_H
|
||||
#define FUSION_MOTION_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
typedef enum {
|
||||
FUSION_MOTION_BOOTSTRAP = 0,
|
||||
FUSION_MOTION_STATIONARY,
|
||||
FUSION_MOTION_MOVING,
|
||||
FUSION_MOTION_STATIONARY_CANDIDATE
|
||||
} fusion_motion_state_t;
|
||||
|
||||
typedef enum {
|
||||
FUSION_GNSS_MOTION_UNKNOWN = 0,
|
||||
FUSION_GNSS_MOTION_STILL,
|
||||
FUSION_GNSS_MOTION_MOVING
|
||||
} fusion_gnss_motion_evidence_t;
|
||||
|
||||
typedef struct {
|
||||
double stationary_accel_limit_mps2;
|
||||
double stationary_gyro_limit_rps;
|
||||
double moving_accel_threshold_mps2;
|
||||
double moving_gyro_threshold_rps;
|
||||
double moving_jerk_threshold_mps3;
|
||||
double jerk_filter_time_constant_s;
|
||||
double stationary_speed_limit_mps;
|
||||
double moving_speed_threshold_mps;
|
||||
double stationary_dwell_s;
|
||||
unsigned int weak_motion_samples;
|
||||
} fusion_motion_config_t;
|
||||
|
||||
typedef struct {
|
||||
fusion_motion_state_t state;
|
||||
double quiet_time_s;
|
||||
double filtered_linear_accel_mps2[3];
|
||||
double previous_filtered_linear_accel_mps2[3];
|
||||
double last_linear_accel_norm_mps2;
|
||||
double last_gyro_norm_rps;
|
||||
double last_jerk_norm_mps3;
|
||||
double last_speed_mps;
|
||||
unsigned int weak_motion_count;
|
||||
uint64_t transition_count;
|
||||
bool have_previous_accel;
|
||||
} fusion_motion_detector_t;
|
||||
|
||||
void fusion_motion_default_config(fusion_motion_config_t* config);
|
||||
void fusion_motion_init(fusion_motion_detector_t* detector,
|
||||
bool bootstrap_stationary);
|
||||
|
||||
fusion_motion_state_t fusion_motion_update(
|
||||
fusion_motion_detector_t* detector,
|
||||
const fusion_motion_config_t* config,
|
||||
const double linear_accel_nav_mps2[3],
|
||||
const double unbiased_gyro_rps[3],
|
||||
double speed_mps,
|
||||
fusion_gnss_motion_evidence_t gnss_evidence,
|
||||
double dt_s);
|
||||
|
||||
bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector);
|
||||
const char* fusion_motion_state_name(fusion_motion_state_t state);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,12 @@
|
||||
#ifndef GPS_FUSION_PUBLISHER_H
|
||||
#define GPS_FUSION_PUBLISHER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "NavEquations.h"
|
||||
|
||||
bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
|
||||
uint64_t measurement_timestamp_ticks);
|
||||
|
||||
#endif
|
||||
@@ -46,7 +46,9 @@
|
||||
#define GPS_EVENT_QUEUE_SIZE 64u
|
||||
#define GPS_SYSTICK_HZ 20u
|
||||
#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
|
||||
#ifndef GPS_NAV_SOLVE_TIMER_TICKS
|
||||
#define GPS_NAV_SOLVE_TIMER_TICKS 50u
|
||||
#endif
|
||||
#define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
|
||||
/* Keep integrity statistics active, but do not let leave-one-out FDE
|
||||
* replace the full-WLS position until the real measurement-noise model
|
||||
@@ -62,7 +64,7 @@
|
||||
#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
|
||||
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
|
||||
|
||||
_Static_assert(GPS_NAV_SOLVE_PERIOD_MS == 1000u, "NAV solve period must be 1 second");
|
||||
_Static_assert(GPS_NAV_SOLVE_TIMER_TICKS > 0u, "NAV solve timer must be non-zero");
|
||||
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
|
||||
"GPS_NAV_ENABLE_FDE must be zero or one");
|
||||
|
||||
|
||||
@@ -30,6 +30,7 @@
|
||||
#define GPS_LOG_CAT_SCHED "SCHED"
|
||||
#define GPS_LOG_CAT_MEM "MEM"
|
||||
#define GPS_LOG_CAT_HWREG "HWREG"
|
||||
#define GPS_LOG_CAT_FUSION "FUSION"
|
||||
|
||||
// Set the current verbosity level
|
||||
#ifdef DEBUG
|
||||
|
||||
@@ -17,6 +17,7 @@ typedef struct gps_ephem_ll {
|
||||
volatile uint32_t gps_nav_solve_consumed_sequence;
|
||||
volatile uint32_t gps_last_nav_solve_abs_sample;
|
||||
volatile double gps_last_nav_solve_corrected_abs_sample;
|
||||
volatile uint64_t gps_last_nav_solve_local_ticks;
|
||||
volatile uint64_t gps_last_nav_solve_pps_counter;
|
||||
volatile uint32_t gps_last_pps_capture_sequence;
|
||||
volatile uint64_t gps_last_measurement_tick;
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
#ifndef NAVIGATION_FUSION_H
|
||||
#define NAVIGATION_FUSION_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_engine.h"
|
||||
#include "fusion_measurement.h"
|
||||
#include "fusion_motion.h"
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
double position_enu_m[3];
|
||||
double velocity_enu_mps[3];
|
||||
double latitude_deg;
|
||||
double longitude_deg;
|
||||
double height_m;
|
||||
double position_std_m[3];
|
||||
double speed_mps;
|
||||
double last_position_nis;
|
||||
fusion_motion_state_t motion_state;
|
||||
uint64_t gps_accepted;
|
||||
uint64_t gps_nis_rejected;
|
||||
bool valid;
|
||||
} navigation_fusion_output_t;
|
||||
|
||||
bool navigation_fusion_init(uint64_t local_timebase_hz);
|
||||
bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement);
|
||||
unsigned int navigation_fusion_service(void);
|
||||
bool navigation_fusion_get_output(navigation_fusion_output_t* output);
|
||||
const fusion_engine_t* navigation_fusion_engine(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,75 @@
|
||||
#ifndef TDK_ICM42688_H
|
||||
#define TDK_ICM42688_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "fusion_measurement.h"
|
||||
|
||||
#ifndef TDK_IMU_RAW_QUEUE_CAPACITY
|
||||
#define TDK_IMU_RAW_QUEUE_CAPACITY 256u
|
||||
#endif
|
||||
|
||||
#ifndef TDK_IMU_ODR_HZ
|
||||
#define TDK_IMU_ODR_HZ 500u
|
||||
#endif
|
||||
|
||||
typedef enum {
|
||||
TDK_ACCEL_FS_2G = 2,
|
||||
TDK_ACCEL_FS_4G = 4,
|
||||
TDK_ACCEL_FS_8G = 8,
|
||||
TDK_ACCEL_FS_16G = 16
|
||||
} tdk_accel_full_scale_t;
|
||||
|
||||
typedef enum {
|
||||
TDK_GYRO_FS_15D625 = 15625,
|
||||
TDK_GYRO_FS_31D25 = 31250,
|
||||
TDK_GYRO_FS_62D5 = 62500,
|
||||
TDK_GYRO_FS_125 = 125000,
|
||||
TDK_GYRO_FS_250 = 250000,
|
||||
TDK_GYRO_FS_500 = 500000,
|
||||
TDK_GYRO_FS_1000 = 1000000,
|
||||
TDK_GYRO_FS_2000 = 2000000
|
||||
} tdk_gyro_full_scale_mdps_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t odr_hz;
|
||||
tdk_accel_full_scale_t accel_full_scale_g;
|
||||
tdk_gyro_full_scale_mdps_t gyro_full_scale_mdps;
|
||||
uint8_t ui_filter_config;
|
||||
} tdk_icm42688_config_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t timestamp_ticks;
|
||||
int16_t temperature_raw;
|
||||
int16_t accel_raw[3];
|
||||
int16_t gyro_raw[3];
|
||||
uint32_t sequence;
|
||||
} tdk_icm42688_raw_sample_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t irq_count;
|
||||
uint64_t sample_count;
|
||||
uint64_t invalid_sample_count;
|
||||
uint64_t spurious_irq_count;
|
||||
uint64_t queue_overflow_count;
|
||||
uint64_t estimated_missed_sample_count;
|
||||
uint64_t last_timestamp_ticks;
|
||||
uint8_t who_am_i;
|
||||
bool initialized;
|
||||
bool running;
|
||||
} tdk_icm42688_diagnostics_t;
|
||||
|
||||
void tdk_icm42688_default_config(tdk_icm42688_config_t* config);
|
||||
bool tdk_icm42688_init(const tdk_icm42688_config_t* config);
|
||||
bool tdk_icm42688_start(void);
|
||||
void tdk_icm42688_stop(void);
|
||||
|
||||
/* Called only by the common PLIC ISR for BSP_PMOD_IRQ. */
|
||||
void tdk_icm42688_irq_handler(void);
|
||||
|
||||
/* Foreground conversion; no floating-point work is performed in the ISR. */
|
||||
bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out);
|
||||
void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,809 @@
|
||||
#include "eskf.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#define ESKF_POSITION_INDEX 0u
|
||||
#define ESKF_VELOCITY_INDEX 3u
|
||||
#define ESKF_ATTITUDE_INDEX 6u
|
||||
#define ESKF_ACCEL_BIAS_INDEX 9u
|
||||
#define ESKF_GYRO_BIAS_INDEX 12u
|
||||
|
||||
#define ESKF_PI 3.14159265358979323846264338327950288
|
||||
|
||||
static double square(double value)
|
||||
{
|
||||
return value * value;
|
||||
}
|
||||
|
||||
static double vector_norm3(const double value[3])
|
||||
{
|
||||
return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
|
||||
}
|
||||
|
||||
static bool vector_is_finite(const double* value, size_t count)
|
||||
{
|
||||
if(value == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(size_t i = 0 ; i < count ; i++)
|
||||
{
|
||||
if(!isfinite(value[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool quaternion_normalize(double quaternion[4])
|
||||
{
|
||||
double norm = sqrt(square(quaternion[0]) + square(quaternion[1]) +
|
||||
square(quaternion[2]) + square(quaternion[3]));
|
||||
if(!(norm > DBL_EPSILON) || !isfinite(norm))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(unsigned int i = 0 ; i < 4u ; i++)
|
||||
{
|
||||
quaternion[i] /= norm;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void quaternion_multiply(const double first[4], const double second[4],
|
||||
double output[4])
|
||||
{
|
||||
output[0] = first[0] * second[0] - first[1] * second[1] -
|
||||
first[2] * second[2] - first[3] * second[3];
|
||||
output[1] = first[0] * second[1] + first[1] * second[0] +
|
||||
first[2] * second[3] - first[3] * second[2];
|
||||
output[2] = first[0] * second[2] - first[1] * second[3] +
|
||||
first[2] * second[0] + first[3] * second[1];
|
||||
output[3] = first[0] * second[3] + first[1] * second[2] -
|
||||
first[2] * second[1] + first[3] * second[0];
|
||||
}
|
||||
|
||||
static void rotation_vector_quaternion(const double rotation_vector[3],
|
||||
double quaternion[4])
|
||||
{
|
||||
double angle = vector_norm3(rotation_vector);
|
||||
if(angle < 1.0e-10)
|
||||
{
|
||||
quaternion[0] = 1.0;
|
||||
quaternion[1] = 0.5 * rotation_vector[0];
|
||||
quaternion[2] = 0.5 * rotation_vector[1];
|
||||
quaternion[3] = 0.5 * rotation_vector[2];
|
||||
(void)quaternion_normalize(quaternion);
|
||||
return;
|
||||
}
|
||||
|
||||
double scale = sin(0.5 * angle) / angle;
|
||||
quaternion[0] = cos(0.5 * angle);
|
||||
quaternion[1] = scale * rotation_vector[0];
|
||||
quaternion[2] = scale * rotation_vector[1];
|
||||
quaternion[3] = scale * rotation_vector[2];
|
||||
}
|
||||
|
||||
static void quaternion_rotation_matrix(const double quaternion[4],
|
||||
double rotation[3][3])
|
||||
{
|
||||
const double w = quaternion[0];
|
||||
const double x = quaternion[1];
|
||||
const double y = quaternion[2];
|
||||
const double z = quaternion[3];
|
||||
|
||||
rotation[0][0] = 1.0 - 2.0 * (y * y + z * z);
|
||||
rotation[0][1] = 2.0 * (x * y - w * z);
|
||||
rotation[0][2] = 2.0 * (x * z + w * y);
|
||||
rotation[1][0] = 2.0 * (x * y + w * z);
|
||||
rotation[1][1] = 1.0 - 2.0 * (x * x + z * z);
|
||||
rotation[1][2] = 2.0 * (y * z - w * x);
|
||||
rotation[2][0] = 2.0 * (x * z - w * y);
|
||||
rotation[2][1] = 2.0 * (y * z + w * x);
|
||||
rotation[2][2] = 1.0 - 2.0 * (x * x + y * y);
|
||||
}
|
||||
|
||||
static void skew_matrix(const double vector[3], double skew[3][3])
|
||||
{
|
||||
skew[0][0] = 0.0;
|
||||
skew[0][1] = -vector[2];
|
||||
skew[0][2] = vector[1];
|
||||
skew[1][0] = vector[2];
|
||||
skew[1][1] = 0.0;
|
||||
skew[1][2] = -vector[0];
|
||||
skew[2][0] = -vector[1];
|
||||
skew[2][1] = vector[0];
|
||||
skew[2][2] = 0.0;
|
||||
}
|
||||
|
||||
static bool inverse_symmetric3(const double input[9], double inverse[9])
|
||||
{
|
||||
double lower[3][3] = {{0.0}};
|
||||
double scale = fmax(fabs(input[0]), fmax(fabs(input[4]), fabs(input[8])));
|
||||
if(!(scale > 0.0) || !isfinite(scale))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col <= row ; col++)
|
||||
{
|
||||
double value = 0.5 * (input[row * 3u + col] + input[col * 3u + row]);
|
||||
for(unsigned int k = 0 ; k < col ; k++)
|
||||
{
|
||||
value -= lower[row][k] * lower[col][k];
|
||||
}
|
||||
if(row == col)
|
||||
{
|
||||
if(value <= scale * DBL_EPSILON * 128.0 || !isfinite(value))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
lower[row][col] = sqrt(value);
|
||||
}
|
||||
else
|
||||
{
|
||||
lower[row][col] = value / lower[col][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for(unsigned int column = 0 ; column < 3u ; column++)
|
||||
{
|
||||
double forward[3] = {0.0, 0.0, 0.0};
|
||||
double solution[3] = {0.0, 0.0, 0.0};
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
double rhs = (row == column) ? 1.0 : 0.0;
|
||||
for(unsigned int k = 0 ; k < row ; k++)
|
||||
{
|
||||
rhs -= lower[row][k] * forward[k];
|
||||
}
|
||||
forward[row] = rhs / lower[row][row];
|
||||
}
|
||||
for(int row = 2 ; row >= 0 ; row--)
|
||||
{
|
||||
double rhs = forward[row];
|
||||
for(unsigned int k = (unsigned int)row + 1u ; k < 3u ; k++)
|
||||
{
|
||||
rhs -= lower[k][row] * solution[k];
|
||||
}
|
||||
solution[row] = rhs / lower[row][row];
|
||||
}
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
inverse[row * 3u + column] = solution[row];
|
||||
}
|
||||
}
|
||||
return vector_is_finite(inverse, 9u);
|
||||
}
|
||||
|
||||
static void covariance_symmetrize_and_floor(eskf_t* filter, double floor_value)
|
||||
{
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = row + 1u ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
double symmetric = 0.5 *
|
||||
(filter->covariance[row][col] + filter->covariance[col][row]);
|
||||
filter->covariance[row][col] = symmetric;
|
||||
filter->covariance[col][row] = symmetric;
|
||||
}
|
||||
if(filter->covariance[row][row] < floor_value)
|
||||
{
|
||||
filter->covariance[row][row] = floor_value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void eskf_default_config(eskf_config_t* config)
|
||||
{
|
||||
if(config == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
memset(config, 0, sizeof(*config));
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
config->accel_noise_density[i] = 0.03;
|
||||
config->gyro_noise_density[i] = 0.003;
|
||||
config->accel_bias_random_walk[i] = 0.002;
|
||||
config->gyro_bias_random_walk[i] = 0.002 * ESKF_PI / 180.0;
|
||||
config->initial_position_std_m[i] = (i < 2u) ? 10.0 : 20.0;
|
||||
config->initial_velocity_std_mps[i] = 1.0;
|
||||
config->initial_attitude_std_rad[i] =
|
||||
((i < 2u) ? 5.0 : 30.0) * ESKF_PI / 180.0;
|
||||
config->initial_accel_bias_std_mps2[i] = 0.1;
|
||||
config->initial_gyro_bias_std_rps[i] = 0.05 * ESKF_PI / 180.0;
|
||||
}
|
||||
config->gravity_mps2 = 9.80665;
|
||||
config->max_propagation_step_s = 0.02;
|
||||
config->position_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */
|
||||
config->covariance_diagonal_floor = 1.0e-12;
|
||||
}
|
||||
|
||||
bool eskf_quaternion_align_vectors(const double source[3],
|
||||
const double target[3],
|
||||
double quaternion_out[4])
|
||||
{
|
||||
if(!vector_is_finite(source, 3u) || !vector_is_finite(target, 3u) ||
|
||||
(quaternion_out == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double source_norm = vector_norm3(source);
|
||||
double target_norm = vector_norm3(target);
|
||||
if(!(source_norm > DBL_EPSILON) || !(target_norm > DBL_EPSILON))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double first[3];
|
||||
double second[3];
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
first[i] = source[i] / source_norm;
|
||||
second[i] = target[i] / target_norm;
|
||||
}
|
||||
double dot = first[0] * second[0] + first[1] * second[1] + first[2] * second[2];
|
||||
if(dot < -0.999999)
|
||||
{
|
||||
double axis[3] = {0.0, first[2], -first[1]};
|
||||
if(vector_norm3(axis) < 1.0e-6)
|
||||
{
|
||||
axis[0] = -first[2];
|
||||
axis[1] = 0.0;
|
||||
axis[2] = first[0];
|
||||
}
|
||||
double axis_norm = vector_norm3(axis);
|
||||
quaternion_out[0] = 0.0;
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
quaternion_out[i + 1u] = axis[i] / axis_norm;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
quaternion_out[0] = 1.0 + dot;
|
||||
quaternion_out[1] = first[1] * second[2] - first[2] * second[1];
|
||||
quaternion_out[2] = first[2] * second[0] - first[0] * second[2];
|
||||
quaternion_out[3] = first[0] * second[1] - first[1] * second[0];
|
||||
return quaternion_normalize(quaternion_out);
|
||||
}
|
||||
|
||||
bool eskf_initialize(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double initial_position_m[3],
|
||||
const double body_to_nav_quaternion[4],
|
||||
const double accel_bias_mps2[3],
|
||||
const double gyro_bias_rps[3])
|
||||
{
|
||||
if((filter == NULL) || (config == NULL) ||
|
||||
!vector_is_finite(initial_position_m, 3u) ||
|
||||
!vector_is_finite(body_to_nav_quaternion, 4u) ||
|
||||
!vector_is_finite(accel_bias_mps2, 3u) ||
|
||||
!vector_is_finite(gyro_bias_rps, 3u) ||
|
||||
!(config->gravity_mps2 > 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
memset(filter, 0, sizeof(*filter));
|
||||
memcpy(filter->position_m, initial_position_m, sizeof(filter->position_m));
|
||||
memcpy(filter->body_to_nav_quaternion, body_to_nav_quaternion,
|
||||
sizeof(filter->body_to_nav_quaternion));
|
||||
memcpy(filter->accel_bias_mps2, accel_bias_mps2,
|
||||
sizeof(filter->accel_bias_mps2));
|
||||
memcpy(filter->gyro_bias_rps, gyro_bias_rps, sizeof(filter->gyro_bias_rps));
|
||||
if(!quaternion_normalize(filter->body_to_nav_quaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
filter->covariance[ESKF_POSITION_INDEX + i][ESKF_POSITION_INDEX + i] =
|
||||
square(config->initial_position_std_m[i]);
|
||||
filter->covariance[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] =
|
||||
square(config->initial_velocity_std_mps[i]);
|
||||
filter->covariance[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] =
|
||||
square(config->initial_attitude_std_rad[i]);
|
||||
filter->covariance[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] =
|
||||
square(config->initial_accel_bias_std_mps2[i]);
|
||||
filter->covariance[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] =
|
||||
square(config->initial_gyro_bias_std_rps[i]);
|
||||
}
|
||||
covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
|
||||
filter->last_position_nis = NAN;
|
||||
filter->initialized = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool eskf_propagate_step(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double accel_mps2[3],
|
||||
const double gyro_rps[3],
|
||||
double dt_s)
|
||||
{
|
||||
double specific_force[3];
|
||||
double angular_rate[3];
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
specific_force[i] = accel_mps2[i] - filter->accel_bias_mps2[i];
|
||||
angular_rate[i] = gyro_rps[i] - filter->gyro_bias_rps[i];
|
||||
filter->last_unbiased_gyro_rps[i] = angular_rate[i];
|
||||
}
|
||||
|
||||
double rotation[3][3];
|
||||
quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
|
||||
double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
linear_accel[row] += rotation[row][col] * specific_force[col];
|
||||
}
|
||||
filter->last_linear_accel_nav_mps2[row] = linear_accel[row];
|
||||
filter->position_m[row] += filter->velocity_mps[row] * dt_s +
|
||||
0.5 * linear_accel[row] * dt_s * dt_s;
|
||||
filter->velocity_mps[row] += linear_accel[row] * dt_s;
|
||||
}
|
||||
|
||||
double rotation_vector[3] = {
|
||||
angular_rate[0] * dt_s,
|
||||
angular_rate[1] * dt_s,
|
||||
angular_rate[2] * dt_s
|
||||
};
|
||||
double delta_quaternion[4];
|
||||
double updated_quaternion[4];
|
||||
rotation_vector_quaternion(rotation_vector, delta_quaternion);
|
||||
quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
|
||||
updated_quaternion);
|
||||
memcpy(filter->body_to_nav_quaternion, updated_quaternion,
|
||||
sizeof(updated_quaternion));
|
||||
if(!quaternion_normalize(filter->body_to_nav_quaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
|
||||
system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0;
|
||||
}
|
||||
double force_skew[3][3];
|
||||
double rate_skew[3][3];
|
||||
skew_matrix(specific_force, force_skew);
|
||||
skew_matrix(angular_rate, rate_skew);
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
double rotated_skew = 0.0;
|
||||
for(unsigned int k = 0 ; k < 3u ; k++)
|
||||
{
|
||||
rotated_skew += rotation[row][k] * force_skew[k][col];
|
||||
}
|
||||
system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
|
||||
-rotated_skew;
|
||||
system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
|
||||
-rotation[row][col];
|
||||
system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
|
||||
-rate_skew[row][col];
|
||||
}
|
||||
}
|
||||
|
||||
double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
transition[row][col] = system[row][col] * dt_s;
|
||||
}
|
||||
transition[row][row] += 1.0;
|
||||
}
|
||||
|
||||
double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
intermediate[row][col] +=
|
||||
transition[row][k] * filter->covariance[k][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
propagated[row][col] += intermediate[row][k] * transition[col][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
double accel_noise = 0.0;
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
accel_noise += rotation[row][axis] *
|
||||
square(config->accel_noise_density[axis]) *
|
||||
rotation[col][axis];
|
||||
}
|
||||
propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] +=
|
||||
accel_noise * dt_s;
|
||||
}
|
||||
propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] +=
|
||||
square(config->gyro_noise_density[row]) * dt_s;
|
||||
propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] +=
|
||||
square(config->accel_bias_random_walk[row]) * dt_s;
|
||||
propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] +=
|
||||
square(config->gyro_bias_random_walk[row]) * dt_s;
|
||||
}
|
||||
memcpy(filter->covariance, propagated, sizeof(propagated));
|
||||
covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
|
||||
filter->propagation_count++;
|
||||
return eskf_state_is_finite(filter);
|
||||
}
|
||||
|
||||
bool eskf_propagate(eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double accel_mps2[3],
|
||||
const double gyro_rps[3],
|
||||
double dt_s)
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized || (config == NULL) ||
|
||||
!vector_is_finite(accel_mps2, 3u) || !vector_is_finite(gyro_rps, 3u) ||
|
||||
!isfinite(dt_s) || (dt_s < 0.0) ||
|
||||
!(config->max_propagation_step_s > 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(dt_s == 0.0)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
unsigned int steps = (unsigned int)ceil(dt_s / config->max_propagation_step_s);
|
||||
if((steps == 0u) || (steps > 10000u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double step_s = dt_s / (double)steps;
|
||||
for(unsigned int step = 0 ; step < steps ; step++)
|
||||
{
|
||||
if(!eskf_propagate_step(filter, config, accel_mps2, gyro_rps, step_s))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
|
||||
{
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
filter->position_m[i] += error[ESKF_POSITION_INDEX + i];
|
||||
filter->velocity_mps[i] += error[ESKF_VELOCITY_INDEX + i];
|
||||
filter->accel_bias_mps2[i] += error[ESKF_ACCEL_BIAS_INDEX + i];
|
||||
filter->gyro_bias_rps[i] += error[ESKF_GYRO_BIAS_INDEX + i];
|
||||
}
|
||||
double attitude_error[3] = {
|
||||
error[ESKF_ATTITUDE_INDEX],
|
||||
error[ESKF_ATTITUDE_INDEX + 1u],
|
||||
error[ESKF_ATTITUDE_INDEX + 2u]
|
||||
};
|
||||
double delta_quaternion[4];
|
||||
double updated_quaternion[4];
|
||||
rotation_vector_quaternion(attitude_error, delta_quaternion);
|
||||
quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
|
||||
updated_quaternion);
|
||||
memcpy(filter->body_to_nav_quaternion, updated_quaternion,
|
||||
sizeof(updated_quaternion));
|
||||
if(!quaternion_normalize(filter->body_to_nav_quaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/* Right attitude-error reset after q <- q (x) Exp(delta_theta). */
|
||||
double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
|
||||
{
|
||||
reset[i][i] = 1.0;
|
||||
}
|
||||
double attitude_skew[3][3];
|
||||
skew_matrix(attitude_error, attitude_skew);
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -=
|
||||
0.5 * attitude_skew[row][col];
|
||||
}
|
||||
}
|
||||
double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
|
||||
}
|
||||
}
|
||||
}
|
||||
memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance));
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool eskf_selected_update(eskf_t* filter,
|
||||
unsigned int selected_state_index,
|
||||
const double residual[3],
|
||||
const double measurement_covariance[9],
|
||||
bool gate_enabled,
|
||||
double gate,
|
||||
double* nis_out)
|
||||
{
|
||||
if((selected_state_index + 3u) > ESKF_ERROR_STATE_DIM ||
|
||||
!vector_is_finite(residual, 3u) ||
|
||||
!vector_is_finite(measurement_covariance, 9u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double covariance_inverse[9];
|
||||
if(!inverse_symmetric3(measurement_covariance, covariance_inverse))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double innovation[9];
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
innovation[row * 3u + col] =
|
||||
filter->covariance[selected_state_index + row][selected_state_index + col] +
|
||||
measurement_covariance[row * 3u + col];
|
||||
}
|
||||
}
|
||||
double innovation_inverse[9];
|
||||
if(!inverse_symmetric3(innovation, innovation_inverse))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double nis = 0.0;
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
nis += residual[row] * innovation_inverse[row * 3u + col] * residual[col];
|
||||
}
|
||||
}
|
||||
if(nis_out != NULL)
|
||||
{
|
||||
*nis_out = nis;
|
||||
}
|
||||
if(!isfinite(nis) || (gate_enabled && (nis > gate)))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double gain[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < 3u ; k++)
|
||||
{
|
||||
gain[row][col] +=
|
||||
filter->covariance[row][selected_state_index + k] *
|
||||
innovation_inverse[k * 3u + col];
|
||||
}
|
||||
}
|
||||
}
|
||||
double error[ESKF_ERROR_STATE_DIM] = {0.0};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
error[row] += gain[row][col] * residual[col];
|
||||
}
|
||||
}
|
||||
|
||||
double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
joseph_left[row][row] = 1.0;
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
joseph_left[row][selected_state_index + col] -= gain[row][col];
|
||||
}
|
||||
}
|
||||
double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
intermediate[row][col] +=
|
||||
joseph_left[row][k] * filter->covariance[k][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
|
||||
{
|
||||
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
|
||||
{
|
||||
updated_covariance[row][col] +=
|
||||
intermediate[row][k] * joseph_left[col][k];
|
||||
}
|
||||
for(unsigned int first = 0 ; first < 3u ; first++)
|
||||
{
|
||||
for(unsigned int second = 0 ; second < 3u ; second++)
|
||||
{
|
||||
updated_covariance[row][col] += gain[row][first] *
|
||||
measurement_covariance[first * 3u + second] * gain[col][second];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance));
|
||||
if(!eskf_inject_error(filter, error))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
covariance_symmetrize_and_floor(filter, 1.0e-12);
|
||||
return eskf_state_is_finite(filter);
|
||||
}
|
||||
|
||||
eskf_update_result_t eskf_position_update(
|
||||
eskf_t* filter,
|
||||
const eskf_config_t* config,
|
||||
const double position_m[3],
|
||||
const double covariance_m2[9],
|
||||
double* nis_out)
|
||||
{
|
||||
if(nis_out != NULL)
|
||||
{
|
||||
*nis_out = NAN;
|
||||
}
|
||||
if((filter == NULL) || !filter->initialized || (config == NULL) ||
|
||||
!vector_is_finite(position_m, 3u) || !vector_is_finite(covariance_m2, 9u))
|
||||
{
|
||||
return ESKF_UPDATE_INVALID;
|
||||
}
|
||||
double residual[3];
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
residual[i] = position_m[i] - filter->position_m[i];
|
||||
}
|
||||
double nis = NAN;
|
||||
if(!eskf_selected_update(filter, ESKF_POSITION_INDEX, residual, covariance_m2,
|
||||
true, config->position_nis_gate, &nis))
|
||||
{
|
||||
filter->last_position_nis = nis;
|
||||
if(isfinite(nis) && (nis > config->position_nis_gate))
|
||||
{
|
||||
filter->position_update_rejected_count++;
|
||||
if(nis_out != NULL)
|
||||
{
|
||||
*nis_out = nis;
|
||||
}
|
||||
return ESKF_UPDATE_REJECTED_NIS;
|
||||
}
|
||||
return ESKF_UPDATE_INVALID;
|
||||
}
|
||||
filter->last_position_nis = nis;
|
||||
filter->position_update_accepted_count++;
|
||||
if(nis_out != NULL)
|
||||
{
|
||||
*nis_out = nis;
|
||||
}
|
||||
return ESKF_UPDATE_ACCEPTED;
|
||||
}
|
||||
|
||||
bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized ||
|
||||
!(velocity_std_mps > 0.0) || !isfinite(velocity_std_mps))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double residual[3] = {
|
||||
-filter->velocity_mps[0],
|
||||
-filter->velocity_mps[1],
|
||||
-filter->velocity_mps[2]
|
||||
};
|
||||
double covariance[9] = {0.0};
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
covariance[i * 3u + i] = square(velocity_std_mps);
|
||||
}
|
||||
return eskf_selected_update(filter, ESKF_VELOCITY_INDEX, residual, covariance,
|
||||
false, 0.0, NULL);
|
||||
}
|
||||
|
||||
bool eskf_zero_angular_rate_update(
|
||||
eskf_t* filter,
|
||||
const double measured_gyro_rps[3],
|
||||
const double measurement_std_rps[3])
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized ||
|
||||
!vector_is_finite(measured_gyro_rps, 3u) ||
|
||||
!vector_is_finite(measurement_std_rps, 3u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double residual[3];
|
||||
double covariance[9] = {0.0};
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
if(!(measurement_std_rps[i] > 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
residual[i] = measured_gyro_rps[i] - filter->gyro_bias_rps[i];
|
||||
covariance[i * 3u + i] = square(measurement_std_rps[i]);
|
||||
}
|
||||
return eskf_selected_update(filter, ESKF_GYRO_BIAS_INDEX, residual, covariance,
|
||||
false, 0.0, NULL);
|
||||
}
|
||||
|
||||
double eskf_speed_mps(const eskf_t* filter)
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized)
|
||||
{
|
||||
return NAN;
|
||||
}
|
||||
return vector_norm3(filter->velocity_mps);
|
||||
}
|
||||
|
||||
bool eskf_state_is_finite(const eskf_t* filter)
|
||||
{
|
||||
if((filter == NULL) || !filter->initialized ||
|
||||
!vector_is_finite(filter->position_m, 3u) ||
|
||||
!vector_is_finite(filter->velocity_mps, 3u) ||
|
||||
!vector_is_finite(filter->body_to_nav_quaternion, 4u) ||
|
||||
!vector_is_finite(filter->accel_bias_mps2, 3u) ||
|
||||
!vector_is_finite(filter->gyro_bias_rps, 3u) ||
|
||||
!vector_is_finite(&filter->covariance[0][0],
|
||||
ESKF_ERROR_STATE_DIM * ESKF_ERROR_STATE_DIM))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
|
||||
{
|
||||
if(filter->covariance[i][i] < 0.0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
#include "fusion_bus.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#if (FUSION_IMU_QUEUE_CAPACITY < 2u)
|
||||
#error "FUSION_IMU_QUEUE_CAPACITY must be at least 2"
|
||||
#endif
|
||||
|
||||
#if (FUSION_GPS_QUEUE_CAPACITY < 2u)
|
||||
#error "FUSION_GPS_QUEUE_CAPACITY must be at least 2"
|
||||
#endif
|
||||
|
||||
static unsigned int next_index(unsigned int index, unsigned int capacity)
|
||||
{
|
||||
index++;
|
||||
return (index == capacity) ? 0u : index;
|
||||
}
|
||||
|
||||
void fusion_bus_init(fusion_bus_t* bus)
|
||||
{
|
||||
if(bus != NULL)
|
||||
{
|
||||
memset(bus, 0, sizeof(*bus));
|
||||
}
|
||||
}
|
||||
|
||||
bool fusion_bus_publish_imu(fusion_bus_t* bus,
|
||||
const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
if((bus == NULL) || (measurement == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
unsigned int head = __atomic_load_n(&bus->imu_head, __ATOMIC_RELAXED);
|
||||
unsigned int next = next_index(head, FUSION_IMU_QUEUE_CAPACITY);
|
||||
if(next == __atomic_load_n(&bus->imu_tail, __ATOMIC_ACQUIRE))
|
||||
{
|
||||
__atomic_fetch_add(&bus->imu_overflow_count, 1u, __ATOMIC_RELAXED);
|
||||
return false;
|
||||
}
|
||||
bus->imu_queue[head] = *measurement;
|
||||
__atomic_store_n(&bus->imu_head, next, __ATOMIC_RELEASE);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool fusion_bus_take_imu(fusion_bus_t* bus,
|
||||
fusion_imu_measurement_t* measurement_out)
|
||||
{
|
||||
if((bus == NULL) || (measurement_out == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
unsigned int tail = __atomic_load_n(&bus->imu_tail, __ATOMIC_RELAXED);
|
||||
if(tail == __atomic_load_n(&bus->imu_head, __ATOMIC_ACQUIRE))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*measurement_out = bus->imu_queue[tail];
|
||||
__atomic_store_n(&bus->imu_tail,
|
||||
next_index(tail, FUSION_IMU_QUEUE_CAPACITY),
|
||||
__ATOMIC_RELEASE);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool fusion_bus_publish_gps(fusion_bus_t* bus,
|
||||
const fusion_gps_measurement_t* measurement)
|
||||
{
|
||||
if((bus == NULL) || (measurement == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
unsigned int head = __atomic_load_n(&bus->gps_head, __ATOMIC_RELAXED);
|
||||
unsigned int next = next_index(head, FUSION_GPS_QUEUE_CAPACITY);
|
||||
if(next == __atomic_load_n(&bus->gps_tail, __ATOMIC_ACQUIRE))
|
||||
{
|
||||
__atomic_fetch_add(&bus->gps_overflow_count, 1u, __ATOMIC_RELAXED);
|
||||
return false;
|
||||
}
|
||||
bus->gps_queue[head] = *measurement;
|
||||
__atomic_store_n(&bus->gps_head, next, __ATOMIC_RELEASE);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool fusion_bus_take_gps(fusion_bus_t* bus,
|
||||
fusion_gps_measurement_t* measurement_out)
|
||||
{
|
||||
if((bus == NULL) || (measurement_out == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
unsigned int tail = __atomic_load_n(&bus->gps_tail, __ATOMIC_RELAXED);
|
||||
if(tail == __atomic_load_n(&bus->gps_head, __ATOMIC_ACQUIRE))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*measurement_out = bus->gps_queue[tail];
|
||||
__atomic_store_n(&bus->gps_tail,
|
||||
next_index(tail, FUSION_GPS_QUEUE_CAPACITY),
|
||||
__ATOMIC_RELEASE);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,730 @@
|
||||
#include "fusion_engine.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#define FUSION_PI 3.14159265358979323846264338327950288
|
||||
|
||||
static double square(double value)
|
||||
{
|
||||
return value * value;
|
||||
}
|
||||
|
||||
static double norm3(const double value[3])
|
||||
{
|
||||
return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
|
||||
}
|
||||
|
||||
static bool finite_values(const double* values, unsigned int count)
|
||||
{
|
||||
if(values == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(unsigned int i = 0 ; i < count ; i++)
|
||||
{
|
||||
if(!isfinite(values[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static double ticks_to_seconds(const fusion_engine_t* engine, uint64_t ticks)
|
||||
{
|
||||
return (double)ticks / (double)engine->config.timebase_hz;
|
||||
}
|
||||
|
||||
static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds)
|
||||
{
|
||||
double ticks = seconds * (double)engine->config.timebase_hz;
|
||||
if(ticks <= 1.0)
|
||||
{
|
||||
return 1u;
|
||||
}
|
||||
return (uint64_t)(ticks + 0.5);
|
||||
}
|
||||
|
||||
void fusion_engine_default_config(fusion_engine_config_t* config,
|
||||
uint64_t timebase_hz)
|
||||
{
|
||||
if(config == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
memset(config, 0, sizeof(*config));
|
||||
eskf_default_config(&config->eskf);
|
||||
fusion_motion_default_config(&config->motion);
|
||||
config->timebase_hz = timebase_hz;
|
||||
config->calibration_duration_s = 2.0;
|
||||
config->calibration_min_samples = 50u;
|
||||
config->calibration_accel_norm_tolerance_mps2 = 0.30;
|
||||
config->calibration_gyro_limit_rps = 2.0 * FUSION_PI / 180.0;
|
||||
config->calibration_accel_std_limit_mps2 = 0.12;
|
||||
config->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
|
||||
config->zero_update_period_s = 0.05;
|
||||
config->zero_velocity_std_mps = 0.03;
|
||||
for(unsigned int i = 0 ; i < 3u ; i++)
|
||||
{
|
||||
config->zero_gyro_std_rps[i] = 0.01 * FUSION_PI / 180.0;
|
||||
}
|
||||
config->gnss_motion_speed_threshold_mps = 0.50;
|
||||
config->gnss_displacement_nis_gate = 9.21034037197618; /* 2 DOF, 99%. */
|
||||
config->gnss_moving_confirmations = 2u;
|
||||
config->gnss_still_confirmations = 2u;
|
||||
}
|
||||
|
||||
static bool config_valid(const fusion_engine_config_t* config)
|
||||
{
|
||||
return (config != NULL) && (config->timebase_hz != 0u) &&
|
||||
isfinite(config->calibration_duration_s) &&
|
||||
(config->calibration_duration_s > 0.0) &&
|
||||
(config->calibration_min_samples >= 2u) &&
|
||||
isfinite(config->calibration_accel_norm_tolerance_mps2) &&
|
||||
(config->calibration_accel_norm_tolerance_mps2 > 0.0) &&
|
||||
isfinite(config->calibration_gyro_limit_rps) &&
|
||||
(config->calibration_gyro_limit_rps > 0.0) &&
|
||||
isfinite(config->zero_update_period_s) &&
|
||||
(config->zero_update_period_s > 0.0) &&
|
||||
isfinite(config->zero_velocity_std_mps) &&
|
||||
(config->zero_velocity_std_mps > 0.0) &&
|
||||
finite_values(config->zero_gyro_std_rps, 3u) &&
|
||||
(config->gnss_moving_confirmations != 0u) &&
|
||||
(config->gnss_still_confirmations != 0u);
|
||||
}
|
||||
|
||||
bool fusion_engine_init(fusion_engine_t* engine,
|
||||
const fusion_engine_config_t* config)
|
||||
{
|
||||
if((engine == NULL) || !config_valid(config))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(engine, 0, sizeof(*engine));
|
||||
engine->config = *config;
|
||||
engine->state = FUSION_ENGINE_CALIBRATING;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool imu_valid(const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
return (measurement != NULL) &&
|
||||
finite_values(measurement->accel_mps2, 3u) &&
|
||||
finite_values(measurement->gyro_rps, 3u) &&
|
||||
isfinite(measurement->temperature_c);
|
||||
}
|
||||
|
||||
static bool position_valid(const fusion_position_measurement_t* measurement)
|
||||
{
|
||||
return (measurement != NULL) &&
|
||||
finite_values(measurement->position_enu_m, 3u) &&
|
||||
finite_values(measurement->covariance_enu_m2, 9u) &&
|
||||
isfinite(measurement->source_quality);
|
||||
}
|
||||
|
||||
static void calibration_reset(fusion_engine_t* engine)
|
||||
{
|
||||
memset(&engine->calibration, 0, sizeof(engine->calibration));
|
||||
engine->diagnostics.calibration_reset_count++;
|
||||
}
|
||||
|
||||
static void calibration_add(fusion_calibration_accumulator_t* calibration,
|
||||
const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
calibration->sample_count++;
|
||||
double count = (double)calibration->sample_count;
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
double accel_delta = measurement->accel_mps2[axis] - calibration->accel_mean[axis];
|
||||
calibration->accel_mean[axis] += accel_delta / count;
|
||||
calibration->accel_m2[axis] +=
|
||||
accel_delta * (measurement->accel_mps2[axis] - calibration->accel_mean[axis]);
|
||||
double gyro_delta = measurement->gyro_rps[axis] - calibration->gyro_mean[axis];
|
||||
calibration->gyro_mean[axis] += gyro_delta / count;
|
||||
calibration->gyro_m2[axis] +=
|
||||
gyro_delta * (measurement->gyro_rps[axis] - calibration->gyro_mean[axis]);
|
||||
}
|
||||
}
|
||||
|
||||
static bool calibration_sample_is_quiet(const fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
double accel_error = fabs(norm3(measurement->accel_mps2) - engine->config.eskf.gravity_mps2);
|
||||
return (accel_error <= engine->config.calibration_accel_norm_tolerance_mps2) &&
|
||||
(norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps);
|
||||
}
|
||||
|
||||
static bool calibration_statistics_are_quiet(const fusion_engine_t* engine)
|
||||
{
|
||||
const fusion_calibration_accumulator_t* calibration = &engine->calibration;
|
||||
if(calibration->sample_count < 2u)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double denominator = (double)(calibration->sample_count - 1u);
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
double accel_std = sqrt(fmax(0.0, calibration->accel_m2[axis] / denominator));
|
||||
double gyro_std = sqrt(fmax(0.0, calibration->gyro_m2[axis] / denominator));
|
||||
if((accel_std > engine->config.calibration_accel_std_limit_mps2) ||
|
||||
(gyro_std > engine->config.calibration_gyro_std_limit_rps))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void history_clear(fusion_engine_t* engine)
|
||||
{
|
||||
engine->imu_history_head = 0u;
|
||||
engine->imu_history_count = 0u;
|
||||
}
|
||||
|
||||
static const fusion_imu_measurement_t* history_at(const fusion_engine_t* engine,
|
||||
unsigned int offset)
|
||||
{
|
||||
unsigned int index = (engine->imu_history_head + offset) % FUSION_IMU_HISTORY_CAPACITY;
|
||||
return &engine->imu_history[index];
|
||||
}
|
||||
|
||||
static bool runtime_apply_imu(fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime,
|
||||
const fusion_imu_measurement_t* measurement,
|
||||
bool count_zero_updates);
|
||||
|
||||
static void history_push(fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
if(engine->imu_history_count == FUSION_IMU_HISTORY_CAPACITY)
|
||||
{
|
||||
const fusion_imu_measurement_t oldest = *history_at(engine, 0u);
|
||||
if(engine->checkpoint_valid &&
|
||||
(oldest.timestamp_ticks > engine->checkpoint.filter_timestamp_ticks))
|
||||
{
|
||||
if(!runtime_apply_imu(engine, &engine->checkpoint, &oldest, false))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return;
|
||||
}
|
||||
engine->diagnostics.checkpoint_advance_count++;
|
||||
}
|
||||
engine->imu_history_head =
|
||||
(engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY;
|
||||
engine->imu_history_count--;
|
||||
engine->diagnostics.imu_history_overflow++;
|
||||
}
|
||||
unsigned int index = (engine->imu_history_head + engine->imu_history_count) %
|
||||
FUSION_IMU_HISTORY_CAPACITY;
|
||||
engine->imu_history[index] = *measurement;
|
||||
engine->imu_history_count++;
|
||||
}
|
||||
|
||||
static void history_drop_through(fusion_engine_t* engine, uint64_t timestamp_ticks)
|
||||
{
|
||||
while((engine->imu_history_count != 0u) &&
|
||||
(history_at(engine, 0u)->timestamp_ticks <= timestamp_ticks))
|
||||
{
|
||||
engine->imu_history_head =
|
||||
(engine->imu_history_head + 1u) % FUSION_IMU_HISTORY_CAPACITY;
|
||||
engine->imu_history_count--;
|
||||
}
|
||||
}
|
||||
|
||||
static bool runtime_propagate_to(fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime,
|
||||
uint64_t timestamp_ticks,
|
||||
const fusion_imu_measurement_t* input)
|
||||
{
|
||||
if((runtime == NULL) || (input == NULL) || !runtime->filter.initialized ||
|
||||
(timestamp_ticks < runtime->filter_timestamp_ticks))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(timestamp_ticks == runtime->filter_timestamp_ticks)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
double dt_s = ticks_to_seconds(engine,
|
||||
timestamp_ticks - runtime->filter_timestamp_ticks);
|
||||
if(!eskf_propagate(&runtime->filter, &engine->config.eskf,
|
||||
input->accel_mps2, input->gyro_rps, dt_s))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
runtime->filter_timestamp_ticks = timestamp_ticks;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool runtime_apply_imu(fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime,
|
||||
const fusion_imu_measurement_t* measurement,
|
||||
bool count_zero_updates)
|
||||
{
|
||||
if(!runtime->have_held_imu)
|
||||
{
|
||||
runtime->held_imu = *measurement;
|
||||
runtime->have_held_imu = true;
|
||||
runtime->filter_timestamp_ticks = measurement->timestamp_ticks;
|
||||
return true;
|
||||
}
|
||||
if((measurement->timestamp_ticks <= runtime->held_imu.timestamp_ticks) ||
|
||||
(measurement->timestamp_ticks < runtime->filter_timestamp_ticks))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double motion_dt_s = ticks_to_seconds(
|
||||
engine, measurement->timestamp_ticks - runtime->held_imu.timestamp_ticks);
|
||||
if(!runtime_propagate_to(engine, runtime, measurement->timestamp_ticks, measurement))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
(void)fusion_motion_update(
|
||||
&runtime->motion, &engine->config.motion,
|
||||
runtime->filter.last_linear_accel_nav_mps2,
|
||||
runtime->filter.last_unbiased_gyro_rps,
|
||||
eskf_speed_mps(&runtime->filter), runtime->gnss_evidence, motion_dt_s);
|
||||
bool inertial_motion =
|
||||
(runtime->motion.last_linear_accel_norm_mps2 >=
|
||||
engine->config.motion.moving_accel_threshold_mps2) ||
|
||||
(runtime->motion.last_gyro_norm_rps >=
|
||||
engine->config.motion.moving_gyro_threshold_rps) ||
|
||||
(runtime->motion.last_jerk_norm_mps3 >=
|
||||
engine->config.motion.moving_jerk_threshold_mps3);
|
||||
if(inertial_motion &&
|
||||
(runtime->gnss_evidence == FUSION_GNSS_MOTION_STILL))
|
||||
{
|
||||
/* A new IMU motion onset makes an older GNSS-still decision stale. */
|
||||
runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
|
||||
runtime->gnss_moving_count = 0u;
|
||||
runtime->gnss_still_count = 0u;
|
||||
}
|
||||
runtime->held_imu = *measurement;
|
||||
|
||||
uint64_t zero_period_ticks =
|
||||
seconds_to_ticks(engine, engine->config.zero_update_period_s);
|
||||
if(fusion_motion_allows_zero_update(&runtime->motion) &&
|
||||
((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >=
|
||||
zero_period_ticks))
|
||||
{
|
||||
if(!eskf_zero_velocity_update(&runtime->filter,
|
||||
engine->config.zero_velocity_std_mps) ||
|
||||
!eskf_zero_angular_rate_update(&runtime->filter,
|
||||
measurement->gyro_rps,
|
||||
engine->config.zero_gyro_std_rps))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
runtime->last_zero_update_ticks = measurement->timestamp_ticks;
|
||||
if(count_zero_updates)
|
||||
{
|
||||
engine->diagnostics.zero_velocity_update_count++;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool finish_calibration(fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* last_measurement)
|
||||
{
|
||||
if(!calibration_statistics_are_quiet(engine))
|
||||
{
|
||||
calibration_reset(engine);
|
||||
return false;
|
||||
}
|
||||
const fusion_calibration_accumulator_t* calibration = &engine->calibration;
|
||||
double gravity_norm = norm3(calibration->accel_mean);
|
||||
if(!(gravity_norm > DBL_EPSILON))
|
||||
{
|
||||
calibration_reset(engine);
|
||||
return false;
|
||||
}
|
||||
double nav_up[3] = {0.0, 0.0, 1.0};
|
||||
double quaternion[4];
|
||||
if(!eskf_quaternion_align_vectors(calibration->accel_mean, nav_up, quaternion))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
}
|
||||
double accel_bias[3];
|
||||
double gyro_bias[3];
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
double expected_gravity = calibration->accel_mean[axis] *
|
||||
engine->config.eskf.gravity_mps2 / gravity_norm;
|
||||
accel_bias[axis] = calibration->accel_mean[axis] - expected_gravity;
|
||||
gyro_bias[axis] = calibration->gyro_mean[axis];
|
||||
}
|
||||
double initial_position[3] = {0.0, 0.0, 0.0};
|
||||
if(engine->have_initial_position)
|
||||
{
|
||||
memcpy(initial_position, engine->initial_position.position_enu_m,
|
||||
sizeof(initial_position));
|
||||
}
|
||||
if(!eskf_initialize(&engine->current.filter, &engine->config.eskf,
|
||||
initial_position, quaternion, accel_bias, gyro_bias))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
}
|
||||
fusion_motion_init(&engine->current.motion, true);
|
||||
engine->current.held_imu = *last_measurement;
|
||||
engine->current.have_held_imu = true;
|
||||
engine->current.filter_timestamp_ticks = last_measurement->timestamp_ticks;
|
||||
engine->current.last_zero_update_ticks = last_measurement->timestamp_ticks;
|
||||
engine->current.gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
|
||||
if(engine->have_initial_position)
|
||||
{
|
||||
engine->current.have_accepted_position = true;
|
||||
engine->current.last_accepted_position_ticks =
|
||||
engine->initial_position.timestamp_ticks;
|
||||
memcpy(engine->current.last_accepted_position_m,
|
||||
engine->initial_position.position_enu_m,
|
||||
sizeof(engine->current.last_accepted_position_m));
|
||||
memcpy(engine->current.last_accepted_position_covariance_m2,
|
||||
engine->initial_position.covariance_enu_m2,
|
||||
sizeof(engine->current.last_accepted_position_covariance_m2));
|
||||
}
|
||||
engine->checkpoint = engine->current;
|
||||
engine->checkpoint_valid = true;
|
||||
history_clear(engine);
|
||||
engine->state = FUSION_ENGINE_RUNNING;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool process_calibration_imu(fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
if(engine->calibration.have_start &&
|
||||
(measurement->timestamp_ticks <= engine->calibration.last_ticks))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(!calibration_sample_is_quiet(engine, measurement))
|
||||
{
|
||||
calibration_reset(engine);
|
||||
return true;
|
||||
}
|
||||
if(!engine->calibration.have_start)
|
||||
{
|
||||
engine->calibration.start_ticks = measurement->timestamp_ticks;
|
||||
engine->calibration.have_start = true;
|
||||
}
|
||||
engine->calibration.last_ticks = measurement->timestamp_ticks;
|
||||
calibration_add(&engine->calibration, measurement);
|
||||
double duration_s = ticks_to_seconds(
|
||||
engine, engine->calibration.last_ticks - engine->calibration.start_ticks);
|
||||
if((engine->calibration.sample_count >= engine->config.calibration_min_samples) &&
|
||||
(duration_s >= engine->config.calibration_duration_s))
|
||||
{
|
||||
(void)finish_calibration(engine, measurement);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static fusion_gnss_motion_evidence_t position_motion_evidence(
|
||||
const fusion_engine_t* engine,
|
||||
const fusion_runtime_t* runtime,
|
||||
const fusion_position_measurement_t* measurement)
|
||||
{
|
||||
if(!runtime->have_accepted_position ||
|
||||
(measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
|
||||
{
|
||||
return FUSION_GNSS_MOTION_UNKNOWN;
|
||||
}
|
||||
double dt_s = ticks_to_seconds(
|
||||
engine, measurement->timestamp_ticks - runtime->last_accepted_position_ticks);
|
||||
if(!(dt_s > 0.0))
|
||||
{
|
||||
return FUSION_GNSS_MOTION_UNKNOWN;
|
||||
}
|
||||
double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0];
|
||||
double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1];
|
||||
double s00 = measurement->covariance_enu_m2[0] +
|
||||
runtime->last_accepted_position_covariance_m2[0];
|
||||
double s01 = measurement->covariance_enu_m2[1] +
|
||||
runtime->last_accepted_position_covariance_m2[1];
|
||||
double s10 = measurement->covariance_enu_m2[3] +
|
||||
runtime->last_accepted_position_covariance_m2[3];
|
||||
double s11 = measurement->covariance_enu_m2[4] +
|
||||
runtime->last_accepted_position_covariance_m2[4];
|
||||
double off_diagonal = 0.5 * (s01 + s10);
|
||||
double determinant = s00 * s11 - off_diagonal * off_diagonal;
|
||||
if(!(s00 > 0.0) || !(s11 > 0.0) || !(determinant > DBL_EPSILON) ||
|
||||
!isfinite(determinant))
|
||||
{
|
||||
return FUSION_GNSS_MOTION_UNKNOWN;
|
||||
}
|
||||
double displacement_nis =
|
||||
(delta_e * delta_e * s11 - 2.0 * delta_e * delta_n * off_diagonal +
|
||||
delta_n * delta_n * s00) / determinant;
|
||||
double speed_mps = sqrt(delta_e * delta_e + delta_n * delta_n) / dt_s;
|
||||
if((speed_mps >= engine->config.gnss_motion_speed_threshold_mps) &&
|
||||
(displacement_nis >= engine->config.gnss_displacement_nis_gate))
|
||||
{
|
||||
return FUSION_GNSS_MOTION_MOVING;
|
||||
}
|
||||
return FUSION_GNSS_MOTION_STILL;
|
||||
}
|
||||
|
||||
static eskf_update_result_t runtime_apply_position(
|
||||
fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime,
|
||||
const fusion_position_measurement_t* measurement,
|
||||
double* nis_out)
|
||||
{
|
||||
eskf_update_result_t result = eskf_position_update(
|
||||
&runtime->filter, &engine->config.eskf,
|
||||
measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
|
||||
if(result == ESKF_UPDATE_ACCEPTED)
|
||||
{
|
||||
fusion_gnss_motion_evidence_t raw_evidence =
|
||||
position_motion_evidence(engine, runtime, measurement);
|
||||
if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
|
||||
{
|
||||
runtime->gnss_still_count = 0u;
|
||||
if(runtime->gnss_moving_count < UINT32_MAX)
|
||||
{
|
||||
runtime->gnss_moving_count++;
|
||||
}
|
||||
if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations)
|
||||
{
|
||||
runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING;
|
||||
}
|
||||
}
|
||||
else if(raw_evidence == FUSION_GNSS_MOTION_STILL)
|
||||
{
|
||||
runtime->gnss_moving_count = 0u;
|
||||
if(runtime->gnss_still_count < UINT32_MAX)
|
||||
{
|
||||
runtime->gnss_still_count++;
|
||||
}
|
||||
if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations)
|
||||
{
|
||||
runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
|
||||
}
|
||||
}
|
||||
runtime->have_accepted_position = true;
|
||||
runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
|
||||
memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
|
||||
sizeof(runtime->last_accepted_position_m));
|
||||
memcpy(runtime->last_accepted_position_covariance_m2,
|
||||
measurement->covariance_enu_m2,
|
||||
sizeof(runtime->last_accepted_position_covariance_m2));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
static bool process_position_now(fusion_engine_t* engine,
|
||||
const fusion_position_measurement_t* measurement,
|
||||
eskf_update_result_t* result_out,
|
||||
double* nis_out)
|
||||
{
|
||||
if(!engine->checkpoint_valid ||
|
||||
(measurement->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks))
|
||||
{
|
||||
engine->diagnostics.position_stale_rejected++;
|
||||
return false;
|
||||
}
|
||||
fusion_runtime_t revised = engine->checkpoint;
|
||||
uint64_t replayed = 0u;
|
||||
for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
|
||||
{
|
||||
const fusion_imu_measurement_t* imu = history_at(engine, i);
|
||||
if(imu->timestamp_ticks > measurement->timestamp_ticks)
|
||||
{
|
||||
break;
|
||||
}
|
||||
if(!runtime_apply_imu(engine, &revised, imu, false))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
}
|
||||
replayed++;
|
||||
}
|
||||
if(revised.filter_timestamp_ticks < measurement->timestamp_ticks)
|
||||
{
|
||||
if(!revised.have_held_imu ||
|
||||
!runtime_propagate_to(engine, &revised,
|
||||
measurement->timestamp_ticks, &revised.held_imu))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
double nis = NAN;
|
||||
eskf_update_result_t result = runtime_apply_position(engine, &revised,
|
||||
measurement, &nis);
|
||||
if(result == ESKF_UPDATE_INVALID)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(result == ESKF_UPDATE_ACCEPTED)
|
||||
{
|
||||
engine->diagnostics.position_accepted++;
|
||||
}
|
||||
else
|
||||
{
|
||||
engine->diagnostics.position_nis_rejected++;
|
||||
}
|
||||
engine->checkpoint = revised;
|
||||
engine->checkpoint_valid = true;
|
||||
history_drop_through(engine, measurement->timestamp_ticks);
|
||||
for(unsigned int i = 0 ; i < engine->imu_history_count ; i++)
|
||||
{
|
||||
if(!runtime_apply_imu(engine, &revised, history_at(engine, i), false))
|
||||
{
|
||||
engine->state = FUSION_ENGINE_FAULT;
|
||||
return false;
|
||||
}
|
||||
replayed++;
|
||||
}
|
||||
engine->current = revised;
|
||||
engine->diagnostics.replay_count++;
|
||||
engine->diagnostics.replayed_imu_count += replayed;
|
||||
if(result_out != NULL)
|
||||
{
|
||||
*result_out = result;
|
||||
}
|
||||
if(nis_out != NULL)
|
||||
{
|
||||
*nis_out = nis;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool pending_insert(fusion_engine_t* engine,
|
||||
const fusion_position_measurement_t* measurement)
|
||||
{
|
||||
if(engine->pending_position_count >= FUSION_POSITION_PENDING_CAPACITY)
|
||||
{
|
||||
engine->diagnostics.position_pending_overflow++;
|
||||
return false;
|
||||
}
|
||||
unsigned int index = engine->pending_position_count;
|
||||
while((index != 0u) &&
|
||||
(engine->pending_positions[index - 1u].timestamp_ticks >
|
||||
measurement->timestamp_ticks))
|
||||
{
|
||||
engine->pending_positions[index] = engine->pending_positions[index - 1u];
|
||||
index--;
|
||||
}
|
||||
engine->pending_positions[index] = *measurement;
|
||||
engine->pending_position_count++;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void process_ready_pending_positions(fusion_engine_t* engine)
|
||||
{
|
||||
while((engine->pending_position_count != 0u) &&
|
||||
(engine->pending_positions[0].timestamp_ticks <=
|
||||
engine->current.filter_timestamp_ticks))
|
||||
{
|
||||
fusion_position_measurement_t measurement = engine->pending_positions[0];
|
||||
for(unsigned int i = 1u ; i < engine->pending_position_count ; i++)
|
||||
{
|
||||
engine->pending_positions[i - 1u] = engine->pending_positions[i];
|
||||
}
|
||||
engine->pending_position_count--;
|
||||
(void)process_position_now(engine, &measurement, NULL, NULL);
|
||||
if(engine->state == FUSION_ENGINE_FAULT)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool fusion_engine_push_imu(fusion_engine_t* engine,
|
||||
const fusion_imu_measurement_t* measurement)
|
||||
{
|
||||
if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
|
||||
!imu_valid(measurement))
|
||||
{
|
||||
if(engine != NULL)
|
||||
{
|
||||
engine->diagnostics.imu_rejected++;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
engine->diagnostics.imu_received++;
|
||||
if(engine->state == FUSION_ENGINE_CALIBRATING)
|
||||
{
|
||||
bool ok = process_calibration_imu(engine, measurement);
|
||||
if(!ok)
|
||||
{
|
||||
engine->diagnostics.imu_rejected++;
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
if(!runtime_apply_imu(engine, &engine->current, measurement, true))
|
||||
{
|
||||
engine->diagnostics.imu_rejected++;
|
||||
return false;
|
||||
}
|
||||
history_push(engine, measurement);
|
||||
if(engine->state == FUSION_ENGINE_FAULT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
process_ready_pending_positions(engine);
|
||||
return engine->state == FUSION_ENGINE_RUNNING;
|
||||
}
|
||||
|
||||
bool fusion_engine_push_position(
|
||||
fusion_engine_t* engine,
|
||||
const fusion_position_measurement_t* measurement,
|
||||
eskf_update_result_t* update_result_out,
|
||||
double* nis_out)
|
||||
{
|
||||
if(update_result_out != NULL)
|
||||
{
|
||||
*update_result_out = ESKF_UPDATE_INVALID;
|
||||
}
|
||||
if(nis_out != NULL)
|
||||
{
|
||||
*nis_out = NAN;
|
||||
}
|
||||
if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) ||
|
||||
!position_valid(measurement))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
engine->diagnostics.position_received++;
|
||||
if(!measurement->source_valid)
|
||||
{
|
||||
engine->diagnostics.position_source_rejected++;
|
||||
return false;
|
||||
}
|
||||
if(engine->state == FUSION_ENGINE_CALIBRATING)
|
||||
{
|
||||
engine->initial_position = *measurement;
|
||||
engine->have_initial_position = true;
|
||||
return true;
|
||||
}
|
||||
if(measurement->timestamp_ticks > engine->current.filter_timestamp_ticks)
|
||||
{
|
||||
return pending_insert(engine, measurement);
|
||||
}
|
||||
return process_position_now(engine, measurement, update_result_out, nis_out);
|
||||
}
|
||||
|
||||
bool fusion_engine_get_runtime(const fusion_engine_t* engine,
|
||||
fusion_runtime_t* runtime_out)
|
||||
{
|
||||
if((engine == NULL) || (runtime_out == NULL) ||
|
||||
(engine->state != FUSION_ENGINE_RUNNING))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*runtime_out = engine->current;
|
||||
return true;
|
||||
}
|
||||
|
||||
const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
|
||||
const fusion_engine_t* engine)
|
||||
{
|
||||
return (engine != NULL) ? &engine->diagnostics : NULL;
|
||||
}
|
||||
@@ -0,0 +1,254 @@
|
||||
#include "fusion_gps_adapter.h"
|
||||
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#define WGS84_SEMI_MAJOR_M 6378137.0
|
||||
#define WGS84_FLATTENING (1.0 / 298.257223563)
|
||||
#define FUSION_GPS_PI 3.14159265358979323846264338327950288
|
||||
|
||||
static double radians(double degrees)
|
||||
{
|
||||
return degrees * (FUSION_GPS_PI / 180.0);
|
||||
}
|
||||
|
||||
static double degrees(double radians_value)
|
||||
{
|
||||
return radians_value * (180.0 / FUSION_GPS_PI);
|
||||
}
|
||||
|
||||
static bool llh_valid(double latitude_deg, double longitude_deg, double height_m)
|
||||
{
|
||||
return isfinite(latitude_deg) && isfinite(longitude_deg) && isfinite(height_m) &&
|
||||
(latitude_deg >= -90.0) && (latitude_deg <= 90.0) &&
|
||||
(longitude_deg >= -180.0) && (longitude_deg <= 180.0) &&
|
||||
(height_m >= -10000.0) && (height_m <= 100000.0);
|
||||
}
|
||||
|
||||
static void geodetic_to_ecef(double latitude_deg, double longitude_deg,
|
||||
double height_m, double ecef_m[3])
|
||||
{
|
||||
double latitude = radians(latitude_deg);
|
||||
double longitude = radians(longitude_deg);
|
||||
double flattening = WGS84_FLATTENING;
|
||||
double eccentricity_squared = flattening * (2.0 - flattening);
|
||||
double sin_latitude = sin(latitude);
|
||||
double cos_latitude = cos(latitude);
|
||||
double prime_vertical = WGS84_SEMI_MAJOR_M /
|
||||
sqrt(1.0 - eccentricity_squared *
|
||||
sin_latitude * sin_latitude);
|
||||
ecef_m[0] = (prime_vertical + height_m) * cos_latitude * cos(longitude);
|
||||
ecef_m[1] = (prime_vertical + height_m) * cos_latitude * sin(longitude);
|
||||
ecef_m[2] = (prime_vertical * (1.0 - eccentricity_squared) + height_m) *
|
||||
sin_latitude;
|
||||
}
|
||||
|
||||
static void configure_origin(fusion_gps_adapter_t* adapter,
|
||||
const fusion_gps_measurement_t* gps)
|
||||
{
|
||||
adapter->origin_latitude_deg = gps->latitude_deg;
|
||||
adapter->origin_longitude_deg = gps->longitude_deg;
|
||||
adapter->origin_height_m = gps->height_m;
|
||||
geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m,
|
||||
adapter->origin_ecef_m);
|
||||
double latitude = radians(gps->latitude_deg);
|
||||
double longitude = radians(gps->longitude_deg);
|
||||
double sin_latitude = sin(latitude);
|
||||
double cos_latitude = cos(latitude);
|
||||
double sin_longitude = sin(longitude);
|
||||
double cos_longitude = cos(longitude);
|
||||
adapter->ecef_to_enu[0][0] = -sin_longitude;
|
||||
adapter->ecef_to_enu[0][1] = cos_longitude;
|
||||
adapter->ecef_to_enu[0][2] = 0.0;
|
||||
adapter->ecef_to_enu[1][0] = -sin_latitude * cos_longitude;
|
||||
adapter->ecef_to_enu[1][1] = -sin_latitude * sin_longitude;
|
||||
adapter->ecef_to_enu[1][2] = cos_latitude;
|
||||
adapter->ecef_to_enu[2][0] = cos_latitude * cos_longitude;
|
||||
adapter->ecef_to_enu[2][1] = cos_latitude * sin_longitude;
|
||||
adapter->ecef_to_enu[2][2] = sin_latitude;
|
||||
adapter->origin_valid = true;
|
||||
}
|
||||
|
||||
void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config)
|
||||
{
|
||||
if(config == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
config->minimum_horizontal_std_m = 0.50;
|
||||
config->minimum_vertical_std_m = 1.00;
|
||||
config->maximum_position_std_m = 1000.0;
|
||||
}
|
||||
|
||||
bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter,
|
||||
const fusion_gps_adapter_config_t* config)
|
||||
{
|
||||
if((adapter == NULL) || (config == NULL) ||
|
||||
!(config->minimum_horizontal_std_m > 0.0) ||
|
||||
!(config->minimum_vertical_std_m > 0.0) ||
|
||||
!(config->maximum_position_std_m >= config->minimum_vertical_std_m))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(adapter, 0, sizeof(*adapter));
|
||||
adapter->config = *config;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool sanitize_covariance(const fusion_gps_adapter_t* adapter,
|
||||
const double input[3][3], double output[9])
|
||||
{
|
||||
double variance_floor[3] = {
|
||||
adapter->config.minimum_horizontal_std_m *
|
||||
adapter->config.minimum_horizontal_std_m,
|
||||
adapter->config.minimum_horizontal_std_m *
|
||||
adapter->config.minimum_horizontal_std_m,
|
||||
adapter->config.minimum_vertical_std_m *
|
||||
adapter->config.minimum_vertical_std_m
|
||||
};
|
||||
double variance_ceiling = adapter->config.maximum_position_std_m *
|
||||
adapter->config.maximum_position_std_m;
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
double variance = input[axis][axis];
|
||||
if(!isfinite(variance) || (variance <= 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
output[axis * 3u + axis] = fmin(fmax(variance, variance_floor[axis]),
|
||||
variance_ceiling);
|
||||
}
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = row + 1u ; col < 3u ; col++)
|
||||
{
|
||||
double covariance = 0.5 * (input[row][col] + input[col][row]);
|
||||
if(!isfinite(covariance))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double limit = 0.95 * sqrt(output[row * 3u + row] *
|
||||
output[col * 3u + col]);
|
||||
covariance = fmin(fmax(covariance, -limit), limit);
|
||||
output[row * 3u + col] = covariance;
|
||||
output[col * 3u + row] = covariance;
|
||||
}
|
||||
}
|
||||
/* Sylvester criterion: the sanitized symmetric covariance must be PD. */
|
||||
double minor2 = output[0] * output[4] - output[1] * output[3];
|
||||
double determinant =
|
||||
output[0] * (output[4] * output[8] - output[5] * output[7]) -
|
||||
output[1] * (output[3] * output[8] - output[5] * output[6]) +
|
||||
output[2] * (output[3] * output[7] - output[4] * output[6]);
|
||||
return (output[0] > 0.0) && (minor2 > DBL_EPSILON) &&
|
||||
(determinant > DBL_EPSILON) && isfinite(determinant);
|
||||
}
|
||||
|
||||
bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter,
|
||||
const fusion_gps_measurement_t* gps,
|
||||
fusion_position_measurement_t* position_out)
|
||||
{
|
||||
if((adapter == NULL) || (gps == NULL) || (position_out == NULL) ||
|
||||
!llh_valid(gps->latitude_deg, gps->longitude_deg, gps->height_m))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(position_out, 0, sizeof(*position_out));
|
||||
position_out->timestamp_ticks = gps->timestamp_ticks;
|
||||
position_out->source_valid = gps->solution_valid;
|
||||
position_out->source_quality =
|
||||
(isfinite(gps->chi_square_ratio) && (gps->chi_square_ratio >= 0.0))
|
||||
? 100.0 / (1.0 + gps->chi_square_ratio) : 0.0;
|
||||
if(!sanitize_covariance(adapter, gps->covariance_enu_m2,
|
||||
position_out->covariance_enu_m2))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(!adapter->origin_valid)
|
||||
{
|
||||
if(!gps->solution_valid)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
configure_origin(adapter, gps);
|
||||
}
|
||||
double ecef_m[3];
|
||||
double delta_ecef_m[3];
|
||||
geodetic_to_ecef(gps->latitude_deg, gps->longitude_deg, gps->height_m, ecef_m);
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
delta_ecef_m[axis] = ecef_m[axis] - adapter->origin_ecef_m[axis];
|
||||
}
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
for(unsigned int col = 0 ; col < 3u ; col++)
|
||||
{
|
||||
position_out->position_enu_m[row] +=
|
||||
adapter->ecef_to_enu[row][col] * delta_ecef_m[col];
|
||||
}
|
||||
}
|
||||
return isfinite(position_out->position_enu_m[0]) &&
|
||||
isfinite(position_out->position_enu_m[1]) &&
|
||||
isfinite(position_out->position_enu_m[2]);
|
||||
}
|
||||
|
||||
bool fusion_gps_adapter_local_to_geodetic(
|
||||
const fusion_gps_adapter_t* adapter,
|
||||
const double position_enu_m[3],
|
||||
double* latitude_deg_out,
|
||||
double* longitude_deg_out,
|
||||
double* height_m_out)
|
||||
{
|
||||
if((adapter == NULL) || !adapter->origin_valid || (position_enu_m == NULL) ||
|
||||
(latitude_deg_out == NULL) || (longitude_deg_out == NULL) ||
|
||||
(height_m_out == NULL))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double ecef_m[3];
|
||||
for(unsigned int row = 0 ; row < 3u ; row++)
|
||||
{
|
||||
ecef_m[row] = adapter->origin_ecef_m[row];
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
/* R^-1 = R^T for the orthonormal ECEF-to-ENU transform. */
|
||||
ecef_m[row] += adapter->ecef_to_enu[axis][row] * position_enu_m[axis];
|
||||
}
|
||||
}
|
||||
double flattening = WGS84_FLATTENING;
|
||||
double eccentricity_squared = flattening * (2.0 - flattening);
|
||||
double longitude = atan2(ecef_m[1], ecef_m[0]);
|
||||
double horizontal = hypot(ecef_m[0], ecef_m[1]);
|
||||
double latitude = atan2(ecef_m[2], horizontal * (1.0 - eccentricity_squared));
|
||||
double height = 0.0;
|
||||
for(unsigned int iteration = 0 ; iteration < 12u ; iteration++)
|
||||
{
|
||||
double sin_latitude = sin(latitude);
|
||||
double prime_vertical = WGS84_SEMI_MAJOR_M /
|
||||
sqrt(1.0 - eccentricity_squared * sin_latitude * sin_latitude);
|
||||
double cos_latitude = cos(latitude);
|
||||
if(fabs(cos_latitude) < 1.0e-12)
|
||||
{
|
||||
height = fabs(ecef_m[2]) - prime_vertical * (1.0 - eccentricity_squared);
|
||||
}
|
||||
else
|
||||
{
|
||||
height = horizontal / cos_latitude - prime_vertical;
|
||||
}
|
||||
double next = atan2(ecef_m[2],
|
||||
horizontal * (1.0 - eccentricity_squared *
|
||||
prime_vertical / (prime_vertical + height)));
|
||||
if(fabs(next - latitude) < 1.0e-13)
|
||||
{
|
||||
latitude = next;
|
||||
break;
|
||||
}
|
||||
latitude = next;
|
||||
}
|
||||
*latitude_deg_out = degrees(latitude);
|
||||
*longitude_deg_out = degrees(longitude);
|
||||
*height_m_out = height;
|
||||
return llh_valid(*latitude_deg_out, *longitude_deg_out, *height_m_out);
|
||||
}
|
||||
@@ -0,0 +1,217 @@
|
||||
#include "fusion_motion.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#define FUSION_MOTION_PI 3.14159265358979323846264338327950288
|
||||
|
||||
static double norm3(const double vector[3])
|
||||
{
|
||||
return sqrt(vector[0] * vector[0] + vector[1] * vector[1] +
|
||||
vector[2] * vector[2]);
|
||||
}
|
||||
|
||||
static bool finite3(const double vector[3])
|
||||
{
|
||||
return (vector != NULL) && isfinite(vector[0]) &&
|
||||
isfinite(vector[1]) && isfinite(vector[2]);
|
||||
}
|
||||
|
||||
static void transition_to(fusion_motion_detector_t* detector,
|
||||
fusion_motion_state_t state)
|
||||
{
|
||||
if(detector->state != state)
|
||||
{
|
||||
detector->state = state;
|
||||
detector->transition_count++;
|
||||
}
|
||||
if(state != FUSION_MOTION_STATIONARY_CANDIDATE)
|
||||
{
|
||||
detector->quiet_time_s = 0.0;
|
||||
}
|
||||
if(state != FUSION_MOTION_STATIONARY)
|
||||
{
|
||||
detector->weak_motion_count = 0u;
|
||||
}
|
||||
}
|
||||
|
||||
void fusion_motion_default_config(fusion_motion_config_t* config)
|
||||
{
|
||||
if(config == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
config->stationary_accel_limit_mps2 = 0.15;
|
||||
config->stationary_gyro_limit_rps = 1.0 * FUSION_MOTION_PI / 180.0;
|
||||
config->moving_accel_threshold_mps2 = 0.40;
|
||||
config->moving_gyro_threshold_rps = 3.0 * FUSION_MOTION_PI / 180.0;
|
||||
config->moving_jerk_threshold_mps3 = 4.0;
|
||||
config->jerk_filter_time_constant_s = 0.10;
|
||||
config->stationary_speed_limit_mps = 0.40;
|
||||
config->moving_speed_threshold_mps = 0.50;
|
||||
config->stationary_dwell_s = 1.0;
|
||||
config->weak_motion_samples = 3u;
|
||||
}
|
||||
|
||||
void fusion_motion_init(fusion_motion_detector_t* detector,
|
||||
bool bootstrap_stationary)
|
||||
{
|
||||
if(detector == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
memset(detector, 0, sizeof(*detector));
|
||||
detector->state = bootstrap_stationary
|
||||
? FUSION_MOTION_STATIONARY
|
||||
: FUSION_MOTION_BOOTSTRAP;
|
||||
}
|
||||
|
||||
fusion_motion_state_t fusion_motion_update(
|
||||
fusion_motion_detector_t* detector,
|
||||
const fusion_motion_config_t* config,
|
||||
const double linear_accel_nav_mps2[3],
|
||||
const double unbiased_gyro_rps[3],
|
||||
double speed_mps,
|
||||
fusion_gnss_motion_evidence_t gnss_evidence,
|
||||
double dt_s)
|
||||
{
|
||||
if((detector == NULL) || (config == NULL) ||
|
||||
!finite3(linear_accel_nav_mps2) || !finite3(unbiased_gyro_rps) ||
|
||||
!isfinite(speed_mps) || (speed_mps < 0.0) ||
|
||||
!isfinite(dt_s) || (dt_s <= 0.0))
|
||||
{
|
||||
return (detector != NULL) ? detector->state : FUSION_MOTION_BOOTSTRAP;
|
||||
}
|
||||
|
||||
double accel_norm = norm3(linear_accel_nav_mps2);
|
||||
double gyro_norm = norm3(unbiased_gyro_rps);
|
||||
double jerk_norm = 0.0;
|
||||
if(!detector->have_previous_accel)
|
||||
{
|
||||
memcpy(detector->filtered_linear_accel_mps2, linear_accel_nav_mps2,
|
||||
sizeof(detector->filtered_linear_accel_mps2));
|
||||
memcpy(detector->previous_filtered_linear_accel_mps2,
|
||||
detector->filtered_linear_accel_mps2,
|
||||
sizeof(detector->previous_filtered_linear_accel_mps2));
|
||||
detector->have_previous_accel = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
double time_constant_s = fmax(0.0, config->jerk_filter_time_constant_s);
|
||||
double alpha = (time_constant_s > 0.0)
|
||||
? dt_s / (time_constant_s + dt_s)
|
||||
: 1.0;
|
||||
double delta[3];
|
||||
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
||||
{
|
||||
detector->previous_filtered_linear_accel_mps2[axis] =
|
||||
detector->filtered_linear_accel_mps2[axis];
|
||||
detector->filtered_linear_accel_mps2[axis] += alpha *
|
||||
(linear_accel_nav_mps2[axis] -
|
||||
detector->filtered_linear_accel_mps2[axis]);
|
||||
delta[axis] = detector->filtered_linear_accel_mps2[axis] -
|
||||
detector->previous_filtered_linear_accel_mps2[axis];
|
||||
}
|
||||
jerk_norm = norm3(delta) / dt_s;
|
||||
}
|
||||
detector->last_linear_accel_norm_mps2 = accel_norm;
|
||||
detector->last_gyro_norm_rps = gyro_norm;
|
||||
detector->last_jerk_norm_mps3 = jerk_norm;
|
||||
detector->last_speed_mps = speed_mps;
|
||||
|
||||
bool gnss_moving = gnss_evidence == FUSION_GNSS_MOTION_MOVING;
|
||||
bool strong_motion = gnss_moving ||
|
||||
(accel_norm >= config->moving_accel_threshold_mps2) ||
|
||||
(gyro_norm >= config->moving_gyro_threshold_rps) ||
|
||||
(detector->have_previous_accel &&
|
||||
(jerk_norm >= config->moving_jerk_threshold_mps3)) ||
|
||||
(speed_mps >= config->moving_speed_threshold_mps);
|
||||
bool quiet = (accel_norm <= config->stationary_accel_limit_mps2) &&
|
||||
(gyro_norm <= config->stationary_gyro_limit_rps);
|
||||
bool stationary_candidate = quiet && !gnss_moving &&
|
||||
(speed_mps <= config->stationary_speed_limit_mps);
|
||||
bool weak_motion = !quiet;
|
||||
|
||||
switch(detector->state)
|
||||
{
|
||||
case FUSION_MOTION_STATIONARY:
|
||||
if(strong_motion)
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_MOVING);
|
||||
}
|
||||
else if(weak_motion)
|
||||
{
|
||||
detector->weak_motion_count++;
|
||||
if(detector->weak_motion_count >= config->weak_motion_samples)
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_MOVING);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
detector->weak_motion_count = 0u;
|
||||
}
|
||||
break;
|
||||
|
||||
case FUSION_MOTION_BOOTSTRAP:
|
||||
case FUSION_MOTION_MOVING:
|
||||
if(strong_motion)
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_MOVING);
|
||||
}
|
||||
else if(stationary_candidate)
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_STATIONARY_CANDIDATE);
|
||||
detector->quiet_time_s = dt_s;
|
||||
}
|
||||
else
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_MOVING);
|
||||
}
|
||||
break;
|
||||
|
||||
case FUSION_MOTION_STATIONARY_CANDIDATE:
|
||||
if(strong_motion || !stationary_candidate)
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_MOVING);
|
||||
}
|
||||
else
|
||||
{
|
||||
detector->quiet_time_s += dt_s;
|
||||
if(detector->quiet_time_s >= config->stationary_dwell_s)
|
||||
{
|
||||
transition_to(detector, FUSION_MOTION_STATIONARY);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
transition_to(detector, FUSION_MOTION_BOOTSTRAP);
|
||||
break;
|
||||
}
|
||||
return detector->state;
|
||||
}
|
||||
|
||||
bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector)
|
||||
{
|
||||
return (detector != NULL) &&
|
||||
(detector->state == FUSION_MOTION_STATIONARY);
|
||||
}
|
||||
|
||||
const char* fusion_motion_state_name(fusion_motion_state_t state)
|
||||
{
|
||||
switch(state)
|
||||
{
|
||||
case FUSION_MOTION_BOOTSTRAP:
|
||||
return "bootstrap";
|
||||
case FUSION_MOTION_STATIONARY:
|
||||
return "stationary";
|
||||
case FUSION_MOTION_MOVING:
|
||||
return "moving";
|
||||
case FUSION_MOTION_STATIONARY_CANDIDATE:
|
||||
return "stationary_candidate";
|
||||
default:
|
||||
return "unknown";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "gps_fusion_publisher.h"
|
||||
|
||||
#include "navigation_fusion.h"
|
||||
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
bool gps_fusion_publish_solution(const gps_nav_solution_t* solution,
|
||||
uint64_t measurement_timestamp_ticks)
|
||||
{
|
||||
if((solution == NULL) || (measurement_timestamp_ticks == 0u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_gps_measurement_t measurement;
|
||||
memset(&measurement, 0, sizeof(measurement));
|
||||
measurement.timestamp_ticks = measurement_timestamp_ticks;
|
||||
measurement.latitude_deg = solution->latitude_deg;
|
||||
measurement.longitude_deg = solution->longitude_deg;
|
||||
measurement.height_m = solution->height_m;
|
||||
memcpy(measurement.covariance_enu_m2, solution->covariance_enu_m2,
|
||||
sizeof(measurement.covariance_enu_m2));
|
||||
measurement.hacc_m = solution->hacc_m;
|
||||
measurement.vacc_m = solution->vacc_m;
|
||||
measurement.residual_rms_m = solution->residual_rms_m;
|
||||
measurement.chi_square_ratio = solution->chi_square_ratio;
|
||||
measurement.used_satellites = solution->used_satellite_count;
|
||||
measurement.solution_valid = solution->solution_valid;
|
||||
return navigation_fusion_publish_gps(&measurement);
|
||||
}
|
||||
@@ -3,6 +3,7 @@
|
||||
#include "Navigation.h"
|
||||
#include "gps_pps.h"
|
||||
#include "gps_time_anchor.h"
|
||||
#include "tdk_icm42688.h"
|
||||
#include <bsp/irq.h>
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
@@ -25,6 +26,7 @@ typedef struct {
|
||||
double corrected_abs_sample;
|
||||
uint32_t nav_solve_timer;
|
||||
uint32_t nav_solve_event_count;
|
||||
uint64_t local_timestamp_ticks;
|
||||
uint64_t pps_counter;
|
||||
uint32_t pps_capture_sequence;
|
||||
bool pps_capture_valid;
|
||||
@@ -393,6 +395,12 @@ void gps_interrupt_init(void)
|
||||
|
||||
void gps_dispatch_irq(unsigned int irq_id)
|
||||
{
|
||||
if(irq_id == BSP_PMOD_IRQ)
|
||||
{
|
||||
tdk_icm42688_irq_handler();
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned char acq_engine_id;
|
||||
if(gps_irq_to_acq_engine(irq_id, &acq_engine_id))
|
||||
{
|
||||
@@ -633,6 +641,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t local_timestamp_ticks = bsp_read64(BSP_CLINT_MTIME);
|
||||
uint32_t status_reg = trck_ip->STATUS_REG;
|
||||
if((status_reg & (1u << GPS_TRCK_INTR_PEND_POS)) == 0u)
|
||||
{
|
||||
@@ -653,6 +662,7 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
|
||||
event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase);
|
||||
event.nav_solve_timer = trck_ip->NAV_SOLVE_TIMER;
|
||||
event.nav_solve_event_count = (status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) ? 1u : 0u;
|
||||
event.local_timestamp_ticks = local_timestamp_ticks;
|
||||
if(event.nav_solve_event_count != 0u)
|
||||
{
|
||||
event.pps_capture_valid = gps_pps_read_capture(trck_ip_no,
|
||||
@@ -1032,6 +1042,7 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
|
||||
gps_nav_solve_period_pending = true;
|
||||
node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
|
||||
node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
|
||||
node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
|
||||
if(event->pps_capture_valid)
|
||||
{
|
||||
node->gps_last_nav_solve_pps_counter = event->pps_counter;
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
#include "linked_list.h"
|
||||
#include "gps_interrupt_handlers.h"
|
||||
#include "gps_pps.h"
|
||||
#include "gps_fusion_publisher.h"
|
||||
#include "defines.h"
|
||||
#include <bsp/irq.h>
|
||||
|
||||
@@ -766,6 +767,7 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS
|
||||
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
|
||||
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
|
||||
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
|
||||
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
|
||||
unsigned int satellite_count)
|
||||
{
|
||||
for(unsigned int i = 0 ; i < satellite_count ; i++)
|
||||
@@ -774,9 +776,34 @@ static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS
|
||||
snapshot_sequences[i] = (node != NULL) ? node->gps_nav_solve_sequence : 0u;
|
||||
snapshot_pps_counters[i] = (node != NULL) ? node->gps_last_nav_solve_pps_counter : 0u;
|
||||
snapshot_pps_sequences[i] = (node != NULL) ? node->gps_last_pps_capture_sequence : 0u;
|
||||
snapshot_local_timestamps[i] =
|
||||
(node != NULL) ? node->gps_last_nav_solve_local_ticks : 0u;
|
||||
}
|
||||
}
|
||||
|
||||
static uint64_t gps_nav_median_local_timestamp(
|
||||
const uint64_t timestamps[GPS_TRCK_IP_COUNT], unsigned int count)
|
||||
{
|
||||
uint64_t sorted[GPS_TRCK_IP_COUNT];
|
||||
unsigned int valid_count = 0u;
|
||||
for(unsigned int i = 0 ; (i < count) && (i < GPS_TRCK_IP_COUNT) ; i++)
|
||||
{
|
||||
if(timestamps[i] == 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
unsigned int insert = valid_count;
|
||||
while((insert != 0u) && (sorted[insert - 1u] > timestamps[i]))
|
||||
{
|
||||
sorted[insert] = sorted[insert - 1u];
|
||||
insert--;
|
||||
}
|
||||
sorted[insert] = timestamps[i];
|
||||
valid_count++;
|
||||
}
|
||||
return (valid_count != 0u) ? sorted[valid_count / 2u] : 0u;
|
||||
}
|
||||
|
||||
static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
|
||||
const uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
|
||||
unsigned int satellite_count)
|
||||
@@ -1069,6 +1096,7 @@ void calculate_position()
|
||||
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
|
||||
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
|
||||
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
|
||||
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
|
||||
unsigned int satellite_count = 0;
|
||||
bool run_nav = false;
|
||||
bool initial_run = false;
|
||||
@@ -1115,6 +1143,7 @@ void calculate_position()
|
||||
{
|
||||
gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
|
||||
snapshot_pps_counters, snapshot_pps_sequences,
|
||||
snapshot_local_timestamps,
|
||||
satellite_count);
|
||||
}
|
||||
}
|
||||
@@ -1157,6 +1186,14 @@ void calculate_position()
|
||||
uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
|
||||
if(nav_ok)
|
||||
{
|
||||
uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
|
||||
snapshot_local_timestamps, satellite_count);
|
||||
if(!gps_fusion_publish_solution(&nav_solution, measurement_timestamp))
|
||||
{
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"fusion GPS publish skipped timestamp=%lu\r\n",
|
||||
(unsigned long)measurement_timestamp);
|
||||
}
|
||||
uint64_t consume_irq_state = bsp_irq_save();
|
||||
gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
|
||||
bsp_irq_restore(consume_irq_state);
|
||||
|
||||
@@ -70,6 +70,7 @@ bool insert_element(volatile gps_ephem_ll_t** node,
|
||||
new_node->gps_nav_solve_consumed_sequence = 0u;
|
||||
new_node->gps_last_nav_solve_abs_sample = 0u;
|
||||
new_node->gps_last_nav_solve_corrected_abs_sample = 0.0;
|
||||
new_node->gps_last_nav_solve_local_ticks = 0u;
|
||||
new_node->gps_last_nav_solve_pps_counter = 0u;
|
||||
new_node->gps_last_pps_capture_sequence = 0u;
|
||||
new_node->gps_last_measurement_tick = 0u;
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
#include "navigation_fusion.h"
|
||||
|
||||
#include "fusion_bus.h"
|
||||
#include "fusion_gps_adapter.h"
|
||||
#include "tdk_icm42688.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
static fusion_bus_t measurement_bus;
|
||||
static fusion_gps_adapter_t gps_adapter;
|
||||
static fusion_engine_t fusion_engine;
|
||||
static bool fusion_initialized;
|
||||
|
||||
bool navigation_fusion_init(uint64_t local_timebase_hz)
|
||||
{
|
||||
fusion_engine_config_t engine_config;
|
||||
fusion_gps_adapter_config_t adapter_config;
|
||||
if(local_timebase_hz == 0u)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_bus_init(&measurement_bus);
|
||||
fusion_engine_default_config(&engine_config, local_timebase_hz);
|
||||
fusion_gps_adapter_default_config(&adapter_config);
|
||||
if(!fusion_engine_init(&fusion_engine, &engine_config) ||
|
||||
!fusion_gps_adapter_init(&gps_adapter, &adapter_config))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
fusion_initialized = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool navigation_fusion_publish_gps(const fusion_gps_measurement_t* measurement)
|
||||
{
|
||||
return fusion_initialized &&
|
||||
fusion_bus_publish_gps(&measurement_bus, measurement);
|
||||
}
|
||||
|
||||
unsigned int navigation_fusion_service(void)
|
||||
{
|
||||
if(!fusion_initialized)
|
||||
{
|
||||
return 0u;
|
||||
}
|
||||
unsigned int processed = 0u;
|
||||
fusion_imu_measurement_t imu;
|
||||
while(tdk_icm42688_take_measurement(&imu))
|
||||
{
|
||||
(void)fusion_engine_push_imu(&fusion_engine, &imu);
|
||||
processed++;
|
||||
}
|
||||
|
||||
/* GPS is intentionally consumed after all currently queued IMU samples.
|
||||
* A delayed WLS result is applied at its own timestamp by fixed-lag replay. */
|
||||
fusion_gps_measurement_t gps;
|
||||
while(fusion_bus_take_gps(&measurement_bus, &gps))
|
||||
{
|
||||
fusion_position_measurement_t position;
|
||||
if(fusion_gps_adapter_convert(&gps_adapter, &gps, &position))
|
||||
{
|
||||
(void)fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
|
||||
}
|
||||
processed++;
|
||||
}
|
||||
return processed;
|
||||
}
|
||||
|
||||
bool navigation_fusion_get_output(navigation_fusion_output_t* output)
|
||||
{
|
||||
if((output == NULL) || !fusion_initialized ||
|
||||
(fusion_engine.state != FUSION_ENGINE_RUNNING))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset(output, 0, sizeof(*output));
|
||||
const fusion_runtime_t* runtime = &fusion_engine.current;
|
||||
output->timestamp_ticks = runtime->filter_timestamp_ticks;
|
||||
memcpy(output->position_enu_m, runtime->filter.position_m,
|
||||
sizeof(output->position_enu_m));
|
||||
memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
|
||||
sizeof(output->velocity_enu_mps));
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
output->position_std_m[axis] =
|
||||
sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
|
||||
}
|
||||
output->speed_mps = eskf_speed_mps(&runtime->filter);
|
||||
output->last_position_nis = runtime->filter.last_position_nis;
|
||||
output->motion_state = runtime->motion.state;
|
||||
output->gps_accepted = fusion_engine.diagnostics.position_accepted;
|
||||
output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
|
||||
if(!fusion_gps_adapter_local_to_geodetic(
|
||||
&gps_adapter, runtime->filter.position_m,
|
||||
&output->latitude_deg, &output->longitude_deg, &output->height_m))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
output->valid = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
const fusion_engine_t* navigation_fusion_engine(void)
|
||||
{
|
||||
return fusion_initialized ? &fusion_engine : NULL;
|
||||
}
|
||||
@@ -0,0 +1,393 @@
|
||||
#include "tdk_icm42688.h"
|
||||
|
||||
#include <bsp/mmio.h>
|
||||
#include <bsp/plic.h>
|
||||
#include <bsp/pmod_spi.h>
|
||||
#include <bsp/soc.h>
|
||||
#include <limits.h>
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
|
||||
#define ICM_DEVICE_CONFIG 0x11u
|
||||
#define ICM_INT_CONFIG 0x14u
|
||||
#define ICM_TEMP_DATA1 0x1du
|
||||
#define ICM_INT_STATUS 0x2du
|
||||
#define ICM_PWR_MGMT0 0x4eu
|
||||
#define ICM_GYRO_CONFIG0 0x4fu
|
||||
#define ICM_ACCEL_CONFIG0 0x50u
|
||||
#define ICM_GYRO_CONFIG1 0x51u
|
||||
#define ICM_GYRO_ACCEL_CONFIG0 0x52u
|
||||
#define ICM_ACCEL_CONFIG1 0x53u
|
||||
#define ICM_INT_CONFIG0 0x63u
|
||||
#define ICM_INT_CONFIG1 0x64u
|
||||
#define ICM_INT_SOURCE0 0x65u
|
||||
#define ICM_WHO_AM_I 0x75u
|
||||
|
||||
#define ICM_SPI_READ 0x80u
|
||||
#define ICM_SPI_DUMMY 0xffu
|
||||
#define ICM_DATA_READY 0x08u
|
||||
#define ICM42688_WHO_AM_I 0x47u
|
||||
#define ICM_PWR_LOW_NOISE 0x0fu
|
||||
#define ICM_INT1_LATCHED_HIGH_PUSH_PULL 0x07u
|
||||
#define ICM_UI_DRDY_INT1_EN 0x08u
|
||||
|
||||
#define TDK_GRAVITY_MPS2 9.80665
|
||||
#define TDK_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
|
||||
|
||||
#if (TDK_IMU_RAW_QUEUE_CAPACITY < 2u)
|
||||
#error "TDK_IMU_RAW_QUEUE_CAPACITY must be at least 2"
|
||||
#endif
|
||||
|
||||
static tdk_icm42688_config_t active_config;
|
||||
static tdk_icm42688_raw_sample_t sample_queue[TDK_IMU_RAW_QUEUE_CAPACITY];
|
||||
static volatile unsigned int sample_head;
|
||||
static volatile unsigned int sample_tail;
|
||||
static volatile tdk_icm42688_diagnostics_t diagnostics;
|
||||
|
||||
static uint64_t time_now(void)
|
||||
{
|
||||
return bsp_read64(BSP_CLINT_MTIME);
|
||||
}
|
||||
|
||||
static void delay_us(uint32_t microseconds)
|
||||
{
|
||||
uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * microseconds + 999999u) /
|
||||
1000000u;
|
||||
uint64_t start = time_now();
|
||||
while((time_now() - start) < ticks)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
static void delay_ms(uint32_t milliseconds)
|
||||
{
|
||||
uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * milliseconds + 999u) /
|
||||
1000u;
|
||||
uint64_t start = time_now();
|
||||
while((time_now() - start) < ticks)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
static void write_register(uint8_t reg, uint8_t value)
|
||||
{
|
||||
bsp_pmod_spi_write_reg8(reg & (uint8_t)~ICM_SPI_READ, value);
|
||||
}
|
||||
|
||||
static uint8_t read_register(uint8_t reg)
|
||||
{
|
||||
return bsp_pmod_spi_read_reg8(reg | ICM_SPI_READ, ICM_SPI_DUMMY);
|
||||
}
|
||||
|
||||
static int16_t big_endian_i16(const uint8_t* bytes)
|
||||
{
|
||||
return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
|
||||
}
|
||||
|
||||
static bool odr_register_value(uint32_t odr_hz, uint8_t* register_value)
|
||||
{
|
||||
if(register_value == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
switch(odr_hz)
|
||||
{
|
||||
case 1000u: *register_value = 0x06u; return true;
|
||||
case 500u: *register_value = 0x0fu; return true;
|
||||
case 200u: *register_value = 0x07u; return true;
|
||||
case 100u: *register_value = 0x08u; return true;
|
||||
case 50u: *register_value = 0x09u; return true;
|
||||
case 25u: *register_value = 0x0au; return true;
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
static bool accel_full_scale_bits(tdk_accel_full_scale_t full_scale,
|
||||
uint8_t* bits)
|
||||
{
|
||||
if(bits == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
switch(full_scale)
|
||||
{
|
||||
case TDK_ACCEL_FS_16G: *bits = 0u; return true;
|
||||
case TDK_ACCEL_FS_8G: *bits = 1u; return true;
|
||||
case TDK_ACCEL_FS_4G: *bits = 2u; return true;
|
||||
case TDK_ACCEL_FS_2G: *bits = 3u; return true;
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
static bool gyro_full_scale_bits(tdk_gyro_full_scale_mdps_t full_scale,
|
||||
uint8_t* bits)
|
||||
{
|
||||
if(bits == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
switch(full_scale)
|
||||
{
|
||||
case TDK_GYRO_FS_2000: *bits = 0u; return true;
|
||||
case TDK_GYRO_FS_1000: *bits = 1u; return true;
|
||||
case TDK_GYRO_FS_500: *bits = 2u; return true;
|
||||
case TDK_GYRO_FS_250: *bits = 3u; return true;
|
||||
case TDK_GYRO_FS_125: *bits = 4u; return true;
|
||||
case TDK_GYRO_FS_62D5: *bits = 5u; return true;
|
||||
case TDK_GYRO_FS_31D25: *bits = 6u; return true;
|
||||
case TDK_GYRO_FS_15D625: *bits = 7u; return true;
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
|
||||
static unsigned int next_queue_index(unsigned int index)
|
||||
{
|
||||
index++;
|
||||
return (index == TDK_IMU_RAW_QUEUE_CAPACITY) ? 0u : index;
|
||||
}
|
||||
|
||||
static bool raw_sample_valid(const tdk_icm42688_raw_sample_t* sample)
|
||||
{
|
||||
if((sample == NULL) || (sample->temperature_raw == INT16_MIN))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
if((sample->accel_raw[axis] == INT16_MIN) ||
|
||||
(sample->gyro_raw[axis] == INT16_MIN))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool queue_push_isr(const tdk_icm42688_raw_sample_t* sample)
|
||||
{
|
||||
unsigned int head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED);
|
||||
unsigned int next = next_queue_index(head);
|
||||
if(next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE))
|
||||
{
|
||||
__atomic_fetch_add(&diagnostics.queue_overflow_count, 1u, __ATOMIC_RELAXED);
|
||||
return false;
|
||||
}
|
||||
sample_queue[head] = *sample;
|
||||
__atomic_store_n(&sample_head, next, __ATOMIC_RELEASE);
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool queue_pop(tdk_icm42688_raw_sample_t* sample)
|
||||
{
|
||||
unsigned int tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED);
|
||||
if(tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*sample = sample_queue[tail];
|
||||
__atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE);
|
||||
return true;
|
||||
}
|
||||
|
||||
void tdk_icm42688_default_config(tdk_icm42688_config_t* config)
|
||||
{
|
||||
if(config == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
config->odr_hz = TDK_IMU_ODR_HZ;
|
||||
/* Motion-capable defaults; stationary characterization may select narrower ranges. */
|
||||
config->accel_full_scale_g = TDK_ACCEL_FS_4G;
|
||||
config->gyro_full_scale_mdps = TDK_GYRO_FS_250;
|
||||
config->ui_filter_config = 0x77u;
|
||||
}
|
||||
|
||||
bool tdk_icm42688_init(const tdk_icm42688_config_t* config)
|
||||
{
|
||||
uint8_t odr_bits;
|
||||
uint8_t accel_bits;
|
||||
uint8_t gyro_bits;
|
||||
if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) ||
|
||||
!accel_full_scale_bits(config->accel_full_scale_g, &accel_bits) ||
|
||||
!gyro_full_scale_bits(config->gyro_full_scale_mdps, &gyro_bits))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
memset((void*)&diagnostics, 0, sizeof(diagnostics));
|
||||
sample_head = 0u;
|
||||
sample_tail = 0u;
|
||||
active_config = *config;
|
||||
|
||||
bsp_pmod_spi_init();
|
||||
bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
|
||||
bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u);
|
||||
delay_us(1000u);
|
||||
diagnostics.who_am_i = read_register(ICM_WHO_AM_I);
|
||||
if((diagnostics.who_am_i != ICM42688_WHO_AM_I) ||
|
||||
(read_register(ICM_DEVICE_CONFIG) != 0x00u))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
write_register(ICM_INT_SOURCE0, 0x00u);
|
||||
write_register(ICM_PWR_MGMT0, 0x00u);
|
||||
delay_ms(1u);
|
||||
write_register(ICM_GYRO_CONFIG0, (uint8_t)((gyro_bits << 5) | odr_bits));
|
||||
write_register(ICM_ACCEL_CONFIG0, (uint8_t)((accel_bits << 5) | odr_bits));
|
||||
write_register(ICM_GYRO_CONFIG1,
|
||||
(uint8_t)((read_register(ICM_GYRO_CONFIG1) & 0x10u) | 0xcau));
|
||||
write_register(ICM_ACCEL_CONFIG1,
|
||||
(uint8_t)((read_register(ICM_ACCEL_CONFIG1) & 0xe1u) | 0x14u));
|
||||
write_register(ICM_GYRO_ACCEL_CONFIG0, config->ui_filter_config);
|
||||
|
||||
/* Active-high, push-pull, latched INT1. Status read clears data-ready. */
|
||||
write_register(ICM_INT_CONFIG, ICM_INT1_LATCHED_HIGH_PUSH_PULL);
|
||||
write_register(ICM_INT_CONFIG0, 0x00u);
|
||||
/* Datasheet requires clearing the reset default bit 4 for pin operation. */
|
||||
write_register(ICM_INT_CONFIG1, 0x00u);
|
||||
write_register(ICM_PWR_MGMT0, ICM_PWR_LOW_NOISE);
|
||||
delay_ms(50u);
|
||||
|
||||
if((read_register(ICM_PWR_MGMT0) != ICM_PWR_LOW_NOISE) ||
|
||||
(read_register(ICM_GYRO_CONFIG0) != (uint8_t)((gyro_bits << 5) | odr_bits)) ||
|
||||
(read_register(ICM_ACCEL_CONFIG0) != (uint8_t)((accel_bits << 5) | odr_bits)) ||
|
||||
(read_register(ICM_GYRO_ACCEL_CONFIG0) != config->ui_filter_config) ||
|
||||
(read_register(ICM_INT_CONFIG) != ICM_INT1_LATCHED_HIGH_PUSH_PULL) ||
|
||||
(read_register(ICM_INT_CONFIG1) != 0x00u))
|
||||
{
|
||||
write_register(ICM_PWR_MGMT0, 0x00u);
|
||||
return false;
|
||||
}
|
||||
(void)read_register(ICM_INT_STATUS);
|
||||
diagnostics.initialized = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool tdk_icm42688_start(void)
|
||||
{
|
||||
if(!diagnostics.initialized || diagnostics.running)
|
||||
{
|
||||
return diagnostics.running;
|
||||
}
|
||||
(void)read_register(ICM_INT_STATUS);
|
||||
bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
|
||||
write_register(ICM_INT_SOURCE0, ICM_UI_DRDY_INT1_EN);
|
||||
if(read_register(ICM_INT_SOURCE0) != ICM_UI_DRDY_INT1_EN)
|
||||
{
|
||||
bsp_plic_disable(BSP_PMOD_IRQ);
|
||||
return false;
|
||||
}
|
||||
(void)read_register(ICM_INT_STATUS);
|
||||
diagnostics.running = true;
|
||||
bsp_plic_enable(BSP_PMOD_IRQ);
|
||||
return true;
|
||||
}
|
||||
|
||||
void tdk_icm42688_stop(void)
|
||||
{
|
||||
bsp_plic_disable(BSP_PMOD_IRQ);
|
||||
write_register(ICM_INT_SOURCE0, 0x00u);
|
||||
(void)read_register(ICM_INT_STATUS);
|
||||
diagnostics.running = false;
|
||||
}
|
||||
|
||||
void tdk_icm42688_irq_handler(void)
|
||||
{
|
||||
uint64_t timestamp = time_now();
|
||||
__atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED);
|
||||
uint8_t status = read_register(ICM_INT_STATUS);
|
||||
if(!diagnostics.running || ((status & ICM_DATA_READY) == 0u))
|
||||
{
|
||||
__atomic_fetch_add(&diagnostics.spurious_irq_count, 1u, __ATOMIC_RELAXED);
|
||||
return;
|
||||
}
|
||||
uint8_t bytes[14];
|
||||
bsp_pmod_spi_read_regs(ICM_TEMP_DATA1 | ICM_SPI_READ, ICM_SPI_DUMMY,
|
||||
bytes, sizeof(bytes));
|
||||
tdk_icm42688_raw_sample_t sample;
|
||||
sample.timestamp_ticks = timestamp;
|
||||
sample.temperature_raw = big_endian_i16(&bytes[0]);
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
sample.accel_raw[axis] = big_endian_i16(&bytes[2u + 2u * axis]);
|
||||
sample.gyro_raw[axis] = big_endian_i16(&bytes[8u + 2u * axis]);
|
||||
}
|
||||
sample.sequence = (uint32_t)(__atomic_load_n(&diagnostics.sample_count,
|
||||
__ATOMIC_RELAXED) + 1u);
|
||||
if(!raw_sample_valid(&sample))
|
||||
{
|
||||
__atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, __ATOMIC_RELAXED);
|
||||
return;
|
||||
}
|
||||
uint64_t previous_timestamp =
|
||||
__atomic_exchange_n(&diagnostics.last_timestamp_ticks, timestamp,
|
||||
__ATOMIC_RELAXED);
|
||||
if(previous_timestamp != 0u)
|
||||
{
|
||||
uint64_t expected_ticks = BSP_CLINT_TIMEBASE_HZ / active_config.odr_hz;
|
||||
uint64_t elapsed = timestamp - previous_timestamp;
|
||||
if((expected_ticks != 0u) && (elapsed > expected_ticks + expected_ticks / 2u))
|
||||
{
|
||||
uint64_t periods = (elapsed + expected_ticks / 2u) / expected_ticks;
|
||||
if(periods > 1u)
|
||||
{
|
||||
__atomic_fetch_add(&diagnostics.estimated_missed_sample_count,
|
||||
periods - 1u, __ATOMIC_RELAXED);
|
||||
}
|
||||
}
|
||||
}
|
||||
(void)queue_push_isr(&sample);
|
||||
__atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED);
|
||||
}
|
||||
|
||||
bool tdk_icm42688_take_measurement(fusion_imu_measurement_t* measurement_out)
|
||||
{
|
||||
if(measurement_out == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
tdk_icm42688_raw_sample_t raw;
|
||||
if(!queue_pop(&raw))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double accel_scale = (double)active_config.accel_full_scale_g *
|
||||
TDK_GRAVITY_MPS2 / 32768.0;
|
||||
double gyro_scale = ((double)active_config.gyro_full_scale_mdps / 1000.0) *
|
||||
TDK_DEGREES_TO_RADIANS / 32768.0;
|
||||
measurement_out->timestamp_ticks = raw.timestamp_ticks;
|
||||
for(unsigned int axis = 0 ; axis < 3u ; axis++)
|
||||
{
|
||||
measurement_out->accel_mps2[axis] = (double)raw.accel_raw[axis] * accel_scale;
|
||||
measurement_out->gyro_rps[axis] = (double)raw.gyro_raw[axis] * gyro_scale;
|
||||
}
|
||||
measurement_out->temperature_c = 25.0 + (double)raw.temperature_raw / 132.48;
|
||||
measurement_out->sequence = raw.sequence;
|
||||
return true;
|
||||
}
|
||||
|
||||
void tdk_icm42688_get_diagnostics(tdk_icm42688_diagnostics_t* diagnostics_out)
|
||||
{
|
||||
if(diagnostics_out == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
diagnostics_out->irq_count =
|
||||
__atomic_load_n(&diagnostics.irq_count, __ATOMIC_RELAXED);
|
||||
diagnostics_out->sample_count =
|
||||
__atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED);
|
||||
diagnostics_out->invalid_sample_count =
|
||||
__atomic_load_n(&diagnostics.invalid_sample_count, __ATOMIC_RELAXED);
|
||||
diagnostics_out->spurious_irq_count =
|
||||
__atomic_load_n(&diagnostics.spurious_irq_count, __ATOMIC_RELAXED);
|
||||
diagnostics_out->queue_overflow_count =
|
||||
__atomic_load_n(&diagnostics.queue_overflow_count, __ATOMIC_RELAXED);
|
||||
diagnostics_out->estimated_missed_sample_count =
|
||||
__atomic_load_n(&diagnostics.estimated_missed_sample_count, __ATOMIC_RELAXED);
|
||||
diagnostics_out->last_timestamp_ticks =
|
||||
__atomic_load_n(&diagnostics.last_timestamp_ticks, __ATOMIC_RELAXED);
|
||||
diagnostics_out->who_am_i = diagnostics.who_am_i;
|
||||
diagnostics_out->initialized = diagnostics.initialized;
|
||||
diagnostics_out->running = diagnostics.running;
|
||||
}
|
||||
Reference in New Issue
Block a user