From 65da9173b92ca251090f92b1bd0c2f86cef3a225 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 17:33:48 +1000 Subject: [PATCH] EKF: capture innovation checks and reset events in separate variables rename the innovation check status class variable and remove the reset flags from it. --- EKF/airspeed_fusion.cpp | 4 ++-- EKF/common.h | 11 +++-------- EKF/ekf.cpp | 2 +- EKF/estimator_interface.h | 3 ++- EKF/mag_fusion.cpp | 8 ++++---- EKF/optflow_fusion.cpp | 4 ++-- EKF/terrain_estimator.cpp | 4 ++-- EKF/vel_pos_fusion.cpp | 12 ++++++------ 8 files changed, 22 insertions(+), 26 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index cf5959a0a5..c0bde855eb 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -138,11 +138,11 @@ void Ekf::fuseAirspeed() // If the innovation consistency check fails then don't fuse the sample and indicate bad airspeed health if (_tas_test_ratio > 1.0f) { - _sensor_fault_status.flags.reject_airspeed = true; + _innov_check_fail_status.flags.reject_airspeed = true; return; } else { - _sensor_fault_status.flags.reject_airspeed = false; + _innov_check_fail_status.flags.reject_airspeed = false; } // Airspeed measurement sample has passed check so record it diff --git a/EKF/common.h b/EKF/common.h index d6eb0e9555..df67565cd7 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -388,8 +388,8 @@ union fault_status_u { }; -// define structure used to communicate innovation test failures and state resets -union sensor_fault_status_u { +// define structure used to communicate innovation test failures +union innovation_fault_status_u { struct { bool reject_vel_NED: 1; // 0 - true if velocity observations have been rejected bool reject_pos_NE: 1; // 1 - true if horizontal position observations have been rejected @@ -402,13 +402,8 @@ union sensor_fault_status_u { bool reject_hagl: 1; // 8 - true if the height above ground observation has been rejected bool reject_optflow_X: 1; // 9 - true if the X optical flow observation has been rejected bool reject_optflow_Y: 1; // 10 - true if the Y optical flow observation has been rejected - bool reset_pos_NE: 1; // 11 - true if the horizontal positoin has been reset - bool reset_pos_D: 1; // 12 - true if the vertical position has been reset - bool reset_vel_NE: 1; // 13 - true if the horizontal velocity has been reset - bool reset_vel_D: 1; // 14 - true if the vertical velocity has been reset - bool reset_yaw: 1; // 15 - true if teh yaw angle has been reset } flags; - uint32_t value; + uint16_t value; }; diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index fabf5e33b8..c25cc208f0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -180,7 +180,7 @@ bool Ekf::init(uint64_t timestamp) _dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS); _fault_status.value = 0; - _sensor_fault_status.value = 0; + _innov_check_fail_status.value = 0; return ret; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 1390bded36..fc67fe1a9d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -259,10 +259,12 @@ protected: float _gps_origin_epv; // vertical position uncertainty of the GPS origin struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) + // innovation consistency check monitoring ratios float _yaw_test_ratio; // yaw innovation consistency check ratio 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 + innovation_fault_status_u _innov_check_fail_status; // data buffer instances RingBuffer _imu_buffer; @@ -285,7 +287,6 @@ protected: uint64_t _time_last_optflow; fault_status_u _fault_status; - sensor_fault_status_u _sensor_fault_status; // allocate data buffers and intialise interface variables bool initialise_interface(uint64_t timestamp); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index e019c70a71..274ca13d6f 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -182,9 +182,9 @@ void Ekf::fuseMag() if (_mag_test_ratio[index] > 1.0f) { mag_fail = false; - _sensor_fault_status.value |= (1 << (index + 3)); + _innov_check_fail_status.value |= (1 << (index + 3)); } else { - _sensor_fault_status.value &= !(1 << (index + 3)); + _innov_check_fail_status.value &= !(1 << (index + 3)); } } @@ -605,7 +605,7 @@ void Ekf::fuseHeading() // set the magnetometer unhealthy if the test fails if (_yaw_test_ratio > 1.0f) { - _sensor_fault_status.flags.reject_yaw = true; + _innov_check_fail_status.flags.reject_yaw = true; // if we are in air we don't want to fuse the measurement // we allow to use it when on the ground because the large innovation could be caused @@ -620,7 +620,7 @@ void Ekf::fuseHeading() } } else { - _sensor_fault_status.flags.reject_yaw = false; + _innov_check_fail_status.flags.reject_yaw = false; } // apply covariance correction via P_new = (I -K*H)*P diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 1418f0cfab..e6b9bc142f 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -407,10 +407,10 @@ void Ekf::fuseOptFlow() for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { if (optflow_test_ratio[obs_index] > 1.0f) { flow_fail = true; - _sensor_fault_status.value |= (1 << (obs_index + 9)); + _innov_check_fail_status.value |= (1 << (obs_index + 9)); } else { - _sensor_fault_status.value &= ~(1 << (obs_index + 9)); + _innov_check_fail_status.value &= ~(1 << (obs_index + 9)); } } diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 19bdb5ed4d..a6b320f96b 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -117,10 +117,10 @@ void Ekf::fuseHagl() _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); // record last successful fusion event _time_last_hagl_fuse = _time_last_imu; - _sensor_fault_status.flags.reject_hagl = false; + _innov_check_fail_status.flags.reject_hagl = false; } else { - _sensor_fault_status.flags.reject_hagl = true; + _innov_check_fail_status.flags.reject_hagl = true; } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 5cb9187259..660359ffc0 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -204,25 +204,25 @@ void Ekf::fuseVelPosHeight() // record the successful velocity fusion event if (vel_check_pass && _fuse_hor_vel) { _time_last_vel_fuse = _time_last_imu; - _sensor_fault_status.flags.reject_vel_NED = false; + _innov_check_fail_status.flags.reject_vel_NED = false; } else if (!vel_check_pass) { - _sensor_fault_status.flags.reject_vel_NED = true; + _innov_check_fail_status.flags.reject_vel_NED = true; } // record the successful position fusion event if (pos_check_pass && _fuse_pos) { _time_last_pos_fuse = _time_last_imu; - _sensor_fault_status.flags.reject_pos_NE = false; + _innov_check_fail_status.flags.reject_pos_NE = false; } else if (!pos_check_pass) { - _sensor_fault_status.flags.reject_pos_NE = true; + _innov_check_fail_status.flags.reject_pos_NE = true; } // record the successful height fusion event if (innov_check_pass_map[5] && _fuse_height) { _time_last_hgt_fuse = _time_last_imu; - _sensor_fault_status.flags.reject_pos_D = false; + _innov_check_fail_status.flags.reject_pos_D = false; } else if (!innov_check_pass_map[5]) { - _sensor_fault_status.flags.reject_pos_D = true; + _innov_check_fail_status.flags.reject_pos_D = true; } for (unsigned obs_index = 0; obs_index < 6; obs_index++) {