Ensemble transform particle filter using regularized optimal transport and measure of nonlinearity

Measurement 146 (2019) 363–371

Contents lists available at ScienceDirect

Measurement journal homepage: www.elsevier.com/locate/measurement

Ensemble transform particle filter using regularized optimal transport and measure of nonlinearity Chang Ho Kang a, Sun Young Kim b, Yeongkwon Choe b, Chan Gook Park c,⇑ a

Research Institute of Engineering and Technology, Korea University, 145 Anam-Ro, Seongbuk-Gu, Seoul 02841, Republic of Korea Department of Mechanical and Aerospace Engineering, Seoul National University, 1 Gwanak-ro, Gwanak-gu, Seoul 08826, Republic of Korea c Department of Mechanical and Aerospace Engineering, Automation and Systems Research Institute, and the Institute of Advanced Aerospace Technology, 1 Gwanak-ro, Gwanak-gu, Seoul 08826, Republic of Korea b

a r t i c l e

i n f o

Article history: Received 1 March 2019 Received in revised form 5 June 2019 Accepted 24 June 2019 Available online 27 June 2019 Keywords: Ensemble transform particle filter Regularized optimal transport Degree of nonlinearity 2D LiDAR tracking

a b s t r a c t In this paper, the proposed method replaces a linear transformation step in an ensemble transform particle filter (ETPF) with an algorithm based on regularized optimal transport (ROT) resulting in improved performance. The linear transformation in the ETPF leads to warping of a posteriori weights into desired weights. However, it is computationally expensive and robust processing entails relaxation matching between posterior weights and desired weights. Therefore, a ROT method was used to address the ETPF challenges using a modified Sinkhorn-Knopp algorithm via the convergence of the process iteration rate. In addition, the proposed algorithm is correlated with the degree of nonlinearity such that the ROT method is only used for nonlinear measurement models to increase the efficiency of the proposed algorithm. In this paper, the proposed method is evaluated using 2D LiDAR via simulations and single object tracking experiments. Accordingly, the proposed method enhances the estimation of the target position compared to conventional methods. Ó 2019 Elsevier Ltd. All rights reserved.

1. Introduction Particle filters are a set of particle Monte Carlo methodologies designed to resolve filtering problems by approximating the filter distribution [1–4]. The filtering challenges involve estimation of the filter states in dynamic systems yielding partial observations with random perturbations. In addition, particle filters deal with nonlinear and non-Gaussian filtering problems due to the higher potential for relaxation of the linearity and Gaussian assumptions compared to conventional Kalman filters. Consequently, particle filters have been used in several fields [1–3,5]. Although particle filters have been used with success in several nonlinear estimations, their application to dynamic systems is associated with challenges, especially particle resampling in the filter. The particle resampling is important for an accurate estimation of the state variables [6]. The resampling step prevents the degeneracy of sequential sampling but can lead to a loss of particle diversity via sample impoverishment involving repeated resampled particles [7]. Consequently, several studies have investigated the resampling methods of the particle filter to address the challenges or improve efficiency, over two decades [6]. Generally, solutions ⇑ Corresponding author. E-mail addresses: [email protected] (C.H. Kang), [email protected] (S.Y. Kim), [email protected] (Y. Choe), [email protected] (C.G. Park). https://doi.org/10.1016/j.measurement.2019.06.046 0263-2241/Ó 2019 Elsevier Ltd. All rights reserved.

increase the noise covariance or trigger random noise in the particle states [8,9]. Most of these methods are based on the heuristic method, which depends on the system dynamics and its performance is affected mostly by the estimation of covariance. In addition, the regularized particle filter [10] and the particle filter using the Markov chain Monte Carlo step [11] are based on the kernel function with the probability of acceptance. Resampling particles obtained by these methods are no longer guaranteed to asymptotically approximate those derived from the posterior; but, the distribution of the resampling particles depends on the designed kernel function. Particle filters with optimization methods are used to estimate the distribution of the resampled particles [12,13] and are proposed from a general class of factorable functions. Most of these approaches are designed to update significant particle weights using the likelihood and cost functions while ignoring the spatial distribution of the states. Thus, in the previous work [14], deterministic resampling was shown to improve the performance of particle filters by allocating particles based on the likelihood function as well as their spatial distribution. However, this method is applicable to low-dimensional states. Therefore, other general resampling methods for particle filters are needed to improve the estimation and prevent degeneracy while avoiding impoverishment regardless of the spatial dimensions.

364

C.H. Kang et al. / Measurement 146 (2019) 363–371

One of the solutions involves linear transform-based resampling in the ensemble transform particle filter (ETPF) [15], which relocates the particles with respect to not only the likelihood function but also their spatial distribution. The ETPF is designed for assimilation of observational data into dynamic models using a combination of ensemble prediction techniques and sequential Bayesian model updates. Several types of ETPF are used in various applications. The general linear ensemble transform filter is an ETPF [16]. The hybrid ensemble bridges the Kalman filter with particle filter using adaptive parameters in the integration [17]. Furthermore, the R-localization method has been shown to improve the performance of ETPF [16,18]. However, the linear transformation step in these ETPFs is computationally expensive and requires relaxation matching between the two sets for robust processing. No general closed solution is available to obtain a transformation map used in the step; however, the solution depends on the dynamic and measurement models of the system. Thus, in this paper, a regularized optimal transport (ROT) method [19], which is designed to minimize the cost of transportation between the two particle sets, was used to address the computational load and relaxation matching in the resampling step. Towards this end, a modified Sinkhorn-Knopp algorithm based on the convergence rate of the process iteration was used. Although ROT reduces the computational complexity compared with the ETPF, the computation load of the proposed algorithm is still excessively high in real-time applications compared with a conventional particle filter, which is based on a simple resampling algorithm. Thus, in this paper, an additional selection algorithm was designed to measure the nonlinearity of the system to determine the need for the proposed ETPF based on the ROT resampling method. The proposed ROT-based ETPF was designed to target the tracking system using 2D LiDAR to evaluate the performance of the proposed method compared to other conventional algorithms. The 2D LiDAR is a survey method designed to measure the distance between a target and a sensor by illuminating the target with pulsed laser light and measuring the reflected pulses with a sensor. The data are similar to those derived using a radar (including noise and clutters), which is used in data assimilation and target tracking. An outline of the paper is as follows. In Section 2, the ETPF is reviewed and distinguished from the proposed algorithm. The proposed resampling method is introduced in Section 3. Subsequently, in this section, the ROT and its role in the proposed algorithm to improve the performance of the particle filter is discussed. In addition, we also elaborate on the selection algorithm, which is based on the degree of nonlinearity. In Section 4, simulations and experiments of target tracking using 2D LiDAR are compared with conventional methods. The conclusions are summarized in Section 5.

