Mathematical Problems in Engineering
Volume 2019 (2019), Article ID 8532601, 13 pages
https://doi.org/10.1155/2019/8532601

Research Article

A Quaternion-Based Robust Adaptive Spherical Simplex Unscented Particle Filter for MINS/VNS/GNS Integrated Navigation System

Ke Jia,¹ Yifei Pei,¹ Zhaohui Gao,¹ Yongmin Zhong,² Shesheng Gao,¹ Wenhui Wei,¹ and Gaoge Hu¹

¹School of Automation, Northwestern Polytechnical University, Xi’an 710129, China
²School of Engineering, RMIT University, Bundoora, Melbourne 3083, Australia

Correspondence should be addressed to Shesheng Gao; gshshnpu@163.com

Received 26 February 2019; Revised 16 April 2019; Accepted 28 April 2019; Published 20 May 2019

Academic Editor: Saeed Eftekhar Azam

Copyright © 2019 Ke Jia et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract.

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.

1. Introduction

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 [1–5]. As the most common sensor, the inertial measurement unit (IMU, such as accelerometers and gyroscopes) has the advantage of high frame rate, relatively small latency, and a high degree of autonomy. Moreover, the microinertial navigation system (MINS) based on microelectromechanical system (MEMS) is superior in cost, weight, size, and power to the traditional INS [6]. However, the MINS does not guarantee robustness and accuracy of the outputted navigation information and its system errors accumulate with time. As a natural consequence, multiple external sensors such as visual sensor and magnetometer are commonly used to aid INS in order to improve the overall performance and reliability of the navigation system [7–10].

The concept of vision-based navigation has been an active area of research over the last decade [7, 8, 11, 12]. Visual localization based on image matching and attitude estimation can greatly contribute to overcoming those disadvantages of MINS and has been paid much attention in recent years [11, 13–17]. The visual navigation system (VNS) aids the MINS by providing position and attitude measurements based on visual features tracked over a series of images [8]. Among various methods of estimating the state (position and attitude) of the vehicle through matching visual features from frame to frame, the scale-invariant feature transform (SIFT) is proposed to accurately match the invariant features [18]. The integration of visual and inertial sensors leads to a better estimation of positon, velocity, and attitude for a moving vehicle. Moreover, the camera used in visual navigation system has the advantages of small size, light weight, and low power consumption. These benefits of VNS perfectly match the advantages of MINS. Therefore, the fusion of measurements from MINS and VNS complements the shortage of single navigation system, thus acquiring reliable and precise position and attitude of the dynamic vehicle.

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 [9, 19, 20]. The magnetometer measurements compensate the horizontal position and heading angle of MINS so as to correct the accumulated system errors of MINS. Thus, the MINS/GNS integrated navigation system, fusing the measurements of MINS and GNS, can improve the navigation accuracy and reliability.

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 [2, 21–23]. As a key navigation system, INS with high data rate is self-contained and does not rely on any external information. It is also inherently stealthy and is immune to jamming. However, the INS suffers from integration drift and the system errors accumulate significantly with time. To complement INS, several kinds of autonomous sensors (whose measurement information is inherent in nature and it is not subject to external interference), such as visual camera, magnetometer, infrared sensor, and laser radar, are applied to correct the accumulated system error. It is natural to consider an integrated navigation system consisting of MINS, VNS, and GNS. In the near Earth space, the visual camera and magnetometer are widely used because of their high precision and robustness. Related research on the vision-aided and geomagnetism aided INS is developed to restrict the error accumulation of the INS [7, 9, 24]. Thus, the properties of MINS, VNS, and GNS are complementary and their integration can significantly improve the accuracy and robustness of this completely autonomous navigation system. However, the combination of MINS, VNS, and GNS has not been explored thoroughly and there is very limited research focusing on MINS/VNS/GNS integrated navigation system.

In the MINS/VNS/GNS integrated navigation system (as shown in Figure 1), MINS is the reference system, and VNS and GNS provide aiding sources to bound the MINS errors. MINS, VNS, and GNS are all completely autonomous and passive navigation systems, providing several navigation parameters which are independent of external information or facilities. By combining these three autonomous navigation systems, the MINS/VNS/GNS integrated navigation system has advantages of high precision, strong robustness, and good anti-interference capability.

Figure 1: 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 [22, 25, 26]. Based on the integral principle of inertial navigation, the inertial error accumulatively grows with time and significantly affects the accuracy and robustness of integrated navigation system. Besides, a linear INS error model is commonly used for integrated navigation system, and it is hard to precisely describe the nonlinear characteristics of the integrated navigation system [27–29]. Moreover, the fusion of multiple sensors to acquire timely and accurately navigation information becomes much difficult in nonlinear situation, and the accuracy and robustness of navigation information deeply rely on the filtering algorithm and fusing algorithm [21, 25]. So, it is necessary to apply a nonlinear filter in the MINS/VNS/GNS integrated navigation system to deal with the nonlinear problems. Besides, abnormal observations may greatly influence the robustness and accuracy of the integrated navigation system [30]. The highly dynamic changes in state model may also fail the integrated navigation system [31]. By robustly estimating the covariance matrix of observation noise and adaptively adjusting the covariance matrix of the state noise based on the predicted residuals, the robust adaptive filter can be used to overcome above problems [32].

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 2 introduces related research on filter algorithms dealing with estimation of navigation state parameters. In Section 3, the mathematical models of MINS/VNS/GNS integrated navigation system are deployed thoroughly. The robust adaptive spherical simplex unscented particle filter algorithm is proposed step by step in Section 4. Simulation results and discussion are expressed in Section 5 and main conclusion is finally drawn in Section 6.

2. Related Work

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 [33–35]. In this situation, the particle filter is a better choice to handle the non-Gaussian noise problem in the nonlinear state and observation models [36].

The PF is an optimal recursive Bayesian filtering algorithm based on Monte Carlo simulation [35, 37, 38] and has been widely used in navigation, target tracking, fault detection, mobile robotic control, computer vision, microelectronic mechanical system, and econometrics [36, 39–42]. But the particle degeneration and high computation burden limit the application of particle filter, and the accuracy of the PF method relies on the selected importance density function and resampling scheme [43, 44]. Therefore, construction of an appropriate importance density function and resampling scheme has been studied with various kinds of methods [45–48]. By selecting the appropriate importance density function and resampling, the particle degeneracy can be eliminated and the accuracy of the PF can be increased. Besides the particle degeneracy problem, large computation cost in PF is another important issue limiting the application of PF, because PF requires a large number of sample points for reliable estimation. By applying unscented transformation, the UPF method leads to a better importance sampling density using 2n+1 sampling points [27, 46, 49, 50]. Adopting the asymmetric sampling strategy, the spherical simplex unscented transformation (SSUT) proposed by Julier [51] needs only n+2 sigma points to fully describe the mean and covariance, thus lowering the computer cost in PF. Therefore, the merits of SSUT and PF can be combined to construct the SSUPF for achieving the navigation information with high accuracy and low computation cost.

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 [27, 39, 46]. To deal with this problem, the robust adaptive filtering algorithm is proposed by robustly estimating the covariance matrix of observation noise and adaptively adjusting the covariance matrix of the state noise based on the predicted residuals [25, 32]. A robust adaptive filter, proposed by Yang and Gao [52], combines the robust maximum-likelihood estimation with the adaptive filtering process to adaptively adjust the weight matrix of predicted parameters according to the difference between system observation and model information [53, 54]. By modifying the weight matrix and adaptive factor, this filter can be adaptively converted into the classical Kalman filter, adaptive Kalman filter, and Sage filter. However, the filtering algorithm difficultly makes the optimal estimation of the parameters of the state in the case of insufficient observation information. A robust adaptive filter with multiple adaptive factors can improve the robustness of system, but it also causes an extra computational load [32]. Gao and Zhong [25] applied the robust adaptive filter to integrated navigation system and effectively resisted disturbances due to system state noise and observation noise, but this method may not guarantee that the selection of robust adaptive factors based on prediction residuals was optimal, thus failing to achieve the optimal filtering effect.

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.

3. Mathematical Models of MINS/VNS/GNS Integrated Navigation System

3.1. MINS/VNS/GNS Integrated Navigation Methodology

The MINS/VNS/GNS integrated navigation strategy is proposed incorporating microinertial sensor, visual camera, and magnetometer, as shown in Figure 2. In this integrated navigation system, MINS is the reference system that provides position, velocity, and attitude information. The VNS provides position and attitude information and GNS provide heading angle and horizontal position to bound the MINS errors. This system comprises two local filters (LF) and one master filter (MF). The LF-A fuses the measurements of MINS and VNS and the LF-B is responsible for the fusion of MINS and GNS. Then, the MF fuses the two local estimates to get the global optimal estimates and feed back the global estimates to LFs according to the information-sharing method. Furthermore, the global optimal estimates are fed back to correct the MINS’s cumulated errors caused by the integrative process, and the corrected outputs of MINS are regarded as the output navigation information of the integrated navigation system.

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

3.2. Inertial Navigation and System State Model

For simplicity, the MINS algorithms adopted here are skipped, and the details are described by Farrell et al. [55] and related references therein. Using the E-N-U geography coordinate system as the base coordinate system for navigation, the quaternion-based MINS error state vector can be described as

where denotes their errors. represents the quaternion of attitude angle error, is the velocity error, is the position error, is the gyro constant drift, and is the accelerometer zero bias.

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

where is the dynamic matrix of the state transition, is the noise coefficient matrix, and is the system noise.

The dynamic matrix of the state transition can be described as the following formula, the formulas in this section are quoted from Zhong et. al. [29].

where , , and represent transformation matrix from navigation frame to platform frame, body frame to platform frame, and platform frame to navigation frame by quaternions, respectively. represent coefficient matrix of velocity and position errors.

where and are the main curvature radii of the prime meridian and the equator.

The noise coefficient matrix is

where represents the transfer matrix from the carrier coordinate system to the computational coordinate system.

The system noise is defined as

where stand for the white noise caused by the gyro’s constant drift; stand for the white noise caused by the accelerometer’s zero deviation.

3.3. Observation Model of MINS/VNS Integrated Navigation System

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

where denote the measurements of pitch angle, roll angle, heading angle, latitude, longitude, and altitude from MINS, respectively, and denote the measurements of pitch angle, roll angle, heading angle, latitude, longitude, and altitude from VNS, respectively. The transformation between quaternion error and attitude angle error is

From formulas (1), (9), and (10), the observation model of VNS/MINS integrated navigation system can be described as

where is a nonlinear function, and the observational white noise with the mean value equals zero and its variance density is .

3.4. Observation Model of MINS/GNS Integrated Navigation System

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

where is the system observation, is the heading angle, latitude, longitude, and altitude of MINS, is the heading angle, latitude, and longitude of GNS, and is the altitude measurement by the barometric altimeter. The relationship between and the quaternion is expressed as

By combining formulas (12) and (13), the nonlinear observation equation of the MINS/GNS integrated navigation system can be described as

where is a nonlinear function, and the observational white noise with the mean value equals zero and its variance density is .

From formulas (11) and (14), the system measurement equation of MINS/VNS/GNS integration can be described as

where

Thus, (2) and (15) provide the mathematical model for MINS/VNS/GNS integrated navigation.

4. Local Filter and Global Filter Algorithm

4.1. Robust Adaptive Spherical Simplex Unscented Particle Filter Algorithm

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.

Step 1. Initialization
Draw sampling points according to . Assume the initial weights are [28]. Let

where represents the mathematical expected value of the matrix.

Step 2. For ,
(a) calculate the equivalent weight and the adaptive factor by following steps.
Construct equivalent weight function by IGG scheme [40] and equivalent weight matrix , where

Alternatively, another expression could be used for different situation.

where and . is the residual vector of , and is the state parameter estimation value at the current epoch.
The adaptive factor is constructed as

where and . , in which represents the norm of the matrix, represents the matrix trace, and is the state prediction.
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 sigma points with the SSUT method.
Update particles using the UKF algorithm to obtain , where satisfies . Taking as the new samples, the sigma points can be shown as

With the weight of the mean value and the weight of the covariance

where represents the mean, represents the covariance, is the factor determining the extent for the distribution of samples with respect to the predicted state mean, and is used to incorporate prior knowledge on the distribution of . In general, , and is optimal for Gaussian distributions [51]. represents the ith sigma point of the j-dimension state vector, which could be shown as

(c) Predict and update particles by UKF algorithm.

Time Update

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

where the adaptive factor is determined by formula (20) and the gain is calculated by

where , and represents the equivalent weight matrix in formulas (18) and (19).

From formula (33), it is obvious that could be adjusted by factor and to make the important density function more close to the practical distribution. Thus, the importance density function of sampling particles from formulas (32)-(34) can be obtained for the importance resampling. When the observation model contains abnormal information, is reduced accordingly. As a result, the use of observation information for state estimation is reduced to resist the disturbances of singular observation. Similarly, the value of the adaptive factor can be decreased when the kinematic model contains noise. Consequently, the use of state prediction information for state estimation is reduced to overcome the difficulty of handling the noise problem in kinematic model.

Step 3. Calculate the weights.

where represents the recommended probability density of generated particles.
And normalize them as

Step 4. Determine the degeneracy of particles by calculating the following formula:

Thus, the degeneracy of particles can be determined by comparing with the given threshold. A smaller represents a worse degeneracy of particles. To overcome the severe degeneracy of particles, the resampling process could be done by getting new particles from the obtained posterior density and assigning a common weight to each new particle.

Step 5. Calculate the estimation of the state and variance.

And go back to Step 2 to continue.

4.2. Federated Filter Algorithm

In the federated filter model described by the system state equation and two LF measurement equations (11) and (14), the corresponding federal filter algorithm in the full fusion mode can be described as follows.

Step 1. Feed back the global solution to each LF:

where and are the ith LF and the global covariance matrix of the state estimation error, respectively. And is information-sharing factor for ith LF, satisfying

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

Step 3. Obtain the global estimation by the following fusion algorithm:

And go back to Step 1 to continue.

5. Theoretical Convergence

Let denote the Borel -algebra on , and we denote . As described by Van Der Merwe and Doucet [56], the following theorem has been proven.

Theorem 1. Given the system equation defined by (2), if the importance weight

is upper bounded for any , there exists a constant , which is independent of for all , such that for any

The left part in (44) is with respect to the randomness introduced by the particle filtering algorithm. From (44), it is shown that the convergence of the unscented particle filter is ensured under very lose assumptions and the convergence rate is independent of the dimension of the state-space. Note that the only assumption is that is upper bounded, which means the distribution has heavier tails than . Thus, it is proven that the proposed algorithm is theoretically converged.

6. Performance Evaluation and Discussion

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.

6.1. Navigation Accuracy Evaluation

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 3. The flight involves various maneuvers such as climbing, pitching, rolling, and turning. The simulation parameters are shown in Table 1. The total simulation time is 1200s.

Table 1: 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
parameters
Attitude error
0.5′

Position error
 3m

Sampling frequency
1Hz

Figure 3: 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 4–9 illustrate the aircraft attitude errors and position error obtained by PF, UPF, and RASSUPF, respectively.

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

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

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

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

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

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

As shown in Figures 4–6, during the time period from 800s to 900s, the head error, pitch error, and roll error of the PF are within (-0.5779′, 1.7777′), (-1.4265′, 1.7820′), and (-0.9764′, 0.4807′). These are larger than the attitude errors of the UPF, which are within (-1.2163′, 0.7647′), (-0.3619′, 1.4631′), and (-0.1769′, 0.7567′), respectively. Apparently, thanks to the robustness against process model error, the proposed RASSUPF obtains the best estimation results. The yaw error, pitch error, and roll error obtained by the proposed method are within (-0.0716′, 0.5479′), (-0.0638′, 0.6680′), and (-0.3341′, -0.1257′), respectively.

Figures 7 and 8 illustrate the position errors obtained by the three mentioned methods. During the time period from 800s to 900s, the position errors of the PF at longitude, latitude, and altitude are within (-11.2796m, 13.8470m), (-18.5440m, 14.0293m), and (-18.7477m, 20.0117m), respectively. Meanwhile, the position errors of the UPF at longitude, latitude, and altitude are within (-8.1030m, 8.6905m), (-11.2466m, 12.5256m), and (-11.6293m, 5.8740m), respectively. Nevertheless the position errors of the proposed RASSUPF are much smaller than those of the both methods mentioned above, which are within (-2.6185m, 3.2206m), (-4.3048m, 3.2568m), and (-4.3521m, 4.6556m).

Table 2 shows the mean absolute errors (MAEs) and root mean square error (RMSE) of the attitude errors and position errors obtained by the three methods during the time period from 800s to 900s. It can be seen that the MAE and RMSE of attitude errors and position errors obtained by the proposed RASSUPF are also much smaller than those of both the other methods.

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

6.2. Computational Performance Evaluation

Trials were conducted to investigate the real-time performance of the proposed RASSUPF. The simulations conducted in Section 6.1 were repeated 10 times under same conditions and the average time for each filtering consumption of the PF, UPF, and proposed RASSUPF was recorded.

Denote the average filtering times (i.e., the execution times for each run) of the PF, UPF, and proposed RASSUPF, as , , and . In order to eliminate the influence resulting from computer differences, a set of interrelated execution times is defined as

Apparently, .The interrelated execution times of the three methods are shown in Figure 10.

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

From Figure 10, it can be seen that the PF has the shortest execution time for the navigation solution. The PF’s execution time is 74.13% of the UPF since the particles are selected directly by the probability density in the process of particle selection. However, in the UPF, the UKF process is used for the initialization of probability density and a vast number of calculations are involved in the unscented transformation and the matrix solving process. Compared with the UPF, the proposed RASSUPF also shows its superiority in terms of the execution time. It can be observed that the proposed method saves about 13.45% computation time of the UPF. This is because the proposed method improves the unscented transformation process while inheriting the advantage of the UKF process for the particle selection. The above evaluation on execution time demonstrates that the proposed RASSUPF can effectively eliminate the drawback of computational complexity in the UPF, making the new method more suitable for real-time applications.

7. Conclusions

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.

Data Availability

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

Conflicts of Interest

The authors declare that they have no conflicts of interest.

Acknowledgments

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

References

M. Bertozzi, A. Broggi, and A. Fascioli, “Vision-based intelligent vehicles: state of the art and perspectives,” Robotics and Autonomous Systems, vol. 32, no. 1, pp. 1–16, 2000.

C. Melendez-Pastor, R. Ruiz-Gonzalez, and J. Gomez-Gil, “A data fusion system of GNSS data and on-vehicle sensors data for improving car positioning precision in urban environments,” Expert Systems with Applications, vol. 80, pp. 28–38, 2017.

S. Saripalli, J. F. Montgomery, and G. S. Sukhatme, “Visually guided landing of an unmanned aerial vehicle,” IEEE Transactions on Robotics and Automation, vol. 19, no. 3, pp. 371–380, 2003.

R. Vepa, “Joint position localisation of spacecraft and debris for autonomous navigation applications using angle measurements only,” Journal of Navigation, vol. 70, no. 4, pp. 748–760, 2017.

G. Hu, W. Wei, Y. Zhong, B. Gao, and C. Gu, “A new direct filtering approach to INS/GNSS integration,” Aerospace Science & Technology, vol. 77, Article ID S1270963817316498, 2018.

C. Coopmans, Architecture, Inertial Navigation, and Payload Designs for Low-Cost Unmanned Aerial Vehicle-Based Personal Remote Sensing [Dissertations & Theses], Gradworks, 2010.

M. K. Kaiser, N. R. Gans, and W. E. Dixon, “Vision-based estimation for guidance, navigation, and control of an aerial vehicle,” IEEE Transactions on Aerospace and Electronic Systems, vol. 46, no. 3, pp. 1064–1077, 2010.

C. Ivancsits and M.-F. R. Lee, “Visual navigation system for small unmanned aerial vehicles,” Sensor Review, vol. 33, no. 3, pp. 267–291, 2013.

Y. Liu, M. Wu, X. Hu, and H. Xie, “Geomagnetism aided inertial navigation system,” in Proceedings of the 2008 2nd International Symposium on Systems and Control in Aerospace and Astronautics, ISSCAA 2008, China, December 2008.

X. Ma, H. Liu, D. Xiao, and H. Li, “Key technologies of geomagnetic aided inertial navigation system,” in Proceedings of the 2009 IEEE Intelligent Vehicles Symposium, pp. 464–469, China, June 2009.

M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,” International Journal of Robotics Research, vol. 32, no. 6, pp. 690–711, 2013.

M. Li and A. I. Mourikis, “Vision-aided inertial navigation with rolling-shutter cameras,” International Journal of Robotics Research, vol. 33, no. 11, pp. 1490–1507, 2014.

E. S. Jones and S. Soatto, “Visual-inertial navigation, mapping and localization: A scalable real-time causal approach,” International Journal of Robotics Research, vol. 30, no. 4, pp. 407–430, 2011.

J. Kelly and G. S. Sukhatme, “Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor Self-calibration,” International Journal of Robotics Research, vol. 30, no. 1, pp. 56–79, 2011.

D. G. Kottas, J. A. Hesch, S. L. Bowman, and S. I. Roumeliotis, “On the consistency of vision-aided inertial navigation,” in Experimental Robotics, vol. 88, pp. 303–317, Springer International Publishing, 2013.

S. Weiss, M. W. Achtelik, S. Lynen, M. Chli, and R. Siegwart, “Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments,” in Proceedings of the 2012 IEEE International Conference on Robotics and Automation, ICRA 2012, pp. 957–964, USA, May 2012.

Y. Xu and X. Ning, “A new INS/VNS integrated navigation method for planetary exploration rover,” Chinese Journal of Space Science, vol. 35, no. 6, pp. 721–729, 2015.

D. G. Lowe, “Object recognition from local scale-invariant features,” in Proceedings of the 7th IEEE International Conference on Computer Vision, vol. 2, pp. 1150–1157, Kerkyra, Greece, 2002.

C. Tyren, “Magnetic anomalies as a reference for ground-speed and map-matching navigation,” Journal of Navigation, vol. 35, no. 2, pp. 242–254, 1982.

F. Goldenberg, “Geomagnetic navigation beyond the magnetic compass,” in Proceedings of the 2006 IEEE/ION Position, Location, and Navigation Symposium, pp. 684–694, IEEE, New York, NY, USA, 2006.

F. Caron, E. Duflos, D. Pomorski, and P. Vanheeghe, “GPS/IMU data fusion using multisensor Kalman filtering: introduction of contextual aspects,” Information Fusion, vol. 7, no. 2, pp. 221–230, 2006.

S. Gao, Y. Zhong, X. Zhang, and B. Shirinzadeh, “Multi-sensor optimal data fusion for INS/GPS/SAR integrated navigation system,” Aerospace Science and Technology, vol. 13, no. 4-5, pp. 232–237, 2009.

S. Gao, G. Hu, and Y. Zhong, “Windowing and random weighting-based adaptive unscented Kalman filter,” International Journal of Adaptive Control and Signal Processing, vol. 29, no. 2, pp. 201–223, 2015.

Z. Dai and C. Kang, “Geomagnetic field aided inertial navigation using the SITAN algorithm,” in Proceedings of the 2014 2nd International Conference on Systems and Informatics, ICSAI 2014, pp. 79–83, China, November 2014.

S. Gao, Y. Zhong, and W. Li, “Robust adaptive filtering method for SINS/SAR integrated navigation system,” Aerospace Science and Technology, vol. 15, no. 6, pp. 425–430, 2011.

G. Hu, S. Gao, and Y. Zhong, “A derivative UKF for tightly coupled INS/GPS integrated navigation,” ISA Transactions, vol. 56, pp. 135–144, 2015.

J. Ali and J. Fang, “Realization of an autonomous integrated suite of strapdown astro-inertial navigation systems using unscented particle filtering,” Computers & Mathematics with Applications, vol. 57, no. 2, pp. 169–183, 2009.

X. Kong, “INS algorithm using quaternion model for low cost IMU,” Robotics and Autonomous Systems, vol. 46, no. 4, pp. 221–246, 2004.

Y. Zhong, S. Gao, and W. Li, “A quaternion-based method for SINS/SAR integrated navigation system,” IEEE Transactions on Aerospace and Electronic Systems, vol. 48, no. 1, pp. 514–524, 2012.

S. Gao, W. Wei, Y. Zhong, and Z. Feng, “Rapid alignment method based on local observability analysis for strapdown inertial navigation system,” Acta Astronautica, vol. 94, no. 2, pp. 790–798, 2014.

J. Wang, H. Lee, S. Hewitson, and H. Lee, “Influence of dynamics and trajectory on integrated GPS/INS navigation performance,” Journal of Global Positioning Systems, vol. 2, no. 2, pp. 109–116, 2003.

Y. Yang and X. Cui, “Adaptively robust filter with multi adaptive factors,” Survey Review, vol. 40, no. 309, pp. 260–270, 2008.

S. Julier, J. Uhlmann, and H. F. Durrant-Whyte, “A new method for the nonlinear transformation of means and covariances in filters and estimators,” IEEE Transactions on Automatic Control, vol. 45, no. 3, pp. 477–482, 2000.

T. Lefebvre, H. Bruyninckx, and J. de Schutter, “Kalman filters for non-linear systems: a comparison of performance,” International Journal of Control, vol. 77, no. 7, pp. 639–653, 2004.

J. B. Rawlings and B. R. Bakshi, “Particle filtering and moving horizon estimation,” Computers & Chemical Engineering, vol. 30, no. 10-12, pp. 1529–1541, 2006.

F. Gustafsson, F. Gunnarsson, N. Bergman et al., “Particle filters for positioning, navigation, and tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 425–437, 2002.

A. Doucet, S. Godsill, and C. Andrieu, “On sequential Monte Carlo sampling methods for Bayesian filtering,” Statistics and Computing, vol. 10, no. 3, pp. 197–208, 2000.

A. Doucet and A. M. Johansen, “A tutorial on particle filtering and smoothing: fifteen years later,” in Handbook of Nonlinear Filtering, vol. 12, pp. 656–704, Oxford Univ. Press, Oxford, UK, 2009.

G. Oppenheim, A. Philippe, and J. de Rigal, “The particle filters and their applications,” Chemometrics and Intelligent Laboratory Systems, vol. 91, no. 1, pp. 87–93, 2008.

B. Zhang, M. Chen, D. Zhou, and Z. Li, “Particle-filter-based estimation and prediction of chaotic states,” Chaos, Solitons & Fractals, vol. 32, no. 4, pp. 1491–1498, 2007.

H. F. Lopes and R. S. Tsay, “Particle filters and Bayesian inference in financial econometrics,” Journal of Forecasting, vol. 30, no. 1, pp. 168–209, 2011.

R. Mirzazadeh, S. E. Azam, and S. Mariani, “Micromechanical characterization of polysilicon films through on-chip tests,” Sensors, vol. 16, no. 8, article no. 1191, 2016.

M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174–188, 2002.

D. Watzenig, M. Brandner, and G. Steiner, “A particle filter approach for tomographic imaging based on different state-space representations,” Measurement Science and Technology, vol. 18, no. 1, pp. 30–40, 2007.

A. Budhiraja, L. Chen, and C. Lee, “A survey of numerical methods for nonlinear filtering problems,” Physica D: Nonlinear Phenomena, vol. 230, no. 1-2, pp. 27–36, 2007.

X. Ning and J. Fang, “Spacecraft autonomous navigation using unscented particle filter-based celestial/Doppler information fusion,” Measurement Science and Technology, vol. 19, no. 9, Article ID 095203, 2008.

N. Yang, W. Tian, and Z. Jin, “An interacting multiple model particle filter for manoeuvring target location,” Measurement Science and Technology, vol. 17, no. 6, pp. 1307–1311, 2006.

B. Zhang, W. Tian, and Z. Jin, “Head tracking based on the integration of two different particle filters,” Measurement Science and Technology, vol. 17, no. 11, pp. 2877–2883, 2006.

Z. Liang, X. Ma, and X. Dai, “Robust tracking of moving sound source using scaled unscented particle filter,” Applied Acoustics, vol. 69, no. 8, pp. 673–680, 2008.

R. Van Der Merwe, A. Doucet, N. De Freitas, and E. Wan, “The unscented particle filter,” in Proceedings of the 14th Annual Neural Information Processing Systems Conference, NIPS 2000, USA, December 2000.

S. J. Julier, “The spherical simplex unscented transformation,” in Proceedings of the American Control Conference, pp. 2430–2434, June 2003.

Y. Yang and W. Gao, “A new learning statistic for adaptive filter based on predicted residuals,” Progress in Natural Science: Materials International, vol. 16, no. 8, pp. 833–837, 2006.

Y. Yang and W. Gao, “An optimal adaptive Kalman filter,” Journal of Geodesy, vol. 80, no. 4, pp. 177–183, 2006.

Y. Yang, H. He, and G. Xu, “Adaptively robust filtering for kinematic geodetic positioning,” Journal of Geodesy, vol. 75, no. 2-3, pp. 109–116, 2001.

J. A. Farrell, H.-S. Tan, and Y. Yang, “Carrier phase GPS-aided INS-based vehicle lateral control,” Journal of Dynamic Systems, Measurement, and Control, vol. 125, no. 3, pp. 339–353, 2003.

R. Van Der Merwe, A. Doucet, N. De Freitas, and E. Wan, “The unscented particle filter,” in Advances in Neural Information Processing Systems, 2001.