Numerical checks on covariances

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 10e9298ed..e21b94c43 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 <string.h>
+
 // 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<n_states; i++)
         storedStates[i][storeIndex] = states[i];
     statetimeStamp[storeIndex] = timestamp_ms;
@@ -1614,6 +1649,26 @@ void StoreStates(uint64_t timestamp_ms)
         storeIndex = 0;
 }
 
+void ResetStates()
+{
+    // reset all stored states
+    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
+
+    // reset store index to first
+    storeIndex = 0;
+
+    // overwrite all existing states
+    for (unsigned i = 0; i < n_states; i++) {
+        storedStates[i][storeIndex] = states[i];
+    }
+
+    statetimeStamp[storeIndex] = millis();
+
+    // increment to next storage index
+    storeIndex++;
+}
+
 // Output the state vector stored at the time that best matches that specified by msec
 void RecallStates(float statesForFusion[n_states], uint64_t msec)
 {
@@ -1740,6 +1795,9 @@ void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef,
 void OnGroundCheck()
 {
     onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    if (staticMode) {
+        staticMode = !(GPSstatus > 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 43bd69793..b8b218e39 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 e0c2215b6..df2608f83 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[])