From 7c81350c7ad03187335962ae7758255b17ed3e8f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 4 Oct 2020 11:30:48 +1100 Subject: [PATCH] EKF: Don't yaw reset if not yaw induced nav failure --- EKF/control.cpp | 25 ++++++++++++++++++++----- EKF/ekf.h | 2 ++ EKF/vel_pos_fusion.cpp | 2 ++ 3 files changed, 24 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 3a305312f6..f672d47e53 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -304,7 +304,8 @@ void Ekf::controlExternalVisionFusion() Vector3f ev_vel_obs_var; Vector2f ev_vel_innov_gates; - _ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame(); + _last_vel_obs = getVisionVelocityInEkfFrame(); + _ev_vel_innov = _state.vel - _last_vel_obs; // check if we have been deadreckoning too long if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) { @@ -580,16 +581,28 @@ void Ekf::controlGpsFusion() _time_last_on_ground_us = _time_last_imu; } - const bool recent_takeoff_nav_failure = _control_status.flags.in_air && + bool recent_takeoff_nav_failure = _control_status.flags.in_air && !isTimedOut(_time_last_on_ground_us, 30000000) && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) && (_time_last_hor_vel_fuse > _time_last_on_ground_us); - const bool inflight_nav_failure = _control_status.flags.in_air && + bool inflight_nav_failure = _control_status.flags.in_air && do_vel_pos_reset && (_time_last_hor_vel_fuse > _time_last_on_ground_us) && (_time_last_hor_pos_fuse > _time_last_on_ground_us); + if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) { + // Do sanity check to see if the innovation failures is likely caused by a yaw angle error + // A 180 deg yaw error would (in the absence of other errors) generate a velocity innovation + // magnitude of twice the measured speed so allow up to 3 times to allow for additional errors. + const float hvel_obs_sq = sq(_last_vel_obs(0)) + sq(_last_vel_obs(1)); + const float hvel_innov_sq = sq(_last_fail_hvel_innov(0)) + sq(_last_fail_hvel_innov(1)); + if (hvel_innov_sq > 9.0f * hvel_obs_sq) { + recent_takeoff_nav_failure = false; + inflight_nav_failure = false; + } + } + // Detect if coming back after significant time without GPS data const bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000); const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || recent_takeoff_nav_failure || inflight_nav_failure) && @@ -665,7 +678,8 @@ void Ekf::controlGpsFusion() gps_vel_obs_var(2) = sq(1.5f) * gps_vel_obs_var(2); // calculate innovations - _gps_vel_innov = _state.vel - _gps_sample_delayed.vel; + _last_vel_obs = _gps_sample_delayed.vel; + _gps_vel_innov = _state.vel - _last_vel_obs; _gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos; // set innovation gate size @@ -1291,7 +1305,8 @@ void Ekf::controlAuxVelFusion() const Vector2f aux_vel_innov_gate(_params.auxvel_gate, _params.auxvel_gate); - _aux_vel_innov = _state.vel - _auxvel_sample_delayed.vel; + _last_vel_obs = _auxvel_sample_delayed.vel; + _aux_vel_innov = _state.vel - _last_vel_obs; fuseHorizontalVelocity(_aux_vel_innov, aux_vel_innov_gate, _auxvel_sample_delayed.velVar, _aux_vel_innov_var, _aux_vel_test_ratio); diff --git a/EKF/ekf.h b/EKF/ekf.h index f2cb22ac02..5c64e5bd28 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -391,6 +391,8 @@ private: Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance + Vector3f _last_vel_obs; ///< last velocity observation (m/s) + Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2 Vector3f _gps_vel_innov; ///< GPS velocity innovations (m/sec) Vector3f _gps_vel_innov_var; ///< GPS velocity innovation variances ((m/sec)**2) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 4e14773f55..911a065d07 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -66,6 +66,8 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga return true; } else { + _last_fail_hvel_innov(0) = innov(0); + _last_fail_hvel_innov(1) = innov(1); _innov_check_fail_status.flags.reject_hor_vel = true; return false; }