diff options
author | Julian Oes <julian@oes.ch> | 2014-06-11 13:16:38 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-06-11 13:16:38 +0200 |
commit | 4247fc7585bce0d37f84730b3d8cf644be02c0cc (patch) | |
tree | 453c5c869f9d16283018858e4ce35585f1b2893e /src | |
parent | a89ed8eed90d455ceb7542c1857adfc1a1421d62 (diff) | |
parent | 5602d5dfa3033b0364a80d10aff794f2a92d62c9 (diff) | |
download | px4-firmware-4247fc7585bce0d37f84730b3d8cf644be02c0cc.tar.gz px4-firmware-4247fc7585bce0d37f84730b3d8cf644be02c0cc.tar.bz2 px4-firmware-4247fc7585bce0d37f84730b3d8cf644be02c0cc.zip |
Merge remote-tracking branch 'px4/mtecs_estimator' into navigator_rewrite_estimator
Diffstat (limited to 'src')
3 files changed, 74 insertions, 26 deletions
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1384db9f0..b20b312a2 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1065,6 +1065,7 @@ FixedwingEstimator::task_main() rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + rep.health_flags |= ((!(check == 4)) << 3); rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 28eab49f2..29b5391ba 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2,12 +2,38 @@ #include <string.h> #include <stdarg.h> +#define EKF_COVARIANCE_DIVERGED 1.0e8f + AttPosEKF::AttPosEKF() /* NOTE: DO NOT initialize class members here. Use ZeroVariables() * instead to allow clean in-air re-initialization. */ { + summedDelAng.zero(); + summedDelVel.zero(); + + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + dAngIMU.zero(); + dVelIMU.zero(); + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); InitialiseParameters(); @@ -2052,10 +2078,45 @@ void AttPosEKF::ForceSymmetry() { P[i][j] = 0.5f * (P[i][j] + P[j][i]); P[j][i] = P[i][j]; + + if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || + (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { + // XXX divergence report error + InitializeDynamic(velNED, magDeclination); + return; + } + + float symmetric = 0.5f * (P[i][j] + P[j][i]); + P[i][j] = symmetric; + P[j][i] = symmetric; } } } +bool AttPosEKF::GyroOffsetsDiverged() +{ + // Detect divergence by looking for rapid changes of the gyro offset + Vector3f current_bias; + current_bias.x = states[10]; + current_bias.y = states[11]; + current_bias.z = states[12]; + + Vector3f delta = current_bias - lastGyroOffset; + float delta_len = delta.length(); + float delta_len_scaled = 0.0f; + + // Protect against division by zero + if (delta_len > 0.0f) { + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); + delta_len_scaled = (5e-7 / cov_mag) * delta_len / dtIMU; + } + + bool diverged = (delta_len_scaled > 1.0f); + lastGyroOffset = current_bias; + + return diverged; +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2265,7 +2326,7 @@ int AttPosEKF::CheckAndBound() } // Reset the filter if gyro offsets are excessive - if (fabs(states[10]) > 1.0f || fabsf(states[11]) > 1.0f || fabsf(states[12]) > 1.0f) { + if (GyroOffsetsDiverged()) { FillErrorReport(&last_ekf_error); InitializeDynamic(velNED, magDeclination); @@ -2408,27 +2469,7 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables - fusionModeGPS = 0; - covSkipCount = 0; - statesInitialised = false; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - numericalProtection = true; - refSet = false; storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; // Do the data structure init for (unsigned i = 0; i < n_states; i++) { @@ -2446,9 +2487,6 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); - dAngIMU.zero(); - dVelIMU.zero(); - for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 851e46371..32c514004 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -72,7 +72,7 @@ public: accelProcessNoise = 0.5f; } - struct { + struct mag_state_struct { unsigned obsIndex; float MagPred[3]; float SH_MAG[9]; @@ -88,7 +88,12 @@ public: float magZbias; float R_MAG; Mat3f DCM; - } magstate; + }; + + struct mag_state_struct magstate; + struct mag_state_struct resetMagState; + + // Global variables @@ -97,6 +102,7 @@ public: float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix + float resetStates[n_states]; float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored @@ -114,6 +120,7 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f lastGyroOffset; // Last gyro offset Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -272,6 +279,8 @@ protected: bool FilterHealthy(); +bool GyroOffsetsDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); |