From 8a012436f938dd29653226538be33006a9defd60 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 22 Mar 2018 22:39:00 +1100 Subject: [PATCH] EKF: Fix reversion from GPS to no-aiding mode (#412) * EKF: Do not delay reversion to no-aiding mode if parameter initiated * EKF: Move no-aid reversion resets to helper functions * EKF: Prevent unwanted fusion of velocity data during no aiding mode --- EKF/control.cpp | 13 ++++++++++--- EKF/ekf_helper.cpp | 10 ++++++++-- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 67439bd42c..291dfd0b92 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1339,6 +1339,9 @@ void Ekf::controlVelPosFusion() { // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz + if (!(_params.fusion_mode & MASK_USE_GPS)) { + _control_status.flags.gps = false; + } if (!_control_status.flags.gps && !_control_status.flags.opt_flow && !_control_status.flags.ev_pos && @@ -1347,9 +1350,8 @@ void Ekf::controlVelPosFusion() { // Reset position and velocity states if we re-commence this aiding method if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) { - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); - _state.vel.setZero(); + resetPosition(); + resetVelocity(); _fuse_hpos_as_odom = false; if (_time_last_fake_gps != 0) { ECL_WARN("EKF stopping navigation"); @@ -1358,6 +1360,8 @@ void Ekf::controlVelPosFusion() } _fuse_pos = true; + _fuse_hor_vel = false; + _fuse_vert_vel = false; _time_last_fake_gps = _time_last_imu; if (_control_status.flags.in_air && _control_status.flags.tilt_align) { @@ -1365,6 +1369,9 @@ void Ekf::controlVelPosFusion() } else { _posObsNoiseNE = 0.5f; } + _vel_pos_innov[0] = 0.0f; + _vel_pos_innov[1] = 0.0f; + _vel_pos_innov[2] = 0.0f; _vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0); _vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 501c22dfcc..a33b5ea7fd 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -64,7 +64,10 @@ bool Ekf::resetVelocity() zeroOffDiag(P,4,6); } else { - return false; + // Used when falling back to non-aiding mode of operation + _state.vel(0) = 0.0f; + _state.vel(1) = 0.0f; + setDiag(P,4,5,25.0f); } // calculate the change in velocity and apply to the output predictor state history @@ -122,7 +125,10 @@ bool Ekf::resetPosition() setDiag(P,7,8,sq(_ev_sample_delayed.posErr)); } else { - return false; + // Used when falling back to non-aiding mode of operation + _state.pos(0) = _last_known_posNE(0); + _state.pos(1) = _last_known_posNE(1); + setDiag(P,7,8,sq(_params.pos_noaid_noise)); } // calculate the change in position and apply to the output predictor state history