Trajectory PHD and CPHD filters

Trajectory PHD and CPHD filters

Ángel F. García-Fernández, Lennart Svensson A. F. García-Fernández is with the Department of Electrical Engineering and Electronics, University of Liverpool, Liverpool L69 3GJ, United Kingdom (email: angel.garcia-fernandez@liverpool.ac.uk). L. Svensson is with the Department of Electrical Engineering, Chalmers University of Technology, SE-412 96 Gothenburg, Sweden (email: lennart.svensson@chalmers.se).
Abstract

This paper presents the probability hypothesis density filter (PHD) and the cardinality PHD (CPHD) filter for sets of trajectories, which are referred to as the trajectory PHD (TPHD) and trajectory CPHD (TCPHD) filters. Contrary to the PHD/CPHD filters, the TPHD/TCPHD filters are able to produce trajectory estimates from first principles. The TPHD filter is derived by recursively obtaining the best Poisson multitrajectory density approximation to the posterior density over the alive trajectories by minimising the Kullback-Leibler divergence. The TCPHD is derived in the same way but propagating an independent identically distributed (IID) cluster multitrajectory density approximation. We also propose the Gaussian mixture implementations of the TPHD and TCPHD recursions, the Gaussian mixture TPHD (GMTPHD) and the Gaussian mixture TCPHD (GMTCPHD), and the -scan computationally efficient implementations, which only update the density of the trajectory states of the last time steps.

Multitarget tracking, random finite sets, sets of trajectories, PHD, CPHD.

I Introduction

The probability hypothesis density (PHD) and cardinality PHD (CPHD) filters are widely used random finite set (RFS) algorithms for multitarget filtering, which aims to estimate the state of the targets at the current time based on a sequence of measurements [1, 2, 3, 4, 5, 6, 7]. These filters have been successfully used in different applications such as multitarget tracking [1], distributed multi-sensor fusion [8, 9], robotics [10, 11], computer vision [12, 13], road mapping [14] and sensor control [15].

The PHD/CPHD filters fit into the assumed density filtering framework and propagate a certain type of multitarget density on the current set of targets through the prediction and update steps [16]. The PHD filter considers a Poisson multitarget density, in which the cardinality of the set is Poisson distributed and, for each cardinality, its elements are independent and identically distributed (IID). On the other hand, the CPHD filter considers an IID cluster multitarget density, in which the cardinality distribution of the set is arbitrary and, for each cardinality, its elements are IID. If the output of either the prediction or the update step is no longer Poisson/IID cluster, the PHD/CPHD filters obtain the best Poisson/IID cluster approximation by minimising the Kullback-Leibler divergence (KLD).

The most important benefit of the PHD/CPHD filters is their low computational burden, as they avoid the measurement-to-target association problem. However, their main drawbacks are their relatively low performance in some scenarios [1, 17] and the fact that they do not build tracks, which denote sequences of target states that belong to the same target. The smoother versions of these filters [1, 18, 19] do not solve these drawbacks.

Despite the fact that the PHD/CPHD filters are unable to provide tracks in a mathematically rigorous way, several track building procedures have been proposed [20, 21, 22, 23, 24]. A track building procedure for PHD/CPHD filters was proposed in [25] by adding labels [26, 27] to the target states. Nonetheless, in the resulting labelled Poisson and labelled IID cluster densities, there is total confusion in the label-to-target association so they are not useful for track formation [25, Sec. III.B][28, Sec. II.B]. To solve this issue in [25], apart from the unique labels, unique tags are added to the PHD components, as in [22], and the original PHD/CPHD recursions are applied. However, in the considered posterior density, the tags are not part of the target state and are marginalised out. Therefore, the posterior is still distributed as labelled Poisson or labelled IID cluster and, theoretically, it does not have information to infer tracks. While tagging PHD components works well in some scenarios, each PHD component does not generally represent information about a unique target, as the corresponding number of targets is Poisson distributed. In fact, adding tags to the PHD components and reporting estimates with unique tags to build trajectories, can lead to track switches, missed detections and false targets when there is more than one target represented by the same tag.

