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

Abstract

Vision-aided inertial navigation is an important and practical mode 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.

1. 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–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–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–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–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–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.

2. 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.

2.1. Conventional Fusion Scheme

CKF is a popular fusion scheme for integrated navigation. Its scheme of one-dimensional data fusion can be formulated as follows [20]. For one-dimensional discrete-time signal {𝑥_𝑖⁽⁰⁾}, there are two estimates {𝑥_𝑖⁽¹⁾} and {𝑥_𝑖⁽²⁾} which can be expressed as follows: 𝑥_𝑖⁽¹⁾=𝑥_𝑖⁽⁰⁾+Δ𝑥_𝑖⁽¹⁾,Δ𝑥_𝑖⁽¹⁾∼𝑁0,𝜎²₁,𝑥_𝑖⁽²⁾=𝑥_𝑖⁽⁰⁾+Δ𝑥_𝑖⁽²⁾,Δ𝑥_𝑖⁽²⁾∼𝑁0,𝜎²₂, (1)(2) where 𝑁 denotes a normal distribution; Δ𝑥_𝑖⁽¹⁾and Δ𝑥_𝑖⁽²⁾ are both the error between the estimated signal and the original signal. Data fusion according to minimum-variance restriction is expressed as [20]: 𝑥_𝑖^(1,2)=𝜎²₂𝑥_𝑖⁽¹⁾+𝜎²₁𝑥_𝑖⁽²⁾𝜎²₁+𝜎²₂ (3) or 𝑥_𝑖^(1,2)=(1−𝑤)𝑥_𝑖⁽¹⁾+𝑤𝑥_𝑖⁽²⁾𝜎,𝑤=²¹𝜎²₁+𝜎²₂, (4) where 𝑤 is the optimal weight factor. When (4) is extended to multidimension, the equations of CKF can be derived [20].

2.2. 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 measurement-noise 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.

Figure 1 

Conventional Kalman filter (left) and reliability-based Kalman filter (right).

One-dimensional matching results via a vision sensor can be expressed as follows: 𝑥_𝑖⁽³⁾=𝑥𝑢_𝑖+Δ𝑥_𝑖⁽³⁾,Δ𝑥_𝑖⁽³⁾∼𝑁0,𝜎²₃. (5)

Matching error is divided into two parts: constant error Δ𝑥_𝑖⁽³⁾ and reliability error 𝑥𝑢_𝑖. Δ𝑥_𝑖⁽³⁾ 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 𝜑_𝑖()=𝑝𝑥𝑢^𝑖(3)𝑥𝑢_𝑖=𝑥_𝑖⁽⁰⁾1−𝑝_𝑖⁽³⁾𝑏−𝑎𝑥𝑢_𝑖≠𝑥_𝑖⁽⁰⁾, (6) where 𝑝_𝑖⁽³⁾ is the reliability of 𝑥_𝑖⁽³⁾, (𝑎,𝑏) is the range of {𝑥_𝑖⁽³⁾}, and 𝑥_𝑖⁽⁰⁾ 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 𝑝_𝑖⁽³⁾≡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.

3. 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.

3.1. 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 𝛿𝑥̇=𝐹𝛿𝑥+𝐺𝑢. (7) In (7), 𝛿𝑥=𝛿𝛼𝛿𝛽𝛿𝛾𝛿𝑣_𝑁𝛿𝑣_𝐸𝛿𝐿𝛿𝑙^𝑇,𝑢=𝛿𝜔_𝑥𝛿𝜔_𝑦𝛿𝜔_𝑧𝛿𝑓_𝑥𝛿𝑓_𝑦𝛿𝑓_𝑧^𝑇, (8) where 𝛿𝑣_𝑁, 𝛿𝑣_𝐸 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:𝐹=^{+𝑣0−Ωsin𝐿𝐸𝑣Retan𝐿𝑁01Re+𝑣Re−Ωsin𝐿0Ωsin𝐿𝐸𝑣Retan𝐿0Ωcos𝐿+𝐸−1Re𝑣Re00𝑁Re2−𝑣𝑁𝑣Re−Ωcos𝐿−𝐸Re00−tan𝐿𝑣Re−Ωcos𝐿−𝐸Recos2𝐿0}_{0−𝑓𝐷𝑓𝐸𝑣𝐷+𝑣Re−2}^Ωsin𝐿_{𝐸Retan𝐿−𝑣𝐸𝑣2Ωcos𝐿+𝐸Recos2𝐿0𝑓𝐷0−𝑓𝑁+𝑣2Ωsin𝐿𝐸1Retan𝐿𝑣Re𝑁tan𝐿+𝑣𝐷𝑣2Ω𝑁cos𝐿−𝑣𝐷+𝑣sin𝐿𝑁𝑣𝐸Recos2𝐿01000𝑅10000000Recos𝐿𝑣𝐸tan𝐿Recos𝐿0}, (9)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 𝐺=−𝑐₁₁−𝑐₁₂−𝑐₁₃00−𝑐₂₁−𝑐₂₂−𝑐₂₃00−𝑐₃₁−𝑐₃₂−𝑐₃₃00000𝑐₁₁𝑐₁₂000𝑐₂₁𝑐₂₂, (10) where 𝑐_𝑖𝑗 is the elements of the direction cosine matrix 𝐶^𝑛_𝑏. The measurement model is expressed as 𝑧=𝐻𝛿𝑥+𝑣, (11) where 𝑣 is the measurement noise. The observation is the difference of positions between INS and vision: 𝑧=𝛿𝐿𝛿𝑙. (12) The corresponding measurement matrix is: 𝐻=00000100000001. (13)

The measurement noise matrix in CKF can be expressed as 𝜎𝑅=^2𝑁00𝜎²_𝐸. (14)

The steps in RKF are as follows.

Inputs. Range of reference image (Range_𝑁,Range_𝐸).

Position error between INS and image matching (𝐶_𝑁,𝐶_𝐸).

Matching reliability of position for the vehicle at epoch 𝑝_𝑖⁽³⁾, let 𝑄=1−𝑝_𝑖⁽³⁾.

RKF Algorithm

Step 1. The possible range of 𝐶_𝑁 and 𝐶_𝐸 is (−Range_𝑁,Range_𝑁) and (−Range_𝐸,Range_𝐸), respectively. Therefore, the scale factors are 𝑠_𝑁=12Range_𝑁,𝑠_𝐸=12Range_𝐸. (15)

Step 2. Compute the mapped observation error according to (15) 𝐶^′_𝑁=𝑠_𝑁𝐶_𝑁+Range_𝑁,𝐶^′_𝐸=𝑠_𝐸𝐶_𝐸+Range_𝐸. (16)

Step 3. Let 𝐴_𝑁=𝑠²_𝑁𝑄−𝑐_𝑁^′2+𝑐^′_𝑁𝑄²+−𝑐_𝑁^′2+𝑐^′_𝑁−14𝑄+𝑐_𝑁^′2−𝑐^′_𝑁+13,𝐴_𝐸=𝑠²_𝐸𝑄−𝑐_𝐸^′2+𝑐^′_𝐸𝑄²+−𝑐_𝐸^′2+𝑐^′_𝐸−14𝑄+𝑐_𝐸^′2−𝑐^′_𝐸+13. (17)

Step 4. Update the measurement noise matrix as follows: 𝑅_new=𝜎²_𝑁+𝐴_𝑁00𝜎²_𝐸+𝐴_𝐸. (18)

Step 5. Execute the CKF with 𝑅_new.

3.2. Reliability-Based Fusion

In this section, the reliability-based fusion scheme is proposed with minimum-variance restriction.

Condition. The following assumptions hold.(a) Rewrite (5) into 𝑥_𝑖⁽³⁾=𝑥𝑢_𝑖+Δ𝑥_𝑖⁽³⁾,Δ𝑥_𝑖⁽³⁾∼𝑁0,𝜎²₃, (19)(b) where 𝑥_𝑖⁽³⁾ denotes the positioning result via the vision sensor. At epoch 𝑖, the probability density function of 𝑥𝑢_𝑖 satisfies 𝜑_𝑖()=𝑝𝑥𝑢^𝑖(3)𝑥𝑢_𝑖=𝑥_𝑖⁽⁰⁾1−𝑝_𝑖⁽³⁾𝑏−𝑎𝑥𝑢_𝑖≠𝑥_𝑖⁽⁰⁾, (20) where 𝑝_𝑖⁽³⁾ is the reliability of 𝑥_𝑖⁽³⁾ and (𝑎,𝑏) is the range of {𝑥_𝑖⁽³⁾}.

