From 99a80387edaea26d49a5af842cc3d9cd9f2e0953 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 18 May 2018 13:57:11 +1000 Subject: [PATCH] EKF: improve optical flow GPS quality checking --- EKF/control.cpp | 63 ++++++++++++++++++++++++---------------------- EKF/ekf.h | 3 ++- EKF/gps_checks.cpp | 25 ++++++------------ 3 files changed, 42 insertions(+), 49 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 65c8dd145c..950d84a8a0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -353,35 +353,38 @@ void Ekf::controlOpticalFlowFusion() } } else { - bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6); - if (good_gps_aiding && !_range_aid_mode_enabled) { - // Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded - // limits for use of range finder for height - _time_bad_motion_us = _imu_sample_delayed.time_us; - } else { - _time_good_motion_us = _imu_sample_delayed.time_us; - } + _time_good_motion_us = _imu_sample_delayed.time_us; } - // Inhibit flow use if motion is un-suitable - // Apply a time based hysteresis to prevent rapid mode switching - if (!_inhibit_gndobs_use) { - if ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) { - _inhibit_gndobs_use = true; + // Inhibit flow use if motion is un-suitable or we have good quality GPS + // Apply hysteresis to prevent rapid mode switching + float gps_err_norm_lim; + if (_control_status.flags.opt_flow) { + gps_err_norm_lim = 0.7f; + } else { + gps_err_norm_lim = 1.0f; + } + if (!_inhibit_flow_use) { + bool movement_not_ok = (_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5; + bool good_gps_aiding = _control_status.flags.gps && _gps_error_norm < gps_err_norm_lim; + if (movement_not_ok || good_gps_aiding || !_range_aid_mode_enabled) { + _inhibit_flow_use = true; } } else { - if ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) { - _inhibit_gndobs_use = false; + bool movement_ok = (_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6; + bool bad_gps_aiding = _control_status.flags.gps && _gps_error_norm > gps_err_norm_lim; + if (movement_ok || bad_gps_aiding) { + _inhibit_flow_use = false; } } // Handle cases where we are using optical flow but are no longer able to because data is old // or its use has been inhibited. if (_control_status.flags.opt_flow) { - if (_inhibit_gndobs_use) { - _control_status.flags.opt_flow = false; - _time_last_of_fuse = 0; + if (_inhibit_flow_use) { + _control_status.flags.opt_flow = false; + _time_last_of_fuse = 0; } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { _control_status.flags.opt_flow = false; @@ -407,8 +410,8 @@ void Ekf::controlOpticalFlowFusion() _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } - // If the heading is valid and use is no tinhibited , start using optical flow aiding - if (_control_status.flags.yaw_align && !_inhibit_gndobs_use) { + // If the heading is valid and use is not inhibited , start using optical flow aiding + if (_control_status.flags.yaw_align && !_inhibit_flow_use) { // set the flag and reset the fusion timeout _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; @@ -1048,15 +1051,15 @@ void Ekf::rangeAidConditionsMet() // 3) Our terrain estimate is stable (needs better checks) // 4) We are in-air if (_control_status.flags.in_air) { - // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching - bool use_range_finder; + // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching + bool can_use_range_finder; if (_range_aid_mode_enabled) { - use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); + can_use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); } 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) && get_terrain_valid(); + can_use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid(); } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) @@ -1066,29 +1069,29 @@ void Ekf::rangeAidConditionsMet() float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); if (_range_aid_mode_enabled) { - use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + can_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; + can_use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid; } } else { - use_range_finder = false; + can_use_range_finder = false; } // use hysteresis to check for hagl if (_range_aid_mode_enabled) { - use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); + can_use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); } else { // if we were not using range aid in the previous iteration then use a much lower (1/100) threshold to avoid // switching to range finder too soon (wait for terrain to update). - use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f); + can_use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f); } - _range_aid_mode_enabled = use_range_finder; + _range_aid_mode_enabled = can_use_range_finder; } else { _range_aid_mode_enabled = false; diff --git a/EKF/ekf.h b/EKF/ekf.h index d127d23f87..4dfe9976d5 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -358,7 +358,7 @@ private: float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec) uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) - bool _inhibit_gndobs_use{false}; ///< true when use of ground observations (optical flow and range finder) is being temporarily inhibited due to excessive on-ground motion + bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad) @@ -379,6 +379,7 @@ private: float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec) uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks + float _gps_error_norm{1.0f}; ///< normalised gps error // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index ea7d5ee387..8b17f804ce 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -119,26 +119,15 @@ bool Ekf::gps_is_good(struct gps_message *gps) _gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop); // Check the reported horizontal and vertical position accuracy - if (_control_status.flags.opt_flow) { - // optical flow is used when low and slow and often in a confined environment - // so tighten required GPS accuracy that affects position and altitude hold - // This also applies some hysteresis to the logic used to activate optical flow - _gps_check_fail_status.flags.hacc = (gps->eph > 0.7f * _params.req_hacc); - _gps_check_fail_status.flags.vacc = (gps->epv > 0.7f * _params.req_vacc); - } else { - _gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc); - _gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc); - } + _gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc); + _gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc); // Check the reported speed accuracy - if (_control_status.flags.opt_flow) { - // Optical flow is used when low and slow and often in a confined environment - // so tighten required GPS accuracy that affects position hold - // This also applies some hysteresis to the logic used to activate optical flow - _gps_check_fail_status.flags.sacc = (gps->sacc > 0.7f * _params.req_sacc); - } else { - _gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc); - } + _gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc); + + // check if GPS quality is degraded + _gps_error_norm = fmaxf((gps->eph / _params.req_hacc) , (gps->epv / _params.req_vacc)); + _gps_error_norm = fmaxf(_gps_error_norm , (gps->sacc / _params.req_sacc)); // Calculate position movement since last measurement float delta_posN = 0.0f;