# Real-Time Terrain-Following of an Autonomous Quadrotor by Multi-Sensor Fusion and Control
## 2 Quadrotor System Models
### Quadrotor Dynamic Models
\begin{array}{l}m(\dot{u}+qw-rv)=-mg(\sin\theta)+f_{wx}\\ m(\dot{v}-pw+ru)=mg(\cos\theta\sin\phi)+f_{wy}\\ m(\dot{w}+pv-qu)=mg(\cos\theta\cos\phi)+f_{wz}-c_{T}\Big{(}\Omega_{1}^{2}+ \Omega_{2}^{2}+\Omega_{3}^{2}+\Omega_{4}^{2}\Big{)}\\ \dot{p}I_{xx}-qrI_{yy}+qrI_{zz}=\frac{\sqrt{2}}{2}c_{T}l\Big{(}-\Omega_{1}^{2} +\Omega_{2}^{2}+\Omega_{3}^{2}-\Omega_{4}^{2}\Big{)}+\tau_{wx}\\ \dot{q}I_{yy}+prI_{xx}-prI_{zz}=\frac{\sqrt{2}}{2}c_{T}l\Big{(}-\Omega_{1}^{2} -\Omega_{2}^{2}+\Omega_{3}^{2}+\Omega_{4}^{2}\Big{)}+\tau_{wy}\\ \dot{r}I_{zz}-pqI_{xx}+pqI_{yy}=c_{M}\Big{(}-\Omega_{1}^{2}+\Omega_{2}^{2}- \Omega_{3}^{2}+\Omega_{4}^{2}\Big{)}+\tau_{uz}\end{array} \right.. \tag{12}\]  
The parameter setting of the quadrotor models is listed in Table 1.  
The state vector is expressed as follows:  
\[x=\big{[} \begin{array}{cccccccccccc}\phi&\theta&\psi&p&q&r&u&v&w&x&y&z\end{array}  \big{]}^{T}. \tag{13}\]  
According to Equations (3) and (10), the dynamic equation of the quadrotor can be rewritten as:  
\[\left\{ \begin{array}{l}\dot{\phi}=p+r(\cos\phi\tan\theta)+q(\sin\phi\tan \theta)\\ \dot{\theta}=q(\cos\phi)-r(\sin\phi)\\ \dot{\psi}=r\frac{\cos\phi}{\cos\phi}+q\frac{\sin\phi}{\cos\theta}\\ \dot{p}=\frac{l_{xx}-l_{xx}}{l_{xx}-r}q+\frac{r_{xx}+l_{xx}}{l_{xx}}\\ \dot{q}=\frac{l_{xx}-l_{xx}}{l_{yy}}pr+\frac{l_{yy}+l_{yy}}{l_{yy}}\\ \dot{r}=\frac{l_{xx}-l_{yy}}{l_{zz}}pq+\frac{r_{xx}+l_{yy}}{l_{zz}}\\ \dot{u}=rv-qw-g(\sin\theta)+\frac{f_{xy}}{m}\\ \dot{v}=pw-ru+g(\sin\phi\cos\theta)+\frac{f_{xy}}{m}\\ \dot{w}=qu-pv+g(\cos\theta\cos\phi)+\frac{f_{xy}-f_{z}}{m}\\ \dot{x}=w(\sin\phi\sin\psi+\cos\phi\cos\psi\sin\theta)\\ -v(\cos\phi\sin\psi-\cos\psi\sin\phi\sin\theta)+u(\cos\psi\cos\theta)\\ \dot{y}=v(\cos\phi\cos\psi+\sin\phi\sin\psi\sin\theta)\\ -w(\cos\psi\sin\phi-\cos\phi\sin\psi\sin\theta)+u(\cos\theta sin\psi)\\ \dot{z}=w(\cos\phi\cos\theta)-u(\sin\theta)+v(\cos\theta\sin\phi)\end{array} \right. \tag{14}\]  
In hovering mode, the attitude of the aircraft changes very little. It can be made that \(\cos\alpha=1,\ \sin\alpha=\tan\alpha=0\) and the state Equation (14) can be approximately expressed as:  
\[\left\{ \begin{array}{l}\dot{\phi}\approx p+r\theta+q\phi\theta\\ \dot{\theta}\approx q-r\phi\\ \dot{\psi}\approx r+q\phi\\ \dot{p}\approx\frac{l_{yy}-l_{zz}}{l_{xx}}rq+\frac{r_{x}+l_{xx}}{l_{xx}}\\ \dot{q}\approx\frac{l_{zz}-l_{xx}}{l_{yy}}pr+\frac{r_{x}+l_{yy}}{l_{yy}}\\ \dot{r}\approx\frac{l_{xx}-l_{yy}}{l_{zz}}pq+\frac{r_{x}+l_{yy}}{l_{zz}}\\ \dot{u}\approx rv-qw-g\theta+\frac{f_{yx}}{m}\\ \dot{v}\approx vu-pv+g\phi+\frac{f_{yy}}{m}\\ \dot{w}\approx qu-pv+g+\frac{f_{yy}}{m}\\ \dot{x}\approx w(\phi\psi+\theta)-v(\psi-\phi\theta)+u\\ \dot{y}\approx v(1+\phi\psi\theta)-w(\phi-\psi\theta)+u\psi\\ \dot{z}\approx w-u\theta+v\phi\end{array} \right. \tag{15}\]  
The above equation can be written in the form of a differential equation:  
\[\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u}). \tag{16}\] \begin{table}
\begin{tabular}{c c c} \hline
**Parameter** & **Unit** & **Measurements** \\ \hline \(g\) & m/s\({}^{2}\) & 9.81 \\ \(m\) & kg & 1.12 \\ \(l\) & m & 0.225 \\ \(c_{T}\) & N s\({}^{2}\)/rad\({}^{2}\) & \(1.394\times 10^{-7}\) \\ \(c_{M}\) & N m s\({}^{2}\)/rad\({}^{2}\) & \(2.857\times 10^{-9}\) \\ \(l_{xx}\) & kg m\({}^{2}\) & 0.0093 \\ \(l_{yy}\) & kg m\({}^{2}\) & 0.0088 \\ \(l_{zz}\) & kg m\({}^{2}\) & 0.0189 \\ \hline \end{tabular}
\end{table} 
Table 1: Parameter list of the quadrotor. In Equation (16), \(\mathbf{u}\) \(=\) \(\left[ \begin{array}{cccccccccccc}f_{t}&\tau_{x}&\tau_{y}&\tau_{z}\end{array}  \right]^{T}\) \(\in\)\(R^{4}\), \(\mathbf{x}=\)\(\left[ \begin{array}{cccccccccccc}\phi&\theta&\psi&p&q&r&u&v&w&x&y&z\end{array}  \right]^{T}\)\(\in\)\(R^{12}\). The balance point of the system can be set as follows:  
\[\overline{\mathbf{x}}=\left[ \begin{array}{cccccccccccc}0&0&0&0&0&0&0&0&0& \overline{x}&\overline{y}&\overline{z}\end{array} \right]^{T}\)\(\in\)\(R^{12}\). (17)  
When the system is in equilibrium, the input of the system is:  
\[\overline{\mathbf{u}}=\left[ \begin{array}{cccccccc}mg&0&0&0\end{array}