EKF: Don't use magnetometer with optical flow only nav if GPS checks are failing

EKF/control.cpp

@@ -472,10 +472,11 @@ void Ekf::controlGpsFusion()
 				// If the heading is not aligned, reset the yaw and magnetic field states
 				// Do not use external vision for yaw if using GPS because yaw needs to be
 				// defined relative to an NED reference frame
-				if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) {
+				if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req) {
 					_control_status.flags.yaw_align = false;
 					_control_status.flags.ev_yaw = false;
 					_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+					_mag_inhibit_yaw_reset_req = false;
 				}
 
 				// If the heading is valid start using gps aiding
@@ -1391,6 +1392,23 @@ void Ekf::controlMagFusion()
 			_control_status.flags.mag_dec = false;
 		}
 
+		// If optical flow is the only aiding source and GPS checks are failing, then assume that we are operating
+		// indoors and the magnetometer is unreliable.
+		if (_control_status.flags.opt_flow
+				&& !_control_status.flags.gps
+				&& !_control_status.flags.ev_pos
+				&& ((_last_gps_fail_us - _time_last_imu) < 5E6)) {
+			_mag_use_inhibit = true;
+		} else {
+			_mag_use_inhibit = false;
+			_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
+		}
+
+		// If magnetomer use has been inhibited continuously then a yaw reset is required for a valid heading
+		if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
+			_mag_inhibit_yaw_reset_req = true;
+		}
+
 		// fuse magnetometer data using the selected methods
 		if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
 			fuseMag();

EKF/ekf.h

@@ -322,6 +322,9 @@ private:
 	bool _yaw_angle_observable{false};	///< true when there is enough horizontal acceleration to make yaw observable
 	uint64_t _time_yaw_started{0};		///< last system time in usec that a yaw rotation moaneouvre was detected
 	uint8_t _num_bad_flight_yaw_events{0};	///< number of times a bad heading has been detected in flight and required a yaw reset
+	uint64_t _mag_use_not_inhibit_us{0};	///< last system time in usec before magnetomer use was inhibited
+	bool _mag_use_inhibit{false};		///< true when magnetomer use is being inhibited
+	bool _mag_inhibit_yaw_reset_req{false};	///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve.
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 

EKF/mag_fusion.cpp

@@ -675,7 +675,13 @@ void Ekf::fuseHeading()
 	measured_hdg = wrap_pi(measured_hdg);
 
 	// calculate the innovation
-	_heading_innov = predicted_hdg - measured_hdg;
+	if (_mag_use_inhibit) {
+		// The magnetomer cannot be trusted but we need to fuse a heading to prevent a badly
+		// conditoned covariance matrix developing over time.
+		_heading_innov = 0.0f;
+	} else {
+		_heading_innov = predicted_hdg - measured_hdg;
+	}
 
 	// wrap the innovation to the interval between +-pi
 	_heading_innov = wrap_pi(_heading_innov);

EKF/optflow_fusion.cpp

@@ -500,7 +500,6 @@ void Ekf::fuseOptFlow()
 			fuse(gain, _flow_innov[obs_index]);
 
 			_time_last_of_fuse = _time_last_imu;
-			_gps_check_fail_status.value = 0;
 		}
 	}
 }