The improved particle filter (PF) based on the geometric center and likelihood estimation is proposed in order to solve the problem of particle dilution and degradation. In the resampling stage, the geometric center is used to resample the particles. The particles are filtered according to the distance between the particles and the geometric center, and then the particles are resampled. The resampled particles are composed of newborn particles and non-resampled particles. The former can help alleviate the degradation problem, while the latter can keep the diversity of the particle set. In order to ensure effectiveness of the PF, the positioning error threshold of the particle filter is introduced. In the phase of particle weighting calculation, in view of the problem of low accuracy and divergence of PF state estimation caused by non-stationary and non-Gaussian noise, it adopts non-Gaussian noise parameter estimation based on likelihood to approximately estimate the measurement noise instead of the Gaussian density function. The proposed model is applied to particle weight calculation to avoid particle degradation caused by Gaussian density function approximation. The simulation results show that, after the improved algorithm, the root-mean-square error is reduced to 0.085, the variance is reduced to 0.014, and the running time is shortened by 14.8% compared with the polynomial resampling algorithm, which can effectively alleviate particle degradation and dilution in the traditional PF algorithm, and the positioning accuracy is also improved.

## I. INTRODUCTION

The particle filter (PF)^{1} is a filtering technology based on the Monte Carlo simulation principle to realize recursive Bayesian estimation. Its characteristic is that the system has no linear model and Gaussian noise requirements, it can solve the nonlinear non-Gaussian filtering problem,^{2} and the calculation results can approach the optimal estimation with an increase in the number of particles. With the rapid improvement in hardware computing power, this method has been applied in the fields of target tracking,^{3} location,^{4–6} aviation/spacecraft attitude estimation,^{7,8} fault detection, and isolation.^{9,10}

References 11 and 12 introduce EKF^{13} and UKF,^{14} respectively, to the PF framework. The core idea of these two methods is to optimize the proposal distribution to make them closer to the real posterior-PDF. In the literature,^{15} the maximum variance weight segmentation resampling algorithm is proposed, which can adaptively find the optimal weight threshold values and avoid particle diversity degradation. This method increases the accuracy and stability in motion trajectory tracking tasks. The literature^{16} puts forward a support vector regression particle filter to overcome the problems from nonlinear and non-Gaussian environments. In this paper, particle degeneracy and impoverishment problems are re-assessed using the concepts of importance region selection and particle density. The paper^{17} introduces a novel general purpose graphics processing unit implementation of the systematic and stratified resampling algorithms that exploit the monotonically increasing nature of the prefix-sum and the evolutionary nature of the particle weighting process to allow the re-indexing portion of the algorithms to occur in a two-phase, multi-threaded manner. In Ref. 18, a mutation operator is introduced to avoid falling into the local optimum and improve particle diversity. In addition, the distribution and diversity of particles in the sampling process are improved through the mutation operation. The defect of the particle filter algorithm where the particles are poor and the utilization rate is not high is also solved. The main contribution of this paper^{19} is to derive the feedback particle filter (FPF) algorithm for this problem. In the literature,^{20} the state of the art of resampling methods is reviewed. The methods were classified, and their properties are compared in the framework of the proposed classifications. Reference 21 found that, in certain circumstances, alternative resamplers can perform significantly better, and to a lesser extent, on a central processing unit (CPU) than the standard approaches. Moreover, in single precision, the standard approaches are numerically biased for upward of hundreds of thousands of particles, while the alternatives are not. This is particularly important, given greater single-than double-precision throughput on modern devices and the consequent temptation to use single precision with a greater number of particles.

The existing particle filtering algorithms almost resample all the particles completely, and this “excessive” resampling can easily lead to particle dilution. If complete resampling is replaced by partial resampling, the particles participating in resampling will help alleviate the degradation problem, and the particles not participating in resampling will help maintain the diversity of particles. Based on this idea, a partial resampling particle filter algorithm based on the geometric center is proposed to find the center of the particle set by the geometric method. According to the Euclidean distance between the particle and its center, the particle that needs resampling is determined, and the problems of dilution and degradation of particle filtering are solved. Finally, in order to ensure the effectiveness of the filtering algorithm, the filter error threshold is set; a value higher than this threshold indicates that the filtering algorithm loses its effectiveness, and re-filtering is reinitialized. In addition, in view of the non-stationary non-Gaussian noise resulting in low accuracy of state estimation and the divergent trend problem of the particle filter algorithm, it adopts non-Gaussian noise parameter estimation based on likelihood estimation to approximately estimate the measurement of noise instead of the Gaussian density function. The proposed model is applied to particle weight calculation to avoid particle degradation caused by Gaussian density function approximation.

