From 904ada124baea8ef744535053a0c3b40871565e3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 22 Apr 2014 02:15:33 +0200
Subject: [PATCH] ekf: Put reset statements after variable zero operation to
 ensure values get initialized correctly

---
 src/modules/ekf_att_pos_estimator/estimator.cpp | 8 ++++----
 1 file 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 5f4bf07293..99de161aac 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