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>
177 lines
5.7 KiB
C
177 lines
5.7 KiB
C
#ifndef FUSION_ENGINE_H
|
|
#define FUSION_ENGINE_H
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "fusion/eskf.h"
|
|
#include "fusion/fusion_imu_quality.h"
|
|
#include "fusion/fusion_measurement.h"
|
|
#include "fusion/fusion_motion.h"
|
|
|
|
#ifndef FUSION_IMU_HISTORY_CAPACITY
|
|
#define FUSION_IMU_HISTORY_CAPACITY 2048u
|
|
#endif
|
|
|
|
#ifndef FUSION_POSITION_PENDING_CAPACITY
|
|
#define FUSION_POSITION_PENDING_CAPACITY 8u
|
|
#endif
|
|
|
|
typedef enum {
|
|
FUSION_ENGINE_CALIBRATING = 0,
|
|
FUSION_ENGINE_RUNNING,
|
|
FUSION_ENGINE_FAULT
|
|
} fusion_engine_state_t;
|
|
|
|
typedef struct {
|
|
eskf_config_t eskf;
|
|
fusion_motion_config_t motion;
|
|
uint64_t timebase_hz;
|
|
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;
|
|
double zero_update_period_s;
|
|
double zero_velocity_std_mps;
|
|
double zero_gyro_std_rps[3];
|
|
double gnss_motion_speed_threshold_mps;
|
|
double gnss_displacement_nis_gate;
|
|
double gnss_motion_window_s;
|
|
unsigned int gnss_moving_confirmations;
|
|
unsigned int gnss_still_confirmations;
|
|
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;
|
|
|
|
typedef struct {
|
|
eskf_t filter;
|
|
fusion_motion_detector_t motion;
|
|
fusion_imu_measurement_t held_imu;
|
|
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;
|
|
|
|
typedef struct {
|
|
uint64_t imu_received;
|
|
uint64_t imu_rejected;
|
|
uint64_t imu_history_overflow;
|
|
uint64_t checkpoint_advance_count;
|
|
uint64_t calibration_reset_count;
|
|
uint64_t position_received;
|
|
uint64_t position_source_rejected;
|
|
uint64_t position_nis_rejected;
|
|
uint64_t position_stale_rejected;
|
|
uint64_t position_pending_overflow;
|
|
uint64_t position_accepted;
|
|
uint64_t position_recovery_count;
|
|
uint64_t velocity_received;
|
|
uint64_t velocity_nis_rejected;
|
|
uint64_t velocity_accepted;
|
|
uint64_t replay_count;
|
|
uint64_t replayed_imu_count;
|
|
uint64_t zero_velocity_update_count;
|
|
uint64_t zero_gyro_update_count;
|
|
} fusion_engine_diagnostics_t;
|
|
|
|
typedef struct {
|
|
uint64_t start_ticks;
|
|
uint64_t last_ticks;
|
|
double accel_mean[3];
|
|
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;
|
|
|
|
typedef struct {
|
|
fusion_engine_config_t config;
|
|
fusion_engine_state_t state;
|
|
fusion_runtime_t current;
|
|
fusion_runtime_t checkpoint;
|
|
fusion_calibration_accumulator_t calibration;
|
|
fusion_imu_measurement_t imu_history[FUSION_IMU_HISTORY_CAPACITY];
|
|
fusion_navigation_measurement_t pending_positions[FUSION_POSITION_PENDING_CAPACITY];
|
|
fusion_position_measurement_t initial_position;
|
|
fusion_position_measurement_t position_recovery_candidate;
|
|
fusion_engine_diagnostics_t diagnostics;
|
|
uint64_t position_recovery_start_ticks;
|
|
unsigned int imu_history_head;
|
|
unsigned int imu_history_count;
|
|
unsigned int pending_position_count;
|
|
unsigned int position_recovery_consistent_count;
|
|
bool checkpoint_valid;
|
|
bool have_initial_position;
|
|
bool position_recovery_active;
|
|
} fusion_engine_t;
|
|
|
|
void fusion_engine_default_config(fusion_engine_config_t* config,
|
|
uint64_t timebase_hz);
|
|
bool fusion_engine_apply_imu_quality(
|
|
fusion_engine_config_t* config,
|
|
const fusion_imu_quality_profile_t* profile);
|
|
bool fusion_engine_init(fusion_engine_t* engine,
|
|
const fusion_engine_config_t* config);
|
|
|
|
bool fusion_engine_push_imu(fusion_engine_t* engine,
|
|
const fusion_imu_measurement_t* measurement);
|
|
bool fusion_engine_push_position(
|
|
fusion_engine_t* engine,
|
|
const fusion_position_measurement_t* measurement,
|
|
eskf_update_result_t* update_result_out,
|
|
double* nis_out);
|
|
bool fusion_engine_push_navigation(
|
|
fusion_engine_t* engine,
|
|
const fusion_navigation_measurement_t* measurement,
|
|
eskf_update_result_t* position_update_result_out,
|
|
double* position_nis_out,
|
|
eskf_update_result_t* velocity_update_result_out,
|
|
double* velocity_nis_out);
|
|
|
|
bool fusion_engine_get_runtime(const fusion_engine_t* engine,
|
|
fusion_runtime_t* runtime_out);
|
|
const fusion_engine_diagnostics_t* fusion_engine_diagnostics(
|
|
const fusion_engine_t* engine);
|
|
|
|
#endif
|