diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
index f7b1786..cc91fa1 100644
--- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
+++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
@@ -784,7 +784,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
       const auto& p = global_plan.poses[i];
       const auto& dx = p.pose.x - p1.pose.x;
       const auto& dy = p.pose.y - p1.pose.y;
-      if(std::hypot(p.pose.x, p.pose.y) > costmap_robot_->getCostmap()->getResolution())
+      if(std::hypot(dx, dy) > costmap_robot_->getCostmap()->getResolution())
       {
         if(fabs(dx) < 1e-9 && fabs(dy) < 1e-9)
           continue;
@@ -795,9 +795,10 @@ bool mkt_algorithm::diff::PredictiveTrajectory::shouldRotateToPath(
   }
   
   // Whether we should rotate robot to rough path heading
-  if(sign_x < 0.0)
-    path_angle += std::copysign(M_PI, path_angle) * (-1.0);
-  angle_to_path = path_angle;
+  // if(sign_x < 0.0)
+  //   path_angle += std::copysign(M_PI, path_angle) * (-1.0);
+  // angle_to_path = path_angle;
+  angle_to_path = sign_x < 0.0 ? angles::normalize_angle(M_PI + path_angle) : path_angle;
   double heading_linear = sqrt(velocity.x * velocity.x + velocity.y * velocity.y);
   // The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
   double heading_rotate = rotate_to_heading_min_angle_;