EKF: Handle flow data without valid gyro data

EKF/common.h

@@ -139,7 +139,6 @@ struct airspeedSample {
 struct flowSample {
 	uint8_t  quality;	///< quality indicator between 0 and 255
 	Vector2f flowRadXY;	///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
-	Vector2f flowRadXYcomp;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 	Vector3f gyroXYZ;	///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
 	float    dt;		///< amount of integration time (sec)
 	uint64_t time_us;	///< timestamp of the integration period mid-point (uSec)

EKF/control.cpp

@@ -455,16 +455,28 @@ void Ekf::controlOpticalFlowFusion()
 			}
 		}
 
-		// fuse the data if the terrain/distance to bottom is valid but use a more relaxed check to enable it to survive bad range finder data
-		if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
-			// Update optical flow bias estimates
-			calcOptFlowBias();
+		// Only fuse optical flow if valid body rate compensation data is available
+		if (calcOptFlowBodyRateComp()) {
 
-			// Fuse optical flow LOS rate observations into the main filter
-			fuseOptFlow();
-			_last_known_posNE(0) = _state.pos(0);
-			_last_known_posNE(1) = _state.pos(1);
+			bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min);
 
+			if (!flow_quality_good && !_control_status.flags.in_air) {
+				// when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate
+				_flowRadXYcomp.zero();
+
+			} else {
+				// compensate for body motion to give a LOS rate
+				_flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0);
+				_flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1);
+			}
+
+
+			// Fuse optical flow LOS rate observations into the main filter if height above ground has been updated recently but use a relaxed time criteria to enable it to coast through bad range finder data
+			if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
+				fuseOptFlow();
+				_last_known_posNE(0) = _state.pos(0);
+				_last_known_posNE(1) = _state.pos(1);
+			}
 		}
 	}
 

EKF/ekf.h

@@ -355,6 +355,7 @@ class Ekf : public EstimatorInterface
 	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_flow_use{false};	///< true when use of optical flow and range finder is being inhibited
+	Vector2f _flowRadXYcomp;	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
 
 	float _mag_declination{0.0f};	///< magnetic declination used by reset and fusion functions (rad)
 
@@ -486,8 +487,9 @@ class Ekf : public EstimatorInterface
 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();
 
-	// calculate optical flow bias errors
-	void calcOptFlowBias();
+	// calculate optical flow body angular rate compensation
+	// returns false if bias corrected body rate data is unavailable
+	bool calcOptFlowBodyRateComp();
 
 	// initialise the terrain vertical position estimator
 	// return true if the initialisation is successful

EKF/ekf_helper.cpp

@@ -71,8 +71,8 @@ bool Ekf::resetVelocity()
 			// 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(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt;
+			vel_optflow_body(1) =   range * _flowRadXYcomp(0) / _flow_sample_delayed.dt;
 			vel_optflow_body(2) = 0.0f;
 
 			// rotate from body to earth frame

EKF/estimator_interface.cpp

@@ -405,20 +405,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
 			// copy the optical and gyro measured delta angles
 			optflow_sample_new.gyroXYZ = - flow->gyrodata;
-
-			if (flow_quality_good) {
-				optflow_sample_new.flowRadXY = - flow->flowdata;
-
-			} else {
-				// when on the ground with poor flow quality, assume zero ground relative velocity
-				optflow_sample_new.flowRadXY(0) = - flow->gyrodata(0);
-				optflow_sample_new.flowRadXY(1) = - flow->gyrodata(1);
-
-			}
-
-			// compensate for body motion to give a LOS rate
-			optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
-			optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);
+			optflow_sample_new.flowRadXY = - flow->flowdata;
 
 			// convert integration interval to seconds
 			optflow_sample_new.dt = delta_time;

EKF/optflow_fusion.cpp

@@ -93,8 +93,8 @@ void Ekf::fuseOptFlow()
 	// correct for gyro bias errors in the data used to do the motion compensation
 	// Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis.
 	Vector2f opt_flow_rate;
-	opt_flow_rate(0) = _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
-	opt_flow_rate(1) = _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
+	opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
+	opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
 
 	if (opt_flow_rate.norm() < _flow_max_rate) {
 		_flow_innov[0] =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
@@ -527,39 +527,51 @@ void Ekf::get_drag_innov_var(float drag_innov_var[2])
 	memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var));
 }
 
-// calculate optical flow gyro bias errors
-void Ekf::calcOptFlowBias()
+// calculate optical flow body angular rate compensation
+// returns false if bias corrected body rate data is unavailable
+bool Ekf::calcOptFlowBodyRateComp()
 {
 	// reset the accumulators if the time interval is too large
 	if (_delta_time_of > 1.0f) {
 		_imu_del_ang_of.setZero();
 		_delta_time_of = 0.0f;
-		return;
+		return false;
 	}
 
-	// if accumulation time differences are not excessive and accumulation time is adequate
-	// compare the optical flow and and navigation rate data and calculate a bias error
-	if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
-		// calculate a reference angular rate
-		Vector3f reference_body_rate;
-		reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
-
-		// calculate the optical flow sensor measured body rate
-		Vector3f of_body_rate;
-		of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
-
-		// calculate the bias estimate using  a combined LPF and spike filter
-		_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
-				     -0.1f, 0.1f);
-		_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
-				     -0.1f, 0.1f);
-		_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
-				     -0.1f, 0.1f);
+	bool use_flow_sensor_gyro =  ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
+
+	if (use_flow_sensor_gyro) {
+
+		// if accumulation time differences are not excessive and accumulation time is adequate
+		// compare the optical flow and and navigation rate data and calculate a bias error
+		if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
+			// calculate a reference angular rate
+			Vector3f reference_body_rate;
+			reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
+
+			// calculate the optical flow sensor measured body rate
+			Vector3f of_body_rate;
+			of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
+
+			// calculate the bias estimate using  a combined LPF and spike filter
+			_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
+					     -0.1f, 0.1f);
+			_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
+					     -0.1f, 0.1f);
+			_flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
+					     -0.1f, 0.1f);
+		}
+
+	} else {
+		// Use the EKF gyro data if optical flow sensor gyro data is not available
+		_flow_sample_delayed.gyroXYZ = _imu_del_ang_of;
+		_flow_gyro_bias.zero();
 	}
 
 	// reset the accumulators
 	_imu_del_ang_of.setZero();
 	_delta_time_of = 0.0f;
+	return true;
 }
 
 // calculate the measurement variance for the optical flow sensor (rad/sec)^2