Cross-Modal Images Matching Based Enhancement to MEMS INS for UAV Navigation in GNSS Denied Environments

Abstract

A new cross-modal image matching method is proposed to solve the problem that unmanned aerial vehicles (UAVs) are difficult to navigate in GPS-free environment and night environment. In this algorithm, infrared image or visible image is matched with satellite visible image. The matching process is divided into two steps, namely, coarse matching and fine alignment. Based on the dense structure features, the coarse matching algorithm can realize the position update above 10 Hz with a small amount of computation. Based on the end-to-end matching network, the fine alignment algorithm can align the multi-sensor image with the satellite image under the condition of interference. In order to obtain the position and heading information with higher accuracy, the fusion of the information after visual matching with the inertial information can restrain the divergence of the inertial navigation position error. The experiment shows that it has the advantages of strong anti-interference ability, strong reliability, and low requirements on hardware, which is expected to be applied in the field of unmanned navigation.

1. Introduction

Unmanned aerial vehicle (UAV) navigation is generally achieved by means of global navigation satellite system (GNSS) [1] or the fusion of GNSS with inertial navigation system (INS [2,3]). As is known, the GNSS signal might be blocked, cheated, or even shut down [4,5]. Under these circumstances, standalone INS generally cannot fulfill the navigation necessity, especially for the micro electro mechanical systems (MEMS) INS, which is widely used in the small- or medium-size UAVs [6]. If the UAVs fly at a proper altitude and have a good view of the ground, a vision-based approach can be used to enhance the performances of the MEMS INS, and an acceptable navigation accuracy might be achieved to ensure the reliable flying of UAV in GNSS denied circumstances [7].

Literatures [3,8] discussed the image matching-based methods for UAV navigation. The position of the UAV can be estimated according to the results produced by matching the real-time image to the referenced image. However, it is still challenging to reliably and accurately match the real-time aerial image and the referenced image when they are captured by different types of sensors at different times, which are defined as multi-sensor temporal images. The distortions between the multi-sensor temporal images, as shown in Figure 1, include nonlinear grayscale variation, severe noise distortion, and even the changes in local ground features with time, which will severely degrade the matching performance [9,10].

The matching algorithms developed in [8,11] are very computationally expensive. To solve the problem, the visual odometer based on optical flow [12] combined with particle filter is adopted to restrict the search area of the matching and eliminate the incorrect results [11]. However, optical flow is not drift-free and can be easily interrupted by motion blur or light changes. Consequently, the position solution is still unstable if both the image matching and the visual odometer produce unreliable input.

Deep learning-based image matching method is employed in [3] for UAV localization; however, the precision of the image matching algorithm is very low to estimate the precise heading of the UAV, which means that the heading estimation from the image matching cannot be used to correct the INS heading. Therefore, only INS position can be corrected by the image matching results. Finally, these image matching methods [3,8,11] are developed mainly for the visible light images, which means their visual positioning methods only work in daylight.

In the last decade, vision-based simultaneous localization and mapping (VSLAM) has been extensively studied [11,13]. However, pure vision-based SLAM is usually not a robust solution for navigation. The main drawback of VSLAM is that the vision tracker could be easily interrupted by light change, motion blurring, or occlusion of moving objects. For solving this problem, vision and inertial fusion-based SLAM (VISLAM) has been proposed [14,15]. VISLAM is more robust than VSLAM since the inertial module would not be affected by light change, motion blurring, or occlusion, which can consistently provide the motion states to the navigation system [16,17,18]. Meanwhile, the vision system can provide environmental perception and states correction, which is supplemental to the internal module. Nevertheless, the VISLAM suffers from the position drift problem. Therefore, closed-loop optimization is employed for correcting the drift [15,19]. However, the long path closed-loop optimization is very expensive in both computation and memory consumption. In addition, closed-loop path is not always available for UAVs navigation, for example, for single-way flying of UAVs. Although VISLAM has been widely and successfully used for indoor (or enclosed environment) navigation, it is still very hard to apply for UAV navigation in large open environment.

In this paper, we proposed an airborne MEMS INS enhancing method based on cross-modal images matching, which can provide long range autonomous navigation for medium- and high-altitude aircraft under GNSS blocking environment. The main contributions of this work are the following:

(I)

A coarse matching algorithm based on the dense structure features is proposed to effectively adapt to nonlinear grayscale variation and noise distortion. In addition, the searching process of the coarse matching is significantly accelerated with fast Fourier transform (FFT). Therefore, the proposed coarse matching algorithm can provide position updates with a rate above 10 Hz on a low power airborne compute. Since the proposed coarse matching method is reliable and time efficient, the visual odometer is no longer required.

(II)

A fine alignment algorithm based on a deep end-to-end trainable matching network is proposed to precisely align the real-time aerial image with the referenced satellite image despite the local ground feature changes and nonlinear grayscale variation. The result of fine alignment is employed to estimate the precise heading angle of the UAV, which is better than three degrees (1σ) during a typical experiment and can be used to correct the heading angle of the MEMS INS.

(III)

Based on the cross-modal image matching approach, a precise position and heading can be obtained, which is used to enhance the MEMS INS performances in this research. A filtering model to fuse the vision-based navigation information and the INS information is developed, which can precisely and reliably estimate the position, velocity, and attitude errors of the UAV and avoid the accumulating errors of the MEMS INS. The enhanced navigation approach can provide long-time autonomous navigation for the UAV at medium- to high-altitude at GNSS denied environments, and it can work effectively in both daylight condition (with visible light camera) and night condition (with infrared camera).

In the remaining parts of this paper, Section 2 describes the cross-modal image matching approach to achieve the position and heading enhancement information. The data fusion approach to achieve the enhancement to MEMS INS with image matching is discussed in Section 3. The experimental tests are discussed in Section 4, and the concluding remarks are provided in Section 5.

2. Position and Heading Estimation Approach Based on Cross-Modal Image Matching

The overall scheme of the proposed visual estimator is shown in Figure 2. The camera calibration is realized with OpenCV camera calibration module [20]. Before the image matching, the real-time image is corrected according to the pose, altitude, and the parameters of the camera. After the correction, the ground resolution and the orientation of the real-time image and the referenced image are significantly the same. The transition of the real-time image relative to referenced image can be estimated by the coarse matching.

After the image correction, the center point of the corrected image is directly below the aircraft. Therefore, the horizontal position of the UAV can be directly calculated according to the result of the coarse matching.

The result of the coarse matching is then inputted into the fine alignment algorithm, which will estimate the homography transformation between the real-time image and the referenced image. Precise point correspondences between the real-time image and the referenced image can be produced with the homography transformation. The points in the referenced image can be mapped to the world coordinate system with zero height. Therefore, correspondences between the 2D point in the real-time image and 3D point in the world coordinate system are established. Then, the position (relative to the ground) and pose of the camera can be calculated with perspective-n-point (PNP) method [21] according to the 2D to 3D point correspondences and the camera parameters provided by the calibration. Finally, the position and heading estimated is sent to the INS for performance enhancement.

2.1. Fast Coarse Image Matching Based on PCA-HOG

2.1.1. PCA-HOG Feature Description

Histogram of oriented gradient (HOG) is a common feature descriptor used in target detection and image matching. HOG is a description based on the statistics of local gradient magnitude and direction, which has good invariability for grayscale variation [22]. However, HOG is very sensitive to the noise distortion since it is formed based on gradient direction, which is easily disturbed by the distortion between cross-modal images.

To improve the noise reliability, we employ principal component analysis (PCA) to enhance the local gradient orientation. The PCA is typically used to calculate the dominant vectors of a given dataset that can reduce the noise component in signals and estimate the local dominant orientation.

For each pixel, the PCA can be applied to the local gradient vectors to obtain their local dominant direction.

Given the original image I with its horizontal derivative image $A$ and vertical derivative image $B$, an $N\times 2$ local gradient matrix is constructed for each pixel:

$$G=[\begin{array}{cc}{A}^T& B^T\end{array}]$$

(1)

$$A=\left\{\begin{array}{cccc}{a}_1& a_2& \cdots & a_n\end{array}\right\}, B=\left\{\begin{array}{cccc}{b}_1& b_2& \cdots & b_n\end{array}\right\}$$

(2)

They can be calculated with Sobel calculators [23]. $N$ is determined by the size of the local window of the PCA calculation. For example, if the size of the window for each pixel is $3\times 3$, then the value of $N$ is 9.

The dominant orientation can be estimated by determining a unit vector $\mathit{u}$ perpendicular to the combined direction of the local gradient vectors ${\left[\begin{array}{cc}{a}_i& b_i\end{array}\right]}^T$. Since the production of vectors perpendicular to each other is 0; therefore, this can be formulated as the following minimization problem:

