From b87d2225e0900f4662fae149338e78f995ac7756 Mon Sep 17 00:00:00 2001 From: maverick64 Date: Thu, 17 Feb 2022 09:31:13 +0900 Subject: [PATCH] Eigen Quaterniond variable reorder --- include/kalman_filter_localization/ekf.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/kalman_filter_localization/ekf.hpp b/include/kalman_filter_localization/ekf.hpp index e19e75e..0bcc8a5 100644 --- a/include/kalman_filter_localization/ekf.hpp +++ b/include/kalman_filter_localization/ekf.hpp @@ -159,16 +159,16 @@ class EKFEstimator pow(dx(ERROR_STATE::DTHZ), 2)); if (norm_quat < 1e-10) { - Eigen::Quaterniond dq = Eigen::Quaterniond(0, 0, 0, cos(norm_quat / 2)); + Eigen::Quaterniond dq = Eigen::Quaterniond(cos(norm_quat / 2), 0, 0, 0); Eigen::Quaterniond q = Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ)); Eigen::Quaterniond q_new = q * dq; x_.segment(STATE::QX, 4) = Eigen::Vector4d(q_new.x(), q_new.y(), q_new.z(), q_new.w()); } else { Eigen::Quaterniond dq = Eigen::Quaterniond( + cos(norm_quat / 2), sin(norm_quat / 2) * dx(ERROR_STATE::DTHX) / norm_quat, sin(norm_quat / 2) * dx(ERROR_STATE::DTHY) / norm_quat, - sin(norm_quat / 2) * dx(ERROR_STATE::DTHZ) / norm_quat, - cos(norm_quat / 2)); + sin(norm_quat / 2) * dx(ERROR_STATE::DTHZ) / norm_quat); Eigen::Quaterniond q = Eigen::Quaterniond(x_(STATE::QW), x_(STATE::QX), x_(STATE::QY), x_(STATE::QZ)); Eigen::Quaterniond q_new = q * dq; x_.segment(STATE::QX, 4) = Eigen::Vector4d(q_new.x(), q_new.y(), q_new.z(), q_new.w());