Added EKF filter health status reporting, added dynamic in-air reset.

5 files changed, 328 insertions, 59 deletions

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index e6f1fee87..bde46ba03 100644
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -74,12 +74,8 @@ 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;
+struct ekf_status_report current_ekf_state;
+struct ekf_status_report last_ekf_error;
 
 bool numericalProtection = true;
 
@@ -965,9 +961,6 @@ void FuseVelposNED()
     uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
     uint32_t hgtRetryTime = 5000; // height measurement retry time
     uint32_t horizRetryTime;
-    static uint32_t velFailTime = 0;
-    static uint32_t posFailTime = 0;
-    static uint32_t hgtFailTime = 0;
 
 // declare variables used to check measurement errors
     float velInnov[3] = {0.0f,0.0f,0.0f};
@@ -1033,16 +1026,16 @@ void FuseVelposNED()
                 varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
             }
             // apply a 5-sigma threshold
-            velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
-            velTimeout = (millis() - velFailTime) > horizRetryTime;
-            if (velHealth || velTimeout)
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
+            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
             {
-                velHealth = true;
-                velFailTime = millis();
+                current_ekf_state.velHealth = true;
+                current_ekf_state.velFailTime = millis();
             }
             else
             {
-                velHealth = false;
+                current_ekf_state.velHealth = false;
             }
         }
         if (fusePosData)
@@ -1053,16 +1046,16 @@ void FuseVelposNED()
             varInnovVelPos[3] = P[7][7] + R_OBS[3];
             varInnovVelPos[4] = P[8][8] + R_OBS[4];
             // apply a 10-sigma threshold
-            posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
-            posTimeout = (millis() - posFailTime) > horizRetryTime;
-            if (posHealth || posTimeout)
+            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
+            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
+            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
             {
-                posHealth = true;
-                posFailTime = millis();
+                current_ekf_state.posHealth = true;
+                current_ekf_state.posFailTime = millis();
             }
             else
             {
-                posHealth = false;
+                current_ekf_state.posHealth = false;
             }
         }
         // test height measurements
@@ -1071,37 +1064,37 @@ void FuseVelposNED()
             hgtInnov = statesAtHgtTime[9] + hgtMea;
             varInnovVelPos[5] = P[9][9] + R_OBS[5];
             // apply a 10-sigma threshold
-            hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
-            hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime;
-            if (hgtHealth || hgtTimeout)
+            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
+            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
             {
-                hgtHealth = true;
-                hgtFailTime = millis();
+                current_ekf_state.hgtHealth = true;
+                current_ekf_state.hgtFailTime = millis();
             }
             else
             {
-                hgtHealth = false;
+                current_ekf_state.hgtHealth = false;
             }
         }
         // Set range for sequential fusion of velocity and position measurements depending
         // on which data is available and its health
-        if (fuseVelData && fusionModeGPS == 0 && velHealth)
+        if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
         {
             fuseData[0] = true;
             fuseData[1] = true;
             fuseData[2] = true;
         }
-        if (fuseVelData && fusionModeGPS == 1 && velHealth)
+        if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
         {
             fuseData[0] = true;
             fuseData[1] = true;
         }
-        if (fusePosData && fusionModeGPS <= 2 && posHealth)
+        if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
         {
             fuseData[3] = true;
             fuseData[4] = true;
         }
-        if (fuseHgtData && hgtHealth)
+        if (fuseHgtData && current_ekf_state.hgtHealth)
         {
             fuseData[5] = true;
         }
@@ -1986,7 +1979,7 @@ bool FilterHealthy()
     // XXX Check state vector for NaNs and ill-conditioning
 
     // Check if any of the major inputs timed out
-    if (posTimeout || velTimeout || hgtTimeout) {
+    if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
         return false;
     }
 
@@ -2042,6 +2035,65 @@ void ResetVelocity(void)
 }
 
 
