Initial import of FaRui Campus ADS v3.2

master/src/Localization/autoware_ekf_localizer/CMakeLists.txt

@@ -0,0 +1,82 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_ekf_localizer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Eigen3 REQUIRED)
+
+include_directories(
+  SYSTEM
+    ${EIGEN3_INCLUDE_DIR}
+)
+
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/ekf_localizer.cpp
+  src/covariance.cpp
+  src/diagnostics.cpp
+  src/mahalanobis.cpp
+  src/measurement.cpp
+  src/state_transition.cpp
+  src/warning_message.cpp
+  src/ekf_module.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::ekf_localizer::EKFLocalizer"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+target_link_libraries(${PROJECT_NAME} Eigen3::Eigen)
+
+function(add_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name ${filename})
+
+  ament_add_gtest(${test_name} ${filepath})
+  target_link_libraries("${test_name}" ${PROJECT_NAME})
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
+if(BUILD_TESTING)
+  add_launch_test(
+    test/test_ekf_localizer_launch.py
+    TIMEOUT "30"
+  )
+  add_launch_test(
+    test/test_ekf_localizer_mahalanobis.py
+    TIMEOUT "30"
+  )
+  find_package(ament_cmake_gtest REQUIRED)
+
+  file(GLOB_RECURSE TEST_FILES test/*.cpp)
+
+  foreach(filepath ${TEST_FILES})
+    add_testcase(${filepath})
+  endforeach()
+endif()
+
+
+# if(BUILD_TESTING)
+#   find_package(ament_cmake_ros REQUIRED)
+#   ament_add_ros_isolated_gtest(ekf_localizer-test test/test_ekf_localizer.test
+#     test/src/test_ekf_localizer.cpp
+#     src/ekf_localizer.cpp
+#     src/kalman_filter/kalman_filter.cpp
+#     src/kalman_filter/time_delay_kalman_filter.cpp
+#   )
+#   target_include_directories(ekf_localizer-test
+#     PRIVATE
+#     include
+#   )
+#   ament_target_dependencies(ekf_localizer-test geometry_msgs rclcpp tf2 tf2_ros)
+# endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

master/src/Localization/autoware_ekf_localizer/README.md

@@ -0,0 +1,213 @@
+# 扩展卡尔曼滤波器定位器概述
+
+**扩展卡尔曼滤波器定位器**通过将二维车辆动力学模型与输入的自车姿态和自车速度信息相结合，估计出更稳健且噪声更少的机器人姿态和速度。该算法特别适用于快速移动的机器人，例如自动驾驶系统。
+
+## 流程图
+
+Autoware EKF 定位器的整体流程图如下所示。
+
+<p align="center">
+  <img src="./media/ekf_flowchart.png" width="800">
+</p>
+
+## 功能特性
+
+该软件包包含以下特性：
+
+- **输入消息的时间延迟补偿**，能够正确整合具有不同时间延迟的输入信息。这对于高速移动的机器人（如自动驾驶车辆）尤为重要。（见下图）
+- **自动估计偏航角偏差**，防止因传感器安装角度误差导致的建模错误，从而提高估计精度。
+- **马氏距离门控**，通过概率方法检测异常值，确定哪些输入应该被使用或忽略。
+- **平滑更新**，卡尔曼滤波器的测量更新通常在获得测量值时执行，但可能会导致估计值发生较大变化，尤其是对于低频测量。由于算法可以考虑测量时间，因此可以将测量数据分成多部分，并在保持一致性的同时进行平滑整合（见下图）。
+- **根据俯仰角计算垂直修正量**，减轻在斜坡上的定位不稳定性。例如，当上坡时，由于 EKF 只考虑 3 自由度（x、y、偏航角），它会表现得好像车辆陷入地面（见“根据俯仰角计算修正量”图的左侧）。因此，EKF 根据公式修正 z 坐标（见“根据俯仰角计算修正量”图的右侧）。
+
+<p align="center">
+<img src="./media/ekf_delay_comp.png" width="800">
+</p>
+
+<p align="center">
+  <img src="./media/ekf_smooth_update.png" width="800">
+</p>
+
+<p align="center">
+  <img src="./media/calculation_delta_from_pitch.png" width="800">
+</p>
+
+## 节点
+
+### 订阅主题
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `measured_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | 带有测量协方差矩阵的输入姿态源。 |
+| `measured_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | 带有测量协方差矩阵的输入速度源。 |
+| `initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF 的初始姿态。在启动时，估计的姿态以零值初始化。每当发布此消息时，它都会被初始化。 |
+
+### 发布主题
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `ekf_odom` | `nav_msgs::msg::Odometry` | 估计的里程计。 |
+| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | 估计的姿态。 |
+| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | 带协方差的估计姿态。 |
+| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | 包括偏航角偏差的估计姿态。 |
+| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | 包括偏航角偏差的带协方差的估计姿态。 |
+| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | 估计的速度。 |
+| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | 带协方差的估计速度。 |
+| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | 诊断信息。 |
+
+### 发布的 TF
+
+- base_link
+  从 `map` 坐标系到估计姿态的变换。
+
+## 功能
+
+### 预测
+
+使用给定的预测模型，从先前估计的数据中预测当前机器人的状态。此计算以恒定间隔（`predict_frequency [Hz]`）调用。预测方程在本页面末尾描述。
+
+### 测量更新
+
+在更新之前，计算测量输入与预测状态之间的马氏距离，对于马氏距离超过给定阈值的输入，不执行测量更新。
+
+使用最新的测量输入（`measured_pose` 和 `measured_twist`）更新预测状态。更新的频率与预测相同，通常为高频率，以便实现平滑的状态估计。
+
+## 参数描述
+
+参数在 `launch/ekf_localizer.launch` 中设置。
+
+### 节点参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json") }}
+
+### 姿态测量参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}
+
+### 速度测量参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}
+
+### 过程噪声参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json") }}
+
+注意：位置 x 和 y 的过程噪声会根据非线性动力学自动计算。
+
+### 简单一维滤波器参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}
+
+### 诊断参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}
+
+### 其他参数
+
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json") }}
+
+## 如何调整 EKF 参数
+
+### 0. 准备工作
+
+- 检查姿态和速度消息中的时间戳是否正确设置为传感器时间，因为时间延迟是从这个值计算的。如果由于时钟同步问题难以设置合适的时间，请使用 `twist_additional_delay` 和 `pose_additional_delay` 来纠正时间。
+- 检查测量姿态和速度之间的关系是否适当（姿态的导数是否与速度相似）。这种差异主要由单位错误（例如混淆弧度/度）或偏差噪声引起，它会导致较大的估计误差。
+
+### 1. 调整传感器参数
+
+为每个传感器设置标准差。`pose_measure_uncertainty_time` 是用于时间戳数据不确定性的参数。
+您还可以通过调整 `*_smoothing_steps` 来调整每个观测到的传感器数据的平滑步数。
+增加步数可以提高估计的平滑度，但也可能对估计性能产生不利影响。
+
+- `pose_measure_uncertainty_time`
+- `pose_smoothing_steps`
+- `twist_smoothing_steps`
+
+### 2. 调整过程模型参数
+
+- `proc_stddev_vx_c`：设置为最大线性加速度。
+- `proc_stddev_wz_c`：设置为最大角加速度。
+- `proc_stddev_yaw_c`：此参数描述偏航角和偏航率之间的相关性。较大的值意味着偏航角的变化与估计的偏航率不相关。如果将其设置为 0，则意味着估计的偏航角变化等于偏航率。通常，应将其设置为 0。
+- `proc_stddev_yaw_bias_c`：此参数是偏航角偏差变化率的标准差。在大多数情况下，偏航角偏差是恒定的，因此可以非常小，但必须非零。
+
+### 3. 调整门控参数
+
+EKF 在通过观测更新之前使用马氏距离进行门控。门控大小由 `pose_gate_dist` 参数和 `twist_gate_dist` 决定。如果马氏距离大于此值，则忽略该观测。
+
+此门控过程基于卡方分布的统计检验。根据模型，我们假设姿态的马氏距离遵循 3 自由度的卡方分布，速度的马氏距离遵循 2 自由度的卡方分布。
+
+目前，协方差估计本身的准确性并不高，因此建议将显著性水平设置为非常小的值，以减少因假阳性而拒绝的情况。
+
+| 显著性水平 | 2 自由度的阈值 | 3 自由度的阈值 |
+| ------------------ | ------------------- | ------------------- |
+| $10^{-2}$        | 9.21                | 11.3                |
+| $10^{-3}$        | 13.8                | 16.3                |
+| $10^{-4}$        | 18.4                | 21.1                |
+| $10^{-5}$        | 23.0                | 25.9                |
+| $10^{-6}$        | 27.6                | 30.7                |
+| $10^{-7}$        | 32.2                | 35.4                |
+| $10^{-8}$        | 36.8                | 40.1                |
+| $10^{-9}$        | 41.4                | 44.8                |
+| $10^{-10}$       | 46.1                | 49.5                |
+
+## 卡尔曼滤波器模型
+
+### 更新函数中的动力学模型
+
+<p align="center">
+<img src="./media/ekf_dynamics.png" width="320">
+</p>
+
+其中，$\theta_k$ 表示车辆的航向角，包括安装角度偏差。
+$b_k$ 是偏航角偏差的修正项，它被建模为使 $(\theta_k + b_k)$ 成为 base_link 的航向角。
+姿态估计器预计会在地图坐标系中发布 base_link。然而，由于校准错误，偏航角可能会偏移。此模型补偿了这一误差，提高了估计精度。
+
+### 时间延迟模型
+
+通过扩展状态 [1]（见第 7.3 节 固定滞后平滑）处理测量时间延迟。
+
+<p align="center">
+<img src="./media/delay_model_eq.png" width="320">
+</p>
+
+注意，尽管由于可以基于扩展状态的特定结构进行解析扩展，维度变大了，但计算复杂度并没有显著变化。
+
+## 使用 Autoware NDT 的测试结果
+
+<p align="center">
+<img src="./media/ekf_autoware_res.png" width="600">
+</p>
+
+## 诊断
+
+<p align="center">
+<img src="./media/ekf_diagnostics.png" width="320">
+</p>
+
+<p align="center">
+<img src="./media/ekf_diagnostics_callback_pose.png" width="320">
+</p>
+<p align="center">
+<img src="./media/ekf_diagnostics_callback_twist.png" width="320">
+</p>
+
+### 导致警告状态的条件
+
+- 节点未处于激活状态。
+- 通过姿态/速度主题进行测量更新的连续次数超过 `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`。
+- 姿态/速度主题的时间戳超出了延迟补偿范围。
+- 姿态/速度主题超出了用于协方差估计的马氏距离范围。
+- 协方差椭圆的长轴或横向方向的大小超过了阈值 `warn_ellipse_size` 或 `warn_ellipse_size_lateral_direction`。
+
+### 导致错误状态的条件
+
+- 通过姿态/速度主题进行测量更新的连续次数超过 `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`。
+- 协方差椭圆的长轴或横向方向的大小超过了阈值 `error_ellipse_size` 或 `error_ellipse_size_lateral_direction`。
+
+## 已知问题
+
+- 如果使用多个姿态估计器，输入到 EKF 的数据将包括每个源对应的多个偏航角偏差。然而，当前的 EKF 假设只有一个偏航角偏差。因此，当前 EKF 状态中的偏航角偏差 $b_k$ 将没有意义，也无法正确处理这些多个偏航角偏差。因此，未来的工作包括为每个传感器引入带有偏航角估计的偏航角偏差。
+
+## 参考文献
+
+[1] Anderson, B. D. O., & Moore, J. B. (1979). Optimal filtering. Englewood Cliffs, NJ: Prentice-Hall.

master/src/Localization/autoware_ekf_localizer/config/ekf_localizer.param.yaml

@@ -0,0 +1,51 @@
+/**:
+  ros__parameters:
+    node:
+      show_debug_info: false
+      enable_yaw_bias_estimation: true
+      predict_frequency: 50.0
+      tf_rate: 50.0
+      publish_tf: true
+      extend_state_step: 50
+
+    pose_measurement:
+      # for Pose measurement
+      pose_additional_delay: 0.0
+      pose_measure_uncertainty_time: 0.01
+      pose_smoothing_steps: 5
+      pose_gate_dist: 49.5 # corresponds to significance level = 10^-10
+
+    twist_measurement:
+      # for twist measurement
+      twist_additional_delay: 0.0
+      twist_smoothing_steps: 2
+      twist_gate_dist: 46.1 # corresponds to significance level = 10^-10
+
+    process_noise:
+      # for process model
+      proc_stddev_yaw_c: 0.005
+      proc_stddev_vx_c: 10.0
+      proc_stddev_wz_c: 5.0
+
+    simple_1d_filter_parameters:
+      #Simple1DFilter parameters
+      z_filter_proc_dev: 1.0
+      roll_filter_proc_dev: 0.1
+      pitch_filter_proc_dev: 0.1
+
+    diagnostics:
+      # for diagnostics
+      pose_no_update_count_threshold_warn: 50
+      pose_no_update_count_threshold_error: 100
+      twist_no_update_count_threshold_warn: 50
+      twist_no_update_count_threshold_error: 100
+      ellipse_scale: 3.0
+      error_ellipse_size: 1.5
+      warn_ellipse_size: 1.2
+      error_ellipse_size_lateral_direction: 0.3
+      warn_ellipse_size_lateral_direction: 0.25
+
+    misc:
+      # for velocity measurement limitation (Set 0.0 if you want to ignore)
+      threshold_observable_velocity_mps: 0.0 # [m/s]
+      pose_frame_id: "map"

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/aged_object_queue.hpp

@@ -0,0 +1,71 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_
+#define AUTOWARE__EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_
+
+#include <cstddef>
+#include <queue>
+
+namespace autoware::ekf_localizer
+{
+
+template <typename Object>
+class AgedObjectQueue
+{
+public:
+  explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {}
+
+  [[nodiscard]] bool empty() const { return this->size() == 0; }
+
+  [[nodiscard]] size_t size() const { return objects_.size(); }
+
+  Object back() const { return objects_.back(); }
+
+  void push(const Object & object)
+  {
+    objects_.push(object);
+    ages_.push(0);
+  }
+
+  Object pop_increment_age()
+  {
+    const Object object = objects_.front();
+    const size_t age = ages_.front() + 1;
+    objects_.pop();
+    ages_.pop();
+
+    if (age < max_age_) {
+      objects_.push(object);
+      ages_.push(age);
+    }
+
+    return object;
+  }
+
+  void clear()
+  {
+    objects_ = std::queue<Object>();
+    ages_ = std::queue<size_t>();
+  }
+
+private:
+  const size_t max_age_;
+  std::queue<Object> objects_;
+  std::queue<size_t> ages_;
+};
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/covariance.hpp

@@ -0,0 +1,28 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__COVARIANCE_HPP_
+#define AUTOWARE__EKF_LOCALIZER__COVARIANCE_HPP_
+
+#include "autoware/ekf_localizer/matrix_types.hpp"
+
+namespace autoware::ekf_localizer
+{
+
+std::array<double, 36> ekf_covariance_to_pose_message_covariance(const Matrix6d & P);
+std::array<double, 36> ekf_covariance_to_twist_message_covariance(const Matrix6d & P);
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__COVARIANCE_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp

@@ -0,0 +1,48 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__DIAGNOSTICS_HPP_
+#define AUTOWARE__EKF_LOCALIZER__DIAGNOSTICS_HPP_
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::ekf_localizer
+{
+
+diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated);
+
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated(
+  const std::string & measurement_type, const size_t no_update_count,
+  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error);
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size(
+  const std::string & measurement_type, const size_t queue_size);
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate(
+  const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time,
+  const double delay_time_threshold);
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate(
+  const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
+  const double mahalanobis_distance, const double mahalanobis_distance_threshold);
+diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse(
+  const std::string & name, const double curr_size, const double warn_threshold,
+  const double error_threshold);
+
+diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
+  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array);
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__DIAGNOSTICS_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp

@@ -0,0 +1,201 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__EKF_LOCALIZER_HPP_
+#define AUTOWARE__EKF_LOCALIZER__EKF_LOCALIZER_HPP_
+
+#include "autoware/ekf_localizer/aged_object_queue.hpp"
+#include "autoware/ekf_localizer/ekf_module.hpp"
+#include "autoware/ekf_localizer/hyper_parameters.hpp"
+#include "autoware/ekf_localizer/warning.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/ros/logger_level_configure.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <std_srvs/srv/set_bool.hpp>
+#include <tier4_debug_msgs/msg/float64_multi_array_stamped.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/utils.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <string>
+#include <vector>
+
+namespace autoware::ekf_localizer
+{
+
+class EKFLocalizer : public rclcpp::Node
+{
+public:
+  explicit EKFLocalizer(const rclcpp::NodeOptions & options);
+
+  // This function is only used in static tools to know when timer callbacks are triggered.
+  std::chrono::nanoseconds time_until_trigger() const
+  {
+    return timer_control_->time_until_trigger();
+  }
+
+private:
+  const std::shared_ptr<Warning> warning_;
+
+  //!< @brief ekf estimated pose publisher
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_pose_;
+  //!< @brief estimated ekf pose with covariance publisher
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_pose_cov_;
+  //!< @brief estimated ekf odometry publisher
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom_;
+  //!< @brief ekf estimated twist publisher
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr pub_twist_;
+  //!< @brief ekf estimated twist with covariance publisher
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr pub_twist_cov_;
+  //!< @brief ekf estimated yaw bias publisher
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_yaw_bias_;
+  //!< @brief ekf estimated yaw bias publisher
+  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_biased_pose_;
+  //!< @brief ekf estimated yaw bias publisher
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_biased_pose_cov_;
+  //!< @brief diagnostics publisher
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr pub_diag_;
+  //!< @brief initial pose subscriber
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initialpose_;
+  //!< @brief measurement pose with covariance subscriber
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pose_with_cov_;
+  //!< @brief measurement twist with covariance subscriber
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    sub_twist_with_cov_;
+  //!< @brief time for ekf calculation callback
+  rclcpp::TimerBase::SharedPtr timer_control_;
+  //!< @brief last predict time
+  std::shared_ptr<const rclcpp::Time> last_predict_time_;
+  //!< @brief trigger_node service
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
+
+  //!< @brief timer to send transform
+  rclcpp::TimerBase::SharedPtr timer_tf_;
+  //!< @brief tf broadcaster
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
+  //!< @brief tf buffer
+  tf2_ros::Buffer tf2_buffer_;
+  //!< @brief tf listener
+  tf2_ros::TransformListener tf2_listener_;
+
+  //!< @brief logger configure module
+  std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
+
+  //!< @brief  extended kalman filter instance.
+  std::unique_ptr<EKFModule> ekf_module_;
+
+  const HyperParameters params_;
+
+  double ekf_dt_;
+
+  bool is_activated_;
+
+  EKFDiagnosticInfo pose_diag_info_;
+  EKFDiagnosticInfo twist_diag_info_;
+
+  AgedObjectQueue<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
+  AgedObjectQueue<geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr> twist_queue_;
+
+  /**
+   * @brief computes update & prediction of EKF for each ekf_dt_[s] time
+   */
+  void timer_callback();
+
+  /**
+   * @brief publish tf for tf_rate [Hz]
+   */
+  void timer_tf_callback();
+
+  /**
+   * @brief set pose with covariance measurement
+   */
+  void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+
+  /**
+   * @brief set twist with covariance measurement
+   */
+  void callback_twist_with_covariance(
+    geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+
+  /**
+   * @brief set initial_pose to current EKF pose
+   */
+  void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+
+  /**
+   * @brief update predict frequency
+   */
+  void update_predict_frequency(const rclcpp::Time & current_time);
+
+  /**
+   * @brief get transform from frame_id
+   */
+  bool get_transform_from_tf(
+    std::string parent_frame, std::string child_frame,
+    geometry_msgs::msg::TransformStamped & transform);
+
+  /**
+   * @brief publish current EKF estimation result
+   */
+  void publish_estimate_result(
+    const geometry_msgs::msg::PoseStamped & current_ekf_pose,
+    const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose,
+    const geometry_msgs::msg::TwistStamped & current_ekf_twist);
+
+  /**
+   * @brief publish diagnostics message
+   */
+  void publish_diagnostics(
+    const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time);
+
+  /**
+   * @brief publish diagnostics message for return
+   */
+  void publish_callback_return_diagnostics(
+    const std::string & callback_name, const rclcpp::Time & current_time);
+
+  /**
+   * @brief trigger node
+   */
+  void service_trigger_node(
+    const std_srvs::srv::SetBool::Request::SharedPtr req,
+    std_srvs::srv::SetBool::Response::SharedPtr res);
+
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
+
+  friend class EKFLocalizerTestSuite;  // for test code
+};
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__EKF_LOCALIZER_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_module.hpp

@@ -0,0 +1,156 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__EKF_MODULE_HPP_
+#define AUTOWARE__EKF_LOCALIZER__EKF_MODULE_HPP_
+
+#include "autoware/ekf_localizer/hyper_parameters.hpp"
+#include "autoware/ekf_localizer/state_index.hpp"
+#include "autoware/ekf_localizer/warning.hpp"
+
+#include <autoware/kalman_filter/kalman_filter.hpp>
+#include <autoware/kalman_filter/time_delay_kalman_filter.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+
+#include <tf2/utils.h>
+
+#include <memory>
+#include <vector>
+
+namespace autoware::ekf_localizer
+{
+using autoware::kalman_filter::TimeDelayKalmanFilter;
+
+struct EKFDiagnosticInfo
+{
+  size_t no_update_count{0};
+  size_t queue_size{0};
+  bool is_passed_delay_gate{true};
+  double delay_time{0.0};
+  double delay_time_threshold{0.0};
+  bool is_passed_mahalanobis_gate{true};
+  double mahalanobis_distance{0.0};
+};
+
+class Simple1DFilter
+{
+public:
+  Simple1DFilter()
+  {
+    initialized_ = false;
+    x_ = 0;
+    var_ = 1e9;
+    proc_var_x_c_ = 0.0;
+  };
+  void init(const double init_obs, const double obs_var)
+  {
+    x_ = init_obs;
+    var_ = obs_var;
+    initialized_ = true;
+  };
+  void update(const double obs, const double obs_var, const double dt)
+  {
+    if (!initialized_) {
+      init(obs, obs_var);
+      return;
+    }
+
+    // Prediction step (current variance)
+    double proc_var_x_d = proc_var_x_c_ * dt * dt;
+    var_ = var_ + proc_var_x_d;
+
+    // Update step
+    double kalman_gain = var_ / (var_ + obs_var);
+    x_ = x_ + kalman_gain * (obs - x_);
+    var_ = (1 - kalman_gain) * var_;
+  };
+  void set_proc_var(const double proc_var) { proc_var_x_c_ = proc_var; }
+  [[nodiscard]] double get_x() const { return x_; }
+  [[nodiscard]] double get_var() const { return var_; }
+
+private:
+  bool initialized_;
+  double x_;
+  double var_;
+  double proc_var_x_c_;
+};
+
+class EKFModule
+{
+private:
+  using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using TwistWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped;
+  using Pose = geometry_msgs::msg::PoseStamped;
+  using Twist = geometry_msgs::msg::TwistStamped;
+
+public:
+  EKFModule(std::shared_ptr<Warning> warning, const HyperParameters & params);
+
+  void initialize(
+    const PoseWithCovariance & initial_pose,
+    const geometry_msgs::msg::TransformStamped & transform);
+
+  [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose(
+    const rclcpp::Time & current_time, bool get_biased_yaw) const;
+  [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist(
+    const rclcpp::Time & current_time) const;
+  [[nodiscard]] double get_yaw_bias() const;
+  [[nodiscard]] std::array<double, 36> get_current_pose_covariance() const;
+  [[nodiscard]] std::array<double, 36> get_current_twist_covariance() const;
+
+  [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const;
+  void accumulate_delay_time(const double dt);
+
+  void predict_with_delay(const double dt);
+  bool measurement_update_pose(
+    const PoseWithCovariance & pose, const rclcpp::Time & t_curr,
+    EKFDiagnosticInfo & pose_diag_info);
+  bool measurement_update_twist(
+    const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
+    EKFDiagnosticInfo & twist_diag_info);
+  geometry_msgs::msg::PoseWithCovarianceStamped compensate_rph_with_delay(
+    const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, const double delay_time);
+
+private:
+  void update_simple_1d_filters(
+    const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step);
+
+  TimeDelayKalmanFilter kalman_filter_;
+
+  std::shared_ptr<Warning> warning_;
+  const int dim_x_;
+  std::vector<double> accumulated_delay_times_;
+  const HyperParameters params_;
+
+  Simple1DFilter z_filter_;
+  Simple1DFilter roll_filter_;
+  Simple1DFilter pitch_filter_;
+
+  /**
+   * @brief last angular velocity for compensating rph with delay
+   */
+  tf2::Vector3 last_angular_velocity_;
+
+  double ekf_dt_;
+};
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__EKF_MODULE_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/hyper_parameters.hpp

@@ -0,0 +1,110 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
+#define AUTOWARE__EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <algorithm>
+#include <string>
+
+namespace autoware::ekf_localizer
+{
+
+class HyperParameters
+{
+public:
+  explicit HyperParameters(rclcpp::Node * node)
+  : show_debug_info(node->declare_parameter<bool>("node.show_debug_info")),
+    ekf_rate(node->declare_parameter<double>("node.predict_frequency")),
+    ekf_dt(1.0 / std::max(ekf_rate, 0.1)),
+    tf_rate_(node->declare_parameter<double>("node.tf_rate")),
+    publish_tf_(node->declare_parameter<bool>("node.publish_tf")),
+    enable_yaw_bias_estimation(node->declare_parameter<bool>("node.enable_yaw_bias_estimation")),
+    extend_state_step(node->declare_parameter<int>("node.extend_state_step")),
+    pose_frame_id(node->declare_parameter<std::string>("misc.pose_frame_id")),
+    pose_additional_delay(
+      node->declare_parameter<double>("pose_measurement.pose_additional_delay")),
+    pose_gate_dist(node->declare_parameter<double>("pose_measurement.pose_gate_dist")),
+    pose_smoothing_steps(node->declare_parameter<int>("pose_measurement.pose_smoothing_steps")),
+    twist_additional_delay(
+      node->declare_parameter<double>("twist_measurement.twist_additional_delay")),
+    twist_gate_dist(node->declare_parameter<double>("twist_measurement.twist_gate_dist")),
+    twist_smoothing_steps(node->declare_parameter<int>("twist_measurement.twist_smoothing_steps")),
+    proc_stddev_vx_c(node->declare_parameter<double>("process_noise.proc_stddev_vx_c")),
+    proc_stddev_wz_c(node->declare_parameter<double>("process_noise.proc_stddev_wz_c")),
+    proc_stddev_yaw_c(node->declare_parameter<double>("process_noise.proc_stddev_yaw_c")),
+    z_filter_proc_dev(
+      node->declare_parameter<double>("simple_1d_filter_parameters.z_filter_proc_dev")),
+    roll_filter_proc_dev(
+      node->declare_parameter<double>("simple_1d_filter_parameters.roll_filter_proc_dev")),
+    pitch_filter_proc_dev(
+      node->declare_parameter<double>("simple_1d_filter_parameters.pitch_filter_proc_dev")),
+    pose_no_update_count_threshold_warn(
+      node->declare_parameter<int>("diagnostics.pose_no_update_count_threshold_warn")),
+    pose_no_update_count_threshold_error(
+      node->declare_parameter<int>("diagnostics.pose_no_update_count_threshold_error")),
+    twist_no_update_count_threshold_warn(
+      node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_warn")),
+    twist_no_update_count_threshold_error(
+      node->declare_parameter<int>("diagnostics.twist_no_update_count_threshold_error")),
+    ellipse_scale(node->declare_parameter<double>("diagnostics.ellipse_scale")),
+    error_ellipse_size(node->declare_parameter<double>("diagnostics.error_ellipse_size")),
+    warn_ellipse_size(node->declare_parameter<double>("diagnostics.warn_ellipse_size")),
+    error_ellipse_size_lateral_direction(
+      node->declare_parameter<double>("diagnostics.error_ellipse_size_lateral_direction")),
+    warn_ellipse_size_lateral_direction(
+      node->declare_parameter<double>("diagnostics.warn_ellipse_size_lateral_direction")),
+    threshold_observable_velocity_mps(
+      node->declare_parameter<double>("misc.threshold_observable_velocity_mps"))
+  {
+  }
+
+  const bool show_debug_info;
+  const double ekf_rate;
+  const double ekf_dt;
+  const double tf_rate_;
+  const bool publish_tf_;
+  const bool enable_yaw_bias_estimation;
+  const size_t extend_state_step;
+  const std::string pose_frame_id;
+  const double pose_additional_delay;
+  const double pose_gate_dist;
+  const size_t pose_smoothing_steps;
+  const double twist_additional_delay;
+  const double twist_gate_dist;
+  const size_t twist_smoothing_steps;
+  const double proc_stddev_vx_c;   //!< @brief  vx process noise
+  const double proc_stddev_wz_c;   //!< @brief  wz process noise
+  const double proc_stddev_yaw_c;  //!< @brief  yaw process noise
+  const double z_filter_proc_dev;
+  const double roll_filter_proc_dev;
+  const double pitch_filter_proc_dev;
+  const size_t pose_no_update_count_threshold_warn;
+  const size_t pose_no_update_count_threshold_error;
+  const size_t twist_no_update_count_threshold_warn;
+  const size_t twist_no_update_count_threshold_error;
+  double ellipse_scale;
+  double error_ellipse_size;
+  double warn_ellipse_size;
+  double error_ellipse_size_lateral_direction;
+  double warn_ellipse_size_lateral_direction;
+
+  const double threshold_observable_velocity_mps;
+};
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__HYPER_PARAMETERS_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/mahalanobis.hpp

@@ -0,0 +1,31 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__MAHALANOBIS_HPP_
+#define AUTOWARE__EKF_LOCALIZER__MAHALANOBIS_HPP_
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+namespace autoware::ekf_localizer
+{
+
+double squared_mahalanobis(
+  const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C);
+
+double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C);
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__MAHALANOBIS_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/matrix_types.hpp

@@ -0,0 +1,28 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__MATRIX_TYPES_HPP_
+#define AUTOWARE__EKF_LOCALIZER__MATRIX_TYPES_HPP_
+
+#include <Eigen/Core>
+
+namespace autoware::ekf_localizer
+{
+
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__MATRIX_TYPES_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/measurement.hpp

@@ -0,0 +1,32 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__MEASUREMENT_HPP_
+#define AUTOWARE__EKF_LOCALIZER__MEASUREMENT_HPP_
+
+#include <Eigen/Core>
+
+namespace autoware::ekf_localizer
+{
+
+Eigen::Matrix<double, 3, 6> pose_measurement_matrix();
+Eigen::Matrix<double, 2, 6> twist_measurement_matrix();
+Eigen::Matrix3d pose_measurement_covariance(
+  const std::array<double, 36ul> & covariance, const size_t smoothing_step);
+Eigen::Matrix2d twist_measurement_covariance(
+  const std::array<double, 36ul> & covariance, const size_t smoothing_step);
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__MEASUREMENT_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/numeric.hpp

@@ -0,0 +1,37 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__NUMERIC_HPP_
+#define AUTOWARE__EKF_LOCALIZER__NUMERIC_HPP_
+
+#include <Eigen/Core>
+
+#include <cmath>
+
+namespace autoware::ekf_localizer
+{
+
+inline bool has_inf(const Eigen::MatrixXd & v)
+{
+  return v.array().isInf().any();
+}
+
+inline bool has_nan(const Eigen::MatrixXd & v)
+{
+  return v.array().isNaN().any();
+}
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__NUMERIC_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_index.hpp

@@ -0,0 +1,32 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__STATE_INDEX_HPP_
+#define AUTOWARE__EKF_LOCALIZER__STATE_INDEX_HPP_
+
+namespace autoware::ekf_localizer
+{
+
+enum IDX {
+  X = 0,
+  Y = 1,
+  YAW = 2,
+  YAWB = 3,
+  VX = 4,
+  WZ = 5,
+};
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__STATE_INDEX_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/state_transition.hpp

@@ -0,0 +1,31 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__STATE_TRANSITION_HPP_
+#define AUTOWARE__EKF_LOCALIZER__STATE_TRANSITION_HPP_
+
+#include "autoware/ekf_localizer/matrix_types.hpp"
+
+namespace autoware::ekf_localizer
+{
+
+double normalize_yaw(const double & yaw);
+Vector6d predict_next_state(const Vector6d & X_curr, const double dt);
+Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt);
+Matrix6d process_noise_covariance(
+  const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d);
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__STATE_TRANSITION_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/string.hpp

@@ -0,0 +1,34 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__STRING_HPP_
+#define AUTOWARE__EKF_LOCALIZER__STRING_HPP_
+
+#include <string>
+
+namespace autoware::ekf_localizer
+{
+
+inline std::string erase_leading_slash(const std::string & s)
+{
+  std::string a = s;
+  if (a.front() == '/') {
+    a.erase(0, 1);
+  }
+  return a;
+}
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__STRING_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning.hpp

@@ -0,0 +1,48 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__WARNING_HPP_
+#define AUTOWARE__EKF_LOCALIZER__WARNING_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace autoware::ekf_localizer
+{
+
+class Warning
+{
+public:
+  explicit Warning(rclcpp::Node * node) : node_(node) {}
+
+  void warn(const std::string & message) const
+  {
+    RCLCPP_WARN(node_->get_logger(), "%s", message.c_str());
+  }
+
+  void warn_throttle(const std::string & message, const int duration_milliseconds) const
+  {
+    RCLCPP_WARN_THROTTLE(
+      node_->get_logger(), *(node_->get_clock()),
+      std::chrono::milliseconds(duration_milliseconds).count(), "%s", message.c_str());
+  }
+
+private:
+  rclcpp::Node * node_;
+};
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__WARNING_HPP_

master/src/Localization/autoware_ekf_localizer/include/autoware/ekf_localizer/warning_message.hpp

@@ -0,0 +1,33 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__EKF_LOCALIZER__WARNING_MESSAGE_HPP_
+#define AUTOWARE__EKF_LOCALIZER__WARNING_MESSAGE_HPP_
+
+#include <string>
+
+namespace autoware::ekf_localizer
+{
+
+std::string pose_delay_step_warning_message(
+  const double delay_time, const double delay_time_threshold);
+std::string twist_delay_step_warning_message(
+  const double delay_time, const double delay_time_threshold);
+std::string pose_delay_time_warning_message(const double delay_time);
+std::string twist_delay_time_warning_message(const double delay_time);
+std::string mahalanobis_warning_message(const double distance, const double max_distance);
+
+}  // namespace autoware::ekf_localizer
+
+#endif  // AUTOWARE__EKF_LOCALIZER__WARNING_MESSAGE_HPP_

master/src/Localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml

@@ -0,0 +1,38 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share autoware_ekf_localizer)/config/ekf_localizer.param.yaml"/>
+
+  <arg name="input_initial_pose_name" default="initialpose3d"/>
+  <arg name="input_trigger_node_service_name" default="trigger_node" description="trigger node service"/>
+
+  <!-- input topic name -->
+  <arg name="input_pose_with_cov_name" default="in_pose_with_covariance"/>
+  <arg name="input_twist_with_cov_name" default="in_twist_with_covariance"/>
+
+  <!-- output topic name -->
+  <arg name="output_odom_name" default="ekf_odom"/>
+  <arg name="output_pose_name" default="ekf_pose"/>
+  <arg name="output_pose_with_covariance_name" default="ekf_pose_with_covariance"/>
+  <arg name="output_biased_pose_name" default="ekf_biased_pose"/>
+  <arg name="output_biased_pose_with_covariance_name" default="ekf_biased_pose_with_covariance"/>
+  <arg name="output_twist_name" default="ekf_twist"/>
+  <arg name="output_twist_with_covariance_name" default="ekf_twist_with_covariance"/>
+
+  <node pkg="autoware_ekf_localizer" exec="autoware_ekf_localizer_node" output="both">
+    <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>
+
+    <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>
+
+    <remap from="initialpose" to="$(var input_initial_pose_name)"/>
+    <remap from="trigger_node_srv" to="$(var input_trigger_node_service_name)"/>
+
+    <remap from="ekf_odom" to="$(var output_odom_name)"/>
+    <remap from="ekf_pose" to="$(var output_pose_name)"/>
+    <remap from="ekf_pose_with_covariance" to="$(var output_pose_with_covariance_name)"/>
+    <remap from="ekf_biased_pose" to="$(var output_biased_pose_name)"/>
+    <remap from="ekf_biased_pose_with_covariance" to="$(var output_biased_pose_with_covariance_name)"/>
+    <remap from="ekf_twist" to="$(var output_twist_name)"/>
+    <remap from="ekf_twist_with_covariance" to="$(var output_twist_with_covariance_name)"/>
+
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

master/src/Localization/autoware_ekf_localizer/package.xml

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_ekf_localizer</name>
+  <version>0.1.0</version>
+  <description>The autoware_ekf_localizer package</description>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="takeshi.ishita@tier4.jp">Takeshi Ishita</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+
+  <build_depend>eigen</build_depend>
+
+  <depend>autoware_kalman_filter</depend>
+  <depend>autoware_localization_util</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>fmt</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_debug_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Localization/autoware_ekf_localizer/schema/ekf_localizer.schema.json

@@ -0,0 +1,52 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration",
+  "type": "object",
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "type": "object",
+          "properties": {
+            "node": {
+              "$ref": "sub/node.sub_schema.json#/definitions/node"
+            },
+            "pose_measurement": {
+              "$ref": "sub/pose_measurement.sub_schema.json#/definitions/pose_measurement"
+            },
+            "twist_measurement": {
+              "$ref": "sub/twist_measurement.sub_schema.json#/definitions/twist_measurement"
+            },
+            "process_noise": {
+              "$ref": "sub/process_noise.sub_schema.json#/definitions/process_noise"
+            },
+            "simple_1d_filter_parameters": {
+              "$ref": "sub/simple_1d_filter_parameters.sub_schema.json#/definitions/simple_1d_filter_parameters"
+            },
+            "diagnostics": {
+              "$ref": "sub/diagnostics.sub_schema.json#/definitions/diagnostics"
+            },
+            "misc": {
+              "$ref": "sub/misc.sub_schema.json#/definitions/misc"
+            }
+          },
+          "required": [
+            "node",
+            "pose_measurement",
+            "twist_measurement",
+            "process_noise",
+            "simple_1d_filter_parameters",
+            "diagnostics",
+            "misc"
+          ],
+          "additionalProperties": false
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json

@@ -0,0 +1,63 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for Diagnostics",
+  "definitions": {
+    "diagnostics": {
+      "type": "object",
+      "properties": {
+        "pose_no_update_count_threshold_warn": {
+          "type": "integer",
+          "description": "The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times",
+          "default": 50
+        },
+        "pose_no_update_count_threshold_error": {
+          "type": "integer",
+          "description": "The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times",
+          "default": 100
+        },
+        "twist_no_update_count_threshold_warn": {
+          "type": "integer",
+          "description": "The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times",
+          "default": 50
+        },
+        "twist_no_update_count_threshold_error": {
+          "type": "integer",
+          "description": "The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times",
+          "default": 100
+        },
+        "ellipse_scale": {
+          "type": "number",
+          "description": "The scale factor to apply the error ellipse size",
+          "default": 3.0
+        },
+        "error_ellipse_size": {
+          "type": "number",
+          "description": "The long axis size of the error ellipse to trigger a ERROR state",
+          "default": 1.5
+        },
+        "warn_ellipse_size": {
+          "type": "number",
+          "description": "The long axis size of the error ellipse to trigger a WARN state",
+          "default": 1.2
+        },
+        "error_ellipse_size_lateral_direction": {
+          "type": "number",
+          "description": "The lateral direction size of the error ellipse to trigger a ERROR state",
+          "default": 0.3
+        },
+        "warn_ellipse_size_lateral_direction": {
+          "type": "number",
+          "description": "The lateral direction size of the error ellipse to trigger a WARN state",
+          "default": 0.25
+        }
+      },
+      "required": [
+        "pose_no_update_count_threshold_warn",
+        "pose_no_update_count_threshold_error",
+        "twist_no_update_count_threshold_warn",
+        "twist_no_update_count_threshold_error"
+      ],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json

@@ -0,0 +1,23 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for MISC",
+  "definitions": {
+    "misc": {
+      "type": "object",
+      "properties": {
+        "threshold_observable_velocity_mps": {
+          "type": "number",
+          "description": "Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor [m/s] (0.0 means disabled)",
+          "default": 0.0
+        },
+        "pose_frame_id": {
+          "type": "string",
+          "description": "Parent frame_id of EKF output pose",
+          "default": "map"
+        }
+      },
+      "required": ["threshold_observable_velocity_mps", "pose_frame_id"],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json

@@ -0,0 +1,50 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for node",
+  "definitions": {
+    "node": {
+      "type": "object",
+      "properties": {
+        "show_debug_info": {
+          "type": "boolean",
+          "description": "Flag to display debug info",
+          "default": false
+        },
+        "predict_frequency": {
+          "type": "number",
+          "description": "Frequency for filtering and publishing [Hz]",
+          "default": 50.0
+        },
+        "tf_rate": {
+          "type": "number",
+          "description": "Frequency for tf broadcasting [Hz]",
+          "default": 50.0
+        },
+        "publish_tf": {
+          "type": "boolean",
+          "description": "Whether to publish tf",
+          "default": true
+        },
+        "extend_state_step": {
+          "type": "integer",
+          "description": "Max delay step which can be dealt with in EKF. Large number increases computational cost.",
+          "default": 50
+        },
+        "enable_yaw_bias_estimation": {
+          "type": "boolean",
+          "description": "Flag to enable yaw bias estimation",
+          "default": true
+        }
+      },
+      "required": [
+        "show_debug_info",
+        "predict_frequency",
+        "tf_rate",
+        "publish_tf",
+        "extend_state_step",
+        "enable_yaw_bias_estimation"
+      ],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json

@@ -0,0 +1,38 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for Pose Measurement",
+  "definitions": {
+    "pose_measurement": {
+      "type": "object",
+      "properties": {
+        "pose_additional_delay": {
+          "type": "number",
+          "description": "Additional delay time for pose measurement [s]",
+          "default": 0.0
+        },
+        "pose_measure_uncertainty_time": {
+          "type": "number",
+          "description": "Measured time uncertainty used for covariance calculation [s]",
+          "default": 0.01
+        },
+        "pose_smoothing_steps": {
+          "type": "integer",
+          "description": "A value for smoothing steps",
+          "default": 5
+        },
+        "pose_gate_dist": {
+          "type": "number",
+          "description": "Limit of Mahalanobis distance used for outliers detection",
+          "default": 49.5
+        }
+      },
+      "required": [
+        "pose_additional_delay",
+        "pose_measure_uncertainty_time",
+        "pose_smoothing_steps",
+        "pose_gate_dist"
+      ],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json

@@ -0,0 +1,28 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for Process Noise",
+  "definitions": {
+    "process_noise": {
+      "type": "object",
+      "properties": {
+        "proc_stddev_vx_c": {
+          "type": "number",
+          "description": "Standard deviation of process noise in time differentiation expression of linear velocity x, noise for d_vx = 0",
+          "default": 10.0
+        },
+        "proc_stddev_wz_c": {
+          "type": "number",
+          "description": "Standard deviation of process noise in time differentiation expression of angular velocity z, noise for d_wz = 0",
+          "default": 5.0
+        },
+        "proc_stddev_yaw_c": {
+          "type": "number",
+          "description": "Standard deviation of process noise in time differentiation expression of yaw, noise for d_yaw = omega",
+          "default": 0.005
+        }
+      },
+      "required": ["proc_stddev_yaw_c", "proc_stddev_vx_c", "proc_stddev_wz_c"],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json

@@ -0,0 +1,28 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration of Simple 1D Filter Parameters",
+  "definitions": {
+    "simple_1d_filter_parameters": {
+      "type": "object",
+      "properties": {
+        "z_filter_proc_dev": {
+          "type": "number",
+          "description": "Simple1DFilter - Z filter process deviation",
+          "default": 1.0
+        },
+        "roll_filter_proc_dev": {
+          "type": "number",
+          "description": "Simple1DFilter - Roll filter process deviation",
+          "default": 0.1
+        },
+        "pitch_filter_proc_dev": {
+          "type": "number",
+          "description": "Simple1DFilter - Pitch filter process deviation",
+          "default": 0.1
+        }
+      },
+      "required": ["z_filter_proc_dev", "roll_filter_proc_dev", "pitch_filter_proc_dev"],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json

@@ -0,0 +1,28 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "EKF Localizer Configuration for Twist Measurement",
+  "definitions": {
+    "twist_measurement": {
+      "type": "object",
+      "properties": {
+        "twist_additional_delay": {
+          "type": "number",
+          "description": "Additional delay time for twist [s]",
+          "default": 0.0
+        },
+        "twist_smoothing_steps": {
+          "type": "integer",
+          "description": "A value for smoothing steps",
+          "default": 2
+        },
+        "twist_gate_dist": {
+          "type": "number",
+          "description": "Limit of Mahalanobis distance used for outliers detection",
+          "default": 46.1
+        }
+      },
+      "required": ["twist_additional_delay", "twist_smoothing_steps", "twist_gate_dist"],
+      "additionalProperties": false
+    }
+  }
+}

master/src/Localization/autoware_ekf_localizer/src/covariance.cpp

@@ -0,0 +1,56 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/covariance.hpp"
+
+#include "autoware/ekf_localizer/state_index.hpp"
+#include "autoware/universe_utils/ros/msg_covariance.hpp"
+
+namespace autoware::ekf_localizer
+{
+
+using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+
+std::array<double, 36> ekf_covariance_to_pose_message_covariance(const Matrix6d & P)
+{
+  std::array<double, 36> covariance{};
+  covariance.fill(0.);
+
+  covariance[COV_IDX::X_X] = P(IDX::X, IDX::X);
+  covariance[COV_IDX::X_Y] = P(IDX::X, IDX::Y);
+  covariance[COV_IDX::X_YAW] = P(IDX::X, IDX::YAW);
+  covariance[COV_IDX::Y_X] = P(IDX::Y, IDX::X);
+  covariance[COV_IDX::Y_Y] = P(IDX::Y, IDX::Y);
+  covariance[COV_IDX::Y_YAW] = P(IDX::Y, IDX::YAW);
+  covariance[COV_IDX::YAW_X] = P(IDX::YAW, IDX::X);
+  covariance[COV_IDX::YAW_Y] = P(IDX::YAW, IDX::Y);
+  covariance[COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW);
+
+  return covariance;
+}
+
+std::array<double, 36> ekf_covariance_to_twist_message_covariance(const Matrix6d & P)
+{
+  std::array<double, 36> covariance{};
+  covariance.fill(0.);
+
+  covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX);
+  covariance[COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ);
+  covariance[COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX);
+  covariance[COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ);
+
+  return covariance;
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/src/diagnostics.cpp

@@ -0,0 +1,207 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/diagnostics.hpp"
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::ekf_localizer
+{
+
+diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "is_activated";
+  key_value.value = is_activated ? "True" : "False";
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_activated) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]process is not activated";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated(
+  const std::string & measurement_type, const size_t no_update_count,
+  const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_no_update_count";
+  key_value.value = std::to_string(no_update_count);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_no_update_count_threshold_warn";
+  key_value.value = std::to_string(no_update_count_threshold_warn);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_no_update_count_threshold_error";
+  key_value.value = std::to_string(no_update_count_threshold_error);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (no_update_count >= no_update_count_threshold_warn) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + measurement_type + " is not updated";
+  }
+  if (no_update_count >= no_update_count_threshold_error) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "[ERROR]" + measurement_type + " is not updated";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size(
+  const std::string & measurement_type, const size_t queue_size)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_queue_size";
+  key_value.value = std::to_string(queue_size);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate(
+  const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time,
+  const double delay_time_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_is_passed_delay_gate";
+  key_value.value = is_passed_delay_gate ? "True" : "False";
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_delay_time";
+  key_value.value = std::to_string(delay_time);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_delay_time_threshold";
+  key_value.value = std::to_string(delay_time_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_passed_delay_gate) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + measurement_type + " topic is delay";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate(
+  const std::string & measurement_type, const bool is_passed_mahalanobis_gate,
+  const double mahalanobis_distance, const double mahalanobis_distance_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = measurement_type + "_is_passed_mahalanobis_gate";
+  key_value.value = is_passed_mahalanobis_gate ? "True" : "False";
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_mahalanobis_distance";
+  key_value.value = std::to_string(mahalanobis_distance);
+  stat.values.push_back(key_value);
+  key_value.key = measurement_type + "_mahalanobis_distance_threshold";
+  key_value.value = std::to_string(mahalanobis_distance_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (!is_passed_mahalanobis_gate) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large";
+  }
+
+  return stat;
+}
+
+diagnostic_msgs::msg::DiagnosticStatus check_covariance_ellipse(
+  const std::string & name, const double curr_size, const double warn_threshold,
+  const double error_threshold)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = name + "_size";
+  key_value.value = std::to_string(curr_size);
+  stat.values.push_back(key_value);
+
+  key_value.key = name + "_warn_threshold";
+  key_value.value = std::to_string(warn_threshold);
+  stat.values.push_back(key_value);
+
+  key_value.key = name + "_error_threshold";
+  key_value.value = std::to_string(error_threshold);
+  stat.values.push_back(key_value);
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (curr_size >= warn_threshold) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "[WARN]" + name + " is large";
+  }
+  if (curr_size >= error_threshold) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "[ERROR]" + name + " is large";
+  }
+
+  return stat;
+}
+
+// The highest level within the stat_array will be reflected in the merged_stat.
+// When all stat_array entries are 'OK,' the message of merged_stat will be "OK"
+diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
+  const std::vector<diagnostic_msgs::msg::DiagnosticStatus> & stat_array)
+{
+  diagnostic_msgs::msg::DiagnosticStatus merged_stat;
+
+  for (const auto & stat : stat_array) {
+    if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
+      if (!merged_stat.message.empty()) {
+        merged_stat.message += "; ";
+      }
+      merged_stat.message += stat.message;
+    }
+    if (stat.level > merged_stat.level) {
+      merged_stat.level = stat.level;
+    }
+    for (const auto & value : stat.values) {
+      merged_stat.values.push_back(value);
+    }
+  }
+
+  if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
+    merged_stat.message = "OK";
+  }
+
+  return merged_stat;
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/src/ekf_localizer.cpp

@@ -0,0 +1,461 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/ekf_localizer.hpp"
+
+#include "autoware/ekf_localizer/diagnostics.hpp"
+#include "autoware/ekf_localizer/string.hpp"
+#include "autoware/ekf_localizer/warning_message.hpp"
+#include "autoware/localization_util/covariance_ellipse.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/math/unit_conversion.hpp>
+#include <autoware/universe_utils/ros/msg_covariance.hpp>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/logging.hpp>
+
+#include <fmt/core.h>
+
+#include <algorithm>
+#include <functional>
+#include <memory>
+#include <queue>
+#include <string>
+#include <utility>
+
+namespace autoware::ekf_localizer
+{
+
+// clang-format off
+#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl // NOLINT
+#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} // NOLINT
+// clang-format on
+
+using std::placeholders::_1;
+
+EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options)
+: rclcpp::Node("ekf_localizer", node_options),
+  warning_(std::make_shared<Warning>(this)),
+  tf2_buffer_(this->get_clock()),
+  tf2_listener_(tf2_buffer_),
+  params_(this),
+  ekf_dt_(params_.ekf_dt),
+  pose_queue_(params_.pose_smoothing_steps),
+  twist_queue_(params_.twist_smoothing_steps)
+{
+  is_activated_ = false;
+
+  /* initialize ros system */
+  timer_control_ = rclcpp::create_timer(
+    this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_),
+    std::bind(&EKFLocalizer::timer_callback, this));
+
+  if (params_.publish_tf_) {
+    timer_tf_ = rclcpp::create_timer(
+      this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(),
+      std::bind(&EKFLocalizer::timer_tf_callback, this));
+  }
+
+  pub_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_pose", 1);
+  pub_pose_cov_ =
+    create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("ekf_pose_with_covariance", 1);
+  pub_odom_ = create_publisher<nav_msgs::msg::Odometry>("ekf_odom", 1);
+  pub_twist_ = create_publisher<geometry_msgs::msg::TwistStamped>("ekf_twist", 1);
+  pub_twist_cov_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "ekf_twist_with_covariance", 1);
+  pub_yaw_bias_ = create_publisher<tier4_debug_msgs::msg::Float64Stamped>("estimated_yaw_bias", 1);
+  pub_biased_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("ekf_biased_pose", 1);
+  pub_biased_pose_cov_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "ekf_biased_pose_with_covariance", 1);
+  pub_diag_ = this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
+  sub_initialpose_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1));
+  sub_pose_with_cov_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "in_pose_with_covariance", 1,
+    std::bind(&EKFLocalizer::callback_pose_with_covariance, this, _1));
+  sub_twist_with_cov_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "in_twist_with_covariance", 1,
+    std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1));
+  service_trigger_node_ = create_service<std_srvs::srv::SetBool>(
+    "trigger_node_srv",
+    std::bind(
+      &EKFLocalizer::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
+    rclcpp::ServicesQoS().get_rmw_qos_profile());
+
+  tf_br_ = std::make_shared<tf2_ros::TransformBroadcaster>(
+    std::shared_ptr<rclcpp::Node>(this, [](auto) {}));
+
+  ekf_module_ = std::make_unique<EKFModule>(warning_, params_);
+  logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
+}
+
+/*
+ * update_predict_frequency
+ */
+void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time)
+{
+  if (last_predict_time_) {
+    if (current_time < *last_predict_time_) {
+      warning_->warn("Detected jump back in time");
+    } else {
+      /* Measure dt */
+      ekf_dt_ = (current_time - *last_predict_time_).seconds();
+      DEBUG_INFO(
+        get_logger(), "[EKF] update ekf_dt_ to %f seconds (= %f hz)", ekf_dt_, 1 / ekf_dt_);
+
+      if (ekf_dt_ > 10.0) {
+        ekf_dt_ = 10.0;
+        RCLCPP_WARN(
+          get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_);
+      } else if (ekf_dt_ > static_cast<double>(params_.pose_smoothing_steps) / params_.ekf_rate) {
+        RCLCPP_WARN(
+          get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_);
+      }
+
+      /* Register dt and accumulate time delay */
+      ekf_module_->accumulate_delay_time(ekf_dt_);
+    }
+  }
+  last_predict_time_ = std::make_shared<const rclcpp::Time>(current_time);
+}
+
+/*
+ * timer_callback
+ */
+void EKFLocalizer::timer_callback()
+{
+  const rclcpp::Time current_time = this->now();
+
+  if (!is_activated_) {
+    warning_->warn_throttle(
+      "The node is not activated. Provide initial pose to pose_initializer", 2000);
+    publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time);
+    return;
+  }
+
+  DEBUG_INFO(get_logger(), "========================= timer called =========================");
+
+  /* update predict frequency with measured timer rate */
+  update_predict_frequency(current_time);
+
+  /* predict model in EKF */
+  stop_watch_.tic();
+  DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------");
+  ekf_module_->predict_with_delay(ekf_dt_);
+  DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc());
+  DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n");
+
+  /* pose measurement update */
+  pose_diag_info_.queue_size = pose_queue_.size();
+  pose_diag_info_.is_passed_delay_gate = true;
+  pose_diag_info_.delay_time = 0.0;
+  pose_diag_info_.delay_time_threshold = 0.0;
+  pose_diag_info_.is_passed_mahalanobis_gate = true;
+  pose_diag_info_.mahalanobis_distance = 0.0;
+
+  bool pose_is_updated = false;
+
+  if (!pose_queue_.empty()) {
+    DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------");
+    stop_watch_.tic();
+
+    // save the initial size because the queue size can change in the loop
+    const size_t n = pose_queue_.size();
+    for (size_t i = 0; i < n; ++i) {
+      const auto pose = pose_queue_.pop_increment_age();
+      bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_);
+      if (is_updated) {
+        pose_is_updated = true;
+      }
+    }
+    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;
+      }
+    }
+    DEBUG_INFO(
+      get_logger(), "[EKF] measurement_update_twist calc time = %f [ms]", stop_watch_.toc());
+    DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n");
+  }
+  twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1);
+
+  const geometry_msgs::msg::PoseStamped current_ekf_pose =
+    ekf_module_->get_current_pose(current_time, false);
+  const geometry_msgs::msg::PoseStamped current_biased_ekf_pose =
+    ekf_module_->get_current_pose(current_time, true);
+  const geometry_msgs::msg::TwistStamped current_ekf_twist =
+    ekf_module_->get_current_twist(current_time);
+
+  /* publish ekf result */
+  publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist);
+  publish_diagnostics(current_ekf_pose, current_time);
+}
+
+/*
+ * timer_tf_callback
+ */
+void EKFLocalizer::timer_tf_callback()
+{
+  if (!is_activated_) {
+    return;
+  }
+
+  if (params_.pose_frame_id.empty()) {
+    return;
+  }
+
+  const rclcpp::Time current_time = this->now();
+
+  geometry_msgs::msg::TransformStamped transform_stamped;
+  transform_stamped = autoware::universe_utils::pose2transform(
+    ekf_module_->get_current_pose(current_time, false), "base_link");
+  transform_stamped.header.stamp = current_time;
+  tf_br_->sendTransform(transform_stamped);
+}
+
+/*
+ * get_transform_from_tf
+ */
+bool EKFLocalizer::get_transform_from_tf(
+  std::string parent_frame, std::string child_frame,
+  geometry_msgs::msg::TransformStamped & transform)
+{
+  parent_frame = erase_leading_slash(parent_frame);
+  child_frame = erase_leading_slash(child_frame);
+
+  try {
+    transform = tf2_buffer_.lookupTransform(parent_frame, child_frame, tf2::TimePointZero);
+    return true;
+  } catch (tf2::TransformException & ex) {
+    warning_->warn(ex.what());
+  }
+  return false;
+}
+
+/*
+ * callback_initial_pose
+ */
+void EKFLocalizer::callback_initial_pose(
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+  geometry_msgs::msg::TransformStamped transform;
+  if (!get_transform_from_tf(params_.pose_frame_id, msg->header.frame_id, transform)) {
+    RCLCPP_ERROR(
+      get_logger(), "[EKF] TF transform failed. parent = %s, child = %s",
+      params_.pose_frame_id.c_str(), msg->header.frame_id.c_str());
+  }
+  ekf_module_->initialize(*msg, transform);
+}
+
+/*
+ * callback_pose_with_covariance
+ */
+void EKFLocalizer::callback_pose_with_covariance(
+  geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+  if (!is_activated_) {
+    return;
+  }
+
+  pose_queue_.push(msg);
+
+  publish_callback_return_diagnostics("pose", msg->header.stamp);
+}
+
+/*
+ * callback_twist_with_covariance
+ */
+void EKFLocalizer::callback_twist_with_covariance(
+  geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
+{
+  // Ignore twist if velocity is too small.
+  // Note that this inequality must not include "equal".
+  if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) {
+    msg->twist.covariance[0 * 6 + 0] = 10000.0;
+  }
+  twist_queue_.push(msg);
+
+  publish_callback_return_diagnostics("twist", msg->header.stamp);
+}
+
+/*
+ * publish_estimate_result
+ */
+void EKFLocalizer::publish_estimate_result(
+  const geometry_msgs::msg::PoseStamped & current_ekf_pose,
+  const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose,
+  const geometry_msgs::msg::TwistStamped & current_ekf_twist)
+{
+  /* publish latest pose */
+  pub_pose_->publish(current_ekf_pose);
+  pub_biased_pose_->publish(current_biased_ekf_pose);
+
+  /* publish latest pose with covariance */
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_cov;
+  pose_cov.header.stamp = current_ekf_pose.header.stamp;
+  pose_cov.header.frame_id = current_ekf_pose.header.frame_id;
+  pose_cov.pose.pose = current_ekf_pose.pose;
+  pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance();
+  pub_pose_cov_->publish(pose_cov);
+
+  geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov;
+  biased_pose_cov.pose.pose = current_biased_ekf_pose.pose;
+  pub_biased_pose_cov_->publish(biased_pose_cov);
+
+  /* publish latest twist */
+  pub_twist_->publish(current_ekf_twist);
+
+  /* publish latest twist with covariance */
+  geometry_msgs::msg::TwistWithCovarianceStamped twist_cov;
+  twist_cov.header.stamp = current_ekf_twist.header.stamp;
+  twist_cov.header.frame_id = current_ekf_twist.header.frame_id;
+  twist_cov.twist.twist = current_ekf_twist.twist;
+  twist_cov.twist.covariance = ekf_module_->get_current_twist_covariance();
+  pub_twist_cov_->publish(twist_cov);
+
+  /* publish yaw bias */
+  tier4_debug_msgs::msg::Float64Stamped yawb;
+  yawb.stamp = current_ekf_twist.header.stamp;
+  yawb.data = ekf_module_->get_yaw_bias();
+  pub_yaw_bias_->publish(yawb);
+
+  /* publish latest odometry */
+  nav_msgs::msg::Odometry odometry;
+  odometry.header.stamp = current_ekf_pose.header.stamp;
+  odometry.header.frame_id = current_ekf_pose.header.frame_id;
+  odometry.child_frame_id = "base_link";
+  odometry.pose = pose_cov.pose;
+  odometry.twist = twist_cov.twist;
+  pub_odom_->publish(odometry);
+}
+
+void EKFLocalizer::publish_diagnostics(
+  const geometry_msgs::msg::PoseStamped & current_ekf_pose, const rclcpp::Time & current_time)
+{
+  std::vector<diagnostic_msgs::msg::DiagnosticStatus> diag_status_array;
+
+  diag_status_array.push_back(check_process_activated(is_activated_));
+
+  if (is_activated_) {
+    diag_status_array.push_back(check_measurement_updated(
+      "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn,
+      params_.pose_no_update_count_threshold_error));
+    diag_status_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size));
+    diag_status_array.push_back(check_measurement_delay_gate(
+      "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time,
+      pose_diag_info_.delay_time_threshold));
+    diag_status_array.push_back(check_measurement_mahalanobis_gate(
+      "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance,
+      params_.pose_gate_dist));
+
+    diag_status_array.push_back(check_measurement_updated(
+      "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn,
+      params_.twist_no_update_count_threshold_error));
+    diag_status_array.push_back(check_measurement_queue_size("twist", twist_diag_info_.queue_size));
+    diag_status_array.push_back(check_measurement_delay_gate(
+      "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time,
+      twist_diag_info_.delay_time_threshold));
+    diag_status_array.push_back(check_measurement_mahalanobis_gate(
+      "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance,
+      params_.twist_gate_dist));
+
+    geometry_msgs::msg::PoseWithCovariance pose_cov;
+    pose_cov.pose = current_ekf_pose.pose;
+    pose_cov.covariance = ekf_module_->get_current_pose_covariance();
+    const autoware::localization_util::Ellipse ellipse =
+      autoware::localization_util::calculate_xy_ellipse(pose_cov, params_.ellipse_scale);
+    diag_status_array.push_back(check_covariance_ellipse(
+      "cov_ellipse_long_axis", ellipse.long_radius, params_.warn_ellipse_size,
+      params_.error_ellipse_size));
+    diag_status_array.push_back(check_covariance_ellipse(
+      "cov_ellipse_lateral_direction", ellipse.size_lateral_direction,
+      params_.warn_ellipse_size_lateral_direction, params_.error_ellipse_size_lateral_direction));
+  }
+
+  diagnostic_msgs::msg::DiagnosticStatus diag_merged_status;
+  diag_merged_status = merge_diagnostic_status(diag_status_array);
+  diag_merged_status.name = "localization: " + std::string(this->get_name());
+  diag_merged_status.hardware_id = this->get_name();
+
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = current_time;
+  diag_msg.status.push_back(diag_merged_status);
+  pub_diag_->publish(diag_msg);
+}
+
+void EKFLocalizer::publish_callback_return_diagnostics(
+  const std::string & callback_name, const rclcpp::Time & current_time)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "topic_time_stamp";
+  key_value.value = std::to_string(current_time.nanoseconds());
+  diagnostic_msgs::msg::DiagnosticStatus diag_status;
+  diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  diag_status.name =
+    "localization: " + std::string(this->get_name()) + ": callback_" + callback_name;
+  diag_status.hardware_id = this->get_name();
+  diag_status.message = "OK";
+  diag_status.values.push_back(key_value);
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = current_time;
+  diag_msg.status.push_back(diag_status);
+  pub_diag_->publish(diag_msg);
+}
+
+/**
+ * @brief trigger node
+ */
+void EKFLocalizer::service_trigger_node(
+  const std_srvs::srv::SetBool::Request::SharedPtr req,
+  std_srvs::srv::SetBool::Response::SharedPtr res)
+{
+  if (req->data) {
+    pose_queue_.clear();
+    twist_queue_.clear();
+    is_activated_ = true;
+  } else {
+    is_activated_ = false;
+  }
+  res->success = true;
+}
+
+}  // namespace autoware::ekf_localizer
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::ekf_localizer::EKFLocalizer)

master/src/Localization/autoware_ekf_localizer/src/ekf_module.cpp

@@ -0,0 +1,459 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/ekf_module.hpp"
+
+#include "autoware/ekf_localizer/covariance.hpp"
+#include "autoware/ekf_localizer/mahalanobis.hpp"
+#include "autoware/ekf_localizer/matrix_types.hpp"
+#include "autoware/ekf_localizer/measurement.hpp"
+#include "autoware/ekf_localizer/numeric.hpp"
+#include "autoware/ekf_localizer/state_transition.hpp"
+#include "autoware/ekf_localizer/warning_message.hpp"
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/ros/msg_covariance.hpp>
+
+#include <fmt/core.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/utils.h>
+
+#include <algorithm>
+#include <utility>
+
+namespace autoware::ekf_localizer
+{
+
+// clang-format off
+#define DEBUG_PRINT_MAT(X) {if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}} // NOLINT
+// clang-format on
+
+EKFModule::EKFModule(std::shared_ptr<Warning> warning, const HyperParameters & params)
+: warning_(std::move(warning)),
+  dim_x_(6),  // x, y, yaw, yaw_bias, vx, wz
+  accumulated_delay_times_(params.extend_state_step, 1.0E15),
+  params_(params),
+  last_angular_velocity_(0.0, 0.0, 0.0)
+{
+  Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1);
+  Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15;  // for x & y
+  p(IDX::YAW, IDX::YAW) = 50.0;                                            // for yaw
+  if (params_.enable_yaw_bias_estimation) {
+    p(IDX::YAWB, IDX::YAWB) = 50.0;  // for yaw bias
+  }
+  p(IDX::VX, IDX::VX) = 1000.0;  // for vx
+  p(IDX::WZ, IDX::WZ) = 50.0;    // for wz
+
+  kalman_filter_.init(x, p, static_cast<int>(params_.extend_state_step));
+  z_filter_.set_proc_var(params_.z_filter_proc_dev * params_.z_filter_proc_dev);
+  roll_filter_.set_proc_var(params_.roll_filter_proc_dev * params_.roll_filter_proc_dev);
+  pitch_filter_.set_proc_var(params_.pitch_filter_proc_dev * params_.pitch_filter_proc_dev);
+}
+
+void EKFModule::initialize(
+  const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform)
+{
+  Eigen::MatrixXd x(dim_x_, 1);
+  Eigen::MatrixXd p = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
+
+  x(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x;
+  x(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y;
+  x(IDX::YAW) =
+    tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation);
+  x(IDX::YAWB) = 0.0;
+  x(IDX::VX) = 0.0;
+  x(IDX::WZ) = 0.0;
+
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X];
+  p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y];
+  p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW];
+
+  if (params_.enable_yaw_bias_estimation) {
+    p(IDX::YAWB, IDX::YAWB) = 0.0001;
+  }
+  p(IDX::VX, IDX::VX) = 0.01;
+  p(IDX::WZ, IDX::WZ) = 0.01;
+
+  kalman_filter_.init(x, p, static_cast<int>(params_.extend_state_step));
+
+  const double z = initial_pose.pose.pose.position.z;
+
+  const auto rpy = autoware::universe_utils::getRPY(initial_pose.pose.pose.orientation);
+
+  const double z_var = initial_pose.pose.covariance[COV_IDX::Z_Z];
+  const double roll_var = initial_pose.pose.covariance[COV_IDX::ROLL_ROLL];
+  const double pitch_var = initial_pose.pose.covariance[COV_IDX::PITCH_PITCH];
+
+  z_filter_.init(z, z_var);
+  roll_filter_.init(rpy.x, roll_var);
+  pitch_filter_.init(rpy.y, pitch_var);
+}
+
+geometry_msgs::msg::PoseStamped EKFModule::get_current_pose(
+  const rclcpp::Time & current_time, bool get_biased_yaw) const
+{
+  const double z = z_filter_.get_x();
+  const double roll = roll_filter_.get_x();
+  const double pitch = pitch_filter_.get_x();
+
+  const double x = kalman_filter_.getXelement(IDX::X);
+  const double y = kalman_filter_.getXelement(IDX::Y);
+  /*
+    getXelement(IDX::YAW) is surely `biased_yaw`.
+    Please note how `yaw` and `yaw_bias` are used in the state transition model and
+    how the observed pose is handled in the measurement pose update.
+  */
+  const double biased_yaw = kalman_filter_.getXelement(IDX::YAW);
+  const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB);
+  const double yaw = biased_yaw + yaw_bias;
+
+  Pose current_ekf_pose;
+  current_ekf_pose.header.frame_id = params_.pose_frame_id;
+  current_ekf_pose.header.stamp = current_time;
+  current_ekf_pose.pose.position = autoware::universe_utils::createPoint(x, y, z);
+  if (get_biased_yaw) {
+    current_ekf_pose.pose.orientation =
+      autoware::universe_utils::createQuaternionFromRPY(roll, pitch, biased_yaw);
+  } else {
+    current_ekf_pose.pose.orientation =
+      autoware::universe_utils::createQuaternionFromRPY(roll, pitch, yaw);
+  }
+  return current_ekf_pose;
+}
+
+geometry_msgs::msg::TwistStamped EKFModule::get_current_twist(
+  const rclcpp::Time & current_time) const
+{
+  const double vx = kalman_filter_.getXelement(IDX::VX);
+  const double wz = kalman_filter_.getXelement(IDX::WZ);
+
+  Twist current_ekf_twist;
+  current_ekf_twist.header.frame_id = "base_link";
+  current_ekf_twist.header.stamp = current_time;
+  current_ekf_twist.twist.linear.x = vx;
+  current_ekf_twist.twist.angular.z = wz;
+  return current_ekf_twist;
+}
+
+std::array<double, 36> EKFModule::get_current_pose_covariance() const
+{
+  std::array<double, 36> cov =
+    ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP());
+
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  cov[COV_IDX::Z_Z] = z_filter_.get_var();
+  cov[COV_IDX::ROLL_ROLL] = roll_filter_.get_var();
+  cov[COV_IDX::PITCH_PITCH] = pitch_filter_.get_var();
+
+  return cov;
+}
+
+std::array<double, 36> EKFModule::get_current_twist_covariance() const
+{
+  return ekf_covariance_to_twist_message_covariance(kalman_filter_.getLatestP());
+}
+
+double EKFModule::get_yaw_bias() const
+{
+  return kalman_filter_.getLatestX()(IDX::YAWB);
+}
+
+size_t EKFModule::find_closest_delay_time_index(double target_value) const
+{
+  // If target_value is too large, return last index + 1
+  if (target_value > accumulated_delay_times_.back()) {
+    return accumulated_delay_times_.size();
+  }
+
+  auto lower = std::lower_bound(
+    accumulated_delay_times_.begin(), accumulated_delay_times_.end(), target_value);
+
+  // If the lower bound is the first element, return its index.
+  // If the lower bound is beyond the last element, return the last index.
+  // If else, take the closest element.
+  if (lower == accumulated_delay_times_.begin()) {
+    return 0;
+  }
+  if (lower == accumulated_delay_times_.end()) {
+    return accumulated_delay_times_.size() - 1;
+  }
+  // Compare the target with the lower bound and the previous element.
+  auto prev = lower - 1;
+  bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value);
+
+  // Return the index of the closer element.
+  return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev)
+                           : std::distance(accumulated_delay_times_.begin(), lower);
+}
+
+void EKFModule::accumulate_delay_time(const double dt)
+{
+  // Shift the delay times to the right.
+  std::copy_backward(
+    accumulated_delay_times_.begin(), accumulated_delay_times_.end() - 1,
+    accumulated_delay_times_.end());
+
+  // Add a new element (=0) and, and add delay time to the previous elements.
+  accumulated_delay_times_.front() = 0.0;
+  for (size_t i = 1; i < accumulated_delay_times_.size(); ++i) {
+    accumulated_delay_times_[i] += dt;
+  }
+}
+
+void EKFModule::predict_with_delay(const double dt)
+{
+  const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX();
+  const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP();
+
+  const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0);
+  const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0);
+  const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0);
+
+  const Vector6d x_next = predict_next_state(x_curr, dt);
+  const Matrix6d a = create_state_transition_matrix(x_curr, dt);
+  const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d);
+  kalman_filter_.predictWithDelay(x_next, a, q);
+  ekf_dt_ = dt;
+}
+
+bool EKFModule::measurement_update_pose(
+  const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info)
+{
+  if (pose.header.frame_id != params_.pose_frame_id) {
+    warning_->warn_throttle(
+      fmt::format(
+        "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
+        pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()),
+      2000);
+  }
+  const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX();
+  DEBUG_PRINT_MAT(x_curr.transpose());
+
+  constexpr int dim_y = 3;  // pos_x, pos_y, yaw, depending on Pose output
+
+  /* Calculate delay step */
+  double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay;
+  if (delay_time < 0.0) {
+    warning_->warn_throttle(pose_delay_time_warning_message(delay_time), 1000);
+  }
+
+  delay_time = std::max(delay_time, 0.0);
+
+  const size_t delay_step = find_closest_delay_time_index(delay_time);
+
+  pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time);
+  pose_diag_info.delay_time_threshold = accumulated_delay_times_.back();
+  if (delay_step >= params_.extend_state_step) {
+    pose_diag_info.is_passed_delay_gate = false;
+    warning_->warn_throttle(
+      pose_delay_step_warning_message(
+        pose_diag_info.delay_time, pose_diag_info.delay_time_threshold),
+      2000);
+    return false;
+  }
+
+  /* Since the kalman filter cannot handle the rotation angle directly,
+    offset the yaw angle so that the difference from the yaw angle that ekf holds internally
+    is less than 2 pi. */
+  double yaw = tf2::getYaw(pose.pose.pose.orientation);
+  const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW);
+  const double yaw_error = normalize_yaw(yaw - ekf_yaw);  // normalize the error not to exceed 2 pi
+  yaw = yaw_error + ekf_yaw;
+
+  /* Set measurement matrix */
+  Eigen::MatrixXd y(dim_y, 1);
+  y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw;
+
+  if (has_nan(y) || has_inf(y)) {
+    warning_->warn(
+      "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
+    return false;
+  }
+
+  /* Gate */
+  const Eigen::Vector3d y_ekf(
+    kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X),
+    kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw);
+  const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP();
+  const Eigen::MatrixXd p_y = p_curr.block(0, 0, dim_y, dim_y);
+
+  const double distance = mahalanobis(y_ekf, y, p_y);
+  pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance);
+  if (distance > params_.pose_gate_dist) {
+    pose_diag_info.is_passed_mahalanobis_gate = false;
+    warning_->warn_throttle(mahalanobis_warning_message(distance, params_.pose_gate_dist), 2000);
+    warning_->warn_throttle("Ignore the measurement data.", 2000);
+    return false;
+  }
+
+  DEBUG_PRINT_MAT(y.transpose());
+  DEBUG_PRINT_MAT(y_ekf.transpose());
+  DEBUG_PRINT_MAT((y - y_ekf).transpose());
+
+  const Eigen::Matrix<double, 3, 6> c = pose_measurement_matrix();
+  const Eigen::Matrix3d r =
+    pose_measurement_covariance(pose.pose.covariance, params_.pose_smoothing_steps);
+
+  kalman_filter_.updateWithDelay(y, c, r, static_cast<int>(delay_step));
+
+  // Update Simple 1D filter with considering change of roll, pitch and height (position z)
+  // values due to measurement pose delay
+  auto pose_with_rph_delay_compensation =
+    compensate_rph_with_delay(pose, last_angular_velocity_, delay_time);
+  update_simple_1d_filters(pose_with_rph_delay_compensation, params_.pose_smoothing_steps);
+
+  // debug
+  const Eigen::MatrixXd x_result = kalman_filter_.getLatestX();
+  DEBUG_PRINT_MAT(x_result.transpose());
+  DEBUG_PRINT_MAT((x_result - x_curr).transpose());
+
+  return true;
+}
+
+geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_rph_with_delay(
+  const PoseWithCovariance & pose, tf2::Vector3 last_angular_velocity, 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 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(
+  const TwistWithCovariance & twist, const rclcpp::Time & t_curr,
+  EKFDiagnosticInfo & twist_diag_info)
+{
+  if (twist.header.frame_id != "base_link") {
+    warning_->warn_throttle("twist frame_id must be base_link", 2000);
+  }
+
+  last_angular_velocity_ = tf2::Vector3(0.0, 0.0, 0.0);
+
+  const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX();
+  DEBUG_PRINT_MAT(x_curr.transpose());
+
+  constexpr int dim_y = 2;  // vx, wz
+
+  /* Calculate delay step */
+  double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay;
+  if (delay_time < -0.6) {
+    warning_->warn_throttle(twist_delay_time_warning_message(delay_time), 1000);
+  }
+  delay_time = std::max(delay_time, 0.0);
+
+  const size_t delay_step = find_closest_delay_time_index(delay_time);
+
+  twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time);
+  twist_diag_info.delay_time_threshold = accumulated_delay_times_.back();
+  if (delay_step >= params_.extend_state_step) {
+    twist_diag_info.is_passed_delay_gate = false;
+    warning_->warn_throttle(
+      twist_delay_step_warning_message(
+        twist_diag_info.delay_time, twist_diag_info.delay_time_threshold),
+      2000);
+    return false;
+  }
+
+  /* Set measurement matrix */
+  Eigen::MatrixXd y(dim_y, 1);
+  y << twist.twist.twist.linear.x, 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.");
+    return false;
+  }
+
+  const Eigen::Vector2d y_ekf(
+    kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX),
+    kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ));
+  const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP();
+  const Eigen::MatrixXd p_y = p_curr.block(4, 4, dim_y, dim_y);
+
+  const double distance = mahalanobis(y_ekf, y, p_y);
+  twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance);
+  if (distance > params_.twist_gate_dist) {
+    twist_diag_info.is_passed_mahalanobis_gate = false;
+    warning_->warn_throttle(mahalanobis_warning_message(distance, params_.twist_gate_dist), 2000);
+    warning_->warn_throttle("Ignore the measurement data.", 2000);
+    return false;
+  }
+
+  DEBUG_PRINT_MAT(y.transpose());
+  DEBUG_PRINT_MAT(y_ekf.transpose());
+  DEBUG_PRINT_MAT((y - y_ekf).transpose());
+
+  const Eigen::Matrix<double, 2, 6> c = twist_measurement_matrix();
+  const Eigen::Matrix2d r =
+    twist_measurement_covariance(twist.twist.covariance, params_.twist_smoothing_steps);
+
+  kalman_filter_.updateWithDelay(y, c, r, static_cast<int>(delay_step));
+
+  last_angular_velocity_ = tf2::Vector3(
+    twist.twist.twist.angular.x, twist.twist.twist.angular.y, twist.twist.twist.angular.z);
+
+  // debug
+  const Eigen::MatrixXd x_result = kalman_filter_.getLatestX();
+  DEBUG_PRINT_MAT(x_result.transpose());
+  DEBUG_PRINT_MAT((x_result - x_curr).transpose());
+
+  return true;
+}
+
+void EKFModule::update_simple_1d_filters(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step)
+{
+  double z = pose.pose.pose.position.z;
+
+  const auto rpy = autoware::universe_utils::getRPY(pose.pose.pose.orientation);
+
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  double z_var = pose.pose.covariance[COV_IDX::Z_Z] * static_cast<double>(smoothing_step);
+  double roll_var = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast<double>(smoothing_step);
+  double pitch_var =
+    pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast<double>(smoothing_step);
+
+  z_filter_.update(z, z_var, ekf_dt_);
+  roll_filter_.update(rpy.x, roll_var, ekf_dt_);
+  pitch_filter_.update(rpy.y, pitch_var, ekf_dt_);
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/src/mahalanobis.cpp

@@ -0,0 +1,32 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/mahalanobis.hpp"
+
+namespace autoware::ekf_localizer
+{
+
+double squared_mahalanobis(
+  const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C)
+{
+  const Eigen::VectorXd d = x - y;
+  return d.dot(C.inverse() * d);
+}
+
+double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C)
+{
+  return std::sqrt(squared_mahalanobis(x, y, C));
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/src/measurement.cpp

@@ -0,0 +1,61 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/measurement.hpp"
+
+#include "autoware/ekf_localizer/state_index.hpp"
+#include "autoware/universe_utils/ros/msg_covariance.hpp"
+
+namespace autoware::ekf_localizer
+{
+
+Eigen::Matrix<double, 3, 6> pose_measurement_matrix()
+{
+  Eigen::Matrix<double, 3, 6> c = Eigen::Matrix<double, 3, 6>::Zero();
+  c(0, IDX::X) = 1.0;    // for pos x
+  c(1, IDX::Y) = 1.0;    // for pos y
+  c(2, IDX::YAW) = 1.0;  // for yaw
+  return c;
+}
+
+Eigen::Matrix<double, 2, 6> twist_measurement_matrix()
+{
+  Eigen::Matrix<double, 2, 6> c = Eigen::Matrix<double, 2, 6>::Zero();
+  c(0, IDX::VX) = 1.0;  // for vx
+  c(1, IDX::WZ) = 1.0;  // for wz
+  return c;
+}
+
+Eigen::Matrix3d pose_measurement_covariance(
+  const std::array<double, 36ul> & covariance, const size_t smoothing_step)
+{
+  Eigen::Matrix3d r;
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW),
+    covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW),
+    covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW);
+  return r * static_cast<double>(smoothing_step);
+}
+
+Eigen::Matrix2d twist_measurement_covariance(
+  const std::array<double, 36ul> & covariance, const size_t smoothing_step)
+{
+  Eigen::Matrix2d r;
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X),
+    covariance.at(COV_IDX::YAW_YAW);
+  return r * static_cast<double>(smoothing_step);
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/src/state_transition.cpp

