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

@@ -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;
 

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

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