EKF: Fix non GPS aiding data reset logic (#418)

* EKF: Move optical flow specific state reset to helper functions

* EKF: Ensure loss of optical flow aiding is handled correctly

If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference.
If flow data is unavailable for too long - declare optical flow use stopped.
Use consistent time periods for all resets

* EKF: Ensure loss of external vision aiding is handled correctly

If data is only source of aiding and has been rejected for too long - reset using data as a position.
Don't reset velocity if there is another source of aiding constraining it.
If data is unavailable for too long, declare external vision use stopped.
Use consistent time periods for all resets.

* EKF: Update parameter documentation

Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer

* EKF: make class variable units consistent with documentation

* EKF: Don't reset states when optical flow use commences if using external vision

* EKF: Stop optical flow fusion when on ground if excessive movement is detected.

* EKF: fix terrain estimator vulnerabilities

Reset estimate to sensor value if rejected for 10 seconds
Protect against user motion when on ground.
Fix unnecessary duplication of terrain validity check and separate validity update and reporting.

* EKF: remove unnecessary Info console prints

Optical flow use information can be obtained from the estimator_status.control_mode_flags message

* EKF: fix inaccurate comment

* EKF: remove duplicate calculation from terrain validity accessor function

EKF/common.h

@@ -321,10 +321,10 @@ struct parameters {
 	float acc_bias_learn_gyr_lim{3.0f};	///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec)
 	float acc_bias_learn_tc{0.5f};		///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec)
 
-	unsigned no_gps_timeout_max{7000000};	///< maximum time we allow dead reckoning while both gps position and velocity measurements are being
+	unsigned no_gps_timeout_max{7000000};	///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement (uSec)
-						///< rejected before attempting to reset the states to the GPS measurement (uSec)
+	unsigned no_aid_timeout_max{1000000};	///< maximum lapsed time from last fusion of measurements that constrain horizontal velocity drift before
-	unsigned no_aid_timeout_max{1000000};	///< maximum lapsed time from last fusion of measurements that constrain drift before
+						///< the EKF will report that it has been inertial dead-reckoning for too long  and needs to revert to a
-						///< the EKF will report that it is dead-reckoning (uSec)
+						/// mode that doesn't privide horizontal vbelocity and position estimates (uSec)
 
 	// multi-rotor drag specific force fusion
 	float drag_noise{2.5f};			///< observation noise variance for drag specific force measurements (m/sec**2)**2

EKF/control.cpp

@@ -289,6 +289,15 @@ void Ekf::controlExternalVisionFusion()
 				// use the absolute position
 				_vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0);
 				_vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1);
+
+				// check if we have been deadreckoning too long
+				if (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) {
+					// don't reset velocity if we have another source of aiding constraining it
+					if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) {
+						resetVelocity();
+					}
+					resetPosition();
+				}
 			}
 
 			// observation 1-STD error
@@ -311,7 +320,7 @@ void Ekf::controlExternalVisionFusion()
 			fuseHeading();
 
 		}
