From 2b7b16986dcb09390a2d566808a9d518f155e9be Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 27 Jan 2022 15:48:24 +0100 Subject: [PATCH] update terrain state un fuse function --- src/modules/ekf2/EKF/ekf_helper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d9d3b55029..e509e7238c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -998,6 +998,7 @@ void Ekf::fuse(const Vector25f &K, float innovation) _state.mag_I -= K.slice<3, 1>(16, 0) * innovation; _state.mag_B -= K.slice<3, 1>(19, 0) * innovation; _state.wind_vel -= K.slice<2, 1>(22, 0) * innovation; + _state.posd_terrain -= K(24) * innovation; } void Ekf::uncorrelateQuatFromOtherStates()