An adaptive particle filter based on PSO and fuzzy inference system for nonlinear state systems

ABSTRACT Particle filters have been widely used in nonlinear/non-Gaussian Bayesian state estimation problems. However, the particle filter (PF) is inconsistent over time. The inconsistency of PF mainly results from the particle depletion in resampling step and an incorrect priori knowledge of process and measurement noise. To cope with this problem and enhance the accuracy and consistency of the state estimation, an adaptive particle filter(APF) is proposed in this paper. In APF, an adaptive fuzzy square-root unscented Kalman filter (AFSRUKF) is used to generate the proposal distribution. This causes that beside the merit of reducing the computational cost, APF has some other advantages such as increasing consistency that leads to more numerical stability and better performance. Moreover,APF can work in unknown statistical noise behaviour and is more robust. This is why the fuzzy inference system (FIS) supervises the performance of square-root unscented particle filter (SRUPF) using tuning statistical noises. In APF, to increase the diversity of particles, the resampling process is done based on the particle swarm optimization (PSO). With this resampling strategy, the small-weight particles are modified to the large-weight ones without duplication and elimination of particles. The effectiveness of APF is demonstrated by using two experiment examples through Monte Carlo simulations. The simulation results demonstrate the effectiveness of the proposed method.


Introduction
Nonlinear filtering problems arise in many fields such as target tracking [1,2], robot navigation [3,4], etc. It is a process of finding a state estimate of a nonlinear system based on a set of noisy measurements [5,6]. The Kalman filter (KF) is a commonly used state estimation technique for the linear systems with Gaussian noise [6].
To extend KF to the nonlinear system with Gaussian noise, modified KFs such as extended KF (EKF) and unscented KF (UKF), have been proposed [7][8][9]. However, for the system with high nonlinearity, the poor state estimation results will be obtained [5,6].
Particles filter is as an effective estimator for the nonlinear filtering problem and can be easily applied to nonlinear/non-Gaussian dynamic models that may be difficult for other filtering techniques [5,6]. The PF is a Monte Carlo method for implementing recursive Bayesian estimation that constructs probability density function (PDF) using a set of random particles with associated weights [5,6]. In addition, in PF, the particles are evolved over time via a combination of importance sampling and resampling step. In comparison with KFs, PF can provide more accurate state estimation results, especially for the highly nonlinear systems [5].
However, there are some serious problems encountered in the general PF [8,9]. One of them is particle impoverishment. Most of the particles share a few distinct values and the posterior distribution is insufficiently approximated [9,10]. This problem can lead to the misleading state estimation results. A common way to overcome the particle impoverishment problem is selecting a good proposal distribution and resampling scheme. In general PF, the state transition is often chosen as the proposal distribution [10,11]. As this proposal distribution does not include information of the new observations, the most particles get negligible weights and it leads to particle degeneracy [12][13][14]. By selecting a good the proposal distribution, which contains the current measurement information, the particle impoverishment problem can also be alleviated.
Following this idea, the extended Kalman particle filter (EPF) is proposed in [15,16], which uses EKF to generate proposal distribution. In [17], the EKF proposal is replaced by a UKF proposal and the unscented particle filter (UPF) is proposed.
However, a serious limitation of UPF is that it requires the complete a priori knowledge of the noise statistics that in most practical applications are unknown [18,19]. Evidences have shown how a poor estimation of the input noise statistics may seriously degrade the performance of UKF and even induce the divergence of it [20][21][22]. In addition, in UPF, an inherent rounding error in the numerical calculation may cause negative definiteness of the state covariance, which will lead to filter divergence and influence the estimation precision [23][24][25]. Finally, the resampling step in UPF leads to a great loss of diversity in particles [26,27].
In this paper, APF is proposed to overcome these problems. The proposed method uses AFSRUKF instead of UKF to generate the proposal distribution. Beside the merit of reducing the computational cost, APF has some other advantages such as increasing consistency that leads to more numerical stability and better performance. This is mainly because of the fact that all resulting covariance matrices are guaranteed to remain positive semi-definite. In addition, APF can work in unknown statistical noise behaviour and thus it is more robust. The fuzzy inference system (FIS) supervises the performance of SRUPF with tuning statistical noises (R t and Q t ) to close theoretical covariance to actual covariance. This adaptive tuning provides more robustness and consistency for the filter, which leads to results that are more accurate.
In order to reduce the impact of resampling on the accuracy and consistency, the particle swarm optimization (PSO)-based resampling scheme is used in APF. In the PSO resampling scheme, the particles recombine by using an iterative process and selection and unlike the traditional resampling schemes; there is no duplication and elimination of particles. The new resampling algorithm can maintain the diversity of particles, and ensure the resampled particles symptotically approximate the samples PDF of the true state.
The rest of the paper is organized as follows. In Section 2, the required background is reviewed. The adaptive particle filter is presented in Section 3. In Section 4, the results are compared and analysed with simulation results.

