diff --git a/CMakeLists.txt b/CMakeLists.txt
index d8210bc..e7136c7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -52,6 +52,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/brushed)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/bldc)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/control_loop/stepper)
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util/pid)
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/util/math)
 
 # Mocks
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/test/mocks)
diff --git a/control_loop/bldc/brushless_foc_control_loop.cpp b/control_loop/bldc/brushless_foc_control_loop.cpp
index 1a851d0..9164a61 100644
--- a/control_loop/bldc/brushless_foc_control_loop.cpp
+++ b/control_loop/bldc/brushless_foc_control_loop.cpp
@@ -2,6 +2,7 @@
 
 #include "math.h"
 #include "math_foc.hpp"
+#include "math_util.hpp"
 
 namespace control_loop {
 
diff --git a/control_loop/bldc/math_foc.cpp b/control_loop/bldc/math_foc.cpp
index 80c6546..894e0f7 100644
--- a/control_loop/bldc/math_foc.cpp
+++ b/control_loop/bldc/math_foc.cpp
@@ -1,6 +1,7 @@
 #include "math_foc.hpp"
 
 #include "math.h"
+#include "math_util.hpp"
 
 namespace math {
 
@@ -104,14 +105,4 @@ svpwm_duty_cycle_t svpwm(float Vd, float Vq, float theta_el, float Vbus) {
     return result;
 }
 
-float trapezoidal_integral(float x, float x_prev, float y, float y_prev) {
-    float integral = (x - x_prev) * (y + y_prev) / 2.0f;
-    return integral;
-}
-
-float low_pass_filter(float input, float prev_output, float tau, float dt) {
-    float alpha = dt / (tau + dt);
-    return alpha * input + (1.0f - alpha) * prev_output;
-}
-
 }  // namespace math
\ No newline at end of file
diff --git a/control_loop/bldc/math_foc.hpp b/control_loop/bldc/math_foc.hpp
index 58aad7b..7535e21 100644
--- a/control_loop/bldc/math_foc.hpp
+++ b/control_loop/bldc/math_foc.hpp
@@ -86,59 +86,6 @@ inverse_clarke_transform_result_t inverse_clarke_transform(float alpha, float be
  */
 svpwm_duty_cycle_t svpwm(float Vd, float Vq, float theta_el, float Vbus);
 
-/**
- * @brief Integrate the given value using the trapezoidal rule.
- * @param x The current x value
- * @param x_prev The previous x value
- * @param y The current y value
- * @param y_prev The previous y value
- */
-float trapezoidal_integral(float x, float x_prev, float y, float y_prev);
-
-/**
- * @brief Clamp the given value between the given min and max values.
- * @param value The value to clamp
- * @param min The minimum value
- * @param max The maximum value
- * @return void
- */
-template <typename T>
-void clamp(T& value, const T& min, const T& max) {
-    if (value < min) {
-        value = min;
-    } else if (value > max) {
-        value = max;
-    } else {
-        // do nothing
-    }
-}
-
-/**
- * @brief Wrap the given value around the given min and max values.
- * @param value The value to wrap
- * @param min The minimum value
- * @param max The maximum value
- * @return void
- */
-template <typename T>
-void wraparound(T& value, const T& min, const T& max) {
-    while (value < min) {
-        value += (max - min);
-    }
-    while (value > max) {
-        value -= (max - min);
-    }
-}
-
-/**
- * @brief Low pass filter the given input value.
- * @param input The input value
- * @param prev_output The previous output value
- * @param tau The time constant of the filter
- * @param dt The time step
- */
-float low_pass_filter(float input, float prev_output, float tau, float dt);
-
 }  // namespace math
 
 #endif  // MATH_CLARKE_PARKE_HPP
diff --git a/test/foc_math_test.cpp b/test/foc_math_test.cpp
index 992335e..7163f75 100644
--- a/test/foc_math_test.cpp
+++ b/test/foc_math_test.cpp
@@ -64,57 +64,4 @@ TEST(MathFOCTest, test_park_transform_45_degrees) {
     EXPECT_FLOAT_EQ(result.q, q_result);
 }
 
-// Test the wraparound function
-TEST(MathFocTest, test_wraparound) {
-    float angle = -0.4f;
-
-    // Wrap the angle around 0 -> 1. Expect the result to be 0.6
-    math::wraparound(angle, 0.0f, 1.0f);
-    EXPECT_FLOAT_EQ(angle, 0.6f);
-
-    // Make the angle 1.2. Expect the result to be 0.2
-    angle = 1.2f;
-    math::wraparound(angle, 0.0f, 1.0f);
-    EXPECT_FLOAT_EQ(angle, 0.2f);
-
-    // Make the angle 0.5. Expect the result to be 0.5
-    angle = 0.5f;
-    math::wraparound(angle, 0.0f, 1.0f);
-    EXPECT_FLOAT_EQ(angle, 0.5f);
-
-    // Make the angle 0.0. Expect the result to be 0.0
-    angle = 0.0f;
-    math::wraparound(angle, 0.0f, 1.0f);
-    EXPECT_FLOAT_EQ(angle, 0.0f);
-
-    // Make the angle -46.0. Expect the result to be 0.0
-    angle = -46.0f;
-    math::wraparound(angle, 0.0f, 1.0f);
-    EXPECT_FLOAT_EQ(angle, 0.0f);
-
-    // Make the angle 45.3. Expect the result to be 0.3
-    angle = 45.3001f;
-    math::wraparound(angle, 0.0f, 1.0f);
-    EXPECT_NEAR(angle, 0.3001f, 0.01);
-}
-
-// Test the trapezoidal integral function
-TEST(MathFOCTest, test_trapezoidal_integral) {
-    float x = 1.0f;
-    float x_prev = 0.0f;
-    float y = 1.0f;
-    float y_prev = 0.0f;
-
-    float result = math::trapezoidal_integral(x, x_prev, y, y_prev);
-    EXPECT_FLOAT_EQ(result, 0.5f);
-
-    // Test it now with x = 2.0f, x_prev = 1.0f, y = -1.0f, y_prev = 1.0f
-    x = 2.0f;
-    x_prev = 1.0f;
-    y = -1.0f;
-    y_prev = 1.0f;
-    result = math::trapezoidal_integral(x, x_prev, y, y_prev);
-    EXPECT_FLOAT_EQ(result, 0.0f);
-}
-
 }  // namespace math
\ No newline at end of file
diff --git a/test/math_util_test.cpp b/test/math_util_test.cpp
new file mode 100644
index 0000000..0a7cbc3
--- /dev/null
+++ b/test/math_util_test.cpp
@@ -0,0 +1,61 @@
+#include "math_util.hpp"
+
+#include "gtest/gtest.h"
+#include "math.h"
+
+namespace math {
+
+// Test the wraparound function
+TEST(MathUtilTest, test_wraparound) {
+    float angle = -0.4f;
+
+    // Wrap the angle around 0 -> 1. Expect the result to be 0.6
+    math::wraparound(angle, 0.0f, 1.0f);
+    EXPECT_FLOAT_EQ(angle, 0.6f);
+
+    // Make the angle 1.2. Expect the result to be 0.2
+    angle = 1.2f;
+    math::wraparound(angle, 0.0f, 1.0f);
+    EXPECT_FLOAT_EQ(angle, 0.2f);
+
+    // Make the angle 0.5. Expect the result to be 0.5
+    angle = 0.5f;
+    math::wraparound(angle, 0.0f, 1.0f);
+    EXPECT_FLOAT_EQ(angle, 0.5f);
+
+    // Make the angle 0.0. Expect the result to be 0.0
+    angle = 0.0f;
+    math::wraparound(angle, 0.0f, 1.0f);
+    EXPECT_FLOAT_EQ(angle, 0.0f);
+
+    // Make the angle -46.0. Expect the result to be 0.0
+    angle = -46.0f;
+    math::wraparound(angle, 0.0f, 1.0f);
+    EXPECT_FLOAT_EQ(angle, 0.0f);
+
+    // Make the angle 45.3. Expect the result to be 0.3
+    angle = 45.3001f;
+    math::wraparound(angle, 0.0f, 1.0f);
+    EXPECT_NEAR(angle, 0.3001f, 0.01);
+}
+
+// Test the trapezoidal integral function
+TEST(MathUtilTest, test_trapezoidal_integral) {
+    float x = 1.0f;
+    float x_prev = 0.0f;
+    float y = 1.0f;
+    float y_prev = 0.0f;
+
+    float result = math::trapezoidal_integral(x, x_prev, y, y_prev);
+    EXPECT_FLOAT_EQ(result, 0.5f);
+
+    // Test it now with x = 2.0f, x_prev = 1.0f, y = -1.0f, y_prev = 1.0f
+    x = 2.0f;
+    x_prev = 1.0f;
+    y = -1.0f;
+    y_prev = 1.0f;
+    result = math::trapezoidal_integral(x, x_prev, y, y_prev);
+    EXPECT_FLOAT_EQ(result, 0.0f);
+}
+
+}  // namespace math
\ No newline at end of file
diff --git a/util/math/math_util.cpp b/util/math/math_util.cpp
new file mode 100644
index 0000000..033f395
--- /dev/null
+++ b/util/math/math_util.cpp
@@ -0,0 +1,13 @@
+#include "math_util.hpp"
+
+namespace math {
+float low_pass_filter(float input, float prev_output, float tau, float dt) {
+    float alpha = dt / (tau + dt);
+    return alpha * input + (1.0f - alpha) * prev_output;
+}
+
+float trapezoidal_integral(float x, float x_prev, float y, float y_prev) {
+    float integral = (x - x_prev) * (y + y_prev) / 2.0f;
+    return integral;
+}
+}  // namespace math
\ No newline at end of file
diff --git a/util/math/math_util.hpp b/util/math/math_util.hpp
new file mode 100644
index 0000000..4edd82e
--- /dev/null
+++ b/util/math/math_util.hpp
@@ -0,0 +1,59 @@
+#ifndef MATH_UTIL_HPP
+#define MATH_UTIL_HPP
+
+namespace math {
+/**
+ * @brief Integrate the given value using the trapezoidal rule.
+ * @param x The current x value
+ * @param x_prev The previous x value
+ * @param y The current y value
+ * @param y_prev The previous y value
+ */
+float trapezoidal_integral(float x, float x_prev, float y, float y_prev);
+
+/**
+ * @brief Clamp the given value between the given min and max values.
+ * @param value The value to clamp
+ * @param min The minimum value
+ * @param max The maximum value
+ * @return void
+ */
+template <typename T>
+void clamp(T& value, const T& min, const T& max) {
+    if (value < min) {
+        value = min;
+    } else if (value > max) {
+        value = max;
+    } else {
+        // do nothing
+    }
+}
+
+/**
+ * @brief Wrap the given value around the given min and max values.
+ * @param value The value to wrap
+ * @param min The minimum value
+ * @param max The maximum value
+ * @return void
+ */
+template <typename T>
+void wraparound(T& value, const T& min, const T& max) {
+    while (value < min) {
+        value += (max - min);
+    }
+    while (value > max) {
+        value -= (max - min);
+    }
+}
+
+/**
+ * @brief Low pass filter the given input value.
+ * @param input The input value
+ * @param prev_output The previous output value
+ * @param tau The time constant of the filter
+ * @param dt The time step
+ */
+float low_pass_filter(float input, float prev_output, float tau, float dt);
+}  // namespace math
+
+#endif  // MATH_UTIL_HPP
diff --git a/util/pid/pid.cpp b/util/pid/pid.cpp
index 0c9aed6..f7edd33 100644
--- a/util/pid/pid.cpp
+++ b/util/pid/pid.cpp
@@ -1,6 +1,6 @@
 #include "pid.hpp"
 
-#include "math_foc.hpp"
+#include "math_util.hpp"
 
 namespace pid {