# Moving Target Tracking Algorithm Based on Improved Resampling Particle Filter in UWB Environment

Authors: Zhihao Li; Junkang Wu; Zhenwu Kuang; Zuqiong Zhang; Shenglan Zhang; Luxi Dong; Lieping Zhang
Journal: Wireless Communications and Mobile Computing
 (2022)

---

## Abstract

In this paper, a moving target tracking (MTT) algorithm based on the improved resampling particle filter (IRPF) was put forward for the reduced accuracy of particle filter (PF) due to the lack of particle diversity resulting from traditional resampling methods. In this algorithm, the influences of the likelihood probability distribution of particles on the PF accuracy were firstly analyzed to stratify the adaptive regions of particles, and a particle diversity measurement index based on stratification was proposed. After that, a threshold was set for the particle diversity after resampling. If the particle diversity failed to reach the set threshold, all new particles would be subjected to a Gaussian random walk in a preset variance matrix to improve the particle diversity. Finally, the performance of related algorithms was tested in both simulation environment and actual indoor ultrawideband (UWB) nonline-of-sight (NLOS) environment. The experimental results revealed that the nonlinear target state estimation accuracy was maximally and minimally improved by 12.83% and 1.97%, respectively, in the simulation environment, and the root mean square error (RMSE) of MTT was reduced from 17.131 cm to 10.471 cm in actual UWB NLOS environment, indicating that the IRPF algorithm can enhance the target estimation accuracy and state tracking capability, manifesting better filter performance.


---

## Body

## 1. Introduction

Recently, the demands for indoor positioning and target tracking technologies have been increasing with continuous development of the Internet of Things and smart cities. Wireless technologies currently used for indoor positioning mainly include wireless fidelity (WiFi), Bluetooth, radio frequency identification (RFID), ZigBee, and ultrawideband (UWB) [1]. Compared with other indoor positioning and tracking technologies, the UWB-based positioning and tracking technology enjoys centimeter-level positioning accuracy, good anti-multipath and anti-interference abilities, and strong penetrability, which is more conducive to the collection of dynamic data as well as the real-time positioning and tracking of moving objects in complex environments. Therefore, UWB has broad application prospects in mobile target positioning and tracking in complex indoor environments [2].The filtering algorithm for moving target tracking (MTT), an essential part of MTT, directly determines the accuracy and time complexity of MTT. Hence, the filtering algorithm becomes the emphasis and difficulty in the target tracking system. Besides, in contrast with Kalman filter (KF) based on deterministic sampling and its extended filtering algorithms [3], particle filter (PF) based on random sample can locate the target with initial position unknown and handle the problem of target tracking state estimation of nonlinear and non-Gaussian systems in a better way [4, 5]. PF is widely used in indoor target location and tracking and robot navigation because it can effectively estimate the motion state of indoor moving targets. In the resampling stage, using curve fitting to predict the particle weight trend improves the PF algorithm [6]. The algorithm is applied to the visual sensor to improve indoor robot positioning and navigation accuracy and robustness. A PF-based indoor positioning system is implemented by Shen et al. [7]. Firstly, the system smooth received signal strength indicator (RSSI) data through KF and then resamples particles through a random general resampling method to improve the positioning accuracy of the PF algorithm for Bluetooth tags. An indoor fusion location algorithm of Bluetooth technology and pedestrian dead reckoning (PDR) based on a particle filter with dynamic adjustment of weights calculation strategy (BPDW) is proposed in the literature [8]. BPDW algorithm first solves the problem of RSSI fluctuation. It adds the strategy of adaptively adjusting particle weight according to the target motion state to the PF to improve the location accuracy of PDR and Bluetooth fusion location. A PDR and WiFi fingerprint fusion location algorithm is proposed by Qian and Chen [9]. Firstly, the location area is divided, and the fingerprint matching of the subdatabase is carried out by a machine learning method to improve the accuracy of WiFi fingerprint location. Then, the degradation problem and insufficient particle diversity of the PF algorithm are enhanced by the clonal selection algorithm. The experimental results show that compared with the traditional PF algorithm and independent algorithm, the positioning accuracy and speed of the proposed algorithm are significantly improved.The basic PF algorithm shares a common problem of particle degradation [10]. In this regard, Gordon et al. propose a resampling method to solve the particle degradation in PF algorithm [11]. However, the traditional resampling methods may lead to a significant decline in particle diversity while tackling the problem of particle degradation by copying particles with larger weights. In addition, the lack of particle diversity can reduce the accuracy and stability of target tracking and may even result in filter divergence and tracking failure [12]. There are two mainstream directions to solve the problems in traditional resampling methods: improving traditional resampling methods and introducing intelligent algorithms to optimize the PF algorithm. In terms of improving the traditional resampling methods, the random resampling algorithm is improved in the literature [13], that is, the particles with the largest global weight are found by resampling when the effective particle number is smaller than a certain threshold, and then, they are replicated to a new particle set, thus improving the particle diversity. Generating new particles by Gaussian expansion around high-weight particles to enrich the particle diversity of the PF is introduced in the literature [14]. According to the literature [15], the degradation coefficient of the threshold resampling is put forward, and the features of posterior probability density of low-weight particles are embedded into replicated high-weight particles in the resampling process to improve the diversity of the particle set. In the literature [16], improvement is made to residual resampling. Specifically, the total number of particle replication is accumulated to ensure the same total number of particles before and after the residual resampling, thus avoiding second resampling and improving the operation efficiency of the PF algorithm. In the literature [17], a hybrid resampling method is proposed by combining multinomial resampling (MR) and residual resampling (RR). By judging the particle degradation degree of the current state, MR is selected when the particle degradation degree is high, and RR is selected when the particle degradation degree is low. The experimental results show that the accuracy of hybrid resampling (HR) in crack detection is better than MR and RR. As shown in the literature [18], a spline resampling PF algorithm has been proposed by combining the Bezier spline and resampling method. Firstly, higher tracking accuracy is obtained through the spline change of particle weight, and then, the diffusion transformation of particle state is introduced into the resampling algorithm; that is, the new child particles are generated around the parent particles at the last time to improve the diversity of particles. Recently, a resampling algorithm based on maximum variance weight division has been reported in the literature [19]. By calculating the median value of the weight value of all particles, the particles are divided, and the average weight difference between the two groups of particles is calculated. Deciding whether to resample the median value of particle weight can balance PF’s accuracy and time complexity. Additionally, partial stratified resampling is improved in the literature [20], where different stratified sampling strategies are proposed for particles with different weights, which can improve the particle diversity and ensure the approximately same probability density distribution before and after resampling at the same time. Moreover, a PF tracking algorithm based on the error ellipse resampling is put forward in the literature [21]. An error ellipse is constructed in accordance with the error covariance matrix of particles during resampling, based on which the particles are screened and optimized. A PF algorithm with significant local resampling is put forward in the literature [22], which uses a weight threshold and a distance threshold to extract significant local particles for MTT and to improve the accuracy of PF in the case of small particles. At the same time, the particle diversity is improved by adding Gaussian noise for particle roughening. As shown in the literature [23], the mean-shift method is introduced into the resampling process of the PF algorithm, through which only high-likelihood particles are allowed to be resampled, so as to improve the effectiveness of the resampling process. Although the particle diversity can be enhanced by the abovementioned improved algorithms, the abandonment of low-weight particles still exists, so the particle impoverishment phenomenon cannot be fundamentally eliminated.As for introducing the intelligent optimization algorithms, the genetic algorithm is introduced in the literature [24] where particles are divided into high-weight ones and low-weight ones, and crossover and mutation are carried out on low-weight particles to improve the particle diversity. In the literature [25], a genetic optimization-based resampling method is proposed, and the effectiveness of such an optimized resampling method is proved by the results of indoor target tracking experiments where the particle distribution is optimized through selection, coarsening, classification, crossover, and mutation. Quantum genetic algorithm with PF algorithm (QGAPF) is proposed in the literature [26]. Compared with the PF of genetic algorithm, since the population of quantum genetic algorithm (QGA) is composed of quantum chromosomes encoded by quantum bits, QGA population diversity has more advantages, and QGAPF particle diversity is better. In the nonlinear target tracking model experiment, the performance of QGAPF is better than PF and particle swarm optimization PF algorithm. According to the literature [27], an idea of combining hierarchical resampling and genetic variation is put forward, and a hierarchical genetic variation resampling algorithm is proposed. It maintains the diversity of resampled particles and increases the diversity of particles through the mutation operation of high-weight particles, which can significantly improve the filtering performance. By virtue of the reduced PF based upon the genetic algorithm proposed in the literature [28], the particles with the highest weight are selected for evolution by the genetic algorithm in the resampling stage, which improves the effective particle number in the resampling process. According to the literature [29], extended Kalman filter (EKF) and genetic algorithm are introduced into the PF algorithm. The former is employed to update the particle importance distribution, and the latter is applied to optimize the resampling process, thereby improving the infrared target tracking performance. In the literature [30], an adaptive PF target tracking algorithm based on Kullback-Leibler Distance (KLD) sampling is proposed. Firstly, the number of particles is dynamically adjusted by KLD sampling. The simulated annealing algorithm is introduced in the iterative update to make the distribution of sampled particles close to the particle likelihood probability density function to improve the particle diversity and the computational efficiency of the algorithm. In addition, the butterfly optimization algorithm is introduced in the literature [31]. In this way, the optimization characteristics of the intelligent algorithm are utilized to make the particles continuously approach the high likelihood interval, avoid the degradation of the particle weight, and increase the particle diversity through Levy flight. In the literature [32], Li et al. draw Levy flight into the grey wolf algorithm to optimize the particle iterative optimization process of the improved grey wolf optimizer PF. At the same time, the weight adaptive adjustment strategy is used to improve the diversity of particles further. In the literature [33], Zhang et al. combine the improved hummingbird optimization algorithm with the standard PF algorithm, improve the problem that the traditional hummingbird algorithm is easy to fall into local optimization, and improve the population diversity. The Gaussian particle swarm optimization algorithm with the weight function of particles as its fitness function in the literature [34] is introduced into the literature [35], and the particles with low weights moved to the region with high posterior probability via velocity updating formula and position updating formula, so that the particle diversity is increased, the particle number needed for the PF algorithm is reduced, and the tracking accuracy of the PF algorithm is improved. Furthermore, in the literature [36], an improved krill herd algorithm is proposed by combining krill herd algorithm in the literature [37] with PF to optimize the PF algorithm. Specifically, the particles are guided to the high likelihood region by means of the induction, foraging, and random diffusion of krill individuals, thereby maintaining the particle diversity on the whole. However, the intelligent optimization algorithms are not suitable for the real-time MTT system in the UWB environment due to their high complexity and slow operation.As for the MTT system in the indoor UWB NLOS environment, an MTT algorithm based on IRPF was proposed in combination with the advantages of traditional resampling improvement and intelligent optimization algorithm introduction in solving the particle degradation in this paper. The contributions of this paper are as follows:(1)
An optimized stratification design method for the likelihood probability interval of particles was proposed. The likelihood probability interval of particles in each layer was defined and gradually expanded to the set layer, so that the layer with a high likelihood probability occupied a large region, thus ensuring that the particle number in the high likelihood region was larger than that in the low likelihood region(2)
On the basis of contribution (1), a particle diversity measurement index based on stratification was proposed, through which the particle diversity after resampling was evaluated. If the diversity measurement index failed to reach the set threshold, all new particles would be subjected to a Gaussian random walk in a preset variance matrix to ensure greater particle diversity than the set threshold, thereby increasing the particle diversity(3)
The IRPF algorithm was designed in combination with the likelihood probability interval stratification and the diversity index of particles, and an indoor MTT system in the UWB environment was built. In addition, the performance of the IRPF algorithm was tested in simulated and actual environments using the basic nonlinear filtering model and the MTT system in the indoor UWB NLOS environment, respectively. The experimental results showed that the IRPF algorithm was able to produce more reasonable particle distribution, increase the filtering accuracy, and improve the tracking performance in the NLOS environmentThe remainder of this paper was structured as follows: Section2 introduced the PF algorithm and its improved algorithms, respectively. Section 3, in detail, designed the indoor MTT system in the UWB environment. Experiments and experimental results were carried out and analyzed in Section 4. Finally, conclusions were drawn in Section 5.