where hð Þ represents the measurement model and v n denotes the white Gaussian measurement noise with zero mean and covariance Rn 2 RNz Nz . In the sequential importance resampling (SIR) particle filter [7], important weights are updated by the following equation:

  
  T  
    exp 12 h xn i  zn R1 h xn i  zn n   
  fxn gi ¼ P T  
   M 1 xn i  zn R1 h xn i  zn n j¼1 exp 2 h

ð3Þ

where f gi refers to the i-th particle and M is the number of total particles. Based on the significant weights and the predicted states, particle filters perform the resampling process to eliminate samples with insignificant weights and multiple samples with high significance. Although the resampling reduces the effects of degeneracy, it leads to sample impoverishment, which refers to a loss of particle diversity as the resampled particles contain multiple repetitive points. Thus, the resampling method based on linear ensemble transform attenuates sample impoverishment [15]. Compared with the conventional resampling methods such as monomial or systematic resampling [6,7] the resampling is selected to minimize the expected distance between the predicted particles obtained by the propagation of prior and posterior particles.
 Given the predicted particles x n i ; i ¼ 1; . . . ; M at an observation time index n, the linear transformation is represented as follows: M X
þ
 xn j ¼ xn i  t ij

ð4Þ

i¼1


 where xþ n j denotes j-th posterior particle and t ij refers to the components of transport matrix T n 2 RMM . The optimal transport matrix T n is obtained by solving the linear transport problem as follows:

T n ¼ arg min T n 2P

M X

t ij cij

ð5Þ

i; j¼1

where cij represents the components of the cost function matrix related to the energy required to transport a probability mass from the source to the desired samples. Generally, the cost is selected as the Euclidian distance between the two sample sets as follows:



 2 cij ¼ k xn i  xn j k

ð6Þ

In Eq. (5), P denotes the candidate set of matrices T n with nonnegative components tij and expressed as follows: M X

t ij ¼ 1 ;

i¼1

M X

tij ¼ xi M

ð7Þ

j¼1

2. Ensemble transform particle filter (ETPF)

Finally, the posterior states are estimated with the optimal transport matrix as follows:

The ETPF incorporates particle resampling based on linear programming of prior and posterior particles.

M
þ  X
 xn j ¼ xn i  t ij :

Nz

The measurements of zn 2 R

obtained at time index n are

related to the filter state variables xn 2 RNx of a system model, which is expressed as follows:

xnþ1 ¼ f ðxn Þ þ wn

ð1Þ

In Eq. (1), f ð Þ represents the system model and wn is the white Gaussian noise with zero mean and covariance Q n 2 RNx Nx . The measurement model is expressed as follows:

zn ¼ hðxn Þ þ v n

ð2Þ

ð8Þ

i¼1

where t ij is components of the optimal transport matrix T n However, linear programming in the ETPF has a high computational load when the data dimension increases. The computational   complexity of linear programming is represented by O N 3 logN using network simplex or interior point methods [19]. Big O notation also called Landau’s symbol, is a symbolism used in complexity theory, computer science, and mathematics to describe the asymptotic behavior of functions. In addition, the general optimal transport mapping between complicated densities is usually irregular. When this transport plan is used to directly perform the point

365

C.H. Kang et al. / Measurement 146 (2019) 363–371

set transfer (one-to-one matching), it is possible to cause the matching error when the shape of input set distribution is altered with time. In order to obtain the optimal transport maps that are less sensitive to the change of input sets’ distribution, it is necessary to distribute the changes to each point’s distribution value. Thus, a relaxation matching (many to many matching) which is not a bijection between the point sets is needed to decrease the matching error in a practical situation. The ROT was used in the resampling process to address these challenges and improve the relaxation matching between the predicted and posterior particles.

point of view, the regularized cost function can be resolved via the differentiation of the cost function with Lagrange multipliers. Lagrange multipliers a 2 RNy 1 and b 2 RNx 1 are added to the regularized cost function and expressed as follows:

Lðp; a; bÞ ¼

X











pij cij þ cpij log pij  1 þ aT pT 1  q

i; j

þ bT ðp1  pÞ

ð13Þ

In order to obtain the optimal transport map p, the above equation is differentiated with respect to p and equate to zero as follows:

  @ pij L ¼ cij þ clog pij þ aj þ bi ¼ 0

3. ETPF with ROT and measure of nonlinearity The proposed resampling method based on ROT [20,21] and the degree of nonlinearity [22] address the computational load and relaxation matching in the resampling step by using a modified Sinkhorn-Knopp algorithm that considers the convergence rate of the process iteration.

ð14Þ

The components of the optimal transport map pij is obtained as follows:









pc;ij ¼ exp cij =c exp aj =c expðbi =cÞ

ð15Þ

Finally, the ROT in vector–matrix form can be expressed as

pc ¼ diag ðv ÞM diag ðuÞ 

3.1. ROT The resampling process of the ETPF is based on the concept of linear transportation between the prior and the posterior particles. However, a transport map of the linear programming is unavailable. Thus, in this section, linear transportation is discussed to address the ROT challenge [19]. Optimal transport is designed to minimize the cost function associated with the transportation between the probability distributions of the source and the desired outcomes using a transport map. In the case of discrete measures based on a finite number of samples, the two distributions can be expressed as

l¼

X

X

pi dxi ; m ¼

i

ð9Þ

q j d yj

j

where dxi ; dyj represent the Dirac at the location xi ; yj , respectively. The functions pi ; qj denote the probability masses associated with PNx PNy the i; jth sample under the condition i¼1 pi ¼ j¼1 qj ¼ 1. The source

and

the

desired

samples are expressed as h iT T x ¼ ½x1 ; . . . ; xNx  ; y ¼ y1 ; . . . ; yNy , respectively. The set of probabilistic couplings between these two distributions is represented by the set of doubly stochastic matrices S defined as

n o Sðp; qÞ ¼ p 2 RNx Ny ; p1 ¼ p; pT 1 ¼ q

ð10Þ

where 1 is a N dimensional vector of ones. The Kantorovitch formulation is used to obtain the optimal transport p as follows [15]:

p ¼ argmin po 2S

( X

)

ð11Þ

pij cij

ij

Generally, the cost is based on the Euclidian distance between the two sample sets as follows: cij ¼ k xi  yj k2 . However, the optimal transport entails a high computational load under increased data dimension. Therefore, the optimal transportation problem can be regularized by an entropic term as follows [19]:

pc 2 argmin 

p2S

X







pij cij þ cpij log pij  1



!

ð12Þ

i; j

where c  is  a  regularization parameter and P EðpÞ ¼ i; j pij log pij  1 refers to entropy. From an optimization



ð16Þ

where v 2 RNx 1 ; u 2 RNy 1 represent vectors with  uj ¼ exp aj =c ; v i ¼ expðbi =cÞ, respectively. diag ðÞ refers to the   diagonal matrix, and M ij ¼ exp cij =c . Accordingly, the complexity   of the algorithm can be reduced from O N3 logN to OðN logNÞ compared to the linear transport in the ETPF [19]. Thus, the map of ROT increases the efficiency of linear programming. The unknown vectors v ; u can be computed using the Sinkhorn-Knopp algorithm [19]. The Sinkhorn-Knopp algorithm is perhaps the simplest method to determine the double stochastic scaling of a non-negative matrix, pc by generating a sequence of matrices with alternating rows and columns normally. The general Sinkhorn-Knopp algorithm comprises a fixed N iter -th iteration number (known as the Sinkhorn’s fixed point iteration) and is expressed as follows [19]:

n o unþ1 ¼ pj =ðMvn Þj ;

v nþ1 ¼

n  o qi = MT unþ1 i

ð17Þ

Since the fixed point iteration involves a constant iteration number, it does not guarantee computational efficiency and the convergence of the Sinkhorn-Knopp algorithm in obtaining ROT. Therefore, a modified Sinkhorn-Knopp algorithm [20] was used in the particle filter resampling method. 3.2. ROT-based resampling The proposed resampling algorithm is based on the combination of ROT using a modified Sinkhorn-Knopp algorithm to avoid sample impoverishment and based on our previous work [21]. In addition, the proposed resampling algorithm increases the efficiency of resampling. In the modified Sinkhorn-Knopp algorithm [20], a stopping criterion for the convergence of the SinkhornKnopp algorithm is generated using the error defined as

en ¼ k v n dn  1 k In Eq. (18), dn ¼ v

ð18Þ 1 n

(with components containing an inverse

v n ), and dn is calculated as dn ¼ MT un1 , which is derived from the condition; v n MT un approximates to 1 following the convalue of

vergence of the Sinkhorn-Knopp algorithm [20]. The iteration algorithm operates until the error between the bound and design parameters exceeds the threshold. In addition, a damping factor g is added to M to ensure that it is fully indecomposable and the convergence of the Sinkhorn-Knopp algorithm as follows [20]:

366

C.H. Kang et al. / Measurement 146 (2019) 363–371



M ¼ M þ g11T

ð19Þ

The modified Sinkhorn-Knopp algorithm balancing the use of the stopping criterion for the iteration algorithm was applied to the resampling process in the particle filter. The ROT resampling under the modified Sinkhorn-Knopp algorithm was designed as shown below: Inputs: estimated posterior states (x n ) and its weight (xn ), regularization parameter (c), error bound (tol), damping factor (g) Initialization: number of particles (N), u0 ¼ 1; u0 2 RN Marginal values calculation: p ¼ N xn ; q ¼ 1; q 2 RN 2 k x  x k =c M calculation: M ¼ e ð n Þi ð n Þj ; M 2 RNN

By using the above assumptions, Lemma 3.6 and 3.7 written in [5], it is confirmed that

qffiffiffiffiffi 

D 2 1=2 E    2 cn kuk   N  D E pffiffiffiffi E Dn ; u  Dn ; u 6  N  N Dn ; 1 qffiffiffiffiffi  k u k ¼ c n pffiffiffiffi N

qffiffiffiffiffi  pffiffiffiffiffiffiffiffiffi pffiffiffiffiffiffiffiffiffi where c n ¼ c1; n þ c2; n and it is independent of N. Eq. (23) represents the error boundary of the predicted value before the resampling process in the ETPF. The coefficients of c1; n and c2; n are expressed as [5]

i; j

en ; dn calculation: en ¼ k v n dn  1 k; dn ¼ MT un1 þ gk un1 k1 While en > tol do n o
 v n ¼ qi =ðdn Þi ; un ¼ pj =ðMvn þ gk v n k1Þj Optimal map calculation: pn ¼ diag ðv n ÞM diag ðun Þ  Output: resampled states xþ n ¼ xn pn , 