In this paper, we address the intrinsic inability of standard PHD/CPHD filters to infer trajectories by developing PHD/CPHD filters that provide tracks from first principles, without adding labels or tags. We propose the trajectory PHD (TPHD) and trajectory CPHD (TCPHD) filters, which follow the same assumed density filtering scheme as the PHD/CPHD filters [29] with a fundamental difference: instead of using a set of targets as the state variable, they use a set of trajectories [30, 28].

The TPHD filter propagates a Poisson multitrajectory density on the space of sets of trajectories through the prediction and update steps, with a KLD minimisation after the update step. A diagram of the resulting Bayesian recursion is given in Figure 1. Similarly, the TCPHD filter propagates an IID cluster multitrajectory density and performs a KLD minimisation after the prediction and update steps, see Figure 2. Due to the widespread use of PHD/CPHD filters, this paper covers an important gap in the literature, as we show how PHD/CPHD filtering can be endowed with the ability to infer trajectories in a rigorous and principled way. In particular, the TPHD and TCPHD filters are able to estimate the trajectories of the alive targets by propagating a Poisson and an IID cluster multitrajectory density through the filtering recursion using KLD minimisations. The TPHD and TCPHD filters also avoid the above-mentioned drawbacks of trajectory estimation based on labelling/tagging the PHD. Apart from theoretically sound track formation, the proposed filters also have the advantage, compared to previous track building procedures used in PHD/CPHD filters, that they can update the information regarding past states of the trajectories.

Figure 1: TPHD filter diagram for estimating the present trajectories at the current time. The TPHD filter assumes that the multitrajectory densities involved are Poisson (on the space of sets of trajectories). The output of Bayes’ rule is no longer Poisson but, in order to be able to perform the Bayesian recursion, it obtains the best Poisson approximation to the filtering density by minimising the KLD.
Figure 2: TCPHD filter diagram for estimating the present trajectories at the current time. The TCPHD filter assumes that the multitrajectory densities involved are IID cluster. The output of the prediction and Bayes’ rule are no longer IID clusters but the TCPHD filter obtains the best IID cluster approximation by minimising the KLD.

In this paper, we also propose Gaussian mixture implementations of the TPHD/TCPHD filters, which follow the spirit of the Gaussian mixture PHD/CPHD filters [3, 5]. The resulting Gaussian mixture TPHD (GMTPHD) and TCPHD (GMTCPHD) filters build trajectories under a Poisson or IID cluster approximation, whose PHD is represented by a Gaussian mixture. In this setting, a Gaussian component of the GMTPHD/GMTCPHD filter represents information over entire trajectories, while a Gaussian component in the Gaussian mixture PHD/CPHD filters (tagged or not) represents information over current target states. It is therefore straightforward to extract trajectory estimates from the GMTPHD/GMTCPHD filters. Additionally, we propose a version of the GMTPHD/GMTCPHD filters with lower computational burden called the -scan GMTPHD/GMTCPHD filters. In practice, these filters only update the multitrajectory density of the trajectory states of the last time instant leaving the rest unaltered, which is quite efficient for implementation. The theoretical foundation of the -scan GMTPHD filter is also based on the assumed density filtering framework and KLD minimisations. Preliminary results of this paper covering the TPHD filter were presented in [29].

The remainder of the paper is organised as follows. Section II presents background material on sets of trajectories. In Section III, we introduce the Poisson and IID cluster multitrajectory densities and some of their properties. The TPHD and TCPHD filters are derived in Sections IV and V, respectively. Their Gaussian mixture implementations are provided in Section VI. Simulation results are provided in Section VII. Finally, conclusions are drawn in Section VIII.

Ii Background

In this section, we describe some background material on multiple target tracking using sets of trajectories [28]. We review the considered variables, the set integral and cardinality distribution for sets of trajectories in Sections II-A, II-B and II-C, respectively. Finally, we introduce the PHD for sets of trajectories in Section II-D.

Ii-a Variables

A single target state contains information of interest about the target, e.g., its position and velocity. A set of single target states belongs to where denotes the set of all finite subsets of . We are interested in estimating all target trajectories, where a trajectory consists of a sequence of target states that can start at any time step and end any time later on. Mathematically, a trajectory is represented as a variable where is the initial time step of the trajectory, is its length and denotes a sequence of length that contains the target states at consecutive time steps of the trajectory.