-	} else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)5e6)) {
+	} else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) {
 		// Turn off EV fusion mode if no data has been received
 		_control_status.flags.ev_pos = false;
 		ECL_INFO("EKF External Vision Data Stopped");
@@ -321,95 +330,97 @@ void Ekf::controlExternalVisionFusion()
 
 void Ekf::controlOpticalFlowFusion()
 {
-	// Check for new optical flow data that has fallen behind the fusion time horizon
+	// Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated.
+	if (!_control_status.flags.in_air) {
+		bool motion_is_excessive = ((_accel_mag_filt > 10.8f)
+					    || (_accel_mag_filt < 8.8f)
+					    || (_ang_rate_mag_filt > 0.5f)
+					    || (_R_to_earth(2,2) < 0.866f));
+		if (motion_is_excessive) {
+			_time_bad_motion_us = _imu_sample_delayed.time_us;
+		} else {
+			_time_good_motion_us = _imu_sample_delayed.time_us;
+		}
+	} else {
+		_time_bad_motion_us = 0;
+		_time_good_motion_us = _imu_sample_delayed.time_us;
+	}
+
+	// Inhibit flow use if on ground and motion is excessive
+	// Apply a time based hysteresis to prevent rapid mode switching
+	if (!_inhibit_gndobs_use) {
+		if ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) {
+			_inhibit_gndobs_use = true;
+		}
+	} else {
+		if ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) {
+			_inhibit_gndobs_use = false;
+		}
+	}
+
+	// Handle cases where we are using optical flow but are no longer able to because data is old
+	// or its use has been inhibited.
+	if (_control_status.flags.opt_flow) {
+	       if (_inhibit_gndobs_use) {
+		       _control_status.flags.opt_flow = false;
+		       _time_last_of_fuse = 0;
+
+	       } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) {
+		       _control_status.flags.opt_flow = false;
+
+	       }
+	}
+
 	if (_flow_data_ready) {
+		// New optical flow data has fallen behind the fusion time horizon and is ready to be fused
+
+		// Accumulate autopilot gyro data across the same time interval as the flow sensor
+		_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
+		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
 
 		// optical flow fusion mode selection logic
 		if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
 				&& !_control_status.flags.opt_flow // we are not yet using flow data
 				&& _control_status.flags.tilt_align // we know our tilt attitude
-				&& (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e5) // we have a valid distance to ground estimate
+				&& get_terrain_valid()) // we have a valid distance to ground estimate
 		{
-
 			// If the heading is not aligned, reset the yaw and magnetic field states
 			if (!_control_status.flags.yaw_align) {
 				_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
 			}
 
-			// If the heading is valid, start using optical flow aiding
+			// If the heading is valid and use is no tinhibited , start using optical flow aiding
-			if (_control_status.flags.yaw_align) {
+			if (_control_status.flags.yaw_align && !_inhibit_gndobs_use) {
 				// set the flag and reset the fusion timeout
 				_control_status.flags.opt_flow = true;
 				_time_last_of_fuse = _time_last_imu;
 				ECL_INFO("EKF Starting Optical Flow Use");
 
 				// if we are not using GPS then the velocity and position states and covariances need to be set
-				if (!_control_status.flags.gps) {
+				if (!_control_status.flags.gps || !_control_status.flags.ev_pos) {
-					// constrain height above ground to be above minimum possible
+					resetVelocity();
-					float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
+					resetPosition();
-
-					// calculate absolute distance from focal point to centre of frame assuming a flat earth
-					float range = heightAboveGndEst / _R_rng_to_earth_2_2;
-
-					if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
-						// we should have reliable OF measurements so
-						// calculate X and Y body relative velocities from OF measurements
-						Vector3f vel_optflow_body;
-						vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
-						vel_optflow_body(1) =   range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
-						vel_optflow_body(2) = 0.0f;
-
-						// rotate from body to earth frame
-						Vector3f vel_optflow_earth;
-						vel_optflow_earth = _R_to_earth * vel_optflow_body;
-
-						// take x and Y components
-						_state.vel(0) = vel_optflow_earth(0);
-						_state.vel(1) = vel_optflow_earth(1);
-
-					} else {
-						_state.vel(0) = 0.0f;
-						_state.vel(1) = 0.0f;
-					}
-
-					// reset the velocity covariance terms
-					zeroRows(P,4,5);
-					zeroCols(P,4,5);
-
-					// reset the horizontal velocity variance using the optical flow noise variance
-					P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
-
-					if (!_control_status.flags.in_air) {
-						// we are likely starting OF for the first time so reset the horizontal position and vertical velocity states
-						_state.pos(0) = 0.0f;
-						_state.pos(1) = 0.0f;
-
-					} else {
-						// set to the last known position
-						_state.pos(0) = _last_known_posNE(0);
-						_state.pos(1) = _last_known_posNE(1);
-
-					}
-
-					// reset the corresponding covariances
-					// we are by definition at the origin at commencement so variances are also zeroed
-					zeroRows(P,7,8);
-					zeroCols(P,7,8);
 
 					// align the output observer to the EKF states
 					alignOutputFilter();
 
 				}
 			}
-
 		} else if (!(_params.fusion_mode & MASK_USE_OF)) {
 			_control_status.flags.opt_flow = false;
 
 		}
 
-		// Accumulate autopilot gyro data across the same time interval as the flow sensor
+		// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
-		_imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias;
+		if (_control_status.flags.opt_flow
-		_delta_time_of += _imu_sample_delayed.delta_ang_dt;
+						&& !_control_status.flags.gps
+						&& !_control_status.flags.ev_pos) {
+			bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max;
+			if (do_reset) {
+				resetVelocity();
+				resetPosition();
+			}
+		}
 
 		// fuse the data if the terrain/distance to bottom is valid
 		if (_control_status.flags.opt_flow && get_terrain_valid()) {
@@ -422,10 +433,6 @@ void Ekf::controlOpticalFlowFusion()
 			_last_known_posNE(1) = _state.pos(1);
 
 		}
-	} else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > (uint64_t)5e6)) {
-		ECL_INFO("EKF Optical Flow Data Stopped");
-		_control_status.flags.opt_flow = false;
-
 	}
 
 }

