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;