Theorem 1. A reliability-based fusion is given as follows.
The fusion algorithm can be expressed as 𝑥_𝑖^(1,3)=(1−𝑤)𝑥_𝑖⁽¹⁾+𝑤𝑥_𝑖⁽³⁾. (21)
Finding the optimal factor with minimum-variance restriction is equivalent to 𝑤^∗=argmin_𝑤𝐷𝑥_𝑖^(1,3). (22) Let 1𝑠=,𝑐𝑏−𝑎_′=𝑥_𝑖⁽⁰⁾−𝑎𝑠,𝑄=1−𝑝_𝑖⁽³⁾. (23)
Then the weight factor can be expressed as 𝑤^∗=𝜎²₁×𝜎²₁+𝜎²₂+𝑠²𝑄×−𝑐^′2+𝑐^′𝑄²+−𝑐^′2+𝑐^′−14𝑄+𝑐^′2−𝑐^′+13⁻¹. (24)

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).
On the right side of (19), the two terms with 𝑥_𝑖⁽³⁾ are independent of each other. According to the property of the variance, then 𝐷𝑥_𝑖⁽³⁾=𝐷𝑥𝑢_𝑖+𝐷Δ𝑥_𝑖⁽³⁾. (25) Obviously, 𝐷Δ𝑥_𝑖⁽³⁾=𝜎²₃, (26) and thus only 𝐷(𝑥𝑢_𝑖) is needed to obtain 𝐷(𝑥_𝑖⁽³⁾).
At epoch 𝑖, let 𝑥=𝑥𝑢_𝑖, 𝑐=𝑥_𝑖⁽⁰⁾, and 𝑝=𝑝_𝑖⁽³⁾; then the probability density function is expressed as 𝜑(𝑥)=𝛿(𝑥−𝑐)⋅𝑝𝑥=𝑐1−𝑝𝑏−𝑎𝑥≠𝑐,𝑎≤𝑥≤𝑏, (27) where 𝛿 is the Kronecker delta function. The expectation can be computed as 𝐸(𝑥)=^+∞_−∞(𝑥)=𝑥⋅𝜑⋅𝑑𝑥_𝑎𝑐−0𝑥⋅𝜑(𝑥)⋅𝑑𝑥+_𝑐𝑥⋅𝜑(𝑥)⋅𝑑𝑥+^𝑎_𝑐+0𝑥⋅𝜑(𝑥)⋅𝑑𝑥. (28)
As 𝜑(𝑐) is the first type of discontinuity point, (28) can be expressed as 𝐸(𝑥)=^𝑏_𝑎𝑥⋅1−𝑝𝑏−𝑎⋅𝑑𝑥+_𝑐()=()𝑥⋅𝛿𝑥−𝑐⋅𝑝⋅𝑑𝑥1−𝑝𝑎+𝑏2+_𝑐()=()𝑥⋅𝛿𝑥−𝑐⋅𝑝⋅𝑑𝑥1−𝑝𝑎+𝑏2+𝑐𝑝. (29)
The variance can be expressed as 𝐷(𝑥)=^+∞_−∞(𝑥−𝐸(𝑥))²𝜑(𝑥)𝑑𝑥. (30) Therefore, 𝐷(𝑥)=^+∞_−∞()𝑥−1−𝑝𝑎+𝑏2−𝑐𝑝²1−𝑝𝑏−𝑎𝑑𝑥+_𝑐()𝑥−1−𝑝𝑎+𝑏2−𝑐𝑝²⋅𝛿(𝑥−𝑐)⋅𝑝⋅𝑑𝑥. (31)
The two terms on the right side of (31) are defined as 𝐷₁(𝑥) and 𝐷₂(𝑥); then 𝐷₁(𝑥)=^+∞_−∞()𝑥−1−𝑝𝑎+𝑏2−𝑐𝑝²1−𝑝=𝑎𝑏−𝑎𝑑𝑥1−𝑝²+𝑎𝑏+𝑏²3+𝑝−1(𝑎+𝑏)²2−𝑐𝑝(𝑎+𝑏)+1−𝑝²(𝑎+𝑏)²4+𝑐²𝑝²+1−𝑝(𝑎+𝑏)𝑐𝑝. (32)
In Kronecker delta function 𝛿(𝑥), ^+∞_−∞𝛿(𝑥−𝑐)𝑑𝑥=_𝑐𝛿(𝑥−𝑐)𝑑𝑥=1,_+∞−∞𝑓(𝑥)𝛿(𝑥−𝑐)𝑑𝑥=𝑓(𝑐). (33) Therefore, 𝐷₂(𝑥)=_𝑐()𝑥−1−𝑝𝑎+𝑏2−𝑐𝑝²⋅𝛿(𝑥−𝑐)⋅𝑝⋅𝑑𝑥=𝑝𝑐²+𝑝1−𝑝²(𝑎+𝑏)²4+𝑐²𝑝³−𝑐𝑝1−𝑝(𝑎+𝑏)−2𝑐²𝑝+𝑐𝑝²1−𝑝(𝑎+𝑏). (34)
To simplify the calculation, (𝑎,𝑏) is mapped into (0,1); {𝑥_𝑖⁽¹⁾} and {𝑥_𝑖⁽³⁾} are mapped to {𝑥_𝑖^′(1)} and {𝑥_𝑖^′(2)}, respectively. They can be expressed as 𝑥_𝑖^′(1)=𝑥_𝑖⁽¹⁾−𝑎,𝑥𝑏−𝑎_𝑖′(3)=𝑥_𝑖⁽³⁾−𝑎,𝑐𝑏−𝑎_′=𝑐−𝑎.𝑏−𝑎 (35)(36)
𝑠 is defined as the scale factor 1𝑠=.𝑏−𝑎 (37)
After mapping, let 𝐷^′₁(𝑥)=𝐷₁(𝑥)𝑠²,𝐷^′₂(𝑥)=𝐷₂(𝑥)𝑠². (38) Likewise, 𝜎^′₁=𝜎₁𝑠,𝜎^′₂=𝜎₂𝑠. (39)
Let 𝑄=1−𝑝; then 𝐷^′₁(𝑥)=14𝑄³+−2𝑐^′2+2𝑐^′−12𝑄²+𝑐^′2−𝑐^′+13𝐷𝑄,_′2(𝑥)=−𝑐^′2+𝑐^′−14𝑄³+𝑐^′2−𝑐^′+14𝑄². (40)(41)
By adding (40) and (41), 𝐷^′(𝑥)=−𝑐^′2+𝑐^′𝑄³+−𝑐^′2+𝑐^′−14𝑄²+𝑐^′2−𝑐^′+13𝑄. (42)
According to (25), (26), and (42), 𝐷^′𝑥_𝑖⁽³⁾=−𝑐^′2+𝑐^′𝑄³+−𝑐^′2+𝑐^′−14𝑄²+𝑐^′2−𝑐^′+13𝑄+𝜎₂^′2. (43) Let 𝑥_𝑖^′(1,3)=𝑤₁𝑥_𝑖^′(1)+𝑤₂𝑥_𝑖^′(3). (44) Therefore, 𝐷𝑥_𝑖^′(1,3)=𝐸𝑥_𝑖^′(1,3)−𝐸𝑥_𝑖^′(1,3)2=𝐸𝑤₁𝑥_𝑖^′(1)+𝑤₂𝑥_𝑖^′(2)−𝑤₁𝐸𝑥_𝑖^′(1)−𝑤₂𝐸𝑥_𝑖^′(2)2=𝐸𝑤²₁𝑥_𝑖^′(1)−𝐸𝑥_𝑖^′(1)2+𝑤²₂𝑥_𝑖^′(2)−𝐸𝑥_𝑖^′(2)2=𝑤²₁𝐸𝑥_𝑖^′(1)−𝐸𝑥_𝑖^′(1)2+𝑤²₂𝐸𝑥_𝑖^′(2)−𝐸𝑥_𝑖^′(2)2=𝑤²₁𝜎₁^′2+𝑤²₂𝜎₂^′2+𝑤²₂−𝑐^′2+𝑐^′𝑄³+−𝑐^′2+𝑐^′−14𝑄²+𝑐^′2−𝑐^′+13𝑄. (45) Let 𝑤₂=𝑤^′, 𝑤₁=1−𝑤^′; then 𝐷𝑥_𝑖^′(1,3)=1−𝑤^′2𝜎₁^′2+𝑤^′2𝜎₂^′2+𝑤^′2−𝑐²+𝑐𝑄³+−𝑐²1+𝑐−4𝑄²+𝑐²1−𝑐+3𝑄. (46) The value of 𝑤^′, which minimizes 𝐷(𝑥_𝑖^′(1,3)), is obtained by differentiating the above equation with respect to 𝑤^′. Hence, 𝑑𝐷𝑥_𝑖^′(1,3)𝑑𝑤^′=−21−𝑤^′𝜎₁^′2+2𝑤^′𝜎₂^′2+−𝑐^′2+𝑐^′𝑄³+−𝑐^′2+𝑐^′−14𝑄²+𝑐^′2−𝑐^′+13𝑄=0. (47) Therefore, the optimal weight factor according to minimum variance restriction is 𝑤^′=𝜎₁^′2×𝜎₁^′2+𝜎₂^′2+−𝑐^′2+𝑐^′𝑄³+−𝑐^′2+𝑐^′−14𝑄²+𝑐^′2−𝑐^′+13𝑄⁻¹. (48)
According to (35), 𝑤^′=𝜎²₁×𝜎²₁+𝜎²₂+𝑠²𝑄×−𝑐^′2+𝑐^′𝑄²+−𝑐^′2+𝑐^′−14𝑄+𝑐^′2−𝑐^′+13⁻¹. (49)
From (21), (35), and (44), 𝑤=𝑤^′. (50) The corresponding data fusion formula is 𝑥_𝑖^(1,3)=(1−𝑤)𝑥_𝑖⁽¹⁾+𝑤𝑥_𝑖⁽³⁾. (51)
Hence, (24) was proved. But in real application, it is usually unable to obtain the value of 𝑥_𝑖⁽⁰⁾. Therefore, 𝑥_𝑖⁽¹⁾ is used instead: 𝑥_𝑖⁽¹⁾≈𝑐=𝑥_𝑖⁽⁰⁾. (52) Apparently, when 𝑝_𝑖⁽³⁾=1, 𝑤=𝜎²₁/(𝜎²₁+𝜎²₂), 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.

