# Backstepping Based Formation Control of Quadrotors with the State Transformation Technique
## 2 Dynamic Model of a Quadrotor  
First of all, to derive the equations for the movement of a quadrotor, we have to define the coordinates of a quadrotor system. The generalized coordinates and each thrust of a quadrotor system are shown in Figure 1, where \(B\) and \(E\) denote the body-fixed and Earth inertial frames, respectively. The kinematics of a generic six-DOF rigid-body is described as follows:  
\[\dot{\xi}= \begin{bmatrix}R&0_{3\times 3}\\ 0_{3\times 3}&T\end{bmatrix} \upsilon, \tag{1}\]  
where \(\dot{\xi}= \begin{bmatrix}\Gamma^{E}&\Theta^{E}\end{bmatrix} ^{T}\) and \(\upsilon= \begin{bmatrix}V^{B}&\omega^{B}\end{bmatrix} ^{T}\) are the velocity vectors with respect to the Earth inertial and body-fixed frames, respectively. Here, \(\Gamma^{E}=(X,\ Y,\ Z)\), \(\Theta^{E}=(\phi,\ \theta,\ \psi)\), \(V^{B}=(u,\ v,\ w)\) and \(\omega^{B}=(p,\ q,\ r)\). Additionally, \(0_{3\times 3}\) is a three by three submatrix filled with all zeros; the coordinate transformation matrix from the body-fixed frame to the Earth inertial frame \(R\), the angle-rates transformation matrix from the body-fixed frame to the Earth-inertial frame \(T\) are defined, respectively, as follows:  
\[R= \begin{bmatrix}c_{\psi}c_{\theta}&-s_{\psi}c_{\phi}+c_{\psi}s_{ \theta}s_{\phi}&s_{\psi}s_{\phi}+c_{\psi}s_{\theta}c_{\phi}\\ s_{\psi}c_{\theta}&c_{\psi}c_{\phi}+s_{\psi}s_{\theta}s_{\phi}&-c_{\psi}s_{ \phi}+s_{\psi}s_{\theta}c_{\phi}\\ -s_{\theta}&s_{\phi}c_{\theta}&c_{\phi}c_{\theta}\end{bmatrix} , \tag{2}\]  
\[T= \begin{bmatrix}1&s_{\phi}t_{\theta}&c_{\phi}t_{\theta}\\ 0&c_{\phi}&-s_{\phi}\\ 0&s_{\phi}/c_{\theta}&c_{\phi}/c_{\theta}\end{bmatrix} , \tag{3}\]  
where \(c_{k}=cos(k)\), \(s_{k}=sin(k)\) and \(t_{k}=tan(k)\). To describe the quadrotor dynamic model, we use the Newton-Euler formalism. The dynamics of a generic six-DOF rigid-body takes into account the mass of the body and its inertia. The dynamics is described by:  
\[ \begin{bmatrix}m&0_{3\times 3}\\ 0_{3\times 3}&I_{3\times 3}\end{bmatrix}\begin{bmatrix}\dot{V}^{B}\\ \dot{\omega}^{B}\end{bmatrix} + \begin{bmatrix}\omega^{B}\times(\mathrm{m}V^{B} )\\ \omega^{B}\times(I\omega^{B})\end{bmatrix} = \begin{bmatrix}F^{B}\\ \tau^{B}\end{bmatrix} , \tag{4}\]  
where \(m\) is the mass of a quadrotor and \(I_{3\times 3}\) denotes a three by three identity matrix. The matrix, \(F^{B}\), is the force vector; \(\tau^{B}\) is the torque vector of the body-fixed frame. Finally, the dynamics of a quadrotor represented by (4) can be rewritten as follows:  
Figure 1: The generalized coordinates and thrusts of a quadrotor. \[ \begin{split}\ddot{X}&=\big{(}cos(\phi)sin(\theta)cos(\psi)+ sin(\phi)sin(\psi)\big{)}\frac{u_{1}}{m},\\ \ddot{Y}&=\big{(}cos(\phi)sin(\theta)sin(\psi)-sin( \phi)cos(\psi)\big{)}\frac{u_{1}}{m},\\ \dot{Z}&=\big{(}cos(\theta)cos(\phi)\big{)}\frac{u_{ 1}}{m}-g,\\ \ddot{\phi}&=\dot{\phi}\dot{\psi}\frac{(I_{yy}-I_{ zz})}{I_{xx}}-\frac{J}{I_{xx}}\dot{\theta}\Omega_{r}+\frac{u_{2}}{I_{xx}},\\ \ddot{\theta}&=\dot{\psi}\dot{\phi}\frac{(I_{zz}-I_{ xx})}{I_{yy}}+\frac{J}{I_{yy}}\dot{\phi}\Omega_{r}+\frac{u_{3}}{I_{yy}},\\ \ddot{\psi}&=\dot{\phi}\dot{\phi}\frac{(I_{xx}-I_{ yy})}{I_{zz}}+\frac{u_{4}}{I_{zz}}.\end{split}  \tag{5}\]  
where \(J\) is the rotor inertia, \(I_{kk}\) (for \(k=x,\ y,\ z\)) is the total inertia moment of each axis of a quadrotor, \(g\) is the gravitational acceleration and \(u_{i}(i=1,2,3,4)\) are the altitude, the roll, the pitch and the yaw control inputs represented by:  
\[ \begin{split} u_{1}&=b({\Omega_{1}}^{2}+{\Omega_{2 }}^{2}+{\Omega_{3}}^{2}+{\Omega_{4}}^{2})\\ u_{2}&=l\cdot b({\Omega_{1}}^{2}-{\Omega_{3}}^{2}) \\ u_{3}&=l\cdot b({\Omega_{2}}^{2}-{\Omega_{4}}^{2}) \\ u_{4}&=d({\Omega_{1}}^{2}+{\Omega_{3}}^{2}-{\Omega_{2 }}^{2}-{\Omega_{4}}^{2})\\ \Omega_{r}&=\Omega_{1}+\Omega_{3}-\Omega_{2}-\Omega_ {4}.\end{split}