From 1e6c6f2d3ccaed470996c9efe88457299608f1d6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 16 Aug 2020 09:39:51 +1000 Subject: [PATCH] EKF: update all output states when aligning output time horizon filter --- EKF/ekf_helper.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index f3f6cdcf60..c52612e19e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -353,9 +353,7 @@ void Ekf::alignOutputFilter() _output_buffer[i].pos += pos_delta; } - _output_new.quat_nominal = q_delta * _output_new.quat_nominal; - _output_new.quat_nominal.normalize(); - // TODO: what about vel and pos of _output_new, shouldn't they be aligned too? + _output_new = _output_buffer.get_newest(); } // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.