Scalable Detection and Tracking
of Geometric Extended Objects

An important aspect of EOT is the modeling and inference of object shapes. The shape of an object determines how the measurements that originate from the object are spatially distributed around its center. An important and widely used model [6, 7, 8, 5, 9] based on random matrix theory has been introduced in [10]. This model considers elliptically shaped objects. Here, unknown reflection points of measurements on extended objects are modeled by a random matrix that acts as the covariance matrix in the measurement model. This random extent state is estimated sequentially together with the kinematic state. The approach proposed in [10] introduces a closed-form update step for linear dynamics. It exploits that the Gaussian inverse Wishart distribution is a conjugate prior for linear Gaussian measurement models.

A major drawback of the original random matrix-based tracking method is that the driving noise variance in the state transition function of the kinematic state must be proportional to the extent of the object [10]. Furthermore, the orientation of the random matrix is constant. These limitations have been addressed by improved and refined random matrix-based extent models introduced in [11, 12, 13] and particle-based methods [14]. A significant number of additional application-dependent object extent models have been proposed recently (see [1] and references therein). The state-of-the-art algorithm for tracking an unknown number of objects with elliptical shape is the Poisson multi-Bernoulli mixture (PMBM) filter [5]. The PMBM filter is based on a model [15] where objects that have produced a measurement are described by a multi-Bernoulli probability distribution and objects that exist but have not yet produced a measurement yet are described by a Poisson distribution. This PMBM model is a conjugate prior with respect to the prediction and update steps of the extended object tracking problem [5].

Important methods for the tracking of point objects include joint probabilistic data association (JPDA) [2], multi-hypothesis tracking (MHT) [16, 17, 18], and approaches based on random finite sets (RFS) [3, 19, 20, 21, 15]. Many of these point object tracking methods have been adapted for extended object tracking. In [22], a probabilistic data association algorithm for the tracking of a single object that can produce more than one measurement has been introduced. A multiple-measurement JPDA filter for the tracking of multiple objects has been independently developed in [23] and [24]. The method in [6] combines a random matrix extent model with multisensor JPDA for the tracking of multiple extended objects. The computational complexity of these methods scales combinatorially in the number of objects and the number of measurements. They rely on gating, a suboptimal preprocessing technique that excludes unlikely data association events. Thus, these methods are only suitable in scenarios where objects produce few measurements and no more than four objects come into close proximity at any time [25].

Another widely used approach to limit computational complexity for data association with extended objects is to perform clustering of spatially close measurements. Here, based on the assumption that all measurements in a cluster correspond to the same object, the majority of possible data association events is discarded in a suboptimal preprocessing step. Based on this approach, tracking methods for objects that produce multiple measurements have been developed in the MHT [26] and the JPDA [27, 28] frameworks. Practical implementations of the update step of RFS-based methods for EOT [7, 8, 5, 9] including the one of the PMBM filter [5] also rely on gating and clustering and suffer from a reduced performance if objects are in close proximity. An alternative approach is to perform data association with extended objects using sampling methods [29, 30]. Here, in scenarios with objects that are in proximity, a better estimation performance can be achieved compared to methods based on clustering. These methods based on sampling, however, also discard the majority of possible data association events, and thus eventually fail if the number of relevant events becomes too large [31].

An innovative approach to high-dimensional estimation problems is the framework probabilistic graphical models [32]. In particular, the SPA [33, 34, 32] can provide scalable solutions to high-dimensional estimation problems. The SPA performs local operations through “messages” passed along the edges of a factor graph. Many traditional sequential estimation methods such as the Kalman filter, the particle filter, and JPDA can be interpreted as instances of SPA. In addition, SPA has led to a variety of new estimation methods in a wide range of applications [35, 36, 37]. For probabilistic data association (PDA) with point objects, an SPA-based method referred to as the sum-product algorithm for data association (SPADA) is obtained by executing the SPA on a bipartite factor graph and simplifying the resulting SPA messages [38, 4, 31]. The computational complexity of this algorithm scales as the product of the number of measurements and the number of objects. Sequential estimation methods that embed the SPADA to reduce computational complexity have been introduced for multiobject tracking (MOT) [15, 39, 40, 41, 42, 43], indoor localization [44, 45, 46, 47], as well as simultaneous localization and tracking [48, 49, 50].

SPA methods for probabilistic data association based on a bipartite factor graph have recently been investigated for the tracking of extended objects [51, 25, 52]. Here, the number of measurements produced by each object is modeled by an arbitrary truncated probability mass function (PMF). An SPA for data association with extended objects with a computational complexity that scales as the product of the number of measurements and the number of objects has been introduced [51, 25]. This method can track multiple objects that potentially generate a large number of measurements but was found unable to reliably determine the probability of existence of objects detected for the first time. An alternative method for scalable data association with extended objects has been developed independently and has been presented in [53, 54]. These methods however either assume that the number of objects are known, or that a certain prior information of new object locations is available. They are thus unsuitable for scenarios with spontaneous object birth.

In this paper, we introduce a Bayesian particle-based SPA for scalable detection and tracking of an unknown number of extended objects. Object birth and the number of measurements generated by an object are described by a Poisson PMF. The proposed method is derived on a factor graph that depicts the statistical model of the extended object tracking problem and is based on a representation of data association uncertainty by means of measurement-oriented association variables. Contrary to the factor graph used in [51], it makes it possible to reliably determine the existence of objects by means of the SPA. In our statistical model, the object extent is modeled as a basic geometric shape that is represented by a positive semidefinite extent state. Contrary to conventional EOT algorithms with a random-matrix model, the proposed fully particle-based method is not limited to elliptical object shapes. A fully particle-based approach also makes it possible to consider a uniform distribution of measurements on the object extent. Furthermore, our method has a computational complexity that scales only quadratically in the number of objects and the number of measurements. It can thus accurately detect, localize, and track multiple closely-spaced extended objects that generate a large number of measurements. The contributions of this paper are as follows.

• •

  We derive a new factor graph for the problem of detection and tracking of multiple extended objects and introduce the corresponding message passing equations.

• •

  We establish a particle-based scalable message passing algorithm for the detection and tracking of an unknown number of extended objects.

• •

  We demonstrate performance advantages in a challenging simulated scenario and by using real lidar measurements acquired by an autonomous vehicle.

This paper advances over the preliminary account of our method provided in the conference publication [55] by (i) simplifying the representation of data association uncertainty, (ii) including object extents in the statistical model, (iii) presenting a detailed derivation of the factor graph, (iv) establishing a particle-based implementation of the proposed method, (v) discussing scaling properties, (vi) demonstrating performance advantages compared to the recently proposed Poisson multi-Bernoulli mixture filter [5] using synthetic and real measurements.

The probabilistic model established in this paper is closely related to the probabilistic model employed by the PMBM filter (cf. [4]). Our focus is on obtaining a factor graph formulation for which the SPA scales well, and on exploiting the accuracy and flexibility of a particle-based implementation.

Notation: Random variables are displayed in sans serif, upright fonts, while their realizations are in serif, italic fonts. Vectors and matrices are denoted by bold lowercase and uppercase letters, respectively. For example, a random variable and its realization are denoted by $\mathsfbr{x}$ and $x$; a random vector and its realization by $\bm{\mathsfbr{x}}$ and $\bm{x}$; and a random matrix and its realization are denoted by $\bm{\mathsfbr{X}}$ and $\bm{X}$. Furthermore, ${\bm{x}}^{\text{T}}$ denotes the transpose of vector $\bm{x}$; $\propto$ indicates equality up to a normalization factor; $f(\bm{x})$ denotes the probability density function (PDF) of random vector $\bm{\mathsfbr{x}}$. ${\cal{N}}(\bm{x};\bm{\mu},\bm{\varSigma})$ denotes the Gaussian PDF (of random vector $\bm{\mathsfbr{x}}$) with mean $\bm{\mu}$ and covariance matrix $\bm{\varSigma}$, ${\cal{U}}(\bm{y};{\cal{S}})$ denotes the uniform PDF (of random vector $\bm{\mathsfbr{y}}$) with support ${\cal{S}}$, and ${\cal{W}}\big{(}\bm{X};q,\bm{Q}\big{)}$ denotes the Wishart distribution (of random matrix $\bm{\mathsfbr{X}}$) with degrees of freedom $q$ and mean $q\hskip 0.85358pt\bm{Q}$. The determinant of matrix $\bm{Q}$ is denoted $|\bm{Q}|$. Finally, $\mathrm{bdiag}(\bm{M}_{1},\dots,\bm{M}_{I})$ denotes the block diagonal matrix that consists of submatrices $\bm{M}_{1},\dots,\bm{M}_{I}$.

