diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 09a244967c..dadb5bfac2 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1003,6 +1003,7 @@ private: #if defined(CONFIG_EKF2_EXTERNAL_VISION) // control fusion of external vision observations void controlExternalVisionFusion(); + void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset); 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 controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_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); @@ -1160,6 +1161,8 @@ private: #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; + AlphaFilter _ev_q_error_filt{0.001f}; + bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index da27f11924..461cf6e79a 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -58,6 +58,7 @@ void Ekf::controlExternalVisionFusion() && ((_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); + updateEvAttitudeErrorFilter(ev_sample, ev_reset); controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); @@ -84,3 +85,20 @@ void Ekf::controlExternalVisionFusion() ECL_WARN("vision data stopped"); } } + +void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset) +{ + const AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + + if (!q_error.isAllFinite()) { + return; + } + + if (!_ev_q_error_initialized || ev_reset) { + _ev_q_error_filt.reset(q_error); + _ev_q_error_initialized = true; + + } else { + _ev_q_error_filt.update(q_error); + } +} diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index aefd7e1681..9dffdc8e38 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -56,10 +56,9 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com Matrix3f pos_cov{matrix::diag(ev_sample.position_var)}; // 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 Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + const Quatf q_error(_ev_q_error_filt.getState()); if (q_error.isAllFinite()) { const Dcmf R_ev_to_ekf(q_error); diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index 980a488672..fc4e2db633 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -95,8 +95,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } 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); + const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth; pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose(); diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 807948ef27..0160ca9fc9 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -80,8 +80,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } 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); + const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); 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();