From 4d59c834ebb181b78294ec3981c3d32b470e6f15 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Jul 2018 08:50:26 +1000 Subject: [PATCH] EKF: Consolidate range finder checking This brings all the range finder data checks (excluding innovation consistency checks) into one place and eliminates the need to perform range checking external to the library. The hard coded optical flow tilt limit is changed to use the same value as the range finder. Variable names are changed to make a clear distinction between the max/min values calculated by the stuck range check and the max/min valid values for the sensor. --- EKF/common.h | 2 +- EKF/control.cpp | 61 +++++++++++++++++++++++++++------------ EKF/ekf.h | 8 ++--- EKF/ekf_helper.cpp | 4 +-- EKF/estimator_interface.h | 8 ++--- 5 files changed, 53 insertions(+), 30 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index d14ddea111..7a749898bc 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -282,7 +282,7 @@ struct parameters { float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1) int32_t range_aid{0}; ///< allow switching primary height source to range finder if certian conditions are met float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion - float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder data + float range_cos_max_tilt{0.7071f}; ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data // vision position fusion float ev_innov_gate{5.0f}; ///< vision estimator fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index c90ce59713..df00e4f0a0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -107,21 +107,21 @@ void Ekf::controlFusionModes() } // calculate 2,2 element of rotation matrix from sensor frame to earth frame + // this is required for use of range finder and flow data _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; - // Get range data from buffer and check that is within limits and the vehicle is not excessively tilted - _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) - && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt) - && (_range_sample_delayed.rng >= _rng_min_distance) - && (_range_sample_delayed.rng <= _rng_max_distance); + // Get range data from buffer and check validity + _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed); - checkForStuckRange(); + if (_range_data_ready) { + checkRangeDataValidity(); + } - // We don't fuse flow data immediately becasue we have to wait for the mid integration point to fall behind the fusion time horizon. + // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. // This means we stop looking for new data until the old data has been fused. if (!_flow_data_ready) { _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) - && (_R_to_earth(2, 2) > 0.7071f); + && (_R_to_earth(2, 2) > _params.range_cos_max_tilt); } _ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); @@ -1140,33 +1140,56 @@ void Ekf::rangeAidConditionsMet() } } -void Ekf::checkForStuckRange() +void Ekf::checkRangeDataValidity() { - if (_range_data_ready && _range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 && + // Check if excessively tilted + if (_R_rng_to_earth_2_2 < _params.range_cos_max_tilt) { + _range_data_ready = false; + return; + } + + // Check if out of range + if ((_range_sample_delayed.rng > _rng_valid_max_val) + || (_range_sample_delayed.rng < _rng_valid_min_val)) { + if (_control_status.flags.in_air) { + _range_data_ready = false; + return; + } else { + // Range finders can fail to provide valid readings when resting on the ground + // or being handled by the user, which prevents use of as a primary height sensor. + // To work around this issue, we replace out of range data with the expected on ground value. + _range_sample_delayed.rng = _params.rng_gnd_clearance; + return; + } + } + + // Check for "stuck" range finder measurements when rng was not valid for certain period + // This handles a failure mode observed with some lidar sensors + if (_range_sample_delayed.time_us - _time_last_rng_ready > (uint64_t)10e6 && _control_status.flags.in_air) { _control_status.flags.rng_stuck = true; - //require a variance of rangefinder values to check for "stuck" measurements - if (_rng_check_max_val - _rng_check_min_val > 1.0f) { + // require a variance of rangefinder values to check for "stuck" measurements + if (_rng_stuck_max_val - _rng_stuck_min_val > 1.0f) { _time_last_rng_ready = _range_sample_delayed.time_us; - _rng_check_min_val = 0.0f; - _rng_check_max_val = 0.0f; + _rng_stuck_min_val = 0.0f; + _rng_stuck_max_val = 0.0f; _control_status.flags.rng_stuck = false; } else { - if (_range_sample_delayed.rng > _rng_check_max_val) { - _rng_check_max_val = _range_sample_delayed.rng; + if (_range_sample_delayed.rng > _rng_stuck_max_val) { + _rng_stuck_max_val = _range_sample_delayed.rng; } - if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) { - _rng_check_min_val = _range_sample_delayed.rng; + if (_rng_stuck_min_val < 0.1f || _range_sample_delayed.rng < _rng_stuck_min_val) { + _rng_stuck_min_val = _range_sample_delayed.rng; } _range_data_ready = false; } - } else if (_range_data_ready) { + } else { _time_last_rng_ready = _range_sample_delayed.time_us; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 89f4873147..ec39810748 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -443,9 +443,9 @@ private: bool _range_aid_mode_enabled{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor - // variables used to check for "stuck" rng data - float _rng_check_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck - float _rng_check_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck + // variables used to check range finder validity data + float _rng_stuck_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck + float _rng_stuck_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck // update the real time complementary filter states. This includes the prediction // and the correction step @@ -598,7 +598,7 @@ private: void rangeAidConditionsMet(); // check for "stuck" range finder measurements when rng was not valid for certain period - void checkForStuckRange(); + void checkRangeDataValidity(); // return the square of two floating point numbers - used in auto coded sections static constexpr float sq(float var) { return var * var; } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 52e962115c..778661c8c0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1060,9 +1060,9 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) { // Calculate range finder limits - float rangefinder_hagl_min = _rng_min_distance; + float rangefinder_hagl_min = _rng_valid_min_val; // Allow use of 75% of rangefinder maximum range to allow for angular motion - float rangefinder_hagl_max = 0.75f * _rng_max_distance; + float rangefinder_hagl_max = 0.75f * _rng_valid_max_val; // Calculate optical flow limits // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 1e49cef6fe..fa45e4fc28 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -237,8 +237,8 @@ public: // set sensor limitations reported by the rangefinder void set_rangefinder_limits(float min_distance, float max_distance) { - _rng_min_distance = min_distance; - _rng_max_distance = max_distance; + _rng_valid_min_val = min_distance; + _rng_valid_max_val = max_distance; } // set sensor limitations reported by the optical flow sensor @@ -429,8 +429,8 @@ protected: float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Sensor limitations - float _rng_min_distance{0.0f}; ///< minimum distance that the rangefinder can measure (m) - float _rng_max_distance{0.0f}; ///< maximum distance that the rangefinder can measure (m) + float _rng_valid_min_val{0.0f}; ///< minimum distance that the rangefinder can measure (m) + float _rng_valid_max_val{0.0f}; ///< maximum distance that the rangefinder can measure (m) float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m)