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

fix(fusion): harden ESKF GPS failure handling

Browse Source
This commit is contained in:
onur
2026-07-24 12:33:26 +03:00
parent 93766b007f
commit ea055a36d2
7 changed files with 661 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/README.md
+2 -1
View File
@@ -165,7 +165,8 @@ uzakta kalirsa, yalniz hard NIS rejection kullanmak filtreyi yanlis baslangica
kilitler. Fusion engine bu nedenle ardisik reddedilen position fix'lerinin
zamansal surekliligini izler. Son başarılı fusion'dan sonra yedi saniye timeout
dolmuşsa ve en az uc fix birbirinin devamı
ise yalniz ESKF konumu ve konum kovaryansi son olcume yeniden ankrajlanir;
ise ve platform IMU/GNSS hareket vetolariyla duragan kabul ediliyorsa yalniz
ESKF konumu ve konum kovaryansi son olcume yeniden ankrajlanir;
attitude, hiz ve IMU bias state'leri korunur, gecersiz hale gelen position
cross-covariance'lari sifirlanir. Tek aykiri fix bu yolu tetiklemez. Bu davranis
100/200/1000 ms GPS oranlarinda `gps_reacquisition_test.c` ile dogrulanir ve
soc/software/gps_system/src/fusion/eskf.c
+80 -117
View File
@@ -390,11 +390,16 @@ static bool eskf_propagate_step(eskf_t* filter,
return false;
}
double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int i = 0 ; i < 3u ; i++)
{
system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0;
transition[i][i] = 1.0;
transition[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
transition[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] = 1.0;
transition[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] = 1.0;
transition[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = 1.0;
transition[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = dt_s;
transition[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -dt_s;
}
double force_skew[3][3];
double rate_skew[3][3];
@@ -409,27 +414,16 @@ static bool eskf_propagate_step(eskf_t* filter,
{
rotated_skew += rotation[row][k] * force_skew[k][col];
}
system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
-rotated_skew;
system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
-rotation[row][col];
system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
-rate_skew[row][col];
transition[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] +=
-rotated_skew * dt_s;
transition[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
-rotation[row][col] * dt_s;
transition[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] +=
-rate_skew[row][col] * dt_s;
}
}
double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
transition[row][col] = system[row][col] * dt_s;
}
transition[row][row] += 1.0;
}
double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
@@ -448,21 +442,18 @@ static bool eskf_propagate_step(eskf_t* filter,
}
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
double value = intermediate[row][k];
if(value == 0.0)
{
continue;
}
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
double value = 0.0;
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
{
double coefficient = transition[col][k];
if(coefficient != 0.0)
{
propagated[row][col] += value * coefficient;
value += intermediate[row][k] * coefficient;
}
}
filter->covariance[row][col] = value;
}
}
@@ -477,17 +468,20 @@ static bool eskf_propagate_step(eskf_t* filter,
square(config->accel_noise_density[axis]) *
rotation[col][axis];
}
propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] +=
filter->covariance[ESKF_VELOCITY_INDEX + row]
[ESKF_VELOCITY_INDEX + col] +=
accel_noise * dt_s;
}
propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] +=
filter->covariance[ESKF_ATTITUDE_INDEX + row]
[ESKF_ATTITUDE_INDEX + row] +=
square(config->gyro_noise_density[row]) * dt_s;
propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] +=
filter->covariance[ESKF_ACCEL_BIAS_INDEX + row]
[ESKF_ACCEL_BIAS_INDEX + row] +=
square(config->accel_bias_random_walk[row]) * dt_s;
propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] +=
filter->covariance[ESKF_GYRO_BIAS_INDEX + row]
[ESKF_GYRO_BIAS_INDEX + row] +=
square(config->gyro_bias_random_walk[row]) * dt_s;
}
memcpy(filter->covariance, propagated, sizeof(propagated));
covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
filter->propagation_count++;
return eskf_state_is_finite(filter);
@@ -591,10 +585,10 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
}
/* Right attitude-error reset after q <- q (x) Exp(delta_theta). */
double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
double attitude_reset[3][3] = {{0.0}};
for(unsigned int i = 0 ; i < 3u ; i++)
{
reset[i][i] = 1.0;
attitude_reset[i][i] = 1.0;
}
double attitude_skew[3][3];
skew_matrix(attitude_error, attitude_skew);
@@ -602,47 +596,48 @@ static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STAT
{
for(unsigned int col = 0 ; col < 3u ; col++)
{
reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -=
0.5 * attitude_skew[row][col];
attitude_reset[row][col] -= 0.5 * attitude_skew[row][col];
}
}
double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
/* G differs from identity only in the 3x3 attitude block. Apply G*P
* using three temporary rows, then apply G^T using three scalar columns.
* This keeps the Joseph/reset path below the bare-metal stack budget. */
double attitude_rows[3][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int row = 0u ; row < 3u ; row++)
{
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
{
double coefficient = reset[row][k];
if(coefficient == 0.0)
for(unsigned int k = 0u ; k < 3u ; k++)
{
continue;
}
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
intermediate[row][col] += coefficient * filter->covariance[k][col];
attitude_rows[row][col] += attitude_reset[row][k] *
filter->covariance[ESKF_ATTITUDE_INDEX + k][col];
}
}
}
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
for(unsigned int row = 0u ; row < 3u ; row++)
{
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
memcpy(filter->covariance[ESKF_ATTITUDE_INDEX + row],
attitude_rows[row], sizeof(attitude_rows[row]));
}
for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
{
double attitude_columns[3] = {0.0, 0.0, 0.0};
for(unsigned int col = 0u ; col < 3u ; col++)
{
double value = intermediate[row][k];
if(value == 0.0)
for(unsigned int k = 0u ; k < 3u ; k++)
{
continue;
}
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
double coefficient = reset[col][k];
if(coefficient != 0.0)
{
reset_covariance[row][col] += value * coefficient;
}
attitude_columns[col] +=
filter->covariance[row][ESKF_ATTITUDE_INDEX + k] *
attitude_reset[col][k];
}
}
for(unsigned int col = 0u ; col < 3u ; col++)
{
filter->covariance[row][ESKF_ATTITUDE_INDEX + col] =
attitude_columns[col];
}
}
memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance));
return true;
}
@@ -719,76 +714,44 @@ static bool eskf_selected_update(eskf_t* filter,
}
}
double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
joseph_left[row][row] = 1.0;
for(unsigned int col = 0 ; col < 3u ; col++)
{
joseph_left[row][selected_state_index + col] -= gain[row][col];
}
}
/* Joseph form with A = I-KH. H selects three direct state components, so
* A*P can be formed without materializing a second 15x15 matrix. */
double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
double coefficient = joseph_left[row][k];
if(coefficient == 0.0)
double value = filter->covariance[row][col];
for(unsigned int selected = 0u ; selected < 3u ; selected++)
{
continue;
}
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
intermediate[row][col] +=
coefficient * filter->covariance[k][col];
}
}
}
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
{
double value = intermediate[row][k];
if(value == 0.0)
{
continue;
}
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
double coefficient = joseph_left[col][k];
if(coefficient != 0.0)
{
updated_covariance[row][col] += value * coefficient;
}
}
}
}
double gain_times_covariance[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int second = 0 ; second < 3u ; second++)
{
for(unsigned int first = 0 ; first < 3u ; first++)
{
gain_times_covariance[row][second] += gain[row][first] *
measurement_covariance[first * 3u + second];
value -= gain[row][selected] *
filter->covariance[selected_state_index + selected][col];
}
intermediate[row][col] = value;
}
}
for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
{
for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
{
for(unsigned int second = 0 ; second < 3u ; second++)
double value = intermediate[row][col];
for(unsigned int selected = 0u ; selected < 3u ; selected++)
{
updated_covariance[row][col] +=
gain_times_covariance[row][second] * gain[col][second];
value -= intermediate[row][selected_state_index + selected] *
gain[col][selected];
}
for(unsigned int first = 0u ; first < 3u ; first++)
{
for(unsigned int second = 0u ; second < 3u ; second++)
{
value += gain[row][first] *
measurement_covariance[first * 3u + second] *
gain[col][second];
}
}
filter->covariance[row][col] = value;
}
}
memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance));
if(!eskf_inject_error(filter, error))
{
return false;
soc/software/gps_system/src/fusion/fusion_engine.c
+75 -23
View File
@@ -136,8 +136,8 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
config->gnss_moving_confirmations = 2u;
config->gnss_still_confirmations = 2u;
/* A hard innovation gate protects against isolated GNSS outliers, but it
* must not permanently lock the filter to a bad startup position. After a
* full second of mutually continuous rejected fixes, re-anchor position
* must not permanently lock a stationary filter to a bad startup position.
* After a timeout of mutually continuous rejected fixes, re-anchor position
* while preserving inertial velocity, attitude and bias states. */
config->position_recovery_timeout_s = 7.0;
config->position_recovery_max_speed_mps = 100.0;
@@ -301,6 +301,31 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
const fusion_imu_measurement_t* measurement,
bool count_zero_updates);
static bool runtime_allows_zero_velocity_update(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
(void)engine;
return runtime->vehicle_at_rest &&
(runtime->gnss_evidence != FUSION_GNSS_MOTION_MOVING);
}
static bool runtime_allows_zero_gyro_update(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
return runtime_allows_zero_velocity_update(engine, runtime) &&
(runtime->motion.last_gyro_norm_rps <=
engine->config.motion.stationary_gyro_limit_rps);
}
static bool runtime_allows_position_recovery(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
return runtime_allows_zero_gyro_update(engine, runtime);
}
static void history_push(fusion_engine_t* engine,
const fusion_imu_measurement_t* measurement)
{
@@ -415,9 +440,12 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
runtime->gnss_still_count = 0u;
}
runtime->vehicle_at_rest = measurement->vehicle_at_rest;
bool at_rest_continuing = measurement->vehicle_at_rest &&
start_measurement.vehicle_at_rest;
if(measurement->vehicle_at_rest)
bool zero_velocity_allowed =
runtime_allows_zero_velocity_update(engine, runtime);
bool zero_gyro_allowed =
runtime_allows_zero_gyro_update(engine, runtime);
bool at_rest_continuing = start_measurement.vehicle_at_rest;
if(zero_gyro_allowed && at_rest_continuing)
{
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
@@ -435,36 +463,51 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
}
uint64_t zero_period_ticks =
seconds_to_ticks(engine, engine->config.zero_update_period_s);
if(at_rest_continuing &&
if(at_rest_continuing && (zero_velocity_allowed || zero_gyro_allowed) &&
((measurement->timestamp_ticks - runtime->last_zero_update_ticks) >=
zero_period_ticks))
{
double mean_gyro_rps[3];
if(!(runtime->zero_gyro_duration_s > 0.0))
bool update_applied = false;
if(zero_velocity_allowed)
{
return false;
if(!eskf_zero_velocity_update(
&runtime->filter, engine->config.zero_velocity_std_mps))
{
return false;
}
update_applied = true;
if(count_zero_updates)
{
engine->diagnostics.zero_velocity_update_count++;
}
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
if(zero_gyro_allowed && (runtime->zero_gyro_duration_s > 0.0))
{
mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
runtime->zero_gyro_duration_s;
double mean_gyro_rps[3];
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
mean_gyro_rps[axis] = runtime->zero_gyro_integral_rad[axis] /
runtime->zero_gyro_duration_s;
}
if(!eskf_zero_angular_rate_update(
&runtime->filter, mean_gyro_rps,
engine->config.zero_gyro_std_rps))
{
return false;
}
update_applied = true;
if(count_zero_updates)
{
engine->diagnostics.zero_gyro_update_count++;
}
}
if(!eskf_zero_velocity_update(
&runtime->filter, engine->config.zero_velocity_std_mps) ||
!eskf_zero_angular_rate_update(
&runtime->filter, mean_gyro_rps, engine->config.zero_gyro_std_rps))
if(update_applied)
{
return false;
runtime->last_zero_update_ticks = measurement->timestamp_ticks;
}
memset(runtime->zero_gyro_integral_rad, 0,
sizeof(runtime->zero_gyro_integral_rad));
runtime->zero_gyro_duration_s = 0.0;
runtime->last_zero_update_ticks = measurement->timestamp_ticks;
if(count_zero_updates)
{
engine->diagnostics.zero_velocity_update_count++;
engine->diagnostics.zero_gyro_update_count++;
}
}
return true;
}
@@ -706,6 +749,15 @@ static bool position_recovery_ready(
const fusion_runtime_t* runtime,
const fusion_position_measurement_t* measurement)
{
/* Position-only GNSS cannot distinguish a coherent receiver restart error
* or spoofed track from a true position discontinuity while moving. Keep
* automatic re-anchoring as a stationary startup/recovery mechanism; a
* mobile reset needs independent velocity/yaw aiding or an external command. */
if(!runtime_allows_position_recovery(engine, runtime))
{
position_recovery_clear(engine);
return false;
}
if(!engine->position_recovery_active)
{
position_recovery_begin(engine, measurement);