From 106482b078ac87ccf9e44330269ac8e02d994e2b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 14:08:03 +1000 Subject: [PATCH 01/26] EKF: add structure to capture innovation test failures and state resets --- EKF/common.h | 24 ++++++++++++++++++++++++ EKF/ekf.cpp | 2 ++ EKF/estimator_interface.h | 1 + 3 files changed, 27 insertions(+) diff --git a/EKF/common.h b/EKF/common.h index cad93dc713..d6eb0e9555 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -388,6 +388,30 @@ union fault_status_u { }; +// define structure used to communicate innovation test failures and state resets +union sensor_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 + bool reject_pos_D: 1; // 2 - true if true if vertical position observations have been rejected + bool reject_mag_x: 1; // 3 - true if the X magnetometer observation has been rejected + bool reject_mag_y: 1; // 4 - true if the Y magnetometer observation has been rejected + bool reject_mag_z: 1; // 5 - true if the Z magnetometer observation has been rejected + bool reject_yaw: 1; // 6 - true if the yaw observation has been rejected + bool reject_airspeed: 1; // 7 - true if the airspeed observation has been rejected + 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; + +}; + // publish the status of various GPS quality checks union gps_check_fail_status_u { struct { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index d98a5aec2d..762c71d175 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -180,6 +180,8 @@ bool Ekf::init(uint64_t timestamp) _dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS); + _sensor_fault_status.value = 0; + return ret; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 364eb15882..0babecb623 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -287,6 +287,7 @@ 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); From aea827aa8ac9732db61c5eb965e8df5e83d733ee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 14:14:30 +1000 Subject: [PATCH 02/26] EKF: ensure filter fault status is initialised --- EKF/ekf.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 762c71d175..73f5a2bb39 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -180,6 +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; return ret; From 52f6eea52b3caddac29991139d1ccb3af9173d3b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 14:20:46 +1000 Subject: [PATCH 03/26] EKF: capture position and velocity innovation test failures --- EKF/vel_pos_fusion.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 8608e29afe..5cb9187259 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -201,19 +201,28 @@ void Ekf::fuseVelPosHeight() innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; - // record the successful velocity fusion time + // 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; + } else if (!vel_check_pass) { + _sensor_fault_status.flags.reject_vel_NED = true; } - // record the successful position fusion time + // 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; + } else if (!pos_check_pass) { + _sensor_fault_status.flags.reject_pos_NE = true; } - // record the successful height fusion time + // 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; + } else if (!innov_check_pass_map[5]) { + _sensor_fault_status.flags.reject_pos_D = true; } for (unsigned obs_index = 0; obs_index < 6; obs_index++) { From 3fb449295e007676b86d48da865ba51e3507de74 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 14:23:08 +1000 Subject: [PATCH 04/26] EKF: capture yaw innovation test failures --- EKF/mag_fusion.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 4356b7a159..966db4172c 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -604,6 +604,7 @@ void Ekf::fuseHeading() // set the magnetometer unhealthy if the test fails if (_yaw_test_ratio > 1.0f) { _mag_healthy = false; + _sensor_fault_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 @@ -619,6 +620,7 @@ void Ekf::fuseHeading() } else { _mag_healthy = true; + _sensor_fault_status.flags.reject_yaw = false; } // apply covariance correction via P_new = (I -K*H)*P From e7690bd8f87989d8684c235f6f64f6d8bdd104b6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 16:25:26 +1000 Subject: [PATCH 05/26] EKF: capture magnetometer innovation test failures --- EKF/mag_fusion.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 966db4172c..9c41a1eb35 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -183,6 +183,9 @@ void Ekf::fuseMag() if (_mag_test_ratio[index] > 1.0f) { _mag_healthy = false; + _sensor_fault_status.value |= (1 << (index + 3)); + } else { + _sensor_fault_status.value &= !(1 << (index + 3)); } } From cf489f424875abb999d710b4386042f1e3c99b72 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 16:33:16 +1000 Subject: [PATCH 06/26] EKF: capture airspeed innovation test failures --- EKF/airspeed_fusion.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 2c071db32d..18455bd650 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -139,10 +139,12 @@ 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) { _airspeed_healthy = false; + _sensor_fault_status.flags.reject_airspeed = true; return; } else { _airspeed_healthy = true; + _sensor_fault_status.flags.reject_airspeed = false; } // Airspeed measurement sample has passed check so record it From 388e50018006f4f38192afe29fb7d34e8929c931 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 16:41:11 +1000 Subject: [PATCH 07/26] EKF: remove un-used magnetometer health class variable replaced by _sensor_health_status --- EKF/ekf.cpp | 1 - EKF/estimator_interface.cpp | 1 - EKF/estimator_interface.h | 5 ++--- EKF/mag_fusion.cpp | 9 +++------ 4 files changed, 5 insertions(+), 11 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 73f5a2bb39..fabf5e33b8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -170,7 +170,6 @@ bool Ekf::init(uint64_t timestamp) _imu_updated = false; _NED_origin_initialised = false; _gps_speed_valid = false; - _mag_healthy = false; _filter_initialised = false; _terrain_initialised = false; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index f615e4ab86..161e74c6a9 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -57,7 +57,6 @@ EstimatorInterface::EstimatorInterface(): _gps_speed_valid(false), _gps_origin_eph(0.0f), _gps_origin_epv(0.0f), - _mag_healthy(false), _airspeed_healthy(false), _yaw_test_ratio(0.0f), _time_last_imu(0), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 0babecb623..8cc9baa5fb 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -259,12 +259,11 @@ 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) - bool _mag_healthy; // computed by mag innovation test - bool _airspeed_healthy; // computed by airspeed innovation test + bool _airspeed_healthy; // computed by airspeed innovation test 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 + float _tas_test_ratio; // tas innovation consistency check ratio // data buffer instances RingBuffer _imu_buffer; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 9c41a1eb35..e019c70a71 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -176,20 +176,19 @@ void Ekf::fuseMag() // Perform an innovation consistency check on each measurement and if one axis fails // do not fuse any data from the sensor because the most common errors affect multiple axes. - _mag_healthy = true; - + bool mag_fail = false; for (uint8_t index = 0; index <= 2; index++) { _mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]); if (_mag_test_ratio[index] > 1.0f) { - _mag_healthy = false; + mag_fail = false; _sensor_fault_status.value |= (1 << (index + 3)); } else { _sensor_fault_status.value &= !(1 << (index + 3)); } } - if (!_mag_healthy) { + if (mag_fail) { return; } @@ -606,7 +605,6 @@ void Ekf::fuseHeading() // set the magnetometer unhealthy if the test fails if (_yaw_test_ratio > 1.0f) { - _mag_healthy = false; _sensor_fault_status.flags.reject_yaw = true; // if we are in air we don't want to fuse the measurement @@ -622,7 +620,6 @@ void Ekf::fuseHeading() } } else { - _mag_healthy = true; _sensor_fault_status.flags.reject_yaw = false; } From 8125717bf5771d6ca32190ac7f95dcc2d78b20f6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 16:43:59 +1000 Subject: [PATCH 08/26] EKF: remove un-used airspeed health class variable Airspeed rejection now is captured in _sensor_fault_status --- EKF/airspeed_fusion.cpp | 2 -- EKF/estimator_interface.cpp | 1 - EKF/estimator_interface.h | 1 - 3 files changed, 4 deletions(-) diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 18455bd650..cf5959a0a5 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -138,12 +138,10 @@ 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) { - _airspeed_healthy = false; _sensor_fault_status.flags.reject_airspeed = true; return; } else { - _airspeed_healthy = true; _sensor_fault_status.flags.reject_airspeed = false; } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 161e74c6a9..7427967785 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -57,7 +57,6 @@ EstimatorInterface::EstimatorInterface(): _gps_speed_valid(false), _gps_origin_eph(0.0f), _gps_origin_epv(0.0f), - _airspeed_healthy(false), _yaw_test_ratio(0.0f), _time_last_imu(0), _time_last_gps(0), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 8cc9baa5fb..1390bded36 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -259,7 +259,6 @@ 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) - bool _airspeed_healthy; // computed by airspeed innovation test 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 From d80e71a4991d1e0cfabcfde583f14e9a7274ba41 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 16:49:44 +1000 Subject: [PATCH 09/26] EKF: capture HAGL innovation test failures --- EKF/terrain_estimator.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index cca1d2b45f..19bdb5ed4d 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -115,10 +115,14 @@ void Ekf::fuseHagl() _terrain_vpos -= gain * _hagl_innov; // correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); - // record last successful fusion time + // record last successful fusion event _time_last_hagl_fuse = _time_last_imu; - } + _sensor_fault_status.flags.reject_hagl = false; + + } else { + _sensor_fault_status.flags.reject_hagl = true; + } } else { return; From 52229da08956e48fad5d0d9e72aaa63d2cf5709f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 16:58:49 +1000 Subject: [PATCH 10/26] EKF: capture optical flow innovation test failures --- EKF/optflow_fusion.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 747d0b29c5..1418f0cfab 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -402,9 +402,23 @@ void Ekf::fuseOptFlow() } } - // if either axis fails, we fail the sensor - if (optflow_test_ratio[0] > 1.0f || optflow_test_ratio[1] > 1.0f) { + // record the innovation test pass/fail + bool flow_fail = false; + 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)); + + } else { + _sensor_fault_status.value &= ~(1 << (obs_index + 9)); + + } + } + + // if either axis fails we abort the fusion + if (flow_fail) { return; + } for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { From 54d90261d5d9e199482348fdd90751e206dfbf01 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 17:13:19 +1000 Subject: [PATCH 11/26] EKF: make output predictor states consistent with velocity reset --- EKF/ekf_helper.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index bda1ed8da7..ec3fdb42a8 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -51,6 +51,10 @@ // gps measurement then use for velocity initialisation bool Ekf::resetVelocity() { + // used to calculate the velocity change due to the reset + Vector3f vel_before_reset = _state.vel; + + // reset EKF states if (_control_status.flags.gps) { // if we have a valid GPS measurement use it to initialise velocity states gpsSample gps_newest = _gps_buffer.get_newest(); @@ -69,6 +73,17 @@ bool Ekf::resetVelocity() } else { return false; } + + // calculate the change in velocity and apply to the output predictor state history + Vector3f velocity_change = _state.vel - vel_before_reset; + outputSample output_states; + unsigned max_index = _output_buffer.get_length() - 1; + for (unsigned index=0; index <= max_index; index++) { + output_states = _output_buffer.get_from_index(index); + output_states.vel += velocity_change; + _output_buffer.push_to_index(index,output_states); + } + } // Reset position states. If we have a recent and valid From 79705da7e69d25735cfdbec92591c14e6a2fe421 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 17:19:28 +1000 Subject: [PATCH 12/26] EKF: make output predictor states consistent with position reset --- EKF/ekf_helper.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ec3fdb42a8..5ab4b978dc 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -90,6 +90,11 @@ bool Ekf::resetVelocity() // gps measurement then use for position initialisation bool Ekf::resetPosition() { + // used to calculate the position change due to the reset + Vector2f posNE_before_reset; + posNE_before_reset(0) = _state.pos(0); + posNE_before_reset(1) = _state.pos(1); + if (_control_status.flags.gps) { // if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay gpsSample gps_newest = _gps_buffer.get_newest(); @@ -134,6 +139,20 @@ bool Ekf::resetPosition() } else { return false; } + + // calculate the change in position and apply to the output predictor state history + Vector3f posNE_change; + posNE_change(0) = _state.pos(0) - posNE_before_reset(0); + posNE_change(1) = _state.pos(1) - posNE_before_reset(1); + outputSample output_states; + unsigned max_index = _output_buffer.get_length() - 1; + for (unsigned index=0; index <= max_index; index++) { + output_states = _output_buffer.get_from_index(index); + output_states.pos(0) += posNE_change(0); + output_states.pos(1) += posNE_change(1); + _output_buffer.push_to_index(index,output_states); + } + } // Reset height state using the last height measurement From 65da9173b92ca251090f92b1bd0c2f86cef3a225 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 17:33:48 +1000 Subject: [PATCH 13/26] 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++) { From 4237269fabc662988fae7a6aba0fd8239d033215 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:23:37 +1000 Subject: [PATCH 14/26] EKF: add struct to capture state reset events --- EKF/estimator_interface.cpp | 1 + EKF/estimator_interface.h | 15 +++++++++++++++ 2 files changed, 16 insertions(+) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 7427967785..ada6de492e 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -71,6 +71,7 @@ EstimatorInterface::EstimatorInterface(): _pos_ref = {}; memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio)); + _state_reset_status = {}; } EstimatorInterface::~EstimatorInterface() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index fc67fe1a9d..9347378857 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -266,6 +266,21 @@ protected: float _tas_test_ratio; // tas innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status; + // reset event monitoring + // structure containing velocity, position, height and yaw reset information + struct { + uint64_t velNE_time_us; // time stamp of the last horizontal velocity reset event (us) + uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us) + uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us) + uint64_t posD_time_us; // time stamp of the last vertical position reset event (us) + uint64_t yaw_time_us; // time stamp of the last yaw angle reset event (us) + Vector2f velNE_change; // North East velocity change due to last reset (m) + float velD_change; // Down velocity change due to last reset (m/s) + Vector2f posNE_change; // North, East position change due to last reset (m) + float posD_change; // Down position change due to last reset (m) + float yaw_change; // Yaw angle change due to last reset (rad) + } _state_reset_status; + // data buffer instances RingBuffer _imu_buffer; RingBuffer _gps_buffer; From e3a1b4a3b3e16990973cf89b28a58f2ad08032f3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:24:42 +1000 Subject: [PATCH 15/26] EKF: capture velocity reset events --- EKF/ekf_helper.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5ab4b978dc..0f344573ed 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -84,6 +84,13 @@ bool Ekf::resetVelocity() _output_buffer.push_to_index(index,output_states); } + // capture the reset event + _state_reset_status.velNE_change(0) = velocity_change(0); + _state_reset_status.velNE_change(1) = velocity_change(1); + _state_reset_status.velD_change = velocity_change(2); + _state_reset_status.velNE_time_us = _imu_sample_delayed.time_us; + _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; + } // Reset position states. If we have a recent and valid From 0605c88eccae178f9ba4999baa5c8eabde73712f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:25:20 +1000 Subject: [PATCH 16/26] EKF: capture horizontal position reset events --- EKF/ekf_helper.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0f344573ed..7ba3e2ada2 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -148,7 +148,7 @@ bool Ekf::resetPosition() } // calculate the change in position and apply to the output predictor state history - Vector3f posNE_change; + Vector2f posNE_change; posNE_change(0) = _state.pos(0) - posNE_before_reset(0); posNE_change(1) = _state.pos(1) - posNE_before_reset(1); outputSample output_states; @@ -160,6 +160,10 @@ bool Ekf::resetPosition() _output_buffer.push_to_index(index,output_states); } + // capture the reset event + _state_reset_status.posNE_change = posNE_change; + _state_reset_status.posNE_time_us = _imu_sample_delayed.time_us; + } // Reset height state using the last height measurement From aca0336392820d9718f8b007ca794c188d371477 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:28:19 +1000 Subject: [PATCH 17/26] EKF: update vertical position and velocity reset capture Use reset event struct members instead of separate variables --- EKF/ekf.cpp | 4 ---- EKF/ekf.h | 6 +----- EKF/ekf_helper.cpp | 16 ++++++++-------- 3 files changed, 9 insertions(+), 17 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c25cc208f0..ffdc8a4814 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -107,10 +107,6 @@ Ekf::Ekf(): _gps_hgt_faulty(false), _rng_hgt_faulty(false), _baro_hgt_offset(0.0f), - _vert_pos_reset_delta(0.0f), - _time_vert_pos_reset(0), - _vert_vel_reset_delta(0.0f), - _time_vert_vel_reset(0), _time_bad_vert_accel(0) { _state = {}; diff --git a/EKF/ekf.h b/EKF/ekf.h index 69b7c1df8a..af6ca6f3fc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -128,7 +128,7 @@ public: void get_gps_check_status(uint16_t *_gps_check_fail_status); // return the amount the local vertical position changed in the last height reset and the time of the reset - void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _vert_pos_reset_delta; *time_us = _time_vert_pos_reset;} + void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;} private: @@ -235,10 +235,6 @@ private: int _primary_hgt_source; // priary source of height data set at initialisation float _baro_hgt_offset; // baro height reading at the local NED origin (m) - float _vert_pos_reset_delta; // increase in vertical position state at the last reset(m) - uint64_t _time_vert_pos_reset; // last system time in usec that the vertical position state was reset - float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m) - uint64_t _time_vert_vel_reset; // last system time in usec that the vertical velocity state was reset // imu fault status uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 7ba3e2ada2..9dd8e52e1c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -289,18 +289,18 @@ void Ekf::resetHeight() // store the reset amount and time to be published if (vert_pos_reset) { - _vert_pos_reset_delta = _state.pos(2) - old_vert_pos; - _time_vert_pos_reset = _time_last_imu; + _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; + _state_reset_status.posD_time_us = _imu_sample_delayed.time_us; } if (vert_vel_reset) { - _vert_vel_reset_delta = _state.vel(2) - old_vert_vel; - _time_vert_vel_reset = _time_last_imu; + _state_reset_status.velD_change = _state.vel(2) - old_vert_vel; + _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; } // add the reset amount to the output observer states - _output_new.pos(2) += _vert_pos_reset_delta; - _output_new.vel(2) += _vert_vel_reset_delta; + _output_new.pos(2) += _state_reset_status.posD_change; + _output_new.vel(2) += _state_reset_status.velD_change; // add the reset amount to the output observer buffered data outputSample output_states; @@ -309,11 +309,11 @@ void Ekf::resetHeight() output_states = _output_buffer.get_from_index(i); if (vert_pos_reset) { - output_states.pos(2) += _vert_pos_reset_delta; + output_states.pos(2) += _state_reset_status.posD_change; } if (vert_vel_reset) { - output_states.vel(2) += _vert_vel_reset_delta; + output_states.vel(2) += _state_reset_status.velD_change; } _output_buffer.push_to_index(i,output_states); From 733862f649a9115512e76a9b054e5e56853e2c1f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:56:25 +1000 Subject: [PATCH 18/26] EKF: move the reset status struct to the Ekf class This protects it from being modified externally --- EKF/ekf.cpp | 1 + EKF/ekf.h | 15 +++++++++++++++ EKF/estimator_interface.cpp | 1 - EKF/estimator_interface.h | 15 --------------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index ffdc8a4814..61d2a57be0 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -128,6 +128,7 @@ Ekf::Ekf(): _flow_gyro_bias = {}; _imu_del_ang_of = {}; _gps_check_fail_status.value = 0; + _state_reset_status = {}; } Ekf::~Ekf() diff --git a/EKF/ekf.h b/EKF/ekf.h index af6ca6f3fc..7de59879b7 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -136,6 +136,21 @@ private: static const float _k_earth_rate; static const float _gravity_mss; + // reset event monitoring + // structure containing velocity, position, height and yaw reset information + struct { + uint64_t velNE_time_us; // time stamp of the last horizontal velocity reset event (us) + uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us) + uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us) + uint64_t posD_time_us; // time stamp of the last vertical position reset event (us) + uint64_t yaw_time_us; // time stamp of the last yaw angle reset event (us) + Vector2f velNE_change; // North East velocity change due to last reset (m) + float velD_change; // Down velocity change due to last reset (m/s) + Vector2f posNE_change; // North, East position change due to last reset (m) + float posD_change; // Down position change due to last reset (m) + float yaw_change; // Yaw angle change due to last reset (rad) + } _state_reset_status; + float _dt_ekf_avg; // average update rate of the ekf stateSample _state; // state struct of the ekf running at the delayed time horizon diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ada6de492e..7427967785 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -71,7 +71,6 @@ EstimatorInterface::EstimatorInterface(): _pos_ref = {}; memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio)); memset(_vel_pos_test_ratio, 0, sizeof(_vel_pos_test_ratio)); - _state_reset_status = {}; } EstimatorInterface::~EstimatorInterface() diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 9347378857..fc67fe1a9d 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -266,21 +266,6 @@ protected: float _tas_test_ratio; // tas innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status; - // reset event monitoring - // structure containing velocity, position, height and yaw reset information - struct { - uint64_t velNE_time_us; // time stamp of the last horizontal velocity reset event (us) - uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us) - uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us) - uint64_t posD_time_us; // time stamp of the last vertical position reset event (us) - uint64_t yaw_time_us; // time stamp of the last yaw angle reset event (us) - Vector2f velNE_change; // North East velocity change due to last reset (m) - float velD_change; // Down velocity change due to last reset (m/s) - Vector2f posNE_change; // North, East position change due to last reset (m) - float posD_change; // Down position change due to last reset (m) - float yaw_change; // Yaw angle change due to last reset (rad) - } _state_reset_status; - // data buffer instances RingBuffer _imu_buffer; RingBuffer _gps_buffer; From b2e432e21198d52c29065c9553838ebee762894d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 19:57:05 +1000 Subject: [PATCH 19/26] EKF: publish position and velocity reset data --- EKF/ekf.h | 13 +++++++++++-- EKF/estimator_interface.h | 13 +++++++++++-- 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 7de59879b7..f971f8d683 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -127,8 +127,17 @@ public: // get GPS check status void get_gps_check_status(uint16_t *_gps_check_fail_status); - // return the amount the local vertical position changed in the last height reset and the time of the reset - void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;} + // return the amount the local vertical position changed in the last reset and the time of the reset + void get_posD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;} + + // return the amount the local vertical velocity changed in the last reset and the time of the reset + void get_velD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.velD_change; *time_us = _state_reset_status.velD_time_us;} + + // return the amount the local horizontal position changed in the last reset and the time of the reset + void get_posNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.posNE_change; *time_us = _state_reset_status.posNE_time_us;} + + // return the amount the local horizontal velocity changed in the last reset and the time of the reset + void get_velNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.velNE_change; *time_us = _state_reset_status.velNE_time_us;} private: diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index fc67fe1a9d..8f36a4c97b 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -217,8 +217,17 @@ public: // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0; - // return the amount the local vertical position changed in the last height reset and the time of the reset - virtual void get_vert_pos_reset(float *delta, uint64_t *time_us) = 0; + // return the amount the local vertical position changed in the last reset and the time of the reset + virtual void get_posD_reset(float *delta, uint64_t *time_us) = 0; + + // return the amount the local vertical velocity changed in the last reset and the time of the reset + virtual void get_velD_reset(float *delta, uint64_t *time_us) = 0; + + // return the amount the local horizontal position changed in the last reset and the time of the reset + virtual void get_posNE_reset(Vector2f *delta, uint64_t *time_us) = 0; + + // return the amount the local horizontal velocity changed in the last reset and the time of the reset + virtual void get_velNE_reset(Vector2f *delta, uint64_t *time_us) = 0; protected: From 2b0d5c28f025925956a5cbfe7f2d1c3f0cc2ad01 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 20:38:49 +1000 Subject: [PATCH 20/26] EKF: capture full quaternion change for reset events --- EKF/ekf.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index f971f8d683..0773c526bc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -152,12 +152,12 @@ private: uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us) uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us) uint64_t posD_time_us; // time stamp of the last vertical position reset event (us) - uint64_t yaw_time_us; // time stamp of the last yaw angle reset event (us) + uint64_t quat_time_us; // time stamp of the last quaternion reset event (us) Vector2f velNE_change; // North East velocity change due to last reset (m) float velD_change; // Down velocity change due to last reset (m/s) Vector2f posNE_change; // North, East position change due to last reset (m) float posD_change; // Down position change due to last reset (m) - float yaw_change; // Yaw angle change due to last reset (rad) + Quaternion quat_change; // quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion } _state_reset_status; float _dt_ekf_avg; // average update rate of the ekf From 9ec09f5f4e3ebc3f5bb0c39d57e18df2e2e6559c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 20:39:38 +1000 Subject: [PATCH 21/26] EKF: update output observer and capture reset event for magnetic heading resets --- EKF/ekf_helper.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 9dd8e52e1c..5a74ddd54b 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -350,6 +350,8 @@ void Ekf::alignOutputFilter() // Reset heading and magnetic field states bool Ekf::resetMagHeading(Vector3f &mag_init) { + // save a copy of the quaternion state for later use in calculating the amount of reset change + Quaternion quat_before_reset = _state.quat_nominal; // calculate the variance for the rotation estimate expressed as a rotation vector // this will be used later to reset the quaternion state covariances @@ -482,6 +484,21 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) P[index][index] = sq(_params.mag_noise); } + // calculate the amount that the quaternion has changed by + _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); + + // add the reset amount to the output observer buffered data + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal *= _state_reset_status.quat_change; + _output_buffer.push_to_index(i,output_states); + } + + // capture the reset event + _state_reset_status.quat_time_us = _imu_sample_delayed.time_us; + return true; } From 3ec9221c18290e4921ea280bdc69e72d85a713e6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 20:45:03 +1000 Subject: [PATCH 22/26] EKF: update output observer and capture reset event for EV yaw resets --- EKF/control.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7a8ff9fff1..751ac41aa3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -107,9 +107,27 @@ void Ekf::controlExternalVisionAiding() matrix::Euler euler_obs(q_obs); euler_init(2) = euler_obs(2); + // save a copy of the quaternion state for later use in calculating the amount of reset change + Quaternion quat_before_reset = _state.quat_nominal; + // calculate initial quaternion states for the ekf _state.quat_nominal = Quaternion(euler_init); + // calculate the amount that the quaternion has changed by + _state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed(); + + // add the reset amount to the output observer buffered data + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + output_states.quat_nominal *= _state_reset_status.quat_change; + _output_buffer.push_to_index(i,output_states); + } + + // capture the reset event + _state_reset_status.quat_time_us = _imu_sample_delayed.time_us; + // flag the yaw as aligned _control_status.flags.yaw_align = true; From 81ca167da888476e0a2da7a16c975adddd39fde7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 20:50:58 +1000 Subject: [PATCH 23/26] EKF: align output observer to EKF states on startup --- EKF/ekf.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 61d2a57be0..35488673d1 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -567,6 +567,9 @@ bool Ekf::initialiseFilter(void) _time_last_hagl_fuse = _time_last_imu; _time_last_of_fuse = _time_last_imu; + // reset the output predictor state history to match the EKF initial values + alignOutputFilter(); + return true; } } From e371a303a9b93aa337e05a33a82fc1fa455db1d2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 23 May 2016 21:07:35 +1000 Subject: [PATCH 24/26] EKF: publish the quaternion reset event --- EKF/ekf.h | 7 +++++++ EKF/estimator_interface.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 0773c526bc..4b83b66379 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -139,6 +139,13 @@ public: // return the amount the local horizontal velocity changed in the last reset and the time of the reset void get_velNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.velNE_change; *time_us = _state_reset_status.velNE_time_us;} + // return the amount the quaternion has changed in the last reset and the time of the reset + void get_quat_reset(Quaternion *delta, uint64_t *time_us) + { + *delta = _state_reset_status.quat_change; + *time_us = _state_reset_status.quat_time_us; + } + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 8f36a4c97b..e2c594720b 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -229,6 +229,9 @@ public: // return the amount the local horizontal velocity changed in the last reset and the time of the reset virtual void get_velNE_reset(Vector2f *delta, uint64_t *time_us) = 0; + // return the amount the quaternion has changed in the last reset and the time of the reset + virtual void get_quat_reset(Quaternion *delta, uint64_t *time_us) = 0; + protected: parameters _params; // filter parameters From 70f76e1a6cff82f082def435b1ea81c1ac00c584 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 May 2016 07:44:43 +1000 Subject: [PATCH 25/26] EKF: publish innovation consistency check fail status --- EKF/estimator_interface.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index e2c594720b..81ff0d3dc8 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -232,6 +232,12 @@ public: // return the amount the quaternion has changed in the last reset and the time of the reset virtual void get_quat_reset(Quaternion *delta, uint64_t *time_us) = 0; + // get EKF innovation consistency check status + virtual void get_innovation_test_status(uint16_t *val) + { + *val = _innov_check_fail_status.value; + } + protected: parameters _params; // filter parameters From 6a55d908c5ea29e85de118b3f7b6e16df3b5955c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 25 May 2016 18:24:31 +1000 Subject: [PATCH 26/26] EKF: replace reset event times with event counters Using a 64bit integer was unnecessary given it was only being used to detect a new reset event. --- EKF/control.cpp | 2 +- EKF/ekf.h | 32 ++++++++++++++++---------------- EKF/ekf_helper.cpp | 12 ++++++------ EKF/estimator_interface.h | 20 ++++++++++---------- 4 files changed, 33 insertions(+), 33 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 751ac41aa3..63288eec45 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -126,7 +126,7 @@ void Ekf::controlExternalVisionAiding() } // capture the reset event - _state_reset_status.quat_time_us = _imu_sample_delayed.time_us; + _state_reset_status.quat_counter++; // flag the yaw as aligned _control_status.flags.yaw_align = true; diff --git a/EKF/ekf.h b/EKF/ekf.h index 4b83b66379..88237bd3f3 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -127,23 +127,23 @@ public: // get GPS check status void get_gps_check_status(uint16_t *_gps_check_fail_status); - // return the amount the local vertical position changed in the last reset and the time of the reset - void get_posD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;} + // return the amount the local vertical position changed in the last reset and the number of reset events + void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;} - // return the amount the local vertical velocity changed in the last reset and the time of the reset - void get_velD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.velD_change; *time_us = _state_reset_status.velD_time_us;} + // return the amount the local vertical velocity changed in the last reset and the number of reset events + void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;} - // return the amount the local horizontal position changed in the last reset and the time of the reset - void get_posNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.posNE_change; *time_us = _state_reset_status.posNE_time_us;} + // return the amount the local horizontal position changed in the last reset and the number of reset events + void get_posNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.posNE_change; *counter = _state_reset_status.posNE_counter;} - // return the amount the local horizontal velocity changed in the last reset and the time of the reset - void get_velNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.velNE_change; *time_us = _state_reset_status.velNE_time_us;} + // return the amount the local horizontal velocity changed in the last reset and the number of reset events + void get_velNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.velNE_change; *counter = _state_reset_status.velNE_counter;} - // return the amount the quaternion has changed in the last reset and the time of the reset - void get_quat_reset(Quaternion *delta, uint64_t *time_us) + // return the amount the quaternion has changed in the last reset and the number of reset events + void get_quat_reset(Quaternion *delta, uint8_t *counter) { *delta = _state_reset_status.quat_change; - *time_us = _state_reset_status.quat_time_us; + *counter = _state_reset_status.quat_counter; } private: @@ -155,11 +155,11 @@ private: // reset event monitoring // structure containing velocity, position, height and yaw reset information struct { - uint64_t velNE_time_us; // time stamp of the last horizontal velocity reset event (us) - uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us) - uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us) - uint64_t posD_time_us; // time stamp of the last vertical position reset event (us) - uint64_t quat_time_us; // time stamp of the last quaternion reset event (us) + uint8_t velNE_counter; // number of horizontal position reset events (allow to wrap if count exceeds 255) + uint8_t velD_counter; // number of vertical velocity reset events (allow to wrap if count exceeds 255) + uint8_t posNE_counter; // number of horizontal position reset events (allow to wrap if count exceeds 255) + uint8_t posD_counter; // number of vertical position reset events (allow to wrap if count exceeds 255) + uint8_t quat_counter; // number of quaternion reset events (allow to wrap if count exceeds 255) Vector2f velNE_change; // North East velocity change due to last reset (m) float velD_change; // Down velocity change due to last reset (m/s) Vector2f posNE_change; // North, East position change due to last reset (m) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5a74ddd54b..a9a91fc595 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -88,8 +88,8 @@ bool Ekf::resetVelocity() _state_reset_status.velNE_change(0) = velocity_change(0); _state_reset_status.velNE_change(1) = velocity_change(1); _state_reset_status.velD_change = velocity_change(2); - _state_reset_status.velNE_time_us = _imu_sample_delayed.time_us; - _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; + _state_reset_status.velNE_counter++; + _state_reset_status.velD_counter++; } @@ -162,7 +162,7 @@ bool Ekf::resetPosition() // capture the reset event _state_reset_status.posNE_change = posNE_change; - _state_reset_status.posNE_time_us = _imu_sample_delayed.time_us; + _state_reset_status.posNE_counter++; } @@ -290,12 +290,12 @@ void Ekf::resetHeight() // store the reset amount and time to be published if (vert_pos_reset) { _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; - _state_reset_status.posD_time_us = _imu_sample_delayed.time_us; + _state_reset_status.posD_counter++; } if (vert_vel_reset) { _state_reset_status.velD_change = _state.vel(2) - old_vert_vel; - _state_reset_status.velD_time_us = _imu_sample_delayed.time_us; + _state_reset_status.velD_counter++; } // add the reset amount to the output observer states @@ -497,7 +497,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) } // capture the reset event - _state_reset_status.quat_time_us = _imu_sample_delayed.time_us; + _state_reset_status.quat_counter++; return true; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 81ff0d3dc8..34d8bc8596 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -217,20 +217,20 @@ public: // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0; - // return the amount the local vertical position changed in the last reset and the time of the reset - virtual void get_posD_reset(float *delta, uint64_t *time_us) = 0; + // return the amount the local vertical position changed in the last reset and the number of reset events + virtual void get_posD_reset(float *delta, uint8_t *counter) = 0; - // return the amount the local vertical velocity changed in the last reset and the time of the reset - virtual void get_velD_reset(float *delta, uint64_t *time_us) = 0; + // return the amount the local vertical velocity changed in the last reset and the number of reset events + virtual void get_velD_reset(float *delta, uint8_t *counter) = 0; - // return the amount the local horizontal position changed in the last reset and the time of the reset - virtual void get_posNE_reset(Vector2f *delta, uint64_t *time_us) = 0; + // return the amount the local horizontal position changed in the last reset and the number of reset events + virtual void get_posNE_reset(Vector2f *delta, uint8_t *counter) = 0; - // return the amount the local horizontal velocity changed in the last reset and the time of the reset - virtual void get_velNE_reset(Vector2f *delta, uint64_t *time_us) = 0; + // return the amount the local horizontal velocity changed in the last reset and the number of reset events + virtual void get_velNE_reset(Vector2f *delta, uint8_t *counter) = 0; - // return the amount the quaternion has changed in the last reset and the time of the reset - virtual void get_quat_reset(Quaternion *delta, uint64_t *time_us) = 0; + // return the amount the quaternion has changed in the last reset and the number of reset events + virtual void get_quat_reset(Quaternion *delta, uint8_t *counter) = 0; // get EKF innovation consistency check status virtual void get_innovation_test_status(uint16_t *val)