Merge pull request #3 from rsasaki0109/devel

Update odom fusion method
rsasaki0109 · May 4, 2020 · 6ac621e · 6ac621e
2 parents 92d18f2 + ea57aa0
commit 6ac621e
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 14 deletions.
diff --git a/include/kalman_filter_localization/ekf.hpp b/include/kalman_filter_localization/ekf.hpp
@@ -40,7 +40,7 @@ class EKFEstimator
 {
 public:
   EKFEstimator()
-  : P_(Eigen::MatrixXd::Identity(num_error_state_, num_error_state_) * 100),
+  : P_(EigenMatrix9d::Identity() * 100),
     var_imu_w_{0.33},
     var_imu_acc_{0.33},
     tau_gyro_bias_{1.0}
@@ -98,8 +98,8 @@ class EKFEstimator
       predicted_quat.x(), predicted_quat.y(), predicted_quat.z(), predicted_quat.w());
 
     // F
-    Eigen::MatrixXd F = Eigen::MatrixXd::Identity(num_error_state_, num_error_state_);
-    F.block<3, 3>(0, 3) = dt_imu * Eigen::MatrixXd::Identity(3, 3);
+    Eigen::MatrixXd F = EigenMatrix9d::Identity();
+    F.block<3, 3>(0, 3) = dt_imu * Eigen::Matrix3d::Identity();
     Eigen::Matrix3d acc_skew;
     acc_skew <<
       0, -acc(2), acc(1),
@@ -108,15 +108,15 @@ class EKFEstimator
     F.block<3, 3>(3, 6) = rot_mat * (-acc_skew) * dt_imu;
 
     // Q
-    Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(6, 6);
+    Eigen::MatrixXd Q = Eigen::Matrix<double, 6, 6>::Identity();
     Q.block<3, 3>(0, 0) = var_imu_acc_ * Q.block<3, 3>(0, 0);
     Q.block<3, 3>(3, 3) = var_imu_w_ * Q.block<3, 3>(3, 3);
     Q = Q * (dt_imu * dt_imu);
 
     // L
-    Eigen::MatrixXd L = Eigen::MatrixXd::Zero(9, 6);
-    L.block<3, 3>(3, 0) = Eigen::MatrixXd::Identity(3, 3);
-    L.block<3, 3>(6, 3) = Eigen::MatrixXd::Identity(3, 3);
+    Eigen::MatrixXd L = Eigen::Matrix<double, num_error_state_, 6>::Zero();
+    L.block<3, 3>(3, 0) = Eigen::Matrix3d::Identity();
+    L.block<3, 3>(6, 3) = Eigen::Matrix3d::Identity();
 
     P_ = F * P_ * F.transpose() + L * Q * L.transpose();
   }
@@ -145,8 +145,8 @@ class EKFEstimator
       variance.x(), 0, 0,
       0, variance.y(), 0,
       0, 0, variance.z();
-    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, num_error_state_);
-    H.block<3, 3>(0, 0) = Eigen::MatrixXd::Identity(3, 3);
+    Eigen::MatrixXd H = Eigen::Matrix<double, 3, num_error_state_>::Zero();
+    H.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
     Eigen::MatrixXd K = P_ * H.transpose() * (H * P_ * H.transpose() + R).inverse();
     Eigen::VectorXd dx = K * (y - x_.segment(STATE::X, 3));
 
@@ -167,7 +167,7 @@ class EKFEstimator
         cos(norm_quat / 2));
     }
 
-    P_ = (Eigen::MatrixXd::Identity(num_error_state_, num_error_state_) - K * H) * P_;
+    P_ = (EigenMatrix9d::Identity() - K * H) * P_;
   }
 
   void setTauGyroBias(const double tau_gyro_bias)
@@ -213,8 +213,10 @@ class EKFEstimator
   static const int num_state_{10};
   static const int num_error_state_{9};
 
+  typedef Eigen::Matrix<double, num_error_state_, num_error_state_> EigenMatrix9d;
+
   Eigen::Matrix<double, num_state_, 1> x_;
-  Eigen::Matrix<double, num_error_state_, num_error_state_> P_;
+  EigenMatrix9d P_;
 
   const Eigen::Vector3d gravity_{0, 0, 9.80665};
 

diff --git a/include/kalman_filter_localization/ekf_localization_component.hpp b/include/kalman_filter_localization/ekf_localization_component.hpp
@@ -143,6 +143,10 @@ class EkfLocalizationComponent : public rclcpp::Node
     const Eigen::Vector3d variance);
   void broadcastPose();
 
+  geometry_msgs::msg::PoseStamped current_pose_odom_;
+  Eigen::Matrix4d previous_odom_mat_{Eigen::Matrix4d::Identity()};
+
+
   enum STATE
   {
     X  = 0, Y = 1, Z = 2,

diff --git a/src/ekf_localization_component.cpp b/src/ekf_localization_component.cpp
@@ -200,12 +200,29 @@ EkfLocalizationComponent::EkfLocalizationComponent(const rclcpp::NodeOptions & o
     [this](const typename nav_msgs::msg::Odometry::SharedPtr msg) -> void
     {
       if (initial_pose_recieved_ && use_odom_) {
+        Eigen::Affine3d affine;
+        tf2::fromMsg(msg->pose.pose, affine);
+        Eigen::Matrix4d odom_mat = affine.matrix();
+        if (previous_odom_mat_ == Eigen::Matrix4d::Identity()) {
+          current_pose_odom_ = current_pose_;
+          previous_odom_mat_ = odom_mat;
+          return;
+        }
+
+        Eigen::Affine3d current_affine;
+        tf2::fromMsg(current_pose_odom_.pose, current_affine);
+        Eigen::Matrix4d current_trans = current_affine.matrix();
+        current_trans = current_trans * previous_odom_mat_.inverse() * odom_mat;
+
         geometry_msgs::msg::PoseStamped pose;
         pose.header = msg->header;
-        pose.pose.position.x = msg->pose.pose.position.x;
-        pose.pose.position.y = msg->pose.pose.position.y;
-        pose.pose.position.z = msg->pose.pose.position.z;
+        pose.pose.position.x = current_trans(0, 3);
+        pose.pose.position.y = current_trans(1, 3);
+        pose.pose.position.z = current_trans(2, 3);
         measurementUpdate(pose, var_odom_);
+
+        current_pose_odom_ = current_pose_;
+        previous_odom_mat_ = odom_mat;
       }
     };