onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(fusion): fuse Doppler velocity in ESKF

Browse Source
This commit is contained in:
onur
2026-07-25 13:11:33 +03:00
parent 2da5fb7153
commit 971be036a0
19 changed files with 647 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/fusion/README.md
+31 -14
View File
@@ -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
soc/software/fusion/tests/eskf_motion_test.c
+143 -13
View File
@@ -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();
soc/software/gps_system/README.md
+34 -12
View File
@@ -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
soc/software/gps_system/header/fusion/eskf.h
+11
View File
@@ -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
soc/software/gps_system/header/fusion/fusion_engine.h
+11 -1
View File
@@ -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);
soc/software/gps_system/header/fusion/fusion_gps_adapter.h
+7
View File
@@ -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],
soc/software/gps_system/header/fusion/fusion_measurement.h
+13
View File
@@ -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
soc/software/gps_system/header/fusion/navigation_fusion.h
+6 -2
View File
@@ -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. */
soc/software/gps_system/header/gps/NavEquations.h
+1
View File
@@ -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;
soc/software/gps_system/header/gps/gps_tracking_measurement.h
+1
View File
@@ -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);
soc/software/gps_system/src/adapters/gps_fusion_publisher.c
+3
View File
@@ -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;
soc/software/gps_system/src/adapters/gps_system_uart.c
+5 -1
View File
@@ -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,
soc/software/gps_system/src/fusion/eskf.c
+50
View File
@@ -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],
soc/software/gps_system/src/fusion/fusion_engine.c
+135 -37
View File
@@ -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,
soc/software/gps_system/src/fusion/fusion_gps_adapter.c
+108 -22
View File
@@ -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(
soc/software/gps_system/src/fusion/navigation_fusion.c
+10 -3
View File
@@ -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;
soc/software/gps_system/src/gps/gps_ip.c
+1
View File
@@ -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)
soc/software/gps_system/src/gps/gps_tracking_measurement.c
+65 -1
View File
@@ -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];
soc/software/gps_system/tests/test_gps_tracking_measurement.c
+12
View File
@@ -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)