diff --git a/source/armarx/navigation/global_planning/optimization/math.h b/source/armarx/navigation/global_planning/optimization/math.h
index 86f1957128f51f9392d94e0e75a90dd458d8f4f9..2007edb3906c9ab58c03c8caaa8cf0bc4fc5b2e5 100644
--- a/source/armarx/navigation/global_planning/optimization/math.h
+++ b/source/armarx/navigation/global_planning/optimization/math.h
@@ -22,6 +22,7 @@
 #pragma once
 
 #include <array>
+
 #include <ceres/ceres.h>
 
 namespace armarx::navigation::global_planning::optimization
@@ -46,8 +47,7 @@ namespace armarx::navigation::global_planning::optimization
         return angleDiff;
     }
 
-    std::array<double, 2>
-    inline periodicDiffJacobian(const auto a, const auto b)
+    std::array<double, 2> inline periodicDiffJacobian(const auto a, const auto b)
     {
         const auto derivative = [&a, &b]() -> double
         {
@@ -56,6 +56,16 @@ namespace armarx::navigation::global_planning::optimization
                 return 0.;
             }
 
+
+            const double denomSquared =
+                1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b), 2);
+
+            // prevent division by 0
+            if(denomSquared < 0.001)
+            {
+                return 0.;
+            }
+
             return (ceres::sin(a) * ceres::cos(b) - ceres::cos(a) * ceres::sin(b)) /
                    (ceres::sqrt(
                        1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b),