From 03ccee289ba6484a889ed6f79fd744b412bc8537 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 09:19:57 +0100 Subject: [PATCH] Numerical checks on covariances --- .../fw_att_pos_estimator/estimator.cpp | 192 ++++++++++++++++-- src/modules/fw_att_pos_estimator/estimator.h | 11 + .../fw_att_pos_estimator_main.cpp | 5 +- 3 files changed, 184 insertions(+), 24 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 10e9298ed1..e21b94c43f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,5 +1,7 @@ #include "estimator.h" +#include + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -67,9 +69,21 @@ bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused bool fuseMagData = false; // boolean true when magnetometer data is to be fused bool fuseVtasData = false; // boolean true when airspeed data is to be fused -bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) -bool useAirspeed = true; // boolean true if airspeed data is being used -bool useCompass = true; // boolean true if magnetometer data is being used +bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) +bool staticMode = true; ///< boolean true if no position feedback is fused +bool useAirspeed = true; ///< boolean true if airspeed data is being used +bool useCompass = true; ///< boolean true if magnetometer data is being used + +bool velHealth; +bool posHealth; +bool hgtHealth; +bool velTimeout; +bool posTimeout; +bool hgtTimeout; + +bool numericalProtection = true; + +static unsigned storeIndex = 0; float Vector3f::length(void) const { @@ -889,7 +903,7 @@ void CovariancePrediction(float dt) { for (uint8_t i=7; i<=8; i++) { - for (uint8_t j=0; j<=20; j++) + for (unsigned j = 0; j < n_states; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -897,19 +911,39 @@ void CovariancePrediction(float dt) } } - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i]; - for (uint8_t i=1; i<=20; i++) - { - for (uint8_t j=0; j<=i-1; j++) - { - P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i < 14; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 12) || (j > 12)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + ForceSymmetry(); } - // + ConstrainVariances(); } void FuseVelposNED() @@ -923,12 +957,6 @@ void FuseVelposNED() static uint32_t velFailTime = 0; static uint32_t posFailTime = 0; static uint32_t hgtFailTime = 0; - bool velHealth; - bool posHealth; - bool hgtHealth; - bool velTimeout; - bool posTimeout; - bool hgtTimeout; // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; @@ -1137,6 +1165,9 @@ void FuseVelposNED() } } + ForceSymmetry(); + ConstrainVariances(); + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } @@ -1442,6 +1473,9 @@ void FuseMagnetometer() } } obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); } void FuseAirspeed() @@ -1568,6 +1602,9 @@ void FuseAirspeed() } } } + + ForceSymmetry(); + ConstrainVariances(); } void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -1604,8 +1641,6 @@ float sq(float valIn) // Store states in a history array along with time stamp void StoreStates(uint64_t timestamp_ms) { - /* static to keep the store index */ - static uint8_t storeIndex = 0; for (unsigned i=0; i GPS_FIX_2D); + } } void calcEarthRateNED(Vector3f &omega, float latitude) @@ -1783,6 +1841,10 @@ float ConstrainFloat(float val, float min, float max) void ConstrainVariances() { + if (!numericalProtection) { + return; + } + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -1831,6 +1893,10 @@ void ConstrainVariances() void ConstrainStates() { + if (!numericalProtection) { + return; + } + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -1882,6 +1948,88 @@ void ConstrainStates() } +void ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (posTimeout || velTimeout || hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 43bd697934..b8b218e396 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -121,10 +121,19 @@ extern uint8_t GPSstatus; extern float baroHgt; extern bool statesInitialised; +extern bool numericalProtection; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions +extern bool staticMode; + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -175,5 +184,7 @@ void ConstrainVariances(); void ConstrainStates(); +void ForceSymmetry(); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e0c2215b6b..df2608f831 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1050,7 +1050,7 @@ void print_status() printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s %s %s %s %s %s\n", + printf("states: %s %s %s %s %s %s %s %s %s %s\n", (statesInitialised) ? "INITIALIZED" : "NON_INIT", (onGround) ? "ON_GROUND" : "AIRBORNE", (fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1059,7 +1059,8 @@ void print_status() (fuseMagData) ? "FUSE_MAG" : "INH_MAG", (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } int fw_att_pos_estimator_main(int argc, char *argv[])