weight of resampled particles xn ¼ N1 pn 1 3.3. The convergence of the ROT resampling in the ETPF The previous papers [5,24,25] presents a mathematical analysis of convergence for the particle filter and gives bounds for the mean-square error. The analysis of the filter convergence is based on the fact that the sum of two sequences converges weakly to the sum of the limits of those sequences, which follows from a basic result of real analysis on the convergence of sequences of real numbers [5]. In this section, those analysis results are extended and applied to the proposed algorithm in order to verify the convergence of the proposed algorithm. Before proving the convergence of the proposed filter, some explanation and consideration are given as follows. If hN is a sequence of measures that depend on the number of   particles N , then hN converges to h when 8u 2 B RM [24].

lim E

h

N!1

2 i  hN ; u  hh; ui ¼0

ð20Þ

  where B RM is the set of bounded Borel measurable function on RM . M is the dimension of the space. When the measure in the inner product h : ; : i is discrete, it defines the summation inner product,

D

N Dþn ;

E

u ¼

N X


x



þ n i


  u xþn i

c1;

n

¼

N h i 4C X E fxn g2i jF n1 6 4C  cw;n ; c2; N i¼1

n

¼ cn1 k h k

6 cn1  hmax

2

ð24Þ

where C is a constant value and is derived from the error boundary

   of fnn gi ¼ uðfxn gi Þ  D 0 ; u , which is a set of conditionally independent random variables given a F n [5,24]. cn1 is a constant obtained by the error boundary at time n  1 of estimate value after the resampling process written in Lemma 3.7 [5] and hmax is the maximum boundary value of the measurement model. Finally, the error boundary of estimate value after the resampling process is expressed as

D  E    2 1=2  N E Dþn ; u  Dþn ; u 6

pffiffiffiffiffiffiffiffiffi k u k cr; n pffiffiffiffi þ N

qffiffiffiffiffi pffiffiffiffiffi k u k  k u k c n pffiffiffiffi ¼ cn pffiffiffiffi N N

ð25Þ

In the case of the proposed algorithm, it is confirmed that the convergence of the mean error depends on the number of particles N, the transport errors including en (a stopping criterion for the convergence of the Sinkhorn-Knopp algorithm), the errors caused by approximation of filter models and the maximum boundary of filter models and weights of particles. In order to maintain the stability of the filter, the number of particles and the ROT-related parameter settings should be optimally selected according to the system to be applied. In the following section, we describe the algorithm that can recognize the system dynamic change for efficient application of the proposed algorithm to the real tracking system and the ROT-related parameter settings. 3.4. The measure of nonlinearity for the ROT resampling

ð21Þ

i¼1

where Dþ n refers to the measure corresponding to the probability density P ðxn jz1:n Þ. It is assumed that the resampling procedure satisfies [25]

D E D  E 2  k u k2 N  N E Dþn ; u  Dn ; u 6 cr; n N

ð23Þ

ð22Þ

where D n refers to the measure corresponding to the probability density Pðxn jz1:n1 Þ. cr; n is a constant value which is independent of Nand is related to the transport errors including en (a stopping criterion for the convergence of the Sinkhorn-Knopp algorithm) and the errors caused by approximation of filter models. Let F n1 be
 N ther-field generated by the particles xþ n1 i i¼1 [5]. It is assumed  that E x2n jF n1 6 cw;n and cw;n refers to a time-dependent sequence of constants. In addition, the measurement model hð Þ is bounded.

Sample impoverishment has less effect on the performance of the particle filter at low degrees of nonlinearity, suggesting that the system is adequate for linearization at a specific time index. Since the change in the number of particles using the KLD resampling method [21] is limited to the pre-defined specific range and it does not determine the dynamic changes in the system that affect the filter performance, there is a limit to efficient algorithm operation. Especially, it is important to decide whether the resampling algorithm is used according to system nonlinearity in order to apply the proposed algorithm to the real tracking system. Thus, as the ROT-based ETPF yields better performance under a relatively high nonlinearity of the system, the proposed algorithm is designed to be selectively used to increase performance efficiency and reduce the computational load. In general, the measure of nonlinearity (MoN) can be determined by two approaches [22]: first, nonlinearity is measured based on its separation from its closest linear one [26]. The other

367

C.H. Kang et al. / Measurement 146 (2019) 363–371

methods use the curvature of the nonlinear function at specific operating points as the measure of nonlinearity [27]. Many of these measurements are used to analyze the degree of nonlinearity, although they do not consider quantitative randomness. Therefore, under the altered quality of the system measurement, the value of MoN is changed in time regardless of nonlinearity. Thus, in this paper, the designed MoN was based on the randomness of the measured quality, which is based on the combination of two previous studies [22,28]. The main concept of the linear behavior detection is based on nonlinearity of the measurement model, hðxn Þ within the bound of x n (prediction values obtained from the time update in the particle filter) compared with the pre-designed threshold. The particle filter transforms the particles based on the function of the nonlinear measurement model, hðxn Þ to yield the predicted measurement
 points, z n i as follows:


 
  zn i ¼ h xn i

ð26Þ

where subscript, i refers to the i-th particle. By using the weighted least square method, it is possible to find the transport parameter, k of a linear transformation (k 2 RM1 , M is the degree of state variables), which minimizes the Euclidean distance between the lin

 earized point set, x n i ki and zn i (ki is the i–th component of k). The nonlinearity of the given system is measured by






z n 1;






z n 2; 




 T z n N ,

ð27Þ X n






x n 1;






x n 2;

where ¼ ; ¼ ;
 T xn N  . Then, optimal k is obtained by minimizing N ðkÞ with the weighted least squares method as follows: 

k ¼ argmin NðkÞ ¼ k



1   T T   X n R1 X n R1 n Xn n Zn

ð29Þ

