diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
index b329a483..483b5074 100755
--- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
+++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
@@ -37,6 +37,7 @@ def __init__(self):
 
         self.init_pos = False
         self.eta_received = False
+        self.eta_logger = True
 
         self.eta = np.array([0, 0, 0])
         self.eta_ref = np.array([0, 0, 0])
@@ -68,7 +69,7 @@ def guidance_callback(self):
                 self.get_logger().info(f"Reference initialized at {self.xd[0:3]}")
                 self.init_pos = True
             last_waypoint = self.waypoints[-1]
-            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, 0])
+            self.eta_ref = np.array([last_waypoint.x, last_waypoint.y, self.eta[2]])
             x_next = self.reference_filter.step(self.eta_ref, self.xd)
             self.xd = x_next
             # self.get_logger().info(f'x_next[0]: {x_next[0]}')
@@ -76,12 +77,14 @@ def guidance_callback(self):
             # self.get_logger().info(f'x_next[0]: {x_next[2]}')
 
             odom_msg = Odometry()
-            odom_msg = state_to_odometrymsg(x_next[:3])
+            # odom_msg = state_to_odometrymsg(x_next[:3])
+            odom_msg = state_to_odometrymsg(self.eta_ref)
             self.guidance_publisher.publish(odom_msg)
 
         else:
-            if not self.eta_received:
+            if not self.eta_received and self.eta_logger:
                 self.get_logger().info("Waiting for eta")
+                self.eta_logger = False
 
             if not self.waiting_message_printed:
                 self.get_logger().info('Waiting for waypoints to be received')