@@ -0,0 +1,102 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/state_transition.hpp"
+
+#include "autoware/ekf_localizer/matrix_types.hpp"
+#include "autoware/ekf_localizer/state_index.hpp"
+
+#include <cmath>
+
+namespace autoware::ekf_localizer
+{
+
+double normalize_yaw(const double & yaw)
+{
+  // FIXME(IshitaTakeshi) I think the computation here can be simplified
+  // FIXME(IshitaTakeshi) Rename the function. This is not normalization
+  return std::atan2(std::sin(yaw), std::cos(yaw));
+}
+
+/*  == Nonlinear model ==
+ *
+ * x_{k+1}   = x_k + vx_k * cos(yaw_k + b_k) * dt
+ * y_{k+1}   = y_k + vx_k * sin(yaw_k + b_k) * dt
+ * yaw_{k+1} = yaw_k + (wz_k) * dt
+ * b_{k+1}   = b_k
+ * vx_{k+1}  = vx_k
+ * wz_{k+1}  = wz_k
+ *
+ * (b_k : yaw_bias_k)
+ */
+
+Vector6d predict_next_state(const Vector6d & X_curr, const double dt)
+{
+  const double x = X_curr(IDX::X);
+  const double y = X_curr(IDX::Y);
+  const double yaw = X_curr(IDX::YAW);
+  const double yaw_bias = X_curr(IDX::YAWB);
+  const double vx = X_curr(IDX::VX);
+  const double wz = X_curr(IDX::WZ);
+
+  Vector6d x_next;
+  x_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt;  // dx = v * cos(yaw)
+  x_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt;  // dy = v * sin(yaw)
+  x_next(IDX::YAW) = normalize_yaw(yaw + wz * dt);          // dyaw = omega + omega_bias
+  x_next(IDX::YAWB) = yaw_bias;
+  x_next(IDX::VX) = vx;
+  x_next(IDX::WZ) = wz;
+  return x_next;
+}
+
+/*  == Linearized model ==
+ *
+ * A = [ 1, 0, -vx*sin(yaw+b)*dt, -vx*sin(yaw+b)*dt, cos(yaw+b)*dt,  0]
+ *     [ 0, 1,  vx*cos(yaw+b)*dt,  vx*cos(yaw+b)*dt, sin(yaw+b)*dt,  0]
+ *     [ 0, 0,                 1,                 0,             0, dt]
+ *     [ 0, 0,                 0,                 1,             0,  0]
+ *     [ 0, 0,                 0,                 0,             1,  0]
+ *     [ 0, 0,                 0,                 0,             0,  1]
+ */
+Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt)
+{
+  const double yaw = X_curr(IDX::YAW);
+  const double yaw_bias = X_curr(IDX::YAWB);
+  const double vx = X_curr(IDX::VX);
+
+  Matrix6d a = Matrix6d::Identity();
+  a(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt;
+  a(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt;
+  a(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt;
+  a(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt;
+  a(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt;
+  a(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt;
+  a(IDX::YAW, IDX::WZ) = dt;
+  return a;
+}
+
+Matrix6d process_noise_covariance(
+  const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d)
+{
+  Matrix6d q = Matrix6d::Zero();
+  q(IDX::X, IDX::X) = 0.0;
+  q(IDX::Y, IDX::Y) = 0.0;
+  q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d;  // for yaw
+  q(IDX::YAWB, IDX::YAWB) = 0.0;
+  q(IDX::VX, IDX::VX) = proc_cov_vx_d;  // for vx
+  q(IDX::WZ, IDX::WZ) = proc_cov_wz_d;  // for wz
+  return q;
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/src/warning_message.cpp

@@ -0,0 +1,60 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/warning_message.hpp"
+
+#include <fmt/core.h>
+
+#include <string>
+
+namespace autoware::ekf_localizer
+{
+
+std::string pose_delay_step_warning_message(
+  const double delay_time, const double delay_time_threshold)
+{
+  const std::string s =
+    "Pose delay exceeds the compensation limit, ignored. "
+    "delay: {:.3f}[s], limit: {:.3f}[s]";
+  return fmt::format(s, delay_time, delay_time_threshold);
+}
+
+std::string twist_delay_step_warning_message(
+  const double delay_time, const double delay_time_threshold)
+{
+  const std::string s =
+    "Twist delay exceeds the compensation limit, ignored. "
+    "delay: {:.3f}[s], limit: {:.3f}[s]";
+  return fmt::format(s, delay_time, delay_time_threshold);
+}
+
+std::string pose_delay_time_warning_message(const double delay_time)
+{
+  const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}";
+  return fmt::format(s, delay_time);
+}
+
+std::string twist_delay_time_warning_message(const double delay_time)
+{
+  const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}";
+  return fmt::format(s, delay_time);
+}
+
+std::string mahalanobis_warning_message(const double distance, const double max_distance)
+{
+  const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}.";
+  return fmt::format(s, distance, max_distance);
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp

@@ -0,0 +1,106 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/aged_object_queue.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(AgedObjectQueue, DiscardsObjectWhenAgeReachesMaximum)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+  EXPECT_EQ(queue.size(), 1U);
+
+  queue.pop_increment_age();  // age = 1
+  EXPECT_EQ(queue.size(), 1U);
+
+  queue.pop_increment_age();  // age = 2
+  EXPECT_EQ(queue.size(), 1U);
+
+  queue.pop_increment_age();  // age = 3
+  EXPECT_EQ(queue.size(), 0U);
+}
+
+TEST(AgedObjectQueue, MultipleObjects)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+  EXPECT_EQ(queue.size(), 1U);
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"a"});  // age of a = 1
+  EXPECT_EQ(queue.size(), 1U);
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"a"});  // age of a = 2
+
+  queue.push("b");
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"a"});  // age of a = 3
+  EXPECT_EQ(queue.size(), 1U);
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"b"});  // age of b = 1
+  EXPECT_EQ(queue.size(), 1U);
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"b"});  // age of b = 2
+  EXPECT_EQ(queue.size(), 1U);
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"b"});  // age of b = 3
+  EXPECT_EQ(queue.size(), 0U);
+}
+
+TEST(AgedObjectQueue, Empty)
+{
+  AgedObjectQueue<std::string> queue(2);
+
+  EXPECT_TRUE(queue.empty());
+
+  queue.push("a");
+
+  EXPECT_FALSE(queue.empty());
+
+  queue.pop_increment_age();
+  queue.pop_increment_age();
+
+  EXPECT_TRUE(queue.empty());
+}
+
+TEST(AgedObjectQueue, Clear)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+  queue.push("b");
+
+  EXPECT_EQ(queue.size(), 2U);
+
+  queue.clear();
+
+  EXPECT_EQ(queue.size(), 0U);
+}
+
+TEST(AgedObjectQueue, Back)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+
+  EXPECT_EQ(queue.back(), std::string{"a"});
+  queue.push("b");
+
+  EXPECT_EQ(queue.back(), std::string{"b"});
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_covariance.cpp

