Visionaided 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 visionaided 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.
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 [
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 [
Several navigation frameworks aided by global positioning system (GPS) [
The wellknown conventional Kalman filter (CKF) is widely implemented in vision/INS integration systems. CKF is a varianceminimizing estimator which fuses various information sources [
In terms of visionaided 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 measurementnoise model.
A key step in the performance of the Kalman filter is to determine the appropriate measurementnoise covariance matrix
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 featurerich areas and lower in featureinsufficient 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
In application of visionaided navigation, CKF is able to obtain the optimal weighted mean value by combining the vision and INS data of its independent estimates [
CKF is a popular fusion scheme for integrated navigation. Its scheme of onedimensional data fusion can be formulated as follows [
A visionaided 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 visionaided 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
Conventional Kalman filter (left) and reliabilitybased Kalman filter (right).
Onedimensional matching results via a vision sensor can be expressed as follows:
Matching error is divided into two parts: constant error
In visionaided navigation, matching reliability can be computed transcendentally by analyzing the reference image before a flight mission offline. For instance, a featurerich 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 visionaided 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.
In this section, a novel visionaided navigation Kalman filter considering reliability (RKF) has been proposed in Section
With the local level frame of NorthEastDown (NED) as the navigational frame, the dynamics of INS can be expressed as
The measurement noise matrix in CKF can be expressed as
The steps in RKF are as follows.
Position error between INS and image matching
Matching reliability of position for the vehicle at epoch
The possible range of
Compute the mapped observation error according to (
Let
Update the measurement noise matrix as follows:
Execute the CKF with
In this section, the reliabilitybased fusion scheme is proposed with minimumvariance restriction.
Rewrite (
where
A reliabilitybased fusion is given as follows.
The fusion algorithm can be expressed as
Finding the optimal factor with minimumvariance restriction is equivalent to
Then the weight factor can be expressed as
The weight factor is consistent with (
The weight factor in (
On the right side of (
At epoch
As
The variance can be expressed as
The two terms on the right side of (
In Kronecker delta function
To simplify the calculation,
After mapping, let
Let
By adding (
According to (
According to (
From (
Hence, (
A Matlab/Simulink platform is constructed for simulation of visionaided inertial navigation. A 600 × 1600 of 4 m resolution synthetic aperture radar (SAR) image is used as the reference. The red lines in Figures
Flight trajectory.
Flight trajectory in real world
Flight trajectory in image
Figure
Generation of matching reliability.
10 image matching errors, different figure represents image matching errors by using different fully observed image.
1st image registration position error
2nd image registration position error
3rd image registration position error
4th image registration position error
5th image registration position error
6th image registration position error
7th image registration position error
8th image registration position error
9th image registration position error
10th image registration position error
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,
Matching reliability versus time, it represents the matching reliability of point at the position for vehicle at epoch
Figure
Tables
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,0 
Specification of INS.
Gyro drift (deg/h)  1 
Acceleration zero bias (g) 

By using the scheme of integrated navigation reliabilitybased Kalman filter (RKF) described in Section
Figures
Latitudinal error (a) and longitudinal error (b) of integral navigation; RKF was compared with AKF and CKF.
Latitudinal error
Longitudinal error
Furthermore, compared with innovationbased AKF [
A reliabilitybased fusion scheme was proposed and developed for the visionaided inertial navigation systems. Different navigation sources are treated equally in the CKF scheme. As for visionaided navigation, missmatching 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.
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.