$${\Vert G^T\mathit{u}\Vert }^2={\displaystyle \sum _{i=1}^N{\left(\left[\begin{array}{cc}{a}_i& b_i\end{array}\right]\cdot \mathit{u}\right)}^2}$$

(3)

Vector $\mathit{u}$ can be solved by applying SVD to the local gradient matrix $G$:

$$G=\mathit{U}\mathit{S}{\mathit{V}}^T$$

(4)

where $\mathit{U}$ is an $N\times N$ orthogonal matrix, $\mathit{S}$ is an $N\times N$ matrix, $\mathit{V}=\left[\begin{array}{cc}v& u\end{array}\right]$ is a $2\times 2$ orthogonal matrix, and $v$ indicates the local dominant orientation of the local gradient.

The PCA enhanced structure orientation can be calculated according to the following equation:

$$\phi =\frac{1}{2}\angle \left(u_1,u_2\right)$$

(5)

where $\mathsf{\phi }\in \left[0°,180°\right)$, $u_1$ is the element in the first row of $u$ and $u_1$ is the element in the second row of $u$. Note that the structure orientation is orthogonal to the orientation of the gradient vector.

The orientation enhanced by PCA is more robust against the distortion between cross-modal images than the orientation directly estimated with local gradient. Figure 3a,b shows two examples of comparing gradient orientation and PCA enhanced ordination. Clearly, PCA enhanced ordination is more stable under the cross-modal distortion.

The PCA-HOG description is a feature vector, which is calculated at each pixel of image. The PCA-HOG description and the HOG description [22] are calculated in the same procedure, except that the local gradient orientation is replaced by the PCA enhanced orientation. The examples of comparing PCA-HOG and HOG are provided in Figure 3c,d. We can see that cross-modal distortion causes more significant change to the patterns of HOG descriptor compared with the patterns of PCA-HOG descriptor.

2.1.2. Searching Process Accelerating

Coarse matching is a full-searching template matching as shown in Figure 4. The template, namely, the aforementioned corrected real-time image, needs to be compared with each candidate window in the referenced image and matched to the most similar window. The similarity of PCA-HOG can be calculated with the sum of squared differences (SSD) as follows:

$$similarit{y}_{\left(u,v\right)}={\displaystyle \sum }_{x,y}SSD\left(H\left(x,y\right),H\left(x-u,y-v\right)\right)$$

(6)

where $H\left(x,y\right)$ represents the PCA-HOG feature vector of the pixel in $\left(x,y\right)$, and $\left(u,v\right)$ is the transition between the template and the corresponding candidate window in the referenced image.

However, the searching process with per-pixel SSD calculation is very computationally expensive. To reduce the computational burden, we developed a fast convolution-based method for the template searching. First, we define the similarity of PCA-HOG feature map as the following equation:

$$similarit{y}_{\left(u,v\right)}={\displaystyle \sum }_{x,y}\left({\mathit{H}}^{\prime }\left(x,y\right)\cdot {\mathit{H}}^{\prime }\left(x-u,y-v\right)\right)$$

(7)

where the operator $·$ represents the dot product, and ${\mathit{H}}^{\prime }\left(x,y\right)$ is the normalized (by L2 norm) PCA-HOG feature vector of the pixel in $\left(x,y\right)$. Then, the full-searching process can be defined as the following convolution:

$$\left(\hat{u},\hat{v}\right)=\underset{\left(\mathrm{u},\mathrm{v}\right)}{\arg }max{\displaystyle \sum }_{x,y}\left(\mathit{H}\left(x,y\right)\cdot \mathit{H}\left(x-u,y-v\right)\right)$$

(8)

where $\left(\hat{u},\hat{v}\right)$is the transition between the template and the referenced image estimate by the searching process. The convolution can be accelerated in frequency domain by using fast Fourier transform (FFT):

$$\left(\hat{u},\hat{v}\right)=\underset{\left(\mathrm{u},\mathrm{v}\right)}{\arg }max{\displaystyle \sum }_{x,y}{F}^{-1}\left[F\left(\mathit{H}\left(x,y\right)\right)\cdot F^{*}\left(\mathit{H}\left(x-u,y-v\right)\right)\right]$$

(9)