We consider trajectories up to the current time step . As a trajectory exists from time step to , variable belongs to the set . A single trajectory up to time step therefore belongs to the space , where stands for disjoint union, which is used to highlight that the sets are disjoint. Similarly to the set of targets, we denote a set of trajectories up to time step as .

Given a trajectory , the set , which can be empty, denotes the corresponding target state at a time step . Given a set of trajectories, the set of target states at time is .

Ii-B Set integral

Given a real-valued function on the single trajectory space , its integral is [28]

(1)

This integral goes through all possible start times, lengths and target states of the trajectory. Given a real-valued function on the space of sets of trajectories, its set integral is [28]

(2)

where . A function is a multitrajectory density if and its set integral is one.

Ii-C Cardinality distribution

Given a multitrajectory density , its cardinality distribution is

(3)

which is analogous to the case where there is a set of targets.

Ii-D Probability hypothesis density

The PHD [1] of a multitrajectory density is

(4)

As in the PHD for RFS of targets, integrating the PHD in a region gives us the expected number of trajectories in this region [1, Eq. (4.76)]:

(5)

where is the indicator function of a subset : if and otherwise. Therefore, the expected number of trajectories up to time step is given by substituting into (5).

Example 1.

Let us consider a multitrajectory density with PHD

(6)
(7)

and for and , where is a Gaussian density with mean and covariance matrix . The expected number of trajectories that start at time one with length 1 is given by substituting into (5) so

The expected number of trajectories up to time step is .

Iii Poisson and IID cluster trajectory RFSs

In this section, we explain the Poisson and IID cluster trajectory RFSs.

Iii-a Multitrajectory densities

Iii-A1 Poisson RFS

For a Poisson RFS, the cardinality of the set is Poisson distributed and, for each cardinality, its elements are IID. A Poisson multitrajectory density has the form

(8)

where is a single trajectory density, which implies

(9)

and . A Poisson multitrajectory density is characterised by either its PHD or by and [1]. As a result, using (5), the expected number of trajectories is . Further, its cardinality distribution is given by

(10)
Example 2.

We consider a Poisson RFS with the PHD of Example 1. Using (10), its cardinality distribution is Poisson with and, therefore, its single trajectory density is .

Iii-A2 IID cluster RFS

For an IID cluster RFS with multitrajectory density , the cardinality is distributed according to the probability mass function and, for each cardinality, its elements are IID according to a single trajectory density . The resulting multitrajectory density is

(11)

As is a single trajectory density, it meets (9). The PHD of (11) is given by [1]

(12)

where the second factor corresponds to the expected number of trajectories. An IID cluster density can be characterised either by and , or by and . How to draw samples from an IID cluster trajectory RFS, which includes the Poisson trajectory RFS as a particular case, is explained in Appendix A in the supplementary material.

Iii-B KLD minimisation

In this subsection, we provide two KLD minimisation theorems for Poisson and IID cluster multitrajectory densities, which will be used to derive the trajectory PHD/CPHD filters.

The KLD between multitrajectory densities and is given by [1]

(13)

Then, the following theorems hold:

Theorem 3.

Given a multitrajectory density , the Poisson multitrajectory density that minimises the KLD is characterised by the PHD .

Theorem 4.

Given a multitrajectory density , the IID cluster multitrajectory density that minimises the KLD is characterised by the PHD and the cardinality distribution .

Theorem 3 is proved in Appendix A in [29]. The analogous theorem for sets of targets was proved in [31]. Theorem 4 is proved in Appendix B in the supplementary material. The analogous theorem for sets of targets was proved in [32, 16]. It should be noted that, as a Poisson RFS is a special type of IID cluster RFS, the best fitting IID cluster RFS always has a lower or equal KLD than the best fitting Poisson RFS.

Iii-C Inference only on alive trajectories

In this section, we explain why the TPHD and TCPHD filters are mainly useful to approximate the posterior multitrajectory density over alive trajectories, but not the posterior over all trajectories, which also include dead trajectories. This serves as a motivation to present the TPHD and TCPHD filters for tracking only the alive trajectories in the next sections.

