From bb790c9bec5e70f8c686750c241e9d521f6dfde3 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 15 Sep 2022 15:16:47 +0200 Subject: [PATCH] ekf2: add vertical dead-reckoning timeout check for height validity --- src/modules/ekf2/EKF/control.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 20 ++++++++-- src/modules/ekf2/EKF/ekf_helper.cpp | 43 +++++++++++++++++----- src/modules/ekf2/EKF/estimator_interface.h | 4 +- src/modules/ekf2/EKF2.cpp | 4 +- 5 files changed, 57 insertions(+), 16 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index c1ad433db6..b5b2afcacf 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -181,7 +181,7 @@ void Ekf::controlFusionModes() controlFakeHgtFusion(); // check if we are no longer fusing measurements that directly constrain velocity drift - update_deadreckoning_status(); + updateDeadReckoningStatus(); } void Ekf::controlExternalVisionFusion() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 1dfa5597f3..49a459f3ea 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -272,7 +272,17 @@ public: // return true if the local position estimate is valid bool local_position_is_valid() const { - return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos); + return (!_horizontal_deadreckon_time_exceeded && !_control_status.flags.fake_pos); + } + + bool isLocalVerticalPositionValid() const + { + return !_vertical_position_deadreckon_time_exceeded && !_control_status.flags.fake_hgt; + } + + bool isLocalVerticalVelocityValid() const + { + return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt; } bool isTerrainEstimateValid() const { return _hagl_valid; }; @@ -430,7 +440,9 @@ private: bool initialiseTilt(); // check if the EKF is dead reckoning horizontal velocity using inertial data only - void update_deadreckoning_status(); + void updateDeadReckoningStatus(); + void updateHorizontalDeadReckoningstatus(); + void updateVerticalDeadReckoningStatus(); void updateTerrainValidity(); @@ -470,7 +482,9 @@ private: bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator uint64_t _time_prev_gps_us{0}; ///< time stamp of previous GPS data retrieved from the buffer (uSec) - uint64_t _time_last_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) + uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) + uint64_t _time_last_v_pos_aiding{0}; + uint64_t _time_last_v_vel_aiding{0}; uint64_t _time_last_hor_pos_fuse{0}; ///< time the last fusion of horizontal position measurements was performed (uSec) uint64_t _time_last_hgt_fuse{0}; ///< time the last fusion of vertical position measurements was performed (uSec) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9a42ccf214..edf423549b 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -731,7 +731,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - if (_deadreckon_time_exceeded && _control_status.flags.gps) { + if (_horizontal_deadreckon_time_exceeded && _control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } @@ -747,7 +747,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors // and using state variances for accuracy reporting is overly optimistic in these situations - if (_deadreckon_time_exceeded) { + if (_horizontal_deadreckon_time_exceeded) { float vel_err_conservative = 0.0f; if (_control_status.flags.opt_flow) { @@ -1016,8 +1016,13 @@ void Ekf::uncorrelateQuatFromOtherStates() P.slice<4, _k_num_states - 4>(0, 4) = 0.f; } -// return true if we are totally reliant on inertial dead-reckoning for position -void Ekf::update_deadreckoning_status() +void Ekf::updateDeadReckoningStatus() +{ + updateHorizontalDeadReckoningstatus(); + updateVerticalDeadReckoningStatus(); +} + +void Ekf::updateHorizontalDeadReckoningstatus() { const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) @@ -1034,20 +1039,40 @@ void Ekf::update_deadreckoning_status() if (!_control_status.flags.inertial_dead_reckoning) { if (_imu_sample_delayed.time_us > _params.no_aid_timeout_max) { - _time_last_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max; + _time_last_horizontal_aiding = _imu_sample_delayed.time_us - _params.no_aid_timeout_max; } } // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present - bool deadreckon_time_exceeded = (_time_last_aiding == 0) - || isTimedOut(_time_last_aiding, (uint64_t)_params.valid_timeout_max); + bool deadreckon_time_exceeded = (_time_last_horizontal_aiding == 0) + || isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max); - if (!_deadreckon_time_exceeded && deadreckon_time_exceeded) { + if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) { // deadreckon time now exceeded ECL_WARN("dead reckon time exceeded"); } - _deadreckon_time_exceeded = deadreckon_time_exceeded; + _horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded; +} + +void Ekf::updateVerticalDeadReckoningStatus() +{ + if (isVerticalPositionAidingActive()) { + _time_last_v_pos_aiding = _time_last_hgt_fuse; + _vertical_position_deadreckon_time_exceeded = false; + + } else if ((_time_last_v_pos_aiding == 0) || isTimedOut(_time_last_v_pos_aiding, (uint64_t)_params.valid_timeout_max)) { + _vertical_position_deadreckon_time_exceeded = true; + } + + if (isVerticalVelocityAidingActive()) { + _time_last_v_vel_aiding = _time_last_ver_vel_fuse; + _vertical_velocity_deadreckon_time_exceeded = false; + + } else if (((_time_last_v_vel_aiding == 0) || isTimedOut(_time_last_v_vel_aiding, (uint64_t)_params.valid_timeout_max)) + && _vertical_position_deadreckon_time_exceeded) { + _vertical_velocity_deadreckon_time_exceeded = true; + } } // calculate the variances for the rotation vector equivalent diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 37dfda4bdb..511f607c46 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -349,7 +349,9 @@ protected: Vector2f _drag_test_ratio{}; // drag innovation consistency check ratio innovation_fault_status_u _innov_check_fail_status{}; - bool _deadreckon_time_exceeded{true}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid + bool _horizontal_deadreckon_time_exceeded{true}; + bool _vertical_position_deadreckon_time_exceeded{true}; + bool _vertical_velocity_deadreckon_time_exceeded{true}; float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s) float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 5d83675da1..525bf3f698 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1098,9 +1098,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) // TODO: better status reporting lpos.xy_valid = _ekf.local_position_is_valid(); - lpos.z_valid = !_preflt_checker.hasVertFailed(); + lpos.z_valid = _ekf.isLocalVerticalPositionValid(); lpos.v_xy_valid = _ekf.local_position_is_valid(); - lpos.v_z_valid = !_preflt_checker.hasVertFailed(); + lpos.v_z_valid = _ekf.isLocalVerticalVelocityValid(); // Position of local NED origin in GPS / WGS84 frame if (_ekf.global_origin_valid()) {