diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-15 12:15:45 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-15 12:15:45 +0200 |
| commit | 2f9415ffd87a09d4ca6d9f54f27306df793cae4d (patch) | |
| tree | 8a5bde28e0dbb94c92004c1735974c8248770340 /src/modules | |
| parent | eda926d7d59e217cb7b5b382032c22b69698aadf (diff) | |
| parent | bd8b071875164f56eb92c8cdc5b4fca92bd7e362 (diff) | |
| download | px4-firmware-2f9415ffd87a09d4ca6d9f54f27306df793cae4d.tar.gz px4-firmware-2f9415ffd87a09d4ca6d9f54f27306df793cae4d.tar.bz2 px4-firmware-2f9415ffd87a09d4ca6d9f54f27306df793cae4d.zip | |
Merge remote-tracking branch 'upstream/mtecs_estimator' into navigator_rewrite_estimator
Diffstat (limited to 'src/modules')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 14 |
1 files 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 c2282fbb9..45790063f 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 |