@@ -0,0 +1,84 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/covariance.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest)
+{
+  {
+    Matrix6d p = Matrix6d::Zero();
+    p(0, 0) = 1.;
+    p(0, 1) = 2.;
+    p(0, 2) = 3.;
+    p(1, 0) = 4.;
+    p(1, 1) = 5.;
+    p(1, 2) = 6.;
+    p(2, 0) = 7.;
+    p(2, 1) = 8.;
+    p(2, 2) = 9.;
+
+    std::array<double, 36> covariance = ekf_covariance_to_pose_message_covariance(p);
+    EXPECT_EQ(covariance[0], 1.);
+    EXPECT_EQ(covariance[1], 2.);
+    EXPECT_EQ(covariance[5], 3.);
+    EXPECT_EQ(covariance[6], 4.);
+    EXPECT_EQ(covariance[7], 5.);
+    EXPECT_EQ(covariance[11], 6.);
+    EXPECT_EQ(covariance[30], 7.);
+    EXPECT_EQ(covariance[31], 8.);
+    EXPECT_EQ(covariance[35], 9.);
+  }
+
+  // ensure other elements are zero
+  {
+    Matrix6d p = Matrix6d::Zero();
+    std::array<double, 36> covariance = ekf_covariance_to_pose_message_covariance(p);
+    for (double e : covariance) {
+      EXPECT_EQ(e, 0.);
+    }
+  }
+}
+
+TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest)
+{
+  {
+    Matrix6d p = Matrix6d::Zero();
+    p(4, 4) = 1.;
+    p(4, 5) = 2.;
+    p(5, 4) = 3.;
+    p(5, 5) = 4.;
+
+    std::array<double, 36> covariance = ekf_covariance_to_twist_message_covariance(p);
+    EXPECT_EQ(covariance[0], 1.);
+    EXPECT_EQ(covariance[5], 2.);
+    EXPECT_EQ(covariance[30], 3.);
+    EXPECT_EQ(covariance[35], 4.);
+  }
+
+  // ensure other elements are zero
+  {
+    Matrix6d p = Matrix6d::Zero();
+    std::array<double, 36> covariance = ekf_covariance_to_twist_message_covariance(p);
+    for (double e : covariance) {
+      EXPECT_EQ(e, 0.);
+    }
+  }
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_diagnostics.cpp

@@ -0,0 +1,197 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/diagnostics.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(TestEkfDiagnostics, check_process_activated)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  bool is_activated = true;
+  stat = check_process_activated(is_activated);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_activated = false;
+  stat = check_process_activated(is_activated);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_updated)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";  // not effect for stat.level
+  const size_t no_update_count_threshold_warn = 50;
+  const size_t no_update_count_threshold_error = 250;
+
+  size_t no_update_count = 0;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  no_update_count = 1;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  no_update_count = 49;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  no_update_count = 50;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  no_update_count = 249;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  no_update_count = 250;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_queue_size)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";  // not effect for stat.level
+
+  size_t queue_size = 0;  // not effect for stat.level
+  stat = check_measurement_queue_size(measurement_type, queue_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  queue_size = 1;  // not effect for stat.level
+  stat = check_measurement_queue_size(measurement_type, queue_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_delay_gate)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";  // not effect for stat.level
+  const double delay_time = 0.1;                // not effect for stat.level
+  const double delay_time_threshold = 1.0;      // not effect for stat.level
+
+  bool is_passed_delay_gate = true;
+  stat = check_measurement_delay_gate(
+    measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_passed_delay_gate = false;
+  stat = check_measurement_delay_gate(
+    measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";        // not effect for stat.level
+  const double mahalanobis_distance = 0.1;            // not effect for stat.level
+  const double mahalanobis_distance_threshold = 1.0;  // not effect for stat.level
+
+  bool is_passed_mahalanobis_gate = true;
+  stat = check_measurement_mahalanobis_gate(
+    measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance,
+    mahalanobis_distance_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_passed_mahalanobis_gate = false;
+  stat = check_measurement_mahalanobis_gate(
+    measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance,
+    mahalanobis_distance_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status)
+{
+  diagnostic_msgs::msg::DiagnosticStatus merged_stat;
+  std::vector<diagnostic_msgs::msg::DiagnosticStatus> stat_array(2);
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(0).message = "OK";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(1).message = "OK";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+  EXPECT_EQ(merged_stat.message, "OK");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(0).message = "WARN0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(1).message = "OK";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(merged_stat.message, "WARN0");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(0).message = "OK";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(1).message = "WARN1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(merged_stat.message, "WARN1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(0).message = "WARN0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(1).message = "WARN1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(merged_stat.message, "WARN0; WARN1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(0).message = "OK";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(1).message = "ERROR1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+  EXPECT_EQ(merged_stat.message, "ERROR1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(0).message = "WARN0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(1).message = "ERROR1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+  EXPECT_EQ(merged_stat.message, "WARN0; ERROR1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(0).message = "ERROR0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(1).message = "ERROR1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+  EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1");
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_ekf_localizer_launch.py

@@ -0,0 +1,170 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import time
+import unittest
+
+from ament_index_python import get_package_share_directory
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import launch
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.logging import get_logger
+import launch_testing
+from nav_msgs.msg import Odometry
+import pytest
+import rclpy
+from std_srvs.srv import SetBool
+
+logger = get_logger(__name__)
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    test_ekf_localizer_launch_file = os.path.join(
+        get_package_share_directory("autoware_ekf_localizer"),
+        "launch",
+        "ekf_localizer.launch.xml",
+    )
+    ekf_localizer = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(test_ekf_localizer_launch_file),
+    )
+
+    return launch.LaunchDescription(
+        [
+            ekf_localizer,
+            # Start tests right away - no need to wait for anything
+            launch_testing.actions.ReadyToTest(),
+        ]
+    )
+
+
+class TestEKFLocalizer(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context for the test node
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        rclpy.shutdown()
+
+    def setUp(self):
+        # Create a ROS node for tests
+        self.test_node = rclpy.create_node("test_node")
+        self.evaluation_time = 0.2  # 200ms
+
+    def tearDown(self):
+        self.test_node.destroy_node()
+
+    @staticmethod
+    def print_message(stat):
+        logger.debug("===========================")
+        logger.debug(stat)
+
+    def test_node_link(self):
+        # Trigger ekf_localizer to activate the node
+        cli_trigger = self.test_node.create_client(SetBool, "/trigger_node")
+        while not cli_trigger.wait_for_service(timeout_sec=1.0):
+            continue
+
+        request = SetBool.Request()
+        request.data = True
+        future = cli_trigger.call_async(request)
+        rclpy.spin_until_future_complete(self.test_node, future)
+
+        if future.result() is not None:
+            self.test_node.get_logger().info("Result of bool service: %s" % future.result().message)
+        else:
+            self.test_node.get_logger().error(
+                "Exception while calling service: %r" % future.exception()
+            )
+
+        # Send initial pose
+        pub_init_pose = self.test_node.create_publisher(
+            PoseWithCovarianceStamped, "/initialpose3d", 10
+        )
+        init_pose = PoseWithCovarianceStamped()
+        init_pose.header.frame_id = "map"
+        init_pose.pose.pose.position.x = 0.0
+        init_pose.pose.pose.position.y = 0.0
+        init_pose.pose.pose.position.z = 0.0
+        init_pose.pose.pose.orientation.x = 0.0
+        init_pose.pose.pose.orientation.y = 0.0
+        init_pose.pose.pose.orientation.z = 0.0
+        init_pose.pose.pose.orientation.w = 1.0
+        init_pose.pose.covariance = [
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+        ]
+        pub_init_pose.publish(init_pose)
+
+        # Receive Odometry
+        msg_buffer = []
+        self.test_node.create_subscription(
+            Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10
+        )
+
+        # Wait until the node publishes some topic
+        end_time = time.time() + self.evaluation_time
+        while time.time() < end_time:
+            rclpy.spin_once(self.test_node, timeout_sec=0.1)
+
+        # Check if the EKF outputs some Odometry
+        self.assertTrue(len(msg_buffer) > 0)
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        # Check that process exits with code 0: no error
+        launch_testing.asserts.assertExitCodes(proc_info)

master/src/Localization/autoware_ekf_localizer/test/test_ekf_localizer_mahalanobis.py

@@ -0,0 +1,229 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import time
+import unittest
+
+from ament_index_python import get_package_share_directory
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import launch
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.logging import get_logger
+import launch_testing
+from nav_msgs.msg import Odometry
+import pytest
+import rclpy
+from std_srvs.srv import SetBool
+
+logger = get_logger(__name__)
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    test_ekf_localizer_launch_file = os.path.join(
+        get_package_share_directory("autoware_ekf_localizer"),
+        "launch",
+        "ekf_localizer.launch.xml",
+    )
+    ekf_localizer = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(test_ekf_localizer_launch_file),
+    )
+
+    return launch.LaunchDescription(
+        [
+            ekf_localizer,
+            # Start tests right away - no need to wait for anything
+            launch_testing.actions.ReadyToTest(),
+        ]
+    )
+
+
+class TestEKFLocalizer(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context for the test node
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        rclpy.shutdown()
+
+    def setUp(self):
+        # Create a ROS node for tests
+        self.test_node = rclpy.create_node("test_node")
+        self.evaluation_time = 0.2  # 200ms
+
+    def tearDown(self):
+        self.test_node.destroy_node()
+
+    @staticmethod
+    def print_message(stat):
+        logger.debug("===========================")
+        logger.debug(stat)
+
+    def test_node_link(self):
+        # Trigger ekf_localizer to activate the node
+        cli_trigger = self.test_node.create_client(SetBool, "/trigger_node")
+        while not cli_trigger.wait_for_service(timeout_sec=1.0):
+            continue
+
+        request = SetBool.Request()
+        request.data = True
+        future = cli_trigger.call_async(request)
+        rclpy.spin_until_future_complete(self.test_node, future)
+
+        if future.result() is not None:
+            self.test_node.get_logger().info("Result of bool service: %s" % future.result().message)
+        else:
+            self.test_node.get_logger().error(
+                "Exception while calling service: %r" % future.exception()
+            )
+
+        # Send initial pose
+        pub_init_pose = self.test_node.create_publisher(
+            PoseWithCovarianceStamped, "/initialpose3d", 10
+        )
+        init_pose = PoseWithCovarianceStamped()
+        init_pose.header.frame_id = "map"
+        init_pose.pose.pose.position.x = 0.0
+        init_pose.pose.pose.position.y = 0.0
+        init_pose.pose.pose.position.z = 0.0
+        init_pose.pose.pose.orientation.x = 0.0
+        init_pose.pose.pose.orientation.y = 0.0
+        init_pose.pose.pose.orientation.z = 0.0
+        init_pose.pose.pose.orientation.w = 1.0
+        init_pose.pose.covariance = [
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+        ]
+        pub_init_pose.publish(init_pose)
+        rclpy.spin_once(self.test_node, timeout_sec=0.1)
+
+        # Send pose that should be ignored by mahalanobis gate in ekf_localizer
+        pub_pose = self.test_node.create_publisher(
+            PoseWithCovarianceStamped, "/in_pose_with_covariance", 10
+        )
+        pose = PoseWithCovarianceStamped()
+        pose.header.frame_id = "map"
+        pose.pose.pose.position.x = 1000000.0
+        pose.pose.pose.position.y = 1000000.0
+        pose.pose.pose.position.z = 10.0
+        pose.pose.pose.orientation.x = 0.0
+        pose.pose.pose.orientation.y = 0.0
+        pose.pose.pose.orientation.z = 0.0
+        pose.pose.pose.orientation.w = 1.0
+        pose.pose.covariance = [
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+        ]
+        pub_pose.publish(pose)
+
+        # Receive Odometry
+        msg_buffer = []
+        self.test_node.create_subscription(
+            Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10
+        )
+
+        # Wait until the node publishes some topic
+        end_time = time.time() + self.evaluation_time
+        while time.time() < end_time:
+            rclpy.spin_once(self.test_node, timeout_sec=0.1)
+
+        # Check if the EKF outputs some Odometry
+        self.assertTrue(len(msg_buffer) > 0)
+
+        # Assert msg to be at the origin
+        self.assertEqual(msg_buffer[-1].pose.pose.position.x, 0.0)
+        self.assertEqual(msg_buffer[-1].pose.pose.position.y, 0.0)
+        self.assertEqual(msg_buffer[-1].pose.pose.position.z, 0.0)
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        # Check that process exits with code 0: no error
+        launch_testing.asserts.assertExitCodes(proc_info)

master/src/Localization/autoware_ekf_localizer/test/test_mahalanobis.cpp

@@ -0,0 +1,66 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/mahalanobis.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+constexpr double tolerance = 1e-8;
+
+TEST(squared_mahalanobis, SmokeTest)
+{
+  {
+    Eigen::Vector2d x(0, 1);
+    Eigen::Vector2d y(3, 2);
+    Eigen::Matrix2d c;
+    c << 10, 0, 0, 10;
+
+    EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance);
+  }
+
+  {
+    Eigen::Vector2d x(4, 1);
+    Eigen::Vector2d y(1, 5);
+    Eigen::Matrix2d c;
+    c << 5, 0, 0, 5;
+
+    EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance);
+  }
+}
+
+TEST(mahalanobis, SmokeTest)
+{
+  {
+    Eigen::Vector2d x(0, 1);
+    Eigen::Vector2d y(3, 2);
+    Eigen::Matrix2d c;
+    c << 10, 0, 0, 10;
+
+    EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance);
+  }
+
+  {
+    Eigen::Vector2d x(4, 1);
+    Eigen::Vector2d y(1, 5);
+    Eigen::Matrix2d c;
+    c << 5, 0, 0, 5;
+
+    EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance);
+  }
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_measurement.cpp

@@ -0,0 +1,86 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/measurement.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(Measurement, pose_measurement_matrix)
+{
+  const Eigen::Matrix<double, 3, 6> m = pose_measurement_matrix();
+  Eigen::Matrix<double, 3, 6> expected;
+  expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+  EXPECT_EQ((m - expected).norm(), 0);
+}
+
+TEST(Measurement, twist_measurement_matrix)
+{
+  const Eigen::Matrix<double, 2, 6> m = twist_measurement_matrix();
+  Eigen::Matrix<double, 2, 6> expected;
+  expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+  EXPECT_EQ((m - expected).norm(), 0);
+}
+
+TEST(Measurement, pose_measurement_covariance)
+{
+  {
+    const std::array<double, 36> covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6,
+                                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                               0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9};
+
+    const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2);
+
+    Eigen::Matrix3d expected;
+    expected << 2, 4, 6, 8, 10, 12, 14, 16, 18;
+
+    EXPECT_EQ((m - expected).norm(), 0.);
+  }
+
+  {
+    // Make sure that other elements are not changed
+    std::array<double, 36> covariance{};
+    covariance.fill(0);
+    const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.);
+    EXPECT_EQ(m.norm(), 0);
+  }
+}
+
+TEST(Measurement, twist_measurement_covariance)
+{
+  {
+    const std::array<double, 36> covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6,
+                                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                               0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4};
+
+    const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2);
+
+    Eigen::Matrix2d expected;
+    expected << 2, 4, 6, 8;
+
+    EXPECT_EQ((m - expected).norm(), 0.);
+  }
+
+  {
+    // Make sure that other elements are not changed
+    std::array<double, 36> covariance{};
+    covariance.fill(0);
+    const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.);
+    EXPECT_EQ(m.norm(), 0);
+  }
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_numeric.cpp

