From 971be036a016ffa9eba9ea0276d3135d6d767a5c Mon Sep 17 00:00:00 2001 From: onur Date: Sat, 25 Jul 2026 13:11:33 +0300 Subject: [PATCH] feat(fusion): fuse Doppler velocity in ESKF --- soc/software/fusion/README.md | 45 +++-- soc/software/fusion/tests/eskf_motion_test.c | 156 ++++++++++++++-- soc/software/gps_system/README.md | 46 +++-- soc/software/gps_system/header/fusion/eskf.h | 11 ++ .../gps_system/header/fusion/fusion_engine.h | 12 +- .../header/fusion/fusion_gps_adapter.h | 7 + .../header/fusion/fusion_measurement.h | 13 ++ .../header/fusion/navigation_fusion.h | 8 +- .../gps_system/header/gps/NavEquations.h | 1 + .../header/gps/gps_tracking_measurement.h | 1 + .../src/adapters/gps_fusion_publisher.c | 3 + .../gps_system/src/adapters/gps_system_uart.c | 6 +- soc/software/gps_system/src/fusion/eskf.c | 50 +++++ .../gps_system/src/fusion/fusion_engine.c | 172 ++++++++++++++---- .../src/fusion/fusion_gps_adapter.c | 130 ++++++++++--- .../gps_system/src/fusion/navigation_fusion.c | 13 +- soc/software/gps_system/src/gps/gps_ip.c | 1 + .../src/gps/gps_tracking_measurement.c | 66 ++++++- .../tests/test_gps_tracking_measurement.c | 12 ++ 19 files changed, 647 insertions(+), 106 deletions(-) diff --git a/soc/software/fusion/README.md b/soc/software/fusion/README.md index a7d00f1..a760988 100644 --- a/soc/software/fusion/README.md +++ b/soc/software/fusion/README.md @@ -66,8 +66,8 @@ Offline model: UART GPS + IMU logları -> Python parser -> fixed/adaptive ESKF -> CSV/grafik/harita Ürün yolu: -GPS WLS ---------> fusion_bus --\ - -> fusion_engine -> 15-state ESKF -> fused LLH/ENU +GPS position + Doppler velocity --\ + -> fusion_engine -> 15-state ESKF -> fused LLH/ENU TDK veya ST IMU -> imu_sensor ----/ ``` @@ -87,12 +87,14 @@ backend'ini seçer, `imu_calibration` SI birimine ve ortak eksen takımına dönüştürür, `fusion_engine` ise sensör markasını bilmeden motion detector ve ESKF'yi çalıştırır. IMU ve GPS asenkrondur; ikisi de aynı monotonik CLINT/CPU tick alanında ölçüm anında damgalanır. GPS WLS geç tamamlanırsa fixed-lag IMU -history replay ile update gerçek ölçüm epoch'unda uygulanır. +history replay ile position ve Doppler velocity update'leri atomik olarak +gerçek ölçüm epoch'unda uygulanır. Doğrulama durumu: - Linux/GCC sentetik hareket, stationary, ani ivmelenme/fren, dönüş, ZUPT, - GPS NIS rejection, gecikmiş GPS replay, covariance invariantları ve + position/velocity NIS rejection, gecikmiş aynı-epoch position+velocity + replay, ECEF->ENU velocity/covariance dönüşümü, covariance invariantları ve ASan/UBSan testleri geçti. Ayrıntılı sonuçlar aşağıdadır. - STEVAL-MKI217V1 üzerindeki LSM6DSOX'un SPI Mode-0 okuması, 60 saniyelik stationary ham veri kaydı ve fiziksel INT1 yolu gerçek Genesys2 kartında @@ -124,7 +126,10 @@ Sonraki ana adımlar: reject, IRQ/sample-loss ve queue sayaçlarını kaydetmek. 2. Stationary -> ani hareket -> sabit hız -> fren -> tekrar stationary testini yapmak; hareket sırasında ZUPT'nin kapalı kaldığını doğrulamak. -3. PPS için aşağıdaki lock teşhisini tamamlamak. +3. Yeni Doppler-aided firmware'i kartta çalıştırıp `pos_nis`, `vel_nis` ve + `velocity=accepted/rejected` sayaçlarıyla position-only koşuya karşı A/B + kayıt almak. +4. PPS için aşağıdaki lock teşhisini tamamlamak. Prototip montaj notu: STEVAL jumper kablolarıyla serbest durduğu için başlangıç sensor yönü sabit kabul edilmemelidir. Her açılışta en az iki saniye stationary @@ -369,9 +374,11 @@ Karar yalniz rejected-candidate suresine degil son basarili position fusion zamanina baglanmalidir. Uydu sayisi, finite covariance, DOP, WLS integrity ve alternatif aiding kaynaklari birlikte degerlendirilmelidir. Tam dort uyduda WLS residual redundancy/chi-square DOF bulunmadigi icin temporal consistency -daha onemlidir. Ileride Doppler velocity ayri innovation, fusion timeout ve -velocity-reset politikasi ile fuse edilmelidir. Yatay position ve height da -barometre gibi ek kaynaklar geldiginde birbirinden bagimsiz yonetilmelidir. +daha onemlidir. Doppler velocity artik ayri 3-DOF innovation/NIS kapisiyla +fuse edilir; bir velocity reject'i ayni epoch'taki position sonucunu bozmaz. +Henuz ayri bir velocity fusion-timeout/state-reset politikasi yoktur. Yatay +position ve height da barometre gibi ek kaynaklar geldiginde birbirinden +bagimsiz yonetilmelidir. Baslangic GNSS'i icin mutlak bir dogruluk referansi yoksa tutarli fakat yanlis bir GNSS kumesini otomatik olarak gercek konumdan ayirmak temel olarak mumkun @@ -546,8 +553,10 @@ Hareket kararıyla ilgili iki koruma sentetik test sonucunda eklendi: - 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. + histerezisli. Doppler velocity artık ESKF hız state'ini düzeltir, fakat kart + verisindeki birkaç m/s stationary saçılım nedeniyle doğrudan rest/ZUPT + kararı olarak kullanılmaz. İleride covariance/NIS'e bağlı çoklu-confirmation + stop evidence ayrı olarak eklenebilir. Linux/GCC testlerini çalıştırma: @@ -636,16 +645,21 @@ ham sample-to-sample noise'un jerk üzerinden sahte `MOVING` üretmediğini ve - 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; +- Doppler velocity NIS accept/reject; `2/2/3 m/s` konservatif covariance ile + 5 Hz velocity aiding'in sentetik `0.05 m/s2` accel-bias hız sürüklenmesini + en az `4x` azaltması; - 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, +- 300 ms gecikmiş aynı-epoch position+velocity 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ü; +- LLH--ECEF--ENU round-trip, ECEF velocity/covariance rotasyonu ve 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 @@ -696,8 +710,11 @@ 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. +Ürün yolu artık WLS'nin ECEF velocity covariance'ini ENU'ya döndürüp ayrı +velocity measurement update'inde kullanır. Default `2.0/2.0/3.0 m/s` +covariance floor ve 3-DOF `%99.9` NIS gate kartta ölçülen zayıf-geometri +saçılımına karşı konservatiftir. Daha hızlı stop confirmation, bu update'ten +ayrı ve çoklu-epoch istatistiksel kanıt gerektiren sonraki adımdır. 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 diff --git a/soc/software/fusion/tests/eskf_motion_test.c b/soc/software/fusion/tests/eskf_motion_test.c index f1e3c67..f2e7edb 100644 --- a/soc/software/fusion/tests/eskf_motion_test.c +++ b/soc/software/fusion/tests/eskf_motion_test.c @@ -231,6 +231,81 @@ static void test_position_nis_gate(void) "GPS accept/reject counters"); } +static void test_velocity_nis_gate(void) +{ + eskf_t filter; + eskf_config_t config; + identity_filter(&filter, &config); + double covariance[9] = { + 1.0, 0.1, 0.0, + 0.1, 1.5, 0.0, + 0.0, 0.0, 2.0 + }; + double normal_velocity[3] = {1.0, -0.5, 0.2}; + double nis = NAN; + CHECK(eskf_velocity_update(&filter, &config, normal_velocity, + covariance, &nis) == ESKF_UPDATE_ACCEPTED, + "normal Doppler velocity accepted"); + CHECK(isfinite(nis) && (nis < config.velocity_nis_gate), + "accepted Doppler velocity has finite in-gate NIS"); + CHECK((filter.velocity_mps[0] > 0.0) && + (filter.velocity_mps[1] < 0.0) && + (filter.velocity_mps[2] > 0.0), + "Doppler update corrects all ESKF velocity axes"); + double before[3]; + memcpy(before, filter.velocity_mps, sizeof(before)); + double outlier[3] = {100.0, -80.0, 50.0}; + CHECK(eskf_velocity_update(&filter, &config, outlier, + covariance, &nis) == ESKF_UPDATE_REJECTED_NIS, + "large Doppler velocity outlier rejected by NIS"); + CHECK(nearly_equal(before[0], filter.velocity_mps[0], 1.0e-12) && + nearly_equal(before[1], filter.velocity_mps[1], 1.0e-12) && + nearly_equal(before[2], filter.velocity_mps[2], 1.0e-12), + "velocity NIS rejection leaves nominal state unchanged"); + CHECK(filter.velocity_update_accepted_count == 1u && + filter.velocity_update_rejected_count == 1u, + "Doppler velocity accept/reject counters"); +} + +static double run_accel_bias_with_optional_doppler(bool fuse_doppler) +{ + eskf_t filter; + eskf_config_t config; + identity_filter(&filter, &config); + const double velocity_covariance[9] = { + 4.0, 0.0, 0.0, + 0.0, 4.0, 0.0, + 0.0, 0.0, 9.0 + }; + const double stationary_velocity[3] = {0.0, 0.0, 0.0}; + for(unsigned int sample = 1u; sample <= 1000u; sample++) + { + /* Unmodelled 0.05 m/s2 x-axis accelerometer bias. */ + double accel[3] = {0.05, 0.0, config.gravity_mps2}; + double gyro[3] = {0.0, 0.0, 0.0}; + CHECK(eskf_propagate(&filter, &config, accel, gyro, 0.01), + "accelerometer-bias profile propagation"); + if(fuse_doppler && ((sample % 20u) == 0u)) + { + CHECK(eskf_velocity_update( + &filter, &config, stationary_velocity, + velocity_covariance, NULL) == ESKF_UPDATE_ACCEPTED, + "5 Hz conservative Doppler velocity update"); + } + } + return eskf_speed_mps(&filter); +} + +static void test_doppler_velocity_limits_inertial_drift(void) +{ + double unaided_speed = run_accel_bias_with_optional_doppler(false); + double doppler_aided_speed = run_accel_bias_with_optional_doppler(true); + CHECK(unaided_speed > 0.45, + "unmodelled accelerometer bias creates expected inertial speed drift"); + CHECK(doppler_aided_speed < 0.25 * unaided_speed, + "conservative Doppler velocity materially limits inertial speed drift"); +} + static double profile_acceleration(double time_s) { if((time_s >= 2.0) && (time_s < 4.0)) @@ -579,6 +654,21 @@ static fusion_position_measurement_t replay_position(uint64_t tick, double east_ return position; } +static fusion_navigation_measurement_t replay_navigation( + uint64_t tick, + double east_m, + double east_velocity_mps) +{ + fusion_navigation_measurement_t navigation = {0}; + navigation.position = replay_position(tick, east_m); + navigation.velocity_enu_mps[0] = east_velocity_mps; + navigation.velocity_covariance_enu_mps2[0] = 0.25; + navigation.velocity_covariance_enu_mps2[4] = 0.25; + navigation.velocity_covariance_enu_mps2[8] = 1.0; + navigation.velocity_valid = true; + return navigation; +} + static void check_runtime_close(const fusion_runtime_t* first, const fusion_runtime_t* second, double tolerance) @@ -621,28 +711,35 @@ static void test_delayed_position_replay(void) 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); + fusion_navigation_measurement_t gps_at_1500 = + replay_navigation(1500u, 0.125, 0.5); 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"); + eskf_update_result_t position_result = ESKF_UPDATE_INVALID; + eskf_update_result_t velocity_result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_navigation( + &on_time, &gps_at_1500, + &position_result, NULL, &velocity_result, NULL), + "on-time position/velocity update processed"); + CHECK((position_result == ESKF_UPDATE_ACCEPTED) && + (velocity_result == ESKF_UPDATE_ACCEPTED), + "on-time position and Doppler velocity 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"); + eskf_update_result_t position_result = ESKF_UPDATE_INVALID; + eskf_update_result_t velocity_result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_navigation( + &delayed, &gps_at_1500, + &position_result, NULL, &velocity_result, NULL), + "300 ms delayed position/velocity update replayed"); + CHECK((position_result == ESKF_UPDATE_ACCEPTED) && + (velocity_result == ESKF_UPDATE_ACCEPTED), + "delayed position and velocity accepted at measurement epoch"); check_runtime_close(&on_time.current, &delayed.current, 1.0e-11); } } @@ -651,6 +748,8 @@ static void test_delayed_position_replay(void) "one delayed position caused one fixed-lag replay"); CHECK(delayed.diagnostics.replayed_imu_count > 0u, "delayed position replayed stored IMU samples"); + CHECK(delayed.diagnostics.velocity_accepted == 1u, + "delayed Doppler velocity is diagnosed as fused"); fusion_position_measurement_t future = replay_position(3500u, 1.0); CHECK(fusion_engine_push_position(&delayed, &future, NULL, NULL), @@ -727,6 +826,35 @@ static void test_gps_adapter(void) invalid.covariance_enu_m2[0][0] = -1.0; CHECK(!fusion_gps_adapter_convert(&adapter, &invalid, &local), "invalid GPS covariance is rejected before ESKF"); + + fusion_gps_adapter_t velocity_adapter; + CHECK(fusion_gps_adapter_init(&velocity_adapter, &config), + "velocity GPS adapter initialization"); + fusion_gps_measurement_t velocity_gps = adapter_gps(0.0, 0.0, 0.0); + velocity_gps.velocity_valid = true; + velocity_gps.velocity_ecef_mps[0] = 3.0; + velocity_gps.velocity_ecef_mps[1] = 1.0; + velocity_gps.velocity_ecef_mps[2] = -2.0; + velocity_gps.velocity_covariance_ecef_mps2[0][0] = 16.0; + velocity_gps.velocity_covariance_ecef_mps2[1][1] = 25.0; + velocity_gps.velocity_covariance_ecef_mps2[2][2] = 36.0; + fusion_navigation_measurement_t navigation; + CHECK(fusion_gps_adapter_convert_navigation( + &velocity_adapter, &velocity_gps, &navigation), + "GPS adapter converts position and Doppler velocity"); + CHECK(navigation.velocity_valid, + "finite Doppler velocity covariance remains valid"); + CHECK(nearly_equal(navigation.velocity_enu_mps[0], 1.0, 1.0e-12) && + nearly_equal(navigation.velocity_enu_mps[1], -2.0, 1.0e-12) && + nearly_equal(navigation.velocity_enu_mps[2], 3.0, 1.0e-12), + "ECEF Doppler velocity rotates to expected ENU axes"); + CHECK(nearly_equal( + navigation.velocity_covariance_enu_mps2[0], 25.0, 1.0e-12) && + nearly_equal( + navigation.velocity_covariance_enu_mps2[4], 36.0, 1.0e-12) && + nearly_equal( + navigation.velocity_covariance_enu_mps2[8], 16.0, 1.0e-12), + "ECEF Doppler covariance rotates to ENU"); } static void test_measurement_bus(void) @@ -795,6 +923,8 @@ int main(void) test_external_rest_cannot_erase_constant_turn(); test_gravity_alignment(); test_position_nis_gate(); + test_velocity_nis_gate(); + test_doppler_velocity_limits_inertial_drift(); test_motion_transitions(); test_variable_rates(); test_irregular_imu_timestamps(); diff --git a/soc/software/gps_system/README.md b/soc/software/gps_system/README.md index c38c2f4..0de8770 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -19,14 +19,17 @@ gps_system.c -> timestamped IMU queue -> navigation_fusion prediction -> 20 ms tracking mailbox -> C/N0 + Doppler/range-rate -> GPS events -> coherent position/velocity WLS -> gps_nav_solution_t - -> valid GPS position -> fusion_gps_measurement_t -> navigation_fusion update + -> valid GPS position + Doppler velocity -> fusion_gps_measurement_t + -> same-epoch navigation_fusion position/velocity update -> navigation_fusion_output_t -> corrected lat/lon publish ``` -Doppler WLS'nin ECEF velocity, speed ve residual sonucu -`fusion_gps_measurement_t` icine aktarilir ve UART'ta yayinlanir. Mevcut -`navigation_fusion` update'i henuz bu velocity alanlarini state correction -olarak kullanmaz; ESKF'ye giren aktif GPS olcumu position'dir. +Doppler WLS'nin ECEF velocity, covariance, speed ve residual sonucu +`fusion_gps_measurement_t` icine aktarilir. GPS adapter velocity ile +covariance'i ECEF'ten ENU'ya dondurur; position ve velocity ayni capture +epoch'unda ESKF'ye uygulanir, sonra daha yeni IMU history replay edilir. +Position ve velocity'nin ayri NIS kapilari/sayaclari vardir; bir velocity +outlier'i ayni epoch'taki gecerli position update'ini geri almaz. Kaynak ve public header agaci ayni modul sinirlarini izler: @@ -791,9 +794,21 @@ weight = 1 / sigma_rate^2 En az dort uydu gerekir. Dort uyduda velocity residual serbestlik derecesi yoktur; bes uydu ve dusuk PDOP'ta bile zayif geometri velocity gurultusunu -buyutebilir. Sonuc `velocity_valid`, ECEF velocity, `speed_mps` ve -`velocity_residual_rms_mps` alanlarinda yayinlanir, fakat henuz ESKF update'ine -fusion edilmez. +buyutebilir. WLS, `inverse(H'WH)` matrisinden ECEF velocity covariance uretir. +`N>4` ise weighted post-fit reduced chi-square yalniz covariance'i buyutebilir; +tek bir sessiz epoch formal belirsizligi kucultemez. Sonuc `velocity_valid`, +ECEF velocity/covariance, `speed_mps` ve `velocity_residual_rms_mps` +alanlarinda yayinlanir. + +Fusion adapter covariance'i `R_ecef_to_enu * P * R^T` ile ENU'ya dondurur. +2026-07-25 sabit kart verisinde zayif geometriyle gorulen +`p95=6.020 m/s` speed hatasi nedeniyle default alt sinirlar yatay eksenlerde +`2.0 m/s`, dikey eksende `3.0 m/s`'dir. Bunlar IIR ile korelasyonlu ard arda +epoch'larin ESKF'yi asiri guvenle cekmesini onler; formal covariance daha +buyukse aynen korunur. Velocity innovation 3-DOF `%99.9` +`NIS=16.266` kapisindan gecer. Kabul/red sayilari ve son velocity NIS'i +`[FUSION]` UART status satirinda `velocity=accepted/rejected` ve `vel_nis` +olarak yayinlanir. - Konum cozumu URA/elevation/CN0 agirlikli WLS kullanir; raporlanan DOP degerleri ise bu normalize relative weight'lerden uretilmez. GDOP/PDOP/TDOP agirliksiz geometri matrisinden, HDOP/VDOP de ayni covariance ECEF'ten lokal ENU eksenlerine donduruldukten sonra hesaplanir. Boylece loglanan degerler klasik geometrik DOP anlamini korur. - Troposfer correction her satellite icin bagimsiz hesaplanir ve dort satellite ile de aktiftir. Ilk konumsuz iteration sabit baslangic correction'i kullanir; receiver konumu olustuktan sonra elevation ve metre cinsinden receiver height modele verilir. Model kilometre bekledigi icin height donusumu yapilir ve dusuk elevation'da gecerli olabilen 5 m uzeri delay degerleri artik keyfi olarak sifirlanmaz. @@ -1071,8 +1086,11 @@ make -C hardware/external/gps/rtl/trck_src/tb test ``` `run` ve `sanitize`, C/N0 estimator'i, Q7 Doppler/range-rate donusumu, -coherent mailbox read'i, weighted velocity WLS ve satellite velocity/clock -drift hesaplarini kapsar. RTL Questa/vlog-vsim testi prompt akumulasyonu, +coherent mailbox read'i, weighted velocity WLS/covariance ve satellite +velocity/clock drift hesaplarini kapsar. Fusion regression'i ayrica +ECEF->ENU velocity/covariance rotasyonunu, velocity NIS kabul/redini ve ayni +epoch position+velocity delayed replay esdegerligini kapsar. RTL +Questa/vlog-vsim testi prompt akumulasyonu, 20 ms hold, synchronizer settle, atomik APB capture ve interrupt yolunu kapsar. @@ -1110,8 +1128,12 @@ Kart ve GPS anteni sabitken canli tracking mailbox olcumlerinde: C/N0 ve Doppler verisi fiziksel olarak makul, IIR velocity gurultusunu azaltiyor ve C/N0 weighting ornek epoch'ta sonucu iyilestiriyor. Buna ragmen position WLS'deki pseudorange/code-phase hata ve zayif geometriyi tek basina -duzeltmiyor. Doppler velocity de henuz ESKF state update'inde kullanilmadigi -icin `imu_corrected` cikisina dogrudan faydasi yoktur. +duzeltmiyor. Doppler velocity artik ESKF state update'ine baglidir; formal +covariance, kart verisine dayali floor ve ayri NIS gate ile asiri guven +sinirlanir. Bu yeni software'in gercek kartta `vel_nis` ve +`velocity=accepted/rejected` sayaclariyla saha A/B sonucu henuz olculmedi; +yukaridaki board istatistikleri ESKF entegrasyonundan onceki ham velocity +dogrulamasidir. ### 2026-07-08 ACQ Debug Notu diff --git a/soc/software/gps_system/header/fusion/eskf.h b/soc/software/gps_system/header/fusion/eskf.h index 84cf8a5..3b27dec 100644 --- a/soc/software/gps_system/header/fusion/eskf.h +++ b/soc/software/gps_system/header/fusion/eskf.h @@ -27,6 +27,7 @@ typedef struct { double gravity_mps2; double max_propagation_step_s; double position_nis_gate; + double velocity_nis_gate; double covariance_diagonal_floor; } eskf_config_t; @@ -40,9 +41,12 @@ typedef struct { double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM]; double last_unbiased_gyro_rps[ESKF_VECTOR_DIM]; double last_position_nis; + double last_velocity_nis; uint64_t propagation_count; uint64_t position_update_accepted_count; uint64_t position_update_rejected_count; + uint64_t velocity_update_accepted_count; + uint64_t velocity_update_rejected_count; bool initialized; } eskf_t; @@ -84,6 +88,13 @@ eskf_update_result_t eskf_position_update( const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM], double* nis_out); +eskf_update_result_t eskf_velocity_update( + eskf_t* filter, + const eskf_config_t* config, + const double velocity_mps[ESKF_VECTOR_DIM], + const double covariance_mps2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM], + double* nis_out); + /* Re-anchor position after a higher-level fusion timeout has established that * repeated, valid position observations are mutually coherent. Velocity, * attitude and IMU biases are preserved; position cross-covariances are diff --git a/soc/software/gps_system/header/fusion/fusion_engine.h b/soc/software/gps_system/header/fusion/fusion_engine.h index cb64022..4ead68a 100644 --- a/soc/software/gps_system/header/fusion/fusion_engine.h +++ b/soc/software/gps_system/header/fusion/fusion_engine.h @@ -80,6 +80,9 @@ typedef struct { uint64_t position_pending_overflow; uint64_t position_accepted; uint64_t position_recovery_count; + uint64_t velocity_received; + uint64_t velocity_nis_rejected; + uint64_t velocity_accepted; uint64_t replay_count; uint64_t replayed_imu_count; uint64_t zero_velocity_update_count; @@ -104,7 +107,7 @@ typedef struct { 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_navigation_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY]; fusion_position_measurement_t initial_position; fusion_position_measurement_t position_recovery_candidate; fusion_engine_diagnostics_t diagnostics; @@ -133,6 +136,13 @@ bool fusion_engine_push_position( const fusion_position_measurement_t* measurement, eskf_update_result_t* update_result_out, double* nis_out); +bool fusion_engine_push_navigation( + fusion_engine_t* engine, + const fusion_navigation_measurement_t* measurement, + eskf_update_result_t* position_update_result_out, + double* position_nis_out, + eskf_update_result_t* velocity_update_result_out, + double* velocity_nis_out); bool fusion_engine_get_runtime(const fusion_engine_t* engine, fusion_runtime_t* runtime_out); diff --git a/soc/software/gps_system/header/fusion/fusion_gps_adapter.h b/soc/software/gps_system/header/fusion/fusion_gps_adapter.h index dcd9e9c..5abdf84 100644 --- a/soc/software/gps_system/header/fusion/fusion_gps_adapter.h +++ b/soc/software/gps_system/header/fusion/fusion_gps_adapter.h @@ -9,6 +9,9 @@ typedef struct { double minimum_horizontal_std_m; double minimum_vertical_std_m; double maximum_position_std_m; + double minimum_horizontal_velocity_std_mps; + double minimum_vertical_velocity_std_mps; + double maximum_velocity_std_mps; } fusion_gps_adapter_config_t; typedef struct { @@ -27,6 +30,10 @@ bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter, 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_convert_navigation( + fusion_gps_adapter_t* adapter, + const fusion_gps_measurement_t* gps, + fusion_navigation_measurement_t* navigation_out); bool fusion_gps_adapter_local_to_geodetic( const fusion_gps_adapter_t* adapter, const double position_enu_m[3], diff --git a/soc/software/gps_system/header/fusion/fusion_measurement.h b/soc/software/gps_system/header/fusion/fusion_measurement.h index 9a08dc9..a66408b 100644 --- a/soc/software/gps_system/header/fusion/fusion_measurement.h +++ b/soc/software/gps_system/header/fusion/fusion_measurement.h @@ -22,6 +22,7 @@ typedef struct { double residual_rms_m; double chi_square_ratio; double velocity_ecef_mps[3]; + double velocity_covariance_ecef_mps2[3][3]; double speed_mps; double velocity_residual_rms_mps; unsigned int used_satellites; @@ -49,4 +50,16 @@ typedef struct { bool source_valid; } fusion_position_measurement_t; +/* + * Position and Doppler velocity from one GNSS solve. Keeping them in one + * message makes the fixed-lag coordinator apply both observations at exactly + * the captured GPS epoch before replaying newer IMU samples. + */ +typedef struct { + fusion_position_measurement_t position; + double velocity_enu_mps[3]; + double velocity_covariance_enu_mps2[9]; + bool velocity_valid; +} fusion_navigation_measurement_t; + #endif diff --git a/soc/software/gps_system/header/fusion/navigation_fusion.h b/soc/software/gps_system/header/fusion/navigation_fusion.h index 8e695f2..0c45759 100644 --- a/soc/software/gps_system/header/fusion/navigation_fusion.h +++ b/soc/software/gps_system/header/fusion/navigation_fusion.h @@ -20,10 +20,13 @@ typedef struct { double position_std_m[3]; double speed_mps; double last_position_nis; + double last_velocity_nis; fusion_motion_state_t motion_state; uint64_t gps_accepted; uint64_t gps_nis_rejected; uint64_t gps_position_recoveries; + uint64_t gps_velocity_accepted; + uint64_t gps_velocity_nis_rejected; uint64_t zero_update_count; uint64_t replay_count; uint64_t stale_gps_count; @@ -54,8 +57,9 @@ void navigation_fusion_set_platform_landed( * ESKF observation and is never rate-selected in this layer. */ bool navigation_fusion_push_imu(const imu_sample_t* measurement); -/* Observation input: convert a valid LLH fix to local ENU, apply it at the - * captured GPS epoch, then replay newer IMU history to current time. */ +/* Observation input: convert a valid LLH fix and optional Doppler velocity to + * local ENU, apply both at the captured GPS epoch, then replay newer IMU + * history to current time. */ bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement); /* local_valid can exist without GPS. global_valid/valid require a GPS origin. */ diff --git a/soc/software/gps_system/header/gps/NavEquations.h b/soc/software/gps_system/header/gps/NavEquations.h index 782e4ef..131aa72 100644 --- a/soc/software/gps_system/header/gps/NavEquations.h +++ b/soc/software/gps_system/header/gps/NavEquations.h @@ -10,6 +10,7 @@ double ecef_m[3]; double clock_bias_m; double velocity_ecef_mps[3]; + double velocity_covariance_ecef_mps2[3][3]; double clock_drift_mps; double speed_mps; double velocity_residual_rms_mps; diff --git a/soc/software/gps_system/header/gps/gps_tracking_measurement.h b/soc/software/gps_system/header/gps/gps_tracking_measurement.h index 74918cf..50143d3 100644 --- a/soc/software/gps_system/header/gps/gps_tracking_measurement.h +++ b/soc/software/gps_system/header/gps/gps_tracking_measurement.h @@ -55,6 +55,7 @@ bool gps_tracking_velocity_wls( const gps_tracking_measurement_t measurements[], unsigned int count, double receiver_velocity_ecef_mps_out[3], + double receiver_velocity_covariance_ecef_mps2_out[3][3], double* receiver_clock_drift_mps_out, double* residual_rms_mps_out); diff --git a/soc/software/gps_system/src/adapters/gps_fusion_publisher.c b/soc/software/gps_system/src/adapters/gps_fusion_publisher.c index 75ffc87..3c2f420 100644 --- a/soc/software/gps_system/src/adapters/gps_fusion_publisher.c +++ b/soc/software/gps_system/src/adapters/gps_fusion_publisher.c @@ -27,6 +27,9 @@ bool gps_fusion_build_measurement( memcpy(measurement_out->velocity_ecef_mps, solution->velocity_ecef_mps, sizeof(measurement_out->velocity_ecef_mps)); + memcpy(measurement_out->velocity_covariance_ecef_mps2, + solution->velocity_covariance_ecef_mps2, + sizeof(measurement_out->velocity_covariance_ecef_mps2)); measurement_out->speed_mps = solution->speed_mps; measurement_out->velocity_residual_rms_mps = solution->velocity_residual_rms_mps; diff --git a/soc/software/gps_system/src/adapters/gps_system_uart.c b/soc/software/gps_system/src/adapters/gps_system_uart.c index 85350fa..923d1b0 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -116,15 +116,19 @@ void gps_system_uart_publish_periodic_fusion_status( { GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION, "state=%s lat=%.10lf lon=%.10lf height=%.3lf " - "speed=%.3lf nis=%.3lf gps=%lu/%lu recoveries=%lu " + "speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf " + "gps=%lu/%lu velocity=%lu/%lu recoveries=%lu " "zupt=%lu replay=%lu " "sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu " "overflow=%lu missed=%lu\r\n", fusion_motion_state_name(output->motion_state), output->latitude_deg, output->longitude_deg, output->height_m, output->speed_mps, output->last_position_nis, + output->last_velocity_nis, (unsigned long)output->gps_accepted, (unsigned long)output->gps_nis_rejected, + (unsigned long)output->gps_velocity_accepted, + (unsigned long)output->gps_velocity_nis_rejected, (unsigned long)output->gps_position_recoveries, (unsigned long)output->zero_update_count, (unsigned long)output->replay_count, diff --git a/soc/software/gps_system/src/fusion/eskf.c b/soc/software/gps_system/src/fusion/eskf.c index 3b5a1a4..7755d9a 100644 --- a/soc/software/gps_system/src/fusion/eskf.c +++ b/soc/software/gps_system/src/fusion/eskf.c @@ -224,6 +224,7 @@ void eskf_default_config(eskf_config_t* config) 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->velocity_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */ config->covariance_diagonal_floor = 1.0e-12; } @@ -319,6 +320,7 @@ bool eskf_initialize(eskf_t* filter, } covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor); filter->last_position_nis = NAN; + filter->last_velocity_nis = NAN; filter->initialized = true; return true; } @@ -806,6 +808,54 @@ eskf_update_result_t eskf_position_update( return ESKF_UPDATE_ACCEPTED; } +eskf_update_result_t eskf_velocity_update( + eskf_t* filter, + const eskf_config_t* config, + const double velocity_mps[3], + const double covariance_mps2[9], + double* nis_out) +{ + if(nis_out != NULL) + { + *nis_out = NAN; + } + if((filter == NULL) || !filter->initialized || (config == NULL) || + !vector_is_finite(velocity_mps, 3u) || + !vector_is_finite(covariance_mps2, 9u)) + { + return ESKF_UPDATE_INVALID; + } + double residual[3]; + for(unsigned int i = 0u; i < 3u; i++) + { + residual[i] = velocity_mps[i] - filter->velocity_mps[i]; + } + double nis = NAN; + if(!eskf_selected_update( + filter, ESKF_VELOCITY_INDEX, residual, covariance_mps2, + true, config->velocity_nis_gate, &nis)) + { + filter->last_velocity_nis = nis; + if(isfinite(nis) && (nis > config->velocity_nis_gate)) + { + filter->velocity_update_rejected_count++; + if(nis_out != NULL) + { + *nis_out = nis; + } + return ESKF_UPDATE_REJECTED_NIS; + } + return ESKF_UPDATE_INVALID; + } + filter->last_velocity_nis = nis; + filter->velocity_update_accepted_count++; + if(nis_out != NULL) + { + *nis_out = nis; + } + return ESKF_UPDATE_ACCEPTED; +} + bool eskf_reset_position( eskf_t* filter, const double position_m[3], diff --git a/soc/software/gps_system/src/fusion/fusion_engine.c b/soc/software/gps_system/src/fusion/fusion_engine.c index 3926390..02743ce 100644 --- a/soc/software/gps_system/src/fusion/fusion_engine.c +++ b/soc/software/gps_system/src/fusion/fusion_engine.c @@ -176,6 +176,10 @@ bool fusion_engine_apply_imu_quality( static bool config_valid(const fusion_engine_config_t* config) { return (config != NULL) && (config->timebase_hz != 0u) && + isfinite(config->eskf.position_nis_gate) && + (config->eskf.position_nis_gate > 0.0) && + isfinite(config->eskf.velocity_nis_gate) && + (config->eskf.velocity_nis_gate > 0.0) && isfinite(config->calibration_duration_s) && (config->calibration_duration_s > 0.0) && (config->calibration_min_samples >= 2u) && @@ -230,6 +234,18 @@ static bool position_valid(const fusion_position_measurement_t* measurement) isfinite(measurement->source_quality); } +static bool navigation_valid( + const fusion_navigation_measurement_t* measurement) +{ + if((measurement == NULL) || !position_valid(&measurement->position)) + { + return false; + } + return !measurement->velocity_valid || + (finite_values(measurement->velocity_enu_mps, 3u) && + finite_values(measurement->velocity_covariance_enu_mps2, 9u)); +} + static void calibration_reset(fusion_engine_t* engine) { memset(&engine->calibration, 0, sizeof(engine->calibration)); @@ -865,13 +881,17 @@ static eskf_update_result_t runtime_apply_position( 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) +static bool process_navigation_now( + fusion_engine_t* engine, + const fusion_navigation_measurement_t* measurement, + eskf_update_result_t* position_result_out, + double* position_nis_out, + eskf_update_result_t* velocity_result_out, + double* velocity_nis_out) { + const fusion_position_measurement_t* position = &measurement->position; if(!engine->checkpoint_valid || - (measurement->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks)) + (position->timestamp_ticks < engine->checkpoint.filter_timestamp_ticks)) { engine->diagnostics.position_stale_rejected++; return false; @@ -882,7 +902,7 @@ static bool process_position_now(fusion_engine_t* engine, 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) + if(imu->timestamp_ticks > position->timestamp_ticks) { next_imu = imu; break; @@ -894,7 +914,7 @@ static bool process_position_now(fusion_engine_t* engine, } replayed++; } - if(revised.filter_timestamp_ticks < measurement->timestamp_ticks) + if(revised.filter_timestamp_ticks < position->timestamp_ticks) { if(!revised.have_held_imu) { @@ -902,12 +922,12 @@ static bool process_position_now(fusion_engine_t* engine, return false; } fusion_imu_measurement_t endpoint = revised.held_imu; - endpoint.timestamp_ticks = measurement->timestamp_ticks; + endpoint.timestamp_ticks = position->timestamp_ticks; if((next_imu != NULL) && (next_imu->timestamp_ticks > revised.held_imu.timestamp_ticks)) { endpoint = interpolate_imu(&revised.held_imu, next_imu, - measurement->timestamp_ticks); + position->timestamp_ticks); } if(!runtime_propagate_to(engine, &revised, &endpoint)) { @@ -915,31 +935,31 @@ static bool process_position_now(fusion_engine_t* engine, return false; } } - double nis = NAN; - eskf_update_result_t result = runtime_apply_position(engine, &revised, - measurement, &nis); - if(result == ESKF_UPDATE_INVALID) + double position_nis = NAN; + eskf_update_result_t position_result = runtime_apply_position( + engine, &revised, position, &position_nis); + if(position_result == ESKF_UPDATE_INVALID) { return false; } - if(result == ESKF_UPDATE_ACCEPTED) + if(position_result == ESKF_UPDATE_ACCEPTED) { position_recovery_clear(engine); } - else if((result == ESKF_UPDATE_REJECTED_NIS) && - position_recovery_ready(engine, &revised, measurement) && - eskf_reset_position(&revised.filter, measurement->position_enu_m, - measurement->covariance_enu_m2)) + else if((position_result == ESKF_UPDATE_REJECTED_NIS) && + position_recovery_ready(engine, &revised, position) && + eskf_reset_position(&revised.filter, position->position_enu_m, + position->covariance_enu_m2)) { /* This is a deliberate position-state reset, not a Kalman update with * a disabled gate. The independent IMU-derived attitude and bias state * survive, while invalid position cross-covariances are discarded. */ - runtime_record_recovered_position(&revised, measurement); + runtime_record_recovered_position(&revised, position); engine->diagnostics.position_recovery_count++; position_recovery_clear(engine); - result = ESKF_UPDATE_ACCEPTED; + position_result = ESKF_UPDATE_ACCEPTED; } - if(result == ESKF_UPDATE_ACCEPTED) + if(position_result == ESKF_UPDATE_ACCEPTED) { engine->diagnostics.position_accepted++; } @@ -947,9 +967,34 @@ static bool process_position_now(fusion_engine_t* engine, { engine->diagnostics.position_nis_rejected++; } + + double velocity_nis = NAN; + eskf_update_result_t velocity_result = ESKF_UPDATE_INVALID; + if(measurement->velocity_valid) + { + velocity_result = eskf_velocity_update( + &revised.filter, &engine->config.eskf, + measurement->velocity_enu_mps, + measurement->velocity_covariance_enu_mps2, + &velocity_nis); + if(velocity_result == ESKF_UPDATE_ACCEPTED) + { + engine->diagnostics.velocity_accepted++; + } + else if(velocity_result == ESKF_UPDATE_REJECTED_NIS) + { + engine->diagnostics.velocity_nis_rejected++; + } + else + { + /* A malformed optional Doppler covariance must not discard a valid + * position update from the same epoch. */ + velocity_result = ESKF_UPDATE_INVALID; + } + } engine->checkpoint = revised; engine->checkpoint_valid = true; - history_drop_through(engine, measurement->timestamp_ticks); + history_drop_through(engine, position->timestamp_ticks); for(unsigned int i = 0 ; i < engine->imu_history_count ; i++) { if(!runtime_apply_imu(engine, &revised, history_at(engine, i), false)) @@ -962,19 +1007,27 @@ static bool process_position_now(fusion_engine_t* engine, engine->current = revised; engine->diagnostics.replay_count++; engine->diagnostics.replayed_imu_count += replayed; - if(result_out != NULL) + if(position_result_out != NULL) { - *result_out = result; + *position_result_out = position_result; } - if(nis_out != NULL) + if(position_nis_out != NULL) { - *nis_out = nis; + *position_nis_out = position_nis; + } + if(velocity_result_out != NULL) + { + *velocity_result_out = velocity_result; + } + if(velocity_nis_out != NULL) + { + *velocity_nis_out = velocity_nis; } return true; } static bool pending_insert(fusion_engine_t* engine, - const fusion_position_measurement_t* measurement) + const fusion_navigation_measurement_t* measurement) { if(engine->pending_position_count >= FUSION_POSITION_PENDING_CAPACITY) { @@ -983,8 +1036,8 @@ static bool pending_insert(fusion_engine_t* engine, } unsigned int index = engine->pending_position_count; while((index != 0u) && - (engine->pending_positions[index - 1u].timestamp_ticks > - measurement->timestamp_ticks)) + (engine->pending_positions[index - 1u].position.timestamp_ticks > + measurement->position.timestamp_ticks)) { engine->pending_positions[index] = engine->pending_positions[index - 1u]; index--; @@ -997,16 +1050,18 @@ static bool pending_insert(fusion_engine_t* engine, static void process_ready_pending_positions(fusion_engine_t* engine) { while((engine->pending_position_count != 0u) && - (engine->pending_positions[0].timestamp_ticks <= + (engine->pending_positions[0].position.timestamp_ticks <= engine->current.filter_timestamp_ticks)) { - fusion_position_measurement_t measurement = engine->pending_positions[0]; + fusion_navigation_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); + (void)process_navigation_now( + engine, &measurement, NULL, NULL, NULL, NULL); if(engine->state == FUSION_ENGINE_FAULT) { return; @@ -1056,6 +1111,7 @@ bool fusion_engine_push_position( eskf_update_result_t* update_result_out, double* nis_out) { + fusion_navigation_measurement_t navigation; if(update_result_out != NULL) { *update_result_out = ESKF_UPDATE_INVALID; @@ -1064,28 +1120,70 @@ bool fusion_engine_push_position( { *nis_out = NAN; } + if(measurement == NULL) + { + return false; + } + memset(&navigation, 0, sizeof(navigation)); + navigation.position = *measurement; + return fusion_engine_push_navigation( + engine, &navigation, update_result_out, nis_out, NULL, NULL); +} + +bool fusion_engine_push_navigation( + fusion_engine_t* engine, + const fusion_navigation_measurement_t* measurement, + eskf_update_result_t* position_update_result_out, + double* position_nis_out, + eskf_update_result_t* velocity_update_result_out, + double* velocity_nis_out) +{ + if(position_update_result_out != NULL) + { + *position_update_result_out = ESKF_UPDATE_INVALID; + } + if(position_nis_out != NULL) + { + *position_nis_out = NAN; + } + if(velocity_update_result_out != NULL) + { + *velocity_update_result_out = ESKF_UPDATE_INVALID; + } + if(velocity_nis_out != NULL) + { + *velocity_nis_out = NAN; + } if((engine == NULL) || (engine->state == FUSION_ENGINE_FAULT) || - !position_valid(measurement)) + !navigation_valid(measurement)) { return false; } engine->diagnostics.position_received++; - if(!measurement->source_valid) + if(measurement->velocity_valid) + { + engine->diagnostics.velocity_received++; + } + if(!measurement->position.source_valid) { engine->diagnostics.position_source_rejected++; return false; } if(engine->state == FUSION_ENGINE_CALIBRATING) { - engine->initial_position = *measurement; + engine->initial_position = measurement->position; engine->have_initial_position = true; return true; } - if(measurement->timestamp_ticks > engine->current.filter_timestamp_ticks) + if(measurement->position.timestamp_ticks > + engine->current.filter_timestamp_ticks) { return pending_insert(engine, measurement); } - return process_position_now(engine, measurement, update_result_out, nis_out); + return process_navigation_now( + engine, measurement, + position_update_result_out, position_nis_out, + velocity_update_result_out, velocity_nis_out); } bool fusion_engine_get_runtime(const fusion_engine_t* engine, diff --git a/soc/software/gps_system/src/fusion/fusion_gps_adapter.c b/soc/software/gps_system/src/fusion/fusion_gps_adapter.c index 0e25072..0410867 100644 --- a/soc/software/gps_system/src/fusion/fusion_gps_adapter.c +++ b/soc/software/gps_system/src/fusion/fusion_gps_adapter.c @@ -80,6 +80,14 @@ void fusion_gps_adapter_default_config(fusion_gps_adapter_config_t* config) config->minimum_horizontal_std_m = 0.50; config->minimum_vertical_std_m = 1.00; config->maximum_position_std_m = 1000.0; + /* + * Board data shows several m/s stationary speed error with weak + * four/five-satellite geometry even after Doppler IIR smoothing. These + * floors keep repeated, correlated velocity fixes useful but conservative. + */ + config->minimum_horizontal_velocity_std_mps = 2.0; + config->minimum_vertical_velocity_std_mps = 3.0; + config->maximum_velocity_std_mps = 100.0; } bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter, @@ -88,7 +96,11 @@ bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter, 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)) + !(config->maximum_position_std_m >= config->minimum_vertical_std_m) || + !(config->minimum_horizontal_velocity_std_mps > 0.0) || + !(config->minimum_vertical_velocity_std_mps > 0.0) || + !(config->maximum_velocity_std_mps >= + config->minimum_vertical_velocity_std_mps)) { return false; } @@ -97,19 +109,19 @@ bool fusion_gps_adapter_init(fusion_gps_adapter_t* adapter, return true; } -static bool sanitize_covariance(const fusion_gps_adapter_t* adapter, - const double input[3][3], double output[9]) +static bool sanitize_covariance( + double input[3][3], + double horizontal_std_floor, + double vertical_std_floor, + double maximum_std, + 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 + horizontal_std_floor * horizontal_std_floor, + horizontal_std_floor * horizontal_std_floor, + vertical_std_floor * vertical_std_floor }; - double variance_ceiling = adapter->config.maximum_position_std_m * - adapter->config.maximum_position_std_m; + double variance_ceiling = maximum_std * maximum_std; for(unsigned int axis = 0 ; axis < 3u ; axis++) { double variance = input[axis][axis]; @@ -124,7 +136,8 @@ static bool sanitize_covariance(const fusion_gps_adapter_t* adapter, { for(unsigned int col = row + 1u ; col < 3u ; col++) { - double covariance = 0.5 * (input[row][col] + input[col][row]); + double covariance = 0.5 * + (input[row][col] + input[col][row]); if(!isfinite(covariance)) { return false; @@ -146,23 +159,32 @@ static bool sanitize_covariance(const fusion_gps_adapter_t* adapter, (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) +bool fusion_gps_adapter_convert_navigation( + fusion_gps_adapter_t* adapter, + const fusion_gps_measurement_t* gps, + fusion_navigation_measurement_t* navigation_out) { - if((adapter == NULL) || (gps == NULL) || (position_out == NULL) || + if((adapter == NULL) || (gps == NULL) || (navigation_out == NULL) || !llh_valid(gps->latitude_deg, gps->longitude_deg, gps->height_m)) { return false; } - memset(position_out, 0, sizeof(*position_out)); + memset(navigation_out, 0, sizeof(*navigation_out)); + fusion_position_measurement_t* position_out = &navigation_out->position; 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)) + double position_covariance_enu_m2[3][3]; + memcpy(position_covariance_enu_m2, gps->covariance_enu_m2, + sizeof(position_covariance_enu_m2)); + if(!sanitize_covariance( + position_covariance_enu_m2, + adapter->config.minimum_horizontal_std_m, + adapter->config.minimum_vertical_std_m, + adapter->config.maximum_position_std_m, + position_out->covariance_enu_m2)) { return false; } @@ -189,9 +211,73 @@ bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter, 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]); + if(!isfinite(position_out->position_enu_m[0]) || + !isfinite(position_out->position_enu_m[1]) || + !isfinite(position_out->position_enu_m[2])) + { + return false; + } + + if(gps->solution_valid && gps->velocity_valid) + { + bool velocity_finite = true; + for(unsigned int row = 0u; row < 3u; row++) + { + for(unsigned int column = 0u; column < 3u; column++) + { + navigation_out->velocity_enu_mps[row] += + adapter->ecef_to_enu[row][column] * + gps->velocity_ecef_mps[column]; + } + velocity_finite = + velocity_finite && + isfinite(navigation_out->velocity_enu_mps[row]); + } + + double covariance_enu_mps2[3][3] = {{0.0}}; + for(unsigned int row = 0u; row < 3u; row++) + { + for(unsigned int column = 0u; column < 3u; column++) + { + for(unsigned int first = 0u; first < 3u; first++) + { + for(unsigned int second = 0u; second < 3u; second++) + { + covariance_enu_mps2[row][column] += + adapter->ecef_to_enu[row][first] * + gps->velocity_covariance_ecef_mps2[first][second] * + adapter->ecef_to_enu[column][second]; + } + } + } + } + navigation_out->velocity_valid = + velocity_finite && + sanitize_covariance( + covariance_enu_mps2, + adapter->config.minimum_horizontal_velocity_std_mps, + adapter->config.minimum_vertical_velocity_std_mps, + adapter->config.maximum_velocity_std_mps, + navigation_out->velocity_covariance_enu_mps2); + } + return true; +} + +bool fusion_gps_adapter_convert(fusion_gps_adapter_t* adapter, + const fusion_gps_measurement_t* gps, + fusion_position_measurement_t* position_out) +{ + if(position_out == NULL) + { + return false; + } + fusion_navigation_measurement_t navigation; + if(!fusion_gps_adapter_convert_navigation(adapter, gps, &navigation)) + { + return false; + } + *position_out = navigation.position; + return true; } bool fusion_gps_adapter_local_to_geodetic( diff --git a/soc/software/gps_system/src/fusion/navigation_fusion.c b/soc/software/gps_system/src/fusion/navigation_fusion.c index f5c5abf..a1b53fd 100644 --- a/soc/software/gps_system/src/fusion/navigation_fusion.c +++ b/soc/software/gps_system/src/fusion/navigation_fusion.c @@ -90,12 +90,14 @@ bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement) { return false; } - fusion_position_measurement_t position; - if(!fusion_gps_adapter_convert(&gps_adapter, measurement, &position)) + fusion_navigation_measurement_t navigation; + if(!fusion_gps_adapter_convert_navigation( + &gps_adapter, measurement, &navigation)) { return false; } - return fusion_engine_push_position(&fusion_engine, &position, NULL, NULL); + return fusion_engine_push_navigation( + &fusion_engine, &navigation, NULL, NULL, NULL, NULL); } bool navigation_fusion_get_output(navigation_fusion_output_t* output) @@ -111,6 +113,10 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output) output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected; output->gps_position_recoveries = fusion_engine.diagnostics.position_recovery_count; + output->gps_velocity_accepted = + fusion_engine.diagnostics.velocity_accepted; + output->gps_velocity_nis_rejected = + fusion_engine.diagnostics.velocity_nis_rejected; output->zero_update_count = fusion_engine.diagnostics.zero_velocity_update_count; output->replay_count = fusion_engine.diagnostics.replay_count; @@ -136,6 +142,7 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output) } output->speed_mps = eskf_speed_mps(&runtime->filter); output->last_position_nis = runtime->filter.last_position_nis; + output->last_velocity_nis = runtime->filter.last_velocity_nis; output->motion_state = runtime->motion.state; output->vehicle_at_rest = runtime->vehicle_at_rest; output->local_valid = true; diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index fdf3175..d903315 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -945,6 +945,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris, velocity_measurements, satellite_count, solution.velocity_ecef_mps, + solution.velocity_covariance_ecef_mps2, &solution.clock_drift_mps, &solution.velocity_residual_rms_mps); if(solution.velocity_valid) diff --git a/soc/software/gps_system/src/gps/gps_tracking_measurement.c b/soc/software/gps_system/src/gps/gps_tracking_measurement.c index dde57fe..16ed743 100644 --- a/soc/software/gps_system/src/gps/gps_tracking_measurement.c +++ b/soc/software/gps_system/src/gps/gps_tracking_measurement.c @@ -232,6 +232,35 @@ static bool solve_four_by_four( return true; } +static bool invert_four_by_four(double input[4][4], double inverse[4][4]) +{ + for(unsigned int column = 0u; column < 4u; column++) + { + double unit[4] = {0.0, 0.0, 0.0, 0.0}; + double solution[4]; + unit[column] = 1.0; + if(!solve_four_by_four(input, unit, solution)) + { + return false; + } + for(unsigned int row = 0u; row < 4u; row++) + { + inverse[row][column] = solution[row]; + } + } + for(unsigned int row = 0u; row < 4u; row++) + { + for(unsigned int column = row + 1u; column < 4u; column++) + { + double symmetric = + 0.5 * (inverse[row][column] + inverse[column][row]); + inverse[row][column] = symmetric; + inverse[column][row] = symmetric; + } + } + return true; +} + bool gps_tracking_velocity_wls( const double receiver_ecef_m[3], const double satellite_positions_m[][3], @@ -240,6 +269,7 @@ bool gps_tracking_velocity_wls( const gps_tracking_measurement_t measurements[], unsigned int count, double receiver_velocity_ecef_mps_out[3], + double receiver_velocity_covariance_ecef_mps2_out[3][3], double* receiver_clock_drift_mps_out, double* residual_rms_mps_out) { @@ -248,6 +278,7 @@ bool gps_tracking_velocity_wls( (satellite_clock_drifts_sps == NULL) || (measurements == NULL) || (receiver_velocity_ecef_mps_out == NULL) || + (receiver_velocity_covariance_ecef_mps2_out == NULL) || (receiver_clock_drift_mps_out == NULL) || (residual_rms_mps_out == NULL) || (count < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) || @@ -260,6 +291,7 @@ bool gps_tracking_velocity_wls( double rhs[4] = {0.0}; double design[GPS_TRACK_MEASUREMENT_MAX_SATELLITES][4]; double observation[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; + double weights[GPS_TRACK_MEASUREMENT_MAX_SATELLITES]; unsigned int used = 0u; for(unsigned int satellite = 0u; satellite < count; satellite++) @@ -309,6 +341,7 @@ bool gps_tracking_velocity_wls( 30.0 * pow(10.0, -measurement->cn0_dbhz / 20.0); sigma_rate_mps = clamp_double(sigma_rate_mps, 0.05, 5.0); double weight = 1.0 / (sigma_rate_mps * sigma_rate_mps); + weights[used] = weight; for(unsigned int row = 0u; row < 4u; row++) { rhs[row] += @@ -329,12 +362,15 @@ bool gps_tracking_velocity_wls( } double solution[4]; - if(!solve_four_by_four(normal, rhs, solution)) + double normal_inverse[4][4]; + if(!solve_four_by_four(normal, rhs, solution) || + !invert_four_by_four(normal, normal_inverse)) { return false; } double residual_square_sum = 0.0; + double weighted_residual_square_sum = 0.0; for(unsigned int row = 0u; row < used; row++) { double predicted = 0.0; @@ -344,8 +380,36 @@ bool gps_tracking_velocity_wls( } double residual = observation[row] - predicted; residual_square_sum += residual * residual; + weighted_residual_square_sum += + weights[row] * residual * residual; } + /* + * The C/N0 model supplies absolute range-rate sigmas. Inflate the formal + * inverse-normal covariance when the post-fit residuals are larger than + * that model predicts; never shrink it from a fortuitously quiet epoch. + * Four-satellite epochs have zero redundancy, so retain scale 1 and let + * the poor geometry appear directly in the inverse normal matrix. + */ + double covariance_scale = 1.0; + if(used > 4u) + { + covariance_scale = fmax( + 1.0, weighted_residual_square_sum / (double)(used - 4u)); + } + for(unsigned int row = 0u; row < 3u; row++) + { + for(unsigned int column = 0u; column < 3u; column++) + { + receiver_velocity_covariance_ecef_mps2_out[row][column] = + covariance_scale * normal_inverse[row][column]; + if(!isfinite( + receiver_velocity_covariance_ecef_mps2_out[row][column])) + { + return false; + } + } + } memcpy(receiver_velocity_ecef_mps_out, solution, 3u * sizeof(solution[0])); *receiver_clock_drift_mps_out = solution[3]; diff --git a/soc/software/gps_system/tests/test_gps_tracking_measurement.c b/soc/software/gps_system/tests/test_gps_tracking_measurement.c index dc7739c..9611f26 100644 --- a/soc/software/gps_system/tests/test_gps_tracking_measurement.c +++ b/soc/software/gps_system/tests/test_gps_tracking_measurement.c @@ -143,6 +143,7 @@ static void test_velocity_wls(void) } double solved_velocity[3]; + double solved_velocity_covariance[3][3]; double solved_clock_drift; double residual_rms; assert(gps_tracking_velocity_wls( @@ -153,6 +154,7 @@ static void test_velocity_wls(void) measurements, TEST_SATELLITES, solved_velocity, + solved_velocity_covariance, &solved_clock_drift, &residual_rms)); @@ -168,6 +170,16 @@ static void test_velocity_wls(void) expected_clock_drift_mps, 1.0e-9)); assert(residual_rms < 1.0e-9); + for(unsigned int component = 0u; component < 3u; component++) + { + assert(isfinite( + solved_velocity_covariance[component][component])); + assert(solved_velocity_covariance[component][component] > 0.0); + } + assert(close_to( + solved_velocity_covariance[0][1], + solved_velocity_covariance[1][0], + 1.0e-12)); } int main(void)