## II. BASIC PARTICLE FILTERING ALGORITHM

The particle filtering algorithm^{1} is a Bayesian inference process based on the sequential Monte Carlo method, which is widely used in robot positioning and tracking.^{24} The basic idea is that using a set of weighted samples to represent the spatial distribution of the object position, and based on the “prediction update” cycle process to achieve location and iterative tracking of a moving object, its characteristic is similar to the system without the linear model, Gaussian noise, and the calculation results with the increase in the number of particles can approximate the optimal estimate.

The discrete time nonlinear dynamic system model^{1,20} is

where Eqs. (1) and (2) are the state transition equation and measurement equation, respectively. The state equation is used to describe the state in which the target is moving, and the measurement equation provides reference for the state estimation of the target. *x*_{t} and z_{t} are the state quantity and observed quantity at time *t*, and *v*_{t−1} and *u*_{t} are the system process noise and observation noise, respectively. The purpose of filtering is to recursively estimate *x*_{t}, according to the given initial state values *x*_{0} and the sequentially obtained quantity sequencing sequence $z1:t=z1,z2,\u2026,zt$. The particle filter predicts the prior probability model of the state through the state equation and then modifies it with the observed value to obtain the posterior probability model of the state. Thus, the optimal estimation of the system state value is obtained.

The above-mentioned filtering problem can be solved by calculating the posterior probability density function (PDF) $pxt|z1:t$ of the state variable *x*_{t} at time *t*, while $pxt|z1:t$ can be approximately expressed by a particle with a weight

where *δ*(·) is the Dirac-delta function, $xti$ represents the *i* of *N* particles at the moment of *t*, and $\omega ti$ is the corresponding weight of this particle, satisfying $\u2211i=1n\omega ti=1$.

In general, particle $xti$ is extracted from the selected importance probability density function $qxt|xt\u22121i,zt$, and the weight recursive calculation formula is

## III. PARTIAL RESAMPLING ALGORITHM BASED ON THE GEOMETRIC CENTER

The main idea of partial resampling based on the geometric center is to find the center point of the particle set by the geometric method and minimize the sum of the Euclidean distance between the center point and each particle. The distance thresholds T_{h}, T_{l}(0 < T_{l} < T_{h}) is set, and how to set them is described below. Particles can be classified into the following three categories according to their distance from the center:

Class A: particles whose distance is less than T

_{l}.Class B: particles with a distance greater than T

_{l}and less than T_{h}.Class C: particles with a distance greater than T

_{h}.

Class B particles are relatively robust and do not need resampling, so resampling only needs to be carried out for class A and class C particles, as shown in Fig. 1.

The selection of distance threshold has an important effect on the computation time of the resampling algorithm, particle diversity, and performance of the particle filter. If the distance between T_{h} and T_{l} is too small, the number of particles selected for resampling will increase, increasing the calculation time. If it is too large, the number of resampled particles will be reduced, and the performance of particle filtering will be reduced, as shown in Fig. 2, where L is the Euclidean distance between the particle farthest from the geometric center and the geometric center point.

In Fig. 2, the following three groups of T_{h} and T_{l} are shown:

Group I: T

_{h}= 0.9L and T_{l}= 0.1L.Group II: T

_{h}= 0.8L and T_{l}= 0.2L.Group III: T

_{h}= 0.7L and T_{l}= 0.3L.

The particle filter execution time and root-mean-square error (RMSE) of positioning results were compared. The comparison shows that group II should be selected: T_{h} = 0.8L and T_{l} = 0.2L.

Suppose that class A and class C particles are $xti,\omega ti$ before resampling, where $\omega ti$ is the weight of the *i*-th particle $xti$ at time t, that is, the particle weight pair $xti,\omega ti$, and the total number of class A and class C particles is set to *N*_{s}, it is easy to know that the weight of class A particles is significant while the weight of class C particles is small. In order to reduce the loss of class C particles in each resampling, we optimize the weight of class A and class C particles, that is, that taking the average value *ϖ*_{t} of class A and class C particle swarm weights $\omega ti,i=0,\u2026,NS$ and then combining *ϖ*_{t} with *ω*_{t} to get a new particle set $xti,\Psi ti$, as follows:

where *K*(1 ≤ *K* ≤ + ∞) is the proportional coefficient.

The selection of the K value will affect the filtering effect. When the K value increases, more particles with small weight will be discarded; when the K value decreases, more particles will be retained to increase the amount of information collected. How to select the K value needs to be reasonably selected according to the specific application. For example, when the terrain changes violently, increasing the K value can reduce the interference of error information; when the terrain changes gently, the K value can be reduced, more particles can be retained, and the amount of new information can be increased. In this paper, we will let K = 3.

