PHD Filter for Object Tracking in Road Traffic Applications Considering Varying Detectability

Abstract

This paper considers the object detection and tracking problem in a road traffic situation from a traffic participant’s perspective. The information source is an automotive radar which is attached to the ego vehicle. The scenario characteristics are varying object visibility due to occlusion and multiple detections of a vehicle during a scanning interval. The goal is to maintain and report the state of undetected though possibly present objects. The proposed algorithm is based on the multi-object Probability Hypothesis Density filter. Because the PHD filter has no memory, the estimate of the number of objects present can change abruptly due to erroneous detections. To reduce this effect, we model the occlusion of the object to calculate the state-dependent detection probability. Thus, the filter can maintain unnoticed but probably valid hypotheses for a more extended period. We use the sequential Monte Carlo method with clustering for implementing the filter. We distinguish between detected, undetected, and hidden particles within our framework, whose purpose is to track hidden but likely present objects. The performance of the algorithm is demonstrated using highway radar measurements.

1. Introduction

Object detection and tracking in road traffic is a developing field of the automotive industry. Estimation in road traffic environments has several peculiarities. The number of objects to be detected and tracked is usually varying in time, as well as the number of incoming measurements, and possibly the number of sensors mapping the environment is not constant. Objects may enter and leave the observed area, and the visibility in the sensor field of view can change abruptly. The number of available sensors and the possible combinations of them offers a great variety in terms of cost and performance [1,2]. Cost-effectiveness is a major driving force in both development and production. A considerable part of the development and testing of autonomous functions are done in simulated environments [3,4]. For vehicular applications, it is an essential requirement to run an algorithm in real time.

Estimation in road traffic is typically a multi-object problem. Besides the well-established approaches such as the joint probabilistic data association filter (JPDAF) and the multiple hypothesis tracking (MHT), random finite set (RFS)-based solutions started to appear in the last two decades [5]. The multi-object tracking problem covers the estimation of the number of objects present at the scene and the corresponding states. The JPDAF needs to know the number of objects to be tracked and expects the object tracks to be initialized. The MHT works with the assumption that the objects to be detected are point-like, and each generates zero or one measurement in a detector. RFS-based methods are emerging approaches to the multi-object tracking problem. RFS can model object birth and death as well as the clutter process and miss-detection. RFSs can be inserted into the Bayesian framework and used to give recursive estimates about multi-object states. The exact solution to the RFS-based multi-object filtering problem has combinatorial complexity and is generally intractable, thus its solution needs approximations. The probability hypothesis density (PHD) filter [6], which scales linearly with both the number of tracked objects and the measurements, is a practical solution to the multi-object filtering problem that uses first-order approximations. It can be realized with Gaussian mixture [7] or Sequential Monte Carlo [8] approaches.

Automotive applications of the PHD filter have already appeared. Maehlisch et al. used an extended Kalman filter to estimate the ego vehicle state and then used it as a control input in the particle PHD filter to estimate the state of observed traffic participants using LiDAR and camera measurements [9]. Kalyan et al. extracted blobs from 3D LiDAR data and used the Gaussian Mixture PHD filter to track pedestrians in an urban environment [10]. Occlusion of traffic participants poses difficulties in estimation [11]. In the case of partial occlusion, an object could be visible to a sensor; however, the measured intensity or extracted features may be affected [12].

Lin et al. presented a track labeling particle PHD filter wherein birth particles are generated efficiently by assigning more weight to particles close to existing tracks [13].

1.1. Related Work

An automotive application of the PHD filter is presented in [14] where vehicles are tracked using features extracted from a monocular camera. Chen et al. reported an underwater tracking application of the PHD and CPHD filters wherein the state-dependent probability of detection values is calculated from a sonar model [15].

An approach to extended target tracking is to model the objects by ellipsoids, which can be represented by positive definite matrices. A multinormal random variable and its covariance matrix from an inverse-Wishart distribution can be inserted into the recursive Bayesian framework to estimate extended target states [16]. The PHD filter using Gaussian inverse-Wishart distributions is reported in [17], and two methods are proposed to partition the measurement set for efficient likelihood computation.

Huang et al. [18] uses the Gaussian inverse-Wishart distribution in the PHD filter to track extended targets and considers the problem to determine whether the source of a measurement is an actual object or clutter.

Zheng and Gao proposed a vehicular application of the PHD filter where roadmap information is used to integrate constraints into the state estimation and fine-tune the process noise covariance matrix to direct the noise along the road. The constraint that the vehicle orientation must be the same as the direction of the road and the velocity vector points in the same direction can be formulated as a linear system of equations and incorporated into the filter [19].

The PHD filter equations have been adopted to the Poisson extended-targets model in [20] to be able to handle extended targets. The applied sensor model involves the combinatorial partitioning of the measurement set, which renders the likelihood computation demanding. Tang et al. derived a multiple detection PHD filter that can be transformed into an extended target or a multisensor PHD filter [21]. Predefined modes represent different measurement models, where the probability of detection is different in each mode. Implementing the multiple detection GM-PHD filter has greater computational requirements, at least a magnitude higher than the classical GM-PHD filter.

The PHD filter’s performance can degrade when a previously observed or tracked target is not detected for a short period [22,23]. In such an event, the PHD mass corresponding to an undetected target is shifted to the detected targets. This phenomenon is addressed as the “spooky effect” at a distance [24]. An approach to reduce this effect is to give a realistic model of the object visibility and consider that the probability of detection is state-dependent, i.e., $p_d=p_d\left(\mathbf{x}\right)$.

Hendeby and Karlsson presented a GM-PHD filter with variable probability of detection. The state-space is divided into segments with different constant $p_d$ values. The proposed method approximates the state-dependent probability of detection with a constant value for every Gaussian component [25].

Yazdian-Dehkordi and Azimifar proposed a GM-PHD filter that adopts state-dependent probability of survival. To track and report undetected targets, each Gaussian component is assigned a probability of confirming on which track management is based [26]. Wang et al. presented a SMC-PHD filter that is suitable for scenarios with low probability of detection. The proposed method uses a state-dependent probability of survival to model that objects can enter and exit the sensor field of view. Upon missed detection, the posterior particle weights are revised. False detection and real targets are distinguished with the help of the sequential probability ratio test [27]. Gao et al. proposed a multi-frame GM-PHD filter to manage the weights of Gaussian components corresponding to undetected targets. This approach tries to add inertia to the weights to achieve more reliable track estimates [28].

1.2. Contributions of the Paper

In this paper, we address the following issues. As the PHD filter has no memory, its estimation about the number of objects present can change abruptly due to misdetections. To reduce this effect, we model object occlusion to compute a state-dependent probability of detection. This way, the filter can maintain undetected but probably valid hypotheses for a more extended period. Regarding filter realization, we use an SMC method with clusterization based on the work presented in [29]. In our framework, we distinguish detected, undetected, and hidden particles. The purpose of hidden particles is to help the tracking of undetected but probably present objects. This approach enables the creation of labeled track estimates without augmenting the state vector with a label variable. The performance of the proposed filter is analyzed in challenging simulations.

The paper is organized as follows. In Section 2, we begin by summarizing the theoretical background and providing motivation for the presented work. Section 3 details the proposed filter. The occlusion model for computing a state-dependent probability of detection is detailed in Section 4. In Section 5 we evaluate the filter performance in abstract simulations and simple traffic scenarios. Section 6 presents a case study with highway radar measurement to validate the proposed method. The results are discussed in Section 7.

