From 5239993c882f4803deaa8ed251cc623dbae57a6b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Oct 2022 11:30:19 -0400 Subject: [PATCH] ekf2: move EV vel to new state machine, introduce EKF2_EV_CTRL param --- .../init.d-posix/airframes/1013_iris_vision | 1 + src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/common.h | 21 +- src/modules/ekf2/EKF/control.cpp | 64 ++--- src/modules/ekf2/EKF/ekf.h | 9 +- src/modules/ekf2/EKF/ekf_helper.cpp | 76 +----- src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- src/modules/ekf2/EKF/ev_height_control.cpp | 24 +- src/modules/ekf2/EKF/ev_vel_control.cpp | 231 ++++++++++++++++++ src/modules/ekf2/EKF2.cpp | 90 ++++--- src/modules/ekf2/EKF2.hpp | 5 +- src/modules/ekf2/ekf2_params.c | 31 ++- .../test/sensor_simulator/ekf_wrapper.cpp | 4 +- .../ekf2/test/sensor_simulator/vio.cpp | 16 +- src/modules/ekf2/test/sensor_simulator/vio.h | 4 +- src/modules/ekf2/test/test_EKF_airspeed.cpp | 1 + .../ekf2/test/test_EKF_externalVision.cpp | 9 +- 18 files changed, 406 insertions(+), 184 deletions(-) create mode 100644 src/modules/ekf2/EKF/ev_vel_control.cpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision index b2f1078cd1..0420919e7a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_iris_vision @@ -10,6 +10,7 @@ # EKF2: Vision position and heading param set-default EKF2_AID_MASK 24 param set-default EKF2_EV_DELAY 5 +param set-default EKF2_EV_CTRL 15 param set-default EKF2_GPS_CTRL 0 # LPE: Vision + baro diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 9da2f0c3ba..7d040abc33 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -92,6 +92,7 @@ px4_add_module( EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp EKF/ev_height_control.cpp + EKF/ev_vel_control.cpp EKF/fake_height_control.cpp EKF/fake_pos_control.cpp EKF/gnss_height_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 5c1814c6da..ad14129159 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(ecl_EKF EKFGSF_yaw.cpp estimator_interface.cpp ev_height_control.cpp + ev_vel_control.cpp fake_height_control.cpp fake_pos_control.cpp gnss_height_control.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index f9a4a0595a..a9c329c8a7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -79,8 +79,9 @@ using math::Utilities::updateYawInRotMat; #define GNDEFFECT_TIMEOUT 10E6 ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) enum class VelocityFrame : uint8_t { - LOCAL_FRAME_FRD = 0, - BODY_FRAME_FRD = 1 + LOCAL_FRAME_NED = 0, + LOCAL_FRAME_FRD = 1, + BODY_FRAME_FRD = 2 }; enum GeoDeclinationMask : uint8_t { @@ -126,6 +127,13 @@ enum RngCtrl : uint8_t { ENABLED = 2 }; +enum class EvCtrl : uint8_t { + HPOS = (1<<0), + VPOS = (1<<1), + VEL = (1<<2), + YAW = (1<<3) +}; + enum SensorFusionMask : uint16_t { // Bit locations for fusion_mode DEPRECATED_USE_GPS = (1<<0), ///< set to true to use GPS data (DEPRECATED, use gnss_ctrl) @@ -136,7 +144,7 @@ enum SensorFusionMask : uint16_t { USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used DEPRECATED_USE_GPS_YAW = (1<<7),///< set to true to use GPS yaw data if available (DEPRECATED, use gnss_ctrl) - USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data + DEPRECATED_USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data }; struct gpsMessage { @@ -227,8 +235,8 @@ struct extVisionSample { Vector3f vel{}; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis Quatf quat{}; ///< quaternion defining rotation from body to earth frame Vector3f posVar{}; ///< XYZ position variances (m**2) - Vector3f velVar{}; ///< XYZ velocity variances ((m/sec)**2) - float angVar{}; ///< angular heading variance (rad**2) + Vector3f velocity_var{}; ///< XYZ velocity variances ((m/sec)**2) + Vector3f orientation_var{}; ///< orientation variance (rad**2) VelocityFrame vel_frame = VelocityFrame::BODY_FRAME_FRD; uint8_t reset_counter{}; int8_t quality{}; ///< quality indicator between 0 and 100 @@ -266,6 +274,7 @@ struct parameters { int32_t baro_ctrl{1}; int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL}; int32_t rng_ctrl{RngCtrl::CONDITIONAL}; + int32_t ev_ctrl{0}; int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator @@ -358,6 +367,8 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check // vision position fusion + float ev_vel_noise{0.1f}; ///< minimum allowed observation noise for EV velocity fusion (m/sec) + 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) float ev_pos_innov_gate{5.0f}; ///< vision position fusion innovation consistency gate size (STD) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0235dbdcd3..f2674aea52 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -217,7 +217,7 @@ void Ekf::controlExternalVisionFusion() // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) && ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) - || (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL)) && !_control_status.flags.ev_yaw) { + || (_params.ev_ctrl & static_cast(EvCtrl::VEL))) && !_control_status.flags.ev_yaw) { // rotate EV measurements into the EKF Navigation frame calcExtVisRotMat(); @@ -232,11 +232,6 @@ void Ekf::controlExternalVisionFusion() if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS && !_control_status.flags.ev_pos) { startEvPosFusion(); } - - // turn on use of external vision measurements for velocity - if (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_VEL && !_control_status.flags.ev_vel) { - startEvVelFusion(); - } } } @@ -331,15 +326,6 @@ void Ekf::controlExternalVisionFusion() // check if we have been deadreckoning too long if (isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)) { - // only reset velocity if we have no another source of aiding constraining it - if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) && - isTimedOut(_time_last_hor_vel_fuse, (uint64_t)1E6)) { - - if (_control_status.flags.ev_vel) { - resetVelocityToVision(); - } - } - resetHorizontalPositionToVision(); } } @@ -354,38 +340,10 @@ void Ekf::controlExternalVisionFusion() fuseHorizontalPosition(_aid_src_ev_pos); } - // determine if we should use the velocity observations - if (_control_status.flags.ev_vel) { - if (reset && _control_status_prev.flags.ev_vel) { - resetVelocityToVision(); - } - - // check if we have been deadreckoning too long - if (isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)) { - // only reset velocity if we have no another source of aiding constraining it - if (isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)1E6) && - isTimedOut(_time_last_hor_pos_fuse, (uint64_t)1E6)) { - resetVelocityToVision(); - } - } - - resetEstimatorAidStatus(_aid_src_ev_vel); - - const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f)); - - const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f); - - updateVelocityAidSrcStatus(_ev_sample_delayed.time_us, getVisionVelocityInEkfFrame(), obs_var, innov_gate, _aid_src_ev_vel); - - _aid_src_ev_vel.fusion_enabled = true; - - fuseVelocity(_aid_src_ev_vel); - } - // determine if we should use the yaw observation resetEstimatorAidStatus(_aid_src_ev_yaw); const float measured_hdg = getEulerYaw(_ev_sample_delayed.quat); - const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.angVar, 1.e-4f); + const float ev_yaw_obs_var = fmaxf(_ev_sample_delayed.orientation_var(2), 1.e-4f); if (PX4_ISFINITE(measured_hdg)) { _aid_src_ev_yaw.timestamp_sample = _ev_sample_delayed.time_us; @@ -414,12 +372,28 @@ void Ekf::controlExternalVisionFusion() } } + + bool ev_reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter); + + // 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 + && ((_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 + && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); + + // EV velocity + controlEvVelFusion(_ev_sample_delayed, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + + // 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) && !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 0ee5ebdace..788d00f5f9 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -504,6 +504,8 @@ private: uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source + uint8_t _nb_ev_vel_reset_available{0}; + Vector3f _last_known_pos{}; ///< last known local position vector (m) uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec) @@ -700,7 +702,6 @@ private: void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var); void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, float vel_var) { resetHorizontalVelocityTo(new_horz_vel, Vector2f(vel_var, vel_var)); } - void resetVelocityToVision(); void resetHorizontalVelocityToZero(); void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); @@ -784,10 +785,6 @@ private: // update the rotation matrix which transforms EV navigation frame measurements into NED void calcExtVisRotMat(); - Vector3f getVisionVelocityInEkfFrame() const; - - Vector3f getVisionVelocityVarianceInEkfFrame() const; - // matrix vector multiplication for computing K<24,1> * H<1,24> * P<24,24> // that is optimized by exploring the sparsity in H template @@ -872,6 +869,7 @@ 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); // control fusion of optical flow observations void controlOpticalFlowFusion(); @@ -1033,7 +1031,6 @@ private: void stopGpsYawFusion(); void startEvPosFusion(); - void startEvVelFusion(); void startEvYawFusion(); void stopEvFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1cbdf456ad..db39044439 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -44,13 +44,6 @@ #include #include -void Ekf::resetVelocityToVision() -{ - _information_events.flags.reset_vel_to_vision = true; - ECL_INFO("reset to vision velocity"); - resetVelocityTo(getVisionVelocityInEkfFrame(), getVisionVelocityVarianceInEkfFrame()); -} - void Ekf::resetHorizontalVelocityToZero() { _information_events.flags.reset_vel_to_zero = true; @@ -297,7 +290,7 @@ bool Ekf::resetMagHeading() bool Ekf::resetYawToEv() { const float yaw_new = getEulerYaw(_ev_sample_delayed.quat); - const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)); + const float yaw_new_variance = fmaxf(_ev_sample_delayed.orientation_var(2), sq(1.0e-2f)); resetQuatStateYaw(yaw_new, yaw_new_variance); _R_ev_to_ekf.setIdentity(); @@ -1156,56 +1149,6 @@ void Ekf::updateGroundEffect() } } -Vector3f Ekf::getVisionVelocityInEkfFrame() const -{ - Vector3f vel; - // 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; - - // rotate measurement into correct earth frame if required - switch (_ev_sample_delayed.vel_frame) { - case VelocityFrame::BODY_FRAME_FRD: - vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); - break; - - case VelocityFrame::LOCAL_FRAME_FRD: - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - vel = _R_ev_to_ekf * _ev_sample_delayed.vel - vel_offset_earth; - - } else { - vel = _ev_sample_delayed.vel - vel_offset_earth; - } - - break; - } - - return vel; -} - -Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const -{ - Matrix3f ev_vel_cov = matrix::diag(_ev_sample_delayed.velVar); - - // rotate measurement into correct earth frame if required - switch (_ev_sample_delayed.vel_frame) { - case VelocityFrame::BODY_FRAME_FRD: - ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose(); - break; - - case VelocityFrame::LOCAL_FRAME_FRD: - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose(); - } - - break; - } - - return ev_vel_cov.diag(); -} - // update the rotation matrix which rotates EV measurements into the EKF's navigation frame void Ekf::calcExtVisRotMat() { @@ -1368,14 +1311,6 @@ void Ekf::startEvPosFusion() ECL_INFO("starting vision pos fusion"); } -void Ekf::startEvVelFusion() -{ - _control_status.flags.ev_vel = true; - resetVelocityToVision(); - _information_events.flags.starting_vision_vel_fusion = true; - ECL_INFO("starting vision vel fusion"); -} - void Ekf::startEvYawFusion() { // turn on fusion of external vision yaw measurements and disable all magnetometer fusion @@ -1405,15 +1340,6 @@ void Ekf::stopEvPosFusion() } } -void Ekf::stopEvVelFusion() -{ - if (_control_status.flags.ev_vel) { - ECL_INFO("stopping EV vel fusion"); - _control_status.flags.ev_vel = false; - resetEstimatorAidStatus(_aid_src_ev_vel); - } -} - void Ekf::stopEvYawFusion() { if (_control_status.flags.ev_yaw) { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 42b2380859..81f229c1af 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -496,7 +496,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 | SensorFusionMask::USE_EXT_VIS_VEL)) { + if ((_params.fusion_mode & (SensorFusionMask::USE_EXT_VIS_POS | SensorFusionMask::USE_EXT_VIS_YAW)) || (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { 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 d360e99abd..01b08e8f89 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -94,8 +94,28 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample) bias_est.setBias(-_state.pos(2) + measurement); // reset vertical velocity - if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) { - resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2)); + if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { + + // 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; + + 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(); diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp new file mode 100644 index 0000000000..979e951512 --- /dev/null +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ev_vel_control.cpp + * Control functions for ekf external vision velocity fusion + */ + +#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) +{ + static constexpr const char *AID_SRC_NAME = "EV velocity"; + + const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) + || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + + // determine if we should use EV velocity aiding + bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VEL)) + && _control_status.flags.tilt_align + && ev_sample.vel.isAllFinite(); + + // 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; + + // rotate measurement into correct earth frame if required + Vector3f vel{NAN, NAN, NAN}; + Matrix3f vel_cov{}; + + switch (ev_sample.vel_frame) { + case VelocityFrame::LOCAL_FRAME_NED: + if (_control_status.flags.yaw_align) { + vel = ev_sample.vel - vel_offset_earth; + vel_cov = matrix::diag(ev_sample.velocity_var); + + } else { + continuing_conditions_passing = false; + } + + break; + + case VelocityFrame::LOCAL_FRAME_FRD: + if (_control_status.flags.ev_yaw) { + // using EV frame + vel = ev_sample.vel - vel_offset_earth; + vel_cov = matrix::diag(ev_sample.velocity_var); + + } else { + // rotate EV to the EKF reference frame + const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + const Dcmf R_ev_to_ekf = Dcmf(q_error); + + vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; + vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose(); + + // increase minimum variance to include EV orientation variance + // TODO: do this properly + const float orientation_var_max = ev_sample.orientation_var.max(); + + for (int i = 0; i < 2; i++) { + vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max); + } + } + + break; + + case VelocityFrame::BODY_FRAME_FRD: + vel = _R_to_earth * (ev_sample.vel - vel_offset_body); + vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); + break; + + default: + continuing_conditions_passing = false; + break; + } + + // increase minimum variance if GPS active (position reference) + if (_control_status.flags.gps) { + for (int i = 0; i < 2; i++) { + vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); + } + } + + const Vector3f measurement{vel}; + + const Vector3f measurement_var{ + math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)), + math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)), + math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f)) + }; + + const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); + + updateVelocityAidSrcStatus(ev_sample.time_us, + measurement, // observation + measurement_var, // observation variance + math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate + aid_src); + + if (!measurement_valid) { + continuing_conditions_passing = false; + } + + 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; + + if (continuing_conditions_passing) { + + if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) { + + if (quality_sufficient) { + ECL_INFO("reset to %s", AID_SRC_NAME); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityTo(measurement, measurement_var); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + } 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 + stopEvVelFusion(); + return; + } + + } else if (quality_sufficient) { + fuseVelocity(aid_src); + + } else { + aid_src.innovation_rejected = true; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second + + if (is_fusion_failing) { + + if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) { + // Data seems good, attempt a reset + _information_events.flags.reset_vel_to_vision = true; + ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); + resetVelocityTo(measurement, measurement_var); + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + if (_control_status.flags.in_air) { + _nb_ev_vel_reset_available--; + } + + } else if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.ev_vel_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + stopEvVelFusion(); + + } else { + // 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); + stopEvVelFusion(); + } + } + + } else { + // Stop fusion but do not declare it faulty + ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); + stopEvVelFusion(); + } + + } else { + if (starting_conditions_passing) { + // activate fusion, only reset if necessary + if (!isHorizontalAidingActive() || yaw_alignment_changed) { + ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityTo(measurement, measurement_var); + + } else { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + } + + aid_src.time_last_fuse = _imu_sample_delayed.time_us; + + _nb_ev_vel_reset_available = 5; + _information_events.flags.starting_vision_vel_fusion = true; + _control_status.flags.ev_vel = true; + } + } +} + +void Ekf::stopEvVelFusion() +{ + if (_control_status.flags.ev_vel) { + resetEstimatorAidStatus(_aid_src_ev_vel); + + _control_status.flags.ev_vel = false; + } +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index deeec9fc9b..cf1aacd056 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -123,7 +123,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), _param_ekf2_rng_qlty_t(_params->range_valid_quality_s), _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_evv_noise(_params->ev_vel_noise), + _param_ekf2_eva_noise(_params->ev_att_noise), _param_ekf2_evv_gate(_params->ev_vel_innov_gate), _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), @@ -706,6 +709,23 @@ void EKF2::VerifyParams() events::send(events::ID("ekf2_hgt_ref_gps"), events::Log::Warning, "GPS enabled by EKF2_HGT_REF", _param_ekf2_gps_ctrl.get()); } + + // EV EKF2_AID_MASK -> EKF2_EV_CTRL + if ((_param_ekf2_aid_mask.get() & SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)) { + + _param_ekf2_ev_ctrl.set(_param_ekf2_ev_ctrl.get() | static_cast(EvCtrl::VEL)); + _param_ekf2_ev_ctrl.commit(); + + _param_ekf2_aid_mask.set(_param_ekf2_aid_mask.get() & ~(SensorFusionMask::DEPRECATED_USE_EXT_VIS_VEL)); + _param_ekf2_aid_mask.commit(); + + mavlink_log_critical(&_mavlink_log_pub, "EKF2 EV use EKF2_EV_CTRL instead of EKF2_AID_MASK\n"); + /* EVENT + * @description EKF2_AID_MASK is set to {1:.0}. + */ + events::send(events::ID("ekf2_aid_mask_ev"), events::Log::Warning, + "Use EKF2_EV_CTRL instead", _param_ekf2_aid_mask.get()); + } } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -1737,45 +1757,44 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo ev_data.vel.setNaN(); ev_data.quat.setNaN(); - // if error estimates are unavailable, use parameter defined defaults - // check for valid velocity data - if (Vector3f(ev_odom.velocity).isAllFinite()) { - bool velocity_valid = true; + const Vector3f ev_odom_vel(ev_odom.velocity); + const Vector3f ev_odom_vel_var(ev_odom.velocity_variance); + + if (ev_odom_vel.isAllFinite()) { + bool velocity_frame_valid = false; switch (ev_odom.velocity_frame) { - // case vehicle_odometry_s::VELOCITY_FRAME_NED: - // ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; - // break; + case vehicle_odometry_s::VELOCITY_FRAME_NED: + ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; + velocity_frame_valid = true; + break; case vehicle_odometry_s::VELOCITY_FRAME_FRD: ev_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; + velocity_frame_valid = true; break; case vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD: ev_data.vel_frame = VelocityFrame::BODY_FRAME_FRD; - break; - - default: - velocity_valid = false; + velocity_frame_valid = true; break; } - if (velocity_valid) { - ev_data.vel(0) = ev_odom.velocity[0]; - ev_data.vel(1) = ev_odom.velocity[1]; - ev_data.vel(2) = ev_odom.velocity[2]; + if (velocity_frame_valid) { + ev_data.vel = ev_odom_vel; const float evv_noise_var = sq(_param_ekf2_evv_noise.get()); // velocity measurement error from ev_data or parameters - if (!_param_ekf2_ev_noise_md.get() && Vector3f(ev_odom.velocity_variance).isAllFinite()) { - ev_data.velVar(0) = fmaxf(evv_noise_var, ev_odom.velocity_variance[0]); - ev_data.velVar(1) = fmaxf(evv_noise_var, ev_odom.velocity_variance[1]); - ev_data.velVar(2) = fmaxf(evv_noise_var, ev_odom.velocity_variance[2]); + if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_vel_var.isAllFinite()) { + + ev_data.velocity_var(0) = fmaxf(evv_noise_var, ev_odom_vel_var(0)); + ev_data.velocity_var(1) = fmaxf(evv_noise_var, ev_odom_vel_var(1)); + ev_data.velocity_var(2) = fmaxf(evv_noise_var, ev_odom_vel_var(2)); } else { - ev_data.velVar.setAll(evv_noise_var); + ev_data.velocity_var.setAll(evv_noise_var); } new_ev_odom = true; @@ -1822,23 +1841,34 @@ bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odo } // check for valid orientation data - if ((Quatf(ev_odom.q).isAllFinite()) - && ((fabsf(ev_odom.q[0]) > 0.f) || (fabsf(ev_odom.q[1]) > 0.f) - || (fabsf(ev_odom.q[2]) > 0.f) || (fabsf(ev_odom.q[3]) > 0.f)) - ) { - ev_data.quat = Quatf(ev_odom.q); + const Quatf ev_odom_q(ev_odom.q); + const Vector3f ev_odom_q_var(ev_odom.orientation_variance); + const bool non_zero = (fabsf(ev_odom_q(0)) > 0.f) || (fabsf(ev_odom_q(1)) > 0.f) + || (fabsf(ev_odom_q(2)) > 0.f) || (fabsf(ev_odom_q(3)) > 0.f); + const float eps = 1e-5f; + const bool no_element_larger_than_one = (fabsf(ev_odom_q(0)) <= 1.f + eps) + && (fabsf(ev_odom_q(1)) <= 1.f + eps) + && (fabsf(ev_odom_q(2)) <= 1.f + eps) + && (fabsf(ev_odom_q(3)) <= 1.f + eps); + const bool norm_in_tolerance = fabsf(1.f - ev_odom_q.norm()) <= eps; + + const bool orientation_valid = ev_odom_q.isAllFinite() && non_zero && no_element_larger_than_one && norm_in_tolerance; + + if (orientation_valid) { + ev_data.quat = ev_odom_q; ev_data.quat.normalize(); // orientation measurement error from ev_data or parameters const float eva_noise_var = sq(_param_ekf2_eva_noise.get()); - if (!_param_ekf2_ev_noise_md.get() && - PX4_ISFINITE(ev_odom.orientation_variance[2]) - ) { - ev_data.angVar = fmaxf(eva_noise_var, ev_odom.orientation_variance[2]); + if ((_param_ekf2_ev_noise_md.get() == 0) && ev_odom_q_var.isAllFinite()) { + + ev_data.orientation_var(0) = fmaxf(eva_noise_var, ev_odom_q_var(0)); + ev_data.orientation_var(1) = fmaxf(eva_noise_var, ev_odom_q_var(1)); + ev_data.orientation_var(2) = fmaxf(eva_noise_var, ev_odom_q_var(2)); } else { - ev_data.angVar = eva_noise_var; + ev_data.orientation_var.setAll(eva_noise_var); } new_ev_odom = true; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 6aea8e4dfb..48627e79a0 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -520,14 +520,15 @@ private: _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) // vision estimate fusion + (ParamExtInt) _param_ekf2_ev_ctrl, ///< external vision (EV) control selection (ParamInt) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamExtInt) _param_ekf2_ev_qmin, (ParamFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) - (ParamFloat) + (ParamExtFloat) _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) - (ParamFloat) + (ParamExtFloat) _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) (ParamExtFloat) _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 36242b190a..e5d29b7361 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -608,7 +608,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * 5 : Set to true to enable multi-rotor drag specific force fusion * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used * 7 : Deprecated, use EKF2_GPS_CTRL instead - * 8 : Set to true to enable vision velocity fusion + * 3 : Deprecated, use EKF2_EV_CTRL instead * * @group EKF2 * @min 0 @@ -621,7 +621,7 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * @bit 5 multi-rotor drag fusion * @bit 6 rotate external vision * @bit 7 unused - * @bit 8 vision velocity fusion + * @bit 8 unused * @reboot_required true */ PARAM_DEFINE_INT32(EKF2_AID_MASK, 0); @@ -654,6 +654,25 @@ PARAM_DEFINE_INT32(EKF2_HGT_REF, 1); */ PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1); +/** + * External vision (EV) sensor aiding + * + * Set bits in the following positions to enable: + * 0 : Horizontal position fusion + * 1 : Vertical position fusion + * 2 : 3D velocity fusion + * 3 : Yaw + * + * @group EKF2 + * @min 0 + * @max 15 + * @bit 0 Horizontal position + * @bit 1 Vertical position + * @bit 2 3D velocity + * @bit 3 Yaw + */ +PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15); + /** * GNSS sensor aiding * @@ -770,11 +789,13 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); /** - * Whether to set the external vision observation noise from the parameter or from vision message + * External vision (EV) noise mode * - * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound. + * If set to 0 (default) the measurement noise is taken from the vision message and the EV noise parameters are used as a lower bound. + * If set to 1 the observation noise is set from the parameters directly, * - * @boolean + * @value 0 EV reported variance (parameter lower bound) + * @value 1 EV noise parameters * @group EKF2 */ PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 3f4cc8bffc..3215dbb5b0 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -152,12 +152,12 @@ bool EkfWrapper::isIntendingExternalVisionPositionFusion() const void EkfWrapper::enableExternalVisionVelocityFusion() { - _ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_VEL; + _ekf_params->ev_ctrl |= static_cast(EvCtrl::VEL); } void EkfWrapper::disableExternalVisionVelocityFusion() { - _ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_VEL; + _ekf_params->ev_ctrl &= ~static_cast(EvCtrl::VEL); } bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const diff --git a/src/modules/ekf2/test/sensor_simulator/vio.cpp b/src/modules/ekf2/test/sensor_simulator/vio.cpp index 42723306e0..ab26808ece 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.cpp +++ b/src/modules/ekf2/test/sensor_simulator/vio.cpp @@ -7,6 +7,7 @@ namespace sensor Vio::Vio(std::shared_ptr ekf): Sensor(ekf) { + _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; } Vio::~Vio() @@ -26,7 +27,7 @@ void Vio::setData(const extVisionSample &vio_data) void Vio::setVelocityVariance(const Vector3f &velVar) { - _vio_data.velVar = velVar; + _vio_data.velocity_var = velVar; } void Vio::setPositionVariance(const Vector3f &posVar) @@ -36,7 +37,7 @@ void Vio::setPositionVariance(const Vector3f &posVar) void Vio::setAngularVariance(float angVar) { - _vio_data.angVar = angVar; + _vio_data.orientation_var(2) = angVar; } void Vio::setVelocity(const Vector3f &vel) @@ -59,11 +60,16 @@ void Vio::setVelocityFrameToBody() _vio_data.vel_frame = VelocityFrame::BODY_FRAME_FRD; } -void Vio::setVelocityFrameToLocal() +void Vio::setVelocityFrameToLocalFRD() { _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; } +void Vio::setVelocityFrameToLocalNED() +{ + _vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED; +} + extVisionSample Vio::dataAtRest() { extVisionSample vio_data; @@ -71,8 +77,8 @@ extVisionSample Vio::dataAtRest() vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};; vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f}; vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f}; - vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f}; - vio_data.angVar = 0.05f; + vio_data.velocity_var = Vector3f{0.1f, 0.1f, 0.1f}; + vio_data.orientation_var(2) = 0.05f; vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD; return vio_data; } diff --git a/src/modules/ekf2/test/sensor_simulator/vio.h b/src/modules/ekf2/test/sensor_simulator/vio.h index 2000865e22..50b81e60c0 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.h +++ b/src/modules/ekf2/test/sensor_simulator/vio.h @@ -58,7 +58,9 @@ public: void setVelocity(const Vector3f &vel); void setPosition(const Vector3f &pos); void setOrientation(const Quatf &quat); - void setVelocityFrameToLocal(); + + void setVelocityFrameToLocalNED(); + void setVelocityFrameToLocalFRD(); void setVelocityFrameToBody(); extVisionSample dataAtRest(); diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index 7a3b2e003e..ff73948bd3 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -83,6 +83,7 @@ TEST_F(EkfAirspeedTest, testWindVelocityEstimation) const Vector2f airspeed_body(2.4f, 0.0f); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator._vio.setVelocity(simulated_velocity_earth); + _sensor_simulator._vio.setVelocityFrameToLocalNED(); _sensor_simulator.startExternalVision(); _ekf->set_in_air_status(true); diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 24744f9e84..50a52a9828 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -162,9 +162,9 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment) const Vector3f estimated_velocity_in_ekf_frame = _ekf->getVelocity(); EXPECT_TRUE(isEqual(estimated_velocity_in_ekf_frame, simulated_velocity_in_ekf_frame, 0.01f)); // And: the frame offset should be estimated correctly - Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); - EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(), - estimatedExternalVisionFrameOffset.canonical())); + // Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion(); + // EXPECT_TRUE(matrix::isEqual(vision_to_ekf.canonical(), + // estimatedExternalVisionFrameOffset.canonical())); // AND: the reset in velocity should be saved correctly reset_logging_checker.capturePostResetState(); @@ -212,7 +212,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment) EXPECT_TRUE(isEqual(estimated_position_in_ekf_frame, simulated_position_in_ekf_frame, 1e-2f)); } - TEST_F(EkfExternalVisionTest, visionVarianceCheck) { _sensor_simulator.runSeconds(_tilt_align_time); @@ -308,7 +307,7 @@ TEST_F(EkfExternalVisionTest, velocityFrameLocal) // WHEN: measurement is given in LOCAL-FRAME and // x variance is bigger than y variance - _sensor_simulator._vio.setVelocityFrameToLocal(); + _sensor_simulator._vio.setVelocityFrameToLocalNED(); const Vector3f vel_cov_earth{2.f, 0.01f, 0.01f}; const Vector3f vel_earth(1.0f, 0.0f, 0.0f);