Code in this repo was developed based on https://github.com/juanmed/quadrotor_sim/blob/master/3D_Quadrotor/3D_control_with_body_drag.py.
The dynamics decribed in Eq. (2.22) from this thesis was adopted. In order to make use of LQR, the nonlinear dynamics is first linearized around the equilibrium (phi = 0 and theta = 0). We refer the interested readers to this paper for details about linearization. Then, LQR is used to calculate a linear controller. The linear controller is then plugged in. The resulting closed-loop linear and nonlinear systems are evaluated either to follow some waypoints or to track a randomly generated trajectory. Finally, the realized trajectories and tracking error for both systems are plotted.
pip install scipy matplotlib# follow waypoints (-w x1 y1 z1 -w x2 y2 z2 -w x3 y3 z3 -w ... -T total simulation time). The quadrotor starts from the origin.
python 3D_quadrotor.py -T 20 -w 1 1 1 -w 1 1 2 -w 0 0 0
# track a randomly genearted trajectory
python 3D_quadrotor.py --seed 0