Hi @HoaiThanh -
Happy to help. When in depth-hold mode, the ArduSub autopilot firmware will automatically control both heading and depth, allowing you to higher level commands to change these set-points. I’m not sure what advantage your “LQR” controller would have, or if it is even necessary vs. what the system already provides!
To not conflict with the autopilot, you would need to either disable it and use Navigator web assistant for direct controls of outputs, or set the vehicle to manual mode and manually send simulated joystick input, or individually control motor throttle levels via ArduSub (for exmple, via pymavlink.)