The diagð Þ in Eq. (29) represents the diagonal matrix and its component, Di refers to the weighted distance which is expressed as follows:


  Di ¼ fxn gi k xn i  E X n k

Initialization: number of particles (N), u0 ¼ 1; u0 2 RN For i ¼ 1 : N
 
þ   xn1 i Predicted particles: x n i ¼ f Calculate fxn gi using Eq. (3) end For P Summation of weights: Xn ¼ N i¼1 fxn gi For i ¼ 1 : N Normalize fxn gi ¼ X1 n fxn gi end For Calculate weighted distance Di using Eq. (30) weight matrix Rn using Eq. (29) 

optimal transport parameter k using Eq. (28)   Measure of the nonlinearity N k using Eq. (27)  If N k > T N Perform the modified ROT resampling algorithm written in Section 3.2  Output: resampled states xþ n ¼ xn pn , weight of resampled particles xn ¼ N1 pn 1 else Randomly select particles according to the weight using systematic resampling Output: resampled states xþ n end If

ð28Þ

The R represents the weight matrix (R 2 RNN ) and is used to increase the accuracy of MoN. With respect to the nonlinear measurement function hðxn Þ, the components of R strongly depend on the distance between the predicted points of the particle filter. This dependence is unexpected for the measurement of nonlinearity and is strongly related to the quality of measurement in the previous step. In order to reduce the effect of this error, the residues are compensated by the weighting factor. The weight matrix is defined by the following equation:

Rn ¼ diag ðD1 ; D2 ;    ; DN Þ

Inputs: estimated posterior state (xþ n1 ), regularization parameter (c), error bound (tol), damping factor (g), MoN threshold (T N ), measurement (zn ), measurement noise variance (Rn )



 T    NðkÞ ¼ Z n  X n k R1 Z n  X n k n Z n

Algorithm 1 ETPF using ROT resampling algorithm with MoN

ð30Þ

In the above equation, fxn gi represents i-th particle weight and   is expressed by Eq. (3). E X  n refers to the expectation value of X n 
 P N and is calculated by N1 i¼1 x n i . Finally, N k is used to measure the nonlinearity of the model and expresses a difference between the measurement function, hð Þ and the linearized function, which 
 is closest to the specific particle, x n i . The value of N k is used in the proposed algorithm for the selection of the appropriate resampling method as follows:

The guidelines for selection of values (non-linearity threshold, damping factor, regularization parameter, and error bound for stopping the iterative computation of optimal transport map) expressed in the proposed algorithm are explained to apply the proposed algorithm to the real tracking system as follows: First, the non-linearity threshold is based on a predicted measurement error derived from updating the estimation error of state variables with the measurement model and covariance (T N P hðeÞ þ R). The effect of the damping factor on the transport matrix is summarized in Fig. 1. In this figure, the x-axis refers to the iteration number for obtaining the transport matrix, the yaxis represents en , and tol is the error bound of en . As shown in this figure, the damping factor g reduces the convergence time of the transport matrix. However, the value of the damping factor should be based on an appropriate value below the minimum distance between the estimated posterior samples to prevent a mismatch between the posterior and the resampled particle sets following defective transport matrix. In this paper, in order to prevent mismatch and to maintain the operation within 50 iterations for efficiency, the tol was set to 0.3. The g was set to 0:001  tol. 4. Simulations and experiments In this section, the resampling performance of the proposed algorithm was compared with conventional resampling algorithm using two performance indices of the resampling for the simulation that replicates the ideal case of a 2D single target tracking system using LiDAR. In addition, the proposed algorithm was

368

C.H. Kang et al. / Measurement 146 (2019) 363–371

Fig. 1. The effect of the damping ratio on the ROT problems.

implemented to target tracking system using 2D LiDAR for evaluation of the tracking performance of the proposed method compared with other conventional algorithms. 4.1. Characteristic of the 2D LiDAR measurements The 2D LiDAR (UTM-30TX) was used in the experiment and configured to 0:25 resolution with 270 field-of-view (FOV). The position of 2D LiDAR was fixed (stationary condition) and set as the reference point of the frame for convenient analysis of the experiments as shown in Fig. 2. In addition, the FOV was restricted to 90 in this experiment. All quantities related to the target with respect to the reference frame are shown in Fig. 2. The figure illustrates the targets, the reference frame, and the target measurements (range and direction angle from the y-axis). Each of the measurements was obtained by sweeping along the reference frame of the x-y plane, and the initial measurement involved the angle pointing in the positive axis [29]. In addition, the shape of the target was cylindrical, which facilitated easy recognition and attachment to a mobile robot (Pioneer 3-AT). Under experimental conditions, the target moves in the twodimensional plane according to a second-order spatial model similar to the targets reported previously [30–32]. The state model is

Fig. 2. Experimental coordinates.

3 3 2 2 1 T 0 0 T =2 0 6 T 60 1 0 07 0 7 7 7 6 6 xk ¼ Fxk1 þ Gvk F ¼ 6 7 7; G ¼ 6 2 4 0 40 0 1 T 5 T =2 5 0 0 0 1 0 T 2

ð31Þ

In Eq. (31), the states are selected as follows:  T xk ¼ px; k ; p_ x; k ; py; k ; p_ y; k . The px; k ; py; k refer to the twodimensional configuration, and p_ x; k ; p_ y; k refer to the two dimensions of velocity. The process noise Gvk refers to  T zero-mean Gaussian white noise, and in v k ¼ v k;1 ; v k;2 ;     v k;1 N 0; r2v ;1 ; v k;2 N 0; r2v ;2 ;rv denotes the standard deviation. Other parameters are set to T ¼ 1s;r2v ;1 ¼ r2v ;2 ¼ 0:001. The measurements for an observer are obtained by

2

 3 Tan1 px; k =py; k ffi 5 þ wk zk ¼ 4 qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi p2x; k þ p2y; k

ð32Þ

where wk is zero-mean Gaussian white noise vector with zero mean and the variance r2h ; r2r . The initial state of the target is x0 ¼ ½0:050; 0:001; 0:700; 0:055T and initial mean and standard deviation of all particles are assumed as follows:

Fig. 3. Experiment equipment (2D LiDAR and mobile robot).

C.H. Kang et al. / Measurement 146 (2019) 363–371 

T

x0 ¼ ½8:7; 0:0; 22:3; 0:0 ;r2x;1 ¼ 0:5;

r2x;2 ¼ 0:05; r2x;3 ¼ 0:3; r2x;4 ¼ 0:01,

respectively. In the experiments, the target on the mobile robot (Pioneer 3-AT) shown in Fig. 3, moves to the right side of the reference frame with constant velocity (shown in Fig. 2). In addition, the measured data are shown in Fig. 2 was calculated using 2D LiDAR (range and bearing). The initial particle number of all particle filters is set to 500. This value is a pre-designed value and the appropriate value in accordance with the tracking system to be deal with in this paper. 4.2. Simulations for evaluation of the proposed resampling algorithm In the studies [30,31], the resampling algorithms of the particle filter were evaluated in terms of resampling quality and computational complexity. Set restriction [23], stratification [33] and unification techniques [29,34] were used to analyze the quality of resampling algorithms and Oð Þ (O-function) and relative computation time obtained by MATLAB function were used to analyze the computational load. In this study, in order to evaluate the resampling performance, the similarity between posterior particles and the resampled particles was used. The similarity, D1 is based on the concept of the set restriction and is defined as follows [21]:

D1 ¼

r ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi h i 2  þ E xk  E xþk

ð33Þ

 where E xþ k represents the expectation of posterior particles and is h i P
 þ refers to the obtained by the sample mean, N1 Ni¼1 xþ k i . E xk expectation of resampled particles. D1 is used to compare the difference between the posterior and the resampled particles. A resampling algorithm is considered efficient if the difference is closer to zero. In addition, the measure of uniformity (D2 ) is based on Euclidean distance between resampled particles and uniform distribution. This parameter is similar to unification and is measured as follows [21]:

D2 ¼

epos

qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi   ffi ^x; n 2 þ py; n  p ^y; n 2 ¼ px; n  p

i

^_ ; p ^_ ^x; k ; p ^y; k ; p where p x; k y; k represent resampled estimation results of state variables and are rearranged in descending order according to the magnitude of value. U k represents the uniform distribution of points and is also rearranged according to the order of the resampled results. D2 is calculated using two rearranged state variables. In the case of uniformly distributed points, the interval of U k is set to by using minimum and maximum values of the resampled estimation results which correspond to U k in each square term, respectively and is generated by MATLAB function (rand). If the resampling method ensures uniform distribution of the resampled particles (suggesting that the resampling algorithm ensures even distribution of the particles in the boundary of sample variance), the value of uniformity is close to zero. Thus, the algorithm with a D2 value closer to zero is deemed to represent a better resampling method. Finally, the tracking error between the true values and the estimates is used for performance comparison. The tracking position error is calculated by the following equation:

ð35Þ

  where px; k ; py; k represents the true target position in the simulation trajectory. Monte Carlo simulations were performed to compare the estimates with 100 runs of random trajectories independently using MATLAB, which uses the same system and measurement model discussed in Section 4.1. Table 1 shows the performance of resampling and tracking based on Monte Carlo simulations. First, the four parameters indicate the resampling performance and the last parameter represents the tracking performance related to the estimation of the target position. The relative computation time is indicated in Table 1 using MATLAB function (tic/toc function, simulation computer specification; CPU: i5-4790K, and RAM 16G) and represents the mean of the whole computation time of the simulations (simulation length: 200-time index data). The initial particle number of all particle filters is written in Table 1. (in the case of KLD resampling, the maximum sample size for the KLD resampling method is 500 [21]). In the simulations, the conventional algorithms were selected to compare the filter performance such as SIR particle filter using systematic resampling representative of the particle filter [6]. In addition, the ETPF [15] discussed in Section 2 was also selected for performance comparison. The ROT-based ETPF with KLD resampling [21] which uses the proposed resampling method except for the selected resampling algorithm based on MoN was included for performance comparison. Instead, KLD resampling technique is added to the ROT-based ETPF. In order to increase the resampling effect of the filter, the resampling process is almost performed when the number of effective numbers becomes smaller than a certain threshold. However, in this paper, we focus on system nonlinearity rather than the effective number influence on the filter and implement an algorithm to operate resampling for efficiently applying the proposed algorithm to the 2D-LiDAR tracking system. In order to improve the resampling effectiveness, we will add the

ffi sffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 2
2 n 2 n 2  o o X 
  ^_ ^_ ^y; k  fU k gi þ p ^x; k  fU k gi þ p  U þ p  U f g f g p k i k i x; k y; k i i i

369

i

ð34Þ

resampling process by effective number later. However, we need further study on the effects of the combination of nonlinearity discrimination and effective number discrimination. As shown in the table, the proposed algorithm yields better resampling performance compared to the conventional algorithm except that the ROT-based ETPF involving selective use of the resampling method. In terms of computational complexity, the proposed algorithm is more complicated than systematic resampling. However, the proposed algorithm potentially reduces the computational complexity by using the modified ROT-based resampling compared to the ETPF, which calculates the cost of optimal transport distance with two point clouds each of size N in a general metric space [35]. Furthermore, using the resampling method based on MoN, the proposed algorithm reduces the computation time compared to the ROT-based ETPF with the KLD resampling (written as ETPF with ROT resampling and KLD in the table) [21]. As the size N of the particle filter with systematic resampling increases approximately similar to the run-time with

370

C.H. Kang et al. / Measurement 146 (2019) 363–371

Table 1 The performance of resampling and tracking. Types of filters with resampling methods

D1

D2

Computational Complexity

Relative computation time (s)

RMSE

Particle filter with systematic resampling (N ¼ 500) Particle filter with systematic resampling (N ¼ 5000) Particle filter with systematic resampling (N ¼ 33000) ETPF (N ¼ 500)

5.7821e-05 5.3125e-05 4.8817e-05 2.0784e-05

1.2572 1.0218 0.9041 0.7930

OðN Þ OðN Þ OðN Þ   O N 3 logN

0.0200 0.2048 0.9426 4.2056

0.2684 0.2167 0.1823 0.0844

ETPF with ROT resampling and KLD (N ¼ 500) ETPF with ROT resampling and MoN (proposed) (N ¼ 500)

1.7010e-05 1.7005e-05

0.0450 0.0455

OðNlogN Þ OðNlogN Þ

1.5110 0.9134

0.0425 0.0422

1e  05 ¼ 1  105

Fig. 4. Estimated positions of the target (unit: meter).

Fig. 5. Position errors of particle filters (unit: meter).

the proposed filter, the RMSE of the particle filter with systematic resampling is reduced, although it is still larger than that of the proposed algorithm as shown in Table 1. In addition, it utilizes data memory, which is required for saving particle points compared to the proposed algorithm. Thus, it might be constrained to apply to embedded systems with limited system memory.

4.3. Experimental results The position of the 2D LiDAR was fixed and the target randomly moved away from the 2D LiDAR.

In the experiment, the reference was to the true trajectory or path originating in the observer location shown in Fig. 3. In addition, the measured trajectory was inferred from the target measurement written in Eq. (32). To fairly compare the performance, the particle number of all particle filters was set to 500 in the experiment. In Fig. 4, the positions estimated from the proposed and comparative algorithms are shown. The SIR particle filters with systematic resampling [6], the ETPF [15], and the ETPF with ROT resampling and KLD [21] were compared to evaluate the resampling performance. Because the y-axis resolution is increased in the figures, the conventional particle filter seems to diverge over time. In addition, the estimated error increases because the target is further away from the observation site (increase measurement uncertainty). However, it is confirmed that the error covariance’s norm of all filters in the experiment is converged to a certain value within the pre-designed error square boundary. The experimental results were similar to those of simulations. Fig. 5 shows the estimated experimental error. In Fig. 5, the ETPF with ROT resampling and MoN yield slightly better performance than the ETPF, and the improved performance compared with the SIR particle filter with systematic resampling. The ETPF with ROT resampling and MoN yielded a similar tracking performance compared with ETPF with ROT resampling and KLD. In the case of ETPF, linear programming between the prior and the posterior particles resulted in a mismatch, and the performance was easily influenced by measurement noise because it does not use the regularized cost function for relaxation matching. On the other hand, the ETPF with ROT resampling and MoN uses the ROT map to ensure the relaxation match between the posterior and the desired particles. Table 2 summarizes the performance comparison results of experiments. Root mean square error (RMSE) represents the tracking performance related to the estimation of the target position. In the previous work [21], the number of particles in the proposed algorithm is adjusted according to the criteria based on KLD resampling. Although the algorithm always uses a small number of particles compared to other resampling methods based on KLD resampling [21], there is a limit to recognize the change of the system model that affects the filter performance by KLD and it can be judged effectively by the MoN. Table 2 Performance comparison results. Types of filters with resampling methods

Relative computation time (s)

RMSE

Particle filter with systematic resampling (N ¼ 500) Particle filter with systematic resampling (N ¼ 5000) Particle filter with systematic resampling (N ¼ 33000) ETPF (N ¼ 500) ETPF with ROT resampling and KLD (N ¼ 500) ETPF with ROT resampling and MoN (proposed) (N ¼ 500)

0.0328

0.2752

0.3557

0.2243

0.9978

0.1897

4.3415 1.5820

0.0883 0.0445

0.9459

0.0439

C.H. Kang et al. / Measurement 146 (2019) 363–371

In addition, the number of particles affects the convergence value of the filter as described in Section 3.3 and its change influences filter stability. If the number of particle changes is large, filter stability may be poor in dynamic situations. As can be shown from the experimental results, it is more efficient to selectively use the resampling method compared to other methods. As with result graphs and simulation results, it is confirmed that the proposed algorithm has the best-estimated performance. 5. Conclusions A resampling method based on ROT was proposed to improve the performance of the ETPF. A modified Sinkhorn-Knopp algorithm based on the convergence of the algorithm was used to obtain the optimal transport map. In addition, the proposed algorithm correlated with the degree of nonlinearity such that the ROT resampling was only used in models considered to be nonlinear for increasing the efficiency of the proposed algorithm. Simulation and experimental results of target tracking using 2D LiDAR showed that the proposed algorithm yielded better performance compared to the particle filters with conventional resampling methods. Declaration of Competing Interest None. Acknowledgments This work was supported by the Ministry of Science and ICT of the Republic of Korea through the Space Core Technology Development Program under Project NRF-2018M1A3A3A02065722. In addition, This research was supported by Unmanned Vehicles Advanced Core Technology Research and Development Program through the National Research Foundation of Korea (NRF), Unmanned Vehicle Advanced Research Center (UVARC) funded by the Ministry of Science, ICT and Future Planning, the Republic of Korea (NRF-2016M1B3A1A01943689). References [1] B. He, L. Ying, S. Zhang, X. Feng, T. Yan, R. Nian, Y. Shen, Autonomous navigation based on unscented-FastSLAM using particle swarm optimization for autonomous underwater vehicles, Measurement 71 (2015) 89–101. [2] W. Hernández, C. Calderón-Córdova, V. González-Posada, Á. Parra-Cerrada, J.L. Jiménez, J.E. González-Garcia, O.Y. Sergiyenko, Bootstrap-based frequency estimation method, Measurement 95 (2017) 193–200. [3] L. Martino, A review of multiple try MCMC algorithms for signal processing, Digital Signal Process. 75 (12) (2018) 134–152. [4] S. Särkkä, Bayesian Filtering and Smoothing, Cambridge University Press, 2013. [5] I.S. Mbalawata, S. Särkkä, Moment conditions for convergence of particle filters with unbounded importance weights, Signal Process. 118 (2016) 133–138. [6] T. Li, M. Bolic, P.M. Djuric, Resampling methods for particle filtering: classification, implementation, and strategies, IEEE Signal Process. Mag. 32 (3) (2015) 70–86.

371

[7] B. Ristic, S. Arulampalam, N.J. Gordon, Beyond the Kalman filter: Particle Filters for Tracking Applications, Artech house, 2004. [8] P. Fearnhead, Sequential Monte Carlo Methods in Filter Theory (Doctoral dissertation), University of Oxford), 1998. [9] N.J. Gordon, D.J. Salmond, A.F. Smith, Novel approach to nonlinear/nonGaussian Bayesian state estimation, IEE Proc. F (Radar and Signal Processing) 140 (2) (1993) 107–113, IET Digital Library. [10] A. Doucet, N. De Freitas, N. Gordon, An introduction to sequential Monte Carlo methods, in: Sequential Monte Carlo Methods in Practice, Springer, New York, 2001, pp. 3–14. [11] W.R. Gilks, C. Berzuini, Following a moving target—Monte Carlo inference for dynamic Bayesian models, J. Royal Stat. Soc.: Series B (Statistical Methodology) 63 (1) (2001) 127–146. [12] S. Park, J.P. Hwang, E. Kim, H.J. Kang, A new evolutionary particle filter for the prevention of sample impoverishment, IEEE Trans. Evol. Comput. 13 (4) (2009) 801–809. [13] U. Orguner, F. Gustafsson, Risk-sensitive particle filters for mitigating sample impoverishment, IEEE Trans. Signal Process. 56 (10) (2008) 5001–5012. [14] T. Li, T.P. Sattar, S. Sun, Deterministic resampling: unbiased sampling to avoid sample impoverishment in particle filters, Signal Process. 92 (7) (2012) 1637– 1645. [15] S. Reich, A nonparametric ensemble transform method for Bayesian inference, SIAM J. Sci. Comput. 35 (4) (2013) A2013–A2024. [16] S. Reich, C. Cotter, Probabilistic Forecasting and Bayesian Data Assimilation, Cambridge University Press, 2015. [17] M. Frei, H.R. Künsch, Bridging the ensemble Kalman and particle filters, Biometrika (2013) ast020. [18] B.R. Hunt, E.J. Kostelich, I. Szunyogh, Efficient data assimilation for spatiotemporal chaos: a local ensemble transform Kalman filter, Phys. D: Nonlinear Phenomena 230 (1) (2007) 112–126. [19] M. Cuturi, Sinkhorn distances: lightspeed computation of optimal transport, in: Advances in Neural Information Processing Systems, 2013, pp. 2292–2300. [20] P.A. Knight, The Sinkhorn-Knopp algorithm: convergence and applications, SIAM J. Matrix Anal. Appl. 30 (1) (2008) 261–275. [21] C.H. Kang, C.G. Park, Particles resampling scheme using regularized optimal transport for sequential Monte Carlo filters, Int. J. Adaptive Control Signal Process. 32 (10) (2018) 1393–1402. [22] Y. Liu, X.R. Li, Measure of nonlinearity for estimation, IEEE Trans. Signal Process. 63 (9) (2015) 2377–2388. [23] C.P. Robert, Monte Carlo methods, John Wiley & Sons Ltd., 2004. [24] D.E. Clark, J. Bell, Convergence results for the particle PHD filter, IEEE Trans. Signal Process. 54 (7) (2006) 2652–2661. [25] D. Crisan, A. Doucet, A survey of convergence results on particle filtering methods for practitioners, IEEE Trans. Signal Process. 50 (3) (2002) 736–746. [26] A. Helbig, W. Marquardt, F. Allgöwer, Nonlinearity measures: definition, computation and applications, J. Process Control 10 (2) (2000) 113–123. [27] D.M. Bates, D.G. Watts, Relative curvature measures of nonlinearity, J. Royal Stat. Soc. Series B Methodol. (1980) 1–25. [28] O. Straka, J. Duník, M. Šimandl, Unscented Kalman filter with advanced adaptation of scaling parameter, Automatica 50 (10) (2014) 2657–2664. [29] S. Chakraborty, S. Chattaraj, A. Mukherjee, Performance evaluation of particle filter resampling techniques for improved estimation of misalignment and trajectory deviation, Multidimen. Syst. Signal Process. (2017) 1–18. [30] A. Vu, S. Kato, Targeted trajectories optimization from 2D LIDAR measurements, in: 2013 IEEE Intelligent Vehicles Symposium (IV), IEEE, 2013, pp. 919–924. [31] T. Kim, J. Kim, S.W. Byun, A comparison of nonlinear filter algorithms for terrain-referenced underwater navigation, Int. J. Control Autom. Syst. 16 (6) (2018) 2977–2989. [32] M. Rabah, A. Rohan, M. Talha, K.H. Nam, S.H. Kim, Autonomous vision-based target detection and safe landing for UAV, Int. J. Control Autom. Syst. 16 (6) (2018) 3013–3025. [33] W.G. Cochran, Sampling Techniques, John Wiley & Sons, 2007. [34] J.D. Hol, T.B. Schon, F. Gustafsson, On resampling algorithms for particle filters, in: 2006 IEEE Nonlinear Statistical Signal Processing Workshop, IEEE, 2006, pp. 79–82. [35] O. Pele, M. Werman, Fast and robust earth mover’s distances, in: 2009 IEEE 12th International Conference on Computer Vision, IEEE, 2009, pp. 460–467.