diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
index 5d773c5be32d9..61dc65c5ea3f2 100644
--- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
+++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -1251,7 +1251,7 @@ calcLongitudinalOffsetPose<std::vector<autoware_auto_planning_msgs::msg::Traject
  * @param offset length of offset from source point
  * @param set_orientation_from_position_direction set orientation by spherical interpolation if
  * false
- * @return offset pase
+ * @return offset pose
  */
 template <class T>
 boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
index c108bec2671b3..cc43da7114630 100644
--- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
+++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp
@@ -20,6 +20,7 @@
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 #include "trajectory_follower_base/lateral_controller_base.hpp"
 #include "trajectory_follower_base/longitudinal_controller_base.hpp"
 #include "trajectory_follower_node/visibility_control.hpp"
@@ -38,6 +39,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
 #include <memory>
 #include <string>
@@ -52,6 +54,8 @@ namespace trajectory_follower_node
 {
 
 using autoware_adapi_v1_msgs::msg::OperationModeState;
+using tier4_autoware_utils::StopWatch;
+using tier4_debug_msgs::msg::Float64Stamped;
 
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
@@ -78,6 +82,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
+  rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_lat_ms_;
+  rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_lon_ms_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
 
   autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_;
@@ -114,6 +120,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
     const trajectory_follower::LateralOutput & lat_out) const;
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
+
+  void publishProcessingTime(
+    const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub);
+  StopWatch<std::chrono::milliseconds> stop_watch_;
 };
 }  // namespace trajectory_follower_node
 }  // namespace autoware::motion::control
diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp
index e35848e9495af..322aa9eef5a79 100644
--- a/control/trajectory_follower_node/src/controller_node.cpp
+++ b/control/trajectory_follower_node/src/controller_node.cpp
@@ -75,6 +75,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; });
   control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
+  pub_processing_time_lat_ms_ =
+    create_publisher<Float64Stamped>("~/lateral/debug/processing_time_ms", 1);
+  pub_processing_time_lon_ms_ =
+    create_publisher<Float64Stamped>("~/longitudinal/debug/processing_time_ms", 1);
   debug_marker_pub_ =
     create_publisher<visualization_msgs::msg::MarkerArray>("~/output/debug_marker", rclcpp::QoS{1});
 
@@ -205,8 +209,13 @@ void Controller::callbackTimerControl()
   }
 
   // 3. run controllers
+  stop_watch_.tic("lateral");
   const auto lat_out = lateral_controller_->run(*input_data);
+  publishProcessingTime(stop_watch_.toc("lateral"), pub_processing_time_lat_ms_);
+
+  stop_watch_.tic("longitudinal");
   const auto lon_out = longitudinal_controller_->run(*input_data);
+  publishProcessingTime(stop_watch_.toc("longitudinal"), pub_processing_time_lon_ms_);
 
   // 4. sync with each other controllers
   longitudinal_controller_->sync(lat_out.sync_data);
@@ -254,6 +263,15 @@ void Controller::publishDebugMarker(
   debug_marker_pub_->publish(debug_marker_array);
 }
 
+void Controller::publishProcessingTime(
+  const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub)
+{
+  Float64Stamped msg{};
+  msg.stamp = this->now();
+  msg.data = t_ms;
+  pub->publish(msg);
+}
+
 }  // namespace autoware::motion::control::trajectory_follower_node
 
 #include "rclcpp_components/register_node_macro.hpp"
diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
index c0d1b00e35d3b..a22e7b1acf6dc 100644
--- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml
@@ -19,6 +19,7 @@
   <depend>image_transport</depend>
   <depend>landmark_parser</depend>
   <depend>lanelet2_extension</depend>
+  <depend>localization_util</depend>
   <depend>rclcpp</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
index 73e7b8b7e3587..bc40fb1532297 100644
--- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp
@@ -44,6 +44,8 @@
 
 #include "ar_tag_based_localizer.hpp"
 
+#include "localization_util/util_func.hpp"
+
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <opencv4/opencv2/calib3d.hpp>
@@ -111,18 +113,19 @@ bool ArTagBasedLocalizer::setup()
   /*
     Subscribers
   */