The state-transition pdf for legacy PO state $\underline{\bm{\mathsfbr{y}}}_{n}$ factorizes as

where the single-object augmented state-transition pdf $f\big{(}\underline{\bm{y}}_{k,n}\big{|}\bm{y}_{k,n-1}\big{)}=f\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n},\underline{r}_{k,n}\big{|}\bm{x}_{k,n-1},\bm{e}_{k,n-1},r_{k,n-1}\big{)}$ is given as follows. If PO $k$ does not exist at time $n-1$, i.e., $\mathsfbr{r}_{k,n-1}\!=0$, then it does not exist at time $n$ either, i.e., $\underline{\mathsfbr{r}}_{k,n}\!=\!0$, and thus its state pdf is $f_{\mathrm{d}}\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n}\big{)}$. This means that

On the other hand, if PO $k$ exists at time $n-1$, i.e., $\mathsfbr{r}_{k,n-1}\!=\!1$, then the probability that it still exists at time $n$, i.e., $\underline{\mathsfbr{r}}_{k,n}\!=\!1$, is given by the survival probability $p_{\mathrm{s}}$, and if it still exists at time $n$, its state $\underline{\bm{\mathsfbr{x}}}_{k,n}$ is distributed according to a single-object state-transition pdf $f\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n}\big{|}\bm{x}_{k,n-1},\bm{e}_{k,n-1}\big{)}$. Thus,

It is assumed that at time $n=0$, the prior distributions $f(\bm{y}_{k,0})$ are statistically independent across POs $k$. If no prior information is available, we have $K_{0}=0$.

A single-object state-transition pdf $f\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n}\big{|}\bm{x}_{k,n-1},$ $\bm{e}_{k,n-1}\big{)}$ that was found useful in many extended object tracking scenarios is given by [12]

where $\mathpzc{f}(\bm{x}_{k-1,n})$ is the state transition function of the kinematic state $\bm{\mathsfbr{x}}_{k-1,n}$, $\bm{\varSigma}_{k,n}$ is the kinematic driving noise covariance matrix, and $\bm{V}(\bm{m}_{k,n})$ is a rotation matrix. The degrees of freedom of the Wishart distribution determine the uncertainty of object extent prediction. A small $q_{k,n}$ implies a large state-transition uncertainty.

At time $n$, a sensor produces measurements $\bm{\mathsfbr{z}}_{l,n}\in\mathbb{R}^{d_{\bm{z}}}$, $l\in\{1,$ $\dots,\mathsf{M}_{n}\}$. The joint measurement vector is denoted as $\bm{\mathsfbr{z}}_{n}\triangleq[\bm{\mathsfbr{z}}^{\text{T}}_{1,n}$ $\cdots\hskip 0.85358pt\bm{\mathsfbr{z}}^{\text{T}}_{{\mathsf{M}_{n},n}}]^{\text{T}}$. (Note that the total number of measurements $\mathsf{M}_{n}$ is random.) Each measurement is either generated by a single object or is a false alarm.

Let $\bm{\mathsfbr{z}}_{l,n}$ be a measurement generated by the $k$th PO.²²2The fact that the PO with index $k$ generates a measurement implies $r_{k,n}\!=\!1$. In particular, let $\bm{\mathsfbr{v}}^{(l)}_{k,n}$ be the relative position (with respect to $\bm{\mathsfbr{p}}_{k,n}$) of the reflection point that generated $\bm{\mathsfbr{z}}_{l,n}$. The considered general nonlinear measurement model is now given by

where $\bm{d}(\cdot)$ is an arbitrary nonlinear function and $\bm{\mathsfbr{u}}_{l,n}\sim\mathcal{N}(\bm{u}_{l,n};\bm{0},\bm{\varSigma}_{\bm{u}_{l,n}})$ is the measurement noise. (Note that the subspace $\bm{\mathsfbr{m}}_{k,n}$ remains unobserved.) This measurement model directly determines the conditional PDF $f\big{(}\bm{z}_{l,n}|\bm{x}_{k,n},\bm{v}^{(l)}_{k,n}\big{)}$.

