diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-30 08:25:36 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-30 08:25:36 +0200 |
commit | e2af77123fb14e9e19d1aa9e02f04b62a342c724 (patch) | |
tree | 4331467ccffc47d72719b8f0c74fc56bbd9c2c38 | |
parent | d54f9973f26e5503f33ad53fd927696089a742ba (diff) | |
download | px4-firmware-e2af77123fb14e9e19d1aa9e02f04b62a342c724.tar.gz px4-firmware-e2af77123fb14e9e19d1aa9e02f04b62a342c724.tar.bz2 px4-firmware-e2af77123fb14e9e19d1aa9e02f04b62a342c724.zip |
ekf: Cleanup init
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator.cpp | 10 |
1 files changed, 1 insertions, 9 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index e1ffc15d3..a29421045 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2455,7 +2455,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) Mat3f DCM; quat2Tbn(DCM, initQuat); Vector3f initMagNED; - initMagXYZ = magData - magBias; initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; @@ -2497,13 +2496,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -2537,7 +2529,7 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); magBias.zero(); - magState.zero(); + magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { |