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();