# Real-Time Terrain-Following of an Autonomous Quadrotor by Multi-Sensor Fusion and Control
## 2 Quadrotor System Models  
### Quadrotor Dynamic Models  
The quadrotor changes its position and attitude by modifying the speed of the four motors [42,43]. The movements of back and forth and left and right are produced by pitch and roll, respectively, and the movements of up and down are achieved by the four motors increasing or decreasing the speed at the same time. In order to study the kinematic law of aircraft, two coordinate systems [44] depicted in Figure 2 are defined as follows:  
1. The geographic coordinate system (\(O_{E}X_{E}Y_{E}Z_{E}\)) is used to study the motion state of aircraft relative to the earth and the space position coordinates in which \(Y_{E}\)-\(X_{E}\)-\(Z_{E}\) point to East-North-Up, respectively. 2. The airframe coordinate system (\(O_{b}X_{b}Y_{b}Z_{b}\)) is used to study the rotational motion of aircraft relative to the center of gravity in which \(Y_{b}\)-\(X_{b}\)-\(Z_{b}\) point to Right-Front-Up, respectively. Figure 1: Multi-sensor based position, attitude and height control loops for terrain-following of the autonomous quadrotor. Figure 2: An airframe coordinate system and a geographic coordinate system. According to the conversion relationship between the Euler angle and the attitude matrix, the attitude transfer matrix \(R_{E}^{b}\) from the geographic coordinate system to the airframe coordinate system can be obtained:  
\[R_{E}^{b}=\left[ \begin{array}{cccc}\cos\theta\sin\psi&\sin\psi\cos\theta&- \sin\theta\\ \sin\phi\sin\theta\cos\psi-\cos\phi\sin\psi&\sin\phi\sin\phi\sin\psi+\cos\phi \cos\psi&\sin\phi\cos\theta\\ \cos\phi\sin\theta\cos\psi+\sin\phi\sin\psi&\cos\phi\sin\psi\sin\theta-\sin \phi\cos\psi&\cos\phi\cos\theta\end{array} \right] \tag{1}\]  
The vector of \(\left[ \begin{array}{cccccccc}x&y&z&\phi&\theta&\psi\end{array} \right]^{T}\) represents the position and angle of the quadrotor in the geographic coordinate system and the vector of \(\left[ \begin{array}{cccccccc}u&v&w&p&q&r\end{array} \right]^{T}\) represents the linear velocity and angular velocity of the quadrotor in the airframe coordinate system. The conversion relationship of these twelve-dimensional variables in the two coordinate systems is:  
\[\left\{ \begin{array}{c}v_{E}=R_{b}^{E}v_{b}\\ \omega=T\omega_{b}\end{array} \right.. \tag{2}\]  
In Equation (2), \(v_{E}=\left[ \begin{array}{cccccccc}\dot{x}&\dot{y}&\dot{z}\end{array} \right] ^{T}\in R^{3}\), \(\omega=\left[ \begin{array}{cccccccc}\dot{\phi}&\dot{\theta}&\dot{\psi} \end{array} \right]^{T}\in R^{3}\), \(v_{b}=\left[ \begin{array}{cccccccc}u&v&w\end{array} \right]^{T}\in R^{3}\), \(\omega_{b}=\left[ \begin{array}{cccccccc}p&q&r\end{array} \right]^{T}\in R^{3}\), the \(R_{b}^{E}\) is the direction cosine matrix from the airframe coordinate system to the geographic coordinate system and \(T\) is the conversion matrix between the airframe rotation angular velocity and the attitude angle change rate. The dynamic model of the quadrotor is:  
\[\left\{ \begin{array}{l}\dot{x}=w(\sin\phi\sin\psi+\cos\phi\cos\psi\sin\theta )-v(\cos\phi\sin\psi-\cos\psi\sin\phi)\\ \dot{y}=v(\cos\phi\cos\psi+\sin\phi\sin\psi\sin\theta)-w(\cos\psi\sin\phi-\cos \phi\sin\psi)\\ \dot{z}=w(\cos\phi\cos\theta)-u(\sin\theta)+v(\cos\theta\sin\phi)\\ \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\theta}+q\frac{\sin\phi}{\cos\theta}\end{array} \right.. \tag{3}\]  
According to [PERSON]'s law of motion, the total force acting on the quadrotor is:  
\[m\big{(}\omega_{b}\otimes v_{b}+\dot{v}_{b}\big{)}=f_{b}, \tag{4}\]  
where \(m\) is the mass of the quadcopter, \(\otimes\) represents the cross product operation and \(f_{b}=\left[ \begin{array}{cccccccc}f_{x}&f_{y}&f_{z}\end{array} \right]\in R ^{3}\) is the total force acting on the quadrotor. According to [PERSON]'s equation, the total torque produced by the four motors is:  
\[I\cdot\dot{\omega}_{b}+\omega_{b}\otimes(I\cdot\omega_{b})=m_{b}. \tag{5}\]  
In (5), \(\tau_{b}=\left[ \begin{array}{cccccccc}\tau_{x}&\tau_{y}&\tau_{z}\end{array}