ekf2: stop gps yaw fusion on numerical error

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -194,6 +194,8 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 
 		_fault_status.flags.bad_airspeed = true;
 
+		stopAirspeedFusion();
+
 		return;
 	}
 

src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp

@@ -100,6 +100,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 		// we reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
 		ECL_ERR("GPS yaw numerical error - covariance reset");
+		stopGpsYawFusion();
 		return;
 	}
 

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -101,6 +101,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 
 			resetMagCov();
 
+			stopMagFusion();
+
 			return false;
 		}
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -63,6 +63,7 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			// we need to reinitialise the covariance matrix and abort this fusion step
 			ECL_ERR("Opt flow error - covariance reset");
 			initialiseCovariance();
+			stopFlowFusion();
 			return false;
 		}