We consider three geometric object shape models. The extent state $\bm{\mathsfbr{E}}_{k,n}=\bm{E}_{k,n}$ either defines (i) the covariance matrix of a Gaussian PDF, i.e., $\bm{\mathsfbr{v}}_{k,n}^{(l)}\sim\mathcal{N}\big{(}\bm{v}^{(l)}_{k,n};\bm{0},\bm{E}^{2}_{k,n}\big{)}$; (ii) the elliptical support ${\cal{S}}_{\mathrm{e}}(\bm{E}_{k,n})$ of a uniform PDF, i.e., $\bm{\mathsfbr{v}}_{k,n}^{(l)}\sim\mathcal{U}\big{(}\bm{v}_{k,n}^{(l)};{\cal{S}}_{\mathrm{e}}(\bm{E}_{k,n})\big{)}$; or (iii) the cubical support ${\cal{S}}_{\mathrm{c}}(\bm{E}_{k,n})$ of a uniform PDF, i.e., $\bm{\mathsfbr{v}}_{k,n}^{(l)}\sim\mathcal{U}\big{(}\bm{v}_{k,n}^{(l)};{\cal{S}}_{\mathrm{c}}(\bm{E}_{k,n})\big{)}$.³³3The cubical support ${\cal{S}}_{\mathrm{c}}(\bm{E}_{k,n})$ and the elliptical support ${\cal{S}}_{\mathrm{e}}(\bm{E}_{k,n})$ are defined by the eigenvalues and eigenvectors of $\bm{E}_{k,n}$ as shown in Fig 1. For example, let us consider a 3-D scenario and let $\lambda_{1,\bm{E}_{k,n}}>\lambda_{2,\bm{E}_{k,n}}>\lambda_{3,\bm{E}_{k,n}}\geqslant 0$ be the eigenvalues of the $3\times 3$ symmetric, positive-semidefinite matrix $\bm{E}_{k,n}$. Length, width, and height of the cube ${\cal{S}}_{\mathrm{c}}(\bm{E}_{k,n})$ are given by $\lambda_{1,\bm{E}_{k,n}}$, $\lambda_{2,\bm{E}_{k,n}}$, and $\lambda_{3,\bm{E}_{k,n}}$, respectively. Similarly, the lengths of the three principal semi-axes of the ellipsoid ${\cal{S}}_{\mathrm{e}}(\bm{E}_{k,n})$, are given by $\lambda_{1,\bm{E}_{k,n}}$, $\lambda_{2,\bm{E}_{k,n}}$, and $\lambda_{3,\bm{E}_{k,n}}.$ The orientation of ${\cal{S}}_{\mathrm{c}}(\bm{E}_{k,n})$ and ${\cal{S}}_{\mathrm{e}}(\bm{E}_{k,n})$ is determined by the eigenvectors $\bm{\lambda}_{1,\bm{E}_{k,n}}$,$\bm{\lambda}_{2,\bm{E}_{k,n}}$, and $\bm{\lambda}_{3,\bm{E}_{k,n}}$. The eigenvalues $\lambda_{1,\bm{E}_{k,n}}$, $\lambda_{2,\bm{E}_{k,n}}$, and $\lambda_{3,\bm{E}_{k,n}}$ are assumed distinct. This assumption is always valid for real sensor data and objects of practical interest. The considered object shape model directly determines the conditional PDF $f\big{(}\bm{v}^{(l)}_{k,n}|\bm{e}_{k,n}\big{)}$. The PDF of measurement vector $\bm{\mathsfbr{z}}_{l,n}$ conditioned on $\bm{\mathsfbr{x}}_{k,n}$ and $\bm{\mathsfbr{e}}_{k,n}$ can now be obtained as

An important special case of this model is the additive-Gaussian case, i.e.,

For extent model (i), in this case, there exist closed-form expressions for (6). In particular, the likelihood function $f(\bm{z}_{l,n}|\bm{x}_{k,n},\bm{e}_{k,n})$ can be expressed as $\mathcal{N}\big{(}\bm{z}_{l,n};\bm{p}_{k,n},\bm{E}^{2}_{k,n}+\bm{\varSigma}_{\bm{u}_{l,n}}\big{)}$. Similarly, for models (ii) and (iii), there exists a simple approximation discussed in [56, Section 2], that also results in a closed-form of (6).

