Combining IMM Method with Particle Filters for 3D Maneuvering ...

Combining IMM Method with Particle Filters for 3D Maneuvering Target Tracking Pek Hui Foo Department of Physics National University of Singapore Singapore [email protected] Abstract - The Interacting Multiple Model (IMM) algorithm is a widely accepted state estimation scheme for solving maneuvering target tracking problems, which are generally nonlinear. During the IMM filtering process, serious errors can arise when a Gaussian mixture of posterior probability density functions is approximated by a single Gaussian. Particle filters (PFs) are effective in dealing with nonlinearity and non-Gaussianity. This work considers an IMM algorithm that includes a constant velocity model, a constant acceleration model and a 3D turning rate (3DTR) model for tracking three-dimensional (3D) target motion, using various combinations of nonlinear filters. In existing literature on combining IMM and particle filtering techniques to tackle difficult target maneuvers, a PF is usually used in every model. In comparison, simulation results show that by using a computationally economical PF in the 3DTR model and Kalman filters in the remaining models, superior performance can be achieved with significant reduction in computational costs. Keywords: Maneuvering target tracking, interacting multiple model, particle filter.

1

Introduction

Due to the increasingly widespread use and sophistication of military and civilian surveillance systems, much interest has been generated in the development of algorithms for target tracking. Target tracking problems can be solved by modeling dynamical systems. For linear Gaussian problems, the Kalman filter (KF) [10] can be applied to obtain optimal solutions. However, nonlinearity and/or non-Gaussianity often exist in target tracking problems. Nonlinear filtering techniques are required to solve such problems. For non-maneuvering target tracking, an extended Kalman filter (EKF) or an unscented Kalman filter (UKF) [1] is usually implemented to provide Gaussian approximation to the posterior probability density function (pdf) in the state space. The former uses the first-order Taylor series expansion of the nonlinear system equations that describe the given problem. The latter uses multiple deterministically chosen points in the state space to

Gee Wah Ng Advanced Analysis and Fusion Laboratory DSO National Laboratories Singapore [email protected] approximate the state distribution. Another group of popular techniques is the class of sequential Monte Carlo (SMC) methods, also known as the particle filters (PFs) [1]. These methods do not have the limitation imposed by the Gaussian assumption required for EKFs and UKFs. Hence, they can be applied to problems with arbitrary nonlinearities in dynamics and/or measurements, including those with non-Gaussian process and/or measurement noises acting on the system. Random samples (or particles) are generated for the direct estimation of the posterior pdf. It is generally difficult to use a single model to represent the motion of a maneuvering target, as the maneuvers are often abrupt deviations from preceding motion. Hence, multiple model (MM) based approaches are often used for maneuvering target tracking [9][10]. The models in these methods run in parallel and describe different aspects of the target behavior. In particular, the Interacting Multiple Model (IMM) algorithm [9][10][12] is widely accepted as one of the most cost-effective dynamic MM methods. It has been shown to achieve high performance with relatively low complexity. However, an essential operation of the IMM filtering process, the mixing/interaction step (details given in Section 3.4), yields a Gaussian mixture of posterior pdfs. The IMM algorithm approximates this non-Gaussian element by a single Gaussian, which often results in serious errors. In the recent years, researchers have developed techniques for solving nonlinear target tracking problems by combining MM based approaches (to account for mode switching) and PF variants (to account for nonlinear and/or non-Gaussian characteristics of the posed problem). A few instances are given below. In [2], a PF with a switching/interaction step of the same form as that in the IMM algorithm was developed for stochastic hybrid systems. A regularized particle filter (RPF) was adopted in every model of the IMM algorithm in [3]. Instead of resampling, a Gaussian sum pdf was computed to approximate the conditional posterior pdf for the state in each mode. This method involved high computational complexity and additional approximations. In an improved approach [6], direct sampling from each mode conditional posterior pdf (a weighted sum of distributions) was implemented, in place of approximation with Gaussian mixtures. An unscented particle filter

