onur cayan
8349f32df1
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>
54 lines
2.3 KiB
C
54 lines
2.3 KiB
C
#ifndef FUSION_IMU_QUALITY_H
|
|
#define FUSION_IMU_QUALITY_H
|
|
|
|
#include <stdbool.h>
|
|
|
|
#define FUSION_IMU_QUALITY_AXIS_COUNT 3u
|
|
|
|
/*
|
|
* Hardware-neutral IMU characteristics. The nominal noise fields describe
|
|
* the sensor/board measurement stream. The process-noise fields are the
|
|
* deliberately conservative values used by the ESKF and may include timing,
|
|
* mounting, calibration and temperature uncertainty in addition to white
|
|
* sensor noise.
|
|
*/
|
|
typedef struct {
|
|
const char* name;
|
|
double nominal_accel_noise_density_mps2_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
|
|
double nominal_gyro_noise_density_rps_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
|
|
double process_accel_noise_density_mps2_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
|
|
double process_gyro_noise_density_rps_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT];
|
|
double accel_bias_random_walk_mps2_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT];
|
|
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);
|
|
bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile);
|
|
|
|
#endif
|