If PO $k$ exists ($\mathsfbr{r}_{k,n}=1$) it generates a random number of object-originated measurements $\bm{\mathsfbr{z}}_{l,n}$, which are distributed according to the conditional PDF $f(\bm{z}_{l,n}|\bm{x}_{k,n},\bm{e}_{k,n})$ discussed above. The number of measurements it generates is Poisson distributed with mean $\mu_{\mathrm{m}}(\bm{\mathsfbr{x}}_{k,n},\bm{\mathsfbr{e}}_{k,n})$. For example, this Poisson distribution can be determined by a spatial measurement density $\rho$ related to the sensor resolution and by the volume or surface area of the object extent, i.e., $\mu_{\mathrm{m}}(\bm{\mathsfbr{x}}_{k,n},\bm{\mathsfbr{e}}_{k,n})=\rho\hskip 0.85358pt|\bm{\mathsfbr{E}}_{k,n}|$. False alarm measurements are modeled by a Poisson point process with mean $\mu_{\mathrm{fa}}$ and strictly positive PDF $f_{\mathrm{fa}}(\bm{z}_{l,n})$.

Newly detected objects, i.e., actual objects that generated a measurement for the first time, are modeled by a Poisson point process with mean $\mu_{\mathrm{n}}$ and PDF $f_{\mathrm{n}}(\overline{\bm{x}}_{k,n},\overline{\bm{e}}_{k,n})$. Following [15, 4, 5], newly detected objects are represented by new PO states $\overline{\bm{\mathsfbr{y}}}_{k,n}$, $k\in\{1,\dots,\mathsf{M}_{n}\}$. Each new PO state corresponds to a measurement $\bm{\mathsfbr{z}}_{k,n}$; $\overline{\mathsfbr{r}}_{k,n}\!=\!1$ means that measurement $\bm{\mathsfbr{z}}_{k,n}$ was generated by a newly detected object. Since newly detected objects can produce more than one measurement, we define a mapping from measurements to new POs by the following rule.⁴⁴4A detailed derivation and discussion is provided in the supplementary material [56]. At time $n$, if multiple measurements $l_{1},\dots,l_{L}$ with $L\leqslant M_{n}$ are generated by the same newly detected object, we have $\overline{\mathsfbr{r}}_{k_{\mathrm{min}},n}=1$ for $k_{\mathrm{min}}=\mathrm{min}(l_{1},\dots,l_{L})$ and $\overline{\mathsfbr{r}}_{k,n}=0$ for all $k\in\big{\{}l_{1},\dots,l_{L}\big{\}}\backslash\big{\{}k_{\mathrm{min}}\big{\}}$. As will be further discussed in Section II-D, with this mapping every association event related to newly detected objects can be represented by exactly one configuration of new existence variables $\overline{\mathsfbr{r}}_{k,n}$, $k\in\{1,\dots,\mathsf{M}_{n}\}$. We also introduce $\overline{\bm{\mathsfbr{y}}}_{n}\triangleq[\overline{\bm{\mathsfbr{y}}}^{\text{T}}_{1,n}\cdots\hskip 0.85358pt\overline{\bm{\mathsfbr{y}}}^{\text{T}}_{\mathsf{M}_{n}}]^{\text{T}}$ representing the joint vector of all new PO states. Note that at time $n$, the total number of POs is given by $\mathsf{K}_{n}\!=\underline{K}_{n}\!+\mathsf{M}_{n}$.

Since new POs are introduced as new measurements are incorporated, the number of PO states would grow indefinitely. Thus, for the development of a feasible method, a suboptimal pruning step is employed. This pruning step removes unlikely POs and will be further discussed in Section III-A.

Tracking of multiple extended objects is complicated by the data association uncertainty: it is unknown which measurement $\bm{\mathsfbr{z}}_{l,n}$ originated from which object $k$. To reduce computational complexity, following [38, 40, 4] we use measurement-oriented association variables