2. Theoretical Background

This section summarizes the theoretical concepts needed for the proposed method. After presenting the original PHD filter, we discuss particle filter implementation questions focusing on road traffic applications. In a multi-target scenario, the number of objects to be tracked is, in general, unknown a priori and time-dependent. Similarly, the set of incoming measurements has variable cardinality. Since the PHD filter equations are defined over the single-object state-space, no multi-object motion and measurement models are needed.

The discrete dynamic model that will be used throughout this work consists of the equations

$$\begin{array}{cc}\hfill {\mathbf{x}}_{k+1}& =f_k\left({\mathbf{x}}_k\right)+{\mathbf{w}}_k\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill {\mathbf{z}}_k& =h_k\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k,\hfill \end{array}$$

(2)

where $f_k$ is the state transition function, $h_k$ is the measurement function and k stands for the time index. The uncertainties are modeled as additive zero mean white Gaussian noises with covariance matrices $Cov\left[\mathbf{w}\right]=Q$ and $Cov\left[\mathbf{v}\right]=R$.

2.1. Random Finite Sets

A random finite set X consisting of random vector variables takes the form $X=\{{\mathbf{x}}_1,\dots ,{\mathbf{x}}_n\}$, where the cardinality $\left|X\right|=n$ is also a random number, distributed according to $\varrho \left(n\right)$. The elements in the random set are unordered, making every permutation equivalent. The mathematical apparatus providing methods to carry out calculations with RFSs is called finite set statistics (FISST), summarized in the monograph [30]. The FISST PDF can describe the distribution of an RFS:

$$f\left(X\right)=n!\varrho \left(n\right)p_n({\mathbf{x}}_1,\dots ,{\mathbf{x}}_n),$$

(3)

where $p_n({\mathbf{x}}_1,\dots ,{\mathbf{x}}_n)$ is the symmetric joint distribution of random vectors ${\mathbf{x}}_1,\dots ,{\mathbf{x}}_n$ and $\varrho \left(n\right)$ is the cardinality distribution. A Bayes type multi-object filter can be formulated with RFSs; however, the exact solution is generally not tractable due to its combinatorial complexity. A computationally tractable solution to the recursive Bayes multi-object filtering problem can be derived in numerous ways [31]. An approach that leads to a linearly scaling filter is to approximate the full multi-object density $f\left(X\right)$ with its first-order moment resulting in the probability hypothesis density (PHD) filter.

The first-order moment of the FISST PDF $f\left(X\right)$ defined over the single-object state-space is written as

$$D\left(\mathbf{x}\right)=\int {\delta }_X\left(\mathbf{x}\right)f\left(X\right)\delta X.$$

(4)

The scalar-valued PHD or intensity function defined in (4) takes higher values at locations in the state-space where an object is likely to be found. Integrating $D\left(\mathbf{x}\right)$ over a region $\Omega$ in the state-space yields the expectation of the cardinality of X, which is the expected number of objects in that region:

$${\int }_{{\Omega }}D\left(\mathbf{x}\right)\mathrm{d}\mathbf{x}=\mathbb{E}\left[\right|X\left|\right].$$

(5)

2.2. PHD Filter

The PHD filter is a recursive algorithm formulated in the Bayes framework. However, instead of the full multi-object PDF, only the PHD function is propagated through the recursion. The PHD filter, to achieve closed-form formulas, uses the Poisson RFS to model targets. The FISST density function of a Poisson RFS is

$$f\left(X\right)={\mathrm{e}}^{-\lambda }\prod _{\mathbf{x}\in X}\lambda p\left(\mathbf{x}\right),$$

(6)

and the distribution of the cardinality is Poisson with parameter $\lambda$:

$$\varrho \left(n\right)=\frac{{\mathrm{e}}^{-\lambda }{\lambda }^n}{n!}.$$

(7)

The PHD function of a Poisson RFS is

$$D\left(\mathbf{x}\right)=\lambda p\left(\mathbf{x}\right),$$

(8)

which integrates to $\lambda$.

Given the PHD at timestep $k-1$ as $D_{k-1|k-1}\left(\mathbf{x}\right)$ the prediction equation of the PHD filter is

$$D_{k|k-1}\left(\mathbf{x}\right)={\gamma }_{k|k-1}\left(\mathbf{x}\right)+p_s\int {\pi }_{k|k-1}\left(\mathbf{x}\right|{\mathbf{x}}^{\prime })D_{k-1|k-1}\left({\mathbf{x}}^{\prime }\right)\mathrm{d}{\mathbf{x}}^{\prime },$$

(9)

where ${\pi }_{k|k-1}\left(\mathbf{x}\right|{\mathbf{x}}^{\prime })$ stands for the transition density from timestep $k-1$ to k and $p_s$ is the probability of object survival. Objects appearing between timestep $k-1$ and k are represented by the birth PHD ${\gamma }_{k|k-1}\left(\mathbf{x}\right)$. Updating the PHD is carried out according to

$$D_{k|k}\left(\mathbf{x}\right)=\left[1-p_d\left(\mathbf{x}\right)+\sum _{\mathbf{z}\in {\mathbf{Z}}_k}\frac{p_d\left(\mathbf{x}\right)g_k\left(\mathbf{z}|\mathbf{x}\right)}{\kappa \left(\mathbf{z}\right)+\int p_d\left(\mathbf{x}\right)g_k\left(\mathbf{z}|\mathbf{x}\right)D_{k|k-1}\left(\mathbf{x}\right)\mathrm{d}\mathbf{x}}\right]D_{k|k-1}\left(\mathbf{x}\right),$$

(10)

where $p_d\left(\mathbf{x}\right)$ is the probability of detection at location $\mathbf{x}$ in the state-space, $g_k\left(\mathbf{z}\right|\mathbf{x})$ is the measurement likelihood function and $\kappa \left(\mathbf{z}\right)$ represents the clutter density from where false measurements are originating.

It is beneficial to construct the birth PHD ${\gamma }_{k|k-1}\left(\mathbf{x}\right)$ with the help of the measurements ${\mathbf{z}}_{k-1}$ [29]. This way, new objects will only appear in regions of the state-space that are covered by measurements. The birth PHD is computed via the time-prediction integral as

$${\gamma }_{k|k-1}\left(\mathbf{x}\right)=p_s\int {\pi }_{k|k-1}\left(\mathbf{x}\right|{\mathbf{x}}^{\prime }){\gamma }_{k-1}\left({\mathbf{x}}^{\prime }\right)\mathrm{d}{\mathbf{x}}^{\prime },$$

(11)

where ${\gamma }_{k-1}\left(\mathbf{x}\right)$ is the spatial PDF of object birth assembled as a mixture:

$${\gamma }_{k-1}\left(\mathbf{x}\right|{\mathbf{Z}}_{k-1})=\frac{1}{|{\mathbf{Z}}_{k-1}|}\sum _{\mathbf{z}\in {\mathbf{Z}}_{k-1}}\beta \left(\mathbf{x}\right|\mathbf{z}).$$

(12)

The density $\beta \left(\mathbf{x}\right|\mathbf{z})$ represents the image of a measurement in the state-space.

Regarding filter implementation, practical methods are the Gaussian mixture and particle filter approaches. In this work, we concentrate on the particle filter implementation, which is based on the weighted sum approximation of the PHD function using N samples at locations ${\mathbf{x}}^{\left(i\right)}$ with weights $w^{\left(i\right)}$:

$$D\left(\mathbf{x}\right)\approx \sum _{i=1}^N{w}^{\left(i\right)}{\delta }_{{\mathbf{x}}^{\left(i\right)}}\left(\mathbf{x}\right).$$

(13)

2.3. Problem Statement and Motivation

The classical PHD filter uses the standard measurement model that makes the assumptions that a target can generate at most one measurement, and a measurement cannot be generated by more than one target [20]. In this case, the PHD filter has linear scaling properties in terms of the tracked objects and in the number of processed measurements.

Implementing the PHD filter with particle approximation can be done relatively easily, as described in [30] (Chapter 16.5.2). The PHD filter does not produce discrete object state estimates. It must be extracted from the PHD function, either represented by an analytical formula or a particle system. Algorithms such as k-means or expectation–maximization can be used to create particle clusters. The number of clusters to be created is guided by the expected number of objects, which is the sum of particle weights. This expectation is, however, not stable and requires averaging [30] (p. 623).

Updating the PHD function using particle approximation can be done, according to (10), in one step. This standard approach, referred to as the pseudo-likelihood update, has drawbacks making the filtering less effective [32]. On the one hand, if measurements do not support a particle, its weight will be decreased by the factor $1-p_d$. If an object is absent from the measurement set due to miss-detection, but present at the scene, the filter is unlikely to resample the corresponding particles. Consequently, upon re-detecting the object, particles need to be drawn again from the birth pool. On the other hand, extracting the posterior multi-object state from the particles is not straightforward. The clustering algorithm needed to give object state estimates increases the computational requirement and is essentially ad-hoc.

In case an object is temporarily occluded, particles representing it are likely to be dropped by the filter in a few timesteps, and no point estimate could be created. If the occlusion is anticipated by means of adjusting the probability of detection to a low value, undetected particles can exist for a longer time. This can be illustrated by writing (10) as a particle approximation:

$$w_{k|k-1}^{\left(i\right)}=\left[1-p_d\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right)\right]w_{k|k-1}^{\left(i\right)}+\left[\sum _{\mathbf{z}\in {\mathbf{Z}}_k}\frac{p_d\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right)g_k\left(\mathbf{z}|{\mathbf{x}}_{k|k-1}^{\left(i\right)}\right)}{\kappa \left(\mathbf{z}\right)+{\sum }_{j=1}^N{p}_d\left({\mathbf{x}}_{k|k-1}^{\left(j\right)}\right)g_k\left(\mathbf{z}|{\mathbf{x}}_{k|k-1}^{\left(j\right)}\right)w_{k|k-1}^{\left(j\right)}}\right]w_{k|k-1}^{\left(i\right)},$$

(14)

where the first term on the right-hand side stands for the undetected, the second term for the detected particle weights. At positions in the state-space where $p_d\left(\mathbf{x}\right)$ has low values, undetected particle weights will reduce by a small amount helping particles to exist longer.

Erdinc et al. analyzed the effect of missed detection to the expected number of objects in a single-target scenario with constant $p_d$ [22]. The underestimation of the expected target number is due to the first-order approximation used in the PHD filter [33] (p. 199). If no measurements arrive at timestep k then the PHD filter estimates

$$N_{k|k}=(1-p_d)N_{k|k-1},$$

(15)

while the exact formula would be

$$N_{k|k}=\frac{(1-p_d)N_{k|k-1}}{1-N_{k|k-1}{p}_d}.$$

(16)

The difference is negligible if $p_d$ is close to unity but significant otherwise.

We mention one more aspect that is relevant to the current application. Since the birth density is generated from the measurements, multiple detections of the same object must be considered. An individual object may be detected numerous times either because it is extended, or the sensor’s refresh rate is higher than the filtering timestep. In this case, the birth density would gain more weight, which can be desired or undesired. On the other hand, the expected number of newborn or already present objects is biased, which is clearly undesired.

3. The Proposed Method

Our approach builds on the PHD particle filter algorithm described in [29], wherein the central idea is to create particle clusters in a probabilistic manner, using the measurement set. Each particle cluster is updated independently by a standard bootstrap particle method. A discrete object is extracted from a cluster and reported by the filter if enough particle weight is accumulated in one. The number of particle clusters and the number of reported objects are upper bounded by the measurement set cardinality. The clustering method above uses a sensor model that generates zero or one measurement per object. This assumption ensures that a probability distribution can be generated that measures how likely a particle belongs to a measurement. It should be noted that only targets that have been detected at the current timestep have a chance to be reported.

We aim to track and report objects that are undetected but possibly present. To achieve this, the proposed filter distinguishes detected, hidden, and undetected particles. The difference between undetected and hidden particles is that objects can be reported based on hidden particles if enough particle weight is concentrated. Suppose the filter considers a cluster of particles to be an object that should be reported based on the sum of the contained particle weights. In that case, the point estimate and the object state’s covariance is stored with the associated label. In the next iteration, birth particles are generated using measurements from the previous timestep. Every particle and the stored point estimates are propagated according to the motion model. In the update step, we create the union of the collected measurements and the stored point estimates and with a given threshold merge the close matches.

The update method for hidden particles is the same as for undetected particles; however, the same clusterization method is used as for detected particles. The likelihood computation happens with the augmented measurement set. For the real measurements, the covariance matrix R is used while for the point estimate components the covariance matrix computed from the sample is substituted into the likelihood function. The resultant likelihood values are used to create particle clusters but not to update particle weights. The actual update step happens cluster-wise with the associated measurement or object generated pseudo-measurement. Hidden particles are updated as undetected particles. This way we introduce no additional information by increasing particle weights with the pseudo-measurements.

The filter is initialized with the assumption that there are no detected objects, thus with zero particle. Objects will emerge from the measurement generated birth particles. For every measurement $n_p$ particles are drawn from the birth pool, which is defined by the inverse measurement model $h^{-1}\left(\mathbf{z}\right)$. Besides the measurement generated birth particles, the filter uses birth particles originating from the assumed hidden objects. A hidden object is a reported point estimate computed from a hidden particle cluster.

At timestep $k-1$ let ${\left\{{\mathbf{x}}_{k-1}^{\left(i\right)},w_{k-1}^{\left(i\right)}\right\}}_{i=1}^{N_{k-1}}$ denote the particle system approximating the posterior PHD function $D_{k-1}\left(\mathbf{x}\right)$. Beside particles, reported object states and labels are also propagated in the recursion. Labels are discrete identifiers, typically a unique integer number for every object. Let $O_{k-1}$ denote the set of estimated object states, covariance matrices and labels with cardinality $|O_{k-1}|=o_{k-1}$ thus

$$O_{k-1}={\left\{{\mathbf{c}}_{k-1}^{\left(i\right)},C_{k-1}^{\left(i\right)},l_{k-1}^{\left(i\right)}\right\}}_{i=1}^{o_{k-1}},$$

(17)

where ${\mathbf{c}}_{k-1}$ is the estimated state and $C_{k-1}$ is the error covariance matrix and $l_{k-1}$ is the label. Similarly, the set of hidden objects is

$$O_{k-1,h}={\left\{{\mathbf{c}}_{k-1,h}^{\left(i\right)},C_{k-1,h}^{\left(i\right)},l_{k-1}^{\left(i\right)}\right\}}_{i=1}^{o_{k-1,h}},$$

(18)