diff --git a/guidance/hybridpath_guidance/CMakeLists.txt b/guidance/hybridpath_guidance/CMakeLists.txt
index e16c1b10..8b4fd5ac 100644
--- a/guidance/hybridpath_guidance/CMakeLists.txt
+++ b/guidance/hybridpath_guidance/CMakeLists.txt
@@ -22,6 +22,7 @@ install(DIRECTORY
 install(PROGRAMS
   hybridpath_guidance/hybridpath_guidance_node.py
   hybridpath_guidance/waypoint_node.py
+  hybridpath_guidance/noisy_odom_publisher.py
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml
index 20fa168f..7279c745 100644
--- a/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml
+++ b/guidance/hybridpath_guidance/config/hybridpath_guidance_config.yaml
@@ -4,4 +4,4 @@
       lambda_val: 0.15 # Curvature constant
       path_generator_order: 1 # Differentiability order
       dt: 0.1 # Time step
-      mu: 0.15 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind
\ No newline at end of file
+      mu: 0.25 # Tuning parameter, changes how much the path will wait for the vessel if it lags behind
\ No newline at end of file
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py
index 9f2a6155..a4a4db3c 100644
--- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py
+++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath.py
@@ -66,6 +66,40 @@ class HybridPathGenerator:
         path (Path): The path object.
     """
     def __init__(self, WP: list[Point], r: int, lambda_val: float):
+        self.WP = WP
+        self.r = r
+        self.lambda_val = lambda_val
+        self.order = 2*r + 1
+
+    def create_path(self, WP: list[Point]) -> None:
+        """
+        Create a path object.
+
+        Args:
+            WP (list[Point]): A list of waypoints.
+
+        Returns:
+            None
+        """
+        self.update_waypoints(WP)
+        self._initialize_path()
+        self._calculate_subpaths()
+
+    def _initialize_path(self):
+        self.path = Path()
+        
+        self.path.coeff.a = []
+        self.path.coeff.b = []
+        self.path.coeff.a_der = []
+        self.path.coeff.b_der = []
+        self.path.LinSys.A = None
+        self.path.LinSys.bx = []
+        self.path.LinSys.by = []
+
+        self.path.NumSubpaths = len(self.WP) - 1
+        self.path.Order = self.order
+
+    def update_waypoints(self, WP: list[Point]) -> None:
         # Convert the waypoints to a numpy array
         WP_arr = np.array([[wp.x, wp.y] for wp in WP])
 
@@ -77,13 +111,6 @@ def __init__(self, WP: list[Point], r: int, lambda_val: float):
         
         else:
             self.WP = WP_arr
-        self.r = r
-        self.lambda_val = lambda_val
-        self.order = 2*r + 1
-        self.path = Path()
-        self.path.NumSubpaths = len(self.WP) - 1
-        self.path.Order = self.order
-        self._calculate_subpaths()
 
     def _construct_A_matrix(self) -> np.ndarray:
         """
@@ -280,6 +307,12 @@ def _calculate_subpaths(self) -> None:
             for k, (a, b) in enumerate(zip(a_derivatives, b_derivatives)):
                 self._append_derivatives(k, a, b)
 
+    def get_path(self) -> Path:
+        """
+        Get the hybrid path.
+        """
+        return self.path
+
     @staticmethod
     def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> float:
         """
@@ -295,7 +328,9 @@ def update_s(path: Path, dt: float, u_desired: float, s: float, w: float) -> flo
             float: The updated position along the hybrid path.
 
         """
-        signals = HybridPathSignals(path, s)
+        signals = HybridPathSignals()
+        signals.update_path(path)
+        signals.update_s(s)
         v_s = signals.get_vs(u_desired)
         s_new = s + (v_s + w) * dt
         return s_new
@@ -314,13 +349,14 @@ class HybridPathSignals:
         Path (Path): The path object.
         s (float): The path parameter.
     """
-    def __init__(self, path: Path, s: float):
-        if not isinstance(path, Path):
-            raise TypeError("path must be an instance of Path")
-        self.path = path
-        self.s = self._clamp_s(s, self.path.NumSubpaths)
+    def __init__(self):
+        # if not isinstance(path, Path):
+        #     raise TypeError("path must be an instance of Path")
+        self.path = None
+        self.s = None
 
-    def _clamp_s(self, s: float, num_subpaths: int) -> float:
+    @staticmethod
+    def _clamp_s(s: float, num_subpaths: int) -> float:
         """
         Ensures s is within the valid range of [0, num_subpaths].
 
@@ -506,4 +542,29 @@ def get_w(self, mu: float, eta: np.ndarray) -> float:
         w = (mu / np.linalg.norm(eta_d_s)) * np.dot(eta_d_s, error)
         
         return w
+    
+    def update_path(self, path: Path) -> None:
+        """
+        Update the path object.
+
+        Args:
+            path (Path): The new path object.
+
+        Returns:
+            None
+        """
+        self.path = path
+        self.update_s(0.)
+
+    def update_s(self, s: float) -> None:
+        """
+        Update the path parameter.
+
+        Args:
+            s (float): The new path parameter.
+
+        Returns:
+            None
+        """
+        self.s = self._clamp_s(s, self.path.NumSubpaths)
     
\ No newline at end of file
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py
index 971791e3..f97da507 100755
--- a/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py
+++ b/guidance/hybridpath_guidance/hybridpath_guidance/hybridpath_guidance_node.py
@@ -2,14 +2,15 @@
 import numpy as np
 import rclpy
 from rclpy.node import Node
-from geometry_msgs.msg import Pose2D
-from std_msgs.msg import Float32
+from geometry_msgs.msg import Pose2D, Point
+from std_msgs.msg import Float32, Float64MultiArray
 from vortex_msgs.msg import HybridpathReference
-from vortex_msgs.srv import Waypoint
+from vortex_msgs.srv import Waypoint, DesiredVelocity
 from nav_msgs.msg import Odometry
 from transforms3d.euler import quat2euler
 from hybridpath_guidance.hybridpath import HybridPathGenerator, HybridPathSignals
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
+import threading
 
 
 qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
@@ -28,8 +29,12 @@ def __init__(self):
             ])
         
         self.waypoint_server = self.create_service(Waypoint, 'waypoint_list', self.waypoint_callback)
+        self.u_desired_server = self.create_service(DesiredVelocity, 'u_desired', self.u_desired_callback)
         self.eta_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.eta_callback, qos_profile=qos_profile)
         self.yaw_publisher = self.create_publisher(Float32, 'yaw', 1)