and define the measurement-oriented association vector as $\bm{\mathsfbr{b}}_{n}=[\mathsfbr{b}_{1,n}\hskip 0.85358pt\cdots\hskip 0.85358pt\mathsfbr{b}_{\mathsf{M}_{n},n}]^{\mathrm{T}}$. This representation of data association makes it possible to develop scalable SPAs for object detection and tracking. Note that the maximum value in the support of $\mathsfbr{b}_{l,n}$ is $\underline{K}_{n}+l$. (As discussed above, a measurement with index $l$ cannot be associated to a new PO with index $l^{\prime}>l$, i.e., the event in which measurements $l^{\prime}$ and $l$ are generated by the same newly detected object is represented through the new PO $l$.) This is a direct result of the mapping introduced in Section II-C. In what follows, we write $\mathsfbr{b}_{l,n}\neq k$ short for $\mathsfbr{b}_{l,n}\in\{0,1,\dots,\underline{K}_{n}+l\}\backslash\{k\}$.

For a better understanding of new POs and measurement-oriented association vectors, we consider simple examples for fixed $M_{n}=3$ and $\underline{K}_{n}=0$. The event where all three measurements are generated by the same newly detected object, is represented by $\overline{r}_{1,n}=1$, $\overline{r}_{2,n}=0$, $\overline{r}_{3,n}=0$, and $\bm{b}_{n}=\big{[}\hskip 0.85358ptb_{1,n}\hskip 0.85358pt\hskip 0.85358ptb_{2,n}\hskip 0.85358pt\hskip 0.85358ptb_{3,n}\big{]}^{\mathrm{T}}=[1\hskip 0.85358pt\hskip 0.85358pt1\hskip 0.85358pt\hskip 0.85358pt1]^{\mathrm{T}}$. Furthermore, the event where all three measurements are generated by three different newly detected objects, is represented by $\overline{r}_{1,n}=1$, $\overline{r}_{2,n}=1$, $\overline{r}_{3,n}=1$, and $\bm{b}_{n}=[1\hskip 0.85358pt\hskip 0.85358pt2\hskip 0.85358pt\hskip 0.85358pt3]^{\mathrm{T}}$. Finally, the event where measurements $m\in\{2,3\}$ are generated by the same newly detected object and measurement $m=1$ is a false alarm, is represented by $\overline{r}_{1,n}=0$, $\overline{r}_{2,n}=1$, $\overline{r}_{3,n}=0$, and $\bm{b}_{n}=[0\hskip 0.85358pt\hskip 0.85358pt2\hskip 0.85358pt\hskip 0.85358pt2]^{\mathrm{T}}$. Note how every event related to newly detected objects is represented by exactly one configuration of new existence variables $\overline{\mathsfbr{r}}_{n,k}$, $k\in\{1,2,3\}$ and association vector $\bm{b}_{n}$.

We consider the problem of detecting legacy and new POs $k\!\in\!\{1,\dots,\underline{K}_{n}+M_{n}\}$ based on the binary existence variables $r_{k,n}$, as well as estimation of the object states $\bm{\mathsfbr{x}}_{k,n}$ and extents $\bm{\mathsfbr{e}}_{k,n}$ from the observed measurement history vector $\bm{z}_{1:n}=\big{[}\bm{z}_{1}^{\text{T}}\cdots\hskip 0.85358pt\bm{z}^{\text{T}}_{n}\big{]}^{\text{T}}\!$. Our Bayesian approach aims to calculate the marginal posterior existence probabilities $p(r_{k,n}\!=\!1|\bm{z}_{1:n})$ and the marginal posterior pdfs $f(\bm{x}_{k,n},\bm{e}_{k,n}|r_{k,n}\!=\!1,\bm{z}_{1:n})$. We perform detection by comparing $p(r_{k,n}\!=\!1|\bm{z}_{1:n})$ with a threshold $P_{\text{th}}$, i.e., if $p(r_{k,n}\!=\!1|\bm{z}_{1:n})>P_{\text{th}}$ [57, Ch. 2], PO $k$ is considered to exist. Estimates of $\bm{\mathsfbr{x}}_{k,n}$ and $\bm{\mathsfbr{e}}_{k,n}$ are then obtained for the detected objects $k$ by the minimum mean-square error (MMSE) estimator [57, Ch. 4]. In particular, an MMSE estimate of the kinematic state is obtained as

Similarly, an MMSE estimate $\hat{\bm{e}}^{\text{MMSE}}_{k,n}$ of the extent state is obtained by replacing $\bm{x}_{k,n}$ with $\bm{e}_{k,n}$ in (LABEL:eq:mmse).

