diff --git a/src/homotopy_comparison/winding_angle.cpp b/src/homotopy_comparison/winding_angle.cpp
index a491422..cb8d4d2 100644
--- a/src/homotopy_comparison/winding_angle.cpp
+++ b/src/homotopy_comparison/winding_angle.cpp
@@ -101,7 +101,7 @@ namespace GuidancePlanner
             double angle = std::atan2(relative_position(1), relative_position(0)); // Current angle
 
             if (k > 1)
-                lambda += angle - prev_angle; // Relative angle
+                lambda += RosTools::angularDifference(prev_angle, angle); // angle - prev_angle, but safe
 
             prev_angle = angle;
         }