Merge remote-tracking branch 'px4/mtecs_estimator' into navigator_rewrite_estimator

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);