diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
index 3042774d16fd3..257feee25691a 100644
--- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
+++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml
@@ -7,6 +7,9 @@
   <!-- When gnss_enabled is false, automatic_pose_initializer will not run, only manual initial position estimation is available. -->
   <arg name="gnss_enabled" default="true" description="gnss availability for initial position estimation"/>
 
+  <!-- When use_pose_covariance_modifier is true, gnss and NDT poses will be used together in localization-->
+  <arg name="use_pose_covariance_modifier" default="true" description="parameter for using gnss and NDT together with covariance modifier"/>
+
   <!-- split string with underscores -->
   <let name="available_args" value="[\'ndt\',\'yabloc\',\'eagleye\',\'artag\']"/>
   <let name="split_function" value="list(set('$(var pose_source)'.split('_')).intersection($(var available_args)))"/>
@@ -80,6 +83,11 @@
     </include>
   </group>
 
+  <!-- AW Pose Covariance Modifier Node -->
+  <group if="$(var use_pose_covariance_modifier)">
+    <include file="$(find-pkg-share aw_pose_covariance_modifier_node)/launch/aw_pose_covariance_modifier_node.launch.xml"/>
+  </group>
+
   <!-- Util Launch -->
   <group>
     <push-ros-namespace namespace="util"/>
diff --git a/localization/aw_pose_covariance_modifier_node/CMakeLists.txt b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt
new file mode 100644
index 0000000000000..d14c20e381713
--- /dev/null
+++ b/localization/aw_pose_covariance_modifier_node/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.14)
+project(aw_pose_covariance_modifier_node)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+set(NODE_NAME ${PROJECT_NAME})
+set(EXEC_NAME ${PROJECT_NAME}_exe)
+
+add_executable(${PROJECT_NAME}  src/aw_pose_covariance_modifier_node.cpp)
+
+ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs rcl_interfaces)
+
+install(TARGETS
+        ${PROJECT_NAME}
+        DESTINATION lib/${PROJECT_NAME}
+)
+include_directories(src/include/)
+
+#ament_auto_add_library(${NODE_NAME}
+#  src/include/aw_pose_covariance_modifier_node.hpp
+#  src/aw_pose_covariance_modifier_node.cpp)
+
+#rclcpp_components_register_node(${NODE_NAME}
+#  PLUGIN "aw_pose_covariance_modifier_node::AWPoseCovarianceModifierNode"
+#  EXECUTABLE ${EXEC_NAME})
+
+
+ament_auto_package(INSTALL_TO_SHARE
+        launch)
diff --git a/localization/aw_pose_covariance_modifier_node/README.md b/localization/aw_pose_covariance_modifier_node/README.md
new file mode 100644
index 0000000000000..4849b6b585efd
--- /dev/null
+++ b/localization/aw_pose_covariance_modifier_node/README.md
@@ -0,0 +1 @@
+# aw_pose_covariance_modifier
diff --git a/localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml b/localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml
new file mode 100755
index 0000000000000..65b533edd87d2
--- /dev/null
+++ b/localization/aw_pose_covariance_modifier_node/launch/aw_pose_covariance_modifier_node.launch.xml
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_trusted_pose_with_cov_topic" default="/sensing/gnss/pose_with_covariance" description="Trusted pose input .. "/>
+  <arg name="output_pose_with_covariance_topic" default="/localization/pose_estimator/pose_with_covariance" description="Estimated self position with covariance"/>
+
+  <node pkg="aw_pose_covariance_modifier_node" exec="aw_pose_covariance_modifier_node" name="aw_pose_covariance_modifier_node" output="screen">
+    <remap from="input_trusted_pose_with_cov_topic" to="$(var input_trusted_pose_with_cov_topic)"/>
+    <remap from="output_pose_with_covariance_topic" to="$(var output_pose_with_covariance_topic)"/>
+  </node>
+</launch>
diff --git a/localization/aw_pose_covariance_modifier_node/package.xml b/localization/aw_pose_covariance_modifier_node/package.xml
new file mode 100644
index 0000000000000..4680527436c66
--- /dev/null
+++ b/localization/aw_pose_covariance_modifier_node/package.xml
@@ -0,0 +1,25 @@
+<?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>aw_pose_covariance_modifier_node</name>
+  <version>1.0.0</version>
+  <description>Converts an autoware_msgs message to autoware_auto_msgs version and publishes it.</description>
+
+  <maintainer email="melike@leodrive.ai">Melike Tanrikulu</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp
new file mode 100644
index 0000000000000..c89e00472409c
--- /dev/null
+++ b/localization/aw_pose_covariance_modifier_node/src/aw_pose_covariance_modifier_node.cpp
@@ -0,0 +1,112 @@
+// Copyright 2024 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "include/aw_pose_covariance_modifier_node.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+AWPoseCovarianceModifierNode::AWPoseCovarianceModifierNode() : Node("AWPoseCovarianceModifierNode")
+{
+  trusted_pose_with_cov_sub_ =
+    this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "input_trusted_pose_with_cov_topic", 10000,
+      std::bind(
+        &AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback, this,
+        std::placeholders::_1));
+
+  new_pose_estimator_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    "output_pose_with_covariance_topic", 10);
+
+  client_ = this->create_client<rcl_interfaces::srv::SetParameters>(
+    "/localization/pose_estimator/ndt_scan_matcher/set_parameters");
+
+  while (!client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) {
+    RCLCPP_INFO(this->get_logger(), "Waiting for aw_pose_covariance_modifier_node service...");
+  }
+
+  activateNDTCovModifier = AWPoseCovarianceModifierNode::callNDTCovarianceModifier();
+  if (activateNDTCovModifier == 1) {
+    RCLCPP_INFO(get_logger(), "NDT pose covariance modifier activated ...");
+  } else {
+    RCLCPP_WARN(get_logger(), "Failed to enable NDT pose covariance modifier ...");
+  }
+}
+
+bool AWPoseCovarianceModifierNode::callNDTCovarianceModifier()
+{
+  auto request = std::make_shared<rcl_interfaces::srv::SetParameters::Request>();
+
+  rcl_interfaces::msg::Parameter parameter;
+  parameter.name = "aw_pose_covariance_modifier.enable";
+  parameter.value.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
+  parameter.value.bool_value = true;
+
+  request->parameters.push_back(parameter);
+
+  client_->async_send_request(
+    request, [this](rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedFuture result_) {
+      auto result_status = result_.wait_for(std::chrono::seconds(1));
+
+      if (result_status == std::future_status::ready) {
+        auto response = result_.get();
+
+        if (response && response->results.data()) {
+          RCLCPP_INFO(
+            this->get_logger(), "aw_pose_covariance_modifier.enable parameter set successfully.");
+          return true;
+        } else {
+          RCLCPP_ERROR(
+            this->get_logger(),
+            "An error occurred while setting the aw_pose_covariance_modifier.enable parameter.");
+          return false;
+        }
+      } else {
+        RCLCPP_ERROR(
+          this->get_logger(), "The request was not completed within the specified time.");
+        return false;
+      }
+    });
+  return true;
+}
+
+void AWPoseCovarianceModifierNode::trusted_pose_with_cov_callback(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg)
+{
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_estimator_pose = *msg;
+
+  trusted_pose_rmse_ = (std::sqrt(pose_estimator_pose.pose.covariance[0]) +
+                        std::sqrt(pose_estimator_pose.pose.covariance[7])) /
+                       2;
+  trusted_pose_yaw_rmse_in_degrees_ =
+    std::sqrt(pose_estimator_pose.pose.covariance[35]) * 180 / M_PI;
+
+  if (trusted_pose_rmse_ > 0.25) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Trusted Pose RMSE is under the threshold. It will not be used as a pose source.");
+  } else {
+    if (trusted_pose_yaw_rmse_in_degrees_ >= 0.3) {
+      pose_estimator_pose.pose.covariance[35] = 1000000;
+    }
+
+    new_pose_estimator_pub_->publish(pose_estimator_pose);
+  }
+}
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AWPoseCovarianceModifierNode>());
+  rclcpp::shutdown();
+  return 0;
+}
diff --git a/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp
new file mode 100644
index 0000000000000..383f1665616a8
--- /dev/null
+++ b/localization/aw_pose_covariance_modifier_node/src/include/aw_pose_covariance_modifier_node.hpp
@@ -0,0 +1,45 @@
+// Copyright 2024 The Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
+#define AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
+
+#include <rcl_interfaces/srv/set_parameters.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+#include <string>
+
+class AWPoseCovarianceModifierNode : public rclcpp::Node
+{
+public:
+  AWPoseCovarianceModifierNode();
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    trusted_pose_with_cov_sub_;
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    new_pose_estimator_pub_;
+  rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr client_;
+
+  void trusted_pose_with_cov_callback(
+    const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr & msg);
+  bool callNDTCovarianceModifier();
+
+private:
+  double trusted_pose_rmse_;
+  double trusted_pose_yaw_rmse_in_degrees_;
+  bool activateNDTCovModifier = 0;
+};
+
+#endif  // AW_POSE_COVARIANCE_MODIFIER_NODE_HPP_
diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt
index b4d656cb1810b..09d47ce198395 100644
--- a/localization/ndt_scan_matcher/CMakeLists.txt
+++ b/localization/ndt_scan_matcher/CMakeLists.txt
@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14)
 project(ndt_scan_matcher)
 
 find_package(autoware_cmake REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 autoware_package()
 
 # Compile flags for SIMD instructions
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 ca69576061e21..2deeaeaeae696 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
@@ -20,6 +20,7 @@
 #include "localization_util/smart_pose_buffer.hpp"
 #include "ndt_scan_matcher/hyper_parameters.hpp"
 #include "ndt_scan_matcher/map_update_module.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/logger_level_configure.hpp>
@@ -83,7 +84,9 @@ class NDTScanMatcher : public rclcpp::Node
   void service_trigger_node(
     const std_srvs::srv::SetBool::Request::SharedPtr req,
     std_srvs::srv::SetBool::Response::SharedPtr res);
-
+  void setParametersCallback(
+    const rcl_interfaces::srv::SetParameters::Request::SharedPtr request,
+    rcl_interfaces::srv::SetParameters::Response::SharedPtr response);
   void callback_timer();
   void callback_sensor_points(
     sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame);
@@ -91,6 +94,8 @@ class NDTScanMatcher : public rclcpp::Node
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr);
   void callback_regularization_pose(
     geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
+  void callback_trusted_source_pose(
+    geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr);
 
   geometry_msgs::msg::PoseWithCovarianceStamped align_pose(
     const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov);
@@ -138,6 +143,8 @@ class NDTScanMatcher : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_points_sub_;
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
     regularization_pose_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    trusted_source_pose_sub_;
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr sensor_aligned_pose_pub_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr no_ground_points_aligned_pose_pub_;
@@ -172,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node
 
   rclcpp::Service<tier4_localization_msgs::srv::PoseWithCovarianceStamped>::SharedPtr service_;
   rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_trigger_node_;
+  rclcpp::Service<rcl_interfaces::srv::SetParameters>::SharedPtr set_parameters_service_;
 
   tf2_ros::TransformBroadcaster tf2_broadcaster_;
   tf2_ros::Buffer tf2_buffer_;
@@ -193,10 +201,27 @@ class NDTScanMatcher : public rclcpp::Node
 
   std::unique_ptr<SmartPoseBuffer> regularization_pose_buffer_;
 
+  geometry_msgs::msg::PoseWithCovarianceStamped trusted_source_pose_;
+  bool activate_pose_covariance_modifier_;
+  bool close_ndt_pose_source_ = false;
+  struct trusted_pose_
+  {
+    double pose_average_rmse_xy = 0.0;
+    double yaw_rmse = 0.0;
+  } trustedPose;
+
   std::atomic<bool> is_activated_;
   std::unique_ptr<MapUpdateModule> map_update_module_;
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
+  std::array<double, 36> covariance_modifier(std::array<double, 36> & in_ndt_covariance);
+
+  // To be used for AW Pose Covariance Modifier - Trusted Pose timeout control
+  bool checkTrustedPoseTimeout();
+  rclcpp::Time trustedPoseCallbackTime;
+  std::mutex mutex_cov_modifier_;
+  void createTrustedSourcePoseSubscriber();
+
   HyperParameters param_;
 };
 
diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
index cbcb0a9f74bc4..33ea5d294a18e 100644
--- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
+++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml
@@ -6,6 +6,7 @@
   <arg name="input_initial_pose_topic" default="ekf_pose_with_covariance" description="Initial position topic to align"/>
   <arg name="input_regularization_pose_topic" default="regularization_pose_with_covariance" description="Regularization pose topic"/>
   <arg name="input_service_trigger_node" default="trigger_node" description="Trigger node service name"/>
+  <arg name="input_covariance_modifier_trusted_pose_topic" default="/sensing/gnss/pose_with_covariance" description="AW Pose cov modifier trusted PoseWithCov topic"/>
 
   <arg name="output_pose_topic" default="ndt_pose" description="Estimated self position"/>
   <arg name="output_pose_with_covariance_topic" default="ndt_pose_with_covariance" description="Estimated self position with covariance"/>
@@ -14,11 +15,12 @@
 
   <arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node"/>
 
-  <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="log">
+  <node pkg="ndt_scan_matcher" exec="ndt_scan_matcher" name="$(var node_name)" output="screen">
     <remap from="points_raw" to="$(var input_pointcloud)"/>
     <remap from="ekf_pose_with_covariance" to="$(var input_initial_pose_topic)"/>
     <remap from="regularization_pose_with_covariance" to="$(var input_regularization_pose_topic)"/>
     <remap from="trigger_node_srv" to="$(var input_service_trigger_node)"/>
+    <remap from="input_trusted_pose_topic" to="$(var input_covariance_modifier_trusted_pose_topic)"/>
 
     <remap from="ndt_pose" to="$(var output_pose_topic)"/>
     <remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)"/>
diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml
index 0913ee04174f5..ba531dfa25a10 100644
--- a/localization/ndt_scan_matcher/package.xml
+++ b/localization/ndt_scan_matcher/package.xml
@@ -28,6 +28,7 @@
   <depend>nav_msgs</depend>
   <depend>ndt_omp</depend>
   <depend>pcl_conversions</depend>
+  <depend>rcl_interfaces</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
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 cd637791f04b6..eae33b7d6ff00 100644
--- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
+++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp
@@ -175,6 +175,12 @@ NDTScanMatcher::NDTScanMatcher()
       &NDTScanMatcher::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2),
     rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
 
+  set_parameters_service_ = this->create_service<rcl_interfaces::srv::SetParameters>(
+    "ndt_scan_matcher/set_parameters",
+    std::bind(
+      &NDTScanMatcher::setParametersCallback, this, std::placeholders::_1, std::placeholders::_2),
+    rclcpp::ServicesQoS().get_rmw_qos_profile(), sensor_callback_group);
+
   ndt_ptr_->setParams(param_.ndt);
 
   initial_pose_buffer_ = std::make_unique<SmartPoseBuffer>(
@@ -186,7 +192,38 @@ NDTScanMatcher::NDTScanMatcher()
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
 }
+void NDTScanMatcher::setParametersCallback(
+  const rcl_interfaces::srv::SetParameters::Request::SharedPtr request,
+  rcl_interfaces::srv::SetParameters::Response::SharedPtr response)
+{
+  std::lock_guard<std::mutex> lock(mutex_cov_modifier_);
+
+  for (const auto & param : request->parameters) {
+    if (param.name == "aw_pose_covariance_modifier.enable") {
+      activate_pose_covariance_modifier_ = param.value.bool_value;
+      RCLCPP_INFO(
+        this->get_logger(), "aw_pose_covariance_modifier.enable set to : %d",
+        activate_pose_covariance_modifier_);
 
+      rcl_interfaces::msg::SetParametersResult result;
+      result.successful = true;
+      result.reason = "Parameter successfully set.";
+
+      response->results.push_back(result);
+    }
+  }
+
+  if (activate_pose_covariance_modifier_ == 1) {
+    NDTScanMatcher::createTrustedSourcePoseSubscriber();
+  }
+}
+void NDTScanMatcher::createTrustedSourcePoseSubscriber()
+{
+  trusted_source_pose_sub_ =
+    this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "input_trusted_pose_topic", 100,
+      std::bind(&NDTScanMatcher::callback_trusted_source_pose, this, std::placeholders::_1));
+}
 void NDTScanMatcher::publish_diagnostic()
 {
   diagnostic_msgs::msg::DiagnosticStatus diag_status_msg;
@@ -303,6 +340,18 @@ void NDTScanMatcher::callback_regularization_pose(
   regularization_pose_buffer_->push_back(pose_conv_msg_ptr);
 }
 
+void NDTScanMatcher::callback_trusted_source_pose(
+  geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr)
+{
+  trusted_source_pose_ = *pose_conv_msg_ptr;
+  trustedPose.pose_average_rmse_xy = (std::sqrt(trusted_source_pose_.pose.covariance[0]) +
+                                      std::sqrt(trusted_source_pose_.pose.covariance[7])) /
+                                     2;
+  trustedPose.yaw_rmse = std::sqrt(trusted_source_pose_.pose.covariance[35]);
+  // To be used for timeout control
+  trustedPoseCallbackTime = this->now();
+}
+
 void NDTScanMatcher::callback_sensor_points(
   sensor_msgs::msg::PointCloud2::ConstSharedPtr sensor_points_msg_in_sensor_frame)
 {
@@ -411,9 +460,15 @@ void NDTScanMatcher::callback_sensor_points(
   const Eigen::Matrix3d map_to_base_link_rotation =
     map_to_base_link_quat.normalized().toRotationMatrix();
 
-  std::array<double, 36> ndt_covariance =
-    rotate_covariance(param_.covariance.output_pose_covariance, map_to_base_link_rotation);
+  std::array<double, 36> ndt_covariance_in_;
+  if (activate_pose_covariance_modifier_) {
+    ndt_covariance_in_ = covariance_modifier(param_.covariance.output_pose_covariance);
+  } else {
+    ndt_covariance_in_ = param_.covariance.output_pose_covariance;
+  }
 
+  std::array<double, 36> ndt_covariance =
+    rotate_covariance(ndt_covariance_in_, map_to_base_link_rotation);
   if (is_converged && param_.covariance.covariance_estimation.enable) {
     const auto estimated_covariance =
       estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time);
@@ -532,6 +587,67 @@ void NDTScanMatcher::publish_tf(
     tier4_autoware_utils::pose2transform(result_pose_stamped_msg, param_.frame.ndt_base_frame));
 }
 
+bool NDTScanMatcher::checkTrustedPoseTimeout()
+{
+  auto timeDiff = this->now() - trustedPoseCallbackTime;
+  if (timeDiff.seconds() > 1.0) {
+    return true;
+  }
+  return false;
+}
+std::array<double, 36> NDTScanMatcher::covariance_modifier(
+  std::array<double, 36> & in_ndt_covariance)
+{
+  std::array<double, 36> ndt_covariance;
+  ndt_covariance = in_ndt_covariance;
+  close_ndt_pose_source_ = false;
+
+  if (NDTScanMatcher::checkTrustedPoseTimeout()) {
+    RCLCPP_WARN(this->get_logger(), "Trusted Pose Timeout");
+    return ndt_covariance;
+  }
+
+  if (
+    trustedPose.pose_average_rmse_xy <= 0.10 &&
+    trustedPose.yaw_rmse < std::sqrt(in_ndt_covariance[35])) {
+    close_ndt_pose_source_ = true;
+  } else if (trustedPose.pose_average_rmse_xy <= 0.10) {
+    ndt_covariance[0] = 1000000;
+    ndt_covariance[7] = 1000000;
+  } else if (trustedPose.pose_average_rmse_xy <= 0.25) {
+    /*
+     * ndt_rmse = ndt_max_rmse_meters - (gnss_rmse * (ndt_max_rmse_meters - ndt_min_rmse_meters) /
+     * normalization_coeff) ndt_min_rmse_meters = in_ndt_rmse ndt_max_rmse_meters = in_ndt_rmse  * 2
+     */
+    double normalization_coeff = 0.1;
+    ndt_covariance[0] = std::pow(
+      ((std::sqrt(in_ndt_covariance[0]) * 2) - std::sqrt(trusted_source_pose_.pose.covariance[0])) *
+        (std::sqrt(in_ndt_covariance[0])) / normalization_coeff,
+      2);
+    ndt_covariance[7] = std::pow(
+      ((std::sqrt(in_ndt_covariance[7]) * 2) - std::sqrt(trusted_source_pose_.pose.covariance[7])) *
+        (std::sqrt(in_ndt_covariance[7])) / normalization_coeff,
+      2);
+    ndt_covariance[14] = std::pow(
+      ((std::sqrt(in_ndt_covariance[14]) * 2) -
+       std::sqrt(trusted_source_pose_.pose.covariance[14])) *
+        (std::sqrt(in_ndt_covariance[14])) / normalization_coeff,
+      2);
+
+    // ndt_rmse = std::max(ndt_rmse, ndt_min_rmse_meters);
+    ndt_covariance[0] = std::max(ndt_covariance[0], in_ndt_covariance[0]);
+    ndt_covariance[7] = std::max(ndt_covariance[7], in_ndt_covariance[7]);
+    ndt_covariance[14] = std::max(ndt_covariance[14], in_ndt_covariance[14]);
+
+    if (trustedPose.yaw_rmse <= std::sqrt(in_ndt_covariance[35])) {
+      ndt_covariance[35] = 1000000;
+    }
+  } else {
+    RCLCPP_INFO(this->get_logger(), "NDT input covariance values will be used ");
+  }
+
+  return ndt_covariance;
+}
 void NDTScanMatcher::publish_pose(
   const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg,
   const std::array<double, 36> & ndt_covariance, const bool is_converged)
@@ -547,9 +663,14 @@ void NDTScanMatcher::publish_pose(
   result_pose_with_cov_msg.pose.pose = result_pose_msg;
   result_pose_with_cov_msg.pose.covariance = ndt_covariance;
 
-  if (is_converged) {
+  if (activate_pose_covariance_modifier_ && close_ndt_pose_source_) {
     ndt_pose_pub_->publish(result_pose_stamped_msg);
-    ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg);
+    RCLCPP_INFO(this->get_logger(), "NDT pose with covariance topic closed.");
+  } else {
+    if (is_converged) {
+      ndt_pose_pub_->publish(result_pose_stamped_msg);
+      ndt_pose_with_covariance_pub_->publish(result_pose_with_cov_msg);
+    }
   }
 }