diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
index 05c452b05d..c165303574 100644
--- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
+++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp
@@ -35,6 +35,7 @@
 
 using matrix::AxisAnglef;
 using matrix::Dcmf;
+using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Vector2f;
 using matrix::Vector3f;
@@ -196,6 +197,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 	const Quatf dq(AxisAnglef{delta_angle_corrected});
 
 	// rotate the previous INS quaternion by the delta quaternions
+	const Quatf quat_nominal_before_update = _output_new.quat_nominal;
 	_output_new.quat_nominal = _output_new.quat_nominal * dq;
 
 	// the quaternions must always be normalised after modification
@@ -244,9 +246,14 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 	}
 
 	// update auxiliary yaw estimate
-	const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled;
-	const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
-	_unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D);
+	// rotate the state quternion by the delta quaternion only corrected for bias without EKF corrections
+	const Vector3f delta_angle_unaided = delta_angle - delta_angle_bias_scaled;
+	const float yaw_state = Eulerf(quat_nominal_before_update).psi();
+	const Quatf quat_unaided = quat_nominal_before_update * Quatf(AxisAnglef(delta_angle_unaided));
+	const float yaw_without_aiding = Eulerf(quat_unaided).psi();
+	// Yaw before delta quaternion applied and yaw after. The difference is the delta yaw. Accumulate it.
+	const float unaided_delta_yaw = yaw_without_aiding - yaw_state;
+	_unaided_yaw = matrix::wrap_pi(_unaided_yaw + unaided_delta_yaw);
 }
 
 void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,