+        self.s_publisher = self.create_publisher(Float32, 's', 1)
+        self.w_publisher = self.create_publisher(Float32, 'w', 1)
+        self.wp_arr_publisher = self.create_publisher(Float64MultiArray, 'waypoints', 1)
         self.guidance_publisher = self.create_publisher(HybridpathReference, 'guidance/hybridpath/reference', 1)
         
         # Get parameters
@@ -39,74 +44,128 @@ def __init__(self):
         self.mu = self.get_parameter('hybridpath_guidance.mu').get_parameter_value().double_value
         self.eta = np.zeros(3)
 
-        self.u_desired = 0.2 # Desired velocity
+        self.u_desired = 0.3 # Desired velocity
+
+        self.waypoints = []
+        self.path = None
+        self.s = 0.
+        self.w = 0.
+
+        # Initialize path generator
+        self.generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val)
+
+        # Initialize signals
+        self.signals = HybridPathSignals()
 
         # Flags for logging
         self.waypoints_received = False
         self.waiting_message_printed = False
+        self.first_pos_flag = False
+        self.eta_received = False
 
         # Timer for guidance
         timer_period = 0.1
         self.timer = self.create_timer(timer_period, self.guidance_callback)
 
+        self.lock = threading.Lock()
+
+    def u_desired_callback(self, request, response):
+        self.u_desired = request.u_desired
+        self.get_logger().info(f"Received desired velocity: {self.u_desired}")
+        response.success = True
+        return response
+
+
     def waypoint_callback(self, request, response):
-        self.get_logger().info('Received waypoints, generating path...')
-        self.waypoints = request.waypoint
 
-        generator = HybridPathGenerator(self.waypoints, self.path_generator_order, self.lambda_val)
-        self.path = generator.path
+        with self.lock:
+            if self.eta_received:
+                self.waypoints = [Point(x=self.eta[0], y=self.eta[1])]
 
-        self.waypoints_received = True
-        self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
+                self.get_logger().info('Received waypoints, generating path...')
 
-        self.s = 0
-        signals = HybridPathSignals(self.path, self.s)
-        self.w = signals.get_w(self.mu, self.eta)
-        
+                new_waypoints = request.waypoint
+
+                for point in new_waypoints:
+                    self.waypoints.append(point)
+
+                self.generator.create_path(self.waypoints)
+                self.path = self.generator.get_path()
+
+                self.waypoints_received = True
+                self.waiting_message_printed = False  # Reset this flag to handle multiple waypoint sets
+                self.first_pos_flag = False
+
+                self.s = 0.
+                self.signals.update_path(self.path)
+                self.w = self.signals.get_w(self.mu, self.eta)
+
+                wp_arr = Float64MultiArray()
+                wp_list = self.generator.WP.tolist()
+                wp_arr.data = [coordinate for wp in wp_list for coordinate in wp]
+                self.wp_arr_publisher.publish(wp_arr)
+            
         response.success = True
+
         return response
     
     def eta_callback(self, msg: Odometry):
         yaw_msg = Float32()
         self.eta = self.odom_to_eta(msg)
-        yaw = float(self.eta[2])
-        yaw_msg.data = yaw
+        self.yaw = float(self.eta[2])
+        yaw_msg.data = self.yaw
         self.yaw_publisher.publish(yaw_msg)
+        self.eta_received = True
 
     def guidance_callback(self):
-        if self.waypoints_received:
-            self.s = HybridPathGenerator.update_s(self.path, self.dt, self.u_desired, self.s, self.w)
-            signals = HybridPathSignals(self.path, self.s)
-            self.w = signals.get_w(self.mu, self.eta)
+        with self.lock:
+            if self.waypoints_received:
+                self.s = self.generator.update_s(self.path, self.dt, self.u_desired, self.s, self.w)
+                self.signals.update_s(self.s)
+                self.w = self.signals.get_w(self.mu, self.eta)
+
+                pos = self.signals.get_position()
 
-            pos = signals.get_position()
-            pos_der = signals.get_derivatives()[0]
-            pos_dder = signals.get_derivatives()[1]
+                if not self.first_pos_flag:
+                    self.get_logger().info(f"First position: {pos}")
+                    self.first_pos_flag = True
 
