From 6cf512f1037d42e53bc6df2ef018feaf531f5dc8 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 8 Mar 2019 14:40:11 +0100 Subject: [PATCH] EKF: Also update the newest and oldest quaternion estimate in alignOutputFilter() Signed-off-by: CarlOlsson --- EKF/ekf_helper.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 837bd82976..cb020b9cb3 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -394,6 +394,12 @@ void Ekf::alignOutputFilter() _output_buffer[i].vel += vel_delta; _output_buffer[i].pos += pos_delta; } + + _output_new.quat_nominal *= q_delta; + _output_new.quat_nominal.normalize(); + + _output_sample_delayed.quat_nominal *= q_delta; + _output_sample_delayed.quat_nominal.normalize(); } // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.