fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 17:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

att pos estimator: on reset use projected gps position instead of [0,0,0] to set position state

Browse Source
This commit is contained in:
Thomas Gubler
2014-06-15 12:15:25 +02:00
parent de5c3580b3
commit bd8b071875
1 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+10 -2
View File
@@ -2327,7 +2327,7 @@ int AttPosEKF::CheckAndBound()
if (StatesNaN(&last_ekf_error)) { if (StatesNaN(&last_ekf_error)) {
ekf_debug("re-initializing dynamic"); ekf_debug("re-initializing dynamic");
InitializeDynamic(velNED, magDeclination); InitializeDynamic(velNED, magDeclination);
ret = 1; ret = 1;
} }
@@ -2474,10 +2474,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
magstate.R_MAG = sq(magMeasurementSigma); magstate.R_MAG = sq(magMeasurementSigma);
magstate.DCM = DCM; magstate.DCM = DCM;
// Calculate position from gps measurement
float posNEDInit[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
calcposNED(posNEDInit, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
// write to state vector // write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel // positions:
states[7] = posNEDInit[0];
states[8] = posNEDInit[1];
states[9] = posNEDInit[2];
for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel
states[16] = initMagNED.x; // Magnetic Field North states[16] = initMagNED.x; // Magnetic Field North
states[17] = initMagNED.y; // Magnetic Field East states[17] = initMagNED.y; // Magnetic Field East
states[18] = initMagNED.z; // Magnetic Field Down states[18] = initMagNED.z; // Magnetic Field Down