4. Simulation and Discussion

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.

(a) Flight trajectory in real world

(b) Flight trajectory in image

Figure 2 

Flight trajectory.

4.2. Generation of Matching Reliability

Figure 3 illustrates the generation of matching reliability. With a single reference image (Figure 2(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 Figure 3, 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 Figure 4. 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 easily cause large matching error; after 10–30 sec, the image’s context becomes relatively rich, which allows more reliable matching.

Figure 3 

Generation of matching reliability.

(a) 1st image registration position error

(b) 2nd image registration position error

(c) 3rd image registration position error

(d) 4th image registration position error

(e) 5th image registration position error

(f) 6th image registration position error

(g) 7th image registration position error

(h) 8th image registration position error

(i) 9th image registration position error

(j) 10th image registration position error

Figure 4 

10 image matching errors, different figure represents image matching errors by using different fully observed image.

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 𝑁𝑝=^correct𝑁_total, (53) 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 𝑡.

Figure 5 

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

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.

4.3. Integrated Navigation Results and Discussion

Tables 1 and 2 show the initial conditions and the specifications of the INS, respectively.

Table 1 

Initial conditions.

Table 2 

Specification of INS.

By using the scheme of integrated navigation reliability-based 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.

(a) Latitudinal error

(b) Longitudinal error

Figure 6 

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

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.

5. 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.

Acknowledgments

This work is jointly supported by the National Natural Science Foundation of China (Grant no. 41204026) and the Open Research Foundation of Science and Technology on Aerospace Flight Dynamics Laboratory (Grant no. 2012afdl010). The authors would thank Dr. Yanlong BU for his good suggestions and inspirations.