diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 02:15:33 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-22 02:15:33 +0200 |
commit | 904ada124baea8ef744535053a0c3b40871565e3 (patch) | |
tree | cdd354b4780dc0d88bbef424e66f7376e37f84e2 | |
parent | 119dfc44e291962ae2340e3cbb6f22e1456fc8b4 (diff) | |
download | px4-firmware-904ada124baea8ef744535053a0c3b40871565e3.tar.gz px4-firmware-904ada124baea8ef744535053a0c3b40871565e3.tar.bz2 px4-firmware-904ada124baea8ef744535053a0c3b40871565e3.zip |
ekf: Put reset statements after variable zero operation to ensure values get initialized correctly
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 8 |
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 |