diff --git a/evaluator/kinematic_evaluator/CMakeLists.txt b/evaluator/kinematic_evaluator/CMakeLists.txt
new file mode 100644
index 000000000000..6c951d610bd8
--- /dev/null
+++ b/evaluator/kinematic_evaluator/CMakeLists.txt
@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.14)
+project(kinematic_evaluator)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/metrics_calculator.cpp
+  src/${PROJECT_NAME}_node.cpp
+  src/kinematic_evaluator_node.cpp
+  src/metrics/kinematic_metrics.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "kinematic_diagnostics::KinematicEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_kinematic_evaluator_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)
diff --git a/evaluator/kinematic_evaluator/README.md b/evaluator/kinematic_evaluator/README.md
new file mode 100644
index 000000000000..6e7caddd1f5b
--- /dev/null
+++ b/evaluator/kinematic_evaluator/README.md
@@ -0,0 +1,3 @@
+# Kinematic Evaluator
+
+TBD
diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp
new file mode 100644
index 000000000000..16b4230fc465
--- /dev/null
+++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp
@@ -0,0 +1,82 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_
+#define KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_
+
+#include "kinematic_evaluator/metrics_calculator.hpp"
+#include "kinematic_evaluator/stat.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace kinematic_diagnostics
+{
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+
+/**
+ * @brief Node for kinematic evaluation
+ */
+class KinematicEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit KinematicEvaluatorNode(const rclcpp::NodeOptions & node_options);
+  ~KinematicEvaluatorNode();
+
+  /**
+   * @brief callback on vehicle twist message
+   * @param [in] twist_msg twist message
+   */
+  void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+  /**
+   * @brief publish the given metric statistic
+   */
+  DiagnosticStatus generateDiagnosticStatus(
+    const Metric & metric, const Stat<double> & metric_stat) const;
+
+private:
+  geometry_msgs::msg::Pose getCurrentEgoPose() const;
+
+  // ROS
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_ptr_;
+  std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
+
+  // Parameters
+  std::string output_file_str_;
+
+  // Calculator
+  MetricsCalculator metrics_calculator_;
+  // Metrics
+  std::vector<Metric> metrics_;
+  std::deque<rclcpp::Time> stamps_;
+  std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
+  std::unordered_map<Metric, Stat<double>> metrics_dict_;
+};
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_
diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp
new file mode 100644
index 000000000000..d20bf079a363
--- /dev/null
+++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp
@@ -0,0 +1,39 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_
+#define KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_
+
+#include "kinematic_evaluator/stat.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace kinematic_diagnostics
+{
+namespace metrics
+{
+using nav_msgs::msg::Odometry;
+
+/**
+ * @brief calculate velocity deviation of the given trajectory from the reference trajectory
+ * @param [in] value new value
+ * @param [in] stat_prev input trajectory
+ * @return calculated statistics
+ */
+Stat<double> updateVelocityStats(const double & value, const Stat<double> stat_prev);
+
+}  // namespace metrics
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_
diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp
new file mode 100644
index 000000000000..2e46d655312d
--- /dev/null
+++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp
@@ -0,0 +1,62 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_
+#define KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_
+
+#include <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace kinematic_diagnostics
+{
+/**
+ * @brief Enumeration of velocity metrics
+ */
+enum class Metric {
+  velocity_stats,
+  SIZE,
+};
+
+static const std::unordered_map<std::string, Metric> str_to_metric = {
+  {"velocity_stats", Metric::velocity_stats}};
+static const std::unordered_map<Metric, std::string> metric_to_str = {
+  {Metric::velocity_stats, "velocity_stats"}};
+
+// Metrics descriptions
+static const std::unordered_map<Metric, std::string> metric_descriptions = {
+  {Metric::velocity_stats, "velocity_stats[m/s]"}};
+
+namespace details
+{
+static struct CheckCorrectMaps
+{
+  CheckCorrectMaps()
+  {
+    if (
+      str_to_metric.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_descriptions.size() != static_cast<size_t>(Metric::SIZE)) {
+      std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: ";
+      std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " "
+                << metric_descriptions.size() << std::endl;
+    }
+  }
+} check;
+
+}  // namespace details
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_
diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp
new file mode 100644
index 000000000000..67f152e30c9e
--- /dev/null
+++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp
@@ -0,0 +1,56 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_
+#define KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_
+
+#include "kinematic_evaluator/metrics/metric.hpp"
+#include "kinematic_evaluator/parameters.hpp"
+#include "kinematic_evaluator/stat.hpp"
+
+#include "geometry_msgs/msg/pose.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace kinematic_diagnostics
+{
+using nav_msgs::msg::Odometry;
+
+class MetricsCalculator
+{
+public:
+  Parameters parameters;
+
+  MetricsCalculator() = default;
+
+  /**
+   * @brief calculate
+   * @param [in] metric Metric enum value
+   * @param [in] odom Odometry
+   * @return string describing the requested metric
+   */
+  Stat<double> calculate(const Metric metric, const Odometry & odom) const;
+
+  /**
+   * @brief update Metrics
+   * @param [in] metric Previous metric enum value
+   * @param [in] odom Odometry
+   * @return string describing the requested metric
+   */
+  Stat<double> updateStat(
+    const Metric metric, const Odometry & odom, const Stat<double> stat_prev) const;
+};  // class MetricsCalculator
+
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_
diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp
new file mode 100644
index 000000000000..a2de980336e5
--- /dev/null
+++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp
@@ -0,0 +1,34 @@
+// Copyright 2021 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 KINEMATIC_EVALUATOR__PARAMETERS_HPP_
+#define KINEMATIC_EVALUATOR__PARAMETERS_HPP_
+
+#include "kinematic_evaluator/metrics/metric.hpp"
+
+#include <array>
+
+namespace kinematic_diagnostics
+{
+/**
+ * @brief Enumeration of trajectory metrics
+ */
+struct Parameters
+{
+  std::array<bool, static_cast<size_t>(Metric::SIZE)> metrics{};  // default values to false
+};                                                                // struct Parameters
+
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__PARAMETERS_HPP_
diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp b/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp
new file mode 100644
index 000000000000..096cb90b3dcf
--- /dev/null
+++ b/evaluator/kinematic_evaluator/include/kinematic_evaluator/stat.hpp
@@ -0,0 +1,93 @@
+// Copyright 2021 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 <iostream>
+#include <limits>
+
+#ifndef KINEMATIC_EVALUATOR__STAT_HPP_
+#define KINEMATIC_EVALUATOR__STAT_HPP_
+
+namespace kinematic_diagnostics
+{
+/**
+ * @brief class to incrementally build statistics
+ * @typedef T type of the values (default to double)
+ */
+template <typename T = double>
+class Stat
+{
+public:
+  /**
+   * @brief add a value
+   * @param value value to add
+   */
+  void add(const T & value)
+  {
+    if (value < min_) {
+      min_ = value;
+    }
+    if (value > max_) {
+      max_ = value;
+    }
+    ++count_;
+    mean_ = mean_ + (value - mean_) / count_;
+  }
+
+  /**
+   * @brief get the mean value
+   */
+  long double mean() const { return mean_; }
+
+  /**
+   * @brief get the minimum value
+   */
+  T min() const { return min_; }
+
+  /**
+   * @brief get the maximum value
+   */
+  T max() const { return max_; }
+
+  /**
+   * @brief get the number of values used to build this statistic
+   */
+  unsigned int count() const { return count_; }
+
+  template <typename U>
+  friend std::ostream & operator<<(std::ostream & os, const Stat<U> & stat);
+
+private:
+  T min_ = std::numeric_limits<T>::max();
+  T max_ = std::numeric_limits<T>::min();
+  long double mean_ = 0.0;
+  unsigned int count_ = 0;
+};
+
+/**
+ * @brief overload << operator for easy print to output stream
+ */
+template <typename T>
+std::ostream & operator<<(std::ostream & os, const Stat<T> & stat)
+{
+  if (stat.count() == 0) {
+    os << "None None None";
+  } else {
+    os << stat.min() << " " << stat.max() << " " << stat.mean();
+  }
+  return os;
+}
+
+}  // namespace kinematic_diagnostics
+
+#endif  // KINEMATIC_EVALUATOR__STAT_HPP_
diff --git a/evaluator/kinematic_evaluator/launch/kinematic_evaluator.launch.xml b/evaluator/kinematic_evaluator/launch/kinematic_evaluator.launch.xml
new file mode 100644
index 000000000000..ebb89c2897ce
--- /dev/null
+++ b/evaluator/kinematic_evaluator/launch/kinematic_evaluator.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <arg name="input/twist" default="/localization/kinematic_state"/>
+
+  <node name="kinematic_evaluator" exec="kinematic_evaluator" pkg="kinematic_evaluator" output="screen">
+    <param from="$(find-pkg-share kinematic_evaluator)/param/kinematic_evaluator.defaults.yaml"/>
+    <remap from="~/input/twist" to="$(var input/twist)"/>
+  </node>
+</launch>
diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml
new file mode 100644
index 000000000000..e761a53cb629
--- /dev/null
+++ b/evaluator/kinematic_evaluator/package.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>kinematic_evaluator</name>
+  <version>0.1.0</version>
+  <description>kinematic evaluator ROS2 node</description>
+
+  <maintainer email="dominik.jargot@robotec.ai">Dominik Jargot</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>eigen</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_cmake_gtest</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>
diff --git a/evaluator/kinematic_evaluator/param/kinematic_evaluator.defaults.yaml b/evaluator/kinematic_evaluator/param/kinematic_evaluator.defaults.yaml
new file mode 100644
index 000000000000..b343943806b7
--- /dev/null
+++ b/evaluator/kinematic_evaluator/param/kinematic_evaluator.defaults.yaml
@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    output_file: kinematic_metrics.results # if empty, metrics are not written to file
+
+    selected_metrics:
+      - velocity_stats
diff --git a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp b/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp
new file mode 100644
index 000000000000..57631e5f861d
--- /dev/null
+++ b/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp
@@ -0,0 +1,145 @@
+// Copyright 2021 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 "kinematic_evaluator/kinematic_evaluator_node.hpp"
+
+#include "boost/lexical_cast.hpp"
+
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace kinematic_diagnostics
+{
+KinematicEvaluatorNode::KinematicEvaluatorNode(const rclcpp::NodeOptions & node_options)
+: Node("kinematic_evaluator", node_options)
+{
+  tf_buffer_ptr_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ptr_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_ptr_);
+
+  odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
+    "~/input/twist", rclcpp::QoS{1},
+    std::bind(&KinematicEvaluatorNode::onOdom, this, std::placeholders::_1));
+
+  output_file_str_ = declare_parameter<std::string>("output_file");
+  if (output_file_str_.empty()) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Output file not specified, the results will NOT be saved!"
+      "Provide output_file parameter to store the results.");
+  }
+  // List of metrics to calculate
+  metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
+  for (const std::string & selected_metric :
+       declare_parameter<std::vector<std::string>>("selected_metrics")) {
+    Metric metric = str_to_metric.at(selected_metric);
+    metrics_dict_[metric] = Stat<double>();
+    metrics_.push_back(metric);
+  }
+}
+
+KinematicEvaluatorNode::~KinematicEvaluatorNode()
+{
+  if (!output_file_str_.empty()) {
+    std::ofstream f(output_file_str_);
+    f << std::left << std::fixed;
+    // header
+    f << "#Data collected over: " << stamps_.back().seconds() - stamps_[0].seconds() << " seconds."
+      << std::endl;
+    f << std::setw(24) << "#Stamp [ns]";
+
+    for (Metric metric : metrics_) {
+      f << std::setw(30) << metric_descriptions.at(metric);
+    }
+    f << std::endl;
+    f << std::setw(24) << "#";
+    for (Metric metric : metrics_) {
+      (void)metric;
+      f << std::setw(9) << "min" << std::setw(9) << "max" << std::setw(12) << "mean";
+    }
+    f << std::endl;
+    // data
+    f << std::setw(24) << stamps_.back().nanoseconds();
+    for (Metric metric : metrics_) {
+      const auto & stat = metric_stats_[static_cast<size_t>(metric)].back();
+      f << stat;
+      f << std::setw(4) << "";
+    }
+    f.close();
+  }
+}
+
+DiagnosticStatus KinematicEvaluatorNode::generateDiagnosticStatus(
+  const Metric & metric, const Stat<double> & metric_stat) const
+{
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = metric_to_str.at(metric);
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "min";
+  key_value.value = boost::lexical_cast<decltype(key_value.value)>(metric_stat.min());
+  status.values.push_back(key_value);
+  key_value.key = "max";
+  key_value.value = boost::lexical_cast<decltype(key_value.value)>(metric_stat.max());
+  status.values.push_back(key_value);
+  key_value.key = "mean";
+  key_value.value = boost::lexical_cast<decltype(key_value.value)>(metric_stat.mean());
+  status.values.push_back(key_value);
+  return status;
+}
+
+void KinematicEvaluatorNode::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
+{
+  DiagnosticArray metrics_msg;
+  metrics_msg.header.stamp = now();
+
+  for (Metric metric : metrics_) {
+    metrics_dict_[metric] = metrics_calculator_.updateStat(metric, *msg, metrics_dict_[metric]);
+    metric_stats_[static_cast<size_t>(metric)].push_back(metrics_dict_[metric]);
+    stamps_.push_back(metrics_msg.header.stamp);
+    if (metrics_dict_[metric].count() > 0) {
+      metrics_msg.status.push_back(generateDiagnosticStatus(metric, metrics_dict_[metric]));
+    }
+  }
+  if (!metrics_msg.status.empty()) {
+    metrics_pub_->publish(metrics_msg);
+  }
+}
+
+geometry_msgs::msg::Pose KinematicEvaluatorNode::getCurrentEgoPose() const
+{
+  geometry_msgs::msg::TransformStamped tf_current_pose;
+
+  geometry_msgs::msg::Pose p;
+  try {
+    tf_current_pose = tf_buffer_ptr_->lookupTransform(
+      "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(get_logger(), "%s", ex.what());
+    return p;
+  }
+
+  p.orientation = tf_current_pose.transform.rotation;
+  p.position.x = tf_current_pose.transform.translation.x;
+  p.position.y = tf_current_pose.transform.translation.y;
+  p.position.z = tf_current_pose.transform.translation.z;
+  return p;
+}
+
+}  // namespace kinematic_diagnostics
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(kinematic_diagnostics::KinematicEvaluatorNode)
diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp
new file mode 100644
index 000000000000..2a22692dc5ac
--- /dev/null
+++ b/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp
@@ -0,0 +1,32 @@
+// Copyright 2021 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 "kinematic_evaluator/metrics/kinematic_metrics.hpp"
+
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+namespace kinematic_diagnostics
+{
+namespace metrics
+{
+
+Stat<double> updateVelocityStats(const double & value, const Stat<double> stat_prev)
+{
+  Stat<double> stat(stat_prev);
+  stat.add(value);
+  return stat;
+}
+
+}  // namespace metrics
+}  // namespace kinematic_diagnostics
diff --git a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp b/evaluator/kinematic_evaluator/src/metrics_calculator.cpp
new file mode 100644
index 000000000000..af1e1114b9c4
--- /dev/null
+++ b/evaluator/kinematic_evaluator/src/metrics_calculator.cpp
@@ -0,0 +1,34 @@
+// Copyright 2021 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 "kinematic_evaluator/metrics_calculator.hpp"
+
+#include "kinematic_evaluator/metrics/kinematic_metrics.hpp"
+
+namespace kinematic_diagnostics
+{
+Stat<double> MetricsCalculator::updateStat(
+  const Metric metric, const Odometry & odom, const Stat<double> stat_prev) const
+{
+  switch (metric) {
+    case Metric::velocity_stats:
+      return metrics::updateVelocityStats(odom.twist.twist.linear.x, stat_prev);
+    default:
+      throw std::runtime_error(
+        "[MetricsCalculator][calculate()] unknown Metric " +
+        std::to_string(static_cast<int>(metric)));
+  }
+}
+
+}  // namespace kinematic_diagnostics
diff --git a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp b/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp
new file mode 100644
index 000000000000..314bace43eb5
--- /dev/null
+++ b/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp
@@ -0,0 +1,141 @@
+// Copyright 2021 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 "ament_index_cpp/get_package_share_directory.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+#include <kinematic_evaluator/kinematic_evaluator_node.hpp>
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+#include "boost/lexical_cast.hpp"
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+using EvalNode = kinematic_diagnostics::KinematicEvaluatorNode;
+using diagnostic_msgs::msg::DiagnosticArray;
+using nav_msgs::msg::Odometry;
+
+class EvalTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+
+    rclcpp::NodeOptions options;
+    const auto share_dir = ament_index_cpp::get_package_share_directory("kinematic_evaluator");
+    options.arguments(
+      {"--ros-args", "--params-file", share_dir + "/param/kinematic_evaluator.defaults.yaml"});
+
+    dummy_node = std::make_shared<rclcpp::Node>("kinematic_evaluator_test_node");
+    eval_node = std::make_shared<EvalNode>(options);
+    // Enable all logging in the node
+    auto ret = rcutils_logging_set_logger_level(
+      dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+    if (ret != RCUTILS_RET_OK) {
+      std::cerr << "Failed to set logging severity to DEBUG\n";
+    }
+    ret = rcutils_logging_set_logger_level(
+      eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+    if (ret != RCUTILS_RET_OK) {
+      std::cerr << "Failed to set logging severity to DEBUG\n";
+    }
+
+    odom_pub_ =
+      rclcpp::create_publisher<Odometry>(dummy_node, "/kinematic_evaluator/input/twist", 1);
+
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
+  }
+
+  ~EvalTest() override
+  { /*rclcpp::shutdown();*/
+  }
+
+  void setTargetMetric(kinematic_diagnostics::Metric metric)
+  {
+    const auto metric_str = kinematic_diagnostics::metric_to_str.at(metric);
+    const auto is_target_metric = [metric_str](const auto & status) {
+      return status.name == metric_str;
+    };
+    metric_sub_ = rclcpp::create_subscription<DiagnosticArray>(
+      dummy_node, "/kinematic_evaluator/metrics", 1,
+      [=](const DiagnosticArray::ConstSharedPtr msg) {
+        const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
+        if (it != msg->status.end()) {
+          metric_value_ = boost::lexical_cast<double>(it->values[2].value);
+          metric_updated_ = true;
+        }
+      });
+  }
+
+  Odometry makeOdometry(const double speed)
+  {
+    Odometry odometry;
+    odometry.header.frame_id = "map";
+    odometry.twist.twist.linear.x = speed;
+    return odometry;
+  }
+
+  void publishOdometry(const Odometry & odom)
+  {
+    odom_pub_->publish(odom);
+    rclcpp::spin_some(eval_node);
+    rclcpp::spin_some(dummy_node);
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  double publishOdometryAndGetMetric(const Odometry & odom)
+  {
+    metric_updated_ = false;
+    odom_pub_->publish(odom);
+    while (!metric_updated_) {
+      rclcpp::spin_some(eval_node);
+      rclcpp::spin_some(dummy_node);
+      rclcpp::sleep_for(std::chrono::milliseconds(100));
+    }
+    return metric_value_;
+  }
+
+  // Latest metric value
+  bool metric_updated_ = false;
+  double metric_value_;
+  // Node
+  rclcpp::Node::SharedPtr dummy_node;
+  EvalNode::SharedPtr eval_node;
+  // Trajectory publishers
+  rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
+  rclcpp::Subscription<DiagnosticArray>::SharedPtr metric_sub_;
+  // TF broadcaster
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+};
+
+TEST_F(EvalTest, TestVelocityStats)
+{
+  setTargetMetric(kinematic_diagnostics::Metric::velocity_stats);
+  Odometry odom = makeOdometry(0.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom), 0.0);
+  Odometry odom2 = makeOdometry(1.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom2), 0.5);
+  Odometry odom3 = makeOdometry(2.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom3), 1.0);
+}
diff --git a/evaluator/localization_evaluator/CMakeLists.txt b/evaluator/localization_evaluator/CMakeLists.txt
new file mode 100644
index 000000000000..d465e7880195
--- /dev/null
+++ b/evaluator/localization_evaluator/CMakeLists.txt
@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 3.5)
+project(localization_evaluator)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/metrics_calculator.cpp
+  src/${PROJECT_NAME}_node.cpp
+  src/localization_evaluator_node.cpp
+  src/metrics/localization_metrics.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "localization_diagnostics::LocalizationEvaluatorNode"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_localization_evaluator_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+    param
+    launch
+)
diff --git a/evaluator/localization_evaluator/README.md b/evaluator/localization_evaluator/README.md
new file mode 100644
index 000000000000..1aada473d8e5
--- /dev/null
+++ b/evaluator/localization_evaluator/README.md
@@ -0,0 +1,3 @@
+# Localization Evaluator
+
+TBD
diff --git a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp
new file mode 100644
index 000000000000..8c93ddd5fa0f
--- /dev/null
+++ b/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp
@@ -0,0 +1,101 @@
+// Copyright 2021 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_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_
+#define LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_
+
+#include "localization_evaluator/metrics_calculator.hpp"
+#include "localization_evaluator/stat.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace localization_diagnostics
+{
+using diagnostic_msgs::msg::DiagnosticArray;
+using diagnostic_msgs::msg::DiagnosticStatus;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using nav_msgs::msg::Odometry;
+/**
+ * @brief Node for localization evaluation
+ */
+class LocalizationEvaluatorNode : public rclcpp::Node
+{
+public:
+  explicit LocalizationEvaluatorNode(const rclcpp::NodeOptions & node_options);
+  ~LocalizationEvaluatorNode();
+
+  /**
+   * @brief synchronized callback on current and gt localization
+   * @param [in] msg odometry message
+   * @param [in] msg_ref reference pose
+   */
+  void syncCallback(
+    const Odometry::ConstSharedPtr & msg,
+    const PoseWithCovarianceStamped::ConstSharedPtr & msg_ref);
+
+  /**
+   * @brief callback on current odometry
+   * @param [in] msg odometry message
+   */
+  void onOdom(const Odometry::SharedPtr msg);
+
+  /**
+   * @brief publish the given metric statistic
+   */
+  DiagnosticStatus generateDiagnosticStatus(
+    const Metric & metric, const Stat<double> & metric_stat) const;
+
+private:
+  // ROS
+  message_filters::Subscriber<Odometry> odom_sub_;
+  message_filters::Subscriber<PoseWithCovarianceStamped> pose_gt_sub_;
+
+  typedef message_filters::sync_policies::ApproximateTime<Odometry, PoseWithCovarianceStamped>
+    SyncPolicy;
+  typedef message_filters::Synchronizer<SyncPolicy> SyncExact;
+  SyncExact sync_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_ptr_;
+  std::unique_ptr<tf2_ros::TransformListener> tf_listener_ptr_;
+
+  // Parameters
+  std::string output_file_str_;
+
+  // Calculator
+  MetricsCalculator metrics_calculator_;
+  // Metrics
+  std::vector<Metric> metrics_;
+  std::deque<rclcpp::Time> stamps_;
+  std::array<std::deque<Stat<double>>, static_cast<size_t>(Metric::SIZE)> metric_stats_;
+  std::unordered_map<Metric, Stat<double>> metrics_dict_;
+};
+}  // namespace localization_diagnostics
+
+#endif  // LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_
diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp
new file mode 100644
index 000000000000..c973d285fd50
--- /dev/null
+++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp
@@ -0,0 +1,50 @@
+// Copyright 2021 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_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_
+#define LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_
+
+#include "localization_evaluator/stat.hpp"
+
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace localization_diagnostics
+{
+namespace metrics
+{
+/**
+ * @brief calculate lateral localization error
+ * @param [in] stat_prev statistics to update
+ * @param [in] lateral_pos lateral position of the vehicle
+ * @param [in] lateral_ref reference lateral position
+ * @return calculated statistics
+ */
+Stat<double> updateLateralStats(
+  const Stat<double> stat_prev, const double & lateral_pos, const double & lateral_ref);
+
+/**
+ * @brief calculate absolute localization error
+ * @param [in] stat_prev statistics to update
+ * @param [in] pos current position of the vehicle
+ * @param [in] pos_ref reference position of the vehicle
+ * @return calculated statistics
+ */
+Stat<double> updateAbsoluteStats(
+  const Stat<double> stat_prev, const geometry_msgs::msg::Point & pos,
+  const geometry_msgs::msg::Point & pos_ref);
+
+}  // namespace metrics
+}  // namespace localization_diagnostics
+
+#endif  // LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_
diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics/metric.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics/metric.hpp
new file mode 100644
index 000000000000..dfe1a538a348
--- /dev/null
+++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics/metric.hpp
@@ -0,0 +1,63 @@
+// Copyright 2021 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_EVALUATOR__METRICS__METRIC_HPP_
+#define LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_
+
+#include <iostream>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+namespace localization_diagnostics
+{
+/**
+ * @brief Enumeration of localization metrics
+ */
+enum class Metric {
+  lateral_error,
+  absolute_error,
+  SIZE,
+};
+
+static const std::unordered_map<std::string, Metric> str_to_metric = {
+  {"lateral_error", Metric::lateral_error}, {"absolute_error", Metric::absolute_error}};
+static const std::unordered_map<Metric, std::string> metric_to_str = {
+  {Metric::lateral_error, "lateral_error"}, {Metric::absolute_error, "absolute_error"}};
+
+// Metrics descriptions
+static const std::unordered_map<Metric, std::string> metric_descriptions = {
+  {Metric::lateral_error, "lateral error [m]"}, {Metric::absolute_error, "absolute error [m]"}};
+
+namespace details
+{
+static struct CheckCorrectMaps
+{
+  CheckCorrectMaps()
+  {
+    if (
+      str_to_metric.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_to_str.size() != static_cast<size_t>(Metric::SIZE) ||
+      metric_descriptions.size() != static_cast<size_t>(Metric::SIZE)) {
+      std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: ";
+      std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " "
+                << metric_descriptions.size() << std::endl;
+    }
+  }
+} check;
+
+}  // namespace details
+}  // namespace localization_diagnostics
+
+#endif  // LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_
diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp
new file mode 100644
index 000000000000..88f8ea85d426
--- /dev/null
+++ b/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp
@@ -0,0 +1,48 @@
+// Copyright 2021 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_EVALUATOR__METRICS_CALCULATOR_HPP_
+#define LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_
+
+#include "localization_evaluator/metrics/metric.hpp"
+#include "localization_evaluator/parameters.hpp"
+#include "localization_evaluator/stat.hpp"
+
+#include "geometry_msgs/msg/pose.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace localization_diagnostics
+{
+class MetricsCalculator
+{
+public:
+  Parameters parameters;
+
+  MetricsCalculator() = default;
+  /**
+   * @brief update Metrics
+   * @param [in] stat_prev Previous statistics
+   * @param [in] metric metric enum value
+   * @param [in] pos current position
+   * @param [in] pos_ref reference position
+   * @return string describing the requested metric
+   */
+  Stat<double> updateStat(
+    const Stat<double> stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos,
+    const geometry_msgs::msg::Point & pos_ref) const;
+};  // class MetricsCalculator
+
+}  // namespace localization_diagnostics
+
+#endif  // LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_
diff --git a/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp b/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp
new file mode 100644
index 000000000000..46cdf2e77bf1
--- /dev/null
+++ b/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp
@@ -0,0 +1,34 @@
+// Copyright 2021 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_EVALUATOR__PARAMETERS_HPP_
+#define LOCALIZATION_EVALUATOR__PARAMETERS_HPP_
+
+#include "localization_evaluator/metrics/metric.hpp"
+
+#include <array>
+
+namespace localization_diagnostics
+{
+/**
+ * @brief Enumeration of trajectory metrics
+ */
+struct Parameters
+{
+  std::array<bool, static_cast<size_t>(Metric::SIZE)> metrics{};  // default values to false
+};                                                                // struct Parameters
+
+}  // namespace localization_diagnostics
+
+#endif  // LOCALIZATION_EVALUATOR__PARAMETERS_HPP_
diff --git a/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp b/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp
new file mode 100644
index 000000000000..fbea5b61813f
--- /dev/null
+++ b/evaluator/localization_evaluator/include/localization_evaluator/stat.hpp
@@ -0,0 +1,93 @@
+// Copyright 2021 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 <iostream>
+#include <limits>
+
+#ifndef LOCALIZATION_EVALUATOR__STAT_HPP_
+#define LOCALIZATION_EVALUATOR__STAT_HPP_
+
+namespace localization_diagnostics
+{
+/**
+ * @brief class to incrementally build statistics
+ * @typedef T type of the values (default to double)
+ */
+template <typename T = double>
+class Stat
+{
+public:
+  /**
+   * @brief add a value
+   * @param value value to add
+   */
+  void add(const T & value)
+  {
+    if (value < min_) {
+      min_ = value;
+    }
+    if (value > max_) {
+      max_ = value;
+    }
+    ++count_;
+    mean_ = mean_ + (value - mean_) / count_;
+  }
+
+  /**
+   * @brief get the mean value
+   */
+  long double mean() const { return mean_; }
+
+  /**
+   * @brief get the minimum value
+   */
+  T min() const { return min_; }
+
+  /**
+   * @brief get the maximum value
+   */
+  T max() const { return max_; }
+
+  /**
+   * @brief get the number of values used to build this statistic
+   */
+  unsigned int count() const { return count_; }
+
+  template <typename U>
+  friend std::ostream & operator<<(std::ostream & os, const Stat<U> & stat);
+
+private:
+  T min_ = std::numeric_limits<T>::max();
+  T max_ = std::numeric_limits<T>::min();
+  long double mean_ = 0.0;
+  unsigned int count_ = 0;
+};
+
+/**
+ * @brief overload << operator for easy print to output stream
+ */
+template <typename T>
+std::ostream & operator<<(std::ostream & os, const Stat<T> & stat)
+{
+  if (stat.count() == 0) {
+    os << "None None None";
+  } else {
+    os << stat.min() << " " << stat.max() << " " << stat.mean();
+  }
+  return os;
+}
+
+}  // namespace localization_diagnostics
+
+#endif  // LOCALIZATION_EVALUATOR__STAT_HPP_
diff --git a/evaluator/localization_evaluator/launch/localization_evaluator.launch.xml b/evaluator/localization_evaluator/launch/localization_evaluator.launch.xml
new file mode 100644
index 000000000000..30232e917bfa
--- /dev/null
+++ b/evaluator/localization_evaluator/launch/localization_evaluator.launch.xml
@@ -0,0 +1,10 @@
+<launch>
+  <arg name="input/localization" default="/localization/kinematic_state"/>
+  <arg name="input/localization/ref" default="/geometry_msgs/PoseWithCovarianceStampedGt"/>
+
+  <node name="localization_evaluator" exec="localization_evaluator" pkg="localization_evaluator" output="screen">
+    <param from="$(find-pkg-share localization_evaluator)/param/localization_evaluator.defaults.yaml"/>
+    <remap from="~/input/localization" to="$(var input/localization)"/>
+    <remap from="~/input/localization/ref" to="$(var input/localization/ref)"/>
+  </node>
+</launch>
diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml
new file mode 100644
index 000000000000..7f99d100233b
--- /dev/null
+++ b/evaluator/localization_evaluator/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>localization_evaluator</name>
+  <version>0.1.0</version>
+  <description>localization evaluator ROS2 node</description>
+
+  <maintainer email="dominik.jargot@robotec.ai">Dominik Jargot</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>eigen</depend>
+  <depend>geometry_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_cmake_gtest</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>
diff --git a/evaluator/localization_evaluator/param/localization_evaluator.defaults.yaml b/evaluator/localization_evaluator/param/localization_evaluator.defaults.yaml
new file mode 100644
index 000000000000..aecfcf426556
--- /dev/null
+++ b/evaluator/localization_evaluator/param/localization_evaluator.defaults.yaml
@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    output_file: loc_metrics.results # if empty, metrics are not written to file
+
+    selected_metrics:
+      - lateral_error
+      - absolute_error
diff --git a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp b/evaluator/localization_evaluator/src/localization_evaluator_node.cpp
new file mode 100644
index 000000000000..db0c04a69872
--- /dev/null
+++ b/evaluator/localization_evaluator/src/localization_evaluator_node.cpp
@@ -0,0 +1,140 @@
+// Copyright 2021 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_evaluator/localization_evaluator_node.hpp"
+
+#include "boost/lexical_cast.hpp"
+
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+namespace localization_diagnostics
+{
+LocalizationEvaluatorNode::LocalizationEvaluatorNode(const rclcpp::NodeOptions & node_options)
+: Node("localization_evaluator", node_options),
+  odom_sub_(this, "~/input/localization", rclcpp::QoS{1}.get_rmw_qos_profile()),
+  pose_gt_sub_(this, "~/input/localization/ref", rclcpp::QoS{1}.get_rmw_qos_profile()),
+  sync_(SyncPolicy(100), odom_sub_, pose_gt_sub_)
+{
+  tf_buffer_ptr_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ptr_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_ptr_);
+
+  sync_.registerCallback(std::bind(
+    &LocalizationEvaluatorNode::syncCallback, this, std::placeholders::_1, std::placeholders::_2));
+
+  output_file_str_ = declare_parameter<std::string>("output_file");
+  if (output_file_str_.empty()) {
+    RCLCPP_INFO(
+      get_logger(),
+      "Output file not specified, the results will NOT be saved!"
+      "Provide output_file parameter to store the results.");
+  }
+  // List of metrics to calculate
+  metrics_pub_ = create_publisher<DiagnosticArray>("~/metrics", 1);
+  for (const std::string & selected_metric :
+       declare_parameter<std::vector<std::string>>("selected_metrics")) {
+    Metric metric = str_to_metric.at(selected_metric);
+    metrics_dict_[metric] = Stat<double>();
+    metrics_.push_back(metric);
+  }
+}
+
+LocalizationEvaluatorNode::~LocalizationEvaluatorNode()
+{
+  if (!output_file_str_.empty()) {
+    std::ofstream f(output_file_str_);
+    f << std::left << std::fixed;
+    // header
+    f << "#Data collected over: " << stamps_.back().seconds() - stamps_[0].seconds() << " seconds."
+      << std::endl;
+    f << std::setw(24) << "#Stamp [ns]";
+    for (Metric metric : metrics_) {
+      f << std::setw(30) << metric_descriptions.at(metric);
+    }
+    f << std::endl;
+    f << std::setw(24) << "#";
+    for (Metric metric : metrics_) {
+      (void)metric;
+      f << std::setw(9) << "min" << std::setw(9) << "max" << std::setw(12) << "mean";
+    }
+    f << std::endl;
+
+    // data
+    f << std::setw(24) << stamps_.back().nanoseconds();
+    for (Metric metric : metrics_) {
+      const auto & stat = metric_stats_[static_cast<size_t>(metric)].back();
+      f << stat;
+      f << std::setw(4) << "";
+    }
+    f.close();
+  }
+}
+
+DiagnosticStatus LocalizationEvaluatorNode::generateDiagnosticStatus(
+  const Metric & metric, const Stat<double> & metric_stat) const
+{
+  DiagnosticStatus status;
+  status.level = status.OK;
+  status.name = metric_to_str.at(metric);
+  diagnostic_msgs::msg::KeyValue key_value;
+  key_value.key = "min";
+  key_value.value = boost::lexical_cast<decltype(key_value.value)>(metric_stat.min());
+  status.values.push_back(key_value);
+  key_value.key = "max";
+  key_value.value = boost::lexical_cast<decltype(key_value.value)>(metric_stat.max());
+  status.values.push_back(key_value);
+  key_value.key = "mean";
+  key_value.value = boost::lexical_cast<decltype(key_value.value)>(metric_stat.mean());
+  status.values.push_back(key_value);
+  return status;
+}
+
+void LocalizationEvaluatorNode::syncCallback(
+  const Odometry::ConstSharedPtr & msg, const PoseWithCovarianceStamped::ConstSharedPtr & msg_ref)
+{
+  RCLCPP_DEBUG(
+    get_logger(), "Received two messages at time stamps: %d.%d and %d.%d", msg->header.stamp.sec,
+    msg->header.stamp.nanosec, msg_ref->header.stamp.sec, msg_ref->header.stamp.nanosec);
+
+  DiagnosticArray metrics_msg;
+  metrics_msg.header.stamp = now();
+
+  geometry_msgs::msg::Point p_lc, p_gt;
+  p_lc = msg->pose.pose.position;
+  p_gt = msg_ref->pose.pose.position;
+  if ((p_lc.x == 0 && p_lc.y == 0 && p_lc.z == 0) || (p_gt.x == 0 && p_gt.y == 0 && p_gt.z == 0)) {
+    RCLCPP_INFO(get_logger(), "Received position equals zero, waiting for valid data.");
+    return;
+  }
+  for (Metric metric : metrics_) {
+    metrics_dict_[metric] = metrics_calculator_.updateStat(
+      metrics_dict_[metric], metric, msg->pose.pose.position, msg_ref->pose.pose.position);
+    metric_stats_[static_cast<size_t>(metric)].push_back(metrics_dict_[metric]);
+    stamps_.push_back(metrics_msg.header.stamp);
+    if (metrics_dict_[metric].count() > 0) {
+      metrics_msg.status.push_back(generateDiagnosticStatus(metric, metrics_dict_[metric]));
+    }
+  }
+  if (!metrics_msg.status.empty()) {
+    metrics_pub_->publish(metrics_msg);
+  }
+}
+}  // namespace localization_diagnostics
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(localization_diagnostics::LocalizationEvaluatorNode)
diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp
new file mode 100644
index 000000000000..240a7dd91e9b
--- /dev/null
+++ b/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp
@@ -0,0 +1,46 @@
+// Copyright 2021 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_evaluator/metrics/localization_metrics.hpp"
+
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+namespace localization_diagnostics
+{
+namespace metrics
+{
+
+Stat<double> updateLateralStats(
+  const Stat<double> stat_prev, const double & lateral_pos, const double & lateral_ref)
+{
+  Stat<double> stat(stat_prev);
+  stat.add(std::abs(lateral_ref - lateral_pos));
+  return stat;
+}
+
+Stat<double> updateAbsoluteStats(
+  const Stat<double> stat_prev, const geometry_msgs::msg::Point & pos,
+  const geometry_msgs::msg::Point & pos_ref)
+{
+  Stat<double> stat(stat_prev);
+  double dist = std::sqrt(
+    (pos_ref.x - pos.x) * (pos_ref.x - pos.x) + (pos_ref.y - pos.y) * (pos_ref.y - pos.y) +
+    (pos_ref.z - pos.z) * (pos_ref.z - pos.z));
+  stat.add(dist);
+
+  return stat;
+}
+
+}  // namespace metrics
+}  // namespace localization_diagnostics
diff --git a/evaluator/localization_evaluator/src/metrics_calculator.cpp b/evaluator/localization_evaluator/src/metrics_calculator.cpp
new file mode 100644
index 000000000000..72184937e846
--- /dev/null
+++ b/evaluator/localization_evaluator/src/metrics_calculator.cpp
@@ -0,0 +1,42 @@
+// Copyright 2021 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_evaluator/metrics_calculator.hpp"
+
+#include "localization_evaluator/metrics/localization_metrics.hpp"
+
+namespace localization_diagnostics
+{
+Stat<double> MetricsCalculator::updateStat(
+  const Stat<double> stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos,
+  const geometry_msgs::msg::Point & pos_ref) const
+{
+  if (
+    (pos_ref.x == 0 && pos_ref.y == 0 && pos_ref.z == 0) ||
+    (pos.x == 0 && pos.y == 0 && pos.z == 0)) {
+    return stat_prev;
+  }
+  switch (metric) {
+    case Metric::lateral_error:
+      return metrics::updateLateralStats(stat_prev, pos.x, pos_ref.x);
+    case Metric::absolute_error:
+      return metrics::updateAbsoluteStats(stat_prev, pos, pos_ref);
+    default:
+      throw std::runtime_error(
+        "[MetricsCalculator][calculate()] unknown Metric " +
+        std::to_string(static_cast<int>(metric)));
+  }
+}
+
+}  // namespace localization_diagnostics
diff --git a/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp b/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp
new file mode 100644
index 000000000000..4fbc88294d26
--- /dev/null
+++ b/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp
@@ -0,0 +1,161 @@
+// Copyright 2021 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 "ament_index_cpp/get_package_share_directory.hpp"
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+
+#include <localization_evaluator/localization_evaluator_node.hpp>
+
+#include "diagnostic_msgs/msg/diagnostic_array.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include <nav_msgs/msg/odometry.hpp>
+
+#include "boost/lexical_cast.hpp"
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+using EvalNode = localization_diagnostics::LocalizationEvaluatorNode;
+using diagnostic_msgs::msg::DiagnosticArray;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using nav_msgs::msg::Odometry;
+
+class EvalTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+
+    rclcpp::NodeOptions options;
+    const auto share_dir = ament_index_cpp::get_package_share_directory("localization_evaluator");
+    options.arguments(
+      {"--ros-args", "--params-file", share_dir + "/param/localization_evaluator.defaults.yaml"});
+
+    dummy_node = std::make_shared<rclcpp::Node>("localization_evaluator_test_node");
+    eval_node = std::make_shared<EvalNode>(options);
+    // Enable all logging in the node
+    auto ret = rcutils_logging_set_logger_level(
+      dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+    if (ret != RCUTILS_RET_OK) {
+      std::cerr << "Failed to set logging severity to DEBUG\n";
+    }
+    ret = rcutils_logging_set_logger_level(
+      eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
+    if (ret != RCUTILS_RET_OK) {
+      std::cerr << "Failed to set logging severity to DEBUG\n";
+    }
+
+    odom_pub_ = rclcpp::create_publisher<Odometry>(
+      dummy_node, "/localization_evaluator/input/localization", 1);
+    pos_ref_pub_ = rclcpp::create_publisher<PoseWithCovarianceStamped>(
+      dummy_node, "/localization_evaluator/input/localization/ref", 1);
+
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(dummy_node);
+  }
+
+  ~EvalTest() override { rclcpp::shutdown(); }
+
+  void setTargetMetric(localization_diagnostics::Metric metric)
+  {
+    const auto metric_str = localization_diagnostics::metric_to_str.at(metric);
+    const auto is_target_metric = [metric_str](const auto & status) {
+      return status.name == metric_str;
+    };
+    metric_sub_ = rclcpp::create_subscription<DiagnosticArray>(
+      dummy_node, "/localization_evaluator/metrics", 1,
+      [=](const DiagnosticArray::ConstSharedPtr msg) {
+        const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric);
+        if (it != msg->status.end()) {
+          metric_value_ = boost::lexical_cast<double>(it->values[2].value);
+          metric_updated_ = true;
+        }
+      });
+  }
+
+  Odometry makeOdometry(const double x, const double y, const double z)
+  {
+    Odometry odometry;
+    odometry.header.frame_id = "map";
+    odometry.pose.pose.position.x = x;
+    odometry.pose.pose.position.y = y;
+    odometry.pose.pose.position.z = z;
+    return odometry;
+  }
+
+  PoseWithCovarianceStamped makePos(const double x, const double y, const double z)
+  {
+    PoseWithCovarianceStamped pos;
+    pos.header.frame_id = "map";
+    pos.pose.pose.position.x = x;
+    pos.pose.pose.position.y = y;
+    pos.pose.pose.position.z = z;
+    return pos;
+  }
+
+  double publishOdometryAndGetMetric(
+    const Odometry & odom, const PoseWithCovarianceStamped & pos_ref)
+  {
+    metric_updated_ = false;
+    odom_pub_->publish(odom);
+    pos_ref_pub_->publish(pos_ref);
+    while (!metric_updated_) {
+      rclcpp::spin_some(eval_node);
+      rclcpp::spin_some(dummy_node);
+      rclcpp::sleep_for(std::chrono::milliseconds(100));
+    }
+    return metric_value_;
+  }
+
+  // Latest metric value
+  bool metric_updated_ = false;
+  double metric_value_;
+  // Node
+  rclcpp::Node::SharedPtr dummy_node;
+  EvalNode::SharedPtr eval_node;
+  // Publishers
+  rclcpp::Publisher<Odometry>::SharedPtr odom_pub_;
+  rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pos_ref_pub_;
+  rclcpp::Subscription<DiagnosticArray>::SharedPtr metric_sub_;
+  // TF broadcaster
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+};
+
+TEST_F(EvalTest, TestLateralErrorStats)
+{
+  setTargetMetric(localization_diagnostics::Metric::lateral_error);
+  Odometry odom = makeOdometry(1.0, 1.0, 0.0);
+  PoseWithCovarianceStamped pos_ref = makePos(4.0, 5.0, 0.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom, pos_ref), 3.0);
+  Odometry odom2 = makeOdometry(1.0, 1.0, 0.0);
+  PoseWithCovarianceStamped pos2_ref = makePos(1.0, 1.0, 0.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom2, pos2_ref), 1.5);
+}
+
+TEST_F(EvalTest, TestAbsoluteErrorStats)
+{
+  setTargetMetric(localization_diagnostics::Metric::absolute_error);
+  Odometry odom = makeOdometry(1.0, 1.0, 0.0);
+  PoseWithCovarianceStamped pos_ref = makePos(4.0, 5.0, 0.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom, pos_ref), 5.0);
+  Odometry odom2 = makeOdometry(1.0, 1.0, 0.0);
+  PoseWithCovarianceStamped pos2_ref = makePos(1.0, 1.0, 0.0);
+  EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom2, pos2_ref), 2.5);
+}