diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 679b9f43ff..2398a2de73 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -418,15 +418,7 @@ void Ekf::calculateOutputStates()
 		// calculate a corrrection to the delta angle
 		// that will cause the INS to track the EKF quaternions
 		_delta_angle_corr = delta_ang_error * att_gain;
-
-		// calculate velocity and position tracking errors
-		const Vector3f vel_err(_state.vel - _output_sample_delayed.vel);
-		const Vector3f pos_err(_state.pos - _output_sample_delayed.pos);
-
-		// collect magnitude tracking error for diagnostics
 		_output_tracking_error(0) = delta_ang_error.norm();
-		_output_tracking_error(1) = vel_err.norm();
-		_output_tracking_error(2) = pos_err.norm();
 
 		/*
 		 * Loop through the output filter state history and apply the corrections to the velocity and position states.
@@ -495,6 +487,13 @@ void Ekf::calculateOutputStates()
 			 * the fusion time horizon. This option provides the most accurate tracking of EKF states.
 			 */
 
+			// calculate velocity and position tracking errors
+			const Vector3f vel_err(_state.vel - _output_sample_delayed.vel);
+			const Vector3f pos_err(_state.pos - _output_sample_delayed.pos);
+
+			_output_tracking_error(1) = vel_err.norm();
+			_output_tracking_error(2) = pos_err.norm();
+
 			// calculate a velocity correction that will be applied to the output state history
 			_vel_err_integ += vel_err;
 			const Vector3f vel_correction = vel_err * vel_gain + _vel_err_integ * sq(vel_gain) * 0.1f;