EKF/covariance.cpp

@@ -148,6 +148,7 @@ void Ekf::predictCovariance()
 	float dvz_b = _state.accel_bias(2);
 
 	float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS);
+	float dt_inv = 1.0f / dt;
 
 	// compute noise variance for stationary processes
 	float process_noise[_k_num_states] = {};
@@ -160,10 +161,11 @@ void Ekf::predictCovariance()
 
 	// 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(_imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt);
+	_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt);
-	_accel_mag_filt = fmaxf(_imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
+	_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
-	if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim
+
-			|| _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim
+	if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
+			|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
 			|| _bad_vert_accel_detected) {
 		// store the bias state variances to be reinstated later
 		if (!_accel_bias_inhibit) {

EKF/ekf.h

@@ -179,6 +179,9 @@ public:
 	// return true if the terrain estimate is valid
 	bool get_terrain_valid();
 
+	// update terrain validity status
+	void update_terrain_valid();
+
 	// get the estimated terrain vertical position relative to the NED origin
 	void get_terrain_vert_pos(float *ret);
 
@@ -298,7 +301,6 @@ private:
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_rng_ready{0};	///< time the last range finder measurement was ready (uSec)
 	Vector2f _last_known_posNE;		///< last known local NE position vector (m)
-	float _last_disarmed_posD{0.0f};	///< vertical position recorded at arming (m)
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
 	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
@@ -346,6 +348,9 @@ private:
 	Vector3f _flow_gyro_bias;	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
 	Vector3f _imu_del_ang_of;	///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
 	float _delta_time_of{0.0f};	///< time in sec that _imu_del_ang_of was accumulated over (sec)
+	uint64_t _time_bad_motion_us{0};	///< last system time that on-ground motion exceeded limits (uSec)
+	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
+	bool _inhibit_gndobs_use{false};	///< true when use of ground observations (optical flow and range finder) is being temporarily inhibited due to excessive on-ground motion
 
 	float _mag_declination{0.0f};	///< magnetic declination used by reset and fusion functions (rad)
 
@@ -410,6 +415,7 @@ private:
 	float _R_rng_to_earth_2_2{0.0f};	///< 2,2 element of the rotation matrix from sensor frame to earth frame
 	bool _range_data_continuous{false};	///< true when we are receiving range finder data faster than a 2Hz average
 	float _dt_last_range_update_filt_us{0.0f};	///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec)
+	bool _hagl_valid{false};		///< true when the height above ground estimate is valid
 
 	// height sensor fault status
 	bool _baro_hgt_faulty{false};		///< true if valid baro data is unavailable for use

EKF/ekf_helper.cpp

@@ -60,7 +60,42 @@ bool Ekf::resetVelocity()
 		// use GPS accuracy to reset variances
 		setDiag(P,4,6,sq(_gps_sample_delayed.sacc));
 
-	} else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) {
+	} else if (_control_status.flags.opt_flow) {
+		// constrain height above ground to be above minimum possible
+		float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
+
+		// calculate absolute distance from focal point to centre of frame assuming a flat earth
+		float range = heightAboveGndEst / _R_rng_to_earth_2_2;
+
+		if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
+			// we should have reliable OF measurements so
+			// calculate X and Y body relative velocities from OF measurements
+			Vector3f vel_optflow_body;
+			vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
+			vel_optflow_body(1) =   range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
+			vel_optflow_body(2) = 0.0f;
+
+			// rotate from body to earth frame
+			Vector3f vel_optflow_earth;
+			vel_optflow_earth = _R_to_earth * vel_optflow_body;
+
+			// take x and Y components
+			_state.vel(0) = vel_optflow_earth(0);
+			_state.vel(1) = vel_optflow_earth(1);
+
+		} else {
+			_state.vel(0) = 0.0f;
+			_state.vel(1) = 0.0f;
+		}
+
+		// reset the velocity covariance terms
+		zeroRows(P,4,5);
+		zeroCols(P,4,5);
+
+		// reset the horizontal velocity variance using the optical flow noise variance
+		P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
+
+	} else if (_control_status.flags.ev_pos) {
 		_state.vel.setZero();
 		zeroOffDiag(P,4,6);
 
@@ -112,11 +147,6 @@ bool Ekf::resetPosition()
 		// use GPS accuracy to reset variances
 		setDiag(P,7,8,sq(_gps_sample_delayed.hacc));
 
-	} else if (_control_status.flags.opt_flow) {
-		_state.pos(0) = 0.0f;
-		_state.pos(1) = 0.0f;
-		zeroOffDiag(P,7,8);
-
 	} else if (_control_status.flags.ev_pos) {
 		// this reset is only called if we have new ev data at the fusion time horizon
 		_state.pos(0) = _ev_sample_delayed.posNED(0);
@@ -125,6 +155,20 @@ bool Ekf::resetPosition()
 		// use EV accuracy to reset variances
 		setDiag(P,7,8,sq(_ev_sample_delayed.posErr));
 
+	} else if (_control_status.flags.opt_flow) {
+		if (!_control_status.flags.in_air) {
+			// we are likely starting OF for the first time so reset the horizontal position
+			_state.pos(0) = 0.0f;
+			_state.pos(1) = 0.0f;
+
+		} else {
+			// set to the last known position
+			_state.pos(0) = _last_known_posNE(0);
+			_state.pos(1) = _last_known_posNE(1);
+
+		}
+		setDiag(P,7,8,sq(_params.pos_noaid_noise));
+
 	} else {
 		// Used when falling back to non-aiding mode of operation
 		_state.pos(0) = _last_known_posNE(0);

EKF/terrain_estimator.cpp

@@ -109,6 +109,9 @@ void Ekf::runTerrainEstimator()
 			_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
 		}
 	}
