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>
128 lines
5.4 KiB
C
128 lines
5.4 KiB
C
#include "adapters/imu_fusion_profile.h"
|
|
|
|
#include <stddef.h>
|
|
|
|
#define PROFILE_GRAVITY_MPS2 9.80665
|
|
#define PROFILE_DEGREES_TO_RADIANS 0.017453292519943295769236907684886
|
|
|
|
static void set_all(double values[3], double value)
|
|
{
|
|
for(unsigned int axis = 0u ; axis < 3u ; axis++)
|
|
{
|
|
values[axis] = value;
|
|
}
|
|
}
|
|
|
|
bool imu_fusion_quality_profile_for_backend(
|
|
imu_sensor_backend_t backend,
|
|
fusion_imu_quality_profile_t* profile_out)
|
|
{
|
|
if(profile_out == NULL)
|
|
{
|
|
return false;
|
|
}
|
|
fusion_imu_quality_default(profile_out);
|
|
switch(backend)
|
|
{
|
|
case IMU_SENSOR_TDK_ICM42688:
|
|
profile_out->name = "tdk-icm42688";
|
|
/* ICM-42688-P: 65/65/70 ug/sqrt(Hz), 2.8 mdps/sqrt(Hz). */
|
|
profile_out->nominal_accel_noise_density_mps2_sqrt_hz[0] =
|
|
65.0e-6 * PROFILE_GRAVITY_MPS2;
|
|
profile_out->nominal_accel_noise_density_mps2_sqrt_hz[1] =
|
|
65.0e-6 * PROFILE_GRAVITY_MPS2;
|
|
profile_out->nominal_accel_noise_density_mps2_sqrt_hz[2] =
|
|
70.0e-6 * PROFILE_GRAVITY_MPS2;
|
|
set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
|
|
2.8e-3 * PROFILE_DEGREES_TO_RADIANS);
|
|
/* Conservative fusion values; tune bias RW with long Allan logs. */
|
|
set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.025);
|
|
set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0022);
|
|
set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0015);
|
|
set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
|
|
0.002 * PROFILE_DEGREES_TO_RADIANS);
|
|
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:
|
|
profile_out->name = "st-lsm6dsox";
|
|
/* LSM6DSOX at +-2 g: 70 ug/sqrt(Hz), 3.8 mdps/sqrt(Hz). */
|
|
set_all(profile_out->nominal_accel_noise_density_mps2_sqrt_hz,
|
|
70.0e-6 * PROFILE_GRAVITY_MPS2);
|
|
set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
|
|
3.8e-3 * PROFILE_DEGREES_TO_RADIANS);
|
|
set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.03);
|
|
set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0030);
|
|
set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0025);
|
|
set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
|
|
0.004 * PROFILE_DEGREES_TO_RADIANS);
|
|
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:
|
|
return false;
|
|
}
|
|
return fusion_imu_quality_is_valid(profile_out);
|
|
}
|