Merge remote-tracking branch 'upstream/paul_estimator_numeric' into

paul_mtecs

7 files changed, 452 insertions, 64 deletions

diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
index e6f1fee87..46e405526 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;
         }
@@ -1681,8 +1674,10 @@ void ResetStoredStates()
 }
 
 // Output the state vector stored at the time that best matches that specified by msec
-void RecallStates(float statesForFusion[n_states], uint64_t msec)
+int RecallStates(float statesForFusion[n_states], uint64_t msec)
 {
+    int ret = 0;
+
     // int64_t bestTimeDelta = 200;
     // unsigned bestStoreIndex = 0;
     // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
@@ -1703,12 +1698,29 @@ void RecallStates(float statesForFusion[n_states], uint64_t msec)
     // }
     // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
     // {
-    //     for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex];
+    //     for (uint8_t i=0; i < n_states; i++) {
+    //         if (isfinite(storedStates[i][bestStoreIndex])) {
+    //             statesForFusion[i] = storedStates[i][bestStoreIndex];
+    //         } else if (isfinite(states[i])) {
+    //             statesForFusion[i] = states[i];
+    //         } else {
+    //             // There is not much we can do here, except reporting the error we just
+    //             // found.
+    //             ret++;
+    //         }
     // }
     // else // otherwise output current state
     // {
-        for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i];
+        for (uint8_t i=0; i < n_states; i++) {
+            if (isfinite(states[i])) {
+                statesForFusion[i] = states[i];
+            } else {
+                ret++;
+            }
+        }
     // }
+
+    return ret;
 }
 
 void quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
@@ -1986,7 +1998,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 +2054,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 +2122,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 +2157,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 +2200,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
-    }
+    // Clear the init flag
+    statesInitialised = false;
 
-    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;
-    }
+    ZeroVariables();
 
     // Calculate initial filter quaternion states from raw measurements
     float initQuat[4];
@@ -2155,10 +2224,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 +2253,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..c5a5e9d8d 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);
@@ -155,8 +171,18 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]);
 // store staes along with system time stamp in msces
 void StoreStates(uint64_t timestamp_ms);
 
-// recall stste vector stored at closest time to the one specified by msec
-void RecallStates(float statesForFusion[n_states], uint64_t msec);
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float statesForFusion[n_states], uint64_t msec);
 
 void ResetStoredStates();
 
@@ -188,11 +214,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..c9d75bce4 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,8 @@
 #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 <uORB/topics/actuator_armed.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -144,6 +146,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 +263,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 +705,68 @@ 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];
+					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);
+				}
+			}
 
 
 			/**
@@ -1083,6 +1148,55 @@ void print_status()
 	       (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
 }
 
+int trip_nan() {
+
+	int ret = 0;
+
+	// If system is not armed, inject a NaN value into the filter
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	struct actuator_armed_s armed;
+	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+
+	if (armed.armed) {
+		warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
+		ret = 1;
+	} else {
+
+		float nan_val = 0.0f / 0.0f;
+
+		warnx("system not armed, tripping state vector with NaN values");
+		states[5] = nan_val;
+		usleep(100000);
+
+		// warnx("tripping covariance #1 with NaN values");
+		// KH[2][2] = nan_val; //  intermediate result used for covariance updates
+		// usleep(100000);
+
+		// warnx("tripping covariance #2 with NaN values");
+		// KHP[5][5] = nan_val; // intermediate result used for covariance updates
+		// usleep(100000);
+
+		warnx("tripping covariance #3 with NaN values");
+		P[3][3] = nan_val; // covariance matrix
+		usleep(100000);
+
+		warnx("tripping Kalman gains with NaN values");
+		Kfusion[0] = nan_val; // Kalman gains
+		usleep(100000);
+
+		warnx("tripping stored states[0] with NaN values");
+		storedStates[0][0] = nan_val;
+		usleep(100000);
+
+		warnx("\nDONE - FILTER STATE:");
+		print_status();
+	}
+
+	close(armed_sub);
+	return ret;
+}
+
 int fw_att_pos_estimator_main(int argc, char *argv[])
 {
 	if (argc < 1)
@@ -1129,6 +1243,17 @@ int fw_att_pos_estimator_main(int argc, char *argv[])
 		}
 	}
 
+	if (!strcmp(argv[1], "trip")) {
+		if (estimator::g_estimator) {
+			int ret = trip_nan();
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
 	warnx("unrecognized command");
 	return 1;
 }
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ad709d27d..3f07eea53 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -83,6 +83,7 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/estimator_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -794,6 +795,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct battery_status_s battery;
 		struct telemetry_status_s telemetry;
 		struct range_finder_report range_finder;
+		struct estimator_status_report estimator_status;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -825,6 +827,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
+			struct log_ESTM_s log_ESTM;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -855,6 +858,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int battery_sub;
 		int telemetry_sub;
 		int range_finder_sub;
+		int estimator_status_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -879,6 +883,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
+	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
 
 	thread_running = true;
 
@@ -1243,6 +1248,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(DIST);
 		}
 
+		/* --- ESTIMATOR STATUS --- */
+		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
+			log_msg.msg_type = LOG_ESTM_MSG;
+			unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
+			memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
+			memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
+			log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
+			log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
+			log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
+			log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
+			LOGBUFFER_WRITE_AND_COUNT(DIST);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fe500ad5f..2b41755de 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -299,6 +299,16 @@ struct log_PARM_s {
 	float value;
 };
 
+/* --- ESTM - ESTIMATOR STATUS --- */
+#define LOG_ESTM_MSG 132
+struct log_ESTM_s {
+	float s[32];
+	uint8_t n_states;
+	uint8_t states_nan;
+	uint8_t covariance_nan;
+	uint8_t kalman_gain_nan;
+};
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -331,6 +341,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
 	LOG_FORMAT(PARM, "Nf", "Name,Value"),
+	LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
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..5530cdb21
--- /dev/null
+++ b/src/modules/uORB/topics/estimator_status.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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 */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(estimator_status);
+
+#endif