diff --git a/motion/thruster_allocator/CMakeLists.txt b/motion/thruster_allocator/CMakeLists.txt
index de9e0920..dfbb544a 100644
--- a/motion/thruster_allocator/CMakeLists.txt
+++ b/motion/thruster_allocator/CMakeLists.txt
@@ -30,7 +30,7 @@ ament_target_dependencies(${PROJECT_NAME}_node
   geometry_msgs 
   vortex_msgs
   Eigen3
-  )
+)
 
 install(
   DIRECTORY include/
@@ -47,4 +47,9 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+find_package(GTest REQUIRED)
+include(GoogleTest)
+enable_testing()
+add_subdirectory(test)
+
 ament_package()
diff --git a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
index 5c861b0b..843bdd63 100644
--- a/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
+++ b/motion/thruster_allocator/include/thruster_allocator/allocator_utils.hpp
@@ -8,7 +8,6 @@
 #define VORTEX_ALLOCATOR_ALLOCATOR_UTILS_HPP
 
 #include <eigen3/Eigen/Eigen>
-#include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <vector>
 
@@ -70,17 +69,17 @@ inline void calculateRightPseudoinverse(const Eigen::MatrixXd &M,
  * otherwise.
  */
 inline bool saturateVectorValues(Eigen::VectorXd &vec, double min, double max) {
-  bool vector_in_range = std::all_of(vec.begin(), vec.end(), [&](double &val) {
-    if (val > max) {
-      val = max;
-      return false;
-    } else if (val < min) {
-      val = min;
-      return false;
-    }
-    return true;
+  bool all_values_in_range =  std::all_of(vec.begin(), vec.end(),
+                                          [min, max](double val) {
+    return val >= min && val <= max;
   });
-  return vector_in_range;
+
+  std::transform(vec.begin(), vec.end(), vec.begin(),
+                 [min, max](double val) {
+      return std::min(std::max(val, min), max);
+  });
+
+  return all_values_in_range;
 }
 
 /**
diff --git a/motion/thruster_allocator/test/CMakeLists.txt b/motion/thruster_allocator/test/CMakeLists.txt
new file mode 100644
index 00000000..7abd1b64
--- /dev/null
+++ b/motion/thruster_allocator/test/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.10)
+
+find_package(GTest REQUIRED)
+
+set(TEST_EXECUTABLE thruster_allocator_tester)
+
+set(TEST_SOURCES
+    ${CMAKE_CURRENT_SOURCE_DIR}/test_allocator_utils.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test_pseudoinverse_allocator.cpp
+    ${CMAKE_SOURCE_DIR}/src/pseudoinverse_allocator.cpp
+)
+
+# Create test executable
+add_executable(${TEST_EXECUTABLE} ${TEST_SOURCES})
+
+# Link libraries
+target_link_libraries(${TEST_EXECUTABLE} ${googletest_LIBRARIES}
+                      GTest::GTest GTest::Main)
+
+ament_target_dependencies(${TEST_EXECUTABLE}
+    vortex_msgs
+    Eigen3
+)
+
+target_include_directories(
+    ${TEST_EXECUTABLE} PUBLIC $<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>
+                            $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+gtest_discover_tests(${TEST_EXECUTABLE})
diff --git a/motion/thruster_allocator/test/test_allocator_utils.cpp b/motion/thruster_allocator/test/test_allocator_utils.cpp
new file mode 100644
index 00000000..6cb02095
--- /dev/null
+++ b/motion/thruster_allocator/test/test_allocator_utils.cpp
@@ -0,0 +1,96 @@
+#include <gtest/gtest.h>
+
+#include "thruster_allocator/allocator_utils.hpp"
+
+class AllocatorUtilsTest : public ::testing::Test {
+protected:
+};
+
+TEST_F(AllocatorUtilsTest, IsInvalidMatrix) {
+    Eigen::MatrixXd valid_matrix = Eigen::MatrixXd::Identity(3, 3);
+    ASSERT_FALSE(isInvalidMatrix(valid_matrix));
+    
+    Eigen::MatrixXd invalid_matrix(3, 3);
+    invalid_matrix << 1, std::nan(""), 3, 4, 5, 6, 7, 8, 9;
+    ASSERT_TRUE(isInvalidMatrix(invalid_matrix));
+}
+
+TEST_F(AllocatorUtilsTest, CalculateRightPseudoinverseNoThrow) {
+    Eigen::MatrixXd T = Eigen::MatrixXd::Random(3, 3);
+    Eigen::MatrixXd pinv;
+    ASSERT_NO_THROW(calculateRightPseudoinverse(T, pinv));
+}
+
+TEST_F(AllocatorUtilsTest, CalculateRightPseudoinverseIdentity) {
+    constexpr int size = 3;
+    Eigen::MatrixXd T = Eigen::MatrixXd::Identity(size, size);
+    Eigen::MatrixXd pinv;
+    calculateRightPseudoinverse(T, pinv);
+
+    constexpr double tolerance = 1e-6;
+    EXPECT_TRUE(pinv.isApprox(T, tolerance));
+}
+
+TEST_F(AllocatorUtilsTest, CalculateRightPseudoinverseSomeMatrix) {
+    constexpr int rows = 3;
+    constexpr int cols = 4;
+
+    Eigen::MatrixXd T(rows, cols);
+    T << 0.70711, 0.70711,  0.70711,  0.70711, 
+        -0.70711, 0.70711, -0.70711,  0.70711, 
+         0.27738, 0.27738, -0.27738, -0.27738;
+
+    Eigen::MatrixXd T_pinv;
+    calculateRightPseudoinverse(T, T_pinv);
+
+    Eigen::MatrixXd T_expected(cols, rows);
+    T_expected << 0.353552, -0.353552,  0.901291, 
+                  0.353552,  0.353552,  0.901291,
+                  0.353552, -0.353552, -0.901291,
+                  0.353552,  0.353552, -0.901291;
+
+    constexpr double tolerance = 1e-6;
+    EXPECT_TRUE(T_pinv.isApprox(T_expected, tolerance));
+}
+
+TEST_F(AllocatorUtilsTest, SaturateVectorValues) {
+    Eigen::VectorXd vec(3);
+    vec << -1, 5, 0;
+    ASSERT_FALSE(saturateVectorValues(vec, 0, 1));
+
+    Eigen::VectorXd expected_output(3);
+    expected_output << 0, 1, 0; 
+    constexpr double tolerance = 1e-6;
+
+    ASSERT_TRUE(vec.isApprox(expected_output, tolerance));
+}
+
+TEST_F(AllocatorUtilsTest, ArrayEigenToMsg) {
+    Eigen::VectorXd u = Eigen::VectorXd::Random(3);
+    vortex_msgs::msg::ThrusterForces msg;
+    arrayEigenToMsg(u, msg);
+
+    constexpr double tolerance = 1e-6;
+    bool isEqual = std::equal(u.begin(), u.end(), msg.thrust.begin(), 
+                              [](double elem1, double elem2) {
+                                  return std::abs(elem1 - elem2) < tolerance; 
+                              });
+
+    ASSERT_TRUE(isEqual);
+}
+
+TEST_F(AllocatorUtilsTest, DoubleArrayToEigenMatrix) {
+    std::vector<double> array = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0};
+    constexpr int rows = 2;
+    constexpr int cols = 3;
+    Eigen::MatrixXd T = doubleArrayToEigenMatrix(array, rows, cols);
+    ASSERT_EQ(T.rows(), rows);
+    ASSERT_EQ(T.cols(), cols);
+
+    Eigen::MatrixXd T_expected(rows, cols);
+    T_expected << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
+
+    constexpr double tolerance = 1e-6;
+    ASSERT_TRUE(T.isApprox(T_expected, tolerance));
+
+}
\ No newline at end of file
diff --git a/motion/thruster_allocator/test/test_pseudoinverse_allocator.cpp b/motion/thruster_allocator/test/test_pseudoinverse_allocator.cpp
new file mode 100644
index 00000000..e42a6b26
--- /dev/null
+++ b/motion/thruster_allocator/test/test_pseudoinverse_allocator.cpp
@@ -0,0 +1,55 @@
+#include <gtest/gtest.h>
+
+#include "thruster_allocator/pseudoinverse_allocator.hpp"
+
+class PseudoinverseTest : public ::testing::Test {
+};
+
+TEST_F(PseudoinverseTest, ZeroPinvThenZeroThrust) {
+    constexpr int size = 3;
+    Eigen::MatrixXd T = Eigen::MatrixXd::Zero(size, size);
+    PseudoinverseAllocator allocator(T);
+
+    Eigen::VectorXd input_thrust = Eigen::VectorXd::Constant(size, 10.0);
+    Eigen::VectorXd calculated_thrust = allocator.calculateAllocatedThrust(input_thrust);
+
+    Eigen::VectorXd expected_thrust = Eigen::VectorXd::Zero(size);
+    
+    EXPECT_EQ(calculated_thrust, expected_thrust);
+}
+
+TEST_F(PseudoinverseTest, IdentityPinvThenSameThrust) {
+    constexpr int size = 3;
+    Eigen::MatrixXd T = Eigen::MatrixXd::Identity(size, size);
+    PseudoinverseAllocator allocator(T);
+
+    Eigen::VectorXd input_thrust = Eigen::VectorXd::Constant(size, 10.0);
+    Eigen::VectorXd calculated_thrust = allocator.calculateAllocatedThrust(input_thrust);
+
+    EXPECT_EQ(calculated_thrust, input_thrust);
+}
+
+TEST_F(PseudoinverseTest, SomePinvThenSameThrust) {
+    constexpr int rows = 3;
+    constexpr int cols = 4;
+
+    Eigen::MatrixXd T(rows, cols);
+    T << 0.70711, 0.70711, 0.70711, 0.70711, 
+          -0.70711, 0.70711, -0.70711, 0.70711, 
+          0.27738, 0.27738, -0.27738, -0.27738;
+
+    Eigen::MatrixXd T_pinv = T.transpose() * (T * T.transpose()).inverse();
+
+    PseudoinverseAllocator allocator(T_pinv);
+
+    Eigen::VectorXd input_thrust(rows);
+    input_thrust << 10.0, 0.0, 0.0;
+
+    Eigen::VectorXd allocated_thrust = allocator.calculateAllocatedThrust(input_thrust);
+
+    Eigen::VectorXd expected_thrust(cols);
+    expected_thrust << 3.53552, 3.53552, 3.53552, 3.53552;
+    constexpr double tolerance = 1e-6;
+    
+    EXPECT_TRUE(allocated_thrust.isApprox(expected_thrust, tolerance));
+}