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

Authors: Ke Jia; Yifei Pei; Zhaohui Gao; Yongmin Zhong; Shesheng Gao; Wenhui Wei; Gaoge Hu
Journal: Mathematical Problems in Engineering
 (2019)

---

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


---

## Body

## 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 Figure1), 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. Section2 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 Figure2. 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 xt can be described as(1)xt=δqT,δVT,δPT,εT,∇TTwhere δ· denotes their errors. δq=δq0,δq1,δq2,δq3T represents the quaternion of attitude angle error, δV=δVE,δVN,δVUT is the velocity error, δP=δL,δλ,δhT is the position error, ε=εx,εy,εzT is the gyro constant drift, and ∇=∇x,∇y,∇zT is the accelerometer zero bias.Thus, the corresponding continuous-time system state equation can be expressed as(2)x˙t=fxt,t+GtWtwhere fxt,t is the dynamic matrix of the state transition, Gt is the noise coefficient matrix, and Wt is the system noise.The dynamic matrix of the state transitionfxt,t can be described as the following formula, the formulas in this section are quoted from Zhong et. al. [29].(3)fxt,t=BI-Cnpωinn-BCbpεbI-CpnCbpfb-2ωien+ωenn×δVn-2δωien+δωenn×Vn+Cbp∇bMδV+NδP03×103×1where Cnp, Cbp, and Cpn represent transformation matrix from navigation frame to platform frame, body frame to platform frame, and platform frame to navigation frame by quaternions, respectively. M,N represent coefficient matrix of velocity and position errors.(4)M=01RM+h0sec⁡LRN+h00001,N=00-VNRM+h2VEtanLsecLRN+h0-VEsecLRM+h2000,(5)δωien=0-ωiesinLδLωiecosLδLδωenn=-δVNRM+h+VNδhRM+h2δVERN+h-VEδhRN+h2δVEtanLRN+h+VEsec2LδLRN+h-VEtanLδhRN+h2(6)B=12-q1-q2-q3q0-q3q2q3q0-q1-q2q1q0where RM and RN are the main curvature radii of the prime meridian and the equator.The noise coefficient matrixGt is(7)Gt=Cbc03×303×3Cbc03×303×303×303×303×303×3where Cbc represents the transfer matrix from the carrier coordinate system to the computational coordinate system.The system noiseWt is defined as(8)Wt=wεx,wεy,wεz,w∇x,w∇y,w∇zTwhere wεx,wεy,wεz stand for the white noise caused by the gyro’s constant drift; w∇x,w∇y,w∇z 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(9)Z1t=θI-θVγI-γVψI-ψVLI-LVλI-λVhI-hV=θ+δθI-θ+δθVγ+δγI-γ+δγVψ+δψI-ψ+δψVL+δLI-L+δLVλ+δλI-λ+δλVh+δhI-h+δhV=δθIδγIδψIδLIδλIδhI-δθVδγVδψVδLVδλVδhVwhere θI,γI,ψI,LI,λI,hI denote the measurements of pitch angle, roll angle, heading angle, latitude, longitude, and altitude from MINS, respectively, and θV,γV,ψV,LV,λV,hV 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(10)δψI=arctan2δq1δq2-δq0δq3δq02-δq12+δq22-δq32δθI=arcsin2q2q3+q0q1δγI=arctan2q0q2-q1q3q02-q12+q22-q32From formulas (1), (9), and (10), the observation model of VNS/MINS integrated navigation system can be described as(11)Z1t=h1xt,t+V1twhere h1xt,t is a nonlinear function, and the observational white noise V1t with the mean value equals zero and its variance density is R1.


### 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(12)Z2t=ψI-ψGLI-LGλI-λGhI-he=ψ+δψI-ψ+δψGLI+δLI-L+δLGλ+δλI-λ+δλGh+δhI-h+δhe=δψIδLIδλIδhI-δψGδLGδλGδhewhere Z2t is the system observation, ψI,LI,λI,hI is the heading angle, latitude, longitude, and altitude of MINS, ψG,LG,λG is the heading angle, latitude, and longitude of GNS, and he is the altitude measurement by the barometric altimeter. The relationship between δψI and the quaternion is expressed as(13)δψI=arctan2q1q2-q0q3q02-q12+q22-q32By combining formulas (12) and (13), the nonlinear observation equation of the MINS/GNS integrated navigation system can be described as(14)Z2t=h2xt,t+V2twhere h2xt,t is a nonlinear function, and the observational white noise V2t with the mean value equals zero and its variance density is R2.From formulas (11) and (14), the system measurement equation of MINS/VNS/GNS integration can be described as(15)Zt=Z1tZ2t=hxt,t+Vtwhere(16)hxt,t=h1xt,th2xt,tVt=V1tV2tT=V1θV1γV1ψV1LV1λV1hV2ψV2LV2λV2hTThus, (2) and (15) provide the mathematical model for MINS/VNS/GNS integrated navigation.


## 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 Figure2. 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 xt can be described as(1)xt=δqT,δVT,δPT,εT,∇TTwhere δ· denotes their errors. δq=δq0,δq1,δq2,δq3T represents the quaternion of attitude angle error, δV=δVE,δVN,δVUT is the velocity error, δP=δL,δλ,δhT is the position error, ε=εx,εy,εzT is the gyro constant drift, and ∇=∇x,∇y,∇zT is the accelerometer zero bias.Thus, the corresponding continuous-time system state equation can be expressed as(2)x˙t=fxt,t+GtWtwhere fxt,t is the dynamic matrix of the state transition, Gt is the noise coefficient matrix, and Wt is the system noise.The dynamic matrix of the state transitionfxt,t can be described as the following formula, the formulas in this section are quoted from Zhong et. al. [29].(3)fxt,t=BI-Cnpωinn-BCbpεbI-CpnCbpfb-2ωien+ωenn×δVn-2δωien+δωenn×Vn+Cbp∇bMδV+NδP03×103×1where Cnp, Cbp, and Cpn represent transformation matrix from navigation frame to platform frame, body frame to platform frame, and platform frame to navigation frame by quaternions, respectively. M,N represent coefficient matrix of velocity and position errors.(4)M=01RM+h0sec⁡LRN+h00001,N=00-VNRM+h2VEtanLsecLRN+h0-VEsecLRM+h2000,(5)δωien=0-ωiesinLδLωiecosLδLδωenn=-δVNRM+h+VNδhRM+h2δVERN+h-VEδhRN+h2δVEtanLRN+h+VEsec2LδLRN+h-VEtanLδhRN+h2(6)B=12-q1-q2-q3q0-q3q2q3q0-q1-q2q1q0where RM and RN are the main curvature radii of the prime meridian and the equator.The noise coefficient matrixGt is(7)Gt=Cbc03×303×3Cbc03×303×303×303×303×303×3where Cbc represents the transfer matrix from the carrier coordinate system to the computational coordinate system.The system noiseWt is defined as(8)Wt=wεx,wεy,wεz,w∇x,w∇y,w∇zTwhere wεx,wεy,wεz stand for the white noise caused by the gyro’s constant drift; w∇x,w∇y,w∇z 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(9)Z1t=θI-θVγI-γVψI-ψVLI-LVλI-λVhI-hV=θ+δθI-θ+δθVγ+δγI-γ+δγVψ+δψI-ψ+δψVL+δLI-L+δLVλ+δλI-λ+δλVh+δhI-h+δhV=δθIδγIδψIδLIδλIδhI-δθVδγVδψVδLVδλVδhVwhere θI,γI,ψI,LI,λI,hI denote the measurements of pitch angle, roll angle, heading angle, latitude, longitude, and altitude from MINS, respectively, and θV,γV,ψV,LV,λV,hV 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(10)δψI=arctan2δq1δq2-δq0δq3δq02-δq12+δq22-δq32δθI=arcsin2q2q3+q0q1δγI=arctan2q0q2-q1q3q02-q12+q22-q32From formulas (1), (9), and (10), the observation model of VNS/MINS integrated navigation system can be described as(11)Z1t=h1xt,t+V1twhere h1xt,t is a nonlinear function, and the observational white noise V1t with the mean value equals zero and its variance density is R1.


## 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(12)Z2t=ψI-ψGLI-LGλI-λGhI-he=ψ+δψI-ψ+δψGLI+δLI-L+δLGλ+δλI-λ+δλGh+δhI-h+δhe=δψIδLIδλIδhI-δψGδLGδλGδhewhere Z2t is the system observation, ψI,LI,λI,hI is the heading angle, latitude, longitude, and altitude of MINS, ψG,LG,λG is the heading angle, latitude, and longitude of GNS, and he is the altitude measurement by the barometric altimeter. The relationship between δψI and the quaternion is expressed as(13)δψI=arctan2q1q2-q0q3q02-q12+q22-q32By combining formulas (12) and (13), the nonlinear observation equation of the MINS/GNS integrated navigation system can be described as(14)Z2t=h2xt,t+V2twhere h2xt,t is a nonlinear function, and the observational white noise V2t with the mean value equals zero and its variance density is R2.From formulas (11) and (14), the system measurement equation of MINS/VNS/GNS integration can be described as(15)Zt=Z1tZ2t=hxt,t+Vtwhere(16)hxt,t=h1xt,th2xt,tVt=V1tV2tT=V1θV1γV1ψV1LV1λV1hV2ψV2LV2λV2hTThus, (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 tox0i~px0,i=1,2,…,N. Assume the initial weights are W0i=1/N(i=1,2,…,N) [28]. Let(17)x^0i=Ex0iP0=Ex0i-x^0ix0i-x^0iTwhere E· represents the mathematical expected value of the matrix.Step 2.
Fork=1,2,…,N,
(a) calculate the equivalent weight and the adaptive factor by following steps.
Construct equivalent weight function by IGG scheme [40] and equivalent weight matrix P-=diagP-1,P-2,…,P-k, where(18)P-k=pkVk≤k0pkk0Vkk0<Vk≤k10Vk>k1
Alternatively, another expression could be used for different situation.(19)P-k=pkVk≤k0pkk0Vkk1-Vk2k1-k02k0<Vk<k10Vk≥k1where k0∈1,1.5 and k1∈3,8. Vk=Akx^k-yk is the residual vector of yk, and x^k is the state parameter estimation value at the current epoch.
The adaptive factor is constructed as(20)αk=1Δx~k≤c0c0Δx~kc1-Δx~k2c1-c02c0≤Δx~k<c10c1≤Δx~kwhere c0∈1,1.5 and c1∈3,8. Δx~k=x^k-x-k/tr∑x-k, in which · represents the norm of the matrix, tr· represents the matrix trace, and x-k 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) Calculaten+2 sigma points with the SSUT method.
Update particlesxk-1i,pk-1i using the UKF algorithm to obtain x-ki,pki, where x-ki satisfies qxki∣xk-1i,yk=Nx-ki,pki. Taking x-ki as the new samples, the sigma points can be shown as(21)χk-1i=x^k-1i+Pk-1ieini=0,1,…,n+1
With the weight of the mean valueWim and the weight of the covariance Wic(22)Wim=W0mi=01-W0mn+1i≠0(23)Wic=W0m+1+β-α2i=01-W0mn+1i≠0where x^k-1 represents the mean, Pk-1 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 x. In general, 10-4≤α≤1, and β=2 is optimal for Gaussian distributions [51]. ein represents theith sigma point of the j-dimension state vector, which could be shown as(24)eij=e0j-10i=0eij-1-1jj+1W1i=1,…,j0j-1jjj+1W1i=j+1
(c) Predict and update particles by UKF algorithm.Time Update (25) χ k ∣ k - 1 i = f χ k - 1 i (26) x - k ∣ k - 1 i = ∑ j = 0 n + 1 W j m χ j , k ∣ k - 1 i (27) Y k ∣ k - 1 i = h χ k ∣ k - 1 i (28) y - k ∣ k - 1 i = ∑ j = 0 n + 1 W j m Y j , k ∣ k - 1 i (29) P k ∣ k - 1 = P x k x k = ∑ j = 0 n + 1 W j c χ j , k ∣ k - 1 i - x - k ∣ k - 1 i · χ j , k ∣ k - 1 i - x - k ∣ k - 1 i T + R k (30) P y k y k = ∑ j = 0 n + 1 W j c Y j , k ∣ k - 1 i - y - k ∣ k - 1 i · Y j , k ∣ k - 1 i - y - k ∣ k - 1 i T + Q k (31) P x k y k = ∑ j = 0 n + 1 W j c χ j , k ∣ k - 1 i - x - k ∣ k - 1 i · Y j , k ∣ k - 1 i - y - k ∣ k - 1 i TIncorporate the new observation, and the estimations of new state and variance are(32)x-ki=x-k∣k-1i+Kkyk-y-k∣k-1i(33)Pki=αkiPk∣k-1i-KkP-ykykKkTwhere the adaptive factor αki is determined by formula (20) and the gain Kk is calculated by(34)Kk=PxyykP-ykyk-1where P-ykyk=P-k-1, and P-k represents the equivalent weight matrix in formulas (18) and (19).From formula (33), it is obvious that Pki could be adjusted by factor αki and P-ykykto make the important density function more close to the practical distribution. Thus, the importance density function of sampling particles Nx-ki,Pki from formulas (32)-(34) can be obtained for the importance resampling. When the observation model contains abnormal information, P-ykyk 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.(35)wki=wk-1ipyk∣x-kipx-ki∣x-k-1iqx-ki∣x-k-1i,ykwhere qx-ki∣x-k-1i,yk represents the recommended probability density of generated particles.
And normalize them as(36)w~ki=wki∑i=1nwkiStep 4.
Determine the degeneracy of particles by calculating the following formula:(37)N^eff=1∑i=1nw~ki2
Thus, the degeneracy of particles can be determined by comparingN^eff with the given threshold. A smaller N^eff represents a worse degeneracy of particles. To overcome the severe degeneracy of particles, the resampling process could be done by getting M new particles from the obtained posterior density and assigning a common weight 1/M to each new particle.Step 5.
Calculate the estimation of the state and variance.(38)x~k=∑i=1nw~kix-ki(39)Pk=∑i=1nwkix-ki-x^kx-ki-x^kTAnd 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 solutionx^kf,Pkf to each LF:(40)x^ki=x^kfPki-1=βiPkf-1where Pki and Pkf are theith LF and the global covariance matrix of the state estimation error, respectively. And βii=1,2 is information-sharing factor forith LF, satisfying(41)∑i=12βi=1,0<βi<1i=1,2Step 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:(42)x^k+1f=P^k+1f∑i=12Pk+1i-1x^k+1iPk+1f-1=∑i=12Pk+1i-1And go back to Step 1 to continue.


## 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 tox0i~px0,i=1,2,…,N. Assume the initial weights are W0i=1/N(i=1,2,…,N) [28]. Let(17)x^0i=Ex0iP0=Ex0i-x^0ix0i-x^0iTwhere E· represents the mathematical expected value of the matrix.Step 2.
Fork=1,2,…,N,
(a) calculate the equivalent weight and the adaptive factor by following steps.
Construct equivalent weight function by IGG scheme [40] and equivalent weight matrix P-=diagP-1,P-2,…,P-k, where(18)P-k=pkVk≤k0pkk0Vkk0<Vk≤k10Vk>k1
Alternatively, another expression could be used for different situation.(19)P-k=pkVk≤k0pkk0Vkk1-Vk2k1-k02k0<Vk<k10Vk≥k1where k0∈1,1.5 and k1∈3,8. Vk=Akx^k-yk is the residual vector of yk, and x^k is the state parameter estimation value at the current epoch.
The adaptive factor is constructed as(20)αk=1Δx~k≤c0c0Δx~kc1-Δx~k2c1-c02c0≤Δx~k<c10c1≤Δx~kwhere c0∈1,1.5 and c1∈3,8. Δx~k=x^k-x-k/tr∑x-k, in which · represents the norm of the matrix, tr· represents the matrix trace, and x-k 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) Calculaten+2 sigma points with the SSUT method.
Update particlesxk-1i,pk-1i using the UKF algorithm to obtain x-ki,pki, where x-ki satisfies qxki∣xk-1i,yk=Nx-ki,pki. Taking x-ki as the new samples, the sigma points can be shown as(21)χk-1i=x^k-1i+Pk-1ieini=0,1,…,n+1
With the weight of the mean valueWim and the weight of the covariance Wic(22)Wim=W0mi=01-W0mn+1i≠0(23)Wic=W0m+1+β-α2i=01-W0mn+1i≠0where x^k-1 represents the mean, Pk-1 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 x. In general, 10-4≤α≤1, and β=2 is optimal for Gaussian distributions [51]. ein represents theith sigma point of the j-dimension state vector, which could be shown as(24)eij=e0j-10i=0eij-1-1jj+1W1i=1,…,j0j-1jjj+1W1i=j+1
(c) Predict and update particles by UKF algorithm.Time Update (25) χ k ∣ k - 1 i = f χ k - 1 i (26) x - k ∣ k - 1 i = ∑ j = 0 n + 1 W j m χ j , k ∣ k - 1 i (27) Y k ∣ k - 1 i = h χ k ∣ k - 1 i (28) y - k ∣ k - 1 i = ∑ j = 0 n + 1 W j m Y j , k ∣ k - 1 i (29) P k ∣ k - 1 = P x k x k = ∑ j = 0 n + 1 W j c χ j , k ∣ k - 1 i - x - k ∣ k - 1 i · χ j , k ∣ k - 1 i - x - k ∣ k - 1 i T + R k (30) P y k y k = ∑ j = 0 n + 1 W j c Y j , k ∣ k - 1 i - y - k ∣ k - 1 i · Y j , k ∣ k - 1 i - y - k ∣ k - 1 i T + Q k (31) P x k y k = ∑ j = 0 n + 1 W j c χ j , k ∣ k - 1 i - x - k ∣ k - 1 i · Y j , k ∣ k - 1 i - y - k ∣ k - 1 i TIncorporate the new observation, and the estimations of new state and variance are(32)x-ki=x-k∣k-1i+Kkyk-y-k∣k-1i(33)Pki=αkiPk∣k-1i-KkP-ykykKkTwhere the adaptive factor αki is determined by formula (20) and the gain Kk is calculated by(34)Kk=PxyykP-ykyk-1where P-ykyk=P-k-1, and P-k represents the equivalent weight matrix in formulas (18) and (19).From formula (33), it is obvious that Pki could be adjusted by factor αki and P-ykykto make the important density function more close to the practical distribution. Thus, the importance density function of sampling particles Nx-ki,Pki from formulas (32)-(34) can be obtained for the importance resampling. When the observation model contains abnormal information, P-ykyk 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.(35)wki=wk-1ipyk∣x-kipx-ki∣x-k-1iqx-ki∣x-k-1i,ykwhere qx-ki∣x-k-1i,yk represents the recommended probability density of generated particles.
And normalize them as(36)w~ki=wki∑i=1nwkiStep 4.
Determine the degeneracy of particles by calculating the following formula:(37)N^eff=1∑i=1nw~ki2
Thus, the degeneracy of particles can be determined by comparingN^eff with the given threshold. A smaller N^eff represents a worse degeneracy of particles. To overcome the severe degeneracy of particles, the resampling process could be done by getting M new particles from the obtained posterior density and assigning a common weight 1/M to each new particle.Step 5.
Calculate the estimation of the state and variance.(38)x~k=∑i=1nw~kix-ki(39)Pk=∑i=1nwkix-ki-x^kx-ki-x^kTAnd 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 solutionx^kf,Pkf to each LF:(40)x^ki=x^kfPki-1=βiPkf-1where Pki and Pkf are theith LF and the global covariance matrix of the state estimation error, respectively. And βii=1,2 is information-sharing factor forith LF, satisfying(41)∑i=12βi=1,0<βi<1i=1,2Step 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:(42)x^k+1f=P^k+1f∑i=12Pk+1i-1x^k+1iPk+1f-1=∑i=12Pk+1i-1And go back to Step 1 to continue.


## 5. Theoretical Convergence

LetBRn denote the Borel σ-algebra on Rn, and we denote f≜supx∈Rn⁡fx. 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(43)wk∝pyk∣xkpxk∣xk-1qxk∣x0:k-1,y1:kis upper bounded for any xk-1,yk, there exists a constant ck, which is independent of N for all k≥0, such that for any fk∈BRnx×k+1(44)E1N∑i=1Nfkx0:ki-∫fkx0:kpdx0:ky1:k2≤ckfk2N
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 wk is upper bounded, which means the distribution qxk∣x0:k-1,y1:k has heavier tails than pyk∣xkpxk∣xk-1. 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 Figure3. 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 30 μ g parameters Random walk coefficient 50 μ g × s  GNS Position error 3m parameters Sampling frequency 1Hz  VNSparameters Attitude error 0.5′ Position error 3m Sampling frequency 1HzFigure 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. Figures4–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 Figures4–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.Figures7 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).Table2 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.8105The 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 Section6.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, astPF, tUPF, and tRASSUPF. In order to eliminate the influence resulting from computer differences, a set of interrelated execution times is defined as(45)tPFI=tPFtUPF,tUPFI=tUPFtUPF,tRASSUPFI=tRASSUPFtUPF.Apparently, tUPFI=1.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 Figure10, 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.


## 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 Figure3. 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 30 μ g parameters Random walk coefficient 50 μ g × s  GNS Position error 3m parameters Sampling frequency 1Hz  VNSparameters Attitude error 0.5′ Position error 3m Sampling frequency 1HzFigure 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. Figures4–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 Figures4–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.Figures7 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).Table2 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.8105The 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 Section6.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, astPF, tUPF, and tRASSUPF. In order to eliminate the influence resulting from computer differences, a set of interrelated execution times is defined as(45)tPFI=tPFtUPF,tUPFI=tUPFtUPF,tRASSUPFI=tRASSUPFtUPF.Apparently, tUPFI=1.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 Figure10, 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.



---

*Source: 8532601-2019-05-20.xml*