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