-  map_bin_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
+  map_bin_sub_ = this->create_subscription<HADMapBin>(
     "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal),
     std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1));
+
   rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
   qos_sub.best_effort();
-  image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
+  image_sub_ = this->create_subscription<Image>(
     "~/input/image", qos_sub,
     std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1));
-  cam_info_sub_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
+  cam_info_sub_ = this->create_subscription<CameraInfo>(
     "~/input/camera_info", qos_sub,
     std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1));
-  ekf_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+  ekf_pose_sub_ = this->create_subscription<PoseWithCovarianceStamped>(
     "~/input/ekf_pose", qos_sub,
     std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1));
 
@@ -132,29 +135,25 @@ bool ArTagBasedLocalizer::setup()
   rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10));
   qos_marker.transient_local();
   qos_marker.reliable();
-  marker_pub_ =
-    this->create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", qos_marker);
+  marker_pub_ = this->create_publisher<MarkerArray>("~/debug/marker", qos_marker);
   rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
   image_pub_ = it_->advertise("~/debug/result", 1);
-  pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "~/output/pose_with_covariance", qos_pub);
-  diag_pub_ =
-    this->create_publisher<diagnostic_msgs::msg::DiagnosticArray>("/diagnostics", qos_pub);
+  pose_pub_ =
+    this->create_publisher<PoseWithCovarianceStamped>("~/output/pose_with_covariance", qos_pub);
+  diag_pub_ = this->create_publisher<DiagnosticArray>("/diagnostics", qos_pub);
 
   RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!");
   return true;
 }
 
-void ArTagBasedLocalizer::map_bin_callback(
-  const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg)
+void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg)
 {
   landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger());
-  const visualization_msgs::msg::MarkerArray marker_msg =
-    convert_to_marker_array_msg(landmark_map_);
+  const MarkerArray marker_msg = convert_to_marker_array_msg(landmark_map_);
   marker_pub_->publish(marker_msg);
 }
 
-void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg)
+void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg)
 {
   if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) {
     RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers");
@@ -166,7 +165,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha
     return;
   }
 
-  builtin_interfaces::msg::Time curr_stamp = msg->header.stamp;
+  const builtin_interfaces::msg::Time curr_stamp = msg->header.stamp;
   cv_bridge::CvImagePtr cv_ptr;
   try {
     cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8);
@@ -185,20 +184,20 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha
 
   // for each marker, draw info and its boundaries in the image
   for (const aruco::Marker & marker : markers) {
-    tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker);
+    const tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker);
 
-    geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped;
+    TransformStamped tf_cam_to_marker_stamped;
     tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform);
     tf_cam_to_marker_stamped.header.stamp = curr_stamp;
     tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id;
     tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id);
     tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped);
 
-    geometry_msgs::msg::PoseStamped pose_cam_to_marker;
+    PoseStamped pose_cam_to_marker;
     tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose);
     pose_cam_to_marker.header.stamp = curr_stamp;
-    pose_cam_to_marker.header.frame_id = std::to_string(marker.id);
-    publish_pose_as_base_link(pose_cam_to_marker, msg->header.frame_id);
+    pose_cam_to_marker.header.frame_id = msg->header.frame_id;
+    publish_pose_as_base_link(pose_cam_to_marker, std::to_string(marker.id));
 
     // drawing the detected markers
     marker.draw(in_image, cv::Scalar(0, 0, 255), 2);
@@ -240,7 +239,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha
   key_value.value = std::to_string(detected_tags);
   diag_status.values.push_back(key_value);
 
-  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  DiagnosticArray diag_msg;
   diag_msg.header.stamp = this->now();
   diag_msg.status.push_back(diag_status);
 
@@ -248,17 +247,13 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha
 }
 
 // wait for one camera info, then shut down that subscriber
-void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & msg)
+void ArTagBasedLocalizer::cam_info_callback(const CameraInfo & msg)
 {
   if (cam_info_received_) {
     return;
   }
 
   cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0);