After resampling class A and class C particles, the weight calculation formula of new particles is shown as follows:

Finally, in order to ensure the effectiveness of the particle filter, this paper introduces the particle filter positioning error threshold, which is the Euclidean distance between the particle filter positioning results and the geometric center. This paper stipulates that if the error distance between them is greater than 0.8 m, the particle filter algorithm needs to be reinitialized.

## IV. PARAMETER ESTIMATION OF NON-GAUSSIAN NOISE BASED ON LIKELIHOOD ESTIMATION

According to the definition, the mathematical expression of the likelihood function of each particle $p(zt|xti)$^{22,23} is

In general, it is assumed that the measurement noise *u*_{t} is Gaussian white noise, but in fact, the measurement noise *u*_{t} is non-stationary non-Gaussian distribution, so the assumption that the measurement noise is Gaussian white noise will lead to serious degradation of particle weight in the algorithm, resulting in large error in state estimation.

When the measurement noise is non-stationary and non-Gaussian noise, the parameter estimation of non-Gaussian noise can be obtained by maximum likelihood estimation.

Assume that the noise *α*_{t} is a non-Gaussian noise sequence, as shown in the following formula:

where $fi\alpha t$ is the probability density of the *i*th Gaussian component, *π*_{i,t}, *u*_{i,t}, and *σ*_{i,t} represent the weight, mean value, and covariance of the *i*th Gaussian component, respectively, and the *i*-th Gaussian component at time *t* is event *A*_{i,k}, *i* = 1, …, *m*.

*A*_{i,k} satisfies the following relationship:

According to the Bayes criterion, we know that

Assume that there are *N* data in the dataset and $\alpha tl$ represents the *l*th data in the dataset at time *t*. It can be seen that the *i*th Gaussian component of $\alpha tl$ [denoted as $fi\alpha tl$] is related to event *A*_{i,t},

From Eq. (9), it can be concluded that

The parameters *π*_{i,t}, *μ*_{i,t}, and *σ*_{i,t} at time *t* are obtained by log maximization of the likelihood function,

According to the maximum principle of the function, the weight *π*_{i,t} of the *i*-th Gaussian component can be obtained by the following formula:

By taking Eq. (15) into the equation, it is simplified as follows:

The same is true for *u*_{i,t},

The following equation is also solved in the same way:

In this paper, the important probability density function is generated by using a cluster of Gaussian Hermite filters (GHFs).^{25} This probability density function integrates the latest observation data into the transition probability of the system state, so it is closer to the posterior probability of the system state; the process is as follows:

- Estimation of the system state and its covariance at
*t*− 1 time: let the estimated distribution of the system state and its covariance be*x*_{t−1|t−1}and*p*_{t−1|t−1}, respectively. According to the formula $xi=pt\u22121|t\u22121Tqi+xt\u22121|t\u22121$ and the transition equation of system state, the next estimation of the system state and its covariance is as follows:(26)$xt|t\u22121=\u2211i=1nwifxi,$(27)$pt|t\u22121=Q+\u2211i=1nwifxi\u2212xt|t\u22121fxi\u2212xt|t\u22121T.$ Update of the system state and its covariance

On the basis of Eq. (1), the system state and its covariance are updated by the formula $xi=pt|t\u22121Tqi+xt|t\u22121$ and the observation model(28)$xt|t=xt|t\u22121+Ktyt\u2212zt,$Among(29)$pt|t\u22121=Q+\u2211i=1nwifxi\u2212xt|t\u22121fxi\u2212xt|t\u22121T.$(30)$zt=\u2211i=1nwihxi,$(31)$pxz=\u2211i=1nwixi\u2212xt|t\u22121hxi\u2212ztT,$(32)$pzz=\u2211i=1nwihxi\u2212zthxi\u2212ztT,$the implementation steps of the Gaussian particle filter algorithm based on the geometric center and likelihood estimation are as follows:(33)$Kt=pxzR+pzz\u22121,$The state estimation and covariance $xt|ti$ and $pt|ti$, respectively, at time

*t*are obtained by Gaussian filtering on*N*particles $xt\u22121i,wt\u22121i,i=1,2,\u2026,N$ at time*t*− 1, where $wt\u22121i$ is the weight of each particle.The prediction samples $x\u0302ti,i=1,2,\u2026,N$ and $x\u0302ti\u223cNx,xt|ti,pt|ti$ are extracted from a family of Gaussian distributions $Nx,xt|t|i,pt\Vert t|i$.

