From bd8b071875164f56eb92c8cdc5b4fca92bd7e362 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Jun 2014 12:15:25 +0200 Subject: [PATCH] att pos estimator: on reset use projected gps position instead of [0,0,0] to set position state --- .../ekf_att_pos_estimator/estimator_23states.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index c2282fbb9d..45790063f0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1504,7 +1504,7 @@ void AttPosEKF::FuseAirspeed() SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - + float H_TAS[n_states]; for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; @@ -2327,7 +2327,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED, magDeclination); + InitializeDynamic(velNED, magDeclination); ret = 1; } @@ -2474,10 +2474,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) magstate.R_MAG = sq(magMeasurementSigma); 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 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=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[17] = initMagNED.y; // Magnetic Field East states[18] = initMagNED.z; // Magnetic Field Down