diff --git a/.gitignore b/.gitignore
index 9af41aca7..39a83c698 100644
--- a/.gitignore
+++ b/.gitignore
@@ -58,3 +58,8 @@ qtcreator-*
 .#*
 
 # End of https://www.gitignore.io/api/ros
+control/pid_controller_dp/test/test_main.cpp
+control/pid_controller_dp/test/test_pid_basic.cpp
+control/pid_controller_dp/test/test_pid_controller.cpp
+control/pid_controller_dp/test/test_type_casting.cpp
+scripts/ci_install_dependencies.sh
diff --git a/control/pid_controller_dp/CMakeLists.txt b/control/pid_controller_dp/CMakeLists.txt
index fc9d851d9..409bce891 100644
--- a/control/pid_controller_dp/CMakeLists.txt
+++ b/control/pid_controller_dp/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8)
 project(pid_controller_dp)
 
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD 20)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -16,17 +16,44 @@ find_package(geometry_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(vortex_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(fmt REQUIRED)
 
 include_directories(include)
+set(LIB_NAME ${PROJECT_NAME}_lib)
 
-add_executable(pid_controller_node
-  src/pid_controller_node.cpp
-  src/pid_controller_ros.cpp
+add_library(${LIB_NAME} SHARED
   src/pid_controller.cpp
   src/pid_controller_utils.cpp
   src/pid_controller_conversions.cpp
 )
 
+ament_target_dependencies(${LIB_NAME} PUBLIC
+  rclcpp
+  geometry_msgs
+  nav_msgs
+  Eigen3
+  tf2
+  vortex_msgs
+  rcl_interfaces
+  vortex_utils
+  spdlog
+  fmt
+)
+
+
+install(TARGETS
+  ${LIB_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+add_executable(pid_controller_node
+  src/pid_controller_node.cpp
+  src/pid_controller_ros.cpp
+)
+
 ament_target_dependencies(pid_controller_node
   rclcpp
   geometry_msgs
@@ -34,6 +61,26 @@ ament_target_dependencies(pid_controller_node
   Eigen3
   tf2
   vortex_msgs
+  rcl_interfaces
+  vortex_utils
+  spdlog
+  fmt
+)
+
+target_link_libraries(
+    pid_controller_node
+    ${LIB_NAME}
+    spdlog::spdlog
+    # vortex_utilis::vortex_utils
+)
+
+ament_export_targets(export_${LIB_NAME})
+
+install(TARGETS ${LIB_NAME}
+  EXPORT export_${LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
 install(TARGETS
@@ -46,4 +93,9 @@ install(DIRECTORY
   DESTINATION share/${PROJECT_NAME}/
 )
 
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
 ament_package()
diff --git a/control/pid_controller_dp/config/pid_params.yaml b/control/pid_controller_dp/config/pid_params.yaml
index 2a77ffd59..c6efcb8bd 100644
--- a/control/pid_controller_dp/config/pid_params.yaml
+++ b/control/pid_controller_dp/config/pid_params.yaml
@@ -1,5 +1,23 @@
 /**:
   ros__parameters:
-    Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0]
-    Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12]
-    Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0]
+    # Kp: [70.0, 70.0, 70.0, 12.0, 12.0, 12.0]
+    # Ki: [2.0, 2.0, 2.0, 0.12, 0.12, 0.12]
+    # Kd: [10.0, 10.0, 10.0, 4.0, 5.0, 4.0]
+    Kp_x: 20.0
+    Kp_y: 0.0
+    Kp_z: 50.0
+    Kp_roll: 0.0
+    Kp_pitch: 0.0
+    Kp_yaw: 0.0
+    Ki_x: 0.08
+    Ki_y: 0.0
+    Ki_z: 0.0
+    Ki_roll: 0.0
+    Ki_pitch: 0.0
+    Ki_yaw: 0.0
+    Kd_x: 0.0
+    Kd_y: 0.0
+    Kd_z: 0.0
+    Kd_roll: 0.0
+    Kd_pitch: 0.0
+    Kd_yaw: 0.0
diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp
index 1017b5bd6..bd3c310de 100644
--- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp
+++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller.hpp
@@ -1,6 +1,7 @@
 #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_HPP_
 #define PID_CONTROLLER_DP__PID_CONTROLLER_HPP_
 
+#include <spdlog/spdlog.h>
 #include "pid_controller_dp/typedefs.hpp"
 
 class PIDController {
@@ -36,6 +37,25 @@ class PIDController {
     // @param dt: Time step
     void set_time_step(double dt);
 
+    types::Matrix6d get_kp();
+    types::Matrix6d get_ki();
+    types::Matrix6d get_kd();
+
+    // parameters for debug
+    types::Eta eta_error_debug;
+    types::Vector6d nu_d_debug;
+    types::Vector6d error_nu_debug;
+    types::Vector6d P_debug;
+    types::Vector6d I_debug;
+    types::Vector6d D_debug;
+    types::Vector6d tau_debug;
+    types::Matrix6x7d J_inv_debug;
+
+    // debug gain
+    types::Matrix6d Kp_debug;
+    types::Matrix6d Ki_debug;
+    types::Matrix6d Kd_debug;
+
    private:
     types::Matrix6d Kp_;
     types::Matrix6d Ki_;
diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp
index 128c8650c..599c4699a 100644
--- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp
+++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_ros.hpp
@@ -7,6 +7,7 @@
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/float64_multi_array.hpp>
@@ -56,6 +57,12 @@ class PIDControllerNode : public rclcpp::Node {
     void guidance_callback(
         const vortex_msgs::msg::ReferenceFilter::SharedPtr msg);
 
+    // TODO: parameter callback for dynamic reconfigure of PID gains
+    //@brief Callback function for parameter updates
+    // @param parameters: vector of parameters to be set
+    rcl_interfaces::msg::SetParametersResult parametersCallback(
+        const std::vector<rclcpp::Parameter>& parameters);
+
     PIDController pid_controller_;
 
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr killswitch_sub_;
@@ -94,6 +101,8 @@ class PIDControllerNode : public rclcpp::Node {
     bool killswitch_on_;
 
     std::string software_mode_;
+
+    OnSetParametersCallbackHandle::SharedPtr callback_handle_;
 };
 
 #endif  // PID_CONTROLLER_DP__PID_CONTROLLER_ROS_HPP_
diff --git a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp
index 1bdce214b..a199033f4 100644
--- a/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp
+++ b/control/pid_controller_dp/include/pid_controller_dp/pid_controller_utils.hpp
@@ -1,12 +1,15 @@
 #ifndef PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_
 #define PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_
 
+#include <spdlog/spdlog.h>
 #include <tf2/LinearMath/Matrix3x3.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <cmath>
 #include <eigen3/Eigen/Geometry>
 #include <std_msgs/msg/float64_multi_array.hpp>
+#include <vortex/utils/types.hpp>
 #include "pid_controller_dp/typedefs.hpp"
+#include "typedefs.hpp"
 
 // @brief Calculate the sine of an angle in degrees
 // @param angle: Angle in degrees
@@ -71,4 +74,6 @@ types::Vector7d anti_windup(const double dt,
                             const types::Eta& error,
                             const types::Vector7d& integral);
 
+// void print_J_transformation(const types::J_transformation& J);
+// void print_Jinv_transformation(const types::Matrix6x7d& J_inv);
 #endif  // PID_CONTROLLER_DP__PID_CONTROLLER_UTILS_HPP_
diff --git a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp
index beeea3174..0fe90cc90 100644
--- a/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp
+++ b/control/pid_controller_dp/include/pid_controller_dp/typedefs.hpp
@@ -7,48 +7,31 @@
 #define PID_CONTROLLER_DP__TYPEDEFS_HPP_
 
 #include <eigen3/Eigen/Dense>
+#include <vortex/utils/types.hpp>
 
 namespace types {
-typedef Eigen::Matrix<double, 3, 1> Vector3d;
-typedef Eigen::Matrix<double, 6, 1> Vector6d;
-typedef Eigen::Matrix<double, 7, 1> Vector7d;
-typedef Eigen::Matrix<double, 4, 1> Vector4d;
-typedef Eigen::Matrix<double, 6, 6> Matrix6d;
-typedef Eigen::Matrix<double, 3, 3> Matrix3d;
-typedef Eigen::Matrix<double, 4, 3> Matrix4x3d;
-typedef Eigen::Matrix<double, 7, 6> Matrix7x6d;
-typedef Eigen::Matrix<double, 6, 7> Matrix6x7d;
-typedef Eigen::Matrix<double, 7, 7> Matrix7d;
-typedef Eigen::Quaterniond Quaterniond;
-
-struct Eta {
-    Eigen::Vector3d pos = Eigen::Vector3d::Zero();
-    Eigen::Quaterniond ori = Eigen::Quaterniond::Identity();
-
-    types::Vector7d as_vector() const {
-        types::Vector7d vec;
-        vec << pos, ori.w(), ori.x(), ori.y(), ori.z();
-        return vec;
-    }
-};
-
-struct Nu {
-    Eigen::Vector3d linear_speed = types::Vector3d::Zero();
-    Eigen::Vector3d angular_speed = types::Vector3d::Zero();
-
-    types::Vector6d as_vector() const {
-        types::Vector6d vec;
-        vec << linear_speed, angular_speed;
-        return vec;
-    }
-};
+using Vector3d = Eigen::Matrix<double, 3, 1>;
+using Vector4d = Eigen::Matrix<double, 4, 1>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+using Vector7d = Eigen::Matrix<double, 7, 1>;
+using Matrix3d = Eigen::Matrix<double, 3, 3>;
+using Matrix4x3d = Eigen::Matrix<double, 4, 3>;
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix7x6d = Eigen::Matrix<double, 7, 6>;
+using Matrix6x7d = Eigen::Matrix<double, 6, 7>;
+using Matrix7d = Eigen::Matrix<double, 7, 7>;
+using Quaterniond = Eigen::Quaterniond;
+
+// Alias canonical types from vortex utils
+using Eta = ::vortex::utils::types::EtaQuat;
+using Nu = ::vortex::utils::types::Nu;
 
 struct J_transformation {
-    Eigen::Matrix3d R = types::Matrix3d::Identity();
-    types::Matrix4x3d T = types::Matrix4x3d::Zero();
+    Matrix3d R = Matrix3d::Identity();
+    Matrix4x3d T = Matrix4x3d::Zero();
 
-    types::Matrix7x6d as_matrix() const {
-        types::Matrix7x6d mat = types::Matrix7x6d::Zero();
+    Matrix7x6d as_matrix() const {
+        Matrix7x6d mat = Matrix7x6d::Zero();
         mat.block<3, 3>(0, 0) = R;
         mat.block<4, 3>(3, 3) = T;
         return mat;
diff --git a/control/pid_controller_dp/package.xml b/control/pid_controller_dp/package.xml
index c1200d187..d21a2fa92 100644
--- a/control/pid_controller_dp/package.xml
+++ b/control/pid_controller_dp/package.xml
@@ -15,6 +15,8 @@
   <depend>eigen</depend>
   <depend>tf2</depend>
   <depend>vortex_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>vortex_utils</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/control/pid_controller_dp/src/pid_controller.cpp b/control/pid_controller_dp/src/pid_controller.cpp
index 427c7dcdf..60edc2014 100644
--- a/control/pid_controller_dp/src/pid_controller.cpp
+++ b/control/pid_controller_dp/src/pid_controller.cpp
@@ -1,6 +1,72 @@
 #include "pid_controller_dp/pid_controller.hpp"
 #include "pid_controller_dp/pid_controller_utils.hpp"
 
+void print_eta(const types::Eta& eta) {
+    // spdlog::info("Eta values:");
+    auto pos = eta.pos_vector();
+    auto ori = eta.ori_quaternion();
+    spdlog::info("Position - North: {}, East: {}, Down: {}", pos[0], pos[1],
+                 pos[2]);
+    spdlog::info("Orientation - w: {}, x: {}, y: {}, z: {}", ori.w(), ori.x(),
+                 ori.y(), ori.z());
+}
+
+void print_nu(const types::Nu& nu) {
+    spdlog::info("Nu values:");
+    auto v = nu.to_vector();
+    spdlog::info("Linear Speed - u: {}, v: {}, w: {}", v(0), v(1), v(2));
+    spdlog::info("Angular Speed - p: {}, q: {}, r: {}", v(3), v(4), v(5));
+}
+
+void print_vect_6d(const types::Vector6d& vec) {
+    spdlog::info("Vector6d values:");
+    for (int i = 0; i < 6; ++i) {
+        spdlog::info("Element[{}]: {}", i, vec[i]);
+    }
+}
+
+void print_J_transformation(const types::J_transformation& J) {
+    spdlog::info("J_transformation:");
+
+    spdlog::info("R (3x3) elements:");
+    for (int i = 0; i < J.R.rows(); ++i) {
+        for (int j = 0; j < J.R.cols(); ++j) {
+            spdlog::info("R[{},{}] = {}", i, j, J.R(i, j));
+        }
+    }
+
+    spdlog::info("T (4x3) elements:");
+    for (int i = 0; i < J.T.rows(); ++i) {
+        for (int j = 0; j < J.T.cols(); ++j) {
+            spdlog::info("T[{},{}] = {}", i, j, J.T(i, j));
+        }
+    }
+
+    spdlog::info("Combined Matrix (7x6) elements:");
+    auto M = J.as_matrix();
+    for (int i = 0; i < M.rows(); ++i) {
+        for (int j = 0; j < M.cols(); ++j) {
+            spdlog::info("M[{},{}] = {}", i, j, M(i, j));
+        }
+    }
+}
+
+void print_Jinv_transformation(const types::Matrix6x7d& J_inv) {
+    spdlog::info("J_pseudo_inverse (6x7):");
+    for (int i = 0; i < J_inv.rows(); ++i) {
+        std::string row;
+        row.reserve(128);
+        row += "[";
+        for (int j = 0; j < J_inv.cols(); ++j) {
+            row += std::to_string(J_inv(i, j));
+            if (j < J_inv.cols() - 1)
+                row += ", ";
+        }
+        row += "]";
+        spdlog::info("{}", row);
+    }
+}
+
 PIDController::PIDController()
     : Kp_(types::Matrix6d::Identity()),
       Ki_(types::Matrix6d::Zero()),
@@ -12,21 +78,53 @@ types::Vector6d PIDController::calculate_tau(const types::Eta& eta,
                                              const types::Eta& eta_d,
                                              const types::Nu& nu,
                                              const types::Eta& eta_dot_d) {
-    types::Eta error = error_eta(eta, eta_d);
+    types::Eta error = error_eta(eta, eta_d);  // calculate eta error
+
+    // set quaternion scalar part w = 0 (only use vector part of quaternion for
+    // error)
+    error.qw = 0.0;
 
-    types::Matrix6x7d J_inv = calculate_J_sudo_inv(error);
+    auto eta_dot_d_copy = eta_dot_d;
+    eta_dot_d_copy.qw = 0.0;  // set w = 0 for desired eta_dot
+    // debug
+    // eta_error_debug = error;
+    spdlog::info("Eta: ");
+    print_eta(eta);
+    spdlog::info("Eta desired: ");
+    print_eta(eta_d);
+    spdlog::info("Eta error:");
+    print_eta(error);
 
-    types::Vector6d nu_d = J_inv * eta_dot_d.as_vector();
+    types::Matrix6x7d J_inv =
+        calculate_J_sudo_inv(eta);  // calculate J pseudo inverse
+    J_inv_debug = J_inv;
+    print_Jinv_transformation(J_inv);
 
-    types::Vector6d error_nu = nu.as_vector() - nu_d;
+    types::Vector6d nu_d =
+        J_inv * eta_dot_d_copy.to_vector();  // calculate velocity
+    // nu_d_debug = nu_d;
+    // print_nu(nu_d);
 
-    types::Vector6d P = Kp_ * J_inv * error.as_vector();
+    types::Vector6d error_nu = nu.to_vector() - nu_d;  // calculate vel error
+    // error_nu_debug = error_nu;
+    // print_vect_6d(error_nu);
 
-    types::Vector6d I = Ki_ * J_inv * integral_;
+    types::Vector6d P = Kp_ * J_inv * error.to_vector();  // P term
+    // P_debug = P;
+    Kp_debug = Kp_;
 
-    types::Vector6d D = Kd_ * error_nu;
+    types::Vector6d I = Ki_ * J_inv * integral_;  // I term
+    // I_debug = I;
+    // Ki_debug = Ki_;
 
+    types::Vector6d D = Kd_ * error_nu;  // D term
+    // D_debug = D;
+    // Kd_debug = Kd_;
     types::Vector6d tau = -clamp_values((P + I + D), -80.0, 80.0);
+    // types::Vector6d tau = -clamp_values((P), -80.0, 80.0);
+
+    // debug: tau = 0
+    //  types::Vector6d tau = types::Vector6d::Zero();
 
     integral_ = anti_windup(dt_, error, integral_);
 
@@ -48,3 +146,13 @@ void PIDController::set_kd(const types::Matrix6d& Kd) {
 void PIDController::set_time_step(double dt) {
     this->dt_ = dt;
 }
+
+types::Matrix6d PIDController::get_kp() {
+    return this->Kp_;
+}
+types::Matrix6d PIDController::get_ki() {
+    return this->Ki_;
+}
+types::Matrix6d PIDController::get_kd() {
+    return this->Kd_;
+}
\ No newline at end of file
diff --git a/control/pid_controller_dp/src/pid_controller_conversions.cpp b/control/pid_controller_dp/src/pid_controller_conversions.cpp
index 8f6ff1970..d13099cac 100644
--- a/control/pid_controller_dp/src/pid_controller_conversions.cpp
+++ b/control/pid_controller_dp/src/pid_controller_conversions.cpp
@@ -7,12 +7,14 @@
 types::Eta eta_convert_from_ros_to_eigen(
     const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
     types::Eta eta;
-    eta.pos << msg->pose.pose.position.x, msg->pose.pose.position.y,
-        msg->pose.pose.position.z;
-    eta.ori.w() = msg->pose.pose.orientation.w;
-    eta.ori.x() = msg->pose.pose.orientation.x;
-    eta.ori.y() = msg->pose.pose.orientation.y;
-    eta.ori.z() = msg->pose.pose.orientation.z;
+    eta.x = msg->pose.pose.position.x;
+    eta.y = msg->pose.pose.position.y;
+    eta.z = msg->pose.pose.position.z;
+
+    eta.qw = msg->pose.pose.orientation.w;
+    eta.qx = msg->pose.pose.orientation.x;
+    eta.qy = msg->pose.pose.orientation.y;
+    eta.qz = msg->pose.pose.orientation.z;
 
     return eta;
 }
@@ -20,9 +22,13 @@ types::Eta eta_convert_from_ros_to_eigen(
 types::Nu nu_convert_from_ros_to_eigen(
     const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) {
     types::Nu nu;
-    nu.linear_speed << msg->twist.twist.linear.x, msg->twist.twist.linear.y,
-        msg->twist.twist.linear.z;
-    nu.angular_speed << msg->twist.twist.angular.x, msg->twist.twist.angular.y,
-        msg->twist.twist.angular.z;
+    nu.u = msg->twist.twist.linear.x;
+    nu.v = msg->twist.twist.linear.y;
+    nu.w = msg->twist.twist.linear.z;
+
+    nu.p = msg->twist.twist.angular.x;
+    nu.q = msg->twist.twist.angular.y;
+    nu.r = msg->twist.twist.angular.z;
+
     return nu;
 }
diff --git a/control/pid_controller_dp/src/pid_controller_node.cpp b/control/pid_controller_dp/src/pid_controller_node.cpp
index f9bf44663..bee6500fc 100644
--- a/control/pid_controller_dp/src/pid_controller_node.cpp
+++ b/control/pid_controller_dp/src/pid_controller_node.cpp
@@ -1,8 +1,18 @@
+#include <spdlog/spdlog.h>
 #include "pid_controller_dp/pid_controller_ros.hpp"
 
+auto start_msg = R"(
+  ____ ___ ____     ____            _             _ _
+ |  _ \_ _|  _ \   / ___|___  _ __ | |_ _ __ ___ | | | ___ _ __
+ | |_) | || | | | | |   / _ \| '_ \| __| '__/ _ \| | |/ _ \ '__|
+ |  __/| || |_| | | |__| (_) | | | | |_| | | (_) | | |  __/ |
+ |_|  |___|____/   \____\___/|_| |_|\__|_|  \___/|_|_|\___|_|
+)";
+
 int main(int argc, char** argv) {
     rclcpp::init(argc, argv);
-    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node");
+    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Started PID Controller Node");
+    spdlog::info(start_msg);
     rclcpp::spin(std::make_shared<PIDControllerNode>());
     rclcpp::shutdown();
     return 0;
diff --git a/control/pid_controller_dp/src/pid_controller_ros.cpp b/control/pid_controller_dp/src/pid_controller_ros.cpp
index 163d221b8..e85892a03 100644
--- a/control/pid_controller_dp/src/pid_controller_ros.cpp
+++ b/control/pid_controller_dp/src/pid_controller_ros.cpp
@@ -1,4 +1,5 @@
 #include <pid_controller_dp/pid_controller_ros.hpp>
+#include <rclcpp/logging.hpp>
 #include <variant>
 #include "pid_controller_dp/pid_controller_conversions.hpp"
 #include "pid_controller_dp/pid_controller_utils.hpp"
@@ -12,6 +13,9 @@ PIDControllerNode::PIDControllerNode() : Node("pid_controller_node") {
     tau_pub_timer_ = this->create_wall_timer(
         time_step_, std::bind(&PIDControllerNode::publish_tau, this));
     set_pid_params();
+
+    callback_handle_ = this->add_on_set_parameters_callback(std::bind(
+        &PIDControllerNode::parametersCallback, this, std::placeholders::_1));
 }
 
 void PIDControllerNode::set_subscribers_and_publisher() {
@@ -99,6 +103,48 @@ void PIDControllerNode::publish_tau() {
 
     types::Vector6d tau =
         pid_controller_.calculate_tau(eta_, eta_d_, nu_, eta_dot_d_);
+    // print for debug
+    RCLCPP_INFO_STREAM(this->get_logger(), "Tau: [" << tau(0) << ", " << tau(1)
+                                                    << ", " << tau(2) << ", "
+                                                    << tau(3) << ", " << tau(4)
+                                                    << ", " << tau(5) << "]");
+    RCLCPP_INFO_STREAM(this->get_logger(),
+                       "Kp: [" << pid_controller_.Kp_debug(0, 0) << ", "
+                               << pid_controller_.Kp_debug(0, 1) << ", "
+                               << pid_controller_.Kp_debug(0, 2) << ", "
+                               << pid_controller_.Kp_debug(0, 3) << ", "
+                               << pid_controller_.Kp_debug(0, 4) << ", "
+                               << pid_controller_.Kp_debug(0, 5) << "; "
+                               << pid_controller_.Kp_debug(1, 0) << ", "
+                               << pid_controller_.Kp_debug(1, 1) << ", "
+                               << pid_controller_.Kp_debug(1, 2) << ", "
+                               << pid_controller_.Kp_debug(1, 3) << ", "
+                               << pid_controller_.Kp_debug(1, 4) << ", "
+                               << pid_controller_.Kp_debug(1, 5) << "; "
+                               << pid_controller_.Kp_debug(2, 0) << ", "
+                               << pid_controller_.Kp_debug(2, 1) << ", "
+                               << pid_controller_.Kp_debug(2, 2) << ", "
+                               << pid_controller_.Kp_debug(2, 3) << ", "
+                               << pid_controller_.Kp_debug(2, 4) << ", "
+                               << pid_controller_.Kp_debug(2, 5) << "; "
+                               << pid_controller_.Kp_debug(3, 0) << ", "
+                               << pid_controller_.Kp_debug(3, 1) << ", "
+                               << pid_controller_.Kp_debug(3, 2) << ", "
+                               << pid_controller_.Kp_debug(3, 3) << ", "
+                               << pid_controller_.Kp_debug(3, 4) << ", "
+                               << pid_controller_.Kp_debug(3, 5) << "; "
+                               << pid_controller_.Kp_debug(4, 0) << ", "
+                               << pid_controller_.Kp_debug(4, 1) << ", "
+                               << pid_controller_.Kp_debug(4, 2) << ", "
+                               << pid_controller_.Kp_debug(4, 3) << ", "
+                               << pid_controller_.Kp_debug(4, 4) << ", "
+                               << pid_controller_.Kp_debug(4, 5) << "; "
+                               << pid_controller_.Kp_debug(5, 0) << ", "
+                               << pid_controller_.Kp_debug(5, 1) << ", "
+                               << pid_controller_.Kp_debug(5, 2) << ", "
+                               << pid_controller_.Kp_debug(5, 3) << ", "
+                               << pid_controller_.Kp_debug(5, 4) << ", "
+                               << pid_controller_.Kp_debug(5, 5) << "]");
 
     geometry_msgs::msg::WrenchStamped tau_msg;
     tau_msg.header.stamp = this->now();
@@ -114,20 +160,68 @@ void PIDControllerNode::publish_tau() {
 }
 
 void PIDControllerNode::set_pid_params() {
-    this->declare_parameter<std::vector<double>>(
-        "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0});
-    this->declare_parameter<std::vector<double>>(
-        "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1});
-    this->declare_parameter<std::vector<double>>(
-        "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1});
+    this->declare_parameter<double>("Kp_x", 1.0);
+    this->declare_parameter<double>("Kp_y", 1.0);
+    this->declare_parameter<double>("Kp_z", 1.0);
+    this->declare_parameter<double>("Kp_roll", 1.0);
+    this->declare_parameter<double>("Kp_pitch", 1.0);
+    this->declare_parameter<double>("Kp_yaw", 1.0);
+    this->declare_parameter<double>("Ki_x", 0.1);
+    this->declare_parameter<double>("Ki_y", 0.1);
+    this->declare_parameter<double>("Ki_z", 0.1);
+    this->declare_parameter<double>("Ki_roll", 0.1);
+    this->declare_parameter<double>("Ki_pitch", 0.1);
+    this->declare_parameter<double>("Ki_yaw", 0.1);
+    this->declare_parameter<double>("Kd_x", 0.1);
+    this->declare_parameter<double>("Kd_y", 0.1);
+    this->declare_parameter<double>("Kd_z", 0.1);
+    this->declare_parameter<double>("Kd_roll", 0.1);
+    this->declare_parameter<double>("Kd_pitch", 0.1);
+    this->declare_parameter<double>("Kd_yaw", 0.1);
+
+    // this->declare_parameter<std::vector<double>>(
+    //     "Kp", {1.0, 1.0, 1.0, 1.0, 1.0, 1.0});
+    // this->declare_parameter<std::vector<double>>(
+    //     "Ki", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1});
+    // this->declare_parameter<std::vector<double>>(
+    //     "Kd", {0.1, 0.1, 0.1, 0.1, 0.1, 0.1});
+
+    // std::vector<double> Kp_vec = this->get_parameter("Kp").as_double_array();
+    // std::vector<double> Ki_vec = this->get_parameter("Ki").as_double_array();
+    // std::vector<double> Kd_vec = this->get_parameter("Kd").as_double_array();
 
-    std::vector<double> Kp_vec = this->get_parameter("Kp").as_double_array();
-    std::vector<double> Ki_vec = this->get_parameter("Ki").as_double_array();
-    std::vector<double> Kd_vec = this->get_parameter("Kd").as_double_array();
+    std::vector<double> Kp_vec = {
+        this->get_parameter("Kp_x").as_double(),
+        this->get_parameter("Kp_y").as_double(),
+        this->get_parameter("Kp_z").as_double(),
+        this->get_parameter("Kp_roll").as_double(),
+        this->get_parameter("Kp_pitch").as_double(),
+        this->get_parameter("Kp_yaw").as_double(),
+    };
+    std::vector<double> Ki_vec = {
+        this->get_parameter("Ki_x").as_double(),
+        this->get_parameter("Ki_y").as_double(),
+        this->get_parameter("Ki_z").as_double(),
+        this->get_parameter("Ki_roll").as_double(),
+        this->get_parameter("Ki_pitch").as_double(),
+        this->get_parameter("Ki_yaw").as_double(),
+    };
+    std::vector<double> Kd_vec = {
+        this->get_parameter("Kd_x").as_double(),
+        this->get_parameter("Kd_y").as_double(),
+        this->get_parameter("Kd_z").as_double(),
+        this->get_parameter("Kd_roll").as_double(),
+        this->get_parameter("Kd_pitch").as_double(),
+        this->get_parameter("Kd_yaw").as_double(),
+    };
 
-    types::Matrix6d Kp_eigen = Eigen::Map<types::Matrix6d>(Kp_vec.data());
-    types::Matrix6d Ki_eigen = Eigen::Map<types::Matrix6d>(Ki_vec.data());
-    types::Matrix6d Kd_eigen = Eigen::Map<types::Matrix6d>(Kd_vec.data());
+    types::Vector6d Kp_vec_eigen(Kp_vec.data());
+    types::Vector6d Ki_vec_eigen(Ki_vec.data());
+    types::Vector6d Kd_vec_eigen(Kd_vec.data());
+
+    types::Matrix6d Kp_eigen = Kp_vec_eigen.asDiagonal().toDenseMatrix();
+    types::Matrix6d Ki_eigen = Ki_vec_eigen.asDiagonal().toDenseMatrix();
+    types::Matrix6d Kd_eigen = Kd_vec_eigen.asDiagonal().toDenseMatrix();
 
     pid_controller_.set_kp(Kp_eigen);
     pid_controller_.set_ki(Ki_eigen);
@@ -136,13 +230,144 @@ void PIDControllerNode::set_pid_params() {
 
 void PIDControllerNode::guidance_callback(
     const vortex_msgs::msg::ReferenceFilter::SharedPtr msg) {
-    eta_d_.pos << msg->x, msg->y, msg->z;
+    // Set desired position
+    eta_d_.x = msg->x;
+    eta_d_.y = msg->y;
+    eta_d_.z = msg->z;
 
+    // Convert desired attitude (roll, pitch, yaw) to quaternion and store
     double roll = msg->roll;
     double pitch = msg->pitch;
     double yaw = msg->yaw;
 
-    eta_d_.ori = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
-                 Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
-                 Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
+    Eigen::Quaterniond quat =
+        Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) *
+        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
+        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
+
+    eta_d_.qw = quat.w();
+    eta_d_.qx = quat.x();
+    eta_d_.qy = quat.y();
+    eta_d_.qz = quat.z();
+}
+
+// TODO: set parameter functions
+rcl_interfaces::msg::SetParametersResult PIDControllerNode::parametersCallback(
+    const std::vector<rclcpp::Parameter>& parameters) {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    result.reason = "success";
+
+    bool kp_x_updated = false;
+    bool kp_y_updated = false;
+    bool kp_z_updated = false;
+    bool kp_roll_updated = false;
+    bool kp_pitch_updated = false;
+    bool kp_yaw_updated = false;
+
+    bool ki_x_updated = false;
+    bool ki_y_updated = false;
+    bool ki_z_updated = false;
+    bool ki_roll_updated = false;
+    bool ki_pitch_updated = false;
+    bool ki_yaw_updated = false;
+
+    bool kd_x_updated = false;
+    bool kd_y_updated = false;
+    bool kd_z_updated = false;
+    bool kd_roll_updated = false;
+    bool kd_pitch_updated = false;
+    bool kd_yaw_updated = false;
+
+    types::Vector6d Kp_vec_eigen = pid_controller_.get_kp().diagonal();
+    types::Vector6d Ki_vec_eigen = pid_controller_.get_ki().diagonal();
+    types::Vector6d Kd_vec_eigen = pid_controller_.get_kd().diagonal();
+
+    for (const auto& param : parameters) {
+        if (param.get_name() == "Kp_x") {
+            Kp_vec_eigen(0) = param.as_double();
+            kp_x_updated = true;
+        } else if (param.get_name() == "Kp_y") {
+            Kp_vec_eigen(1) = param.as_double();
+            kp_y_updated = true;
+        } else if (param.get_name() == "Kp_z") {
+            Kp_vec_eigen(2) = param.as_double();
+            kp_z_updated = true;
+        } else if (param.get_name() == "Kp_roll") {
+            Kp_vec_eigen(3) = param.as_double();
+            kp_roll_updated = true;
+        } else if (param.get_name() == "Kp_pitch") {
+            Kp_vec_eigen(4) = param.as_double();
+            kp_pitch_updated = true;
+        } else if (param.get_name() == "Kp_yaw") {
+            Kp_vec_eigen(5) = param.as_double();
+            kp_yaw_updated = true;
+        } else if (param.get_name() == "Ki_x") {
+            Ki_vec_eigen(0) = param.as_double();
+            ki_x_updated = true;
+        } else if (param.get_name() == "Ki_y") {
+            Ki_vec_eigen(1) = param.as_double();
+            ki_y_updated = true;
+        } else if (param.get_name() == "Ki_z") {
+            Ki_vec_eigen(2) = param.as_double();
+            ki_z_updated = true;
+        } else if (param.get_name() == "Ki_roll") {
+            Ki_vec_eigen(3) = param.as_double();
+            ki_roll_updated = true;
+        } else if (param.get_name() == "Ki_pitch") {
+            Ki_vec_eigen(4) = param.as_double();
+            ki_pitch_updated = true;
+        } else if (param.get_name() == "Ki_yaw") {
+            Ki_vec_eigen(5) = param.as_double();
+            ki_yaw_updated = true;
+        } else if (param.get_name() == "Kd_x") {
+            Kd_vec_eigen(0) = param.as_double();
+            kd_x_updated = true;
+        } else if (param.get_name() == "Kd_y") {
+            Kd_vec_eigen(1) = param.as_double();
+            kd_y_updated = true;
+        } else if (param.get_name() == "Kd_z") {
+            Kd_vec_eigen(2) = param.as_double();
+            kd_z_updated = true;
+        } else if (param.get_name() == "Kd_roll") {
+            Kd_vec_eigen(3) = param.as_double();
+            kd_roll_updated = true;
+        } else if (param.get_name() == "Kd_pitch") {
+            Kd_vec_eigen(4) = param.as_double();
+            kd_pitch_updated = true;
+        } else if (param.get_name() == "Kd_yaw") {
+            Kd_vec_eigen(5) = param.as_double();
+            kd_yaw_updated = true;
+        }
+    }
+
+    // Only set the gains if the parameter update was successful
+    if (result.successful) {
+        if (kp_x_updated || kp_y_updated || kp_z_updated || kp_roll_updated ||
+            kp_pitch_updated || kp_yaw_updated) {
+            types::Matrix6d Kp_eigen =
+                Kp_vec_eigen.asDiagonal().toDenseMatrix();
+            pid_controller_.set_kp(Kp_eigen);
+        }
+        if (ki_x_updated || ki_y_updated || ki_z_updated || ki_roll_updated ||
+            ki_pitch_updated || ki_yaw_updated) {
+            types::Matrix6d Ki_eigen =
+                Ki_vec_eigen.asDiagonal().toDenseMatrix();
+            pid_controller_.set_ki(Ki_eigen);
+        }
+        if (kd_x_updated || kd_y_updated || kd_z_updated || kd_roll_updated ||
+            kd_pitch_updated || kd_yaw_updated) {
+            types::Matrix6d Kd_eigen =
+                Kd_vec_eigen.asDiagonal().toDenseMatrix();
+            pid_controller_.set_kd(Kd_eigen);
+        }
+    }
+
+    // print
+    for (const auto& param : parameters) {
+        RCLCPP_INFO(this->get_logger(), "%s", param.get_name().c_str());
+        RCLCPP_INFO(this->get_logger(), "%s", param.get_type_name().c_str());
+        RCLCPP_INFO(this->get_logger(), "%s", param.value_to_string().c_str());
+    }
+    return result;
 }
diff --git a/control/pid_controller_dp/src/pid_controller_utils.cpp b/control/pid_controller_dp/src/pid_controller_utils.cpp
index 246e15500..d85c9c4fa 100644
--- a/control/pid_controller_dp/src/pid_controller_utils.cpp
+++ b/control/pid_controller_dp/src/pid_controller_utils.cpp
@@ -1,75 +1,87 @@
 #include "pid_controller_dp/pid_controller_utils.hpp"
 #include <algorithm>
+#include <vortex/utils/math.hpp>
+#include <vortex/utils/types.hpp>
 #include "pid_controller_dp/pid_controller_conversions.hpp"
 #include "pid_controller_dp/typedefs.hpp"
 
+// void print_J_transformation(const types::J_transformation& J) {
+//     spdlog::info("J_transformation:");
+
+//     spdlog::info("R (3x3) elements:");
+//     for (int i = 0; i < J.R.rows(); ++i) {
+//         for (int j = 0; j < J.R.cols(); ++j) {
+//             spdlog::info("R[{},{}] = {}", i, j, J.R(i, j));
+//         }
+//     }
+
+//     spdlog::info("T (4x3) elements:");
+//     for (int i = 0; i < J.T.rows(); ++i) {
+//         for (int j = 0; j < J.T.cols(); ++j) {
+//             spdlog::info("T[{},{}] = {}", i, j, J.T(i, j));
+//         }
+//     }
+
+//     spdlog::info("Combined Matrix (7x6) elements:");
+//     auto M = J.as_matrix();
+//     for (int i = 0; i < M.rows(); ++i) {
+//         for (int j = 0; j < M.cols(); ++j) {
+//             spdlog::info("M[{},{}] = {}", i, j, M(i, j));
+//         }
+//     }
+// }
+
+// void print_Jinv_transformation(const types::Matrix6x7d& J_inv) {
+//     spdlog::info("J (6x7) elements:");
+//     for (int i = 0; i < J_inv.rows(); ++i) {
+//         for (int j = 0; j < J_inv.cols(); ++j) {
+//             spdlog::info("J_inv[{},{}] = {}", i, j, J_inv(i, j));
+//         }
+//     }
+// }
+
 types::Matrix3d calculate_R_quat(const types::Eta& eta) {
-    return eta.ori.normalized().toRotationMatrix();
+    return eta.as_rotation_matrix();
 }
 
 types::Matrix4x3d calculate_T_quat(const types::Eta& eta) {
-    types::Quaterniond quaternion_norm = eta.ori.normalized();
-
-    double w = quaternion_norm.w();
-    double x = quaternion_norm.x();
-    double y = quaternion_norm.y();
-    double z = quaternion_norm.z();
-
-    types::Matrix4x3d transformation_matrix;
-
-    transformation_matrix << -x, -y, -z, w, -z, y, z, w, -x, -y, x, w;
-
-    return transformation_matrix * 0.5;
+    return eta.as_transformation_matrix();
 }
 
 types::Matrix6x7d calculate_J_sudo_inv(const types::Eta& eta) {
-    types::Eta eta_norm;
-
-    eta_norm.pos = eta.pos;
-    eta_norm.ori = eta.ori;
-
-    types::Matrix3d R = calculate_R_quat(eta_norm);
-    types::Matrix4x3d T = calculate_T_quat(eta_norm);
-
-    types::J_transformation J;
-    J.R = R;
-    J.T = T;
-
-    types::Matrix6x7d J_transpose = J.as_matrix().transpose();
-    types::Matrix6x7d J_pseudo_inv =
-        (J_transpose * J.as_matrix()).inverse() * J_transpose;
+    auto J_matrix = eta.as_j_matrix();
+    Eigen::MatrixXd J_pseudo_inv_dynamic =
+        vortex::utils::math::pseudo_inverse(J_matrix);
 
+    types::Matrix6x7d J_pseudo_inv;
+    J_pseudo_inv = J_pseudo_inv_dynamic;
     return J_pseudo_inv;
 }
 
 types::Eta error_eta(const types::Eta& eta, const types::Eta& eta_d) {
-    types::Eta eta_error;
-
-    eta_error.pos = eta.pos - eta_d.pos;
-    eta_error.ori = eta_d.ori.conjugate() * eta.ori;
-
-    eta_error.ori = eta_error.ori.normalized();
-
-    return eta_error;
+    return eta - eta_d;
 }
 
 Eigen::VectorXd clamp_values(const Eigen::VectorXd& values,
                              double min_val,
                              double max_val) {
-    return values.cwiseMax(min_val).cwiseMin(max_val);
+    // return values.cwiseMax(min_val).cwiseMin(max_val);
+    return vortex::utils::math::clamp_values(values, min_val, max_val);
 }
 
 types::Vector7d anti_windup(const double dt,
                             const types::Eta& error,
                             const types::Vector7d& integral) {
-    types::Eta error_norm;
-
-    error_norm.pos = error.pos;
-    error_norm.ori = error.ori;
-
-    types::Vector7d integral_anti_windup =
-        integral + (error_norm.as_vector() * dt);
-
-    integral_anti_windup = clamp_values(integral_anti_windup, -80.0, 80.0);
-    return integral_anti_windup;
+    // Eigen::VectorXd integral_eig = integral;
+    // Eigen::VectorXd updated = vortex::utils::math::anti_windup(
+    //     dt, error.to_vector(), integral_eig, -80.0, 80.0);
+
+    // types::Vector7d integral_anti_windup;
+    // for (int i = 0; i < 7; ++i) {
+    //     integral_anti_windup(i) = updated(i);
+    // }
+
+    // return integral_anti_windup;
+    return vortex::utils::math::anti_windup(dt, error.to_vector(), integral,
+                                            -80.0, 80.0);
 }
diff --git a/control/pid_controller_dp/test/CMakeLists.txt b/control/pid_controller_dp/test/CMakeLists.txt
new file mode 100644
index 000000000..d47998914
--- /dev/null
+++ b/control/pid_controller_dp/test/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+
+find_package(GTest REQUIRED)
+include(GoogleTest)
+
+set(TEST_BINARY_NAME ${PROJECT_NAME}_test)
+add_executable(
+    ${TEST_BINARY_NAME}
+    pid_controller_tests.cpp
+)
+
+target_link_libraries(
+    ${TEST_BINARY_NAME}
+    PRIVATE
+    ${LIB_NAME}
+    GTest::GTest
+    spdlog::spdlog
+)
+
+ament_target_dependencies(${TEST_BINARY_NAME} PUBLIC Eigen3 tf2 vortex_utils)
+
+gtest_discover_tests(${TEST_BINARY_NAME})
diff --git a/control/pid_controller_dp/test/pid_controller_tests.cpp b/control/pid_controller_dp/test/pid_controller_tests.cpp
new file mode 100644
index 000000000..18b2a7e6f
--- /dev/null
+++ b/control/pid_controller_dp/test/pid_controller_tests.cpp
@@ -0,0 +1,522 @@
+#include <gtest/gtest.h>
+#include <spdlog/spdlog.h>
+
+#include <vortex/utils/math.hpp>
+#include <vortex/utils/types.hpp>
+#include "pid_controller_dp/pid_controller.hpp"
+#include "pid_controller_dp/pid_controller_utils.hpp"
+#include "pid_controller_dp/typedefs.hpp"
+
+void print_tau(const types::Vector6d& tau) {
+    spdlog::info("Tau values:");
+    spdlog::info("Surge: {}", tau[0]);
+    spdlog::info("Sway: {}", tau[1]);
+    spdlog::info("Heave: {}", tau[2]);
+    spdlog::info("Roll: {}", tau[3]);
+    spdlog::info("Pitch: {}", tau[4]);
+    spdlog::info("Yaw: {}", tau[5]);
+}
+
+class PIDControllerTests : public ::testing::Test {
+   protected:
+    PIDControllerTests() : pid_controller_() {
+        // Set PID gains for testing
+        types::Matrix6d Kp = types::Matrix6d::Identity() * 10.0;
+        types::Matrix6d Ki = types::Matrix6d::Identity() * 0.5;
+        types::Matrix6d Kd = types::Matrix6d::Identity() * 2.0;
+
+        pid_controller_.set_kp(Kp);
+        pid_controller_.set_ki(Ki);
+        pid_controller_.set_kd(Kd);
+    }
+
+    types::Eta generate_current_pose(const double north_pos,
+                                     const double east_pos,
+                                     const double down_pos,
+                                     const double roll_angle,
+                                     const double pitch_angle,
+                                     const double yaw_angle) {
+        types::Eta current_pose;
+        current_pose.x = north_pos;
+        current_pose.y = east_pos;
+        current_pose.z = down_pos;
+        Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(
+            roll_angle, pitch_angle, yaw_angle);
+        current_pose.qw = q.w();
+        current_pose.qx = q.x();
+        current_pose.qy = q.y();
+        current_pose.qz = q.z();
+        return current_pose;
+    }
+
+    types::Eta generate_reference_pose(const double north_pos,
+                                       const double east_pos,
+                                       const double down_pos,
+                                       const double roll_angle,
+                                       const double pitch_angle,
+                                       const double yaw_angle) {
+        types::Eta reference_pose;
+        reference_pose.x = north_pos;
+        reference_pose.y = east_pos;
+        reference_pose.z = down_pos;
+        Eigen::Quaterniond q = vortex::utils::math::euler_to_quat(
+            roll_angle, pitch_angle, yaw_angle);
+        reference_pose.qw = q.w();
+        reference_pose.qx = q.x();
+        reference_pose.qy = q.y();
+        reference_pose.qz = q.z();
+        return reference_pose;
+    }
+
+    types::Nu generate_current_velocity(const double surge_vel,
+                                        const double sway_vel,
+                                        const double heave_vel,
+                                        const double roll_rate,
+                                        const double pitch_rate,
+                                        const double yaw_rate) {
+        types::Nu current_velocity;
+        current_velocity.u = surge_vel;
+        current_velocity.v = sway_vel;
+        current_velocity.w = heave_vel;
+        current_velocity.p = roll_rate;
+        current_velocity.q = pitch_rate;
+        current_velocity.r = yaw_rate;
+        return current_velocity;
+    }
+
+    PIDController pid_controller_;
+};
+
+/*
+Test that negative north error only (in body) gives positive surge command only.
+*/
+
+TEST_F(PIDControllerTests,
+       T01_neg_north_error_with_zero_heading_gives_surge_only_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_GT(tau[0], 0.0);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative north error with positive heading gives a positive surge
+command and negative sway command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T02_neg_north_error_with_positive_heading_gives_pos_surge_and_neg_sway_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)};
+    types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, 1.5)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+    EXPECT_GT(tau[0], 0.0);
+    EXPECT_LT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative north error with negative heading gives a positive surge
+command and positive sway command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T03_neg_north_error_with_negative_heading_gives_pos_surge_and_pos_sway_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)};
+    types::Eta eta_d{generate_reference_pose(10.0, 0.0, 0.0, 0.0, 0.0, -1.5)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_GT(tau[0], 0.0);
+    EXPECT_GT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative down error with zero roll and pitch gives a positive heave
+command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T04_neg_down_error_with_zero_roll_and_pitch_gives_positive_heave_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_GT(tau[2], 0.0);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative down error with zero roll and negative pitch gives a positive
+heave and positive surge command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T05_neg_down_error_with_zero_roll_and_neg_pitch_gives_positive_heave_and_positive_surge_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, -0.5, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, -0.5, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_GT(tau[0], 0.0);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_GT(tau[2], 0.0);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative down error with zero roll and positive pitch gives a positive
+heave and negative surge command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T06_neg_down_error_with_zero_roll_and_pos_pitch_gives_positive_heave_and_negative_surge_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.5, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 2.0, 0.0, 0.5, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_LT(tau[0], 0.0);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_GT(tau[2], 0.0);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative east error with zero heading gives a positive sway command.
+*/
+
+TEST_F(PIDControllerTests,
+       T07_neg_east_error_with_zero_heading_gives_positive_sway_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_GT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that positive east error with zero heading gives a negative sway command.
+*/
+
+TEST_F(PIDControllerTests,
+       T08_pos_east_error_with_zero_heading_gives_pos_sway_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, -10.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_LT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative east error with positive heading gives a positive surge and
+sway command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T09_neg_east_error_with_positive_heading_gives_pos_sway_and_pos_surge_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.5)};
+    types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, 1.5)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_GT(tau[0], 0.0);
+    EXPECT_GT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative east error with negative heading gives a negative surge and
+positive sway command.
+*/
+
+TEST_F(
+    PIDControllerTests,
+    T10_neg_east_error_with_negative_heading_gives_pos_sway_and_neg_surge_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.5)};
+    types::Eta eta_d{generate_reference_pose(0.0, 10.0, 0.0, 0.0, 0.0, -1.5)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_LT(tau[0], 0.0);
+    EXPECT_GT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative roll error gives positive roll command.
+*/
+
+TEST_F(PIDControllerTests, T11_neg_roll_error_gives_positive_roll_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 1.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_GT(tau[3], 0.0);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that positive roll error gives negative roll command.
+*/
+
+TEST_F(PIDControllerTests, T12_pos_roll_error_gives_neg_roll_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, -1.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_LT(tau[3], 0.0);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative pitch error gives positive pitch command.
+*/
+
+TEST_F(PIDControllerTests, T13_neg_pitch_error_gives_pos_pitch_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 1.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_GT(tau[4], 0.0);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that positive pitch error gives negative pitch command.
+*/
+
+TEST_F(PIDControllerTests, T14_pos_pitch_error_gives_neg_pitch_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, -1.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_LT(tau[4], 0.0);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that negative yaw error gives positive yaw command.
+*/
+
+TEST_F(PIDControllerTests, T15_neg_yaw_error_gives_pos_yaw_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 1.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_GT(tau[5], 0.0);
+}
+
+/*
+Test that positive yaw error gives negative yaw command.
+*/
+
+TEST_F(PIDControllerTests, T16_pos_yaw_error_gives_neg_yaw_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, -1.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    print_tau(tau);
+
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_LT(tau[5], 0.0);
+}
+
+/*
+Test that positive surge velocity only results in negative surge command
+(breaking effect).
+*/
+
+TEST_F(PIDControllerTests, T17_pos_surge_vel_gives_negative_surge_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_LT(tau[0], 0.0);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that positive sway velocity only results in negative sway command (breaking
+effect).
+*/
+
+TEST_F(PIDControllerTests, T18_pos_sway_vel_gives_negative_sway_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_LT(tau[1], 0.0);
+    EXPECT_NEAR(tau[2], 0.0, 0.01);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+/*
+Test that positive heave velocity only results in negative heave command
+(breaking effect).
+*/
+
+TEST_F(PIDControllerTests, T19_pos_heave_vel_gives_negative_heave_command) {
+    types::Eta eta{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_d{generate_reference_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+    types::Eta eta_dot_d{generate_current_pose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)};
+
+    types::Nu nu{generate_current_velocity(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)};
+    types::Vector6d tau{
+        pid_controller_.calculate_tau(eta, eta_d, nu, eta_dot_d)};
+    EXPECT_NEAR(tau[0], 0.0, 0.01);
+    EXPECT_NEAR(tau[1], 0.0, 0.01);
+    EXPECT_LT(tau[2], 0.0);
+    EXPECT_NEAR(tau[3], 0.0, 0.01);
+    EXPECT_NEAR(tau[4], 0.0, 0.01);
+    EXPECT_NEAR(tau[5], 0.0, 0.01);
+}
+
+int main(int argc, char** argv) {
+    testing::InitGoogleTest(&argc, argv);
+
+    return RUN_ALL_TESTS();
+}
diff --git a/scripts/ci_install_dependencies.sh b/scripts/ci_install_dependencies.sh
old mode 100644
new mode 100755