diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index b098fcfab4..fb444959ac 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -41,6 +41,7 @@ void Ekf::controlExternalVisionFusion() { _ev_pos_b_est.predict(_dt_ekf_avg); + _ev_hgt_b_est.predict(_dt_ekf_avg); // Check for new external vision data extVisionSample ev_sample; diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index 87c8224526..76c85354e5 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -44,8 +44,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, bool starting_ static constexpr const char *AID_SRC_NAME = "EV height"; HeightBiasEstimator &bias_est = _ev_hgt_b_est; - - bias_est.predict(_dt_ekf_avg); + //bias_est.predict(_dt_ekf_avg); // handled by controlExternalVisionFusion() // correct position for offset relative to IMU const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;