## 3 Enhancement of MEMS INS for UAV Navigation
### Measurement Model for Data Fusion  
In our research, the measurement information comes from the image matching observations as discussed in Section 2, and this information is used to correct the MEMS INS degradation with time by the Kalman filtering mechanism. The image matching procedure can provide position, attitude, and heading observations. However, considering that the attitude observation from image matching is generally at the level of a few tenths of a degree; therefore, it is not accurate enough for correcting the attitude of MEMS INS. As a result, in this research, the position and heading observations from image matching are chosen as the measurements for Kalman filter of data fusion. For the aircraft position measurement, define \(z_{p}^{IMG}\) as the position obtained from the image matching, and \(z_{p}^{INS}\) as the position obtained from the MEMS INS. Therefore, the position error of the inertial navigation can be expressed as follows:  
\[\widetilde{z}_{p}=z_{p}^{INS}-z_{p}^{IMG} \tag{19}\]  
Considering the definition of the state vector \(X\) in Equation (15), the above position error can be expressed as:  
\[\widetilde{z}_{p}=H_{p}\cdot X \tag{20}\]  
where the coefficient matrix \(H_{p}\) can be expressed as:  
\[H_{p}= \begin{bmatrix}I_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3 \times 3}\end{bmatrix}  \tag{21}\]  
For the aircraft heading measurement, define \(z_{head}^{IMG}\) as the heading angle obtained from the image matching, and \(z_{p}^{INS}\) as the heading angle obtained from the MEMS INS. Therefore, the heading error of the inertial navigation can be expressed as follows:  
\[\widetilde{z}_{head}=z_{head}^{INS}-z_{head}^{IMG} \tag{22}\]  
Herein, to construct the measurement model for the data fusion Kalman filter, we also need to provide the relation between the heading error and the state vector \(X\), as in Equation (20). However, the relation between the heading error and the state vector \(X\) is not very straightforward. The heading error described in Equation (22) is defined as the Euler error; however, the heading error component in state vector \(X\) is defined as the mathematical platform error [33]. Their relation can be derived as follows:  
As discussed in the previous sections, \(C_{b}^{n}\) represents the attitude matrix from the body frame to the navigation frame, which can be expressed as follows if \(H,\theta,\phi\) are defined to be the heading, pitch, and roll, respectively [34]. \[C_{b}^{n}= \begin{bmatrix}\cos\theta\cos H&-\cos\phi\sin H+\sin\phi\sin\theta \cos H&\sin\phi\sin H+\cos\phi\sin\theta\cos H\\ \cos\theta\sin H&\cos\phi\cos H+\sin\phi\sin\theta\sin H&-\sin\phi\cos H+\cos \phi\sin\theta\sin H\\ -\sin\theta&\sin\phi\cos\theta&\cos\phi\cos\theta\end{bmatrix}  \tag{23}\]  
For brevity, we use \(c_{ij}\) to denote the element of \(C_{b}^{n}\) in row \(i\) and column \(j\). Therefore, the heading angle can be calculated from \(C_{b}^{n}\) as:  
\[H=\arctan\frac{c_{21}}{c_{11}} \tag{24}\]  
The heading error \(\delta H\) can be obtained from the variation of Equation (24), and can be expressed as follows:  
\[\delta H=\frac{c_{11}\cdot\delta c_{21}-c_{21}\cdot\delta c_{11}}{c_{11}^{2}+ c_{21}^{2}} \tag{25}\]  
In Equation (25), \(\delta c_{11},\delta c_{21}\) are the elements of the attitude error matrix \(\delta C_{b}^{n}\), which can be calculated as:  
\[\delta C_{b}^{n}=C_{b}^{p}-C_{b}^{n} \tag{26}\]  
where \(C_{b}^{p}\) is defined as the attitude matrix from the body frame \(b\) to mathematical platform \(p\), and it can be calculated as follows:  
\[C_{b}^{p}=C_{b}^{p}C_{b}^{n} \tag{27}\]In Equation (27), \(C_{n}^{p}\) represents the attitude matrix from the navigation frame \(n\) to mathematical platform \(p\), and it can be calculated by using the platform attitude error vector \(\Psi^{n}\) in Equation (15) as follows [33]:  
\[C_{n}^{p}=I-(\psi^{n}\times)= \begin{bmatrix}1&\psi_{D}&-\psi_{E}\\ -\psi_{D}&1&\psi_{N}\\ \psi_{E}&-\psi_{N}&1\end{bmatrix}