ekf: Put reset statements after variable zero operation to ensure values get initialized correctly

1 files changed, 4 insertions, 4 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 5f4bf0729..99de161aa 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -2407,10 +2407,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
 
     ZeroVariables();
 
-    ResetVelocity();
-    ResetPosition();
-    ResetHeight();
-
     // Calculate initial filter quaternion states from raw measurements
     float initQuat[4];
     Vector3f initMagXYZ;
@@ -2452,6 +2448,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
     states[21] = magBias.z; // Magnetic Field Bias Z
     states[22] = 0.0f; // terrain height
 
+    ResetVelocity();
+    ResetPosition();
+    ResetHeight();
+
     statesInitialised = true;
 
     // initialise the covariance matrix