fix(ekf_localizer): add_delay_compensation_for_roll_and_pitch

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -239,6 +239,11 @@ class EKFLocalizer : public rclcpp::Node
 
   autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
 
+  /**
+   * @brief last angular velocity for compensating rph with delay
+   */
+  tf2::Vector3 last_angular_velocity_;
+
   friend class EKFLocalizerTestSuite;  // for test code
 };
 #endif  // EKF_LOCALIZER__EKF_LOCALIZER_HPP_

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,8 +79,8 @@ 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(
-    const PoseWithCovariance & pose, const double delay_time);
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay(
+    const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time);
 
 private:
   TimeDelayKalmanFilter kalman_filter_;

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -47,7 +47,8 @@
   params_(this),
   ekf_dt_(params_.ekf_dt),
   pose_queue_(params_.pose_smoothing_steps),
-  twist_queue_(params_.twist_smoothing_steps)
+  twist_queue_(params_.twist_smoothing_steps),
+  last_angular_velocity_(0.0, 0.0, 0.0)
 {
   /* convert to continuous to discrete */
   proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);
@@ -187,41 +188,47 @@
       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, last_angular_velocity_, delay_time);
+        update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps);
       }
     }
     DEBUG_INFO(
       get_logger(), "[EKF] measurement_update_pose calc time = %f [ms]", stop_watch_.toc());
     DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n");
   }
   pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1);
 
   /* twist measurement update */
   twist_diag_info_.queue_size = twist_queue_.size();
   twist_diag_info_.is_passed_delay_gate = true;
   twist_diag_info_.delay_time = 0.0;
   twist_diag_info_.delay_time_threshold = 0.0;
   twist_diag_info_.is_passed_mahalanobis_gate = true;
   twist_diag_info_.mahalanobis_distance = 0.0;
 
   bool twist_is_updated = false;
 
   if (!twist_queue_.empty()) {
     DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------");
     stop_watch_.tic();
 
     // save the initial size because the queue size can change in the loop
     const size_t n = twist_queue_.size();
     for (size_t i = 0; i < n; ++i) {
       const auto twist = twist_queue_.pop_increment_age();
       bool is_updated =
         ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_);
       if (is_updated) {
         twist_is_updated = true;
+        last_angular_velocity_ = tf2::Vector3(
+          twist->twist.twist.angular.x, twist->twist.twist.angular.y, twist->twist.twist.angular.z);
+      } else {
+        last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0);
       }
     }
     DEBUG_INFO(

localization/ekf_localizer/src/ekf_module.cpp

@@ -282,15 +282,39 @@
   return true;
 }
 
-geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay(
-  const PoseWithCovariance & pose, const double delay_time)
+geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay(
+  const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time)
 {
-  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;
+  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 prev_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 = prev_orientation * delta_orientation;
+  curr_orientation.normalize();
+
+  PoseWithCovariance pose_with_delay;
+  pose_with_delay = pose;
+  pose_with_delay.header.stamp =
+    rclcpp::Time(pose.header.stamp) + rclcpp::Duration::from_seconds(delay_time);
+  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();
+
+  const auto rpy = autoware::universe_utils::getRPY(pose_with_delay.pose.pose.orientation);
+  const double delta_z = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y);
+  pose_with_delay.pose.pose.position.z += delta_z;
+
+  return pose_with_delay;
 }
 
 bool EKFModule::measurement_update_twist(