From b0ad9f022696962ecd2560ce81a725f4189af23a Mon Sep 17 00:00:00 2001 From: kamilritz Date: Fri, 3 Jul 2020 17:33:09 +0200 Subject: [PATCH] Replace componentwise with matrix operation --- EKF/ekf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 81d0ed7428..427428c448 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -269,8 +269,8 @@ void Ekf::predictState() // calculate a filtered horizontal acceleration with a 1 sec time constant // this are used for manoeuvre detection elsewhere float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt; - _accel_lpf_NE(0) = _accel_lpf_NE(0) * alpha + corrected_delta_vel_ef(0); - _accel_lpf_NE(1) = _accel_lpf_NE(1) * alpha + corrected_delta_vel_ef(1); + _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); + // save the previous value of velocity so we can use trapzoidal integration const Vector3f vel_last = _state.vel;