diff --git a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
index 317332d..1ddbf8c 100755
--- a/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
+++ b/guidance/dp_guidance/dp_guidance/dp_guidance_node.py
@@ -41,7 +41,7 @@ def __init__(self):
         self.eta_received = False
         self.eta_logger = True
 
-        self.yaw_ref = 0
+        self.yaw_ref = 0.
 
         self.xd = np.zeros(9)
 
@@ -52,6 +52,7 @@ def __init__(self):
         self.timer = self.create_timer(timer_period, self.guidance_callback)
 
     def waypoint_callback(self, request, response):
+        self.init_pos = False
         self.waypoints = request.waypoint
         self.get_logger().info(f'Received waypoints: {self.waypoints}')
         self.xd = np.zeros(9)
@@ -64,6 +65,7 @@ def eta_callback(self, msg: Odometry):
         self.eta = odometrymsg_to_state(msg)[:3]
         self.eta_received = True
         self.yaw_publisher.publish(Float32(data=self.eta[2]))
+        self.yaw_ref_publisher.publish(Float32(data=self.yaw_ref))
 
     def guidance_callback(self):
         if self.waypoints_received and self.eta_received:
diff --git a/guidance/dp_guidance/dp_guidance/reference_filter.py b/guidance/dp_guidance/dp_guidance/reference_filter.py
index 1ad4f34..45b243f 100644
--- a/guidance/dp_guidance/dp_guidance/reference_filter.py
+++ b/guidance/dp_guidance/dp_guidance/reference_filter.py
@@ -2,7 +2,6 @@
 
 import numpy as np
 import control
-import matplotlib.pyplot as plt
 
 class ReferenceFilter:
     def __init__(self):
@@ -50,23 +49,3 @@ def rotationMatrix(psi):
                 [np.sin(psi), np.cos(psi), 0],
                 [0, 0, 1]])
         return R
-
-
-if __name__ == "__main__":
-    refFilter = ReferenceFilter()
-    ref_pos = np.array([5, 10, 0])
-    t = np.arange(0, 100, 0.1)
-    N = len(t)
-    x_d = np.zeros((N, 9))
-
-    for k in range(1,N):
-        x_d[k,:] = refFilter.step(ref_pos, x_d[k-1, :])
-
-    eta_d = refFilter.get_eta(x_d)
-    nu_d = refFilter.get_nu(x_d)
-    plt.figure()
-    plt.plot(t, eta_d[:,0])
-
-    plt.figure()
-    plt.plot(t, nu_d[:,0])
-    plt.show()
\ 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 59b5df3..c0e6066 100755
--- a/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
+++ b/guidance/hybridpath_guidance/hybridpath_guidance/waypoint_node.py
@@ -39,7 +39,8 @@ 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] + 0.]]
+            wp_list = [[self.eta[0] + 5., self.eta[1]], [self.eta[0] + 5., self.eta[1] + 5.], [self.eta[0], self.eta[1] + 5.]]
+            # 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/pid_controller/pid_controller/pid_controller.py b/motion/pid_controller/pid_controller/pid_controller.py
index 1bd218e..29624c6 100644
--- a/motion/pid_controller/pid_controller/pid_controller.py
+++ b/motion/pid_controller/pid_controller/pid_controller.py
@@ -10,6 +10,9 @@ def __init__(self):
         self.Ki = np.eye(3)
         self.Kd = np.eye(3)
 
+        self.upper = np.array([40, 40, 40])
+        self.lower = np.array([-40, -40, -40])
+
         # self.tau_max = np.array([100, 100, 100])
 
     def update_parameters(self, Kp, Ki, Kd):
@@ -23,7 +26,7 @@ def calculate_control_input(self, eta, eta_d, eta_dot, dt):
         self.error_sum = np.clip(self.error_sum, -20, 20)
 
         p = self.Kp @ error
-        i = 0 #self.Ki @ self.error_sum
+        i = self.Ki @ self.error_sum
         d = self.Kd @ eta_dot
 
         self.last_error = error
@@ -45,4 +48,15 @@ def calculate_control_input(self, eta, eta_d, eta_dot, dt):
         # elif tau[2] < -self.tau_max[2]:
         #     tau[2] = -self.tau_max[2]
 
-        return tau
\ No newline at end of file
+        return tau
+    
+    @staticmethod
+    def cap_array(arr: np.ndarray, lower: np.ndarray, upper: np.ndarray):
+        new_arr = np.zeros(3)
+        for i in range(len(arr)):
+            if arr[i] > upper[i]:
+                new_arr[i] = upper[i]
+            elif arr[i] < lower[i]:
+                new_arr[i] = lower
+            else:
+                new_arr[i] = arr[i]
diff --git a/motion/pid_controller/pid_controller/pid_controller_node.py b/motion/pid_controller/pid_controller/pid_controller_node.py
index dfff3a0..a8927ea 100755
--- a/motion/pid_controller/pid_controller/pid_controller_node.py
+++ b/motion/pid_controller/pid_controller/pid_controller_node.py
@@ -60,6 +60,8 @@ def __init__(self):
         self.get_logger().info("pid_controller_node started")
 
     def update_controller_parameters(self):
+        self.pid_controller.error_sum = np.zeros(3)
+
         Kp = self.get_parameter('pid_controller.Kp').get_parameter_value().double_array_value
         Ki = self.get_parameter('pid_controller.Ki').get_parameter_value().double_array_value
         Kd = self.get_parameter('pid_controller.Kd').get_parameter_value().double_array_value