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

Authors: Ming Xiao; Liang Pan; Tianjiang Hu; Lincheng Shen
Journal: Mathematical Problems in Engineering
 (2013)

---

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


---

## Body

## 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 matrixR [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 R 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 R by using the previous m 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 R, 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 Section2, 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 {xi(0)}, there are two estimates {xi(1)} and {xi(2)} which can be expressed as follows:
(1)xi(1)=xi(0)+Δxi(1),{Δxi(1)}~N(0,σ12),(2)xi(2)=xi(0)+Δxi(2),{Δxi(2)}~N(0,σ22),
where N denotes a normal distribution; Δxi(1)and Δxi(2) are both the error between the estimated signal and the original signal. Data fusion according to minimum-variance restriction is expressed as [20]:
(3)xi(1,2)=σ22xi(1)+σ12xi(2)σ12+σ22
or
(4)xi(1,2)=(1-w)xi(1)+wxi(2),w=σ12σ12+σ22,
where w 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 factorw 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:(5)xi(3)=xui+Δxi(3),{Δxi(3)}~N(0,σ32).Matching error is divided into two parts: constant errorΔxi(3) and reliability error xui. Δxi(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; xui represents the miss-matching error due to unreliability. Without any prior knowledge, uniform distribution is a suitable assumption for miss-matching. Thus, at epoch i, the probability density function of xui satisfies
(6)φi(xu)={pi(3)xui=xi(0)1-pi(3)b-axui≠xi(0),
where pi(3) is the reliability of xi(3), (a,b) is the range of {xi(3)}, and xi(0) is the ideal position matching result at epoch i. 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 pi(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.


## 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 {xi(0)}, there are two estimates {xi(1)} and {xi(2)} which can be expressed as follows:
(1)xi(1)=xi(0)+Δxi(1),{Δxi(1)}~N(0,σ12),(2)xi(2)=xi(0)+Δxi(2),{Δxi(2)}~N(0,σ22),
where N denotes a normal distribution; Δxi(1)and Δxi(2) are both the error between the estimated signal and the original signal. Data fusion according to minimum-variance restriction is expressed as [20]:
(3)xi(1,2)=σ22xi(1)+σ12xi(2)σ12+σ22
or
(4)xi(1,2)=(1-w)xi(1)+wxi(2),w=σ12σ12+σ22,
where w 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 factorw 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:(5)xi(3)=xui+Δxi(3),{Δxi(3)}~N(0,σ32).Matching error is divided into two parts: constant errorΔxi(3) and reliability error xui. Δxi(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; xui represents the miss-matching error due to unreliability. Without any prior knowledge, uniform distribution is a suitable assumption for miss-matching. Thus, at epoch i, the probability density function of xui satisfies
(6)φi(xu)={pi(3)xui=xi(0)1-pi(3)b-axui≠xi(0),
where pi(3) is the reliability of xi(3), (a,b) is the range of {xi(3)}, and xi(0) is the ideal position matching result at epoch i. 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 pi(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.


## 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 Section3.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)δx˙=Fδx+Gu.
In (7),
(8)δx=[δαδβδγδvNδvEδLδl]T,u=[δωxδωyδωzδfxδfyδfz]T,
where δvN, δvE are the velocity errors in the north and east, respectively; δα, δβ, and δγ are the attitude errors; δωx, δωy, and δωz are the gyro’s biases; δfx, δfy, and δfz are the accelerometer’s biases:(9)F=[0-(ΩsinL+vERetanL)vNRe01Re-ΩsinL0(ΩsinL+vERetanL)0ΩcosL+vERe-1Re00vNRe2-vNRe-ΩcosL-vERe00-tanLRe-ΩcosL-vERecos2L00-fDfEvDRe-2(ΩsinL+vERetanL)-vE(2ΩcosL+vERecos2L)0fD0-fN(2ΩsinL+vERetanL)1Re(vNtanL+vD)(2Ω(vNcosL-vDsinL)+vNvERecos2L)00001R00000001Re cosLvEtanLRe cosL0],where L is the geographic latitude; l is the geographic longitude; Ω is the skew matrix of the Earth’s rotation rate; Re is the Earth’s transverse radius; f is the accelerometer’s measurement, and V is the vehicle’s velocity; and
(10)G=[-c11-c12-c1300-c21-c22-c2300-c31-c32-c3300000c11c12000c21c22],
where cij is the elements of the direction cosine matrix Cbn. The measurement model is expressed as
(11)z=Hδx+v,
where v is the measurement noise. The observation is the difference of positions between INS and vision:
(12)z=[δLδl].
The corresponding measurement matrix is:
(13)H=[00000100000001].The measurement noise matrix in CKF can be expressed as(14)R=[σN200σE2].The steps in RKF are as follows.Inputs. Range of reference image (RangeN,RangeE).Position error between INS and image matching(CN,CE).Matching reliability of position for the vehicle at epochpi(3), let Q=1-pi(3).RKF AlgorithmStep 1.
The possible range ofCN and CE is (-RangeN,RangeN) and (-RangeE,RangeE), respectively. Therefore, the scale factors are
(15)sN=12RangeN,sE=12RangeE.Step 2.
Compute the mapped observation error according to (15)
(16)CN′=sN(CN+RangeN),CE′=sE(CE+RangeE).Step 3.
Let(17)AN=sN2Q[(-cN′2+cN′)Q2+(-cN′2+cN′-14)Q+(cN′2-cN′+13)],AE=sE2Q[(-cE′2+cE′)Q2+(-cE′2+cE′-14)Q+(cE′2-cE′+13)].Step 4.
Update the measurement noise matrix as follows:(18)Rnew=[σN2+AN00σE2+AE].Step 5.
Execute the CKF withRnew.


### 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
(19)xi(3)=xui+Δxi(3),{Δxi(3)}~N(0,σ32),(b)
wherexi(3) denotes the positioning result via the vision sensor. At epoch i, the probability density function of xui satisfies
(20)φi(xu)={pi(3)xui=xi(0)1-pi(3)b-axui≠xi(0),
where pi(3) is the reliability of xi(3) and (a,b) is the range of {xi(3)}.Theorem 1.
A reliability-based fusion is given as follows.
The fusion algorithm can be expressed as(21)xi(1,3)=(1-w)xi(1)+wxi(3).
Finding the optimal factor with minimum-variance restriction is equivalent to(22)w*=argminw(D(xi(1,3))).
Let
(23)s=1b-a,c′=xi(0)-as,Q=1-pi(3).
Then the weight factor can be expressed as(24)w*=σ12×((-c′2+c′-14)σ12+σ22+s2Q×[(-c′2+c′-14)(-c′2+c′)Q2+(-c′2+c′-14)Q+(c′2-c′+13)])-1.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 xi(3) are independent of each other. According to the property of the variance, then
(25)D(xi(3))=D(xui)+D(Δxi(3)).
Obviously,
(26)D(Δxi(3))=σ32,
and thus only D(xui) is needed to obtain D(xi(3)).
At epochi, let x=xui, c=xi(0), and p=pi(3); then the probability density function is expressed as
(27)φ(x)={δ(x-c)·px=c1-pb-ax≠c,a≤x≤b,
where δ is the Kronecker delta function. The expectation can be computed as
(28)E(x)=∫-∞+∞x·φ(x)·dx=∫ac-0x·φ(x)·dx+∫cx·φ(x)·dx+∫c+0ax·φ(x)·dx.
Asφ(c) is the first type of discontinuity point, (28) can be expressed as
(29)E(x)=∫abx·1-pb-a·dx+∫cx·δ(x-c)·p·dx=(1-p)(a+b)2+∫cx·δ(x-c)·p·dx=(1-p)(a+b)2+cp.
The variance can be expressed as(30)D(x)=∫-∞+∞(x-E(x))2φ(x)dx.
Therefore,
(31)D(x)=∫-∞+∞(x-(1-p)(a+b)2-cp)21-pb-adx+∫c(x-(1-p)(a+b)2-cp)2·δ(x-c)·p·dx.
The two terms on the right side of (31) are defined as D1(x) and D2(x); then
(32)D1(x)=∫-∞+∞(x-(1-p)(a+b)2-cp)21-pb-adx=(1-p){(a2+ab+b2)3+(p-1)(a+b)22-cp(a+b)+(1-p)2(a+b)24+c2p2+(1-p)(a+b)cp(a2+ab+b2)3}.
In Kronecker delta functionδ(x),
(33)∫-∞+∞δ(x-c)dx=∫cδ(x-c)dx=1,∫-∞+∞f(x)δ(x-c)dx=f(c).
Therefore,
(34)D2(x)=∫c(x-(1-p)(a+b)2-cp)2·δ(x-c)·p·dx=pc2+p(1-p)2(a+b)24+c2p3-cp(1-p)(a+b)-2c2p+cp2(1-p)(a+b).
To simplify the calculation,(a,b) is mapped into (0,1); {xi(1)} and {xi(3)} are mapped to {xi′(1)} and {xi′(2)}, respectively. They can be expressed as
(35)xi′(1)=xi(1)-ab-a,xi′(3)=xi(3)-ab-a,(36)c′=c-ab-a.
s is defined as the scale factor
(37)s=1b-a.
After mapping, let(38)D1′(x)=D1(x)s2,D2′(x)=D2(x)s2.
Likewise,
(39)σ1′=σ1s,σ2′=σ2s.
LetQ=1-p; then
(40)D1′(x)=14Q3+(-2c′2+2c′-12)Q2+(c′2-c′+13)Q,(41)D2′(x)=(-c′2+c′-14)Q3+(c′2-c′+14)Q2.
By adding (40) and (41),
(42)D′(x)=(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q.
According to (25), (26), and (42),
(43)D′(xi(3))=(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q+σ2′2.
Let
(44)xi′(1,3)=w1xi′(1)+w2xi′(3).
Therefore,
(45)D(xi′(1,3))=E{[xi′(1,3)-E(xi′(1,3))]2}=E{[w1xi′(1)+w2xi′(2)-w1E(xi′(1))-w2E(xi′(2))]2}=E{w12[xi′(1)-E(xi′(1))]2+w22[xi′(2)-E(xi′(2))]2}=w12E{[xi′(1)-E(xi′(1))]2}+w22E{[xi′(2)-E(xi′(2))]2}=w12σ1′2+w22σ2′2+w22((-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q).
Let w2=w′, w1=1-w′; then
(46)D(xi′(1,3))=(1-w′2)σ1′2+w′2σ2′2+w′2((-c2+c)Q3+(-c2+c-14)Q2+(c2-c+13)Q).
The value of w′, which minimizes D(xi′(1,3)), is obtained by differentiating the above equation with respect to w′. Hence,
(47)d[D(xi′(1,3))]dw′=-2(1-w′)σ1′2+2w′((-c′2+c′-14)σ2′2+(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q)=0.
Therefore, the optimal weight factor according to minimum variance restriction is
(48)w′=σ1′2×((-c′2+c′-14)σ1′2+σ2′2+(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q)-1.
According to (35),
(49)w′=σ12×((-c′2+c′-14)σ12+σ22+s2Q×[(-c′2+c′-14)(-c′2+c′)Q2+(-c′2+c′-14)Q+(c′2-c′+13)])-1.
From (21), (35), and (44),
(50)w=w′.
The corresponding data fusion formula is
(51)xi(1,3)=(1-w)xi(1)+wxi(3).
Hence, (24) was proved. But in real application, it is usually unable to obtain the value of xi(0). Therefore, xi(1) is used instead:
(52)xi(1)≈c=xi(0).
Apparently, when pi(3)=1, w=σ12/(σ12+σ22), 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.


## 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)δx˙=Fδx+Gu.
In (7),
(8)δx=[δαδβδγδvNδvEδLδl]T,u=[δωxδωyδωzδfxδfyδfz]T,
where δvN, δvE are the velocity errors in the north and east, respectively; δα, δβ, and δγ are the attitude errors; δωx, δωy, and δωz are the gyro’s biases; δfx, δfy, and δfz are the accelerometer’s biases:(9)F=[0-(ΩsinL+vERetanL)vNRe01Re-ΩsinL0(ΩsinL+vERetanL)0ΩcosL+vERe-1Re00vNRe2-vNRe-ΩcosL-vERe00-tanLRe-ΩcosL-vERecos2L00-fDfEvDRe-2(ΩsinL+vERetanL)-vE(2ΩcosL+vERecos2L)0fD0-fN(2ΩsinL+vERetanL)1Re(vNtanL+vD)(2Ω(vNcosL-vDsinL)+vNvERecos2L)00001R00000001Re cosLvEtanLRe cosL0],where L is the geographic latitude; l is the geographic longitude; Ω is the skew matrix of the Earth’s rotation rate; Re is the Earth’s transverse radius; f is the accelerometer’s measurement, and V is the vehicle’s velocity; and
(10)G=[-c11-c12-c1300-c21-c22-c2300-c31-c32-c3300000c11c12000c21c22],
where cij is the elements of the direction cosine matrix Cbn. The measurement model is expressed as
(11)z=Hδx+v,
where v is the measurement noise. The observation is the difference of positions between INS and vision:
(12)z=[δLδl].
The corresponding measurement matrix is:
(13)H=[00000100000001].The measurement noise matrix in CKF can be expressed as(14)R=[σN200σE2].The steps in RKF are as follows.Inputs. Range of reference image (RangeN,RangeE).Position error between INS and image matching(CN,CE).Matching reliability of position for the vehicle at epochpi(3), let Q=1-pi(3).RKF AlgorithmStep 1.
The possible range ofCN and CE is (-RangeN,RangeN) and (-RangeE,RangeE), respectively. Therefore, the scale factors are
(15)sN=12RangeN,sE=12RangeE.Step 2.
Compute the mapped observation error according to (15)
(16)CN′=sN(CN+RangeN),CE′=sE(CE+RangeE).Step 3.
Let(17)AN=sN2Q[(-cN′2+cN′)Q2+(-cN′2+cN′-14)Q+(cN′2-cN′+13)],AE=sE2Q[(-cE′2+cE′)Q2+(-cE′2+cE′-14)Q+(cE′2-cE′+13)].Step 4.
Update the measurement noise matrix as follows:(18)Rnew=[σN2+AN00σE2+AE].Step 5.
Execute the CKF withRnew.


## 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
(19)xi(3)=xui+Δxi(3),{Δxi(3)}~N(0,σ32),(b)
wherexi(3) denotes the positioning result via the vision sensor. At epoch i, the probability density function of xui satisfies
(20)φi(xu)={pi(3)xui=xi(0)1-pi(3)b-axui≠xi(0),
where pi(3) is the reliability of xi(3) and (a,b) is the range of {xi(3)}.Theorem 1.
A reliability-based fusion is given as follows.
The fusion algorithm can be expressed as(21)xi(1,3)=(1-w)xi(1)+wxi(3).
Finding the optimal factor with minimum-variance restriction is equivalent to(22)w*=argminw(D(xi(1,3))).
Let
(23)s=1b-a,c′=xi(0)-as,Q=1-pi(3).
Then the weight factor can be expressed as(24)w*=σ12×((-c′2+c′-14)σ12+σ22+s2Q×[(-c′2+c′-14)(-c′2+c′)Q2+(-c′2+c′-14)Q+(c′2-c′+13)])-1.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 xi(3) are independent of each other. According to the property of the variance, then
(25)D(xi(3))=D(xui)+D(Δxi(3)).
Obviously,
(26)D(Δxi(3))=σ32,
and thus only D(xui) is needed to obtain D(xi(3)).
At epochi, let x=xui, c=xi(0), and p=pi(3); then the probability density function is expressed as
(27)φ(x)={δ(x-c)·px=c1-pb-ax≠c,a≤x≤b,
where δ is the Kronecker delta function. The expectation can be computed as
(28)E(x)=∫-∞+∞x·φ(x)·dx=∫ac-0x·φ(x)·dx+∫cx·φ(x)·dx+∫c+0ax·φ(x)·dx.
Asφ(c) is the first type of discontinuity point, (28) can be expressed as
(29)E(x)=∫abx·1-pb-a·dx+∫cx·δ(x-c)·p·dx=(1-p)(a+b)2+∫cx·δ(x-c)·p·dx=(1-p)(a+b)2+cp.
The variance can be expressed as(30)D(x)=∫-∞+∞(x-E(x))2φ(x)dx.
Therefore,
(31)D(x)=∫-∞+∞(x-(1-p)(a+b)2-cp)21-pb-adx+∫c(x-(1-p)(a+b)2-cp)2·δ(x-c)·p·dx.
The two terms on the right side of (31) are defined as D1(x) and D2(x); then
(32)D1(x)=∫-∞+∞(x-(1-p)(a+b)2-cp)21-pb-adx=(1-p){(a2+ab+b2)3+(p-1)(a+b)22-cp(a+b)+(1-p)2(a+b)24+c2p2+(1-p)(a+b)cp(a2+ab+b2)3}.
In Kronecker delta functionδ(x),
(33)∫-∞+∞δ(x-c)dx=∫cδ(x-c)dx=1,∫-∞+∞f(x)δ(x-c)dx=f(c).
Therefore,
(34)D2(x)=∫c(x-(1-p)(a+b)2-cp)2·δ(x-c)·p·dx=pc2+p(1-p)2(a+b)24+c2p3-cp(1-p)(a+b)-2c2p+cp2(1-p)(a+b).
To simplify the calculation,(a,b) is mapped into (0,1); {xi(1)} and {xi(3)} are mapped to {xi′(1)} and {xi′(2)}, respectively. They can be expressed as
(35)xi′(1)=xi(1)-ab-a,xi′(3)=xi(3)-ab-a,(36)c′=c-ab-a.
s is defined as the scale factor
(37)s=1b-a.
After mapping, let(38)D1′(x)=D1(x)s2,D2′(x)=D2(x)s2.
Likewise,
(39)σ1′=σ1s,σ2′=σ2s.
LetQ=1-p; then
(40)D1′(x)=14Q3+(-2c′2+2c′-12)Q2+(c′2-c′+13)Q,(41)D2′(x)=(-c′2+c′-14)Q3+(c′2-c′+14)Q2.
By adding (40) and (41),
(42)D′(x)=(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q.
According to (25), (26), and (42),
(43)D′(xi(3))=(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q+σ2′2.
Let
(44)xi′(1,3)=w1xi′(1)+w2xi′(3).
Therefore,
(45)D(xi′(1,3))=E{[xi′(1,3)-E(xi′(1,3))]2}=E{[w1xi′(1)+w2xi′(2)-w1E(xi′(1))-w2E(xi′(2))]2}=E{w12[xi′(1)-E(xi′(1))]2+w22[xi′(2)-E(xi′(2))]2}=w12E{[xi′(1)-E(xi′(1))]2}+w22E{[xi′(2)-E(xi′(2))]2}=w12σ1′2+w22σ2′2+w22((-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q).
Let w2=w′, w1=1-w′; then
(46)D(xi′(1,3))=(1-w′2)σ1′2+w′2σ2′2+w′2((-c2+c)Q3+(-c2+c-14)Q2+(c2-c+13)Q).
The value of w′, which minimizes D(xi′(1,3)), is obtained by differentiating the above equation with respect to w′. Hence,
(47)d[D(xi′(1,3))]dw′=-2(1-w′)σ1′2+2w′((-c′2+c′-14)σ2′2+(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q)=0.
Therefore, the optimal weight factor according to minimum variance restriction is
(48)w′=σ1′2×((-c′2+c′-14)σ1′2+σ2′2+(-c′2+c′)Q3+(-c′2+c′-14)Q2+(c′2-c′+13)Q)-1.
According to (35),
(49)w′=σ12×((-c′2+c′-14)σ12+σ22+s2Q×[(-c′2+c′-14)(-c′2+c′)Q2+(-c′2+c′-14)Q+(c′2-c′+13)])-1.
From (21), (35), and (44),
(50)w=w′.
The corresponding data fusion formula is
(51)xi(1,3)=(1-w)xi(1)+wxi(3).
Hence, (24) was proved. But in real application, it is usually unable to obtain the value of xi(0). Therefore, xi(1) is used instead:
(52)xi(1)≈c=xi(0).
Apparently, when pi(3)=1, w=σ12/(σ12+σ22), 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 Figures2(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 (x,y), 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.Flight trajectory.
(a)
Flight trajectory in real world
(b)
Flight trajectory in image


### 4.2. Generation of Matching Reliability

Figure3 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.10 image matching errors, different figure represents image matching errors by using different fully observed image.
(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 errorAt 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,σN and σE in (14) are set as 4 m, which is the constant error in (5). The matching reliability of point is
(53)p=NcorrectNtotal,
where Ntotal=10 denotes the total number of matches for a single point in the flight trajectory; Ncorrect 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 t.Figure 5
Matching reliability versus time, it represents the matching reliability of point at the position for vehicle at epocht.Figure5 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

Tables1 and 2 show the initial conditions and the specifications of the INS, respectively.Table 1
Initial conditions.
INS
SAR
Longitude, latitude (deg)
120,30
120,30
Height  (m)
10000
10000
Velocity  (NED) (m/s)
0,100,0
0,100,0
Attitude  (roll, pitch,  and yaw) (deg)
0,0,0
0,0,0Table 2
Specification of INS.
Gyro drift (deg/h)
1
Acceleration zero bias (g)
10
-
3By using the scheme of integrated navigation reliability-based Kalman filter (RKF) described in Section3.1, we randomly choose an image matching result for our experiment (Figure 4(j) is used in the test).Figures6(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.Latitudinal error (a) and longitudinal error (b) of integral navigation; RKF was compared with AKF and CKF.
(a)
Latitudinal error
(b)
Longitudinal errorFurthermore, compared with innovation-based AKF [27] (window size m=30), different window experiment results have similar performance and AKF has a slower response since it estimates the measurement noise covariance matrix R by using the previous m observations, while RKF estimates R via the matching reliability at each single point.


## 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 Figures2(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 (x,y), 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.Flight trajectory.
(a)
Flight trajectory in real world
(b)
Flight trajectory in image


## 4.2. Generation of Matching Reliability

Figure3 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.10 image matching errors, different figure represents image matching errors by using different fully observed image.
(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 errorAt 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,σN and σE in (14) are set as 4 m, which is the constant error in (5). The matching reliability of point is
(53)p=NcorrectNtotal,
where Ntotal=10 denotes the total number of matches for a single point in the flight trajectory; Ncorrect 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 t.Figure 5
Matching reliability versus time, it represents the matching reliability of point at the position for vehicle at epocht.Figure5 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

Tables1 and 2 show the initial conditions and the specifications of the INS, respectively.Table 1
Initial conditions.
INS
SAR
Longitude, latitude (deg)
120,30
120,30
Height  (m)
10000
10000
Velocity  (NED) (m/s)
0,100,0
0,100,0
Attitude  (roll, pitch,  and yaw) (deg)
0,0,0
0,0,0Table 2
Specification of INS.
Gyro drift (deg/h)
1
Acceleration zero bias (g)
10
-
3By using the scheme of integrated navigation reliability-based Kalman filter (RKF) described in Section3.1, we randomly choose an image matching result for our experiment (Figure 4(j) is used in the test).Figures6(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.Latitudinal error (a) and longitudinal error (b) of integral navigation; RKF was compared with AKF and CKF.
(a)
Latitudinal error
(b)
Longitudinal errorFurthermore, compared with innovation-based AKF [27] (window size m=30), different window experiment results have similar performance and AKF has a slower response since it estimates the measurement noise covariance matrix R by using the previous m observations, while RKF estimates R 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.



---

*Source: 819565-2013-10-02.xml*