## 2 Mathematical Model of Quadcopter
### Forces and Moment  
The following terms are used in the control optimization process presented in this work:  
* **The moment of inertia or angular mass** is the rotating body's resistance to angular acceleration or angular deceleration. It is best described as the product of the mass and the square of its perpendicular distance from the axis of rotation.
* **Torque** is a measure of how much force is applied on an object, causing the object to rotate. It can be defined mathematically as \(\tau=r\times F\), where \(F\) is the force and \(r\) is the moment arm.  
After getting the relationship between the body frame and the inertial frame, we can calculate the forces and torque acting on the quadcopter so that we can control the movements of the quadcopter. The thrust force acting upwards is given as follows:  
\[T_{B}=\sum_{i=1}^{4}T_{i}=k\begin{bmatrix}0\\ 0\\ \sum_{i=1}^{4}\omega_{i}^{2}\end{bmatrix} \tag{9}\]  
The torque force is the rotational force operating on the body of the quadcopter, making the body rotate in a specific direction.  
\[\tau_{B}=\begin{bmatrix}L\cdot k\cdot(-\omega_{1}^{2}+\omega_{2}^{2})\\ L\cdot k\cdot(-\omega_{2}^{2}+\omega_{4}^{2})\\ b\cdot(-\omega_{1}^{2}+\omega_{2}^{2}-\omega_{3}^{2}+\omega_{4}^{2}\end{bmatrix} \tag{10}\]  
Figure 2: Block diagram of a quadcopter model.  
In the \(E\) frame, the linear acceleration of a quadcopter is a combination of gravitational force and thrust force.  
\[\begin{bmatrix}\ddot{x}\\ \dot{y}\\ \dot{z}\end{bmatrix}=\begin{bmatrix}0\\ 0\\ -g\end{bmatrix}+T_{B}/m\cdot\begin{bmatrix}\cos\psi\sin\theta\cos\phi+\sin\phi \sin\theta\\ \sin\phi\sin\theta\cos\phi-\cos\psi\sin\phi\\ \cos\theta\cos\phi\end{bmatrix} \tag{11}\]  
where \(x\) is a positional vector, \(\dot{x}\) is the linear acceleration, \(g\) is the gravity force, and \(T_{B}\) is the thrust vector. The motion of angular acceleration can be defined as follows:  
\[\begin{bmatrix}M_{x}\\ M_{y}\\ M_{z}\end{bmatrix} = \begin{bmatrix}I_{xx}&0&0\\ 0&I_{yy}&0\\ 0&0&I_{zz}\end{bmatrix}\cdot\begin{bmatrix}\dot{p}\\ \dot{q}\\ \dot{r}\end{bmatrix}\] \[+ \begin{bmatrix}0&-r&q\\ r&0&-p\\ -q&p&0\end{bmatrix}\cdot\begin{bmatrix}I_{xx}&0&0\\ 0&I_{yy}&0\\ 0&0&I_{zz}\end{bmatrix}\cdot\begin{bmatrix}p\\ q\\ r\end{bmatrix}\]  
\[\begin{bmatrix}M_{x}\\ M_{y}\\ M_{z}\end{bmatrix} = \begin{bmatrix}I_{xx}\cdot\dot{p}\\ I_{yy}\cdot\dot{q}\\ I_{zz}\cdot\dot{r}\end{bmatrix}\begin{bmatrix}-I_{yy}\cdot qr+I_{zz}\cdot qr\\ I_{xx}\cdot pr-I_{zz}\cdot pr\\ -I_{xx}\cdot pq+I_{yy}\cdot pq\end{bmatrix}\] \[\begin{bmatrix}\dot{p}\\ \dot{q}\\ \dot{r}\end{bmatrix} = \begin{bmatrix}\frac{M_{x}+(I_{yy}-I_{zz})\cdot qr}{I_{xx}}\\ \frac{M_{y}+(I_{zz}-I_{xx})\cdot qr}{I_{yy}}\\ \frac{M_{z}+(I_{xx}-I_{yy})\cdot pq}{I_{zz}}\end{bmatrix} \tag{14}\]  
where \(\omega\) is the angular velocity, \(\tau\) is the torque and \(I\) is the moment of inertia. The translation and rotation movements can be defined as follows:  
\[\dot{x} = \frac{(\cos\psi\sin\theta\cos\phi+\sin\phi\sin\theta)\cdot T_{B}}{ m} \tag{15}\] \[\dot{y} = \frac{(\sin\phi\sin\phi\cos\phi-\cos\psi\sin\phi)\cdot T_{B}}{m}\] (16) \[\dot{z} = \frac{(\cos\phi\cos\theta)\cdot T_{B}}{m}\] (17) \[\dot{p} = \frac{M_{x}+(I_{yy}-I_{zz})\cdot qr}{I_{xx}}\] (18) \[\dot{q} = \frac{M_{y}+(I_{zz}-I_{xx})\cdot qr}{I_{yy}}\] (19) \[\dot{r} = \frac{M_{z}+(I_{xx}-I_{yy})\cdot pq}{I_{zz}} \tag{20}\]