We can compute the marginal posterior existence probability $p(r_{k,n}\!=\!1\hskip 0.85358pt|\hskip 0.85358pt\bm{z}_{1:n})$ needed for object detection as discussed above, from the marginal posterior pdf, $f(\bm{y}_{k,n}|\bm{z}_{1:n})=f(\bm{x}_{k,n},\bm{e}_{k,n},r_{k,n}|\bm{z}_{1:n})$, as

Note that $p(r_{k,n}\!=\!1|\bm{z}_{1:n})$ is also needed for the suboptimal pruning step discussed in Section V-D.

Similarly, we can calculate the marginal posterior pdf $f(\bm{x}_{k,n},\bm{e}_{k,n}|r_{k,n}\!=\!1,\bm{z}_{1:n})$ underlying MMSE state estimation (see (LABEL:eq:mmse)) from $f(\bm{x}_{k,n},\bm{e}_{k,n},r_{k,n}|\bm{z}_{1:n})$ according to

For this reason, the fundamental task is to compute the pdf $f(\bm{y}_{k,n}|\bm{z}_{1:n})=f(\bm{x}_{k,n},\bm{e}_{k,n},r_{k,n}|\bm{z}_{1:n})$. This pdf is a marginal density of $f(\bm{y}_{0:n},\bm{b}_{1:n}|\bm{z}_{1:n})$, which involves all the augmented states and measurement-oriented association variables up to the current time $n$.

The main problem to be solved is to find a computationally feasible recursive (sequential) calculation of marginal posterior PDFs $f(\bm{y}_{k,n}|\bm{z}_{1:n})$. By performing message passing by means of the SPA rules [33] on the factor graph that represents our statistical model discussed in Section II, approximations (“beliefs”) $\tilde{f}(\bm{y}_{k,n})$ of this marginal posterior pdfs can be obtained in an efficient way for all legacy and new POs.

We now assume that the measurements $\bm{z}_{1:n}$ are observed and thus fixed. By using common assumptions [2, 3, 4, 5, 1], the joint posterior PDF of $\bm{\mathsfbr{y}}_{0:n}$ and $\bm{\mathsfbr{b}}_{1:n}$, conditioned on $\bm{z}_{1:n}$ is given by⁵⁵5A detailed derivation of this joint posterior PDF is provided in [56, Section 1].

where we introduced the functions $h_{k}\big{(}\bm{y}_{k,n},b_{l,n};\bm{z}_{l,n}\big{)}$, $g_{k}\big{(}\overline{\bm{y}}_{k,n},b_{k,n};\bm{z}_{k,n}\big{)}$, $\underline{q}\big{(}\underline{\bm{y}}_{k,n}|\bm{y}_{k,n-1}\big{)}$, and $\overline{q}\big{(}\overline{\bm{y}}_{k,n}\big{)}$ that will be discussed next.

The pseudo likelihood functions $h_{k}\big{(}\bm{y}_{k,n},b_{l,n};\bm{z}_{l,n}\big{)}=h_{k}\big{(}\bm{x}_{k,n},\bm{e}_{k,n},r_{k,n},b_{l,n};\bm{z}_{l,n}\big{)}$ and $g_{k}\big{(}\overline{\bm{y}}_{k,n},b_{k,n};\bm{z}_{k,n}\big{)}=g_{k}\big{(}\overline{\bm{x}}_{k,n},\overline{\bm{e}}_{k,n},\overline{r}_{k,n},b_{k,n};\bm{z}_{k,n}\big{)}$ are given by

and $h_{k}\big{(}\bm{x}_{k,n},\bm{e}_{k,n},0,b_{l,n};\bm{z}_{l,n}\big{)}\triangleq 1-\delta\big{(}b_{l,n}-k\big{)}$ as well as

and $g_{k}\big{(}\overline{\bm{x}}_{k,n},\overline{\bm{e}}_{k,n},0,b_{k,n};\bm{z}_{k,n}\big{)}\triangleq 1-\delta\big{(}b_{k,n}-(\underline{K}_{n}+k)\big{)}$. Note that the second line in (13) is zero because, as discussed in Section II-D, the new PO with index $k$ exists ($\overline{r}_{k,n}=1$) if and only if it produced measurement $k$.

Furthermore, the factors containing prior distributions for new POs $\overline{q}(\overline{\bm{y}}_{k,n})=\overline{q}(\overline{\bm{x}}_{k,n},\overline{\bm{e}}_{k,n},\overline{r}_{k,n})$, $k\in\{1,\dots,M_{n}\}$ are given by

