fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf_att_pos_estimator: Fixed mag initialization, now starts with initial measurement instead of defaults for faster convergence

Browse Source
This commit is contained in:
Lorenz Meier 2014-04-21 11:02:27 +02:00
parent aa3aafb1e5
commit 595eb679b3
1 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2419,15 +2419,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
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;
magstate.q3 = 1.0f;
magstate.magN = 0.4f;
magstate.magE = 0.0f;
magstate.magD = 0.3f;
magstate.magXbias = 0.0f;
magstate.magYbias = 0.0f;
magstate.magZbias = 0.0f;
magstate.q0 = initQuat[0];
magstate.q1 = initQuat[1];
magstate.q2 = initQuat[2];
magstate.q3 = initQuat[3];
magstate.magN = magData.x;
magstate.magE = magData.y;
magstate.magD = magData.z;
magstate.magXbias = magBias.x;
magstate.magYbias = magBias.y;
magstate.magZbias = magBias.z;
magstate.R_MAG = sq(magMeasurementSigma);
magstate.DCM.identity();
magstate.DCM = DCM;
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions