diff --git a/soc/software/gps_system/header/fusion/fusion_engine.h b/soc/software/gps_system/header/fusion/fusion_engine.h index 4ead68a..5c9a1d3 100644 --- a/soc/software/gps_system/header/fusion/fusion_engine.h +++ b/soc/software/gps_system/header/fusion/fusion_engine.h @@ -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; diff --git a/soc/software/gps_system/header/fusion/fusion_imu_quality.h b/soc/software/gps_system/header/fusion/fusion_imu_quality.h index a529fc8..c887e5c 100644 --- a/soc/software/gps_system/header/fusion/fusion_imu_quality.h +++ b/soc/software/gps_system/header/fusion/fusion_imu_quality.h @@ -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); diff --git a/soc/software/gps_system/src/adapters/imu_fusion_profile.c b/soc/software/gps_system/src/adapters/imu_fusion_profile.c index 6e7ec84..3522ed7 100644 --- a/soc/software/gps_system/src/adapters/imu_fusion_profile.c +++ b/soc/software/gps_system/src/adapters/imu_fusion_profile.c @@ -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: diff --git a/soc/software/gps_system/src/fusion/fusion_engine.c b/soc/software/gps_system/src/fusion/fusion_engine.c index 02743ce..d9163f3 100644 --- a/soc/software/gps_system/src/fusion/fusion_engine.c +++ b/soc/software/gps_system/src/fusion/fusion_engine.c @@ -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,