From 7983e105bf1aa6b8cf13ed49dac36c4f1b3a034f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 12 Jun 2014 15:28:21 +0200 Subject: Much more aggressive reset logic bounding the filter effectively --- .../ekf_att_pos_estimator/estimator_23states.cpp | 64 +++++++++++++++++++--- .../ekf_att_pos_estimator/estimator_23states.h | 8 +++ .../ekf_att_pos_estimator/estimator_utilities.h | 8 +-- 3 files changed, 69 insertions(+), 11 deletions(-) (limited to 'src/modules') diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 29b5391ba..de3c9d60e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,5 +1,6 @@ #include "estimator_23states.h" #include +#include #include #define EKF_COVARIANCE_DIVERGED 1.0e8f @@ -33,6 +34,8 @@ AttPosEKF::AttPosEKF() magDeclination = 0.0f; dAngIMU.zero(); dVelIMU.zero(); + ekfDiverged = false; + delAngTotal.zero(); memset(&last_ekf_error, 0, sizeof(last_ekf_error)); ZeroVariables(); @@ -70,6 +73,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z - states[13]; + delAngTotal.x += correctedDelAng.x; + delAngTotal.y += correctedDelAng.y; + delAngTotal.z += correctedDelAng.z; + // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -201,7 +208,8 @@ void AttPosEKF::CovariancePrediction(float dt) float nextP[n_states][n_states]; // calculate covariance prediction process noise - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; @@ -2081,7 +2089,7 @@ void AttPosEKF::ForceSymmetry() if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { - // XXX divergence report error + last_ekf_error.covariancesExcessive = true; InitializeDynamic(velNED, magDeclination); return; } @@ -2117,6 +2125,24 @@ bool AttPosEKF::GyroOffsetsDiverged() return diverged; } +bool AttPosEKF::VelNEDDiverged() +{ + Vector3f current_vel; + current_vel.x = states[4]; + current_vel.y = states[5]; + current_vel.z = states[6]; + + Vector3f gps_vel; + gps_vel.x = velNED[0]; + gps_vel.y = velNED[1]; + gps_vel.z = velNED[2]; + + Vector3f delta = current_vel - gps_vel; + float delta_len = delta.length(); + + return (delta_len > 8.0f); +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2188,6 +2214,7 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { err->states[i] = states[i]; } + err->n_states = n_states; err->velHealth = current_ekf_state.velHealth; err->posHealth = current_ekf_state.posHealth; @@ -2290,13 +2317,19 @@ int AttPosEKF::CheckAndBound() // Store the old filter state bool currStaticMode = staticMode; + if (ekfDiverged) { + ekfDiverged = false; + } + + int ret = 0; + // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED, magDeclination); - return 1; + ret = 1; } // Reset the filter if the IMU data is too old @@ -2308,7 +2341,7 @@ int AttPosEKF::CheckAndBound() ResetStoredStates(); // that's all we can do here, return - return 2; + ret = 2; } // Check if we're on ground - this also sets static mode. @@ -2322,7 +2355,7 @@ int AttPosEKF::CheckAndBound() ResetHeight(); ResetStoredStates(); - return 3; + ret = 3; } // Reset the filter if gyro offsets are excessive @@ -2331,10 +2364,23 @@ int AttPosEKF::CheckAndBound() InitializeDynamic(velNED, magDeclination); // that's all we can do here, return - return 4; + ret = 4; } - return 0; + // Reset the filter if it diverges too far from GPS + if (VelNEDDiverged()) { + FillErrorReport(&last_ekf_error); + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 5; + } + + if (ret) { + ekfDiverged = true; + } + + return ret; } void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) @@ -2386,6 +2432,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + ZeroVariables(); + // Fill variables with valid data velNED[0] = initvelNED[0]; velNED[1] = initvelNED[1]; @@ -2469,6 +2517,7 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables + storeIndex = 0; // Do the data structure init @@ -2486,6 +2535,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 32c514004..1bf1312b0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -2,6 +2,9 @@ #include "estimator_utilities.h" +const unsigned int n_states = 23; +const unsigned int data_buffer_size = 50; + class AttPosEKF { public: @@ -121,6 +124,7 @@ public: 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 + Vector3f delAngTotal; Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -180,6 +184,8 @@ public: bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool ekfDiverged; + struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -281,6 +287,8 @@ bool FilterHealthy(); bool GyroOffsetsDiverged(); +bool VelNEDDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 714dfe623..e5f76d7cd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -46,9 +46,6 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 23; -const unsigned int data_buffer_size = 50; - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, @@ -65,7 +62,8 @@ struct ekf_status_report { uint32_t velFailTime; uint32_t posFailTime; uint32_t hgtFailTime; - float states[n_states]; + float states[32]; + unsigned n_states; bool angNaN; bool summedDelVelNaN; bool KHNaN; @@ -74,6 +72,8 @@ struct ekf_status_report { bool covarianceNaN; bool kalmanGainsNaN; bool statesNaN; + bool gyroOffsetsExcessive; + bool covariancesExcessive; }; void ekf_debug(const char *fmt, ...); \ No newline at end of file -- cgit v1.2.3