diff --git a/EKF/control.cpp b/EKF/control.cpp
index a23ffe2414..9ebd9976e0 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -518,28 +518,31 @@ void Ekf::controlGpsFusion()
 		// Detect if coming back after significant time without GPS data
 		bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
-		// GPS yaw aiding selection logic
-		if ((_params.fusion_mode & MASK_USE_GPSYAW)
-				&& ISFINITE(_gps_sample_delayed.yaw)
-				&& _control_status.flags.tilt_align
-				&& !_control_status.flags.gps_yaw
-				&& !_gps_hgt_intermittent) {
+		if (!(_params.fusion_mode & MASK_USE_GPSYAW)) {
+			stopGpsYawFusion();
 
-			if (resetGpsAntYaw()) {
-				// flag the yaw as aligned
-				_control_status.flags.yaw_align = true;
+		} else {
+			if (ISFINITE(_gps_sample_delayed.yaw)) {
+				if (!_control_status.flags.gps_yaw
+				    && _control_status.flags.tilt_align
+				    && !_gps_hgt_intermittent) {
 
-				startGpsYawFusion();
+					// Activate GPS yaw fusion
+					if (resetGpsAntYaw()) {
+						_control_status.flags.yaw_align = true;
 
-				ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
+						startGpsYawFusion();
+
+						ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
+					}
+				}
+
+				if (_control_status.flags.gps_yaw) {
+					fuseGpsAntYaw();
+				}
 			}
 		}
 
-		// fuse the yaw observation
-		if (_control_status.flags.gps_yaw) {
-			fuseGpsAntYaw();
-		}
-
 		// Determine if we should use GPS aiding for velocity and horizontal position
 		// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
 		bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
diff --git a/EKF/ekf.h b/EKF/ekf.h
index bc4b6ba71e..6e5108ab61 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -351,6 +351,8 @@ private:
 	uint64_t _time_last_beta_fuse{0};	///< time the last fusion of synthetic sideslip measurements were performed (uSec)
 	uint64_t _time_last_fake_pos{0};	///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
 
+	uint64_t _time_last_gps_yaw_fuse{0};	///< time the last fusion of GPS yaw measurements were performed (uSec)
+
 	Vector2f _last_known_posNE;		///< last known local NE position vector (m)
 	float _imu_collection_time_adj{0.0f};	///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
 
diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp
index ca713ccdb6..7fdbe45548 100644
--- a/EKF/gps_yaw_fusion.cpp
+++ b/EKF/gps_yaw_fusion.cpp
@@ -58,83 +58,76 @@ void Ekf::fuseGpsAntYaw()
 	float H_YAW[4];
 	float measured_hdg;
 
-	// check if data has been set to NAN indicating no measurement
-	if (ISFINITE(_gps_sample_delayed.yaw)) {
-		// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-		measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
+	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
+	measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
 
-		// define the predicted antenna array vector and rotate into earth frame
-		Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-		Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
+	// define the predicted antenna array vector and rotate into earth frame
+	Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+	Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
-		// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-		if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
-			return;
-		}
-
-		// calculate predicted antenna yaw angle
-		predicted_hdg =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
-
-		// calculate observation jacobian
-		float t2 = sinf(_gps_yaw_offset);
-		float t3 = cosf(_gps_yaw_offset);
-		float t4 = q0*q3*2.0f;
-		float t5 = q0*q0;
-		float t6 = q1*q1;
-		float t7 = q2*q2;
-		float t8 = q3*q3;
-		float t9 = q1*q2*2.0f;
-		float t10 = t5+t6-t7-t8;
-		float t11 = t3*t10;
-		float t12 = t4+t9;
-		float t13 = t3*t12;
-		float t14 = t5-t6+t7-t8;
-		float t15 = t2*t14;
-		float t16 = t13+t15;
-		float t17 = t4-t9;
-		float t19 = t2*t17;
-		float t20 = t11-t19;
-		float t18 = (t20*t20);
-		if (t18 < 1e-6f) {
-			return;
-		}
-		t18 = 1.0f / t18;
-		float t21 = t16*t16;
-		float t22 = sq(t11-t19);
-		if (t22 < 1e-6f) {
-			return;
-		}
-		t22 = 1.0f/t22;
-		float t23 = q1*t3*2.0f;
-		float t24 = q2*t2*2.0f;
-		float t25 = t23+t24;
-		float t26 = 1.0f/t20;
-		float t27 = q1*t2*2.0f;
-		float t28 = t21*t22;
-		float t29 = t28+1.0f;
-		if (fabsf(t29) < 1e-6f) {
-			return;
-		}
-		float t30 = 1.0f/t29;
-		float t31 = q0*t3*2.0f;
-		float t32 = t31-q3*t2*2.0f;
-		float t33 = q3*t3*2.0f;
-		float t34 = q0*t2*2.0f;
-		float t35 = t33+t34;
-
-		H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
-		H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
-		H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
-		H_YAW[3] = t30*(t26*t32+t16*t22*t35);
-
-		// using magnetic heading tuning parameter
-		R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
-
-	} else {
-		// there is nothing to fuse
+	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
 		return;
 	}
 
+	// calculate predicted antenna yaw angle
+	predicted_hdg =  atan2f(ant_vec_ef(1),ant_vec_ef(0));
+
+	// calculate observation jacobian
+	float t2 = sinf(_gps_yaw_offset);
+	float t3 = cosf(_gps_yaw_offset);
+	float t4 = q0*q3*2.0f;
+	float t5 = q0*q0;
+	float t6 = q1*q1;
+	float t7 = q2*q2;
+	float t8 = q3*q3;
+	float t9 = q1*q2*2.0f;
+	float t10 = t5+t6-t7-t8;
+	float t11 = t3*t10;
+	float t12 = t4+t9;
+	float t13 = t3*t12;
+	float t14 = t5-t6+t7-t8;
+	float t15 = t2*t14;
+	float t16 = t13+t15;
+	float t17 = t4-t9;
+	float t19 = t2*t17;
+	float t20 = t11-t19;
+	float t18 = (t20*t20);
+	if (t18 < 1e-6f) {
+		return;
+	}
+	t18 = 1.0f / t18;
+	float t21 = t16*t16;
+	float t22 = sq(t11-t19);
+	if (t22 < 1e-6f) {
+		return;
+	}
+	t22 = 1.0f/t22;
+	float t23 = q1*t3*2.0f;
+	float t24 = q2*t2*2.0f;
+	float t25 = t23+t24;
+	float t26 = 1.0f/t20;
+	float t27 = q1*t2*2.0f;
+	float t28 = t21*t22;
+	float t29 = t28+1.0f;
+	if (fabsf(t29) < 1e-6f) {
+		return;
+	}
+	float t30 = 1.0f/t29;
+	float t31 = q0*t3*2.0f;
+	float t32 = t31-q3*t2*2.0f;
+	float t33 = q3*t3*2.0f;
+	float t34 = q0*t2*2.0f;
+	float t35 = t33+t34;
+
+	H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
+	H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
+	H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
+	H_YAW[3] = t30*(t26*t32+t16*t22*t35);
+
+	// using magnetic heading tuning parameter
+	R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+
 	// wrap the heading to the interval between +-pi
 	measured_hdg = wrap_pi(measured_hdg);
 
@@ -210,7 +203,6 @@ void Ekf::fuseGpsAntYaw()
 	// we are no longer using 3-axis fusion so set the reported test levels to zero
 	memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
 
-	// set the magnetometer unhealthy if the test fails
 	if (_yaw_test_ratio > 1.0f) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 
@@ -273,6 +265,7 @@ void Ekf::fuseGpsAntYaw()
 
 	// only apply covariance and state corrections if healthy
 	if (healthy) {
+		_time_last_gps_yaw_fuse = _time_last_imu;
 		// apply the covariance corrections
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
@@ -291,26 +284,20 @@ void Ekf::fuseGpsAntYaw()
 
 bool Ekf::resetGpsAntYaw()
 {
-	// check if data has been set to NAN indicating no measurement
-	if (ISFINITE(_gps_sample_delayed.yaw)) {
+	// define the predicted antenna array vector and rotate into earth frame
+	const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
+	const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
 
-		// define the predicted antenna array vector and rotate into earth frame
-		const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
-		const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
-
-		// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
-		if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
-			return false;
-		}
-
-		// get measurement and correct for antenna array yaw offset
-		const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
-
-		const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
-		resetQuatStateYaw(measured_yaw, yaw_variance, true);
-
-		return true;
+	// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
+	if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f)))  {
+		return false;
 	}
 
-	return false;
+	// get measurement and correct for antenna array yaw offset
+	const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
+
+	const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
+	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+
+	return true;
 }