diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch
index d629c868..150be2e2 100644
--- a/launch/azimut3/az3_mapping_robot_nav.launch
+++ b/launch/azimut3/az3_mapping_robot_nav.launch
@@ -17,7 +17,8 @@
       <param name="subscribe_laserScan" type="bool" value="true"/>
       <param name="use_action_for_goal" type="bool" value="true"/>
       <param name="cloud_decimation" type="int" value="1"/>                    <!-- we already decimate in memory below -->
-	
+      <param name="grid_eroded" type="bool" value="true"/>
+
       <remap from="odom" to="/base_controller/odom"/>
       <remap from="scan" to="/base_scan"/>
       <remap from="mapData" to="mapData"/>
@@ -28,7 +29,7 @@
 
       <remap from="goal_out" to="current_goal"/>	
       <remap from="move_base" to="/planner/move_base"/>
-      <remap from="proj_map" to="/map"/>
+      <remap from="grid_map" to="/map"/>
 
       <!-- RTAB-Map's parameters -->
       <param name="RGBD/PoseScanMatching" type="string" value="true"/>           
@@ -43,7 +44,8 @@
       <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
       <param name="Mem/RehearsedNodesKept" type="string" value="false"/>
       <param name="Mem/ImageDecimation" type="string" value="4"/>
-
+      
+      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
       <param name="Rtabmap/TimeThr" type="string" value="600"/>
       <param name="Rtabmap/DetectionRate" type="string" value="1"/>
 
@@ -82,6 +84,7 @@
      <remap from="move_base_simple/goal" to="/planner_goal"/>
         
      <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
+        <param name="base_global_planner" value="navfn/NavfnROS"/>
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
      	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
     	<rosparam file="$(find rtabmap_ros)/launch/azimut3/config/local_costmap_params_laser.yaml" command="load" />
diff --git a/launch/azimut3/config/base_local_planner_params.yaml b/launch/azimut3/config/base_local_planner_params.yaml
index 3ead9a12..1384545d 100644
--- a/launch/azimut3/config/base_local_planner_params.yaml
+++ b/launch/azimut3/config/base_local_planner_params.yaml
@@ -17,11 +17,11 @@ TrajectoryPlannerROS:
   xy_goal_tolerance:  0.20
   yaw_goal_tolerance: 0.20
 
-  sim_time: 1
+  sim_time: 1.5
   sim_granularity: 0.025
   vx_samples: 3
-  vtheta_samples: 20
-  controller_frequency: 10
+  vtheta_samples: 30
+  controller_frequency: 20
 
   pdist_scale: 0.6
   gdist_scale: 0.8
@@ -30,4 +30,4 @@ TrajectoryPlannerROS:
   dwa: true
 
   oscillation_reset_dist: 0.05
-  meter_scoring: true
+  meter_scoring: false
diff --git a/launch/azimut3/config/costmap_common_params.yaml b/launch/azimut3/config/costmap_common_params.yaml
index 12f69fe0..b4f92b4f 100644
--- a/launch/azimut3/config/costmap_common_params.yaml
+++ b/launch/azimut3/config/costmap_common_params.yaml
@@ -4,12 +4,13 @@ footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
 footprint_padding: 0.03
 #robot_radius: ir_of_robot
 inflation_radius: 0.55
-transform_tolerance: 1
+transform_tolerance: 2
 
 controller_patience: 2.0
+#planner_frequency: 0.2
 
 NavfnROS:
-    allow_unknown: true
+    allow_unknown: false
 
 recovery_behaviors: [
     {name: conservative_clear, type: clear_costmap_recovery/ClearCostmapRecovery},
diff --git a/launch/azimut3/config/global_costmap_params.yaml b/launch/azimut3/config/global_costmap_params.yaml
index 78118723..f4e49595 100644
--- a/launch/azimut3/config/global_costmap_params.yaml
+++ b/launch/azimut3/config/global_costmap_params.yaml
@@ -1,6 +1,7 @@
 global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
-  update_frequency: 0.5
-  publish_frequency: 0.5
+  update_frequency: 1
+  publish_frequency: 1
   static_map: true
+  always_send_full_costmap: true
diff --git a/launch/azimut3/config/local_costmap_params.yaml b/launch/azimut3/config/local_costmap_params.yaml
index 40efc56e..10c8c0dd 100644
--- a/launch/azimut3/config/local_costmap_params.yaml
+++ b/launch/azimut3/config/local_costmap_params.yaml
@@ -11,15 +11,15 @@ local_costmap:
   origin_x: -2.0
   origin_y: -2.0
 
-  #observation_sources: laser_scan_sensor point_cloud_sensor
-  observation_sources: point_cloud_sensor
+  observation_sources: laser_scan_sensor point_cloud_sensor
+  #observation_sources: point_cloud_sensor
 
   laser_scan_sensor: {
     data_type: LaserScan, 
     topic: base_scan, 
     expected_update_rate: 0.2, 
     marking: true, 
-    clearing: true}
+    clearing: false}
 
   # assuming receiving a cloud from rtabmap/obstacles_detection node
   point_cloud_sensor: {
@@ -30,4 +30,4 @@ local_costmap:
     marking: true, 
     clearing: true,
     min_obstacle_height: -99999.0,
-    max_obstacle_height: 99999.0}
+    max_obstacle_height: 0.5}