Particle filter
Consider the following nonlinear state space model: where f (.),h(.)are known nonlinear functions with appropriate dimensions,x t ∈ R n is the state vector, y t ∈ R m is the measurement vector, w t−1 , v t are independent white-noise variables. The objective of filtering is to estimate the posterior density of the states given the past measurements p(x t |y 1:t ) [5,6]. In PF, the posterior density can be calculated by a set of weighted particles drawn as follows: where δ( is Dirac's delta function, and N is the number of particles. Unfortunately, the p(x t |y 1:t ) is often unknown, so actually, it is impossible to sample directly from the p(x t |y 1:t ). Aiming to the problem, PF circumvents the difficulty by sampling from known, easy-to-sample, the proposal distribution q(x t |y 1:t ) which should be as far as possible close to p(x t |y 1:t ). The associated importance weight of the particle is defined as follows [5]: . ( The resampling step is performed to avoid degeneracy in the particle filter.

Particle swarm optimization
The particle swarm algorithm is a population-based metaheuristic optimization method that is based on the simulation of the social behaviour of birds within a flock [30,31]. In PSO, the potential solutions of the optimization problem that are called particles, move in the search space with a velocity, which is adjusted as a function of the position of other particles. All the particles follow the best particle during the iterations and converge together to the global optimum of the considered objective function. Suppose that the search space dimension is D, and the number of particles is NP, the position and velocity of the i-th particle are represented by denotes the best position that particle i is achieved so far, and P g denote the best position of P bi for any i = 1, . . . , NP. The PSO algorithm may be performed by the following formulations [30,31]: where k represents the iteration number and c 1 , c 2 are positive coefficients. Usually c 1 = c 2 = 2, and r 1 , r 2 are random numbers in the interval (0, 1), w is the inertial weight.

Adaptive particle filter
In this section, the adaptive particle filter is presented to overcome the drawbacks of PF. The proposed algorithm consists of sampling, importance weighting and PSO resampling. In the following subsections, details of these steps are given.

Sampling
The choice of proposal distribution is one of the most critical issues in the design of a particle filter [5,6]. In APF, the proposal distribution is as follows: where N( t|t ) obtained by using SRUKF. The SRUKF is based on the idea that the square root of a matrix has a better condition number than that of the original number and its use reduces numerical errors [24,25]. In the proposed method, the squareroot matrix S t|t of P t|t is updated, whereP t|t = S t|t S T t|t . In addition, the state update and Kalman gain equation use S t|t instead ofP t|t . This is particularly useful since the unscented transform (UT) uses the square root S t|t of P t|t and propagating S t|t eliminates the Cholesky factorization. Let x t−1|t−1 be the estimated state and S t−1|t−1 be the square root of P t−1|t−1 based on the available measurements up to current time instant t − 1. To draw samples from above proposal distribution, a set of 2n + 1 points is generated by where (S (i) t−1|t−1 ) i means the i-th column of the square root of the covariance matrix. The sigma propagated through the nonlinear system and measurement models as t|t−1 of the state are as follows: x (i) where the weights ω m and ω c are as , Finally, the state mean x (i) t|t and square root of the covariance matrix S (i) t|t are updated as where S y t is square and triangular, h(.) is the observation model and K (i) t is the gain matrix: and cross covariance P (i) δυ is as follows: From the Gaussian distribution generated by the estimated mean and covariance of the vehicle, the state of each particle is sampled as where the covariance matrix of the robot pose is as follows: From the above equations, it can be seen that the covariance matricesR t and Q t will affect the precision of the SRUPF directly. In practical systems, the priori knowledge of the process noise and measurement noise is unknown. This is why it is hard to determine process noise and measurement noise covariances. The use of wrong priori statistics in SRUPF may lead to large estimation errors or even to the divergence. So the accuracy of R t and Q t has become the key problem which affects the precision and stability of the filter and needs to be tuned online. In this paper, an online adaptive scheme of SRUPF is presented. The adaptation is in the sense of adaptively adjusting the process noise and measurement noise covariances using the covariance-matching technique.
The basic idea behind the covariance-matching technique is to make the actual value of the covariance of the innovation sequence match its theoretical value [20][21][22]. If the theoretical covariance and its actual covariance are discrepant, FIS will adjust R t and Q t online in order to reduce the discrepancy. The adjusting factor is generated by FIS based on the degree of the discrepancy. Assuming the noise covariance matrix Q t is known, the adaptive adjustment of the matrix R t is done. For this purpose, the innovation sequence denotes as follows: From the measurement equation of UKF, the theoretical covariance of υ t is computed by The actual covariance of υ t can be approximated as where m is the width of the moving window and is chosen empirically to give some statistical smoothing, υ is the residual vector. If the window size is too small, the measurement estimation covariance can be noisy. In general, the window size is chosen empirically for statistical smoothing [21][22]. It should be pointed out that too large a window width suffers from the severe computational inefficiency and too small a window width might lead to large variance. Thus, it is important to choose a reasonable window width. If a discrepancy is found between the actual value and the theoretical value, then R t is tuned based on the knowledge of the degree of the discrepancy using FIS. The tuning process is shown in Figure 1. Define the discrepancy between the actual covariance C t and the theoretical covariance S t as DOM t (Degree of Matching) as follows: If the actual covariance is less than its theoretical then the value of R t should be increased. If the actual value of covariance C t is greater than its theoretical value S t , then the value of R t should be decreased. Thus, the DOM t can be used to reduce the discrepancy between S t and C t . To implement the above covariancematching technique using FIS, the general rules of adaptation are as follows: Adjustment of R t is performed using FIS is as the following relation: Figure 1 shows tuning SRUPF using FIS where R t is the FIS output, DOM t is the FIS input. The membership function for DOM t and R t are shown in Figures  2 and 3.

Importance weighting
The importance weight of each particle is computed through Equation (21) when the observation at time t is obtained:  then, the particle weight is normalized as follows:

PSO resampling
It is known that the objective of resampling steps is to eliminate particle degeneracy problem [26][27][28]. The idea of resampling is that particles with large weights are duplicated while those with small weights are abandoned. The common resampling techniques are residual, multinomial, systematic and stratified [27][28][29]. These algorithms use weight strata to decide how many copies of each particle should be made. Although these techniques have good performance, they inevitably lead to the loss of diversity among the particles. In this paper, in order to restrain losing diversity, an effective resampling scheme based on the PSO (PSO resampling) is introduced. The PSO resampling is performed when the effective number of samples reduces below a threshold N eff , which is calculated as In the PSO resampling scheme, the particles {x (i) t i = 1, . . . , N} are regarded as the target vectors of the current population, and the corresponding weights {ω (i) t ; i = 1, . . . , N} are as the objective functions of the target vectors, respectively. Unlike the traditional resampling schemes, there is no elimination and replication of particles. In fact, the PSO resampling recombines the particles by using an iterative process. Let t,k }denote the current population of PSO, which consists of N D-dimensional target vectors. where x k t,i represents each candidate solution i to the optimization problem at iteration k for a given time step t.
The PSO algorithm updates the velocity and position of each particle as follows [30]: The fitness function of x t,k is calculated as PSO moves all particles towards the particle with the best fitness function. When the best fitness value reaches a certain threshold, the iteration is stopped. At the end of iterations, P gbest and P pbest are calculated and the particles are distributed according to the values of P gbest and P pbest . With this set of particles, the sampling process will be conducted on the basis of proposal distribution. In summary, a single iteration of the Adaptive particle filter is given in Figure 4.