-  cv::Mat distortion_coeff(4, 1, CV_64FC1);
-  cv::Size size(static_cast<int>(msg.width), static_cast<int>(msg.height));
-
-  camera_matrix.setTo(0);
   camera_matrix.at<double>(0, 0) = msg.p[0];
   camera_matrix.at<double>(0, 1) = msg.p[1];
   camera_matrix.at<double>(0, 2) = msg.p[2];
@@ -272,111 +267,88 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo &
   camera_matrix.at<double>(2, 2) = msg.p[10];
   camera_matrix.at<double>(2, 3) = msg.p[11];
 
+  cv::Mat distortion_coeff(4, 1, CV_64FC1);
   for (int i = 0; i < 4; ++i) {
     distortion_coeff.at<double>(i, 0) = 0;
   }
 
+  const cv::Size size(static_cast<int>(msg.width), static_cast<int>(msg.height));
+
   cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size);
 
   cam_info_received_ = true;
 }
 
-void ArTagBasedLocalizer::ekf_pose_callback(
-  const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
+void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped & msg)
 {
   latest_ekf_pose_ = msg;
 }
 
 void ArTagBasedLocalizer::publish_pose_as_base_link(
-  const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id)
+  const PoseStamped & sensor_to_tag, const std::string & tag_id)
 {
-  // Check if frame_id is in target_tag_ids
-  if (
-    std::find(target_tag_ids_.begin(), target_tag_ids_.end(), msg.header.frame_id) ==
-    target_tag_ids_.end()) {
-    RCLCPP_INFO_STREAM(
-      this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in target_tag_ids");
+  // Check tag_id
+  if (std::find(target_tag_ids_.begin(), target_tag_ids_.end(), tag_id) == target_tag_ids_.end()) {
+    RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in target_tag_ids");
+    return;
+  }
+  if (landmark_map_.count(tag_id) == 0) {
+    RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in landmark_map_");
     return;
   }
 
   // Range filter
-  const double distance_squared = msg.pose.position.x * msg.pose.position.x +
-                                  msg.pose.position.y * msg.pose.position.y +
-                                  msg.pose.position.z * msg.pose.position.z;
+  const double distance_squared = sensor_to_tag.pose.position.x * sensor_to_tag.pose.position.x +
+                                  sensor_to_tag.pose.position.y * sensor_to_tag.pose.position.y +
+                                  sensor_to_tag.pose.position.z * sensor_to_tag.pose.position.z;
   if (distance_threshold_squared_ < distance_squared) {
     return;
   }
 
-  // Transform map to tag
-  if (landmark_map_.count(msg.header.frame_id) == 0) {
-    RCLCPP_INFO_STREAM(
-      this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in landmark_map_");
-    return;
-  }
-  const geometry_msgs::msg::Pose & tag_pose = landmark_map_.at(msg.header.frame_id);
-  geometry_msgs::msg::TransformStamped map_to_tag_tf;
-  map_to_tag_tf.header.stamp = msg.header.stamp;
-  map_to_tag_tf.header.frame_id = "map";
-  map_to_tag_tf.child_frame_id = msg.header.frame_id;
-  map_to_tag_tf.transform.translation.x = tag_pose.position.x;
-  map_to_tag_tf.transform.translation.y = tag_pose.position.y;
-  map_to_tag_tf.transform.translation.z = tag_pose.position.z;
-  map_to_tag_tf.transform.rotation = tag_pose.orientation;
-
-  // Transform camera to base_link
-  geometry_msgs::msg::TransformStamped camera_to_base_link_tf;
+  // Transform to base_link
+  PoseStamped base_link_to_tag;
   try {
-    camera_to_base_link_tf =
-      tf_buffer_->lookupTransform(camera_frame_id, "base_link", tf2::TimePointZero);
+    const TransformStamped transform =
+      tf_buffer_->lookupTransform("base_link", sensor_to_tag.header.frame_id, tf2::TimePointZero);
+    tf2::doTransform(sensor_to_tag, base_link_to_tag, transform);
+    base_link_to_tag.header.frame_id = "base_link";
   } catch (tf2::TransformException & ex) {
     RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what());
     return;
   }
 
-  // Convert ROS Transforms to Eigen matrices for easy matrix multiplication and inversion
-  Eigen::Affine3d map_to_tag = tf2::transformToEigen(map_to_tag_tf.transform);
-  Eigen::Affine3d camera_to_base_link = tf2::transformToEigen(camera_to_base_link_tf.transform);
-  Eigen::Affine3d camera_to_tag = Eigen::Affine3d(
-    Eigen::Translation3d(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) *
-    Eigen::Quaterniond(
-      msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y,
-      msg.pose.orientation.z));
-  Eigen::Affine3d tag_to_camera = camera_to_tag.inverse();
+  // (1) map_to_tag
+  const Pose & map_to_tag = landmark_map_.at(tag_id);
+  const Eigen::Affine3d map_to_tag_affine = pose_to_affine3d(map_to_tag);
 
-  // Final pose calculation
-  Eigen::Affine3d map_to_base_link = map_to_tag * tag_to_camera * camera_to_base_link;
+  // (2) tag_to_base_link
+  const Eigen::Affine3d base_link_to_tag_affine = pose_to_affine3d(base_link_to_tag.pose);
+  const Eigen::Affine3d tag_to_base_link_affine = base_link_to_tag_affine.inverse();
 
-  // Construct output message
-  geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance_stamped;
-  pose_with_covariance_stamped.header.stamp = msg.header.stamp;
-  pose_with_covariance_stamped.header.frame_id = "map";
-  pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link);
+  // calculate map_to_base_link
+  const Eigen::Affine3d map_to_base_link_affine = map_to_tag_affine * tag_to_base_link_affine;
+  const Pose map_to_base_link = tf2::toMsg(map_to_base_link_affine);
 
   // If latest_ekf_pose_ is older than <ekf_time_tolerance_> seconds compared to current frame, it
   // will not be published.
-  const rclcpp::Duration tolerance{
-    static_cast<int32_t>(ekf_time_tolerance_),
-    static_cast<uint32_t>((ekf_time_tolerance_ - std::floor(ekf_time_tolerance_)) * 1e9)};
-  if (rclcpp::Time(latest_ekf_pose_.header.stamp) + tolerance < rclcpp::Time(msg.header.stamp)) {
+  const rclcpp::Duration diff_time =
+    rclcpp::Time(sensor_to_tag.header.stamp) - rclcpp::Time(latest_ekf_pose_.header.stamp);
+  if (diff_time.seconds() > ekf_time_tolerance_) {
     RCLCPP_INFO(
       this->get_logger(),
       "latest_ekf_pose_ is older than %f seconds compared to current frame. "
-      "latest_ekf_pose_.header.stamp: %d.%d, msg.header.stamp: %d.%d",
+      "latest_ekf_pose_.header.stamp: %d.%d, sensor_to_tag.header.stamp: %d.%d",
       ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec,
-      msg.header.stamp.sec, msg.header.stamp.nanosec);
+      sensor_to_tag.header.stamp.sec, sensor_to_tag.header.stamp.nanosec);
     return;
   }
 
   // If curr_pose differs from latest_ekf_pose_ by more than <ekf_position_tolerance_>, it will not
   // be published.
-  const geometry_msgs::msg::Pose curr_pose = pose_with_covariance_stamped.pose.pose;
-  const geometry_msgs::msg::Pose latest_ekf_pose = latest_ekf_pose_.pose.pose;
-  const double diff_x = curr_pose.position.x - latest_ekf_pose.position.x;
-  const double diff_y = curr_pose.position.y - latest_ekf_pose.position.y;
-  const double diff_z = curr_pose.position.z - latest_ekf_pose.position.z;
-  const double diff_distance_squared = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z;
-  const double threshold = ekf_position_tolerance_ * ekf_position_tolerance_;
-  if (threshold < diff_distance_squared) {
+  const Pose curr_pose = map_to_base_link;
+  const Pose latest_ekf_pose = latest_ekf_pose_.pose.pose;
+  const double diff_position = norm(curr_pose.position, latest_ekf_pose.position);
+  if (diff_position > ekf_position_tolerance_) {
     RCLCPP_INFO(
       this->get_logger(),
       "curr_pose differs from latest_ekf_pose_ by more than %f m. "
@@ -386,6 +358,12 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(
     return;
   }
 
+  // Construct output message
+  PoseWithCovarianceStamped pose_with_covariance_stamped;
+  pose_with_covariance_stamped.header.stamp = sensor_to_tag.header.stamp;
+  pose_with_covariance_stamped.header.frame_id = "map";
+  pose_with_covariance_stamped.pose.pose = curr_pose;
+
   // ~5[m]: base_covariance
   // 5~[m]: scaling base_covariance by std::pow(distance/5, 3)
   const double distance = std::sqrt(distance_squared);
diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
index 6bd66eead653f..fe33a64b995a3 100644
--- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
+++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp
@@ -66,17 +66,26 @@
 
 class ArTagBasedLocalizer : public rclcpp::Node
 {
+  using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+  using Image = sensor_msgs::msg::Image;
+  using CameraInfo = sensor_msgs::msg::CameraInfo;
+  using Pose = geometry_msgs::msg::Pose;
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
+  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
+  using TransformStamped = geometry_msgs::msg::TransformStamped;
+  using MarkerArray = visualization_msgs::msg::MarkerArray;
+  using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray;
+
 public:
   explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   bool setup();
 
 private:
-  void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg);
-  void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg);
-  void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg);
-  void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg);
-  void publish_pose_as_base_link(
-    const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id);
+  void map_bin_callback(const HADMapBin::ConstSharedPtr & msg);
+  void image_callback(const Image::ConstSharedPtr & msg);
+  void cam_info_callback(const CameraInfo & msg);
+  void ekf_pose_callback(const PoseWithCovarianceStamped & msg);
+  void publish_pose_as_base_link(const PoseStamped & sensor_to_tag, const std::string & tag_id);
   static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker);
 
   // Parameters
@@ -96,23 +105,23 @@ class ArTagBasedLocalizer : public rclcpp::Node
   std::unique_ptr<image_transport::ImageTransport> it_;
 
   // Subscribers
-  rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_bin_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr cam_info_sub_;
-  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr ekf_pose_sub_;
+  rclcpp::Subscription<HADMapBin>::SharedPtr map_bin_sub_;
+  rclcpp::Subscription<Image>::SharedPtr image_sub_;
+  rclcpp::Subscription<CameraInfo>::SharedPtr cam_info_sub_;
+  rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr ekf_pose_sub_;
 
   // Publishers
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr marker_pub_;
   image_transport::Publisher image_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diag_pub_;
+  rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+  rclcpp::Publisher<DiagnosticArray>::SharedPtr diag_pub_;
 
   // Others
   aruco::MarkerDetector detector_;
   aruco::CameraParameters cam_param_;
   bool cam_info_received_;
-  geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{};
-  std::map<std::string, geometry_msgs::msg::Pose> landmark_map_;
+  PoseWithCovarianceStamped latest_ekf_pose_{};
+  std::map<std::string, Pose> landmark_map_;
 };
 
 #endif  // AR_TAG_BASED_LOCALIZER_HPP_
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
index 13721c03ca22e..78052cb8198a3 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp
@@ -51,8 +51,7 @@ class MapUpdateModule
     rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
     std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
     std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
