diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
index 0f5a17a335..67bfec4ea3 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
@@ -61,8 +61,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 
 // Apply corrections for earths rotation rate and coning errors
 // * and + operators have been overloaded
-    correctedDelAng   = dAngIMU;//correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
-    correctedDelAng.z = 0;
+    correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
 // Convert the rotation vector to its equivalent quaternion
     rotationMag = correctedDelAng.length();
     if (rotationMag < 1e-12f)
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 89e6d3948c..28eab49f22 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2266,7 +2266,7 @@ int AttPosEKF::CheckAndBound()
 
     // Reset the filter if gyro offsets are excessive
     if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) {
-        
+        FillErrorReport(&last_ekf_error);
         InitializeDynamic(velNED, magDeclination);
 
         // that's all we can do here, return