From 696eeb9a494224ca83548c196e4cfc6144cb7aae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 13 Dec 2022 13:29:18 -0500 Subject: [PATCH] ekf2: update EV height state machine (small piece of EV overhaul) - respect new EKF2_EV_CTRL parameter for VPOS usage - EV hgt rotate EV position before usage (there's often a small offset in frames) - EV hgt reset use proper EV velocity body frame - try to keep EV hgt and EV vel state machines consistent - small incremental piece of https://github.com/PX4/PX4-Autopilot/pull/19128 --- src/modules/ekf2/EKF/bias_estimator.hpp | 14 +- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/control.cpp | 7 +- src/modules/ekf2/EKF/ekf.h | 4 +- src/modules/ekf2/EKF/ekf_helper.cpp | 1 + src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- src/modules/ekf2/EKF/ev_height_control.cpp | 217 ++++++++++-------- src/modules/ekf2/EKF/ev_vel_control.cpp | 8 +- src/modules/ekf2/EKF/height_control.cpp | 1 - src/modules/ekf2/EKF2.cpp | 1 + src/modules/ekf2/EKF2.hpp | 2 +- .../test/sensor_simulator/ekf_wrapper.cpp | 2 +- 12 files changed, 149 insertions(+), 111 deletions(-) diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp index bfc3409f57..10448beb64 100644 --- a/src/modules/ekf2/EKF/bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,11 +57,11 @@ class BiasEstimator { public: struct status { - float bias; - float bias_var; - float innov; - float innov_var; - float innov_test_ratio; + float bias{0.f}; + float bias_var{0.f}; + float innov{0.f}; + float innov_var{0.f}; + float innov_test_ratio{INFINITY}; }; BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {}; @@ -107,7 +107,7 @@ private: float _time_since_last_negative_innov{0.f}; float _time_since_last_positive_innov{0.f}; - status _status; + status _status{}; void constrainStateVar(); float computeKalmanGain(float innov_var) const; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 6f876bff1e..1beac4b6fa 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -380,6 +380,7 @@ struct parameters { // vision position fusion float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec) + float ev_pos_noise{0.1f}; ///< minimum allowed observation noise for EV position fusion (m) float ev_att_noise{0.1f}; ///< minimum allowed observation noise for EV attitude fusion (rad/sec) int32_t ev_quality_minimum{0}; ///< vision minimum acceptable quality integer float ev_vel_innov_gate{3.0f}; ///< vision velocity fusion innovation consistency gate size (STD) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 5acba0d7d4..e1b91e0deb 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -400,7 +400,7 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the horizontal position observations bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (_ev_sample_delayed.quality >= _params.ev_quality_minimum); - bool starting_conditions_passing = quality_sufficient + const bool starting_conditions_passing = quality_sufficient && ((_ev_sample_delayed.time_us - _ev_sample_delayed_prev.time_us) < EV_MAX_INTERVAL) && ((_params.ev_quality_minimum <= 0) || (_ev_sample_delayed_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient @@ -409,13 +409,16 @@ void Ekf::controlExternalVisionFusion() // EV velocity controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + // EV height + controlEvHeightFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt); + // record observation and estimate for use next time _ev_sample_delayed_prev = _ev_sample_delayed; _hpos_pred_prev = _state.pos.xy(); _yaw_pred_prev = getEulerYaw(_state.quat_nominal); - } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) + } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt) && !isRecent(_ev_sample_delayed.time_us, (uint64_t)_params.reset_timeout_max)) { // Turn off EV fusion mode if no data has been received diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 08ff69bec4..b3d2a832b4 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -851,7 +851,8 @@ private: // control fusion of external vision observations void controlExternalVisionFusion(); - void controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, bool quality_sufficient, estimator_aid_source3d_s& aid_src); + void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s& aid_src); + void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s& aid_src); // control fusion of optical flow observations void controlOpticalFlowFusion(); @@ -922,7 +923,6 @@ private: void controlBaroHeightFusion(); void controlGnssHeightFusion(const gpsSample &gps_sample); void controlRangeHeightFusion(); - void controlEvHeightFusion(const extVisionSample &ev_sample); bool isConditionalRangeAidSuitable(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 690027bb1f..900a334776 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1346,6 +1346,7 @@ void Ekf::stopEvFusion() stopEvPosFusion(); stopEvVelFusion(); stopEvYawFusion(); + stopEvHgtFusion(); } void Ekf::stopEvPosFusion() diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 7764259ca1..3954d2adcd 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -542,7 +542,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); } - if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { + if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl > 0)) { max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); } diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 01b08e8f89..1335d4f0cc 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -38,94 +38,79 @@ #include "ekf.h" -void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) +void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src) { - static constexpr const char *HGT_SRC_NAME = "EV"; + static constexpr const char *AID_SRC_NAME = "EV height"; - auto &aid_src = _aid_src_ev_hgt; HeightBiasEstimator &bias_est = _ev_hgt_b_est; bias_est.predict(_dt_ekf_avg); - if (_ev_data_ready) { + // correct position for offset relative to IMU + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const float measurement = ev_sample.pos(2); - const float measurement_var = ev_sample.posVar(2); + // rotate measurement into correct earth frame if required + Vector3f pos{ev_sample.pos}; + Matrix3f pos_cov{matrix::diag(ev_sample.posVar)}; - const float innov_gate = math::max(_params.ev_pos_innov_gate, 1.f); + // rotate EV to the EKF reference frame unless we're operating entirely in vision frame + // TODO: only necessary if there's a roll/pitch offset between VIO and EKF + if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) { - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); + const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); - updateVerticalPositionAidSrcStatus(ev_sample.time_us, - measurement - bias_est.getBias(), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); + if (q_error.isAllFinite()) { + const Dcmf R_ev_to_ekf(q_error); - // update the bias estimator before updating the main filter but after - // using its current state to compute the vertical position innovation - if (measurement_valid) { - bias_est.setMaxStateNoise(sqrtf(measurement_var)); - bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); - bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); + pos = R_ev_to_ekf * ev_sample.pos; + pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.posVar) * R_ev_to_ekf.transpose(); + + // increase minimum variance to include EV orientation variance + // TODO: do this properly + const float orientation_var_max = math::max(ev_sample.orientation_var(0), ev_sample.orientation_var(1)); + pos_cov(2, 2) = math::max(pos_cov(2, 2), orientation_var_max); } + } - const bool continuing_conditions_passing = ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) || (_params.height_sensor_ref == HeightSensor::EV)) // TODO: (_params.ev_ctrl & EvCtrl::VPOS) - && measurement_valid; + const float measurement = pos(2) - pos_offset_earth(2); + float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f)); - const bool starting_conditions_passing = continuing_conditions_passing - && isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL); + // increase minimum variance if GPS active + if (_control_status.flags.gps_hgt) { + measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise)); + } - if (_control_status.flags.ev_hgt) { - aid_src.fusion_enabled = true; + const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); - if (continuing_conditions_passing) { + updateVerticalPositionAidSrcStatus(ev_sample.time_us, + measurement - bias_est.getBias(), + measurement_var + bias_est.getBiasVar(), + math::max(_params.ev_pos_innov_gate, 1.f), + aid_src); - fuseVerticalPosition(aid_src); + // update the bias estimator before updating the main filter but after + // using its current state to compute the vertical position innovation + if (measurement_valid && quality_sufficient) { + bias_est.setMaxStateNoise(sqrtf(measurement_var)); + bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); + bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); + } - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); + const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) && measurement_valid; - if (isHeightResetRequired()) { - // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing; - _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); - bias_est.setBias(-_state.pos(2) + measurement); + if (_control_status.flags.ev_hgt) { + aid_src.fusion_enabled = true; - // reset vertical velocity - if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { + if (continuing_conditions_passing) { - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + if (ev_reset) { - switch (ev_sample.vel_frame) { - case VelocityFrame::LOCAL_FRAME_NED: - case VelocityFrame::LOCAL_FRAME_FRD: { - const Vector3f reset_vel = ev_sample.vel - vel_offset_earth; - resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise))); - } - break; - - case VelocityFrame::BODY_FRAME_FRD: { - const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body); - const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); - resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise))); - } - break; - } - - } else { - resetVerticalVelocityToZero(); - } - - aid_src.time_last_fuse = _imu_sample_delayed.time_us; - - } else if ((_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) && !aid_src.fused) { - // fusion failed and EV sample indicates reset - ECL_INFO("%s height reset", HGT_SRC_NAME); + if (quality_sufficient) { + ECL_INFO("reset to %s", AID_SRC_NAME); if (_height_sensor_ref == HeightSensor::EV) { _information_events.flags.reset_hgt_to_ev = true; @@ -138,43 +123,91 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) aid_src.time_last_fuse = _imu_sample_delayed.time_us; - } else if (is_fusion_failing) { - // Some other height source is still working - ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); + } else { + // EV has reset, but quality isn't sufficient + // we have no choice but to stop EV and try to resume once quality is acceptable stopEvHgtFusion(); + return; } + } else if (quality_sufficient) { + fuseVerticalPosition(aid_src); + } else { - ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); + aid_src.innovation_rejected = true; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); + + if (isHeightResetRequired() && quality_sufficient) { + // All height sources are failing + ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); + _information_events.flags.reset_hgt_to_ev = true; + resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); + bias_est.setBias(-_state.pos(2) + measurement); + + // reset vertical velocity + if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { + + // correct velocity for offset relative to IMU + const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + + switch (ev_sample.vel_frame) { + case VelocityFrame::LOCAL_FRAME_NED: + case VelocityFrame::LOCAL_FRAME_FRD: { + const Vector3f reset_vel = ev_sample.vel - vel_offset_earth; + resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise))); + } + break; + + case VelocityFrame::BODY_FRAME_FRD: { + const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body); + const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); + resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise))); + } + break; + } + + } else { + resetVerticalVelocityToZero(); + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + } else if (is_fusion_failing) { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); stopEvHgtFusion(); } } else { - if (starting_conditions_passing) { - if (_params.height_sensor_ref == HeightSensor::EV) { - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); - _height_sensor_ref = HeightSensor::EV; - - _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); - bias_est.reset(); - - } else { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(-_state.pos(2) + measurement); - } - - aid_src.time_last_fuse = _imu_sample_delayed.time_us; - bias_est.setFusionActive(); - _control_status.flags.ev_hgt = true; - } + // Stop fusion but do not declare it faulty + ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); + stopEvHgtFusion(); } - } else if (_control_status.flags.ev_hgt - && !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) { - // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); - stopEvHgtFusion(); + } else { + if (starting_conditions_passing) { + // activate fusion, only reset if necessary + if (_params.height_sensor_ref == HeightSensor::EV) { + ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + _information_events.flags.reset_hgt_to_ev = true; + resetVerticalPositionTo(measurement, measurement_var); + + _height_sensor_ref = HeightSensor::EV; + bias_est.reset(); + + } else { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + bias_est.setBias(-_state.pos(2) + measurement); + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + bias_est.setFusionActive(); + _control_status.flags.ev_hgt = true; + } } } diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 979e951512..9638b3228b 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -38,8 +38,8 @@ #include "ekf.h" -void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_conditions_passing, bool ev_reset, - bool quality_sufficient, estimator_aid_source3d_s &aid_src) +void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, + const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV velocity"; @@ -134,8 +134,8 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, bool starting_con continuing_conditions_passing = false; } - starting_conditions_passing &= continuing_conditions_passing - && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); + const bool starting_conditions_passing = common_starting_conditions_passing && continuing_conditions_passing + && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { aid_src.fusion_enabled = true; diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 1ad9d372e6..82dc3174f6 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -46,7 +46,6 @@ void Ekf::controlHeightFusion() controlBaroHeightFusion(); controlGnssHeightFusion(_gps_sample_delayed); controlRangeHeightFusion(); - controlEvHeightFusion(_ev_sample_delayed); checkHeightSensorRefFallback(); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 28010d8554..bb794cb9f1 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -125,6 +125,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_k_gate(_params->range_kin_consistency_gate), _param_ekf2_ev_ctrl(_params->ev_ctrl), _param_ekf2_ev_qmin(_params->ev_quality_minimum), + _param_ekf2_evp_noise(_params->ev_pos_noise), _param_ekf2_evv_noise(_params->ev_vel_noise), _param_ekf2_eva_noise(_params->ev_att_noise), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 227fa9ebe4..fdfe92f12e 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -525,7 +525,7 @@ private: (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamExtInt) _param_ekf2_ev_qmin, - (ParamFloat) + (ParamExtFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) (ParamExtFloat) _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 3215dbb5b0..8f4ff8ae25 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -77,7 +77,7 @@ void EkfWrapper::setExternalVisionHeightRef() void EkfWrapper::enableExternalVisionHeightFusion() { - setExternalVisionHeightRef(); // TODO: replace by EV control parameter + _ekf_params->ev_ctrl |= static_cast(EvCtrl::VPOS); } bool EkfWrapper::isIntendingExternalVisionHeightFusion() const