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