diff --git a/EKF/control.cpp b/EKF/control.cpp
index e9e6aa5ff4..df97aedd87 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -526,6 +526,7 @@ void Ekf::controlGpsFusion()
 
 		} else {
 			if (ISFINITE(_gps_sample_delayed.yaw)) {
+
 				if (!_control_status.flags.gps_yaw
 				    && _control_status.flags.tilt_align
 				    && !_gps_hgt_intermittent) {
@@ -548,7 +549,8 @@ void Ekf::controlGpsFusion()
 			// Check if the data is constantly fused by the estimator,
 			// if not, declare the sensor faulty and stop the fusion
 			// By doing this, another source of yaw aiding is allowed to start
-			if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
+			if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)
+			    && _control_status.flags.gps_yaw) {
 				_is_gps_yaw_faulty = true;
 				stopGpsYawFusion();
 			}
diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp
index 1d4fc1d1d2..263b16b91f 100644
--- a/test/sensor_simulator/gps.cpp
+++ b/test/sensor_simulator/gps.cpp
@@ -102,7 +102,7 @@ gps_message Gps::getDefaultGpsData()
 	gps_data.lat = 473566094;
 	gps_data.lon = 85190237;
 	gps_data.alt = 422056;
-	gps_data.yaw = 0.0f;
+	gps_data.yaw = NAN;
 	gps_data.yaw_offset = 0.0f;
 	gps_data.fix_type = 3;
 	gps_data.eph = 0.5f;
diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h
index 46016a0e22..4dcfecd702 100644
--- a/test/sensor_simulator/gps.h
+++ b/test/sensor_simulator/gps.h
@@ -66,7 +66,7 @@ public:
 	gps_message getDefaultGpsData();
 
 private:
-	gps_message _gps_data;
+	gps_message _gps_data{};
 	Vector3f _gps_pos_rate{};
 
 	void send(uint64_t time) override;
diff --git a/test/test_EKF_gps_yaw.cpp b/test/test_EKF_gps_yaw.cpp
index 50629470e5..dee81174d2 100644
--- a/test/test_EKF_gps_yaw.cpp
+++ b/test/test_EKF_gps_yaw.cpp
@@ -58,8 +58,10 @@ class EkfGpsHeadingTest : public ::testing::Test {
 	void SetUp() override
 	{
 		_ekf->init(0);
+		_sensor_simulator._gps.setYaw(NAN);
 		_sensor_simulator.runSeconds(2);
 		_ekf_wrapper.enableGpsFusion();
+		_ekf_wrapper.enableGpsHeadingFusion();
 		_sensor_simulator.startGps();
 		_sensor_simulator.runSeconds(11);
 	}
@@ -121,11 +123,13 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
 TEST_F(EkfGpsHeadingTest, fallBackToMag)
 {
 	// GIVEN: an initial GPS yaw, not aligned with the current one
+	// GPS yaw is expected to arrive a bit later, first feed some NANs
+	// to the filter
+	_sensor_simulator.runSeconds(6);
 	float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
 	_sensor_simulator._gps.setYaw(gps_heading);
 
 	// WHEN: the GPS yaw fusion is activated
-	_ekf_wrapper.enableGpsHeadingFusion();
 	_sensor_simulator.runSeconds(1);
 
 	// THEN: GPS heading fusion should have started, and mag