-            psi = 0.#signals.get_heading()
-            psi_der = 0.#signals.get_heading_derivative()
-            psi_dder = 0.#signals.get_heading_second_derivative()
+                # pos[0] = self.eta[0]
+                pos_der = self.signals.get_derivatives()[0]
+                pos_dder = self.signals.get_derivatives()[1]
 
-            hp_msg = HybridpathReference()
-            hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi)
-            hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der)
-            hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder)
+                psi = 0. #signals.get_heading()
+                psi_der = 0.#signals.get_heading_derivative()
+                psi_dder = 0.#signals.get_heading_second_derivative()
 
-            hp_msg.w = signals.get_w(self.mu, self.eta)
-            hp_msg.v_s = signals.get_vs(self.u_desired)
-            hp_msg.v_ss = signals.get_vs_derivative(self.u_desired)
+                hp_msg = HybridpathReference()
+                hp_msg.eta_d = Pose2D(x=pos[0], y=pos[1], theta=psi)
+                hp_msg.eta_d_s = Pose2D(x=pos_der[0], y=pos_der[1], theta=psi_der)
+                hp_msg.eta_d_ss = Pose2D(x=pos_dder[0], y=pos_dder[1], theta=psi_dder)
 
-            self.guidance_publisher.publish(hp_msg)
+                hp_msg.w = self.signals.get_w(self.mu, self.eta)
+                hp_msg.v_s = self.signals.get_vs(self.u_desired)
+                hp_msg.v_ss = self.signals.get_vs_derivative(self.u_desired)
 
-            if self.s >= self.path.NumSubpaths:
-                self.waypoints_received = False
-                self.waiting_message_printed = False
-                self.get_logger().info('Last waypoint reached')
+                self.guidance_publisher.publish(hp_msg)
 
-        else:
-            if not self.waiting_message_printed:
-                self.get_logger().info('Waiting for waypoints to be received')
-                self.waiting_message_printed = True
+                if self.s >= self.path.NumSubpaths:
+                    self.waypoints_received = False
+                    self.waiting_message_printed = False
+                    self.get_logger().info('Last waypoint reached')
+
+            else:
+                if not self.waiting_message_printed:
+                    self.get_logger().info('Waiting for waypoints to be received')
+                    self.waiting_message_printed = True
+
+        s_msg = Float32()
+        s_msg.data = self.s
+        self.s_publisher.publish(s_msg)
+
+        w_msg = Float32()
+        w_msg.data = self.w
+        self.w_publisher.publish(w_msg)
 
     @staticmethod
     def odom_to_eta(msg: Odometry) -> np.ndarray:
@@ -129,7 +188,10 @@ def odom_to_eta(msg: Odometry) -> np.ndarray:
         # Convert quaternion to Euler angles
         yaw = quat2euler(orientation_list)[2]
 
+        # yaw = np.deg2rad(yaw)
+
         state = np.array([x, y, yaw])