@@ -0,0 +1,52 @@
+// Copyright 2022 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/numeric.hpp"
+
+#include <Eigen/Core>
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(Numeric, has_nan)
+{
+  const Eigen::VectorXd empty(0);
+  const double inf = std::numeric_limits<double>::infinity();
+  const double nan = std::nan("");
+
+  EXPECT_FALSE(has_nan(empty));
+  EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.)));
+  EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.)));
+  EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf)));
+
+  EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.)));
+}
+
+TEST(Numeric, has_inf)
+{
+  const Eigen::VectorXd empty(0);
+  const double inf = std::numeric_limits<double>::infinity();
+  const double nan = std::nan("");
+
+  EXPECT_FALSE(has_inf(empty));
+  EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.)));
+  EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.)));
+  EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.)));
+
+  EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf)));
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_state_transition.cpp

@@ -0,0 +1,101 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#define _USE_MATH_DEFINES
+#include "autoware/ekf_localizer/state_index.hpp"
+#include "autoware/ekf_localizer/state_transition.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(StateTransition, normalize_yaw)
+{
+  const double tolerance = 1e-6;
+  EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance);
+  EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance);
+  EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance);
+  EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance);
+}
+
+TEST(predict_next_state, predict_next_state)
+{
+  // This function is the definition of state transition so we just check
+  // if the calculation is done according to the formula
+  Vector6d x_curr;
+  x_curr(0) = 2.;
+  x_curr(1) = 3.;
+  x_curr(2) = M_PI / 2.;
+  x_curr(3) = M_PI / 4.;
+  x_curr(4) = 10.;
+  x_curr(5) = 2. * M_PI / 3.;
+
+  const double dt = 0.5;
+
+  const Vector6d x_next = predict_next_state(x_curr, dt);
+
+  const double tolerance = 1e-10;
+  EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance);
+  EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance);
+  EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance);
+  EXPECT_NEAR(x_next(3), x_curr(3), tolerance);
+  EXPECT_NEAR(x_next(4), x_curr(4), tolerance);
+  EXPECT_NEAR(x_next(5), x_curr(5), tolerance);
+}
+
+TEST(create_state_transition_matrix, NumericalApproximation)
+{
+  // The transition matrix A = df / dx
+  // We check if df = A * dx approximates f(x + dx) - f(x)
+
+  {
+    // check around x = 0
+    const double dt = 0.1;
+    const Vector6d dx = 0.1 * Vector6d::Ones();
+    const Vector6d x = Vector6d::Zero();
+
+    const Matrix6d a = create_state_transition_matrix(x, dt);
+    const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt);
+
+    EXPECT_LT((df - a * dx).norm(), 2e-3);
+  }
+
+  {
+    // check around random x
+    const double dt = 0.1;
+    const Vector6d dx = 0.1 * Vector6d::Ones();
+    const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished();
+
+    const Matrix6d a = create_state_transition_matrix(x, dt);
+    const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt);
+
+    EXPECT_LT((df - a * dx).norm(), 5e-3);
+  }
+}
+
+TEST(process_noise_covariance, process_noise_covariance)
+{
+  const Matrix6d q = process_noise_covariance(1., 2., 3.);
+  EXPECT_EQ(q(2, 2), 1.);  // for yaw
+  EXPECT_EQ(q(4, 4), 2.);  // for vx
+  EXPECT_EQ(q(5, 5), 3.);  // for wz
+
+  // Make sure other elements are zero
+  EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.);
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_string.cpp