+void FillErrorReport(struct ekf_status_report *err)
+{
+    for (int i = 0; i < n_states; i++)
+    {
+        err->states[i] = states[i];
+    }
+
+    err->velHealth = current_ekf_state.velHealth;
+    err->posHealth = current_ekf_state.posHealth;
+    err->hgtHealth = current_ekf_state.hgtHealth;
+    err->velTimeout = current_ekf_state.velTimeout;
+    err->posTimeout = current_ekf_state.posTimeout;
+    err->hgtTimeout = current_ekf_state.hgtTimeout;
+}
+
+bool StatesNaN(struct ekf_status_report *err_report) {
+    bool err = false;
+
+    // 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;
+                err = true;
+            } //  intermediate result used for covariance updates
+            if (!isfinite(KHP[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // intermediate result used for covariance updates
+            if (!isfinite(P[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // covariance matrix
+        }
+
+        if (!isfinite(Kfusion[i])) {
+
+            err_report->kalmanGainsNaN = true;
+            err = true;
+        } // Kalman gains
+
+        if (!isfinite(states[i])) {
+
+            err_report->statesNaN = true;
+            err = true;
+        } // state matrix
+    }
+
+    if (err) {
+        FillErrorReport(err_report);
+    }
+
+    return err;
+
+}
+
 /**
  * Check the filter inputs and bound its operational state
  *
@@ -2051,21 +2103,30 @@ void ResetVelocity(void)
  * updated, but before any of the fusion steps are
  * executed.
  */
-void CheckAndBound()
+int CheckAndBound()
 {
 
     // Store the old filter state
     bool currStaticMode = staticMode;
 
+    // Reset the filter if the states went NaN
+    if (StatesNaN(&last_ekf_error)) {
+
+        InitializeDynamic(velNED);
+
+        return 1;
+    }
+
     // Reset the filter if the IMU data is too old
     if (dtIMU > 0.2f) {
+
         ResetVelocity();
         ResetPosition();
         ResetHeight();
         ResetStoredStates();
 
         // that's all we can do here, return
-        return;
+        return 2;
     }
 
     // Check if we're on ground - this also sets static mode.
@@ -2077,7 +2138,11 @@ void CheckAndBound()
         ResetPosition();
         ResetHeight();
         ResetStoredStates();
+
+        return 3;
     }
+
+    return 0;
 }
 
 void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
@@ -2116,28 +2181,13 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
     initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
 }
 
-void InitialiseFilter(float (&initvelNED)[3])
+void InitializeDynamic(float (&initvelNED)[3])
 {
-    // Do the data structure init
-    for (unsigned i = 0; i < n_states; i++) {
-        for (unsigned j = 0; j < n_states; j++) {
-            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
-            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
-            P[i][j] = 0.0f; // covariance matrix
-        }
-
-        Kfusion[i] = 0.0f; // Kalman gains
-        states[i] = 0.0f; // state matrix
-    }
-
-    for (unsigned i = 0; i < data_buffer_size; i++) {
 
-        for (unsigned j = 0; j < n_states; j++) {
-            storedStates[j][i] = 0.0f;
-        }
+    // Clear the init flag
+    statesInitialised = false;
 
-        statetimeStamp[i] = 0;
-    }
+    ZeroVariables();
 
     // Calculate initial filter quaternion states from raw measurements
     float initQuat[4];
@@ -2155,10 +2205,7 @@ void InitialiseFilter(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;
 
-    //store initial lat,long and height
-    latRef = gpsLat;
-    lonRef = gpsLon;
-    hgtRef = gpsHgt;
+
 
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
@@ -2187,3 +2234,51 @@ void InitialiseFilter(float (&initvelNED)[3])
     summedDelVel.y = 0.0f;
     summedDelVel.z = 0.0f;
 }
+
+void InitialiseFilter(float (&initvelNED)[3])
+{
+    //store initial lat,long and height
+    latRef = gpsLat;
+    lonRef = gpsLon;
+    hgtRef = gpsHgt;
+
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+
+    InitializeDynamic(initvelNED);
+}
+
+void ZeroVariables()
+{
+    // Do the data structure init
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
+            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+            P[i][j] = 0.0f; // covariance matrix
+        }
+
+        Kfusion[i] = 0.0f; // Kalman gains
+        states[i] = 0.0f; // state matrix
+    }
+
+    for (unsigned i = 0; i < data_buffer_size; i++) {
+
+        for (unsigned j = 0; j < n_states; j++) {
+            storedStates[j][i] = 0.0f;
+        }
+
+        statetimeStamp[i] = 0;
+    }
+
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+}
+
+void GetFilterState(struct ekf_status_report *state)
+{
+    memcpy(state, &current_ekf_state, sizeof(state));
+}
+
+void GetLastErrorState(struct ekf_status_report *last_error)
+{
+    memcpy(last_error, &last_ekf_error, sizeof(last_error));
+}
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
index 256aff771..55e6eb12e 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -134,6 +134,22 @@ enum GPS_FIX {
     GPS_FIX_3D = 3
 };
 
