fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp

@@ -29,6 +29,8 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 
+#include <tf2/utils.h>
+
 #include <memory>
 #include <vector>
 
@@ -77,7 +79,7 @@ class EKFModule
   bool measurement_update_twist(
     const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
     EKFDiagnosticInfo & twist_diag_info);
-  geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay(
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay(
     const PoseWithCovariance & pose, const double delay_time);
 
 private:
@@ -86,6 +88,7 @@ class EKFModule
   std::shared_ptr<Warning> warning_;
   const int dim_x_;
   std::vector<double> accumulated_delay_times_;
+  tf2::Vector3 last_angular_velocity_;
   const HyperParameters params_;
 };
 

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -187,11 +187,13 @@
       if (is_updated) {
         pose_is_updated = true;
 
-        // Update Simple 1D filter with considering change of z value due to measurement pose delay
+        // Update Simple 1D filter with considering change of roll, pitch and height (position z)
+        // values due to measurement pose delay
         const double delay_time =
           (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay;
-        const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time);
-        update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps);
+        auto pose_with_rph_delay_compensation =
+          ekf_module_->compensate_rph_with_delay(*pose, delay_time);
+        update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps);
       }
     }
     DEBUG_INFO(

localization/ekf_localizer/src/ekf_module.cpp

@@ -37,6 +37,7 @@
 : warning_(std::move(warning)),
   dim_x_(6),  // x, y, yaw, yaw_bias, vx, wz
   accumulated_delay_times_(params.extend_state_step, 1.0E15),
+  last_angular_velocity_(0, 0, 0),
   params_(params)
 {
   Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1);
@@ -282,15 +283,37 @@
   return true;
 }
 
-geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay(
+geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay(
   const PoseWithCovariance & pose, const double delay_time)
 {
+  tf2::Quaternion delta_orientation;
+  if (last_angular_velocity_.length() > 0.0) {
+    delta_orientation.setRotation(
+      last_angular_velocity_.normalized(), last_angular_velocity_.length() * delay_time);
+  } else {
+    delta_orientation.setValue(0.0, 0.0, 0.0, 1.0);
+  }
+
+  tf2::Quaternion prew_orientation = tf2::Quaternion(
+    pose.pose.pose.orientation.x, pose.pose.pose.orientation.y, pose.pose.pose.orientation.z,
+    pose.pose.pose.orientation.w);
+
+  tf2::Quaternion curr_orientation;
+  curr_orientation = prew_orientation * delta_orientation;
+  curr_orientation.normalize();
+
   const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation);
-  const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
-  PoseWithCovariance pose_with_z_delay;
-  pose_with_z_delay = pose;
-  pose_with_z_delay.pose.pose.position.z += dz_delay;
-  return pose_with_z_delay;
+  const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
+
+  PoseWithCovariance pose_with_delay;
+  pose_with_delay = pose;
+  pose_with_delay.pose.pose.position.z += delta_z;
+  pose_with_delay.pose.pose.orientation.x = curr_orientation.x();
+  pose_with_delay.pose.pose.orientation.y = curr_orientation.y();
+  pose_with_delay.pose.pose.orientation.z = curr_orientation.z();
+  pose_with_delay.pose.pose.orientation.w = curr_orientation.w();
+
+  return pose_with_delay;
 }
 
 bool EKFModule::measurement_update_twist(
@@ -330,6 +353,9 @@
   Eigen::MatrixXd y(dim_y, 1);
   y << twist.twist.twist.linear.x, twist.twist.twist.angular.z;
 
+  last_angular_velocity_ = tf2::Vector3(
+    twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z);
+
   if (has_nan(y) || has_inf(y)) {
     warning_->warn(
       "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");