Let us first explain why the TPHD filter, which considers a Poisson approximation, is only useful for the alive trajectories [29, Sec. V.B], though it was derived in [29] for dead and alive trajectories. In the prediction step, the part of the PHD that represents a trajectory that dies at the current time step is multiplied by the probability of death (one minus the probability of survival) [29, Thm. 5], which is usually low. As time goes on, the part of the PHD that represents dead trajectories never changes. As a result, even if a trajectory exists with a very high probability at some point in time, once it dies, the TPHD filter over all trajectories indicates that it existed with a very low probability. Therefore, the TPHD does not contain accurate information about dead trajectories, though it does contain useful information about alive trajectories.

In the following, we argue with an example why the TCPHD filter, which considers an IID cluster approximation, should only consider alive trajectories, as the TPHD filter.

Example 5.

Let us consider that the posterior over the set of trajectories at time has trajectories with probability 1 so . In addition, indicates that there are dead trajectories with independent (single trajectory) densities and alive trajectories with independent densities , where . Note that we can obtain this kind of true posterior, without TPHD/TCPHD approximations, if the probability of detection is one, there is no clutter, targets are born independently and they are far from each other at all time steps. In Appendix C (see supplementary material), we compute the best IID cluster density approximation to using Theorem 4 and show that the cardinality distribution of the alive targets in is

(14)

where . As the filtering recursion continues, the total number of trajectories can only increase. On the contrary, does not necessarily increase so after a sufficiently long time may become very small. Then, using the Poisson limit theorem, the cardinality distribution of the alive targets can be approximated as Poisson with parameter [33]. Therefore, even in this simple example in which the cardinality of the alive targets is known, the best IID cluster approximation of the whole trajectory posterior approximates the cardinality of the alive targets as a Poisson distribution.

The conclusion of the previous example is that, in the long run, an IID cluster RFS is not necessarily better than a Poisson RFS, both considered over all trajectories, to approximate the cardinality distribution of the alive targets. In most applications, the cardinality of the alive trajectories is considerably more important than the cardinality of the total number of trajectories. In this paper, we therefore focus on an IID cluster approximation of the alive trajectories to develop the TCPHD filter. This implies that the TCPHD filter has an arbitrary cardinality distribution for the alive targets, as the CPHD filter.

Iv Trajectory PHD filter

In this section, we derive the TPHD filter for tracking the alive targets. The TPHD propagates the multitrajectory density of a Poisson RFS through the filtering recursion. In the update step, the TPHD filter uses Bayes’ rule followed by a KLD minimisation to approximate the posterior as Poisson, see Figure 1. In Section IV-A, we present the Bayesian filtering recursion for sets of trajectories. The prediction and update steps of the TPHD filter are given in Sections IV-B and IV-C, respectively.

Iv-a Bayesian filtering recursion

The posterior multitrajectory density at time , which denotes the density of set of trajectories present at time given all measurements up to time , is calculated via the prediction and update steps:

(15)
(16)

where is the transition density, is the predicted density at time , is the set of measurements at time , is the density of the measurements given the current RFS of targets and

is the density of the measurements given the predicted density . The predicted density at time is the density of the set of trajectories present at time step given the measurements up to time step . As we only take into account the present trajectories, the only term that changes in (15)-(16) with respect to considering all trajectories is , see [34, Sec. IV.A] for a detailed explanation. The description of these models will be given in Sections IV-B and IV-C.

Iv-B Prediction

We make the following assumptions in the prediction step:

  • P1 Given the current set of targets, each target survives with probability and moves to a new state with a transition density , or dies with probability .

  • P2 The multitarget state at the next time step is the union of the surviving targets and new targets, which are born independently with a Poisson multitarget density .

  • P3 The multitrajectory density of the trajectories present at time represents a Poisson RFS.

Note that we use subindex in densities on RFS of targets, as in . Let . Then, the relation between predicted PHD at time and the PHD of the posterior at time is given by the following theorem.

Theorem 6 (TPHD filter prediction).

Under Assumptions P1-P3, the predicted PHD of the trajectories present at time is

(17)

where

if or zero otherwise.

This theorem is proved in [29] for a more general case in which dead trajectories are considered. As mentioned in Section III-C, in this paper, we only present the results for alive trajectories, as the results are mainly useful in this case. The predicted PHD is the sum of the PHD of the trajectories born at time step and the PHD of the surviving trajectories. The end time of trajectory is so is zero if . For the surviving trajectories, we multiply the PHD by the transition density and the survival probability. Note that the provided PHD characterises the Poisson RFS that represents the predicted density.

