Gaussian Process Gaussian Mixture PHD Filter for 3D Multiple Extended Target Tracking

Abstract

This paper addresses the problem of tracking multiple extended targets in three-dimensional space. We propose the Gaussian process Gaussian mixture probability hypothesis density (GP-PHD) filter, which is capable of tracking multiple extended targets with complex shapes in the presence of clutter. Our approach combines the Gaussian process regression measurement model with the probability hypothesis density filter to estimate both the kinematic state and the shape of the targets. The shape of the extended target is described by a 3D radial function and is estimated recursively using the Gaussian process regression model. Furthermore, we transform the recursive Gaussian process regression problem into a state estimation problem by deriving a state space model such that the estimation of the extent can be integrated into the kinematic part. We derive the predicted likelihood function of the PHD filter and provide a closed-form Gaussian mixture implementation. To evaluate the performance of the proposed filter, we simulate a typical extended target tracking scenario and compare the GP-PHD filter with the traditional Gamma Gaussian Inverse-Wishart PHD (GGIW-PHD) filter. Our results demonstrate that the proposed algorithm outperforms the GGIW-PHD filter in terms of estimating both kinematic states and shape. We also investigate the impact of the measurement rates on both filters; it is observed that the proposed filter exhibits robustness across various measurement rates, while the GGIW-PHD filter suffers under low-measurement-rate conditions.

1. Introduction

The tracking of moving targets within surveillance areas plays a crucial role in various fields such as automotive driving and robotic systems [1]. With the increasing demand for accurate tracking, high-resolution sensors such as Radar and Lidar have gained significant importance. In traditional tracking scenarios, e.g., air traffic control, each target produces at most one detection. However, in many recent applications, such as autonomous driving, this is not the case, because the resolution of the sensor is high enough that the detected object may occupy multiple resolution cells and thus generate multiple measurements. In the literature [2], such scenarios are defined as the extended object tracking (EOT) problem, which is also referred to as the extended target tracking (ETT) problem. In the ETT problem, the shape of the target is not negligible, requiring the tracking algorithm to estimate both its shape and kinematic states. Moreover, the data association and the track management in the ETT problem are far more complicated than in the point target tracking scenario. The ETT has become a key problem in the automotive system. Given its importance in automotive systems, extensive research has been conducted on the ETT problem, utilizing the random finite set (RFS) theory and nonlinear estimation methods [3,4,5,6]. Recently, deep learning-based methods have also been proposed to address the ETT problem [7,8]. A comprehensive tutorial for the ETT problem is given in [1].

The ETT problem can be divided into two parts: the non-point target measurement model and the multiple-target tracking (MTT) problem. Some commonly used non-point target measurement models are the random matrix (RM) [9,10] model and the random hypersurface model (RHM) [11]. The RM model, first introduced in [9], assumes that the measurements of extended targets are distributed over the entire ellipse surface, following a spatial Gaussian distribution. A semi-positive definite (SPD) matrix is employed to describe the contours of the ellipse. However, the original RM model neglects the measurement errors caused by the sensor, which can accumulate and lead to an overestimation of the object’s shape. To address this limitation, Koch enhances the measurement model in [10] by taking the measurement error into account. In addition, the interacting multiple model (IMM) is integrated into the RM model, enabling the filter to track maneuvering targets. Further investigation into the IMM model’s utilization for maneuvering extended target tracking is presented in [12]. A joint detection, tracking, and classification (JDTC) approach for multiple extended objects is proposed in [13]. This also uses an RM to model the ellipse. In many situations, it is not accurate enough to model the target as an ellipse. Therefore, Lan [14] proposes an approach that uses multiple sub-ellipses to model the extended targets with complex shapes. Another common method assumes that extended targets are star-convex and use the RHM to derive the measurement model. RHM-based methods assume that the measurement sources have certain spatial distributions and are located on differently scaled inner surfaces of the target. The implicit measurement model is then updated with a non-linear filter such as an extended Kalman filter (EKF). Based on the RHM, the radial function is introduced in [15] to explicitly describe the surface of the non-point target, and the Fourier series expansion is used to approximate the radial function. Sun et al. [16] propose a filter to track number-variable maneuvering extended targets using the RHM and the IMM. The B-spline model is another approach to modeling star-convex targets and is first introduced by [17]. Compared to the RHM and the RM model, the B-spline model is capable of describing arbitrary shape extended targets using the control points on the B-spline. In [18], the B-spline model is used under the Poisson multi-Bernoulli mixture (PMBM) filter framework to track non-ellipsoidal targets. The B-spline model is also used to track elongated targets such as trains and pedestrians [19]. The Gaussian process (GP) has been widely used in machine learning and other fields because of its easy-to-compute posterior probability and excellent analytical properties [20]. It is first proposed in [21] to use the GP regression to estimate the radial function. The GP model can describe arbitrary shaped extended targets and is computationally more efficient than the B-spline model. At the same time, the estimation of the target shape is more accurate than the ellipse approximation models. A new temporal covariance kernel for GP regression is proposed in [22] that could improve the accuracy of object shape estimation. The dependency of connected extended targets is utilized in [23] and the GP model is used to estimate the shape of the extended target.

The MTT problem is challenging due to the track associations, miss detections, etc. However, the RFS theory [24] proposed by Mahler provides a rigorous treatment of the MTT. Their method is completely free of explicit data associations, unlike traditional MTT techniques such as the joint probabilistic data association (JPDA) filter and the multiple hypothesis tracker (MHT). Furthermore, with the improvement of hardware [25,26,27,28], it is possible to employ algorithms based on the RFS theory. The optimal Bayesian filter derived from the RFS theory involves high-dimensional integrals. Therefore, Mahler proposes several theoretical approximation methods, such as the PHD filter [29] and the MB filter [24]. Among these methods, the PHD filter stands out as the most computationally efficient and memory-friendly, making it a practical choice given current computational resources. The first attempt to address the ETT problem within the framework of the PHD filter is proposed by [2]. A Gaussian mixture implementation of [2] is given in [30] called the extended target Gaussian mixture PHD filter (ET-GM-PHD). Nonetheless, both methods in [2,30] only estimate the kinematic states of the target and omit the shape which contains crucial information about the target. In [31], the RM approach is first used in the PHD framework to estimate the shape of an unknown number of extended targets in the presence of clutter and miss detections. Granström [32] further considers target spawning and combination and proposes the GGIW-PHD filter, which is the state-of-the-art ETT tracker and has been successfully tested in a real marine surveillance scenario [33].

