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/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);