LQR control for quadrotor reference tracking

I am trying to implement an LQR controller for a quadrotor to perform reference tracking. My goal is to deploy this controller in a MuJoCo simulation using ROS2. The actuators of the drone are abstracted to resemble thrust, and x, y, and z moments. I use the linear model obtained from here (pages 20-21). From what I understand, this should be enough to get a good enough controller for non-extreme reference/trajectory tracking.

I computed the LQR gain $K$ using MATLAB. First, the linear dynamics are converted to discrete-time using c2d(), whence a discrete-time LQR controller is implemented using dlqr(). The system is discretized using the same sample time as the controller in ROS2, that is, $Ts = 0.002$. The closed-loop eigenvalues are checked, and indeed the closed-loop is stable with all eigenvalues $\lambda$ satisfying $\lambda < |1| $. The control law is $u = -K(x - x_{ref})$. Below is the plot for 3D position $(1, 1, 1)$. Note that $(x,y,z,u,v,w)$ are the position (world frame) and corresponding linear velocities (body frame) and $(phi, theta, psi, p, q, r)$ are the angles (world frame) and corresponding angular velocities (body frame).

My problem is the following. When I implement the LQR controller in my ROS2 node to control the drone in the simulation, with the gain obtained from MATLAB, only the thrust control input does what it's supposed to. However, the roll, pitch and yaw commands seem to be faulty since it gradually starts rolling and pitch until it crashes (despite the reference being $(0, 0, 0.5)$). When setting the yaw reference to $1$, it just starts spinning. My current suspicion is that the angles obtained from the resulting control input must be manipulated in some way before being published to the actuators of the drone.

Why does the LQR gain stabilize the linear system in MATLAB but not the drone in the simulation? Could there be a significant discrepancy in the dynamics? According to this project, I believe we have the same framework, so I guess it is doable. I'm really at a loss as to what the problem is.

#!/usr/bin/env python3 import rclpy from rclpy.node import Node import numpy as np from example_interfaces.msg import Float64, Float64MultiArray import math class LQRController(Node): def __init__(self): super().__init__('lqr_controller') # ---------- TARGETS ---------- self.x_ref = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5]) # ---------- STATES ---------- self.position = np.zeros(3) self.quaternion = np.array([1.0, 0.0, 0.0, 0.0]) self.x = np.zeros(12) self.last_position = np.zeros(3) self.last_angles = np.zeros(3) # ---------- GAIN ---------- self.K = np.array([ [-0.0000, -0.0000, -0.0000, -0.0000, -0.0000, -0.0000, 0.0000, -0.0000, -13.3334, 0.0000, -0.0000, -41.8947], [ 0.1223, 0.0000, 0.0000, 0.0121, 0.0000, -0.0000, -0.0000, 0.0572, 0.0000, 0.0000, 0.0750, -0.0000], [ 0.0000, 0.1223, -0.0000, -0.0000, 0.0121, -0.0000, -0.0572, 0.0000, 0.0000, -0.0750, 0.0000, 0.0000], [-0.0000, -0.0000, 0.0510, -0.0000, -0.0000, 0.0162, 0.0000, -0.0000, 0.0000, 0.0000, -0.0000, 0.0000] ]) # ---------- TIMING ---------- self.dt = 0.002 # 500 Hz # ---------- ROS ---------- # Publishers self.thrust_pub = self.create_publisher(Float64, '/cf2/actuators/body_thrust/command', 10) self.roll_pub = self.create_publisher(Float64, '/cf2/actuators/x_moment/command', 10) self.pitch_pub = self.create_publisher(Float64, '/cf2/actuators/y_moment/command', 10) self.yaw_pub = self.create_publisher(Float64, '/cf2/actuators/z_moment/command', 10) # Subscribers self.pos_sub = self.create_subscription(Float64MultiArray, '/cf2/pos', self.position_callback, 10) self.quat_sub = self.create_subscription(Float64MultiArray, '/cf2/quat', self.quaternion_callback, 10) # Loop timers self.loop_timer = self.create_timer(self.dt, self.control_loop) # Logging self.log_cycle = 0 self.get_logger().info("LQR initialized.") # ---------- CALLBACKS ---------- def position_callback(self, msg): self.position = np.array(msg.data) def quaternion_callback(self, msg): self.quaternion = list(msg.data) # ---------- Control LOOP ---------- def control_loop(self): # Extract Euler angles from quaternion qw, qx, qy, qz = self.quaternion roll = math.atan2(2*(qw*qx + qy*qz), 1 - 2*(qx*qx + qy*qy)) pitch = math.asin(max(min(2*(qw*qy - qz*qx), 1.0), -1.0)) yaw = math.atan2(2*(qw*qz + qx*qy), 1 - 2*(qy*qy + qz*qz)) cphi = np.cos(roll) sphi = np.sin(roll) ctheta = np.cos(pitch) stheta = np.sin(pitch) cpsi = np.cos(yaw) spsi = np.sin(yaw) ttheta = np.tan(pitch) R = np.array([ [ctheta * cpsi, sphi * stheta * cpsi - cphi * spsi, cphi * stheta * cpsi + sphi * spsi], [ctheta * spsi, sphi * stheta * spsi + cphi * cpsi, cphi * stheta * spsi - sphi * cpsi], [-stheta, sphi * ctheta, cphi * ctheta] ]) T = np.array([ [1, sphi * ttheta, cphi * ttheta], [0, cphi, -sphi], [0, sphi / ctheta, cphi / ctheta] ]) angles = np.array([roll, pitch, yaw]) avel = (angles - self.last_angles) / self.dt self.last_angles = angles avel = T.transpose() @ avel pos = self.position vel = (pos - self.last_position) / self.dt self.last_position = self.position vel = R.transpose() @ vel self.x = np.array([roll, pitch, yaw, avel[0], avel[1], avel[2], vel[0], vel[1], vel[2], pos[0], pos[1], pos[2]]) error = self.x - self.x_ref u = -self.K.dot(error) # Publish actuator commands self.thrust_pub.publish(Float64(data=-u[0])) self.roll_pub.publish(Float64(data=u[1])) self.pitch_pub.publish(Float64(data=u[2])) self.yaw_pub.publish(Float64(data=u[3])) # Log at 10Hz self.log_cycle += 1 if self.log_cycle % 50 == 0: self.get_logger().info( f"Pos [{float(pos[0]):.3f}, {float(pos[1]):.3f}, {float(pos[2]):.3f}] | " f"Thrust {float(u[0]):.3f}" ) def main(args=None): rclpy.init(args=args) node = LQRController() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()