## 2. PF Algorithm and Its Improvements

### 2.1. Bayesian Filtering

In the discrete-time nonlinear non-Gaussian dynamic tracking system, the state transition equation of the state sequence and the observation equation of the target tracking system are shown in(1)xk=fxk−1,wk−1,(2)zk=hxk,vk,wherexk∈Rn represents the state vector of the system, and zk∈Rm stands for the observation vector of the system. wk−1 and vk refer to the process noise and observation noise, respectively, with QK−1 and Rk as the corresponding variances. f. and h. stand for the nonlinear state function and observation function, respectively.According to the nonlinear Bayesian estimation theory, target tracking referred to recursive estimation of the system statexk at the moment k by virtue of the observation data z1:k=zi,i=1,⋯,k from the moments 1 to k. Assuming that the initial prior probability density function px0z0=px0 is given, the posterior probability density function pxkz1:k can be obtained by two recursive steps of prediction and updating.Assuming that the posterior probability density function of the system ispxk−1z1:k−1 at the moment k−1, the prior probability density function pxkz1:k−1 of the system state at the moment k can be calculated using the Chapman-Kolmogorov equation. Then, the posterior probability density function pxkz1:k of the system state at the moment k could be obtained using Bayesian formula in combination with the observed value zk at the moment k are shown in
(3)pxkz1:k−1=∫pxkxk−1pxk−1z1:k−1dxk−1,(4)pzkz1:k−1=∫pzkxkpxkz1:k−1dxk,(5)pxkz1:k=pzkxkpxkz1:k−1pzkz1:k−1,where Equation (3) represents the prediction formula, and Equations (4) and (5) stand for the updating formulas.The state estimationx^k of the target system was obtained through the minimum mean square error (MSE) estimation of the posterior probability density function pxkz1:k of the current state xk, which is shown in
(6)x^k=Epxkz1:kxk=∫xkpxkz1:kdxk.


### 2.2. Importance Sampling