-    rclcpp::CallbackGroup::SharedPtr main_callback_group,
-    std::shared_ptr<std::map<std::string, std::string>> state_ptr);
+    rclcpp::CallbackGroup::SharedPtr main_callback_group);
 
 private:
   friend class NDTScanMatcher;
@@ -82,7 +81,6 @@ class MapUpdateModule
   rclcpp::Logger logger_;
   rclcpp::Clock::SharedPtr clock_;
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module_;
-  std::shared_ptr<std::map<std::string, std::string>> state_ptr_;
 
   std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
   std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
index 9eb62230dad9e..f0d90fdfa0e08 100644
--- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
+++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp
@@ -132,7 +132,7 @@ class NDTScanMatcher : public rclcpp::Node
     const rclcpp::Time & sensor_ros_time);
   void add_regularization_pose(const rclcpp::Time & sensor_ros_time);
 
-  void timer_diagnostic();
+  void publish_diagnostic();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_sub_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
@@ -203,8 +203,6 @@ class NDTScanMatcher : public rclcpp::Node
   std::mutex ndt_ptr_mtx_;
   std::mutex initial_pose_array_mtx_;
 
-  std::thread diagnostic_thread_;
-
   // variables for regularization
   const bool regularization_enabled_;
   std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr>
diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp
index 7512ae2e77a21..f0a583417fb76 100644
--- a/localization/ndt_scan_matcher/src/map_update_module.cpp
+++ b/localization/ndt_scan_matcher/src/map_update_module.cpp
@@ -26,15 +26,13 @@ MapUpdateModule::MapUpdateModule(
   rclcpp::Node * node, std::mutex * ndt_ptr_mutex,
   std::shared_ptr<NormalDistributionsTransform> ndt_ptr,
   std::shared_ptr<Tf2ListenerModule> tf2_listener_module, std::string map_frame,
-  rclcpp::CallbackGroup::SharedPtr main_callback_group,
-  std::shared_ptr<std::map<std::string, std::string>> state_ptr)
+  rclcpp::CallbackGroup::SharedPtr main_callback_group)
 : ndt_ptr_(std::move(ndt_ptr)),
   ndt_ptr_mutex_(ndt_ptr_mutex),
   map_frame_(std::move(map_frame)),
   logger_(node->get_logger()),
   clock_(node->get_clock()),
   tf2_listener_module_(std::move(tf2_listener_module)),
