diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index db21a6b3f1..afaa6e65e8 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1155,6 +1155,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_init{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_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index fe2513f319..6a95696870 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -95,8 +95,16 @@ 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 AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + if (_ev_q_error_init) { + _ev_q_error_filt.update(q_error); + + } else { + _ev_q_error_filt.reset(q_error); + _ev_q_error_init = true; + } + + 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();