diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 652d9b0597cd6..b8104c1bdce99 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -3,6 +3,7 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.j
 common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
 common/autoware_geography_utils/** koji.minoda@tier4.jp
 common/autoware_grid_map_utils/** maxime.clement@tier4.jp
+common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp
 common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
 common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai
 common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai
@@ -22,7 +23,6 @@ common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp
 common/glog_component/** takamasa.horibe@tier4.jp
 common/goal_distance_calculator/** taiki.tanaka@tier4.jp
 common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
-common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp
 common/object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp
 common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
 common/perception_utils/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
diff --git a/common/kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt
similarity index 54%
rename from common/kalman_filter/CMakeLists.txt
rename to common/autoware_kalman_filter/CMakeLists.txt
index 6cbdf67a0affa..076d2d3cad4e8 100644
--- a/common/kalman_filter/CMakeLists.txt
+++ b/common/autoware_kalman_filter/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(kalman_filter)
+project(autoware_kalman_filter)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -12,18 +12,18 @@ include_directories(
     ${EIGEN3_INCLUDE_DIR}
 )
 
-ament_auto_add_library(kalman_filter SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/kalman_filter.cpp
   src/time_delay_kalman_filter.cpp
-  include/kalman_filter/kalman_filter.hpp
-  include/kalman_filter/time_delay_kalman_filter.hpp
+  include/autoware/kalman_filter/kalman_filter.hpp
+  include/autoware/kalman_filter/time_delay_kalman_filter.hpp
 )
 
 if(BUILD_TESTING)
   file(GLOB_RECURSE test_files test/*.cpp)
-  ament_add_ros_isolated_gtest(test_kalman_filter ${test_files})
+  ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files})
 
-  target_link_libraries(test_kalman_filter kalman_filter)
+  target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
 endif()
 
 ament_auto_package()
diff --git a/common/kalman_filter/README.md b/common/autoware_kalman_filter/README.md
similarity index 100%
rename from common/kalman_filter/README.md
rename to common/autoware_kalman_filter/README.md
diff --git a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp
similarity index 96%
rename from common/kalman_filter/include/kalman_filter/kalman_filter.hpp
rename to common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp
index b500cffb92279..74db04f6e838b 100644
--- a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp
+++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp
@@ -12,12 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KALMAN_FILTER__KALMAN_FILTER_HPP_
-#define KALMAN_FILTER__KALMAN_FILTER_HPP_
+#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_
+#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_
 
 #include <Eigen/Core>
 #include <Eigen/LU>
 
+namespace autoware::kalman_filter
+{
+
 /**
  * @file kalman_filter.h
  * @brief kalman filter class
@@ -207,4 +210,5 @@ class KalmanFilter
   Eigen::MatrixXd R_;  //!< @brief covariance matrix for measurement model y[k] = C * x[k]
   Eigen::MatrixXd P_;  //!< @brief covariance of estimated state
 };
-#endif  // KALMAN_FILTER__KALMAN_FILTER_HPP_
+}  // namespace autoware::kalman_filter
+#endif  // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_
diff --git a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp
similarity index 89%
rename from common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp
rename to common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp
index cdc03f3558854..80375b7579e62 100644
--- a/common/kalman_filter/include/kalman_filter/time_delay_kalman_filter.hpp
+++ b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp
@@ -12,16 +12,18 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
-#define KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
 #include <Eigen/Core>
 #include <Eigen/LU>
 
 #include <iostream>
 
+namespace autoware::kalman_filter
+{
 /**
  * @file time_delay_kalman_filter.h
  * @brief kalman filter with delayed measurement class
@@ -83,4 +85,5 @@ class TimeDelayKalmanFilter : public KalmanFilter
   int dim_x_;           //!< @brief dimension of latest state
   int dim_x_ex_;        //!< @brief dimension of extended state with dime delay
 };
-#endif  // KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
+}  // namespace autoware::kalman_filter
+#endif  // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_
diff --git a/common/kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml
similarity index 96%
rename from common/kalman_filter/package.xml
rename to common/autoware_kalman_filter/package.xml
index 200440b5774c7..7d36bfc47aecf 100644
--- a/common/kalman_filter/package.xml
+++ b/common/autoware_kalman_filter/package.xml
@@ -1,7 +1,7 @@
 <?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>kalman_filter</name>
+  <name>autoware_kalman_filter</name>
   <version>0.1.0</version>
   <description>The kalman filter package</description>
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
diff --git a/common/kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp
similarity index 96%
rename from common/kalman_filter/src/kalman_filter.cpp
rename to common/autoware_kalman_filter/src/kalman_filter.cpp
index 450d40936db2e..bbd963675f9e2 100644
--- a/common/kalman_filter/src/kalman_filter.cpp
+++ b/common/autoware_kalman_filter/src/kalman_filter.cpp
@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
+namespace autoware::kalman_filter
+{
 KalmanFilter::KalmanFilter()
 {
 }
@@ -156,3 +158,4 @@ bool KalmanFilter::update(const Eigen::MatrixXd & y)
 {
   return update(y, C_, R_);
 }
+}  // namespace autoware::kalman_filter
diff --git a/common/kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp
similarity index 95%
rename from common/kalman_filter/src/time_delay_kalman_filter.cpp
rename to common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp
index c4143429e6c05..31c1c768d7173 100644
--- a/common/kalman_filter/src/time_delay_kalman_filter.cpp
+++ b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp
@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/time_delay_kalman_filter.hpp"
+#include "autoware/kalman_filter/time_delay_kalman_filter.hpp"
 
+namespace autoware::kalman_filter
+{
 TimeDelayKalmanFilter::TimeDelayKalmanFilter()
 {
 }
@@ -102,3 +104,4 @@ bool TimeDelayKalmanFilter::updateWithDelay(
 
   return true;
 }
+}  // namespace autoware::kalman_filter
diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp
similarity index 96%
rename from common/kalman_filter/test/test_kalman_filter.cpp
rename to common/autoware_kalman_filter/test/test_kalman_filter.cpp
index 4f4e9ce44669b..34e23ef9d06e2 100644
--- a/common/kalman_filter/test/test_kalman_filter.cpp
+++ b/common/autoware_kalman_filter/test/test_kalman_filter.cpp
@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
 #include <gtest/gtest.h>
 
+using autoware::kalman_filter::KalmanFilter;
+
 TEST(kalman_filter, kf)
 {
   KalmanFilter kf_;
diff --git a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp
similarity index 97%
rename from common/kalman_filter/test/test_time_delay_kalman_filter.cpp
rename to common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp
index 32fefd8ceff70..50c22fae123bc 100644
--- a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp
+++ b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp
@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "kalman_filter/time_delay_kalman_filter.hpp"
+#include "autoware/kalman_filter/time_delay_kalman_filter.hpp"
 
 #include <gtest/gtest.h>
 
+using autoware::kalman_filter::TimeDelayKalmanFilter;
+
 TEST(time_delay_kalman_filter, td_kf)
 {
   TimeDelayKalmanFilter td_kf_;
diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt
index c05dbdaaef644..18ef18303fb1b 100644
--- a/common/perception_utils/CMakeLists.txt
+++ b/common/perception_utils/CMakeLists.txt
@@ -4,8 +4,6 @@ project(perception_utils)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_package()
-
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/run_length_encoder.cpp
 )
@@ -14,3 +12,5 @@ find_package(OpenCV REQUIRED)
 target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBS}
 )
+
+ament_auto_package()
diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
index b26a2630ed994..bf61b69857849 100644
--- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
+++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp
@@ -533,62 +533,55 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
     return;
   }
 
-  Commands filtered_commands;
-
   // Set default commands
-  {
-    filtered_commands.control = commands.control;
-    filtered_commands.gear = commands.gear;  // tmp
-  }
+  Control filtered_control = commands.control;
 
   if (moderate_stop_interface_->is_stop_requested()) {  // if stop requested, stop the vehicle
-    filtered_commands.control.longitudinal.velocity = 0.0;
-    filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_;
+    filtered_control.longitudinal.velocity = 0.0;
+    filtered_control.longitudinal.acceleration = moderate_stop_service_acceleration_;
   }
 
   // Check emergency
   if (use_emergency_handling_ && is_system_emergency_) {
     RCLCPP_WARN_THROTTLE(
       get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "Emergency!");
-    filtered_commands.control = emergency_commands_.control;
-    filtered_commands.gear = emergency_commands_.gear;  // tmp
+    filtered_control = emergency_commands_.control;
   }
 
   // Check engage
   if (!is_engaged_) {
-    filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+    filtered_control.longitudinal = createLongitudinalStopControlCmd();
   }
 
   // Check pause. Place this check after all other checks as it needs the final output.
-  adapi_pause_->update(filtered_commands.control);
+  adapi_pause_->update(filtered_control);
   if (adapi_pause_->is_paused()) {
     if (is_engaged_) {
-      filtered_commands.control.longitudinal = createLongitudinalStopControlCmd();
+      filtered_control.longitudinal = createLongitudinalStopControlCmd();
     } else {
-      filtered_commands.control = createStopControlCmd();
+      filtered_control = createStopControlCmd();
     }
   }
 
   // Check if command filtering option is enable
   if (enable_cmd_limit_filter_) {
     // Apply limit filtering
-    filtered_commands.control = filterControlCommand(filtered_commands.control);
+    filtered_control = filterControlCommand(filtered_control);
   }
   // tmp: Publish vehicle emergency status
   VehicleEmergencyStamped vehicle_cmd_emergency;
   vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_);
-  vehicle_cmd_emergency.stamp = filtered_commands.control.stamp;
+  vehicle_cmd_emergency.stamp = filtered_control.stamp;
 
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
-  control_cmd_pub_->publish(filtered_commands.control);
-  published_time_publisher_->publish_if_subscribed(
-    control_cmd_pub_, filtered_commands.control.stamp);
+  control_cmd_pub_->publish(filtered_control);
+  published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp);
   adapi_pause_->publish();
   moderate_stop_interface_->publish();
 
   // Save ControlCmd to steering angle when disengaged
-  prev_control_cmd_ = filtered_commands.control;
+  prev_control_cmd_ = filtered_control;
 }
 
 void VehicleCmdGate::publishEmergencyStopControlCommands()
@@ -903,7 +896,6 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a
   }
   if (filter_activated.is_activated_on_steering_rate) {
     reason += first_msg ? " steer_rate" : ", steer_rate";
-    first_msg = false;
   }
 
   msg.markers.emplace_back(createStringMarker(
diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 5f6632e76cdfa..5dec24c66f6ca 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -5,13 +5,18 @@
   <arg name="laserscan_based_occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
+
+  <arg name="localization_error_monitor_param_path"/>
+  <arg name="ekf_localizer_param_path"/>
   <arg name="pose_initializer_param_path"/>
+  <arg name="twist2accel_param_path"/>
+
   <arg name="launch_simulator_perception_modules" default="true"/>
   <arg name="launch_dummy_perception"/>
   <arg name="launch_dummy_vehicle"/>
   <arg
     name="localization_sim_mode"
-    description="Select localization mode. Options are 'none' or 'api'. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
+    description="Select localization mode. Options are 'none', 'api' or 'pose_twist_estimator'. 'pose_twist_estimator' starts most of the localization modules except for the ndt_scan_matcher. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
   />
   <arg name="launch_dummy_doors"/>
   <arg name="launch_diagnostic_converter"/>
@@ -130,7 +135,7 @@
     </group>
   </group>
 
-  <!-- Dummy localization -->
+  <!-- localization -->
   <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;none&quot;')">
     <!-- Do nothing -->
   </group>
@@ -148,6 +153,50 @@
     </include>
   </group>
 
+  <group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')">
+    <group>
+      <!-- start name space localization -->
+      <push-ros-namespace namespace="localization"/>
+      <group>
+        <push-ros-namespace namespace="util"/>
+        <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+          <arg name="user_defined_initial_pose/enable" value="false"/>
+          <arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
+          <arg name="ndt_enabled" value="false"/>
+          <arg name="gnss_enabled" value="false"/>
+          <arg name="ekf_enabled" value="true"/>
+          <arg name="yabloc_enabled" value="false"/>
+          <arg name="stop_check_enabled" value="false"/>
+          <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+        </include>
+      </group>
+
+      <group>
+        <push-ros-namespace namespace="twist_estimator"/>
+        <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
+      </group>
+
+      <!-- pose_twist_fusion_filter module -->
+      <group>
+        <push-ros-namespace namespace="pose_twist_fusion_filter"/>
+        <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
+      </group>
+    </group>
+    <!-- end name space localization -->
+    <group>
+      <push-ros-namespace namespace="sensing"/>
+      <arg name="input_vehicle_velocity_topic" default="/vehicle/status/velocity_status"/>
+      <arg name="output_twist_with_covariance" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
+      <arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>
+
+      <node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter_node" output="both">
+        <param from="$(var config_file)"/>
+        <remap from="velocity_status" to="$(var input_vehicle_velocity_topic)"/>
+        <remap from="twist_with_covariance" to="$(var output_twist_with_covariance)"/>
+      </node>
+    </group>
+  </group>
+
   <!-- Dummy doors -->
   <group if="$(var launch_dummy_doors)">
     <include file="$(find-pkg-share vehicle_door_simulator)/launch/vehicle_door_simulator.launch.xml"/>
@@ -163,11 +212,15 @@
   <!-- Simulator model -->
   <group if="$(var launch_dummy_vehicle)">
     <arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
+    <!-- 'pose_only' mode publishes the pose topic as an alternative to ndt_localizer. 'full_motion' mode publishes the odometry and acceleration topics as an alternative to localization modules. -->
+    <let name="motion_publish_mode" value="pose_only" if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
+    <let name="motion_publish_mode" value="full_motion" unless="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
     <include file="$(find-pkg-share simple_planning_simulator)/launch/simple_planning_simulator.launch.py">
       <arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
       <arg name="simulator_model_param_file" value="$(var simulator_model)"/>
       <arg name="initial_engage_state" value="$(var initial_engage_state)"/>
       <arg name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
+      <arg name="motion_publish_mode" value="$(var motion_publish_mode)"/>
     </include>
   </group>
 </launch>
diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp
index 5eda1156327b5..a127d2ae81433 100644
--- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp
+++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp
@@ -19,8 +19,8 @@
 #include "ekf_localizer/state_index.hpp"
 #include "ekf_localizer/warning.hpp"
 
-#include <kalman_filter/kalman_filter.hpp>
-#include <kalman_filter/time_delay_kalman_filter.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>
@@ -34,6 +34,8 @@
 #include <memory>
 #include <vector>
 
+using autoware::kalman_filter::TimeDelayKalmanFilter;
+
 struct EKFDiagnosticInfo
 {
   size_t no_update_count{0};
diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml
index b53538de11f15..34881ddd3c68c 100644
--- a/localization/ekf_localizer/package.xml
+++ b/localization/ekf_localizer/package.xml
@@ -22,11 +22,11 @@
 
   <build_depend>eigen</build_depend>
 
+  <depend>autoware_kalman_filter</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>fmt</depend>
   <depend>geometry_msgs</depend>
-  <depend>kalman_filter</depend>
   <depend>localization_util</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
diff --git a/perception/autoware_bytetrack/lib/include/strack.h b/perception/autoware_bytetrack/lib/include/strack.h
index 260f38ea93770..b66005eadce0f 100644
--- a/perception/autoware_bytetrack/lib/include/strack.h
+++ b/perception/autoware_bytetrack/lib/include/strack.h
@@ -38,8 +38,7 @@
 
 #pragma once
 
-// #include "kalman_filter.h"
-#include <kalman_filter/kalman_filter.hpp>
+#include <autoware/kalman_filter/kalman_filter.hpp>
 #include <opencv2/opencv.hpp>
 
 #include <boost/uuid/uuid.hpp>
@@ -49,6 +48,8 @@
 
 enum TrackState { New = 0, Tracked, Lost, Removed };
 
+using autoware::kalman_filter::KalmanFilter;
+
 /** manage one tracklet*/
 class STrack
 {
diff --git a/perception/autoware_bytetrack/package.xml b/perception/autoware_bytetrack/package.xml
index ef2d96e7930d4..581d908b61a51 100644
--- a/perception/autoware_bytetrack/package.xml
+++ b/perception/autoware_bytetrack/package.xml
@@ -15,12 +15,12 @@
   <buildtool_export_depend>cudnn_cmake_module</buildtool_export_depend>
   <buildtool_export_depend>tensorrt_cmake_module</buildtool_export_depend>
 
+  <depend>autoware_kalman_filter</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>cuda_utils</depend>
   <depend>cv_bridge</depend>
   <depend>eigen</depend>
   <depend>image_transport</depend>
-  <depend>kalman_filter</depend>
   <depend>libboost-system-dev</depend>
   <depend>libopencv-dev</depend>
   <depend>rclcpp</depend>
diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
index cfbb2fa4a18cb..9a0f4838d1fac 100644
--- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
+++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp
@@ -41,6 +41,7 @@
 
 #include "autoware_perception_msgs/msg/shape.hpp"
 #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
+#include <sensor_msgs/msg/camera_info.hpp>
 
 #include <image_geometry/pinhole_camera_model.h>
 #include <pcl/point_cloud.h>
@@ -62,6 +63,8 @@ struct PointData
   size_t orig_index;
 };
 
+bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info);
+
 Eigen::Vector2d calcRawImageProjectedPoint(
   const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);
 
diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
index be14d9a0b0fb3..3d9b51ae7add7 100644
--- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -271,6 +271,8 @@ void PointPaintingFusionNode::fuseOnSingleImage(
     return;
   }
 
+  if (!checkCameraInfo(camera_info)) return;
+
   std::vector<sensor_msgs::msg::RegionOfInterest> debug_image_rois;
   std::vector<Eigen::Vector2d> debug_image_points;
 
diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
index d4d5de3cfd381..060b7f292c403 100644
--- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -83,6 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
+  if (!checkCameraInfo(camera_info)) return;
+
   image_geometry::PinholeCameraModel pinhole_camera_model;
   pinhole_camera_model.fromCameraInfo(camera_info);
 
diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
index e2197c148bcf9..39835b465d8af 100644
--- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp
@@ -75,6 +75,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
   const sensor_msgs::msg::CameraInfo & camera_info,
   DetectedObjects & output_object_msg __attribute__((unused)))
 {
+  if (!checkCameraInfo(camera_info)) return;
+
   Eigen::Affine3d object2camera_affine;
   {
     const auto transform_stamped_optional = getTransformStamped(
diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
index 40406d6e420b7..e10c7e94f4dae 100644
--- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp
@@ -82,6 +82,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
+  if (!checkCameraInfo(camera_info)) return;
+
   std::vector<DetectedObjectWithFeature> output_objs;
   // select ROIs for fusion
   for (const auto & feature_obj : input_roi_msg.feature_objects) {
diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
index 5c93fa6c2fd1b..4b4d940a05ab3 100644
--- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp
@@ -57,6 +57,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (input_pointcloud_msg.data.empty()) {
     return;
   }
+  if (!checkCameraInfo(camera_info)) return;
+
   cv_bridge::CvImagePtr in_image_ptr;
   try {
     in_image_ptr = cv_bridge::toCvCopy(
diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
index ac17c419efca4..1b303e3f01ff5 100644
--- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp
@@ -14,8 +14,32 @@
 
 #include "autoware/image_projection_based_fusion/utils/utils.hpp"
 
+#include <sensor_msgs/distortion_models.hpp>
+
 namespace autoware::image_projection_based_fusion
 {
+bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info)
+{
+  const bool is_supported_model =
+    (camera_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
+     camera_info.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL);
+  if (!is_supported_model) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("image_projection_based_fusion"),
+      "checkCameraInfo: Unsupported distortion model: " << camera_info.distortion_model);
+    return false;
+  }
+  const bool is_supported_distortion_param =
+    (camera_info.d.size() == 5 || camera_info.d.size() == 8);
+  if (!is_supported_distortion_param) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("image_projection_based_fusion"),
+      "checkCameraInfo: Unsupported distortion coefficients size: " << camera_info.d.size());
+    return false;
+  }
+  return true;
+}
+
 Eigen::Vector2d calcRawImageProjectedPoint(
   const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
 {
diff --git a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml
index b57f37675d4f1..6f9bfcf6210dd 100644
--- a/perception/autoware_multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/autoware_multi_object_tracker/config/input_channels.param.yaml
@@ -40,8 +40,8 @@
           name: "apollo"
           short_name: "Lap"
       # LIDAR-CAMERA - DNN
-      # cspell:ignore lidar_pointpainitng pointpainting
-      lidar_pointpainitng:
+      # cspell:ignore lidar_pointpainting pointpainting
+      lidar_pointpainting:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
         can_spawn_new_tracker: true
         optional:
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp
index 5374e28d5f9cf..7cb2963d38ef1 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
index c3f824aff35b4..227e6cd01f4dc 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
index a0a6bd7781761..868aa1606d4ff 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <rclcpp/time.hpp>
 
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
index f7edebfc31378..8f5bab65c6aed 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp"
 #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp
index e7f0a5fd699e1..45cd0f31a4e85 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp
@@ -19,7 +19,7 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "tracker_base.hpp"
 
 namespace autoware::multi_object_tracker
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
index f593280c2e183..4287e0f99d5ee 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
index 1e0f778a69137..500148ba41081 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
@@ -19,10 +19,10 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp"
 #include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp
index 4bc03f439ffc2..9f128c864ad6c 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp
@@ -19,9 +19,9 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 namespace autoware::multi_object_tracker
 {
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp
index 6e20d31aad168..123eb30e63d6c 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp
@@ -19,8 +19,8 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp
index 3cca786d9f65b..2632d99047053 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp
@@ -19,8 +19,8 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp
index ad3061285a80c..26799f1916741 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp
@@ -19,8 +19,8 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp
index 130053fafd2ed..dff60a82c4c44 100644
--- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp
+++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp
@@ -19,7 +19,7 @@
 #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_
 #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_
 
-#include "kalman_filter/kalman_filter.hpp"
+#include "autoware/kalman_filter/kalman_filter.hpp"
 
 #include <Eigen/Core>
 #include <rclcpp/rclcpp.hpp>
@@ -33,6 +33,7 @@
 
 namespace autoware::multi_object_tracker
 {
+using autoware::kalman_filter::KalmanFilter;
 
 class MotionModel
 {
diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml
index 3d941239976ea..405ce7eced625 100644
--- a/perception/autoware_multi_object_tracker/package.xml
+++ b/perception/autoware_multi_object_tracker/package.xml
@@ -13,12 +13,12 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_kalman_filter</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_updater</depend>
   <depend>eigen</depend>
   <depend>glog</depend>
-  <depend>kalman_filter</depend>
   <depend>mussp</depend>
   <depend>object_recognition_utils</depend>
   <depend>rclcpp</depend>
diff --git a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json
index 3d7bba9daae87..6a35ca4f0e3bc 100644
--- a/perception/autoware_multi_object_tracker/schema/input_channels.schema.json
+++ b/perception/autoware_multi_object_tracker/schema/input_channels.schema.json
@@ -111,7 +111,7 @@
                     }
                   }
                 },
-                "lidar_pointpainitng": {
+                "lidar_pointpainting": {
                   "$ref": "#/definitions/input_channel",
                   "default": {
                     "topic": "/perception/object_recognition/detection/pointpainting/objects",
@@ -185,7 +185,7 @@
                 "lidar_centerpoint_validated",
                 "lidar_apollo",
                 "lidar_apollo_validated",
-                "lidar_pointpainitng",
+                "lidar_pointpainting",
                 "lidar_pointpainting_validated",
                 "camera_lidar_fusion",
                 "detection_by_tracker",
diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
index dab4d3f8efa24..6b33552c65891 100644
--- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
+++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp
@@ -15,13 +15,14 @@
 #ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
 #define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <string>
 namespace autoware::radar_object_tracker
 {
 using Label = autoware_perception_msgs::msg::ObjectClassification;
+using autoware::kalman_filter::KalmanFilter;
 class ConstantTurnRateMotionTracker : public Tracker  // means constant turn rate motion tracker
 {
 private:
diff --git a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp
index 7c96aa8fbaa36..b73e064635974 100644
--- a/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp
+++ b/perception/autoware_radar_object_tracker/include/autoware_radar_object_tracker/tracker/model/linear_motion_tracker.hpp
@@ -15,8 +15,8 @@
 #ifndef AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
 #define AUTOWARE_RADAR_OBJECT_TRACKER__TRACKER__MODEL__LINEAR_MOTION_TRACKER_HPP_
 
+#include "autoware/kalman_filter/kalman_filter.hpp"
 #include "autoware_radar_object_tracker/tracker/model/tracker_base.hpp"
-#include "kalman_filter/kalman_filter.hpp"
 
 #include <string>
 
@@ -24,6 +24,8 @@ namespace autoware::radar_object_tracker
 {
 
 using Label = autoware_perception_msgs::msg::ObjectClassification;
+using autoware::kalman_filter::KalmanFilter;
+
 class LinearMotionTracker : public Tracker
 {
 private:
diff --git a/perception/autoware_radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml
index 50dd70e6cec67..6829c26f797c7 100644
--- a/perception/autoware_radar_object_tracker/package.xml
+++ b/perception/autoware_radar_object_tracker/package.xml
@@ -13,12 +13,12 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_kalman_filter</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_universe_utils</depend>
   <depend>eigen</depend>
   <depend>glog</depend>
-  <depend>kalman_filter</depend>
   <depend>mussp</depend>
   <depend>nlohmann-json-dev</depend>
   <depend>object_recognition_utils</depend>
diff --git a/perception/autoware_tensorrt_yolox/README.md b/perception/autoware_tensorrt_yolox/README.md
index b88693f18d00f..cc7113e7dfcee 100644
--- a/perception/autoware_tensorrt_yolox/README.md
+++ b/perception/autoware_tensorrt_yolox/README.md
@@ -31,8 +31,8 @@ Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Se
 
 ## Parameters
 
-{{ json_to_markdown("perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json") }}
-{{ json_to_markdown("perception/tensorrt_yolox/schema/yolox_tiny.schema.json") }}
+{{ json_to_markdown("perception/autoware_tensorrt_yolox/schema/yolox_s_plus_opt.schema.json") }}
+{{ json_to_markdown("perception/autoware_tensorrt_yolox/schema/yolox_tiny.schema.json") }}
 
 ## Assumptions / Known limits
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
index 9c501b7b2ae82..9137e71accf87 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
@@ -207,7 +207,7 @@ void GoalSearcher::countObjectsToAvoid(
   const double backward_length = parameters_.backward_goal_search_length;
 
   // calculate search start/end pose in pull over lanes
-  const auto [search_start_pose, search_end_pose] = std::invoke([&]() -> std::pair<Pose, Pose> {
+  const auto search_start_end_poses = std::invoke([&]() -> std::pair<Pose, Pose> {
     const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
       *route_handler, left_side_parking_, parameters_.backward_goal_search_length,
       parameters_.forward_goal_search_length);
@@ -221,6 +221,8 @@ void GoalSearcher::countObjectsToAvoid(
     return std::make_pair(
       center_line_path.points.front().point.pose, center_line_path.points.back().point.pose);
   });
+  const auto search_start_pose = std::get<0>(search_start_end_poses);
+  const auto search_end_pose = std::get<1>(search_start_end_poses);
 
   // generate current lane center line path to check collision with objects
   const auto current_lanes = utils::getExtendedCurrentLanes(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
index 928506fa448c3..45cbb122d1e93 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
@@ -92,8 +92,9 @@ double calcMinimumLaneChangeLength(
   }
 
   const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity;
-  const auto [min_lat_acc, max_lat_acc] =
-    lane_change_parameters.lane_change_lat_acc_map.find(min_vel);
+  const auto min_max_lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(min_vel);
+  // const auto min_lat_acc = std::get<0>(min_max_lat_acc);
+  const auto max_lat_acc = std::get<1>(min_max_lat_acc);
   const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk;
   const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer;
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
index 68d81f4d257c1..1e8532eba2f3a 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md
@@ -135,23 +135,23 @@ Scene modules receives necessary data and RTC command, and outputs candidate pat
   <figcaption>scene module</figcaption>
 </figure>
 
-| I/O | Type                                          | Description                                                                                                                                                                     |
-| :-- | :-------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| IN  | `behavior_path_planner::BehaviorModuleOutput` | previous module output. contains data necessary for path planning.                                                                                                              |
-| IN  | `behavior_path_planner::PlannerData`          | contains data necessary for path planning.                                                                                                                                      |
-| IN  | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md))    |
-| OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc...                                                                                                                         |
-| OUT | `tier4_planning_msgs::msg::CooperateStatus`   | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md))                                  |
-| OUT | `autoware_planning_msgs::msg::Path`           | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) |
-| OUT | `autoware_planning_msgs::msg::Path`           | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization)                                                                   |
-| OUT | `visualization_msgs::msg::MarkerArray`        | virtual wall, debug info, etc...                                                                                                                                                |
+| I/O | Type                                          | Description                                                                                                                                                                           |
+| :-- | :-------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
+| IN  | `behavior_path_planner::BehaviorModuleOutput` | previous module output. contains data necessary for path planning.                                                                                                                    |
+| IN  | `behavior_path_planner::PlannerData`          | contains data necessary for path planning.                                                                                                                                            |
+| IN  | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md)) |
+| OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc...                                                                                                                               |
+| OUT | `tier4_planning_msgs::msg::CooperateStatus`   | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md))                               |
+| OUT | `autoware_planning_msgs::msg::Path`           | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization)       |
+| OUT | `autoware_planning_msgs::msg::Path`           | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization)                                                                         |
+| OUT | `visualization_msgs::msg::MarkerArray`        | virtual wall, debug info, etc...                                                                                                                                                      |
 
 Scene modules running on the manager are stored on the **candidate modules stack** or **approved modules stack** depending on the condition whether the path modification has been approved or not.
 
-| Stack             | Approval condition | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         |
-| :---------------- | :----------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| candidate modules | Not approved       | The candidate modules whose modified path has not been approved by [RTC](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md) is stored in vector `candidate_module_ptrs_` in the manager. The candidate modules stack is updated in the following order. 1. The manager selects only those modules that can be executed based on the configuration of the sub-manager whose scene module requests execution. 2. Determines the execution priority. 3. Executes them as candidate module. All of these modules receive the decided (approved) path from approved modules stack and **RUN in PARALLEL**. <br>![candidate_modules_stack](../image/manager/candidate_modules_stack.svg) |
-| approved modules  | Already approved   | When the path modification is approved via RTC commands, the manager moves the candidate module to approved modules stack. These modules are stored in `approved_module_ptrs_`. In this stack, all scene modules **RUN in SERIES**. <br>![approved_modules_stack](../image/manager/approved_modules_stack.svg)                                                                                                                                                                                                                                                                                                                                                                                                                      |
+| Stack             | Approval condition | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  |
+| :---------------- | :----------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| candidate modules | Not approved       | The candidate modules whose modified path has not been approved by [RTC](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_rtc_interface/README.md) is stored in vector `candidate_module_ptrs_` in the manager. The candidate modules stack is updated in the following order. 1. The manager selects only those modules that can be executed based on the configuration of the sub-manager whose scene module requests execution. 2. Determines the execution priority. 3. Executes them as candidate module. All of these modules receive the decided (approved) path from approved modules stack and **RUN in PARALLEL**. <br>![candidate_modules_stack](../image/manager/candidate_modules_stack.svg) |
+| approved modules  | Already approved   | When the path modification is approved via RTC commands, the manager moves the candidate module to approved modules stack. These modules are stored in `approved_module_ptrs_`. In this stack, all scene modules **RUN in SERIES**. <br>![approved_modules_stack](../image/manager/approved_modules_stack.svg)                                                                                                                                                                                                                                                                                                                                                                                                                               |
 
 ## Process flow
 
@@ -346,7 +346,7 @@ On this manager, it is possible that multiple scene modules may request path mod
 
 Push back the modules that make a request to `request_modules`.
 
-![request_step1](../image/manager/request_step1.svg)
+![request_step1](../image/manager/request_step1.drawio.svg)
 
 ### Step2
 
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index 0ed603960a6c5..547689dc847a0 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -137,7 +137,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
   rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
   rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
   rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
-  rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
+  rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_current_pose_;
 
   rclcpp::Subscription<GearCommand>::SharedPtr sub_gear_cmd_;
   rclcpp::Subscription<GearCommand>::SharedPtr sub_manual_gear_cmd_;
@@ -346,6 +346,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
    */
   void publish_odometry(const Odometry & odometry);
 
+  /**
+   * @brief publish pose
+   * @param [in] odometry The odometry to publish its pose
+   */
+  void publish_pose(const Odometry & odometry);
+
   /**
    * @brief publish steering
    * @param [in] steer The steering to publish
diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
index afdbeb120e2d3..e6a35bd420bf3 100644
--- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
+++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -41,6 +41,50 @@ def launch_setup(context, *args, **kwargs):
         param_file=simulator_model_param_path, allow_substs=True
     )
 
+    # Base remappings
+    remappings = [
+        ("input/vector_map", "/map/vector_map"),
+        ("input/initialpose", "/initialpose3d"),
+        ("input/ackermann_control_command", "/control/command/control_cmd"),
+        ("input/actuation_command", "/control/command/actuation_cmd"),
+        ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
+        ("input/gear_command", "/control/command/gear_cmd"),
+        ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
+        ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
+        ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
+        ("input/trajectory", "/planning/scenario_planning/trajectory"),
+        ("input/engage", "/vehicle/engage"),
+        ("input/control_mode_request", "/control/control_mode_request"),
+        ("output/twist", "/vehicle/status/velocity_status"),
+        ("output/imu", "/sensing/imu/imu_data"),
+        ("output/steering", "/vehicle/status/steering_status"),
+        ("output/gear_report", "/vehicle/status/gear_status"),
+        ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
+        ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
+        ("output/control_mode_report", "/vehicle/status/control_mode"),
+    ]
+
+    # Additional remappings
+    if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only":
+        remappings.extend(
+            [
+                ("output/odometry", "/simulation/debug/localization/kinematic_state"),
+                ("output/acceleration", "/simulation/debug/localization/acceleration"),
+                ("output/pose", "/localization/pose_estimator/pose_with_covariance"),
+            ]
+        )
+    elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion":
+        remappings.extend(
+            [
+                ("output/odometry", "/localization/kinematic_state"),
+                ("output/acceleration", "/localization/acceleration"),
+                (
+                    "output/pose",
+                    "/simulation/debug/localization/pose_estimator/pose_with_covariance",
+                ),
+            ]
+        )
+
     simple_planning_simulator_node = Node(
         package="simple_planning_simulator",
         executable="simple_planning_simulator_exe",
@@ -55,29 +99,7 @@ def launch_setup(context, *args, **kwargs):
                 "initial_engage_state": LaunchConfiguration("initial_engage_state"),
             },
         ],
-        remappings=[
-            ("input/vector_map", "/map/vector_map"),
-            ("input/initialpose", "/initialpose3d"),
-            ("input/ackermann_control_command", "/control/command/control_cmd"),
-            ("input/actuation_command", "/control/command/actuation_cmd"),
-            ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
-            ("input/gear_command", "/control/command/gear_cmd"),
-            ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
-            ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
-            ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
-            ("input/trajectory", "/planning/scenario_planning/trajectory"),
-            ("input/engage", "/vehicle/engage"),
-            ("input/control_mode_request", "/control/control_mode_request"),
-            ("output/twist", "/vehicle/status/velocity_status"),
-            ("output/odometry", "/localization/kinematic_state"),
-            ("output/acceleration", "/localization/acceleration"),
-            ("output/imu", "/sensing/imu/imu_data"),
-            ("output/steering", "/vehicle/status/steering_status"),
-            ("output/gear_report", "/vehicle/status/gear_status"),
-            ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
-            ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
-            ("output/control_mode_report", "/vehicle/status/control_mode"),
-        ],
+        remappings=remappings,
     )
 
     # Determine if we should launch raw_vehicle_cmd_converter based on the vehicle_model_type
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index 3095e060a71bf..4252ace6c1920 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -165,7 +165,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     create_publisher<TurnIndicatorsReport>("output/turn_indicators_report", QoS{1});
   pub_hazard_lights_report_ =
     create_publisher<HazardLightsReport>("output/hazard_lights_report", QoS{1});
-  pub_current_pose_ = create_publisher<PoseStamped>("output/debug/pose", QoS{1});
+  pub_current_pose_ = create_publisher<PoseWithCovarianceStamped>("output/pose", QoS{1});
   pub_velocity_ = create_publisher<VelocityReport>("output/twist", QoS{1});
   pub_odom_ = create_publisher<Odometry>("output/odometry", QoS{1});
   pub_steer_ = create_publisher<SteeringReport>("output/steering", QoS{1});
@@ -444,6 +444,7 @@ void SimplePlanningSimulator::on_timer()
 
   // publish vehicle state
   publish_odometry(current_odometry_);
+  publish_pose(current_odometry_);
   publish_velocity(current_velocity_);
   publish_steering(current_steer_);
   publish_acceleration();
@@ -749,6 +750,26 @@ void SimplePlanningSimulator::publish_odometry(const Odometry & odometry)
   pub_odom_->publish(msg);
 }
 
+void SimplePlanningSimulator::publish_pose(const Odometry & odometry)
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped msg;
+
+  msg.pose = odometry.pose;
+  using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  constexpr auto COV_POS = 0.0225;      // same value as current ndt output
+  constexpr auto COV_ANGLE = 0.000625;  // same value as current ndt output
+  msg.pose.covariance.at(COV_IDX::X_X) = COV_POS;
+  msg.pose.covariance.at(COV_IDX::Y_Y) = COV_POS;
+  msg.pose.covariance.at(COV_IDX::Z_Z) = COV_POS;
+  msg.pose.covariance.at(COV_IDX::ROLL_ROLL) = COV_ANGLE;
+  msg.pose.covariance.at(COV_IDX::PITCH_PITCH) = COV_ANGLE;
+  msg.pose.covariance.at(COV_IDX::YAW_YAW) = COV_ANGLE;
+
+  msg.header.frame_id = origin_frame_id_;
+  msg.header.stamp = get_clock()->now();
+  pub_current_pose_->publish(msg);
+}
+
 void SimplePlanningSimulator::publish_steering(const SteeringReport & steer)
 {
   SteeringReport msg = steer;