Iv-C Update

We make the following assumptions in the update step:

  • U1 For a given multi-target state at time , each target state is either detected with probability and generates one measurement with density , or missed with probability .

  • U2 The measurement is the union of the target-generated measurements and Poisson clutter with density .

  • U3 The multitrajectory density represents a Poisson RFS.

Let denote the set that contains all the vectors that indicate associations of measurements to targets, which can be either detected or undetected. If , indicates that measurement is associated with target and indicates that target has not been detected. Under Assumptions U1 and U2, which define the standard measurement model, the density of the measurement given the state is [1, Eq. (7.21)]

(18)

where and characterise , see (8).

Let denote the PHD filter pseudolikelihood function, which is given by [1, Sec. 8.4.3]

with representing the PHD of the targets at time of density , which is given by [29]

(19)

Then, the TPHD filter update step is given by the following theorem:

Theorem 7 (TPHD filter update).

Under Assumptions U1-U3, the updated PHD at time is

if or zero, otherwise.

This theorem is proved in [29] for a more general case in which dead trajectories are included. It should be noted that Bayes’ update (16) uses a likelihood (18) that involves a summation over all target-to-measurement associations in the multitarget space. In contrast, the TPHD filter update is similar to the PHD filter update in the sense that it uses a pseudolikelihood function which is defined on the single target space and only involves associations between a single target and the measurements.

V Trajectory CPHD filter

In this section we present the trajectory CPHD (TCPHD) filter for tracking the alive targets. The TCPHD propagates the multitrajectory density of an IID cluster RFS through the filtering recursion. In the prediction and update steps, the TCPHD filter makes use of a KLD minimisation to obtain an IID cluster approximation, see Figure 2.

Prior to deriving the TCPHD filter, we provide some notation. Given two sequences and , , we denote

Given a set , the elementary symmetric function of order is [5]

(20)

with by convention. We also use to denote set subtraction.

V-a Prediction

The TCPHD filter prediction is obtained under Assumptions P1 and the additional assumptions

  • P4 The multitarget state at the next time step is the union of the surviving targets and new targets, which are born independently with an IID cluster multitarget density .

  • P5 The multitrajectory density represents an IID cluster RFS.

Under Assumption P4, the set of new born trajectories at time has cardinality and PHD

The TCPHD filter prediction consists of applying the usual prediction step plus a KLD minimisation, which is performed by calculating the cardinality distribution and PHD of the predicted density, see Figure 2. The result is given in the following theorem.

Theorem 8 (TCPHD filter prediction).

Under Assumptions P1, P4 and P5, the PHD of the predicted density is the same as in the PHD filter, see Theorem 6. The cardinality distribution of the predicted density is

(21)

where is the PHD of the targets at time according to , which is calculated as in (19).

Theorem 8 is proved in Appendix E (see supplementary material). The TCPHD filter prediction updates the cardinality distribution as the CPHD filter. The TCPHD does not integrate out past states of the trajectories in the PHD to keep trajectory information, while the CPHD filter does.

V-B Update

The TCPHD filter update is derived under Assumption U1 and the additional assumptions

  • U4 The measurement is the union of the target-generated measurements and IID cluster clutter with density .

  • U5 The multitrajectory density represents an IID cluster RFS.

As indicated in Figure 2, the TCPHD update consists of applying Bayes’ rule, see (16), followed by a KLD minimisation, which is performed as indicated by Theorem 4. We first consider the distribution of the present targets at the current time. Under Assumption U5, it is direct to obtain that the distribution of the targets present at time is also an IID cluster with cardinality distribution and PHD (19). The resulting TCPHD filter update is given in the following theorem.

Theorem 9 (TCPHD filter update).

Under Assumptions U1, U4 and U5, the cardinality distribution and the PHD of the posterior at time are

(22)
(23)

if or , otherwise, and

(24)

Theorem 9 is proved in Appendix D (see supplementary material). The update step of the TCPHD filter is equivalent to the CPHD filter, with the main difference that the updated PHD contains information about previous states of the trajectories.

Vi Gaussian mixture implementations

In this section, we present the Gaussian mixture implementations of the TPHD and TCPHD filters. We use the notation

(25)

where . Equation (25) represents a single trajectory Gaussian density with start time , duration , mean and covariance matrix evaluated at . We use to indicate the Kronecker product and is the zero matrix.

