fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 17:20:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf: Cleanup init

Browse Source
This commit is contained in:
Lorenz Meier
2014-04-30 08:25:36 +02:00
parent d54f9973f2
commit e2af77123f
1 changed files with 1 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator.cpp
+1 -9
View File
@@ -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++) {