From ad7f7af03be36333adb603fb0370cb8f3cdf5d0c Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Tue, 21 May 2019 11:31:36 +0200 Subject: [PATCH] ekf: correct quaternion multiplication order --- EKF/ekf_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 91a7f05551..459a5e9d2f 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -516,7 +516,7 @@ bool Ekf::realignYawGPS() // apply the change in attitude quaternion to our newest quaternion estimate // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + _output_new.quat_nominal = _output_new.quat_nominal * _state_reset_status.quat_change; // capture the reset event _state_reset_status.quat_counter++; @@ -756,7 +756,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update // apply the change in attitude quaternion to our newest quaternion estimate // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + _output_new.quat_nominal = _output_new.quat_nominal * _state_reset_status.quat_change; } // capture the reset event