We make the additional assumptions

  • A1 The probabilities and are constants.

  • A2 .

  • A3 .

  • A4 The PHD of the birth density is

    (26)

    where is the number of components, is the weight of the th component, its mean and its covariance matrix.

It should be noted that is the single-target transition matrix, is the covariance matrix of the single-target process noise, is the single-measurement matrix and is the covariance matrix of the single-measurement noise. In addition, the models provided by A1-A4 could be time varying but time is omitted for notational convenience. In the rest of this section, we present the Gaussian mixture implementations of the TPHD and TCPHD filters in Sections VI-A and VI-B, respectively. The -scan versions of the filters and trajectory estimation are addressed in Sections VI-C and VI-D. Finally, a discussion is provided in Section VI-E.

Vi-a Gaussian mixture TPHD filter

Under Assumptions A1-A4, P1-P3 and U1-U3, we can calculate the TPHD filter in closed form giving rise to the GMTPHD filter, whose prediction and update steps are provided in the following propositions.

Proposition 10 (GMTPHD filter prediction).

Assume has a PHD

where with . Then, the PHD of is

(27)

where

Proposition 10 is a consequence of Theorem 6 and conventional properties of Gaussian densities. Compared to the GMPHD filter prediction, the main difference is that previous states are not integrated out.

Proposition 11 (GMTPHD filter update).

Assume has a PHD

(28)

Then, the PHD of is

(29)

where

and

Proposition 11 is a consequence of Theorem 7 and the Kalman filter update equations [35]. Also, the GMTPHD filter update is similar to the GMPHD filter update. The main differences is that the GMTPHD updates the whole trajectories.

Vi-B Gaussian mixture TCPHD filter

The GMTCPHD filtering recursion requires Assumptions A1-A4, P1, P4, P5, U1, U4 and U5, and is given by the following propositions.

Proposition 12 (GMTCPHD filter prediction).

Assume the posterior has a cardinality distribution and a PHD

(30)

Then, the TCPHD filter prediction yields

where and are given in Proposition 10.

Proposition 12 is a consequence of Theorem 8. The main difference with the GMCPHD filter is that the GMTCPHD filter does not integrate out previous states in the PHD. It should also be noted that the GMTPHD and the GMTCPHD propagate the PHD in the same way. The difference is that the GMTCPHD also considers the cardinality of .

Proposition 13 (GMTCPHD filter update).

Assume the prior has a cardinality distribution and a PHD

(31)

Then, the TCPHD filter update yields

where

and , and are given in Proposition 11.

Proposition 13 is a consequence of Theorem 9 and the Kalman filter update [35]. The GMTCPHD filter update is analogous to the GMCPHD filter [5], with the difference that previous states of the target trajectories are also included.

Vi-C -scan implementations

In this section, we propose the use of pruning and absorption to limit the number of components in the Gaussian mixture and a computationally efficient implementation of the Gaussian mixture filters: the -scan GMTPHD and GMTCPHD filters.

The PHD of the GMTPHD/GMTCPHD filters has an increasing number of components as time progresses and, to limit complexity, we need to bound the number of components. We use the following techniques: pruning with threshold , setting a maximum number of components and absorption [29]. Absorption consists of removing components of the PHD whose distribution of the current target state is close to the distribution of the current target state of another component with a higher weight, and adding the weights of the removed components to the weight of the component that has not been removed. Absorption is motivated by the fact that if two components have a very similar distribution over the current target state, based on a Mahalanobis distance criterion, future measurements will affect both component weights and future states in a similar way. Therefore, without absorption, we would have two components with practically the same Gaussian densities for the trajectory states corresponding to recent time steps, for which we would be repeating the same calculations. It should also be noted that single trajectory densities can be quite different in the past even if they are similar for the current target state. Therefore, a direct use of merging [36] for single trajectory densities, which would use moment matching at all time steps, can provide poor results and absorption is preferred. The steps of the pruning and absorption algorithms for the GMTPHD/GMTCPHD filters are given in Algorithm 1, where we use the notation .

Input: Posterior PHD parameters , pruning threshold , absorption threshold , maximum number of terms .

Output: Pruned posterior PHD parameters

- Set and .
while  do
     - Set .
     - .
     -