diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index fa1dcc62c4..a7d74332a8 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -167,7 +167,8 @@ def calc_to_goal_cost(traj, goal, config):
     dx = goal[0] - traj[-1, 0]
     dy = goal[1] - traj[-1, 1]
     error_angle = math.atan2(dy, dx)
-    cost = abs(error_angle - traj[-1, 2])
+    cost_angle = error_angle - traj[-1, 2]
+    cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))
 
     return cost