# Robust Interacting Multiple Model Unscented Particle Filter for Navigation

Authors: Li Xue; Yulan Han; Chunning Na
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

In order to solve the problems of particle degradation and difficulty in selecting importance density function in particle filter algorithm, a robust interacting multiple model unscented particle filter algorithm is presented, which is based on the advantages of interacting multiple model and particle filter algorithms. This algorithm can use the unscented transformation to get the particles that contain the latest measurement information of each model and calculate the robust equivalent weight function. This robust factor is designed to adjust the estimation and variance, and the important distribution function adaptively obtained is closer to the true distribution. Then, the particles weights can be flexibly adjusted in real time by using Euclidean distance to improve the computational efficiency during the resampling process. In addition, this filter process can comprehensively describe the uncertainty of the statistics characteristic of observation noise between different models. The diversity of available particles is increased, and the filter precision is improved. The proposed algorithm is applied to the SINS/GPS integrated navigation system, and the simulation analysis results demonstrate that the algorithm can effectively improve the filter performance and the calculation precision in positioning of integrated navigation system; thus, it provides a new method for nonlinear model filter.


---

## Body

## 1. Introduction

The problem of nonlinear filter has its origins in the areas of target tracking and signal processing; nevertheless, the underlying setting is extremely general and is ubiquitous in many applied areas such as integrated navigation, geodetic positioning, automatic control, finance, and artificial intelligence, where random processes are used to model the complex dynamical phenomenon. At present, the extended Kalman filter (EKF), unscented Kalman filter (UKF), central difference filter (CDKF), the particle filter (PF), and so on are widely used nonlinear filter algorithms. So, these methods are applied in many fields. Among them, the PF has also received much attention in recent years [1, 2]. In essence, the EKF, UKF, and CDKF algorithms are used to estimate the state of a nonlinear and non-Gaussian stochastic system from observation data. The UKF and CDKF are generally superior to the EKF in terms of accuracy, and the computational complexities of the three algorithms are roughly in the same order. However, this process inevitably involves a gross error when there is a deviation between the theoretical model and the practical model, leading to the biased or even divergent filter solution [3]. The PF algorithm [4, 5] is the realization of Bayesian estimation under the nonlinear conditions. It can obtain highly efficient density function that can fully reflect the measurement information. How to sample high quality particles, reduce degradation, and improve estimation accuracy is the research focus of current particle filter algorithm. On this basis, the unscented particle filter (UPF) algorithm [6–8] is proposed, which makes a good optimization and supplement in the calculation process. However, when the number of particles is not distributed properly and the models involve errors, the UPF algorithm still suffers from the phenomenon of particle degradation, and it can affect the estimation accuracy.In addition, the multiple model algorithm is suitable for the complex system, including interacting multiple model (IMM), autonomous multiple model (AMM), and cooperating multiple model (CMM) estimations. Among them, the IMM algorithm uses Markov transition probability to switch multiple models through reasonable assumption management, which has received extensive attention and application research [9, 10]. This IMM is one of the several approaches to handle the maneuvering problem, and multiple models are used to describe the motion of the maneuvering target [11]. However, in many cases, the precise system model sets design of the IMM algorithm is complicated. When the prior information is uncertain, the filter accuracy will be reduced by using fixed Markov transition probability matrix. The KF or EKF is usually used in each model filter process in nonlinear and non-Gaussian system model, and its filter performance will be greatly reduced [12, 13]. In the IMM algorithm, with the unscented particle algorithm, the multimodel structures can adapt to high mobility target, and the particle filter can handle nonlinear non-Gaussian problems with unscented transition. The robust estimation is based on the statistical analysis method, and the robust factors are calculated according to the regional segmentation that can better reflect the latest measurement information. Therefore, a simple and effective robust interacting multiple models unscented particle filter (RIMMUPF) algorithm is proposed to obtain more accurate state variable estimation in the system, which could ensure the diversity of particles and improve the accuracy of filter calculation. In addition to this, the new algorithm should not only utilize the richness of all available information as much as possible, but also satisfy the high-precision requirements of complex systems due to the combination of different observation modes.


## 2. Interacting Multiple Model Unscented Particle Filter

### 2.1. Nonlinear System Model

Consider the following nonlinear system:(1)xk=fxk−1,wk−1,yk=hxk,vk,where xk∈Rn is the state vector at epoch k, yk∈Rn is the system observation, wk−1∈Rn is the system state noise with variance Qk, vk∈Rn is the measurement noise with variance Rk, f⋅ and h⋅ are nonlinear functions, and k=0,1,…,N is the sampling time. The initial state obeys any distribution, Ex0=x¯0 denotes the initial mean, and covx0=P0 denotes the initial covariance matrix [14, 15].


### 2.2. The Main Steps of UPF Algorithm

(1)
InitializationFork=0, draw N sampling points according to the initial mean and variance, x0i∼pxo, where i=1,2,…,N. Assume the initial weights are W0i=1/N.(2)
Importance SamplingFork=1,2,…,N, j=0,…,2L, for each sample point xk−1i use unscented transformation and can further calculate update particle get x^ki and Pki [16, 17].First, calculate the Sigma pointχki and that of the particle weight ωki. In the process of time update, χj,k−1i is calculated according to the sigma point sampling strategy, and χj,k/k−1i is calculated by the nonlinear state transfer function f⋅, so as to obtain one-step state prediction x^k/k−1 and covariance matrix Pk/k−1,(2)χj,k/k−1i=fχj,k−1i,ωk−1;j=0,…,2N,x^k/k−1i=∑j=02Nωjmχj,k/k−1i,Pk/k−1i=∑j=02Nωjχj,k/k−1i−x^k/k−1iχj,k/k−1i−x^k/k−1iT.Similarly,γj,k/k−1i is calculated by the nonlinear measurement function h⋅, and the output prediction y^k/k−1, Pyy can be obtained as follows:(3)γj,k/k−1i=hχj,k/k−1i,(4)y^k/k−1i=∑j=02Nωjγj,k/k−1i=∑j=02Nωjhχj,k/k−1i,(5)Pyyi=∑j=02Nωjγj,k/k−1i−y^k/k−1iγj,k/k−1i−y^k/k−1iT,(6)Pxyi=∑j=02Nωjχj,k/k−1i−x^k/k−1iγj,k/k−1i−y^k/k−1iT.Then, the state estimation and covariance of the analysis can be computed by [18, 19](7)x^ki=x^k/k−1i+Kkiyk−y^k/k−1,(8)Kki=PxyiPyyi−1,(9)Pki=Pk/k−1i−KkiPyyiKkiT,whereKki denotes filter gain matrix.(3)
Calculate the particle weights(10)ωki=ωk−1ipyk|xkipxki|xk−1iqxki|xk−1i,y1:k.And normalize them as(11)ω˜ki=ωki∑i=1Nωki,i=1,…,N.(4)
Calculate the state estimate and return to step (2).


### 2.3. The Main Steps of IMMUPF Algorithm

The IMM estimation algorithm is an iterative recursive projection method. Combined with the UPF algorithm above, the interacting multiple model unscented particle filter (IMMUPF) algorithm [20, 21] is obtained as follows: (1)
InitializationFork=0, draw N sampling points according to the initial mean and variance x0i∼pxo, where i=1,2,…,N. Assume the initial weights are W0i=1/N, and randomly draw particles at time k. Assume the particle state value of each model is x¯k|kj, and variance is P¯k|kj.(2)
Input interaction: Input interaction for the corresponding particles of each model.(12)x¯k|kj=∑i=1muk|ki|jx^k|ki,(13)P¯k|kj=∑i=1muk|ki|jP^k|ki+x^k|ki−x¯k|kjx^k|ki−x¯k|kjT,where the matching model probability isuk|ki|j=1/C¯jnπijui,kn, C¯jn=∑i=1mπijuj,kn. In the model transformation process, πij is the transition probability under unconstrained conditions, then ∑i=1mπij=1, i=1,2,…,m, j=1,2,…,m, n=1,2,…,N.(3)
The model matches UPF filter:x¯k|kj,P¯k|kj,n=1,2,…,N can be brought into the UPF filter, and the likelihood function under the nonlinear model is obtained as(14)Λj,k+1n=1n∑j=1nNyj,k+1−y^j,k+1/kn;0;Sj,k+1,whereSj,k+1 is the covariance of the residual.(4)
Resampling: To evaluate the weight of particles, the procedure is the same as that of the previous particle filter.(5)
Model probability update(15)uk+1j=1CnΛj,k+1nC¯jn,whereCn=∑j=1mΛj,k+1nC¯jn, Λj,k+1n is the likelihood function of the nth particle at time k+1.(6)
Combined output: The weighted sum calculation is completed for them models corresponding particle sets.In the filter process, the IMMUPF algorithm can be to autonomously integrate the multiple models through Markov parameters to switch between different models [22].


## 2.1. Nonlinear System Model

Consider the following nonlinear system:(1)xk=fxk−1,wk−1,yk=hxk,vk,where xk∈Rn is the state vector at epoch k, yk∈Rn is the system observation, wk−1∈Rn is the system state noise with variance Qk, vk∈Rn is the measurement noise with variance Rk, f⋅ and h⋅ are nonlinear functions, and k=0,1,…,N is the sampling time. The initial state obeys any distribution, Ex0=x¯0 denotes the initial mean, and covx0=P0 denotes the initial covariance matrix [14, 15].


## 2.2. The Main Steps of UPF Algorithm

(1)
InitializationFork=0, draw N sampling points according to the initial mean and variance, x0i∼pxo, where i=1,2,…,N. Assume the initial weights are W0i=1/N.(2)
Importance SamplingFork=1,2,…,N, j=0,…,2L, for each sample point xk−1i use unscented transformation and can further calculate update particle get x^ki and Pki [16, 17].First, calculate the Sigma pointχki and that of the particle weight ωki. In the process of time update, χj,k−1i is calculated according to the sigma point sampling strategy, and χj,k/k−1i is calculated by the nonlinear state transfer function f⋅, so as to obtain one-step state prediction x^k/k−1 and covariance matrix Pk/k−1,(2)χj,k/k−1i=fχj,k−1i,ωk−1;j=0,…,2N,x^k/k−1i=∑j=02Nωjmχj,k/k−1i,Pk/k−1i=∑j=02Nωjχj,k/k−1i−x^k/k−1iχj,k/k−1i−x^k/k−1iT.Similarly,γj,k/k−1i is calculated by the nonlinear measurement function h⋅, and the output prediction y^k/k−1, Pyy can be obtained as follows:(3)γj,k/k−1i=hχj,k/k−1i,(4)y^k/k−1i=∑j=02Nωjγj,k/k−1i=∑j=02Nωjhχj,k/k−1i,(5)Pyyi=∑j=02Nωjγj,k/k−1i−y^k/k−1iγj,k/k−1i−y^k/k−1iT,(6)Pxyi=∑j=02Nωjχj,k/k−1i−x^k/k−1iγj,k/k−1i−y^k/k−1iT.Then, the state estimation and covariance of the analysis can be computed by [18, 19](7)x^ki=x^k/k−1i+Kkiyk−y^k/k−1,(8)Kki=PxyiPyyi−1,(9)Pki=Pk/k−1i−KkiPyyiKkiT,whereKki denotes filter gain matrix.(3)
Calculate the particle weights(10)ωki=ωk−1ipyk|xkipxki|xk−1iqxki|xk−1i,y1:k.And normalize them as(11)ω˜ki=ωki∑i=1Nωki,i=1,…,N.(4)
Calculate the state estimate and return to step (2).


## 2.3. The Main Steps of IMMUPF Algorithm

The IMM estimation algorithm is an iterative recursive projection method. Combined with the UPF algorithm above, the interacting multiple model unscented particle filter (IMMUPF) algorithm [20, 21] is obtained as follows: (1)
InitializationFork=0, draw N sampling points according to the initial mean and variance x0i∼pxo, where i=1,2,…,N. Assume the initial weights are W0i=1/N, and randomly draw particles at time k. Assume the particle state value of each model is x¯k|kj, and variance is P¯k|kj.(2)
Input interaction: Input interaction for the corresponding particles of each model.(12)x¯k|kj=∑i=1muk|ki|jx^k|ki,(13)P¯k|kj=∑i=1muk|ki|jP^k|ki+x^k|ki−x¯k|kjx^k|ki−x¯k|kjT,where the matching model probability isuk|ki|j=1/C¯jnπijui,kn, C¯jn=∑i=1mπijuj,kn. In the model transformation process, πij is the transition probability under unconstrained conditions, then ∑i=1mπij=1, i=1,2,…,m, j=1,2,…,m, n=1,2,…,N.(3)
The model matches UPF filter:x¯k|kj,P¯k|kj,n=1,2,…,N can be brought into the UPF filter, and the likelihood function under the nonlinear model is obtained as(14)Λj,k+1n=1n∑j=1nNyj,k+1−y^j,k+1/kn;0;Sj,k+1,whereSj,k+1 is the covariance of the residual.(4)
Resampling: To evaluate the weight of particles, the procedure is the same as that of the previous particle filter.(5)
Model probability update(15)uk+1j=1CnΛj,k+1nC¯jn,whereCn=∑j=1mΛj,k+1nC¯jn, Λj,k+1n is the likelihood function of the nth particle at time k+1.(6)
Combined output: The weighted sum calculation is completed for them models corresponding particle sets.In the filter process, the IMMUPF algorithm can be to autonomously integrate the multiple models through Markov parameters to switch between different models [22].


## 3. Robust Interacting Multiple Model Unscented Particle Filter

The proposed robust interacting multiple model unscented particle filter algorithm improves the robust adjustment capability of the system model by modifying the measurement and state covariance in real time, so as to ensure the accuracy of the calculation. The IMM process can take into consideration the probability of each model by Markov transition probability. The basic design idea of the algorithm is as follows: firstly, these groups of corresponding particles are interacted as inputs to each model, and this process can reinitialize the filter input value; secondly, the unscented transformation is carried out for each model, and at the timek the Sigma point is calculated to obtain the estimations with the latest measurement information. It can calculate the equivalent weight functions at different time periods to obtain the robust factor. This process can not only adjust state estimations and variances, but also take into account the latest measurements information. So, the model is matched with the robust unscented particle filter; thirdly, in the resampling process, the calculated weights of the particles can be adjusted by the Euclidean distance to increase the weights of the useful particles; then, it can update the corresponding model probability and calculate the output interaction for the particle sets of each model; finally, the state estimation is realized by constantly cyclic updating of the particles. So, this filter process can also make efficient use of the latest measurement information and can be widely employed in the positioning applications to describe the uncertainty of the system model and statistics characteristic of observation noise. The RIMMUPF algorithm includes the following steps: Step 1: Assume that the Markov model transition probability matrix isT=πlj;l,j=1,2,…,m, and each model probability matrix is U=ul,kn,l=1,2,…,m;n=1,2,…,N, where m is the number of models, and N is the number of particles.Step 2: Initialization. Fork=0, draw N particle sampling points according to the initial mean and variance, x0i∼pxo, where i=1,2,…,N. Assume that the initial weights are W0i=1/N, where x¯0i=Ex0i and P0i=Ex0i−x^0ix0i−x^0iT.Step 3: Input interaction. It can complete input interaction for the corresponding particles of each model by using the IMMUPF algorithm formulae (12) and (13). This process can reinitialize the filter input value, and the initial value of each filter obtained from the previous filter process. The state estimation and the conditional transition probability of the model are calculated using the system prior information.Step 4: Fork time, calculate the robust equivalent weight function and adjust particles completed unscented transformation, then get x^ki∗ and Pki∗, as follows, (1)
For robust factor, namely, equivalent weight function, it can select the equivalent weight matrix constructed by using the IGG descending weight function [23, 24]. The measured value can be controlled by robust factor. If the reciprocal function is taken, it is defined as variance expansion factor function. Assume that P¯=diagP¯1,P¯2,…,P¯k, and the IGG weight function followed:(16)P¯i=pi,Vi≤k0,pik0Vi,k0<Vi≤k1,0,Vi>k1,or(17)P¯i=pi,Vi≤k0,pik0Vik1−Vi2k1−k02,k0≤Vi<k1,0,k1≤Vi,wherek is a constant, k0∈1,1.5 and k1∈3,8, Pi is obtained by formula (5), and Vk is the residual vector of the observed value yk.And calculate Sigma points and update particlesxk−1i,pk−1i by unscented transformation; χki and ωki are calculated as follows,(18)χ0,k−1i=x^k−1i,χj,k−1i=x^k−1i+N+λPk−1ij;j=1,…,N,χj,k−1i=x^k−1i−N+λPk−1ij;j=n+1,…,2N,ω0m=λN+λ,ω0c=λN+λ+1−α2+β,ωjm=ωjc=λ2N+λ;j=1,…,2N,whereλ=α2N+κ denotes the scale factor, κ is zero, and α at the range 10−3<α≤1, then the value is 1, and β is 2; ωj is the weight of the jth Sigma point, which satisfies j=0,1,…,2N.The particles are used to predict and update, andx^ki,pki can be obtained by formulae (7) to (9). The robust equivalent weight factor is used to calculate the state estimation and variance x^ki∗,Pki∗ of the sampled particles.(19)x^ki∗=x^k/k−1i+Kki∗yk−y^k/k−1,Kki∗=PxyiP¯yyi−1,Pki∗=Pk/k−1i−KkiP¯yyiKkiT.The above formula shows that factorsPki∗ can be influenced and regulated by P¯yyi. This estimation scheme fully takes into account the actual situation of the measurement data and can classify gross errors. It contains three sections: When observation error is small, equivalent weight is selected the original; when the observation error is greater, but not significant, the observations should fall weight; when the observations are notably abnormal, it should be eliminated; that is, take the weight zero. If P¯yyi=Pyyi, the x^ki∗ and Pki∗ are obtained by the UKF algorithm.(2)
The time and measurement are updated according to UPF algorithm in Section2.2; then the particle state values and covariance of the m models are calculated.(3)
Calculate the particle weights and normalize.(4)
The state variables and covariance at thek time can be calculated as follows,(20)x^i,k|kn∗=∑n=1Nω˜kixki∗,i=1,2,…,m,P^i,k|kn∗=∑n=1Nω˜kixki∗−x^ki∗xki∗−x^ki∗T,wherei=1,2,…,m;n=1,2,…,N.Step 5: The model matches robust UPF filter.x^k|kj∗,P^k|kj∗,n=1,2,…,N is applied to the jth model by the robust UPF filter, then x^k+1|k+1j∗ and P^k+1|k+1j∗ can be calculated at time k+1. The other parameters are also calculated such as Sk+1|k+1j∗ and Λk+1|k+1j∗ etc.Step 6: Residual resampling.All particle weights in each model are evaluated separately to generate new samples with similar weights. This process sometimes reduces the diversity of particles, which can be selected according to the specific situation.According to formula (10), the weight is improved as follows:(21)wki∗=wki∗+wk−maxi∗N⋅sinLiLmax⋅π/2⋅β,wherewk−maxi∗ denotes the maximum weight value, and Lmax and Li denote the Euclidean distance of the measurement residual yk−maxi and yk−mini, as follows:(22)Lmax=yk−mini−yk−maxiT⋅yk−mini−yk−maxi,Li=yki−yk−maxiT⋅yki−yk−maxi,whereβ is the adaptive coefficient, which is determined by the statistical characteristics of the measurement noise. When the equivalent measurement noise is low, take β=0, and it does not adjust the likelihood distribution; on the contrary, it can artificially make the likelihood distribution wider [25]. Then, particle weight normalization is done as formula (11).Step 7: Model probability update. At the current moment, the model probability is obtained according to the previous step results. Update the probability of the model by using the IMMUPF algorithm formula (15).Step 8: Output interaction. The corresponding particles of them model can be output interaction, and the formula superscript ∗ is omitted here.(23)xk+1|k+1n=∑j=1mx^j,k+1|k+1nuj,k+1n,P^k+1|k+1n=∑j=1muj,k+1nP^j,k+1|k+1n+x^j,k+1|k+1n−x^k+1|k+1x^j,k+1|k+1n−x^k+1|k+1T.This algorithm can recursively calculate the iteration sample particles, to accomplish the robust adaptive adjustment of the state estimation and variance. The robust factor has a good adaptive ability to the make use of the measurement information of system noise, and the important distribution function obtained is closer to the true distribution. The diversity of particles is increased during the sampling process, and the filter precision is improved.


## 4. Experimental Results and Analysis

Experiments about vehicle testing have been conducted to comprehensively evaluate and analyze the performance of the proposed method for SINS/GPS integrated navigation system [26, 27]. The comparison with the IMMUPF algorithms is also discussed in this section. The testing requires the vehicle to run steadily. The actual navigation position information of the testing car was obtained by using the high-precision vehicle-mounted differential GNSS receiver UT-206 (<10cm position accuracy) and was used as the reference value of the experimental results. Test instruments and equipment include SINS/GPS integrated navigation system, industrial control computer, testing power supply, current voltmeter, and equipment fixing board. In the testing simulation conditions, the vehicle was carried out on a highway, with total mileage about 15km and the average running speed at 40km/h. The vehicle testing and loading equipment is shown in Figure 1.Figure 1
Vehicle testing and loading equipment.The sensor main parameters of SINS/GPS integrated navigation system followed: SINS replacement rate100Hz, GPS replacement 5Hz; Gyroscope measurement range ±200°/s, zero-bias stability 0.5°/hr (normal temperature), resolution ratio 0.12°/h, bandwidth 100Hz, random walk 0.15°/hr1/2; the accelerometer measurement range ±12g, zero-bias stability 0.05mg (normal temperature), resolution ratio 1.9ug, bandwidth 100Hz, random walk 0.06m/s/hr1/2; during the testing process, the high-precision wide area difference system was used as the position reference system, and the test data was obtained by data storage. In the post-processing, it was used to solve the filtering and verify the performance of filter algorithms. The GPS has a good observation environment, and velocity measurement accuracy was 0.02m/sRMS. At the beginning, the vehicle trajectory position coordinates in SINS/GPS integrated navigation system were initialized and shown in Figure 2.Figure 2
Vehicle trajectory position coordinates.In SINS/GPS integrated navigation system, the state variablesxt are selected as (24)xt=δqT,δVT,δPT,εT,∇TT,where δq=δq0,δq1,δq2,δq3T denotes attitude error quaternion of SINS, δV=δVE,δVN,δVUT denotes velocity error in three direction of SINS, δP=δL,δλ,δHT denotes the errors of latitude, longitude and height, ε=εx,εy,εxT denotes random drift of the gyro, and ∇=∇x,∇y,∇zT denotes constant bias of the accelerometer. So, the system state equation can be expressed as(25)x˙=fx,t+wt,where fx,t is the system nonlinear state function, and wt is the system process noise vector, where (26)fx,t=BI−Cncωinn−BCbcεbI−CcnCbcfb−2ωien+ωenn×δVn−2δωien+δωenn×Vn+Cbc∇bMδV+NδL03×103×1,where B=1/2MQpn, and MQpn denotes matrix representation of quaternion multiplication; Cnc and Cbc are, respectively, the attitude transfer matrices of the navigation coordinate system n and the carrier coordinate system b to the computational coordinate system c; ωinn denotes the projection of the angular velocity of the carrier navigation system into the navigation coordinate system, ωien and ωenn are the angular velocity of the Earth’s rotation and the position angular velocity, Vn denotes vehicle speed, ∇b denotes accelerometer error, fb is the measured value of the accelerometer, and M and N are velocity error and position error coefficient matrix, respectively. wt=−BCbpwgT,CbpwaT,01×9T, where wg denotes gyro white noise, and wa denotes accelerometer white noise. The dynamic model of SINS/GPS integration in discrete-time form can be expressed as (27)xk=fxk−1+wk.The position and speed output information of SINS is subtracted from the corresponding information of GPS as measurement, and the system measurement equation can be written as(28)zt=hxt+vt,where h is the measurement matrix, vt is the measurement of white noise, and the mean value is zero, then, in discrete-time form, it can be expressed as (29)zk=Hkxk+vk.And take measurement vector as(30)zk=VES−VEGVNS−VNGVUS−VUGLS−LGλS−λGHS−HGT.So,(31)zk=Hkxk+vk=δVS−δVGδPS−δPG=HV,kHP,kxk+vv,kvp,k,where (32)Hk=03×4I3×303×303×303×303×403×3I3×303×303×3.AndVS and PS, respectively, represent the output velocity and position of SINS, VG and PG represent the results output velocity and position of GPS, and vv,k and vp,k are the measurement noises corresponding to the velocity and position errors of GNSS receiver.Assume model setsM=m1,m2,m3, m1 at constant velocity Q1=Q0, R1=R0; m2 at constant acceleration Q2=5Q0, R2=10R0; m3 at coordinated turn, the noise is high Q3=25Q0, R3=100R0. Consider that the model initial Markov transition probability matrix πij and initial transfer probability vector u are chosen as follows, (33)πij=0.80.10.10.10.80.10.10.10.8,uij=0.80.10.1.The root mean squared errors (RMSE) of the estimation error are computed to show the navigation performance of the comparison methods from a statistical perspective. The RMSE is defined as(34)RMSE=1M∑i=1Mx^i−xtrue,where M is the Monte Carlo runs. Take the estimated velocity and position error as an example, and the simulation results obtained by the two algorithms are shown in the Figures 3–6.Figure 3
The velocity estimation errors of IMMUPF.Figure 4
The velocity estimation errors of RIMMUPF.Figure 5
The position estimation errors of IMMUPF.Figure 6
The position estimation errors of RIMMUPF.It can be seen from the velocity error estimation results that the longitude and latitude errors obtained by the IMMUPF algorithm are controlled within−0.121,0.110 and −0.011,0.110 range, while the height errors are controlled within the −0.071,0.080 range after 200s. The longitude error obtained by the RIMMUPF algorithm is basically controlled within the −0.082,0.071 range, the latitude error is basically controlled within the −0.071,0.101 range, and the height error is basically controlled within the −0.051,0.053 range except for the fluctuation closed to1200s. Table 1 describes the RMSEs of the velocity errors by the two algorithms and demonstrates that the RMSE and error range of the proposed RIMMUPF method are much smaller than those of the IMMUPF method.Table 1
Velocity error comparison for the simulation case (s/m).
Filtering methodsRMSE ofδVERMSE ofδVNRMSE ofδVUVelocity error rangeIMMUPF0.05870.05410.0354−0.121,0.110, −0.011,0.110, −0.071,0.080RIMMUPF0.04300.04030.0227−0.082,0.071, −0.071,0.101, −0.051,0.053The position estimation errors simulation results show that the longitude, latitude, and height estimation errors are basically controlled within the−20.51,19.70, −24.09,24.01, and −16.37,0.88 ranges by the IMMUPF algorithm, while the estimation errors are controlled within the −18.31,16.10, −14.11,15.01, and −15.07,14.85 ranges by the RIMMUPF algorithm. Table 2 provides the error comparison for these two methods, showing that the proposed robust method has much smaller RMSE and position estimation error. These simulation results also verify that the proposed RIMMUPF has a stronger ability to resist the unknown dynamic model errors, thus leading to improved navigation accuracy for the vehicle navigation with SINS/GPS integration.Table 2
Position error comparison for the simulation case (m).
Filtering methodsRMSE ofδLRMSE ofδλRMSE ofδHPosition error rangeIMMUPF7.9267.8475.165−20.51,19.70, −24.09,24.01, −16.37,0.88RIMMUPF6.2864.5874.097−18.31,16.10, −14.11,15.01, −15.07,14.85


## 5. Conclusion

The state estimation and variance calculated by the proposed RIMMUPF can better describe the characteristics of the system model. It can increase the robust adaptive adjustment capability of the system model and restrain the influence of abnormal interference on the state and measurement. By making full use of the effective information, a better particle importance density function is obtained, and the precision of particle solution in multiple model is improved. In the resampling process, the Euclidean distance can be used to adaptively adjust the particles weight in real time according to the specific situation, so as to improve the calculation efficiency. The experiments and comparison analysis demonstrate that the proposed algorithm can effectively improve the filter performance and the calculation precision in positioning of SINS/GPS integrated navigation system;thus, it provides a new method for nonlinear model filter.Future work will focus on the improvement of the proposed algorithm to making the performance of algorithm more perfect, and how to reduce the complexity of calculation and improve the speed of the filter.



---

*Source: 8871358-2020-11-10.xml*