Added debug macro for EKF. Fixed mag state handling which was only partially stored in correct states and not properly reset on init / dynamic reset

2 files changed, 123 insertions, 35 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 2a7dab890..18e17d4d9 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -1,7 +1,37 @@
 #include "estimator.h"
-//#include <stdio.h>
 #include <string.h>
 
+// Define EKF_DEBUG here to enable the debug print calls
+// if the macro is not set, these will be completely
+// optimized out by the compiler.
+//#define EKF_DEBUG
+
+#ifdef EKF_DEBUG
+#include <stdio.h>
+
+static void
+ekf_debug_print(const char *fmt, va_list args)
+{
+    fprintf(stderr, "%s: ", "[ekf]");
+    vfprintf(stderr, fmt, args);
+
+    fprintf(stderr, "\n");
+}
+
+static void
+ekf_debug(const char *fmt, ...)
+{
+    va_list args;
+
+    va_start(args, fmt);
+    ekf_debug_print(fmt, args);
+}
+
+#else
+
+static void ekf_debug(const char *fmt, ...) { while(0){} }
+#endif
+
 float Vector3f::length(void) const
 {
     return sqrt(x*x + y*y + z*z);
@@ -15,6 +45,10 @@ void Vector3f::zero(void)
 }
 
 Mat3f::Mat3f() {
+    identity();
+}
+
+void Mat3f::identity() {
     x.x = 1.0f;
     x.y = 0.0f;
     x.z = 0.0f;
@@ -122,6 +156,7 @@ AttPosEKF::AttPosEKF() :
     storeIndex(0)
 {
     InitialiseParameters();
+    ZeroVariables();
 }
 
 AttPosEKF::~AttPosEKF()
@@ -1219,26 +1254,23 @@ void AttPosEKF::FuseVelposNED()
 
 void AttPosEKF::FuseMagnetometer()
 {
-    static uint8_t obsIndex;
-    static float MagPred[3];
-    static float SH_MAG[9];
-    static float q0 = 0.0f;
-    static float q1 = 0.0f;
-    static float q2 = 0.0f;
-    static float q3 = 1.0f;
-    static float magN = 0.4f;
-    static float magE = 0.0f;
-    static float magD = 0.3f;
-    static float magXbias = 0.0f;
-    static float magYbias = 0.0f;
-    static float magZbias = 0.0f;
-    float R_MAG = sq(magMeasurementSigma);
-    float DCM[3][3] =
-    {
-        {1.0f,0.0f,0.0f} ,
-        {0.0f,1.0f,0.0f} ,
-        {0.0f,0.0f,1.0f}
-    };
+
+    float &q0 = magstate.q0;
+    float &q1 = magstate.q1;
+    float &q2 = magstate.q2;
+    float &q3 = magstate.q3;
+    float &magN = magstate.magN;
+    float &magE = magstate.magE;
+    float &magD = magstate.magD;
+    float &magXbias = magstate.magXbias;
+    float &magYbias = magstate.magYbias;
+    float &magZbias = magstate.magZbias;
+    unsigned &obsIndex = magstate.obsIndex;
+    Mat3f &DCM = magstate.DCM;
+    float *MagPred = &magstate.MagPred[0];
+    float &R_MAG = magstate.R_MAG;
+    float *SH_MAG = &magstate.SH_MAG[0];
+
     float SK_MX[6];
     float SK_MY[5];
     float SK_MZ[6];
@@ -1286,18 +1318,18 @@ void AttPosEKF::FuseMagnetometer()
 
             // rotate predicted earth components into body axes and calculate
             // predicted measurments
-            DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
-            DCM[0][1] = 2*(q1*q2 + q0*q3);
-            DCM[0][2] = 2*(q1*q3-q0*q2);
-            DCM[1][0] = 2*(q1*q2 - q0*q3);
-            DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
-            DCM[1][2] = 2*(q2*q3 + q0*q1);
-            DCM[2][0] = 2*(q1*q3 + q0*q2);
-            DCM[2][1] = 2*(q2*q3 - q0*q1);
-            DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
-            MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE  + DCM[0][2]*magD + magXbias;
-            MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE  + DCM[1][2]*magD + magYbias;
-            MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE  + DCM[2][2]*magD + magZbias;
+            DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+            DCM.x.y = 2*(q1*q2 + q0*q3);
+            DCM.x.z = 2*(q1*q3-q0*q2);
+            DCM.y.x = 2*(q1*q2 - q0*q3);
+            DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+            DCM.y.z = 2*(q2*q3 + q0*q1);
+            DCM.z.x = 2*(q1*q3 + q0*q2);
+            DCM.z.y = 2*(q2*q3 - q0*q1);
+            DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+            MagPred[0] = DCM.x.x*magN + DCM.x.y*magE  + DCM.x.z*magD + magXbias;
+            MagPred[1] = DCM.y.x*magN + DCM.y.y*magE  + DCM.y.z*magD + magYbias;
+            MagPred[2] = DCM.z.x*magN + DCM.z.y*magE  + DCM.z.z*magD + magZbias;
 
             // scale magnetometer observation error with total angular rate
             R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU);
@@ -2206,22 +2238,44 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
 bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
     bool err = false;
 
+    // check all integrators
+    if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
+        err_report->statesNaN = true;
+        ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z);
+        err = true;
+    } // delta angles
+
+    if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
+        err_report->statesNaN = true;
+        ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z);
+        err = true;
+    } // delta angles
+
+    if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
+        err_report->statesNaN = true;
+        ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z);
+        err = true;
+    } // delta velocities
+
     // check all states and covariance matrices
     for (unsigned i = 0; i < n_states; i++) {
         for (unsigned j = 0; j < n_states; j++) {
             if (!isfinite(KH[i][j])) {
 
                 err_report->covarianceNaN = true;
+                ekf_debug("KH NaN");
                 err = true;
             } //  intermediate result used for covariance updates
             if (!isfinite(KHP[i][j])) {
 
                 err_report->covarianceNaN = true;
+                ekf_debug("KHP NaN");
                 err = true;
             } // intermediate result used for covariance updates
             if (!isfinite(P[i][j])) {
 
                 err_report->covarianceNaN = true;
+                ekf_debug("P NaN");
                 err = true;
             } // covariance matrix
         }
@@ -2229,12 +2283,14 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
         if (!isfinite(Kfusion[i])) {
 
             err_report->kalmanGainsNaN = true;
+            ekf_debug("Kfusion NaN");
             err = true;
         } // Kalman gains
 
         if (!isfinite(states[i])) {
 
             err_report->statesNaN = true;
+            ekf_debug("states NaN: i: %u val: %f", i, states[i]);
             err = true;
         } // state matrix
     }
