From e3af91c9cec817288940a7f9ce0d6b1501a2c954 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 24 Jan 2020 09:13:38 +0100 Subject: [PATCH] Ekf: use helper functions to centralize the checks of horizontal aiding (gps, flow, ev_pos, ev_vel) --- EKF/control.cpp | 34 ++++++++++------------------------ EKF/ekf_helper.cpp | 6 ++---- EKF/estimator_interface.cpp | 26 +++++++++++++++++++++++++- EKF/estimator_interface.h | 21 +++++++++++++++++++++ EKF/gps_checks.cpp | 2 +- 5 files changed, 59 insertions(+), 30 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 6025d270f7..0648c43b97 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -440,7 +440,7 @@ void Ekf::controlOpticalFlowFusion() // Check if we are in-air and require optical flow to control position drift bool flow_required = _control_status.flags.in_air && (_is_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || (_control_status.flags.opt_flow && !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel) // is completely reliant on optical flow + || (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad if (!_inhibit_flow_use && _control_status.flags.opt_flow) { @@ -491,7 +491,7 @@ void Ekf::controlOpticalFlowFusion() _time_last_of_fuse = _time_last_imu; // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set - const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); + const bool flow_aid_only = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow); if (flow_aid_only) { resetVelocity(); resetPosition(); @@ -506,10 +506,7 @@ void Ekf::controlOpticalFlowFusion() } // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period - if (_control_status.flags.opt_flow - && !_control_status.flags.gps - && !_control_status.flags.ev_pos - && !_control_status.flags.ev_vel) { + if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { bool do_reset = ((_time_last_imu - _time_last_of_fuse) > _params.reset_timeout_max); @@ -639,7 +636,7 @@ void Ekf::controlGpsFusion() } // Handle the case where we are using GPS and another source of aiding and GPS is failing checks - if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { + if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { stopGpsFusion(); // Reset position state to external vision if we are going to use absolute values if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { @@ -703,7 +700,7 @@ void Ekf::controlGpsFusion() const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); - if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { + if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // if we are using other sources of aiding, then relax the upper observation // noise limit which prevents bad GPS perturbing the position estimate gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit)); @@ -738,7 +735,7 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { stopGpsFusion(); ECL_WARN_TIMESTAMPED("GPS data stopped"); - } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel)) { + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal stopGpsFusion(); @@ -1109,8 +1106,6 @@ void Ekf::controlHeightFusion() if (_fuse_height) { - - if (_control_status.flags.baro_hgt) { Vector2f baro_hgt_innov_gate; Vector3f baro_hgt_obs_var; @@ -1192,15 +1187,10 @@ void Ekf::controlHeightFusion() void Ekf::checkRangeAidSuitability() { - const bool horz_vel_valid = _control_status.flags.gps - || _control_status.flags.ev_pos - || _control_status.flags.ev_vel - || _control_status.flags.opt_flow; - if (_control_status.flags.in_air && _rng_hgt_valid && isTerrainEstimateValid() - && horz_vel_valid) { + && isHorizontalAidingActive()) { // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice const bool is_in_range = _is_range_aid_suitable @@ -1328,11 +1318,8 @@ void Ekf::controlFakePosFusion() // if we aren't doing any aiding, fake position 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 (!_control_status.flags.gps && - !_control_status.flags.opt_flow && - !_control_status.flags.ev_pos && - !_control_status.flags.ev_vel && - !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) { + if (!isHorizontalAidingActive() + && !(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta)) { // We now need to use a synthetic position observation to prevent unconstrained drift of the INS states. _using_synthetic_position = true; @@ -1383,9 +1370,8 @@ void Ekf::controlFakePosFusion() void Ekf::controlAuxVelFusion() { bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed); - bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow; - if (data_ready && primary_aiding) { + if (data_ready && isHorizontalAidingActive()) { Vector2f aux_vel_innov_gate; Vector3f aux_vel_obs_var; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a2139c82e5..2e5da68ea3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1231,7 +1231,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; - const bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel); + const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); // Do not require limiting by default *vxy_max = NAN; @@ -1313,7 +1313,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) ekf_solution_status soln_status; // TODO: Is this accurate enough? soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0); - soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos|| _control_status.flags.ev_vel || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); + soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); @@ -1375,8 +1375,6 @@ void Ekf::uncorrelateQuatStates() P.uncorrelateCovariance<4>(0); } - - bool Ekf::global_position_is_valid() { // return true if the origin is set we are not doing unconstrained free inertial navigation diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 4e82d9ab8e..a7439dca63 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -374,7 +374,7 @@ void EstimatorInterface::setOpticalFlowData(const flowSample& flow) delta_time = delta_time_min; } - const bool relying_on_flow = !_control_status.flags.gps && !_control_status.flags.ev_pos && !_control_status.flags.ev_vel; + const bool relying_on_flow = !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow); const bool flow_quality_good = (flow.quality >= _params.flow_qual_min); @@ -571,6 +571,30 @@ bool EstimatorInterface::local_position_is_valid() return !_deadreckon_time_exceeded; } +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +{ + return int(_control_status.flags.gps) + + int(_control_status.flags.opt_flow) + + int(_control_status.flags.ev_pos) + + int(_control_status.flags.ev_vel); +} + +bool EstimatorInterface::isHorizontalAidingActive() const +{ + return getNumberOfActiveHorizontalAidingSources() > 0; +} + void EstimatorInterface::printBufferAllocationFailed(const char * buffer_name) { if(buffer_name) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 2ebeb30f66..a820277f91 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -252,6 +252,27 @@ public: // return true if the global position estimate is valid virtual bool global_position_is_valid() = 0; + // the flags considered are opt_flow, gps, ev_vel and ev_pos + bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; + + /* + * Check if there are any other active source of horizontal aiding + * Warning: does not tell if the selected source is + * active, use isOnlyActiveSourceOfHorizontalAiding() for this + * + * The flags considered are opt_flow, gps, ev_vel and ev_pos + * + * @param aiding_flag a flag in _control_status.flags + * @return true if an other source than aiding_flag is active + */ + bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + + // Return true if at least one source of horizontal aiding is active + // the flags considered are opt_flow, gps, ev_vel and ev_pos + bool isHorizontalAidingActive() const; + + int getNumberOfActiveHorizontalAidingSources() const; + // return true if the EKF is dead reckoning the position using inertial data only bool inertial_dead_reckoning() {return _is_dead_reckoning;} diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 613bac59f3..e070f1caaa 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -67,7 +67,7 @@ bool Ekf::collect_gps(const gps_message &gps) map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, correct for the change in position since the EKF started navigationg - if (_control_status.flags.opt_flow || _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel) { + if (isHorizontalAidingActive()) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);