From bf902e5eca49dcb911d97411fe17c9acc88fd4f2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 May 2018 07:47:17 +1000 Subject: [PATCH] EKF: Prevent flow motion check false positives The previous implementation could false trigger if there was significant vibration below 200Hz during startup. --- EKF/control.cpp | 10 ++++++---- EKF/covariance.cpp | 10 +++++++--- EKF/ekf.h | 3 ++- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 795a0bef23..329c27f896 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -339,10 +339,12 @@ void Ekf::controlOpticalFlowFusion() { // Check if motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { - bool motion_is_excessive = ((_accel_mag_filt > 14.7f) - || (_accel_mag_filt < 4.9f) - || (_ang_rate_mag_filt > _params.flow_rate_max) - || (_R_to_earth(2,2) < 0.866f)); + // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation + float accel_norm = _accel_vec_filt.norm(); + bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g + || (accel_norm < 4.9f) // accel less than 0.5g + || (_ang_rate_mag_filt > _params.flow_rate_max) // angular rate exceeds flow sensor limit + || (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 47a324144c..37d4474f11 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -163,9 +163,13 @@ void Ekf::predictCovariance() float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); // inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected - float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); - _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); - _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); + float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); + float beta = 1.0f - alpha; + _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt); + _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt); + _accel_vec_filt(0) = alpha * dt_inv * _imu_sample_delayed.delta_vel(0) + beta * _accel_vec_filt(0); + _accel_vec_filt(1) = alpha * dt_inv * _imu_sample_delayed.delta_vel(1) + beta * _accel_vec_filt(1); + _accel_vec_filt(2) = alpha * dt_inv * _imu_sample_delayed.delta_vel(2) + beta * _accel_vec_filt(2); if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim || _accel_mag_filt > _params.acc_bias_learn_acc_lim diff --git a/EKF/ekf.h b/EKF/ekf.h index 26154e251c..bfe6af0c70 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -402,7 +402,8 @@ private: // variables used to inhibit accel bias learning bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited - float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (m/sec**2) + Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2) + float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2