From 9192ced7bb6bd55b75ddcd50833ad497fc571e06 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 25 Feb 2016 18:20:29 +0100 Subject: [PATCH] do not reset output attitude state after heading reset to avoid jumps in attitude --- 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 af75b87b4a..7723c2eae7 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -113,9 +113,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init; euler_init(2) = _mag_declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0)); - // calculate initial quaternion states + // calculate initial quaternion states for the ekf + // we don't change the output attitude to avoid jumps _state.quat_nominal = Quaternion(euler_init); - _output_new.quat_nominal = _state.quat_nominal; // reset the angle error variances because the yaw angle could have changed by a significant amount // by setting them to zero we avoid 'kicks' in angle when 3-D fusion starts and the imu process noise