Results
In order to verify the performance of APF, the performance of it is evaluated and compared with UPF, PF, MCMC-based particle filtering and EPF on simulated data under different condition. Two examples are investigated in this section. The first example is a univariate growth model, which is popular in econometrics and has been used previously in [11]. The second example is robot localization, which is of interest in robot applications.

Univariate growth model
This model is a benchmark model that is commonly used in PF testing. This system has been used before in many papers [5][6]. The state space model of this system  is as follows: where the initial state is x0 = 0.1, the covariance of the measurement noise is R t = 1, and the covariance of the process noise is Q t−1 = 1. Fifty particles (i.e. N = 50) are used in all algorithms. At first, the performance of the proposed method is compared with UPF, EPF, PF and MCMC-based particle filtering when the statistical properties of noises are known a priori. In Figure 5, the tracking performance of the algorithms is depicted. It can be observed that the best state estimation results belong to the proposed method.
To evaluate the estimations accuracy, the root mean square error (RMSE) is used to evaluate each algorithm. The RMSE is obtained over 100 runs. The RMSE of estimations over time is shown in Figure 6. It can be seen that APF provides a more accurate estimation than the EPF and UPF. The reason is that PSO resampling can obviously increase the number of meaningful particles and enhance the performance of estimation. As result, it can enrich the particle species and capture the distribution more comprehensively and accurately. The number of distinct particles is shown in Figure 7. As can be seen,  the number of distinct particles of the proposed method is more than that of other algorithms. As a result, the consistency of APF is increased.
To examine the sensitivity of the performance of algorithms with the number of particles, three different particle numbers of 100, 70, 50, 30, and 10, are used for simulation tests. Table 1 shows the performance of algorithms with the number of particles. As can be seen, in general, when the more particles are used, the estimation accuracy of the algorithms is higher. However, the APF gives the least averaged variance with different particles. The Table 1 shows also the computational cost of algorithms. As can be observed, the smallest computation time belongs to EPF. However, the APF requires less number of particles to obtain the same estimation accuracy compare to EPF and UPF.
Second, the performance of algorithms is evaluated when the statistical properties of noises are unknown. In all algorithms, the number of particles is 50. The results are shown in Figures 8 and 9. Compared with the proposed method, the state estimate of UPF, MCMC, EPF and PF deviated from the true state. From a comparison of Figures 6 and 9, it is easy to conclude that the performance of APF, in this case, is almost close to the previous case, while the performance of other methods is worse than that of them in the previous case. The reason is that the APF can estimate the unknown measurement noises online whereas other method depends on the fixed prior knowledge about the measurement noises.

