From c5f750dcca156ce84d1156ba66f7d25b797c3f45 Mon Sep 17 00:00:00 2001 From: Roman Date: Mon, 24 Oct 2016 09:40:22 +0200 Subject: [PATCH] ekf reset: more granular reset / timeout strategy for gps fusion - if both gps position and velocity measurements are rejected for 7 seconds do a reset - if only gps position measurements are rejected then wait for 14 seconds as we still have velocity measurements to constrain the drift in position - introduced ecl internal parameter for the timeout Signed-off-by: Roman --- EKF/common.h | 5 +++++ EKF/control.cpp | 34 +++++++++++++++++++--------------- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 84f3275e4b..f8aefced8d 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -261,6 +261,9 @@ struct parameters { float vel_Tau; // velocity state correction time constant (1/sec) float pos_Tau; // postion state correction time constant (1/sec) + unsigned no_gps_timeout_max; // maximum time we allow dead reckoning while both gps position and velocity measurements are being + // rejected + // Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility. parameters() { @@ -351,6 +354,8 @@ struct parameters { vel_Tau = 0.25f; pos_Tau = 0.25f; + no_gps_timeout_max = 7e6; // maximum seven seconds of dead reckoning time for gps + } }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 6e31ad2cdf..d693a7b810 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -355,9 +355,7 @@ void Ekf::controlGpsFusion() // handle the case when we are relying on GPS fusion and lose it if (_control_status.flags.gps && !_control_status.flags.opt_flow) { - // We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something - if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) { - if (_time_last_imu - _time_last_gps > 5e5) { + if (_time_last_imu - _time_last_gps > 5e5) { // if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states // and set the synthetic GPS position to the current estimate _control_status.flags.gps = false; @@ -365,19 +363,25 @@ void Ekf::controlGpsFusion() _last_known_posNE(1) = _state.pos(1); _state.vel.setZero(); ECL_WARN("EKF GPS fusion timout - stopping GPS aiding"); - - } else { - // Reset states to the last GPS measurement - resetPosition(); - resetVelocity(); - ECL_WARN("EKF GPS fusion timout - resetting to GPS"); - - // Reset the timeout counters - _time_last_pos_fuse = _time_last_imu; - _time_last_vel_fuse = _time_last_imu; - - } } + + // We are relying on GPS aiding to constrain attitude drift so after 7 seconds without 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_vel_fuse > _params.no_gps_timeout_max); + + // Our position measurments have been rejected for more than 14 seconds + do_reset |= _time_last_imu - _time_last_pos_fuse > 2 * _params.no_gps_timeout_max; + + if (do_reset) { + // Reset states to the last GPS measurement + resetPosition(); + resetVelocity(); + ECL_WARN("EKF GPS fusion timout - resetting to GPS"); + + // Reset the timeout counters + _time_last_pos_fuse = _time_last_imu; + _time_last_vel_fuse = _time_last_imu; + } + } // Only use GPS data for position and velocity aiding if enabled