## 3 Enhancement of MEMS INS for UAV Navigation
### System Model for Data Fusion  
The SINS error model is commonly used by the design of Kalman filter model. In this proposed system, the errors are always defined in the navigation frame. First, we choose the local-level geographic frame as the navigation frame, which is denoted as n frame. Then, the body frame is defined as b frame, which is aligned with its longitudinal, lateral, and vertical axes. The attitude error equation of INS is defined as follows [32; 33]:  
\[\dot{\Psi}^{n}=-\mathbf{\omega}_{in}^{n}\times\Psi^{n}+\mathbf{C}_{b}^{n}\varepsilon ^{b} \tag{11}\]  
where \(\Psi^{n}\) refers to the attitude error vector projected in navigation frame, \(\mathbf{\omega}_{in}^{n}\) refers to the rotation rate of the navigation frame with respect to the inertial frame, \(\mathbf{C}_{b}^{n}\) represents the attitude matrix from the body frame to the navigation frame, and \(\varepsilon^{n}\) refers to the gyro bias vector in the navigation frame. The velocity error equation of INS is defined as follows [32; 33]:  
\[\mathbf{\delta}\dot{\mathbf{V}}^{n}=-(\mathbf{\omega}_{ie}^{n}+\mathbf{\omega}_{in}^{n}) \times\mathbf{\delta}\mathbf{V}^{n}+\
abla^{n}-\mathbf{\psi}^{n}\times\mathbf{\ell}^{n} \tag{12}\]  
where \(\mathbf{\delta}\mathbf{V}^{n}\) represents the velocity error in the navigation system, \(\mathbf{\omega}_{ie}^{n}\) refers to the rotation rate of the earth frame with respect to the inertial frame, \(\mathbf{\ell}^{n}\) is the specific force projected in the navigation frame, and \(\
abla^{n}\) represents the accelerometer zero bias expressed in the navigation frame. Figure 7: Integrated navigation algorithm overall scheme. The position error equation of INS is defined as follows [32; 33]:  
\[\delta\dot{\mathbf{P}}^{n}=-\mathbf{\omega}_{en}^{n}\times\delta\mathbf{P}^{n}+\mathbf{ \delta}\mathbf{V}^{n} \tag{13}\]  
where \(\delta\mathbf{P}\) is the position error in the navigation system. The biases of the gyroscopes and accelerometers can be regarded as a constant within a certain period of time, and they are modeled as follows:  
\[\dot{\
abla}^{n}=0,\ \dot{\varepsilon}^{n}=0 \tag{14}\]  
Based on the above INS error model, we select the attitude error, velocity error, position error, biases of the gyroscopes and accelerometers as the states of the data fusion filter. This yields a 15-element state vector \(\mathbf{X}\):  
\[\mathbf{X}= \begin{bmatrix}(\delta\mathbf{P}^{n})^{T}&(\delta\mathbf{V}^{n})^{T }&(\mathbf{\psi}^{n})^{T}&(\
abla^{n})^{T}&(\varepsilon^{n})^{T}\end{bmatrix}  \tag{15}\]  
Considering Equations (11)-(15), the state equation of the data fusion filter can be written as follows:  
\[\dot{\mathbf{X}}=F\mathbf{X}+W \tag{16}\]  
where \(W\) is the 15-dimension state noise variance vector, and the system matrix \(F\) can be expressed as follows:  
\[F= \begin{bmatrix}F_{11}&F_{12}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&F_{22}&F_{23}&F_{24}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&F_{33}&0_{3\times 3}&F_{35}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\end{bmatrix}  \tag{17}\]  
The submatrix in system matrix \(F\) can be expressed as follows:  
\[F_{11}= \begin{bmatrix}0&\mathbf{\omega}_{enD}^{n}&-\mathbf{\omega}_{enE} ^{n}\\ -\mathbf{\omega}_{enD}^{n}&0&\mathbf{\omega}_{enN}^{n}\\ \mathbf{\omega}_{enE}^{n}&-\mathbf{\omega}_{enN}^{n}&0\end{bmatrix} ,\quad F_{12}= \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\end{bmatrix}  \tag{18}\] \[F_{22}= \begin{bmatrix}0&\begin{pmatrix}2\mathbf{\omega}_{ieD}^{n}+\bm {\omega}_{enD}^{n}\\ -\mathbf{(2\mathbf{\omega}_{ieD}^{n}+\mathbf{\omega}_{enD}^{n})}&0&\begin{pmatrix}2\mathbf{ \omega}_{ieN}^{n}+\mathbf{\omega}_{enN}^{n}\\ 2\mathbf{\omega}_{ieE}^{n}+\mathbf{\omega}_{enE}^{n}\end{pmatrix}\\ -\mathbf{(2\mathbf{\omega}_{inN}^{n}+\mathbf{\omega}_{enN}^{n})}&0\end{bmatrix} \] \[F_{23}= \begin{bmatrix}0&-f_{D}^{n}&f_{E}^{n}\\ f_{D}^{n}&0&-f_{N}^{n}\\ -f_{E}^{n}&f_{N}^{n}&0\end{bmatrix} ,\quad F_{24}=\mathbf{C}_{b}^{n}\] \[F_{33}=