- The weight of each particle is calculated byAmong$wti=wt\u22121ipzt|x\u0302tipx\u0302ti|xt\u22121iNx,xt|ti,pt|ti.$the relevant parameters are obtained according to Eqs. (21), (24), and (25).$pzt|x\u0302ti=\u2211i=0m\pi i,t2\pi \sigma i,texp\u2212\alpha t\u2212\mu i,t2\sigma i,t2,$
The normalized weight is $w\u0304ti=wti/\u2211i=1Nwti$.

Partial resampling based on geometric center:

Based on the principle of partial resampling, class A and class C particles $xti(i=1,\u2026,Ns)$ are resampled according to their importance weight $\Psi ti$ to get the set of

*N*_{s}particles $(xti,i=1,\u2026,Ns)$, and the particle weight $\omega ti=1/Ns$ is redistributed.Output

state estimation:(34)$x\u0304t=\u2211i=1NS\omega tixti+\u2211i=Ns+1N\omega tixti.$Covariance estimation:(35)$p\u0304t=\u2211i=1NS\omega tix\u0304t\u2212xtix\u0304t\u2212xtiT+\u2211i=1N\u2212Ns\omega tix\u0304t\u2212xtix\u0304t\u2212xtiT.$If the positioning error threshold is less than 0.8 m, return to step 2; otherwise, return to step 1.

## V. SIMULATION AND ANALYSIS

### A. Simulation analysis of the partial resampling algorithm based on the geometric center

In order to verify the performance of the improved particle filter algorithm, the polynomial resampling algorithm (PR), residual resampling algorithm (RR), and hierarchical resampling algorithm (HR) were compared with the partial resampling algorithm based on the geometric center (GCPR) proposed in this paper by computer simulation as follows: The nonlinear and non-Gaussian dynamic system model in Ref. 26 was adopted,

Among them, the process noise *v*_{t−1} ∼ *Gamma*(3, 2), the observation noise *n*_{t} ∼N(0, 1*e* − 2), Φ_{1} = Φ_{3} = 0.5, Φ_{2} = 0.2, the observation time *T* = 50, the initial value *x*_{0} = 1, and the particle number *N* = 200. The simulation test of M = 100 was conducted for the above-mentioned algorithm. In order to better evaluate the performance of the algorithm, the root-mean-square error of Monte Carlo tests was defined as

where $xtk$ and $x\u0304tk$ represent the true and estimated value of the target state quantity, respectively. The accuracy and time complexity of the four resampling algorithms are simulated and analyzed in the following paragraphs.

#### 1. Accuracy analysis

The estimation accuracy of the four algorithms is analyzed. Figure 3 shows the tracking estimation diagram of the four algorithms mentioned above for the system state in an independent test. Figure 4 shows the root-mean-square error curves obtained by the four algorithms after 100 tests. Table I shows the mean and variance of the mean square error obtained after 100 tests, where the number of particles is N = 200.

. | RMSE . | |
---|---|---|

Mean . | Variance . | |

PR | 0.7856 | 0.2045 |

RR | 0.7443 | 0.1823 |

HR | 0.7097 | 0.1754 |

GCPR | 0.3856 | 0.0954 |

. | RMSE . | |
---|---|---|

Mean . | Variance . | |

PR | 0.7856 | 0.2045 |

RR | 0.7443 | 0.1823 |

HR | 0.7097 | 0.1754 |

GCPR | 0.3856 | 0.0954 |

It can be seen from Figs. 3 and 4 and Table I that the performance of the improved resampling particle filter is better than the other three resampling algorithms, which indicates that the resampling particle filter algorithm based on the geometric center is feasible for partial resampling of sampled particles, and the estimation accuracy of the improved algorithm is improved.

#### 2. Time complexity analysis

In order to objectively compare the running time of the four resampling algorithms, the following experiments are carried out in this paper. The duration of the filtering process of the four algorithms is compared on the basis of different particle numbers, as shown in Fig. 5. Table II shows the average of the elapsed times shown in Fig. 5.

. | PR . | RR . | HR . | GCPF . |
---|---|---|---|---|

Mean | 370.3500 | 365.8500 | 364.4600 | 315.8000 |

. | PR . | RR . | HR . | GCPF . |
---|---|---|---|---|

Mean | 370.3500 | 365.8500 | 364.4600 | 315.8000 |

It can be seen from Fig. 5 and Table II that the time of the improved resampled particle filter is the shortest and the time of the other three resampled particle filters is basically the same, which also shows the advantage of the improved resampled particle filter in the execution time of the algorithm.

### B. Simulation and analysis of particle filtering based on the geometric center and likelihood estimation

In order to compare the improved particle filter algorithm and improved particle filter algorithm, such as classic PF,^{1} PF-EKF,^{27,28} and PF-UKF,^{29,30} the performance of adopting type (37) nonlinear, non-Gaussian dynamic system model, the process noise *v*_{t−1} ∼ *Gamma*(3, 2), Gaussian noise component can be set to 2, according to the type (21), (22), and (25) can get the parameter estimates in Table III, Φ_{1} = Φ_{3} = 0.5, Φ_{2} = 0.2, *T* = 50 observation time, initial value *x*_{0} = 1, particle number *N* = 200. The estimated state of a single independent test is shown in Fig. 6. Figure 7 shows the root-mean-square error curve obtained by the four algorithms after 100 tests. Table IV shows the mean and variance of the mean square error obtained after 100 tests, where the number of particles is N = 200.

. | Theoretical . | N = 500 . | N = 1000 . | N = 2000 . |
---|---|---|---|---|

$\sigma 12$ | 0.02 | 0.0310 | 0.0252 | 0.0211 |

$\sigma 12$ | 0.01 | 0.0215 | 0.0153 | 0.0132 |

μ_{1} | 0 | 0.2225 | 0.2059 | 0.1500 |

μ_{2} | 0 | 0.2309 | 0.1808 | 0.1498 |

π_{1} | 0.3 | 0.35 | 0.33 | 0.31 |

π_{2} | 0.7 | 0.65 | 0.67 | 0.69 |

. | Theoretical . | N = 500 . | N = 1000 . | N = 2000 . |
---|---|---|---|---|

$\sigma 12$ | 0.02 | 0.0310 | 0.0252 | 0.0211 |

$\sigma 12$ | 0.01 | 0.0215 | 0.0153 | 0.0132 |

μ_{1} | 0 | 0.2225 | 0.2059 | 0.1500 |

μ_{2} | 0 | 0.2309 | 0.1808 | 0.1498 |

π_{1} | 0.3 | 0.35 | 0.33 | 0.31 |

π_{2} | 0.7 | 0.65 | 0.67 | 0.69 |

. | RMSE . | |
---|---|---|

Average . | Variance . | |

NEW-PF | 0.0856 | 0.0145 |

PF | 0.5054 | 0.1500 |

PF-EKF | 0.3467 | 0.0943 |

PF-UKF | 0.1345 | 0.0390 |

. | RMSE . | |
---|---|---|

Average . | Variance . | |

NEW-PF | 0.0856 | 0.0145 |

PF | 0.5054 | 0.1500 |

PF-EKF | 0.3467 | 0.0943 |

PF-UKF | 0.1345 | 0.0390 |

It can be seen from Figs. 6 and 7 and Table IV that the performance of the improved particle filter is better than the other three classical improved particle filter algorithms, which indicates that the Gaussian particle filter algorithm based on the geometric center and likelihood estimation is feasible and the estimation accuracy of the improved algorithm is improved.

## VI. CONCLUSION

A particle filter algorithm based on the geometric center and likelihood estimation is proposed to solve the problem of particle dilution and degradation. Partial particle resampling is realized by using the geometric center idea, which reduces the computation and improves the estimation accuracy. The positioning error threshold of the particle filter is introduced to ensure validity of particles. The non-Gaussian noise parameter estimation based on the likelihood estimation replaces the Gaussian density function approximate estimation of the measurement noise in the traditional algorithm, and its model is applied to the particle weight calculation to avoid the particle degradation caused by the Gaussian density function approximate estimation noise model. Through simulation, it can be concluded that the improved particle filtering algorithm presented in this paper is obviously better than PF, PF-EKF, and PF-UKF. In the next step, the algorithm proposed in this paper is applied to the field of physical aviation/spacecraft altitude estimation.

## ACKNOWLEDGMENTS

This work was supported by the National Key R&D Program of China (Grant No. 2016QY02D0305), the Key Research Program of the Chinese Academy of Sciences (Grant No. ZDRW-XH-2017-3), the National Natural Science Foundation of China (Grant Nos. 61671450 and 71621002), the Key Research Program of the Chinese Academy of Sciences (Grant No. ZDRW-XH-2017-3) and the National Key Research and Development Program of China under Grant 2020AAA0103405, the National Natural Science Foundation of China under Grants 71621002, as well as the Strategic Priority Research Program of Chinese Academy of Sciences under Grant XDA27030100.

## DATA AVAILABILITY

The data that support the findings of this study are available from the corresponding author upon reasonable request.