+
+	// Update terrain validity
+	update_terrain_valid();
 }
 
 void Ekf::fuseHagl()
@@ -134,37 +137,50 @@ void Ekf::fuseHagl()
 		float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
 		_terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
 
-		if (_terr_test_ratio <= 1.0f) {
+		if (!_inhibit_gndobs_use) {
-			// calculate the Kalman gain
+			if (_terr_test_ratio <= 1.0f) {
-			float gain = _terrain_var / _hagl_innov_var;
+				// calculate the Kalman gain
-			// correct the state
+				float gain = _terrain_var / _hagl_innov_var;
-			_terrain_vpos -= gain * _hagl_innov;
+				// correct the state
-			// correct the variance
+				_terrain_vpos -= gain * _hagl_innov;
-			_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
+				// correct the variance
-			// record last successful fusion event
+				_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
-			_time_last_hagl_fuse = _time_last_imu;
+				// record last successful fusion event
-			_innov_check_fail_status.flags.reject_hagl = false;
+				_time_last_hagl_fuse = _time_last_imu;
-
+				_innov_check_fail_status.flags.reject_hagl = false;
-		} else {
-			_innov_check_fail_status.flags.reject_hagl = true;
 
+			} else {
+				// If we have been rejecting range data for too long, reset to measurement
+				if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) {
+					_terrain_vpos = _state.pos(2) + meas_hagl;
+					_terrain_var = obs_variance;
+				} else {
+					_innov_check_fail_status.flags.reject_hagl = true;
+				}
+			}
 		}
 
+
 	} else {
 		_innov_check_fail_status.flags.reject_hagl = true;
 		return;
 	}
 }
 
-// return true if the terrain estimate is valid
+// return true if the terrain height estimate is valid
 bool Ekf::get_terrain_valid()
+{
+		return _hagl_valid;
+}
+
+// determine terrain validity
+void Ekf::update_terrain_valid()
 {
 	if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck &&
 		  (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) {
-		return true;
+		_hagl_valid = true;
-
 	} else {
-		return false;
+		_hagl_valid = false;
 	}
 }