From 01f33df70718b7a7dba342fb5920d91b3cd83c09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:04:35 +0100 Subject: Added EKF filter health status reporting, added dynamic in-air reset. --- src/modules/fw_att_pos_estimator/estimator.cpp | 209 +++++++++++++++------ src/modules/fw_att_pos_estimator/estimator.h | 29 ++- .../fw_att_pos_estimator_main.cpp | 65 ++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/estimator_status.h | 81 ++++++++ 5 files changed, 328 insertions(+), 59 deletions(-) create mode 100644 src/modules/uORB/topics/estimator_status.h (limited to 'src') 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(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_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 #include #include +#include #include #include #include @@ -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 + */ + +#ifndef ESTIMATOR_STATUS_H_ +#define ESTIMATOR_STATUS_H_ + +#include +#include +#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 -- cgit v1.2.3