@@ -0,0 +1,31 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/string.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(erase_leading_slash, SmokeTest)
+{
+  EXPECT_EQ(erase_leading_slash("/topic"), "topic");
+  EXPECT_EQ(erase_leading_slash("topic"), "topic");  // do nothing
+
+  EXPECT_EQ(erase_leading_slash(""), "");
+  EXPECT_EQ(erase_leading_slash("/"), "");
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_warning_message.cpp

@@ -0,0 +1,67 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/ekf_localizer/warning_message.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(pose_delay_step_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    pose_delay_step_warning_message(6.0, 4.0).c_str(),
+    "Pose delay exceeds the compensation limit, ignored. "
+    "delay: 6.000[s], limit: 4.000[s]");
+}
+
+TEST(twist_delay_step_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    twist_delay_step_warning_message(10.0, 6.0).c_str(),
+    "Twist delay exceeds the compensation limit, ignored. "
+    "delay: 10.000[s], limit: 6.000[s]");
+}
+
+TEST(pose_delay_time_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    pose_delay_time_warning_message(-1.0).c_str(),
+    "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000");
+  EXPECT_STREQ(
+    pose_delay_time_warning_message(-0.4).c_str(),
+    "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400");
+}
+
+TEST(twist_delay_time_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    twist_delay_time_warning_message(-1.0).c_str(),
+    "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000");
+  EXPECT_STREQ(
+    twist_delay_time_warning_message(-0.4).c_str(),
+    "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400");
+}
+
+TEST(mahalanobis_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    mahalanobis_warning_message(1.0, 0.5).c_str(),
+    "The Mahalanobis distance 1.0000 is over the limit 0.5000.");
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_geo_pose_projector/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_geo_pose_projector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/geo_pose_projector.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::geo_pose_projector::GeoPoseProjector"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

master/src/Localization/autoware_geo_pose_projector/README.md

@@ -0,0 +1,28 @@
+# Autoware 地理姿态投影器
+
+## 概述
+
+该节点是一个简单的节点，订阅地理参考姿态主题，并在地图坐标系中发布姿态。
+
+## 订阅主题
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `input_geo_pose` | `geographic_msgs::msg::GeoPoseWithCovarianceStamped` | 地理参考姿态 |
+| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectedObjectInfo` | 地图投影信息 |
+
+## 发布主题
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `output_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | 地图坐标系中的姿态 |
+| `/tf` | `tf2_msgs::msg::TFMessage` | 从父链接到子链接的变换 |
+
+## 参数
+
+{{ json_to_markdown("localization/autoware_geo_pose_projector/schema/geo_pose_projector.schema.json") }}
+
+## 限制
+
+根据你使用的投影类型，协方差转换可能不正确。输入主题的协方差以（纬度，经度，高度）表示为对角矩阵。
+目前，我们假设 x 轴是东方向，y 轴是北方向。因此，当这个假设被打破时，转换可能不正确，特别是当纬度和经度的协方差不同时。

master/src/Localization/autoware_geo_pose_projector/config/geo_pose_projector.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    publish_tf: true
+    parent_frame: "map"
+    child_frame: "pose_estimator_base_link"

master/src/Localization/autoware_geo_pose_projector/launch/geo_pose_projector.launch.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_geo_pose" default="/geo_pose_with_covariance"/>
+  <arg name="output_pose" default="/pose_with_covariance"/>
+  <arg name="param_path" default="$(find-pkg-share autoware_geo_pose_projector)/config/geo_pose_projector.param.yaml"/>
+
+  <node pkg="autoware_geo_pose_projector" exec="autoware_geo_pose_projector_node" output="both">
+    <remap from="input_geo_pose" to="$(var input_geo_pose)"/>
+    <remap from="output_pose" to="$(var output_pose)"/>
+
+    <param from="$(var param_path)"/>
+  </node>
+</launch>

master/src/Localization/autoware_geo_pose_projector/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_geo_pose_projector</name>
+  <version>0.1.0</version>
+  <description>The autoware_geo_pose_projector package</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="koji.minoda@tier4.jp">Koji Minoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_component_interface_specs</depend>
+  <depend>autoware_geography_utils</depend>
+  <depend>component_interface_utils</depend>
+  <depend>geographic_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Localization/autoware_geo_pose_projector/schema/geo_pose_projector.schema.json

@@ -0,0 +1,43 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for geo_pose_projector",
+  "type": "object",
+  "definitions": {
+    "geo_pose_projector": {
+      "type": "object",
+      "properties": {
+        "publish_tf": {
+          "type": "boolean",
+          "description": "whether to publish tf",
+          "default": true
+        },
+        "parent_frame": {
+          "type": "string",
+          "description": "parent frame for published tf",
+          "default": "map"
+        },
+        "child_frame": {
+          "type": "string",
+          "description": "child frame for published tf",
+          "default": "pose_estimator_base_link"
+        }
+      },
+      "required": ["publish_tf", "parent_frame", "child_frame"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/geo_pose_projector"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

master/src/Localization/autoware_geo_pose_projector/src/geo_pose_projector.cpp

@@ -0,0 +1,110 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "geo_pose_projector.hpp"
+
+#include <autoware/geography_utils/height.hpp>
+#include <autoware/geography_utils/projection.hpp>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <string>
+
+namespace autoware::geo_pose_projector
+{
+GeoPoseProjector::GeoPoseProjector(const rclcpp::NodeOptions & options)
+: rclcpp::Node("geo_pose_projector", options), publish_tf_(declare_parameter<bool>("publish_tf"))
+{
+  // Subscribe to map_projector_info topic
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_sub(
+    sub_map_projector_info_,
+    [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { projector_info_ = *msg; });
+
+  // Subscribe to geo_pose topic
+  geo_pose_sub_ = create_subscription<GeoPoseWithCovariance>(
+    "input_geo_pose", 10,
+    [this](const GeoPoseWithCovariance::ConstSharedPtr msg) { on_geo_pose(msg); });
+
+  // Publish pose topic
+  pose_pub_ = create_publisher<PoseWithCovariance>("output_pose", 10);
+
+  // Publish tf
+  if (publish_tf_) {
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+    parent_frame_ = declare_parameter<std::string>("parent_frame");
+    child_frame_ = declare_parameter<std::string>("child_frame");
+  }
+}
+
+void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg)
+{
+  if (!projector_info_) {
+    RCLCPP_WARN_THROTTLE(
+      get_logger(), *get_clock(), 1000 /* ms */, "map_projector_info is not received yet.");
+    return;
+  }
+
+  // get position
+  geographic_msgs::msg::GeoPoint gps_point;
+  gps_point.latitude = msg->pose.pose.position.latitude;
+  gps_point.longitude = msg->pose.pose.position.longitude;
+  gps_point.altitude = msg->pose.pose.position.altitude;
+  geometry_msgs::msg::Point position =
+    autoware::geography_utils::project_forward(gps_point, projector_info_.value());
+  position.z = autoware::geography_utils::convert_height(
+    position.z, gps_point.latitude, gps_point.longitude, MapProjectorInfo::Message::WGS84,
+    projector_info_.value().vertical_datum);
+
+  // Convert geo_pose to pose
+  PoseWithCovariance projected_pose;
+  projected_pose.header = msg->header;
+  projected_pose.pose.pose.position = position;
+  projected_pose.pose.pose.orientation = msg->pose.pose.orientation;
+  projected_pose.pose.covariance = msg->pose.covariance;
+
+  // Covariance in GeoPoseWithCovariance is in Lat/Lon/Alt coordinate.
+  // TODO(TIER IV): This swap may be invalid when using other projector type.
+  projected_pose.pose.covariance[0] = msg->pose.covariance[7];
+  projected_pose.pose.covariance[7] = msg->pose.covariance[0];
+
+  pose_pub_->publish(projected_pose);
+
+  // Publish tf
+  if (publish_tf_) {
+    tf2::Transform transform;
+    transform.setOrigin(tf2::Vector3(
+      projected_pose.pose.pose.position.x, projected_pose.pose.pose.position.y,
+      projected_pose.pose.pose.position.z));
+    const auto localization_quat = tf2::Quaternion(
+      projected_pose.pose.pose.orientation.x, projected_pose.pose.pose.orientation.y,
+      projected_pose.pose.pose.orientation.z, projected_pose.pose.pose.orientation.w);
+    transform.setRotation(localization_quat);
+
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped.header = msg->header;
+    transform_stamped.header.frame_id = parent_frame_;
+    transform_stamped.child_frame_id = child_frame_;
+    transform_stamped.transform = tf2::toMsg(transform);
+    tf_broadcaster_->sendTransform(transform_stamped);
+  }
+}
+}  // namespace autoware::geo_pose_projector
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::geo_pose_projector::GeoPoseProjector)

master/src/Localization/autoware_geo_pose_projector/src/geo_pose_projector.hpp

@@ -0,0 +1,61 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GEO_POSE_PROJECTOR_HPP_
+#define GEO_POSE_PROJECTOR_HPP_
+
+#include <autoware/component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geographic_msgs/msg/geo_pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <memory>
+#include <optional>
+#include <string>
+
+namespace autoware::geo_pose_projector
+{
+class GeoPoseProjector : public rclcpp::Node
+{
+private:
+  using GeoPoseWithCovariance = geographic_msgs::msg::GeoPoseWithCovarianceStamped;
+  using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo;
+
+public:
+  explicit GeoPoseProjector(const rclcpp::NodeOptions & options);
+
+private:
+  void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg);
+
+  component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
+  rclcpp::Subscription<GeoPoseWithCovariance>::SharedPtr geo_pose_sub_;
+  rclcpp::Publisher<PoseWithCovariance>::SharedPtr pose_pub_;
+
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  std::optional<MapProjectorInfo::Message> projector_info_ = std::nullopt;
+
+  const bool publish_tf_;
+
+  std::string parent_frame_;
+  std::string child_frame_;
+};
+}  // namespace autoware::geo_pose_projector
+
+#endif  // GEO_POSE_PROJECTOR_HPP_

master/src/Localization/autoware_gyro_odometer/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_gyro_odometer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/gyro_odometer_core.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} fmt)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_gyro_odometer
+    test/test_main.cpp
+    test/test_gyro_odometer_pubsub.cpp
+    test/test_gyro_odometer_helper.cpp
+  )
+  ament_target_dependencies(test_gyro_odometer
+    rclcpp
+  )
+  target_link_libraries(test_gyro_odometer
+    ${PROJECT_NAME}
+  )
+  target_include_directories(test_gyro_odometer PRIVATE
+    src
+  )
+endif()
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::gyro_odometer::GyroOdometerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

master/src/Localization/autoware_gyro_odometer/README.md

