^{1}

^{1}

^{1}

^{2}

^{1}

^{1}

^{1}

^{1}

^{2}

An improved filtering algorithm-robust adaptive spherical simplex unscented particle filter (RASSUPF) is proposed to achieve high accuracy, induce the amount of computation, and resist the influence of abnormal interference for the MINS/VNS/GNS integrated navigation system. This algorithm adopts spherical simplex unscented transformation (SSUT) to approximate the probability distribution, employs the spherical simplex unscented Kalman filter (SSUKF) to generate the importance sampling density of particle filter, and applies robust and adaptive estimation to control the influence of the abnormal information on the state model and the observation model. Simulation results demonstrate the proposed algorithm can effectively reduce the navigation error, improve the navigation positioning precision, and decrease the computation cost.

Reliable and accurate estimation of location, velocity, and attitude of a dynamic vehicle is key to a number of applications, particularly to autonomous navigation of ground and air vehicles. Since the autonomous navigation has been paid growing interesting in recent years, various methods and sensory modalities have been introduced to tackle this problem [

The concept of vision-based navigation has been an active area of research over the last decade [

The geomagnetic navigation system (GNS) is a novel, passive, and completely autonomous navigation system. The magnetometer measures the geometric intensity of local magnetic field to get a real-time magnetic sequence, which is to be correlated with the prestored magnetic map to determine the differences so as to acquire the matching horizontal position and heading angle of the vehicle [

Any kind of single navigation system has its limitations and cannot provide absolutely reliable and accurate navigation information in any case. In this situation, the integrated navigation system based on fusion method becomes an effective way to improve the robustness and accuracy of the navigation system. Thus, integrated navigation based on multisensor has received more attention over last decades [

In the MINS/VNS/GNS integrated navigation system (as shown in Figure

Framework of MINS/VNS/GNS integrated navigation system.

However, developing a reliable and high-accuracy MINS/VNS/GNS integrated navigation system is a challenging task [

In this paper, a quaternion-based robust adaptive spherical simplex unscented particle filter is proposed to fuse the visual, geomagnetic, and inertial sensors of the MINS/VNS/GNS integrated navigation system. The quaternion-based nonlinear error and nonlinear observation models are established to describe the nonlinear characteristics of the MINS/VNS/GNS integrated navigation system. The robust adaptive spherical simplex unscented particle filter is developed to deal with the nonlinear and non-Gaussian state and observation models in order to enhance the accuracy, robustness, and adaptability of the integrated system.

The organization of the paper is constructed as follows. Section

In order to exploit the complementary properties of IMU, visual cameras, and magnetometer, proper filtering algorithms are applied to fuse their measurements. There are a variety of commonly used filtering algorithms, such as Kalman filter (KF) and its derived versions, i.e., extended Kalman filter (EKF) and unscented Kalman filter (UKF), as well as particle filter (PF) and its derived versions, e.g., unscented particle filter (UPF). Kalman filter is suitable for a linear system with Gaussian noise, but it is not appropriate for a nonlinear system. Using EKF or UKF, the state estimation of a nonlinear system with Gaussian noise can be solved, while the non-Gaussian noise problem in system still cannot be handled [

The PF is an optimal recursive Bayesian filtering algorithm based on Monte Carlo simulation [

The discrepancy between the theoretical model and the actual model is another issue in estimating the state parameters for navigation. Due to the disturbances caused by singular observations or uncertain factors, it is normal that the dynamic system contains noise [

Different from the existing studies, the main contribution of this paper is described as follows. (1) This paper presents a MINS/VNS/GNS integrated navigation system consisting of three completely autonomous navigation systems, which complements the disadvantages of each single navigation. (2) Based on spherical simplex unscented transformation and robust adaptive estimation, a quaternion-based RASSUPF is proposed to deal with the nonlinear and non-Gaussian system with a lower computation cost, high independence, precision, and reliability. Simulations and comparison analysis have been conducted to evaluate the performance of the proposed filtering algorithm for the MINS/VNS/GNS integrated navigation system.

The MINS/VNS/GNS integrated navigation strategy is proposed incorporating microinertial sensor, visual camera, and magnetometer, as shown in Figure

MINS/VNS/GNS integrated navigation strategy based on federal filter.

For simplicity, the MINS algorithms adopted here are skipped, and the details are described by Farrell et al. [

Thus, the corresponding continuous-time system state equation can be expressed as

The dynamic matrix of the state transition

The noise coefficient matrix

The system noise

Both VNS and MINS output the attitude and location of a dynamic vehicle, so the observation of MINS/VNS integrated navigation system can be treated as the subtraction in the latitude, longitude, altitude, and the roll, pitch, and heading angle between VNS and MINS. Thus, the observation of the MINS/VNS integrated navigation system can be expressed as

From formulas (

The MINS/GNS integrated navigation system compares the measurements of MINS and GNS to extract and correct their system errors. The GNS only outputs the heading angle and the horizontal location of a dynamic vehicle, and the MINS outputs the attitude, velocity, and location of the vehicle. Because the GNS cannot acquire the altitude, a barometric altimeter is used to get precise altitude to avoid the instability of altitude measurement from MINS. Thus, the observation of MINS/GNS integrated navigation system can be treated as the subtraction in the heading angle and horizontal positions between VNS and MINS and the subtraction in altitude between MINS and the barometric altimeter. Thus, the observation of the MINS/GNS integrated navigation system can be expressed as

By combining formulas (

From formulas (

Thus, (

Because of the fact that the state and observation models of MINS/VNS and MINS/GNS integrated navigation system are strongly nonlinear and non-Gaussian, the new robust adaptive spherical simplex UPF (RASSUPF) algorithm is proposed to overcome the difficulty so as to enhance the navigation accuracy and decrease the computation cost. This detailed RASSUPF algorithm is described in the following steps.

Initialization

Draw sampling points according to

For

(a) calculate the equivalent weight and the adaptive factor by following steps.

Construct equivalent weight function by IGG scheme [

Alternatively, another expression could be used for different situation.

The adaptive factor is constructed as

It could be seen that the construction of equivalent weight matrix and adaptive factor are in a similar form. The equivalent weight matrix is determined from the residual of observation and the adaptive factor is chosen by the difference between the state estimation and state prediction.

(b) Calculate

Update particles

With the weight of the mean value

(c) Predict and update particles by UKF algorithm.

Incorporate the new observation, and the estimations of new state and variance are

From formula (

Calculate the weights.

And normalize them as

Determine the degeneracy of particles by calculating the following formula:

Thus, the degeneracy of particles can be determined by comparing

Calculate the estimation of the state and variance.

In the federated filter model described by the system state equation and two LF measurement equations (

Feed back the global solution

Update each LF by proposed RASSUPF algorithm and get the new estimated measurements.

Obtain the global estimation by the following fusion algorithm:

Let

Given the system equation defined by (

The left part in (

Simulations were conducted to comprehensively evaluate the performance of the proposed RASSUPF for the MINS/VNS/GNS integrated navigation system. For the purpose of comparison analysis, the comparison of the proposed RASSUPF with PF and UPF is also discussed.

Monte Carlo simulation trials were conducted to evaluate the performance of the proposed method for the flight of aircraft. The dynamic flight trajectory, which was designed according to the actual flight of a highly dynamic aircraft, is shown in Figure

Simulation parameters.

North latitude | 34.246° | |

Initial position | East longitude | 108.997° |

Altitude | 20m | |

| ||

North | 200m/s | |

Initial velocity | East | 0m/s |

Up | 0m/s | |

| ||

Yaw | 0 | |

Initial orientation | Pitch | 0 |

Roll | 0 | |

| ||

North latitude | 10m | |

Initial position error | East longitude | 10m |

Altitude | 10m | |

| ||

North | 0.5m/s | |

Initial velocity error | East | 0.5m/s |

Up | 0.5m/s | |

| ||

Yaw | 1′ | |

Initial attitude error | Pitch | 1′ |

Roll | 1′ | |

| ||

Gyro parameters | Constant drift | 0.1°/h |

Random walk coefficient | 0.5°/h | |

| ||

Accelerometer | Zero bias | |

parameters | Random walk coefficient | |

| ||

GNS | Position error | 3m |

parameters | Sampling frequency | 1Hz |

| ||

VNS | Attitude error | 0.5′ |

Position error | 3m | |

Sampling frequency | 1Hz |

The flight trajectory of an aircraft.

Simulation trials were conducted under the same conditions to estimate the attitude error and position errors of the aircraft by using the PF, UPF, and proposed RASSUPF, respectively.

To evaluate the performances of PF, UPF, and RASSUPF under kinematic model error, a non-Gaussian model error obeying uniform distribution is added between 800s and 900s to draw a clear difference of kinematic model error within the simulation test period for the analysis of kinematic model error’s effect on the filtering solution. Figures

The head errors obtained by PF, UPF, and RASSUPF for the aircraft simulation.

The pitch errors obtained by PF, UPF, and RASSUPF for the aircraft simulation.

The roll errors obtained by PF, UPF, and RASSUPF for the aircraft simulation.

The longitude errors obtained by PF, UPF, and RASSUPF for the aircraft simulation.

The latitude errors obtained by PF, UPF, and RASSUPF for the aircraft simulation.

The altitude errors obtained by PF, UPF, and RASSUPF for the aircraft simulation.

As shown in Figures

Figures

Table

Mean and RMSE of the position estimation errors obtained by PF, UPF, and RASSUPF.

Method | Error type | attitude (′) | Position (m) | ||||
---|---|---|---|---|---|---|---|

Head | Pitch | Roll | Longitude | Latitude | Altitude | ||

PF | MAE | 0.6190 | 0.5363 | 0.2950 | 6.0582 | 7.5205 | 6.5240 |

RMSE | 0.8647 | 0.6859 | 0.4478 | 7.5845 | 9.2858 | 8.3523 | |

UPF | MAE | 0.5358 | 0.3600 | 0.1928 | 5.5828 | 5.5414 | 5.5488 |

RMSE | 0.6387 | 0.7500 | 0.4384 | 6.4805 | 7.1587 | 6.5483 | |

RASSUPF | MAE | 0.1167 | 0.1544 | 0.1234 | 1.5832 | 1.5824 | 1.4575 |

RMSE | 0.3085 | 0.3272 | 0.1660 | 2.5825 | 3.5877 | 3.8105 |

The results and analysis from simulations which contains PF, UPF and the proposed RASSUPF demonstrate that the proposed RASSUPF can effectively overcome the shortcoming of a large amount of computation of UPF while improving robust adaptability of classical PF by using the robust adaptive factor and the SSUT method and thus significantly improving the navigation accuracy for MINS/VNS/GNS integration.

Simulation results show that the RASSUPF is feasible for solving the problem of nonlinear and non-Gaussian system. Meanwhile, the RASSUPF obtains more accurate position than the UPF and PF in the MINS/VNS/GNS integrated navigation system described by nonlinear or non-Gaussian error models.

Trials were conducted to investigate the real-time performance of the proposed RASSUPF. The simulations conducted in Section

Denote the average filtering times (i.e., the execution times for each run) of the PF, UPF, and proposed RASSUPF, as

Comparison of the execution times by the PF, UPF, and proposed RASSSUPF.

From Figure

To deal with the nonlinear and non-Gaussian problem in the MINS/VNS/GNS integrated navigation system, we have proposed a robust adaptive spherical simplex unscented particle filter. This algorithm adopts spherical simplex unscented transformation to approximate the probability distribution, employs the spherical simplex unscented Kalman filter to generate the importance sampling density of particle filter, and applies robust and adaptive estimation to control the influence of the abnormal information on the state model and the observation model. The simulation results show that the proposed algorithm performs better than PF and UPF, with higher accuracy, less computation, and more stability in case of abnormal interference.

The simulation data used to support the findings of this study are available from the corresponding author upon request.

The authors declare that they have no conflicts of interest.

This research is supported jointly by the Fundamental Research Funds for the Central Universities under Grant 3102017OQD024 and 3102018ZY027, Aerospace Science and Technology Fund (2017-HT-XG), and the National Natural Science Foundation of China projects (Grant Number 41804048).