where $|O_{k-1}|=o_{k-1,h}$. The measurement set from timestep k is $Z_k=\{{\mathbf{z}}_k^{\left(1\right)},\dots ,{\mathbf{z}}_k^{\left(m_k\right)}\}$.

For birth particles the distribution to be drawn from is created as a mixture generated by the measurements and the hidden object estimates. For the measurement driven birth pool the procedure is the following [34] (p. 68). Given $m_{k-1}$ measurements the birth pool $b_{k-1,z}\left(\mathbf{x}\right|Z_{k-1})$ is created by inverting $h\left(\mathbf{x}\right)$ for every measurement yielding the mixture

$$b_{k-1,z}\left(\mathbf{x}\right|Z_{k-1})=\sum _{i=1}^{m_{k-1}}{\beta }_{k-1}\left(\mathbf{x}\right|{\mathbf{z}}_{k-1}^{\left(i\right)}),$$

(19)

where the individual Gaussians have the form

$${\beta }_{k-1}\left(\mathbf{x}\right|{\mathbf{z}}_{k-1}^{\left(i\right)})=\mathcal{N}\left(\mathbf{x};h_{k-1}^{-1}\left({\mathbf{z}}_{k-1}^{\left(i\right)}\right),H_{k-1}{R}_{k-1}{H}_{k-1}^{\top }\right).$$

(20)

For the birth particles that are generated by the hidden objects, the birth pool is

$$b_{k-1,h}\left(\mathbf{x}|O_{k-1,h}\right)=\sum _{i=1}^{o_{k-1,h}}{\beta }_{k-1,h}\left(\mathbf{x}\right|{\mathbf{c}}_{k-1,h}^{\left(i\right)}),$$

(21)

where the individual Gaussians are

$${\beta }_{k-1,h}\left(\mathbf{x}|{\mathbf{c}}_{k-1,h}^{\left(i\right)}\right)=\mathcal{N}\left(\mathbf{x};{\mathbf{c}}_{k-1,h}^{\left(i\right)},C_{k-1,h}^{\left(i\right)}\right).$$

(22)

The overall birth density function assembles as a normalized mixture:

$$b_{k-1}\left(\mathbf{x}\right|Z_k\bigcup O_{k-1,h})=\frac{1}{m_{k-1}}{b}_{k-1,z}\left(\mathbf{x}\right|Z_{k-1})+\frac{1}{o_{k-1,h}}{b}_{k-1,h}\left(\mathbf{x}\right|O_{k-1,h}).$$

(23)

Sampling from the distributions $b_{k-1}$ results the birth particle system $\left\{{\mathbf{x}}_{b,k-1}^{\left(i\right)},w_{b,k-1}^{\left(i\right)}\right\}$${}_{i=1}^{N_{b,k-1}}$ where the weights are set such that the particles represent ${\nu }_{b,k-1}$ objects: $w_{b,k-1}^{\left(i\right)}={\nu }_{b,k-1}/N_{b,k-1}$ and the total number of birth particles is

$$N_{b,k-1}=n_p\left(m_{k-1}+o_{k-1,h}\right).$$

(24)

The posterior birth intensity, ${\nu }_{b,k}$ can be evaluated, according to [35], by summing up the newborn particle weights and if it is found to be unrealistic the prior ${\nu }_{b,k-1}$ values can be modified, and changes propagated to the update step.

The particles coming from timestep $k-1$ and the birth particles create the ensemble ${\left\{{\mathbf{x}}_{k-1}^{\left(i\right)},w_{k-1}^{\left(i\right)}\right\}}_{i=1}^{N_{k-1}+N_{b,k-1}}$ on which the prediction step is performed according to the motion model:

$${\mathbf{x}}_{k|k-1}^{\left(i\right)}\sim \mathcal{N}\left(f_{k-1}\left(x_{k-1}^{\left(i\right)}\right),Q_{k-1}\right),$$

(25)

and the predicted weights are

$$w_{k|k-1}^{\left(i\right)}=p_s{w}_{k-1}^{\left(i\right)}.$$

(26)

Beside particles, the object states and covariances are also propagated as

$$\begin{array}{cc}\hfill {\mathbf{c}}_{k|k-1}^{\left(i\right)}& =f_{k-1}\left({\mathbf{c}}_{k-1}^{\left(i\right)}\right)\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill C_{k|k-1}^{\left(i\right)}& =F_{k-1}^{\left(i\right)}{C}_{k-1}^{\left(i\right)}{F}_{k-1}^{\left(i\right)\top }+Q_{k-1}\left(i=1\dots o_{k-1}\right)\hfill \end{array}$$

(28)

and

$$\begin{array}{cc}\hfill {\mathbf{c}}_{k|k-1,h}^{\left(j\right)}& =f_{k-1}\left({\mathbf{c}}_{k-1,h}^{\left(j\right)}\right)\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill C_{k|k-1,h}^{\left(j\right)}& =F_{k-1}^{\left(j\right)}{C}_{k-1,h}^{\left(j\right)}{F}_{k-1}^{\left(j\right)\top }+Q_{k-1}\left(j=O_{k-1}+1\dots o_{k-1,h}\right),\hfill \end{array}$$

(30)

where $F_{k-1}^{\left(i\right)}$ is the Jacobian of $f_{k-1}$ evaluated at ${\mathbf{c}}_{k-1}^{\left(i\right)}$ or ${\mathbf{c}}_{k-1,h}^{\left(i\right)}$ and the labels are propagated without change: $l_{k|k-1}^{\left(i\right)}=l_{k-1}^{\left(i\right)}$.

At the update step, likelihood values for every particle-measurement pair must be computed. At this stage, particles will be classified as detected, hidden, or not detected. The predicted but not detected objects will be considered to be pseudo-measurements taking values from (27)–(30).

The algorithm to match the predicted objects and measurements considers only the position coordinates, thus works on a two-dimensional marginal distribution. The Kullback–Leibler (KL) divergence is used to compare measurements and the predicted objects. For every measurement-object pair, a KL divergence value is computed. The null hypothesis that the measurement does not belong to any object is represented by a uniform distribution covering the area of interest. The size of the area sets the intensity of the distribution.

The KL divergence between two n-dimensional normal distribution with means ${\mu }_1,{\mu }_2$ and covariance matrices ${\Sigma }_1,{\Sigma }_2$ is

$$D_{KL}\left({\mathcal{N}}_1,{\mathcal{N}}_2\right)=\frac{1}{2}\left(log\frac{det{\Sigma }_2}{det{\Sigma }_1}-n+\mathrm{tr}\left({\Sigma }_2^{-1}{\Sigma }_1\right)+{({\mu }_2-{\mu }_1)}^{\top }{\Sigma }_2^{-1}({\mu }_2-{\mu }_1)\right).$$

(31)

For the null hypothesis, the KL divergence between a uniform $\mathcal{U}(\Gamma )$ and a normal distribution is computed:

$$D_{KL}\left(\mathcal{U}(\Gamma ),\mathcal{N}(\mu ,\Sigma )\right)={\int }_{\Gamma }\mathcal{U}(\Gamma )log\frac{\mathcal{U}(\Gamma )}{\mathcal{N}(\mu ,\Sigma )}\mathrm{d}\mathbf{x}.$$

(32)

Considering a rectangular region of $\Gamma =(b-a)(d-c)$ and a two-dimensional normal distribution the integral evaluates to