Although 3D data, generated by sensors such as depth cameras and Lidars, are becoming increasingly accessible, there have been few attempts to address the ETT problem in 3D space. The relationship between the ellipse and symmetrical SPD matrix is analyzed and the ellipse fitting approach (EFA) is proposed [34]. In [35], a generalization of the RHM to 3D space is proposed; however, only cylinder targets are suitable for this method. Current approaches for 3D ETT usually use simple geometries such as cylinders and ellipses and are not able to model complexly shaped extended targets. The GP model approach to tracking 3D targets is proposed in [36,37], but it only considers the single-target situation. Therefore, to solve the 3D ETT problem, the combination of a complex-target model and multiple-target tracking filters, such as the PHD filter, is required.

In this paper, we propose a method that uses a 3D radial function to describe the shape of the target. The GP regression model is adopted to estimate the value of the radial function. We transform the regression problem into a state estimation problem. This approach allows us to use the Bayesian filter paradigm to estimate the unknown shape of the target and can be integrated into the kinematic estimation process. The computationally efficient PHD filter is combined with the measurement model to track an unknown number of extended targets in the presence of clutter. The kinematic states and the shape are then inferred by the EKF. The proposed filter is capable of tracking multiple extended targets with complex shapes in the presence of clutter. To demonstrate the performance of the algorithms, we simulate point cloud measurements in MATLAB for two dynamic objects with different shapes in the presence of clutter. We evaluate the performance of the filter using both the optimal sub-pattern assignment (OSPA) [38] and the intersection-over-union (IoU) value. The proposed method outperforms the traditional GGIW-PHD filter in the simulation experiment. We also test the stability of the two filters at different measurement rates. It is shown that the proposed filter performs better than the GGIW-PHD filter under low-measurement-rate conditions.

2. Materials and Methods

2.1. Extended Target Tracking Problem Formulation

In the multiple extended target tracking problem, the states of extended targets at time $k$ can be represented by an RFS:

$$X_k={\left\{{\xi }_k^i\right\}}_{i=1}^{N_{x,k}}\in \mathcal{F}(\mathbb{X}),$$

(1)

where $N_{x,k}$ is the number of unknown targets, ${\xi }_k^i$ is the kinematic and extent states of the ith extended target, and $\mathcal{F}(\mathbb{X})$ is the multiple targets’ state space. We use operation $|\cdot |$ to denote the cardinality of a set, and then $\left|X_k\right|=N_{x,k}$. Measurements at time $k$ can also be represented by an RFS $Z_k={\left\{z_k^{(j)}\right\}}_{j=1}^{N_{z,k}}$, where $N_{z,k}$ is the number of measurements. The ETT problem is to estimate the posterior density ${\pi }_k(X_k|Z_{1:k})$ of the target’s state using measurements $Z_{1:k}=\{Z_1,\cdots ,Z_k\}$ from time 1 to $k$.

2.2. The PHD Filter

The multi-target Bayesian filter based on RFS theory includes the prediction and the update process. According to the Chapman–Kolmogorov equation, the posterior density at time $k$ is

$${\pi }_{k|k-1}(X_k|Z_{1:k-1})={\displaystyle \int f_{k|k-1}(X_k|X_{k-1}){\pi }_{k-1}(X_{k-1}|Z_{1:k-1})\delta X_{k-1}},$$

(2)

where $f_{k|k-1}(X_k|X_{k-1})$ is the probability transition density. Given the predicted density and measurements, the updated density at time $k$ is

$${\pi }_k(X_k|Z_{1:k})=\frac{g_k(Z_k|X_k){\pi }_{k-1}(X_{k-1}|Z_{1:k-1})}{{\displaystyle \int g_k(Z_k|X_k){\pi }_{k|k-1}(X_k|Z_{1:k-1})\delta X_k}},$$

(3)

where $g_k(Z_k|X_k)$ is the likelihood density of the measurements. There is no closed solution to Equation (3) that involves high-dimensional integral. The PHD filter is an approximation of the optimal Bayesian filter that estimates multiple target states by passing the first moment of the multi-object probability density, or PHD. The PHD can be calculated by multi-object density

$$D(x)={\displaystyle \int \pi (\{x\}\cup X)\delta X}.$$

(4)

The PHD filter contains a prediction and an update process to estimate the density $D_{k+1|k+1}(\cdot )$ of the target’s RFS $X_k$. The prediction equation of the extended target PHD filter is given in [2] as

$$D_{k+1|k}({\xi }_{k+1})={\displaystyle \int p_s({\xi }_k)p_{k+1|k}({\xi }_{k+1}|{\xi }_k)D_{k|k}({\xi }_k)\mathrm{d}{\xi }_k}+D_{k+1}^b({\xi }_{k+1}),$$

(5)

where $p_s(\cdot )$ is the survival probability, $p_{k+1|k}({\xi }_{k+1}|{\xi }_k)$ is the state transition density and $D_{k+1}^b({\xi }_{k+1})$ is the birth PHD. The update equation for the extended target PHD filter is given as follows

$$D_{k|k}({\xi }_k|Z^k)=L_{Z_k}({\xi }_k)D_{k|k-1}({\xi }_k|Z^{k-1}),$$

(6)

where $L_{Z_k}(\cdot )$ is the pseudo-measurement likelihood function. $L_{Z_k}(\cdot )$ is given as follows

$$\begin{array}{ll}{L}_{Z_k}({\xi }_k)\triangleq & (1-{\mathrm{e}}^{-\gamma ({\xi }_k)})p_D({\xi }_k)\\ & +{\mathrm{e}}^{-\gamma ({\xi }_k)}{p}_D({\xi }_k){\displaystyle \sum _{\mathcal{P}\angle Z_k}{\omega }_{\mathcal{P}}{\displaystyle \sum _{W\in \mathcal{P}}\frac{\gamma {({\xi }_k)}^{|W|}}{d_W}{\displaystyle \prod _{z_k\in W}\frac{{\varphi }_{z_k}({\xi }_k)}{{\lambda }_k{c}_k(z_k)}}}},\end{array}$$

(7)

where

• $\gamma ({\xi }_k)$ is the expected number of measurements from targets;
• $p_D({\xi }_k)$ is the probability of detection;
• $c_k(z_k)$ is the spatial distribution of clutter over the surveillance area;
• ${\varphi }_{z_k}({\xi }_k)$ is the single-target measurement likelihood function;
• $\mathcal{P}\angle Z$ denotes that $\mathcal{P}$ partitions the measurement set $Z_k$ into different nonempty measurement cells $W$. $W\in \mathcal{P}$ denotes that the cell $W$ is in partition $\mathcal{P}$.

$d_W$ and ${\mathcal{L}}_k^{(j,W)}$ are non-negative coefficients. We use $\delta$ denoting the Kronecker delta function, and the coefficients are given as follows

$$w_{\mathcal{P}}=\frac{{\displaystyle {\prod }_{W\in \mathcal{P}}{d}_W}}{{\displaystyle {\sum }_{\mathcal{P}\prime \angle Z_k}{\displaystyle {\prod }_{W\prime \in {\mathcal{P}}^{\prime }}{d}_{W\prime }}}},$$

(8)

$$d_W={\delta }_{\left|W\right|,1}+D_{k+1|k}\left[p_d{\gamma }_k^{|W|}\exp (-{\gamma }_k){\displaystyle \prod _{z_k\in W}\frac{{\varphi }_{z_k}(\cdot )}{{\lambda }_k{c}_k(z_k)}}\right].$$

(9)

3. Gaussian Process Regression Measurement Model

The Gaussian process has been widely used in tasks such as classification and regression. A Gaussian process $f(u)$ can be defined by its mean function $\mu (u)$ and the correlation function $C(u,u\prime )$, which are given as

$$\mu (u)=\mathbb{E}\left[f(u)\right],$$

(10)

$$C(u,u\prime )=\mathbb{E}\left[(f(u)-\mu (u)){(f(u^{\prime })-\mu (u^{\prime }))}^T\right].$$

(11)

Therefore, the GP $f(u)$ can be denoted as

$$f(u)~\mathcal{G}\mathcal{P}(\mu (u),C(u,u^{\prime })).$$

(12)

The Gaussian process could be regarded as the generalization in the infinite dimension of a joint Gaussian distribution, and so the function values of $N$ input $u_1\cdots u_N$ are Gaussian distributed as

$$\left[\begin{array}{c}f(u_1)\\ ⋮\\ f(u_N)\end{array}\right]~\mathcal{N}(\mu ,K),$$

(13)

where

$$\mu =\left[\begin{array}{c}\mu (u_1)\\ ⋮\\ \mu (u_N)\end{array}\right], K=\left[\begin{array}{ccc}C(u_1,u_1)& \cdots & C(u_1,u_N)\\ ⋮& & ⋮\\ C(u_N,u_1)& \cdots & C(u_N,u_N)\end{array}\right].$$

(14)

We assume that a training set is given, whose input is $u={[u_1,\cdots ,u_N]}^T$ and output is $z={[z_1\cdots ,z_N]}^T$. The measurement model of the training set is

$$z_k=f(u_k)+e_k, e_k~\mathcal{N}(0,R).$$

(15)

The Gaussian process regression is used to estimate the output $f={[f(u_1^f),\cdots f(u_N^f)]}^T$ of some given test set inputs $u^f={[u_1^f,\cdots ,u_N^f]}^T$. The training set, together with the measurement model, leads to the following distribution

$$\left[\begin{array}{c}z\\ f\end{array}\right]~\mathcal{N}\left(0,\left[\begin{array}{cc}K(u,u)+I_N\otimes R& K(u,u^f)\\ K(u^f,u)& K(u^f,u^f)\end{array}\right]\right).$$

(16)

The conditional distribution of the joint Gaussian distribution is still a Gaussian distribution that gives

$$p(f|z)=\mathcal{N}(Az,P),$$

(17)

where

$$A=K(u^f,u)K_y,$$

(18)

$$P=K(u^f,u^f)-K(u^f,u)K_y^{-1}K(u,u^f),$$

(19)

$$K_y=K(u,u)+I_N\otimes R.$$

(20)

The measurements are collected recursively rather than in the batch form in the multiple-target tracking problem, and so the Gaussian process regression needs to be improved in order to estimate the output of the test set recursively. Ref. [39] introduces basis points which are considered to be the inputs of the training set. This article adopts the same approach as [39], where a state-space model is also introduced to estimate the output recursively. $u^f={[u_1^f,\cdots ,u_N^f]}^T$ and $x_k^f$ denote the input and the output of the test set, respectively, and so the state-space model is

$$x_{k+1}^f=x_k^f,$$

(21)

$$z_k=H^f(u_k)x_k^f+e_k^f, e_k^f~\mathcal{N}\left(0,R^f(u_k)\right),$$

(22)

where $z_k$ is the measurement at time $k$. We assume $x^f$ is sufficient statistics for $z_k$. Under this assumption

$$p(z_k|x^f,z_{1:k-1})\approx p(z_k|x^f),$$

(23)

the recursive optimal Bayesian filter gives the posterior probability density function (PDF) $p(x^f|z_{1:k})$ as

$$\begin{array}{c}p(x^f|z_{1:k})\propto p(z_k|x^f,z_{1:k-1})p(x^f|z_{1:k-1})\\ \approx p(z_k|x^f)p(x^f|z_{1:k-1}).\end{array}$$

(24)

According to Equation (17), the joint distribution of $z_k$ and $x^f$ is

$$\left[\begin{array}{c}{z}_k\\ x^f\end{array}\right]~\mathcal{N}\left(0,\left[\begin{array}{cc}K(u_k,u_k)+R& K(u_k,u^f)\\ K(u^f,u_k)& K(u^f,u^f)\end{array}\right]\right).$$

