diff --git a/modules/common/math/math_utils.cc b/modules/common/math/math_utils.cc
index 33bdb2f8cbe3e3b05cf948421710edb4fd5f8aa2..242a6bb44a7804f47741dcb7ecfabf4ef8b17877 100644
--- a/modules/common/math/math_utils.cc
+++ b/modules/common/math/math_utils.cc
@@ -52,16 +52,15 @@ double WrapAngle(const double angle) {
 }
 
 double NormalizeAngle(const double angle) {
-  const double new_angle = std::fmod(angle + M_PI, M_PI * 2.0);
-  return (new_angle < 0 ? new_angle + M_PI * 2.0 : new_angle) - M_PI;
+  double a = std::fmod(angle + M_PI, 2.0 * M_PI);
+  if (a < 0.0) {
+    a += (2.0 * M_PI);
+  }
+  return a - M_PI;
 }
 
 double AngleDiff(const double from, const double to) {
-  double angle = std::fmod((to - from) + M_PI, 2.0 * M_PI);
-  if (angle < 0.0) {
-    angle += (2.0 * M_PI);
-  }
-  return angle - M_PI;
+  return NormalizeAngle(to - from);
 }
 
 int RandomInt(const int s, const int t, unsigned int rand_seed) {
diff --git a/modules/common/math/math_utils_test.cc b/modules/common/math/math_utils_test.cc
index e54bc3b36ee749b534afea24fd293ce3dbfc928e..4ab21b45cad22a2513ab51941d4e6cd5b9a0e58d 100644
--- a/modules/common/math/math_utils_test.cc
+++ b/modules/common/math/math_utils_test.cc
@@ -53,6 +53,7 @@ TEST(MathUtilsTest, WrapAngle) {
 
 TEST(MathUtilsTest, NormalizeAngle) {
   EXPECT_DOUBLE_EQ(1.5, NormalizeAngle(1.5));
+  EXPECT_DOUBLE_EQ(1.5 - M_PI, NormalizeAngle(1.5 + M_PI));
   EXPECT_DOUBLE_EQ(1.5, NormalizeAngle(1.5 + M_PI * 2));
   EXPECT_DOUBLE_EQ(1.5, NormalizeAngle(1.5 - M_PI * 2));
   EXPECT_DOUBLE_EQ(-1.5, NormalizeAngle(-1.5));