and the pseudo transition functions (cf. (2) and (3)) for legacy POs $\underline{q}\big{(}\underline{\bm{y}}_{k,n}|\bm{y}_{k,n-1}\big{)}\triangleq\underline{q}\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n},\underline{r}_{k,n}|\bm{x}_{k,n-1},\bm{e}_{k,n-1},$ $r_{k,n-1}\big{)}$ are given by $\underline{q}\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n},\underline{r}_{k,n}=1|\bm{x}_{k,n-1},\bm{e}_{k,n-1},$ $r_{k,n-1}\big{)}=\mathpzc{e}^{-\mu_{\mathrm{m}}(\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n})}f\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n},\underline{r}_{k,n}=1|\bm{x}_{k,n-1},$ $\bm{e}_{k,n-1},r_{k,n-1}\big{)}$ and $\underline{q}\big{(}\underline{\bm{x}}_{k,n},\underline{\bm{e}}_{k,n},\underline{r}_{k,n}=0|\bm{x}_{k,n-1},\bm{e}_{k,n-1},$ $r_{k,n-1}\big{)}=f\big{(}\underline{\bm{x}}_{k,n},$ $\underline{\bm{e}}_{k,n},$ $\underline{r}_{k,n}=0\hskip 0.85358pt|\hskip 0.85358pt\bm{x}_{k,n-1},\bm{e}_{k,n-1},r_{k,n-1}\big{)}$.

A detailed derivation of this factor graph is provided in the supplementary material [56]. The factor graph representing factorization (11) is shown in Fig. 2. An interesting observation is that this factor graph has the same structure as a conventional multi-scan tracking problem [4, 58] with $M$ scans. (Here, every measurement is considered an individual scan.) In what follows, we consider a single time step and remove the time index $n$ for the sake of readability.

First, a prediction step is performed for all legacy POs $k\in\underline{K}$. Based on SPA rule [33, Eq. (6)], we obtain

where $\tilde{f}(\bm{x}^{-}_{k},\bm{e}^{-}_{k},r^{-}_{k})$ is the belief that was calculated at the previous time step. Recall that the integration $\int\mathrm{d}\bm{e}^{-}_{k}$ in (14) is performed over the support of $\bm{e}^{-}_{k},$ which corresponds to all positive-semidefinite matrices $\bm{E}^{-}_{k}$.

Next, we use the expression for $\underline{q}(\underline{\bm{x}}_{k},\underline{\bm{e}}_{k},\underline{r}_{k}\hskip 0.85358pt|$ $\bm{x}^{-}_{k},\bm{e}^{-}_{k},r^{-}_{k})$ as introduced in Section III-B and in turn (2) and (3) for $f(\underline{\bm{x}}_{k},\underline{\bm{e}}_{k},\underline{r}_{k}\hskip 0.85358pt|\bm{x}^{-}_{k},\bm{e}^{-}_{k},r^{-}_{k})$. In this way, we obtain the following expressions for (14)

and $\underline{\alpha}(\underline{\bm{x}}_{k},\underline{\bm{e}}_{k},\underline{r}_{k}=0)=\underline{\alpha}^{\mathrm{n}}_{k}f_{\mathrm{d}}(\underline{\bm{x}}_{k},\underline{\bm{e}}_{k})$ with

We note that $\tilde{f}^{-}_{k}=\int\int\tilde{f}(\bm{x}^{-}_{k},\bm{e}^{-}_{k},0)\hskip 0.85358pt\mathrm{d}\bm{x}^{-}_{k}\mathrm{d}\bm{e}^{-}_{k}$ approximates the probability of non-existence of legacy PO $k$ at the previous time step. After the prediction step, the iterative message passing is performed. For future reference, we also introduce $\underline{\alpha}_{k}\triangleq\int\int\underline{\alpha}_{k}(\underline{\bm{x}}_{k},\underline{\bm{e}}_{k},\underline{r}_{k}=1)\hskip 0.85358pt\mathrm{d}\underline{\bm{x}}_{k}\hskip 0.85358pt\mathrm{d}\underline{\bm{e}}_{k}+\underline{\alpha}^{\mathrm{n}}_{k}$.