+struct ekf_status_report {
+    bool velHealth;
+    bool posHealth;
+    bool hgtHealth;
+    bool velTimeout;
+    bool posTimeout;
+    bool hgtTimeout;
+    uint32_t velFailTime;
+    uint32_t posFailTime;
+    uint32_t hgtFailTime;
+    float states[n_states];
+    bool statesNaN;
+    bool covarianceNaN;
+    bool kalmanGainsNaN;
+};
+
 void  UpdateStrapdownEquationsNED();
 
 void CovariancePrediction(float dt);
@@ -188,11 +204,22 @@ void ConstrainStates();
 
 void ForceSymmetry();
 
-void CheckAndBound();
+int CheckAndBound();
 
 void ResetPosition();
 
 void ResetVelocity();
 
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN(struct ekf_status_report *err_report);
+void FillErrorReport(struct ekf_status_report *err);
+
+void InitializeDynamic(float (&initvelNED)[3]);
+
 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 e46dffde3..e75b844fd 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
@@ -71,6 +71,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/estimator_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -144,6 +145,7 @@ private:
 	orb_advert_t	_att_pub;			/**< vehicle attitude */
 	orb_advert_t	_global_pos_pub;		/**< global position */
 	orb_advert_t	_local_pos_pub;			/**< position in local frame */
+	orb_advert_t	_estimator_status_pub;		/**< status of the estimator */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct gyro_report				_gyro;
@@ -260,6 +262,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_att_pub(-1),
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
+	_estimator_status_pub(-1),
 
 	_baro_ref(0.0f),
 	_baro_gps_offset(0.0f),
@@ -701,7 +704,67 @@ FixedwingEstimator::task_main()
 			/**
 			 *    CHECK IF THE INPUT DATA IS SANE
 			 */
-			CheckAndBound();
+			int check = CheckAndBound();
+
+			switch (check) {
+				case 0:
+					/* all ok */
+					break;
+				case 1:
+				{
+					const char* str = "NaN in states, resetting";
+					warnx(str);
+					mavlink_log_critical(_mavlink_fd, str);
+					break;
+				}
+				case 2:
+				{
+					const char* str = "stale IMU data, resetting";
+					warnx(str);
+					mavlink_log_critical(_mavlink_fd, str);
+					break;
+				}
+				case 3:
+				{
+					const char* str = "switching dynamic / static state";
+					warnx(str);
+					mavlink_log_critical(_mavlink_fd, str);
+					break;
+				}
+			}
+
+			// If non-zero, we got a problem
+			if (check) {
+
+				struct ekf_status_report ekf_report;
+
+				GetLastErrorState(&ekf_report);
+
+				struct estimator_status_report rep;
+				memset(&rep, 0, sizeof(rep));
+				rep.timestamp = hrt_absolute_time();
+
+				rep.states_nan = ekf_report.statesNaN;
+				rep.covariance_nan = ekf_report.covarianceNaN;
+				rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
+
+				// Copy all states or at least all that we can fit
+				int i = 0;
+				unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
+				unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
+				rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
+
+				while ((i < ekf_n_states) && (i < max_states)) {
+
+					rep.states[i] = ekf_report.states[i];
+				}
+
+				if (_estimator_status_pub > 0) {
+					orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+				} else {
+					_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
+				}
+			}
 
 
 			/**
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index fb24de8d1..4b31cc8a4 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -193,3 +193,6 @@ ORB_DEFINE(esc_status, struct esc_status_s);
 
 #include "topics/encoders.h"
 ORB_DEFINE(encoders, struct encoders_s);
+
+#include "topics/estimator_status.h"
+ORB_DEFINE(estimator_status, struct estimator_status_report);
diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h
new file mode 100644
index 000000000..66ca03019
--- /dev/null
+++ b/src/modules/uORB/topics/estimator_status.h
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file estimator_status.h
+ * Definition of the estimator_status_report uORB topic.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#ifndef ESTIMATOR_STATUS_H_
+#define ESTIMATOR_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Estimator status report.
+ *
+ * This is a generic status report struct which allows any of the onboard estimators
+ * to write the internal state to the system log.
+ *
+ */
+struct estimator_status_report {
+
+	/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only   */
+
+	uint64_t timestamp;			/**< Timestamp in microseconds since boot */
+	float states[32];			/**< Internal filter states */
+	float n_states;				/**< Number of states effectively used */
+	bool states_nan;			/**< If set to true, one of the states is NaN */
+	bool covariance_nan;			/**< If set to true, the covariance matrix went NaN */
+	bool kalman_gain_nan;			/**< If set to true, the Kalman gain matrix went NaN */
+	bool states_out_of_bounds;		/**< If set to true, one of the states is out of bounds */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(estimator_status);
+
+#endif