As for general nonlinear target tracking systems, there is usually no analytical solution for the integral operation of the posterior probability density functionpxkz1:k, so direct sampling can be hardly realized. Therefore, indirect sampling was conducted on the probability density function qx0:kz1:k, also named importance density function, which is known to be easy for sampling. Equation (7) can be obtained in accordance with the principles of importance sampling.
(7)Efx0:k=∫fx0:kpxkz1:kqxkz1:kqxkz1:kdx0:k=∫fx0:kwx0:kpz1:kqxkz1:kdx0:k,wherewx0:k represents the unnormalized importance weight, and the expression is shown in
(8)wx0:k=pz1:kx0:kpx0:kqx0:kz1:k.N particles x0:kii=1N were obtained through the Monte Carlo sampling of the importance density function, and the posterior mean could be calculated by summing the particles with normalized importance weights, as shown in
(9)E^fx0:k=∑i=1Nfx0:kiw~ki.Given that the Bayesian importance sampling could not be directly used for recursive estimation, the importance density function is decomposed by sequential importance sampling (SIS) as shown in(10)qx0:kz1:k=qxkx0:k−1,z1:kqx0:k−1z1:k.The model of the target tracking system should meet three assumptions: (1) The system state complies with the first-order Markov process. (2) The observed values of the system at different moments are mutually independent in a given state. (3) The initial prior probability is set aspx0. Equations (11)–(12) can be obtained based on the three assumptions.
(11)px0:k=px0∏j=1kpxjxj−1,(12)pz1:kx0:k=px0∏j=1kpzjxj.The recursion formula of importance weight shown in Equation (13) can be obtained by substituting Equations (11) and (12) into Equation (8).
(13)wk=wk−1pzkxkpxkxk−1qxkx0:k−1,z1:k.Since all the state and observation information from the initial to the current moments were stored in the importance density functionqxkx0:k−1,z1:k, the importance density function could be equated by regarding the target tracking system as a first-order Markov process. The equivalent process is shown in
(14)qxkx0:k−1,z1:k=qxkxk−1,zk.The importance weight updating function and PF posterior probability density function were obtained by substituting Equation (14) into Equation (13) and selecting suboptimal state transition density function qxkxk−1,zk=pxkxk−1, as shown in
(15)wki=wk−1ipzkxki,(16)p^xkz1:k=∑i=1Nw~kiδxk−xki.


### 2.3. Particle Degradation and Particle Diversity Decline

After many iterations of SIS algorithm, the distribution of the importance weightwki failed to approximate the posterior probability density function of the target system. Only a few particles had nonzero weights, and the rest particles had weights approaching zero, which leads to the particle degradation and the PF algorithm failure. As for the particle degradation, the effective particle number N^eff was proposed to measure the degree of particle shortage. The calculation equation is shown in
(17)N^eff=1∑i=1Nw~ki2.WhenN^eff was smaller than the threshold Nth set for the effective particle number, the particles were resampled to reduce the particle number with relatively low weights and increase that with higher weights.Although the particle degradation was suppressed by the introduction of traditional resampling to some extent, the high-weight particles might be replicated for multiple times after resampling, and the low-weight particles might gradually disappear after several iterations. The particles concentrated in the same likelihood region after resampling might cause insufficient sample point distribution of the posterior probability density function and particle diversity decline. Meanwhile, the newly generated particles after resampling were still concentrated in the same likelihood region where a large number of high-weight particles were concentrated. When the likelihood region was divorced from the true value, a large amount of time would be wasted in computing the invalid resampling process, and the filtering error would be significantly increased, thereby failing to meet the real-time requirements of the tracking system.


### 2.4. IRPF Algorithm

Assuming that there areN particles xkii=1N in PF at the moment k, the likelihood probability of each particle is pki=pzkxki, and pkmax and pkmin represent the maximum and minimum likelihood probabilities of all particles at the moment k, respectively. The likelihood probability interval of the particles is shown in
(18)pk=pkmax−pkmin.In the idea of optimized PF based on swarm intelligence algorithm [38], the particles were guided to move to the high likelihood region via intelligent algorithm optimization, which could improve the PF efficiency and keep the particle diversity simultaneously. For this reason, the particle number in the high likelihood region was larger than that in the low likelihood region in PF state estimation with high filtering accuracy. Considering the influences of the likelihood probability region on the PF accuracy, a particle stratification method was proposed in this paper, where the likelihood probability interval of particles was increased to the layer K in a layer-by-layer manner, and the likelihood probability interval ski of the layer i at the moment k is shown in
(19)ski=ai−1−1aK−1pk,ai−1aK−1pk1≤i≤K,wherea represents the zoom coefficient greater than 1, which is used to adjust the interval of each layer. a is determined by the actual system, and a=1.5 was set in this paper. K stands for the maximum number of layers, and the likelihood ratio of the likelihood interval of the layer i is manifested in
(20)sRi=ai−ai−1aK−1.The adaptive likelihood region of the particles was stratified in accordance with Equation (20) (Figure 1), where the particle adaptive stratification region was increased with the rising particle likelihood probability.Figure 1
Schematic diagram of adaptive likelihood region stratification.Supposing that the particle likelihood region is reasonably stratified, that is, when the ratio of the particle numberni to the total particle number N in each layer of likelihood region is closer to the likelihood ratio sRi of the likelihood probability interval of each layer, the particle diversity will be better, and the likelihood probability distribution of the particles can approach the actual posterior probability density of the target. In this paper, a particle diversity measurement index based on stratification was proposed, which is shown in
(21)D=1−∑i=1Kni/N−sRi1−sR11≤i≤K.The value range of the particle diversity measurement indexD was [0, 1]. In other words, when the proportion of particles actually falling on each layer was completely consistent with the ratio of the likelihood probability interval of each layer, D=1 indicated the best particle diversity. In the case of a difference between the proportions of particles actually falling on each layer and the ratio of the likelihood probability interval of each layer, D was decreased with the increase of the difference. When the particles were actually concentrated in the likelihood interval with a low likelihood probability, D would approach 0. Under limiting conditions, the particle diversity was the worst, with D=0, if all particles were located in the first layer of the likelihood interval. Hence, the diversity threshold ϑ could be set for the particle diversity measurement index. When the particle diversity was smaller than the diversity threshold ϑ, the Gaussian random walk was performed for all new particles in the set variance matrix ξ to improve the particle diversity and the tracking accuracy of PF. Considering the real-time performance of the IRPF algorithm, we limited the maximum number of Gaussian random walks. When the number of random walks was greater than the maximum number of walks C or meets the particle diversity threshold ϑ, normalized the particle weight and outputted the state, then carried out the filtering process at the next moment.Figure2 shows the whole process of improved resampling of IRPF algorithm. When the number of effective particles was less than the resampling threshold. Firstly, resampling was performed to generate new particles. According to the particle likelihood probability region, all particles were divided into the likelihood interval of each layer. Then, the particle diversity D was calculated by the proportion of the preset reasonable particle likelihood probability interval at the current moment and the actual likelihood probability interval distribution of particles. If D<ϑ and times<C, all particles randomly walked between layers to approach the preset particle likelihood probability interval distribution. Finally, the particles would be reasonably distributed in the overall likelihood probability interval, so as to effectively avoid the problem of particle degradation.Figure 2
Schematic diagram of improved resampling of IRPF algorithm (for 20 random particles).The realization process of the IRPF algorithm proposed in this paper is listed in Algorithm1.Algorithm 1: IRPF Algorithm.
Input:zk, K, N, Nth, T, a, ϑ, ξOutput:xk1. Initialization2. Sample to get the initial particles of position estimation:x0ii=1N3. fork=1:T do4.   fori=1:N do5.xki~pxkxk−1i6.wki=wk−1ipzkxki/∑i=1Nwki7.   end for8.     ifN^eff<Nth9.       Resample to generate new particles10.       Calculate new particles likelihood probability:pzkxk11.       Particle likelihood interval stratification, according to Equation (19)12.       Calculate particle diversity, according to Equation (21)13.       ifD<ϑ and times<C14.         Gaussian random walk and repeat from step 1315.       end if16.         Compute the weight of each particle:wki=w~ki=1/N17.   Compute state estimation:x⌢k=∑i=1Nw~kixki18. end for19. returnxk


## 2.1. Bayesian Filtering

