From 744b79c1b266fcbb0ea96680c991deaada1f41b2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 5 Oct 2016 16:52:21 +1100 Subject: [PATCH] EKF: Publish innovation test ratios --- EKF/ekf.h | 7 +++++++ EKF/ekf_helper.cpp | 23 +++++++++++++++++++++++ EKF/estimator_interface.cpp | 2 ++ EKF/estimator_interface.h | 14 +++++++++----- EKF/terrain_estimator.cpp | 4 ++-- 5 files changed, 43 insertions(+), 7 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 53182f0baa..ce7380d2af 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -162,6 +162,13 @@ public: *counter = _state_reset_status.quat_counter; } + // get EKF innovation consistency check status information comprising of: + // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check + // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. + // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF + // Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned. + void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl); + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 39626f6eb6..bb9fa568a5 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -696,6 +696,29 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) memcpy(dead_reckoning, &temp3, sizeof(bool)); } +// get EKF innovation consistency check status information comprising of: +// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check +// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. +// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF +// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned. +void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl) +{ + // return the integer bitmask containing the consistency check pass/fail satus + *status = _innov_check_fail_status.value; + // return the largest magnetometer innovation test ratio + *mag = sqrtf(math::max(_yaw_test_ratio,math::max(math::max(_mag_test_ratio[0],_mag_test_ratio[1]),_mag_test_ratio[2]))); + // return the largest NED velocity innovation test ratio + *vel = sqrtf(math::max(math::max(_vel_pos_test_ratio[0],_vel_pos_test_ratio[1]),_vel_pos_test_ratio[2])); + // return the largest NE position innovation test ratio + *pos = sqrtf(math::max(_vel_pos_test_ratio[3],_vel_pos_test_ratio[4])); + // return the vertical position innovation test ratio + *hgt = sqrtf(_vel_pos_test_ratio[5]); + // return the airspeed fusion innovation test ratio + *tas = sqrtf(_tas_test_ratio); + // return the terrain height innovation test ratio + *hagl = sqrt(_terr_test_ratio); +} + // fuse measurement void Ekf::fuse(float *K, float innovation) { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index deeebb9dd7..467e8fc28c 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -58,6 +58,8 @@ EstimatorInterface::EstimatorInterface(): _gps_origin_eph(0.0f), _gps_origin_epv(0.0f), _yaw_test_ratio(0.0f), + _tas_test_ratio(0.0f), + _terr_test_ratio(0.0f), _time_last_imu(0), _time_last_gps(0), _time_last_mag(0), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index ec0b5fa6fe..8b59f1968d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -44,6 +44,7 @@ #include "RingBuffer.h" #include "geo.h" #include "common.h" +#include "mathlib.h" using namespace estimator; class EstimatorInterface @@ -241,11 +242,13 @@ public: // return the amount the quaternion has changed in the last reset and the number of reset events virtual void get_quat_reset(float delta_quat[4], uint8_t *counter) = 0; - // get EKF innovation consistency check status - virtual void get_innovation_test_status(uint16_t *val) - { - *val = _innov_check_fail_status.value; - } + // get EKF innovation consistency check status information comprising of: + // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check + // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. + // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF + // Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned. + virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl) = 0; + protected: @@ -291,6 +294,7 @@ protected: float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios float _tas_test_ratio; // tas innovation consistency check ratio + float _terr_test_ratio; // height above terrain measurement innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status; // data buffer instances diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 4256398e28..7c91cf9262 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -106,9 +106,9 @@ void Ekf::fuseHagl() // perform an innovation consistency check and only fuse data if it passes float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - float test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + _terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); - if (test_ratio <= 1.0f) { + if (_terr_test_ratio <= 1.0f) { // calculate the Kalman gain float gain = _terrain_var / _hagl_innov_var; // correct the state