From 342c3ab202b62ef0d0ea45e6c98535934ac2674a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 28 Aug 2018 07:00:53 +1000 Subject: [PATCH] EKF: Fix timeout parameter documentation and name The parameter used to control the maximum dead reckoning time had 'gps' in the parameter name which was confusing because it was used for all measurement types capable of constraining horizontal velocity error growth. The parameter variable has been renamed and the documentation for it improved. The parameter used to control the maximum time since fusing a measurement before the measurement is considered to be not contributing to aiding had misleading documentation which has been updated. --- EKF/common.h | 6 ++---- EKF/control.cpp | 18 +++++++++--------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index eef4102415..54badc3f10 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -324,10 +324,8 @@ struct parameters { float acc_bias_learn_gyr_lim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) - unsigned no_gps_timeout_max{7000000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement (uSec) - unsigned no_aid_timeout_max{1000000}; ///< maximum lapsed time from last fusion of measurements that constrain horizontal velocity drift before - ///< the EKF will report that it has been inertial dead-reckoning for too long and needs to revert to a - /// mode that doesn't privide horizontal vbelocity and position estimates (uSec) + unsigned reset_timeout_max{7000000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement or change _control_status if the data is unavailable (uSec) + unsigned no_aid_timeout_max{1000000}; ///< maximum lapsed time from last fusion of a measurement that constrains horizontal velocity drift before the EKF will determine that the sensor is no longer contributing to aiding (uSec) int32_t valid_timeout_max{5000000}; ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9b37f151d6..700e118a28 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -310,7 +310,7 @@ void Ekf::controlExternalVisionFusion() _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); // check if we have been deadreckoning too long - if (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) { + if (_time_last_imu - _time_last_pos_fuse > _params.reset_timeout_max) { // don't reset velocity if we have another source of aiding constraining it if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) { resetVelocity(); @@ -343,7 +343,7 @@ void Ekf::controlExternalVisionFusion() } else if (_control_status.flags.ev_pos && (_time_last_imu >= _time_last_ext_vision) - && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) { + && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received _control_status.flags.ev_pos = false; @@ -420,7 +420,7 @@ void Ekf::controlOpticalFlowFusion() _control_status.flags.opt_flow = false; _time_last_of_fuse = 0; - } else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.no_gps_timeout_max) { + } else if (_time_last_imu - _time_last_of_fuse > (uint64_t)_params.reset_timeout_max) { _control_status.flags.opt_flow = false; } @@ -465,7 +465,7 @@ void Ekf::controlOpticalFlowFusion() && !_control_status.flags.gps && !_control_status.flags.ev_pos) { - bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max; + bool do_reset = _time_last_imu - _time_last_of_fuse > _params.reset_timeout_max; if (do_reset) { resetVelocity(); @@ -572,13 +572,13 @@ void Ekf::controlGpsFusion() if (_control_status.flags.gps) { // We are relying on aiding to constrain drift so after a specified time // with no aiding we need to do something - bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) - && (_time_last_imu - _time_last_delpos_fuse > _params.no_gps_timeout_max) - && (_time_last_imu - _time_last_vel_fuse > _params.no_gps_timeout_max) - && (_time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max); + bool do_reset = (_time_last_imu - _time_last_pos_fuse > _params.reset_timeout_max) + && (_time_last_imu - _time_last_delpos_fuse > _params.reset_timeout_max) + && (_time_last_imu - _time_last_vel_fuse > _params.reset_timeout_max) + && (_time_last_imu - _time_last_of_fuse > _params.reset_timeout_max); // We haven't had an absolute position fix for a longer time so need to do something - do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max); + do_reset = do_reset || (_time_last_imu - _time_last_pos_fuse > 2 * _params.reset_timeout_max); if (do_reset) { // use GPS velocity data to check and correct yaw angle if a FW vehicle