From 39983a7d55ba29b2e801fa9fb451f35048a24b41 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 6 Apr 2017 10:07:55 +0200 Subject: [PATCH] range aid: added hysteresis for switching in and out of range aid - prevents rapid switching - added innovation consistency check for using range aid Signed-off-by: Roman --- EKF/common.h | 1 + EKF/control.cpp | 38 ++++++++++++++++++++++++++------------ EKF/ekf.cpp | 1 + EKF/ekf.h | 5 ++++- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 4b841df072..9c467f4030 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -260,6 +260,7 @@ struct parameters { float max_hagl_for_range_aid{5.0f}; // maximum height above ground for which we allow to use the range finder as height source (if range_aid == 1) float max_vel_for_range_aid{1.0f}; // maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1) int range_aid{0}; // allow switching primary height source to range finder if certian conditions are met + float range_aid_innov_gate{1.0f}; // gate size used for innovation consistency checks for range aid fusion // vision position fusion float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index 5441291696..c4907d720d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -704,9 +704,9 @@ void Ekf::controlHeightFusion() if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { - bool can_use_range = rangeAidConditionsMet(); + _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (can_use_range && _range_data_ready && !_rng_hgt_faulty) { + if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -716,7 +716,7 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); } - } else if (_baro_data_ready && !_baro_hgt_faulty && !can_use_range) { + } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { setControlBaroHeight(); _fuse_height = true; @@ -736,9 +736,9 @@ void Ekf::controlHeightFusion() // Determine if GPS should be used as the height source if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - bool can_use_range = rangeAidConditionsMet(); + _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (can_use_range && _range_data_ready && !_rng_hgt_faulty) { + if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -748,7 +748,7 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); } - } else if (_gps_data_ready && !_gps_hgt_faulty && !can_use_range) { + } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) { setControlGPSHeight(); _fuse_height = true; @@ -786,7 +786,7 @@ void Ekf::controlHeightFusion() } -bool Ekf::rangeAidConditionsMet() +bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) { // if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source // under the following conditions @@ -794,8 +794,15 @@ bool Ekf::rangeAidConditionsMet() // 2) our ground speed is not higher than max_vel_for_dual_fusion // 3) Our terrain estimate is stable (needs better checks) if (_params.range_aid) { - // check if we should use range finder measurements to estimate height - bool use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; + // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching + bool use_range_finder; + if (in_range_aid_mode) { + use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; + } else { + // if we were not using range aid in the previous iteration then require the current height above terrain to be + // smaller than 70 % of the maximum allowed ground distance for range aid + use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised; + } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); @@ -803,14 +810,21 @@ bool Ekf::rangeAidConditionsMet() if (horz_vel_valid) { float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); - use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + if (in_range_aid_mode) { + use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + + } else { + // if we were not using range aid in the previous iteration then require the ground velocity to be + // smaller than 70 % of the maximum allowed ground velocity for range aid + use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid; + + } } else { use_range_finder = false; } - // XXX need a way to tell if terrain is stable - use_range_finder &= fabsf(_hagl_innov) < 0.3f; + use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); return use_range_finder; } else { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2c35687f86..6a9e4b6b84 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -57,6 +57,7 @@ #define ISFINITE(x) __builtin_isfinite(x) #endif + bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); diff --git a/EKF/ekf.h b/EKF/ekf.h index 6b0efa4e41..9f9202aec8 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -375,6 +375,9 @@ private: uint64_t _time_good_vert_accel{0}; // last time a good vertical accel was detected (usec) bool _bad_vert_accel_detected{false}; // true when bad vertical accelerometer data has been detected + // variables used to control range aid functionality + bool _in_range_aid_mode; + // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates(); @@ -505,7 +508,7 @@ private: // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(); - bool rangeAidConditionsMet(); + bool rangeAidConditionsMet(bool in_range_aid_mode); // return the square of two floating point numbers - used in auto coded sections inline float sq(float var)