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

fix(fusion): break the zero-velocity/innovation-gate deadlock

Browse Source 
A cruising vehicle produces almost no specific force and almost no
angular rate, so an inertial rest supervisor declares rest while moving.
The engine trusted that flag, and three circular dependencies then locked
the filter out of GNSS entirely:

  - GNSS motion evidence was derived only from accepted fixes, so it
    froze exactly when a rejection storm made it the one signal able to
    veto a false zero-velocity update.
  - The zero-velocity update pinned filter velocity near zero, which kept
    eskf_speed_mps() below the rest-detector speed limit and latched the
    mistake, and clamped the position covariance to metres while the true
    error grew to hundreds.
  - Position recovery, the intended escape, was gated on being at rest --
    unreachable during the moving divergence that needs it.

Field logs show the result: 91 m from GNSS with a 5 m sigma, every fix
failing the NIS gate, gps_age_ms climbing past 40 s and recoveries=0.

Motion evidence now comes from every fix on its own displacement
baseline; accepted Doppler speed vetoes the zero-velocity update and is
taken from the measurement rather than the update outcome, since a filter
that has drifted far enough to reject it is the case that matters; and
that veto latches until a later fix shows low speed, because an unaided
platform gives no evidence that it has stopped.

Startup calibration is fixed alongside it. One tolerance served both as
the sensor zero-g offset allowance and as the motion gate, so the bench
part's in-spec +28 mg offset consumed 93% of it and left no headroom:
calibration_samples thrashed 0,3,0,4,2,1 and never reached the minimum,
so the engine never left CALIBRATING and no fused location was published
at all. The offset allowance is now applied to the calibration window
mean, where a fixed bias actually shows up, leaving the full tolerance to
reject sustained acceleration.

Rest-detector thresholds move into the per-sensor quality profile and are
sized from a stationary bench capture rather than datasheet noise
densities, which describe the bare part and not this board or bandwidth.

