From bd59e381dbf26a71bbae3befa531adf27bcda6a6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 26 Feb 2018 12:12:41 +1100 Subject: [PATCH 01/17] EKF: always run GPS checks --- EKF/gps_checks.cpp | 75 ++++++++++++++++++++++------------------------ 1 file changed, 35 insertions(+), 40 deletions(-) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index d10007c248..139ceadb45 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -58,46 +58,41 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { - // Run GPS checks whenever the WGS-84 origin is not set or the vehicle is not using GPS - // Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks - if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) { - bool gps_checks_pass = gps_is_good(gps); - - if (!_NED_origin_initialised && gps_checks_pass) { - // If we have good GPS data set the origin's WGS-84 position to the last gps fix - double lat = gps->lat / 1.0e7; - double lon = gps->lon / 1.0e7; - map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); - - // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating - if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); - map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu); - } - - // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin - _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); - _NED_origin_initialised = true; - _last_gps_origin_time_us = _time_last_imu; - // set the magnetic declination returned by the geo library using the current GPS position - _mag_declination_gps = math::radians(get_mag_declination(lat, lon)); - // save the horizontal and vertical position uncertainty of the origin - _gps_origin_eph = gps->eph; - _gps_origin_epv = gps->epv; - - // if the user has selected GPS as the primary height source, switch across to using it - if (_primary_hgt_source == VDIST_SENSOR_GPS) { - ECL_INFO("EKF GPS checks passed (WGS-84 origin set, using GPS height)"); - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = true; - _control_status.flags.rng_hgt = false; - // zero the sensor offset - _hgt_sensor_offset = 0.0f; - - } else { - ECL_INFO("EKF GPS checks passed (WGS-84 origin set)"); - } + // Run GPS checks always + bool gps_checks_pass = gps_is_good(gps); + if (!_NED_origin_initialised && gps_checks_pass) { + // If we have good GPS data set the origin's WGS-84 position to the last gps fix + double lat = gps->lat / 1.0e7; + double lon = gps->lon / 1.0e7; + map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); + + // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating + if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); + map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu); + } + + // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin + _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); + _NED_origin_initialised = true; + _last_gps_origin_time_us = _time_last_imu; + // set the magnetic declination returned by the geo library using the current GPS position + _mag_declination_gps = math::radians(get_mag_declination(lat, lon)); + // save the horizontal and vertical position uncertainty of the origin + _gps_origin_eph = gps->eph; + _gps_origin_epv = gps->epv; + + // if the user has selected GPS as the primary height source, switch across to using it + if (_primary_hgt_source == VDIST_SENSOR_GPS) { + ECL_INFO("EKF GPS checks passed (WGS-84 origin set, using GPS height)"); + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = true; + _control_status.flags.rng_hgt = false; + // zero the sensor offset + _hgt_sensor_offset = 0.0f; + } else { + ECL_INFO("EKF GPS checks passed (WGS-84 origin set)"); } } From 67d71ca4061548242f5e35361feb25d399b339a3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 26 Feb 2018 13:49:16 +1100 Subject: [PATCH 02/17] EKF: allow GPS fallback if quality bad and alternative aiding available --- EKF/control.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a18c0dec9..79abd042d3 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -460,10 +460,9 @@ void Ekf::controlGpsFusion() // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently + bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6); if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { - if (_control_status.flags.tilt_align && _NED_origin_initialised - && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) { - + if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); @@ -500,6 +499,12 @@ void Ekf::controlGpsFusion() } + // Handle the case where we are using GPS and another source of aiding and GPS is failing checks + if (_control_status.flags.gps && !gps_checks_passing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { + _control_status.flags.gps = false; + ECL_WARN("EKF GPS data quality poor - stopping use"); + } + // handle the case when we now have GPS, but have not been using it for an extended period if (_control_status.flags.gps) { // We are relying on aiding to constrain drift so after a specified time From 4ab78230e67c9aef863c5eaa1173c3b9cd05a8da Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 26 Feb 2018 14:56:20 +1100 Subject: [PATCH 03/17] EKF: Add persistence criteria to GPS fail check --- EKF/control.cpp | 3 ++- EKF/ekf.h | 1 + EKF/gps_checks.cpp | 2 ++ 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 79abd042d3..8a18c65655 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -461,6 +461,7 @@ void Ekf::controlGpsFusion() // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6); + bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6); if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states @@ -500,7 +501,7 @@ void Ekf::controlGpsFusion() } // Handle the case where we are using GPS and another source of aiding and GPS is failing checks - if (_control_status.flags.gps && !gps_checks_passing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { + if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) { _control_status.flags.gps = false; ECL_WARN("EKF GPS data quality poor - stopping use"); } diff --git a/EKF/ekf.h b/EKF/ekf.h index c7bd4ff6ce..e3877f5acc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -372,6 +372,7 @@ class Ekf : public EstimatorInterface float _gps_velN_filt{0.0f}; ///< GPS filtered North velocity (m/sec) 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 // 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 139ceadb45..c9cbc5e66c 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -228,6 +228,8 @@ bool Ekf::gps_is_good(struct gps_message *gps) (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) ) { _last_gps_fail_us = _time_last_imu; + } else { + _last_gps_pass_us = _time_last_imu; } // continuous period without fail of 10 seconds required to return a healthy status From 389786ef1bbac0d1844348e0539bf23aea319786 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 12 Mar 2018 21:01:41 +1100 Subject: [PATCH 04/17] EKF: Stop using EV for yaw when GPS fusion starts --- EKF/control.cpp | 14 +++++++------- EKF/ekf_helper.cpp | 10 ++++------ 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a18c65655..32bb61e0a2 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -151,9 +151,7 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) - && !(_params.fusion_mode & MASK_USE_EVYAW)) { - + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); } @@ -182,7 +180,7 @@ void Ekf::controlExternalVisionFusion() } // external vision yaw aiding selection logic - if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // don't start using EV data unless daa is arriving frequently if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // reset the yaw angle to the value from the observaton quaternion @@ -465,9 +463,12 @@ void Ekf::controlGpsFusion() if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states - if (!_control_status.flags.yaw_align) { + // Do not use external vision for yaw if using GPS because yaw needs to be + // defined relative to an NED reference frame + if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) { + _control_status.flags.yaw_align = false; + _control_status.flags.ev_yaw = false; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - } // If the heading is valid start using gps aiding @@ -490,7 +491,6 @@ void Ekf::controlGpsFusion() if (_control_status.flags.gps) { ECL_INFO("EKF commencing GPS fusion"); _time_last_gps = _time_last_imu; - } } } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 70cb873582..5ecc258e82 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -567,7 +567,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) Dcmf R_to_earth(euler321); // calculate the observed yaw angle - if (_params.fusion_mode & MASK_USE_EVYAW) { + if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence @@ -621,7 +621,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) R_to_earth(2, 1) = s1; // calculate the observed yaw angle - if (_params.fusion_mode & MASK_USE_EVYAW) { + if (_control_status.flags.ev_yaw) { // convert the observed quaternion to a rotation matrix Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame // calculate the yaw angle for a 312 sequence @@ -703,14 +703,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) _R_to_earth = quat_to_invrotmat(_state.quat_nominal); // reset the rotation from the EV to EKF frame of reference if it is being used - if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) - && !(_params.fusion_mode & MASK_USE_EVYAW)) { - + if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) { resetExtVisRotMat(); } // update the yaw angle variance using the variance of the measurement - if (_params.fusion_mode & MASK_USE_EVYAW) { + if (!_control_status.flags.ev_yaw) { // using error estimate from external vision data angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)); From 82de3141f00a22361c449543a004a7c091a046b7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 18 Apr 2018 08:26:03 +1000 Subject: [PATCH 05/17] EKF: Don't use optical flow if GPS is good and the vehicle is not using range finder for height --- EKF/control.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 32bb61e0a2..08ec2351ad 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -352,8 +352,13 @@ void Ekf::controlOpticalFlowFusion() } } else { - _time_bad_motion_us = 0; - _time_good_motion_us = _imu_sample_delayed.time_us; + // inhibit flow use if GPS quality is good and we are not using range aiding + // this enables use of optical flow to improve station keeping when low and slow + if (_control_status.flags.gps && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6) && !_in_range_aid_mode) { + _time_bad_motion_us = _imu_sample_delayed.time_us; + } else { + _time_good_motion_us = _imu_sample_delayed.time_us; + } } // Inhibit flow use if on ground and motion is excessive From f9d4934db6842191804067358295578d1d57e9c0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 19 Apr 2018 16:43:58 +1000 Subject: [PATCH 06/17] EKF: update comments --- EKF/control.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 08ec2351ad..5752fff5fd 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -337,8 +337,9 @@ void Ekf::controlExternalVisionFusion() void Ekf::controlOpticalFlowFusion() { - // Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated. + // Check if motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { + // Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated. bool motion_is_excessive = ((_accel_mag_filt > 10.8f) || (_accel_mag_filt < 8.8f) || (_ang_rate_mag_filt > 0.5f) @@ -352,16 +353,16 @@ void Ekf::controlOpticalFlowFusion() } } else { - // inhibit flow use if GPS quality is good and we are not using range aiding - // this enables use of optical flow to improve station keeping when low and slow if (_control_status.flags.gps && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6) && !_in_range_aid_mode) { + // 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; } } - // Inhibit flow use if on ground and motion is excessive + // 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) { From 0a6305275320a6b65614d4f2a0d322d9ac358af4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 26 Apr 2018 17:13:34 +1000 Subject: [PATCH 07/17] EKF: Fix false triggering of optical flow bad motion checks --- EKF/control.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 5752fff5fd..651b378894 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -340,8 +340,8 @@ void Ekf::controlOpticalFlowFusion() // Check if motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { // Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated. - bool motion_is_excessive = ((_accel_mag_filt > 10.8f) - || (_accel_mag_filt < 8.8f) + bool motion_is_excessive = ((_accel_mag_filt > 15.0f) + || (_accel_mag_filt < 5.0f) || (_ang_rate_mag_filt > 0.5f) || (_R_to_earth(2, 2) < 0.866f)); @@ -353,7 +353,8 @@ void Ekf::controlOpticalFlowFusion() } } else { - if (_control_status.flags.gps && (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6) && !_in_range_aid_mode) { + bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6); + if (good_gps_aiding && !_in_range_aid_mode) { // 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; From 6708bec1b45aaf0f4e9a6db2284b7777c369ab2f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 29 Apr 2018 17:28:30 +1000 Subject: [PATCH 08/17] EKF: Don't auto select range finder for height when on ground. --- EKF/control.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 651b378894..815915c977 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1042,7 +1042,8 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) // 1) we are not further than max_range_for_dual_fusion away from the ground // 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) { + // 4) We are in-air + if (_params.range_aid && _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; From 0c0a6602b07ce08fcb687a58a8b5cd8542cc6911 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 3 May 2018 09:44:32 +1000 Subject: [PATCH 09/17] EKF: Decouple range finder use criteria checking and selection --- EKF/control.cpp | 40 ++++++++++++++++++++-------------------- EKF/ekf.h | 6 ++++-- 2 files changed, 24 insertions(+), 22 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 815915c977..2f9574a68c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -354,7 +354,7 @@ 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 && !_in_range_aid_mode) { + 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; @@ -861,11 +861,13 @@ void Ekf::controlHeightFusion() _range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2; } + rangeAidConditionsMet(); + + _range_aid_mode_selected = (_params.range_aid == 1) && _range_aid_mode_enabled; if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { - _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { + if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -883,7 +885,7 @@ void Ekf::controlHeightFusion() } else if (!_in_range_aid_mode && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); _fuse_height = true; - _in_range_aid_mode = false; + _range_aid_mode_enabled = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -905,7 +907,7 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) { // switch to gps if there was a reset to gps _fuse_height = true; - _in_range_aid_mode = false; + _range_aid_mode_enabled = false; // we have just switched to using gps height, calculate height sensor offset such that current // measurment matches our current height estimate @@ -951,9 +953,8 @@ void Ekf::controlHeightFusion() // Determine if GPS should be used as the height source if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { + if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -971,7 +972,7 @@ void Ekf::controlHeightFusion() } else if (!_in_range_aid_mode && _gps_data_ready && !_gps_hgt_faulty) { setControlGPSHeight(); _fuse_height = true; - _in_range_aid_mode = false; + _range_aid_mode_enabled = false; // we have just switched to using gps height, calculate height sensor offset such that current // measurment matches our current height estimate @@ -982,7 +983,7 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro _fuse_height = true; - _in_range_aid_mode = false; + _range_aid_mode_enabled = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -997,7 +998,7 @@ void Ekf::controlHeightFusion() if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro _fuse_height = true; - _in_range_aid_mode = false; + _range_aid_mode_enabled = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -1035,19 +1036,18 @@ void Ekf::controlHeightFusion() } -bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) +void Ekf::rangeAidConditionsMet() { // 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 - // 1) we are not further than max_range_for_dual_fusion away from the ground - // 2) our ground speed is not higher than max_vel_for_dual_fusion + // 1) we are not further than max_hagl_for_range_aid away from the ground + // 2) our ground speed is not higher than max_vel_for_range_aid // 3) Our terrain estimate is stable (needs better checks) // 4) We are in-air - if (_params.range_aid && _control_status.flags.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; - - if (in_range_aid_mode) { + if (_range_aid_mode_enabled) { use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); } else { @@ -1062,7 +1062,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) if (horz_vel_valid) { float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); - if (in_range_aid_mode) { + if (_range_aid_mode_enabled) { use_range_finder &= ground_vel < _params.max_vel_for_range_aid; } else { @@ -1076,7 +1076,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) } // use hysteresis to check for hagl - if (in_range_aid_mode) { + if (_range_aid_mode_enabled) { use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); } else { @@ -1085,10 +1085,10 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f); } - return use_range_finder; + _range_aid_mode_enabled = use_range_finder; } else { - return false; + _range_aid_mode_enabled = false; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index e3877f5acc..26154e251c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -432,7 +432,8 @@ class Ekf : public EstimatorInterface 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{false}; ///< true when range finder is to be used as the height reference instead of the primary height sensor + bool _range_aid_mode_enabled{false}; ///< true when range finder can be used as the height reference instead of the primary height sensor + bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor // variables used to check for "stuck" rng data float _rng_check_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck @@ -584,7 +585,8 @@ class Ekf : public EstimatorInterface // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(); - bool rangeAidConditionsMet(bool in_range_aid_mode); + // determine if flight condition is suitable so use range finder instead of the primary height senor + void rangeAidConditionsMet(); // check for "stuck" range finder measurements when rng was not valid for certain period void checkForStuckRange(); From 24b005ed578ae3d728bc4d2298f9c80e84494484 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 3 May 2018 11:32:26 +1000 Subject: [PATCH 10/17] EKF: range finder aiding logic fixes --- EKF/control.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 2f9574a68c..b9ff3812fa 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -882,10 +882,9 @@ void Ekf::controlHeightFusion() } } - } else if (!_in_range_aid_mode && _baro_data_ready && !_baro_hgt_faulty) { + } else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) { setControlBaroHeight(); _fuse_height = true; - _range_aid_mode_enabled = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -907,7 +906,6 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) { // switch to gps if there was a reset to gps _fuse_height = true; - _range_aid_mode_enabled = false; // we have just switched to using gps height, calculate height sensor offset such that current // measurment matches our current height estimate @@ -969,10 +967,9 @@ void Ekf::controlHeightFusion() } } - } else if (!_in_range_aid_mode && _gps_data_ready && !_gps_hgt_faulty) { + } else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_faulty) { setControlGPSHeight(); _fuse_height = true; - _range_aid_mode_enabled = false; // we have just switched to using gps height, calculate height sensor offset such that current // measurment matches our current height estimate @@ -983,7 +980,6 @@ void Ekf::controlHeightFusion() } else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro _fuse_height = true; - _range_aid_mode_enabled = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset @@ -998,7 +994,6 @@ void Ekf::controlHeightFusion() if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) { // switch to baro if there was a reset to baro _fuse_height = true; - _range_aid_mode_enabled = false; // we have just switched to using baro height, we don't need to set a height sensor offset // since we track a separate _baro_hgt_offset From 02963a85c418fcc9df48dc33ded4fbaf65fbcaee Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 5 May 2018 16:28:52 +1000 Subject: [PATCH 11/17] EKF: relax optical flow on ground motion checks --- EKF/control.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index b9ff3812fa..04cb8c974b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -339,12 +339,10 @@ void Ekf::controlOpticalFlowFusion() { // Check if motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { - // Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated. - bool motion_is_excessive = ((_accel_mag_filt > 15.0f) - || (_accel_mag_filt < 5.0f) - || (_ang_rate_mag_filt > 0.5f) - || (_R_to_earth(2, 2) < 0.866f)); - + bool motion_is_excessive = ((_accel_mag_filt > 14.7f) + || (_accel_mag_filt < 4.9f) + || (_ang_rate_mag_filt > _params.flow_rate_max) + || (_R_to_earth(2,2) < 0.866f)); if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; From 868bc010878adaf5a124fc0c9d6cef4045fa06ac Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 5 May 2018 16:33:01 +1000 Subject: [PATCH 12/17] EKF: Relax minimum required range finder measurement rate --- EKF/terrain_estimator.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index c36721d156..221088ff77 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -207,16 +207,17 @@ void Ekf::get_hagl_innov_var(float *hagl_innov_var) // check that the range finder data is continuous void Ekf::checkRangeDataContinuity() { - // update range data continuous flag (2Hz ie 500 ms) + // update range data continuous flag (1Hz ie 1000 ms) /* Timing in micro seconds */ - /* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */ - _dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - _dt_update) + _dt_update * + /* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */ + float alpha = 0.5f * _dt_update; + _dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha * (_time_last_imu - _time_last_range); - _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 1e6f); + _dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 2e6f); - if (_dt_last_range_update_filt_us < 5e5f) { + if (_dt_last_range_update_filt_us < 1e6f) { _range_data_continuous = true; } else { From e26711af7d98811433ffd566ebe957dc07c26dfb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 5 May 2018 16:34:55 +1000 Subject: [PATCH 13/17] EKF: relax terrain update requirements for continuing optical flow use --- EKF/control.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 04cb8c974b..795a0bef23 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -441,8 +441,8 @@ void Ekf::controlOpticalFlowFusion() } } - // fuse the data if the terrain/distance to bottom is valid - if (_control_status.flags.opt_flow && get_terrain_valid()) { + // fuse the data if the terrain/distance to bottom is valid but use a more relaxed check to enable it to survive bad range finder data + if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) { // Update optical flow bias estimates calcOptFlowBias(); From bdf5b3e00324ea3a9357c8e916f57554f514b179 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 7 May 2018 11:20:17 +1000 Subject: [PATCH 14/17] EKF: Don't assume large position uncertainty when starting optical flow nav --- EKF/ekf_helper.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 5ecc258e82..aea0ad8fbf 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -168,7 +168,9 @@ bool Ekf::resetPosition() } - setDiag(P, 7, 8, sq(_params.pos_noaid_noise)); + // estimate is relative to initial positon in this mode, so we start with zero error. + zeroCols(P,7,8); + zeroRows(P,7,8); } else { // Used when falling back to non-aiding mode of operation From bf902e5eca49dcb911d97411fe17c9acc88fd4f2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 May 2018 07:47:17 +1000 Subject: [PATCH 15/17] EKF: Prevent flow motion check false positives The previous implementation could false trigger if there was significant vibration below 200Hz during startup. --- EKF/control.cpp | 10 ++++++---- EKF/covariance.cpp | 10 +++++++--- EKF/ekf.h | 3 ++- 3 files changed, 15 insertions(+), 8 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 795a0bef23..329c27f896 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -339,10 +339,12 @@ void Ekf::controlOpticalFlowFusion() { // Check if motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { - bool motion_is_excessive = ((_accel_mag_filt > 14.7f) - || (_accel_mag_filt < 4.9f) - || (_ang_rate_mag_filt > _params.flow_rate_max) - || (_R_to_earth(2,2) < 0.866f)); + // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation + float accel_norm = _accel_vec_filt.norm(); + bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g + || (accel_norm < 4.9f) // accel less than 0.5g + || (_ang_rate_mag_filt > _params.flow_rate_max) // angular rate exceeds flow sensor limit + || (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees if (motion_is_excessive) { _time_bad_motion_us = _imu_sample_delayed.time_us; diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 47a324144c..37d4474f11 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -163,9 +163,13 @@ void Ekf::predictCovariance() float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); // inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected - float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); - _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); - _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); + float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); + float beta = 1.0f - alpha; + _ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt); + _accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt); + _accel_vec_filt(0) = alpha * dt_inv * _imu_sample_delayed.delta_vel(0) + beta * _accel_vec_filt(0); + _accel_vec_filt(1) = alpha * dt_inv * _imu_sample_delayed.delta_vel(1) + beta * _accel_vec_filt(1); + _accel_vec_filt(2) = alpha * dt_inv * _imu_sample_delayed.delta_vel(2) + beta * _accel_vec_filt(2); if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim || _accel_mag_filt > _params.acc_bias_learn_acc_lim diff --git a/EKF/ekf.h b/EKF/ekf.h index 26154e251c..bfe6af0c70 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -402,7 +402,8 @@ class Ekf : public EstimatorInterface // variables used to inhibit accel bias learning bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited - float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (m/sec**2) + Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2) + float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2 From 6cadc922855c67120044a8b30ab78d5377cc1bd9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 9 May 2018 15:32:57 +1000 Subject: [PATCH 16/17] EKF: Don't reject saturated flow data when it is the only aiding source --- EKF/estimator_interface.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a4f4868ec0..40a55bbdef 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -364,20 +364,24 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // check magnitude is within sensor limits + // use this to prevent use of a saturated flow sensor when there are other aiding sources available float flow_rate_magnitude; bool flow_magnitude_good = true; - if (delta_time_good) { flow_rate_magnitude = flow->flowdata.norm() / delta_time; flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max); } + bool relying_on_flow = _control_status.flags.opt_flow + && !_control_status.flags.gps + && !_control_status.flags.ev_pos; + // check quality metric bool flow_quality_good = (flow->quality >= _params.flow_qual_min); // Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling // If flow quality fails checks on ground, assume zero flow rate after body rate compensation - if ((delta_time_good && flow_quality_good && flow_magnitude_good) || !_control_status.flags.in_air) { + if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) { flowSample optflow_sample_new; // calculate the system time-stamp for the mid point of the integration period optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2; From e8e9e34a73d3eda0923245e4ac1495b9fc52ae0d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 10 May 2018 13:14:09 +1000 Subject: [PATCH 17/17] EKF: fix bug causing height offset when GPS use stops This bug causes the last vertical velocity observation to be continuously fused. --- EKF/vel_pos_fusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 1b9b2f1204..e87f1899d6 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -183,7 +183,7 @@ void Ekf::fuseVelPosHeight() innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align; // record the successful velocity fusion event - if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) { + if ((_fuse_hor_vel || _fuse_hor_vel_aux || _fuse_vert_vel) && vel_check_pass) { _time_last_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_vel_NED = false; @@ -191,7 +191,7 @@ void Ekf::fuseVelPosHeight() _innov_check_fail_status.flags.reject_vel_NED = true; } - _fuse_hor_vel = _fuse_hor_vel_aux = false; + _fuse_hor_vel = _fuse_hor_vel_aux = _fuse_vert_vel = false; // record the successful position fusion event if (pos_check_pass && _fuse_pos) {