diff --git a/config/params.yaml b/config/params.yaml
index bd2e5d991ca6c67e6cab27a9b192206a3f0f7add..621fb7cd8c332ed549b236907aa40d80c39461c9 100644
--- a/config/params.yaml
+++ b/config/params.yaml
@@ -1,4 +1,4 @@
-rate: 120
+rate: 150
 debug: True
 fixed_frame: map
 base_link_frame: base_link
@@ -7,7 +7,7 @@ err_msg_rate: 0.5
 calculate_ang_vel: True
 watchdog_t: 1.0
 
-collision_timeout: 4.0
+collision_timeout: 5.0
 collision_acc_transition_counter_en: True
 collision_acc_th: 5.0
 collision_acc_counter_limit: 20
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index db49eddd68f4cd51cdb5bc5d1fb80f0dcdbc3f08..f762fe2f748f24a5020cf3a2bf53d1937a55abbb 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -152,7 +152,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
   if (this->new_imu_input_)
   {
-    if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to fixed frame coordinates
+    if (get_base_link_imu_data(blink_acc, blink_ang_vel))//Transform data to base_link frame coordinates
     {
       double acc_norm_2d = std::sqrt(std::pow(blink_acc(0),2) + std::pow(blink_acc(1),2));
       if (this->in_collision_)
@@ -256,7 +256,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                   }
                   else
                   {
-                    this->relocalization_msg_.front_icp_check = true;
                     this->relocalization_msg_.front_blink_pose.x = blink_pose(0);
                     this->relocalization_msg_.front_blink_pose.y = blink_pose(1);
                     this->relocalization_msg_.front_blink_pose.theta = blink_pose(2);
@@ -264,14 +263,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                       this->relocalization_msg_.front_pose_covariance[i] = blink_cov(i);
                     if (rotate_icp_to_blink(new_laser_pose, covariances, blink_icp, blink_icp_cov, true))
                     {
+                      this->relocalization_msg_.front_icp_check = true;
                       this->relocalization_msg_.front_icp_odom.x = blink_icp(0);
                       this->relocalization_msg_.front_icp_odom.y = blink_icp(1);
                       this->relocalization_msg_.front_icp_odom.theta = blink_icp(2);
                       for (unsigned int i=0; i<9; i++)
                         this->relocalization_msg_.front_odom_covariance[i] = blink_icp_cov(i);
                     }
-                    else
-                      this->relocalization_msg_.front_icp_check = false;
                   }
                 }
                 else
@@ -373,7 +371,6 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                   }
                   else
                   {
-                    this->relocalization_msg_.rear_icp_check = true;
                     this->relocalization_msg_.rear_blink_pose.x = blink_pose(0);
                     this->relocalization_msg_.rear_blink_pose.y = blink_pose(1);
                     this->relocalization_msg_.rear_blink_pose.theta = blink_pose(2);
@@ -381,14 +378,13 @@ void CollisionManagerAlgNode::mainNodeThread(void)
                       this->relocalization_msg_.rear_pose_covariance[i] = blink_cov(i);
                     if (rotate_icp_to_blink(new_laser_pose, covariances, blink_icp, blink_icp_cov, false))
                     {
+                      this->relocalization_msg_.rear_icp_check = true;
                       this->relocalization_msg_.rear_icp_odom.x = blink_icp(0);
                       this->relocalization_msg_.rear_icp_odom.y = blink_icp(1);
                       this->relocalization_msg_.rear_icp_odom.theta = blink_icp(2);
                       for (unsigned int i=0; i<9; i++)
                         this->relocalization_msg_.rear_odom_covariance[i] = blink_icp_cov(i);
                     }
-                    else
-                      this->relocalization_msg_.rear_icp_check = false;
                   }
                 }
                 else
@@ -437,7 +433,10 @@ void CollisionManagerAlgNode::mainNodeThread(void)
 
           //publish collision.
           this->collisions_publisher_.publish(this->collision_msg_);
-          this->relocalization_publisher_.publish(this->relocalization_msg_);
+          if (this->relocalization_msg_.front_icp_check || this->relocalization_msg_.rear_icp_check)
+            this->relocalization_publisher_.publish(this->relocalization_msg_);
+          else
+            ROS_WARN_STREAM("CollisionManagerAlgNode::mainNodeThread -> Posible collision detected but relocalization can't be calculated.");
         }
         else 
         {
@@ -445,7 +444,7 @@ void CollisionManagerAlgNode::mainNodeThread(void)
           if (acc_norm_2d > max_acc_norm_2d)
             this->max_acc_ = blink_acc;
           if (std::fabs(blink_ang_vel(2) - this->angular_vel_) > this->max_ang_vel_diff_)
-            this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2));
+            this->max_ang_vel_diff_ = std::fabs(blink_ang_vel(2) - this->angular_vel_);
 
           //Update angle rotated
           this->front_icp_srv_.request.rot_angle_estimation += blink_ang_vel(2)*(this->imu_stamp_ - this->last_imu_t_).toSec();
@@ -917,10 +916,11 @@ bool CollisionManagerAlgNode::rotate_icp_to_blink(const Eigen::Vector3d& _new_la
   Eigen::Vector3d aux;
   aux << _new_laser_pose(0), _new_laser_pose(1), 0.0;
   _blink_icp = blink_laser_rot*aux;
-  _blink_icp(2) = _new_laser_pose(2) + tf2::getYaw((_front? this->tf_blink_front_laser_: this->tf_blink_rear_laser_).getRotation());
+  if ((_front? this->z_axis_blink_front_laser_inv_: this->z_axis_blink_rear_laser_inv_))
+    _blink_icp(2) = _new_laser_pose(2)*-1.0;
+  else
+    _blink_icp(2) = _new_laser_pose(2);
   _blink_icp_cov = blink_laser_rot.rotation()*_covariances*blink_laser_rot.rotation().transpose();
-  ROS_INFO_STREAM("input " << _new_laser_pose);
-  ROS_INFO_STREAM("output " << _blink_icp);
   return true;
 }