diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 51b17931c24f..7e3f260a46b1 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -84,23 +84,16 @@ void Ekf::initialiseCovariance() P.uncorrelateCovarianceSetVariance(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var)); - // gyro bias - const float gyro_bias_var = sq(_params.switch_on_gyro_bias); - P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, gyro_bias_var); - _prev_gyro_bias_var.setAll(gyro_bias_var); + resetGyroBiasCov(); - // accel bias - const float accel_bias_var = sq(_params.switch_on_accel_bias); - P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, accel_bias_var); - _prev_accel_bias_var.setAll(accel_bias_var); + resetAccelBiasCov(); #if defined(CONFIG_EKF2_MAGNETOMETER) resetMagCov(); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) - // wind - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); + resetWindCov(); #endif // CONFIG_EKF2_WIND } @@ -317,14 +310,6 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) constrainStateVar(State::pos, 1e-6f, pos_var_max); constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max); - // force symmetry on the quaternion, velocity and position state covariances - if (force_symmetry) { - P.makeRowColSymmetric(State::quat_nominal.idx); - P.makeRowColSymmetric(State::vel.idx); - P.makeRowColSymmetric(State::pos.idx); - P.makeRowColSymmetric(State::gyro_bias.idx); //TODO: needed? - } - // the following states are optional and are deactivated when not required // by ensuring the corresponding covariance matrix values are kept at zero @@ -370,84 +355,16 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - P.uncorrelateCovariance(State::accel_bias.idx); - } - - // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong - // calculate accel bias term aligned with the gravity vector - const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; - const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg; - const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); - - // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation - bool bad_acc_bias = false; - if (fabsf(down_dvel_bias) > dVel_bias_lim) { -#if defined(CONFIG_EKF2_GNSS) - bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f); -#else - bool bad_vz_gps = false; -#endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f); -#else - bool bad_vz_ev = false; -#endif // CONFIG_EKF2_EXTERNAL_VISION - - if (bad_vz_gps || bad_vz_ev) { -#if defined(CONFIG_EKF2_BAROMETER) - bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f); -#else - bool bad_z_baro = false; -#endif -#if defined(CONFIG_EKF2_GNSS) - bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f); -#else - bool bad_z_gps = false; -#endif - -#if defined(CONFIG_EKF2_RANGE_FINDER) - bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f); -#else - bool bad_z_rng = false; -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.0f); -#else - bool bad_z_ev = false; -#endif // CONFIG_EKF2_EXTERNAL_VISION - - if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) { - bad_acc_bias = true; - } - } - } - - // record the pass/fail - if (!bad_acc_bias) { - _fault_status.flags.bad_acc_bias = false; - _time_acc_bias_check = _time_delayed_us; - - } else { - _fault_status.flags.bad_acc_bias = true; - } - - // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of - // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue - if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { - - P.uncorrelateCovariance(State::accel_bias.idx); - - _time_acc_bias_check = _time_delayed_us; - _fault_status.flags.bad_acc_bias = false; - _warning_events.flags.invalid_accel_bias_cov_reset = true; - ECL_WARN("invalid accel bias - covariance reset"); - - } else if (force_symmetry) { - // ensure the covariance values are symmetrical - P.makeRowColSymmetric(State::accel_bias.idx); + resetAccelBiasCov(); } + } + if (force_symmetry) { + P.makeRowColSymmetric(State::quat_nominal.idx); + P.makeRowColSymmetric(State::vel.idx); + P.makeRowColSymmetric(State::pos.idx); + P.makeRowColSymmetric(State::gyro_bias.idx); + P.makeRowColSymmetric(State::accel_bias.idx); } #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index c1df5e4f3b3a..42bcf8d5b313 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -358,8 +358,12 @@ class Ekf final : public EstimatorInterface // Reset all IMU bias states and covariances to initial alignment values. void resetImuBias(); + void resetGyroBias(); + void resetGyroBiasCov(); + void resetAccelBias(); + void resetAccelBiasCov(); // return true if the global position estimate is valid // return true if the origin is set we are not doing unconstrained free inertial navigation @@ -1163,6 +1167,7 @@ class Ekf final : public EstimatorInterface void stopAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL + void checkVerticalAccelerationBias(const imuSample &imu_delayed); void checkVerticalAccelerationHealth(const imuSample &imu_delayed); Likelihood estimateInertialNavFallingLikelihood() const; @@ -1190,6 +1195,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_WIND) // perform a reset of the wind states and related covariances void resetWind(); + void resetWindCov(); void resetWindToZero(); #endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index cf27006ccf82..69c1c764b334 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -623,6 +623,11 @@ void Ekf::resetGyroBias() // Zero the gyro bias states _state.gyro_bias.zero(); + resetGyroBiasCov(); +} + +void Ekf::resetGyroBiasCov() +{ // Zero the corresponding covariances and set // variances to the values use for initial alignment P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); @@ -636,6 +641,11 @@ void Ekf::resetAccelBias() // Zero the accel bias states _state.accel_bias.zero(); + resetAccelBiasCov(); +} + +void Ekf::resetAccelBiasCov() +{ // Zero the corresponding covariances and set // variances to the values use for initial alignment P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); @@ -1088,7 +1098,12 @@ void Ekf::resetWindToZero() // If we don't have an airspeed measurement, then assume the wind is zero _state.wind_vel.setZero(); + resetWindCov(); +} + +void Ekf::resetWindCov() +{ // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, _params.initial_wind_uncertainty); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); } #endif // CONFIG_EKF2_WIND diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index d5c354289755..0d954663d0a9 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -39,6 +39,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) { + checkVerticalAccelerationBias(imu_delayed); checkVerticalAccelerationHealth(imu_delayed); #if defined(CONFIG_EKF2_BAROMETER) @@ -119,6 +120,107 @@ void Ekf::checkHeightSensorRefFallback() } } +void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed) +{ + // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong + // calculate accel bias term aligned with the gravity vector + const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; + const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg; + const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); + + // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation + bool bad_acc_bias = false; + + if (fabsf(down_dvel_bias) > dVel_bias_lim) { + + bool bad_vz = false; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_hgt) { + if (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.f) { + bad_vz = true; + } + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps) { + if (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.f) { + bad_vz = true; + } + } + +#endif // CONFIG_EKF2_GNSS + + if (bad_vz) { +#if defined(CONFIG_EKF2_BAROMETER) + + if (_control_status.flags.baro_hgt) { + if (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_hgt) { + if (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps_hgt) { + if (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_control_status.flags.rng_hgt) { + if (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.f) { + bad_acc_bias = true; + } + } + +#endif // CONFIG_EKF2_RANGE_FINDER + } + } + + // record the pass/fail + if (!bad_acc_bias) { + _fault_status.flags.bad_acc_bias = false; + _time_acc_bias_check = _time_delayed_us; + + } else { + _fault_status.flags.bad_acc_bias = true; + } + + // if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of + // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue + if (_fault_status.flags.bad_acc_bias && isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { + + resetAccelBiasCov(); + + _time_acc_bias_check = imu_delayed.time_us; + + _fault_status.flags.bad_acc_bias = false; + _warning_events.flags.invalid_accel_bias_cov_reset = true; + ECL_WARN("invalid accel bias - covariance reset"); + } +} + void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) { // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical