An Improved Gaussian Particle Filter Algorithm Using KLD-Sampling

，，，

College of Automation Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 211106，P.R.China

（Received 26 June 2019；revised 27 December 2019；accepted 25 July 2020）

Abstract: To adjust the samples of filtering adaptively，an improved Gaussian particle filter algorithm based on Kullback-Leibler divergence（KLD）-sampling（KLGPF）is proposed in this paper. During the process of sampling，the algorithm calculates the KLD to adjust the size of the particle set between the discrete probability density function of particles and the true posterior probability density function. KLGPF has significant effect when the noise obeys Gaussian distribution and the statistical characteristics of noise change abruptly. Simulation results show that KLGPF could maintain a good estimation effect when the noise statistics changes abruptly. Compared with the particle filter algorithm using KLD-sampling（KLPF），the speed of KLGPF increases by 28% under the same conditions.

Key words:particle filter；Gaussian particle filter；KLD-sampling；noise mutation；adaptive particle numbers

0 Introduction

Nonlinear filtering problems may occur in many fields，including target tracking［1］，strapdown inertial navigation system（SINS），and attitude estimation［2］. The particle filter（PF） algorithm proposed by Doucet is a filtering algorithm based on the Monte Carlo method，whose dimension，confidence，sampling and many other issues have received extensive attention and research［3-4］.The resampling strategy in PF algorithm will directly affect the performance of filtering. Based on the framework of PF，Gaussian particle filter（GPF）［5］uses the Gaussian distribution to approximate the prior and posterior distribution of the state，which is a kind of resampling-free filtering algorithm and has better real-time performance than PF［6］.

In fact，the noise of the system is not constant all the time［7］. For example，in attitude estimation，the noise of gyroscope is affected by air pressure and temperature. Similarly，in speech recognition［8］，it is affected by the surrounding environment. In such cases，GPF and PF frequently have divergence due to the sudden change of noise. Over the past years，adaptive particle filters which use the Kullback-Leibler divergence（KLD）-sampling have been applied in many areas［9-11］. Hereinafter，the method is called KLPF. However，when it is applied to other occasions，due to the resampling of PF，the realtime performance of KLPF is poor.

Aiming at solving the above problem，an improved Gaussian particle filter algorithm named KLGPF is proposed using KLD-sampling in this paper. By introducing the KLD-sampling strategy，the algorithm calculates the KLD between the discrete probability density function and the true posterior probability density function，which is represented by the particle to adapt to the number of samples［12］.In contrast to KLPF，the proposed algorithm is integrated into GPF so that the real-time performance has great improvement.

The remainder of this paper is organized as fol-

lows. In the next section，the background information about KLGPF is reviewed. In Section 2，the improved Gaussian particle filtering using KLDsampling is introduced. Simulation results are presented in Section 3 and the conclusions are drawn in Section 4.

1 Background

1.1 Gaussian particle filter

For the abovementioned nonlinear filtering problem，most dynamic state space（DSS）model with such problems can be written as［13］

wherexnandynare state and observational variables，respectively；andunandvnwhite noises. The nonlinear functions of the system are represented byf(·) andh(·).nis a timestamp depicting system time.

The PF method is to getMparticles from the importance probability densityπ(·). Samples{xin}M

i=1can be used to describe the importance probability density functionπ(·) of the statexnatntime［14］.

Fromw(i)=(p(xin)/π(xi)n)，the weighted value of the particles can be obtained，wherep(xn) is a posterior distribution. The posterior distribution can be represented by a sample set{x，W}.

The estimate of statexncan be calculated as

It is obtained from the strong law of large numbers that(xn) →Ep(xn) withM→∞. Then the approximation of the posterior probability density can be written as

whereδ(·)is a Dirac delta function.

Assume that the distribution of statexnat initial time isN (·) is a Gaussian distribution that can be expressed as

whereandare decided by prior information.

In general，GPF is divided into two processes including measurement update and time update，which will be introduced in the following section.

1.1.1 Measurement update

Then，the weights are normalized as Eq.（7）to ensure the correctness of the weighted sum.

Finally，the posterior distribution of the statexnis approximated to the Gaussian distribution，and the mean and variance of the Gaussian distribution are calculated as

1.1.2 Time update

Update the state of each particle of setto get the updated particle，which is obtained from the posterior probability distribution N (xn；μn，Σn).The predicted probability density function is approximated to the Gaussian distribution，and the mean and variance of the Gaussian distribution are calculated as

1.2 KLD-sampling

The KLD-sampling method keeps the KLD between two probability density functions under a threshold，where KLD is defined as

Therefore，the posterior distribution is first viewed as a discrete piecewise function. From this discrete distribution，Msamples= (x1，x2，…，xk)are obtained，which falls into a different interval.

Obviously，conforms to multinomial distribution that is denoted as～PN(M)，where=p1，p2，…，pkis the corresponding probability for each bins. The maximum likelihood estimate ofpiisi=1，2，…，k.

Substitute the maximum likelihood estimation probability and the real posterior distribution into the KLD equation as

ForM→∞，Eq.（11）can be written as

After performing interval estimation，the particle sizeMcan be computed by

A good approximation was provided by the Wilson-Hilferty transformation［15］，which yields

wherez1-δis the upper 1-δquantile of the standard normal distribution andethe threshold of the KLD.

2 Gaussian Particle Filter Using KLD-Sampling

Under the framework of GPF，the KLGPF obtains particles when the time update process is conducted. KLGPF sets an interval range by introducing Mahalanobis distance，which includes almost the range of the prior distribution of each state at any time. Then，the interval is divided into several subsections. The prior distribution is selected as an importance probability density function，from which the particles are extracted. Finally，KLGPF counts the number of particles falling into different intervals，so as to calculate the number of particles needed in real time and adjust them accordingly.

2.1 Setting the range of interval

As the mentioned above，the Mahalanobis distance is used to set the range of the interval. The Mahalanobis distance between the sample and the ensemble is defined as

Set the boundary value asxi(max)=μ±aBb，and the probability of the sample that falls within the boundary at any time is 1-θ，whereais unknown quantity andΣ=BBT.μandΣare the mean and variance of the ensemble.Then

Sincexconforms to normal distribution，Eq.（16）can be written as

wherebis a vector of those with the same dimension asB，a chi-square distribution withγdegrees of freedom，andthe 1-θquantile of the chi-square distribution. From Eq.（17），it is obtained that

According to the rightmost term of Eq.（18），agradually decreases with the increase ofγand thus gets a maximum value ofwhenγ=1. To sum up，the range of interval can be obtained by substituting the boundary value into Eq.（15），which can be written as

Therefore，a small enoughθvalue needs to be set to ensure that the probability of KLD-sampling particles that falls into intervallarge enough，whereγis set to the dimension of the state vector.

2.2 Counting the number of subintervals into which particles fall

According to the selected interval range in Section 2.1，it is evenly divided intomsubintervals and the size isas shown in Fig.1.

Fig.1 Interval segmentation method

An interval mapping table is presented in Table 1.

Table 1 Interval mapping table

Then an interval mapping function is built by using Eq.（15）as

where [·]flooris a floor function，and index is an index of subintervals. A detailed description of the KLGPF algorithm is given as follows.

The implementation of KLGPF:

Step 1Initialize the particle setbased on the prior information.

Measurement update:

Step 2Compute the weights byand normalize the respective weights by

Step 3Estimate the mean and variance at the current time

Time update:

Step 4Get particlesf rom the posterior distribution N (xn；μn，Σn).

Step 5Obtain the updated particle setby using the process equation to update the state of each particle.

Step 6Calculate the mean and variance of the updated particle set by

KLD-sampling:

Step 7Set the interval range asaccording to the state dimension and divide the interval intomsubintervals.

Step 8

（1）Draw a particle from the importance probability density function andM=M+1，whereMdescribes the number of particles. At the same time，calculate the Mahalanobis distance between particles and the ensemble.

（2）Iffall into an empty subinterval，then add one to the non-empty subinterval，i.e.，k=k+1.

3 Simulation Results

In this paper，KLGPF，KLPF，GPF，and PF algorithms are separately applied to one-dimensional nonlinear model for numerical simulation. The model is a basic univariate nonstationary growth model（UNGM），which is a strongly nonlinear model. It can be described by DSS equation［16］as

wheren=1，2，…，N，Qn～N (0，) andRn～N (0，)are process and measurement noises，x0=0 ，andN=100 are specified. The simulation step size is set as 1.Some key parameters in KLPF are set，i.e.，e=0.15，Δ=0.2，andδ=0.99. The parameters of KLGPF are set thate=0.15，δ=0.99，θ=1×10-9，and the subinterval numbersm=[20Σn]ceil. The simulation parameters are set based on Ref.10 whereΣnis a variance of the prior distribution atntime and[·]ceilis a ceiling function.The statistical property of process noise and measurement noise are specified by=1，=1 under normal conditions. The prior information of the initial state of the filter isp(x0) ～N (3，32)，and the particles of PF and GPF are the average number of KLGPF particles. In some abnormal cases，however，these noises will mutate. Therefore，it is firstly assumed that the system is under normal conditions，and then the noise is suddenly changed at some span. Fig.2 shows the true states and the estimates obtained using GPF and PF with fixed particle numbers and the estimates obtained using KLGPF and KLPF with adaptive particle numbers under normal conditions. It can be seen that the estimation of KLGPF is not much different from that of GPF，which uses the framework of GPF，and so are the KLPF and PF. The particles used by these algorithms are plotted in Fig.3. FIR result is used to display the outline information of the change in the number of particles of KLGPF. The changes in particle number of KLGPF and KLPF should also be noticed that the number of KLGPF particles changes with a priori probability，whereas that of KLPF changes with a posteriori probability. Then，KLGPF introduces the Mahalanobis distance and the size of the interval is automatically set，while KLPF does not. The time elapsed by using these algorithms are shown in Table 2. KLGPF inherits the characteristics of GPF that eliminates the process of resampling， whose complexity isO(N). Since KLGPF is an algorithm that adjusts the number of the particle online in real time，the computation time increases a little compared to GPF.

Fig.2 True state value and state estimation of KLGPF,GPF, PF, and KLPF under normal conditions

Fig.3 Changes in particle number of KLGPF, KLPF,GPF, and PF under normal conditions

Table 2 Computation time of KLGPF, KLPF, GPF, and PF under the same simulation condition

Fig.4 True state value and state estimation of KLGPF,GPF, PF, and KLPF under abnormal conditions

Fig.4 presents the estimation results of the four approaches. The process noise is mutated to 500Qnat time 20―30 and time 50―60 mutation period，which is 500 times that of normal conditions. As shown in Fig.4，KLGPF could remain stable at the mutation period，which is the same with KLPF.Fig.5 further illustrates this point that the errors of GPF and PF both exceed their 3σerrorline at mutation period. The change of particle number is shown in Fig.6. It is obvious that the number of particles changes significantly during the mutation period，and the number of particles used by KLGPF is more than that of KLPF. However，since KLGPF is within the framework of GPF，the computation time for KLGPF is less than KLPF，as shown in Table 3.

The comparison of the root mean square error（RMSE）value of the four algorithms is presented in Fig.7，in which the RMSE value of KLGPF is lower than those of GPF and PF. However，the performance of KLGPF in terms of RMSE value is slightly inferior to that of KLPF. This is because the RMSE value for GPF is marginally higher than that of PF. When the process noise mutates to 1 000Qnat the mutation period，both GPF and PF are divergent so that the filter could not be carried out，as shown in Fig.8. In Table 4，the concrete average RMSE value is listed for 500Qnprocess noise mutation and 1 000Qnprocess noise mutation. Similarly，the computation time is presented in Table 3.

Fig.5 Estimation error and 3σ line of KLGPF, KLPF,GPF, and PF under abnormal conditions (500Qn)

Fig.6 Changes in particle numbers of KLGPF, KLPF,GPF, and PF under abnormal conditions (500Qn)

Fig.7 RMSE values of KLGPF, KLPF, GPF, and PF under abnormal conditions (500Qn)

Fig.8 RMSE values of KLGPF, KLPF, GPF, and PF under abnormal conditions (1 000Qn)

Table 3 Average computation time of 100 random realizations under different abnormal conditions

Table 4 Average RMSE value of 100 random realizations under different abnormal conditions

From the above information，it can be concluded that the KLGPF greatly improves the filtering speed while slightly losting its algorithm accuracy compared with KLPF. In Fig.9，the noise of mutation is gradually increased. KLGPF and KLPF both maintain good estimates，while the divergence frequently occurs for GPF and PF when the noise mutation value exceeds 700Qn.

Fig.9 RMSE values of KLGPF, KLPF, GPF, and PF

4 Conclusions

KLGPF is proposed in this paper to cope with the divergence problems in the case of system noise changes. On one hand，the algorithm which combines GPF with KLD-sampling can adjust the size of particle sets in real time during the span of system noise changes. Therefore，the KLGPF can remain stable when the system is subjected to strong interference and the process noise experiences sudden changes.

On the other hand，the speed of KLGPF is much faster than that of KLPF，albeit the RMSE value of KLGPF is marginally higher. This is because the improved algorithm has the feature of GPF that it does not require the systematic resampling procedure with complexityO（N）. The predictive distribution is approximated by Gaussian distribution in KLGPF，which results in higher RMSE value than KLPF. Moreover，KLGPF introduces the Mahalanobis distance to set the interval that is independent of the original data unit，so the KLGPF can be easily combined with other applications.