{"id":16457,"date":"2024-10-08T14:43:09","date_gmt":"2024-10-08T06:43:09","guid":{"rendered":"http:\/\/192.168.10.115\/?p=16457"},"modified":"2024-10-08T14:43:09","modified_gmt":"2024-10-08T06:43:09","slug":"2024-10-08-autoware-%e5%ae%9a%e4%bd%8d%e4%b9%8bekf-%e6%bb%a4%e6%b3%a2%e5%ae%9a%e4%bd%8d%ef%bc%88%e5%9b%9b%ef%bc%89","status":"publish","type":"post","link":"http:\/\/222.128.65.89:18086\/index.php\/2024\/10\/08\/16457\/","title":{"rendered":"2024-10-08 Autoware \u5b9a\u4f4d\u4e4bEKF \u6ee4\u6ce2\u5b9a\u4f4d\uff08\u56db\uff09"},"content":{"rendered":"\n

1. \u4e3b\u8981\u529f\u80fd<\/p>\n\n\n\n

    \n
  1. \u8f93\u5165\u6d88\u606f\u7684\u65f6\u95f4\u5ef6\u8fdf\u8865\u507f\uff0c\u53ef\u4ee5\u6709\u6548\u6574\u5408\u5177\u6709\u4e0d\u540c\u65f6\u95f4\u5ef6\u8fdf\u7684\u8f93\u5165\u4fe1\u606f\u3002\u8fd9\u5bf9\u4e8e\u9ad8\u901f\u79fb\u52a8\u7684\u673a\u5668\u4eba\uff0c\u6bd4\u5982\u81ea\u52a8\u9a7e\u9a76\u8f66\u8f86\uff0c\u5c24\u4e3a\u91cd\u8981\uff08\u89c1\u4e0b\u56fe\uff09\u3002<\/li>\n\n\n\n
  2. \u81ea\u52a8\u4f30\u8ba1\u504f\u822a\u8bef\u5dee\uff0c\u53ef\u4ee5\u9632\u6b62\u7531\u4e8e\u4f20\u611f\u5668\u5b89\u88c5\u89d2\u5ea6\u8bef\u5dee\u5bfc\u81f4\u7684\u5efa\u6a21\u9519\u8bef\uff0c\u4ece\u800c\u63d0\u9ad8\u4f30\u8ba1\u7cbe\u5ea6\u3002<\/li>\n\n\n\n
  3. \u9a6c\u6c0f\u8ddd\u79bb\u95e8\uff0c\u53ef\u4ee5\u8fdb\u884c\u6982\u7387\u5f02\u5e38\u503c\u68c0\u6d4b\uff0c\u786e\u5b9a\u54ea\u4e9b\u8f93\u5165\u5e94\u8be5\u88ab\u4f7f\u7528\u6216\u5ffd\u7565\u3002<\/li>\n\n\n\n
  4. \u5e73\u6ed1\u66f4\u65b0\uff0c\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u7684\u6d4b\u91cf\u66f4\u65b0\u901a\u5e38\u5728\u83b7\u5f97\u6d4b\u91cf\u503c\u65f6\u6267\u884c\uff0c\u4f46\u8fd9\u53ef\u80fd\u5bfc\u81f4\u4f30\u8ba1\u503c\u7684\u5927\u5e45\u53d8\u5316\uff0c\u5c24\u5176\u662f\u5bf9\u4e8e\u4f4e\u9891\u6d4b\u91cf\u3002\u7531\u4e8e\u7b97\u6cd5\u53ef\u4ee5\u8003\u8651\u6d4b\u91cf\u65f6\u95f4\uff0c\u6d4b\u91cf\u6570\u636e\u53ef\u4ee5\u88ab\u5206\u6210\u591a\u4e2a\u90e8\u5206\uff0c\u5e76\u5728\u4fdd\u6301\u4e00\u81f4\u6027\u7684\u540c\u65f6\u5e73\u6ed1\u96c6\u6210\uff08\u89c1\u4e0b\u56fe\uff09\u3002<\/li>\n\n\n\n
  5. \u4ece\u4fef\u4ef0\u89d2\u8ba1\u7b97\u5782\u76f4\u6821\u6b63\u91cf\uff0c\u53ef\u4ee5\u51cf\u8f7b\u5761\u5ea6\u4e0a\u7684\u5b9a\u4f4d\u4e0d\u7a33\u5b9a\u6027\u3002\u4f8b\u5982\uff0c\u4e0a\u5761\u65f6\uff0c\u7531\u4e8eEKF\u53ea\u8003\u86513DoF\uff08x\uff0cy\uff0c\u504f\u822a\uff09\uff0c\u5b83\u7684\u884c\u4e3a\u5c31\u50cf\u88ab\u57cb\u5728\u5730\u4e0b\u4e00\u6837\uff08\u89c1\u201c\u4ece\u4fef\u4ef0\u89d2\u8ba1\u7b97\u589e\u91cf\u201d\u56fe\u7684\u5de6\u4fa7\uff09\u3002\u56e0\u6b64\uff0cEKF\u6839\u636e\u516c\u5f0f\u6821\u6b63z\u5750\u6807\uff08\u89c1\u201c\u4ece\u4fef\u4ef0\u89d2\u8ba1\u7b97\u589e\u91cf\u201d\u56fe\u7684\u53f3\u4fa7\uff09\u3002<\/li>\n<\/ol>\n\n\n\n

    2. \u4ee3\u7801\u9605\u8bfb<\/p>\n\n\n\n

    2.1 covariance.cpp<\/p>\n\n\n\n

    \/\/\/ @brief \u5c06 EKF \u7684\u72b6\u6001\u534f\u65b9\u5dee\u77e9\u9635\u8f6c\u6362\u4e3a\u59ff\u6001\u6d88\u606f\u7684\u534f\u65b9\u5dee\u6570\u7ec4\u3002\u901a\u8fc7\u4ece\u8f93\u5165\u7684\u72b6\u6001\u534f\u65b9\u5dee\u77e9\u9635 P \u4e2d\u63d0\u53d6\u7279\u5b9a\u7d22\u5f15\u4f4d\u7f6e\u7684\u503c\uff0c\u5e76\u5c06\u8fd9\u4e9b\u503c\u5b58\u50a8\u5230\u8f93\u51fa\u7684 covariance \u6570\u7ec4\u4e2d\uff0c\u4ee5\u6784\u5efa\u59ff\u6001\u6d88\u606f\u7684\u534f\u65b9\u5dee\u77e9\u9635\n\/\/\/ @param P EKF \u7684\u72b6\u6001\u534f\u65b9\u5dee\u77e9\u9635\u3002\n\/\/\/ @return \nstd::array&lt;double, 36&gt; ekfCovarianceToPoseMessageCovariance(const Matrix6d &amp; P)\n{\n  std::array&lt;double, 36&gt; covariance;\n  covariance.fill(0.);\n\n  covariance[COV_IDX::X_X] = P(IDX::X, IDX::X);\n  covariance[COV_IDX::X_Y] = P(IDX::X, IDX::Y);\n  covariance[COV_IDX::X_YAW] = P(IDX::X, IDX::YAW);\n  covariance[COV_IDX::Y_X] = P(IDX::Y, IDX::X);\n  covariance[COV_IDX::Y_Y] = P(IDX::Y, IDX::Y);\n  covariance[COV_IDX::Y_YAW] = P(IDX::Y, IDX::YAW);\n  covariance[COV_IDX::YAW_X] = P(IDX::YAW, IDX::X);\n  covariance[COV_IDX::YAW_Y] = P(IDX::YAW, IDX::Y);\n  covariance[COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);\n\n  return covariance;\n}\n\/\/\/ @brief \u5c06 EKF \u7684\u72b6\u6001\u534f\u65b9\u5dee\u77e9\u9635\u8f6c\u6362\u4e3a\u626d\u8f6c\u6d88\u606f\u7684\u534f\u65b9\u5dee\u6570\u7ec4\n\/\/\/ @param P EKF \u7684\u72b6\u6001\u534f\u65b9\u5dee\u77e9\u9635\u3002\n\/\/\/ @return \nstd::array&lt;double, 36&gt; ekfCovarianceToTwistMessageCovariance(const Matrix6d &amp; P)\n{\n  std::array&lt;double, 36&gt; covariance;\n  covariance.fill(0.);\n\n  covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX);\n  covariance[COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ);\n  covariance[COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX);\n  covariance[COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ);\n\n  return covariance;\n}<\/code><\/pre>\n\n\n\n
    2.2 diagnostics.cpp<\/p>\n\n\n\n
    \/\/\/ @brief \u68c0\u67e5\u8fdb\u7a0b\u662f\u5426\u5df2\u6fc0\u6d3b\uff0c\u5e76\u8fd4\u56de\u76f8\u5e94\u7684\u8bca\u65ad\u72b6\u6001.\u6839\u636e\u4f20\u5165\u7684is_activated\u53c2\u6570\u8bbe\u7f6estat.values\u3001stat.level\u548cstat.message\n\/\/\/ @param is_activated \u8fdb\u7a0b\u662f\u5426\u5df2\u6fc0\u6d3b\n\/\/\/ @return \ndiagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated)\n{\n  diagnostic_msgs::msg::DiagnosticStatus stat;\n\n  diagnostic_msgs::msg::KeyValue key_value;\n  key_value.key = \"is_activated\";\n  key_value.value = is_activated ? \"True\" : \"False\";\n  stat.values.push_back(key_value);\n\n  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;\n  stat.message = \"OK\";\n  if (!is_activated) {\n    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;\n    stat.message = \"[WARN]process is not activated\";\n  }\n\n  return stat;\n}\n\/\/\/ @brief \u68c0\u67e5\u6d4b\u91cf\u6570\u636e\u662f\u5426\u66f4\u65b0\uff0c\u5e76\u8fd4\u56de\u76f8\u5e94\u7684\u8bca\u65ad\u72b6\u6001\u3002\u6839\u636e\u4f20\u5165\u7684no_update_count\u53c2\u6570\u8bbe\u7f6estat.values\u3001stat.level\u548cstat.message\n\/\/\/ @param measurement_type \u6d4b\u91cf\u7c7b\u578b\n\/\/\/ @param no_update_count \u6ca1\u6709\u66f4\u65b0\u7684\u8ba1\u6570\n\/\/\/ @param no_update_count_threshold_warn \u8b66\u544a\u9608\u503c\n\/\/\/ @param no_update_count_threshold_error \u9519\u8bef\u9608\u503c\n\/\/\/ @return \ndiagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated(\n  const std::string &amp; measurement_type, const size_t no_update_count,\n  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error)\n{\n  diagnostic_msgs::msg::DiagnosticStatus stat;\n    \/\/\u5c06\u5404\u79cd\u6570\u636e\u8f6c\u6362\u4e3a\u5b57\u7b26\u4e32\u5f62\u5f0f\uff0c\u5b58\u5165 stat.values \u4e2d\u3002\n  diagnostic_msgs::msg::KeyValue key_value;\n  key_value.key = measurement_type + \"_no_update_count\";\n  key_value.value = std::to_string(no_update_count);\n  stat.values.push_back(key_value);\n  key_value.key = measurement_type + \"_no_update_count_threshold_warn\";\n  key_value.value = std::to_string(no_update_count_threshold_warn);\n  stat.values.push_back(key_value);\n  key_value.key = measurement_type + \"_no_update_count_threshold_error\";\n  key_value.value = std::to_string(no_update_count_threshold_error);\n  stat.values.push_back(key_value);\n\n  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;\n  stat.message = \"OK\";\n  \/\/\u6839\u636e no_update_count \u662f\u5426\u8d85\u8fc7\u9608\u503c\u8bbe\u7f6e stat.level \u548c stat.message\u3002\n  if (no_update_count &gt;= no_update_count_threshold_warn) {\n    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;\n    stat.message = \"[WARN]\" + measurement_type + \" is not updated\";\n  }\n  if (no_update_count &gt;= no_update_count_threshold_error) {\n    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;\n    stat.message = \"[ERROR]\" + measurement_type + \" is not updated\";\n  }\n\n  return stat;\n}\n\/\/\/ @brief \u68c0\u67e5\u6d4b\u91cf\u961f\u5217\u7684\u5927\u5c0f\uff0c\u5e76\u8fd4\u56de\u76f8\u5e94\u7684\u8bca\u65ad\u72b6\u6001\n\/\/\/ @param measurement_type \u6d4b\u91cf\u7c7b\u578b\n\/\/\/ @param queue_size \u961f\u5217\u5927\u5c0f\n\/\/\/ @return \ndiagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize(\n  const std::string &amp; measurement_type, const size_t queue_size)\n{\n  diagnostic_msgs::msg::DiagnosticStatus stat;\n\n  diagnostic_msgs::msg::KeyValue key_value;\n  key_value.key = measurement_type + \"_queue_size\";\n  key_value.value = std::to_string(queue_size);\n  stat.values.push_back(key_value);\n\n  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;\n  stat.message = \"OK\";\n\n  return stat;\n}\n\/\/\/ @brief \u68c0\u67e5\u6d4b\u91cf\u662f\u5426\u901a\u8fc7\u5ef6\u8fdf\u95e8\uff0c\u5e76\u8fd4\u56de\u76f8\u5e94\u7684\u8bca\u65ad\u72b6\u6001\n\/\/\/ @param measurement_type \u6d4b\u91cf\u7c7b\u578b\n\/\/\/ @param is_passed_delay_gate \u8868\u793a\u5ef6\u8fdf\u95e8\u662f\u5426\u901a\u8fc7\n\/\/\/ @param delay_time \u5ef6\u8fdf\u65f6\u95f4\n\/\/\/ @param delay_time_threshold \u5ef6\u8fdf\u65f6\u95f4\u9608\u503c \n\/\/\/ @return \ndiagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate(\n  const std::string &amp; measurement_type, const bool is_passed_delay_gate, const double delay_time,\n  const double delay_time_threshold)\n{\n  diagnostic_msgs::msg::DiagnosticStatus stat;\n\n  diagnostic_msgs::msg::KeyValue key_value;\n  key_value.key = measurement_type + \"_is_passed_delay_gate\";\n  key_value.value = is_passed_delay_gate ? \"True\" : \"False\";\n  stat.values.push_back(key_value);\n  key_value.key = measurement_type + \"_delay_time\";\n  key_value.value = std::to_string(delay_time);\n  stat.values.push_back(key_value);\n  key_value.key = measurement_type + \"_delay_time_threshold\";\n  key_value.value = std::to_string(delay_time_threshold);\n  stat.values.push_back(key_value);\n\n  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;\n  stat.message = \"OK\";\n  if (!is_passed_delay_gate) {\n    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;\n    stat.message = \"[WARN]\" + measurement_type + \" topic is delay\";\n  }\n\n  return stat;\n}\n\/\/\/ @brief \u68c0\u67e5\u6d4b\u91cf\u662f\u5426\u901a\u8fc7\u9a6c\u6c0f\u8ddd\u79bb\u95e8\uff0c\u5e76\u8fd4\u56de\u76f8\u5e94\u7684\u8bca\u65ad\u72b6\u6001\n\/\/\/ @param measurement_type \u6d4b\u91cf\u7c7b\u578b\n\/\/\/ @param is_passed_mahalanobis_gate \u8868\u793a\u9a6c\u6c0f\u8ddd\u79bb\u95e8\u662f\u5426\u901a\u8fc7 \n\/\/\/ @param mahalanobis_distance \u9a6c\u6c0f\u8ddd\u79bb\n\/\/\/ @param mahalanobis_distance_threshold  \u9a6c\u6c0f\u8ddd\u79bb\u9608\u503c \n\/\/\/ @return \ndiagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate(\n  const std::string &amp; measurement_type, const bool is_passed_mahalanobis_gate,\n  const double mahalanobis_distance, const double mahalanobis_distance_threshold)\n{\n  diagnostic_msgs::msg::DiagnosticStatus stat;\n\n  diagnostic_msgs::msg::KeyValue key_value;\n  key_value.key = measurement_type + \"_is_passed_mahalanobis_gate\";\n  key_value.value = is_passed_mahalanobis_gate ? \"True\" : \"False\";\n  stat.values.push_back(key_value);\n  key_value.key = measurement_type + \"_mahalanobis_distance\";\n  key_value.value = std::to_string(mahalanobis_distance);\n  stat.values.push_back(key_value);\n  key_value.key = measurement_type + \"_mahalanobis_distance_threshold\";\n  key_value.value = std::to_string(mahalanobis_distance_threshold);\n  stat.values.push_back(key_value);\n\n  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;\n  stat.message = \"OK\";\n  if (!is_passed_mahalanobis_gate) {\n    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;\n    stat.message = \"[WARN]mahalanobis distance of \" + measurement_type + \" topic is large\";\n  }\n\n  return stat;\n}\n\n\/\/ The highest level within the stat_array will be reflected in the merged_stat.\n\/\/ When all stat_array entries are 'OK,' the message of merged_stat will be \"OK\"\n\n\/\/\/ @brief \u5408\u5e76\u8bca\u65ad\u72b6\u6001\u6570\u7ec4\uff0c\u5e76\u8fd4\u56de\u5408\u5e76\u540e\u7684\u8bca\u65ad\u72b6\u6001\n\/\/\/ @param stat_array \u6839\u636e\u6bcf\u4e2a\u8bca\u65ad\u72b6\u6001\u7684\u7ea7\u522b\u548c\u6d88\u606f\u8fdb\u884c\u5408\u5e76\n\/\/\/ @return \ndiagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus(\n  const std::vector&lt;diagnostic_msgs::msg::DiagnosticStatus&gt; &amp; stat_array)\n{\n  diagnostic_msgs::msg::DiagnosticStatus merged_stat;\n\n  for (const auto &amp; stat : stat_array) {\n    if ((stat.level &gt; diagnostic_msgs::msg::DiagnosticStatus::OK)) {\n      if (!merged_stat.message.empty()) {\n        merged_stat.message += \"; \";\n      }\n      merged_stat.message += stat.message;\n    }\n    if (stat.level &gt; merged_stat.level) {\n      merged_stat.level = stat.level;\n    }\n    for (const auto &amp; value : stat.values) {\n      merged_stat.values.push_back(value);\n    }\n  }\n\n  if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {\n    merged_stat.message = \"OK\";\n  }\n\n  return merged_stat;\n}<\/code><\/pre>\n\n\n\n
    2.3 ekf_localizer.cpp<\/p>\n\n\n\n
    \/\/ clang-format off\n#define PRINT_MAT(X) std::cout &lt;&lt; #X &lt;&lt; \":\\n\" &lt;&lt; X &lt;&lt; std::endl &lt;&lt; std::endl \/\/\u6253\u5370\u77e9\u9635\n#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} \/\/\u6253\u5370\u8c03\u8bd5\u4fe1\u606f\n\/\/ clang-format on\n\nusing std::placeholders::_1;  \/\/ \u5360\u4f4d\u7b26\n\n\/\/\/ @brief \u6784\u9020\u51fd\u6570\n\/\/\/ @param node_name \u8282\u70b9\u540d\u79f0\n\/\/\/ @param node_options \u8282\u70b9\u9009\u9879\nEKFLocalizer::EKFLocalizer(const std::string&amp; node_name, const rclcpp::NodeOptions&amp; node_options)\n    : rclcpp::Node(node_name, node_options),\n      warning_(std::make_shared&lt;Warning&gt;(\n          this)),  \/\/ \u8b66\u544a\u7ba1\u7406\u5668(warning_)\u3001\u53c2\u6570\u914d\u7f6e\u5bf9\u8c61(params_)\u3001EKF\u7684\u66f4\u65b0\u9891\u7387(ekf_rate_)\u3001\u65f6\u95f4\u6b65\u957f(ekf_dt_)\u4ee5\u53ca\u59ff\u6001\u548c\u901f\u5ea6\u961f\u5217\n      params_(this),\n      ekf_rate_(params_.ekf_rate),\n      ekf_dt_(params_.ekf_dt),\n      pose_queue_(params_.pose_smoothing_steps),\n      twist_queue_(params_.twist_smoothing_steps) {\n    \/* convert to continuous to discrete *\/\n    proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);\n    proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);\n    proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);\n\n    is_activated_ = false;\n\n    \/\/ ROS\u7cfb\u7edf\u7684\u521d\u59cb\u5316\n    \/* initialize ros system *\/\n    timer_control_ = rclcpp::create_timer(this,\n                                          get_clock(),\n                                          rclcpp::Duration::from_seconds(ekf_dt_),\n                                          std::bind(&amp;EKFLocalizer::timerCallback, this));\n\n    timer_tf_ = rclcpp::create_timer(this,\n                                     get_clock(),\n                                     rclcpp::Rate(params_.tf_rate_).period(),\n                                     std::bind(&amp;EKFLocalizer::timerTFCallback, this));\n\n    pub_pose_ = create_publisher&lt;geometry_msgs::msg::PoseStamped&gt;(\"ekf_pose\", 1);\n    pub_pose_cov_ =\n        create_publisher&lt;geometry_msgs::msg::PoseWithCovarianceStamped&gt;(\"ekf_pose_with_covariance\",\n                                                                        1);\n    pub_odom_ = create_publisher&lt;nav_msgs::msg::Odometry&gt;(\"ekf_odom\", 1);\n    pub_twist_ = create_publisher&lt;geometry_msgs::msg::TwistStamped&gt;(\"ekf_twist\", 1);\n    pub_twist_cov_ = create_publisher&lt;geometry_msgs::msg::TwistWithCovarianceStamped&gt;(\n        \"ekf_twist_with_covariance\",\n        1);\n    pub_yaw_bias_ =\n        create_publisher&lt;tier4_debug_msgs::msg::Float64Stamped&gt;(\"estimated_yaw_bias\", 1);\n    pub_biased_pose_ = create_publisher&lt;geometry_msgs::msg::PoseStamped&gt;(\"ekf_biased_pose\", 1);\n    pub_biased_pose_cov_ = create_publisher&lt;geometry_msgs::msg::PoseWithCovarianceStamped&gt;(\n        \"ekf_biased_pose_with_covariance\",\n        1);\n    pub_diag_ = this-&gt;create_publisher&lt;diagnostic_msgs::msg::DiagnosticArray&gt;(\"\/diagnostics\", 10);\n    sub_initialpose_ = create_subscription&lt;geometry_msgs::msg::PoseWithCovarianceStamped&gt;(\n        \"initialpose\",\n        1,\n        std::bind(&amp;EKFLocalizer::callbackInitialPose, this, _1));\n    sub_pose_with_cov_ = create_subscription&lt;geometry_msgs::msg::PoseWithCovarianceStamped&gt;(\n        \"in_pose_with_covariance\",\n        1,\n        std::bind(&amp;EKFLocalizer::callbackPoseWithCovariance, this, _1));\n    sub_twist_with_cov_ = create_subscription&lt;geometry_msgs::msg::TwistWithCovarianceStamped&gt;(\n        \"in_twist_with_covariance\",\n        1,\n        std::bind(&amp;EKFLocalizer::callbackTwistWithCovariance, this, _1));\n    service_trigger_node_ =\n        create_service&lt;std_srvs::srv::SetBool&gt;(\"trigger_node_srv\",\n                                               std::bind(&amp;EKFLocalizer::serviceTriggerNode,\n                                                         this,\n                                                         std::placeholders::_1,\n                                                         std::placeholders::_2),\n                                               rclcpp::ServicesQoS().get_rmw_qos_profile());\n\n    tf_br_ = std::make_shared&lt;tf2_ros::TransformBroadcaster&gt;(\n        std::shared_ptr&lt;rclcpp::Node&gt;(this, [](auto) {}));\n\n    ekf_module_ = std::make_unique&lt;EKFModule&gt;(warning_, params_);\n    logger_configure_ = std::make_unique&lt;tier4_autoware_utils::LoggerLevelConfigure&gt;(this);\n\n    z_filter_.set_proc_dev(1.0);\n    roll_filter_.set_proc_dev(0.01);\n    pitch_filter_.set_proc_dev(0.01);\n}\n\n\/\/\/ @brief \u66f4\u65b0\u9884\u6d4b\u9891\u7387,\u6839\u636e\u4e0a\u6b21\u9884\u6d4b\u65f6\u95f4\u6765\u66f4\u65b0EKF\u7684\u9884\u6d4b\u9891\u7387\nvoid EKFLocalizer::updatePredictFrequency() {\n    if (last_predict_time_) {\n        \/\/ \u5982\u679c\u68c0\u6d4b\u5230\u65f6\u95f4\u5411\u540e\u8df3\u8dc3\uff0c\u5219\u53d1\u51fa\u8b66\u544a\n        if (get_clock()-&gt;now() &lt; *last_predict_time_) {\n            warning_-&gt;warn(\"Detected jump back in time\");\n        } else {\n            \/\/ \u6839\u636e\u65f6\u95f4\u95f4\u9694\u8ba1\u7b97\u65b0\u7684EKF\u9891\u7387\u548c\u65f6\u95f4\u6b65\u957f\uff0c\u5e76\u66f4\u65b0\u79bb\u6563\u8fc7\u7a0b\u566a\u58f0\u65b9\u5dee\u3002\n            ekf_rate_ = 1.0 \/ (get_clock()-&gt;now() - *last_predict_time_).seconds();\n            DEBUG_INFO(get_logger(), \"[EKF] update ekf_rate_ to %f hz\", ekf_rate_);\n            ekf_dt_ = 1.0 \/ std::max(ekf_rate_, 0.1);\n\n            \/* Update discrete proc_cov*\/\n            proc_cov_vx_d_ = std::pow(params_.proc_stddev_vx_c * ekf_dt_, 2.0);\n            proc_cov_wz_d_ = std::pow(params_.proc_stddev_wz_c * ekf_dt_, 2.0);\n            proc_cov_yaw_d_ = std::pow(params_.proc_stddev_yaw_c * ekf_dt_, 2.0);\n        }\n    }\n    last_predict_time_ = std::make_shared&lt;const rclcpp::Time&gt;(get_clock()-&gt;now());\n}\n\n\/\/\/ @brief\n\/\/\/ \u6267\u884c\u5b9a\u65f6\u56de\u8c03\u64cd\u4f5c\uff0c\u7528\u4e8e\u66f4\u65b0\u9884\u6d4b\u9891\u7387\u3001\u6267\u884cEKF\u6a21\u578b\u7684\u9884\u6d4b\u548c\u59ff\u6001\u4e0e\u901f\u5ea6\u7684\u6d4b\u91cf\u66f4\u65b0\uff0c\u5e76\u53d1\u5e03EKF\u7ed3\u679c\u3002\nvoid EKFLocalizer::timerCallback() {\n    \/\/ \u68c0\u67e5\u8282\u70b9\u662f\u5426\u5df2\u6fc0\u6d3b\uff0c\u82e5\u672a\u6fc0\u6d3b\u5219\u53d1\u51fa\u8b66\u544a\u5e76\u8fd4\u56de\n    if (!is_activated_) {\n        warning_-&gt;warnThrottle(\n            \"The node is not activated. Provide initial pose to pose_initializer\",\n            2000);\n        publishDiagnostics();\n        return;\n    }\n\n    DEBUG_INFO(get_logger(), \"========================= timer called =========================\");\n\n    \/* update predict frequency with measured timer rate *\/\n    \/\/ \u66f4\u65b0\u9884\u6d4b\u9891\u7387\n    updatePredictFrequency();\n\n    \/* predict model in EKF *\/\n    \/\/ \u6267\u884cEKF\u6a21\u578b\u7684\u9884\u6d4b\n    stop_watch_.tic();\n    DEBUG_INFO(get_logger(),\n               \"------------------------- start prediction -------------------------\");\n    ekf_module_-&gt;predictWithDelay(ekf_dt_);\n    DEBUG_INFO(get_logger(), \"[EKF] predictKinematicsModel calc time = %f [ms]\", stop_watch_.toc());\n    DEBUG_INFO(get_logger(),\n               \"------------------------- end prediction -------------------------\\n\");\n\n    \/* pose measurement update *\/\n    pose_diag_info_.queue_size = pose_queue_.size();\n    pose_diag_info_.is_passed_delay_gate = true;\n    pose_diag_info_.delay_time = 0.0;\n    pose_diag_info_.delay_time_threshold = 0.0;\n    pose_diag_info_.is_passed_mahalanobis_gate = true;\n    pose_diag_info_.mahalanobis_distance = 0.0;\n\n    bool pose_is_updated = false;\n\n    if (!pose_queue_.empty()) {\n        DEBUG_INFO(get_logger(), \"------------------------- start Pose -------------------------\");\n        stop_watch_.tic();\n\n        \/\/ save the initial size because the queue size can change in the loop\n        const auto t_curr = this-&gt;now();\n        const size_t n = pose_queue_.size();\n        for (size_t i = 0; i &lt; n; ++i) {\n            const auto pose = pose_queue_.pop_increment_age();\n            \/\/ \u5bf9\u59ff\u6001\u548c\u901f\u5ea6\u8fdb\u884c\u6d4b\u91cf\u66f4\u65b0,\u5e76\u8fd4\u56de\u662f\u5426\u66f4\u65b0\u6210\u529f\u7684\u6807\u5fd7\n            bool is_updated =\n                ekf_module_-&gt;measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_);\n            if (is_updated) {\n                pose_is_updated = true;\n\n                \/\/ Update Simple 1D filter with considering change of z value due to measurement\n                \/\/ pose\n                const double delay_time = (t_curr - pose-&gt;header.stamp).seconds() +\n                                          params_.pose_additional_delay;  \/\/ \u8ba1\u7b97\u5ef6\u8fdf\u65f6\u95f4\n                \/\/ \u6839\u636e\u6d4b\u91cf\u59ff\u6001\u7684\u65f6\u95f4\u6233\u548c\u5f53\u524d\u65f6\u95f4\u6233\u8ba1\u7b97\u5ef6\u8fdf\u65f6\u95f4\uff0c\u7136\u540e\u4f7f\u7528\u5ef6\u8fdf\u65f6\u95f4\u5bf9\u59ff\u6001\u8fdb\u884c\u8865\u507f\n                const auto pose_with_z_delay =\n                    ekf_module_-&gt;compensatePoseWithZDelay(*pose, delay_time);\n                updateSimple1DFilters(pose_with_z_delay,\n                                      params_.pose_smoothing_steps);  \/\/ \u66f4\u65b0\u6ee4\u6ce2\u5668\n            }\n        }\n        DEBUG_INFO(get_logger(),\n                   \"[EKF] measurementUpdatePose calc time = %f [ms]\",\n                   stop_watch_.toc());\n        DEBUG_INFO(get_logger(), \"------------------------- end Pose -------------------------\\n\");\n    }\n    pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1);\n\n    \/* twist measurement update *\/\n    twist_diag_info_.queue_size = twist_queue_.size();\n    twist_diag_info_.is_passed_delay_gate = true;\n    twist_diag_info_.delay_time = 0.0;\n    twist_diag_info_.delay_time_threshold = 0.0;\n    twist_diag_info_.is_passed_mahalanobis_gate = true;\n    twist_diag_info_.mahalanobis_distance = 0.0;\n\n    bool twist_is_updated = false;\n\n    if (!twist_queue_.empty()) {\n        DEBUG_INFO(get_logger(), \"------------------------- start Twist -------------------------\");\n        stop_watch_.tic();\n\n        \/\/ \u4fdd\u5b58\u521d\u59cb\u5927\u5c0f\uff0c\u56e0\u4e3a\u5faa\u73af\u4e2d\u961f\u5217\u5927\u5c0f\u53ef\u80fd\u4f1a\u6539\u53d8\n        const auto t_curr = this-&gt;now();\n        const size_t n = twist_queue_.size();\n        for (size_t i = 0; i &lt; n; ++i) {\n            const auto twist = twist_queue_.pop_increment_age();\n            bool is_updated = ekf_module_-&gt;measurementUpdateTwist(\n                *twist,\n                ekf_dt_,\n                t_curr,\n                twist_diag_info_);  \/\/ \u5bf9\u59ff\u6001\u548c\u901f\u5ea6\u8fdb\u884c\u6d4b\u91cf\u66f4\u65b0,\u5e76\u8fd4\u56de\u662f\u5426\u66f4\u65b0\u6210\u529f\u7684\u6807\u5fd7\n            if (is_updated) {\n                twist_is_updated = true;\n            }\n        }\n        DEBUG_INFO(get_logger(),\n                   \"[EKF] measurementUpdateTwist calc time = %f [ms]\",\n                   stop_watch_.toc());\n        DEBUG_INFO(get_logger(), \"------------------------- end Twist -------------------------\\n\");\n    }\n    twist_diag_info_.no_update_count =\n        twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1);  \/\/ \u66f4\u65b0\u6d4b\u91cf\u66f4\u65b0\u6b21\u6570\n\n    const double z = z_filter_.get_x();\n    const double roll = roll_filter_.get_x();\n    const double pitch = pitch_filter_.get_x();\n    \/\/ \u83b7\u53d6\u5f53\u524d\u7684\u59ff\u6001\u548c\u901f\u5ea6\u4fe1\u606f\uff0c\u5e76\u53d1\u5e03EKF\u7ed3\u679c\n    const geometry_msgs::msg::PoseStamped current_ekf_pose =\n        ekf_module_-&gt;getCurrentPose(this-&gt;now(), z, roll, pitch, false);\n    const geometry_msgs::msg::PoseStamped current_biased_ekf_pose =\n        ekf_module_-&gt;getCurrentPose(this-&gt;now(), z, roll, pitch, true);\n    const geometry_msgs::msg::TwistStamped current_ekf_twist =\n        ekf_module_-&gt;getCurrentTwist(this-&gt;now());\n\n    \/* publish ekf result *\/\n    publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist);\n    publishDiagnostics();\n}\n\n\/\/\/ @brief \u6267\u884cTF\u53d8\u6362\u7684\u5b9a\u65f6\u56de\u8c03\u64cd\u4f5c\uff0c\u7528\u4e8e\u83b7\u53d6\u5f53\u524d\u59ff\u6001\u4fe1\u606f\u5e76\u53d1\u9001TF\u53d8\u6362\nvoid EKFLocalizer::timerTFCallback() {\n    \/\/ \u68c0\u67e5\u8282\u70b9\u662f\u5426\u5df2\u6fc0\u6d3b\u6216\u8005\u59ff\u6001\u6846\u67b6ID\u662f\u5426\u4e3a\u7a7a\uff0c\u82e5\u662f\u5219\u76f4\u63a5\u8fd4\u56de\n    if (!is_activated_) {\n        return;\n    }\n\n    if (params_.pose_frame_id == \"\") {\n        return;\n    }\n\n    const double z = z_filter_.get_x();\n    const double roll = roll_filter_.get_x();\n    const double pitch = pitch_filter_.get_x();\n    \/\/ \u83b7\u53d6\u5f53\u524d\u59ff\u6001\u4fe1\u606f\uff0c\u5e76\u5c06\u5176\u8f6c\u6362\u4e3aTF\u6d88\u606f\u5e76\u53d1\u9001\n    geometry_msgs::msg::TransformStamped transform_stamped;\n    transform_stamped = tier4_autoware_utils::pose2transform(\n        ekf_module_-&gt;getCurrentPose(this-&gt;now(), z, roll, pitch, false),\n        \"base_link\");\n    transform_stamped.header.stamp = this-&gt;now();\n    tf_br_-&gt;sendTransform(transform_stamped);\n}\n\n\/\/\/ @brief \u4eceTF\u7f13\u5b58\u4e2d\u83b7\u53d6\u4e24\u4e2a\u5750\u6807\u7cfb\u4e4b\u95f4\u7684\u53d8\u6362\u5173\u7cfb\u3002\n\/\/\/ @param parent_frame \u7236\u5750\u6807\u7cfb\u540d\u79f0\n\/\/\/ @param child_frame  \u5b50\u5750\u6807\u7cfb\u540d\u79f0\n\/\/\/ @param transform   \u53d8\u6362\u5173\u7cfb\n\/\/\/ @return \nbool EKFLocalizer::getTransformFromTF(std::string parent_frame,\n                                      std::string child_frame,\n                                      geometry_msgs::msg::TransformStamped&amp; transform) {\n    tf2::BufferCore tf_buffer;\n    tf2_ros::TransformListener tf_listener(tf_buffer);\/\/\u521b\u5efaTF\u7f13\u5b58\u5bf9\u8c61\u548c\u76d1\u542c\u5668\n    rclcpp::sleep_for(std::chrono::milliseconds(100));\n\n    parent_frame = eraseLeadingSlash(parent_frame);\n    child_frame = eraseLeadingSlash(child_frame);\n\n    for (int i = 0; i &lt; 50; ++i) {\n        try {\n            transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero);\/\/\u5faa\u73af\u5c1d\u8bd550\u6b21\u83b7\u53d6\u7236\u5750\u6807\u7cfb\u5230\u5b50\u5750\u6807\u7cfb\u7684\u53d8\u6362\u5173\u7cfb\uff0c\u5982\u679c\u6210\u529f\u5219\u8fd4\u56detrue\n            return true;\n        } catch (tf2::TransformException&amp; ex) {\n            \/\/ \u5426\u5219\u6253\u5370\u8b66\u544a\u4fe1\u606f\uff0c\u7b49\u5f85100ms\u540e\u518d\u6b21\u5c1d\u8bd5\uff0c\u5171\u5faa\u73af50\u6b21\n            warning_-&gt;warn(ex.what());\n            rclcpp::sleep_for(std::chrono::milliseconds(100));\n        }\n    }\n    return false;\n}\n\n\/\/\/ @brief \u5904\u7406\u521d\u59cb\u59ff\u6001\u7684\u56de\u8c03\u51fd\u6570\uff0c\u7528\u4e8e\u521d\u59cb\u5316EKF\u6a21\u578b\n\/\/\/ @param initialpose \u521d\u59cb\u59ff\u6001\nvoid EKFLocalizer::callbackInitialPose(\n    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) {\n    geometry_msgs::msg::TransformStamped transform;\n    \/\/ \u8c03\u7528getTransformFromTF()\u51fd\u6570\u83b7\u53d6\u7236\u5750\u6807\u7cfb\u5230\u521d\u59cb\u59ff\u6001\u5750\u6807\u7cfb\u7684\u53d8\u6362\u5173\u7cfb\n    if (!getTransformFromTF(params_.pose_frame_id, initialpose-&gt;header.frame_id, transform)) {\n        RCLCPP_ERROR(get_logger(),\n                     \"[EKF] TF transform failed. parent = %s, child = %s\",\n                     params_.pose_frame_id.c_str(),\n                     initialpose-&gt;header.frame_id.c_str());\n    }\n    \/\/\u83b7\u53d6\u7684\u53d8\u6362\u5173\u7cfb\u548c\u521d\u59cb\u59ff\u6001\u4fe1\u606f\u521d\u59cb\u5316EKF\u6a21\u578b\n    ekf_module_-&gt;initialize(*initialpose, transform);\n    initSimple1DFilters(*initialpose);\n}\n\n\/\/\/ @brief \u5f53\u63a5\u6536\u5230\u5305\u542b\u59ff\u6001\u4e0e\u534f\u65b9\u5dee\u4fe1\u606f\u7684\u6d88\u606f\u65f6\uff0c\u5c06\u5176\u5b58\u5165\u59ff\u6001\u961f\u5217\u4e2d\n\/\/\/ @param msg \u5305\u542b\u59ff\u6001\u4e0e\u534f\u65b9\u5dee\u4fe1\u606f\u7684\u6307\u9488\nvoid EKFLocalizer::callbackPoseWithCovariance(\n    geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {\n    if (!is_activated_) {\n        return;\n    }\n    \/\/\u5c06\u6536\u5230\u7684\u6d88\u606f\u5b58\u5165\u59ff\u6001\u961f\u5217\u4e2d,\u5e76\u7b49\u5f85timer\u56de\u8c03\u51fd\u6570\u5904\u7406\n    pose_queue_.push(msg);\n}\n\n\/\/\/ @brief \u5f53\u63a5\u6536\u5230\u5305\u542b\u901f\u5ea6\u4e0e\u534f\u65b9\u5dee\u4fe1\u606f\u7684\u6d88\u606f\u65f6\uff0c\u6839\u636e\u901f\u5ea6\u5927\u5c0f\u5224\u65ad\u662f\u5426\u5ffd\u7565\u8be5\u6d88\u606f\uff0c\u5e76\u5728\u9700\u8981\u65f6\u4fee\u6539\u534f\u65b9\u5dee\u4fe1\u606f\u3002\n\/\/\/ @param msg \u5305\u542b\u901f\u5ea6\u4e0e\u534f\u65b9\u5dee\u4fe1\u606f\u7684\u6307\u9488\nvoid EKFLocalizer::callbackTwistWithCovariance(\n    geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {\n    \/\/ \u5ffd\u7565\u901f\u5ea6\u592a\u5c0f\u7684\u6d88\u606f\u3002\u6ce8\u610f\uff0c\u8fd9\u4e2a\u4e0d\u7b49\u5f0f\u4e0d\u80fd\u5305\u542b\u201c\u7b49\u4e8e\u201d\u3002\n    if (std::abs(msg-&gt;twist.twist.linear.x) &lt; params_.threshold_observable_velocity_mps) {\n        \/\/ \u6839\u636e\u901f\u5ea6\u5927\u5c0f\u5224\u65ad\u662f\u5426\u5ffd\u7565\u6d88\u606f\uff0c\u5e76\u5728\u9700\u8981\u65f6\u4fee\u6539\u534f\u65b9\u5dee\u4fe1\u606f\n        msg-&gt;twist.covariance[0 * 6 + 0] = 10000.0;\n    }\n    twist_queue_.push(msg);\n}\n\n\/\/\/ @brief \u53d1\u5e03\u5b9a\u4f4d\u7ed3\u679c\uff0c\u5305\u62ec\u59ff\u6001\u3001\u5e26\u504f\u7f6e\u7684\u59ff\u6001\u3001\u901f\u5ea6\u4ee5\u53ca\u76f8\u5e94\u7684\u534f\u65b9\u5dee\u4fe1\u606f\u7b49\n\/\/\/ @param current_ekf_pose \u5f53\u524d\u4f30\u8ba1\u7684\u59ff\u6001\n\/\/\/ @param current_biased_ekf_pose \u5f53\u524d\u4f30\u8ba1\u7684\u5e26\u504f\u7f6e\u7684\u59ff\u6001\n\/\/\/ @param current_ekf_twist \u5f53\u524d\u4f30\u8ba1\u7684\u901f\u5ea6\nvoid EKFLocalizer::publishEstimateResult(\n    const geometry_msgs::msg::PoseStamped&amp; current_ekf_pose,\n    const geometry_msgs::msg::PoseStamped&amp; current_biased_ekf_pose,\n    const geometry_msgs::msg::TwistStamped&amp; current_ekf_twist) {\n    rclcpp::Time current_time = this-&gt;now();\n\n    \/* publish latest pose *\/\n    pub_pose_-&gt;publish(current_ekf_pose);\n    pub_biased_pose_-&gt;publish(current_biased_ekf_pose);\n\n    \/* publish latest pose with covariance *\/\n    geometry_msgs::msg::PoseWithCovarianceStamped pose_cov;\n    pose_cov.header.stamp = current_time;\n    pose_cov.header.frame_id = current_ekf_pose.header.frame_id;\n    pose_cov.pose.pose = current_ekf_pose.pose;\n    pose_cov.pose.covariance = ekf_module_-&gt;getCurrentPoseCovariance();\n    pub_pose_cov_-&gt;publish(pose_cov);\n\n    geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;\n    biased_pose_cov.pose.pose = current_biased_ekf_pose.pose;\n    pub_biased_pose_cov_-&gt;publish(biased_pose_cov);\n\n    \/* publish latest twist *\/\n    pub_twist_-&gt;publish(current_ekf_twist);\n\n    \/* publish latest twist with covariance *\/\n    geometry_msgs::msg::TwistWithCovarianceStamped twist_cov;\n    twist_cov.header.stamp = current_time;\n    twist_cov.header.frame_id = current_ekf_twist.header.frame_id;\n    twist_cov.twist.twist = current_ekf_twist.twist;\n    twist_cov.twist.covariance = ekf_module_-&gt;getCurrentTwistCovariance();\n    pub_twist_cov_-&gt;publish(twist_cov);\n\n    \/* publish yaw bias *\/\n    tier4_debug_msgs::msg::Float64Stamped yawb;\n    yawb.stamp = current_time;\n    yawb.data = ekf_module_-&gt;getYawBias();\n    pub_yaw_bias_-&gt;publish(yawb);\n\n    \/* publish latest odometry *\/\n    nav_msgs::msg::Odometry odometry;\n    odometry.header.stamp = current_time;\n    odometry.header.frame_id = current_ekf_pose.header.frame_id;\n    odometry.child_frame_id = \"base_link\";\n    odometry.pose = pose_cov.pose;\n    odometry.twist = twist_cov.twist;\n    pub_odom_-&gt;publish(odometry);\n}\n\/\/\/ @brief \u53d1\u5e03\u8bca\u65ad\u4fe1\u606f\uff0c\u5305\u62ec\u6fc0\u6d3b\u72b6\u6001\u68c0\u67e5\u3001\u6d4b\u91cf\u66f4\u65b0\u60c5\u51b5\u3001\u961f\u5217\u5927\u5c0f\u3001\u5ef6\u8fdf\u60c5\u51b5\u3001Mahalanobis\u95e8\u9650\u7b49\nvoid EKFLocalizer::publishDiagnostics() {\n    std::vector&lt;diagnostic_msgs::msg::DiagnosticStatus&gt; diag_status_array;\n\n    diag_status_array.push_back(checkProcessActivated(is_activated_));\n\n    if (is_activated_) {\n        diag_status_array.push_back(\n            checkMeasurementUpdated(\"pose\",\n                                    pose_diag_info_.no_update_count,\n                                    params_.pose_no_update_count_threshold_warn,\n                                    params_.pose_no_update_count_threshold_error));\n        diag_status_array.push_back(checkMeasurementQueueSize(\"pose\", pose_diag_info_.queue_size));\n        diag_status_array.push_back(\n            checkMeasurementDelayGate(\"pose\",\n                                      pose_diag_info_.is_passed_delay_gate,\n                                      pose_diag_info_.delay_time,\n                                      pose_diag_info_.delay_time_threshold));\n        diag_status_array.push_back(\n            checkMeasurementMahalanobisGate(\"pose\",\n                                            pose_diag_info_.is_passed_mahalanobis_gate,\n                                            pose_diag_info_.mahalanobis_distance,\n                                            params_.pose_gate_dist));\n\n        diag_status_array.push_back(\n            checkMeasurementUpdated(\"twist\",\n                                    twist_diag_info_.no_update_count,\n                                    params_.twist_no_update_count_threshold_warn,\n                                    params_.twist_no_update_count_threshold_error));\n        diag_status_array.push_back(\n            checkMeasurementQueueSize(\"twist\", twist_diag_info_.queue_size));\n        diag_status_array.push_back(\n            checkMeasurementDelayGate(\"twist\",\n                                      twist_diag_info_.is_passed_delay_gate,\n                                      twist_diag_info_.delay_time,\n                                      twist_diag_info_.delay_time_threshold));\n        diag_status_array.push_back(\n            checkMeasurementMahalanobisGate(\"twist\",\n                                            twist_diag_info_.is_passed_mahalanobis_gate,\n                                            twist_diag_info_.mahalanobis_distance,\n                                            params_.twist_gate_dist));\n    }\n\n    diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;\n    diag_merged_status = mergeDiagnosticStatus(diag_status_array);\n    diag_merged_status.name = \"localization: \" + std::string(this-&gt;get_name());\n    diag_merged_status.hardware_id = this-&gt;get_name();\n\n    diagnostic_msgs::msg::DiagnosticArray diag_msg;\n    diag_msg.header.stamp = this-&gt;now();\n    diag_msg.status.push_back(diag_merged_status);\n    pub_diag_-&gt;publish(diag_msg);\n}\n\/\/\/ @brief \u66f4\u65b0\u4e00\u7ef4\u6ee4\u6ce2\u5668\n\/\/\/ @param pose \u4f4d\u7f6e\u548c\u65b9\u5411\u7684\u5e26\u534f\u65b9\u5dee\u7684\u59ff\u6001\u6d88\u606f\n\/\/\/ @param smoothing_step \u5e73\u6ed1\u6b65\u6570\nvoid EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped&amp; pose,\n                                         const size_t smoothing_step) {\n    \/\/ \u4ece\u59ff\u6001\u6d88\u606f\u4e2d\u63d0\u53d6\u9ad8\u5ea6z\u548c\u59ff\u6001\u7684\u6eda\u8f6c\u3001\u4fef\u4ef0\u89d2\n    double z = pose.pose.pose.position.z;\n\n    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);\n\n    \/\/\u6839\u636e\u59ff\u6001\u6d88\u606f\u7684\u534f\u65b9\u5dee\u77e9\u9635\u548c\u5e73\u6ed1\u6b65\u6570\u8ba1\u7b97\u9ad8\u5ea6z\u3001\u6eda\u8f6c\u3001\u4fef\u4ef0\u89d2\u7684\u504f\u5dee\n    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;\n    double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast&lt;double&gt;(smoothing_step);\n    double roll_dev =\n        pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast&lt;double&gt;(smoothing_step);\n    double pitch_dev =\n        pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast&lt;double&gt;(smoothing_step);\n\n    z_filter_.update(z, z_dev, pose.header.stamp);\n    roll_filter_.update(rpy.x, roll_dev, pose.header.stamp);\n    pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp);\n}\n\n\/\/\/ @brief \u521d\u59cb\u5316\u4e00\u7ef4\u6ee4\u6ce2\u5668\n\/\/\/ @param pose \u4f4d\u7f6e\u548c\u65b9\u5411\u7684\u5e26\u534f\u65b9\u5dee\u7684\u59ff\u6001\u6d88\u606f\nvoid EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped&amp; pose) {\n    \/\/\u4ece\u59ff\u6001\u6d88\u606f\u4e2d\u63d0\u53d6\u9ad8\u5ea6z\u548c\u59ff\u6001\u7684\u6eda\u8f6c\u3001\u4fef\u4ef0\u89d2\n    double z = pose.pose.pose.position.z;\n\n    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);\n\n    \/\/ \u6839\u636e\u59ff\u6001\u6d88\u606f\u7684\u534f\u65b9\u5dee\u77e9\u9635\u8ba1\u7b97\u9ad8\u5ea6z\u3001\u6eda\u8f6c\u3001\u4fef\u4ef0\u89d2\u7684\u504f\u5dee\n    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;\n    double z_dev = pose.pose.covariance[COV_IDX::Z_Z];\n    double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL];\n    double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH];\n\n    z_filter_.init(z, z_dev, pose.header.stamp);\n    roll_filter_.init(rpy.x, roll_dev, pose.header.stamp);\n    pitch_filter_.init(rpy.y, pitch_dev, pose.header.stamp);\n}\n\n\/\/\/ @brief \u89e6\u53d1\u8282\u70b9\u670d\u52a1\n\/\/\/ @param req \u8bf7\u6c42\n\/\/\/ @param res \u54cd\u5e94\nvoid EKFLocalizer::serviceTriggerNode(const std_srvs::srv::SetBool::Request::SharedPtr req,\n                                      std_srvs::srv::SetBool::Response::SharedPtr res) {\n    if (req-&gt;data) {\n        pose_queue_.clear();\n        twist_queue_.clear();\n        is_activated_ = true;\n    } else {\n        is_activated_ = false;\n    }\n    res-&gt;success = true;\n    return;\n}<\/code><\/pre>\n\n\n\n
    2.4 ekf_module.cpp<\/p>\n\n\n\n
    \/\/ clang-format off\n#define DEBUG_PRINT_MAT(X) {\\\n  if (params_.show_debug_info) {std::cout &lt;&lt; #X &lt;&lt; \": \" &lt;&lt; X &lt;&lt; std::endl;}\\\n}\/\/ \u8c03\u8bd5\u6a21\u5f0f\u4e0b\u6253\u5370\u77e9\u9635\u7684\u503c\n\/\/ clang-format on\n\n\/\/\/ @brief \u521d\u59cb\u5316 EKF \u6a21\u5757\n\/\/\/ @param warning \u8b66\u544a\u6a21\u5757\n\/\/\/ @param params \u8d85\u53c2\u6570\nEKFModule::EKFModule(std::shared_ptr&lt;Warning&gt; warning, const HyperParameters params)\n    : warning_(std::move(warning)),\n      dim_x_(6),  \/\/ x, y, yaw, yaw_bias, vx, wz\n      params_(params) {\n    Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1);\n    Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15;  \/\/ for x &amp; y\n    P(IDX::YAW, IDX::YAW) = 50.0;                                            \/\/ for yaw\n    if (params_.enable_yaw_bias_estimation) {\n        P(IDX::YAWB, IDX::YAWB) = 50.0;  \/\/ for yaw bias\n    }\n    P(IDX::VX, IDX::VX) = 1000.0;  \/\/ for vx\n    P(IDX::WZ, IDX::WZ) = 50.0;    \/\/ for wz\n\n    kalman_filter_.init(\n        X,\n        P,\n        params_\n            .extend_state_step);  \/\/ \u521d\u59cb\u5316\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668,kalman_filter_\u7684\u4ee3\u7801\u5728common\/kalman_filter\/src\/time_delay_kalman_filter.cpp\u4e2d\n}\n\n\/\/\/ @brief \u521d\u59cb\u5316\u51fd\u6570\uff0c\u7528\u4e8e\u8bbe\u7f6e\u521d\u59cb\u59ff\u6001\u548c\u534f\u65b9\u5dee\u77e9\u9635\n\/\/\/ @param initial_pose \u521d\u59cb\u59ff\u6001\n\/\/\/ @param transform \u521d\u59cb\u59ff\u6001\u7684\u53d8\u6362\nvoid EKFModule::initialize(const PoseWithCovariance&amp; initial_pose,\n                           const geometry_msgs::msg::TransformStamped&amp; transform) {\n    Eigen::MatrixXd X(dim_x_, 1);\n    Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);\n\n    \/\/ \u8bbe\u7f6e\u72b6\u6001\u5411\u91cf X \u548c\u72b6\u6001\u534f\u65b9\u5dee\u77e9\u9635 P\uff0c\u7136\u540e\u8c03\u7528 kalman_filter_.init \u8fdb\u884c\u521d\u59cb\u5316\u3002\n    X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x;\n    X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y;\n    X(IDX::YAW) =\n        tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation);\n    X(IDX::YAWB) = 0.0;\n    X(IDX::VX) = 0.0;\n    X(IDX::WZ) = 0.0;\n\n    using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;\n    P(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X];\n    P(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y];\n    P(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW];\n\n    if (params_.enable_yaw_bias_estimation) {\n        P(IDX::YAWB, IDX::YAWB) = 0.0001;\n    }\n    P(IDX::VX, IDX::VX) = 0.01;\n    P(IDX::WZ, IDX::WZ) = 0.01;\n\n    kalman_filter_.init(X, P, params_.extend_state_step);\n}\n\n\/\/\/ @brief \u83b7\u53d6\u5f53\u524d\u59ff\u6001\u7684\u51fd\u6570\u3002\u4ece EKF \u4e2d\u83b7\u53d6\u5f53\u524d\u7684\u4f4d\u7f6e\u548c\u59ff\u6001\u4fe1\u606f\uff0c\u5e76\u6839\u636e\u9700\u8981\u8ba1\u7b97\u504f\u822a\u89d2\n\/\/\/ @param current_time \u5f53\u524d\u65f6\u95f4\n\/\/\/ @param z \u9ad8\u5ea6\n\/\/\/ @param roll \u6eda\u52a8\u89d2\n\/\/\/ @param pitch \u4fef\u4ef0\u89d2\n\/\/\/ @param get_biased_yaw \u662f\u5426\u83b7\u53d6\u504f\u822a\u89d2\n\/\/\/ @return\ngeometry_msgs::msg::PoseStamped EKFModule::getCurrentPose(const rclcpp::Time&amp; current_time,\n                                                          const double z,\n                                                          const double roll,\n                                                          const double pitch,\n                                                          bool get_biased_yaw) const {\n    \/\/ \u4ece EKF \u4e2d\u83b7\u53d6\u5f53\u524d\u7684x\u3001y\u3001\u504f\u822a\u89d2\u548c\u504f\u822a\u89d2\u504f\u5dee\n    const double x = kalman_filter_.getXelement(IDX::X);\n    const double y = kalman_filter_.getXelement(IDX::Y);\n    const double biased_yaw = kalman_filter_.getXelement(IDX::YAW);\n    const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB);\n    const double yaw = biased_yaw + yaw_bias;\n    Pose current_ekf_pose;\n    current_ekf_pose.header.frame_id = params_.pose_frame_id;\n    current_ekf_pose.header.stamp = current_time;\n    current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z);\n    \/\/ \u6839\u636e\u9700\u8981\u8ba1\u7b97\u504f\u822a\u89d2\n    if (get_biased_yaw) {\n        current_ekf_pose.pose.orientation =\n            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw);\n    } else {\n        current_ekf_pose.pose.orientation =\n            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);\n    }\n    return current_ekf_pose;\n}\n\n\/\/\/ @brief \u83b7\u53d6\u5f53\u524d\u901f\u5ea6\u7684\u51fd\u6570\n\/\/\/ @param current_time \u5f53\u524d\u65f6\u95f4\n\/\/\/ @return\ngeometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(\n    const rclcpp::Time&amp; current_time) const {\n    \/\/ \u4ece\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u4e2d\u83b7\u53d6\u5f53\u524d\u7684\u901f\u5ea6\u4fe1\u606f\n    const double vx = kalman_filter_.getXelement(IDX::VX);\n    const double wz = kalman_filter_.getXelement(IDX::WZ);\n\n    Twist current_ekf_twist;\n    current_ekf_twist.header.frame_id = \"base_link\";\n    current_ekf_twist.header.stamp = current_time;\n    current_ekf_twist.twist.linear.x = vx;\n    current_ekf_twist.twist.angular.z = wz;\n    return current_ekf_twist;\n}\n\n\/\/\/ @brief \u83b7\u53d6\u5f53\u524d\u59ff\u6001\u7684\u534f\u65b9\u5dee\u77e9\u9635\n\/\/\/ @return\nstd::array&lt;double, 36&gt; EKFModule::getCurrentPoseCovariance() const {\n    \/\/ \u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u4e2d\u6700\u65b0\u7684\u534f\u65b9\u5dee\u77e9\u9635\u8f6c\u6362\u6210\u6570\u7ec4\u5f62\u5f0f\u8fd4\u56de\n    return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP());\n}\n\n\/\/\/ @brief \u83b7\u53d6\u5f53\u524d\u901f\u5ea6\u7684\u534f\u65b9\u5dee\u77e9\u9635\n\/\/\/ @return\nstd::array&lt;double, 36&gt; EKFModule::getCurrentTwistCovariance() const {\n    \/\/ \u8c03\u7528 ekfCovarianceToTwistMessageCovariance\n    \/\/ \u51fd\u6570\u5c06\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u4e2d\u6700\u65b0\u7684\u901f\u5ea6\u534f\u65b9\u5dee\u77e9\u9635\u8f6c\u6362\u6210\u6570\u7ec4\u5f62\u5f0f\u8fd4\u56de\u3002\n    return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP());\n}\n\/\/\/ @brief \u83b7\u53d6\u504f\u822a\u89d2\u7684\u504f\u5dee\n\/\/\/ @return\ndouble EKFModule::getYawBias() const {\n    \/\/ \u4ece\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u4e2d\u83b7\u53d6\u6700\u65b0\u7684\u504f\u822a\u89d2\u504f\u5dee\u5e76\u8fd4\u56de\n    return kalman_filter_.getLatestX()(\n        IDX::YAWB);  \/\/ kalman_filter_\u7684\u4ee3\u7801\u5728common\/kalman_filter\/src\/time_delay_kalman_filter.cpp\u4e2d\n}\n\n\/\/\/ @brief \u6839\u636e\u5ef6\u8fdf\u9884\u6d4b\u4e0b\u4e00\u4e2a\u72b6\u6001\n\/\/\/ @param dt \u65f6\u95f4\u95f4\u9694\nvoid EKFModule::predictWithDelay(const double dt) {\n    const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();\n    const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();\n\n    const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0);\n    const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0);\n    const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0);\n\n    const Vector6d X_next = predictNextState(X_curr, dt);        \/\/ \u9884\u6d4b\u4e0b\u4e00\u4e2a\u72b6\u6001\n    const Matrix6d A = createStateTransitionMatrix(X_curr, dt);  \/\/ \u521b\u5efa\u72b6\u6001\u8f6c\u79fb\u77e9\u9635\n    const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d,\n                                              proc_cov_vx_d,\n                                              proc_cov_wz_d);  \/\/ \u521b\u5efa\u8fc7\u7a0b\u566a\u58f0\u534f\u65b9\u5dee\u77e9\u9635\n    kalman_filter_.predictWithDelay(\n        X_next,\n        A,\n        Q);  \/\/ \u9884\u6d4b\u4e0b\u4e00\u4e2a\u72b6\u6001,kalman_filter_\u7684\u4ee3\u7801\u5728common\/kalman_filter\/src\/time_delay_kalman_filter.cpp\u4e2d\n}\n\n\/\/\/ @brief \u8fd9\u4e2a\u51fd\u6570\u7528\u4e8e\u5904\u7406\u59ff\u6001\u6d4b\u91cf\u7684\u66f4\u65b0\n\/\/\/ @param pose \u5f53\u524d\u59ff\u6001\u6d4b\u91cf\u503c\u53ca\u5176\u534f\u65b9\u5dee\u77e9\u9635\n\/\/\/ @param dt \u65f6\u95f4\u95f4\u9694\n\/\/\/ @param t_curr \u5f53\u524d\u65f6\u95f4\n\/\/\/ @param pose_diag_info \u7528\u4e8e\u8bb0\u5f55\u59ff\u6001\u6d4b\u91cf\u66f4\u65b0\u8bca\u65ad\u4fe1\u606f\u7684\u7ed3\u6784\u4f53\n\/\/\/ @return\nbool EKFModule::measurementUpdatePose(const PoseWithCovariance&amp; pose,\n                                      const double dt,\n                                      const rclcpp::Time&amp; t_curr,\n                                      EKFDiagnosticInfo&amp; pose_diag_info) {\n    \/\/ \u51fd\u6570\u4f1a\u68c0\u67e5pose\u7684\u5750\u6807\u7cfb\u662f\u5426\u4e0e\u8bbe\u5b9a\u7684\u59ff\u6001\u5750\u6807\u7cfb\u4e00\u81f4\n    if (pose.header.frame_id != params_.pose_frame_id) {\n        warning_-&gt;warnThrottle(\n            fmt::format(\"pose frame_id is %s, but pose_frame is set as %s. They must be same.\",\n                        pose.header.frame_id.c_str(),\n                        params_.pose_frame_id.c_str()),\n            2000);\n    }\n    \/\/ \u4ece\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u4e2d\u83b7\u53d6\u5f53\u524d\u72b6\u6001\u4f30\u8ba1\u503c\n    const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();\n    DEBUG_PRINT_MAT(X_curr.transpose());\n\n    constexpr int dim_y = 3;  \/\/ pos_x, pos_y, yaw, depending on Pose output\n\n    \/* Calculate delay step *\/\n    \/\/ \u8ba1\u7b97\u59ff\u6001\u6d4b\u91cf\u7684\u5ef6\u8fdf\u6b65\u6570\uff0c\u5e76\u66f4\u65b0\u5ef6\u8fdf\u65f6\u95f4\u76f8\u5173\u7684\u8bca\u65ad\u4fe1\u606f\n    double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay;\n    if (delay_time &lt; 0.0) {\n        warning_-&gt;warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000);\n    }\n\n    delay_time = std::max(delay_time, 0.0);\n\n    const int delay_step = std::roundf(delay_time \/ dt);\n\n    pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time);\n    pose_diag_info.delay_time_threshold = params_.extend_state_step * dt;\n    if (delay_step &gt;= params_.extend_state_step) {\n        pose_diag_info.is_passed_delay_gate = false;\n        warning_-&gt;warnThrottle(\n            poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt),\n            2000);\n        return false;\n    }\n\n    \/* Set yaw *\/\n    \/\/ \u4ece\u59ff\u6001\u6d4b\u91cf\u4e2d\u63d0\u53d6\u59ff\u6001\u4fe1\u606f\uff0c\u5e76\u8fdb\u884c\u4e00\u4e9b\u5f02\u5e38\u503c\u68c0\u6d4b\u548c\u8b66\u544a\u8f93\u51fa\n    double yaw = tf2::getYaw(pose.pose.pose.orientation);\n    const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW);\n    const double yaw_error = normalizeYaw(yaw - ekf_yaw);  \/\/ normalize the error not to exceed 2 pi\n    yaw = yaw_error + ekf_yaw;\n\n    \/* Set measurement matrix *\/\n    Eigen::MatrixXd y(dim_y, 1);\n    y &lt;&lt; pose.pose.pose.position.x, pose.pose.pose.position.y, yaw;\n\n    if (hasNan(y) || hasInf(y)) {\n        warning_-&gt;warn(\n            \"[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose \"\n            \"message.\");\n        return false;\n    }\n\n    \/* Gate *\/\n    const Eigen::Vector3d y_ekf(kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X),\n                                kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y),\n                                ekf_yaw);\n    const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();\n    const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y);\n    \/\/ \u8ba1\u7b97\u9a6c\u6c0f\u8ddd\u79bb\uff0c\u5e76\u8fdb\u884c\u95e8\u9650\u5224\u65ad\uff0c\u82e5\u8d85\u51fa\u95e8\u9650\u5219\u8f93\u51fa\u8b66\u544a\u4fe1\u606f\u3002\n    const double distance = mahalanobis(y_ekf, y, P_y);\n    pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance);\n    if (distance &gt; params_.pose_gate_dist) {\n        pose_diag_info.is_passed_mahalanobis_gate = false;\n        warning_-&gt;warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000);\n        warning_-&gt;warnThrottle(\"Ignore the measurement data.\", 2000);\n        return false;\n    }\n\n    DEBUG_PRINT_MAT(y.transpose());\n    DEBUG_PRINT_MAT(y_ekf.transpose());\n    DEBUG_PRINT_MAT((y - y_ekf).transpose());\n\n    \/\/ \u6784\u9020\u59ff\u6001\u6d4b\u91cf\u7684\u6d4b\u91cf\u77e9\u9635C\u548c\u534f\u65b9\u5dee\u77e9\u9635R\u3002\n    const Eigen::Matrix&lt;double, 3, 6&gt; C = poseMeasurementMatrix();\n    const Eigen::Matrix3d R =\n        poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps);\n\n    \/\/ \u8c03\u7528\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u7684updateWithDelay\u51fd\u6570\u8fdb\u884c\u72b6\u6001\u66f4\u65b0\n    kalman_filter_.updateWithDelay(y, C, R, delay_step);\n\n    \/\/ debug\n    const Eigen::MatrixXd X_result = kalman_filter_.getLatestX();\n    DEBUG_PRINT_MAT(X_result.transpose());\n    DEBUG_PRINT_MAT((X_result - X_curr).transpose());\n\n    return true;\n}\n\n\/\/\/ @brief \u8fd9\u4e2a\u51fd\u6570\u7528\u4e8e\u8fdb\u884c\u59ff\u6001\u6d4b\u91cf\u7684\u9ad8\u5ea6\u5ef6\u8fdf\u8865\u507f\n\/\/\/ @param pose \u5f53\u524d\u59ff\u6001\u6d4b\u91cf\u503c\n\/\/\/ @param delay_time \u5ef6\u8fdf\u65f6\u95f4\n\/\/\/ @return\ngeometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay(\n    const PoseWithCovariance&amp; pose,\n    const double delay_time) {\n    const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);\n    const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time *\n                            std::sin(-rpy.y);  \/\/ \u51fd\u6570\u4f1a\u6839\u636e\u59ff\u6001\u7684\u65b9\u5411\u89d2\u8ba1\u7b97\u51fa\u9ad8\u5ea6\u5ef6\u8fdf\u7684\u8865\u507f\u503c\n    PoseWithCovariance pose_with_z_delay;\n    pose_with_z_delay = pose;\n    pose_with_z_delay.pose.pose.position.z +=\n        dz_delay;  \/\/ \u59ff\u6001\u7684\u9ad8\u5ea6\u503c\u8fdb\u884c\u5ef6\u8fdf\u8865\u507f\uff0c\u5e76\u8fd4\u56de\u8865\u507f\u540e\u7684\u59ff\u6001\u6d4b\u91cf\u503c\n    return pose_with_z_delay;\n}\n\n\/\/\/ @brief \u8fd9\u4e2a\u51fd\u6570\u7528\u4e8e\u5904\u7406\u901f\u5ea6\u6d4b\u91cf\u7684\u66f4\u65b0\n\/\/\/ @param twist \u5f53\u524d\u901f\u5ea6\u6d4b\u91cf\u503c\u53ca\u5176\u534f\u65b9\u5dee\u77e9\u9635\n\/\/\/ @param dt \u65f6\u95f4\u95f4\u9694\n\/\/\/ @param t_curr \u5f53\u524d\u65f6\u95f4\n\/\/\/ @param twist_diag_info \u7528\u4e8e\u8bb0\u5f55\u901f\u5ea6\u6d4b\u91cf\u66f4\u65b0\u8bca\u65ad\u4fe1\u606f\u7684\u7ed3\u6784\u4f53\n\/\/\/ @return\nbool EKFModule::measurementUpdateTwist(const TwistWithCovariance&amp; twist,\n                                       const double dt,\n                                       const rclcpp::Time&amp; t_curr,\n                                       EKFDiagnosticInfo&amp; twist_diag_info) {\n    \/\/ \u51fd\u6570\u4f1a\u68c0\u67e5\u901f\u5ea6\u6d4b\u91cf\u7684\u5750\u6807\u7cfb\u662f\u5426\u4e3a\"base_link\"\n    if (twist.header.frame_id != \"base_link\") {\n        warning_-&gt;warnThrottle(\"twist frame_id must be base_link\", 2000);\n    }\n\n    \/\/ \u4ece\u5361\u5c14\u66fc\u6ee4\u6ce2\u5668\u4e2d\u83b7\u53d6\u5f53\u524d\u72b6\u6001\u4f30\u8ba1\u503c\uff0c\u5e76\u8fdb\u884c\u4e00\u4e9b\u8c03\u8bd5\u4fe1\u606f\u7684\u6253\u5370\n    const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX();\n    DEBUG_PRINT_MAT(X_curr.transpose());\n\n    constexpr int dim_y = 2;  \/\/ vx, wz\n\n    \/* Calculate delay step *\/\n    \/\/ \u8ba1\u7b97\u901f\u5ea6\u6d4b\u91cf\u7684\u5ef6\u8fdf\u65f6\u95f4\n    double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay;\n    if (delay_time &lt; 0.0) {\n        warning_-&gt;warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000);\n    }\n    delay_time = std::max(delay_time, 0.0);\n\n    const int delay_step = std::roundf(delay_time \/ dt);\n\n    twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time);\n    twist_diag_info.delay_time_threshold = params_.extend_state_step * dt;\n    if (delay_step &gt;= params_.extend_state_step) {\n        twist_diag_info.is_passed_delay_gate = false;\n        warning_-&gt;warnThrottle(\n            twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt),\n            2000);\n        return false;\n    }\n\n    \/* Set measurement matrix *\/\n    Eigen::MatrixXd y(dim_y, 1);\n    y &lt;&lt; twist.twist.twist.linear.x, twist.twist.twist.angular.z;\n\n    if (hasNan(y) || hasInf(y)) {\n        warning_-&gt;warn(\n            \"[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist \"\n            \"message.\");\n        return false;\n    }\n    \/\/ \u4ece\u901f\u5ea6\u6d4b\u91cf\u4e2d\u63d0\u53d6\u901f\u5ea6\u4fe1\u606f\n    const Eigen::Vector2d y_ekf(kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX),\n                                kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ));\n\n    const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();\n    const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y);\n\n    \/\/ \u8ba1\u7b97\u9a6c\u6c0f\u8ddd\u79bb\uff0c\u5e76\u8fdb\u884c\u95e8\u9650\u5224\u65ad\uff0c\u82e5\u8d85\u51fa\u95e8\u9650\u5219\u8f93\u51fa\u8b66\u544a\u4fe1\u606f\n    const double distance = mahalanobis(y_ekf, y, P_y);\n    twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance);\n    if (distance &gt; params_.twist_gate_dist) {\n        twist_diag_info.is_passed_mahalanobis_gate = false;\n        warning_-&gt;warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000);\n        warning_-&gt;warnThrottle(\"Ignore the measurement data.\", 2000);\n        return false;\n    }\n\n    DEBUG_PRINT_MAT(y.transpose());\n    DEBUG_PRINT_MAT(y_ekf.transpose());\n    DEBUG_PRINT_MAT((y - y_ekf).transpose());\n\n    \/\/ \u6784\u9020\u901f\u5ea6\u6d4b\u91cf\u7684\u6d4b\u91cf\u77e9\u9635C\u548c\u534f\u65b9\u5dee\u77e9\u9635R\n    const Eigen::Matrix&lt;double, 2, 6&gt; C = twistMeasurementMatrix();\n    const Eigen::Matrix2d R =\n        twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps);\n\n    kalman_filter_.updateWithDelay(y, C, R, delay_step);\n\n    \/\/ debug\n    const Eigen::MatrixXd X_result = kalman_filter_.getLatestX();\n    DEBUG_PRINT_MAT(X_result.transpose());\n    DEBUG_PRINT_MAT((X_result - X_curr).transpose());\n\n    return true;\n}<\/code><\/pre>\n\n\n\n
    3. \u91cd\u70b9\u5185\u5bb9\u89e3\u6790<\/p>\n\n\n\n
    3.1 \u8f93\u5165\u65f6\u95f4\u5ef6\u8fdf\u8865\u507f<\/p>\n\n\n\n
    \u8fd9\u4e2a\u64cd\u4f5c\u5176\u5b9e\u6bd4\u8f83\u7b80\u5355\u57fa\u672c\uff0c\u90a3\u5f53\u524d\u65f6\u95f4 t_curr \u4e0e pose \u7684\u65f6\u95f4\u6233\u4e4b\u5dee\u3002\u5e76\u4e0e\u9884\u8bbe\u7684\u5ef6\u8fdf\u65f6\u95f4\uff0c\u5f97\u5230\u4e00\u4e2a\u5ef6\u8fdf\u7684\u65f6\u95f4\uff0c\u8fd9\u4e2a\u65f6\u95f4\u4f1a\u52a0\u5165\u5230pose\u4e2d\u5b8c\u6210\u66f4\u65b0\uff0c\u7136\u540e\u5c31\u662f\u6839\u636e\u65f6\u95f4\u5dee\u53bb\u62ff\u5230z\u8f74\u4e0a\u7684\u9ad8\u5ea6\u3002<\/p>\n\n\n\n
    const double delay_time = (t_curr - pose-&gt;header.stamp).seconds() +\n                          params_.pose_additional_delay;  \/\/ \u8ba1\u7b97\u5ef6\u8fdf\u65f6\u95f4\n\/\/ \u6839\u636e\u6d4b\u91cf\u59ff\u6001\u7684\u65f6\u95f4\u6233\u548c\u5f53\u524d\u65f6\u95f4\u6233\u8ba1\u7b97\u5ef6\u8fdf\u65f6\u95f4\uff0c\u7136\u540e\u4f7f\u7528\u5ef6\u8fdf\u65f6\u95f4\u5bf9\u59ff\u6001\u8fdb\u884c\u8865\u507f\nconst auto pose_with_z_delay =\n    ekf_module_-&gt;compensatePoseWithZDelay(*pose, delay_time);<\/code><\/pre>\n\n\n\n
    \"\"<\/figure>\n\n\n\n
    3.2 \u81ea\u52a8\u4f30\u8ba1\u504f\u822a\u8bef\u5dee<\/p>\n\n\n\n
    \u8fd9\u4e2a\u91cc\u9762\u7684\u4f30\u7b97\u5176\u5b9e\u90fd\u5728common\/kalman_filter\/src\/time_delay_kalman_filter.cpp<\/code>\u4e2d\uff0c\u4e3b\u8981\u6839\u636eyaw\u4ee5\u53cayaw_bias\u6765\u5b8c\u6210\u504f\u822a\u89d2\u72b6\u6001\u4f30\u7b97<\/p>\n\n\n\n
     const double biased_yaw = kalman_filter_.getXelement(IDX::YAW);\n    const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB);\n    const double yaw = biased_yaw + yaw_bias;\n    Pose current_ekf_pose;\n    current_ekf_pose.header.frame_id = params_.pose_frame_id;\n    current_ekf_pose.header.stamp = current_time;\n    current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z);\n    \/\/ \u6839\u636e\u9700\u8981\u8ba1\u7b97\u504f\u822a\u89d2\n    if (get_biased_yaw) {\n        current_ekf_pose.pose.orientation =\n            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw);\n    } else {\n        current_ekf_pose.pose.orientation =\n            tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw);\n    }\n<\/code><\/pre>\n\n\n\n
    3.3 \u9a6c\u6c0f\u8ddd\u79bb\u95e8<\/p>\n\n\n\n
      \n
    1. \u7b2c\u4e00\u884c\u8ba1\u7b97\u4e86\u89c2\u6d4b\u503cy_ekf\u76f8\u5bf9\u4e8e\u5b9e\u9645\u89c2\u6d4b\u503cy\u7684\u9a6c\u6c0f\u8ddd\u79bb\uff0c\u4f7f\u7528\u4e86\u4e4b\u524d\u5b9a\u4e49\u7684P_y\u4f5c\u4e3a\u534f\u65b9\u5dee\u77e9\u9635\u3002<\/li>\n\n\n\n
    2. \u63a5\u7740\u66f4\u65b0\u4e86\u8bca\u65ad\u4fe1\u606fpose_diag_info\u4e2d\u7684mahalanobis_distance\uff0c\u786e\u4fdd\u8bb0\u5f55\u4e0b\u6700\u5927\u7684\u9a6c\u6c0f\u8ddd\u79bb\u3002<\/li>\n\n\n\n
    3. \u6700\u540e\uff0c\u901a\u8fc7\u6bd4\u8f83\u9a6c\u6c0f\u8ddd\u79bb\u548c\u95e8\u9650\u503cparams_.pose_gate_dist\u6765\u8fdb\u884c\u95e8\u9650\u5224\u65ad\uff0c\u82e5\u8d85\u51fa\u95e8\u9650\u5219\u8bbe\u7f6epose_diag_info.is_passed_mahalanobis_gate\u4e3afalse\uff0c\u5e76\u8f93\u51fa\u8b66\u544a\u4fe1\u606f\u3002<\/li>\n<\/ol>\n\n\n\n
      \u9700\u8981\u6ce8\u610f\u7684\u662f\u6211\u4eec\u62ff\u5230\u7684\u503c\u662f\u52a0\u5165\u5ef6\u8fdf\u7684\uff0c\u5373\u4ee3\u7801\u4e2d\u7684 delay_step * dim_x_<\/code> \u8868\u793a\u4e00\u4e2a\u5ef6\u8fdf\u6b65\u957f\u4e58\u4ee5\u7ef4\u5ea6X\u7684\u5927\u5c0f\uff0c\u800c IDX::X<\/code> \u5219\u8868\u793a\u5728X\u65b9\u5411\u4e0a\u7684\u7d22\u5f15\u3002 dim_x_\u662f\u4e00\u4e2a6\u5143\u7d20\u7684\u6570\u7ec4\uff0c\u5206\u522b\u5b58\u50a8\u7740\uff1ax, y, yaw, yaw_bias, vx, wz\u3002<\/p>\n\n\n\n
      const Eigen::Vector3d y_ekf(kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X),\n                            kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y),\n                            ekf_yaw);\nconst Eigen::MatrixXd P_curr = kalman_filter_.getLatestP();\nconst Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y);\n\/\/ \u8ba1\u7b97\u9a6c\u6c0f\u8ddd\u79bb\uff0c\u5e76\u8fdb\u884c\u95e8\u9650\u5224\u65ad\uff0c\u82e5\u8d85\u51fa\u95e8\u9650\u5219\u8f93\u51fa\u8b66\u544a\u4fe1\u606f\u3002\nconst double distance = mahalanobis(y_ekf, y, P_y);\npose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance);\nif (distance &gt; params_.pose_gate_dist) {\n    pose_diag_info.is_passed_mahalanobis_gate = false;\n    warning_-&gt;warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000);\n    warning_-&gt;warnThrottle(\"Ignore the measurement data.\", 2000);\n    return false;\n}\n\n<\/code><\/pre>\n\n\n\n
      \"\"<\/figure>\n\n\n\n
      3.4 \u5e73\u6ed1\u66f4\u65b0<\/p>\n\n\n\n
      \u5e73\u6ed1\u66f4\u65b0\u5176\u5b9e\u91cc\u9762\u6ca1\u6709\u4ec0\u4e48\u64cd\u4f5c\uff0c\u57283.1\u5ef6\u8fdf\u8865\u507f\u4e0b\u9762\u5c31\u662f\u5e73\u6ed1\u66f4\u65b0\uff0c\u4e3b\u8981\u864e\u7259\u80a1\u6839\u636e\u5e73\u6ed1\u53c2\u6570\u4e0e\u534f\u65b9\u5dee\u77e9\u9635\u5b8c\u6210\u8f68\u8ff9\u7684\u5e73\u6ed1<\/p>\n\n\n\n
      if (is_updated) {\n                pose_is_updated = true;\n\n                \/\/ Update Simple 1D filter with considering change of z value due to measurement\n                \/\/ pose\n                const double delay_time = (t_curr - pose-&gt;header.stamp).seconds() +\n                                          params_.pose_additional_delay;  \/\/ \u8ba1\u7b97\u5ef6\u8fdf\u65f6\u95f4\n                \/\/ \u6839\u636e\u6d4b\u91cf\u59ff\u6001\u7684\u65f6\u95f4\u6233\u548c\u5f53\u524d\u65f6\u95f4\u6233\u8ba1\u7b97\u5ef6\u8fdf\u65f6\u95f4\uff0c\u7136\u540e\u4f7f\u7528\u5ef6\u8fdf\u65f6\u95f4\u5bf9\u59ff\u6001\u8fdb\u884c\u8865\u507f\n                const auto pose_with_z_delay =\n                    ekf_module_-&gt;compensatePoseWithZDelay(*pose, delay_time);\n                updateSimple1DFilters(pose_with_z_delay,\n                                      params_.pose_smoothing_steps);  \/\/ \u66f4\u65b0\u6ee4\u6ce2\u5668\n            }<\/code><\/pre>\n\n\n\n
      \"\"<\/figure>\n\n\n\n
      3.5 \u4fef\u4ef0\u89d2\u8ba1\u7b97\u5782\u76f4\u6821\u6b63\u91cf<\/p>\n\n\n\n
      \u8fd9\u90e8\u5206\u5728\u300a3.1 \u8f93\u5165\u65f6\u95f4\u5ef6\u8fdf\u8865\u507f\u300b\u5df2\u7ecf\u8bb2\u5230\uff0c\u793a\u610f\u56fe\u5982\u4e0b<\/p>\n\n\n\n
      \"\"\/<\/figure>\n","protected":false},"excerpt":{"rendered":"
      1. \u4e3b\u8981\u529f\u80fd 2. \u4ee3\u7801\u9605\u8bfb 2.1 cov […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[3,1],"tags":[],"class_list":["post-16457","post","type-post","status-publish","format-standard","hentry","category-technology-frontier","category-home"],"views":0,"_links":{"self":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16457"}],"collection":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/comments?post=16457"}],"version-history":[{"count":1,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16457\/revisions"}],"predecessor-version":[{"id":16461,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16457\/revisions\/16461"}],"wp:attachment":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/media?parent=16457"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/categories?post=16457"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/tags?post=16457"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}