diff --git a/launch/azimut3/az3_mapping_robot_nav.launch b/launch/azimut3/az3_mapping_robot_nav.launch
index 70822a35..d629c868 100644
--- a/launch/azimut3/az3_mapping_robot_nav.launch
+++ b/launch/azimut3/az3_mapping_robot_nav.launch
@@ -14,9 +14,9 @@
   <group ns="rtabmap">
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
       <param name="frame_id" type="string" value="base_footprint"/>
-	
-      <param name="subscribe_depth" type="bool" value="true"/>
       <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 -->
 	
       <remap from="odom" to="/base_controller/odom"/>
       <remap from="scan" to="/base_scan"/>
@@ -30,44 +30,37 @@
       <remap from="move_base" to="/planner/move_base"/>
       <remap from="proj_map" to="/map"/>
 
-      <param name="use_action_for_goal" type="bool" value="true"/>
-      <param name="queue_size" type="int" value="10"/>
-
-      <param name="cloud_decimation" type="int" value="1"/>
-	
+      <!-- RTAB-Map's parameters -->
       <param name="RGBD/PoseScanMatching" type="string" value="true"/>           
       <param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
   
       <param name="LccIcp/Type" type="string" value="2"/>                    
-      <param name="LccIcp2/Iterations" type="string" value="100"/>
-      <param name="LccIcp2/VoxelSize" type="string" value="0"/>
 	 
-      <param name="RGBD/AngularUpdate" type="string" value="0.01"/>             <!-- Update map only if the robot is moving -->
-      <param name="RGBD/LinearUpdate" type="string" value="0.01"/>              <!-- Update map only if the robot is moving -->
+      <param name="RGBD/AngularUpdate" type="string" value="0.1"/>             <!-- Update map only if the robot is moving -->
+      <param name="RGBD/LinearUpdate" type="string" value="0.1"/>              <!-- Update map only if the robot is moving -->
       <param name="RGBD/LocalRadius" type="string" value="5"/>
 	  
       <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="Mem/LocalSpaceLinksKeptInWM" type="string" value="true"/>
 
-      <param name="Rtabmap/TimeThr" type="string" value="700"/>
+      <param name="Rtabmap/TimeThr" type="string" value="600"/>
       <param name="Rtabmap/DetectionRate" type="string" value="1"/>
 
-      <param name="RGBD/ToroIterations" type="string" value="100"/>
+      <param name="Bayes/PredictionLC" type="string" value="0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.00035"/>
+
+      <param name="RGBD/OptimizeSlam2d" type="string" value="true"/>
+      <param name="RGBD/OptimizeIterations" type="string" value="100"/>
       <param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
 	 
-      <param name="Kp/TfIdfLikelihoodUsed" type="string" value="true"/>
       <param name="Kp/DetectorStrategy" type="string" value="0"/>
       <param name="Kp/WordsPerImage" type="string" value="200"/>
       <param name="Kp/NNStrategy" type="string" value="1"/>
-	  
-      <param name="Bayes/FullPredictionUpdate" type="string" value="false"/>
-	  
-      <param name="SURF/HessianThreshold" type="string" value="1000"/>
+
+      <param name="SURF/HessianThreshold" type="string" value="500"/>
 	  
       <param name="LccBow/Force2D" type="string" value="true"/>
-      <param name="LccBow/MaxDepth" type="string" value="4"/>
+      <param name="LccBow/MaxDepth" type="string" value="5"/>
       <param name="LccBow/MinInliers" type="string" value="5"/>
       <param name="LccBow/InlierDistance" type="string" value="0.1"/>
     </node>
@@ -88,7 +81,7 @@
      <remap from="map" to="/map"/>
      <remap from="move_base_simple/goal" to="/planner_goal"/>
         
-     <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+     <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
     	<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" />
@@ -126,10 +119,10 @@
 
     <!-- for the planner -->
     <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
-      <remap from="depth/image"     to="data_resized_image_depth"/>
-      <remap from="depth/camera_info" to="data_resized_camera_info"/>
-      <remap from="cloud"           to="/local_costmap_cloud" />
-      <param name="decimation" type="int" value="1"/>
+      <remap from="depth/image"            to="data_resized_image_depth"/>
+      <remap from="depth/camera_info"      to="data_resized_camera_info"/>
+      <remap from="cloud"                  to="/local_costmap_cloud" />
+      <param name="decimation" type="int" value="1"/>                     <!-- already decimated above -->
       <param name="max_depth" type="double" value="6.0"/>
       <param name="noise_filter_radius" type="double" value="0.05"/>
       <param name="noise_filter_min_neighbors" type="int" value="10"/>
diff --git a/launch/azimut3/config/base_local_planner_params.yaml b/launch/azimut3/config/base_local_planner_params.yaml
index 0fd03f7b..3ead9a12 100644
--- a/launch/azimut3/config/base_local_planner_params.yaml
+++ b/launch/azimut3/config/base_local_planner_params.yaml
@@ -8,23 +8,23 @@ TrajectoryPlannerROS:
   # minimal distance of 0.48 m. 
   # Basically, max_rotational_vel * rho_min <= min_vel_x
   max_vel_x:  0.700
-  min_vel_x:  0.212
-  max_rotational_vel: 0.550
+  min_vel_x:  0.24
+  max_rotational_vel: 0.5
   min_in_place_rotational_vel: 0.15
   escape_vel: -0.10
-  holonomic_robot: false
+  holonomic_robot: true
 
   xy_goal_tolerance:  0.20
   yaw_goal_tolerance: 0.20
 
-  sim_time: 1.7
+  sim_time: 1
   sim_granularity: 0.025
   vx_samples: 3
-  vtheta_samples: 3
   vtheta_samples: 20
+  controller_frequency: 10
 
-  goal_distance_bias: 0.8
-  path_distance_bias: 0.6
+  pdist_scale: 0.6
+  gdist_scale: 0.8
   occdist_scale: 0.01
   heading_lookahead: 0.325
   dwa: true
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 7bd40472..a4021a64 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -1070,7 +1070,9 @@ void CoreWrapper::process(
 						bool lastPoseModified = false;
 						if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first && rtabmap_.getLocalOptimizedPoses().size())
 						{
-							if(latestNodeWasReached_ || rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius())
+							if(latestNodeWasReached_ ||
+							   rtabmap_.getLocalOptimizedPoses().rbegin()->second.getDistance(currentMetricGoal_) < rtabmap_.getGoalReachedRadius() ||
+							   rtabmap_.getPathTransformToGoal().getNorm() < rtabmap_.getGoalReachedRadius())
 							{
 								if(!latestNodeWasReached_)
 								{
@@ -2372,7 +2374,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
 		{
 			if(rtabmap_.getPath().size() &&
 				rtabmap_.getPathCurrentGoalId() != rtabmap_.getPath().back().first &&
-				!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first))
+				(!uContains(rtabmap_.getLocalOptimizedPoses(), rtabmap_.getPath().back().first) || !latestNodeWasReached_))
 			{
 				ROS_WARN("Planning: move_base reached current goal but it is not "
 						 "the last one planned by rtabmap. A new goal should be sent when "