(25)

Therefore, the likelihood function and the prior PDF are

$$p(z_k|x^f)=\mathcal{N}(z_k;H_k^f{x}^f,R_k^f),$$

(26)

$$p(x^f)=\mathcal{N}(0,P_0^f).$$

(27)

where

$$H^f(u_k)=K(u_k,u^f){[K(u^f,u^f)]}^{-1},$$

(28)

$$R^f(u_k)={\rm K}(u_k,u_k)+R-K(u_k,u^f){[K(u^f,u^f)]}^{-1}K(u^f,u_k),$$

(29)

$$P_0^f=K(u^f,u^f).$$

(30)

The advantage of the method is that the extent states can be inserted into the kinematic states and be estimated using a single-state space model.

Measurement Model

In this article, we assume that the measurement sources are all on the object’s surface. A 3D radial function $r=f(\theta ,\varphi )$ is used to model the shape of the target, whose inputs $(\theta ,\varphi )$ are the azimuth and the elevation. For simplicity, we use $\gamma$ to denote the pair $(\theta ,\varphi )$. The function value $r$ of the radial function is the distance between the basis points and the center of the extended target. The radial function is assumed to be a GP, denoted as

$$f(\gamma )~\mathcal{G}\mathcal{P}\left({\mu }_r,C(\gamma ,\gamma \prime )+{\sigma }_r^2\right).$$

(31)

The mean function of the GP model is assumed to be some unknown constant ${\mu }_r$, which is Gaussian-distributed, and we assume that ${\mu }_r=0$ as in [37]. The correlation function in [37] is used, which is

$$C(\gamma ,\gamma \prime )={\sigma }_f^2\exp \left(-\frac{d^2(\gamma ,\gamma \prime )}{2l^2}\right),$$

(32)

where ${\sigma }_f^2$ represents the prior variance, $l$ is the length scale, and $d(\gamma ,\gamma \prime )$ calculates the relative distance between two inputs, given as follows

$$\begin{array}{cc}\hfill d(\gamma ,\gamma \prime )=\arccos & \left(\cos (\varphi )\cos (\varphi \prime )\cos (\theta )\cos ({\theta }^{\prime })\right.\hfill \\ & \left.+\cos (\varphi )\cos (\varphi \prime )\sin (\theta )\sin ({\theta }^{\prime })+\sin (\varphi )\sin (\varphi \prime )\right).\hfill \end{array}$$

(33)

The output range of the distance function is $[0,\pi ]$, and any coincident input is mapped to $0$, while the opposite pair is mapped to $\pi$. The employed distance $d(\gamma ,\gamma \prime )$ is specified to imply a higher correlation for closer regions compared to separated regions.

Together with the radial function and the GP model, both the kinematic model and measurement model can be derived. We denote the extended target state as $x_k\triangleq {[{\overline{x}}_k^T,f_k^T]}^T$, where ${\overline{x}}_k={[x_k^t{}^T,x_k^r{}^T]}^T$ is the extended target kinematic state including the translation vector $x_k^t$ and the orientation vector $x_k^r$. The values of the radial function are denoted as $f_k$. The kinematic model of the extended target is

$$x_{k+1}=F_k{x}_k+w_k, w_k~\mathcal{N}(0,Q_k),$$

(34)

$$F_k=\left[\begin{array}{cc}{\overline{F}}_k& \\ & F_k^f\end{array}\right], Q_k=\left[\begin{array}{cc}{\overline{Q}}_k& \\ & Q_k^f\end{array}\right],$$

(35)

$${\overline{F}}_k=\left[\begin{array}{cc}{F}_k^t& \\ & F_k^r\end{array}\right], {\overline{Q}}_k=\left[\begin{array}{cc}{Q}_k^t& \\ & Q_k^r\end{array}\right].$$

(36)

The constant velocity (CV) model is considered for the translation of the extended target in this article, which gives

$$x_k^t\triangleq {[x_k^c,{\dot{x}}_k^c]}^T,$$

(37)

$$F_k^t=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\otimes I_3,$$

(38)

$$Q_k^t=\left[\begin{array}{cc}{T}^3/3& T^2/2\\ T^2/2& T\end{array}\right]\otimes {\sigma }_x{I}_3.$$

(39)

A $3\times 3$ orthogonal matrix called a rotation matrix is usually used to describe the orientation of the target. However, a rotation matrix is singular or discontinuous at some points. Quaternion is used in this article instead of a rotation matrix to describe the orientation of the target. A quaternion $q$ is a 4D vector which contains two parts: a three-dimensional vector and a constant, denoted as

$$q={\left[\begin{array}{cc}{\overline{q}}^T& q_4\end{array}\right]}^T.$$

(40)

The translation between the rotation matrix and the quaternion is

$$R_G^L(q_k)={\left((q_4^2-{\overline{q}}^T\overline{q})I_3+2{\overline{q}}^T\overline{q}-2q_4[\overline{q}\times ]\right)}^T,$$

(41)

where $R(\cdot )$ represents the rotation matrix and $[\overline{q}\times ]$ is the cross-product matrix

$$[\overline{q}\times ]=\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right].$$

(42)

The multiplication of two quaternions $q^{\prime }$ and $q$ is defined as

$$q^{\prime }\odot q=\left[\begin{array}{c}{q^{\prime }}_{4}q+q_4{q}^{\prime }-\overline{q^{\prime }}\times \overline{q}\\ {q^{\prime }}_4{q}_4-{\overline{q^{\prime }}}^T\overline{q}\end{array}\right].$$

(43)

A useful property of quaternion is

$$R(q^{\prime })R(q)=R(q^{\prime }\odot q),$$

(44)

which indicates that the product of two rotation matrices can be represented by the product of quaternions. Therefore, the continuous rotation of a target can be represented by the product of quaternions. Ref. [40] introduces an approach to estimate the orientation of the target recursively using quaternions based on the property of Equation (44). This method can be augmented in the process of estimating the target’s kinematic states. The orientation of the target at time $k$ can be expressed as

$$q=\delta q(a)\odot q_{\mathrm{ref}}$$

(45)

where $q_{\mathrm{ref}}$ is the reference orientation and $\delta q(a)$ is the deviation from the reference orientation. To estimate the orientation recursively, the estimated orientation at time $k-1$ is used as the reference orientation. $a$ is the deviation vector, and $\delta q(a)$ is defined using Rodrigue parameterization as

$$\delta q(a)=\frac{1}{\sqrt{4+{\left|a\right|}^2}}\left[\begin{array}{c}a\\ 2\end{array}\right].$$

(46)

The core idea is to estimate the deviation vector $a$, and the orientation at any moment can be calculated together with the reference orientation. Ref. [41] gives the dynamic model of the deviation vector

$$\begin{array}{cc}\hfill \dot{a}& =\left(I_3+\frac{1}{4}a{a}^T+\frac{1}{2}[a\times ]\right)\omega \hfill \\ & \approx \left(I_3+\frac{1}{2}[a\times ]\right)\omega .\hfill \end{array}$$

(47)

We denote the rotation vector as $x_k^r\triangleq {\left[\begin{array}{cc}{a}^T& {\omega }^T\end{array}\right]}^T$, and the dynamic model of the rotational part of the extended target can be derived using Equation (47) as

$$\dot{x}=\left[\begin{array}{c}\left(I_3+\frac{1}{2}[a\times ]\right)\omega \\ 0_{3\times 1}\end{array}\right]+\left[\begin{array}{c}{0}_3\\ I_3\end{array}\right]\alpha ,$$

(48)

where $\alpha$ is the acceleration of the rotation vector, and is assumed to be the Gaussian white noise in the article. Equation (48) indicates a nonlinear system, and therefore the first-moment Taylor expansion is used to derive the approximated linear model. The first-order Taylor expansion of the estimated orientation $x^r=x_{k|k}^r$ at time $k$ is given as

$$\begin{array}{cc}\hfill {\dot{x}}^r& =f(x^r)+B\alpha \hfill \\ & \approx f({\widehat{x}}^r)+A_k^r(x^r-{\widehat{x}}^r).\hfill \end{array}$$

(49)

The deviation vector is set to zero after each measurement update, and so the Taylor approximation in Equation (49) is

$$\begin{array}{cc}\hfill A_k^r& ={\left.\frac{d}{d{x}^r}f(x^r)\right|}_{x^r={\widehat{x}}_{k|k}^r}\hfill \\ & =\left[\begin{array}{cc}\frac{1}{2}[-{\widehat{\omega }}_{k|k}\times ]& I_3\\ 0_3& 0_3\end{array}\right].\hfill \end{array}$$

(50)

The discrete system is

$$x_{k+1}^r=F_k^r{x}_k^r+w_k^r, w_k^r~\mathcal{N}(0,Q_k^r),$$

(51)

where

$$\begin{array}{cc}\hfill F_k^r& =\exp (A_{k}T)\hfill \\ & =\left[\begin{array}{cc}\exp \left(\frac{T}{2}[-{\widehat{\omega }}_{k|k}\times ]\right)& T\exp \left(\frac{T}{2}[-{\omega }_{k|k}\times ]\right)\\ 0_3& I_3\end{array}\right].\hfill \end{array}$$

(52)

Substituting Equation (42) into Equation (52) gives

$$\exp \left(\frac{T}{2}[-{\widehat{\omega }}_{k|k}\times ]\right)=I_3+\frac{\sin \left(\frac{T}{2}\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|\right)}{\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|}[-{\widehat{\mathsf{\omega }}}_{k|k}\times ]+\frac{1-\cos \left(\frac{T}{2}\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|\right)}{{\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|}^2}{[-{\widehat{\mathsf{\omega }}}_{k|k}\times ]}^2,$$

(53)

where $T$ is the sampling time. The process noise covariance matrix is

$$Q_k^r=G_k{\Sigma }_{\alpha }{G}_k^T.$$

(54)

Because the deviation vector is set to zero, $G_k$ is

$$G_k=\left[\begin{array}{cc}{\displaystyle {\int }_0^T\exp }\left(\frac{\tau }{2}\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]\right)d\tau & {\displaystyle {\int }_0^T\exp }\left(\frac{\tau }{2}\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]\right)d\tau \\ 0_3& {\displaystyle {\int }_0^T{I}_3}d\tau \end{array}\right].$$

(55)

The following are the details of the matrices

$$\begin{array}{cc}\hfill {\displaystyle {\int }_0^T\exp }\left(\frac{\tau }{2}\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]\right)d\tau =& T{I}_3+\frac{2\left(1-\cos \left(\frac{T}{2}\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|\right)\right)}{{\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|}^2}\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]\hfill \\ & +\frac{T-\frac{2}{\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|}\sin \left(\frac{T}{2}\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|\right)}{{\left|{\widehat{\mathsf{\omega }}}_{k|k}\right|}^2}{\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]}^2,\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill {\displaystyle {\int }_0^T\tau }\exp \left(\frac{\tau }{2}\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]\right)d\tau & =\frac{T^2}{2}{I}_3+\hfill \\ & \frac{1}{{|{\widehat{\mathsf{\omega }}}_{k|k}|}^2}\left(\frac{4}{|{\widehat{\mathsf{\omega }}}_{k|k}|}\sin \left(\frac{T}{2}|{\widehat{\mathsf{\omega }}}_{k|k}|\right)-2T\cos \left(\frac{T}{2}|{\widehat{\mathsf{\omega }}}_{k|k}|\right)\right)\hfill \\ & \times \left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]+\frac{1}{{|{\widehat{\mathsf{\omega }}}_{k|k}|}^2}(\frac{T^2}{2}+\frac{2T}{|{\widehat{\mathsf{\omega }}}_{k|k}|}\sin \left(\frac{T}{2}|{\widehat{\mathsf{\omega }}}_{k|k}|\right)\hfill \\ & +\frac{4}{{|{\widehat{\mathsf{\omega }}}_{k|k}|}^2}\left(\cos \left(\frac{T}{2}|{\widehat{\mathsf{\omega }}}_{k|k}|\right)-1\right)){\left[-{\widehat{\mathsf{\omega }}}_{k|k}\times \right]}^2,\hfill \end{array}$$

(57)

$${\displaystyle {\int }_0^T{I}_3}d\tau =T{I}_3.$$

(58)

We assume that the shape of the target does not change, and that therefore the dynamic model for the extent state of the extended target is

$$f_{k+1}=f_k+w_k,w_k~\mathcal{N}(0,Q_k^f),$$

(59)

$$Q_k^f=(\frac{1}{\lambda }-1)P_{k|k}^f,$$

(60)

which indicates that $F_k^f=I$.

Two coordinate systems are used in the article to derive the measurement model. The first one is the global coordinate denoted as the upper letter $G$, and the second one is the local coordinate of the target denoted as the upper letter $L$. Figure 1 shows the two coordinates.

We assume $n_k$ measurements ${\left\{z_{k,i}\right\}}_{i=1}^{n_k}$ at time $k$ are received, and so the ith measurement $z_{k,i}$ can be expressed as

$$z_{k,i}^G=x_k^c+p_{k,i}f({\gamma }_{k,i}^L)+{\overline{e}}_k, {\overline{e}}_k~\mathcal{N}(0,\overline{R})$$

(61)

where $p_{k,i}=\frac{z_{k,i}^G-x_k^c}{‖z_{k,i}^G-x_k^c‖}$ is the direction vector. Measurements in the local coordinate can be derived using the global coordinate measurements and the reference quaternion as

$$z_{k,i}^L=R_G^L(q_k)(z_{k,i}^G-x_k^c).$$

(62)

The azimuth and elevation of the measurements are

$${\theta }_k^L=\arctan \left(\frac{y^L}{x^L}\right),$$

(63)

$${\varphi }_k^L=\arctan \left(\frac{z^L}{\sqrt{{(x^L)}^2+{(y^L)}^2}}\right),$$

(64)

$${\gamma }_k^L=({\theta }_k^L,{\varphi }_k^L).$$

(65)

Substituting Equation (21) into Equation (61) gives

$$z_{k,i}^G=x_k^c+p_{k,i}\left[H^f({\gamma }_k^L)f_k+e_k^f\right]+{\overline{e}}_k,$$

(66)

which can be further simplified to obtain the pseudo-measurement function

$$h(x_k,z_{k,i}^G)+e_{k,i}=0, e_{k,i}~\mathcal{N}(0,R_{k,i}),$$

(67)

where

$$h(x_k,z_{k,i}^G)=x_k^c-z_{k,i}^G+p_{k,i}{H}^f({\gamma }_k^L)f_k,$$

(68)

$$e_{k,i}=p_{k,i}{e}_k^f+{\overline{e}}_k,$$

(69)

$$R_{k,i}=p_{k,i}{R}_{k,i}^f{p}_{k,i}^T+{\overline{R}}_{k,i}.$$

(70)

4. Gaussian Process Gaussian Mixture PHD Filter

4.1. Prediction Process

We use $\mathcal{G}\mathcal{A}\mathcal{M}(\cdot )$ to denote the Gamma distribution. The prediction process of the extended target PHD filter is given as [30]

$$D_{k+1|k}(\xi )=D_{k+1|k}^s(\xi )+D_k^b(\xi ),$$

(71)

$$D_{k+1|k}^s(\xi )={\displaystyle \sum }_{j=1}^{J_{k-1|k-1}}{w}_{k+1|k}^{(j)}\mathcal{G}\mathcal{A}\mathcal{M}({\gamma }_{k+1|k};{\alpha }_{k+1|k}^{(j)},{\beta }_{k+1|k}^{(j)})\mathcal{N}(x_{k+1|k};m_{k+1|k}^{(j)},P_{k+1|k}^{(j)}),$$

(72)

where $D_{k+1|k}^s(\xi )$ is the survival PHD, and $D_{k+1|k}^b(\xi )$ is the birth PHD representing new targets. The survival PHD is given as

$$w_{k+1|k}^{(j)}=p_s{w}_{k|k}^{(j)},$$

(73)

$${\alpha }_{k+1|k}^{(j)}=\frac{{\alpha }_{k|k}^{(j)}}{{\eta }_k},{\beta }_{k+1|k}^{(j)}=\frac{{\beta }_{k|k}^{(j)}}{{\eta }_k},$$

(74)

$$m_{k+1|k}^{(j)}=F_k{m}_{k|k}^{(j)},$$

(75)

$$P_{k+1|k}^{(j)}=F_k{P}_{k+1|k}^{(j)}{F}_k{}^T+Q_k.$$

(76)

The birth PHD is approximated using a Gaussian mixture model

$$D_{k+1|k}^b(\xi )={\displaystyle \sum }_{j=1}^{J_{b,k}}{w}_{b,k}^{(j)}\mathcal{G}\mathcal{A}\mathcal{M}({\gamma }_{b,k};{\alpha }_{b,k}^{(j)},{\beta }_{b,k}^{(j)})\mathcal{N}(x_{b,k};m_{b,k}^{(j)},P_{b,k}^{(j)}).$$

(77)

4.2. Update Process

Without considering the target spawn, the update process of the PHD filter is

$$D_{k+1|k+1}(\xi )=D_{k+1|k+1}^{\mathrm{ND}}(\xi )+D_{k+1|k+1}^{\mathrm{D}}(\xi ),$$

(78)

where $D_{k+1|k+1}^{\mathrm{ND}}(\xi )$ is the miss detection PHD given as

$$D_{k+1|k+1}^{\mathrm{ND}}(\xi )={\displaystyle \sum _{j=1}^{J_{k+1|k}}{w}_{k+1|k+1}^{\mathrm{ND},(j)}}\mathcal{G}\mathcal{A}\mathcal{M}({\gamma }_{k+1|k+1};{\alpha }_{k+1|k+1}^{\mathrm{ND},(j)},{\beta }_{k+1|k+1}^{\mathrm{ND},(j)})\times \mathcal{N}(x_{k+1|k+1};m_{k+1|k+1}^{\mathrm{ND},(j)},P_{k+1|k+1}^{\mathrm{ND},(j)})$$

(79)

where

$$w_{k+1|k+1}^{\mathrm{ND},(j)}=(1-p_D)w_{k+1|k}^{(j)},$$

(80)

$${\alpha }_{k+1|k+1}^{\mathrm{ND},(j)}={\alpha }_{k+1|k}^{(j)},{\beta }_{k+1|k+1}^{\mathrm{ND},(j)}={\beta }_{k+1|k}^{(j)},$$

(81)

$$m_{k+1|k+1}^{\mathrm{ND},(j)}=m_{k+1|k}^{(j)},$$

(82)

$$P_{k+1|k+1}^{\mathrm{ND},(j)}=P_{k+1|k}^{(j)}.$$

(83)

The updated target PHD is $D_{k+1|k+1}^{\mathrm{D}}(\xi )$, which is given as

$$\begin{array}{l}{D}_{k+1|k+1}^{\mathrm{D}}(\xi )={\displaystyle \sum _{\mathcal{P}\angle Z_k}{\displaystyle \sum _{W\angle \mathcal{P}}{\displaystyle \sum _{j=1}^{J_{k+1|k}}{w}_{k+1|k+1}^{(j,W)}}\mathcal{G}\mathcal{A}\mathcal{M}({\gamma }_{k+1|k+1};{\alpha }_{k+1|k+1}^{(j,W)},{\beta }_{k+1|k+1}^{(j,W)})}}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\times \mathcal{N}(x_{k+1|k+1};m_{k+1|k+1}^{(j,W)},P_{k+1|k+1}^{(j,W)}).\end{array}$$

(84)

The partition method in [30] is adopted in this article to reduce the computational load. The update of the Gamma distribution is

$${\alpha }_{k+1|k+1}^{(j,W)}={\alpha }_{k|k+1}^{(j)}+\left|W\right|,$$

(85)

$${\beta }_{k+1|k+1}^{(j,W)}={\beta }_{k|k+1}^{(j)}+1.$$

(86)

The GP regression measurement model derived in the previous section is used to update the Gaussian mixture component. The pseudo-measurement model Equation (67) is a non-linear system. Therefore, the EKF is used to update the extended target states, which can be given as follows

$$m_{k+1|k+1}^{(j,W)}=m_{k|k+1}^{(j)}+K_{k+1}\left(0-h\left(m_{k|k+1}^{(j)},z_k^{(W)}\right)\right),$$

(87)

$$P_{k+1|k+1}^{(j,W)}=P_{k|k+1}^{(j)}-K_{k+1}{H}_k{P}_{k|k+1}^{(j)}.$$

(88)

where $h(m_{k|k+1}^{(j)},z_k^{(W)})$ is given in (68) and the Kalman gain $K_{k+1}$ is

$$K_{k+1}=P_{k|k+1}^{(j)}{H}_k^T{S}_k{}^{-1},$$

(89)

$$S_k=H_k{P}_{k|k+1}^{(j)}{H}_k^T+R_k,$$

(90)

$$H_k=\frac{d}{d{x}_k}{h}_k(m_{k|k+1}^{(j)},z_k^{(W)})\left|{}_{x_k={\widehat{m}}_{k|k+1}^{(j)}}.\right.$$

(91)

The weight of the gamma Gaussian mixture component is

$$w_{k+1|k+1}^{(j,W)}=\frac{w_{\mathcal{p}}{p}_D{ℒ}_k^{(j,W)}{w}_{k|k-1}^{(j)}}{d_W{\beta }_{\mathrm{FA},k}^{|W|}},$$

(92)

$$\begin{array}{cc}\hfill {\mathcal{L}}_k^{(j,W)}& ={\displaystyle \prod _{z_k\in W}\varphi (z_k|x_k)}\hfill \\ & =\mathcal{N}\left(W;h\left(m_{k|k+1}^{(j)},z_k^{(W)}\right),R_k\right),\hfill \end{array}$$

(93)

where ${\varphi }_{z_k}(\cdot )$ is the single extended target measurement likelihood.

5. Simulation Results and Discussion

In this section, a simulation experiment is conducted to compare the performance of the GP-PHD filter proposed in the article with the traditional GGIW-PHD filter [33]. A three-dimensional area with size $[-30 \mathrm{m},-30 \mathrm{m},-5 \mathrm{m}]\times [30 \mathrm{m}, 30 \mathrm{m}, 5 \mathrm{m}]$ is considered. We assume that two extended targets move in the x–y–z plane. Target 1 (T1) is a cylinder with $r=1 \mathrm{m}, h=2 \mathrm{m}$ and target 2 (T2) is a cube with $w=2 \mathrm{m}, l=4 \mathrm{m},$ and $h=2 \mathrm{m}$. We denote the target states as $x={[m_x,m_y,m_z,v_x,v_y,v_z]}^T$. The initial states of T1 is $x_1={[0,0,0,1,0,0]}^T$ and the initial states of T2 is $x_2={[10,0,1,1,0,0]}^T$. T1 moves at constant speed $v=1 \mathrm{m}/\mathrm{s}$ along the y axis. T2 moves at constant speed $v=1.5 \mathrm{m}/\mathrm{s}$ in the first 10 s, and then makes a constant turn at speed $\omega =\frac{\pi }{20} \mathrm{rad}/\mathrm{s}$ for 10 s. After the constant turn, T2 moves along the y axis at a constant speed. Both targets’ moving duration is $30 \mathrm{s}$ and the sampling time is $0.1 \mathrm{s}$. The experiment is a simulation of a typical traffic scenario, where T1 corresponds to the pedestrian and T2 corresponds to a practical medium size vehicle. Both linear and non-linear motion models of extended targets are considered. Although we use a linear motion model, the filter is expected to be robust enough to handle such model mismatches, which are common in most tracking applications.

We assume that all the measurements come from the surface of the target and that the number of the measurements is Poisson-distributed. The Poisson rate of the measurements is $\lambda =20$ and the covariance matrix of the measurement noise is $\overline{R}={0.1}^2{{\rm I}}_3{ \mathrm{m}}^2$. The Poisson rate of the number of clutter is ${\lambda }_c=2$, and clutter is uniformly distributed in the surveillance area. Figure 2 shows clutter and measurements collected in the surveillance area. All experiments in the following section are carried out with different realizations of the measurement noise, the measurement source, and the clutter. The result numbers of the experiments are the averages of Monte Carlo (MC) runs.

The CV model is considered for both the GGIW-PHD filter and the GP-PHD filter. The process noise of the dynamic model is ${\sigma }_x=0.1$. For the GP-PHD filter, $642$ basis points equally located in $[0,2\pi ]$ are used and the hyperparameters of the GP model are set to $\mu =0, {\sigma }_f=1, {\sigma }_r=0.2, l=\pi /3$. The parameters for the GGIW-PHD filter are given in

Table 1. Parameters of the GGIW-PHD filter.

Parameter	Value
Forgetting factor ${\eta }_k$	1.05
Detection probability $p_D$	0.95
Survival probability $p_s$	0.99
Birth weight ${\omega }_k{}^{(b)}$	0.11
Birth rate $a_k^{(b)}, b_k^{(b)}$	100, 5
Birth extension $v_k^{(b)}, V_k^{(b)}$	10, 10
Pruning threshold T	${10}^{-3}$
Merging threshold U	25
Temporal decay τ	120
Scaling parameter ρ	1/4

.

The number of the gamma Gaussian mixture component is no more than $M_{max}=100$ and the number of Gaussian components per target is no more than $J_{max}=100$. The tracking results of the two filters are shown in Figure 3. Figure 3 shows that both filters are able to deal with the non-linear movements of the targets, while the GP-PHD filter is more capable of handling the motion model mismatches and the estimated trajectory is closer to the truth.

We use the OSPA distance to evaluate the performance of the filter, which takes into account both errors made in estimating target states and the errors in estimating target cardinality. The order and the penalty factor are set as $c=15, p=1$. The average result is obtained through 100 MC runs, which is shown in Figure 4.

The estimation of the shape is illustrated in Figure 5. The GP-PHD filter shows superior performance compared to the GGIW-PHD filter in terms of shape estimation. The GP-PHD filter uses a radial function to describe the shape of the target, which is suitable for arbitrary shape extended targets. However, the GGIW-PHD filter is limited to ellipsoidal shape targets. Therefore, both filters show satisfactory performance in estimating the shape of T1 in Figure 5a. However, the GGIW-PHD filter shows a poor performance in estimating the shape of T2, especially when the target is making a turn. The GP-PHD filter takes into account the orientation of the target. Figure 5b shows that the GP-PHD filter is able to estimate the shape of the target, even when the target is making a turn.

The performance of the estimation of the shape is evaluated using the IoU, which is defined as

$$\mathrm{IoU}(S_{\mathrm{true}},\widehat{S})=\frac{\mathrm{volume}(S_{\mathrm{true}}\cap \widehat{S})}{\mathrm{volume}(S_{\mathrm{true}}\cup \widehat{S})},$$

(94)

where $S_{\mathrm{true}}$ is the true target shape and $\widehat{S}$ is the estimated target shape. The IoU value not only accounts for the quality of the estimation of the kinematics states but also the extent. The average results of 100 MC runs are shown in Figure 6. Overall, the GP-PHD filter estimates the shape of the extended targets better than the GGIW-PHD filter. The performance of the GGIW-PHD filter is not stable, particularly when the target is making a turn.

We further examine the measurement rates’ impact on the proposed filter and the GGIW-PHD filter. The measurement rates are set as $\lambda =10,30$. The experiments are repeated for 50 MC runs, respectively, to obtain the average results shown in Figure 7 and Figure 8. It is reported that the GP-PHD filter shows good performance at both low and high measurement rates, while the measurement rate has a great impact on the performance of the GGIW-PHD filter. The GP-PHD filter is more robust compared to the GGIW-PHD filter. The main reason for the impact of the measurement rate on the GGIW-PHD filter is that there is less information about the shape that can be exploited by the GGIW-PHD filter when the measurement rate is low. However, the covariance function of the GP model assumes that the radial function has a period of $\pi$. This assumption suggests that the target is symmetric, which is the usual case in tracking. Therefore, under low-measurement-rate conditions, the GP-PHD filter can obtain more information about the target and a more accurate estimation of the shape can be expected.

All the simulations are conducted in MATLAB 2022b on a computer with an AMD Ryzen 5 3600X 6-Core processor and 16 GB RAM. The average computation time for the GP-PHD filter and the GGIW-PHD filter are 459 s and 5.11 $\mathrm{s}$ for 300 updates. Both the GGIW-PHD filter and the GP-PHD filter are recursively updated by the standard EKF, and therefore the computational load is mainly determined by the dimension of the state vector. The state dimension of the GP-PHD filter is $\dim (x_k)=654$, which is much greater than that of the GGIW-PHD filter. Further improvements, such as projecting the contour of the 3D target into 2D space to reduce the computational burden, can be considered.

6. Conclusions

In this paper, we propose a new approach for tracking multiple extended targets in three-dimensional space. The key contribution of our method is the utilization of a 3D Gaussian process radial function to describe the shape of the extended targets. Furthermore, we also present a state estimation method that allows for the recursive estimation of the target’s shape. A pseudo-measurement equation is derived, and a Gaussian mixture implementation of the filter is presented in the article. The proposed filter is capable of tracking the target and estimating the shape of the target simultaneously in the presence of clutter. The method is flexible enough to express a variety of different shapes. We evaluate the performance of the proposed filter and the traditional GGIW-PHD filter in the simulation experiment using the OSPA distance and the IoU value. The results demonstrate that our proposed filter outperforms the traditional GGIW-PHD filter in both kinematic states and shape estimation. Furthermore, we compare the performance of the filters under different-measurement-rate conditions. The proposed filter is robust under different-measurement-rate conditions, while the GGIW-PHD filter suffers under low-measurement-rate conditions. The algorithm provides an effective solution to the ETT problem in 3D space, which can be further used in scenarios such as autonomous driving.