# Quadrotor Formation Strategies Based on Distributed Consensus and Model Predictive Controls
## 3 Dynamic Model of a Quadrotor
### EOM of the Quadrotor
\begin{array}{ccc}C_{\theta}C_{\psi}&C_{\theta}S_{\psi}& -S_{\theta}\\ S_{\phi}S_{\theta}C_{\psi}-C_{\phi}S_{\psi}&S_{\phi}S_{\theta}S_{\psi}+C_{\phi} C_{\psi}&S_{\phi}C_{\theta}\\ C_{\phi}S_{\theta}C_{\psi}+S_{\phi}S_{\psi}&C_{\phi}S_{\theta}S_{\psi}-S_{\phi} C_{\psi}&C_{\phi}C_{\theta}\end{array} \right], \tag{5}\]  
where \(S_{(\cdot)}=\sin(\cdot)\) and \(C_{(\cdot)}=\cos(\[\Rightarrow\left[ \begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{z}\end{array} \right]=\left[ \begin{array}{cc}C_{\theta}C_{\phi}&S_{\phi}S_{ \theta}C_{\psi}-C_{\phi}S_{\psi}&C_{\phi}S_{\theta}C_{\psi}+S_{\phi}S_{\psi}\\ C_{\theta}S_{\psi}&S_{\phi}S_{\theta}S_{\psi}+C_{\phi}C_{\psi}&C_{\phi}S_{\theta} S_{\psi}-S_{\phi}C_{\psi}\\ -S_{\theta}&S_{\phi}C_{\theta}&C_{\phi}C_{\theta}\end{array} \right]\left[ \begin{array}{c}u\\ v\\ w\end{array} \right], \tag{8}\]  
where \((x,y,z)\) are the position of the quadrotor with respect to frame \(\mathbf{I}\). Additionally, the transformation between the angular velocity and the rate change of the Euler angles is shown as follows:  
\[\left[ \begin{array}{c}\dot{\phi}\\ \dot{\phi}\\ \dot{\psi}\end{array} \right]=\left[ \begin{array}{cc}1&S_{\phi}\tan\theta&C _{\phi}\tan\theta\\ 0&C_{\phi}&-S_{\phi}\\ 0&S_{\phi}\sec\theta&C_{\phi}\sec\theta\end{array} \right]\left[ \begin{array}[ ]{c}p\\ q\\ r\end{array} \right]. \tag{9}\]  
Finally, the EOMs of the quadrotor can be obtained with the above definitions and transformations. In this study, the formation control is decoupled into horizontal and vertical motions. Therefore, the EOMs of the quadrotor are arranged in a similar form. In the horizontal motion, states \((x,y,u,v,\theta,\phi,q,p)\) are considered, and the EOMs are arranged as follows:  
\[ \begin{array}{c}\mathit{Pitch\ Axis:}\ \left[\begin{array}{c}\dot{x}\\ \dot{u}\\ \dot{\theta}\\ \dot{q}\end{array}\right]=\ \left[\begin{array}{c}(C_{\theta}C_{\psi})u+(S_{ \phi}S_{\theta}C_{\phi}-C_{\phi}S_{\psi})v+(C_{\phi}S_{\theta}C_{\psi}+S_{\phi} S_{\psi})w\\ rv-qw-g\sin\theta\\ qC_{\phi}-rS_{\phi}\\ \frac{rp(I_{xx}-I_{xx})}{I_{yy}}+\frac{1}{I_{yy}}M_{\theta}\end{array}\right], \\ \mathit{Roll\ Axis:}\ \left[\begin{array}{c}\dot{y}\\ \dot{v}\\ \dot{\phi}\\ \dot{p}\end{array}\right]=\ \left[\begin{array}{c}(C_{\theta}S_{\psi})u+(S_{ \phi}S_{\theta}S_{\psi}+C_{\phi}C_{\phi})v+(C_{\phi}S_{\theta}S_{\psi}-S_{ \phi}C_{\psi})w\\ pw-ru+g\sin\phi\cos\theta\\ p+qS_{\phi}\tan\theta+rC_{\phi}\tan\theta\\ \frac{qr(I_{yy}-I_{xx})}{I_{xx}}+\frac{1}{I_{xx}}M_{\phi}\end{array}\right]. \\ \end{array}  \tag{10}\]  
On the other hand, the EOMs considering the states \((z,w,\psi,r)\) in the vertical motion are arranged as follows:  
\[\left[ \begin{array}{c}\dot{z}\\ \dot{w}\\ \dot{\psi}\\ \dot{r}\end{array} \right]=\left[ \begin{array}{c}(-S_{\theta})u+(S_{\phi}C_{ \theta})v+(C_{\phi}C_{\theta})w\\ qu-pv+g\cos\phi\cos\theta-\frac{1}{m}T_{total}\\ qS_{\phi}\sec\theta+rC_{\phi}\sec\theta\\ \frac{pq(I_{xx}-I_{yy})}{I_{zz}}+\frac{1}{I_{zz}}M_{\psi}\end{array} \right]. \tag{11}\]  
In Equations (10) and (11), \(M_{\theta},M_{\phi},M_{\psi}\), and \(T_{total}\) are the controls and defined as follows:  
\[\begin{array}{c}M_{\phi}=\ \ \ \frac{\sqrt{2}}{2}L(T_{1}+T_{2}-T_{3}-T_{4}),\\ M_{\theta}=\ \ \ \frac{\sqrt{2}}{2}L(T_{1}-T_{2}-T_{3}+T_{4}),\\ M_{\psi}=\Replacing the states in Equations (10) and (11) by the sum of the equilibrium point and the small disturbance of the states, and ignoring the cross-product terms of the small disturbance of the states, the linearized EOMs of the quadrotor in the horizontal and vertical motions can be obtained as follows:  
\[\begin{split}\text{\emph{Pitch Axis}}:\left[ \begin{array}{c} \Delta\dot{x}\\ \Delta\dot{u}\\ \Delta\dot{\theta}\\ \Delta\dot{q}\\ \end{array} \right]&=\ \ \left[ \begin{array}{cccc}0&1&0&0\\ 0&0&-g&0\\ 0&0&0&1\\ 0&0&0&0\\ \end{array} \right]\left[ \begin{array}{c}\Delta x\\ \Delta u\\ \Delta\theta\\ \Delta q\\ \end{array} \right]+\left[ \begin{array}{c}0\\ 0\\ 0\\ \frac{1}{I_{yy}}\\ \end{array} \right]\Delta M_{\theta},\\ \text{\emph{Roll Axis}}:\left[