Robot localization
In this example, the performance of the proposed algorithm in robot localization is studied. The robot localization is one of many issues in mobile robot study. Localization is an essential ability for a mobile robot to determine its pose (location and orientation angle) from sensor data so that it can plan a movement and go to the desired location. Suppose the kinematic model of the mobile robot is as follows: where (x, y) are the location robot, φ is the orientation robot respectively to the global environment, t is the time interval,Bis the base line of the robot, u = V γ T is the control input at time tconsisting of a velocity input Vand a steer inputγ ,v = v v v γ T is the process noise. The robot is assumed to be equipped with a range-bearing sensor that provides a measurement of range r i and bearing θ i to an observed feature ρ i relative to the robot. The observation z of feature ρ i in the map can be expressed as [32]  where (x i , y i ) is the landmark position in the map, and ω = [ ω r ω θ ] T is measurement noise. The robot localization based on the proposed algorithm is tested for experiment environment as shown in Figure 10. The star points (*) depicts the location of the landmarks that are known and stationary in the environment and the blue line is the path of the robot. The initial position of the robot is assumed to be (0, 0, 0) and the robot moves at a speed of 3 m/s and with a maximum steering angle of 30 o . The robot has 4 m wheel base and is equipped with a range-bearing sensor with a maximum range of 30 m and 180 o frontal field-of-view. The control noise is σ v = 0.3m/s, σ γ = 3 o and the measurement noise is 0.1m in range and 0.1 o in bearing. A control frequency is 40Hz and observation scans are obtained at 5Hz. Figures 11-14 show the comparison of the RMSE of the localization based on proposed algorithm and UPF when the noise statistics are known and unknown to both algorithms. The results are obtained over 100 Monte Carlo runs with 20 particles.
As can be seen, the localization based on APF outperforms all the other counterparts in both the robot position and heading errors. This is because the localization based on APF not only improves the numerical stability but also reduce the negative effects caused by the resampling step. In addition, APF can estimate the unknown measurement noises online.

Experimental
The robot localization on the proposed algorithm is compared with localization based on EPF using the car park data set. The environment of this test is the top level of the car park building of the university campus. The full data set and the documentation is available at [33]. A standard utility vehicle is a four-wheeled vehicle equipped with Sick laser range and bearing sensor, linear variable differential transformer sensor for the steering, back wheel velocity encoder and GPS as shown in Figure 15.
In this experiment, artificial landmarks are used that consisted of 60 mm steel poles covered with reflective tape. The true position of the landmarks is also obtained with GPS and is known. In addition, GPS is used to provide accuracy ground truth that is used to evaluate the estimated path of the robot. When the vehicle was driven around the park, the velocity and steering angle is measured using encoders, but uneven terrain induced additional non-systematic errors because of wheel slippage and vehicle attitude. Hence, the trajectory of dead reckoning deviated from the ground truth provided by GPS as shown in Figure 16.
The trajectory is estimated by EPF and the proposed method is shown in Figure 17. As can be seen, the path errors of localization based on proposed method is lower than that of localization based on EPF,    because the result of the localization based on proposed algorithm better matches with the GPS ground truth. Figure 18 shows the RMSE of the robot position. The results show that the performance of the proposed algorithm is better than that of EPF. The proposed algorithm starts with a wrongly known statistics noise and then adapts them through FIS and attempts to minimize the mismatch between the theoretical and actual values of the innovation sequence. Figures 14 and 15 show the comparison between the proposed algorithm and the EPF. This is because that the proposed method tunes Q t andR t adaptively and diversity particles are more than that of EPF. This improves data association, estimation accuracy and consistency.

Conclusion
The PF is a novel technique that has sufficiently good estimation results for the nonlinear/non-Gaussian systems. However, the general PF is inconsistent, what is caused mainly by unknown a priori knowledge of the noise statistics and loss of particle diversity in resampling step. The soft computing (PSO and FIS) may be used in order to overcome particle filtering weaknesses. In this paper, APF that takes advantage of soft computing in order to overcome the problems of PF is presented. The APF uses AFSRUKF to generate the proposal distribution, in which the covariance of the measurement and process of the state are adjusted online by predicted residual as an adaptive factor based on a covariance-matching technique using FIS. As a result, the proposed method can work in unknown statistical noise and does not require a priori knowledge about the system. In addition, to increase diversity, the resampling process is done based on PSO. The validity of the proposed method is evaluated against two distinct examples: the univariate growth model and robot localization. By simulation analysis and evaluation, it is verified that applying APF would contribute to significant estimation performance improvements.

Disclosure statement
No potential conflict of interest was reported by the author.