-  state_ptr_(std::move(state_ptr)),
   dynamic_map_loading_update_distance_(
     node->declare_parameter<double>("dynamic_map_loading_update_distance")),
   dynamic_map_loading_map_radius_(
diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
index cb66998147220..a2e89540a172f 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -280,16 +280,12 @@ NDTScanMatcher::NDTScanMatcher()
       &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
     rclcpp::ServicesQoS().get_rmw_qos_profile(), main_callback_group);
 
-  diagnostic_thread_ = std::thread(&NDTScanMatcher::timer_diagnostic, this);
-  diagnostic_thread_.detach();
-
   tf2_listener_module_ = std::make_shared<Tf2ListenerModule>(this);
 
   use_dynamic_map_loading_ = this->declare_parameter<bool>("use_dynamic_map_loading");
   if (use_dynamic_map_loading_) {
     map_update_module_ = std::make_unique<MapUpdateModule>(
-      this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group,
-      state_ptr_);
+      this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group);
   } else {
     map_module_ = std::make_unique<MapModule>(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group);
   }
@@ -297,76 +293,71 @@ NDTScanMatcher::NDTScanMatcher()
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
 
-void NDTScanMatcher::timer_diagnostic()
+void NDTScanMatcher::publish_diagnostic()
 {
-  rclcpp::Rate rate(100);
-  while (rclcpp::ok()) {
-    diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
-    diag_status_msg.name = "ndt_scan_matcher";
-    diag_status_msg.hardware_id = "";
-
-    for (const auto & key_value : (*state_ptr_)) {
-      diagnostic_msgs::msg::KeyValue key_value_msg;
-      key_value_msg.key = key_value.first;
-      key_value_msg.value = key_value.second;
-      diag_status_msg.values.push_back(key_value_msg);
-    }
-
+  diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
+  diag_status_msg.name = "ndt_scan_matcher";
+  diag_status_msg.hardware_id = "";
+
+  for (const auto & key_value : (*state_ptr_)) {
+    diagnostic_msgs::msg::KeyValue key_value_msg;
+    key_value_msg.key = key_value.first;
+    key_value_msg.value = key_value.second;
+    diag_status_msg.values.push_back(key_value_msg);
+  }
+
+  diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  diag_status_msg.message = "";
+  if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "Initializing State. ";
+  }
+  if (
+    state_ptr_->count("lidar_topic_delay_time_sec") &&
+    std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
+  }
+  if (
+    state_ptr_->count("skipping_publish_num") &&
+    std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
+    std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "skipping_publish_num > 1. ";
+  }
+  if (
+    state_ptr_->count("skipping_publish_num") &&
+    std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+    diag_status_msg.message += "skipping_publish_num exceed limit. ";
+  }
+  if (
+    state_ptr_->count("nearest_voxel_transformation_likelihood") &&
+    std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
+      converged_param_nearest_voxel_transformation_likelihood_) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message += "NDT score is unreliably low. ";
+  }
+  if (
+    state_ptr_->count("execution_time") &&
+    std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) {
+    diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+    diag_status_msg.message +=
+      "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])";
+  }
+  // Ignore local optimal solution
+  if (
+    state_ptr_->count("is_local_optimal_solution_oscillation") &&
+    std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
     diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-    diag_status_msg.message = "";
-    if (state_ptr_->count("state") && (*state_ptr_)["state"] == "Initializing") {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "Initializing State. ";
-    }
-    if (
-      state_ptr_->count("lidar_topic_delay_time_sec") &&
-      std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. ";
-    }
-    if (
-      state_ptr_->count("skipping_publish_num") &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "skipping_publish_num > 1. ";
-    }
-    if (
-      state_ptr_->count("skipping_publish_num") &&
-      std::stoi((*state_ptr_)["skipping_publish_num"]) >= 5) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
-      diag_status_msg.message += "skipping_publish_num exceed limit. ";
-    }
-    if (
-      state_ptr_->count("nearest_voxel_transformation_likelihood") &&
-      std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) <
-        converged_param_nearest_voxel_transformation_likelihood_) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message += "NDT score is unreliably low. ";
-    }
-    if (
-      state_ptr_->count("execution_time") &&
-      std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
-      diag_status_msg.message +=
-        "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])";
-    }
-    // Ignore local optimal solution
-    if (
-      state_ptr_->count("is_local_optimal_solution_oscillation") &&
-      std::stoi((*state_ptr_)["is_local_optimal_solution_oscillation"])) {
-      diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-      diag_status_msg.message = "local optimal solution oscillation occurred";
-    }
-
-    diagnostic_msgs::msg::DiagnosticArray diag_msg;
-    diag_msg.header.stamp = this->now();
-    diag_msg.status.push_back(diag_status_msg);
+    diag_status_msg.message = "local optimal solution oscillation occurred";
+  }
 