$$\begin{array}{c}{D}_{KL}\left(\mathcal{U}(\Gamma ),\mathcal{N}(\mu ,\Sigma )\right)=log\frac{2\pi \sqrt{det\Sigma }}{\Gamma }+\\ +\frac{1}{6\Gamma }\left({\sigma }_{11}(a^3-b^3)(c-d)+\frac{3}{2}{\sigma }_{12}(a^2-b^2)(c^2-d^2)+{\sigma }_{22}(c^3-d^3)(a-b)\right)\end{array}$$

(33)

where ${\sigma }_{ij}$ are the elements of $\Sigma$.

An object, be it hidden or detected, is considered to be covered by a measurement j if

$$D_{KL}\left(\mathcal{N}({\mathbf{z}}_k^{\left(j\right)},R),\mathcal{N}(\mathbf{c},C)\right)>D_{KL}\left(\mathcal{U}\left({\Gamma }_k\right),\mathcal{N}(\mathbf{c},C)\right)$$

(34)

in which case the object $\mathbf{c}$, which can be ${\mathbf{c}}_{k|k-1}^{\left(i\right)}$ or ${\mathbf{c}}_{k|k-1,h}^{\left(i\right)}$ is appended to the measurement set.

Depending on whether the measurements come with labels, two strategies can be used. In the case of labeled measurements, the received label is used if no object can be associated. If an object matches the measurement, then the received label will be replaced with the object’s label. In unlabeled measurements, an unmatched measurement should be assigned an unused label chosen from a designed pool.

The augmented set ${\widehat{Z}}_k\supseteq Z_k$ may or may not contain more elements than the original measurement set and $|{\widehat{Z}}_k|=|Z_k|+{\widehat{o}}_k+{\widehat{o}}_{k,h}$ where ${\widehat{o}}_k$ and ${\widehat{o}}_{k,h}$ are the number of detected and hidden object inserted into the set. The numbers ${\widehat{o}}_k+{\widehat{o}}_{k,h}$ are coming from the condition (34): if the inequality holds for a certain object then $o_k$ or $o_{k,h}$ is incremented, depending on whether it was a detected or hidden object. Using ${\widehat{Z}}_k$ the particles are partitioned to form clusters. Partitioning is performed in a probabilistic manner as described in [32]. A probability is computed to every particle-measurement pair $\left({\mathbf{x}}_{k|k-1}^{\left(i\right)},{\widehat{\mathbf{z}}}_k^{\left(j\right)}\right),{\widehat{\mathbf{z}}}_k^{\left(j\right)}\in {\widehat{Z}}_k$ which measures how likely measurement ${\widehat{\mathbf{z}}}_k^{\left(j\right)}$ is generated by an object at state ${\mathbf{x}}_{k|k-1}^{\left(i\right)}$:

$$P_{ij}=\frac{p_d\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right)g_k\left({\widehat{\mathbf{z}}}_k^{\left(j\right)}\right|{\mathbf{x}}_{k|k-1}^{\left(i\right)})w_{k|k-1}^{\left(i\right)}}{\kappa \left({\mathbf{z}}_j\right)+{\sum }_{l=1}^{N_{k|k-1}}{p}_d\left({\mathbf{x}}_{k|k-1}^{\left(l\right)}\right)g_k\left({\widehat{\mathbf{z}}}_k^{\left(j\right)}\right|{\mathbf{x}}_{k|k-1}^{\left(l\right)})w_{k|k-1}^{\left(l\right)}},$$

(35)

where $N_{k|k-1}=N_{k-1}+N_{b,k-1}$. The likelihood value for a measurement is

$$g_k\left({\widehat{\mathbf{z}}}_k^{\left(j\right)}\right|{\mathbf{x}}_{k|k-1}^{\left(i\right)})=\mathcal{N}\left({\widehat{\mathbf{z}}}_k^{\left(j\right)};h_k\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right),R\right),\mathrm{for}j=1\dots \left|Z_k\right|$$

(36)

and for a pseudo-measurement

$$g_k\left({\widehat{\mathbf{z}}}_k^{\left(j\right)}\right|{\mathbf{x}}_{k|k-1}^{\left(i\right)})=\mathcal{N}\left(\mathbf{c};h_k\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right),C\right),$$

(37)

where $\mathbf{c}$ is a detected object for $j=|Z_k|+1,\dots ,|Z_k|+{\widehat{o}}_k$ and is a hidden object for $j=|Z_k|+{\widehat{o}}_k+1,\dots ,\left|Z_k\right|+{\widehat{o}}_k+{\widehat{o}}_{k,h}$. A particle is regarded as not detected with probability

$$P_{i0}=\left(1-p_d\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right)\right)w_{k|k-1}^{\left(i\right)}.$$

(38)

Normalizing the probabilities $P_{ij},(j=0,1,\dots ,|{\widehat{Z}}_k\left|\right)$ generates a discrete distribution above the indices j for every particle i:

$$p_i\left(j\right)=\frac{P_{ij}}{{\sum }_{l=0}^{|{\widehat{Z}}_k|}{P}_{il}}.$$

(39)

For every particle i an index j is chosen with probability $p_i\left(j\right)$ thus generating $1+|{\widehat{Z}}_k|$ clusters. Particle clusters are categorized according to the value of j as:

$$\begin{array}{cc}\mathrm{not}\mathrm{detected}:\hfill & j=0\hfill \\ \mathrm{detected}\mathrm{particle}:\hfill & 0<j<=|Z_k|\hfill \\ \mathrm{hidden}\mathrm{particle}:\hfill & |Z_k|<j<=|{\widehat{Z}}_k|\hfill \end{array}$$

The formed particle clusters are denoted by ${\left\{{𝒞}_k^{\left(j\right)},l_k^{\left(j\right)}\right\}}_{j=0}^{|{\widehat{Z}}_k|}$, where $l_k^{\left(j\right)}$ is the label associated with the cluster, which is the same as the label of the measurement the particles in the cluster were assigned to. The particle weights in the hidden and the not detected clusters are updated the same way:

$$w_{k|k}^{\left(i\right)}=(1-p_d\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right))w_{k|k-1}^{\left(i\right)}.$$

(40)

Particle weights in the detected clusters are computed using a bootstrap particle filter update:

$$w_{k|k}^{\left(i\right)}=\frac{p_d\left({\mathbf{x}}_{k|k-1}^{\left(i\right)}\right)g_k\left({\mathbf{z}}_k^{\left(j\right)}\right|{\mathbf{x}}_{k|k-1}^{\left(i\right)})w_{k|k-1}^{\left(i\right)}}{\kappa \left({\mathbf{z}}_j\right)+{\sum }_{l=1}^{N_{k|k-1}}{p}_d\left({\mathbf{x}}_{k|k-1}^{\left(l\right)}\right)g_k\left({\mathbf{z}}_j\right|{\mathbf{x}}_{k|k-1}^{\left(l\right)})w_{k|k-1}^{\left(l\right)}}.$$

(41)

For the not detected and hidden clusters the updated and predicted particles states are identical:

$${\mathbf{x}}_{k|k}^{\left(i\right)}={\mathbf{x}}_{k|k-1}^{\left(i\right)},$$

(42)

while for detected clusters, particles are resampled with probability proportional to their weights.

The sum of particle weights in a detected or hidden cluster is between 0 and 1 thus it is regarded as the existence probability of an object represented by the particles:

$$r_k^{\left(j\right)}=\sum _{i=1}^{|{𝒞}_k^{\left(j\right)}|}{w}_{k|k}^{\left(i\right)}.$$

(43)

If $r_k^{\left(j\right)}$ is above a given threshold, a point estimate is computed from the particle cluster, and an object is registered as detected or hidden, based on the index j. The detected object set is

$$O_k={\left\{{\mathbf{c}}_k^{\left(i\right)},C_k^{\left(i\right)},l^{\left(i\right)}\right\}}_{i=1}^{o_k},$$

(44)

where the point estimates are computed cluster-wise as a weighted sum of the corresponding particle states:

$${\mathbf{c}}_k^{\left(i\right)}=\sum _{j=1}^{|{𝒞}_k^{\left(\alpha \right)}|}{w}_{k|k}^{\left(j\right)}{\mathbf{x}}_{k|k}^{\left(j\right)}$$

(45)

for a cluster $|{\mathcal{C}}_k^{\left(\alpha \right)}|$, $\alpha \in 1,\dots ,|Z_k|$. The error covariance matrices are computed from the samples:

$$C_k^{\left(i\right)}=\sum _{j=1}^{|{𝒞}_k^{\left(\alpha \right)}|}{w}_{k|k}^{\left(i\right)}\left({\mathbf{c}}_k^{\left(i\right)}-{\mathbf{x}}_{k|k}^{\left(j\right)}\right){\left({\mathbf{c}}_k^{\left(i\right)}-{\mathbf{x}}_{k|k}^{\left(j\right)}\right)}^{\top }.$$

(46)

For hidden objects, the calculations are the same for indices $\alpha \in |Z_k|+1,\dots ,|{\widehat{Z}}_k|$ resulting in the set

$$O_{k,h}={\left\{{\mathbf{c}}_{k,h}^{\left(i\right)},C_{k,h}^{\left(i\right)},l^{\left(i\right)}\right\}}_{i=1}^{o_{k,h}}.$$

(47)

It should be noted that more sophisticated procedures can be carried out to extract point estimates from particle clouds. Differentiating newborn and already persisting particles and normalizing weight can improve estimation performance, as reported in [36,37].

Handling Multiple Measurements in One Timestep

Since the PHD filter uses the standard measurement model, it can only perform efficiently if an object cannot generate more than one measurement. In other words, multi-detection does not arise.

Due to the high refresh rate, radar can detect and report an object multiple times during the filtering timestep. To handle the multiple detection instead of using a non-standard measurement model that captures this peculiarity, we process the measurements online. We collect the measurements that arrived during the filter timestep and form unique clusters. We use the fact that vehicles cannot overlap in the position subspace of the state-space. The algorithm is summarized in Algorithm 1.

Algorithm 1 Measurement clusterization.

1: procedure CreateUniqueClusters(Z) 2:     From measurement set $Z={\left\{{\mathbf{z}}_i\right\}}_{i=1}^m$ create matrix $\mathcal{Z}=[{\mathbf{z}}_1,\dots ,{\mathbf{z}}_m]$ 3:     Sort the columns of $\mathcal{Z}$ in increasing order by the rows 1 and 2 3: Segment $\mathcal{Z}$ to sets $C_n$ as follows: 4:     $n=1$, $C_1=\left\{{\mathcal{Z}}_{*,1}\right\}$                              ▹* denotes all elements 5:     for $i\leftarrow 2$ to m do 6:         if $({\mathcal{Z}}_{1,i}-{\mathcal{Z}}_{1,i-1})<d_1\wedge ({\mathcal{Z}}_{2,i}-{\mathcal{Z}}_{2,i-1})<d_2$then▹ Given thresholds $d_1$ and $d_2$ 7:            $C_n=C_n\bigcup \left\{{\mathcal{Z}}_{*,i}\right\}$ 8:         else 9:            $n=n+1$ 10:            $C_n=\left\{{\mathcal{Z}}_{*,i}\right\}$ 11:         end if 12:     end for 12: Compute average state vector ${\mathbf{c}}_i$ from every set $C_i$: 13:     for $i\leftarrow 1$ to n do 14:         ${\mathbf{c}}_i=\frac{1}{|C_i|}{\displaystyle \sum _{\alpha \in C_i}}\mathit{\alpha }$ 15:     end for 16:     return $Z^{*}=\{{\mathbf{c}}_1,\dots ,{\mathbf{c}}_m\}$                             ▹ Clustered measurements 17: end procedure

4. Occlusion Model

The $p_d\left(\mathbf{x}\right)$ coefficient in the PHD filter equations indicates how likely the sensor detects an object. Its value depends on the state $\mathbf{x}$, which permits modeling of the occlusion due to other objects present at the scene. To create a model for visibility, we use the radar cross-section (RCS) values reported by the radar for every detected object. As the RCS takes the cross-sectional area of a spherical body with the same reflective capability as the detected object, we model every road participant as a spherical object described by the RCS. If no RCS values are available, a predefined value can be assigned that is appropriate for the considered application, e.g., the cross-sectional area of a typical road vehicle.

The coefficient $p_d\left(\mathbf{x}\right)$ represents a probability thus, we regard it as the integral of a PDF $\xi$ that accounts for the visibility. If the whole object lies in the sensor field of view (FOV), and no obstacles are blocking visibility, the PDF integrates to approximate unity. The occlusion model (Figure 1) should set the limits of the integral to compute $p_d\left(\mathbf{x}\right)$. As we consider a road traffic situation, only planar geometry will be used, hence the angular parametrization of the problem naturally arises.

The probability of detection depends on the state and time-varying, which is manifested through the time-dependent positions of the objects at the scene. $p_d$ is expressed as a function of the state and the predicted objects:

$$p_{d,k}\left(\mathbf{x},{\left\{O_{k|k-1,h}^{\left(i\right)}\right\}}_{i=1}^{o_{k-1}}\right)={\int }_{{\Omega }_k}\xi \left(\psi \right)\mathrm{d}\psi ,$$

(48)

where $\psi$ is the angular parameter. The domain of integration, ${\Omega }_k$ is defined by the sensor FOV and the predicted object set:

$$\Omega \left(\mathbf{x}\right)=\mathrm{FOV}-\bigcup _{i=1}^{o_{k-1}}{\Psi }_i,$$

(49)

where ${\Psi }_i$ is the angular interval covered by object i. If a location described by $\mathbf{x}$ is closer to the observer than object i then ${\Psi }_i$ is empty.

A suitable density function needs to be chosen with the following requirements to model an object’s visibility. It should be bell-shaped with parameters associated with the problem’s geometry and possibly with an analytic cumulative distribution function (CDF) on finite support. The raised cosine distribution with PDF

$$\xi (\psi ;m,s)=\left\{\begin{array}{cc}\frac{1}{2s}+\frac{1}{2s}cos\left(\frac{\psi -\mu }{s}\pi \right)\hfill & \mathrm{for}-\mu \le \psi \le \mu \hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(50)

is suitable for the problem. The function is centered at $\mu$ which is computed from the position coordinates as $m=atan2(y,x)$. The scale parameter s is the subtended angle by the diameter of the object which is approximated as the square root of the RCS:

$$s=2arctan\left(\frac{1}{2}\sqrt{\frac{RCS}{x^2+y^2}}\right).$$

(51)

The CDF of the raised cosine distribution has the form

$$\Xi (\psi ;m,s)=\frac{1}{2}+\frac{\psi -\mu }{s}+\frac{1}{2\pi }sin\left(\frac{\psi -\mu }{s}\pi \right)$$

(52)

thus the evaluation of the integral (48) is computationally cheap.

5. Simulation Results

Various tests were performed to evaluate the performance of the proposed filter. Two abstract scenarios, one with crossing and one with random trajectories, were used to present the filter’s general performance. In these scenarios, the occlusion model was not used, and predefined values were set for the probability of detection. Besides the abstract tests, two situations that model real traffic scenarios were designed. In these setups, the occlusion model was used to estimate the state-dependent probability of detection.

The discrete dynamic system model for a tracked object uses the nearly constant velocity motion model

$$\begin{array}{cc}\hfill {\mathbf{x}}_{k+1}& =F_k{\mathbf{x}}_k+G_k{\mathbf{w}}_k\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill {\mathbf{z}}_k& =H_k{\mathbf{x}}_k+{\mathbf{v}}_k,\hfill \end{array}$$

(54)

where the system matrix describes a constant velocity motion with sampling time $T_s$:

$$F={\mathbf{I}}_2\otimes \left[\begin{array}{cc}1\hfill & T_s\hfill \\ 0\hfill & 1\hfill \end{array}\right]$$

(55)

and ⊗ denotes the Kronecker product. The state vector has position and velocity components: $\mathbf{x}=[x,\dot{x},y,\dot{y}]$. In the following the measurement vector will have the same components as the state vector, thus the measurement matrix is identified: $H_k={\mathbf{I}}_4$. The process and measurement noises, ${\mathbf{w}}_k$ and ${\mathbf{v}}_k$ respectively, are zero mean additive white Gaussian terms with covariances:

$$\begin{array}{cc}\hfill \mathrm{Cov}\left[{\mathbf{w}}_k\right]& =q_w^2{\mathbf{I}}_2\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill \mathrm{Cov}\left[{\mathbf{v}}_k\right]& ={\mathbf{I}}_2\otimes \left[\begin{array}{cc}{\sigma }_1^2\hfill & 0\hfill \\ 0\hfill & {\sigma }_2^2\hfill \end{array}\right],\hfill \end{array}$$

(57)

where ${\sigma }_1$, ${\sigma }_2$ and $q_w$ are the noise intensities. The process noise acts through the matrix

$$G={\mathbf{I}}_2\otimes \left[\begin{array}{c}\frac{T_s^2}{2}\hfill \\ T_s\hfill \end{array}\right].$$

(58)

5.1. Measuring Filter Performance

To compute the multi-object estimation errors, the optimal subpattern assignment (OSPA) metric is used. The OSPA metric was introduced in [38] for the purpose of creating a multi-object estimation error metric. It considers, beside errors in state-space, cardinality differences between the ground truth and the estimated set. Later, the OSPA metric was extended to include labels thus it can measure track estimations errors [39]. The labeled OSPA metric has the form

$$d_{p,c,\alpha }(X,Y)={\left(\underset{\pi \in \Pi }{min}\frac{1}{n}\sum _{i=1}^m{d}_c{\left({\mathbf{x}}_i,{\mathbf{y}}_{\pi \left(i\right)}\right)}^p+\frac{(n-m)}{n}·c^p+\frac{{\alpha }^p}{n}\sum _{i=1}^m\left(1-{\delta }_{\ell \left({\mathbf{x}}_i\right),\ell \left({\mathbf{y}}_{\widehat{\pi }\left(i\right)}\right)}\right)\right)}^{1/p},$$

(59)

where $p\ge 1$ is the order of the metric, the ground truth set has $\left|X\right|=m$ elements and the estimated set cardinality is $\left|Y\right|=n$. $\Pi$ is the set of permutations of length m from elements $\{1,\dots ,n\}$. The parameter $c>0$ is the penalty for a cardinality error and serves as a cutoff value for the base distance:

$$d_c\left(\mathbf{x},\mathbf{y}\right)=min(c,d(\mathbf{x},\mathbf{y})).$$

(60)

The base distance typically, and in this work also, comes from the p-norm: $d(\mathbf{x},\mathbf{y})={\parallel \mathbf{x}-\mathbf{y}\parallel }_p$. The value $\alpha \in [0,c]$ is the labeling error parameter and $\delta$ is the Kronecker delta. The label associated with state $\mathbf{x}$ is denoted by $\ell \left(\mathbf{x}\right)$. The optimal permutation $\widehat{\pi }$ is given as

$$\widehat{\pi }=arg\underset{\pi }{min}\sum _{i=1}^n{d}_c{\left({\mathbf{x}}_i,{\mathbf{y}}_{\pi \left(i\right)}\right)}^p.$$

(61)

Based on the three terms in (59) the localization, cardinality and labeling errors are:

$$\begin{array}{cc}\hfill d_{p,c}^{\mathrm{loc}}& ={\left(\underset{\pi \in \Pi }{min}\frac{1}{n}\sum _{i=1}^m{d}_c{\left({\mathbf{x}}_i,{\mathbf{y}}_{\pi \left(i\right)}\right)}^p\right)}^{1/p}\hfill \end{array}$$

(62)

$$\begin{array}{cc}\hfill d_{p,c}^{\mathrm{card}}& =c{\left(\frac{n-m}{n}\right)}^{1/p}\hfill \end{array}$$

(63)

$$\begin{array}{cc}\hfill d_{p,\alpha }^{\mathrm{lab}}& ={\left(\frac{{\alpha }^p}{n}\sum _{i=1}^m\left(1-{\delta }_{\ell \left({\mathbf{x}}_i\right),\ell \left({\mathbf{y}}_{\widehat{\pi }\left(i\right)}\right)}\right)\right)}^{1/p}.\hfill \end{array}$$

(64)

If $m>n$ then X and Y in (59) should be swapped. For a detailed discussion see [33] (Chapter 6.2).

5.2. Abstract Simulations

In the abstract simulations several objects are moving in the scene (see Figure 2) described by the nearly constant velocity motion model (53). The process noise has intensity $q_w=1\mathrm{m}{\mathrm{s}}^{-1}$. The initial velocity components take values randomly from the interval between −20 and 20 $\mathrm{m}{\mathrm{s}}^{-1}$. The standard deviation of the measurement error is ${\sigma }_1=10\mathrm{m}$ for the position and ${\sigma }_2=1\mathrm{m}{\mathrm{s}}^{-1}$ for the velocity components.

Although occlusion is not considered, measurement outages are directly inserted into the simulations. For every object, there is a 10 s period when no measurements arrive. These periods are positioned randomly, except for two objects. Periods starting at 10 s, and 25 s were hard-coded for every simulation run. To get reliable performance measures, 200 Monte Carlo simulations were performed, and the error metrics were averaged. The proposed filter’s performance was compared to the PHD filter using the original clustering method without hidden particles. Example scenarios are presented in Figure 2. To evaluate filter performance, a second order OSPA metric was used with a cutoff distance of 100 m and a labeling error of 30 m.

5.2.1. Crossing Trajectories

In the crossing trajectories scenario, seven objects are present. The trajectories are designed in a way that they cross each other around the origin at the same time. Otherwise, the starting and ending positions are random. This setup is challenging regarding object labeling. Figure 3 shows the estimated object positions in an example scenario for the proposed and the basic filter.

Figure 4 shows the performance of the basic PHD filter in a single run. The OSPA error is dominated by the cardinality component. The number of tracked objects is exactly following the measurement set cardinality. In comparison, the performance of the proposed filter is shown in Figure 5. The localization component in the OSPA error is relatively higher, which is due to the fact that the filter gives estimates about hidden objects, as can be seen around the 20 s. The reduced cardinality and labeling error makes the overall OSPA error less.

The results from the averaged MC runs are shown in Figure 6. The hard-coded outages are observable for the basic filter as two trapezoidal bumps in the cardinality and OSPA error graphs. For the proposed filter, the OSPA error values in the same two regions are less but increase with time. This is due to the increasing localization error while tracking hidden objects and the growing cardinality error, which is the result of the filter dropping tracks as their existence probability falls under the reporting threshold. The labeling error is minimal for the proposed filter, and for the original filter, its maximum value is around 40 s, which is when all trajectories cross each other. A spike that can be seen at the 20th s in the localization error graph for the original filter can be explained by frequent mislabeling of objects, as indicated by a spike in the labeling error graph at the same place.

5.2.2. Random Trajectories

In the random trajectories scenario, the objects are moving according to the nearly constant velocity model (53) with random starting positions. Trajectories may or may not cross each other. The measurement outages are designed the same way as described previously. Figure 7 shows the estimated object positions in an example scenario for the proposed and the original filter.

Figure 8 shows the performance of the original filter for the example scenario. The cardinality component dominates the OSPA error again. The labeling error is smaller because there is less crossing in this setup. In Figure 9 the performance of the proposed filter can be seen. The main components of the OSPA error are the cardinality and localization errors. After the 40th s the localization error has a spike due to subsequent re-detection of two objects.

The overall filter performances are compared in Figure 10. The advantage of the proposed filter regarding the OSPA error is similar to in the crossing trajectories scenario. Regarding labeling error, the original filter performs reasonably; however, the predefined measurement outages are recognizable. For the proposed filter, labeling errors are minimal.

5.3. Road Traffic Simulations

Two scenarios were designed to evaluate filter performance in simple simulated traffic situations (Figure 11). Rectangles represent the vehicles, and the color indicates whether one is observed (black) or in an occluding object (red). The ego vehicle, drawn as blue, is the observer. Lines represent the trajectories of other vehicles relative to the ego vehicle. The first scenario consists of a two-lane road and three vehicles (Figure 11a). The vehicles in the right lane are moving at the same constant speed. The vehicle in the left lane moves with $5\mathrm{m}{\mathrm{s}}^{-1}$ speed difference and performs two-lane changing maneuvers, and while moving in the right lane, it occludes the observed vehicle. The simulation’s duration is 100 s, and the observed object is occluded in the time interval [35 s, 64 s].

The second scenario (Figure 11b) consists of a four-lane road segment and four vehicles. The ego vehicles and the two occluding vehicles are moving at the same speed. Two observed vehicles are moving with $5\mathrm{m}{\mathrm{s}}^{-1}$ relative speed and perform multiple lane-changing maneuvers. The vehicle starting from the right lane is occluded in the time interval of [30 s, 43 s] and [68 s, 80 s]. The other observed vehicle is occluded in the time interval [78 s, 83 s]. To evaluate filter performance, a second order OSPA metric was used with a cutoff distance of 30 m and labeling error of 10 m; however, no labeling error occurred.

The original filter’s performance in the simulated traffic situations can be seen in Figure 12. As expected, during occlusion, the OSPA error is dominated by the cardinality component, and after re-detection, the localization error component is significant. The performance of the proposed filter is shown in Figure 13. In this case, no cardinality error occurs, and the localization error increases gradually as the estimation becomes more uncertain during occlusion. The actual scaling of the cardinality error depends on the cutoff distance, hence the information gain manifested through the correct cardinality estimates should be tuned via this parameter.

6. Highway Measurements

The effectiveness of the proposed filter is validated using highway radar measurements acquired in traffic. The road segment used is the Hungarian M1 motorway. The vehicle is equipped with a Mobileye 630 camera and an automotive radar (Continental ARS408-21). The camera is used to give lane information and to provide approximate ground truth for visual checking. The filter is only supplied with the radar measurements. The radar communicates via CAN bus and provides information about the detected objects in the environment. Every object is assigned an ID, an integer number which remains constant while the object lives continuously. When an object disappears and then reappears, the previous ID may or may not be assigned again. It depends on whether the radar identifies that object the same as seen previously or considers it a new one. The old ID may be assigned to another object that appeared or moved near the location where one disappeared. This radar behavior is directly addressed in the proposed filter; thus, we can get continuous object trajectories. The measurements are logged using a Vector CAN device and processed offline in Matlab. The filter is equipped with the lateral and longitudinal object position and velocity coordinates, the IDs, and RCS values. The filtering timestep is 0.1 s.

Due to the high refresh rate, the radar detects and reports an object multiple times during the filtering timestep. Instead of using a non-standard measurement model that captures this peculiarity, we process the measurements online to handle the numerous detections. We collect the measurements that arrived during the filter timestep and form unique clusters. We use the fact that vehicles cannot overlap in the position subspace of the state-space. The algorithm is summarized in Algorithm 1.

The traffic scenario is the following. At the beginning the ego vehicle is in the left lane of the three-lane motorway. There are four other observable vehicles (see Figure 14 and Figure 15a). The one in the middle lane, about 20 m ahead (bottom trajectory in the longitudinal plots, with ID 12) remains in that lane and moves with constant speed during the measurement. The ego vehicle performs a double lane-changing maneuver and arrives to right lane while the vehicle in the middle occludes other traffic participants (Figure 15b). The vehicle in the right lane, about 55 m ahead at the beginning, labeled with ID 8, also performs a double lane-changing maneuver. The vehicle in the left lane (ID 6) moves with constant speed and remains in that lane.

Figure 15b belongs to timestep 60 where there are two radar measurements and six estimated objects. Please note that two objects are out of the plot range: the red trajectory at 150 m longitudinal position and the black trajectory at 20 m lateral position in Figure 14.

Figure 14 shows the unprocessed (top figures), and the estimated (bottom figures) object positions in a sequence. In several cases, the filter gives continuous trajectories for temporarily occluded objects. In these cases, a jump can be observed when the object reappears because of correcting the state estimates based purely on the motion model. This can be observed for the blue trajectory at timestep 60. The filter drops the ceased trajectories with a time delay, which is a natural consequence of the proposed method: memory is inserted into the PHD filter that manifests as if inertia is associated with the hypotheses of object tracks. Track initialization is, however, not delayed by this inertial effect.

Considering the maneuvers of the ego vehicle and inserting its motion into the dynamic model would help the filter to get better state estimates during outages as trajectory shifts would be anticipated.

7. Conclusions

The presented work has three contributions that aim to address the limitations of the classical PHD filter. Using the standard measurement model, we proposed a method to handle multiple detections of the same object in a single time frame. By adopting a model to estimate the state-dependent detection probability, we could reduce the effect of missed detection on the cardinality estimate. Regarding track management and reporting of detected objects, the proposed method introduces inertia, which the classical PHD filter lacks, to gain additional cardinality estimate stability. The presented algorithm’s performance is evaluated in synthetic tests using simulations and logged sensor data originating from real-world traffic situations. Besides the state-dependent probability of detection, the filter could benefit from the introduction of state-dependent survival probabilities that would allow the modeling of objects exiting the scene or discard objects in irrelevant regions. Modeling the ego vehicle motion is another possible extension as it would make tracking more effective during a lane-changing maneuver.