<feat> LQR controller for pitch yaw and surge with working and stable…

… values.

control/velocity_controller_lqr/scripts/velocity_controller_lqr.py

@@ -29,15 +29,20 @@ def quaternion_to_euler_angle(w, x, y, z):
 
     return X, Y, Z
 
+# A function to convert the angle to the smallest signed angle
+def ssa(angle):
+    if angle > np.pi:
+        angle -= 2 * np.pi
+    elif angle < -np.pi:
+        angle += 2 * np.pi
+    return angle    
 
 #  Function to calculate the coriolis matrix
-def calculate_coriolis_matrix(self, pitch_rate, yaw_rate):
-    return np.array([[1.629 * pitch_rate, 0], [0, 1.769 * yaw_rate]])
-
+def calculate_coriolis_matrix(self, pitch_rate, yaw_rate, sway, heave):
+    return np.array([[0.2,-30*sway*0.01, -30*heave*0.01], [30*sway*0.01, 0, 1.629 * pitch_rate], [30*heave*0.01 ,1.769 * yaw_rate, 0]])
 
 #----------------------------------------------------------------Controller Node----------------------------------------------------------------
 
-
 class VelocityLQRNode(Node):
 
     def __init__(self):
@@ -56,50 +61,57 @@ def __init__(self):
         self.topic_subscriber = self.create_subscription(
             Odometry, "/nucleus/odom", self.nucleus_callback, 10)
 
