EKF: Fix on ground yaw drift when using EKF2_MAG_TYPE = 4

EKF/control.cpp

@@ -1582,7 +1582,7 @@ void Ekf::controlMagFusion()
 				save_mag_cov_data();
 			}
 
-		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
+		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING || _params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) {
 			// always use heading fusion
 			_control_status.flags.mag_hdg = true;
 

EKF/ekf.h

@@ -359,7 +359,6 @@ private:
 	bool _mag_use_inhibit_prev{false};	///< true when magnetometer use was being inhibited the previous frame
 	bool _mag_inhibit_yaw_reset_req{false};	///< true when magnetometer inhibit has been active for long enough to require a yaw reset when conditions improve.
 	float _last_static_yaw{0.0f};		///< last yaw angle recorded when on ground motion checks were passing (rad)
-	bool _vehicle_at_rest_prev{false};	///< true when the vehicle was at rest the previous time the status was checked
 	bool _mag_yaw_reset_req{false};		///< true when a reset of the yaw using the magnetometer data has been requested
 	bool _mag_decl_cov_reset{false};	///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
 	bool _synthetic_mag_z_active{false};	///< true if we are generating synthetic magnetometer Z measurements

EKF/mag_fusion.cpp

@@ -511,9 +511,15 @@ void Ekf::fuseHeading()
 			float Tbn_0_0 = sq(_ev_sample_delayed.quat(0))+sq(_ev_sample_delayed.quat(1))-sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
 			measured_hdg = atan2f(Tbn_1_0,Tbn_0_0);
 
+		} else if (_mag_use_inhibit) {
+			// Special case where we either use the current or last known stationary value
+			// so set to current value as a safe default
+			measured_hdg = predicted_hdg;
+
 		} else {
-			// there is no yaw observation
+			// Should not be doing yaw fusion
 			return;
+
 		}
 
 	} else {
@@ -604,8 +610,13 @@ void Ekf::fuseHeading()
 			float Tbn_1_1 = sq(_ev_sample_delayed.quat(0))-sq(_ev_sample_delayed.quat(1))+sq(_ev_sample_delayed.quat(2))-sq(_ev_sample_delayed.quat(3));
 			measured_hdg = atan2f(Tbn_0_1_neg,Tbn_1_1);
 
+		} else if (_mag_use_inhibit) {
+			// Special case where we either use the current or last known stationary value
+			// so set to current value as a safe default
+			measured_hdg = predicted_hdg;
+
 		} else {
-			// there is no yaw observation
+			// Should not be doing yaw fusion
 			return;
 
 		}
@@ -621,8 +632,8 @@ void Ekf::fuseHeading()
 		R_YAW = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));
 
 	} else {
-		// there is no yaw observation
-		return;
+		// default value
+		R_YAW = 0.01f;
 	}
 
 	// wrap the heading to the interval between +-pi
@@ -637,22 +648,20 @@ void Ekf::fuseHeading()
 			// Vehicle is not at rest so fuse a zero innovation and record the
 			// predicted heading to use as an observation when movement ceases.
 			_heading_innov = 0.0f;
-			_vehicle_at_rest_prev = false;
+			_last_static_yaw = predicted_hdg;
+
 		} else {
 			// Vehicle is at rest so use the last moving prediction as an observation
 			// to prevent the heading from drifting and to enable yaw gyro bias learning
 			// before takeoff.
-			if (!_vehicle_at_rest_prev || !_mag_use_inhibit_prev) {
-				_last_static_yaw = predicted_hdg;
-				_vehicle_at_rest_prev = true;
-			}
 			_heading_innov = predicted_hdg - _last_static_yaw;
-			R_YAW = 0.01f;
 			innov_gate = 5.0f;
+
 		}
 	} else {
 		_heading_innov = predicted_hdg - measured_hdg;
 		_last_static_yaw = predicted_hdg;
+
 	}
 	_mag_use_inhibit_prev = _mag_use_inhibit;