Robust Interacting Multiple Model Unscented Particle Filter for Navigation

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. *is 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. *is robust factor is designed to adjust the estimation and variance, and the important distribution function adaptively obtained is closer to the true distribution. *en, 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. *e diversity of available particles is increased, and the filter precision is improved. *e 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.


Introduction
e 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. e 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]. e 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][7][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]. is 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. e 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. e 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. erefore, 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.

Interacting Multiple Model Unscented
Particle Filter 2.1. Nonlinear System Model. Consider the following nonlinear system: where x k ∈ R n is the state vector at epoch k, y k ∈ R n is the system observation, w k−1 ∈ R n is the system state noise with variance Q k , v k ∈ R n is the measurement noise with variance R k , f(·) and h(·) are nonlinear functions, and k � 0, 1, . . . , N is the sampling time. e initial state obeys any distribution, E(x 0 ) � x 0 denotes the initial mean, and cov(x 0 ) � P 0 denotes the initial covariance matrix [14,15].

e Main Steps of UPF Algorithm
(1) Initialization For k � 0, draw N sampling points according to the initial mean and variance,  [16,17].
First, calculate the Sigma point χ i k and that of the particle weight ω i k . In the process of time update, χ i j,k−1 is calculated according to the sigma point sampling strategy, and χ i j,k/k−1 is calculated by the nonlinear state transfer function f(·), so as to obtain one-step state prediction x k/k−1 and covariance matrix P k/k−1 , Similarly, c i j,k/k−1 is calculated by the nonlinear measurement function h(·), and the output prediction y k/k−1 , P yy can be obtained as follows: en, the state estimation and covariance of the analysis can be computed by [18,19] where K i k denotes filter gain matrix. (3) Calculate the particle weights

Mathematical Problems in Engineering
And normalize them as (4) Calculate the state estimate and return to step (2).

e Main
Steps of IMMUPF Algorithm. e 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) Initialization For k � 0, draw N sampling points according to the initial mean and variance Assume the initial weights are W (i) 0 � 1/N, and randomly draw particles at time k. Assume the particle state value of each model is x j k|k , and variance is P j k|k .
(2) Input interaction: Input interaction for the corresponding particles of each model.
where the matching model probability is u In the model transformation process, π ij is the transition probability under unconstrained conditions, then (3) e model matches UPF filter: x j k|k , P j k|k , n � 1, 2, . . . , N can be brought into the UPF filter, and the likelihood function under the nonlinear model is obtained as where S j,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 where C n � m j�1 Λ n j,k+1 C n j , Λ n j,k+1 is the likelihood function of the nth particle at time k + 1. (6) Combined output: e weighted sum calculation is completed for the m 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].

Robust Interacting Multiple Model
Unscented Particle Filter e 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. e IMM process can take into consideration the probability of each model by Markov transition probability. e 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 time k 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. is 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. e RIMMUPF algorithm includes the following steps: Step 1: Assume that the Markov model transition probability matrix is T � π lj ; l, j � 1, 2, . . . , m , and each model probability matrix is U � u n l,k , l � 1, 2, . . . , m; n � 1, 2, . . . , N , where m is Mathematical Problems in Engineering the number of models, and N is the number of particles.
Step 2: Initialization. For k � 0, draw N particle sampling points according to the initial mean and variance, 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). is process can reinitialize the filter input value, and the initial value of each filter obtained from the previous filter process.
e state estimation and the conditional transition probability of the model are calculated using the system prior information.
Step 4: For k time, calculate the robust equivalent weight function and adjust particles completed unscented transformation, then get x i * k and P i * k , 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]. e 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 � diag(P 1 , P 2 , . . . , P k ), and the IGG weight function followed: or where k is a constant, k 0 ∈ (1, 1.5) and k 1 ∈ (3, 8), P i is obtained by formula (5), and V k is the residual vector of the observed value y k .
And calculate Sigma points and update particles x i k−1 , p i k−1 by unscented transformation; χ i k and ω i k are calculated as follows, , where λ � α 2 (N + κ) 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. e particles are used to predict and update, and x i k , p i k can be obtained by formulae (7) to (9). e robust equivalent weight factor is used to calculate the state estimation and variance x i * k , P i * k of the sampled particles.
e above formula shows that factors P i * k can be influenced and regulated by P i yy .
is 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 i yy � P i yy , the x i * k and P i * k are obtained by the UKF algorithm.
(2) e time and measurement are updated according to UPF algorithm in Section 2.2; then the particle state values and covariance of the m models are calculated. (3) Calculate the particle weights and normalize. (4) e state variables and covariance at the k time can be calculated as follows, where i � 1, 2, . . . , m; n � 1, 2, . . . , N.
Step Step 6: Residual resampling. All particle weights in each model are evaluated separately to generate new samples with similar weights. is 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: where w i * k−max denotes the maximum weight value, and L max and L i denote the Euclidean distance of the measurement residual y i k−max and y i k−min , as follows: 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]. en, 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. e corresponding particles of the m model can be output interaction, and the formula superscript * is omitted here.
x n k+1|k+1 � m j�1 x n j,k+1|k+1 u n j,k+1 , is algorithm can recursively calculate the iteration sample particles, to accomplish the robust adaptive adjustment of the state estimation and variance. e 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. e diversity of particles is increased during the sampling process, and the filter precision is improved.

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]. e comparison with the IMMUPF algorithms is also discussed in this section. e testing requires the vehicle to run steadily. e actual navigation position information of the testing car was obtained by using the high-precision vehiclemounted differential GNSS receiver UT-206 ( < 10 cm 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 15 km and the average running speed at 40 km/h. e vehicle testing and loading equipment is shown in Figure 1.
e sensor main parameters of SINS/GPS integrated navigation system followed: SINS replacement rate 100 Hz, GPS replacement 5 Hz; Gyroscope measurement range ± 200°/s, zero-bias stability 0.5°/hr (normal temperature), resolution ratio 0.12°/h, bandwidth 100 Hz, random walk Mathematical Problems in Engineering 5 0.15°/hr 1/2 ; the accelerometer measurement range ± 12 g, zero-bias stability 0.05 mg (normal temperature), resolution ratio 1.9 ug, bandwidth 100 Hz, random walk 0.06 m/s/hr 1/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 postprocessing, it was used to solve the filtering and verify the performance of filter algorithms. e GPS has a good observation environment, and velocity measurement accuracy was 0.02 m/s(RMS). At the beginning, the vehicle trajectory position coordinates in SINS/GPS integrated navigation system were initialized and shown in Figure 2.
In SINS/GPS integrated navigation system, the state variables x(t) are selected as where δq � [δq 0 , δq 1 , δq 2 , δq 3 ] T denotes attitude error quaternion of SINS, δV � [δV E , δV N , δV U ] T denotes velocity error in three direction of SINS, δP � [δL, δλ, δH] T denotes the errors of latitude, longitude and height, ε � [ε x , ε y , ε x ] T denotes random drift of the gyro, and ∇ � [∇ x , ∇ y , ∇ z ] T denotes constant bias of the accelerometer. So, the system state equation can be expressed as where f(x, t) is the system nonlinear state function, and w(t) is the system process noise vector, where where B � (1/2)M(Q n p ), and M(Q n p ) denotes matrix representation of quaternion multiplication; C c n and C c b are, respectively, the attitude transfer matrices of the navigation coordinate system n and the carrier coordinate system b to the computational coordinate system c; ω n in denotes the projection of the angular velocity of the carrier navigation system into the navigation coordinate system, ω n ie and ω n en are the angular velocity of the Earth's rotation and the position angular velocity, V n denotes vehicle speed, ∇ b denotes accelerometer error, f b is the measured value of the accelerometer, and M and N are velocity error and position error coefficient matrix, respectively.
where w g denotes gyro white noise, and w a denotes accelerometer white noise. e dynamic model of SINS/GPS integration in discretetime form can be expressed as e 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   where h is the measurement matrix, v(t) is the measurement of white noise, and the mean value is zero, then, in discretetime form, it can be expressed as And take measurement vector as So, where And V S and P S , respectively, represent the output velocity and position of SINS, V G and P G represent the results output velocity and position of GPS, and v v,k and v p,k are the measurement noises corresponding to the velocity and position errors of GNSS receiver.
Assume model sets M � (m 1 , m 2 , m 3 ), m 1 at constant velocity Q 1 � Q 0 , R 1 � R 0 ; m 2 at constant acceleration Q 2 � 5Q 0 , R 2 � 10R 0 ; m 3 at coordinated turn, the noise is high Q 3 � 25Q 0 , R 3 � 100R 0 . Consider that the model initial Markov transition probability matrix π ij and initial transfer probability vector u are chosen as follows, e root mean squared errors (RMSE) of the estimation error are computed to show the navigation performance of the comparison methods from a statistical perspective. e RMSE is defined as 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. 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 200 s. e 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 to1200 s. 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 2 provides the error comparison for these two methods, showing that the proposed robust method has much smaller RMSE and position estimation error. ese simulation results also verify that the proposed RIMMUPF has a stronger ability to resist the unknown dynamic model errors, thus leading to

Conclusion
e 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. e 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.

Data Availability
e data used to support the findings of this study are available from the corresponding author upon request.