A Novel Fusion Scheme for Vision Aided Inertial Navigation of Aerial Vehicles

Vision-aided inertial navigation is an important and practicalmode of integrated navigation for aerial vehicles. In this paper, a novel fusion scheme is proposed and developed by using the information from inertial navigation system (INS) and vision matching subsystem. This scheme is different from the conventional Kalman filter (CKF); CKF treats these two information sources equally even though vision-aided navigation is linked to uncertainty and inaccuracy. Eventually, by concentrating on reliability of vision matching, the fusion scheme of integrated navigation is upgraded. Not only matching positions are used, but also their reliable extents are considered. Moreover, a fusion algorithm is designed and proved to be the optimal as it minimizes the variance in terms of mean square error estimation. Simulations are carried out to validate the effectiveness of this novel navigation fusion scheme. Results show the new fusion scheme outperforms CKF and adaptive Kalman filter (AKF) in vision/INS estimation under given scenarios and specifications.


Introduction
Over the past decade, aerial vehicles have been widely developed and used in military and civilian cases, such as reconnaissance, surveying, mapping, and geophysical exploration [1].Navigation focuses on monitoring the movement of vehicles and has been a key component in the application of aerial vehicles.Therefore, it is of significance to develop reliable navigation technologies to ensure that the vehicles know their own positions and orientations during missions or tasks.
Inertial navigation system (INS) is the most applicable mode since no external references are required to determine its position, orientation, or velocity once it is initialized [2,3].However, as a dead reckoning process, it lacks accuracy in long-term performance because of the noises and biases contained in inertial measurements [4].
Several navigation frameworks aided by global positioning system (GPS) [1,5], vision [1,[6][7][8][9][10], or terrain [11,12] are usually employed to restrict the growth of INS error.Along with the development of visual sensors and matching algorithms, the vision matching method is a quite potential mode [13][14][15][16].Moreover, infrared and radar imaging ensures that the vision-aided navigation is available regardless of darkness, clouds, or rain (all-weather condition).Navigation accuracy is also improved by these high-resolution measurements.Vision-aided inertial navigation has received considerable attention from scientists and engineers in the past few years [1,[6][7][8][9][10].In image registration (or matching), the sensed images are geometrically overlaid on the reference images prestored in the vehicles [17,18].That is, the position and attitude of a vehicle can be produced by an onboard matcher during flight.The vision-aided approach has advantages in long-term navigation because the matched information is independent of the INS and has no accumulation effect [8].
The well-known conventional Kalman filter (CKF) is widely implemented in vision/INS integration systems.CKF is a variance-minimizing estimator which fuses various information sources [19][20][21], but CKF treats information sources equally, regardless of their origins or principles.
In terms of vision-aided navigation, the matching performance is related with the source reference images.For instance, images from ocean or desert have fewer matchable features, and therefore, the matching error may exceed hundreds of meters.In that case, CKF is not directly suitable for vision/INS integrated navigators because of the incorrect measurement-noise model.
A key step in the performance of the Kalman filter is to determine the appropriate measurement-noise covariance matrix  [22].To solve this problem, several adaptive Kalman filter (AKF) approaches have been proposed to estimate noise covariance [23][24][25], and most of them estimate  by using the innovation or the residual sequence [26,27].That is, the estimated covariance matrix of the innovations or residuals should match its theoretical form [28]. AKF estimates  by using the previous  observations.But for vision-aided navigation, because of the context of source image, matching error may suddenly be transferred from the current epoch to the next epoch.Therefore, AKF may not obtain the appropriate , which will slow the convergence of filtering response and degrade filtering performance.
Different from AKF, a novel fusion scheme was proposed and developed in this paper to automatically set higher weights to reliable vision navigation data and vice versa.The scheme fuses the vision and INS estimates by virtue of reliability.The reliability of image matching data relates to image context and can be calculated transcendentally.It is the probability of successful matching, which is higher in feature-rich areas and lower in feature-insufficient areas.The matching error is divided into two parts: constant error and reliability error.Constant error is decided by vision sensor instrument, installation, and image registration (matching) algorithm, while reliability error is caused by the matching reliability.
The remainder of this paper is organized as follows.The vision/INS navigation fusion problem is formulated in Section 2, and then a novel fusion scheme is proposed and developed for vision-aided inertial navigation in Section 3. The proposed scheme is proved to be optimal to minimize the variance of the mean square error (MSE) estimation.In Section 4, simulations are carried out to validate the effectiveness of the developed vision/INS fusion scheme in comparison with CKF and AKF.Finally, concluding remarks are made in Section 5.

Formulation on Vision/INS Data Fusion Problem
In application of vision-aided navigation, CKF is able to obtain the optimal weighted mean value by combining the vision and INS data of its independent estimates [20].The fusion scheme of CKF is formulated in this section.Both estimates are considered reliable by CKF.But because of the uncertainly in image registration, the position matching error can exceed hundreds of meters in some area with less information content, sometimes even leading to filtering divergence.Hence, this challenge of vision/INS fusion is analyzed.

Conventional Fusion Scheme.
CKF is a popular fusion scheme for integrated navigation.Its scheme of onedimensional data fusion can be formulated as follows [20].

Challenges of Vision/INS Fusion.
A vision-aided navigation sensor can obtain the position and attitude of a vehicle.This paper only concerns the simple situation where position is the only observation.
In vision-aided navigation, when an aerial vehicle passes areas with less information content (e.g., ocean, desert, and campagna), the position matching error can exceed hundreds of meters, leading to a changed measurementnoise covariance matrix.In this circumstance, CKF is not the optimal fusion scheme, and thus the weight factor  in (4) should be changed.Furthermore, not only the errors between the estimated signals but also the reliability of estimated signals should be considered, and the reliability of image matching data relates to image context and can be calculated transcendentally.It is the probability of successful matching, which is higher in feature-rich areas and lower in feature-insufficient areas.Thus, a reliability-based Kalman filter (RKF) was proposed.Figure 1 explains the difference between CKF (left) and RKF (right), and the INS signals of both CKF and RKF were considered reliable (always equals to 1).In contrast with CKF, the reliability of image matching data was considered in RKF.Or, in other words, CKF is the special case of RKF in which the reliability is always equals to 1.
One-dimensional matching results via a vision sensor can be expressed as follows: Matching error is divided into two parts: constant error Δ (3)    and reliability error   .Δ (3)    is the white Gaussian noise decided by the vision sensor, installation, and image registration (matching) algorithm and is constant and independent of the region of the image;   represents the miss-matching error due to unreliability.Without any prior knowledge, uniform distribution is a suitable assumption for miss-matching.Thus, at epoch , the probability density function of   satisfies where  (3)   is the reliability of  (3)   , (, ) is the range of { (3)   }, and  (0)   is the ideal position matching result at epoch .In other words, (6) shows that when the constant error is zero, the reliability is the probability that the estimated signal is equal to the original signal.When  (3)   ≡ 1, which means the vision information is always reliable, ( 5) is equivalent to (2).
In vision-aided navigation, matching reliability can be computed transcendentally by analyzing the reference image before a flight mission off-line.For instance, a feature-rich region has a higher reliability while a smooth region such as campagna has a lower reliability.CKF is not the optimal fusion scheme via the above assumptions.Therefore, a novel vision-aided navigation fusion scheme considering reliability is proposed in the next section, and it is proved to be the optimal fusion scheme since it minimizes the variance in terms of MSE estimation.

Novel Fusion Scheme for Vision-Aided Navigation
In this section, a novel vision-aided navigation Kalman filter considering reliability (RKF) has been proposed in Section 3.1.Then we introduced the reliability-based fusion algorithm in Section 3.2, and it is proved to be optimal with minimum variance restriction in theorem.

Scheme of RKF for Vision/INS Fusion.
With the local level frame of North-East-Down (NED) as the navigational frame, the dynamics of INS can be expressed as In (7), where V  , V  are the velocity errors in the north and east, respectively; , , and  are the attitude errors;   ,   , and   are the gyro's biases;   ,   , and   are the accelerometer's biases: where  is the geographic latitude;  is the geographic longitude; Ω is the skew matrix of the Earth's rotation rate; Re is the Earth's transverse radius;  is the accelerometer's measurement, and  is the vehicle's velocity; and where   is the elements of the direction cosine matrix    .The measurement model is expressed as where V is the measurement noise.The observation is the difference of positions between INS and vision: The corresponding measurement matrix is: The measurement noise matrix in CKF can be expressed as The steps in RKF are as follows.
Inputs.Range of reference image (Range  , Range  ).
Position error between INS and image matching (  ,   ).

RKF Algorithm
Step 1.The possible range of   and   is (−Range  , Range  ) and (−Range  , Range  ), respectively.Therefore, the scale factors are Step 2. Compute the mapped observation error according to (15) Step 3. Let Step 4. Update the measurement noise matrix as follows: Step 5. Execute the CKF with  new .
Mathematical Problems in Engineering 5

Reliability-Based Fusion.
In this section, the reliabilitybased fusion scheme is proposed with minimum-variance restriction.
Condition.The following assumptions hold.

Theorem 1. A reliability-based fusion is given as follows.
The fusion algorithm can be expressed as Finding the optimal factor with minimum-variance restriction is equivalent to Then the weight factor can be expressed as The weight factor is consistent with (17) which is extended to two dimensions.Then we will prove that (24) is the optimal weight factor with minimum-variance restriction.
Proof.The weight factor in (24) is the optimal theoretically.This is shown from ( 25) to (52).
Apparently, when , the fusion formula is equivalent to the traditional form in (4).In other words, the traditional fusion method is a special case of the proposed data fusion method.At each point in the flight trajectory, if the error is less than 4 m (1 pixel), the matching is regarded as correct; otherwise, it is incorrect.Hence,   and   in ( 14) are set as 4 m, which is the constant error in (5).The matching reliability of point is

Generation of Matching Reliability.
where  total = 10 denotes the total number of matches for a single point in the flight trajectory;  correct is the number of correct matches.Matching reliability is shown in Figure 5.It illustrated the matching reliability of point at the position for vehicle at epoch .

Integrated Navigation Results and Discussion
. Tables 1  and 2 show the initial conditions and the specifications of the INS, respectively.
By using the scheme of integrated navigation reliabilitybased Kalman filter (RKF) described in Section 3.1, we randomly choose an image matching result for our experiment (Figure 4(j) is used in the test).
Figures 6(a) and 6(b) show the CKF, RKF, and AKF position errors of integrated navigation.Figure 6 also shows that in this condition with miss-matching, the integral navigation error from CKF is extremely large so CKF cannot fulfill the integrated navigation requirement.Furthermore, the performance of integrated navigation was improved significantly by using matching reliability in the proposed method (RKF).The position error converged to within 2.5 m.
Furthermore, compared with innovation-based AKF [27] (window size  = 30), different window experiment results have similar performance and AKF has a slower response since it estimates the measurement noise covariance matrix  by using the previous  observations, while RKF estimates  via the matching reliability at each single point.

Conclusions
A reliability-based fusion scheme was proposed and developed for the vision-aided inertial navigation systems.Different navigation sources are treated equally in the CKF scheme.As for vision-aided navigation, miss-matching may cause large position errors.It can lead to deficiency of the vision/INS integrated navigation system if two information sources are treated equally in the fusion method.Therefore, a novel fusion scheme is presented, which regulates the fusion weights in terms of reliable extents of the reference scenes.Higher weights are set to the matched information overlaying  on the more reliable regions, and vice versa.Furthermore, the proposed fusion scheme is also proved to minimize the integrated estimated variance such that MSE is achieved.Simulations are also used to validate the effectiveness.The proposed scheme has improved the performance of the integrated navigation, compared with CKF and AKF.

4. 1 .
Scenarios and Specifications.A Matlab/Simulink platform is constructed for simulation of vision-aided inertial navigation.A 600 × 1600 of 4 m resolution synthetic aperture radar (SAR) image is used as the reference.The red lines in Figures 2(a) and 2(b) show the flight trajectory in the real world (55 s) and the reference image, respectively.At each epoch during simulation (50 Hz), the reference image was cropped into a rectangular subimage of 150 × 150 at the central point (, ), which denotes the vehicle's position at that epoch.Then the observed image is obtained by adding white Gaussian noise.And the starting point is (250, 75) in reference image.
Figure 3  illustrates the generation of matching reliability.With a single reference image (Figure2(b)), 10 fully-observed images were obtained by adding different Gaussian white noises (variance = {1 × 0.25% 2 × 0.25% ⋅ ⋅ ⋅ 10 × 0.25%}).As shown in Figure3, at each point in the flight trajectory, the observed images are obtained by cropping the fully-observed image at that point, and thus 10 fully-observed images will generate 10 different matching results.The matching reliability can be obtained by analyzing these matching results (elaborated in the next paragraph), and the 10 image matching errors were illustrated in Figure4.Apparently, the matching error increases because of the increased noise of the observed image.The matching error finally converges because at the beginning of the flight, the image is very smooth and can

Figure 5
clearly shows that the matching reliability gradually increases with time.It is consistent with Figure 2(b) that the features in the reference image increase during the flight course.
Flight trajectory in real world X (north) Y (east) (b) Flight trajectory in image

Figure 5 :
Figure 5: Matching reliability versus time, it represents the matching reliability of point at the position for vehicle at epoch .

Figure 6 :
Figure 6: Latitudinal error (a) and longitudinal error (b) of integral navigation; RKF was compared with AKF and CKF.

Table 2 :
Specification of INS.