# Real-Time Terrain-Following of an Autonomous Quadrotor by Multi-Sensor Fusion and Control
## 2 Quadrotor System Models
### Quadrotor Dynamic Models
\right] \in R^{3}\) represents the total torque, \(I\) is the inertia matrix of the quadrotor, which can be simplified as follows according to the symmetry of the quadrotor:  
\[I=\left[ \begin{array}{cccccccc}I_{xx}&0&0\\ 0&I_{yy}&0\\ 0&0&I_{zz}\end{array} \right], \tag{6}\]  
where \(I_{xx},\;I_{yy},\;I_{zz}\) are called the central principal moments of inertia and can be measured through experiments. Therefore, the dynamic model of the quadrotor in the airframe coordinate system is:  
\[\left\{ \begin{array}{l}f_{x}=m(\dot{u}+qw-rv)\\ f_{y}=m(\dot{v}-pv+ru)\\ f_{z}=m(\dot{w}+pv-qu)\\ \tau_{x}=\dot{p}I_{xx}-qrI_{yy}+qrI_{zz}\\ \tau_{y}=\dot{q}I_{yy}+prI_{xx}-prI_{zz}\\ \tau_{z}=\dot{r}I_{zz}-pqI_{xx}+pqI_{yy}\end{array} \right.. \tag{7}\]The above equation is established under the condition that the origin and axis of the airframe coordinate system coincide with the centroid and main axis of the quadrotor. The total external force received by the quadrotor in the airframe coordinate system is:  
\[f_{b}=mgR^{T}\cdot\hat{e}_{z}-f_{t}\hat{e}_{3}+f_{w}, \tag{8}\]  
where \(\hat{e}_{z}\) is the unit vector of the z-axis of the geographic coordinate system, \(\hat{e}_{3}\) is the unit vector of the z-axis of the airframe coordinate system, \(g\) is the acceleration of gravity, \(f_{t}\) is the total pulling force generated by motors and \(f_{w}\) is the force generated by the wind acting on the quadrotor. Ignoring the rotational torque in the control equation, the total external torque T\({}_{b}\) in the airframe coordinate system can be expressed as:  
\[\text{T}_{b}=\tau_{b}+\tau_{w}, \tag{9}\]  
where \(\tau_{b}=\left[ \begin{array}{cc}\tau_{x}&\tau_{y}&\tau_{z}\end{array} \right] ^{T}\in R^{3}\), \(\tau_{w}=\left[ \begin{array}{cc}\tau_{ux}&\tau_{uy}&\tau_{uz}\end{array}  \right]^{T}\in R^{3}\) are the control torques generated by the difference in motor speed and the movement generated by the wind acting on the quadrotor. Substituting Equations (8) and (9) into Equation (7), the complete dynamic model of the quadrotor in the airframe coordinate system is obtained as:  
\[\left\{ \begin{array}{l}-mg(\sin\theta)+f_{wx}=m(\dot{u}+qw-rv)\\ mg(\cos\theta\sin\phi)+f_{wy}=m(\dot{v}-pw+ru)\\ mg(\cos\theta\cos\phi)+f_{wz}-f_{t}=m(\dot{w}+pv-qu)\\ \tau_{x}+\tau_{wx}=\dot{p}I_{xx}-qrI_{yy}+qrI_{zz}\\ \tau_{y}+\tau_{wy}=\dot{q}I_{yy}+prI_{xx}-prI_{zz}\\ \tau_{z}+\tau_{uz}=\dot{r}I_{zz}-pqI_{xx}+pqI_{yy}\end{array} \right. \tag{10}\]  
Considering that the force and torque input by the control system are proportional to the square of the motor speed, the parameters in (10) are as follows:  
\[\left\{ \begin{array}{l}f_{t}=c_{T}\Big{(}\Omega_{1}^{2}+\Omega_{2}^{2}+ \Omega_{3}^{2}+\Omega_{4}^{2}\Big{)}\\ \tau_{x}=\frac{\sqrt{2}}{2}c_{T}l\Big{(}-\Omega_{1}^{2}+\Omega_{2}^{2}+\Omega_ {3}^{2}-\Omega_{4}^{2}\Big{)}\\ \tau_{y}=\frac{\sqrt{2}}{2}c_{T}l\Big{(}-\Omega_{1}^{2}-\Omega_{2}^{2}+\Omega _{3}^{2}+\Omega_{4}^{2}\Big{)}\\ \tau_{z}=c_{M}\Big{(}-\Omega_{1}^{2}+\Omega_{2}^{2}-\Omega_{3}^{2}+\Omega_{4} ^{2}\Big{)}\end{array} \right., \tag{11}\]  
where \(l\) is the distance between the center of the motor and the center of the airframe, \(c_{T}\) is the tensile coefficient of the motor, and \(c_{M}\) is the torque coefficient. All of the above parameters can be measured through experiments. Substituting (11) into (10), the complete dynamic model of the quadrotor in the airframe coordinate system can be further expressed as:  
\[\left\{