Merge pull request #6 from Sang-Beom-Woo/debug-review

Eigen Quaterniond variable reorder
rsasaki0109 · Feb 17, 2022 · 8e3a446 · 8e3a446
2 parents 8d96763 + b87d222
commit 8e3a446
Showing 1 changed file with 3 additions and 3 deletions.
diff --git 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());