Verified on hardware: calibration completes in ~2 s, 1980 fixes accepted
with 1 rejection, sub-metre sigma, gps_age_ms mean 223 ms.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
onur cayan
2026-08-07 22:05:40 +03:00
committed by onur
parent b4920d31e3
commit 8349f32df1
4 changed files with 391 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/header/fusion/fusion_engine.h
+24
View File
@@ -30,6 +30,7 @@ typedef struct {
double calibration_duration_s;
unsigned int calibration_min_samples;
double calibration_accel_norm_tolerance_mps2;
double calibration_accel_offset_allowance_mps2;
double calibration_gyro_limit_rps;
double calibration_accel_std_limit_mps2;
double calibration_gyro_std_limit_rps;
@@ -44,6 +45,7 @@ typedef struct {
double position_recovery_timeout_s;
double position_recovery_max_speed_mps;
double position_recovery_sigma_multiplier;
double gnss_velocity_motion_timeout_s;
unsigned int position_recovery_min_samples;
} fusion_engine_config_t;
@@ -54,16 +56,33 @@ typedef struct {
fusion_gnss_motion_evidence_t gnss_evidence;
double last_accepted_position_m[3];
double last_accepted_position_covariance_m2[9];
/*
* Displacement baseline for GNSS motion evidence. Held separately from
* the accepted-fix baseline because the evidence must keep updating while
* the innovation gate is rejecting fixes -- that is precisely when it is
* the only signal able to release the zero-velocity update.
*/
double last_evidence_position_m[3];
double last_evidence_position_covariance_m2[9];
double last_gnss_speed_mps;
double zero_gyro_integral_rad[3];
double zero_gyro_duration_s;
uint64_t filter_timestamp_ticks;
uint64_t last_zero_update_ticks;
uint64_t last_accepted_position_ticks;
uint64_t last_evidence_position_ticks;
uint64_t last_gnss_velocity_ticks;
uint64_t last_position_fuse_ticks;
unsigned int gnss_moving_count;
unsigned int gnss_still_count;
bool have_held_imu;
bool have_accepted_position;
bool have_evidence_position;
bool have_gnss_velocity;
/* Latched once GNSS Doppler has shown the platform moving, and cleared
* only by a later fix showing it slow. A timeout cannot clear it: an
* unaided platform gives no evidence that it has stopped. */
bool gnss_confirmed_moving;
bool vehicle_at_rest;
} fusion_runtime_t;
@@ -96,6 +115,11 @@ typedef struct {
double gyro_mean[3];
double accel_m2[3];
double gyro_m2[3];
/* Running statistics of the accelerometer magnitude. Comparing a sample
* against the window's own mean cancels any fixed sensor offset, which
* leaves the whole norm tolerance available to detect real acceleration. */
double norm_mean;
double norm_m2;
unsigned int sample_count;
bool have_start;
} fusion_calibration_accumulator_t;
soc/software/gps_system/header/fusion/fusion_imu_quality.h
+22
View File
@@ -22,7 +22,29 @@ typedef struct {
double gyro_bias_random_walk_rps_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT];
double calibration_accel_std_limit_mps2;
double calibration_gyro_std_limit_rps;
/*
* How far the stationary accelerometer magnitude may sit from gravity
* purely because of the part's zero-g offset and sensitivity tolerance.
* This is a fixed per-unit property, not motion headroom:
* finish_calibration() estimates it as accel bias and hands it to the
* ESKF, so admitting it here costs nothing. Sized from the datasheet
* zero-g offset; a unit near this limit is worth characterizing.
*/
double calibration_accel_offset_allowance_mps2;
double zero_gyro_measurement_std_rps[FUSION_IMU_QUALITY_AXIS_COUNT];
/*
* Rest-detector thresholds. These separate genuine platform motion from
* the sensor's own residual noise, so they scale with the part rather than
* with the vehicle: a quieter IMU sits below a fixed quiescence limit more
* of the time and therefore declares rest more readily. The speed and
* dwell thresholds stay in fusion_motion_default_config() because they
* describe the platform, not the sensor.
*/
double stationary_accel_limit_mps2;
double stationary_gyro_limit_rps;
double moving_accel_threshold_mps2;
double moving_gyro_threshold_rps;
double moving_jerk_threshold_mps3;
} fusion_imu_quality_profile_t;
void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile);
soc/software/gps_system/src/adapters/imu_fusion_profile.c
+53
View File
@@ -44,8 +44,50 @@ bool imu_fusion_quality_profile_for_backend(
profile_out->calibration_accel_std_limit_mps2 = 0.09;
profile_out->calibration_gyro_std_limit_rps =
0.10 * PROFILE_DEGREES_TO_RADIANS;
/*
* ICM-42688-P zero-g offset is specified at +-40 mg (0.392 m/s^2).
* The bench unit measured 0.278 m/s^2 (+28 mg): in spec, but it
* consumed 93% of the old combined 0.30 tolerance and left the
* motion gate no usable headroom, so startup calibration stalled.
*
* 0.42 is deliberately only just above the datasheet limit. This
* allowance also sets how much sustained acceleration calibration
* cannot distinguish from offset, so widening it further would
* trade startup robustness for a worse initial attitude.
*/
profile_out->calibration_accel_offset_allowance_mps2 = 0.42;
set_all(profile_out->zero_gyro_measurement_std_rps,
0.02 * PROFILE_DEGREES_TO_RADIANS);
/*
* Rest-detector limits from a stationary bench capture of this
* board (soc/software/tdk_noise_test, 3000 samples at the
* production 50 Hz / +-4 g / +-250 dps / UI filter 0x77), reported
* as the norms fusion_motion_update() actually gates on:
*
* accel_norm p99.9 = 0.014 m/s^2 max = 0.0154 m/s^2
* gyro_norm p99.9 = 0.040 deg/s max = 0.0417 deg/s
* jerk_norm p99.9 = 0.10 m/s^3 max = 0.0915 m/s^3
*
* Gyro and jerk sit far above their noise floors here because they
* are measured directly: 6x p99.9 still leaves room for gyro bias
* drift between ESKF updates, and a static attitude error cancels
* in the jerk derivative. Jerk is therefore the most sensitive
* motion discriminator available and is tightened hardest.
*
* The accel limit deliberately keeps far more headroom than its
* 0.014 m/s^2 noise floor would suggest. linear_accel_nav carries
* the attitude error times gravity, so a 0.1 deg tilt error alone
* contributes 0.017 m/s^2 -- more than the entire sensor noise
* budget. Sizing this limit to the noise floor would break rest
* detection whenever attitude estimation is slightly off.
*/
profile_out->stationary_accel_limit_mps2 = 0.1125;
profile_out->stationary_gyro_limit_rps =
0.25 * PROFILE_DEGREES_TO_RADIANS;
profile_out->moving_accel_threshold_mps2 = 0.30;
profile_out->moving_gyro_threshold_rps =
0.75 * PROFILE_DEGREES_TO_RADIANS;
profile_out->moving_jerk_threshold_mps3 = 0.60;
break;
case IMU_SENSOR_ST_LSM6DSOX:
@@ -63,8 +105,19 @@ bool imu_fusion_quality_profile_for_backend(
profile_out->calibration_accel_std_limit_mps2 = 0.12;
profile_out->calibration_gyro_std_limit_rps =
0.20 * PROFILE_DEGREES_TO_RADIANS;
/* LSM6DSOX zero-g offset is specified at +-40 mg over temperature. */
profile_out->calibration_accel_offset_allowance_mps2 = 0.42;
set_all(profile_out->zero_gyro_measurement_std_rps,
0.04 * PROFILE_DEGREES_TO_RADIANS);
/* Unchanged from the historical fusion_motion defaults; these are
* the field-tuned values this platform was validated against. */
profile_out->stationary_accel_limit_mps2 = 0.15;
profile_out->stationary_gyro_limit_rps =
1.00 * PROFILE_DEGREES_TO_RADIANS;
profile_out->moving_accel_threshold_mps2 = 0.40;
profile_out->moving_gyro_threshold_rps =
3.00 * PROFILE_DEGREES_TO_RADIANS;
profile_out->moving_jerk_threshold_mps3 = 4.0;
break;
default:
soc/software/gps_system/src/fusion/fusion_engine.c
+292 -56
View File
@@ -71,6 +71,16 @@ void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile)
}
profile->calibration_accel_std_limit_mps2 = 0.12;
profile->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
/* About 46 mg: covers a typical consumer MEMS zero-g offset plus its
* sensitivity tolerance without assuming a specific part. */
profile->calibration_accel_offset_allowance_mps2 = 0.42;
/* Matches fusion_motion_default_config() so an unprofiled backend keeps
* the historical rest-detector behaviour. */
profile->stationary_accel_limit_mps2 = 0.15;
profile->stationary_gyro_limit_rps = 1.0 * FUSION_PI / 180.0;
profile->moving_accel_threshold_mps2 = 0.40;
profile->moving_gyro_threshold_rps = 3.0 * FUSION_PI / 180.0;
profile->moving_jerk_threshold_mps3 = 4.0;
}
bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile)
@@ -79,10 +89,38 @@ bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile)
!isfinite(profile->calibration_accel_std_limit_mps2) ||
!(profile->calibration_accel_std_limit_mps2 > 0.0) ||
!isfinite(profile->calibration_gyro_std_limit_rps) ||
!(profile->calibration_gyro_std_limit_rps > 0.0))
!(profile->calibration_gyro_std_limit_rps > 0.0) ||
!isfinite(profile->calibration_accel_offset_allowance_mps2) ||
!(profile->calibration_accel_offset_allowance_mps2 > 0.0))
{
return false;
}
{
const double motion[] = {
profile->stationary_accel_limit_mps2,
profile->stationary_gyro_limit_rps,
profile->moving_accel_threshold_mps2,
profile->moving_gyro_threshold_rps,
profile->moving_jerk_threshold_mps3
};
for(unsigned int value = 0u ;
value < sizeof(motion) / sizeof(motion[0]) ; value++)
{
if(!isfinite(motion[value]) || !(motion[value] > 0.0))
{
return false;
}
}
/* A quiescence limit at or above the motion threshold would let the
* detector call the same sample both stationary and moving. */
if(!(profile->stationary_accel_limit_mps2 <
profile->moving_accel_threshold_mps2) ||
!(profile->stationary_gyro_limit_rps <
profile->moving_gyro_threshold_rps))
{
return false;
}
}
for(unsigned int axis = 0u ; axis < 3u ; axis++)
{
const double values[] = {
@@ -120,6 +158,9 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
config->calibration_duration_s = 2.0;
config->calibration_min_samples = 50u;
config->calibration_accel_norm_tolerance_mps2 = 0.30;
/* Fixed sensor offset, kept separate from the motion tolerance above so
* that a healthy part's zero-g offset cannot consume the motion budget. */
config->calibration_accel_offset_allowance_mps2 = 0.42;
config->calibration_gyro_limit_rps = 2.0 * FUSION_PI / 180.0;
config->calibration_accel_std_limit_mps2 = 0.12;
config->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0;
@@ -143,6 +184,15 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
config->position_recovery_max_speed_mps = 100.0;
config->position_recovery_sigma_multiplier = 5.0;
config->position_recovery_min_samples = 3u;
/*
* How long an accepted GNSS Doppler velocity keeps vetoing zero-velocity
* updates. Doppler velocity is observed independently of the position
* state, so it stays trustworthy while the position innovation gate is
* rejecting fixes. At a 200 ms solve period this tolerates several missed
* epochs yet still lapses quickly enough that a genuine stop is not held
* out of ZUPT for long.
*/
config->gnss_velocity_motion_timeout_s = 1.5;
}
bool fusion_engine_apply_imu_quality(
@@ -170,6 +220,18 @@ bool fusion_engine_apply_imu_quality(
profile->calibration_accel_std_limit_mps2;
config->calibration_gyro_std_limit_rps =
profile->calibration_gyro_std_limit_rps;
config->calibration_accel_offset_allowance_mps2 =
profile->calibration_accel_offset_allowance_mps2;
config->motion.stationary_accel_limit_mps2 =
profile->stationary_accel_limit_mps2;
config->motion.stationary_gyro_limit_rps =
profile->stationary_gyro_limit_rps;
config->motion.moving_accel_threshold_mps2 =
profile->moving_accel_threshold_mps2;
config->motion.moving_gyro_threshold_rps =
profile->moving_gyro_threshold_rps;
config->motion.moving_jerk_threshold_mps3 =
profile->moving_jerk_threshold_mps3;
return true;
}
@@ -185,6 +247,8 @@ static bool config_valid(const fusion_engine_config_t* config)
(config->calibration_min_samples >= 2u) &&
isfinite(config->calibration_accel_norm_tolerance_mps2) &&
(config->calibration_accel_norm_tolerance_mps2 > 0.0) &&
isfinite(config->calibration_accel_offset_allowance_mps2) &&
(config->calibration_accel_offset_allowance_mps2 > 0.0) &&
isfinite(config->calibration_gyro_limit_rps) &&
(config->calibration_gyro_limit_rps > 0.0) &&
isfinite(config->zero_update_period_s) &&
@@ -202,6 +266,8 @@ static bool config_valid(const fusion_engine_config_t* config)
(config->position_recovery_max_speed_mps > 0.0) &&
isfinite(config->position_recovery_sigma_multiplier) &&
(config->position_recovery_sigma_multiplier > 0.0) &&
isfinite(config->gnss_velocity_motion_timeout_s) &&
(config->gnss_velocity_motion_timeout_s > 0.0) &&
(config->position_recovery_min_samples >= 2u);
}
@@ -257,6 +323,10 @@ static void calibration_add(fusion_calibration_accumulator_t* calibration,
{
calibration->sample_count++;
double count = (double)calibration->sample_count;
double norm = norm3(measurement->accel_mps2);
double norm_delta = norm - calibration->norm_mean;
calibration->norm_mean += norm_delta / count;
calibration->norm_m2 += norm_delta * (norm - calibration->norm_mean);
for(unsigned int axis = 0 ; axis < 3u ; axis++)
{
double accel_delta = measurement->accel_mps2[axis] - calibration->accel_mean[axis];
@@ -273,9 +343,33 @@ static void calibration_add(fusion_calibration_accumulator_t* calibration,
static bool calibration_sample_is_quiet(const fusion_engine_t* engine,
const fusion_imu_measurement_t* measurement)
{
double accel_error = fabs(norm3(measurement->accel_mps2) - engine->config.eskf.gravity_mps2);
return (accel_error <= engine->config.calibration_accel_norm_tolerance_mps2) &&
(norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps);
double norm = norm3(measurement->accel_mps2);
/*
* Plausibility. A healthy part's zero-g offset shifts this magnitude
* permanently, so the allowance for it is added to -- not taken out of --
* the motion tolerance. What remains rejects a misconfigured full-scale
* range, a dead axis or a mounting fault.
*/
if(fabs(norm - engine->config.eskf.gravity_mps2) >
(engine->config.calibration_accel_offset_allowance_mps2 +
engine->config.calibration_accel_norm_tolerance_mps2))
{
return false;
}
/*
* Motion. Measuring against the window's own mean magnitude rather than
* against gravity makes this test independent of the fixed offset, so the
* full tolerance detects acceleration instead of being spent on a
* per-unit constant. A slow ramp that tracks the mean still fails the
* per-axis standard-deviation gate before calibration completes.
*/
if((engine->calibration.sample_count != 0u) &&
(fabs(norm - engine->calibration.norm_mean) >
engine->config.calibration_accel_norm_tolerance_mps2))
{
return false;
}
return norm3(measurement->gyro_rps) <= engine->config.calibration_gyro_limit_rps;
}
static bool calibration_statistics_are_quiet(const fusion_engine_t* engine)
@@ -317,11 +411,51 @@ static bool runtime_apply_imu(fusion_engine_t* engine,
const fusion_imu_measurement_t* measurement,
bool count_zero_updates);
/* Age of the most recent accepted GNSS Doppler velocity, or a negative value
* when there is none to age. */
static double gnss_velocity_age_s(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
if(!runtime->have_gnss_velocity ||
(runtime->filter_timestamp_ticks < runtime->last_gnss_velocity_ticks))
{
return -1.0;
}
return ticks_to_seconds(
engine,
runtime->filter_timestamp_ticks - runtime->last_gnss_velocity_ticks);
}
static bool gnss_velocity_is_fresh(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
double age_s = gnss_velocity_age_s(engine, runtime);
return (age_s >= 0.0) &&
(age_s <= engine->config.gnss_velocity_motion_timeout_s);
}
static bool runtime_allows_zero_velocity_update(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
(void)engine;
/*
* A cruising vehicle produces almost no specific force and almost no
* angular rate, so inertial data alone cannot distinguish constant
* velocity from rest. Doppler velocity can, and it survives a position
* innovation-gate lockout because it is observed independently of the
* position state. Without this veto a false ZUPT pins filter velocity to
* zero, which keeps eskf_speed_mps() below the rest-detector speed limit
* and latches the mistake.
*/
if(runtime->gnss_confirmed_moving ||
(gnss_velocity_is_fresh(engine, runtime) &&
(runtime->last_gnss_speed_mps >=
engine->config.gnss_motion_speed_threshold_mps)))
{
return false;
}
return runtime->vehicle_at_rest &&
(runtime->gnss_evidence != FUSION_GNSS_MOTION_MOVING);
}
@@ -339,7 +473,21 @@ static bool runtime_allows_position_recovery(
const fusion_engine_t* engine,
const fusion_runtime_t* runtime)
{
return runtime_allows_zero_gyro_update(engine, runtime);
/* Stationary re-anchoring: recovers a bad startup position. */
if(runtime_allows_zero_gyro_update(engine, runtime))
{
return true;
}
/*
* Moving re-anchoring. Gating recovery on being at rest made the escape
* hatch unreachable in exactly the case that needs it, because divergence
* large enough to lock the innovation gate happens while driving. A
* mobile reset still needs aiding that is not derived from the locked-out
* position state: accepted Doppler velocity supplies it, and
* position_recovery_is_continuous() separately establishes that the
* candidate GNSS track is self-consistent before any reset occurs.
*/
return gnss_velocity_is_fresh(engine, runtime);
}
static void history_push(fusion_engine_t* engine,
@@ -564,6 +712,25 @@ static bool finish_calibration(fusion_engine_t* engine,
calibration_reset(engine);
return false;
}
/*
* A zero-g offset is constant, so it shows up in the window mean and
* nowhere else; a sustained acceleration does too. Bounding the mean is
* therefore the only test that separates a tolerable sensor offset from
* an intolerable one, and it is what rejects calibrating under power.
*
* The two cases are not fully separable: any allowance A accepts sustained
* accelerations whose magnitude shift stays under A. Horizontal
* acceleration adds in quadrature, so A = 0.42 m/s^2 still admits roughly
* 2.9 m/s^2 of it -- the same blind spot the original tolerance had, since
* 1.5 m/s^2 shifts this magnitude by only 0.11 m/s^2. A part whose offset
* approaches A needs stored per-unit calibration rather than a wider gate.
*/
if(fabs(gravity_norm - engine->config.eskf.gravity_mps2) >
engine->config.calibration_accel_offset_allowance_mps2)
{
calibration_reset(engine);
return false;
}
double nav_up[3] = {0.0, 0.0, 1.0};
double quaternion[4];
if(!eskf_quaternion_align_vectors(calibration->accel_mean, nav_up, quaternion))
@@ -611,6 +778,15 @@ static bool finish_calibration(fusion_engine_t* engine,
memcpy(engine->current.last_accepted_position_covariance_m2,
engine->initial_position.covariance_enu_m2,
sizeof(engine->current.last_accepted_position_covariance_m2));
engine->current.have_evidence_position = true;
engine->current.last_evidence_position_ticks =
engine->initial_position.timestamp_ticks;
memcpy(engine->current.last_evidence_position_m,
engine->initial_position.position_enu_m,
sizeof(engine->current.last_evidence_position_m));
memcpy(engine->current.last_evidence_position_covariance_m2,
engine->initial_position.covariance_enu_m2,
sizeof(engine->current.last_evidence_position_covariance_m2));
}
engine->checkpoint = engine->current;
engine->checkpoint_valid = true;
@@ -659,13 +835,13 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
{
*window_complete_out = false;
}
if(!runtime->have_accepted_position ||
(measurement->timestamp_ticks <= runtime->last_accepted_position_ticks))
if(!runtime->have_evidence_position ||
(measurement->timestamp_ticks <= runtime->last_evidence_position_ticks))
{
return FUSION_GNSS_MOTION_UNKNOWN;
}
double dt_s = ticks_to_seconds(
engine, measurement->timestamp_ticks - runtime->last_accepted_position_ticks);
engine, measurement->timestamp_ticks - runtime->last_evidence_position_ticks);
if(!(dt_s > 0.0))
{
return FUSION_GNSS_MOTION_UNKNOWN;
@@ -678,16 +854,16 @@ static fusion_gnss_motion_evidence_t position_motion_evidence(
{
*window_complete_out = true;
}
double delta_e = measurement->position_enu_m[0] - runtime->last_accepted_position_m[0];
double delta_n = measurement->position_enu_m[1] - runtime->last_accepted_position_m[1];
double delta_e = measurement->position_enu_m[0] - runtime->last_evidence_position_m[0];
double delta_n = measurement->position_enu_m[1] - runtime->last_evidence_position_m[1];
double s00 = measurement->covariance_enu_m2[0] +
runtime->last_accepted_position_covariance_m2[0];
runtime->last_evidence_position_covariance_m2[0];
double s01 = measurement->covariance_enu_m2[1] +
runtime->last_accepted_position_covariance_m2[1];
runtime->last_evidence_position_covariance_m2[1];
double s10 = measurement->covariance_enu_m2[3] +
runtime->last_accepted_position_covariance_m2[3];
runtime->last_evidence_position_covariance_m2[3];
double s11 = measurement->covariance_enu_m2[4] +
runtime->last_accepted_position_covariance_m2[4];
runtime->last_evidence_position_covariance_m2[4];
double off_diagonal = 0.5 * (s01 + s10);
double determinant = s00 * s11 - off_diagonal * off_diagonal;
if(!(s00 > 0.0) || !(s11 > 0.0) || !(determinant > DBL_EPSILON) ||
@@ -819,6 +995,15 @@ static void runtime_record_recovered_position(
memcpy(runtime->last_accepted_position_covariance_m2,
measurement->covariance_enu_m2,
sizeof(runtime->last_accepted_position_covariance_m2));
/* The re-anchor discards the old position track, so the displacement
* baseline must restart here too rather than measure across the jump. */
runtime->have_evidence_position = true;
runtime->last_evidence_position_ticks = measurement->timestamp_ticks;
memcpy(runtime->last_evidence_position_m, measurement->position_enu_m,
sizeof(runtime->last_evidence_position_m));
memcpy(runtime->last_evidence_position_covariance_m2,
measurement->covariance_enu_m2,
sizeof(runtime->last_evidence_position_covariance_m2));
runtime->gnss_evidence = FUSION_GNSS_MOTION_UNKNOWN;
runtime->gnss_moving_count = 0u;
runtime->gnss_still_count = 0u;
@@ -833,50 +1018,66 @@ static eskf_update_result_t runtime_apply_position(
eskf_update_result_t result = eskf_position_update(
&runtime->filter, &engine->config.eskf,
measurement->position_enu_m, measurement->covariance_enu_m2, nis_out);
/*
* Motion evidence is a displacement test between raw GNSS fixes and does
* not depend on the filter state, so it is derived from every fix rather
* than only from accepted ones. Updating it solely on acceptance froze it
* for as long as the innovation gate rejected fixes, which deadlocked the
* one signal that could veto a false ZUPT or authorise a re-anchor.
*/
bool motion_window_complete = false;
fusion_gnss_motion_evidence_t raw_evidence =
position_motion_evidence(engine, runtime, measurement,
&motion_window_complete);
if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
{
runtime->gnss_still_count = 0u;
if(runtime->gnss_moving_count < UINT32_MAX)
{
runtime->gnss_moving_count++;
}
if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations)
{
runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING;
}
}
else if(raw_evidence == FUSION_GNSS_MOTION_STILL)
{
runtime->gnss_moving_count = 0u;
if(runtime->gnss_still_count < UINT32_MAX)
{
runtime->gnss_still_count++;
}
if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations)
{
runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
}
}
/* Keep a long-enough displacement baseline. At a 200 ms GNSS period,
* replacing the reference on every fix would classify statistically
* inconclusive 10--20 cm steps as stationary evidence. */
if(!runtime->have_evidence_position || motion_window_complete)
{
runtime->have_evidence_position = true;
runtime->last_evidence_position_ticks = measurement->timestamp_ticks;
memcpy(runtime->last_evidence_position_m, measurement->position_enu_m,
sizeof(runtime->last_evidence_position_m));
memcpy(runtime->last_evidence_position_covariance_m2,
measurement->covariance_enu_m2,
sizeof(runtime->last_evidence_position_covariance_m2));
}
if(result == ESKF_UPDATE_ACCEPTED)
{
runtime->last_position_fuse_ticks = measurement->timestamp_ticks;
bool motion_window_complete = false;
fusion_gnss_motion_evidence_t raw_evidence =
position_motion_evidence(engine, runtime, measurement,
&motion_window_complete);
if(raw_evidence == FUSION_GNSS_MOTION_MOVING)
{
runtime->gnss_still_count = 0u;
if(runtime->gnss_moving_count < UINT32_MAX)
{
runtime->gnss_moving_count++;
}
if(runtime->gnss_moving_count >= engine->config.gnss_moving_confirmations)
{
runtime->gnss_evidence = FUSION_GNSS_MOTION_MOVING;
}
}
else if(raw_evidence == FUSION_GNSS_MOTION_STILL)
{
runtime->gnss_moving_count = 0u;
if(runtime->gnss_still_count < UINT32_MAX)
{
runtime->gnss_still_count++;
}
if(runtime->gnss_still_count >= engine->config.gnss_still_confirmations)
{
runtime->gnss_evidence = FUSION_GNSS_MOTION_STILL;
}
}
/* Keep a long-enough displacement baseline. At a 200 ms GNSS period,
* replacing the reference on every fix would classify statistically
* inconclusive 10--20 cm steps as stationary evidence. */
if(!runtime->have_accepted_position || motion_window_complete)
{
runtime->have_accepted_position = true;
runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
sizeof(runtime->last_accepted_position_m));
memcpy(runtime->last_accepted_position_covariance_m2,
measurement->covariance_enu_m2,
sizeof(runtime->last_accepted_position_covariance_m2));
}
runtime->have_accepted_position = true;
runtime->last_accepted_position_ticks = measurement->timestamp_ticks;
memcpy(runtime->last_accepted_position_m, measurement->position_enu_m,
sizeof(runtime->last_accepted_position_m));
memcpy(runtime->last_accepted_position_covariance_m2,
measurement->covariance_enu_m2,
sizeof(runtime->last_accepted_position_covariance_m2));
}
return result;
}
@@ -972,6 +1173,41 @@ static bool process_navigation_now(
eskf_update_result_t velocity_result = ESKF_UPDATE_INVALID;
if(measurement->velocity_valid)
{
/*
* Taken from the measurement rather than from the update outcome.
* Doppler speed is observed independently of the filter, so a filter
* that has drifted far enough to reject it is precisely the case this
* signal exists to break. Gating it on acceptance rebuilt the same
* circular dependency that deriving motion evidence from accepted
* fixes did: a false zero-velocity update pins filter velocity near
* zero, the resulting innovation then fails the NIS gate, and the one
* observation able to veto that update is never recorded.
*
* Recorded before the IMU replay below so the zero-velocity and
* position-recovery gates see this epoch's speed. Trusting a
* spurious high speed only suppresses a zero-velocity update, which
* forgoes a drift correction; trusting a spurious low one re-opens the
* lockout, so the asymmetry is deliberate.
*/
if(finite_values(measurement->velocity_enu_mps, 3u))
{
revised.last_gnss_speed_mps = norm3(measurement->velocity_enu_mps);
revised.last_gnss_velocity_ticks = position->timestamp_ticks;
revised.have_gnss_velocity = true;
/*
* Latched rather than aged out. Losing GNSS produces no evidence
* that the platform stopped, and a mount that isolates road
* vibration leaves the rest supervisor asserting rest throughout.
* Letting the veto lapse on a timer therefore resumed zero-velocity
* updates mid-journey, which clamps the position covariance to
* metres while the true error grows to kilometres -- the
* overconfidence that makes returning fixes fail the innovation
* gate. Only a later fix showing low speed clears this.
*/
revised.gnss_confirmed_moving =
revised.last_gnss_speed_mps >=
engine->config.gnss_motion_speed_threshold_mps;
}
velocity_result = eskf_velocity_update(
&revised.filter, &engine->config.eskf,
measurement->velocity_enu_mps,