From 12ba97f4edfa4310c1137db2c03138b410f44a10 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 8 Nov 2022 12:13:18 -0500 Subject: [PATCH] EV HGT bias est predict central --- src/modules/ekf2/EKF/ev_control.cpp | 1 + src/modules/ekf2/EKF/ev_height_control.cpp | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) 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;