In the discrete-time nonlinear non-Gaussian dynamic tracking system, the state transition equation of the state sequence and the observation equation of the target tracking system are shown in(1)xk=fxk−1,wk−1,(2)zk=hxk,vk,wherexk∈Rn represents the state vector of the system, and zk∈Rm stands for the observation vector of the system. wk−1 and vk refer to the process noise and observation noise, respectively, with QK−1 and Rk as the corresponding variances. f. and h. stand for the nonlinear state function and observation function, respectively.According to the nonlinear Bayesian estimation theory, target tracking referred to recursive estimation of the system statexk at the moment k by virtue of the observation data z1:k=zi,i=1,⋯,k from the moments 1 to k. Assuming that the initial prior probability density function px0z0=px0 is given, the posterior probability density function pxkz1:k can be obtained by two recursive steps of prediction and updating.Assuming that the posterior probability density function of the system ispxk−1z1:k−1 at the moment k−1, the prior probability density function pxkz1:k−1 of the system state at the moment k can be calculated using the Chapman-Kolmogorov equation. Then, the posterior probability density function pxkz1:k of the system state at the moment k could be obtained using Bayesian formula in combination with the observed value zk at the moment k are shown in
(3)pxkz1:k−1=∫pxkxk−1pxk−1z1:k−1dxk−1,(4)pzkz1:k−1=∫pzkxkpxkz1:k−1dxk,(5)pxkz1:k=pzkxkpxkz1:k−1pzkz1:k−1,where Equation (3) represents the prediction formula, and Equations (4) and (5) stand for the updating formulas.The state estimationx^k of the target system was obtained through the minimum mean square error (MSE) estimation of the posterior probability density function pxkz1:k of the current state xk, which is shown in
(6)x^k=Epxkz1:kxk=∫xkpxkz1:kdxk.


## 2.2. Importance Sampling

As for general nonlinear target tracking systems, there is usually no analytical solution for the integral operation of the posterior probability density functionpxkz1:k, so direct sampling can be hardly realized. Therefore, indirect sampling was conducted on the probability density function qx0:kz1:k, also named importance density function, which is known to be easy for sampling. Equation (7) can be obtained in accordance with the principles of importance sampling.
(7)Efx0:k=∫fx0:kpxkz1:kqxkz1:kqxkz1:kdx0:k=∫fx0:kwx0:kpz1:kqxkz1:kdx0:k,wherewx0:k represents the unnormalized importance weight, and the expression is shown in
(8)wx0:k=pz1:kx0:kpx0:kqx0:kz1:k.N particles x0:kii=1N were obtained through the Monte Carlo sampling of the importance density function, and the posterior mean could be calculated by summing the particles with normalized importance weights, as shown in
(9)E^fx0:k=∑i=1Nfx0:kiw~ki.Given that the Bayesian importance sampling could not be directly used for recursive estimation, the importance density function is decomposed by sequential importance sampling (SIS) as shown in(10)qx0:kz1:k=qxkx0:k−1,z1:kqx0:k−1z1:k.The model of the target tracking system should meet three assumptions: (1) The system state complies with the first-order Markov process. (2) The observed values of the system at different moments are mutually independent in a given state. (3) The initial prior probability is set aspx0. Equations (11)–(12) can be obtained based on the three assumptions.
(11)px0:k=px0∏j=1kpxjxj−1,(12)pz1:kx0:k=px0∏j=1kpzjxj.The recursion formula of importance weight shown in Equation (13) can be obtained by substituting Equations (11) and (12) into Equation (8).
(13)wk=wk−1pzkxkpxkxk−1qxkx0:k−1,z1:k.Since all the state and observation information from the initial to the current moments were stored in the importance density functionqxkx0:k−1,z1:k, the importance density function could be equated by regarding the target tracking system as a first-order Markov process. The equivalent process is shown in
(14)qxkx0:k−1,z1:k=qxkxk−1,zk.The importance weight updating function and PF posterior probability density function were obtained by substituting Equation (14) into Equation (13) and selecting suboptimal state transition density function qxkxk−1,zk=pxkxk−1, as shown in
(15)wki=wk−1ipzkxki,(16)p^xkz1:k=∑i=1Nw~kiδxk−xki.


## 2.3. Particle Degradation and Particle Diversity Decline

After many iterations of SIS algorithm, the distribution of the importance weightwki failed to approximate the posterior probability density function of the target system. Only a few particles had nonzero weights, and the rest particles had weights approaching zero, which leads to the particle degradation and the PF algorithm failure. As for the particle degradation, the effective particle number N^eff was proposed to measure the degree of particle shortage. The calculation equation is shown in
(17)N^eff=1∑i=1Nw~ki2.WhenN^eff was smaller than the threshold Nth set for the effective particle number, the particles were resampled to reduce the particle number with relatively low weights and increase that with higher weights.Although the particle degradation was suppressed by the introduction of traditional resampling to some extent, the high-weight particles might be replicated for multiple times after resampling, and the low-weight particles might gradually disappear after several iterations. The particles concentrated in the same likelihood region after resampling might cause insufficient sample point distribution of the posterior probability density function and particle diversity decline. Meanwhile, the newly generated particles after resampling were still concentrated in the same likelihood region where a large number of high-weight particles were concentrated. When the likelihood region was divorced from the true value, a large amount of time would be wasted in computing the invalid resampling process, and the filtering error would be significantly increased, thereby failing to meet the real-time requirements of the tracking system.


## 2.4. IRPF Algorithm

Assuming that there areN particles xkii=1N in PF at the moment k, the likelihood probability of each particle is pki=pzkxki, and pkmax and pkmin represent the maximum and minimum likelihood probabilities of all particles at the moment k, respectively. The likelihood probability interval of the particles is shown in
(18)pk=pkmax−pkmin.In the idea of optimized PF based on swarm intelligence algorithm [38], the particles were guided to move to the high likelihood region via intelligent algorithm optimization, which could improve the PF efficiency and keep the particle diversity simultaneously. For this reason, the particle number in the high likelihood region was larger than that in the low likelihood region in PF state estimation with high filtering accuracy. Considering the influences of the likelihood probability region on the PF accuracy, a particle stratification method was proposed in this paper, where the likelihood probability interval of particles was increased to the layer K in a layer-by-layer manner, and the likelihood probability interval ski of the layer i at the moment k is shown in
(19)ski=ai−1−1aK−1pk,ai−1aK−1pk1≤i≤K,wherea represents the zoom coefficient greater than 1, which is used to adjust the interval of each layer. a is determined by the actual system, and a=1.5 was set in this paper. K stands for the maximum number of layers, and the likelihood ratio of the likelihood interval of the layer i is manifested in
(20)sRi=ai−ai−1aK−1.The adaptive likelihood region of the particles was stratified in accordance with Equation (20) (Figure 1), where the particle adaptive stratification region was increased with the rising particle likelihood probability.Figure 1
Schematic diagram of adaptive likelihood region stratification.Supposing that the particle likelihood region is reasonably stratified, that is, when the ratio of the particle numberni to the total particle number N in each layer of likelihood region is closer to the likelihood ratio sRi of the likelihood probability interval of each layer, the particle diversity will be better, and the likelihood probability distribution of the particles can approach the actual posterior probability density of the target. In this paper, a particle diversity measurement index based on stratification was proposed, which is shown in
(21)D=1−∑i=1Kni/N−sRi1−sR11≤i≤K.The value range of the particle diversity measurement indexD was [0, 1]. In other words, when the proportion of particles actually falling on each layer was completely consistent with the ratio of the likelihood probability interval of each layer, D=1 indicated the best particle diversity. In the case of a difference between the proportions of particles actually falling on each layer and the ratio of the likelihood probability interval of each layer, D was decreased with the increase of the difference. When the particles were actually concentrated in the likelihood interval with a low likelihood probability, D would approach 0. Under limiting conditions, the particle diversity was the worst, with D=0, if all particles were located in the first layer of the likelihood interval. Hence, the diversity threshold ϑ could be set for the particle diversity measurement index. When the particle diversity was smaller than the diversity threshold ϑ, the Gaussian random walk was performed for all new particles in the set variance matrix ξ to improve the particle diversity and the tracking accuracy of PF. Considering the real-time performance of the IRPF algorithm, we limited the maximum number of Gaussian random walks. When the number of random walks was greater than the maximum number of walks C or meets the particle diversity threshold ϑ, normalized the particle weight and outputted the state, then carried out the filtering process at the next moment.Figure2 shows the whole process of improved resampling of IRPF algorithm. When the number of effective particles was less than the resampling threshold. Firstly, resampling was performed to generate new particles. According to the particle likelihood probability region, all particles were divided into the likelihood interval of each layer. Then, the particle diversity D was calculated by the proportion of the preset reasonable particle likelihood probability interval at the current moment and the actual likelihood probability interval distribution of particles. If D<ϑ and times<C, all particles randomly walked between layers to approach the preset particle likelihood probability interval distribution. Finally, the particles would be reasonably distributed in the overall likelihood probability interval, so as to effectively avoid the problem of particle degradation.Figure 2
Schematic diagram of improved resampling of IRPF algorithm (for 20 random particles).The realization process of the IRPF algorithm proposed in this paper is listed in Algorithm1.Algorithm 1: IRPF Algorithm.
Input:zk, K, N, Nth, T, a, ϑ, ξOutput:xk1. Initialization2. Sample to get the initial particles of position estimation:x0ii=1N3. fork=1:T do4.   fori=1:N do5.xki~pxkxk−1i6.wki=wk−1ipzkxki/∑i=1Nwki7.   end for8.     ifN^eff<Nth9.       Resample to generate new particles10.       Calculate new particles likelihood probability:pzkxk11.       Particle likelihood interval stratification, according to Equation (19)12.       Calculate particle diversity, according to Equation (21)13.       ifD<ϑ and times<C14.         Gaussian random walk and repeat from step 1315.       end if16.         Compute the weight of each particle:wki=w~ki=1/N17.   Compute state estimation:x⌢k=∑i=1Nw~kixki18. end for19. returnxk


## 3. Design of the Indoor MTT System in the UWB Environment

### 3.1. Indoor MTT Modeling in the UWB Environment

Since the indoor motion was dominated by the uniform motion, the speed changed slightly. The constant velocity (CV) model and the coordinate turn (CT) model in the ideal state were selected for the indoor MTT system to construct the moving target motion model in this paper.The state transition equation of the uniform linear motion system is expressed as(22)xkvxkykvyk=1T000100001T0001xk−1vxk−1yk−1vyk−1+wxk−1wvxk−1wyk−1wvyk−1.The state transition equation of the uniform turning motion system is displayed in(23)xkvxkykvyk=1sinwTw0−1−coswTw0coswT0−sinwT01−coswTw1sinwTw0sinwT0coswTxk−1vxk−1yk−1vyk−1+wxk−1wvxk−1wyk−1wvyk−1.The target state equation model could be obtained through further simplification, as shown in(24)xk=FCVxk−1+wk−1CV,xk=FCTxk−1+wk−1CT,whereFCV and FCT represent the state transition matrices of the uniform linear motion the and uniform turning motion, respectively, and wk−1CV and wk−1CT stand for the process noise of the two motions at the moment k−1, which comply with Gaussian distribution with mean value of 0 and variances of Qk−1CV and Qk−1CT, respectively.The UWB positioning mode was adopted to detect the moving vehicle in this paper. The ranging information was collected by the UWB target tracking system at the momentk and then sent to a PC host computer for positioning algorithm solution [39]. Finally, the positioning result x^k,y^k of UWB at the moment k obtained was regarded as the observed value zk=x∧k,y∧kT in the observation model. The observation equation of the target system at the moment k is shown in
(25)zk=10000010xkvxkykvyk+rxkryk.The target observation model could be obtained through further simplification(26)zk=Hxk+rk,whereH represents the observation matrix, and rk refers to the observation noise of the system at the moment k, complying with the Gaussian distribution with mean value of 0 and variance of Rk−1.


### 3.2. Design of the Indoor MTT Experiment in the UWB Environment

The indoor MTT system in the UWB environment was composed of the NLOS environment, the UWB positioning and tracking unit, and the moving target to be tracked. The system framework is shown in Figure3. Specifically, the NLOS environment was mainly built by placing wood block and iron sheet obstacles. The UWB positioning and tracking unit mainly consisted of a primary base station, three secondary base stations A0, A1, and A2, and a PC host computer. The moving target to be tracked was a moving vehicle equipped with UWB tag node. The hardware circuit of base station and label node was mainly composed of main controller STM32F103RCT6, DWM1000 chip, Serial Wire Debug (SWD) download interface, serial interface, and power circuit. STM32F103RCT6 mainly communicated with DWM1000 through Serial Peripheral Interface (SPI) communication interface to complete the reading of ranging data and channel impulse response signal characteristics. The power supply circuit mainly converted 5 V voltage into 3.3 through AMS1117 chip to supply power to the chip. SWD interface could download the program to MCU. The SPI was used for data communication with the PC host computer.Figure 3
Framework of the indoor MTT system in the UWB environment.The UWB tracking system experiment was conducted mainly by setting the NLOS environment, laying out UWB base stations, and presetting the MTT trajectory. The NLOS environment was established in a rectangular region with a length of 200 cm and a width of 300 cm, and two iron sheet obstacles and a wood block obstacle were placed at positions (11, 35), (64, 240), and (155, 75), respectively. The primary base station was located at (100, 0), and the secondary base stations A0, A1, and A2 were put at positions (0, 0), (200, 0), and (0, 300), respectively. The starting and ending points of the preset trajectory of the moving vehicle were located at (50, 50) and (150, 249.7), respectively. Considering the actual length of the track, the motion state parameters of the moving vehicle are set in Table1. The MTT process of UWB system was shown in Figure 4.Table 1
Motion state parameters of the moving vehicle.
TimeMotion modelSpeed/turn rate0-2.5 sCV19.77 cm/s2.5-6.5 sCTRight turnπ/8rad/s6.5-10.5 sCTLeft turnπ/8rad/s10.5-13 sCV19.77 cm/sFigure 4
The workflow of the UWB MTT system.


## 3.1. Indoor MTT Modeling in the UWB Environment

Since the indoor motion was dominated by the uniform motion, the speed changed slightly. The constant velocity (CV) model and the coordinate turn (CT) model in the ideal state were selected for the indoor MTT system to construct the moving target motion model in this paper.The state transition equation of the uniform linear motion system is expressed as(22)xkvxkykvyk=1T000100001T0001xk−1vxk−1yk−1vyk−1+wxk−1wvxk−1wyk−1wvyk−1.The state transition equation of the uniform turning motion system is displayed in(23)xkvxkykvyk=1sinwTw0−1−coswTw0coswT0−sinwT01−coswTw1sinwTw0sinwT0coswTxk−1vxk−1yk−1vyk−1+wxk−1wvxk−1wyk−1wvyk−1.The target state equation model could be obtained through further simplification, as shown in(24)xk=FCVxk−1+wk−1CV,xk=FCTxk−1+wk−1CT,whereFCV and FCT represent the state transition matrices of the uniform linear motion the and uniform turning motion, respectively, and wk−1CV and wk−1CT stand for the process noise of the two motions at the moment k−1, which comply with Gaussian distribution with mean value of 0 and variances of Qk−1CV and Qk−1CT, respectively.The UWB positioning mode was adopted to detect the moving vehicle in this paper. The ranging information was collected by the UWB target tracking system at the momentk and then sent to a PC host computer for positioning algorithm solution [39]. Finally, the positioning result x^k,y^k of UWB at the moment k obtained was regarded as the observed value zk=x∧k,y∧kT in the observation model. The observation equation of the target system at the moment k is shown in
(25)zk=10000010xkvxkykvyk+rxkryk.The target observation model could be obtained through further simplification(26)zk=Hxk+rk,whereH represents the observation matrix, and rk refers to the observation noise of the system at the moment k, complying with the Gaussian distribution with mean value of 0 and variance of Rk−1.


## 3.2. Design of the Indoor MTT Experiment in the UWB Environment

The indoor MTT system in the UWB environment was composed of the NLOS environment, the UWB positioning and tracking unit, and the moving target to be tracked. The system framework is shown in Figure3. Specifically, the NLOS environment was mainly built by placing wood block and iron sheet obstacles. The UWB positioning and tracking unit mainly consisted of a primary base station, three secondary base stations A0, A1, and A2, and a PC host computer. The moving target to be tracked was a moving vehicle equipped with UWB tag node. The hardware circuit of base station and label node was mainly composed of main controller STM32F103RCT6, DWM1000 chip, Serial Wire Debug (SWD) download interface, serial interface, and power circuit. STM32F103RCT6 mainly communicated with DWM1000 through Serial Peripheral Interface (SPI) communication interface to complete the reading of ranging data and channel impulse response signal characteristics. The power supply circuit mainly converted 5 V voltage into 3.3 through AMS1117 chip to supply power to the chip. SWD interface could download the program to MCU. The SPI was used for data communication with the PC host computer.Figure 3
Framework of the indoor MTT system in the UWB environment.The UWB tracking system experiment was conducted mainly by setting the NLOS environment, laying out UWB base stations, and presetting the MTT trajectory. The NLOS environment was established in a rectangular region with a length of 200 cm and a width of 300 cm, and two iron sheet obstacles and a wood block obstacle were placed at positions (11, 35), (64, 240), and (155, 75), respectively. The primary base station was located at (100, 0), and the secondary base stations A0, A1, and A2 were put at positions (0, 0), (200, 0), and (0, 300), respectively. The starting and ending points of the preset trajectory of the moving vehicle were located at (50, 50) and (150, 249.7), respectively. Considering the actual length of the track, the motion state parameters of the moving vehicle are set in Table1. The MTT process of UWB system was shown in Figure 4.Table 1
Motion state parameters of the moving vehicle.
TimeMotion modelSpeed/turn rate0-2.5 sCV19.77 cm/s2.5-6.5 sCTRight turnπ/8rad/s6.5-10.5 sCTLeft turnπ/8rad/s10.5-13 sCV19.77 cm/sFigure 4
The workflow of the UWB MTT system.


## 4. Experiments and Analysis

### 4.1. Simulation Experiment and Result Analysis

The Matlab 2018a for Windows 10 operating system was used as the simulation software, and the univariate nonstationary growth model [40] was selected for the simulation experiment. The state equation and observation equation of the model system are shown in
(27)xk=12xk−1+20xk−11+xk−12+8cos1.2k−1+wk−1,zk=xk220+vk.The PF and IRPF algorithms were adopted for state estimation and tracking of the nonlinear system, whose parameters were determinedas per the literature [40] in Table 2.Table 2
Parameters for simulation experiment.
ParameterValueProcess noise variance1 (unitless)Observation noise variance1 (unitless)Time step50 sSampling interval1 sInitial state1 (unitless)Particle number50 (unitless)Resample threshold17 (unitless)Maximum number of walks20 (unitless)Particle diversity threshold0.4 (unitless)Gaussian random walk variance0.5 (unitless)As for the experimental results, RMSE of target state estimation served as the standard for evaluating the performance of PF and IRPF algorithms, which was calculated according to the formula shown(28)RMSE=1k∑k=1Kyk−y∧k2,whereyk and y^k represent the true value of the target and the estimated value of the state, respectively, and K stands for the maximum iteration step. The effects of particle likelihood interval layer number and particle diversity threshold on the tracking performance of IRPF are studied, respectively. The simulation results of IRPF state estimation and filtering error curve under different parameter conditions were shown in Figures 5–8.Figure 5
Target state estimation of PF and IRPF under different likelihood interval stratification conditions.Figure 6
RMSE of PF and IRPF under different likelihood interval stratification conditions.Figure 7
Target state estimation of PF and IRPF under different particle diversity thresholds.Figure 8
RMSE of PF and IRPF under different particle diversity thresholds.The tracking performance of PF and IRPF with different likelihood interval layers was shown in Figures5 and 6. In Figure 5, the black curve represented the true value of the target state, the red curve represented the tracking curve of PF, and the green, blue, and yellow curves represented the tracking curve of IRPF with different likelihood interval layers, respectively. It could be observed in the tracking curve that the number of layers of different likelihood intervals would affect the performance of IRPF. To obtain the best parameter of IRPF, the filtering errors of PF and IRPF were further calculated and compared. From Figure 6, the filtering error of IRPF was the smallest and the fluctuation of tracking trajectory was the smallest, with k=4. So the number of layers of particle likelihood interval was 4.The influence of different particle diversity thresholds on the tracking performance of IRPF was studied by changing the values of particle diversity threshold. As shown in Figure7, different particle diversity thresholds seriously affected the performance of IRPF. Figure 8 showed the RMSE obtained by PF and IRPF. The comparison result suggested that the RMSE of IRPF was the smallest when the particle diversity threshold is reached 0.4.The reason for the reduction of filtering accuracy of IRPF was that the high particle diversity threshold reduces the number of resampling iterations. Meanwhile, low particle diversity threshold led to multiple Gaussian random walks of particles, and the improvement of particle diversity was not obvious, which could not significantly improve the filtering accuracy of IRPF. Compared with PF, the filtering accuracy of IRPF was significantly improved. It suggests that IRPF made the spatial distribution of particles more rational and could further improve the accuracy of target state estimation.A total of 1,000 times of Monte Carlo simulation experiments with different particle numbers were carried out using the two algorithms to ensure the objectivity of data. IRPF and PF algorithms with particle numberN=50 at t=50 particle distribution were shown in Figure 9. The RMSEs and effective particle numbers of PF and IRPF algorithms were calculated when the particle number N was 50, 100, 150, and 200, and the relevant results were shown in Table 3. It could be seen from the Figure 9 that in the final time step, the diversity of PF particles was poor, and most particles were concentrated near the real state value, resulting in particle degradation. Compared with PF algorithm, particles of IRPF algorithm were mostly concentrated in the high likelihood region, but a small number of particles were located in the low likelihood region. The particle distribution was more reasonable, and the particle diversity is significantly improved.Figure 9
Particle distribution of the PF and IRPF att=50.Table 3
RMSEs and effective particle numbers of PF and IRPF algorithms with different particle numbers.
AlgorithmN=50N=100N=150N=200RMSEN^effRMSEN^effRMSEN^effRMSEN^effPF3.7417.443.1534.963.0951.553.0468.46IRPF3.2633.833.0668.763.02103.952.98138.71According to Table3, there was a big difference in the filtering accuracy between PF and IRPF when the particle number was small, and the filtering accuracy of the IRPF was 12.83% higher than that of PF when N=50, indicating that the fewer the particles are, the greater the influence of the likelihood probability distribution of particles on the PF accuracy will be. The IRPF optimized the probability distribution of particles and improved the particle diversity. In addition, when the particle number was small, the probability of PF divergence could be reduced by improving the resampling and optimizing the spatial distribution of particles. The RMSEs of both PF and IRPF were decreased with the increase of the particle number, but the RMSE of the IRPF was always smaller than that of the PF algorithm, and the average effective particle number of the IRPF was larger than that of the PF. Therefore, the IRPF proposed in this paper was superior to the PF in the nonlinear target tracking.IRPF algorithm modified the resampling process of PF algorithm, so it was necessary to calculate and analyze the time complexity of IRPF algorithm and PF algorithm. For the PF algorithm with the number of particles isN and the time step is T, the time complexity of the PF algorithm was OTN2 because all particles participated in the resampling operation. The time complexity of IRPF algorithm was mainly increased compared with PF algorithm in two parts: the stratification of particle likelihood probability interval and the calculation of particle diversity measurement index after particle Gaussian random walk. The time complexity of the two parts was O2K+1 and OTCN+2, respectively. The number of stratification layers of particle likelihood probability interval was K, and the number of particle Gaussian random walks was C. Therefore, the overall IRPF algorithm could be approximately equal to OTNC+N. The overall time complexity of IRPF algorithm was higher than that of PF algorithm, and 1,000 times of Monte Carlo simulation experiments showed that the total running time of IRPF algorithm was 0.35 s when the number of particles was 50, which was higher than 0.19 s of PF algorithm. When the number of particles was 100, 150, and 200, the total running time of IRPF algorithm was 0.24 s, 0.33 s, and 0.51 s higher than PF algorithm, respectively.


### 4.2. Actual Environmental Experiment and Result Analysis

The PF MTT algorithm and the IRPF MTT algorithm were validated by an experiment using the indoor MTT system in the UWB environment to better verify the effectiveness of the IRPF proposed in this paper in the NLOS environment. The experimental site is shown in Figure10.Figure 10
Experimental environment of the indoor MTT system in the UWB environment.The specific experimental steps are as follows: (1) The primary base station, secondary base stationsA0, A1, and A2, and tag nodes were started in sequence for network communication. (2) The tracking system software was started, and the corresponding parameters were set. Parameters were set by reference to simulation experiment parameters except for the initial state of moving target, time step, and sampling interval. The experimental parameter settings of the PF and IRPF are shown in Table 4. (3) The serial port was opened to receive data, and the power supply of the moving vehicle was turned on. Then, the moving vehicle started uniform linear motion at 19.77 cm/s along the preset trajectory, and the data were collected by the target tracking system every 0.5 s until the vehicle reached the ending point.Table 4
Parameters for actual environmental experiment.
ParameterValueProcess noise variance1 (unitless)Observation noise variance10 (unitless)Sampling interval0.5 sInitial state50cm0m/s50cm19.77m/sParticle number100 (unitless)Likelihood interval layer4 (unitless)Particle diversity threshold0.4 (unitless)Gaussian random walk variance0.5 (unitless)The real trajectory of the moving vehicle, the tracking trajectory of the PF, and the tracking trajectory of the IRPF are shown in Figure11. The error curves of tracking trajectories of PF and IRPF are shown in Figure 12.Figure 11
Real trajectory of the moving vehicle and tracking trajectories of PF and IRPF.Figure 12
Absolute values of tracking errors of tracking trajectories of PF and IRPF.The experimental results revealed that the moving vehicle tracking trajectory of the IRPF was smoother and closer to the real trajectory than that of the PF algorithm. Besides, the IRPF resulted in a smaller tracking error of the tracking trajectory at most moments, without obvious filter divergence phenomenon of the PF. The RMSEs of tracking trajectories in X and Y directions of the two algorithms were calculated to reflect the filtering effect of the IRPF in the NLOS environment, and the relevant results were shown in Table5.Table 5
RMSEs of PF and IRPF tracking algorithms in the NLOS experimental environment.
Tracking algorithmX direction RMSE (cm)Y direction RMSE (cm)RMSE (cm)PF7.877215.212617.1310IRPF9.74863.821910.4710Based on the data in Table5, compared with the PF, the IRPF improved the RMSE of the tracking trajectory by 23.75% in the X direction and reduced it by 74.87% in the Y direction, and the RMSE of the overall tracking trajectory was decreased by 38.87%. Therefore, the experimental results proved that the accuracy and stability of PF could be obviously improved by the IRPF in the indoor NLOS environment, thus achieving better target tracking effects.


## 4.1. Simulation Experiment and Result Analysis

The Matlab 2018a for Windows 10 operating system was used as the simulation software, and the univariate nonstationary growth model [40] was selected for the simulation experiment. The state equation and observation equation of the model system are shown in
(27)xk=12xk−1+20xk−11+xk−12+8cos1.2k−1+wk−1,zk=xk220+vk.The PF and IRPF algorithms were adopted for state estimation and tracking of the nonlinear system, whose parameters were determinedas per the literature [40] in Table 2.Table 2
Parameters for simulation experiment.
ParameterValueProcess noise variance1 (unitless)Observation noise variance1 (unitless)Time step50 sSampling interval1 sInitial state1 (unitless)Particle number50 (unitless)Resample threshold17 (unitless)Maximum number of walks20 (unitless)Particle diversity threshold0.4 (unitless)Gaussian random walk variance0.5 (unitless)As for the experimental results, RMSE of target state estimation served as the standard for evaluating the performance of PF and IRPF algorithms, which was calculated according to the formula shown(28)RMSE=1k∑k=1Kyk−y∧k2,whereyk and y^k represent the true value of the target and the estimated value of the state, respectively, and K stands for the maximum iteration step. The effects of particle likelihood interval layer number and particle diversity threshold on the tracking performance of IRPF are studied, respectively. The simulation results of IRPF state estimation and filtering error curve under different parameter conditions were shown in Figures 5–8.Figure 5
Target state estimation of PF and IRPF under different likelihood interval stratification conditions.Figure 6
RMSE of PF and IRPF under different likelihood interval stratification conditions.Figure 7
Target state estimation of PF and IRPF under different particle diversity thresholds.Figure 8
RMSE of PF and IRPF under different particle diversity thresholds.The tracking performance of PF and IRPF with different likelihood interval layers was shown in Figures5 and 6. In Figure 5, the black curve represented the true value of the target state, the red curve represented the tracking curve of PF, and the green, blue, and yellow curves represented the tracking curve of IRPF with different likelihood interval layers, respectively. It could be observed in the tracking curve that the number of layers of different likelihood intervals would affect the performance of IRPF. To obtain the best parameter of IRPF, the filtering errors of PF and IRPF were further calculated and compared. From Figure 6, the filtering error of IRPF was the smallest and the fluctuation of tracking trajectory was the smallest, with k=4. So the number of layers of particle likelihood interval was 4.The influence of different particle diversity thresholds on the tracking performance of IRPF was studied by changing the values of particle diversity threshold. As shown in Figure7, different particle diversity thresholds seriously affected the performance of IRPF. Figure 8 showed the RMSE obtained by PF and IRPF. The comparison result suggested that the RMSE of IRPF was the smallest when the particle diversity threshold is reached 0.4.The reason for the reduction of filtering accuracy of IRPF was that the high particle diversity threshold reduces the number of resampling iterations. Meanwhile, low particle diversity threshold led to multiple Gaussian random walks of particles, and the improvement of particle diversity was not obvious, which could not significantly improve the filtering accuracy of IRPF. Compared with PF, the filtering accuracy of IRPF was significantly improved. It suggests that IRPF made the spatial distribution of particles more rational and could further improve the accuracy of target state estimation.A total of 1,000 times of Monte Carlo simulation experiments with different particle numbers were carried out using the two algorithms to ensure the objectivity of data. IRPF and PF algorithms with particle numberN=50 at t=50 particle distribution were shown in Figure 9. The RMSEs and effective particle numbers of PF and IRPF algorithms were calculated when the particle number N was 50, 100, 150, and 200, and the relevant results were shown in Table 3. It could be seen from the Figure 9 that in the final time step, the diversity of PF particles was poor, and most particles were concentrated near the real state value, resulting in particle degradation. Compared with PF algorithm, particles of IRPF algorithm were mostly concentrated in the high likelihood region, but a small number of particles were located in the low likelihood region. The particle distribution was more reasonable, and the particle diversity is significantly improved.Figure 9
Particle distribution of the PF and IRPF att=50.Table 3
RMSEs and effective particle numbers of PF and IRPF algorithms with different particle numbers.
AlgorithmN=50N=100N=150N=200RMSEN^effRMSEN^effRMSEN^effRMSEN^effPF3.7417.443.1534.963.0951.553.0468.46IRPF3.2633.833.0668.763.02103.952.98138.71According to Table3, there was a big difference in the filtering accuracy between PF and IRPF when the particle number was small, and the filtering accuracy of the IRPF was 12.83% higher than that of PF when N=50, indicating that the fewer the particles are, the greater the influence of the likelihood probability distribution of particles on the PF accuracy will be. The IRPF optimized the probability distribution of particles and improved the particle diversity. In addition, when the particle number was small, the probability of PF divergence could be reduced by improving the resampling and optimizing the spatial distribution of particles. The RMSEs of both PF and IRPF were decreased with the increase of the particle number, but the RMSE of the IRPF was always smaller than that of the PF algorithm, and the average effective particle number of the IRPF was larger than that of the PF. Therefore, the IRPF proposed in this paper was superior to the PF in the nonlinear target tracking.IRPF algorithm modified the resampling process of PF algorithm, so it was necessary to calculate and analyze the time complexity of IRPF algorithm and PF algorithm. For the PF algorithm with the number of particles isN and the time step is T, the time complexity of the PF algorithm was OTN2 because all particles participated in the resampling operation. The time complexity of IRPF algorithm was mainly increased compared with PF algorithm in two parts: the stratification of particle likelihood probability interval and the calculation of particle diversity measurement index after particle Gaussian random walk. The time complexity of the two parts was O2K+1 and OTCN+2, respectively. The number of stratification layers of particle likelihood probability interval was K, and the number of particle Gaussian random walks was C. Therefore, the overall IRPF algorithm could be approximately equal to OTNC+N. The overall time complexity of IRPF algorithm was higher than that of PF algorithm, and 1,000 times of Monte Carlo simulation experiments showed that the total running time of IRPF algorithm was 0.35 s when the number of particles was 50, which was higher than 0.19 s of PF algorithm. When the number of particles was 100, 150, and 200, the total running time of IRPF algorithm was 0.24 s, 0.33 s, and 0.51 s higher than PF algorithm, respectively.


## 4.2. Actual Environmental Experiment and Result Analysis

The PF MTT algorithm and the IRPF MTT algorithm were validated by an experiment using the indoor MTT system in the UWB environment to better verify the effectiveness of the IRPF proposed in this paper in the NLOS environment. The experimental site is shown in Figure10.Figure 10
Experimental environment of the indoor MTT system in the UWB environment.The specific experimental steps are as follows: (1) The primary base station, secondary base stationsA0, A1, and A2, and tag nodes were started in sequence for network communication. (2) The tracking system software was started, and the corresponding parameters were set. Parameters were set by reference to simulation experiment parameters except for the initial state of moving target, time step, and sampling interval. The experimental parameter settings of the PF and IRPF are shown in Table 4. (3) The serial port was opened to receive data, and the power supply of the moving vehicle was turned on. Then, the moving vehicle started uniform linear motion at 19.77 cm/s along the preset trajectory, and the data were collected by the target tracking system every 0.5 s until the vehicle reached the ending point.Table 4
Parameters for actual environmental experiment.
ParameterValueProcess noise variance1 (unitless)Observation noise variance10 (unitless)Sampling interval0.5 sInitial state50cm0m/s50cm19.77m/sParticle number100 (unitless)Likelihood interval layer4 (unitless)Particle diversity threshold0.4 (unitless)Gaussian random walk variance0.5 (unitless)The real trajectory of the moving vehicle, the tracking trajectory of the PF, and the tracking trajectory of the IRPF are shown in Figure11. The error curves of tracking trajectories of PF and IRPF are shown in Figure 12.Figure 11
Real trajectory of the moving vehicle and tracking trajectories of PF and IRPF.Figure 12
Absolute values of tracking errors of tracking trajectories of PF and IRPF.The experimental results revealed that the moving vehicle tracking trajectory of the IRPF was smoother and closer to the real trajectory than that of the PF algorithm. Besides, the IRPF resulted in a smaller tracking error of the tracking trajectory at most moments, without obvious filter divergence phenomenon of the PF. The RMSEs of tracking trajectories in X and Y directions of the two algorithms were calculated to reflect the filtering effect of the IRPF in the NLOS environment, and the relevant results were shown in Table5.Table 5
RMSEs of PF and IRPF tracking algorithms in the NLOS experimental environment.
Tracking algorithmX direction RMSE (cm)Y direction RMSE (cm)RMSE (cm)PF7.877215.212617.1310IRPF9.74863.821910.4710Based on the data in Table5, compared with the PF, the IRPF improved the RMSE of the tracking trajectory by 23.75% in the X direction and reduced it by 74.87% in the Y direction, and the RMSE of the overall tracking trajectory was decreased by 38.87%. Therefore, the experimental results proved that the accuracy and stability of PF could be obviously improved by the IRPF in the indoor NLOS environment, thus achieving better target tracking effects.


## 5. Conclusion

(1)
An MTT algorithm based on IRPF was proposed in this paper. An optimized stratification design method for the likelihood probability interval of particles was proposed by analyzing the influences of the likelihood probability distribution of particles on the PF accuracy, and a particle diversity measurement index based on stratification was put forward to solve the lack of particle diversity. The particle diversity was judged by virtue of the index. If the particle diversity was lower than the set thresholdϑ, all the newly generated particles after resampling would be subjected to the Gaussian random walk in the set variance matrix ξ to ensure that the particle diversity remained above the set threshold. The IRPF not only improved the particle diversity and PF tracking accuracy but also optimized the particle distribution(2)
The simulation experiment was conducted in the environment simulated by the univariate nonstationary growth model, and the field experiment was carried out using the indoor MTT system in the UWB environment, so as to verify the PF and IRPF, respectively. The RMSEs of the target state estimation and tracking trajectory of PF and IRPF were calculated and analyzed in the simulation and actual environments, respectively, which proved that the filtering error of the IRPF was obviously reduced, the target state estimation accuracy and the target tracking capability in the NLOS environment were improved, and the filter performance of IRPF was improved more prominently compared with that of the PF(3)
Although an MTT algorithm based on IRPF was proposed by studying the insufficient particle diversity of PF in this paper, only resampling was improved, and the importance density function was not investigated. Therefore, an appropriate importance density function can be proposed and combined with the improved resampling method put forward in this paper, so as to enhance the tracking accuracy in the future



---

*Source: 9974049-2022-06-20.xml*