@@ -0,0 +1,47 @@
+# Autoware 陀螺仪里程计
+
+## 目的
+
+`autoware_gyro_odometer` 是一个用于通过结合 IMU 和车辆速度来估计速度的软件包。
+
+## 输入/输出
+
+### 输入
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `vehicle/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | 来自车辆的速度及其协方差 |
+| `imu` | `sensor_msgs::msg::Imu` | 来自传感器的 IMU 数据 |
+
+### 输出
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | 估计得到的速度及其协方差 |
+
+## 参数
+
+{{ json_to_markdown("localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json") }}
+
+## 假设/已知限制
+
+- [假设] 输入速度消息的 frame_id 必须设置为 base_link。
+- [假设] 输入消息中的协方差必须正确分配。
+- [假设] 如果纵向车辆速度和绕偏航轴的角速度都足够小，则将角速度设置为零。这是为了抑制 IMU 角速度偏置。没有这个过程，我们在静止时会误估计车辆状态。
+- [限制] 输出消息的频率取决于输入 IMU 消息的频率。
+- [限制] 我们无法产生可靠的横向和垂直速度值。因此，我们将较大的值分配给输出协方差矩阵中相应的元素。
+
+## 诊断
+
+<img src="./media/diagnostic.png" alt="drawing" width="600"/>
+
+| 名称 | 说明 | 转为警告的条件 | 转为错误的条件 |
+| --- | --- | --- | --- |
+| `topic_time_stamp` | 服务调用的时间戳。[纳秒] | 无 | 无 |
+| `is_arrived_first_vehicle_twist` | 车辆速度主题是否至少接收过一次。 | 尚未到达 | 无 |
+| `is_arrived_first_imu` | IMU 主题是否至少接收过一次。 | 尚未到达 | 无 |
+| `vehicle_twist_time_stamp_dt` | 当前时间与最新车辆速度主题之间的时间差。[秒] | 无 | 时间 **超过** `message_timeout_sec` |
+| `imu_time_stamp_dt` | 当前时间与最新 IMU 主题之间的时间差。[秒] | 无 | 时间 **超过** `message_timeout_sec` |
+| `vehicle_twist_queue_size` | 车辆速度队列的大小。 | 无 | 无 |
+| `imu_queue_size` | 陀螺仪队列的大小。 | 无 | 无 |
+| `is_succeed_transform_imu` | 是否成功转换 IMU 数据。 | 无 | 转换失败 |

master/src/Localization/autoware_gyro_odometer/config/gyro_odometer.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    output_frame: "base_link"
+    message_timeout_sec: 0.4

master/src/Localization/autoware_gyro_odometer/launch/gyro_odometer.launch.xml

@@ -0,0 +1,27 @@
+<launch>
+  <arg name="input_vehicle_twist_with_covariance_topic" default="/sensing/vehicle_velocity_converter/twist_with_covariance" description="input twist with covariance topic name from vehicle"/>
+
+  <arg name="input_imu_topic" default="/sensing/imu/hipnuc/imu_data" description="input imu topic name"/>
+
+  <arg name="output_twist_raw_topic" default="gyro_twist_raw" description="output raw twist topic name"/>
+  <arg name="output_twist_with_covariance_raw_topic" default="gyro_twist_with_covariance_raw" description="output raw twist with covariance topic name"/>
+
+  <arg name="output_twist_topic" default="gyro_twist" description="output twist topic name"/>
+  <arg name="output_twist_with_covariance_topic" default="gyro_twist_with_covariance" description="output twist with covariance topic name"/>
+
+  <arg name="config_file" default="$(find-pkg-share autoware_gyro_odometer)/config/gyro_odometer.param.yaml"/>
+
+  <node pkg="autoware_gyro_odometer" exec="autoware_gyro_odometer_node" output="both">
+    <remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
+
+    <remap from="imu" to="$(var input_imu_topic)"/>
+
+    <remap from="twist_raw" to="$(var output_twist_raw_topic)"/>
+    <remap from="twist_with_covariance_raw" to="$(var output_twist_with_covariance_raw_topic)"/>
+
+    <remap from="twist" to="$(var output_twist_topic)"/>
+    <remap from="twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/>
+
+    <param from="$(var config_file)"/>
+  </node>
+</launch>

master/src/Localization/autoware_gyro_odometer/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_gyro_odometer</name>
+  <version>0.1.0</version>
+  <description>The autoware_gyro_odometer package as a ROS 2 node</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="yamato.ando@tier4.jp">Yamato Ando</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_localization_util</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>fmt</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Localization/autoware_gyro_odometer/schema/gyro_odometer.schema.json

@@ -0,0 +1,38 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for gyro odometer",
+  "type": "object",
+  "definitions": {
+    "gyro_odometer": {
+      "type": "object",
+      "properties": {
+        "output_frame": {
+          "type": "string",
+          "description": "output's frame id",
+          "default": "base_link"
+        },
+        "message_timeout_sec": {
+          "type": "number",
+          "description": "delay tolerance time for message",
+          "default": 0.2
+        }
+      },
+      "required": ["output_frame", "message_timeout_sec"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/gyro_odometer"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}

master/src/Localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp

@@ -0,0 +1,307 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gyro_odometer_core.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+#include <fmt/core.h>
+
+#include <algorithm>
+#include <cmath>
+#include <memory>
+#include <sstream>
+#include <string>
+
+namespace autoware::gyro_odometer
+{
+
+std::array<double, 9> transform_covariance(const std::array<double, 9> & cov)
+{
+  using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+  double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]});
+
+  std::array<double, 9> cov_transformed = {};
+  cov_transformed.fill(0.);
+  cov_transformed[COV_IDX::X_X] = max_cov;
+  cov_transformed[COV_IDX::Y_Y] = max_cov;
+  cov_transformed[COV_IDX::Z_Z] = max_cov;
+  return cov_transformed;
+}
+
+GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options)
+: Node("gyro_odometer", node_options),
+  output_frame_(declare_parameter<std::string>("output_frame")),
+  message_timeout_sec_(declare_parameter<double>("message_timeout_sec")),
+  vehicle_twist_arrived_(false),
+  imu_arrived_(false)
+{
+  transform_listener_ = std::make_shared<autoware::universe_utils::TransformListener>(this);
+  logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
+
+  vehicle_twist_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "vehicle/twist_with_covariance", rclcpp::QoS{100},
+    std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1));
+
+  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+    "imu", rclcpp::QoS{100},
+    std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1));
+
+  twist_raw_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist_raw", rclcpp::QoS{10});
+  twist_with_covariance_raw_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "twist_with_covariance_raw", rclcpp::QoS{10});
+
+  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
+  twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "twist_with_covariance", rclcpp::QoS{10});
+
+  diagnostics_ =
+    std::make_unique<autoware::localization_util::DiagnosticsModule>(this, "gyro_odometer_status");
+
+  // TODO(YamatoAndo) createTimer
+}
+
+void GyroOdometerNode::callback_vehicle_twist(
+  const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr)
+{
+  diagnostics_->clear();
+  diagnostics_->add_key_value(
+    "topic_time_stamp",
+    static_cast<rclcpp::Time>(vehicle_twist_msg_ptr->header.stamp).nanoseconds());
+
+  vehicle_twist_arrived_ = true;
+  latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp;
+  vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr);
+  concat_gyro_and_odometer();
+
+  diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp);
+}
+
+void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
+{
+  diagnostics_->clear();
+  diagnostics_->add_key_value(
+    "topic_time_stamp", static_cast<rclcpp::Time>(imu_msg_ptr->header.stamp).nanoseconds());
+  // std::cout << "111111111111111111111111111111111111111111111111111" << std::endl;
+  imu_arrived_ = true;
+  latest_imu_ros_time_ = imu_msg_ptr->header.stamp;
+  gyro_queue_.push_back(*imu_msg_ptr);
+  concat_gyro_and_odometer();
+
+  diagnostics_->publish(imu_msg_ptr->header.stamp);
+}
+
+void GyroOdometerNode::concat_gyro_and_odometer()
+{
+  // check arrive first topic
+  diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_);
+  diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_);
+  if (!vehicle_twist_arrived_) {
+    std::stringstream message;
+    message << "Twist msg is not subscribed";
+    RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
+    diagnostics_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+
+    vehicle_twist_queue_.clear();
+    gyro_queue_.clear();
+    return;
+  }
+  if (!imu_arrived_) {
+    std::stringstream message;
+    message << "Imu msg is not subscribed";
+    RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
+    diagnostics_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+
+    vehicle_twist_queue_.clear();
+    gyro_queue_.clear();
+    return;
+  }
+
+  // check timeout
+  const double vehicle_twist_dt =
+    std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds());
+  const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds());
+  diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt);
+  diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt);
+  if (vehicle_twist_dt > message_timeout_sec_) {
+    const std::string message = fmt::format(
+      "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]",
+      vehicle_twist_dt, message_timeout_sec_);
+    RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message);
+    diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message);
+
+    vehicle_twist_queue_.clear();
+    gyro_queue_.clear();
+    return;
+  }
+  if (imu_dt > 0.5) {
+    const std::string message = fmt::format(
+      "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_);
+    RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message);
+    diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message);
+
+    vehicle_twist_queue_.clear();
+    gyro_queue_.clear();
+    return;
+  }
+
+  // check queue size
+  diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size());
+  diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size());
+  if (vehicle_twist_queue_.empty()) {
+    // not output error and clear queue
+    return;
+  }
+  if (gyro_queue_.empty()) {
+    // not output error and clear queue
+    return;
+  }
+
+  // get transformation
+  geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr =
+    transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_);
+
+  const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr);
+  diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu);
+  if (!is_succeed_transform_imu) {
+    std::stringstream message;
+    message << "Please publish TF " << output_frame_ << " to "
+            << gyro_queue_.front().header.frame_id;
+    RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str());
+    diagnostics_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str());
+
+    vehicle_twist_queue_.clear();
+    gyro_queue_.clear();
+    return;
+  }
+
+  // transform gyro frame
+  for (auto & gyro : gyro_queue_) {
+    geometry_msgs::msg::Vector3Stamped angular_velocity;
+    angular_velocity.header = gyro.header;
+    angular_velocity.vector = gyro.angular_velocity;
+
+    geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
+    transformed_angular_velocity.header = tf_imu2base_ptr->header;
+    tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr);
+
+    gyro.header.frame_id = output_frame_;
+    gyro.angular_velocity = transformed_angular_velocity.vector;
+    gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance);
+  }
+
+  using COV_IDX_XYZ = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX;
+  using COV_IDX_XYZRPY = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+
+  // calc mean, covariance
+  double vx_mean = 0;
+  geometry_msgs::msg::Vector3 gyro_mean{};
+  double vx_covariance_original = 0;
+  geometry_msgs::msg::Vector3 gyro_covariance_original{};
+  for (const auto & vehicle_twist : vehicle_twist_queue_) {
+    vx_mean += vehicle_twist.twist.twist.linear.x;
+    vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0];
+  }
+  vx_mean /= static_cast<double>(vehicle_twist_queue_.size());
+  vx_covariance_original /= static_cast<double>(vehicle_twist_queue_.size());
+
+  for (const auto & gyro : gyro_queue_) {
+    gyro_mean.x += gyro.angular_velocity.x;
+    gyro_mean.y += gyro.angular_velocity.y;
+    gyro_mean.z += gyro.angular_velocity.z;
+    gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X];
+    gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y];
+    gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z];
+  }
+  gyro_mean.x /= static_cast<double>(gyro_queue_.size());
+  gyro_mean.y /= static_cast<double>(gyro_queue_.size());
+  gyro_mean.z /= static_cast<double>(gyro_queue_.size());
+  gyro_covariance_original.x /= static_cast<double>(gyro_queue_.size());
+  gyro_covariance_original.y /= static_cast<double>(gyro_queue_.size());
+  gyro_covariance_original.z /= static_cast<double>(gyro_queue_.size());
+
+  // concat
+  geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov;
+  const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp);
+  const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp);
+  if (latest_vehicle_twist_stamp < latest_imu_stamp) {
+    twist_with_cov.header.stamp = latest_imu_stamp;
+  } else {
+    twist_with_cov.header.stamp = latest_vehicle_twist_stamp;
+  }
+  twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id;
+  twist_with_cov.twist.twist.linear.x = vx_mean;
+  twist_with_cov.twist.twist.angular = gyro_mean;
+
+  // From a statistical point of view, here we reduce the covariances according to the number of
+  // observed data
+  twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] =
+    vx_covariance_original / static_cast<double>(vehicle_twist_queue_.size());
+  twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0;
+  twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0;
+  twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] =
+    gyro_covariance_original.x / static_cast<double>(gyro_queue_.size());
+  twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] =
+    gyro_covariance_original.y / static_cast<double>(gyro_queue_.size());
+  twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] =
+    gyro_covariance_original.z / static_cast<double>(gyro_queue_.size());
+
+  publish_data(twist_with_cov);
+
+  vehicle_twist_queue_.clear();
+  gyro_queue_.clear();
+}
+
+void GyroOdometerNode::publish_data(
+  const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw)
+{
+  geometry_msgs::msg::TwistStamped twist_raw;
+  twist_raw.header = twist_with_cov_raw.header;
+  twist_raw.twist = twist_with_cov_raw.twist.twist;
+
+  twist_raw_pub_->publish(twist_raw);
+  twist_with_covariance_raw_pub_->publish(twist_with_cov_raw);
+
+  geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance = twist_with_cov_raw;
+  geometry_msgs::msg::TwistStamped twist = twist_raw;
+
+  // clear imu yaw bias if vehicle is stopped
+  if (
+    std::fabs(twist_with_cov_raw.twist.twist.angular.z) < 0.01 &&
+    std::fabs(twist_with_cov_raw.twist.twist.linear.x) < 0.01) {
+    twist.twist.angular.x = 0.0;
+    twist.twist.angular.y = 0.0;
+    twist.twist.angular.z = 0.0;
+    twist_with_covariance.twist.twist.angular.x = 0.0;
+    twist_with_covariance.twist.twist.angular.y = 0.0;
+    twist_with_covariance.twist.twist.angular.z = 0.0;
+  }
+
+  twist_pub_->publish(twist);
+  twist_with_covariance_pub_->publish(twist_with_covariance);
+}
+
+}  // namespace autoware::gyro_odometer
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode)

master/src/Localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp

@@ -0,0 +1,88 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GYRO_ODOMETER_CORE_HPP_
+#define GYRO_ODOMETER_CORE_HPP_
+
+#include "autoware/localization_util/diagnostics_module.hpp"
+#include "autoware/universe_utils/ros/logger_level_configure.hpp"
+#include "autoware/universe_utils/ros/msg_covariance.hpp"
+#include "autoware/universe_utils/ros/transform_listener.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
+#include <tf2/transform_datatypes.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <deque>
+#include <memory>
+#include <string>
+
+namespace autoware::gyro_odometer
+{
+
+class GyroOdometerNode : public rclcpp::Node
+{
+private:
+  using COV_IDX = autoware::universe_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+public:
+  explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void callback_vehicle_twist(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
+  void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+  void concat_gyro_and_odometer();
+  void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
+
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    vehicle_twist_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_raw_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    twist_with_covariance_raw_pub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    twist_with_covariance_pub_;
+
+  std::shared_ptr<autoware::universe_utils::TransformListener> transform_listener_;
+  std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
+
+  std::string output_frame_;
+  double message_timeout_sec_;
+
+  bool vehicle_twist_arrived_;
+  bool imu_arrived_;
+  rclcpp::Time latest_vehicle_twist_ros_time_;
+  rclcpp::Time latest_imu_ros_time_;
+  std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
+  std::deque<sensor_msgs::msg::Imu> gyro_queue_;
+
+  std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_;
+};
+
+}  // namespace autoware::gyro_odometer
+
+#endif  // GYRO_ODOMETER_CORE_HPP_

master/src/Localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.cpp

@@ -0,0 +1,46 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_gyro_odometer_helper.hpp"
+
+using geometry_msgs::msg::TwistWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+
+Imu generate_sample_imu()
+{
+  Imu imu;
+  imu.header.frame_id = "base_link";
+  imu.angular_velocity.x = 0.1;
+  imu.angular_velocity.y = 0.2;
+  imu.angular_velocity.z = 0.3;
+  return imu;
+}
+
+TwistWithCovarianceStamped generate_sample_velocity()
+{
+  TwistWithCovarianceStamped twist;
+  twist.header.frame_id = "base_link";
+  twist.twist.twist.linear.x = 1.0;
+  return twist;
+}
+
+rclcpp::NodeOptions get_node_options_with_default_params()
+{
+  rclcpp::NodeOptions node_options;
+
+  // for gyro_odometer
+  node_options.append_parameter_override("output_frame", "base_link");
+  node_options.append_parameter_override("message_timeout_sec", 1e12);
+  return node_options;
+}

master/src/Localization/autoware_gyro_odometer/test/test_gyro_odometer_helper.hpp

@@ -0,0 +1,27 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_GYRO_ODOMETER_HELPER_HPP_
+#define TEST_GYRO_ODOMETER_HELPER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
+sensor_msgs::msg::Imu generate_sample_imu();
+geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity();
+rclcpp::NodeOptions get_node_options_with_default_params();
+
+#endif  // TEST_GYRO_ODOMETER_HELPER_HPP_

master/src/Localization/autoware_gyro_odometer/test/test_gyro_odometer_pubsub.cpp

@@ -0,0 +1,156 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "gyro_odometer_core.hpp"
+#include "test_gyro_odometer_helper.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+#include <vector>
+
+/*
+ * This test checks if twist is published from gyro_odometer
+ */
+using geometry_msgs::msg::TwistWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+
+class ImuGenerator : public rclcpp::Node
+{
+public:
+  ImuGenerator() : Node("imu_generator"), imu_pub(create_publisher<Imu>("/imu", 1)) {}
+  rclcpp::Publisher<Imu>::SharedPtr imu_pub;
+};
+
+class VelocityGenerator : public rclcpp::Node
+{
+public:
+  VelocityGenerator()
+  : Node("velocity_generator"),
+    vehicle_velocity_pub(
+      create_publisher<TwistWithCovarianceStamped>("/vehicle/twist_with_covariance", 1))
+  {
+  }
+  rclcpp::Publisher<TwistWithCovarianceStamped>::SharedPtr vehicle_velocity_pub;
+};
+
+class GyroOdometerValidator : public rclcpp::Node
+{
+public:
+  GyroOdometerValidator()
+  : Node("gyro_odometer_validator"),
+    twist_sub(create_subscription<TwistWithCovarianceStamped>(
+      "/twist_with_covariance", 1,
+      [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) {
+        received_latest_twist_ptr = msg;
+      })),
+    received_latest_twist_ptr(nullptr)
+  {
+  }
+
+  rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr twist_sub;
+  TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr;
+};
+
+void wait_spin_some(rclcpp::Node::SharedPtr node_ptr)
+{
+  for (int i = 0; i < 50; ++i) {
+    rclcpp::spin_some(node_ptr);
+    rclcpp::WallRate(100).sleep();
+  }
+}
+
+bool is_twist_valid(
+  const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth)
+{
+  if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) {
+    return false;
+  }
+  if (twist.twist.twist.linear.y != twist_ground_truth.twist.twist.linear.y) {
+    return false;
+  }
+  if (twist.twist.twist.linear.z != twist_ground_truth.twist.twist.linear.z) {
+    return false;
+  }
+  if (twist.twist.twist.angular.x != twist_ground_truth.twist.twist.angular.x) {
+    return false;
+  }
+  if (twist.twist.twist.angular.y != twist_ground_truth.twist.twist.angular.y) {
+    return false;
+  }
+  if (twist.twist.twist.angular.z != twist_ground_truth.twist.twist.angular.z) {
+    return false;
+  }
+  return true;
+}
+
+// IMU & Velocity test
+// Verify that the gyro_odometer successfully publishes the fused twist message when both IMU and
+// velocity data are provided
+TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity)
+{
+  Imu input_imu = generate_sample_imu();
+  TwistWithCovarianceStamped input_velocity = generate_sample_velocity();
+
+  TwistWithCovarianceStamped expected_output_twist;
+  expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x;
+  expected_output_twist.twist.twist.angular.x = input_imu.angular_velocity.x;
+  expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y;
+  expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z;
+
+  auto gyro_odometer_node = std::make_shared<autoware::gyro_odometer::GyroOdometerNode>(
+    get_node_options_with_default_params());
+  auto imu_generator = std::make_shared<ImuGenerator>();
+  auto velocity_generator = std::make_shared<VelocityGenerator>();
+  auto gyro_odometer_validator_node = std::make_shared<GyroOdometerValidator>();
+
+  velocity_generator->vehicle_velocity_pub->publish(
+    input_velocity);  // need this for now, which should eventually be removed
+  imu_generator->imu_pub->publish(input_imu);
+  velocity_generator->vehicle_velocity_pub->publish(input_velocity);
+
+  // gyro_odometer receives IMU and velocity, and publishes the fused twist data.
+  wait_spin_some(gyro_odometer_node);
+
+  // validator node receives the fused twist data and store in "received_latest_twist_ptr".
+  wait_spin_some(gyro_odometer_validator_node);
+
+  EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr);
+  EXPECT_TRUE(is_twist_valid(
+    *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist));
+}
+
+// IMU-only test
+// Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided
+TEST(GyroOdometer, TestGyroOdometerImuOnly)
+{
+  Imu input_imu = generate_sample_imu();
+
+  auto gyro_odometer_node = std::make_shared<autoware::gyro_odometer::GyroOdometerNode>(
+    get_node_options_with_default_params());
+  auto imu_generator = std::make_shared<ImuGenerator>();
+  auto gyro_odometer_validator_node = std::make_shared<GyroOdometerValidator>();
+
+  imu_generator->imu_pub->publish(input_imu);
+
+  // gyro_odometer receives IMU
+  wait_spin_some(gyro_odometer_node);
+
+  // validator node waits for the output fused twist from gyro_odometer
+  wait_spin_some(gyro_odometer_validator_node);
+
+  EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr);
+}

master/src/Localization/autoware_gyro_odometer/test/test_main.cpp

@@ -0,0 +1,26 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+int main(int argc, char ** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}

master/src/Localization/autoware_localization_error_monitor/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_localization_error_monitor)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/localization_error_monitor.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::localization_error_monitor::LocalizationErrorMonitor"
+  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTOR SingleThreadedExecutor
+)
+
+if(BUILD_TESTING)
+  function(add_testcase filepath)
+    get_filename_component(filename ${filepath} NAME)
+    string(REGEX REPLACE ".cpp" "" test_name ${filename})
+    ament_add_gtest(${test_name} ${filepath})
+    target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
+    target_link_libraries(${test_name} ${PROJECT_NAME})
+    ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+  endfunction()
+
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_testcase(test/test_diagnostics.cpp)
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

master/src/Localization/autoware_localization_error_monitor/README.md

@@ -0,0 +1,32 @@
+# Autoware 定位误差监控器
+
+## 目的
+
+<p align="center">
+<img src="./media/diagnostics.png" width="400">
+</p>
+
+`autoware_localization_error_monitor` 是一个用于通过监控定位结果的不确定性来诊断定位误差的软件包。
+该软件包监控以下两个值：
+
+- 置信椭圆的长轴大小
+- 沿横向方向（车身坐标系）的置信椭圆大小
+
+## 输入/输出
+
+### 输入
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `input/odom` | `nav_msgs::msg::Odometry` | 定位结果 |
+
+### 输出
+
+| 名称 | 类型 | 说明 |
+| --- | --- | --- |
+| `debug/ellipse_marker` | `visualization_msgs::msg::Marker` | 椭圆标记 |
+| `diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | 诊断输出 |
+
+## 参数
+
+{{ json_to_markdown("localization/autoware_localization_error_monitor/schema/localization_error_monitor.schema.json") }}

master/src/Localization/autoware_localization_error_monitor/config/localization_error_monitor.param.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    scale: 3.0
+    error_ellipse_size: 1.5
+    warn_ellipse_size: 1.2
+    error_ellipse_size_lateral_direction: 0.3
+    warn_ellipse_size_lateral_direction: 0.25

master/src/Localization/autoware_localization_error_monitor/launch/localization_error_monitor.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input/odom" default="/localization/kinematic_state"/>
+  <arg name="param_file" default="$(find-pkg-share autoware_localization_error_monitor)/config/localization_error_monitor.param.yaml"/>
+
+  <node pkg="autoware_localization_error_monitor" exec="autoware_localization_error_monitor_node" output="both">
+    <remap from="input/odom" to="$(var input/odom)"/>
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

