diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 7e3f260a46..0692c9d469 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -102,91 +102,12 @@ 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; - // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected - // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector - const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); - const float beta = 1.0f - alpha; - _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); - _accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt); - _accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt; - - 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(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; - } - } - } - - // accel bias inhibit - const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) - || is_manoeuvre_level_high - || _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) { - is_bias_observable = true; - - } else if (_control_status.flags.fake_hgt) { - is_bias_observable = false; - - } else if (_control_status.flags.fake_pos) { - // when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector - 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; - } - } - } - - // assign IMU noise variances - // inputs to the system are 3 delta angles and 3 delta velocities - float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); + // delta angle noise variance + float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f); const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise); - float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); - + // delta velocity noise variance + float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f); Vector3f d_vel_var; for (unsigned i = 0; i < 3; i++) { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 9d2ac69309..f1f5204e98 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -195,6 +195,14 @@ bool Ekf::update() // TODO: explicitly pop at desired time horizon const imuSample imu_sample_delayed = _imu_buffer.get_oldest(); + // calculate an average filter update time + // filter and limit input between -50% and +100% of nominal value + float input = 0.5f * (imu_sample_delayed.delta_vel_dt + imu_sample_delayed.delta_ang_dt); + float filter_update_s = 1e-6f * _params.filter_update_interval_us; + _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s); + + updateIMUBiasInhibit(imu_sample_delayed); + // perform state and covariance prediction for the main filter predictCovariance(imu_sample_delayed); predictState(imu_sample_delayed); @@ -305,13 +313,6 @@ void Ekf::predictState(const imuSample &imu_delayed) constrainStates(); - // calculate an average filter update time - float input = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt); - - // filter and limit input between -50% and +100% of nominal value - const float filter_update_s = 1e-6f * _params.filter_update_interval_us; - input = math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s); - _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; // 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 diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 42bcf8d5b3..1691b9a7aa 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -822,6 +822,8 @@ private: bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; + void updateIMUBiasInhibit(const imuSample &imu_delayed); + #if defined(CONFIG_EKF2_MAGNETOMETER) // ekf sequential fusion of magnetometer measurements bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 69c1c764b3..76360fb018 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1107,3 +1107,94 @@ void Ekf::resetWindCov() P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); } #endif // CONFIG_EKF2_WIND + +void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) +{ + // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected + // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector + { + const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f); + const float beta = 1.f - alpha; + _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); + } + + { + const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f); + const float beta = 1.f - alpha; + + _accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt); + _accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt; + } + + + 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(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; + } + } + } + + // accel bias inhibit + const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) + || is_manoeuvre_level_high + || _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) { + is_bias_observable = true; + + } else if (_control_status.flags.fake_hgt) { + is_bias_observable = false; + + } else if (_control_status.flags.fake_pos) { + // when using fake position (but not fake height) only consider an accel bias observable if aligned with the gravity vector + 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; + } + } + } + +}