@@ -2264,6 +2320,7 @@ int AttPosEKF::CheckAndBound()
 
     // Reset the filter if the states went NaN
     if (StatesNaN(&last_ekf_error)) {
+        ekf_debug("re-initializing dynamic");
 
         InitializeDynamic(velNED);
 
@@ -2362,7 +2419,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
     initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
     initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
 
-
+    magstate.q3 = 1.0f;
+    magstate.magN = 0.4f;
+    magstate.magE = 0.0f;
+    magstate.magD = 0.3f;
+    magstate.magXbias = 0.0f;
+    magstate.magYbias = 0.0f;
+    magstate.magZbias = 0.0f;
+    magstate.R_MAG = sq(magMeasurementSigma);
+    magstate.DCM.identity();
 
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
@@ -2420,6 +2485,8 @@ void AttPosEKF::ZeroVariables()
     }
 
     correctedDelAng.zero();
+    summedDelAng.zero();
+    summedDelVel.zero();
 
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
@@ -2430,6 +2497,10 @@ void AttPosEKF::ZeroVariables()
         statetimeStamp[i] = 0;
     }
 
+    memset(&magstate, 0, sizeof(magstate));
+    magstate.q0 = 1.0f;
+    magstate.DCM.identity();
+
     memset(&current_ekf_state, 0, sizeof(current_ekf_state));
 }
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h
index 3a8f45d9a..734ad0c05 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -33,6 +33,7 @@ public:
 
     Mat3f();
 
+    void identity();
     Mat3f transpose(void) const;
 };
 
@@ -140,7 +141,23 @@ public:
         accelProcessNoise = 0.5f;
     }
 
-
+    struct {
+        unsigned obsIndex;
+        float MagPred[3];
+        float SH_MAG[9];
+        float q0;
+        float q1;
+        float q2;
+        float q3;
+        float magN;
+        float magE;
+        float magD;
+        float magXbias;
+        float magYbias;
+        float magZbias;
+        float R_MAG;
+        Mat3f DCM;
+    } magstate;
 
 
     // Global variables