diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index 22ac4ec764..0780f41118 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 c0980a71b3..0552ace53c 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -45,7 +45,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com HeightBiasEstimator &bias_est = _ev_hgt_b_est; - bias_est.predict(_dt_ekf_avg); + // bias_est.predict(_dt_ekf_avg) called by controlExternalVisionFusion() // correct position for offset relative to IMU const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;