dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Lorenz Meier 2014-04-22 02:15:33 +02:00
parent 119dfc44e2
commit 904ada124b
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf_att_pos_estimator/estimator.cpp
View File

@ -2407,10 +2407,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
ZeroVariables(); ZeroVariables();
ResetVelocity();
ResetPosition();
ResetHeight();
// Calculate initial filter quaternion states from raw measurements // Calculate initial filter quaternion states from raw measurements
float initQuat[4]; float initQuat[4];
Vector3f initMagXYZ; Vector3f initMagXYZ;
@ -2452,6 +2448,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
states[21] = magBias.z; // Magnetic Field Bias Z states[21] = magBias.z; // Magnetic Field Bias Z
states[22] = 0.0f; // terrain height states[22] = 0.0f; // terrain height
ResetVelocity();
ResetPosition();
ResetHeight();
statesInitialised = true; statesInitialised = true;
// initialise the covariance matrix // initialise the covariance matrix