fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 07:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: treat inhibited states as "consider states" and remove conservative covariance matrix fixes (#22597)

Browse Source 
When a state stops to be estimated it becomes a "consider state". Its value and variance are frozen but its covariance with other states continue to evolve normally.

 - removes conservative accel bias variance limiting
 - force symmetry is skipped after fusion of NED vel/pos (a direct measurement)

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar
2024-01-24 14:14:09 -05:00
committed by GitHub
co-authored by bresch
parent 2c81c9fdea
commit 51155f7a29
13 changed files with 874 additions and 1039 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+15 -23
View File
@@ -345,14 +345,7 @@ public:
const Vector3f &getMagEarthField() const { return _state.mag_I; }
const Vector3f &getMagBias() const { return _state.mag_B; }
Vector3f getMagBiasVariance() const
{
if (_control_status.flags.mag) {
return getStateVariance<State::mag_B>();
}
return _saved_mag_bf_covmat.diag();
}
Vector3f getMagBiasVariance() const { return getStateVariance<State::mag_B>(); } // get the mag bias variance in Gauss
float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss
#endif // CONFIG_EKF2_MAGNETOMETER
@@ -496,7 +489,8 @@ public:
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
constrainStateVariances();
forceCovarianceSymmetry();
// apply the state corrections
fuse(K, innovation);
@@ -523,6 +517,14 @@ private:
void updateHorizontalDeadReckoningstatus();
void updateVerticalDeadReckoningStatus();
static constexpr float kGyroBiasVarianceMin{1e-9f};
static constexpr float kAccelBiasVarianceMin{1e-9f};
#if defined(CONFIG_EKF2_MAGNETOMETER)
static constexpr float kMagVarianceMin = 1e-6f;
#endif // CONFIG_EKF2_MAGNETOMETER
struct StateResetCounts {
uint8_t velNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255)
uint8_t velD{0}; ///< number of vertical velocity reset events (allow to wrap if count exceeds 255)
@@ -720,8 +722,6 @@ private:
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
uint64_t _time_last_mag_check_failing{0};
Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2)
#endif // CONFIG_EKF2_MAGNETOMETER
// variables used to inhibit accel bias learning
@@ -730,9 +730,6 @@ private:
float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_gyro_bias_var{}; ///< saved gyro XYZ bias variances
Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances
// imu fault status
uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec)
uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec)
@@ -950,13 +947,12 @@ private:
bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP);
// limit the diagonal of the covariance matrix
// force symmetry when the argument is true
void fixCovarianceErrors(bool force_symmetry);
void constrainStateVariances();
void forceCovarianceSymmetry();
void constrainStateVar(const IdxDof &state, float min, float max);
// constrain the ekf states
void constrainStates();
void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f);
// generic function which will perform a fusion step given a kalman gain K
// and a scalar innovation value
@@ -1062,10 +1058,6 @@ private:
void stopMagHdgFusion();
void stopMagFusion();
// load and save mag field state covariance data for re-use
void loadMagCovData();
void saveMagCovData();
// calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter
// sensor measurement
float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted);