diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2a0f509..f949b57 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -74,6 +74,10 @@ if (NOT TARGET robot_nav_2d_utils)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils) 
 endif()
 
+if (NOT TARGET laser_filter)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/laser_filter) 
+endif()
+
 if (NOT TARGET robot_nav_core)
     add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
 endif()
diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt
index ae8564e..f1af110 100644
--- a/src/Navigations/Packages/move_base/CMakeLists.txt
+++ b/src/Navigations/Packages/move_base/CMakeLists.txt
@@ -45,6 +45,7 @@ if (NOT BUILDING_WITH_CATKIN)
     robot_nav_2d_utils
     robot_cpp
     robot_move_base_msgs
+    laser_filter
   )
   find_library(TF3_LIBRARY NAMES tf3 PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu)
 else()
diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp
index 0b22f55..aef61aa 100644
--- a/src/Navigations/Packages/move_base/src/move_base.cpp
+++ b/src/Navigations/Packages/move_base/src/move_base.cpp
@@ -31,6 +31,7 @@
 #include <robot_sensor_msgs/PointCloud.h>
 #include <robot_sensor_msgs/PointCloud2.h>
 #include <robot_nav_2d_utils/conversions.h>
+#include <laser_filter/laser_filter.h>
 
 move_base::MoveBase::MoveBase()
   : initialized_(false),
@@ -499,13 +500,17 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin
 void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
 {
   auto it = laser_scans_.find(laser_scan_name);
+  laser_filter::LaserScanSOR sor;
+  sor.setMeanK(10);              // xét 10 điểm láng giềng gần nhất
+  sor.setStddevMulThresh(1.0);   // ngưỡng = mean + 1.0 * stddev
+  robot_sensor_msgs::LaserScan laser_scan_filter = sor.filter(laser_scan);
   if (it == laser_scans_.end())
   {
-    laser_scans_[laser_scan_name] = laser_scan;
+    laser_scans_[laser_scan_name] = laser_scan_filter;
   }
   else
   {
-    it->second = laser_scan;
+    it->second = laser_scan_filter;
   }
   // robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
   // robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
@@ -529,8 +534,8 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
   // }
   // robot::log_error("intensities: %s", intensities_str.str().c_str());
 
-  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
-  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
+  updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan_filter, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
 }
 
 robot_sensor_msgs::LaserScan move_base::MoveBase::getLaserScan(const std::string &laser_scan_name)