-    diagnostics_pub_->publish(diag_msg);
+  diagnostic_msgs::msg::DiagnosticArray diag_msg;
+  diag_msg.header.stamp = this->now();
+  diag_msg.status.push_back(diag_status_msg);
 
-    rate.sleep();
-  }
+  diagnostics_pub_->publish(diag_msg);
 }
 
 void NDTScanMatcher::callback_initial_pose(
@@ -476,13 +467,11 @@ void NDTScanMatcher::callback_sensor_points(
   }
 
   // perform ndt scan matching
-  (*state_ptr_)["state"] = "Aligning";
   const Eigen::Matrix4f initial_pose_matrix =
     pose_to_matrix4f(interpolator.get_current_pose().pose.pose);
   auto output_cloud = std::make_shared<pcl::PointCloud<PointSource>>();
   ndt_ptr_->align(*output_cloud, initial_pose_matrix);
   const pclomp::NdtResult ndt_result = ndt_ptr_->getResult();
-  (*state_ptr_)["state"] = "Sleeping";
 
   const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose);
   std::vector<geometry_msgs::msg::Pose> transformation_msg_array;
@@ -573,6 +562,7 @@ void NDTScanMatcher::callback_sensor_points(
       make_float32_stamped(sensor_ros_time, no_ground_nearest_voxel_transformation_likelihood));
   }
 
