Merge remote-tracking branch 'upstream/paul_estimator_numeric' into

paul_mtecs

.ycm_extra_conf.py

@@ -0,0 +1,173 @@
+# This file is NOT licensed under the GPLv3, which is the license for the rest
+# of YouCompleteMe.
+#
+# Here's the license text for this file:
+#
+# This is free and unencumbered software released into the public domain.
+#
+# Anyone is free to copy, modify, publish, use, compile, sell, or
+# distribute this software, either in source code form or as a compiled
+# binary, for any purpose, commercial or non-commercial, and by any
+# means.
+#
+# In jurisdictions that recognize copyright laws, the author or authors
+# of this software dedicate any and all copyright interest in the
+# software to the public domain. We make this dedication for the benefit
+# of the public at large and to the detriment of our heirs and
+# successors. We intend this dedication to be an overt act of
+# relinquishment in perpetuity of all present and future rights to this
+# software under copyright law.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+# For more information, please refer to <http://unlicense.org/>
+
+import os
+import ycm_core
+
+# These are the compilation flags that will be used in case there's no
+# compilation database set (by default, one is not set).
+# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
+flags = [
+'-Wall',
+'-Wextra',
+'-Werror',
+#'-Wc++98-compat',
+'-Wno-long-long',
+'-Wno-variadic-macros',
+'-fexceptions',
+'-DNDEBUG',
+# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM
+# source code needs it.
+#'-DUSE_CLANG_COMPLETER',
+# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
+# language to use when compiling headers. So it will guess. Badly. So C++
+# headers will be compiled as C headers. You don't want that so ALWAYS specify
+# a "-std=<something>".
+# For a C project, you would set this to something like 'c99' instead of
+# 'c++11'.
+'-std=c++11',
+# ...and the same thing goes for the magic -x option which specifies the
+# language that the files to be compiled are written in. This is mostly
+# relevant for c++ headers.
+# For a C project, you would set this to 'c' instead of 'c++'.
+'-x',
+'c++',
+'-undef', # get rid of standard definitions to allow us to include arm math header
+'-I', os.path.join(os.path.expanduser("~"),'gcc-arm-none-eabi-4_7-2013q3/arm-none-eabi/include'),
+'-I', 'Build/px4io-v1_default.build/nuttx-export/include/',
+'-I', 'Build/px4io-v2_default.build/nuttx-export/include/',
+'-I', './NuttX/nuttx/arch/arm/include',
+'-include', './src/include/visibility.h',
+'-I', './src',
+'-I', './src/modules',
+'-I', './src/include',
+'-I', './src/lib',
+'-I', './NuttX',
+]
+
+
+# Set this to the absolute path to the folder (NOT the file!) containing the
+# compile_commands.json file to use that instead of 'flags'. See here for
+# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
+#
+# Most projects will NOT need to set this to anything; you can just change the
+# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
+compilation_database_folder = ''
+
+if os.path.exists( compilation_database_folder ):
+  database = ycm_core.CompilationDatabase( compilation_database_folder )
+else:
+  database = None
+
+SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
+
+def DirectoryOfThisScript():
+  return os.path.dirname( os.path.abspath( __file__ ) )
+
+
+def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
+  if not working_directory:
+    return list( flags )
+  new_flags = []
+  make_next_absolute = False
+  path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
+  for flag in flags:
+    new_flag = flag
+
+    if make_next_absolute:
+      make_next_absolute = False
+      if not flag.startswith( '/' ):
+        new_flag = os.path.join( working_directory, flag )
+
+    for path_flag in path_flags:
+      if flag == path_flag:
+        make_next_absolute = True
+        break
+
+      if flag.startswith( path_flag ):
+        path = flag[ len( path_flag ): ]
+        new_flag = path_flag + os.path.join( working_directory, path )
+        break
+
+    if new_flag:
+      new_flags.append( new_flag )
+  return new_flags
+
+
+def IsHeaderFile( filename ):
+  extension = os.path.splitext( filename )[ 1 ]
+  return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
+
+
+def GetCompilationInfoForFile( filename ):
+  # The compilation_commands.json file generated by CMake does not have entries
+  # for header files. So we do our best by asking the db for flags for a
+  # corresponding source file, if any. If one exists, the flags for that file
+  # should be good enough.
+  if IsHeaderFile( filename ):
+    basename = os.path.splitext( filename )[ 0 ]
+    for extension in SOURCE_EXTENSIONS:
+      replacement_file = basename + extension
+      if os.path.exists( replacement_file ):
+        compilation_info = database.GetCompilationInfoForFile(
+          replacement_file )
+        if compilation_info.compiler_flags_:
+          return compilation_info
+    return None
+  return database.GetCompilationInfoForFile( filename )
+
+
+def FlagsForFile( filename, **kwargs ):
+  if database:
+    # Bear in mind that compilation_info.compiler_flags_ does NOT return a
+    # python list, but a "list-like" StringVec object
+    compilation_info = GetCompilationInfoForFile( filename )
+    if not compilation_info:
+      return None
+
+    final_flags = MakeRelativePathsInFlagsAbsolute(
+      compilation_info.compiler_flags_,
+      compilation_info.compiler_working_dir_ )
+
+    # NOTE: This is just for YouCompleteMe; it's highly likely that your project
+    # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR
+    # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT.
+    #try:
+    #  final_flags.remove( '-stdlib=libc++' )
+    #except ValueError:
+    #  pass
+  else:
+    relative_to = DirectoryOfThisScript()
+    final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
+
+  return {
+    'flags': final_flags,
+    'do_cache': True
+  }

ROMFS/px4fmu_common/init.d/rc.usb

@@ -9,6 +9,8 @@ mavlink start -r 10000 -d /dev/ttyACM0
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10
 

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;
+struct ekf_status_report current_ekf_state;
-bool posHealth;
+struct ekf_status_report last_ekf_error;
-bool hgtHealth;
-bool velTimeout;
-bool posTimeout;
-bool hgtTimeout;
 
 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]);
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
-            velTimeout = (millis() - velFailTime) > horizRetryTime;
+            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
-            if (velHealth || velTimeout)
+            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
             {
-                velHealth = true;
+                current_ekf_state.velHealth = true;
-                velFailTime = millis();
+                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]);
+            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
-            posTimeout = (millis() - posFailTime) > horizRetryTime;
+            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
-            if (posHealth || posTimeout)
+            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
             {
-                posHealth = true;
+                current_ekf_state.posHealth = true;
-                posFailTime = millis();
+                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];
+            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
-            hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime;
+            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
-            if (hgtHealth || hgtTimeout)
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
             {
-                hgtHealth = true;
+                current_ekf_state.hgtHealth = true;
-                hgtFailTime = millis();
+                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
+    // Clear the init flag
-        states[i] = 0.0f; // state matrix
+    statesInitialised = false;
-    }
 
-    for (unsigned i = 0; i < data_buffer_size; i++) {
+    ZeroVariables();
-
-        for (unsigned j = 0; j < n_states; j++) {
-            storedStates[j][i] = 0.0f;
-        }
-
-        statetimeStamp[i] = 0;
-    }
 
     // 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));
+}

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

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

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 */

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

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

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