fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 15:03: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/covariance.cpp
+79 -163
View File
@@ -103,14 +103,14 @@ void Ekf::initialiseCovariance()
void Ekf::predictCovariance(const imuSample &imu_delayed)
{
// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
const float dt = _dt_ekf_avg;
// predict the covariance
const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt);
// delta angle noise variance
// gyro noise variance
float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f);
const float gyro_var = sq(gyro_noise);
// delta velocity noise variance
// accel noise variance
float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f);
Vector3f accel_var;
@@ -124,211 +124,112 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
}
}
// predict the covariance
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
P = sym::PredictCovariance(_state.vector(), P,
imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt));
imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
dt);
// Construct the process noise variance diagonal for those states with a stationary process model
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
// gyro bias: add process noise, or restore previous gyro bias var if state inhibited
const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
const float gyro_bias_process_noise = sq(gyro_bias_sig);
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned i = State::gyro_bias.idx + index;
// gyro bias: add process noise unless state is inhibited
{
const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f);
const float gyro_bias_process_noise = sq(gyro_bias_sig);
if (!_gyro_bias_inhibit[index]) {
P(i, i) += gyro_bias_process_noise;
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned i = State::gyro_bias.idx + index;
} else {
P.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index));
if (!_gyro_bias_inhibit[index]) {
P(i, i) += gyro_bias_process_noise;
}
}
}
// accel bias: add process noise, or restore previous accel bias var if state inhibited
const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
const float accel_bias_process_noise = sq(accel_bias_sig);
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned i = State::accel_bias.idx + index;
// accel bias: add process noise unless state is inhibited
{
const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f);
const float accel_bias_process_noise = sq(accel_bias_sig);
if (!_accel_bias_inhibit[index]) {
P(i, i) += accel_bias_process_noise;
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned i = State::accel_bias.idx + index;
} else {
P.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index));
if (!_accel_bias_inhibit[index]) {
P(i, i) += accel_bias_process_noise;
}
}
}
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_control_status.flags.mag) {
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
if (P.trace<State::mag_I.dof>(State::mag_I.idx) < 0.1f) {
// mag_I: add process noise
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
P.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) += sq(mag_I_sig);
float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f);
float mag_I_process_noise = sq(mag_I_sig);
for (unsigned index = 0; index < State::mag_I.dof; index++) {
unsigned i = State::mag_I.idx + index;
P(i, i) += mag_I_process_noise;
}
}
// Don't continue to grow the body field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
if (P.trace<State::mag_B.dof>(State::mag_B.idx) < 0.1f) {
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
float mag_B_process_noise = sq(mag_B_sig);
for (unsigned index = 0; index < State::mag_B.dof; index++) {
unsigned i = State::mag_B.idx + index;
P(i, i) += mag_B_process_noise;
}
}
// mag_B: add process noise
float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f);
P.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) += sq(mag_B_sig);
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
if (_control_status.flags.wind) {
// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
if (P.trace<State::wind_vel.dof>(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) {
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
for (unsigned index = 0; index < State::wind_vel.dof; index++) {
unsigned i = State::wind_vel.idx + index;
P(i, i) += wind_vel_process_noise;
}
}
// wind vel: add process noise
float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
P.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) += sq(wind_vel_nsd_scaled) * dt;
}
#endif // CONFIG_EKF2_WIND
// covariance matrix is symmetrical, so copy upper half to lower half
for (unsigned row = 0; row < State::size; row++) {
for (unsigned column = 0 ; column < row; column++) {
for (unsigned column = 0; column < row; column++) {
P(row, column) = P(column, row);
}
}
// fix gross errors in the covariance matrix and ensure rows and
// columns for un-used states are zero
fixCovarianceErrors(false);
constrainStateVariances();
}
void Ekf::fixCovarianceErrors(bool force_symmetry)
void Ekf::constrainStateVariances()
{
// NOTE: This limiting is a last resort and should not be relied on
// TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions
// and set corresponding entries in Q to zero when states exceed 50% of the limit
// Covariance diagonal limits. Use same values for states which
// belong to the same group (e.g. vel_x, vel_y, vel_z)
const float quat_var_max = 1.0f;
const float vel_var_max = 1e6f;
const float pos_var_max = 1e6f;
const float gyro_bias_var_max = 1.0f;
constrainStateVar(State::quat_nominal, 0.f, quat_var_max);
constrainStateVar(State::vel, 1e-6f, vel_var_max);
constrainStateVar(State::pos, 1e-6f, pos_var_max);
constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max);
// the following states are optional and are deactivated when not required
// by ensuring the corresponding covariance matrix values are kept at zero
// accelerometer bias states
if (!_accel_bias_inhibit[0] || !_accel_bias_inhibit[1] || !_accel_bias_inhibit[2]) {
// Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum
const float minSafeStateVar = 1e-9f / sq(_dt_ekf_avg);
float maxStateVar = minSafeStateVar;
bool resetRequired = false;
for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
const unsigned stateIndex = State::accel_bias.idx + axis;
if (_accel_bias_inhibit[axis]) {
// Skip the check for the inhibited axis
continue;
}
if (P(stateIndex, stateIndex) > maxStateVar) {
maxStateVar = P(stateIndex, stateIndex);
} else if (P(stateIndex, stateIndex) < minSafeStateVar) {
resetRequired = true;
}
}
// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
// not exceed 100 and the minimum variance must not fall below the target minimum
// Also limit variance to a maximum equivalent to a 0.1g uncertainty
const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg);
float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) {
const unsigned stateIndex = State::accel_bias.idx + axis;
if (_accel_bias_inhibit[axis]) {
// Skip the check for the inhibited axis
continue;
}
P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G));
}
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
if (resetRequired) {
resetAccelBiasCov();
}
}
if (force_symmetry) {
P.makeRowColSymmetric<State::quat_nominal.dof>(State::quat_nominal.idx);
P.makeRowColSymmetric<State::vel.dof>(State::vel.idx);
P.makeRowColSymmetric<State::pos.dof>(State::pos.idx);
P.makeRowColSymmetric<State::gyro_bias.dof>(State::gyro_bias.idx);
P.makeRowColSymmetric<State::accel_bias.dof>(State::accel_bias.idx);
}
constrainStateVar(State::quat_nominal, 1e-9f, 1.f);
constrainStateVar(State::vel, 1e-6f, 1e6f);
constrainStateVar(State::pos, 1e-6f, 1e6f);
constrainStateVarLimitRatio(State::gyro_bias, kGyroBiasVarianceMin, 1.f);
constrainStateVarLimitRatio(State::accel_bias, kAccelBiasVarianceMin, 1.f);
#if defined(CONFIG_EKF2_MAGNETOMETER)
// magnetic field states
if (!_control_status.flags.mag) {
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, 0.0f);
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, 0.0f);
} else {
const float mag_I_var_max = 1.f;
constrainStateVar(State::mag_I, 0.f, mag_I_var_max);
const float mag_B_var_max = 1.f;
constrainStateVar(State::mag_B, 0.f, mag_B_var_max);
if (force_symmetry) {
P.makeRowColSymmetric<State::mag_I.dof>(State::mag_I.idx);
P.makeRowColSymmetric<State::mag_B.dof>(State::mag_B.idx);
}
if (_control_status.flags.mag) {
constrainStateVarLimitRatio(State::mag_I, kMagVarianceMin, 1.f);
constrainStateVarLimitRatio(State::mag_B, kMagVarianceMin, 1.f);
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
// wind velocity states
if (!_control_status.flags.wind) {
P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, 0.0f);
} else {
const float wind_vel_var_max = 1e6f;
constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max);
if (force_symmetry) {
P.makeRowColSymmetric<State::wind_vel.dof>(State::wind_vel.idx);
}
if (_control_status.flags.wind) {
constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f);
}
#endif // CONFIG_EKF2_WIND
}
void Ekf::forceCovarianceSymmetry()
{
// DEBUG
// P.isBlockSymmetric(0, 1e-9f);
P.makeRowColSymmetric<State::size>(0);
}
void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
{
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
@@ -336,6 +237,25 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max)
}
}
void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio)
{
// the ratio of a max and min variance must not exceed max_ratio
float state_var_max = 0.f;
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
if (P(i, i) > state_var_max) {
state_var_max = P(i, i);
}
}
float limited_max = math::constrain(state_var_max, min, max);
float limited_min = math::constrain(limited_max / max_ratio, min, max);
for (unsigned i = state.idx; i < (state.idx + state.dof); i++) {
P(i, i) = math::constrain(P(i, i), limited_min, limited_max);
}
}
// if the covariance correction will result in a negative variance, then
// the covariance matrix is unhealthy and must be corrected
bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
@@ -344,7 +264,7 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP)
for (int i = 0; i < State::size; i++) {
if (P(i, i) < KHP(i, i)) {
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
P.uncorrelateCovarianceSetVariance<1>(i, 0.f);
healthy = false;
}
}
@@ -382,14 +302,10 @@ void Ekf::resetMagCov()
P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
saveMagCovData();
}
#endif // CONFIG_EKF2_MAGNETOMETER
void Ekf::resetGyroBiasZCov()
{
const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias);
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var);
P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
}
src/modules/ekf2/EKF/ekf.cpp
+3 -5
View File
@@ -89,9 +89,6 @@ void Ekf::reset()
_fault_status.value = 0;
_innov_check_fail_status.value = 0;
_prev_gyro_bias_var.zero();
_prev_accel_bias_var.zero();
#if defined(CONFIG_EKF2_GNSS)
resetGpsDriftCheckFilters();
_gps_checks_passed = false;
@@ -312,8 +309,9 @@ void Ekf::predictState(const imuSample &imu_delayed)
// predict position states via trapezoidal integration of velocity
_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
constrainStates();
// constrain states
_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
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);
src/modules/ekf2/EKF/ekf_helper.cpp
+27 -76
View File
@@ -233,28 +233,6 @@ void Ekf::resetVerticalVelocityToZero()
resetVerticalVelocityTo(0.0f, 10.f);
}
void Ekf::constrainStates()
{
_state.quat_nominal = matrix::constrain(_state.quat_nominal, -1.0f, 1.0f);
_state.vel = matrix::constrain(_state.vel, -1000.0f, 1000.0f);
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
const float gyro_bias_limit = getGyroBiasLimit();
_state.gyro_bias = matrix::constrain(_state.gyro_bias, -gyro_bias_limit, gyro_bias_limit);
const float accel_bias_limit = getAccelBiasLimit();
_state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit);
#if defined(CONFIG_EKF2_MAGNETOMETER)
_state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f);
_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f);
#endif // CONFIG_EKF2_WIND
}
#if defined(CONFIG_EKF2_BARO_COMPENSATION)
float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
{
@@ -548,9 +526,6 @@ void Ekf::resetGyroBiasCov()
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
// Set previous frame values
_prev_gyro_bias_var = getStateVariance<State::gyro_bias>();
}
void Ekf::resetAccelBias()
@@ -566,9 +541,6 @@ void Ekf::resetAccelBiasCov()
// Zero the corresponding covariances and set
// variances to the values use for initial alignment
P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
// Set previous frame values
_prev_accel_bias_var = getStateVariance<State::accel_bias>();
}
// get EKF innovation consistency check status information comprising of:
@@ -755,23 +727,39 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
void Ekf::fuse(const VectorState &K, float innovation)
{
// quat_nominal
Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0) * (-1.f * innovation)));
_state.quat_nominal *= delta_quat;
_state.quat_nominal.normalize();
_R_to_earth = Dcmf(_state.quat_nominal);
_state.vel -= K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation;
_state.pos -= K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation;
_state.gyro_bias -= K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation;
_state.accel_bias -= K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation;
// vel
_state.vel = matrix::constrain(_state.vel - K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f);
// pos
_state.pos = matrix::constrain(_state.pos - K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f);
// gyro_bias
_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation,
-getGyroBiasLimit(), getGyroBiasLimit());
// accel_bias
_state.accel_bias = matrix::constrain(_state.accel_bias - K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation,
-getAccelBiasLimit(), getAccelBiasLimit());
#if defined(CONFIG_EKF2_MAGNETOMETER)
_state.mag_I -= K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation;
_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
// mag_I, mag_B
if (_control_status.flags.mag) {
_state.mag_I = matrix::constrain(_state.mag_I - K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation, -1.f, 1.f);
_state.mag_B = matrix::constrain(_state.mag_B - K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation, -getMagBiasLimit(), getMagBiasLimit());
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
// wind_vel
if (_control_status.flags.wind) {
_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f);
}
#endif // CONFIG_EKF2_WIND
}
@@ -996,32 +984,13 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
const bool is_manoeuvre_level_high = (_ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim)
|| (_accel_magnitude_filt > _params.acc_bias_learn_acc_lim);
// gyro bias inhibit
const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GyroBias));
for (unsigned index = 0; index < State::gyro_bias.dof; index++) {
const unsigned stateIndex = State::gyro_bias.idx + index;
bool is_bias_observable = true;
// TODO: gyro bias conditions
const bool do_inhibit_axis = do_inhibit_all_gyro_axes || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_gyro_bias_inhibit[index]) {
_prev_gyro_bias_var(index) = P(stateIndex, stateIndex);
_gyro_bias_inhibit[index] = true;
}
} else {
if (_gyro_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_gyro_bias_var(index);
_gyro_bias_inhibit[index] = false;
}
}
bool is_bias_observable = true; // TODO: gyro bias conditions
_gyro_bias_inhibit[index] = do_inhibit_all_gyro_axes || !is_bias_observable;
}
// accel bias inhibit
@@ -1030,8 +999,6 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
|| _fault_status.flags.bad_acc_vertical;
for (unsigned index = 0; index < State::accel_bias.dof; index++) {
const unsigned stateIndex = State::accel_bias.idx + index;
bool is_bias_observable = true;
if (_control_status.flags.vehicle_at_rest) {
@@ -1045,22 +1012,6 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
is_bias_observable = (fabsf(_R_to_earth(2, index)) > 0.966f); // cos 15 degrees ~= 0.966
}
const bool do_inhibit_axis = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
if (do_inhibit_axis) {
// store the bias state variances to be reinstated later
if (!_accel_bias_inhibit[index]) {
_prev_accel_bias_var(index) = P(stateIndex, stateIndex);
_accel_bias_inhibit[index] = true;
}
} else {
if (_accel_bias_inhibit[index]) {
// reinstate the bias state variances
P(stateIndex, stateIndex) = _prev_accel_bias_var(index);
_accel_bias_inhibit[index] = false;
}
}
_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
}
}
src/modules/ekf2/EKF/mag_3d_control.cpp
+2 -24
View File
@@ -159,15 +159,13 @@ void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_star
_control_status.flags.mag = true;
loadMagCovData();
// activate fusion, reset mag states and initialize variance if first init or in flight reset
if (!_control_status.flags.yaw_align
|| wmm_updated
|| !_mag_decl_cov_reset
|| !_state.mag_I.longerThan(0.f)
|| (getStateVariance<State::mag_I>().min() < sq(0.0001f))
|| (getStateVariance<State::mag_B>().min() < sq(0.0001f))
|| (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
|| (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
) {
ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
@@ -209,25 +207,5 @@ void Ekf::stopMagFusion()
_fault_status.flags.bad_mag_z = false;
_fault_status.flags.bad_mag_decl = false;
saveMagCovData();
}
}
void Ekf::saveMagCovData()
{
// save the NED axis covariance sub-matrix
_saved_mag_ef_covmat = getStateCovariance<State::mag_I>();
// save the XYZ body covariance sub-matrix
_saved_mag_bf_covmat = getStateCovariance<State::mag_B>();
}
void Ekf::loadMagCovData()
{
// re-instate the NED axis covariance sub-matrix
resetStateCovariance<State::mag_I>(_saved_mag_ef_covmat);
// re-instate the XYZ body axis covariance sub-matrix
resetStateCovariance<State::mag_B>(_saved_mag_bf_covmat);
}
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -220,7 +220,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int s
// apply the covariance corrections
P -= KHP;
fixCovarianceErrors(true);
constrainStateVariances();
// apply the state corrections
fuse(Kfusion, innov);