From 021dd0d0af96abc3cd5512e8bb84b7843bb22d91 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Feb 2024 21:56:23 -0500 Subject: [PATCH] ekf2: fix EV height bias predict call - needs to be called every iteration --- src/modules/ekf2/EKF/ev_control.cpp | 1 + src/modules/ekf2/EKF/ev_height_control.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index 22ac4ec76474..0780f4111814 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 c0980a71b3c4..0552ace53c76 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;