master/src/Localization/autoware_localization_error_monitor/package.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_localization_error_monitor</name>
+  <version>0.1.0</version>
+  <description>ros node for monitoring localization error</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="taichi.higashide@tier4.jp">Taichi Higashide</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+  <buildtool_depend>eigen</buildtool_depend>
+
+  <depend>autoware_localization_util</depend>
+  <depend>autoware_universe_utils</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Localization/autoware_localization_error_monitor/schema/localization_error_monitor.schema.json

@@ -0,0 +1,56 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Localization Error Monitor node",
+  "type": "object",
+  "definitions": {
+    "localization_error_monitor": {
+      "type": "object",
+      "properties": {
+        "scale": {
+          "type": "number",
+          "default": 3.0,
+          "description": "scale factor for monitored values"
+        },
+        "error_ellipse_size": {
+          "type": "number",
+          "default": 1.5,
+          "description": "error threshold for long radius of confidence ellipse [m]"
+        },
+        "warn_ellipse_size": {
+          "type": "number",
+          "default": 1.2,
+          "description": "warning threshold for long radius of confidence ellipse [m]"
+        },
+        "error_ellipse_size_lateral_direction": {
+          "type": "number",
+          "default": 0.3,
+          "description": "error threshold for size of confidence ellipse along lateral direction [m]"
+        },
+        "warn_ellipse_size_lateral_direction": {
+          "type": "number",
+          "default": 0.25,
+          "description": "warning threshold for size of confidence ellipse along lateral direction [m]"
+        }
+      },
+      "required": [
+        "scale",
+        "error_ellipse_size",
+        "warn_ellipse_size",
+        "error_ellipse_size_lateral_direction",
+        "warn_ellipse_size_lateral_direction"
+      ]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/localization_error_monitor"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

master/src/Localization/autoware_localization_error_monitor/src/diagnostics_helper.hpp

@@ -0,0 +1,65 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DIAGNOSTICS_HELPER_HPP_
+#define DIAGNOSTICS_HELPER_HPP_
+
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::localization_error_monitor
+{
+inline diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy(
+  const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (ellipse_size >= warn_ellipse_size) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "ellipse size is too large";
+  }
+  if (ellipse_size >= error_ellipse_size) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "ellipse size is over the expected range";
+  }
+
+  return stat;
+}
+
+inline diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction(
+  const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat.message = "OK";
+  if (ellipse_size >= warn_ellipse_size) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    stat.message = "ellipse size along lateral direction is too large";
+  }
+  if (ellipse_size >= error_ellipse_size) {
+    stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    stat.message = "ellipse size along lateral direction is over the expected range";
+  }
+
+  return stat;
+}
+
+}  // namespace autoware::localization_error_monitor
+
+#endif  // DIAGNOSTICS_HELPER_HPP_

master/src/Localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp

@@ -0,0 +1,96 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "localization_error_monitor.hpp"
+
+#include "diagnostics_helper.hpp"
+
+#include <Eigen/Dense>
+
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace autoware::localization_error_monitor
+{
+LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & options)
+: Node("localization_error_monitor", options)
+{
+  scale_ = this->declare_parameter<double>("scale");
+  error_ellipse_size_ = this->declare_parameter<double>("error_ellipse_size");
+  warn_ellipse_size_ = this->declare_parameter<double>("warn_ellipse_size");
+
+  error_ellipse_size_lateral_direction_ =
+    this->declare_parameter<double>("error_ellipse_size_lateral_direction");
+  warn_ellipse_size_lateral_direction_ =
+    this->declare_parameter<double>("warn_ellipse_size_lateral_direction");
+
+  odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+    "input/odom", 1, std::bind(&LocalizationErrorMonitor::on_odom, this, std::placeholders::_1));
+
+  // QoS setup
+  rclcpp::QoS durable_qos(1);
+  durable_qos.transient_local();  // option for latching
+  ellipse_marker_pub_ =
+    this->create_publisher<visualization_msgs::msg::Marker>("debug/ellipse_marker", durable_qos);
+
+  logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
+
+  diagnostics_error_monitor_ =
+    std::make_unique<autoware::localization_util::DiagnosticsModule>(this, "ellipse_error_status");
+}
+
+void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg)
+{
+  diagnostics_error_monitor_->clear();
+
+  ellipse_ = autoware::localization_util::calculate_xy_ellipse(input_msg->pose, scale_);
+
+  const auto ellipse_marker = autoware::localization_util::create_ellipse_marker(
+    ellipse_, input_msg->header, input_msg->pose);
+  ellipse_marker_pub_->publish(ellipse_marker);
+
+  // update localization accuracy diagnostics
+  const auto accuracy_status =
+    check_localization_accuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_);
+  diagnostics_error_monitor_->add_key_value("localization_error_ellipse", ellipse_.long_radius);
+  diagnostics_error_monitor_->update_level_and_message(
+    accuracy_status.level, accuracy_status.message);
+
+  // update lateral direction error diagnostics
+  const auto lateral_direction_status = check_localization_accuracy_lateral_direction(
+    ellipse_.size_lateral_direction, warn_ellipse_size_lateral_direction_,
+    error_ellipse_size_lateral_direction_);
+  diagnostics_error_monitor_->add_key_value(
+    "localization_error_ellipse_lateral_direction", ellipse_.size_lateral_direction);
+  diagnostics_error_monitor_->update_level_and_message(
+    lateral_direction_status.level, lateral_direction_status.message);
+
+  diagnostics_error_monitor_->publish(this->now());
+}
+}  // namespace autoware::localization_error_monitor
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::localization_error_monitor::LocalizationErrorMonitor)

master/src/Localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp

@@ -0,0 +1,58 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LOCALIZATION_ERROR_MONITOR_HPP_
+#define LOCALIZATION_ERROR_MONITOR_HPP_
+
+#include "autoware/localization_util/covariance_ellipse.hpp"
+#include "autoware/localization_util/diagnostics_module.hpp"
+
+#include <Eigen/Dense>
+#include <autoware/universe_utils/ros/logger_level_configure.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <memory>
+
+namespace autoware::localization_error_monitor
+{
+class LocalizationErrorMonitor : public rclcpp::Node
+{
+private:
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr ellipse_marker_pub_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
+
+  std::unique_ptr<autoware::localization_util::DiagnosticsModule> diagnostics_error_monitor_;
+
+  double scale_;
+  double error_ellipse_size_;
+  double warn_ellipse_size_;
+  double error_ellipse_size_lateral_direction_;
+  double warn_ellipse_size_lateral_direction_;
+  autoware::localization_util::Ellipse ellipse_;
+
+  void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg);
+
+public:
+  explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options);
+};
+}  // namespace autoware::localization_error_monitor
+
+#endif  // LOCALIZATION_ERROR_MONITOR_HPP_

master/src/Localization/autoware_localization_error_monitor/test/test_diagnostics.cpp

@@ -0,0 +1,83 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "diagnostics_helper.hpp"
+
+#include <gtest/gtest.h>
+
+TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const double warn_ellipse_size = 0.8;
+  const double error_ellipse_size = 1.0;
+
+  double ellipse_size = 0.0;
+  stat = autoware::localization_error_monitor::check_localization_accuracy(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  ellipse_size = 0.7;
+  stat = autoware::localization_error_monitor::check_localization_accuracy(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  ellipse_size = 0.8;
+  stat = autoware::localization_error_monitor::check_localization_accuracy(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  ellipse_size = 0.9;
+  stat = autoware::localization_error_monitor::check_localization_accuracy(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  ellipse_size = 1.0;
+  stat = autoware::localization_error_monitor::check_localization_accuracy(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+}
+
+TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDirection)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const double warn_ellipse_size = 0.25;
+  const double error_ellipse_size = 0.3;
+
+  double ellipse_size = 0.0;
+  stat = autoware::localization_error_monitor::check_localization_accuracy_lateral_direction(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  ellipse_size = 0.24;
+  stat = autoware::localization_error_monitor::check_localization_accuracy_lateral_direction(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  ellipse_size = 0.25;
+  stat = autoware::localization_error_monitor::check_localization_accuracy_lateral_direction(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  ellipse_size = 0.29;
+  stat = autoware::localization_error_monitor::check_localization_accuracy_lateral_direction(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  ellipse_size = 0.3;
+  stat = autoware::localization_error_monitor::check_localization_accuracy_lateral_direction(
+    ellipse_size, warn_ellipse_size, error_ellipse_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+}

master/src/Localization/autoware_localization_util/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_localization_util)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/util_func.cpp
+  src/diagnostics_module.cpp
+  src/smart_pose_buffer.cpp
+  src/tree_structured_parzen_estimator.cpp
+  src/covariance_ellipse.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_auto_add_gtest(test_smart_pose_buffer
+    test/test_smart_pose_buffer.cpp
+    src/smart_pose_buffer.cpp
+  )
+
+  ament_auto_add_gtest(test_tpe
+    test/test_tpe.cpp
+    src/tree_structured_parzen_estimator.cpp
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+)

master/src/Localization/autoware_localization_util/README.md

@@ -0,0 +1,5 @@
+# autoware_localization_util
+
+`autoware_localization_util` is a localization utility package.
+
+This package does not have a node, it is just a library.

master/src/Localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp

@@ -0,0 +1,44 @@
+// Copyright 2024 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/msg/pose_with_covariance.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+namespace autoware::localization_util
+{
+
+struct Ellipse
+{
+  double long_radius;
+  double short_radius;
+  double yaw;
+  Eigen::Matrix2d P;
+  double size_lateral_direction;
+};
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale);
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance);
+
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_

master/src/Localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp

@@ -0,0 +1,64 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <string>
+#include <vector>
+
+namespace autoware::localization_util
+{
+class DiagnosticsModule
+{
+public:
+  DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
+  void clear();
+  void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
+  template <typename T>
+  void add_key_value(const std::string & key, const T & value);
+  void update_level_and_message(const int8_t level, const std::string & message);
+  void publish(const rclcpp::Time & publish_time_stamp);
+
+private:
+  [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array(
+    const rclcpp::Time & publish_time_stamp) const;
+
+  rclcpp::Clock::SharedPtr clock_;
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
+
+  diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_;
+};
+
+template <typename T>
+void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = std::to_string(value);
+  add_key_value(key_value);
+}
+
+template <>
+void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
+template <>
+void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
+
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_

master/src/Localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp

@@ -0,0 +1,26 @@
+// Copyright 2021 TierIV
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_
+
+#include <Eigen/Core>
+
+namespace autoware::localization_util
+{
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_

master/src/Localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp

@@ -0,0 +1,71 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_
+
+#include "autoware/localization_util/util_func.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <deque>
+
+namespace autoware::localization_util
+{
+class SmartPoseBuffer
+{
+private:
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+
+public:
+  struct InterpolateResult
+  {
+    PoseWithCovarianceStamped old_pose;
+    PoseWithCovarianceStamped new_pose;
+    PoseWithCovarianceStamped interpolated_pose;
+  };
+
+  SmartPoseBuffer() = delete;
+  SmartPoseBuffer(
+    const rclcpp::Logger & logger, const double & pose_timeout_sec,
+    const double & pose_distance_tolerance_meters);
+
+  std::optional<InterpolateResult> interpolate(const rclcpp::Time & target_ros_time);
+
+  void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr);
+
+  void pop_old(const rclcpp::Time & target_ros_time);
+
+  void clear();
+
+private:
+  rclcpp::Logger logger_;
+  std::deque<PoseWithCovarianceStamped::ConstSharedPtr> pose_buffer_;
+  std::mutex mutex_;  // This mutex is for pose_buffer_
+
+  const double pose_timeout_sec_;
+  const double pose_distance_tolerance_meters_;
+
+  [[nodiscard]] bool validate_time_stamp_difference(
+    const rclcpp::Time & target_time, const rclcpp::Time & reference_time,
+    const double time_tolerance_sec) const;
+  [[nodiscard]] bool validate_position_difference(
+    const geometry_msgs::msg::Point & target_point,
+    const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const;
+};
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_

master/src/Localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp

@@ -0,0 +1,87 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+
+/*
+A implementation of tree-structured parzen estimator (TPE)
+See below pdf for the TPE algorithm detail.
+https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf
+
+Optuna is also used as a reference for implementation.
+https://github.com/optuna/optuna
+*/
+
+#include <cstdint>
+#include <random>
+#include <vector>
+
+namespace autoware::localization_util
+{
+class TreeStructuredParzenEstimator
+{
+public:
+  using Input = std::vector<double>;
+  using Score = double;
+  struct Trial
+  {
+    Input input;
+    Score score;
+  };
+
+  enum Direction {
+    MINIMIZE = 0,
+    MAXIMIZE = 1,
+  };
+
+  enum Index {
+    TRANS_X = 0,
+    TRANS_Y = 1,
+    TRANS_Z = 2,
+    ANGLE_X = 3,
+    ANGLE_Y = 4,
+    ANGLE_Z = 5,
+    INDEX_NUM = 6,
+  };
+
+  TreeStructuredParzenEstimator() = delete;
+  TreeStructuredParzenEstimator(
+    const Direction direction, const int64_t n_startup_trials, std::vector<double> sample_mean,
+    std::vector<double> sample_stddev);
+  void add_trial(const Trial & trial);
+  [[nodiscard]] Input get_next_input() const;
+
+private:
+  static constexpr double max_good_rate = 0.10;
+  static constexpr int64_t n_ei_candidates = 100;
+
+  static std::mt19937_64 engine;
+
+  [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const;
+  [[nodiscard]] static double log_gaussian_pdf(
+    const Input & input, const Input & mu, const Input & sigma);
+
+  std::vector<Trial> trials_;
+  int64_t above_num_;
+  const Direction direction_;
+  const int64_t n_startup_trials_;
+  const int64_t input_dimension_;
+  const std::vector<double> sample_mean_;
+  const std::vector<double> sample_stddev_;
+  Input base_stddev_;
+};
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_

master/src/Localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp

@@ -0,0 +1,88 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_
+#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <algorithm>
+#include <cmath>
+#include <deque>
+#include <random>
+#include <string>
+#include <vector>
+
+namespace autoware::localization_util
+{
+// ref by http://takacity.blog.fc2.com/blog-entry-69.html
+std_msgs::msg::ColorRGBA exchange_color_crc(double x);
+
+double calc_diff_for_radian(const double lhs_rad, const double rhs_rad);
+
+// x: roll, y: pitch, z: yaw
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose);
+geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+
+geometry_msgs::msg::Quaternion rpy_rad_to_quaternion(
+  const double r_rad, const double p_rad, const double y_rad);
+geometry_msgs::msg::Quaternion rpy_deg_to_quaternion(
+  const double r_deg, const double p_deg, const double y_deg);
+
+geometry_msgs::msg::Twist calc_twist(
+  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b);
+
+geometry_msgs::msg::PoseStamped interpolate_pose(
+  const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b,
+  const rclcpp::Time & time_stamp);
+
+geometry_msgs::msg::PoseStamped interpolate_pose(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a,
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp);
+
+Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose);
+Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose);
+geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix);
+Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos);
+
+template <class T>
+T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform)
+{
+  T output;
+  tf2::doTransform<T>(input, output, transform);
+  return output;
+}
+
+double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
+
+void output_pose_with_cov_to_log(
+  const rclcpp::Logger & logger, const std::string & prefix,
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov);
+
+}  // namespace autoware::localization_util
+
+#endif  // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_

master/src/Localization/autoware_localization_util/package.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_localization_util</name>
+  <version>0.1.0</version>
+  <description>The autoware_localization_util package</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <maintainer email="masahiro.sakamoto@tier4.jp">Masahiro Sakamoto</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="anh.nguyen.2@tier4.jp">NGUYEN Viet Anh</maintainer>
+  <maintainer email="taiki.yamada@tier4.jp">Taiki Yamada</maintainer>
+  <maintainer email="ryu.yamamoto@tier4.jp">Ryu Yamamoto</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="yamato.ando@tier4.jp">Yamato Ando</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Localization/autoware_localization_util/src/covariance_ellipse.cpp

@@ -0,0 +1,90 @@
+// Copyright 2024 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/localization_util/covariance_ellipse.hpp"
+
+#include <tf2/utils.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+namespace autoware::localization_util
+{
+
+Ellipse calculate_xy_ellipse(
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale)
+{
+  // input geometry_msgs::PoseWithCovariance contain 6x6 matrix
+  Eigen::Matrix2d xy_covariance;
+  const auto cov = pose_with_covariance.covariance;
+  xy_covariance(0, 0) = cov[0 * 6 + 0];
+  xy_covariance(0, 1) = cov[0 * 6 + 1];
+  xy_covariance(1, 0) = cov[1 * 6 + 0];
+  xy_covariance(1, 1) = cov[1 * 6 + 1];
+
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(xy_covariance);
+
+  Ellipse ellipse;
+
+  // eigen values and vectors are sorted in ascending order
+  ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1));
+  ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0));
+
+  // principal component vector
+  const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1);
+  ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x());
+
+  // ellipse size along lateral direction (body-frame)
+  ellipse.P = xy_covariance;
+  const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation);
+  const Eigen::Matrix2d & p_inv = ellipse.P.inverse();
+  Eigen::MatrixXd e(2, 1);
+  e(0, 0) = std::cos(yaw_vehicle);
+  e(1, 0) = std::sin(yaw_vehicle);
+  const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant());
+  ellipse.size_lateral_direction = scale * d;
+
+  return ellipse;
+}
+
+visualization_msgs::msg::Marker create_ellipse_marker(
+  const Ellipse & ellipse, const std_msgs::msg::Header & header,
+  const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance)
+{
+  tf2::Quaternion quat;
+  quat.setEuler(0, 0, ellipse.yaw);
+
+  const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0);
+  const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0);
+  visualization_msgs::msg::Marker marker;
+  marker.header = header;
+  marker.ns = "error_ellipse";
+  marker.id = 0;
+  marker.type = visualization_msgs::msg::Marker::SPHERE;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.pose = pose_with_covariance.pose;
+  marker.pose.orientation = tf2::toMsg(quat);
+  marker.scale.x = ellipse_long_radius * 2;
+  marker.scale.y = ellipse_short_radius * 2;
+  marker.scale.z = 0.01;
+  marker.color.a = 0.1;
+  marker.color.r = 0.0;
+  marker.color.g = 0.0;
+  marker.color.b = 1.0;
+  return marker;
+}
+
+}  // namespace autoware::localization_util

master/src/Localization/autoware_localization_util/src/diagnostics_module.cpp

@@ -0,0 +1,108 @@
+// Copyright 2023 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "autoware/localization_util/diagnostics_module.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <diagnostic_msgs/msg/diagnostic_array.hpp>
+
+#include <algorithm>
+#include <string>
+
+namespace autoware::localization_util
+{
+DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
+: clock_(node->get_clock())
+{
+  diagnostics_pub_ =
+    node->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", 10);
+
+  diagnostics_status_msg_.name =
+    std::string(node->get_name()) + std::string(": ") + diagnostic_name;
+  diagnostics_status_msg_.hardware_id = node->get_name();
+}
+
+void DiagnosticsModule::clear()
+{
+  diagnostics_status_msg_.values.clear();
+  diagnostics_status_msg_.values.shrink_to_fit();
+
+  diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  diagnostics_status_msg_.message = "";
+}
+
+void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
+{
+  auto it = std::find_if(
+    std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
+    [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; });
+
+  if (it != std::cend(diagnostics_status_msg_.values)) {
+    it->value = key_value_msg.value;
+  } else {
+    diagnostics_status_msg_.values.push_back(key_value_msg);
+  }
+}
+
+template <>
+void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = value;
+  add_key_value(key_value);
+}
+
+template <>
+void DiagnosticsModule::add_key_value(const std::string & key, const bool & value)
+{
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = key;
+  key_value.value = value ? "True" : "False";
+  add_key_value(key_value);
+}
+
+void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
+{
+  if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
+    if (!diagnostics_status_msg_.message.empty()) {
+      diagnostics_status_msg_.message += "; ";
+    }
+    diagnostics_status_msg_.message += message;
+  }
+  if (level > diagnostics_status_msg_.level) {
+    diagnostics_status_msg_.level = level;
+  }
+}
+
+void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
+{
+  diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp));
+}
+
+diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array(
+  const rclcpp::Time & publish_time_stamp) const
+{
+  diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
+  diagnostics_msg.header.stamp = publish_time_stamp;
+  diagnostics_msg.status.push_back(diagnostics_status_msg_);
+
+  if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) {
+    diagnostics_msg.status.at(0).message = "OK";
+  }
+
+  return diagnostics_msg;
+}
+}  // namespace autoware::localization_util