+  (*state_ptr_)["state"] = "Aligned";
   (*state_ptr_)["transform_probability"] = std::to_string(ndt_result.transform_probability);
   (*state_ptr_)["nearest_voxel_transformation_likelihood"] =
     std::to_string(ndt_result.nearest_voxel_transformation_likelihood);
@@ -584,6 +574,8 @@ void NDTScanMatcher::callback_sensor_points(
     (*state_ptr_)["is_local_optimal_solution_oscillation"] = "0";
   }
   (*state_ptr_)["execution_time"] = std::to_string(exe_time);
+
+  publish_diagnostic();
 }
 
 void NDTScanMatcher::transform_sensor_measurement(
@@ -859,8 +851,6 @@ void NDTScanMatcher::service_trigger_node(
   if (is_activated_) {
     std::lock_guard<std::mutex> initial_pose_array_lock(initial_pose_array_mtx_);
     initial_pose_msg_ptr_array_.clear();
-  } else {
-    (*state_ptr_)["state"] = "Initializing";
   }
   res->success = true;
 }
@@ -897,9 +887,7 @@ void NDTScanMatcher::service_ndt_align(
     return;
   }
 
-  (*state_ptr_)["state"] = "Aligning";
   res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame);
-  (*state_ptr_)["state"] = "Sleeping";
   res->success = true;
   res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance;
 }
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
index 6c522c79a8774..077e0ecec0f29 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
@@ -273,29 +273,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     parameters_ = std::any_cast<std::shared_ptr<DynamicAvoidanceParameters>>(parameters);
   }
 