(UPF) and a generic/standard particle filter (SPF) were applied in each model of the IMM algorithm in [5] and [13] respectively. In this work, we implement an IMM algorithm that comprises a constant velocity (CV) model, a constant acceleration (CA) model and a 3D turning rate (3DTR) model, with various combinations of nonlinear filters used for the models. Existing variants usually use a KF or a PF in every model. A new approach which uses a PF in the 3DTR model and KFs in the remaining models is considered. Simulation results show that new variants that use computationally economical PFs in the 3DTR models perform well for a maneuvering target tracking problem. The rest of the paper is organized as follows. Section 2 presents the formulation of the dynamical system model for maneuvering target tracking in this work. Section 3 gives a brief discussion on the IMM algorithm and the nonlinear filters to be used for the models. Section 4 contains details of the simulation tests conducted. Analysis and comparison of the numerical results obtained are also reported. Concluding remarks are given in Section 5.

Equation (1) defines the probabilistic model of the state evolution (transition prior), p(Xk|Xk-1), while equation (2) defines the likelihood function of the current measurement given the current state, p(Zk|Xk). In Bayesian tracking, it is of interest to obtain the posterior pdf via the recursive computation of • the prior pdf (prediction density) of the state, p(Xk|Z1:k-1), and • the marginal posterior pdf (filtering density) of the state, p(Xk|Z1:k). For most nonlinear problems, the posterior pdf cannot be determined analytically. Hence, tractable filtering methods are required to obtain approximate solutions for such problems.

2

EKFs [1][11] are commonly used for solving nonlinear target tracking problems. An EKF uses first-order Taylor series expansions to approximate nonlinear system functions in equations (1) and (2). It provides a Gaussian approximation to the marginal posterior pdf of the system state through its conditional mean and covariance. For problems with mild nonlinearity, an EKF provides satisfactory results with much efficiency. However, when problems are highly nonlinear and the effects of the higher order terms of the Taylor series expansions are not negligible, approximation results obtained with an EKF are prone to errors. In such situations, the filter is likely to perform badly or even diverge. There exists a second-order EKF which takes into consideration second-order terms in the Taylor series expansions. However, it is not widely used because of increased complexity and implementation costs. There is also no guarantee of improved results.

Problem Formulation

Consider a state-space model of a maneuvering target tracking system [10] given by the process equation Xk +1 = f(Xk , wk ),

(1)

and the observation equation Zk +1 = h(Xk +1, vk +1 ),

(2)

where the system transition function f(⋅) and measurement function h(⋅) are usually nonlinear. The discrete-time forms of equations (1) and (2) are given by

and

Xk +1 = Fk Xk + Gk w k

(3)

Zk +1 = Hk +1Xk +1 + vk +1

(4)

respectively. For the k-th scan/step, Xk is an nx × 1 state vector, Zk is an nz × 1 measurement vector, wk is an nw × 1 process noise vector, vk is an nz × 1 (additive) measurement noise vector, Fk is the nx × nx transition matrix, Gk is the nx × nw process noise gain matrix and Hk is the nz × nx measurement matrix. Let the initial pdf, p(X0|Z0) ≔ p(X0), of the state vector (also known as the prior) be assumed to be available, with Z0 being the set of no measurements. The complete solution to the state estimation problem is the true posterior pdf, p(X0:k|Z1:k), where X0:k ≔ {X0, …, Xk} and

Z1:k ≔ {Z1, …, Zk}.

3

Filtering Algorithms

The nonlinear filters to be used for the models in the IMM algorithm in this work are briefly discussed in this section. Details on the filters can be found in the respective cited references.

3.1

3.2

Extended Kalman Filters

Unscented Kalman Filters

A UKF [11] uses a minimal set of deterministically chosen sample points (or “sigma points”) to obtain a Gaussian approximation to the marginal posterior pdf of the system state. For an arbitrary nonlinear problem, the posterior mean and covariance of the state can be accurately estimated to the second order (third order for a Gaussian prior) of the Taylor series expansions. UKFs have been reported to outperform EKFs in many problems at no additional computational costs (the two groups of filters have almost the same computational complexity). However, UKFs also have the limitation that they cannot be applied to general non-Gaussian problems.

3.3

Particle Filters

Most PFs developed over the past years are based on sequential importance sampling (SIS) [1]. In practice, the posterior pdf is usually represented by a set of random particles drawn from a known importance density function (also known as proposal distribution). Then a corresponding importance weight is computed for each particle. Posterior estimates are computed based on the samples and their associated weights. As the sample size becomes increasingly large, by the Strong Law of Large Numbers, the approximation converges to the true posterior pdf [11]. It is not easy to design an importance density function that provides reasonable approximation of the posterior pdf. A convenient choice of importance density function is the transition prior, which is simple and easy to implement. The idea of resampling [1][11] was introduced to eliminate particles with low importance weights and multiply particles with high importance weights, with the number of particles, Ns, kept unchanged. This is useful in alleviating the adverse effects of the degeneracy phenomenon in which all but very few of the importance weights become negligible. Resampling is to be carried out when the effective sample size Neff (a measure of degeneracy) falls below some predetermined threshold NT (here, NT = 2Ns/3). As it is not possible to evaluate Neff exactly, an estimate of Neff is used during practical implementation. A variety of resampling schemes is available in the literature [1]. In this work, systematic resampling [1] is used. There are several PF variants that do not require resampling. An example, the Gaussian particle filter (GPF), is reviewed in Section 3.3.5.

3.3.1 Standard Particle Filter The SIS and resampling techniques discussed above form the core composition of the SPF described by Algorithm 3 in [1].

3.3.2 Regularized Particle Filter The introduction of resampling can result in the loss of diversity among the particles, especially when the noise in the dynamical system is small or even negligible. In such a case, the resultant sample set would contain many repeated points in the state space, hence giving a poor representation of the posterior pdf. RPFs [1] were developed with the aim of providing a remedy for the above mentioned problem, called sample impoverishment. A regularization step is added when resampling is being conducted in the SPF. The kernel method is used to obtain a regularized empirical representation of the the true posterior pdf. In this work, the RPF used is adapted from Algorithm 6 in [1], with the Epanechnikov kernel replaced by the Gaussian kernel.

The importance density function used is the transition prior.

3.3.3 Extended Kalman Particle Filter The importance density function used in the SPF, the transition prior, does not take into consideration the most recent measurement data. Consequently, deficiency may arise in PFs, especially when there is little overlap between the importance density function and the posterior pdf. To avoid problems that may arise from using the transition prior as the importance density function, researchers have come up with various linearization-based approaches which incorporate the latest measurement data. One example is the extended Kalman particle filter (EKPF) [11]. For each particle in this framework, a separate EKF is used to generate and propagate a Gaussian importance density function.

3.3.4 Unscented Particle Filter For each particle in the framework of the EKPF, use a UKF instead of an EKF for generating the importance density function. The resultant filter is known as the UPF [11].

3.3.5 Gaussian Particle Filter The GPF is an instance of several particle filtering techniques that do not require resampling [7][8]. Like SIS based PFs, the GPF uses importance sampling to obtain particles. It approximates the posterior pdf by single Gaussians, similar to Gaussian filters such as EKFs. The GPF propagates only the mean and covariance of the posterior pdf. However, higher moments can be propagated due to the fact that all moments can be estimated using importance sampling. The GPF usually outperforms conventional Gaussian filters, especially for solving problems with nontrivial nonlinearities. It also has lower computational complexity than PFs which require resampling, a process that may be computationally expensive.

3.4

The IMM Algorithm

The IMM algorithm is built from several dynamic motion models that represent different target behavioral traits. The models can switch from one to another according to a set of transition probabilities governed by an underlying Markov chain. One complete cycle of the IMM algorithm comprises four parts, namely, an input mixer (interaction), a filter for each model, a model probability evaluator and an output mixer (combination). The flow diagram of an IMM algorithm with r models is shown in Figure 1. Here, r = 3. Table 1 shows the various combinations of filters considered for the respective models.

An outline of the k-th (k ≥ 1) cycle of a typical IMM algorithm variant implemented in this work is given below [10][12]. Step 1: Input mixer (interaction) Mode-conditioned state estimates and state error covariances of the previous step are merged using mixing probabilities for the initialization of the current step. For model j at time k, Mj(k), compute • the initial state estimate,

• the combined state estimate, r

xˆ(k | k) = ∑ µi (k)xˆ i (k | k), i =1

• and the corresponding state error covariance, r Pˆ(k | k) = ∑ µ (k ){Pˆ (k | k) + [ xˆ (k | k) − xˆ (k | k )][⋅]T }. i =1

i

i

i

{ xˆ j ( k − 1 | k − 1), Pˆ j ( k − 1 | k − 1)}

r

xˆ 0j (k -1 | k -1) = ∑ xˆ i (k −1 | k − 1)µi| j (k − 1 | k − 1),

{µ j ( k − 1)}

Mixing/Interaction

i=1

• and the corresponding state error covariance, Pˆj0 (k - 1 | k - 1) =

{xˆ0j (k −1| k −1),Pˆj0(k −1| k −1)}

z (k )

r T ~ ⋅ }, ∑ µ i| j (k − 1 | k − 1){Pˆi (k - 1 | k -1) + [x i (k − 1 | k − 1)][] i=1

where xˆ j (k − 1 | k − 1) is the prior state estimate, Pˆ (k − 1 | k − 1) is the corresponding state error covariance,

Filtering M1(k)

…

Mr(k)

j

µ i| j (k − 1 | k − 1) = c -1j p ij µ i (k − 1) is mixing probability,

{Λj (k)}

{xˆ j (k | k ), Pˆj (k | k )}

Mode probability update

r

c j = ∑ pijµi (k −1) is a normalizing constant,

{µ j ( k )}

Combination

i=1

~ x i (k − 1 | k − 1) = xˆ i (k − 1 | k − 1) − xˆ 0j (k − 1 | k − 1), and pij is the assumed transition probability for switching from model i (at time k-1) to model j (at time k).

{ xˆ ( k | k ), Pˆ ( k | k )}

Figure 1. The IMM algorithm (r models). Table 1. Filters for the models in the IMM algorithm.

Step 2: Filtering Determine the relevant filter from Table 1. With the initial state estimates, their corresponding state error covariances and an exogenous measurement data z(k) as inputs, model updates for Mj(k) are performed by computing • the state estimate xˆ j (k | k) , and

Notation for algorithm variant IEK IUK IEE IEG IER IES IEU IUE IUG IUR IUS IUU IEP IGP IRP ISP IUP IEUE IEUG IEUR IEUS IEUU

• the corresponding state error covariance Pˆj (k | k) . Step 3: Mode probability update For Mj(k), with the use of filter residual ~zj (k) , the corresponding filter residual covariance Sˆj (k) and the assumption of a Gaussian distribution, the likelihood function is computed as 1 Λ j (k ) = exp(−0.5(~zj (k))T (Sˆj (k))−1 ~zj (k )). ˆ | 2πS (k) | j

The mode probability of Mj(k) is then updated as µ j (k ) = c -1Λ j (k ) c j , r

where c = ∑ Λ i (k)ci .

Model 1 (CV) EKF UKF EKF EKF EKF EKF EKF UKF UKF UKF UKF UKF EKPF GPF RPF SPF UPF EKF EKF EKF EKF EKF

Model 2 (CA) EKF UKF EKF EKF EKF EKF EKF UKF UKF UKF UKF UKF EKPF GPF RPF SPF UPF UKF UKF UKF UKF UKF

Model 3 (3DTR) EKF UKF EKPF GPF RPF SPF UPF EKPF GPF RPF SPF UPF EKPF GPF RPF SPF UPF EKPF GPF RPF SPF UPF

i=1

Step 4: Output mixer (combination) All the state estimates and their corresponding state error covariances output from the individual models are utilized for the computation of

4

Simulation Tests and Results

The IMM algorithm variants listed in Table 1 are implemented for simulated target trajectories depicted in Figures 2 to 5. With reference to equations (3) and (4), the

variables for the models and system equations are as follows. The sampling interval is T = 1 (second). At time step k, T the state vector is Xk = [x k y k z k x k y k z k x k yk zk ] . The process noise vector wk is zero-mean multivariate Cauchy-distributed (multivariate t distribution, with one degree of freedom) with correlation matrix Qj for model j, j = 1,…, 3. The measurement matrix is Hk = [I3 03 03], where I3 and 03 are the 3×3 identity and zero matrices respectively. The measurement noise vector vk is zeromean Gaussian with covariance R = 105I3. The transition probability matrix is [0.96 0.02 0.02; 0.02 0.96 0.02; 0.02 0.02 0.96] and the initial mode probability is [0.96 0.02 0.02]. For each PF variant used, the number of particles is Ns = 600. The total number of simulation runs is C = 100. • Model 1 (CV model): 0.5T2 I3   I3 TI3 03    Gk =  TI3  , Fk = 03 I3 TI3 , Q1 = 52I3.  0  0 0 I   3 3 3   3  • Model 2 (CA model):  I3 TI3 0.5T2I3  0.5T2I3  Fk = 03 I3 TI3  , Gk =  TI3  , Q2 = 202I3. 0 0    I3   3 3  I3  • Model 3 (3DTR model) [12]:  I3 ω−k1 sin(ωk T)I3 ω−k2 (1 − cos(ωkT))I3  Fk = 03 cos(ωk T)I3 ω−k1 sin(ωkT)I3  ,   cos(ωk T)I3 03 − ωk sin(ωk T)I3 

• Trajectory 3: 774.35 (to two d. p.), and • Trajectory 4: 778.95 (to two d. p.).

Figure 2. Trajectory 1.

Figure 3. Trajectory 2.

0.5T2I3  [x k y k zk ] 2 Gk =  TI3  , Q3 = 102I3, where ωk =  I  [x k y k z k ] 2  3 

is the turning rate. Let L denote the total number of scans for the duration of tracking a target (that is, the number of points on a target trajectory). The root mean square errors (RMSEs) in the estimation of position (respectively, velocity and L C

2

acceleration) is defined as RMSE:= {L1 C1 ∑∑ uˆik − uik 2 }1/ 2 , k =1i =1

where • uˆik is the position (respectively, velocity and acceleration) estimate, and • u ik is the true target position (respectively, velocity and acceleration), at the k-th scan in the i-th simulation run. Tables 2 to 5 show the RMSEs in state estimation for the IMM algorithm variants used. For each of these tables, the last column displays the mean processing time (in seconds) per simulation run for each variant. The entries “diverge” indicate occurrence(s) of divergence of the variant. The notation “O(10n)” (with n being a relevant positive integer) is used to represent very large RMSEs in state estimation. In addition, the RMSEs (in meters) in position estimation with measurement data are as follows: • Trajectory 1: 765.57 (to two decimal places (d. p.)), • Trajectory 2: 776.32 (to two d. p.),

Figure 4. Trajectory 3.

Figure 5. Trajectory 4.

Table 2. Estimation errors for Trajectory 1. Algorithm variant IEK IUK IEE IEG IER IES IEU IUE IUG IUR IUS IUU IEP IGP IRP ISP IUP IEUE IEUG IEUR IEUS IEUU

position (m) 564.45 560.82 610.14 564.79 568.94 654.42 661.69 563.38 564.19 656.27 O(105) O(105) 669.36 O(103) 563.17 564.48 653.15

RMSE (to 2 d. p.) velocity acceleration (m/s) (m/s2) 396.62 188.56 393.10 182.53 diverge 509.54 203.16 394.77 187.51 399.48 188.20 O(103) O(103) 787.34 929.60 diverge 393.91 184.08 394.39 183.73 O(103) O(104) diverge diverge O(103) 603.17 O(104) 338.69 949.89 O(103) O(103) 769.51 diverge 392.19 183.92 393.77 183.61 O(103) O(104)

Mean time (s) per run 0.07 0.09 37.69 12.83 6.33 6.31 39.49 30.20 12.80 6.33 6.32 39.16 85.68 35.11 17.70 17.62 92.73 28.96 12.95 6.35 6.34 39.39

Table 4. Estimation errors for Trajectory 3. Algorithm variant IEK IUK IEE IEG IER IES IEU IUE IUG IUR IUS IUU IEP IGP IRP ISP IUP IEUE IEUG IEUR IEUS IEUU

Table 3. Estimation errors for Trajectory 2. Algorithm variant IEK IUK IEE IEG IER IES IEU IUE IUG IUR IUS IUU IEP IGP IRP ISP IUP IEUE IEUG IEUR IEUS IEUU

position (m) 465.70 450.08 588.34 529.94 464.38 463.64 734.12 584.35 530.70 458.85 458.12 739.39

574.24 612.55

530.56 466.72 465.00 745.83

RMSE (to 2 d. p.) velocity acceleration (m/s) (m/s2) 166.31 73.22 138.11 28.15 581.48 O(103) 245.60 148.47 158.02 38.41 160.86 40.83 O(105) O(106) 543.41 O(104) 261.14 170.28 147.73 31.38 147.27 31.50 O(105) O(106) diverge diverge 467.06 O(103) 335.44 186.51 diverge diverge 253.42 169.72 156.35 33.03 156.18 32.42 O(105) O(106)

Mean time (s) per run 0.14 0.18 45.88 19.20 11.06 11.03 72.20 48.82 19.18 11.05 11.03 72.57 172.68 53.07 30.98 30.87 204.62 61.60 19.23 11.07 11.05 72.07

position (m) 489.53

496.38 493.92 609.75 529.05 491.37 490.91

O(1016) O(1015)

531.44 492.66 495.74

RMSE (to 2 d. p.) velocity acceleration (m/s) (m/s2) 209.22 203.33 diverge diverge diverge 308.01 O(103) 190.00 58.62 diverge 629.89 O(103) 248.58 167.62 178.80 47.83 176.62 44.13 diverge diverge diverge O(1015) O(1027) 13 O(10 ) O(1012) diverge diverge 256.28 181.89 180.32 46.02 182.95 44.52 diverge

Mean time (s) per run 0.25 0.32 90.00 26.29 21.20 20.95 128.59 68.14 26.00 21.18 21.15 128.77 280.67 71.88 56.49 56.52 390.51 79.16 26.03 21.32 21.15 128.18

Table 5. Estimation errors for Trajectory 4. Algorithm variant IEK IUK IEE IEG IER IES IEU IUE IUG IUR IUS IUU IEP IGP IRP ISP IUP IEUE IEUG IEUR IEUS IEUU

position (m) 498.85 499.10 558.94 502.05 504.88 690.24 559.21 501.94 501.12 687.07 O(103) O(103) 601.96 558.02 502.45 499.75 678.80

RMSE (to 2 d. p.) velocity acceleration (m/s) (m/s2) 215.82 74.78 205.00 59.02 diverge 302.17 128.05 225.34 87.32 228.57 92.44 O(104) O(105) diverge 305.99 135.72 213.96 66.43 213.96 66.80 O(104) O(105) diverge diverge 697.05 215.29 716.25 154.25 diverge 537.37 O(103) 302.13 124.37 212.95 64.07 214.94 64.33 O(104) O(105)

Mean time (s) per run 0.11 0.12 46.77 12.55 8.23 8.21 56.30 47.18 12.56 8.24 8.21 55.81 123.70 34.67 23.06 22.96 170.70 40.97 12.57 8.25 8.22 56.35

Let t(X) denote the mean processing time (in seconds) per simulation run for the algorithm variant with notation X in Table 1. The processing times are related by: t(IEK) ≈ t(IUK)