Drone unstable in offboard direct motor control

Hello community,

Background:
I have implemented an LQR controller to control a quadcopter using direct motor commands. I finally want this to perform some trajectory tracking. I developed and evaluated the code and performance in sim using the toolchain available for ROS2 and PX4. Then I moved to hardware implementation, but the drone is unstable, and it crashes in the end. I need help, ideas from you all to figure out why it is happening in hardware implementation but not in sim.

Setup:
Drone: Holybro S500
Board: pixhawk 4
firmware: release/1.14
motion capture: Optitrack mocap

A simple hover experiment:
In sim and experiment I am trying to get the drone to hover at some altitude.

Simulation:
I subscribe to VehicleLocalPosition, VehicleAttitude and VehicleAngularVelocity messages to get the full state feedback. Then calculate the thrust using this equation u = u_0 + K * (x - x_0) and publish the thrust values at 100hz to ActuatorMotors message. Here is the video of the simulation VIDEO.

Experiment:
I do the same as above. One thing extra I do is take the mocap position and attitude data and pass it to EKF2 message for better estimates.
Here is the video of the experiment VIDEO. After the crash I kill the motors.

Question:

  • Is the publishing frequency of thrust values slow? Should I go higher than 100hz? It works in sim at 100hz but some assumptions in sim might not be valid in experiment.
  • In the sim I tried going higher, but nothing works above 125hz. Drone is stable between only 40-125hz. Weird! Why?
  • Is there any way to get EKF2 feedback at higher frequencies? I get feedback at 80hz, 165hz, 350hz for the above three messages that I subscribe respectively in experiment. I get feedback at 125hz, 250hz, 250hz for the above three messages that I subscribe respectively in gazebo simulation.