+
         return state
 
 def main(args=None):
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py
new file mode 100755
index 00000000..5d4395d5
--- /dev/null
+++ b/guidance/hybridpath_guidance/hybridpath_guidance/noisy_odom_publisher.py
@@ -0,0 +1,57 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from vortex_msgs.srv import Waypoint
+from geometry_msgs.msg import Point
+from nav_msgs.msg import Odometry
+from transforms3d.euler import quat2euler
+import numpy as np
+from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
+
+
+qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
+                         reliability=QoSReliabilityPolicy.BEST_EFFORT)
+
+class OdomPublisher(Node):
+    def __init__(self):
+        super().__init__('odom_publisher')
+
+        self.eta_odom_publisher = self.create_publisher(Odometry, '/seapath/odom/ned', qos_profile)
+
+        timer_period = 0.01
+        self.timer = self.create_timer(timer_period, self.odom_callback)
+
+        self.get_logger().info("Odom publisher started")
+
+    def odom_callback(self):
+
+        noise = np.random.normal(0, 0.005, 3)
+
+        msg = Odometry()
+        msg.pose.pose.position.x = 0.0 + noise[0]
+        msg.pose.pose.position.y = 0.0 + noise[1]
+        msg.pose.pose.position.z = 0.0
+        msg.pose.pose.orientation.x = 0.0
+        msg.pose.pose.orientation.y = 0.0
+        msg.pose.pose.orientation.z = 0.0
+        msg.pose.pose.orientation.w = 1.0
+        msg.twist.twist.linear.x = 0.0 + noise[0]
+        msg.twist.twist.linear.y = 0.0 + noise[1]
+        msg.twist.twist.linear.z = 0.0
+        msg.twist.twist.angular.x = 0.0
+        msg.twist.twist.angular.y = 0.0
+        msg.twist.twist.angular.z = 0.0 + noise[2]
+
+        self.eta_odom_publisher.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = OdomPublisher()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py
new file mode 100644
index 00000000..033afd22
--- /dev/null
+++ b/guidance/hybridpath_guidance/hybridpath_guidance/u_desired_node.py
@@ -0,0 +1,48 @@
+import rclpy
+from rclpy.node import Node
+from vortex_msgs.srv import DesiredVelocity
+
+class DesiredVelocityClient(Node):
+    def __init__(self):
+        super().__init__('desired_velocity_client')
+        self.client = self.create_client(DesiredVelocity, 'u_desired')
+
+        while not self.client.wait_for_service(timeout_sec=2.0):
+            self.get_logger().info('service not available, waiting again...')
+
+        self.send_request()
+
+    def send_request(self):
+        req = DesiredVelocity.Request()
+        req.u = 0.3
+        self.get_logger().info(f'Sending request: {req}')
+        self.future = self.client.call_async(req)
+        self.future.add_done_callback(self.get_response)
+
+    def get_response(self, future):
+        try:
+            response = future.result()
+            self.get_logger().info(f'Received response: {response}')
+            if response.success:
+                self.destroy_node()
+                rclpy.shutdown()
+
+        except Exception as e:
+            self.get_logger().error('Service call failed %r' % (e,))
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    client = DesiredVelocityClient()
+
+    try:
+        rclpy.spin(client)
+    except Exception as e:
+        client.get_logger().error('Error in DesiredVelocityClient: %r' % (e,))
+    finally:
+        if rclpy.ok():
+            client.destroy_node()
+            rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
index d9e024bd..59b5df38 100755
--- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
+++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
@@ -39,7 +39,7 @@ def eta_callback(self, msg: Odometry):
     def send_request(self):
         if self.eta_received:
             req = Waypoint.Request()
-            wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1]]]
+            wp_list = [[self.eta[0], self.eta[1]], [self.eta[0] + 3., self.eta[1] + 0.]]
             req.waypoint = [Point(x=float(wp[0]), y=float(wp[1]), z=0.0) for wp in wp_list]
             self.get_logger().info(f'Sending request: {req}')
             self.future = self.client.call_async(req)
diff --git a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml
index ef6650ff..f01ea966 100644
--- a/motion/hybridpath_controller/config/hybridpath_controller_config.yaml
+++ b/motion/hybridpath_controller/config/hybridpath_controller_config.yaml
@@ -1,5 +1,5 @@
 /**:
   ros__parameters:
     hybridpath_controller:
-      K1_diag: [0.5, 0.5, 0.5] # First gain matrix
-      K2_diag: [20.0, 20.0, 20.0] # Second gain matrix
\ No newline at end of file
+      K1_diag: [1., 1., 1.] # First gain matrix
+      K2_diag: [40.0, 30.0, 20.0] # Second gain matrix
\ No newline at end of file
diff --git a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py
index ff95a51c..ecc0008e 100644
--- a/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py
+++ b/motion/hybridpath_controller/hybridpath_controller/adaptive_backstep.py
@@ -4,7 +4,13 @@
 from transforms3d.euler import quat2euler
 
 class AdaptiveBackstep:
-    def __init__(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None:
+    def __init__(self):
+        self.K_1 = np.eye(3)
+        self.K_2 = np.eye(3)
+        self.M = np.eye(3)
+        self.D = np.eye(3)
+        
+    def update_parameters(self, K1: np.ndarray, K2: np.ndarray, M: np.ndarray, D: np.ndarray) -> None:
         self.K_1 = K1
         self.K_2 = K2
         self.M = M
@@ -27,6 +33,7 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda
 
         # Extract values from the state and reference
         eta = state[:3]
+        # eta[0] = 0.
         nu = state[3:]
         w = reference.w
         v_s = reference.v_s
@@ -54,8 +61,34 @@ def control_law(self, state: Odometry, reference: HybridpathReference) -> np.nda
 
         tau = -self.K_2 @ z2 + self.calculate_coriolis_matrix(nu) + self.D @ nu + self.M @ sigma1 + self.M @ ds_alpha1 * (v_s + w)
 
+        self.eta_error = eta_error
+        self.z1 = z1
+        self.alpha1 = alpha1
+        self.z2 = z2
+        self.ds_alpha1 = ds_alpha1
+        self.sigma1 = sigma1
+
         return tau
     
+    def get_eta_error(self):
+        return self.eta_error
+    
+    def get_z1(self):
+        return self.z1
+    
+    def get_alpha1(self):
+        return self.alpha1
+    
+    def get_z2(self):
+        return self.z2
+    
+    def get_sigma1(self):
+        return self.sigma1
+    
+    def get_ds_alpha1(self):
+        return self.ds_alpha1
+    
+    
     @staticmethod
     def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray: 
         """
@@ -66,7 +99,7 @@ def calculate_coriolis_matrix(nu: np.ndarray) -> np.ndarray:
             [0, 0, 5.5],
             [82.5, -5.5, 0]
         ])
-        return C_A @ nu
+        return (C_A @ nu) * 0
 
     @staticmethod
     def rotationmatrix_in_yaw_transpose(psi: float) -> np.ndarray:
@@ -118,6 +151,8 @@ def odom_to_state(msg: Odometry) -> np.ndarray:
         # Convert quaternion to Euler angles
         yaw = quat2euler(orientation_list)[2]
 
+        # yaw = np.deg2rad(yaw)
+
         u = msg.twist.twist.linear.x
         v = msg.twist.twist.linear.y
         r = msg.twist.twist.angular.z 
diff --git a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
index 07c69eaa..86718344 100755
--- a/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
+++ b/motion/hybridpath_controller/hybridpath_controller/hybridpath_controller_node.py
@@ -7,7 +7,9 @@
 from geometry_msgs.msg import Wrench
 from nav_msgs.msg import Odometry
 from vortex_msgs.msg import HybridpathReference
+from std_msgs.msg import Float64MultiArray
 from rclpy.qos import QoSProfile, qos_profile_sensor_data, QoSReliabilityPolicy
+from rcl_interfaces.msg import SetParametersResult
 
 
 qos_profile = QoSProfile(depth=1, history=qos_profile_sensor_data.history, 
@@ -25,31 +27,61 @@ def __init__(self):
                 ('physical.damping_matrix_diag', [10.0, 10.0, 5.0])
             ])
         
+        self.parameters_updated = False
+        
         self.state_subscriber_ = self.state_subscriber_ = self.create_subscription(Odometry, '/seapath/odom/ned', self.state_callback, qos_profile=qos_profile)
         self.hpref_subscriber_ = self.create_subscription(HybridpathReference, 'guidance/hybridpath/reference', self.reference_callback, 1)
         self.wrench_publisher_ = self.create_publisher(Wrench, 'thrust/wrench_input', 1)
 
-        # Get parameters
+        # Debug publishers
+        self.eta_error_publisher = self.create_publisher(Float64MultiArray, 'eta_error', 10)
+        self.z1_publisher = self.create_publisher(Float64MultiArray, 'z1', 10)
+        self.alpha1_publisher = self.create_publisher(Float64MultiArray, 'alpha1', 10)
+        self.z2_publisher = self.create_publisher(Float64MultiArray, 'z2', 10)
+        self.ds_alpha1_publisher = self.create_publisher(Float64MultiArray, 'ds_alpha1', 10)
+        self.sigma1_publisher = self.create_publisher(Float64MultiArray, 'sigma1', 10)
+        self.tau_publisher = self.create_publisher(Float64MultiArray, 'tau', 10)
+
+        self.AB_controller_ = AdaptiveBackstep()
+
+        self.update_controller_parameters()
+        
+        controller_period = 0.1
+        self.controller_timer_ = self.create_timer(controller_period, self.controller_callback)
+        self.add_on_set_parameters_callback(self.parameter_callback)
+
+        self.get_logger().info("hybridpath_controller_node started")
+
+    def update_controller_parameters(self):
+
         K1_diag = self.get_parameter('hybridpath_controller.K1_diag').get_parameter_value().double_array_value
         K2_diag = self.get_parameter('hybridpath_controller.K2_diag').get_parameter_value().double_array_value
         D_diag = self.get_parameter('physical.damping_matrix_diag').get_parameter_value().double_array_value
         M = self.get_parameter('physical.inertia_matrix').get_parameter_value().double_array_value
 
-        # Transform parameters to diagonal matrices
         K1 = np.diag(K1_diag)
         K2 = np.diag(K2_diag)
         D = np.diag(D_diag)
-
-        # Reshape M to a 3x3 matrix
         M = np.reshape(M, (3, 3))
         
-        # Create controller object
-        self.AB_controller_ = AdaptiveBackstep(K1, K2, M, D)
+        self.AB_controller_.update_parameters(K1, K2, M, D)
 
-        controller_period = 0.1
-        self.controller_timer_ = self.create_timer(controller_period, self.controller_callback)
+        self.get_logger().info("Updated controller parameters")
 
-        self.get_logger().info("hybridpath_controller_node started")
+    def parameter_callback(self, params):
+        self.parameters_updated = True
+        for param in params:
+            if param.name == 'hybridpath_controller.K1_diag':
+                self.get_logger().info(f"Updated K1_diag to {param.value}")
+            elif param.name == 'hybridpath_controller.K2_diag':
+                self.get_logger().info(f"Updated K2_diag to {param.value}")
+            elif param.name == 'physical.damping_matrix_diag':
+                self.get_logger().info(f"Updated damping_matrix_diag to {param.value}")
+            elif param.name == 'physical.inertia_matrix':
+                self.get_logger().info(f"Updated inertia_matrix to {param.value}")
+
+        # self.update_controller_parameters()
+        return SetParametersResult(successful=True)
 
     def state_callback(self, msg: Odometry):
         """
@@ -64,6 +96,10 @@ def controller_callback(self):
         """
         Callback function for the controller timer. This function calculates the control input and publishes the control input.
         """
+        if self.parameters_updated:
+                self.update_controller_parameters()
+                self.parameters_updated = False
+
         if hasattr(self, 'state_odom') and hasattr(self, 'reference'):
             control_input = self.AB_controller_.control_law(self.state_odom, self.reference)
             wrench_msg = Wrench()
@@ -72,6 +108,36 @@ def controller_callback(self):
             wrench_msg.torque.z = control_input[2]
             self.wrench_publisher_.publish(wrench_msg)
 
+            # Debug publishers
+            eta_error_msg = Float64MultiArray()
+            eta_error_msg.data = self.AB_controller_.get_eta_error().tolist()
+            self.eta_error_publisher.publish(eta_error_msg)
+
+            z1_msg = Float64MultiArray()
+            z1_msg.data = self.AB_controller_.get_z1().tolist()
+            self.z1_publisher.publish(z1_msg)
+
+            alpha1_msg = Float64MultiArray()
+            alpha1_msg.data = self.AB_controller_.get_alpha1().tolist()
+            self.alpha1_publisher.publish(alpha1_msg)
+
+            z2_msg = Float64MultiArray()
+            z2_msg.data = self.AB_controller_.get_z2().tolist()
+            self.z2_publisher.publish(z2_msg)
+
+            ds_alpha1_msg = Float64MultiArray()
+            ds_alpha1_msg.data = self.AB_controller_.get_ds_alpha1().tolist()
+            self.ds_alpha1_publisher.publish(ds_alpha1_msg)
+
+            sigma1_msg = Float64MultiArray()
+            sigma1_msg.data = self.AB_controller_.get_sigma1().tolist()
+            self.sigma1_publisher.publish(sigma1_msg)
+
+            tau_msg = Float64MultiArray()
+            tau_msg.data = control_input.tolist()
+            self.tau_publisher.publish(tau_msg)
+
+
 def main(args=None):
     rclpy.init(args=args)
     node = HybridPathControllerNode()