-  // TODO(someone): remove this, and use base class function
-  [[deprecated]] BehaviorModuleOutput run() override
-  {
-    updateData();
-
-    if (!isWaitingApproval()) {
-      return plan();
-    }
-
-    // module is waiting approval. Check it.
-    if (isActivated()) {
-      RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan().");
-      return plan();
-    } else {
-      RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate().");
-      return planWaitingApproval();
-    }
-  }
-
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
   // TODO(someone): remove this, and use base class function
-  [[deprecated]] ModuleStatus updateState() override;
   BehaviorModuleOutput plan() override;
   BehaviorModuleOutput planWaitingApproval() override;
   CandidateOutput planCandidate() const override;
@@ -315,7 +295,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     const double min_lon_offset;
   };
 
-  bool canTransitSuccessState() override { return false; }
+  bool canTransitSuccessState() override;
 
   bool canTransitFailureState() override { return false; }
 
diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
index 2c5e78a267d5a..7aba1d1b2d97f 100644
--- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -251,7 +251,7 @@ bool DynamicAvoidanceModule::isExecutionRequested() const
   }
 
   // check if the planner is already running
-  if (current_state_ == ModuleStatus::RUNNING) {
+  if (getCurrentStatus() == ModuleStatus::RUNNING) {
     return true;
   }
 
@@ -279,15 +279,9 @@ void DynamicAvoidanceModule::updateData()
   }
 }
 
-ModuleStatus DynamicAvoidanceModule::updateState()
+bool DynamicAvoidanceModule::canTransitSuccessState()
 {
-  const bool has_avoidance_target = !target_objects_.empty();
-
-  if (!has_avoidance_target) {
-    return ModuleStatus::SUCCESS;
-  }
-
-  return ModuleStatus::RUNNING;
+  return target_objects_.empty();
 }
 
 BehaviorModuleOutput DynamicAvoidanceModule::plan()
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 d7e25860c8abf..f2629a0586045 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
@@ -326,9 +326,9 @@ void SimplePlanningSimulator::on_timer()
   }
 
   // calculate longitudinal acceleration by slope
-  const double ego_pitch_angle = calculate_ego_pitch();
-  const double acc_by_slope =
-    enable_road_slope_simulation_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0;
+  constexpr double gravity_acceleration = -9.81;
+  const double ego_pitch_angle = enable_road_slope_simulation_ ? calculate_ego_pitch() : 0.0;
+  const double acc_by_slope = gravity_acceleration * std::sin(ego_pitch_angle);
 
   // update vehicle dynamics
   {