-        self.guidance_values = np.array([np.pi / 4, np.pi / 4
+        self.guidance_values = np.array([0.4, -np.pi / 8, -np.pi / 4
                                          ])  # guidance values TEMPORARY
         self.guidance_values_aug = np.array(
-            [np.pi / 4, np.pi / 4, 0.0])  # augmented guidance values TEMPORARY
+            [0.4, -np.pi / 8, -np.pi / 4, 0.0, 0.0, 0.0])  # augmented guidance values TEMPORARY
 
         # TODO: state space model, Anders showed me the chapter in the book from page 55 on for this
-        self.M = np.array([[1.629, 0],
-                           [0, 1.769]])  # mass matrix with mass = 30kg
+        self.M = np.array([[30, 0.6, 0],
+                           [0.6, 1.629, 0],
+                           [0, 0, 1.769]])  # mass matrix with mass = 30kg
+
         self.M_inv = np.linalg.inv(self.M)  # inverse of mass matrix
-        self.C = np.array([[0, 0], [0, 0]])
+
+        self.C = np.zeros((3, 3))  # Coriolis matrix
+        self.A = np.eye(3)  # depending on number of states
+        self.B = np.eye(3)  # depending on number of control inputs
 
-        self.A = np.eye(2)  # depending on number of states
-        self.B = np.eye(2)  # depending on number of control inputs
-
-        self.A_aug = np.eye(3)  # Augmented A matrix
-        self.B_aug = np.eye(3)  # Augmented B matrix
+        self.A_aug = np.eye(6)  # Augmented A matrix
+        self.B_aug = np.eye(6, 3)  # Augmented B matrix
 
         # LQR controller parameters
-        self.Q = np.diag([300, 25])  # state cost matrix
-        self.R = np.eye(2) * 0.5  # control cost matrix
-
-        P_I = 0.5  # Augmented state cost for pitch
-        Y_I = 0.0  # Augmented state cost for yaw
+        self.Q = np.diag([100, 100, 25])  # state cost matrix
+        self.R = np.diag([0.5, 0.5, 0.5]) # control cost matrix
 
-        self.Q_aug = np.block([[self.Q, np.zeros((2, 1))],
-                               [np.zeros((1, 2)),
-                                P_I]])  # Augmented state cost matrix
+        self.I_surge = 0.5 # Augmented state cost for surge
+        self.I_pitch = 0.5  # Augmented state cost for pitch
+        self.I_yaw = 0.1 # Augmented state cost for yaw
+
+        self.Q_aug = np.block([[self.Q, np.zeros((3, 3))],
+                               [np.zeros((3, 3)), np.diag([self.I_surge, self.I_pitch, self.I_yaw])]])  # Augmented state cost matrix
 
-        self.z = 0.0
+        self.z_surge = 0.0
+        self.z_pitch = 0.0
+        self.z_yaw = 0.0    # Augmented states for surge, pitch and yaw
 
-        # State vector 1. pitch, 2. yaw
-        self.states = np.array([0, 0])
+        # State vector 1. surge, 2. pitch, 3. yaw
+        self.states = np.array([0.0, 0.0, 0.0])
 
 #---------------------------------------------------------------Callback Functions---------------------------------------------------------------
 
     def nucleus_callback(self, msg: Odometry):  # callback function
 
-        dummy, self.states[0], self.states[1] = quaternion_to_euler_angle(
+        dummy, self.states[1], self.states[2] = quaternion_to_euler_angle(
             msg.pose.pose.orientation.w, msg.pose.pose.orientation.x,
             msg.pose.pose.orientation.y, msg.pose.pose.orientation.z)
 
-    def guidance_callback(
-            self,
-            msg: Float32MultiArray):  # Function to read data from guidance
+        self.states[0] = msg.twist.twist.linear.x
+
+        self.C = calculate_coriolis_matrix(
+            self, msg.twist.twist.angular.y, msg.twist.twist.angular.z, msg.twist.twist.linear.y, msg.twist.twist.angular.z) #coriolis matrix
+
+    def guidance_callback(self, msg: Float32MultiArray):  # Function to read data from guidance
         # self.guidance_values = msg.data
         pass
 
@@ -111,47 +123,50 @@ def LQR_controller(self):  # The LQR controller function
         # Coriolis matrix
         #self.C = calculate_coriolis_matrix(self, msg.twist.twist.angular.x, msg.twist.twist.angular.z)
 
-        self.C = calculate_coriolis_matrix(
-            self, 1.0, 1.0)  # TEMPORARY linearization of the coriolis matrix
         self.A = -self.M_inv @ self.C
         self.B = self.M_inv
 
-        self.A_aug = np.block([[self.A, np.zeros((2, 1))],
-                               [-1, 0, 0]])  # Augmented A matrix for pitch
-        self.B_aug = np.block([[self.B], [0, 0]])  # Augmented B matrix
+        # Augment the A and B matrices for integrators for surge, pitch, and yaw
+        self.A_aug = np.block([[self.A, np.zeros((3, 3))], [-np.eye(3), np.zeros((3,3))]])  # Integrators added for surge, pitch, and yaw
+        self.B_aug = np.block([[self.B], [np.zeros((3, 3))]])  # Control input does not affect integrators directly
 
         # CT LQR controller from control library python
         self.K, self.S, self.E = ct.lqr(self.A, self.B, self.Q, self.R)
-        self.K_aug, self.S_aug, self.E_aug = ct.lqr(self.A_aug, self.B_aug,
-                                                    self.Q_aug, self.R)
+        self.K_aug, self.S_aug, self.E_aug = ct.lqr(self.A_aug, self.B_aug, self.Q_aug, self.R)
+
+        surge_error = self.guidance_values[0] - self.states[0]  # Surge error no need for angle wrapping
+        pitch_error = ssa(self.guidance_values[1] - self.states[1])  # Apply ssa to pitch error
+        yaw_error = ssa(self.guidance_values[2] - self.states[2])    # Apply ssa to yaw error
+
+        # self.get_logger().info(f"{surge_error}, {pitch_error}, {yaw_error}")
+
+        # Update the integrators for surge, pitch, and yaw
 
-        # Control input like: u = -self.K * (desired-states)
-        #u = self.K @ (self.guidance_values - self.states)
-        self.z = self.z + self.guidance_values[0] - self.states[0]
-        u_aug = self.K_aug @ (self.guidance_values_aug -
-                              np.block([self.states, self.z]))
+        self.z_surge += self.I_surge*surge_error*1.5  # surge integrator
+        self.z_pitch += self.I_pitch*pitch_error*2 # pitch integrator
+        self.z_yaw += self.I_yaw*yaw_error # yaw integrator
 
-        # Control input
-        #msg.torque.y = u[0]
-        #msg.torque.z = u[1]
+         # Augmented state vector including integrators
+        u_aug = -self.K_aug @ (-surge_error, -pitch_error, -yaw_error, self.z_surge, self.z_pitch, self.z_yaw)  # Augmented control input
 
         # Augmented control input
-        msg.torque.y = u_aug[0]
-        msg.torque.z = u_aug[1]
+        msg.force.x = u_aug[0]
+        msg.torque.y = u_aug[1]
+        msg.torque.z = u_aug[2]
 
         #Publish the control input
         self.publisherLQR.publish(msg)
+
         self.get_logger().info(
-            f"Pitch input: {msg.torque.y}, Yaw input: {msg.torque.z}")
+            f"Surge input: {msg.force.x} Pitch input: {msg.torque.y}, Yaw input: {msg.torque.z}")
 
     def publish_states(self):
         msg = Float32MultiArray()
 
-        # DATA: 1: pitch, 2: yaw, 3: guidance pitch, 4: guidance yaw
+        # DATA: 1: surge 2: pitch, 3: yaw, 4: guidance surge 5: guidance pitch, 6: guidance yaw
         msg.data = [
-            self.states[0], self.states[1], self.guidance_values[0],
-            self.guidance_values[1]
-        ]
+            self.states[0], self.states[1], self.states[2], self.guidance_values[0], self.guidance_values[1],
+            self.guidance_values[2]]
         self.publisher_states.publish(msg)