where $F$ is fast Fourier transform, $F^{*}$ is the complex conjugate of $F$, and $F^{-1}$ is inverse fast Fourier transform (IFFT). IFFT and FFT are the main parts of the computation of the accelerated convolution process. Suppose the size of the template is $n\times n$ and the size of the referenced image is $m\times m$. The computation complexity of the searching process with SSD is $O\left(n^2{\left(m-n+1\right)}^2\right)$, while the computation complexity of the searching process accelerated with FFT is O(2m²logm). The accelerating ratio is varied with the size of image, which is given in

Table 1. Computation comparison of the two search methods.

Image Size	Computation Amount	Accelerating Ratio
$m=128,n=32$	$O(n^2{\left(m-n+1\right)}^2=9,634,816$	42.0
	$O\left(2m^{2}logm\right)=229,376$
$m=256,n=64$	$O(n^2{\left(m-n+1\right)}^2=152,571,904$	145.5
	$O\left(2m^{2}logm\right)=1,048,576$
$m=512,n=128$	$O(n^2{\left(m-n+1\right)}^2=2,428,518,400$	514.7
	$O\left(2m^{2}logm\right)=4,718,592$

by examples. We can see that a significant computation decrease can be achieved by the accelerating method.

2.2. Fine Alignment Based on End-to-End Trainable Matching Network

Fine alignment for cross-modal images is very challenging due to the complicated grayscale variation and the local ground feature changes. The image matching methods based on handcrafted features have very limited ability for addressing the challenges [10,24].

Rocco et al. [24] proposed an image matching method based on convolutional neural network (CNN) for both instance-level and category-level matching, which can handle substantial appearance differences, background clutter, and large deformable transformation. Based on this method, Park et al. [10] proposed a two-stream symmetric network for aerial image matching which can adapt to the light change and the variation of ground appearance, and the matching precision is further improved in paper [25]. However, the matching precision of these methods is low, mainly due to the lower resolution of the input image and the lower resolution of the features extraction, as pointed out in paper [24].

2.2.1. Loss Function

Suppose the ground truth homography transformation between a real-time image and a referenced image is $H_{gt}$, and the homography transformation estimated between the subareas of this training pair is ${\hat{H}}_k$, then the loss function is calculated by the following:

$$\mathrm{loss}={\displaystyle \sum }_{k=1}^4{\displaystyle \sum }_{i=1}^N{\Vert H_{gt}\left(x_i,y_i\right)-\left({\hat{H}}_k\left(x_i-t_x,y_i-t_y\right)+\left(t_x,t_y\right)\right)\Vert }^2$$

(10)

where $N$ is the number of grid points, and $\left(t_x,t_y\right)$ is the translation between the subarea and the real-time image, as shown in Figure 5. The loss function is based on the transformed grid loss proposed in [24], which minimizes the discrepancy between the estimated transformation and the ground truth transformation.

2.2.2. Implementation Details and Discussion

The network is developed based on [24] which is originally trained on visible light images. Therefore, we build a training dataset with 5000 pairs of infrared aerial image and visible light satellite image. We generate the ground true pairs by applying homography transformation to the manually aligned pairs, since there are no datasets with completely correct transformation parameters between infrared aerial image and visible light satellite image.

The resolution of the input image is 640 × 480. The input image will be divided into four subareas and each subarea is then resized to 240 × 240. The feature map is extracted with ResNet-101 network [26] rather than VGG-16 network [27], since the former achieves better matching performance as reported in [28].

The partition approach increases the resolution of the input image and the resolution of the features extraction, which helps in improving the matching precision. Of course, one can also enhance the precision by inputting the image with high resolution rather than using partition, but that may introduce an enormous increase in parameters to the network, which makes the retraining process (for cross-modal images) difficult and eventually lowers the performance of the inference.

In the partition approach, the four subarea pairs share the backbone network and the regression network, which means the number of the parameters of the network is not increased. This facilitates the retraining process and the deployment of network, which will eventually enhance the performance of the homography estimation with a modest training dataset.

During the inference, the global homography transformation for the fine alignment is estimated with RANSAC algorithm [29], which minimizes the re-projection errors of the four homography transformations of the subarea pairs.

Examples of the coarse matching and the fine alignment are shown in Figure 6. We can see that the features in the results of fine alignment are aligned more accurately compared with the results of coarse matching.

3. Enhancement of MEMS INS for UAV Navigation

3.1. Overall Scheme

According to the above discussion, the drift-free position and heading of the UAV can be calculated from the cross-modal image matching, in which the information will be used to enhance the MEMS INS to improve the autonomous navigation performance of UAV.

INS obtains the position, velocity, and attitude of the UAV by integrating the acceleration and angular rate from the accelerometers and gyros, respectively. However, the navigation errors of the INS will accumulate due to the drifts of the accelerometers and the gyros used [30,31], which is especially challenging for the MEMS INS due to the unstable characteristics of the MEMS inertial sensors [1,2]. The cross-modal image matching is used to enhance the performance of the MEMS INS herein. The overall scheme is shown in Figure 7, and the main steps are as follows:

(I)

The real-time images from the camera mounted in the UAV are transformed and corrected by the reference attitude from the MEMS INS initially, and then from the extended Kalman filter which will be discussed below.

(II)

The transformed real-time images are used to match with the geo-referenced images, which is pre-prepared and stored in the UAV navigation computer before the task, and obtain the position and heading of the UAV by solving the PNP pose as discussed in Section 2.

(III)

The MEMS INS position, velocity, attitude, and heading are calculated by the inertial navigation algorithm.

(IV)

An extended Kalman filter is designed to fuse the navigation data from image matching and from MEMS INS.

Figure 7. Integrated navigation algorithm overall scheme.

Figure 7. Integrated navigation algorithm overall scheme.

The system model and measurement model for data fusion are derived, respectively. The extended Kalman filter compares the navigation results from the image matching and from MEMS INS, and estimates and corrects the position, velocity, attitude errors, and compensates the drifts of the inertial sensors.

3.2. 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=-{\omega }_{in}^n\times {\psi }^n+C_b^n{\epsilon }^b$$

(11)

where ${\psi }^n$ refers to the attitude error vector projected in navigation frame, ${\omega }_{in}^n$ refers to the rotation rate of the navigation frame with respect to the inertial frame, $C_b^n$ represents the attitude matrix from the body frame to the navigation frame, and ${\epsilon }^n$ refers to the gyro bias vector in the navigation frame.

The velocity error equation of INS is defined as follows [32,33]:

$$\delta {\dot{V}}^n=-({\omega }_{ie}^n+{\omega }_{in}^n)\times \delta V^n+{\nabla }^n-{\psi }^n\times f^n$$

(12)

where $\delta V^n$ represents the velocity error in the navigation system, ${\omega }_{ie}^n$ refers to the rotation rate of the earth frame with respect to the inertial frame, $f^n$ is the specific force projected in the navigation frame, and ${\nabla }^n$ represents the accelerometer zero bias expressed in the navigation frame.

The position error equation of INS is defined as follows [32,33]:

$$\delta {\dot{P}}^n=-{\omega }_{en}^n\times \delta P^n+\delta V^n$$

(13)

where $\delta 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{\nabla }}^n=0, {\dot{\epsilon }}^n=0$$

(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 $X$:

$$X=\left[\begin{array}{ccccc}{\left(\delta P^n\right)}^T& {\left(\delta V^n\right)}^T& {\left({\psi }^n\right)}^T& {\left({\nabla }^n\right)}^T& {\left({\epsilon }^n\right)}^T\end{array}\right]$$

(15)

Considering Equations (11)–(15), the state equation of the data fusion filter can be written as follows:

$$\dot{X}=FX+W$$

(16)

where $W$ is the 15-dimension state noise variance vector, and the system matrix $F$ can be expressed as follows:

$$F=\left[\begin{array}{ccccc}{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{array}\right]$$

(17)

The submatrix in system matrix $F$ can be expressed as follows:

$$\begin{gathered} F_{11}=\left[\begin{array}{ccc}0& {\omega }_{enD}^n& -{\omega }_{enE}^n\\ -{\omega }_{enD}^n& 0& {\omega }_{enN}^n\\ {\omega }_{enE}^n& -{\omega }_{enN}^n& 0\end{array}\right], F_{12}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right] \\ {F}_{22}=\left[\begin{array}{ccc}0& \left(2{\omega }_{ieD}^n+{\omega }_{enD}^n\right)& -\left(2{\omega }_{ieE}^n+{\omega }_{enE}^n\right)\\ -\left(2{\omega }_{ieD}^n+{\omega }_{enD}^n\right)& 0& \left(2{\omega }_{ieN}^n+{\omega }_{enN}^n\right)\\ \left(2{\omega }_{ieE}^n+{\omega }_{enE}^n\right)& -\left(2{\omega }_{inN}^n+{\omega }_{enN}^n\right)& 0\end{array}\right] \\ {F}_{23}=\left[\begin{array}{ccc}0& -f_D^n& f_E^n\\ f_D^n& 0& -f_N^n\\ -f_E^n& f_N^n& 0\end{array}\right], F_{24}=C_b^n \\ {F}_{33}=\left[\begin{array}{ccc}0& {\omega }_{inD}^n& -{\omega }_{inE}^n\\ -{\omega }_{inD}^n& 0& {\omega }_{inN}^n\\ {\omega }_{inE}^n& -{\omega }_{inN}^n& 0\end{array}\right], F_{35}=-C_b^n \end{gathered}$$

(18)

3.3. 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:

$${\tilde{z}}_p=z_p^{INS}-z_p^{IMG}$$

(19)

Considering the definition of the state vector X in Equation (15), the above position error can be expressed as:

$${\tilde{z}}_p=H_p\cdot X$$

(20)

where the coefficient matrix $H_p$ can be expressed as:

$$H_p=\left[\begin{array}{ccccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$

(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:

$${\tilde{z}}_{head}=z_{head}^{INS}-z_{head}^{IMG}$$

(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 ,\varphi$ are defined to be the heading, pitch, and roll, respectively [34].

$$C_b^n=\left[\begin{array}{ccc}\cos \theta \cos H& -\cos \varphi \sin H+\sin \varphi \sin \theta \cos H& \sin \varphi \sin H+\cos \varphi \sin \theta \cos H\\ \cos \theta \sin H& \cos \varphi \cos H+\sin \varphi \sin \theta \sin H& -\sin \varphi \cos H+\cos \varphi \sin \theta \sin H\\ -\sin \theta & \sin \varphi \cos \theta & \cos \varphi \cos \theta \end{array}\right]$$

(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}}$$

(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}$$

(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$$

(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_n^p{C}_b^n$$

(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-\left({\psi }^n\times \right)=\left[\begin{array}{ccc}1& {\mathrm{\psi }}_D& -{\mathrm{\psi }}_E\\ -{\mathrm{\psi }}_D& 1& {\mathrm{\psi }}_N\\ {\mathrm{\psi }}_E& -{\mathrm{\psi }}_N& 1\end{array}\right]$$

(28)

where ${\mathrm{\psi }}_N,{\mathrm{\psi }}_E,{\mathrm{\psi }}_D$ are the components of ${\psi }^n$ projected in navigation frame $n$.

Combine Equations (16)–(18) into (15), and the heading error $\delta H$ can be expressed as follows:

$$\delta H=\frac{-(c_{11}^2+c_{21}^2){\psi }_D+c_{11}{c}_{31}{\psi }_E+c_{21}{c}_{31}{\psi }_N}{c_{11}^2+c_{21}^2}$$

(29)

Compared with Equation (22), it can be found that the following relationship holds:

$${\tilde{z}}_{head}=z_{head}^{INS}-z_{head}^{IMG}=\delta H$$

(30)

Consider the definition of state vector X, the following equation can be obtained:

$${\tilde{z}}_{head}=\delta H=\left[\begin{array}{ccccc}{0}_{1\times 3}& 0_{1\times 3}& M_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}\end{array}\right]X$$

(31)

where $M_{1\times 3}$ is defined as follows according to Equation (29):

$$M_{1\times 3}=\left[\begin{array}{ccc}\frac{c_{21}{c}_{31}}{c_{11}^2+c_{21}^2}& \frac{c_{11}{c}_{31}}{c_{11}^2+c_{21}^2}& -1\end{array}\right]$$

(32)

Define $H_{head}$ as the coefficient matrix of Equation (31), and the attitude error equation can be expressed as follows:

$${\tilde{z}}_{head}=H_{head}\cdot X$$

(33)

Combine Equations (20) and (31), the measurement model for data fusion can be obtained as follows:

$$\tilde{z}=H\cdot X$$

(34)

where the measurement model coefficient matrix $H$ can be expressed as follows:

$$H=\left[\begin{array}{c}{H}_p\\ H_{head}\end{array}\right]$$

(35)

4. Experimental Tests

4.1. Test Conditions

The proposed enhanced navigation approach is evaluated by field tests. In these tests, a visible light camera and an infrared camera are carried by the UAV (DJI M600Pro made by DJI Technology Co., Ltd. in Shenzhen, China) to capture the real-time aerial image, as shown in Figure 8. All the data of the sensors are real-timely collected and processed by an onboard computer (Jetson Nano made by NVIDIA Corp. in Beijing, China), which has a CPU composed of 4-core ARM A57 @1.43 GHz and a GPU composed of 128-core Maxwell @912 Hz.

The visible light and the infrared camera parameters are listed in

Table 2. Camera parameters.

Type of Sensor	Infrared	Visible Light
Image Resolution	640 × 480 Pixels	2048 × 2048 Pixels
Field of View	73 × 58 deg	78 × 78 deg
Wave Length	1.0~1.6 um	-
Maximum Frames per Second	60 fps	60 fps

, and the MEMS inertial sensor parameters are shown in

Table 3. MEMS inertial sensor parameters.

Sensor	Parameters	Values
Gyroscope	Range	400 deg/s
	Bias Repeatability	<0.1 deg/h
	Bias Instability (at 25 °C)	<0.5 deg/h
	Scale Factor Repeatability	<15 ppm
	Scale Factor Nonlinearity	<100 ppm
Accelerometer	Range	8 g
	Bias Repeatability	<3.5 mg
	Noise Density	<25 ug/sqrt (Hz)
	Scale Factor Nonlinearity	<1000 ppm

.

4.2. Test Results

The proposed cross-modal image matching approach (CMM) is compared with HOP [11], which combines histograms of oriented gradients (HOG), optical flow (OF), and particle filter (PF). HOP uses a matching method based on HOG for positioning, and, to accelerate the matching process, OF and PF are employed to decrease the searching area.

The positioning accuracy of CMM and HOP is evaluated by comparing their positioning results with the GPS RTK, which generally has an accuracy of 0.02 m under benign environments.

The correct positioning ratio (CPR) is selected for evaluating the positioning reliability of CMM and HOP. Herein, the CPR is defined as $\mathrm{CPR}=\mathrm{CP}/\mathrm{P}$, where $\mathrm{P}$ is the total number of positioning, and $\mathrm{CP}$ is the correct number of positioning. According to the positioning requirement of the UAV, herein, we define that a positioning result is correct if its error is less than 30 m compared with the GPS RTK. The root-mean-squares error (RMSE) of the correct positioning is selected for evaluating the positioning accuracy.

Eight UAV flights are carried out to evaluate the aforementioned approaches, namely, CMM and HOP. The flight parameters are provided in

Table 4. UAV flight parameters.

Flight No.	Average Height	Distance	Average Velocity	Camera Used
Flight 1	273 m	412.1 m	5.1 m/s	Visible light
Flight 2	328 m	1130.6 m	4.7 m/s	Visible light
Flight 3	484 m	1078.6 m	4.8 m/s	Infrared
Flight 4	521 m	1248.6 m	4.0 m/s	Infrared
Flight 5	496 m	967.6 m	4.9 m/s	Infrared
Flight 6	501 m	1213.6 m	4.9 m/s	Infrared
Flight 7	492 m	1245.5 m	5.1 m/s	Infrared
Flight 8	321 m	350.4 m	4.0 m/s	Visible light

. The flight trajectories are shown in Figure 9. The correct positioning ratio and the positioning RMS error of CMM and HOP are compared in

Table 5. CMM and HOP performance comparison.

Flight No.	CMM CPR	CMM Positioning RMS Error	HOP CPR	HOP Positioning RMS Error
Flight 1	99.8%	3.777 m	95.5%	18.986 m
Flight 2	99.2%	4.683 m	97.3%	23.659 m
Flight 3	99.6%	4.416 m	55.8%	22.867 m
Flight 4	99.2%	4.149 m	63.7%	22.005 m
Flight 5	99.1%	6.142 m	62.3%	19.215 m
Flight 6	99.5%	6.021 m	67.5%	24.612 m
Flight 7	98.9%	6.354 m	52.9%	37.624 m
Flight 8	99.7%	4.882 m	72.1%	14.863 m

.

It can be noted from Table 5 that both the CPR and positioning accuracy of HOP are significantly lower than those of CMM for all the flights. There are mainly two reasons for the low performances of HOP. First, the image matching algorithm of HOP is developed for visible light images, which is unreliable for matching cross-modal images; therefore, its CPRs are very low in the flights with infrared camera. Second, the searching area of HOP is small (about 30 m × 30 m) due to the high computation complexity of the matching; therefore, the true position could be out of the searching area when a false prediction is introduced by a false matching.

Furthermore, the contribution of the coarse matching of CMM can be observed by comparing the CPR of HOP with that of CMM. The feature descriptor of the proposed coarse matching is enhanced by PCA, which is more reliable for matching cross-modal images. In addition, the searching process of the coarse matching of CMM is significantly accelerated, which means we can afford a significantly larger searching area $\left(\ge 150 \mathrm{m}\times 150 \mathrm{m}\right)$. Therefore, the CMM can re-track the flight path even when the true position is far from the predict position, which also helps in improving the reliability of CMM.

Table 6. Time-consuming statistics of CMM.

Image Type	Image Resolution	Referenced Image	Average Time Cost of Coarse Matching	Average Time Cost of Fine Alignment
Infrared	640 × 480	1024 × 768	79.1 ms	2451.1 ms
Visible light	512 × 512	1024 × 768	82.6 ms	2454.4 ms

shows the time-consuming statistics of CMM, where the visible light real-time image is resized from 2048 × 2048 pixels to 512 × 512 pixels. Since the ground resolution of visible light real-time image is significantly higher than the reference image, we can save the computation without losing the position precision by decreasing the resolution of real-time image properly. From the statistics of Table 6, it can be noted that the proposed coarse matching algorithm can realize an average position update rate of 12.6 Hz (79.1 ms) for infrared image, and an average position update rate of 12.1 Hz (82.6 ms) for visible light.

The positioning and heading obtained from the image matching are finally used to enhance the MEMS INS in order to improve the autonomous navigation performance of UAV according to the approach proposed in Section 3. Herein, the positioning and heading results of CMM and those of HOP are separately used to aid the MEMS INS, whose raw sensors data are collected during the flying tests.

The CMM enhanced MEMS INS positioning errors are shown in Figure 10, and the comparative HOP enhanced MEMS INS positioning errors are shown in Figure 11. By comparing Figure 10; Figure 11, it can be seen that both the positioning accuracy and the positioning stability are improved dramatically. It can be noted that there exists some error “jumps” in Figure 11 during the HOP enhanced MEMS INS process. This phenomenon arises from the fact that the HOP imaging matching approach has a low CPR, which affects the integration of HOP and MEMS INS. The statistics of the enhanced MEMS INS positioning results are listed in

Table 7. CMM and HOP enhanced MEMS INS performance comparison.

Flight No.	CMM Enhanced Approach		HOP Enhanced Approach
	North RMS Error	East RMS Error	North RMS Error	East RMS Error
Flight 1	3.644 m	3.440 m	20.155 m	19.037 m
Flight 2	4.124 m	3.966 m	21.686 m	21.175 m
Flight 3	3.519 m	3.683 m	20.822 m	20.242 m
Flight 4	3.079 m	4.113 m	16.024 m	22.358 m
Flight 5	5.192 m	5.047 m	12.279 m	15.584 m
Flight 6	4.859 m	5.818 m	16.078 m	20.278 m
Flight 7	3.902 m	5.125 m	29.580 m	32.507 m
Flight 8	3.111 m	3.878 m	10.084 m	11.329 m

, which shows that the CMM enhanced system achieves significantly better performances than the HOP enhanced system.

5. Concluding Remarks

In this paper, we proposed a novel, practical, and robust UAV navigation method. A novel cross-modal image matching method is proposed to efficiently and reliably match the aerial infrared image or aerial visible light image with the visible light satellite image.

With the enhancement of the proposed cross-modal image matching method, our navigation method can provide long-time autonomous navigation for the UAV at GNSS denied environments, and it can work effectively in both daylight and night conditions.

In addition, our method can efficiently run at a lower power common embedded platform and the experimental tests show that the states estimation is reliable and precise enough for most applications with UAVs.

The real-time image has to be rectified before the coarse image matching, which means the proposed method cannot be applied to the system with uncalibrated camera. In addition, the attitude of the camera with a standard error of less than 3 degrees is required for real-time image rectification.

In the future work, we will focus on enhancing the precision of the positioning and improving the weather adaptability of the algorithm.