An implementation of the onboard state dependent LQR algorithm from Foehn and Scaramuzza (ICRA 2018) for agile quadrotor trajectory tracking. The goal was to verify the algorithm and extend it to support both Euler angle and quaternion representations for attitude.
The controller linearizes the nonlinear quadrotor dynamics along a reference trajectory at runtime and computes full state feedback gains using LQR. Implemented in C++ as a ROS node using MAVROS to interface with the PX4 autopilot. Tested on real hardware tracking minimum snap trajectories with motion capture localization.