From f3be32e02f7c504bcde3c60d334ca10c69e18f84 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 30 May 2022 09:22:40 -0400 Subject: [PATCH] ekf2: add estimator aid source status (EV pos, EV vel) --- msg/estimator_aid_source_1d.msg | 1 + msg/estimator_aid_source_3d.msg | 1 + src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/common.h | 2 +- src/modules/ekf2/EKF/control.cpp | 174 ++++++++++---------- src/modules/ekf2/EKF/covariance.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 28 ++-- src/modules/ekf2/EKF/ekf_helper.cpp | 183 ++++++++------------- src/modules/ekf2/EKF/estimator_interface.h | 2 - src/modules/ekf2/EKF/ev_fusion.cpp | 182 ++++++++++++++++++++ src/modules/ekf2/EKF/gps_fusion.cpp | 2 +- src/modules/ekf2/EKF/height_fusion.cpp | 16 -- src/modules/ekf2/EKF2.cpp | 4 + src/modules/ekf2/EKF2.hpp | 6 + 15 files changed, 373 insertions(+), 234 deletions(-) create mode 100644 src/modules/ekf2/EKF/ev_fusion.cpp diff --git a/msg/estimator_aid_source_1d.msg b/msg/estimator_aid_source_1d.msg index c0f6695fd4..51e23c985e 100644 --- a/msg/estimator_aid_source_1d.msg +++ b/msg/estimator_aid_source_1d.msg @@ -20,3 +20,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_source_1d # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt +# TOPICS estimator_aid_src_ev_yaw diff --git a/msg/estimator_aid_source_3d.msg b/msg/estimator_aid_source_3d.msg index b2d158487d..04dacfed12 100644 --- a/msg/estimator_aid_source_3d.msg +++ b/msg/estimator_aid_source_3d.msg @@ -20,3 +20,4 @@ bool[3] fused # true if the sample was successfully fused # TOPICS estimator_aid_source_3d # TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel +# TOPICS estimator_aid_src_ev_pos estimator_aid_src_ev_vel diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 7fdf48dfaf..4df6761b6b 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -56,6 +56,7 @@ px4_add_module( EKF/ekf_helper.cpp EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp + EKF/ev_fusion.cpp EKF/fake_pos_control.cpp EKF/gps_checks.cpp EKF/gps_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 3fce773cb0..45e05c3241 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -41,6 +41,7 @@ add_library(ecl_EKF ekf_helper.cpp EKFGSF_yaw.cpp estimator_interface.cpp + ev_fusion.cpp fake_pos_control.cpp gps_checks.cpp gps_control.cpp diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 521f108fe6..4174d7083c 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index aa06c0d354..ad9a657060 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015-2022 Estimation and Control Library (ECL). All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -162,6 +162,63 @@ void Ekf::controlFusionModes() if (_ext_vision_buffer) { _ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); + + if (_ev_data_ready) { + + // Use an incremental position fusion method for EV position data if GPS is also used + _fuse_hpos_as_odom = (_params.fusion_mode & SensorFusionMask::USE_GPS); + + // 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::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw) { + + // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon. + const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized()); + _R_ev_to_ekf = Dcmf(q_error); + + } else { + _R_ev_to_ekf.setIdentity(); + } + + // correct position and height 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 Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + + if ((_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) || _fuse_hpos_as_odom) { + // rotate measurement into body frame is required when fusing with GPS + _ev_sample_delayed.pos = _R_ev_to_ekf * (_ev_sample_delayed.pos - pos_offset_earth); + + Matrix3f obs_var = matrix::diag(_ev_sample_delayed.posVar); + obs_var = _R_ev_to_ekf * obs_var * _R_ev_to_ekf.transpose(); + _ev_sample_delayed.posVar = obs_var.diag(); + + } else { + _ev_sample_delayed.pos = (_ev_sample_delayed.pos - pos_offset_earth); + } + + // rotate measurement into correct earth frame if required + switch (_ev_sample_delayed.vel_frame) { + case VelocityFrame::BODY_FRAME_FRD: + _ev_sample_delayed.vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); + _ev_sample_delayed.velCov = _R_to_earth * _ev_sample_delayed.velCov * _R_to_earth.transpose(); + 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) { + _ev_sample_delayed.vel = _R_ev_to_ekf * (_ev_sample_delayed.vel - vel_offset_earth); + _ev_sample_delayed.velCov = _R_ev_to_ekf * _ev_sample_delayed.velCov * _R_ev_to_ekf.transpose(); + + } else { + _ev_sample_delayed.vel = _ev_sample_delayed.vel - vel_offset_earth; + // _ev_sample_delayed.velCov no change + } + + break; + } + } } if (_airspeed_buffer) { @@ -200,9 +257,21 @@ void Ekf::controlFusionModes() void Ekf::controlExternalVisionFusion() { + if (!_control_status_prev.flags.gps && _control_status.flags.gps) { + // GPS now enabled + stopEvYawFusion(); + + _fuse_hpos_as_odom = true; + _hpos_prev_available = false; + } + // Check for new external vision data if (_ev_data_ready) { + // reset flags + resetEstimatorAidStatusFlags(_aid_src_ev_vel); + resetEstimatorAidStatusFlags(_aid_src_ev_pos); + bool reset = false; if (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) { @@ -213,15 +282,6 @@ void Ekf::controlExternalVisionFusion() stopEvYawFusion(); } - // 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) { - - // rotate EV measurements into the EKF Navigation frame - calcExtVisRotMat(); - } - // external vision aiding selection logic if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) { @@ -241,8 +301,10 @@ void Ekf::controlExternalVisionFusion() // external vision yaw aiding selection logic if (!_inhibit_ev_yaw_use && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_YAW) && !_control_status.flags.ev_yaw - && _control_status.flags.tilt_align) { - + && _control_status.flags.tilt_align + && !_control_status.flags.ev_yaw + && !_control_status.flags.gps + ) { // don't start using EV data unless data is arriving frequently if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { if (resetYawToEv()) { @@ -260,57 +322,7 @@ void Ekf::controlExternalVisionFusion() } } - Vector3f ev_pos_obs_var; - - // correct position and height 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; - _ev_sample_delayed.pos -= pos_offset_earth; - - // Use an incremental position fusion method for EV position data if GPS is also used - if (_params.fusion_mode & SensorFusionMask::USE_GPS) { - _fuse_hpos_as_odom = true; - - } else { - _fuse_hpos_as_odom = false; - } - - if (_fuse_hpos_as_odom) { - if (!_hpos_prev_available) { - // no previous observation available to calculate position change - _hpos_prev_available = true; - - } else { - // calculate the change in position since the last measurement - // rotate measurement into body frame is required when fusing with GPS - Vector3f ev_delta_pos = _R_ev_to_ekf * Vector3f(_ev_sample_delayed.pos - _ev_sample_delayed_prev.pos); - - // use the change in position since the last measurement - _ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0); - _ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1); - - // observation 1-STD error, incremental pos observation is expected to have more uncertainty - Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); - ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); - ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f)); - ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f)); - } - } else { - // use the absolute position - Vector3f ev_pos_meas = _ev_sample_delayed.pos; - Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar); - - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - ev_pos_meas = _R_ev_to_ekf * ev_pos_meas; - ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose(); - } - - _ev_pos_innov(0) = _state.pos(0) - ev_pos_meas(0); - _ev_pos_innov(1) = _state.pos(1) - ev_pos_meas(1); - - ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.01f)); - ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.01f)); - + if (!_fuse_hpos_as_odom) { // 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 @@ -325,11 +337,6 @@ void Ekf::controlExternalVisionFusion() resetHorizontalPositionToVision(); } } - - // innovation gate size - const float ev_pos_innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.0f); - - fuseHorizontalPosition(_ev_pos_innov, ev_pos_innov_gate, ev_pos_obs_var, _ev_pos_innov_var, _ev_pos_test_ratio); } // determine if we should use the velocity observations @@ -338,8 +345,6 @@ void Ekf::controlExternalVisionFusion() resetVelocityToVision(); } - _ev_vel_innov = _state.vel - getVisionVelocityInEkfFrame(); - // 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 @@ -348,15 +353,20 @@ void Ekf::controlExternalVisionFusion() resetVelocityToVision(); } } - - const Vector3f obs_var = matrix::max(getVisionVelocityVarianceInEkfFrame(), sq(0.05f)); - - const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f); - - fuseHorizontalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); - fuseVerticalVelocity(_ev_vel_innov, innov_gate, obs_var, _ev_vel_innov_var, _ev_vel_test_ratio); } + updateEvVel(_ev_sample_delayed); + updateEvPos(_ev_sample_delayed); + + if (_control_status.flags.ev_vel) { + fuseEvVel(); + } + + if (_control_status.flags.ev_pos) { + fuseEvPos(); + } + + // External Vision Yaw // determine if we should use the yaw observation if (_control_status.flags.ev_yaw) { if (reset && _control_status_prev.flags.ev_yaw) { @@ -375,7 +385,8 @@ void Ekf::controlExternalVisionFusion() // record observation and estimate for use next time _ev_sample_delayed_prev = _ev_sample_delayed; - _hpos_pred_prev = _state.pos.xy(); + _hpos_pred_prev = _state.pos; + _hpos_prev_available = true; } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw) && isTimedOut(_time_last_ext_vision, (uint64_t)_params.reset_timeout_max)) { @@ -966,13 +977,6 @@ void Ekf::controlHeightFusion() fuseRngHgt(_aid_src_rng_hgt); } } - - if (_control_status.flags.ev_hgt) { - - if (_control_status.flags.ev_hgt && _ev_data_ready) { - fuseEvHgt(); - } - } } void Ekf::checkRangeAidSuitability() diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index a4631004a4..02a57c945e 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -988,13 +988,13 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) if (fabsf(down_dvel_bias) > dVel_bias_lim) { bool bad_vz_gps = _control_status.flags.gps && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f); - bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _ev_vel_innov(2) < 0.0f); + bool bad_vz_ev = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f); if (bad_vz_gps || bad_vz_ev) { bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f); bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_pos.innovation[2] < 0.0f); bool bad_z_rng = _control_status.flags.rng_hgt && (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.0f); - bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _ev_pos_innov(2) < 0.0f); + bool bad_z_ev = _control_status.flags.ev_hgt && (down_dvel_bias * _aid_src_ev_pos.innovation[2] < 0.0f); if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ed8cbf0430..5661ad409c 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -348,6 +348,9 @@ public: const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; } const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; } + const auto &aid_src_ev_vel() const { return _aid_src_gnss_vel; } + const auto &aid_src_ev_pos() const { return _aid_src_gnss_pos; } + private: // set the internal states and status to their default value @@ -381,7 +384,7 @@ private: // variables used when position data is being fused using a relative position odometry model bool _fuse_hpos_as_odom{false}; ///< true when the NE position data is being fused using an odometry assumption - Vector2f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m) + Vector3f _hpos_pred_prev{}; ///< previous value of NE position state used by odometry fusion (m) bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) @@ -450,12 +453,6 @@ private: float _vert_vel_innov_ratio{0.f}; ///< standard deviation of vertical velocity innovation uint64_t _vert_vel_fuse_time_us{0}; ///< last system time in usec time vertical velocity measurement fuson was attempted - Vector3f _ev_vel_innov{}; ///< external vision velocity innovations (m/sec) - Vector3f _ev_vel_innov_var{}; ///< external vision velocity innovation variances ((m/sec)**2) - - Vector3f _ev_pos_innov{}; ///< external vision position innovations (m) - Vector3f _ev_pos_innov_var{}; ///< external vision position innovation variances (m**2) - Vector3f _aux_vel_innov{}; ///< horizontal auxiliary velocity innovations: (m/sec) Vector3f _aux_vel_innov_var{}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2) @@ -499,6 +496,9 @@ private: estimator_aid_source_3d_s _aid_src_gnss_vel{}; estimator_aid_source_3d_s _aid_src_gnss_pos{}; + estimator_aid_source_3d_s _aid_src_ev_vel{}; + estimator_aid_source_3d_s _aid_src_ev_pos{}; + // output predictor states Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m) @@ -647,7 +647,6 @@ private: void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt); void fuseRngHgt(estimator_aid_source_1d_s &range_hgt); - void fuseEvHgt(); void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt); void updateRngHgt(estimator_aid_source_1d_s &rng_hgt); @@ -701,6 +700,12 @@ private: void updateGpsPos(const gpsSample &gps_sample); void fuseGpsPos(); + void updateEvVel(const extVisionSample &ev_sample); + void fuseEvVel(); + + void updateEvPos(const extVisionSample &ev_sample); + void fuseEvPos(); + // calculate optical flow body angular rate compensation // returns false if bias corrected body rate data is unavailable bool calcOptFlowBodyRateComp(); @@ -748,13 +753,6 @@ private: // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(); - // 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 diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index fc89d182b3..85cbe8f00f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -87,8 +87,8 @@ void Ekf::resetVelocityToVision() { _information_events.flags.reset_vel_to_vision = true; ECL_INFO("reset to vision velocity"); - resetVelocityTo(getVisionVelocityInEkfFrame()); - P.uncorrelateCovarianceSetVariance<3>(4, getVisionVelocityVarianceInEkfFrame()); + resetVelocityTo(_ev_sample_delayed.vel); + P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velCov.diag()); } void Ekf::resetHorizontalVelocityToZero() @@ -156,13 +156,8 @@ void Ekf::resetHorizontalPositionToVision() { _information_events.flags.reset_pos_to_vision = true; ECL_INFO("reset position to ev position"); - Vector3f _ev_pos = _ev_sample_delayed.pos; - if (_params.fusion_mode & SensorFusionMask::ROTATE_EXT_VIS) { - _ev_pos = _R_ev_to_ekf * _ev_sample_delayed.pos; - } - - resetHorizontalPositionTo(Vector2f(_ev_pos)); + resetHorizontalPositionTo(Vector2f(_ev_sample_delayed.pos)); P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0)); // let the next odometry update know that the previous value of states cannot be used to calculate the change in position @@ -637,30 +632,33 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const { - hvel[0] = _ev_vel_innov(0); - hvel[1] = _ev_vel_innov(1); - vvel = _ev_vel_innov(2); - hpos[0] = _ev_pos_innov(0); - hpos[1] = _ev_pos_innov(1); - vpos = _ev_pos_innov(2); + hvel[0] = _aid_src_ev_vel.innovation[0]; + hvel[1] = _aid_src_ev_vel.innovation[1]; + vvel = _aid_src_ev_vel.innovation[2]; + + hpos[0] = _aid_src_ev_pos.innovation[0]; + hpos[1] = _aid_src_ev_pos.innovation[1]; + vpos = _aid_src_ev_pos.innovation[2]; } void Ekf::getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const { - hvel[0] = _ev_vel_innov_var(0); - hvel[1] = _ev_vel_innov_var(1); - vvel = _ev_vel_innov_var(2); - hpos[0] = _ev_pos_innov_var(0); - hpos[1] = _ev_pos_innov_var(1); - vpos = _ev_pos_innov_var(2); + hvel[0] = _aid_src_ev_vel.innovation_variance[0]; + hvel[1] = _aid_src_ev_vel.innovation_variance[1]; + vvel = _aid_src_ev_vel.innovation_variance[2]; + + hpos[0] = _aid_src_ev_pos.innovation_variance[0]; + hpos[1] = _aid_src_ev_pos.innovation_variance[1]; + vpos = _aid_src_ev_pos.innovation_variance[2]; } void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const { - hvel = _ev_vel_test_ratio(0); - vvel = _ev_vel_test_ratio(1); - hpos = _ev_pos_test_ratio(0); - vpos = _ev_pos_test_ratio(1); + hvel = fmaxf(_aid_src_ev_vel.test_ratio[0], _aid_src_ev_vel.test_ratio[1]); + vvel = _aid_src_ev_vel.test_ratio[2]; + + hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]); + vpos = _aid_src_ev_pos.test_ratio[2]; } void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const @@ -748,7 +746,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const } if (_control_status.flags.ev_pos) { - hpos_err = math::max(hpos_err, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); + hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); } } @@ -793,11 +791,11 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } else if (_control_status.flags.ev_pos) { - vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_pos_innov(0)) + sq(_ev_pos_innov(1)))); + vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm()); } if (_control_status.flags.ev_vel) { - vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_ev_vel_innov(0)) + sq(_ev_vel_innov(1)))); + vel_err_conservative = math::max(vel_err_conservative, Vector3f(_aid_src_ev_vel.innovation).norm()); } hvel_err = math::max(hvel_err, vel_err_conservative); @@ -925,12 +923,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f } if (_control_status.flags.ev_vel) { - float ev_vel = sqrtf(math::max(_ev_vel_test_ratio(0), _ev_vel_test_ratio(1))); + float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max()); vel = math::max(vel, ev_vel, FLT_MIN); } if (_control_status.flags.ev_pos) { - float ev_pos = sqrtf(_ev_pos_test_ratio(0)); + float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max()); pos = math::max(pos, ev_pos, FLT_MIN); } @@ -950,7 +948,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f hgt = math::max(sqrtf(_aid_src_rng_hgt.test_ratio), FLT_MIN); } else if (_control_status.flags.ev_hgt) { - hgt = math::max(sqrtf(_ev_pos_test_ratio(1)), FLT_MIN); + hgt = math::max(sqrtf(_aid_src_ev_pos.test_ratio[2]), FLT_MIN); } else { hgt = NAN; @@ -1415,64 +1413,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 = _ev_sample_delayed.velCov; - - // 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() -{ - // Calculate the quaternion delta that rotates from the EV to the EKF reference frame at the EKF fusion time horizon. - const Quatf q_error((_state.quat_nominal * _ev_sample_delayed.quat.inversed()).normalized()); - _R_ev_to_ekf = Dcmf(q_error); -} - // Increase the yaw error variance of the quaternions // Argument is additional yaw variance in rad**2 void Ekf::increaseQuatYawErrVariance(float yaw_variance) @@ -1638,31 +1578,45 @@ void Ekf::stopGpsYawFusion() void Ekf::startEvPosFusion() { - _control_status.flags.ev_pos = true; - resetHorizontalPositionToVision(); - _information_events.flags.starting_vision_pos_fusion = true; - ECL_INFO("starting vision pos fusion"); + if (!_control_status.flags.ev_pos) { + ECL_INFO("starting vision pos fusion"); + _information_events.flags.starting_vision_pos_fusion = true; + + if (!_control_status.flags.gps) { + resetHorizontalPositionToVision(); + } + + _control_status.flags.ev_pos = true; + } } void Ekf::startEvVelFusion() { - _control_status.flags.ev_vel = true; - resetVelocityToVision(); - _information_events.flags.starting_vision_vel_fusion = true; - ECL_INFO("starting vision vel fusion"); + if (!_control_status.flags.ev_vel) { + ECL_INFO("starting vision vel fusion"); + _information_events.flags.starting_vision_vel_fusion = true; + + if (!_control_status.flags.gps) { + resetVelocityToVision(); + } + + _control_status.flags.ev_vel = true; + } } void Ekf::startEvYawFusion() { - // turn on fusion of external vision yaw measurements and disable all magnetometer fusion - _control_status.flags.ev_yaw = true; - _control_status.flags.mag_dec = false; + if (!_control_status.flags.ev_yaw) { + // turn on fusion of external vision yaw measurements and disable all magnetometer fusion + ECL_INFO("starting vision yaw fusion"); + _information_events.flags.starting_vision_yaw_fusion = true; - stopMagHdgFusion(); - stopMag3DFusion(); + _control_status.flags.ev_yaw = true; + _control_status.flags.mag_dec = false; - _information_events.flags.starting_vision_yaw_fusion = true; - ECL_INFO("starting vision yaw fusion"); + stopMagHdgFusion(); + stopMag3DFusion(); + } } void Ekf::stopEvFusion() @@ -1674,23 +1628,28 @@ void Ekf::stopEvFusion() void Ekf::stopEvPosFusion() { - _control_status.flags.ev_pos = false; - _ev_pos_innov.setZero(); - _ev_pos_innov_var.setZero(); - _ev_pos_test_ratio.setZero(); + if (_control_status.flags.ev_pos) { + ECL_INFO("stopping EV position fusion"); + resetEstimatorAidStatus(_aid_src_ev_pos); + _control_status.flags.ev_pos = false; + } } void Ekf::stopEvVelFusion() { - _control_status.flags.ev_vel = false; - _ev_vel_innov.setZero(); - _ev_vel_innov_var.setZero(); - _ev_vel_test_ratio.setZero(); + if (_control_status.flags.ev_vel) { + ECL_INFO("stopping EV velocity fusion"); + resetEstimatorAidStatus(_aid_src_ev_vel); + _control_status.flags.ev_vel = false; + } } void Ekf::stopEvYawFusion() { - _control_status.flags.ev_yaw = false; + if (_control_status.flags.ev_yaw) { + ECL_INFO("stopping EV yaw fusion"); + _control_status.flags.ev_yaw = false; + } } void Ekf::stopAuxVelFusion() diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7144ee60fe..841c4b6db7 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -331,8 +331,6 @@ protected: float _yaw_test_ratio{}; // yaw innovation consistency check ratio AlphaFilter _yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state Vector3f _mag_test_ratio{}; // magnetometer XYZ innovation consistency check ratios - Vector2f _ev_vel_test_ratio{}; // EV velocity innovation consistency check ratios - Vector2f _ev_pos_test_ratio{}; // EV position innovation consistency check ratios Vector2f _aux_vel_test_ratio{}; // Auxiliary horizontal velocity innovation consistency check ratio float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio float _tas_test_ratio{}; // tas innovation consistency check ratio diff --git a/src/modules/ekf2/EKF/ev_fusion.cpp b/src/modules/ekf2/EKF/ev_fusion.cpp new file mode 100644 index 0000000000..6301e92164 --- /dev/null +++ b/src/modules/ekf2/EKF/ev_fusion.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4. 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_fusion.cpp + * Function for fusing external vision (EV) velocity and position measurements + */ + +#include "ekf.h" + +void Ekf::updateEvVel(const extVisionSample &ev_sample) +{ + // innovation gate size + const float innov_gate = fmaxf(_params.ev_vel_innov_gate, 1.f); + + auto &ev_vel = _aid_src_ev_vel; + + for (int i = 0; i < 3; i++) { + ev_vel.observation[i] = ev_sample.vel(i); + ev_vel.observation_variance[i] = fmaxf(ev_sample.velCov(i, i), sq(0.05f)); + + ev_vel.innovation[i] = _state.vel(i) - ev_sample.vel(i); + ev_vel.innovation_variance[i] = P(4 + i, 4 + i) + ev_vel.observation_variance[i]; + ev_vel.test_ratio[i] = sq(ev_vel.innovation[i]) / (sq(innov_gate) * ev_vel.innovation_variance[i]); + + ev_vel.innovation_rejected[i] = (ev_vel.test_ratio[i] > 1.f); + } + + // vz special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && ev_vel.innovation_rejected[2]) { + const float innov_limit = innov_gate * sqrtf(ev_vel.innovation_variance[2]); + ev_vel.innovation[2] = math::constrain(ev_vel.innovation[2], -innov_limit, innov_limit); + ev_vel.innovation_rejected[2] = false; + } + + ev_vel.timestamp_sample = ev_sample.time_us; +} + +void Ekf::updateEvPos(const extVisionSample &ev_sample) +{ + bool updated = false; + + // innovation gate size + float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f); + + auto &ev_pos = _aid_src_ev_pos; + + if (!_fuse_hpos_as_odom) { + // use the absolute position + for (int i = 0; i < 3; i++) { + ev_pos.observation[i] = _ev_sample_delayed.pos(i); + ev_pos.observation_variance[i] = fmaxf(_ev_sample_delayed.posVar(i), sq(0.01f)); + + ev_pos.innovation[i] = _state.pos(i) - _ev_sample_delayed.pos(i); + } + + updated = true; + + } else if (_hpos_prev_available) { + // calculate the change in position since the last measurement + Vector3f delta_pos = _ev_sample_delayed.pos - _ev_sample_delayed_prev.pos; + + for (int i = 0; i < 3; i++) { + ev_pos.observation[i] = delta_pos(i); + + // observation 1-STD error, incremental pos observation is expected to have more uncertainty + ev_pos.observation_variance[i] = fmaxf(_ev_sample_delayed.posVar(i), sq(0.5f)); + + ev_pos.innovation[i] = _state.pos(i) - _hpos_pred_prev(i) - delta_pos(i); + } + + updated = true; + } + + if (updated) { + + for (int i = 0; i < 3; i++) { + ev_pos.innovation_variance[i] = P(7 + i, 7 + i) + ev_pos.observation_variance[i]; + ev_pos.test_ratio[i] = sq(ev_pos.innovation[i]) / (sq(innov_gate) * ev_pos.innovation_variance[i]); + + ev_pos.innovation_rejected[i] = (ev_pos.test_ratio[i] > 1.f); + } + + // z special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && ev_pos.innovation_rejected[2]) { + const float innov_limit = innov_gate * sqrtf(ev_pos.innovation_variance[2]); + ev_pos.innovation[2] = math::constrain(ev_pos.innovation[2], -innov_limit, innov_limit); + ev_pos.innovation_rejected[2] = false; + } + + ev_pos.timestamp_sample = ev_sample.time_us; + } +} + +void Ekf::fuseEvVel() +{ + // velocity + auto &ev_vel = _aid_src_ev_vel; + + // vx & vy + ev_vel.fusion_enabled[0] = true; + ev_vel.fusion_enabled[1] = true; + + if (!ev_vel.innovation_rejected[0] && !ev_vel.innovation_rejected[1]) { + for (int i = 0; i < 2; i++) { + if (fuseVelPosHeight(ev_vel.innovation[i], ev_vel.innovation_variance[i], i)) { + ev_vel.fused[i] = true; + ev_vel.time_last_fuse[i] = _time_last_imu; + } + } + } + + // vz + ev_vel.fusion_enabled[2] = true; + + if (ev_vel.fusion_enabled[2] && !ev_vel.innovation_rejected[2]) { + if (fuseVelPosHeight(ev_vel.innovation[2], ev_vel.innovation_variance[2], 2)) { + ev_vel.fused[2] = true; + ev_vel.time_last_fuse[2] = _time_last_imu; + } + } +} + +void Ekf::fuseEvPos() +{ + auto &ev_pos = _aid_src_ev_pos; + + // x & y + ev_pos.fusion_enabled[0] = true; + ev_pos.fusion_enabled[1] = true; + + if (!ev_pos.innovation_rejected[0] && !ev_pos.innovation_rejected[1]) { + for (int i = 0; i < 2; i++) { + if (fuseVelPosHeight(ev_pos.innovation[i], ev_pos.innovation_variance[i], 3 + i)) { + ev_pos.fused[i] = true; + ev_pos.time_last_fuse[i] = _time_last_imu; + } + } + } + + // z + ev_pos.fusion_enabled[2] = _control_status.flags.ev_hgt; + + if (ev_pos.fusion_enabled[2] && !ev_pos.innovation_rejected[2]) { + if (fuseVelPosHeight(ev_pos.innovation[2], ev_pos.innovation_variance[2], 2)) { + ev_pos.fused[2] = true; + ev_pos.time_last_fuse[2] = _time_last_imu; + } + } +} diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 86948f957c..2d843a304c 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4. All rights reserved. + * Copyright (c) 2021-2022 PX4. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp index 5c80466e31..99a163a900 100644 --- a/src/modules/ekf2/EKF/height_fusion.cpp +++ b/src/modules/ekf2/EKF/height_fusion.cpp @@ -142,19 +142,3 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt) rng_hgt.time_last_fuse = _time_last_imu; } } - -void Ekf::fuseEvHgt() -{ - // calculate the innovation assuming the external vision observation is in local NED frame - _ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2); - - // innovation gate size - float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f); - - // observation variance - defined externally - float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)); - - // _ev_pos_test_ratio(1) is the vertical test ratio - fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var, - _ev_pos_innov_var(2), _ev_pos_test_ratio(1)); -} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 8d8108f646..9e0e167fc8 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -649,6 +649,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_gnss_vel(), _status_gnss_vel_pub_last, _estimator_aid_src_gnss_vel_pub); PublishAidSourceStatus(_ekf.aid_src_gnss_pos(), _status_gnss_pos_pub_last, _estimator_aid_src_gnss_pos_pub); + // EV velocity & position + PublishAidSourceStatus(_ekf.aid_src_ev_vel(), _status_ev_vel_pub_last, _estimator_aid_src_ev_vel_pub); + PublishAidSourceStatus(_ekf.aid_src_ev_pos(), _status_ev_pos_pub_last, _estimator_aid_src_ev_pos_pub); + } void EKF2::PublishAttitude(const hrt_abstime ×tamp) diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 714f3a1ff7..b765f8a69f 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -263,6 +263,9 @@ private: hrt_abstime _status_gnss_vel_pub_last{0}; hrt_abstime _status_gnss_pos_pub_last{0}; + hrt_abstime _status_ev_vel_pub_last{0}; + hrt_abstime _status_ev_pos_pub_last{0}; + float _last_baro_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements @@ -330,6 +333,9 @@ private: uORB::PublicationMulti _estimator_aid_src_gnss_vel_pub{ORB_ID(estimator_aid_src_gnss_vel)}; uORB::PublicationMulti _estimator_aid_src_gnss_pos_pub{ORB_ID(estimator_aid_src_gnss_pos)}; + uORB::PublicationMulti _estimator_aid_src_ev_vel_pub{ORB_ID(estimator_aid_src_ev_vel)}; + uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; + // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub;