Abstract
In order to obtain the relatively appropriate importance density function and alleviate the problem of particle degradation, a new improved auxiliary particle filter algorithm is proposed. After calculating the auxiliary variable, the adaptive regulator is employed to obtain the state estimation. So, the latest measurement information is efficiently utilized to establish a better importance density function in the importance sampling process. Then, the process of particle weights’ adaptive adjustment and random-weighted calculation can keep the diversity of particles and improve the filter precision; thus, it can better solve the filter problem of nonlinear system model error and noise interference. The simulation and analysis result show that the proposed algorithm can optimize the filter performance and improve the calculation precision in the positioning of the SINS/SAR integrated navigation system, compared with the other two existing filters.
1. Introduction
The particle filter (PF) algorithm can use a series of weighted random sample sets in the state space to approximate the posterior probability density function of the system state, which is applicable to any nonlinear non-Gaussian system that can be represented by the state space models [1, 2]. The importance sampling process of the particle filter is simple and easy to calculate. However, the change of measurement information at the current moment is not considered when the prior distribution is selected as the important density function. This sampling process only extracts particle samples according to the particle motion and previous state, so it can make them too sensitive to outliers and lose a large number of low weight particles [3, 4]. At the same time, the increase in the variance of particle weight will cause serious particle degradation. There are two key techniques to mitigate the particle degradation that are selection of importance density function and resampling process [5, 6].
Since the 1990s, the scholar Gordon proposed the particle filter algorithm [7]; this method has attracted great attention from many scholars around the world, which has also promoted the particle filter to improve continuously. Although it can generate many new developed algorithms based on particle filter, such as central difference particle filter (CDPF), unscented particle filter (UPF), marginalized particle filter (MPF), Gaussian particle filter (GPF), and auxiliary particle filter (APF). Inevitably, there are still many shortcomings and limitations in the sampling theory, method, and application of the particle filter. In different development particle filter algorithms, it is important to complete effectively importance sampling and alleviate weight degradation and sample depletion in the filter process. There is a general problem that needs to be solved urgently to obtain better filter performance under different model noises’ conditions. The CDPF method based on CDKF can approximate the posterior mean value and variance by numerical integration or polynomial interpolation. The UPF method based unscented transformation is proposed as an improvement to PF. It has been demonstrated in many literature that the UPF is generally superior to the PF in terms of accuracy. The unscented transformation can obtain a set of deterministic weighted sampling points by means of the state posterior probability distribution at the last moment, the mean and variance probability distribution of the process, and the observation noise at the current moment [8, 9]. There is no need to the linearization of the nonlinear function, and the calculation accuracy can reach at least the second order of the Taylor series expansion. However, in many application cases, CDPF, UPF, and so on are required to obey the Gaussian distribution and cause particle degradation [10].
In 1999, Pitt et al. had proposed an auxiliary particle filter (APF) [11]. This method designs the sampling distribution by introducing an auxiliary variable and shows better performance compared with PF. But its improvement is limited, especially when these two kinds of problems occur: the state transfer density is very dispersed and the likelihood density changes significantly relative to the state transfer density [12]. If the prior probability density function is used for particle importance sampling and the current measurement is not taken into account, the APF could not significantly improve estimation accuracy in many engineering applications cases. The sampled particles will seriously deviate from the actual target position, thus reducing the accuracy of the state prediction mean value and covariance.
In order to enhance the filter capability of particle filter algorithm for nonlinear environmental measurement data, the influence of measurement information, model error, and noise interference should also be fully considered in the design of importance density function during APF sampling and resampling process. So, this paper proposes a new improved auxiliary particle filter (IAPF), and it is applied to the navigation position system. This filter algorithm designs an appropriate importance density function to carry out importance sampling, which could take the latest measurement information into account and adjust adaptively the particle weight distribution. Then, these obtained state estimation and variance will perform random weighted calculation to improve the filter performance.
With the rapid development of navigation technology, the demand for the reliability and high precision of the navigation system is getting much higher. The single navigation system can no longer meet the increased demand. So the integrated navigation system can get much attention. The strap-down inertial navigation system (SINS) has many advantages, especially high autonomy, strong concealment, continuous measurement carrier, and not easily disturbed by the outside interference. So, it occupies the core position in the navigation system application. Synthetic aperture radar (SAR) has the advantages of high resolution, long time operation, remote imaging, and detection of relatively hidden targets. However, it can easily be affected by coherent lights and so on. So, the SINS/SAR integrated navigation system is adopted in this paper. The proposed algorithm was applied to the SINS/SAR integrated navigation system and compared with the other filters.
2. Auxiliary Particle Filter Analysis
2.1. Nonlinear System Model and Particle Filter Algorithm
In practical engineering applications, many filter conditions are nonlinear environment, so the high-efficiency filters method suitable for nonlinear systems is very important. For the linear systems, the most commonly used solution of the optimal filter is the famous Kalman filter (KF). But for the nonlinear systems, it is difficult or even impossible to obtain the optimal filter solution, because it needs to deal with infinite-dimensional integral calculation. The PF can use sample sets’ form to describe prior information and posterior information, rather than function forms, which can deal with the problem of solving nonlinear filter. Consider the following nonlinear system:where is the state vector at epoch k, is the system observation, is the system state noise with variance , is the measurement noise with variance , and are nonlinear functions, and is the sampling time. The initial state obeying any distribution is denoting the initial mean and denotes the initial covariance matrix.
The PF approximates the probability density function through a set of random samples and replaces the integral operation with the sample mean in the state space. Then, it can adjust continuously weights and positions of particles according to the observation information, to obtain the minimum variance of states estimation [13, 14]. The particle importance weight can be updated recursively bywhere denotes the likelihood density and denotes the transition density of the state. When the importance density function only depends on the previous moment state, it can be expressed as . At time , the weighted posterior probability density can be expressed aswhere , is normalization, and is the Dirac function [1].
In the importance sampling process, the traditional PF algorithm selects the prior density as the importance density function and uses the transfer probability to obtain the new importance density function. However, it does not consider the influence of the latest measurement value and cannot approximate the real distribution function very well, so the filter accuracy is affected. In addition, when the likelihood distribution is located at the tail of the transfer prior distribution or the observation model has a high precision, many samples will become invalid samples due to the small normalized weight. Then, PF could show degeneration phenomenon and the sample diversity will decrease. The effective method is to increase the number of particles, but this method is unrealistic.
2.2. The Main Steps of Auxiliary Particle Filter Algorithm
The APF algorithm is approximated by the empirical distribution made by recursive computation of N weighted particles. In this algorithm, sampled particles are obtained from the joint density function , where is the auxiliary variable and represents the sampled particles that are available for . By Bayesian theory, the joint density function can be expressed as
When this algorithm is to select the importance function, the following proportions should be satisfied:where is the representation of certain features of , is known by conditions, and could be the conditional mean calculated by . After the importance density function performs decomposition calculation, it can be seen that the sample and auxiliary variables can be generated by the following two steps [15–17]:(1)Sampling from previous indicators, (2)Sampling from prior density under given conditions,
The main steps of APF are as follows:(1)Initialization: for , draw N sampling points according to the initial prior information, , where .(2)For , calculate the particle sample .(3)Calculate the corresponding weights of auxiliary variable and normalize it as(4)Use the resampling procedure in the PF filter algorithm to obtain news state estimation and particle index sets .(5)The basic PF step is used to calculate the predicted state values’ sampling and update the weights as
According to the particle filter step, calculate the state estimation and return to Step (2).
The APF differs from PF in that the resampling process is calculated before the state prediction and weight update steps. So, the particles to be sampled are intuitively pushed to the high likelihood region. Theoretically, it can obtain better filter performance when the process noise is reduced. However, when the conditional likelihood is not insensitive to the state, this filter slightly alleviates the lack of particle diversity. In addition to that, the auxiliary variables sampling process cannot fully consider the influence of the latest measurement, and it will easily cause the system model error. To solve the above problems, a new improved algorithm is proposed based on APF for nonlinear systems.
3. A New Improved Auxiliary Particle Filter Algorithm
Because the APF algorithm is widely applied and it is crucial to design an appropriate importance density function, therefore, the latest measurement information, model error, and noise interference should be fully taken into account when the importance sampling and resampling process of the APF will be improvement and optimization. This paper proposes the IAPF algorithm that can adaptively adjust the state estimation and consider the measurement information change after calculating auxiliary variables; then, the importance density function will be close to the true distribution obtained by importance sampling. The particles’ weights are selected and adjusted adaptively, and, finally, the state estimation and variance are calculated by random weighted, so that the filter accuracy can be improved. The IAPF algorithm includes the following steps: Step 1 (initialization): for , , where , assume the initial values and . Step 2: calculate particle index sets , , and . Step 3: calculate the corresponding particle weights of and normalized weights. Then, a new particle set is obtained by auxiliary resampling, . Step 4: if the distribution of these weighted particle sets is used to approximate the importance density function, i.e., the traditional auxiliary variable is only to be sampled from the transition density distribution. When the process noise is small, the performance of APF is usually better than that of the PF method; however, when there appears large variation of measurement information and noise changes in the system model, the sampled particle does not provide sufficient information and will seriously deviate from the actual target position, thus reducing the accuracy of the state predicted value and covariance. Therefore, the IAPF should not only fully consider the influence of the latest measurement information but also adaptively adjust parameters to adapt to model errors and noise interference.(1)When the interval of observation time increases, the system model error will also increase. If the target state is not updated for a long time, it is difficult to accurately estimate the system motion model with limited observation information. Therefore, for , where the adaptive regulator can be expressed as where is a constant, denotes the time interval, denotes the position target velocity, denotes the standard deviation of measurement residual, denotes the standard deviation of measurement error, , and is generated based on the prior particle and the measurement information at time , which is closer to the real state value [18, 19]. This step can use many iterative computations and adaptive regulator to update the state estimation that will be to approximate the nonlinear observation, which further reduces the state estimation error and thus optimizes the filter estimation to obtain the importance density function, so is close to .(2)Importance sampling (): calculate the particle weights: Step5: residual resampling. All particle weights 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. The weight is improved by Euclidean distance [20, 21], such as where and denote the Euclidean distance of the measurement residual and : where denotes the maximum weight value, is the adaptive coefficient, is the proportion constant value, and is determined by the statistical characteristics of the measurement noise. Then, normalized weights In the resampling process, we can get particle again. Step 6: random-weighted calculation.
The particles carried out a random weighted calculation, and the random weighted vectors all follow the Dirichlet distribution [22] and satisfy . Calculate the state estimation and variance such as
Return to Step (2) to recursively calculate the state estimation and variance based on the new measurement information.
This algorithm can recursively calculate the sample particles through the adaptive regulator. It has a better adaptive ability to deal with measurement information and other system interference factors, and the residual resampling and random weighted calculation process makes the filter algorithm have better adaptability and robustness.
4. Simulation Results and Analysis
4.1. SINS/SAR Integrated Navigation System
The simulation experiment has been conducted to comprehensively evaluate and analyze the performance of the proposed algorithm for SINS/SAR integrated navigation system, and the comparison with the other two existing filters APF and PF is also discussed in this section. The SINS/SAR integrated navigation system makes use of the complementary characteristics of the SINS and SAR to improve the performance of the whole system [23, 24]. The schematic diagram of the SINS/SAR integrated navigation system is as in Figure 1.
The local navigation frame (n-frame) is selected as the east-north-up (e-n-u) geography frame. Denote the inertial frame by i, the Earth frame by e, and the body frame by b. The dynamic model of the SINS/SAR integration navigation system can be expressed aswhere is the system nonlinear state function, is the system process noise vector, is the measurement matrix, is the measurement of white noise, and the mean value is zero. We define the system state vector aswhere denote attitude error quaternion of SINS, denote velocity error in three directions of SINS, denote the errors of latitude, longitude, and height, denote random drift of the gyro, denote constant bias of the accelerometer, and is such as
where represent different rotation matrices. The attitude, velocity, and position error equations of SINS are given by [25, 26]. The system state equation in discrete-time form can be expressed aswhere is a discrete-time nonlinear function describing the dynamics of system state and is the discrete-time process noise [27].
In this integrated system, the horizontal direction measurement is composed of the difference of heading angle, longitude, and latitude between SINS and SAR. The altitude direction measurement is composed of the difference of height between the SINS and the barometric altimeter. The system measurement equation in discrete-time form can be expressed asand take the measurement vector aswhere , and are, respectively, the heading angle and the position information for SINS; are the heading angle, latitude, and longitude position information for SAR. Due to the instability of the SINS system altitude measurement and the fact that the barometric altimeter is introduced to measure height information with , denotes the corresponding error. can be rewritten as
4.2. Simulation Analysis
Assume the initial position of the aircraft is , , altitude is , and the level flight speed is . In SINS, the error characteristics of the three gyros and accelerometers are, respectively, consistent, the constant drift of the gyro is and the random walk is , the constant error of the accelerometer is and the random walk is , the initial alignment error is , the initial velocity error is , and the initial position error is . In SAR, the image matching calculation time is , the antenna attitude angle error is , distance measurement white noise is , distance rate measurement white noise is , horizontal positioning accuracy is , the barometric altimeter error is , and the simulation time is . The flight trajectory simulation of the aircraft is shown in Figure 2.
This paper uses root mean squared errors (RMSE) of the estimation error to show the integrated navigation system filter performance of the comparison methods from a statistical perspective. The RMSE is defined aswhere M is the Monte Carlo runs. Take the estimated heading angle, velocity, and position error as an example, and the simulation results are shown in Figures 3–9, obtained by the proposed IAPF, APF, and PF algorithms.
It can be seen from the heading angle estimation errors simulation figures that the three-filter algorithms error estimation curve fluctuates significantly at the beginning of the simulation. After stabilization, the error estimation curve converges best by the IAPF algorithm and the RMSE of heading angle error is 6.218. Then, the RMSE by APF is 8.512, and the RMSE by PF is 13.570.
It can be seen from the three directions of velocity error simulation figures and Table 1 that the initial velocity errors of the three filters are relatively large, and the error curve basically tends to be stable after . The RMSE of velocity error obtained by the proposed IAPF is the smallest, and the value is 0.288, 0.218, and 0.307, respectively. APF is second, and the RMSE of velocity error value is 0.347, 0.328, and 0.357, respectively. The east velocity error curve is the most obvious fluctuation; the RMSE of velocity error value is 0.686, 0.461, and 0.375, respectively.
It can be seen from the three directions of position error simulation figures and Table 2 that the RMSE of the position error obtained by IAPF is 3.397, 4.918, and 3.829, respectively. The RMSE of APF position error is 4.185, 7.063, and 4.663, respectively, and the RMSE of PF position error is 6.044, 8.219, and 6.388, respectively. These simulation results show that the IAPF can reduce location errors and improve the resolution accuracy for the SINS/SAR integrated system.
5. Conclusion
The proposed IAPF algorithm based on APF can obtain state estimation and covariance through adaptive regulation in the sampling process. The latest measurement information is efficiently utilized to establish a better importance density distribution function so that it is closer to the real distribution function. The process of weight adaptive adjustment and random weighted calculation can keep the diversity of particles and improve the filter precision. The simulations and comparison analysis have demonstrated that the proposed IAPF has preferable estimation accuracy than APF and PF in positioning calculation. It has been conducted to comprehensively verify the effectiveness and superiority of the proposed IAPF for SINS/SAR integrated navigation system, which provides a new method for the nonlinear model filter.
Future work focuses on the improvement of the proposed algorithm to making the performance of the algorithm more perfect and how to reduce the complexity of calculation and apply this method to other integrated navigation systems.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the Ningxia Key Research and Development Program (Special Talents) under Grant (China) (Project nos. 2018BEB04003 and 2019BDE03007) and Natural Science Foundation (China) (Project no. 61863030).