# Multisensor Multiobject Tracking
With High-Dimensional Object States

Wenyu Zhang, and Florian Meyer This material is based upon work supported by
the Under Secretary of Defense for Research and Engineering under Air Force
Contract No. FA8702-15-D-0001 and by the National Science Foundation (NSF)
under CAREER Award No. 2146261. Parts of this work have been presented at the
IEEE RADAR-21, Atlanta, Georgia, May 2021.Wenyu Zhang is with the Department
of Electrical and Computer Engineering, University of California San Diego, La
Jolla, CA, USA (e-mail: wez078@eng.ucsd.edu).Florian Meyer is with the Scripps
Institution of Oceanography and the Department of Electrical and Computer
Engineering, University of California San Diego, La Jolla, CA, USA (e-mail:
flmeyer@ucsd.edu).

###### Abstract

Passive monitoring of acoustic or radio sources has important applications in
modern convenience, public safety, and surveillance. A key task in passive
monitoring is multiobject tracking (MOT). This paper presents a Bayesian
method for multisensor MOT for challenging tracking problems where the object
states are high-dimensional, and the measurements follow a nonlinear model.
Our method is developed in the framework of factor graphs and the sum-product
algorithm (SPA). The multimodal probability density functions provided by the
SPA are effectively represented by a Gaussian mixture model (GMM). To perform
the operations of the SPA in high-dimensional spaces, we make use of Particle
flow (PFL). Here, particles are migrated towards regions of high likelihood
based on the solution of a partial differential equation. This makes it
possible to obtain good object detection and tracking performance even in
challenging multisensor MOT scenarios with single sensor measurements that
have a lower dimension than the object positions. We perform a numerical
evaluation in a passive acoustic monitoring scenario where multiple sources
are tracked in 3-D from 1-D time-difference-of-arrival (TDOA) measurements
provided by pairs of hydrophones. Our numerical results demonstrate favorable
detection and estimation accuracy compared to state-of-the-art reference
techniques.

###### Index Terms:

Multiobject tracking, particle flow, factor graphs, sum-product algorithm.

##  I Introduction

MOT is an important capability for a variety of applications, including
maritime surveillance and marine mammal research. MOT is a high-dimensional
nonlinear filtering problem complicated by measurement-origin uncertainty
(MOU), i.e., the associations between measurements and objects, and an unknown
number of objects to be tracked. In this paper, we develop a sequential
Bayesian MOT framework for the particularly challenging scenarios where object
states are high-dimensional and measurement models are nonlinear. We expect
that our approach is particularly useful for the passive monitoring of
acoustic [1] or radio [2] sources in 3-D.

###  I-A State-of-the-Art

Traditional methods for MOT include PDA [3], multi-hypothesis tracking (MHT)
[4], and methods based on random finite sets (RFS) [5, 6, 7, 8]. Most of these
traditional approaches suffer from a computational complexity that is
exponential in important system parameters, including the number of
measurements, objects, and sensors. MOT methods that are scalable with respect
to these parameters have been recently developed in the framework of factor
graphs and the SPA [9, 10, 11, 12]. The SPA is known to efficiently provide
accurate solutions to high-dimensional Bayesian estimation problems. In
particular, by performing local operations (“messages”) on the factor graph
that represents statistical independences of random variables, accurate
approximations (“beliefs”) of the marginal posterior pdfs of unknown states
[13] are computed. SPA-based methods are versatile and have been successfully
applied to a variety of applications, including cooperative localization[14,
15, 16, 17], simultaneous localization and mapping (SLAM) [18, 19, 20], and
focalization for underwater localization [21].

To calculate messages that, due to nonlinearities in the system model, cannot
be evaluated in closed form, SPA-based methods for MOT typically rely on
particle-based computations that closely follow the bootstrap particle filter
(BPF) [22, 23, 24] and rely on importance sampling. A known drawback of this
approach is that it suffers from the curse of dimensionality. If the state of
the individual object is high-dimensional or if measurements are informative,
particle-based computations can fail due to particle degeneracy [25]. The
particle degeneracy problem is related to the fact that predicted pdfs are
used as proposal pdfs for sampling. Since predicted pdfs can have completely
different shapes than the posterior pdfs, this sampling strategy is highly
inefficient, i.e., few or none of the generated particles are suitable to
represent the posterior pdfs. Sometimes particle degeneracy can be avoided by
using vast numbers of particles or by implementing regularization strategies
[26, 24, 15]. The most promising approach to avoid particle degeneracy is to
design proposal pdfs that are similar to the posterior pdfs [22, 23, 24].
However, finding a distribution that is easy to sample from and simultaneously
similar to posterior pdfs [22, 23, 24] is often challenging. A promising
approach is to perform the update step of an unscented Kalman filter [27] and
use the resulting Gaussian pdf as a proposal pdf for particle filtering. The
unscented particle filter [28, 29] combines this idea with a Gaussian mixture
representation of predicted and posterior pdfs. To the best of our knowledge,
the unscented particle filtering approach has not yet been extended to
problems with MOU and an unknown number of states to be estimated.

PFL [30, 31, 32, 33, 34] is a promising strategy for estimation in nonlinear
systems with high-dimensional states and informative measurements. PFL makes
use of a homotopy function to incrementally move a set of particles sampled
from the predicted pdf. In particular, a partial differential equation (PDE)
is obtained by combining the homotopy function with the Fokker-Planck
equation. The solution to the PDE can be discretized and used as a transport
equation for particle migration. After migration, the set of particles
represents the posterior pdf. There are two different types of PFL resulting
in the exact Daum and Huang (EDH) filter and the localized exact Duam and
Huang (LEDH) filter. In the EDH filter, the PFL equations are computed once
for the mean of all particles. In contrast, in the computationally more
demanding LEDH filter, the PFL equations are computed for each particle
individually.

Traditional PFL methods avoid importance sampling and can only provide an
approximate representation of posterior pdfs in general nonlinear systems [30,
31, 32, 33, 34]. Nevertheless, these traditional methods often lead to
accurate estimation results at a significantly reduced computational
complexity compared to BPF [35]. Recently, it has been shown that PFL can be
described by an invertible mapping and can thus be used as a measurement-
driven proposal pdf for importance sampling [35]. The resulting PFL filter
[35] is an asymptotically optimal approach to nonlinear filtering that avoids
particle degeneracy and can provide accurate estimation results in high-
dimensional and nonlinear problems.

A significant limitation of the PFL filter presented in [35] is that it
assumes that the prior or predicted pdfs follow Gaussian distributions. It is
thus unsuitable for problems that involve multimodal pdfs. For problems where
the measurement noise follows a Gaussian mixture pdf, [36] introduces the
Gaussian sum PFL filter. Here, the means of the Gaussian mixture components
are updated by performing an update step similar to the LEDH. On the other
hand, the covariance matrices of the components are updated by extended Kalman
filters that also run in parallel. An extension of [36] to the case where both
driving noise and measurement noise are distributed by a Gaussian mixture pdf
is presented in [37]. Here, invertible flow is used for particle weight update
in an importance sampling step. For problems where both driving noise and
measurement noise can be multimodal, [38] combines the invertible PFL with a
sequential Markov chain Monte Carlo (SMCMC) method that relies on the
Metropolis-Hastings approach, i.e., a Metropolis-Hastings kernel is
constructed using a PFL algorithm based on a GMM. However, aforementioned PFL
approaches that can represent multimodal pdfs [36, 37, 38] are unsuitable for
MOT since they neither model MOU nor an unknown number of states to be
estimated.

A variant of the PFL filter has been proposed for MOT [8]. In particular, EDH
and LEDH implementations of the single-sensor $\delta$-Generalized Labeled
Multi-Bernoulli filter [7] with invertible flow are presented.

###  I-B Contributions, Paper Organization, and Notation

We develop a method for multisensor MOT for scenarios with high-dimensional
object states and informative measurements. Of particular interest are
multisensor MOT problems, where the single sensor measurement has a lower
dimension than the object positions. Consider a scenario where object
positions are 3-D, but sensors only provide 1-D measurements, e.g., times of
arrival (TOAs), time differences of arrival (TDOAs), or directions of arrival
(DOAs). In this type of MOT problem, prior or predicted pdfs can have
complicated multimodal shapes, e.g., spheres, hyperboloids, or cones at the
initial step after the appearance of a new object. Our approach performs SPA-
based message passing on the factor graph for scalable multisensor MOT
developed in [10]. The messages of the SPA are computed sequentially across
sensors. To represent beliefs with complicated shapes, we make use of a GMM
for object states. The parameters of the GMMs are computed by embedding
invertible PFL into SPA-based computations. This enables the migration of
particles towards regions of high likelihood and, in turn, leads to an
accurate approximation of SPA messages with a relatively small number of
particles. We demonstrated that the resulting multisensor MOT can outperform
reference methods based on bootstrap and unscented particle filtering in a 3-D
passive source tracking scenario.

Key contributions of this paper are as follows.

  * •

We develop a graph-based MOT method based on a GMM and invertible PFL for
challenging scenarios with high-dimensional object states.

  * •

We demonstrate that the proposed method can significantly outperform reference
techniques in a challenging 3-D passive source tracking scenario.

This paper advances over the preliminary account of our method provided in the
conference publication [39] by (i) introducing a GMM; (ii) considering the
multisensor MOT problem; and (iii) performing a comprehensive numerical
evaluation in a 3-D passive source tracking scenario111An application of the
proposed method to the problem of tracking multiple whales underwater by
performing TDOA measurements of their echolocation clicks, is presented in the
companion paper [40]. . Contrary to the approach presented in [8], the
proposed method is suitable for multisensor scenarios. In addition, SPA-based
processing makes our approach scalable with respect to relevant system
parameters, including the number of sensors, objects, and measurements.

_Notation:_ Random variables are displayed in sans serif, upright fonts and
their realizations 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$,
respectively, and a random vector and its realization by $\bm{\mathsfbr{x}}$
and $\bm{x}$, respectively. Furthermore, $\|\bm{x}\|$ and
${\bm{x}}^{\text{T}}$ denote the Euclidean norm and the transpose of vector
$\bm{x}$, respectively; and $\propto$ indicates equality up to a normalization
factor. ${\cal{N}}(\bm{x};\bm{x}^{\ast},\bm{P})$ denotes the Gaussian pdf (of
random vector $\bm{\mathsfbr{x}}$) with mean $\bm{x}^{\ast}$ and covariance
matrix $\bm{P}$. The trace of matrix $\bm{M}$ is denoted as
$\mathrm{Tr}\\{\bm{M}\\}$. Finally, $1(a)$ denotes the indicator function of
the event $a=0$, i.e., $1(a)=1$ if $a=0$ and $0$ otherwise.

##  II Review of Invertible PFL

We consider the general setting of calculating the posterior pdf based on
Bayes rule

$f(\bm{x}|\bm{z})\propto f(\bm{x})\hskip
0.85358ptf(\bm{z}|\bm{x})\vspace{.5mm}$ (1)

with the state of interest $\bm{x}$ and the observed (fixed) measurement
$\bm{z}$. If the prior pdf $f(\bm{x})$ follows a Gaussian distribution and the
likelihood function $f(\bm{z}|\bm{x})$ represents a linear measurement model
$\bm{\mathsfbr{z}}=\bm{H}\bm{\mathsfbr{x}}+\bm{\mathsfbr{v}}$ with Gaussian
measurement noise $\bm{\mathsfbr{v}}$, the posterior pdf $f(\bm{x}|\bm{z})$
also follows a Gaussian distribution. In this special case, the mean and
covariance of the Gaussian posterior pdf $f(\bm{x}|\bm{z})$ can be calculated
in closed form by the Kalman update step [41].

If the measurement model is nonlinear, e.g.,
$\bm{\mathsfbr{z}}=\bm{h}(\bm{\mathsfbr{x}})+\bm{\mathsfbr{v}}$, a popular
approach is to approximate the posterior pdf $f(\bm{x}|\bm{z})$ by a set of
$N_{\mathrm{p}}$ weighted particles
$\\{(\bm{x}^{(i)},w^{(i)})\\}^{N_{\mathrm{p}}}_{i=1}$. Note that the weights
are normalized to one, i.e., $\sum^{N_{\mathrm{p}}}_{i=1}w^{(i)}=1$ and can be
computed based on the importance sampling principle [24] as follows

$w^{(i)}\propto\frac{f(\bm{x}^{(i)})f(\bm{z}|\bm{x}^{(i)})}{q(\bm{x}^{(i)}|\bm{z})}.\vspace{1mm}$
(2)

Here, the proposal pdf $q(\bm{x}|\bm{z})$ is used to sample the particles
$\\{\bm{x}^{(i)}\\}^{N_{\mathrm{p}}}_{i=1}$. It is an arbitrary pdf that has
the same support as $f(\bm{x}|\bm{z})$. Importance sampling is asymptotically
optimal if $q(\bm{x}|\bm{z})$ is “heavier tailed”, i.e., less informative,
than $f(\bm{x}|\bm{z})$ [23]. In particular, importance sampling can provide
an approximation of $f(\bm{x}|\bm{z})$ that can be made arbitrarily good by
choosing $N_{\mathrm{p}}$ sufficiently large [24]. For $N_{\mathrm{p}}$ fixed,
if the proposal $q(\bm{x}|\bm{z})$ is “more similar” to the posterior
$f(\bm{x}|\bm{z})$ [23], importance sampling is “more accurate”.

A simple choice for the proposal pdf used in the update step of the
conventional “bootstrap” particle filter [22, 24] is the prior pdf
$f(\bm{x})$. However, for a feasible number of particles $N_{\mathrm{p}}$ and
most choices of the proposal pdf, importance sampling can suffer from particle
degeneracy [25]. Particle degeneracy is especially severe if the state
$\bm{x}$ is high-dimensional and the measurement $\bm{z}$ is informative
(i.e., the likelihood function has narrow and sharp peaks).

###  II-A Particle Flow (PFL)

PFL [30, 31, 32, 33] is an approach that aims at avoiding particle degeneracy.
Here, particles are smoothly migrated in the state space from a representation
of the prior pdf to a representation of the posterior pdf by solving a PDE.
Let us introduce the homotopy function $\pi_{\lambda}(\bm{x})=f(\bm{x})\hskip
0.85358ptl^{\lambda}(\bm{x})$ where $\lambda\\!\in\\![0,1]$ is the pseudo time
of the flow process and $l(\bm{x})=f(\bm{z}|\bm{x})$ is the likelihood
function. Note that for $\lambda=1$, the homotopy function is equal to the
unnormalized posterior pdf, i.e.,
$\pi(\bm{x})\triangleq\pi_{1}(\bm{x})=f(\bm{x})\hskip 0.85358ptl(\bm{x})$. The
log-homotopy function is then given by [30, 31],

$\phi(\bm{x},\lambda)=\log f(\bm{x})+\lambda\log l(\bm{x}).\vspace{0mm}$ (3)

The log-homotopy function is a pseudo posterior pdf in the log domain that
defines a smooth and continuous deformation from $\phi(\bm{x},0)=\log
f(\bm{x})$ to $\phi(\bm{x},1)=\log\pi(\bm{x})$. This deformation describes the
PFL process.

It can be shown that the stochastic process defined by $\pi_{\lambda}(\bm{x})$
satisfies the Fokker-Planck equation [34, 33, 32] under common assumptions.
Combining the Fokker-Planck equation for the zero-diffusion case with (3)
results in the following PDE [33, 32]

$\frac{\partial\phi(\bm{x},\lambda)}{\partial\bm{x}}\bm{\zeta}(\bm{x},\lambda)+\log
l(\bm{x})=-\mathrm{Tr}\Big{(}\frac{\partial\bm{\zeta}(\bm{x},\lambda)}{\partial\bm{x}}\Big{)}\vspace{1mm}$
(4)

where $\bm{\zeta}(\bm{x},\lambda)=\frac{\mathrm{d}\bm{x}}{\mathrm{d}\lambda}$
describes the migration of particles (samples of $\bm{\mathsfbr{x}}$) as the
pseudo time $\lambda$ increases from $0$ to $1$. This migration is referred to
as the PFL.

###  II-B PFL Update Step

If $f(\bm{x})$ and $l(\bm{x})$ are Gaussians or in another exponential family,
then an exact and closed form solution for (4) is available. The EDH filter
[42, 32] makes use of this closed-form solution in its update step. More
precisely, let $f(\bm{x})={\cal{N}}(\bm{x};\bm{x}_{0}^{\ast},\bm{P})$ and
$\bm{\mathsfbr{z}}=\bm{H}\bm{\mathsfbr{x}}+\bm{\mathsfbr{v}}$ be a linear
measurement model with measurement noise
$\bm{\mathsfbr{v}}\sim{\cal{N}}(\bm{x};\bm{0},\bm{R})$. The exact flow
solution [42, 32] now reads

$\bm{\zeta}(\bm{x},\lambda)=\bm{A}(\lambda)\bm{x}+\bm{b}(\lambda)\vspace{0mm}$

where we introduce

$\displaystyle\bm{A}(\lambda)$
$\displaystyle=-\frac{1}{2}\bm{P}\bm{H}^{\mathrm{T}}(\lambda\bm{H}\bm{P}\bm{H}^{\mathrm{T}}+\bm{R})^{-1}\bm{H}$
(5)

and

$\displaystyle\bm{b}(\lambda)$
$\displaystyle=(\bm{I}+2\lambda\bm{A}(\lambda))\big{[}(\bm{I}+\lambda\bm{A}(\lambda))\bm{P}\bm{H}^{\mathrm{T}}\bm{R}^{-1}\bm{z}+\bm{A}(\lambda){\bm{x}}^{\ast}_{0}\big{]}.$

Note that in (LABEL:EDH_b), $\bm{z}$ is the observed and thus fixed
measurement.

This solution is extended to the nonlinear measurement model
$\bm{\mathsfbr{z}}=\bm{h}(\bm{\mathsfbr{x}})+\bm{\mathsfbr{v}}$ by performing
a suboptimal linearization step. In particular, in a first-order
approximation, a Jacobian matrix is computed, i.e.
$\bm{H}(\lambda)=\left.\frac{\partial\bm{h}(\bm{x})}{\partial\bm{x}}\right|_{\bm{x}={\bm{x}}^{\ast}_{\lambda}}$
where ${\bm{x}}^{\ast}_{\lambda}$ is the approximated mean of $\bm{x}$ at
pseudo time $\lambda$.

In a practical implementation, we calculate $\bm{\zeta}(\bm{x},\lambda)$ at
$N_{\lambda}$ discrete values of $\lambda$, i.e.,
$0=\lambda_{0}<\lambda_{1}<...<\lambda_{N_{\lambda}}=1$, to perform the PFL.
Here, we first sample $N_{\mathrm{p}}$ particles
$\big{\\{}\bm{x}_{0}^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\\!\triangleq\\!\big{\\{}\bm{x}_{\lambda_{0}}^{(i)}\\}_{i=1}^{N_{\mathrm{p}}}$
from $f({\bm{x}})$. Next, at each discrete pseudo time step
$l\in\\{1,\dots,N_{\lambda}\\}$, particles are migrated according to

$\bm{x}^{(i)}_{\lambda_{l}}=\bm{x}^{(i)}_{\lambda_{l-1}}+\tilde{\bm{\zeta}}(\bm{x}^{(i)}_{\lambda_{l-1}},\lambda_{l})(\lambda_{l}-\lambda_{l-1})\hskip
4.2679pt\vspace{1mm}$ (7)

for all $i\in\\{1,\dots,N_{\mathrm{p}}\\}$. Here, the linearized flow solution
$\tilde{\bm{\zeta}}(\bm{x}^{(i)}_{\lambda_{l-1}},\lambda_{l})=\bm{A}_{l}\bm{x}^{(i)}_{\lambda_{l-1}}+\bm{b}_{l}$
is computed based on $\bm{A}_{l}$ and $\bm{b}_{l}$ given by (cf. (5) and
(LABEL:EDH_b))

$\bm{A}_{l}=-\frac{1}{2}\bm{P}{\bm{H}_{l}}^{\mathrm{T}}(\lambda_{l}\bm{H}_{l}\bm{P}{\bm{H}_{l}}^{\mathrm{T}}+\bm{R})^{-1}\bm{H}_{l}$
(8)
$\bm{b}_{l}=(\bm{I}+2\lambda_{l}\bm{A}_{l})[(\bm{I}+\lambda_{l}\bm{A}_{l})\bm{P}{\bm{H}_{l}}^{\mathrm{T}}\bm{R}^{-1}(\bm{z}-\bm{e}_{l})+\bm{A}_{l}{\bm{x}}^{\ast}_{0}].\vspace{.5mm}$
(9)

Note that
$\bm{e}_{l}=\bm{h}({\bm{x}}^{\ast}_{\lambda_{l-1}},0)-\bm{H}_{l}{\bm{x}}^{\ast}_{\lambda_{l-1}}$
is the error of linear- ization and that the linearized measurement model,
$\bm{H}_{l}$, is computed based on the mean of the last step $l-1$, i.e.,

$\bm{H}_{l}=\left.\frac{\partial\bm{h}(\bm{x})}{\partial\bm{x}}\right|_{\bm{x}={\bm{x}}^{\ast}_{\lambda_{l-1}}}.$
(10)

The mean at which the measurement model is linearized is typically propagated
in parallel to the particles, i.e.,
${\bm{x}}^{\ast}_{\lambda_{l}}={\bm{x}}^{\ast}_{\lambda_{l-1}}+\tilde{\bm{\zeta}}({\bm{x}}^{\ast}_{\lambda_{l-1}},\lambda_{l})(\lambda_{l}-\lambda_{l-1})$.

After the last discrete pseudo time step, $l=N_{\lambda}$, particles
$\\{\bm{x}_{1}^{(i)}\\}_{i=1}^{N_{\mathrm{p}}}\\!\triangleq\\!\\{\bm{x}_{\lambda_{N_{\lambda}}}^{(i)}\\}_{i=1}^{N_{\mathrm{p}}}$
that approximately represent the unnormalized posterior pdf $\pi(\bm{x})$ are
finally obtained. Pseudocode for the PFL update step is provided in Algorithm
1. PFL based on a linearized model has no optimality guarantees. However, it
has been demonstrated numerically to typically provide an accurate
representation of the posterior pdf $f(\bm{x}|\bm{z})$[30, 31, 32, 33, 34,
35]. In what follows, the PFL related to the measurement $\bm{z}$ as defined
by (7)–(9), is denoted as
$\bm{\mathsfbr{x}}_{0}\xrightarrow[]{}\bm{z}\xrightarrow[]{}\bm{\mathsfbr{x}}_{1}$
or for notational convenience in future derivations as
$\bm{\mathsfbr{x}}_{0}\xrightarrow[]{}\bm{z}\xrightarrow[]{}\bm{\mathsfbr{x}}$.

1
$\Big{[}\big{\\{}{\bm{x}}^{(i)}_{1}\big{\\}}_{i=1}^{N_{\mathrm{p}}},\\{A_{l}\\}_{l=1}^{N_{\lambda}}\Big{]}=\text{ParticleFlow}\Big{(}\big{\\{}{\bm{x}}^{(i)}_{0}\big{\\}}_{i=1}^{N_{\mathrm{p}}},\bm{x}_{0}^{\ast},\bm{P},\bm{z}\Big{)}$

2 Define pseudo time steps
$0=\lambda_{0}<\lambda_{1}<...<\lambda_{N_{\lambda}}=1$;

3

4for  _$l=1:N_{\lambda}$_ do

5 Calculate the linearized measurement model $\bm{H}_{l}$ according to (10);

6 Compute $\bm{A}_{l}$ and $\bm{b}_{l}$ according to (8) and (9);

7 for  _$i=1:N_{\mathrm{p}}$_ do

8
$\tilde{\bm{\zeta}}(\bm{x}^{(i)}_{\lambda_{l-1}},\lambda_{l})=\bm{A}_{l}\bm{x}^{(i)}_{\lambda_{l-1}}+\bm{b}_{l}$;

9
$\bm{x}^{(i)}_{\lambda_{l}}=\bm{x}^{(i)}_{\lambda_{l-1}}+\tilde{\bm{\zeta}}(\bm{x}^{(i)}_{\lambda_{l-1}},\lambda_{l})(\lambda_{l}-\lambda_{l-1})$;

10

11 end for

12
$\tilde{\bm{\zeta}}(\bm{x}^{\ast}_{\lambda_{l-1}},\lambda_{l})=\bm{A}_{l}\bm{x}^{\ast}_{\lambda_{l-1}}+\bm{b}_{l}$;

13
$\bm{x}^{\ast}_{\lambda_{l}}=\bm{x}^{\ast}_{\lambda_{l-1}}+\tilde{\bm{\zeta}}(\bm{x}^{\ast}_{\lambda_{l-1}},\lambda_{l})(\lambda_{l}-\lambda_{l-1})$;

14

15 end for

Output:
$\big{\\{}{\bm{x}}^{(i)}_{1}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\triangleq\big{\\{}{\bm{x}}^{(i)}_{\lambda_{N_{\lambda}}}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$
and $\\{A_{l}\\}_{l=1}^{N_{\lambda}}$

Algorithm 1 PFL Update Step

As an alternative to EDH PFL as discussed above, LEDH PFL has been introduced
in [43, 35]. Here, a linearization is performed for each particle location
individually instead of only at the mean particle location, i.e., individual
flow parameters $\bm{H}_{l}^{(i)},\bm{A}_{l}^{(i)}$, and $\bm{b}_{l}^{(i)}$
are computed to migrate the particles along their individual flows. Although
the LEDH flow usually outperforms the EDH flow, it suffers from considerable
computational complexity, since the main computational burden is related to
calculating the flow parameters.

###  II-C Importance Sampling with Invertible Flow

PFL can be used to compute a measurement-driven proposal pdf
$q(\bm{x}|\bm{z})$ for importance sampling (cf. (2)) to perform asymptotically
optimal estimation [35]. Here, the mapping as performed by PFL
$\bm{\mathsfbr{x}}_{0}\xrightarrow[]{}\bm{z}\xrightarrow[]{}\bm{\mathsfbr{x}}$
is invertible, i.e., there exists an invertible mapping of the particles after
the flow $\big{\\{}\bm{x}^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$ to the
particles $\big{\\{}\bm{x}_{0}^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$ if
certain constraints on the differences of consecutive discrete pseudo times
$\lambda_{l}-\lambda_{l-1}$, $l\in\\{1,\dots,N_{\lambda}\\}$ are satisfied
[35].

By exploiting the invertible mapping, the proposal pdf resulting from PFL can
evaluated at the particles as [35]

$q_{\mathrm{PFL}}(\bm{x}^{{(i)}}|\bm{z})=\frac{f(\bm{x}_{0}^{(i)})}{\theta}.\vspace{2mm}$
(11)

Here, the “mapping factor” $\theta$ is defined as

$\theta=\prod_{l=1}^{N_{\lambda}}\big{|}\hskip
0.85358pt\mathrm{det}\big{[}\bm{I}+(\lambda_{l}-\lambda_{l-1})\hskip
0.85358pt\bm{A}_{l}\big{]}\big{|}.\vspace{0mm}$ (12)

By plugging (11) into (2) the weight of the particle $\bm{x}^{(i)}$ is
obtained as

$w^{(i)}\propto\frac{\theta f(\bm{z}|\bm{x}^{(i)})\hskip
0.85358ptf(\bm{x}^{(i)})}{f(\bm{x}_{0}^{(i)})}.\vspace{1.5mm}$ (13)

The resulting particle set $\\{\bm{x}^{(i)},w^{(i)}\\}_{i=1}^{N_{\mathrm{p}}}$
is an asymptotically optimal sample representation of the posterior pdf
$f(\bm{x}|\bm{z})$ that can often provide accurate estimation results in
nonlinear and high-dimensional estimation problems even if the number of
particles is moderate [35]. Pseudocode for importance sampling with invertible
PFL is provided in Algorithm 2. Note that since the PFL used for the
measurement-driven proposal pdf is typically based on the EDH filter update
step, a Gaussian prior pdf is assumed.

1
$\Big{[}\big{\\{}{\bm{x}}^{(i)}_{1},w_{1}^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{]}=\text{InvertibleFlow}\big{(}\bm{x}^{\ast},\bm{P},\bm{z}\big{)}$

2[1mm] for  _$i=1:N_{\mathrm{p}}$_ do

3 Draw
$\bm{x}_{0}^{(i)}\sim\mathcal{N}\big{(}\bm{x};\bm{x}^{\ast},\bm{P}\big{)}$;

4

5 end for

6 Perform PF according
$\Big{[}\big{\\{}{\bm{x}}^{(i)}_{1}\big{\\}}_{i=1}^{N_{\mathrm{p}}},\\{A_{l}\\}_{l=1}^{N_{\lambda}}\Big{]}=\text{ParticleFlow}\Big{(}\big{\\{}{\bm{x}}^{(i)}_{0}\big{\\}}_{i=1}^{N_{\mathrm{p}}},\bm{x}^{\ast},\bm{P},$
$\bm{z}\Big{)}\vspace{.5mm}$;

// see Alg. 1

7 Compute the mapping factor $\theta$ from $\\{A_{l}\\}_{l=1}^{N_{\lambda}}$
following (12);

8 for  _$i=1:N_{\mathrm{p}}$_ do

9 Perform weight update according to (13), i.e.,
$w_{1}^{(i)}=\frac{\theta\hskip
0.85358ptf\big{(}\bm{z}|\bm{x}_{1}^{(i)}\big{)}\hskip
0.85358pt\mathcal{N}\big{(}\bm{x}_{1}^{(i)};\bm{x}^{\ast},\bm{P}\big{)}}{\mathcal{N}\big{(}\bm{x}_{0}^{(i)};\bm{x}^{\ast},\bm{P}\big{)}}$;

10

11 end for

Output:
$\big{\\{}{\bm{x}}^{(i)}_{1},w_{1}^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$

Algorithm 2 Importance Sampling with Invertible Flow

An approximate Gaussian representation of this posterior distribution can be
subsequently obtained by applying Algorithm 3 which calculates a mean
${\bm{x}}^{\ast}_{1}$ and a covariance matrix $\bm{P}_{1}$ from the
unnormalized weighted particles
$\big{\\{}{\bm{x}}^{(i)}_{1},w_{1}^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$.
Note that in Algorithm 2, the same mapping factor $\theta$ is used to
calculate all particle weights. If Algorithm 3 is applied after Algorithm 2,
this factor is irrelevant since all weights are normalized in Algorithm 3.
However, making use of $\theta$ is important if multiple flows are performed
in parallel, as will be discussed in Section III.

1
$\big{[}{\bm{x}}^{\ast},\bm{P}\big{]}=\text{GaussianRepresentation}\Big{(}\big{\\{}{\bm{x}}^{(i)},\omega^{(i)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{)}$

2 Normalize particles, i.e.,

3 for  _$i=1:N_{\mathrm{p}}$_ do

4
$w^{(i)}=\frac{\omega^{(i)}}{\sum^{N_{\mathrm{p}}}_{i^{\prime}=1}\omega^{(i^{\prime})}}$
;

5

6 end for

7 Compute mean and covariance matrix from particles, i.e.,

8 $\bm{x}^{\ast}=\sum^{N_{\mathrm{p}}}_{i=1}w^{(i)}\bm{x}^{(i)}$

9[.5mm]
$\bm{P}=\sum^{N_{\mathrm{p}}}_{i=1}w^{(i)}\bm{x}^{(i)}\bm{x}^{(i)\hskip
0.85358pt\mathrm{T}}-\bm{x}^{\ast}\hskip 0.85358pt\bm{x}^{\ast\hskip
0.85358pt\mathrm{T}}$

10[.5mm]

Output: ${\bm{x}}^{\ast},\bm{P}$

Algorithm 3 Computation of Gaussian Representation

Note that instead of a particle-based covariance matrix computation as
performed in Line 8 of Algorithm 3, an extended or unscented Kalman update
step can be used [35]. For a large number of particles $N_{\mathrm{p}}$, a
particle-based computation is more accurate than a computation based on the
extended or unscented Kalman update step.

##  III Gaussian Mixture Representation for Nonlinear Estimation in High-
Dimensions

In this section, we use GMM representations [44, 45, 46] for nonlinear
estimation in high dimensions. In particular, we developed methods for
updating the parameters of GMMs based on PFL. This approach is suitable for
high-dimensional prior and posterior pdfs that are multimodal and thus
relevant for estimation problems in multiobject tracking and SLAM [47]. The
presented GMM parameter computation generalizes invertible PFL based on the
EDH or LEDH filter update steps as presented in [35].

###  III-A GMM Importance Sampling with Invertible Flow

A GMM aims at representing multimodal distributions based on an additive
weighted combination of multiple Gaussian components. Each Gaussian component
is typically referred to as a “kernel”. Let $N_{\mathrm{k}}$ be the total
number of kernels and let $h\in\\{1,\dots,N_{\mathrm{k}}\\}$ be the kernel
index. A multimodal prior pdf that follows a GMM representation can then be
written as

$f(\bm{x})=\frac{1}{N_{\mathrm{k}}}\sum_{h=1}^{N_{\mathrm{k}}}{\cal{N}}\big{(}\bm{x};\bm{x}^{(h)\ast},\bm{P}^{(h)}\big{)}.\vspace{.5mm}$
(14)

The corresponding multimodal posterior pdf $f(\bm{x}|\bm{z})$ can be computed
by performing Algorithm 2 and Algorithm 3 $N_{\mathrm{k}}$ times in parallel,
i.e., one instance of both algorithms is performed for each kernel
${\cal{N}}\big{(}\bm{x};\bm{x}^{(h)\ast},\bm{P}^{(h)}\big{)}$,
$h\in\\{1,\dots,N_{\mathrm{k}}\\}$. To obtain a GMM representation composed of
an arbitrary number $N^{\prime}_{\mathrm{k}}$ of kernels, a resampling step is
then performed, i.e., $N^{\prime}_{\mathrm{k}}$ particles are drawn from the
overall $N_{\mathrm{k}}N_{\mathrm{p}}$ particles based on their weights
$w_{1}^{(h,i)}$. The resampled particles represent the mean of
$N^{\prime}_{\mathrm{k}}$ new kernels. The covariance of the new kernels is
inherited from the original kernel the mean was sampled from. Pseudocode for
GMM importance sampling with invertible PFL is provided in Algorithm 4.
Importance sampling with invertible PFL makes use of resampling as presented
in Algorithm 5.

For $N_{\mathrm{k}}=1$ and $N_{\mathrm{p}}>1$, this importance sampling
approach is equivalent to invertible PFL based on the EDH update step.
Furthermore, if $N_{\mathrm{p}}=1$, $N_{\mathrm{k}}>1$, this importance
sampling approach is equivalent to invertible PFL based on the LEDH update
step. Note that for $N_{\mathrm{p}}=1$, as performed by the LEDH, an
additional extended or unscented Kalman update step needs to be used to
calculate an approximate covariance matrix [35].

1
$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1}\Big{]}=\text{InvertibleFlowGMM}\Big{(}\big{\\{}\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\bm{z}\Big{)}$

2[1mm] for  _$h=1:N_{\mathrm{k}}$_ do

3
$\Big{[}\big{\\{}{\bm{x}}^{(i,h)}_{1},w_{1}^{(i,h)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{]}=\text{InvertibleFlow}\Big{(}\bm{x}^{(h)\ast},\bm{P}^{(h)},\bm{z}\vspace{.75mm}\Big{)}$

[1mm] // see Alg. 2

4

5
$\Big{[}{\bm{x}}^{\ast(h)}_{1},\bm{P}^{(h)}_{1}\Big{]}=\text{GaussianRepresentation}\Big{(}\big{\\{}{\bm{x}}^{(i,h)}_{1},w_{1}^{(i,h)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{)}$

[1mm] // see Alg. 3

6

7 end for

8

9$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N^{\prime}_{\mathrm{k}}}_{h=1}\Big{]}\vspace{.3mm}=\text{Resampling}\Big{(}\Big{\\{}\bm{x}_{1}^{\ast(h)},\bm{P}_{1}^{(h)},\big{\\{}w_{1}^{(i,h)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{\\}}^{N_{\mathrm{k}}}_{h=1}\Big{)}$

[.8mm] // see Alg. 5

10

Output:
$\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N^{\prime}_{\mathrm{k}}}_{h=1}$

Algorithm 4 GMM Importance Samp. with Invertible Flow

1
$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h^{\prime})},\bm{P}_{+}^{(h^{\prime})}\big{\\}}^{N^{\prime}_{\mathrm{k}}}_{h^{\prime}=1}\Big{]}\vspace{.3mm}=\text{Resampling}\Big{(}\Big{\\{}\bm{x}_{1}^{\ast(h)},\bm{P}_{1}^{(h)},\big{\\{}w_{1}^{(i,h)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{\\}}^{N_{\mathrm{k}}}_{h=1}\hskip
0.85358pt\Big{)}$

2[1mm]

3for  _$h=1:N_{\mathrm{k}}$_ do

4 for  _$i=1:N_{\mathrm{p}}$_ do

5
$w_{1}^{(h,i)}=\frac{w_{1}^{(i,h)}}{\sum^{N_{\mathrm{k}}}_{h^{\prime}=1}\sum^{N_{\mathrm{p}}}_{i^{\prime}=1}w_{1}^{(i^{\prime},h^{\prime})}}$
;

6

7 end for

8

9 end for

10

11for  _$h^{\prime}=1:N^{\prime}_{\mathrm{k}}$_ do

12 Sample index $(i^{\prime},\hbar)$ using
$\Big{\\{}\big{\\{}w_{1}^{(i,h)}\big{\\}}^{N_{\mathrm{p}}}_{i=1}\Big{\\}}^{N_{\mathrm{k}}}_{h=1}$;

13 Set $\bm{x}_{+}^{\ast(h^{\prime})}=\bm{x}^{(i^{\prime},\hbar)}_{1}$ and
$\bm{P}_{+}^{(h^{\prime})}=\bm{P}^{(\hbar)}_{1}$;

14

15 end for

16

Output:
$\big{\\{}\bm{x}_{+}^{\ast(h^{\prime})},\bm{P}_{+}^{(h^{\prime})}\big{\\}}^{N^{\prime}_{\mathrm{k}}}_{h^{\prime}=1}$

Algorithm 5 Resampling

###  III-B Measurement-Origin Uncertainty (MOU)

In a variety of estimation problems, the measurement model suffers from a
deficiency beyond measurement noise referred to as MOU [3]. Here, there is a
single object but multiple measurements and it is not known which measurement
was generated by the object. Consider a single object with state
$\bm{\mathsfbr{x}}$ and measurements $\bm{\mathsfbr{z}}^{(m)}$,
$m\in\\{1,\dots,M\\}$. If $m^{\prime}$ is the measurement that was generated
by the object, the corresponding measurement model is given by
$\bm{\mathsfbr{z}}^{(m^{\prime})}=\bm{h}(\bm{\mathsfbr{x}})+\bm{\mathsfbr{v}}$.
Based on this model, the conditional pdf of the object-originated measurement
$\bm{z}^{(m^{\prime})}$ reads
$f_{\mathrm{o}}\big{(}\bm{z}^{(m^{\prime})}|\bm{x}\big{)}$. All the other
measurements are false positives (FPs) that follow the FP pdf
$f_{\mathrm{fp}}\big{(}\bm{z}^{(m^{\prime})}\big{)}$. It is assumed that at
most one measurement originates from the object. The probability that the
object generates a measurement is $p_{\mathrm{d}}$, and the mean number of FPs
is $\mu_{\mathrm{fp}}$. Since it is unknown which measurement was generated by
the object, a discrete and random association variable
$\mathsfbr{a}\in\\{0,1,\dots,M\\}$ is introduced. Here, $\mathsfbr{a}=0$
describes the event where no measurement originated from the object and
$\mathsfbr{a}=m$, $m\in\\{1,\dots,M\\}$ describes the event where measurement
$\bm{\mathsfbr{z}}^{(m)}$ was originated from the object. Let
$\bm{\mathsfbr{z}}=[\bm{\mathsfbr{z}}^{(1)\mathrm{T}},\dots,\bm{\mathsfbr{z}}^{(M)\mathrm{T}}]^{\mathrm{T}}$
be the joint measurement vector. Following common assumptions [3], conditioned
on $\bm{\mathsfbr{x}}$, the joint pdf of $\bm{\mathsfbr{z}}$ and
$\mathsfbr{a}$ is given by

$f(\bm{z},a|\bm{x})\propto\begin{cases}\frac{p_{\mathrm{d}}f(\bm{z}^{(m)}|\hskip
0.85358pt\bm{x})}{\mu_{\mathrm{fp}}f_{\mathrm{fp}}(\bm{z}^{(m)})},&a=m\in\\{1,\dots,M\\}\\\\[2.84526pt]
1-p_{\mathrm{d}}\hskip 0.85358pt,&a=0.\end{cases}\vspace{1mm}$ (15)

For $\bm{z}$ fixed, one can use this conditional pdf to directly compute the
MOU likelihood function

$\displaystyle f(\bm{z}|\bm{x})$
$\displaystyle=\sum^{M}_{a=0}f(\bm{z},a|\bm{x})$ $\displaystyle\propto
1-p_{\mathrm{d}}+\sum^{M}_{a=1}\frac{p_{\mathrm{d}}f(\bm{z}^{(a)}|\hskip
0.85358pt\bm{x})}{\mu_{\mathrm{fp}}f_{\mathrm{fp}}(\bm{z}^{(a)})}$ (16)

and the marginal probability mass function (pmf)

$\displaystyle p(a|\bm{z})$ $\displaystyle\propto f(a,\bm{z})$
$\displaystyle=\int f(\bm{z},a|\bm{x})f(\bm{x})\mathrm{d}\bm{x}.$ (17)

The values of the pmf $p(a|\bm{z})$ are also referred to as marginal
association probabilities [3], i.e., they represent the probability of a
particular association event $a\in\\{0,\dots,M\\}$ conditioned on an observed
$\bm{z}$.

###  III-C Importance Sampling with Invertible PFL for Problems with MOU

In principle, the MOU likelihood function in (16) can be directly used for
importance sampling as in (2). However, in problems with MOU, importance
sampling based on invertible PFL is complicated by the fact that there are
multiple measurements, and it is thus not clear which measurement should be
used to compute PFL parameters (8) and (9), i.e., the PFL proposal pdf
$q_{\mathrm{PFL}}(\bm{x}_{1}^{{(i)}}|\bm{z})$ in (11) cannot be directly used.
To address this problem, we propose the combined proposal pdf

$\displaystyle q(\bm{x}|\bm{z})$ $\displaystyle=p(a=0|\bm{z})f(\bm{x})$
$\displaystyle\hskip 14.22636pt+\sum^{M}_{m=1}p(a=m|\bm{z})\hskip
0.85358ptq_{\mathrm{PFL}}\big{(}\bm{x}|\bm{z}^{(m)}\big{)}$ (18)

where we used the marginal association probabilities $p(a|\bm{z})$ to weight
the proposal pdfs $q_{\mathrm{PFL}}\big{(}\bm{x}|\bm{z}^{(m)}\big{)}$ related
to PFL based on measurements $\bm{z}^{(m)}$, $m\in\\{1,\dots,M\\}$ (cf. (11))
and the Gaussian prior pdf
$f(\bm{x})={\cal{N}}(\bm{x};\bm{x}_{0}^{\ast},\bm{P})$. In particular, recall
that the proposal distribution $q_{\mathrm{PFL}}\big{(}\bm{x}|\bm{z}^{(m)})$
related to the PFL
$\bm{\mathsfbr{x}}_{0}\xrightarrow[]{}\bm{z}^{(m)}\xrightarrow[]{}\bm{\mathsfbr{x}}^{(m)}$,
can be evaluated as

$q_{\mathrm{PFL}}\big{(}\bm{x}^{{(i,m)}}|\bm{z}^{(m)}\big{)}=\frac{{\cal{N}}(\bm{x}_{0}^{(i,m)};\bm{x}_{0}^{\ast},\bm{P})}{\theta^{(m)}}\vspace{.5mm}$
(19)

where $\theta^{(m)}$ is the mapping factor.

A total of $(M+1)N_{\mathrm{p}}$ particles representing the proposal pdf in
(18) is obtained by drawing $N_{\mathrm{p}}$ particles for each of the $M+1$
components in (18) and calculating corresponding marginal association
probabilities and weights. For the first component related to association
event $\mathsfbr{a}=0$, $N_{\mathrm{p}}$ particles
$\big{\\{}\bm{x}^{(i,0)}\big{\\}}^{N_{\mathrm{p}}}_{i=1}$ are directly drawn
from $f(\bm{x})$, the corresponding marginal association probability is
obtained by using (15) in (17), i.e., $p(a=0|\bm{z})\propto 1-p_{\mathrm{d}}$,
and the corresponding combined proposal weights are set to
$\omega^{\prime(i,0)}=p(a=0|\bm{z})\hskip 0.85358pt\hskip
0.85358pt{\cal{N}}(\bm{x}_{0}^{(i,0)};\bm{x}_{0}^{\ast},\bm{P})$ (cf. (18)).
For each other component related to association event $\mathsfbr{a}=m$,
$m\in\\{1,\dots,M\\}$, first $N_{\mathrm{p}}$ particles
$\big{\\{}\bm{x}^{(i,m)}_{0}\big{\\}}^{N_{\mathrm{p}}}_{i=1}$ are drawn from
$f(\bm{x})$. Next, the PFL
$\bm{\mathsfbr{x}}_{0}\xrightarrow[]{}\bm{\mathsfbr{z}}^{(m)}\xrightarrow[]{}\bm{\mathsfbr{x}}^{(m)}$
is applied to the particles
$\big{\\{}\bm{x}^{(i,m)}_{0}\big{\\}}^{N_{\mathrm{p}}}_{i=1}$ and new
particles $\big{\\{}\bm{x}^{(i,m)}\big{\\}}^{N_{\mathrm{p}}}_{i=1}$ are
obtained for each $m\in\\{1,\dots,M\\}$. An approximation of each marginal
association probability $\tilde{p}(a=m|\bm{z})$, $m\in\\{1,\dots,M\\}$ is
finally calculated from these particles by using (15) in (17) and performing
Monte Carlo integration [23] based on the proposal pdf
$q_{\mathrm{PFL}}\big{(}\bm{x}^{{(i,m)}}|\bm{z}^{(i,m)}\big{)}$ in (19), i.e.,

$\tilde{p}(a=m|\bm{z})\propto\sum^{N_{\mathrm{p}}}_{i=1}\frac{p_{\mathrm{d}}\hskip
0.85358pt\theta^{(m)}f\big{(}\bm{z}^{(m)}|\hskip
0.85358pt\bm{x}^{(i,m)}\big{)}\hskip
0.85358pt{\cal{N}}(\bm{x}^{(i,m)};\bm{x}_{0}^{\ast},\bm{P})}{\mu_{\mathrm{fp}}\hskip
0.85358ptN_{\mathrm{p}}\hskip
0.85358ptf_{\mathrm{fp}}\big{(}\bm{z}^{(m)}\big{)}\hskip
0.85358pt{\cal{N}}(\bm{x}_{0}^{(i,m)};\bm{x}_{0}^{\ast},\bm{P})}.\vspace{2mm}$
(20)

The corresponding combined proposal weights are set according to (cf. (18) and
(19))

$\omega^{\prime(i,m)}\hskip 0.85358pt=\hskip
0.85358pt\tilde{p}(a=m|\bm{z})\hskip
0.85358pt\frac{{\cal{N}}(\bm{x}_{0}^{(i,m)};\bm{x}_{0}^{\ast},\bm{P})}{\theta^{(m)}}.\vspace{.4mm}$
(21)

Finally, we reindex the resulting particles and weights
$\big{\\{}\big{(}\hskip 0.85358pt\omega^{\prime(i,a)},$
$\bm{x}^{(i,a)}\big{)}\big{\\}}^{N_{\mathrm{p}}}_{i=1}$, $a\in\\{0,\dots,M\\}$
to obtain $\big{\\{}\big{(}\hskip 0.85358pt\omega^{(l)},$
$\bm{x}^{(l)}\big{)}\big{\\}}^{L}_{l=1}$ where $l=i(a+1)$ and
$L=N_{\mathrm{p}}\hskip 0.85358pt(M+1)$.

Following the importance sampling principle, we next aim to compute particles
$\big{\\{}\big{(}\hskip 0.85358ptw^{(\hskip 0.56905ptl)},\bm{x}^{(\hskip
0.56905ptl)}\big{)}\big{\\}}^{L}_{l=1}$ that represent the posterior pdf
$f(\bm{x}|\bm{z})$. In particular, by plugging (16) into (2) and by using
$\big{\\{}\big{(}\hskip 0.85358pt\omega^{(l)},\bm{x}^{(l)}\big{)}\big{\\}}$
(cf. (21)) to represent $q(\bm{x}|\bm{z})$ in (2), we obtain

$\displaystyle w^{(\hskip
0.56905ptl)}\propto\frac{{\cal{N}}(\bm{x}^{(l)};\bm{x}_{0}^{\ast},\bm{P})\Big{(}1-p_{\mathrm{d}}+\sum^{M}_{m=1}\frac{p_{\mathrm{d}}f(\bm{z}^{(m)}|\hskip
0.85358pt\bm{x}^{(l)})f(\bm{x}^{(l)})}{\mu_{\mathrm{fp}}f_{\mathrm{fp}}(\bm{z}^{(m)})}\Big{)}}{\omega^{(l)}}.$

The resulting set of particles $\big{\\{}\big{(}\hskip 0.85358ptw^{(\hskip
0.56905ptl)},\bm{x}^{(\hskip 0.56905ptl)}\big{)}\big{\\}}^{L}_{l=1}$ is an
asymptotically optimal representation of $f(\bm{x}|\bm{z})$ for scenarios with
MOU.

###  III-D GMM Importance Sampling with Invertible Flow for Problems with MOU

For problems where the prior distribution is non-Gaussian and potentially
multimodal, GMM PFL with invertible flow discussed in Section III-A can be
directly applied to problems with MOU. Pseudocode for GMM importance sampling
with the invertible flow for MOU problems is provided in Algorithm 6.
Algorithm 6 relies on the measurement evaluation presented in Algorithm 7 and
the measurement update presented in Algorithm 8.

Note that for later use in Section V-D, Algorithm 7 performs measurements
evaluation by also taking a probability of existence, $p$, into account.
Similarly, Algorithm 8 also updates $p$. By using $p=1$ as input for Algorithm
7 (see Algorithm 6, line 2), Algorithm 6 is equivalent to the estimation
method discussed in Sections III-B and III-C. Furthermore, note that for
consistency with Section V-D, we introduced the notation
$\beta^{(a)}\propto\tilde{p}(a|\bm{z})$, $a\in\\{0,\dots,M\\}$ in Algorithm 6,
Algorithm 7, and Algorithm 8.

1

2$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1}\Big{]}=\text{InvertibleFlowGMMwithDA}\Big{(}\big{\\{}\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\bm{z}\Big{)}$

3[1mm]

4$\Big{[}\Big{\\{}\Big{\\{}\big{\\{}\bm{x}^{(i,h,a)},\omega^{(i,h,a)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{(a)}\Big{\\}}^{M}_{a=0}\hskip
2.84526pt\Big{]}\vspace{1mm}=\hskip
14.22636pt\text{Evaluation}\Big{(}\big{\\{}\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},1,\bm{z}\Big{)}$

[.5mm] // see Alg. 7

5

6$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\sim\Big{]}\vspace{.7mm}=\hskip
14.22636pt\text{Update}\Big{(}\Big{\\{}\Big{\\{}\big{\\{}\bm{x}^{(i,h,a)},\omega^{(i,h,a)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{(a)}\Big{\\}}^{M}_{a=0}\hskip
0.56905pt\Big{)}$

[1.7mm] // see Alg. 8

7

Output:
$\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1}$

Algorithm 6 GMM Importance Sampling with Invertible Flow and DA

1

2$\Big{[}\Big{\\{}\Big{\\{}\big{\\{}\bm{x}^{(i,h,a)},\omega^{(i,h,a)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{(a)}\Big{\\}}^{M}_{a=0}\hskip
0.56905pt\Big{]}\vspace{1mm}=\text{Evaluation}\Big{(}\big{\\{}\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},p,\bm{z}\Big{)}$

3[1mm] Initialize association variables as $\beta^{(0)}=(1-p_{\text{d}})\hskip
0.85358ptp+(1-p)\vspace{.3mm}$;

4

5for  _$h=1:N_{\mathrm{k}}$_ do

6 for  _$i=1:N_{\mathrm{p}}$_ do

7 Draw
$\bm{x}_{0}^{(i,h)}\sim\mathcal{N}\big{(}\bm{x}^{(h)};\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{)}$;

8

9 end for

10 Initialize particles as
$\big{\\{}{\bm{x}}^{(i,h,0)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}=\big{\\{}{\bm{x}}^{(i,h)}_{0}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\vspace{.7mm}$;

11 Initialize proposal weights according to

12 [.5mm]
$\omega^{(i,h,0)}=\mathcal{N}\big{(}\bm{x}^{(i,h,0)};\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{)},i=1,\dots,N_{\mathrm{p}}$;

13

14 end for

15 Initialized mapping factor $\theta^{(h,0)}=1$;

16 for  _$m=1:M$_ do

17 for  _$h=1:N_{\mathrm{k}}$_ do

18

19 Perform PFL, i.e.,

20
$\Big{[}\big{\\{}{\bm{x}}^{(i,h,m)}\big{\\}}_{i=1}^{N_{\mathrm{p}}},\big{\\{}A^{(h,m)}_{l}\big{\\}}_{l=1}^{N_{\lambda}}\Big{]}$

21 [.2mm] $\hskip
14.22636pt=\text{ParticleFlow}\Big{(}\big{\\{}{\bm{x}}^{(i,h)}_{0}\big{\\}}_{i=1}^{N_{\mathrm{p}}},\bm{x}^{\ast(h)},\bm{P}^{(h)},\bm{z}^{(m)}\Big{)}$

[.2mm] // see Alg. 1

22

23 Compute mapping factor $\theta^{(h,m)}$ from
$\big{\\{}A^{(h,m)}_{l}\big{\\}}_{l=1}^{N_{\lambda}}$ as in (12);

24 end for

25

26 Precompute weights (cf. (21) and (LABEL:eq:weightsFinal)), i.e.,

27 [1mm]
$\omega^{(i,h,m)}=\frac{\mathcal{N}\big{(}\bm{x}^{(i,h,0)};\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{)}}{\mathcal{N}\big{(}\bm{x}^{(i,h,m)};\bm{x}^{\ast(h)},\bm{P}^{(h)}\big{)}\theta^{(h,m)}}$,
$i=1,\dots,N_{\mathrm{p}}$;

28

29 Compute approximate association variables following (20), i.e.,
$\beta^{(m)}=\frac{p\hskip
0.85358ptp_{\mathrm{d}}}{N_{\mathrm{p}}N_{\mathrm{k}}\mu_{\mathrm{fp}}}\sum^{N_{\mathrm{k}}}_{h=1}\sum^{N_{\mathrm{p}}}_{i=1}\frac{f\big{(}\bm{z}^{(m)}\big{|}\hskip
0.85358pt\bm{x}^{(i,h,m)}\big{)}}{f_{\mathrm{fp}}\big{(}\bm{z}^{(m)}\big{)}\omega^{(i,h,m)}}\vspace{1mm}$;

30

31 end for

Output:
$\Big{\\{}\Big{\\{}\big{\\{}\bm{x}^{(i,h,a)},\omega^{(i,h,a)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{(a)}\Big{\\}}^{M}_{a=0}$

Algorithm 7 GMM Importance Sampling with Invertible Flow and DA – Measurement
Evaluation

1

2$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},p_{+}\Big{]}\vspace{.7mm}=\hskip
5.69054pt\text{Update}\Big{(}\Big{\\{}\Big{\\{}\big{\\{}\bm{x}^{(i,h,a)},\omega^{(i,h,a)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{(a)}\Big{\\}}^{M}_{a=0},p\Big{)}$

3[1mm]

4Compute association probabilities
$\tilde{p}(a|\bm{z})=\frac{\beta^{(a)}}{\sum^{M}_{a=0}\beta^{(a)}},a=0,\dots,M$

5for  _$h=1:N_{\mathrm{k}}$_ do

6

7 Update proposal weights according to

8 [.8mm] $\omega^{\prime(i,h,0)}=\tilde{p}(a=0|\bm{z})\hskip
0.85358pt\omega^{(i,h,0)},i=1,\dots,N_{\mathrm{p}}$;

9

10 for  _$m=1:M$_ do

11

12 Update proposal weights (cf. (21) and (LABEL:eq:weightsFinal)), i.e.,

13 [.6mm] $\omega^{\prime(i,h,m)}=\tilde{p}(a=m|\bm{z})\omega^{(i,h,m)}$,
$i=1,\dots,N_{\mathrm{p}}$;

14

15 end for

16

17 end for

18

19for  _$h=1:N_{\mathrm{k}}$_ do

20 Reindex particles and proposal weights using $l=i(a+1)\vspace*{.3mm}$ to
obtain $\big{\\{}\big{(}\hskip 0.85358pt\omega^{(\hskip
0.56905ptl,h)},\bm{x}^{(\hskip 0.56905ptl,h)}\big{)}\big{\\}}^{L}_{l=1}$ from
$\big{\\{}\big{(}\hskip 0.85358pt\omega^{\prime(i,h,a)},$
$\bm{x}^{(i,h,a)}\big{)}\big{\\}}^{N_{\mathrm{p}}}_{i=1}\vspace*{-.3mm}$,
$a\in\\{0,\dots,M\\}$;

21

22 Compute final particle weights according to (cf. (LABEL:eq:weightsFinal))

23 for  _$l=1:L$_ do

24 $w^{(\hskip 0.56905ptl,h)}=\frac{\Big{(}\hskip
0.85358pt1-p_{\mathrm{d}}\hskip 0.85358pt+\hskip
0.85358pt\sum^{M}_{m=1}\frac{p_{\mathrm{d}}f\big{(}\bm{z}^{(m)}\big{|}\hskip
0.85358pt\bm{x}^{(\hskip
0.56905ptl,h)}\big{)}}{\mu_{\mathrm{fp}}f_{\mathrm{fp}}\big{(}\bm{z}^{(m)}\big{)}}\hskip
0.85358pt\Big{)}}{\omega^{(\hskip 0.56905ptl,h)}}$;

25

26 end for

27
$\Big{[}{\bm{x}}^{\ast(h)},\bm{P}^{(h)}\Big{]}=\text{GaussianRepresentation}\Big{(}\big{\\{}{\bm{x}}^{(l,h)},w^{(l,h)}\big{\\}}_{l=1}^{L_{\mathrm{s}}}\Big{)}$

[.8mm] // see Alg. 1

28

29 end for

30

31$w=\frac{p}{N_{\mathrm{k}}L_{\mathrm{s}}}\sum_{h=1}^{N_{\mathrm{k}}}\sum_{l=1}^{L_{\mathrm{s}}}w^{(l,h)}$

32$p_{+}=\frac{w}{(1-p)\tilde{p}(a=0|\bm{z})+w}$

33

34$\Big{[}\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1}\Big{]}\vspace{.3mm}=\text{Resampling}\Big{(}\Big{\\{}\bm{x}^{\ast(h)},\bm{P}^{(h)},\big{\\{}w^{(l,h)}\big{\\}}_{l=1}^{L_{\mathrm{s}}}\Big{\\}}^{N_{\mathrm{k}}}_{h=1}\hskip
0.85358pt\Big{)}\vspace{.3mm}$

[.8mm] // see Alg. 5

35

Output:
$\big{\\{}\bm{x}_{+}^{\ast(h)},\bm{P}_{+}^{(h)}\big{\\}}^{N_{\mathrm{k}}}_{h=1}$,
$p_{+}$

Algorithm 8 GMM Importance Sampling with Invertible Flow and DA – Measurement
Update

##  IV Review of Graph-based MOT Using Multiple Sensors

In what follows, we will review the system model, and the corresponding
statistical formulation of graph-based MOT using multiple sensors as presented
in [9, 10]. Graph-based MOT will be combined with a Gaussian mixture
representation and PFL-based processing in Section V.

###  IV-A System Model

We will first review the concept of potential object (PO) states, which is
used to model an unknown number of objects. Then we review the state-
transition model and the MOU likelihood function. Finally, we derive the
posterior pdf and the corresponding factor graph used for statistical
inference.

####  IV-A1 PO States and State-Transition Model

As in [9, 10], we consider MOT for an unknown, time-varying number of objects
by introducing PO states. The number of POs $\mathsfbr{J}_{k-1}$ at discrete
time $k-1\geqslant 0$ is the maximum possible number of objects that have
generated a measurement up to time $k-1$. At time $k$, a new PO is introduced
for each of the $\mathsfbr{M}_{k}$ observed measurements, and the total number
of POs is updated as $\mathsfbr{J}_{k}=\mathsfbr{J}_{k-1}+\mathsfbr{M}_{k}$.
All POs that have been introduced at previous time steps are referred to as
legacy POs, i.e., at time $k$, there are $\mathsfbr{J}_{k-1}$ legacy POs and
$\mathsfbr{M}_{k}$ new POs.

The augmented state of PO $j\in\\{1,\dots,J_{k}\\}$ is given by
$\bm{\mathsfbr{y}}_{k}^{(j)}\triangleq\big{[}\bm{\mathsfbr{x}}^{(j)\mathrm{T}}_{k}\hskip
0.85358pt\mathsfbr{r}^{(j)}_{k}\big{]}^{\mathrm{T}}$, where the state
$\bm{\mathsfbr{x}}^{(j)}_{k}$ of PO $j$ consists of the position and possibly
further parameters of the object represented by the PO. Furthermore, the
existence variable $\mathsfbr{r}^{(j)}_{k}\in\\{0,1\\}$ models the
existence/nonexistence of PO $j$ in the sense that PO $j$ exists at time $k$
if and only if $\mathsfbr{r}^{(j)}_{k}=1$. For nonexistent POs, i.e.,
$\mathsfbr{r}^{(j)}_{k}\\!=\\!0$, the state $\bm{\mathsfbr{x}}^{(j)}_{k}$ is
obviously irrelevant. Thus, all pdfs of augmented PO states
$f\big{(}\bm{y}^{(j)}_{k}\big{)}=f\big{(}\bm{x}^{(j)}_{k},r^{(j)}_{k}\big{)}$
can be expressed as
$f\big{(}\bm{x}^{(j)}_{k},0\big{)}=f^{(j)}_{k}f_{\text{D}}\big{(}\bm{x}^{(j)}_{k}\big{)}$,
where $f_{\text{D}}\big{(}\bm{x}^{(j)}_{k}\big{)}$ is an arbitrary “dummy pdf”
and $f^{(j)}_{k}\\!\in[0,1]$ is a constant. To distinguish between legacy and
new POs, we denote by
$\underline{\bm{\mathsfbr{y}}}_{k}\triangleq\big{[}\underline{\bm{\mathsfbr{y}}}^{(1)\hskip
0.85358pt\mathrm{T}}_{k}\cdots\hskip
0.85358pt\underline{\bm{\mathsfbr{y}}}^{(J_{k-1})\hskip
0.85358pt\mathrm{T}}_{k}\big{]}^{\mathrm{T}}$ and by
$\overline{\bm{\mathsfbr{y}}}_{k}\triangleq\big{[}\overline{\bm{\mathsfbr{y}}}^{(1)\hskip
0.85358pt\mathrm{T}}_{k}\cdots\hskip
0.85358pt\overline{\bm{\mathsfbr{y}}}^{(\mathsfbr{M}_{k})\hskip
0.85358pt\mathrm{T}}_{k}\big{]}^{\mathrm{T}}$ as the joint vector of all
legacy PO and all new PO states, respectively. Finally the vector of all the
PO states at time $k$ is denoted as
${\bm{\mathsfbr{y}}}_{k}\triangleq\big{[}\underline{\bm{\mathsfbr{y}}}^{\hskip
0.85358pt\mathrm{T}}_{k}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\overline{\bm{\mathsfbr{y}}}^{\hskip
0.85358pt\mathrm{T}}_{k}\big{]}^{\mathrm{T}}\triangleq\big{[}{\bm{\mathsfbr{y}}}^{(1)\hskip
0.85358pt\mathrm{T}}_{k}\cdots\hskip
0.85358pt{\bm{\mathsfbr{y}}}^{(J_{k})\hskip
0.85358pt\mathrm{T}}_{k}\big{]}^{\mathrm{T}}$.

The concept of legacy and new POs can be extended to scenarios with $S$
sensors as follows. Let $\mathsfbr{M}_{k,s}$ be the number of measurements at
time $k$ and sensor $s\in(1,\dots,S)$, where $(1,\dots,S)$ is an arbitrary
processing order of the sensors. The maximum possible number of objects that
have generated a measurement up to time $k$ and sensor $s$ is
$J_{k,s}=J_{k,s-1}+M_{k,s}$. New PO states become legacy PO states when the
measurements of the next time step or the next sensor are considered, i.e.,
$\overline{\bm{\mathsfbr{y}}}_{k,s-1}^{(m)}$ and
$\underline{\bm{\mathsfbr{y}}}_{k,s}^{(J_{k,s-1}+m)}$ denote the augmented
state of the same PO. Note that $J_{k,0}=J_{k-1}$,
$J_{k}=J_{k-1}+\sum_{s=1}^{S}M_{k,s}$, and
${\bm{\mathsfbr{y}}}_{k,s}\triangleq\big{[}\underline{\bm{\mathsfbr{y}}}^{\hskip
0.85358pt\mathrm{T}}_{k,s}\hskip 0.85358pt\overline{\bm{\mathsfbr{y}}}^{\hskip
0.85358pt\mathrm{T}}_{k,s}\big{]}$.

For each augmented PO state $\bm{\mathsfbr{y}}_{k-1}^{(j)}$,
$j\in\\{1,\dots,J_{k-1}\\}$ at time $k-1$, there is one “legacy” PO state
$\underline{\bm{\mathsfbr{y}}}_{k,1}^{(j)}$, $j\in\\{1,\dots,J_{k,0}\\}$ at
time $k$ when measurements of sensor $s=1$ are considered. The evolution of
the augmented PO state between consecutive steps is modeled by a single-object
state transition pdf
$f\big{(}\underline{\bm{y}}_{k,1}^{(j)}\big{|}\bm{y}_{k-1}^{(j)}\big{)}$. It
is assumed that objects evolve independently in time, i.e.,
$f\big{(}\underline{\bm{y}}_{k,1}\big{|}\bm{y}_{k-1}\big{)}=\prod_{j=1}^{J_{k-1}}f\big{(}\underline{\bm{y}}_{k,1}^{(j)}\big{|}\bm{y}_{k-1}^{(j)}\big{)}$.
The state-transition pdf of the augmented PO
state$f\big{(}\underline{\bm{y}}_{k,1}^{(j)}\big{|}\bm{y}_{k-1}^{(j)}\big{)}$
is a function of the probability of object survival $p_{\mathrm{su}}$ and the
state-transition pdf
$f\big{(}\underline{\bm{x}}_{k,1}^{(j)}\big{|}\bm{x}_{k-1}^{(j)}\big{)}$.

The functional form of the single-object state transition model and how it
models the motion and disappearance of objects is presented in [10, Section
VIII-C]. At time $k=0$, the prior augmented states $\bm{y}^{(j)}_{0}$ are
statistically independent across POs $j$. Often no prior information is
available, i.e., $J_{0}=0$. Object birth at time $k$ is modeled by a Poisson
point process with mean $\mu_{\mathrm{b}}$ and arbitrary pdf
$f_{\mathrm{b}}\big{(}\overline{\bm{x}}_{k}\big{)}$.

####  IV-A2 MOU Measurement Model for Multiple Objects

At time $k$, a random number, $\mathsfbr{M}_{k,s}$, of measurements are
generated at the sensor $s$. The measurements are subject to MOU, i.e., each
measurement $\bm{\mathsfbr{z}}^{(m)}_{k,s}$,
$m\in\big{\\{}1,\dots,\mathsfbr{M}_{k,s}\big{\\}}$ can originate from one of
the following three sources, i.e., it is either (i) originated from an object
(represented by a legacy PO) that has already generated a measurement in the
past; (ii) originated from an object (represented by a new PO) that has never
generated a measurement in the past; or (iii) a FP. The object represented by
(new or legacy) PO $j\in\\{1,\dots,$ $\mathsfbr{J}_{k,s}\\}$ generates a
measurement $\bm{\mathsfbr{z}}_{k,s}^{(m)}$ with probability of detection
$p_{\text{d}}$. In case PO with index $j$ generates a measurement with index
$m$, the statistical relationship of a measurement
$\bm{\mathsfbr{z}}_{k,s}^{(m)}$ and a PO state $\bm{\mathsfbr{x}}_{k,s}^{(j)}$
is described by the conditional pdf
$f\big{(}\bm{z}^{(m)}_{k,s}|\bm{x}^{(j)}_{k,s}\big{)}$, which can be derived
based on the measurement model of the sensor. FP measurements are independent
of the object states and modeled by a Poisson point process with mean
$\mu_{\text{fp}}$ and pdf $f_{\text{fp}}\big{(}\bm{z}^{(m)}_{k,s}\big{)}$.

We make use of the well-established data association assumption [3, 4, 5],
i.e., it is assumed that at any time $k$ and sensor $s$, an object can
generate at most one measurement and a measurement can originate from at most
one object. The association between the $\mathsfbr{M}_{k,s}$ measurements and
the $\mathsfbr{J}_{k,s-1}$ legacy POs at time $k$ and sensor $s$ is
represented by an “object-oriented” data association (DA) vector
$\bm{\mathsfbr{a}}_{k,s}=\big{[}\mathsfbr{a}_{k,s}^{(1)}\cdots\hskip
0.85358pt\mathsfbr{a}_{k,s}^{(\mathsfbr{J}_{k,s-1})}\big{]}^{\mathrm{T}}$ with
object-oriented association variables
$\mathsfbr{a}_{k,s}^{(j)}\in\big{\\{}0,1,\dots,\mathsfbr{M}_{k,s}\big{\\}}$.
Every association vector represents an association event that explains the
origin of each measurement. In particular,
$\mathsfbr{a}_{k,s}^{(j)}=m\in\\{1,\dots,\mathsfbr{M}_{k,s}\\}$ if legacy PO
$j$ generates measurement $m$ at sensor $s$ and $\mathsfbr{a}_{k,s}^{(j)}=0$
if legacy PO $j$ is missed by the sensor [3, 10]. Note that only existing
legacy POs can generate measurements, i.e.,
$\underline{\mathsfbr{r}}_{k,s}^{(j)}=0$ implies $\mathsfbr{a}_{k,s}^{(j)}=0$.
MOU for multiple objects leads to a combinatorial number of association events
$\bm{\mathsfbr{a}}_{k,s}$. To obtain a scalable and efficient SPA (see [48, 9,
10] for details), we also introduce the “measurement-oriented” DA vector
$\bm{\mathsfbr{b}}_{k,s}=\big{[}\mathsfbr{b}_{k,s}^{(1)}\cdots\hskip
0.85358pt\mathsfbr{b}_{k,s}^{(\mathsfbr{M}_{k,s})}\big{]}^{\mathrm{T}}$ with
measurement-oriented association variables
$\mathsfbr{b}_{k,s}^{(m)}=j\in\\{0,1,\dots,\mathsfbr{J}_{k,s-1}\\}$. In
particular, $\mathsfbr{b}_{k,s}^{(m)}=j\in\\{1,\dots,\mathsfbr{J}_{k,s-1}\\}$
if measurement $m$ originated from legacy PO $j$, and
$\mathsfbr{b}_{k,s}^{(m)}=0$ if it originated from a new PO or is a FP. Since
only existing new POs can generate a measurement, the fact that
$\overline{\mathsfbr{r}}_{k,s}^{(m)}=0$ and $\mathsfbr{b}_{k,s}^{(m)}=0$
implies that measurement $m$ is a FP.

####  IV-A3 Joint Posterior pdf and Factor Graph

Let us denote by $\bm{\mathsfbr{y}}_{0:k}$, $\bm{\mathsfbr{a}}_{1:k}$,
$\bm{\mathsfbr{b}}_{1:k}$, and $\bm{z}_{1:k}$ all the PO states, object-
oriented association variables, measurement-oriented association variables,
and measurements of all sensors up to time $k$. For example,
$\bm{\mathsfbr{z}}_{1:k}=[\bm{\mathsfbr{z}}^{\mathrm{T}}_{1}\cdots\bm{\mathsfbr{z}}^{\mathrm{T}}_{k}]^{\mathrm{T}}$
with
$\bm{\mathsfbr{z}}_{k}=\big{[}\bm{\mathsfbr{z}}_{k,s}^{(1)\mathrm{T}}\cdots\hskip
0.85358pt\bm{\mathsfbr{z}}_{k,s}^{(\mathsfbr{M}_{k,s})\mathrm{T}}\big{]}^{\mathrm{T}}$.
Using common assumptions [10], the joint posterior pdf of
$\bm{\mathsfbr{y}}_{0:k}$, $\bm{\mathsfbr{a}}_{1:k}$, and
$\bm{\mathsfbr{b}}_{1:k}$ conditioned on observed $\bm{z}_{1:k}$ can be
obtained as

$\displaystyle
f\big{(}\bm{y}_{0:k},\bm{a}_{1:k},\bm{b}_{1:k}\big{|}\bm{z}_{1:k}\big{)}$
$\displaystyle\propto\Bigg{(}\prod^{J_{0}}_{j^{\prime\prime}=1}f\big{(}\bm{y}^{(j^{\prime\prime})}_{0}\big{)}\Bigg{)}\prod^{k}_{k^{\prime}=1}\Bigg{(}\prod^{J_{k^{\prime}-1}}_{j^{\prime}=1}f\big{(}\underline{\bm{y}}^{(j^{\prime})}_{k^{\prime}}\big{|}\bm{y}^{(j^{\prime})}_{k^{\prime}-1}\big{)}\Bigg{)}\prod^{S}_{s=1}$
$\displaystyle\hskip
5.69054pt\times\Bigg{(}\prod^{J_{k^{\prime}\\!,s-1}}_{j=1}q\big{(}\underline{\bm{x}}^{(j)}_{k^{\prime}\\!,s},\underline{r}^{(j)}_{k^{\prime}\\!,s},a^{(j)}_{k^{\prime}\\!,s};\bm{z}_{k^{\prime}\\!,s}\big{)}\prod^{M_{k^{\prime}\\!,s}}_{m^{\prime}=1}\Psi_{j,m^{\prime}}\big{(}a_{k^{\prime}\\!,s}^{(j)},b_{k^{\prime}\\!,s}^{(m^{\prime})}\big{)}\Bigg{)}$
$\displaystyle\hskip
5.69054pt\times\prod^{M_{k^{\prime}\\!,s}}_{m=1}v\big{(}\overline{\bm{x}}^{(m)}_{k^{\prime}\\!,s},\overline{r}^{(m)}_{k^{\prime}\\!,s},b^{(m)}_{k^{\prime}\\!,s};\bm{z}_{k^{\prime}\\!,s}^{(m)}\big{)}.$
(23)

Here, the legacy PO factor
$q\big{(}\underline{\bm{x}}^{(j)}_{k,s},\underline{r}^{(j)}_{k,s},a^{(j)}_{k,s};\bm{z}_{k,s}\big{)}$
is given by

$\displaystyle
q\big{(}\underline{\bm{x}}^{(j)}_{k,s},1,a^{(j)}_{k,s};\bm{z}_{k,s}\big{)}$
$\displaystyle\triangleq\begin{cases}\frac{p_{\text{d}}}{\mu_{\mathrm{fp}}f_{\mathrm{fp}}\big{(}\bm{z}_{k,s}^{(m)}\big{)}}f\big{(}\bm{z}_{k,s}^{(m)}\big{|}\hskip
0.85358pt\underline{\bm{x}}_{k,s}^{(j)}\big{)},&a^{(j)}_{k,s}=m\in\\{1,\dots,M_{k,s}\\}\\\\[5.69054pt]
1-p_{\text{d}}\hskip 0.85358pt,&a^{(j)}_{k,s}=0\end{cases}$

and
$q\big{(}\underline{\bm{x}}^{(j)}_{k,s}\\!,0,a^{(j)}_{k,s};\bm{z}_{k,s}\big{)}\triangleq
1(a^{(j)}_{k,s})$. Furthermore, the new PO factor
$v\big{(}\overline{\bm{x}}^{(m)}_{k,s}\\!,\overline{r}^{(m)}_{k,s},$
$b^{(m)}_{k,s};\bm{z}_{k,s}^{(m)}\big{)}$ reads

$\displaystyle
v\big{(}\overline{\bm{x}}^{(m)}_{k,s}\\!,1,b^{(m)}_{k,s};\bm{z}_{k,s}^{(m)}\big{)}$
$\displaystyle\triangleq\begin{cases}0\hskip
0.85358pt,&b^{(m)}_{k,s}\in\\{1,\dots,J_{k,s-1}\\}\\\\[5.69054pt] {\hskip
0.85358pt\frac{p_{\mathrm{d}}\hskip 0.85358pt\mu_{\mathrm{b}}\hskip
0.85358ptf_{\mathrm{b}}\big{(}\overline{\bm{x}}^{(m)}_{k,s}\big{)}}{\mu_{\mathrm{fp}}f_{\mathrm{fp}}\big{(}\bm{z}_{k,s}^{(m)}\big{)}}}f\big{(}\bm{z}_{k,s}^{(m)}\big{|}\overline{\bm{x}}^{(m)}_{k,s}\big{)}\hskip
0.85358pt,&b^{(m)}_{k,s}\\!=0\end{cases}$

and
$v\big{(}\overline{\bm{x}}^{(m)}_{k,s}\\!,0,b^{(m)}_{k,s};\bm{z}_{k,s}^{(m)}\big{)}\triangleq
f_{\text{D}}\big{(}\overline{\bm{x}}^{(m)}_{k,s}\big{)}$.

Finally, the binary indicator function
$\Psi_{j,m}\big{(}a_{k,s}^{(j)},b_{k,s}^{(m)}\big{)}$ checks association
consistency of a pair of object-oriented and measurement-oriented variables
$\big{(}a_{k,s}^{(j)},b_{k,s}^{(m)}\big{)}$ in that
$\Psi_{j,m}\big{(}a_{k,s}^{(j)},b_{k,s}^{(m)}\big{)}$ is zero if
$a_{k,s}^{(j)}=m,b^{(m)}_{k,s}\neq j$ or $b^{(m)}_{k,s}=j,a_{k,s}^{(j)}\neq m$
and one otherwise (see [48, 9, 10] for details). If and only if all pairs of
object-oriented and measurement-oriented association variables are consistent,
the data association assumption is satisfied. Formulating the MOU measurement
model for multiple objects using both object-oriented and measurement-oriented
association variables makes it possible to significantly reduce computational
complexity and increase the scalability of probabilistic data association [48,
9, 10] by making use of the loopy SPA. A detailed derivation of this joint
posterior is provided in [10, Section VIII-G]. The joint posterior in (23) can
be represented by the factor graph in Fig. 1.

###  IV-B Problem Formulation and Selected Messages of the SPA

At each time step $k\geqslant 1$, we consider the tracking of an unknown
number of objects based on measurements $\bm{z}_{1:k}$. Object detection is
performed by comparing the existence probability
$p\big{(}r_{k}^{(j)}\\!=1\big{|}\bm{z}_{1:k}\big{)}$ with a threshold
$P_{\text{th}}$, i.e., PO $j\in\\{1,\dots,J_{k}\\}$ is declared to exist if
$p\big{(}r_{k}^{(j)}\\!=1\big{|}\bm{z}_{1:k}\big{)}>P_{\text{th}}$. Note that
$p\big{(}r_{k}^{(j)}\\!=1\big{|}\bm{z}_{1:k}\big{)}=\int
f\big{(}\bm{x}_{k}^{(j)},r_{k}^{(j)}\\!=1\big{|}\bm{z}_{1:k}\big{)}\hskip
0.85358pt\mathrm{d}\bm{x}_{k}^{(j)}$. For existent POs, state estimation is
performed by calculating the minimum mean-square error (MMSE) estimate [49] as

$\hat{\bm{x}}_{k}^{(j)}\hskip
0.85358pt\triangleq\int\bm{x}_{k}^{(j)}f\big{(}\bm{x}_{k}^{(j)}\big{|}r_{k}^{(j)}=1,\bm{z}_{1:k}\big{)}\hskip
0.85358pt\mathrm{d}\bm{x}_{k}^{(j)}$ (26)

where
$f\big{(}\bm{x}_{k}^{(j)}\big{|}r_{k}^{(j)}=1,\bm{z}_{1:k}\big{)}=f\big{(}\bm{x}_{k}^{(j)},r_{k}^{(j)}=1\big{|}\bm{z}_{1:k}\big{)}/p\big{(}r_{k}^{(j)}$
$=1\big{|}\bm{z}_{1:k}\big{)}$.

Both object detection and estimation require the marginal posterior pdfs
$f\big{(}\bm{x}_{k}^{(j)},r_{k}^{(j)}\big{|}\bm{z}_{1:k})\triangleq
f\big{(}\bm{y}_{k}^{(j)}\big{|}\bm{z}_{1:k})$, $j\in\\{1,\dots,J_{k}\\}$.
However, calculating
$f\big{(}\bm{x}_{k}^{(j)},r_{k}^{(j)}\big{|}\bm{z}_{1:k}\big{)}$ by direct
marginalization of (23) is infeasible due to the high dimensionality of
$\bm{y}_{1:k}$, $\bm{a}_{1:k}$, and $\bm{b}_{1:k}$. As in [9, 10], we consider
approximate calculation by performing the loopy SPA on the factor graph in
Fig. 1 and passing messages only forward in time. This makes it possible to
efficiently calculate so-called beliefs
$\tilde{f}\big{(}\bm{x}_{k}^{(j)}\\!,r_{k}^{(j)}\big{)}\triangleq\tilde{f}\big{(}\bm{y}_{k}^{(j)}\big{)},\hskip
0.85358ptj\in\\{1,\dots,J_{k}\\}$ which accurately approximate the marginal
posterior pdfs
$f\big{(}\bm{x}_{k}^{(j)}\\!,r_{k}^{(j)}\big{|}\bm{z}_{1:k}\big{)},\hskip
0.85358ptj\in\\{1,\dots,J_{k}\\}$ needed for object detection and estimation.
To keep computational complexity feasible, at the end of each time $k$ with
all sensors processed, a suboptimal pruning step has to be performed. Here,
POs with probability of existence
$p^{(j)}_{k}\\!\triangleq\\!\tilde{p}(r^{(j)}_{k}\\!=\\!1|\bm{z}_{1:n})$ below
a threshold $P_{\text{pr}}$ are removed from the state space.

Figure 1: Factor graph for MOT of an unknown, time-varying number of objects,
corresponding to the propagation of the joint pdf
$f\big{(}\bm{y}_{1:k},\bm{a}_{1:k},\bm{b}_{1:k}\big{|}\bm{z}_{1:k}\big{)}$ in
(23). The part of the factor graph corresponding to one sensor update is
shown. Messages calculated using PFL are depicted in red. These messages are
calculated based on messages depicted in blue. The time index $k$ and sensor
index $s$ are omitted, and the following short notation is used:
$n_{\mathrm{m}}\triangleq m_{k}$, $n_{\mathrm{p}}\triangleq j_{k-1}$,
$\underline{\bm{y}}^{j}\triangleq\underline{\bm{y}}_{k}^{(j)}$,
$\overline{\bm{y}}^{\hskip 0.85358ptm}\triangleq\overline{\bm{y}}_{k}^{(m)}$,
$\bm{a}^{j}\triangleq\bm{a}^{(j)}_{k,s}$,
$\bm{b}^{m}\triangleq\bm{b}^{(m)}_{k,s}$, $f^{j}\triangleq
f\big{(}\underline{\bm{y}}^{(j)}_{k,1}\big{|}\bm{y}^{(j)}_{k-1}\big{)}$ (for
$s=1$),
$f^{j}\triangleq\delta\big{(}\underline{\bm{y}}^{(j)}_{k,s}-\bm{y}^{(j)}_{k,s-1}\big{)}$
(for $s>1$), $q^{j}\triangleq
q\big{(}\underline{\bm{x}}^{(j)}_{k,s},\underline{r}^{(j)}_{k,s},a_{k,s}^{(j)};\bm{z}_{k,s}\big{)}$,
$v^{m}\triangleq
v\big{(}\overline{\bm{x}}^{(m)}_{k,s},\overline{r}^{(m)}_{k,s},b_{k,s}^{(m)};\bm{z}^{(m)}_{k,s}\big{)}$,
$\Psi^{j,m}\triangleq\Psi_{j,m}\big{(}a^{(j)}_{k,s},b^{(m)}_{k,s}\big{)}$,
$\gamma_{j}\triangleq\gamma_{k,s}^{(j)}\big{(}\underline{\bm{y}}^{(j)}_{k,s}\big{)}$,
$\beta_{j}\triangleq\beta^{(j)}_{k,s}\big{(}a^{(j)}_{k,s}\big{)}$,
$\xi_{m}\triangleq\xi^{(m)}_{k,s}\big{(}b^{(m)}_{k,s}\big{)}$,
$\varsigma_{m}\triangleq\varsigma^{(m)}_{k,s}\big{(}\overline{\bm{y}}^{(m)}_{k,s}\big{)}$,
$\alpha_{j}\triangleq\alpha^{(j)}_{k,s}\big{(}\underline{\bm{y}}^{(j)}_{k,s}\big{)}$,
$\kappa_{j}\triangleq\kappa^{(j)}_{k,s}\big{(}a^{(j)}_{k,s}\big{)}$, and
$\iota_{m}\triangleq\iota_{k,s}^{(m)}\big{(}b^{(m)}_{k,s}\big{)}$.

Next, we present the SPA messages that will later be calculated based on PFL.
We will limit our discussion to messages and beliefs related to legacy PO
states. Messages and beliefs related to new PO states are obtained by
performing similar steps. (A complete description of message passing for MOT
is provided in [10, Section IX-A].)

We consider sequential sensor processing, where the SPA incorporates sensor
measurements sequentially and in an arbitrary processing order at each time
step. The part of the factor graph that represents the processing of the
measurements of one sensor at one time step is shown in Fig. 1. At the
processing step related to time $k$ and sensor $s$, the “prior messages” of
legacy PO states are denoted as
$\alpha_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}\big{)}=\alpha_{k,s}^{(j)}\big{(}\underline{\bm{y}}_{k,s}^{(j)}\big{)}$.
At time $k$ and sensor $s=1$, these message are computed by a prediction step
[10, Section IX-A1], i.e.,

$\displaystyle\alpha_{k,1}^{(j)}\big{(}\underline{\bm{x}}_{k,1}^{(j)},\underline{r}_{k,1}^{(j)}\big{)}=\sum_{r_{k-1}^{(j)}\in\\{0,1\\}}\int
f\big{(}\underline{\bm{x}}_{k,1}^{(j)},\underline{r}_{k,1}^{(j)}\big{|}\bm{x}_{k-1}^{(j)}\\!,r_{k-1}^{(j)}\big{)}$
$\displaystyle\hskip
111.81934pt\times\tilde{f}\big{(}\bm{x}_{k-1}^{(j)},r_{k-1}^{(j)}\big{)}\mathrm{d}\bm{x}_{k-1}^{(j)}$
(27)

that makes use of the state-transition function
$f\big{(}\underline{\bm{x}}_{k,1}^{(j)},\underline{r}_{k,1}^{(j)}\big{|}$
$\bm{x}_{k-1}^{(j)},r_{k-1}^{(j)}\big{)}\triangleq
f\big{(}\underline{\bm{y}}_{k,1}^{(j)},\big{|}\bm{y}_{k-1}^{(j)}\big{)}$. At
the time $k$ and sensor $s>1$, the “prior message” is the same as the belief
after the previous sensor update, i.e.,
$\alpha_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}\big{)}\triangleq\tilde{f}\big{(}\bm{x}_{k,s-1}^{(j)}\\!,r_{k,s-1}^{(j)}\big{)}$.
For future reference, we also introduce
$\underline{p}^{(j)}_{k,s}=\int\alpha_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}=1\big{)}\mathrm{d}\underline{\bm{x}}_{k,s}^{(j)}$
as the predicted probability of existence for each legacy PO. Note that
$\underline{p}^{(j)}_{k,s}+\int\alpha_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}=0\big{)}\mathrm{d}\underline{\bm{x}}_{k,s}^{(j)}=1$.

After the prior messages have been computed, a “measurement evaluation” step
for each legacy and each new PO is performed. Here, the SPA messages that are
passed from the factor nodes
$q(\underline{\bm{x}}^{(j)}_{k,s},\underline{r}^{(j)}_{k,s},a^{(j)}_{k,s};\bm{z}_{k,s})$
in (LABEL:eq:measurementFactorLegacy) and
$v\big{(}\overline{\bm{x}}^{(m)}_{k,s}\\!,\overline{r}^{(m)}_{k,s},b^{(m)}_{k,s};\bm{z}_{k,s}^{(m)}\big{)}$
in (LABEL:eq:measurementFactorNew) to the adjacent variables nodes
$a^{(j)}_{k,s}$ and $b^{(m)}_{k,s}$, respectively, are computed. For legacy
POs, these messages are given by (see [10, Section IX])

$\displaystyle\beta_{k,s}^{(j)}\big{(}a_{k,s}^{(j)}\big{)}=\int
q\big{(}\underline{\bm{x}}^{(j)}_{k,s},1,a^{(j)}_{k,s};\bm{z}_{k,s}\big{)}\alpha_{k,s}^{(j)}\\!\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}\mathrm{d}\underline{\bm{x}}_{k,s}^{(j)}$
$\displaystyle\hskip 141.69473pt+1(a)\hskip
0.85358pt\big{(}1-\underline{p}^{(j)}_{k,s}\big{)}\hskip 0.85358pt.$ (28)

For new POs, the corresponding messages are denoted as
$\xi_{k,s}^{(m)}\big{(}b_{k,s}^{(m)}\big{)}$, $m\in\\{1,\dots,M_{k,s}\\}$ and
are calculated similarly (see [10, Section IX]).

Next, probabilistic DA is performed by means of iterative SPA message passing
with input messages $\beta_{k,s}^{(j)}\big{(}a_{k,s}^{(j)}\big{)}$,
$j\in\\{1,\dots,J_{k,s-1}\\}$ and
$\xi_{k,s}^{(m)}\big{(}b_{k,s}^{(m)}\big{)}$, $m\in\\{1,\dots,M_{k,s}\\}$ (see
[10, Section IX-A3] for details). After convergence, corresponding output
messages $\kappa_{k,s}^{(j)}\big{(}a_{k,s}^{(j)}\big{)}$,
$j\in\\{1,\dots,J_{k,s-1}\\}$ and
$\iota_{k,s}^{(m)}\big{(}b^{(m)}_{k,s}\big{)}$, $m\in\\{1,\dots,M_{k,s}\\}$
are available for legacy POs and new POs, respectively. Probabilistic DA is
followed by a “measurement update” step. Here, for legacy POs, messages
$\gamma_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}\big{)}$
passed from
$q\big{(}\underline{\bm{x}}^{(j)}_{k,s},\underline{r}^{(j)}_{k,s},a^{(j)}_{k,s};\bm{z}_{k,s}\big{)}$
to $\underline{\bm{y}}_{k,s}^{(j)}$ are calculated as

$\gamma_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}=\sum_{a_{k,s}^{(j)}=0}^{M_{k,s}}q\big{(}\underline{\bm{x}}^{(j)}_{k,s},1,a^{(j)}_{k,s};\bm{z}_{k,s}\big{)}\kappa_{k,s}^{(j)}\big{(}a_{k,s}^{(j)}\big{)}\vspace{0mm}$
(29)

and as
$\gamma_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},0\big{)}=\gamma_{k,s}^{(j)}=\kappa_{k,s}^{(j)}\big{(}0\big{)}$.
Measurement update for new POs is performed by following similar steps [10,
Section IX-A].

Finally, beliefs are calculated to approximate the posterior pdfs of POs. For
legacy POs, beliefs
$\tilde{f}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}\big{)}$
approximating
$f\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}\big{|}\bm{z}_{1:k}\big{)}$
are obtained as

$\tilde{f}\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}=\frac{1}{\underline{C}_{k,s}^{(j)}}\alpha_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}\gamma_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}$
(30)

and as
$\tilde{f}\big{(}\underline{\bm{x}}_{k,s}^{(j)},0\big{)}=\underline{f}_{k,s}^{(j)}\hskip
0.85358ptf_{\text{D}}\big{(}\underline{\bm{x}}^{(j)}_{k,s}\big{)}$ with
$\underline{f}_{k,s}^{(j)}=\big{(}1-\underline{p}^{(j)}_{k,s}\big{)}\hskip
0.85358pt\gamma_{k,s}^{(j)}/\underline{C}_{k,s}^{(j)}$. The constant
$\underline{C}_{k,s}^{(j)}$ is given by
$\underline{C}_{k,s}^{(j)}\triangleq\int\alpha_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}\hskip
0.85358pt\gamma_{k,s}^{(j)}\big{(}\underline{\bm{x}}_{k,s}^{(j)},1\big{)}\hskip
0.85358pt\mathrm{d}\underline{\bm{x}}_{k,s}^{(j)}+\big{(}1-\underline{p}^{(j)}_{k,s}\big{)}\hskip
0.85358pt\gamma_{k,s}^{(j)}$.

Calculating the beliefs
$\tilde{f}\big{(}\overline{\bm{x}}_{k,s}^{(m)},\overline{r}_{k,s}^{(m)}\big{)}$,
$m\in\\{1,\dots,M_{k,s}\\}$ for new POs is performed by following similar
steps [10, Section IX-A]. Note that this calculation of new POs involves the
messages $\iota_{k,s}^{(m)}\big{(}b^{(m)}_{k,s}\big{)}$ and
$\varsigma^{(m)}_{k,s}\big{(}\overline{\bm{y}}^{(m)}_{k,s}\big{)}$,
$m\in\\{1,\dots,M_{k,s}\\}$ also shown in Fig. 1. The resulting beliefs for
legacy and new POs, $\tilde{f}\big{(}\bm{x}_{k,s}^{(j)},r_{k,s}^{(j)}\big{)}$,
$j\in\\{1,\dots,J_{k,s}\\}$ are used as input for the update step related to
the measurements of sensor $s+1$ as discussed above. (Note that
$p^{(j)}_{k,s}=\int\tilde{f}\big{(}\bm{x}_{k,s}^{(j)},r_{k,s}^{(j)}=1\big{)}\mathrm{d}\bm{x}_{k,s}^{(j)}$.)
When the measurements of the last sensor in the sequence have been processed,
i.e., $s=S$, the resulting beliefs are used in the prediction steps (27) of
the next time step $k+1$.

##  V Graph-Based Multisensor MOT
with Invertible PFL

In nonlinear MOT scenarios, calculation of
$\beta_{k,s}^{(j)}\big{(}a_{k,s}^{(j)}\big{)}$ in (28) and
$\tilde{f}\big{(}\underline{\bm{x}}_{k,s}^{(j)},\underline{r}_{k,s}^{(j)}\big{)}$
in (30) related to legacy PO states as well as their counterparts
$\xi_{k,s}^{(m)}\big{(}b_{k,s}^{(m)}\big{)}$ and
$\tilde{f}\big{(}\overline{\bm{x}}_{k,s}^{(m)},\overline{r}_{k,s}^{(m)}\big{)}$,
related to new PO states cannot be performed in closed form. We propose a
particle-based implementation where a proposal pdf is established using
invertible PFL as introduced in Section II-B. This makes it possible to
implement multisensor MOT with high dimensional states and nonlinear
measurement models. A single time step of the proposed particle-based
implementation is discussed next. At first, we assume a single Gaussian kernel
as the prior knowledge for each PO. An extension to GMM is also presented. In
what follows, we consider a single time step, remove the time index $k$, and
use the index $-$ short for $k-1$.

###  V-A Prediction

It is assumed that the beliefs of legacy POs at time $k-1$ are represented by
a single Gaussian distribution, i.e.,
$\tilde{f}\big{(}\bm{x}_{-}^{(j)},r_{-}^{(j)}=1\big{)}=p^{(j)}_{-}\hskip
0.85358pt\hskip
0.85358pt\mathcal{N}(\bm{x}_{-}^{(j)};\bm{x}_{-}^{(j)\ast},\bm{P}_{-}^{(j)})$,
$j\\!\in\\!\\{1,$ $\dots,J_{-}\\}$. In MOT problems, the state transition
function underlying the state transition model
$f\big{(}\underline{\bm{x}}_{1}^{(j)}\big{|}\bm{x}_{-}^{(j)}\big{)}$ discussed
in Section IV-A is typically linear with additive Gaussian noise, i.e.,

$\underline{\bm{\mathsfbr{x}}}^{(j)}_{1}=\bm{G}\bm{\mathsfbr{x}}^{(j)}_{-}+\bm{\mathsfbr{u}}^{(j)}$
(31)

where $\bm{G}$ is the state transition matrix and $\bm{\mathsfbr{u}}^{(j)}$ is
an additive Gaussian noise vector with mean $\bm{u}^{\ast}$ and covariance
matrix $\bm{P}_{\bm{\mathsfbr{u}}}$. Consequently, the messages computed in
the prediction step are also represented by a Gaussian distribution, i.e.,
$\alpha_{1}^{(j)}\big{(}\underline{\bm{x}}_{1}^{(j)},\underline{r}_{1}^{(j)}\big{)}=\underline{p}^{(j)}_{1}\hskip
0.85358pt\mathcal{N}(\underline{\bm{x}}_{1}^{(j)};\underline{\bm{x}}_{1}^{(j)\ast},\underline{\bm{P}}_{1}^{(j)})$,
$j\\!\in\\!\\{1,\dots,J_{1}\\}$ with mean, covariance matrix, and existence
probability given by
$\underline{\bm{x}}_{1}^{\ast(j)}=\bm{G}\bm{x}^{\ast(j)}_{1}+\bm{u}^{\ast}$,
$\underline{\bm{P}}_{1}^{(j)}=\bm{G}\bm{P}_{-}^{(j)}\bm{G}^{\mathrm{T}}+\bm{P}_{\bm{\mathsfbr{u}}}$,
and $\tilde{\underline{p}}^{(j)}_{1}=p_{\mathrm{su}}\hskip
0.85358ptp^{(j)}_{-}$, respectively. If the state transition function is not
linear with additive Gaussian noise, for each $j\\!\in\\!\\{1,\dots,J_{-}\\}$,
$N_{\mathrm{p}}$ particles are drawn from
$\mathcal{N}(\bm{x}_{-}^{(j)};\bm{x}_{-}^{(j)\ast},\bm{P}_{-}^{(j)})$, the
prediction step of a conventional particle filter is performed [24], and a
predicted Gaussian representation
$\mathcal{N}(\underline{\bm{x}}_{1}^{(j)};\underline{\bm{x}}_{1}^{(j)\ast},\underline{\bm{P}}_{1}^{(j)})$
is computed from the resulting particles using Algorithm 3.

###  V-B Measurement Evaluation

The following steps are performed sequentially for each sensor $s=1,\dots,S$.
First, a particle representation
$\big{\\{}\big{(}\underline{\bm{x}}^{(i,j)}_{0,s},\underline{\omega}^{(i,j)}_{0,s}\big{)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$
is obtained by drawing particles $\underline{\bm{x}}^{(i,j)}_{0,s}$,
$i\in\\{1,\dots,N_{\mathrm{p}}\\}$ from
$\mathcal{N}(\underline{\bm{x}}_{s}^{(j)};\underline{\bm{x}}_{s}^{(j)\ast},\underline{\bm{P}}_{s}^{(j)})$
and setting the corresponding weights to
$\underline{\omega}^{(i,j)}_{0,s}=\underline{p}^{(j)}_{s}/N_{\mathrm{p}}$.
Next, we compute an extended set
$\big{\\{}\big{\\{}\big{(}\underline{\bm{x}}^{(i,a,j)}_{s},\underline{\omega}^{(i,a,j)}_{s}\big{)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\big{\\}}_{a=0}^{M_{s}}$,
that consists of $N_{\mathrm{p}}$ particles and weights for each value of
$a_{s}^{(j)}\in\\{0,\dots,M_{s}\\}$ and $j\\!\in\\!\\{1,\dots,J_{s-1}\\}$.
(Note that measurement gating [3] can be employed to reduce the number of
measurements used for PFL.) For $a_{s}^{(j)}=0$, we perform no flow, i.e., we
set
$\big{\\{}\big{(}\underline{\bm{x}}^{(i,0,j)}_{s},\underline{\omega}^{(i,0,j)}_{s}\big{)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\\!=\\!\big{\\{}\big{(}\underline{\bm{x}}^{(i,j)}_{0,s},\underline{\omega}^{(i,j)}_{0,s}\big{)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$.
For $a_{s}^{(j)}=m\\!\in\\!\\{1,\dots,M_{s}\\}$, the PFL
$\underline{\bm{\mathsfbr{x}}}^{(j)}_{0,s}\xrightarrow[]{}\bm{z}_{s}^{(m)}\xrightarrow[]{}\underline{\bm{\mathsfbr{x}}}_{s}^{(j,m)}$
is applied to obtain new particles
$\big{\\{}\underline{\bm{x}}^{(i,m,j)}_{s}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$ by
migrating the particles
$\big{\\{}\underline{\bm{x}}^{(i,m,j)}_{0,s}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$.
By making use of the invertible PFL principle (cf. (11)) [35], the weights
$\underline{\omega}_{s}^{(i,m,j)}$ corresponding to the migrated particles
$\underline{\bm{x}}^{(i,m,j)}_{s}$ are obtained as

$\underline{\omega}_{s}^{(i,m,j)}=\frac{\mathcal{N}(\underline{\bm{x}}_{s}^{(i,m,j)};\underline{\bm{x}}_{s}^{(j)\ast},\underline{\bm{P}}_{s}^{(j)})\hskip
0.85358pt\underline{\theta}^{(j)}_{m,s}\\!}{\mathcal{N}(\underline{\bm{x}}_{0,s}^{(i,j)};\underline{\bm{x}}_{s}^{(j)\ast},\underline{\bm{P}}_{s}^{(j)})}\hskip
0.85358pt\underline{\omega}_{0,s}^{(i,j)},i\in\\{1,\dots,N_{\mathrm{s}}\\}\vspace{.5mm}$

with mapping factor $\underline{\theta}^{(j)}_{m,s}$ (cf. (12)). Note that the
sets of weighted particles
$\big{\\{}\underline{\bm{x}}_{s}^{(i,m,j)},\underline{\omega}_{s}^{(i,m,j)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$,
$m\in\\{1,\dots,$ $M_{s}\\}$, despite all being based on a different proposal
pdf
$q_{\mathrm{PFL}}\big{(}\underline{\bm{x}}_{s}^{(j)}|\bm{z}_{s}^{(m)}\big{)}$,
$m\in\\{1,\dots,$ $M_{s}\\}$, still represent
$\alpha_{s}^{(j)}\big{(}\underline{\bm{x}}_{s}^{(j)},1\big{)}$. The result of
this particle migration along the flow defined by a measurement
$\bm{z}_{s}^{(m)}$, $m\in\\{1,\dots,M_{s}\\}$, is that the particles are now
at locations where the evaluation of the corresponding likelihood function
$f\big{(}\bm{z}_{s}^{(m)}\big{|}\hskip
0.85358pt\underline{\bm{x}}_{s}^{(j)}\big{)}$ will produce a significant
particle weight. This makes it possible to avoid particle degeneracy and thus
approximate message-passing operations accurately even if the dimension of the
state is high [9].

The measurement evaluation step can now be performed on weighted particles
$\big{\\{}\big{\\{}\big{(}\underline{\bm{x}}_{s}^{(i,a,j)},\underline{\omega}_{s}^{(i,a,j)}\big{)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\big{\\}}_{a=0}^{M_{s}}$
by calculating an approximation $\tilde{\beta}_{s}^{(j)}(a)$ of the messages
$\beta_{s}^{(j)}(a)$ in (28) for all $j\\!\in\\!\\{1,\dots,J_{s-1}\\}$,
$a\\!\in\\!\\{0,\dots,M_{s}\\}$ as

$\displaystyle\tilde{\beta}_{s}^{(j)}\big{(}a_{s}^{(j)}=a\big{)}$
$\displaystyle=\sum_{i=1}^{N_{\mathrm{p}}}q\big{(}\underline{\bm{x}}^{(i,a,j)}_{s}\\!,1,a;\bm{z}_{s}\big{)}\hskip
0.85358pt\underline{\omega}_{s}^{(i,a,j)}$ $\displaystyle\hskip
65.44133pt+1(a)\hskip
0.85358pt\big{(}1-\tilde{\underline{p}}^{(j)}_{s}\big{)}.$

For the computation of a particle representation of new PO states, we first
draw particles $\overline{\bm{x}}^{(i)}_{0,s}$,
$i\in\\{1,\dots,N_{\mathrm{p}}\\}$ from
$f_{\mathrm{b}}\big{(}\overline{\bm{x}}\big{)}$ as introduced in Section
IV-A1. Next, for each new PO $j=J_{s-1}+m$, $m\\!\in\\!\\{1,\dots,M_{s}\\}$,
new particles
$\big{\\{}\overline{\bm{x}}^{(m,i)}_{s}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$ are
obtained from
$\big{\\{}\overline{\bm{x}}^{(i)}_{0,s}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$ by
performing the PFL
$\overline{\bm{\mathsfbr{x}}}_{0,s}\xrightarrow[]{}\bm{z}_{s}^{(m)}\xrightarrow[]{}\overline{\bm{\mathsfbr{x}}}_{s}^{(m)}$.
Note that this flow relies on the mean $\overline{\bm{x}}_{\text{b}}$ and
covariance matrix $\overline{\bm{P}}_{\text{b}}$ of
$f_{\mathrm{b}}\big{(}\overline{\bm{x}}\big{)}$. For all particles
$\big{\\{}\overline{\bm{x}}^{(m,i)}_{s}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$, the
weights are set as $\overline{w}^{(m,i)}_{s}=1/N_{\mathrm{p}}$. Finally, for
each $m\\!\in\\!\\{1,\dots,M_{s}\\}$ approximate messages
$\tilde{\xi}_{s}^{(m)}\big{(}b_{s}^{(m)}\big{)}$ are calculated from
$\big{\\{}\overline{\bm{x}}^{(m,i)}_{s},\overline{w}^{(m,i)}_{s}\big{\\}}_{i=1}^{N_{\mathrm{p}}}$
by performing the same steps as described above for the calculation of
$\tilde{\beta}_{s}^{(j)}\big{(}a_{s}^{(j)}\big{)}$. These messages are used as
an input for the iterative SPA for data association [48, 9, 10] performed next
(see [10, Sec. VI] for details).

###  V-C Measurement Update and Belief Calculation

After the iterative loopy SPA for data association has been converged, the
messages $\tilde{\kappa}_{s}^{(j)}\big{(}a_{s}^{(j)}\big{)}$,
$j\\!\in\\!\\{1,\dots,J_{s-1}\\}$ and
$\tilde{\iota}_{s}^{(m)}\big{(}b_{s}^{(m)}\big{)}$,
$m\\!\in\\!\\{1,\dots,M_{s}\\}$ are available. These messages are used to
obtain an approximation
$\tilde{\gamma}_{s}^{(j)}\big{(}\underline{\bm{x}}_{s}^{(j)},1\big{)}$ of the
messages $\gamma_{s}^{(j)}\big{(}\underline{\bm{x}}_{s}^{(j)}\\!,1\big{)}$,
$j\\!\in\\!\\{1,\dots,J_{s-1}\\}$ in (29) as well as an approximation
$\tilde{\varsigma}_{s}^{(m)}\big{(}\overline{\bm{x}}_{s}^{(m)}\\!,1\big{)}$ of
the messages
$\varsigma_{s}^{(m)}\big{(}\overline{\bm{x}}_{s}^{(m)}\\!,1\big{)}$,
$m\\!\in\\!\\{1,\dots,M_{s}\\}$ in [10, Section IX].

Beliefs approximating the posterior pdf of POs are now computed by means of
importance sampling. As in Section III-C, we use the marginal association
probabilities $p(a^{(j)}_{s}|\bm{z}_{s})$ to weight the proposal pdfs
$q_{\mathrm{PFL}}\big{(}\underline{\bm{x}}_{s}^{(j)}|\bm{z}_{s}^{(m)}\big{)}$
related to PFL based on measurements $\bm{z}_{s}^{(m)}$,
$m\in\\{1,\dots,M\\}$. Note that in MOT scenarios, accurate approximations of
$p(a^{(j)}_{s}|\bm{z}_{s})$ can be obtained as
$\tilde{p}(a^{(j)}_{s}|\bm{z}_{s})\propto\tilde{\beta}_{s}^{(j)}\big{(}a_{s}^{(j)}\big{)}\tilde{\kappa}_{s}^{(j)}\big{(}a_{s}^{(j)}\big{)}$
(see [10, Section IX] for details). Consequently, we obtain a new set of
reindex particles and weights $\big{\\{}\big{(}\hskip
0.85358pt\underline{\omega}_{s}^{(l,j)},\underline{\bm{x}}_{s}^{(l,j)}$
$\big{)}\big{\\}}^{L_{s}}_{l=1}$ from $\big{\\{}\big{(}\hskip
0.85358pt\underline{\omega}^{(i,a,j)},$
$\underline{\bm{x}}^{(i,a,j)}\big{)}\big{\\}}^{N_{\mathrm{p}}}_{i=1}$,
$a\in\\{0,\dots,M_{s}\\}$ by using $l=i(a+1)$,
$\underline{\omega}_{s}^{(l,j)}=\underline{\omega}^{(i,a,j)}/\tilde{p}(a^{(j)}_{s}=a|\bm{z}_{s})$,
and $L_{s}=N_{\mathrm{p}}\hskip 0.85358pt(M_{s}+1)$.

Next, based on (30), we update the particle weights of the legacy POs
$j\\!\in\\!\\{1,\dots,J_{s-1}\\}$ by first computing

$\underline{w}_{s}^{\prime(l,j)}=\tilde{\gamma}_{s}^{(j)}\\!\big{(}\underline{\bm{x}}_{s}^{(l,j)},1\big{)}\hskip
0.85358pt\underline{\omega}_{s}^{(l,j)},\quad
l\in\\{1,\dots,L_{s}\\}\vspace{.5mm}$

and then calculating normalized weights as

$\displaystyle\underline{w}_{s}^{(l,j)}=\frac{\underline{w}_{s}^{\prime(l,j)}}{\sum_{l=1}^{L_{s}}\underline{w}_{s}^{\prime(l,j)}+\big{(}1-\tilde{\underline{p}}^{(j)}_{s}\big{)}\tilde{\gamma}_{s}^{(j)}},\quad
l\in\\{1,\dots,L_{s}\\}.$

Note that denominator of (LABEL:eq:weightNormLegacy) is a particle-based
approximation of $\underline{C}_{s}^{(j)}$ in (30). The resulting particles
and weights
$\big{\\{}\big{(}\underline{\bm{x}}_{s}^{(l,j)},\underline{w}_{s}^{(l,j)}\big{)}\big{\\}}_{l=1}^{L_{s}}$
represent the belief $\tilde{f}\big{(}\underline{\bm{x}}_{s}^{(j)},1\big{)}$
of legacy PO $j\in\\{1,\dots,\underline{J}_{s}\\}$.

For legacy POs $j\in\\{1,\dots,J_{s-1}\\}$, the particles and weights
$\big{\\{}\big{(}\underline{\bm{x}}_{s}^{(l,j)},\underline{w}_{s}^{(l,j)}\big{)}\big{\\}}_{l=1}^{L_{s}}$
can be used to calculate an approximation of the existence probability as
$\tilde{p}_{s}^{(j)}=\sum_{l=1}^{L_{s}}\underline{w}_{s}^{(l,j)}$. A Gaussian
representation of the belief
$\tilde{f}\big{(}\bm{x}_{s}^{(j)},r_{s}^{(j)}\big{)}\triangleq\tilde{f}\big{(}\underline{\bm{x}}_{s}^{(j)},\underline{r}_{s}^{(j)}\big{)}$
is furthermore obtained by applying Algorithm 3, which calculates a mean
$\bm{x}_{s}^{(j)\ast}$ and a covariance matrix $\bm{P}_{s}^{(j)}$ from
$\big{\\{}\big{(}\underline{\bm{x}}_{s}^{(l,j)},\underline{w}_{s}^{(l,j)}/\tilde{p}_{s}^{(j)}\big{)}\big{\\}}_{l=1}^{L_{s}}$.

For new POs $j\in\\{J_{s-1}+1,\dots,J_{s}\\}$, approximate existence
probabilities $\tilde{p}_{s}^{(j)}$ and a Gaussian representation of the
beliefs
$\tilde{f}\big{(}\bm{x}_{s}^{(j)},r_{s}^{(j)}\big{)}\triangleq\tilde{f}\big{(}\overline{\bm{x}}_{s}^{(m)},\overline{r}_{s}^{(m)}\big{)}$,
for $m=j-J_{s-1}$ are calculated by performing similar steps as discussed
above for legacy POs. To avoid that the number of POs grows with time, a
pruning step removes POs whose approximate existence probabilities
$\tilde{p}_{s}^{(j)}$ are below a threshold $P_{\text{pr}}$. Existence
probabilities and Gaussian representations of the beliefs related to PO that
have not been pruned are then used as input for processing measurements of the
next sensor $s+1$ or, in case $s=S$, for processing at the next time step.

For POs that have been declared to exist after the last sensor update, i.e.,
$\tilde{p}_{S}^{(j)}>P_{\text{th}}$, an approximate MMSE state estimate (cf.
(26)) is directly given by the mean of the Gaussian representation, i.e.,
$\hat{\bm{x}}^{(j)}\approx\bm{x}_{S}^{(j)\ast}$.

###  V-D The Proposed Multisensor MOT Method

In multisensor MOT problems with nonlinear measurement models, object beliefs
are non-Gaussian and potentially multimodal. Here, graph-based multisensor MOT
with invertible PFL has to be combined with a GMM discussed in Section III-A.
Pseudocode for a single time step of the resulting multisensor MOT method is
provided in Algorithm 11. Algorithm 11 relies on the single sensor update step
provided in Algorithm 9 which, in turn, relies on the introduction of new POs
presented in Algorithm 10, measurement evaluation presented in Algorithm 7 and
measurement update presented in Algorithm 8. Note that at time $k=0$,
Algorithm 11 is typically initialized by setting $J_{-}=0$. However, if prior
information is available, it can be incorporated in the form of the set
$\Big{\\{}\big{\\{}\bm{x}_{-}^{\ast(h,j)},\bm{P}_{-}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{-}^{(j)}\Big{\\}}_{j=1}^{J_{-}}$.
Note that in a GMM, an approximate MMSE estimate can be obtained as
$\hat{\bm{x}}^{(j)}\approx\frac{1}{N_{\mathrm{k}}}\sum^{N_{\mathrm{k}}}_{h=1}\bm{x}_{S}^{(h,j)\ast}$.

1
$\Big{[}\Big{\\{}\big{\\{}{\bm{x}}^{\ast(h,j)}_{+},\bm{P}_{+}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{+}^{(j)}\Big{\\}}_{j=1}^{J_{s}}\Big{]}=\hskip
15.36446pt\text{SingleSensorUpdate}\Big{(}\Big{\\{}\big{\\{}\underline{\bm{x}}^{\ast(h,j)},\underline{\bm{P}}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},\underline{p}^{(j)}\Big{\\}}_{j=1}^{J_{s-1}},\bm{z}_{s}\Big{)}$

2

3$\Big{[}\Big{\\{}\big{\\{}\overline{\bm{x}}_{+}^{\ast(h,m)},\overline{\bm{P}}_{+}^{(h,m)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\xi^{(m)}\Big{\\}}^{M_{s}}_{m=1}\Big{]}=\text{NewObjects}\big{(}\bm{z}_{s}\big{)}$

[.6mm] // see Alg. 10

4

5for  _$j=1:J_{s-1}$_ do

6

7
$\Big{[}\Big{\\{}\Big{\\{}\big{\\{}\underline{\bm{x}}^{(i,h,a,j)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a,j)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{(a,j)}\Big{\\}}^{M_{s}}_{a=0}\hskip
0.56905pt\Big{]}\vspace{1mm}=\text{Evaluation}\Big{(}\big{\\{}\underline{\bm{x}}^{\ast(h,j)},\underline{\bm{P}}^{(h,j)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\underline{p}^{(j)},\bm{z}_{s}\Big{)}$

[.5mm] // see Alg. 7

8

9 end for

10

11$\Big{[}\big{\\{}\big{\\{}\kappa^{(a,j)}\big{\\}}^{M_{s}}_{a=0}\big{\\}}^{J_{s-1}}_{j=1},\big{\\{}\iota^{(a)}\big{\\}}^{M_{s}}_{a=0}\vspace{0mm}\Big{]}=\vspace{.5mm}$

12 $\hskip
22.76219pt\text{DataAssociation}\Big{(}\big{\\{}\big{\\{}\beta^{(a,j)}\big{\\}}^{M_{s}}_{a=0}\big{\\}}^{J_{s-1}}_{j=1},\big{\\{}\xi^{(a)}\big{\\}}^{M_{s}}_{a=0}\Big{)}$

[1mm] // see [10, Sec. VI]

13

14Compute
$\overline{p}_{+}^{(m)}=\frac{(\xi^{(m)}-1)\iota^{(m)}}{(\xi^{(m)}-1)\iota^{(m)}+1}$,
$m\in\\{1,\dots,M_{s}\\}$

15[2mm]

16for  _$j=1:J_{s-1}$_ do

17 Compute $\beta^{\prime(a,j)}=\beta^{(a,j)}\kappa^{(a,j)}$,
$a\in\\{0,\dots,M_{s}\\}$

18
$\Big{[}\big{\\{}\underline{\bm{x}}_{+}^{\ast(h,j)},\underline{\bm{P}}_{+}^{(h,j)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\underline{p_{+}}^{(j)}\Big{]}\vspace{.7mm}=$

19
$\text{Update}\Big{(}\Big{\\{}\Big{\\{}\big{\\{}\underline{\bm{x}}^{(i,h,a,j)}\big{\\}}^{N_{\mathrm{p}}}_{i=1},\theta^{(h,a,j)}\Big{\\}}^{N_{\mathrm{k}}}_{h=1},\beta^{\prime(a,j)}\Big{\\}}^{M}_{a=0},\underline{p}^{(j)}\Big{)}$

[2mm] // see Alg. 8

20

21 end for

22 For $\mathpzc{j}=1,\dots,J_{s-1}$, reindex legacy object state information
according to
$\big{\\{}{\bm{x}}^{\ast(h,\mathpzc{j})}_{+},\bm{P}_{+}^{(h,\mathpzc{j})}\big{\\}}_{h=1}^{N_{\mathrm{k}}}=\big{\\{}{\underline{\bm{x}}}^{\ast(h,\mathpzc{j})}_{+},\underline{\bm{P}}_{+}^{(h,\mathpzc{j})}\big{\\}}_{h=1}^{N_{\mathrm{k}}}$
and $p_{+}^{(\hskip 0.42677pt\mathpzc{j})}=\underline{p}_{+}^{(\mathpzc{j})}$.

23

24For $\mathpzc{j}=J_{s-1}+1,\dots J_{s}$, reindex new object state
information following
$\big{\\{}{\bm{x}}^{\ast(h,\mathpzc{j})}_{+},\bm{P}_{+}^{(h,\mathpzc{j})}\big{\\}}_{h=1}^{N_{\mathrm{k}}}=\big{\\{}{\overline{\bm{x}}}^{\ast(h,m)}_{+},\overline{\bm{P}}_{+}^{(h,m)}\big{\\}}_{h=1}^{N_{\mathrm{k}}}$
and $p_{+}^{(\hskip 0.42677pt\mathpzc{j})}=\overline{p}_{+}^{(m)}$ using
$m=\mathpzc{j}-J_{s-1}$ and $J_{s}=J_{s-1}+M_{s}$.

25

Output:
$\Big{\\{}\big{\\{}{\bm{x}}^{\ast(h,\mathpzc{j})}_{+},\bm{P}_{+}^{(h,\mathpzc{j})}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{+}^{(\hskip
0.42677pt\mathpzc{j})}\Big{\\}}_{\mathpzc{j}=1}^{J_{s}}$

Algorithm 9 Gaussian Mixture Implementation of Multisensor MOT – Single Sensor
Update Step

1
$\Big{[}\Big{\\{}\big{\\{}\overline{\bm{x}}_{+}^{\ast(h,m)},\overline{\bm{P}}_{+}^{(h,m)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\xi^{(m)}\Big{\\}}^{M}_{m=1}\Big{]}=\text{NewObjects}\big{(}\bm{z}_{s}\big{)}$

2[.8mm]

3Draw $\overline{\bm{x}}^{\ast(h)}_{\text{b}},h=1,\dots,N_{\mathrm{k}}$ from
$f_{\mathrm{b}}(\overline{\bm{x}})$;

4

5Compute covariance matrix $\overline{\bm{P}}_{\text{b}}$ from
$f_{\mathrm{b}}(\overline{\bm{x}})$;

6

7for  _$m=1:M_{s}$_ do

8 for  _$h=1:N_{\mathrm{k}}$_ do

9
$\Big{[}\big{\\{}{\overline{\bm{x}}}^{(i,h,m)},\overline{w}^{(i,h,m)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{]}=$

10 [.5mm] $\hskip 15.649pt\text{InvertibleFlow}\Big{(}\hskip
0.85358pt\overline{\bm{x}}^{\ast(h)}_{\mathrm{b}},\overline{\bm{P}}_{\mathrm{b}},\bm{z}^{(m)}_{s}\Big{)}$;

// see Alg. 4

11

12 end for

13
$\xi^{(m)}=1+\frac{\mu_{\mathrm{b}}}{N_{\mathrm{k}}N_{\mathrm{p}}\mu_{\mathrm{fp}}f_{\mathrm{fp}}\big{(}\bm{z}_{s}^{(m)}\big{)}}\sum_{h=1}^{N_{\mathrm{k}}}\sum_{i=1}^{N_{\mathrm{p}}}\overline{w}^{(i,h,m)}$
;

14 for  _$h=1:N_{\mathrm{k}}$_ do

15
$\Big{[}\\{\overline{\bm{x}}^{\ast(h,m)},\overline{\bm{P}}^{(h,m)}\Big{]}\vspace{.5mm}=\hskip
8.53581pt\text{GaussianRepresentation}\Big{(}\big{\\{}{\overline{\bm{x}}}^{(i,h,m)},\overline{w}^{(i,h,m)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{)}\vspace{1.5mm}$;

// see Alg. 3

16

17 end for

18

19
$\Big{[}\big{\\{}\overline{\bm{x}}_{+}^{\ast(h,m)},\overline{\bm{P}}_{+}^{(h,m)}\big{\\}}^{N_{\mathrm{k}}}_{h=1}\Big{]}\vspace{1mm}=$

20 [-.5mm] $\hskip
31.29802pt\text{Resampling}\Big{(}\Big{\\{}\overline{\bm{x}}^{\ast(h,m)},\overline{\bm{P}}^{(h,m)},\big{\\{}\overline{w}^{(i,h,m)}\big{\\}}_{i=1}^{N_{\mathrm{p}}}\Big{\\}}^{N_{\mathrm{k}}}_{h=1}\hskip
0.85358pt\Big{)}$;

// see Alg. 5

21

22 end for

Output:
$\Big{\\{}\big{\\{}\overline{\bm{x}}_{+}^{\ast(h,m)},\overline{\bm{P}}_{+}^{(h,m)}\big{\\}}^{N_{\mathrm{k}}}_{h=1},\xi^{(m)}\Big{\\}}^{M}_{m=1}$

Algorithm 10 Gaussian Mixture Implementation of Multisensor MOT – Generation
of New POs

1 $\Big{[}\hskip
0.85358pt\Big{\\{}\big{\\{}{\bm{x}}^{\ast(h,j)},\bm{P}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p^{(j)}\Big{\\}}_{j=1}^{J}\hskip
0.85358pt\Big{]}=\text{MultisensorMOT}\Big{[}\Big{\\{}\big{\\{}\bm{x}_{-}^{\ast(h,j)},\bm{P}_{-}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{-}^{(j)}\Big{\\}}_{j=1}^{J_{-}},\bm{z}\Big{]}$

2[.5mm]

3Perform prediction step according to

4for  _$j=1:J$_ do

5 for  _$h=1:N_{\mathrm{k}}$_ do

6

7 $\underline{\bm{x}}_{1}^{(h,j)\ast}=\bm{G}\hskip
0.85358pt\bm{x}^{\ast(h,j)}_{-}+\bm{u}^{\ast}$

8 [1.5mm] $\underline{\bm{P}}_{1}^{(h,j)}=\bm{G}\hskip
0.28453pt\bm{P}_{-}^{(h,j)}\bm{G}^{\mathrm{T}}+\bm{P}_{\bm{\mathsfbr{u}}}$

9 [1.5mm]

10 end for

11 $\underline{p}^{(j)}_{1}=p_{\mathrm{su}}\hskip 0.85358ptp^{(j)}_{-}$

12 end for

13

14Perform single sensor update steps sequentially, i.e.,

15for  _$s=1:S$_ do

16
$\Big{[}\Big{\\{}\big{\\{}{\bm{x}}_{s}^{\ast(h,j)},\bm{P}_{s}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{s}^{(j)}\Big{\\}}_{j=1}^{J_{s}}\Big{]}\vspace{.8mm}=\hskip
5.69054pt\text{SingleSensorUpdate}\Big{[}\Big{\\{}\big{\\{}\underline{\bm{x}}_{s}^{\ast(h,j)},\underline{\bm{P}}_{s}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},\underline{p}_{s}^{(j)}\Big{\\}}_{j=1}^{\underline{J}_{s}},\bm{z}_{s}\Big{]}$

[1mm] // see Alg. 9

17

18 Perform pruning to obtain
$\Big{\\{}\big{\\{}{\underline{\bm{x}}}_{s+1}^{\ast(h,j)},\underline{\bm{P}}_{s+1}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},\underline{p}_{s+1}^{(j)}\Big{\\}}_{j=1}^{\underline{J}_{s+1}}$

19 [.5mm] from
$\Big{\\{}\big{\\{}\bm{x}_{s}^{\ast(h,j)},\bm{P}_{s}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{s}^{(j)}\Big{\\}}_{j=1}^{J_{s}}$
where $\underline{J}_{s+1}\leqslant J_{s}$

20 end for

21

22Set
$\Big{\\{}\big{\\{}{\bm{x}}^{\ast(h,j)},\bm{P}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p^{(j)}\Big{\\}}_{j=1}^{J}=\Big{\\{}\big{\\{}{\bm{x}_{S}}^{\ast(h,j)},\bm{P}_{S}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p_{S}^{(j)}\Big{\\}}_{j=1}^{J_{S}}$
where $J=J_{S}$

Output:
$\Big{\\{}\big{\\{}{\bm{x}}^{\ast(h,j)},\bm{P}^{(h,j)}\big{\\}}_{h=1}^{N_{\mathrm{k}}},p^{(j)}\Big{\\}}_{j=1}^{J}$

Algorithm 11 Single Time Step of Multisensor MOT with GMM and Invertible Flow

##  VI Numerical Results

###  VI-A 3-D Passive Acoustic Tracking Scenario

We consider an underwater 3-D tracking scenario where eight objects are
tracked by two static sonar hydrophone arrays. $200$ time steps were
considered. The hydrophones are deployed about 1300 m below sea level. The
object states at time $k$ consist of 3-D position and velocity, i.e.,
$\bm{\mathsfbr{x}}^{(j)}_{k}=[\mathsfbr{x}^{(j)}_{1,k}\hskip
2.84526pt\mathsfbr{x}^{(j)}_{2,k}\hskip
2.84526pt\mathsfbr{x}^{(j)}_{3,k}\hskip
2.84526pt\mathsfbr{\dot{x}}^{(j)}_{1,k}\hskip
2.84526pt\mathsfbr{\dot{x}}^{(j)}_{2,k}\hskip
2.84526pt\mathsfbr{\dot{x}}^{(j)}_{3,k}]^{\text{T}}$ and evolve according to a
constant-velocity model [50, Sec. 6.3.2], where the dynamic noise has the
physical interpretation as an acceleration with variance
$\sigma^{2}_{\bm{\mathsfbr{w}}}$. The region of interest (ROI) is
$[-1000\hskip 0.85358pt\text{m},\hskip 0.85358pt1000\hskip
0.85358pt\text{m}]\times[-1000\hskip 0.85358pt\text{m},\hskip
0.85358pt1000\hskip 0.85358pt\text{m}]\times[-1500\hskip
0.85358pt\text{m},\hskip 0.85358pt0\hskip 0.85358pt\text{m}]$. Objects appear
at $k\in\\{1,10,20,30,40,50,60,70\\}$ and disappear at
$k\in\\{130,140,150,160,170,180,190,200\\}$. The initial state of the objects
is uniformly distributed around a circle with radius 300 m
($\sqrt{{\mathsfbr{x}^{(j)2}_{1,0}}+{\mathsfbr{x}^{(j)2}_{2,0}}}=300$) and
depth 50 m ($\mathsfbr{x}^{(j)}_{3,0}=-50$), with a vertical speed as
$\mathsfbr{\dot{x}^{(j)}_{3,0}}=-4\hskip 0.85358pt$m/s and horizontal speed as
$\sqrt{\mathsfbr{\dot{x}}^{(j)2}_{1,0}+\mathsfbr{\dot{x}}^{(j)2}_{2,0}}=1\hskip
0.85358pt$m/s pointing to the circle center. As a result, the generated tracks
simulate a diving behavior with tracks intersecting at the ROI center.

The two hydrophone arrays have the geometry of the array described in [51] and
are located at $[519\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt137\hskip
0.85358pt\hskip 0.85358pt-1300]^{\mathrm{T}}$ and $[-519\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt-137\hskip 0.85358pt\hskip
0.85358pt-1300]^{\mathrm{T}}$, respectively. Each hydrophone array consists of
4 receivers. Hence, there are 6 receiver pairs. Each receiver pair generates
TDOA measurements and is considered a sensor for MOT. This means that
multisensor measurements $\bm{\mathsfbr{z}}^{(m)}_{k,s}$,
$m\in\big{\\{}1,\dots,\mathsfbr{M}_{k,s}\big{\\}}$ and
$s\in\big{\\{}1,\dots,12\big{\\}}$ are obtained by the two arrays at time $k$.
At each sensor $s$, a random number of $\mathsfbr{M}_{k,s}$ measurements are
generated independently following the measurement model as (VI-A) with random
FPs and probability of missed detection. In particular, the noisy TDOA
$\mathsfbr{z}^{(m)}_{k,s}$ of a detected object with state
$\bm{\mathsfbr{x}}^{(j)}_{k}$ is modeled as

$\displaystyle\mathsfbr{z}^{(m)}_{k,s}$
$\displaystyle=\frac{1}{c}\Big{(}\big{\|}\big{[}\mathsfbr{x}^{(j)}_{1,k}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt\mathsfbr{x}^{(j)}_{2,k}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\mathsfbr{x}^{(j)}_{3,k}\big{]}^{\text{T}}-\bm{p}_{s_{\text{L}}}\big{\|}$
$\displaystyle\hskip 31.29802pt-\big{\|}\big{[}\mathsfbr{x}^{(j)}_{1,k}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt\mathsfbr{x}^{(j)}_{2,k}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\mathsfbr{x}^{(j)}_{3,k}\big{]}^{\text{T}}-\bm{p}_{s_{\text{R}}}\big{\|}\Big{)}+\mathsfbr{v}_{k,s}^{(m)}$
(33)

where $\bm{p}_{s_{\text{L}}}$ and $\bm{p}_{s_{\text{R}}}$ are the paired
receiver positions of sensor $s$, $c=1500\hskip 0.85358pt$m/s is the
propagation speed, and $\mathsfbr{v}_{k,s}^{(m)}$ is additive zero-mean
Gaussian noise with standard deviation $\sigma_{\mathsfbr{v}}$ that is assumed
statistically independent across $s$, $k$, and $m$. The individual flase
positives pdfs $f_{\mathrm{fp}}\big{(}z^{(m)}_{k,s}\big{)}$ are assumed
uniform on
$\frac{1}{c}\big{[}-\|\bm{p}_{s_{\text{L}}}-\bm{p}_{s_{\text{R}}}\|,\|\bm{p}_{s_{\text{L}}}-\bm{p}_{s_{\text{R}}}\|\big{]}$.
The pdf for object birth $f_{\mathrm{b}}\big{(}\overline{\bm{x}}_{k}\big{)}$
is uniform on the ROI, and the mean number of newborn objects is
$\mu_{\text{b}}=0.05$. The survival probability is $p_{\mathrm{s}}=0.95$. The
object declaration threshold is set to $P_{\text{th}}=0.5$ and the pruning
threshold to $P_{\text{pr}}=10^{-4}$.

(a) $\sigma_{\bm{\mathsfbr{w}}}=0.05$ m/s2

(b) $\sigma_{\bm{\mathsfbr{w}}}=0.1$ m/s2

(c) $\sigma_{\bm{\mathsfbr{w}}}=0.5$ m/s2

Figure 2: OSPA performance w.r.t. $\sigma_{\bm{\mathsfbr{w}}}$. Other
parameters are set as $\mu_{\mathrm{fp}}=10$,
$\sigma_{\bm{\mathsfbr{v}}}=10^{-6}\\!$ s, $p_{\text{d}}=0.85$.

(a) $\sigma_{\bm{\mathsfbr{v}}}=5\\!\times\\!10^{-7}\\!$ s

(b) $\sigma_{\bm{\mathsfbr{v}}}=1\\!\times\\!10^{-6}\\!$ s

(c) $\sigma_{\bm{\mathsfbr{v}}}=2\\!\times\\!10^{-6}\\!$ s

Figure 3: OSPA performance w.r.t. $\sigma_{\bm{\mathsfbr{v}}}$. Other
parameters are set as $\mu_{\mathrm{fp}}=10$, $\sigma_{\bm{\mathsfbr{w}}}=0.1$
m/s2, $p_{\text{d}}=0.85$.

(a) $p_{\text{d}}=0.75$

(b) $p_{\text{d}}=0.85$

(c) $p_{\text{d}}=0.95$

Figure 4: OSPA performance w.r.t. $p_{\text{d}}$. Other parameters are set as
$\mu_{\mathrm{fp}}=10$, $\sigma_{\bm{\mathsfbr{w}}}=0.1$ m/s2,
$\sigma_{\bm{\mathsfbr{v}}}=1\\!\times\\!10^{-6}\\!$ s.

(a) $\mu_{\mathrm{fp}}=3$

(b) $\mu_{\mathrm{fp}}=10$

(c) $\mu_{\mathrm{fp}}=30$

Figure 5: OSPA performance w.r.t. $\mu_{\mathrm{fp}}$. Other parameters are
set as $\sigma_{\bm{\mathsfbr{v}}}=1\\!\times\\!10^{-6}\\!$ s,
$\sigma_{\bm{\mathsfbr{w}}}=0.1$ m/s2, $p_{\text{d}}=0.85$.

###  VI-B Performance Comparison

We compare the proposed SPA-based MOT algorithm with embedded particle flow
(“SPA-PF”) ($N_{\lambda}=20$) with two reference sampling methods also based
on the SPA-based MOT framework in [10]. The first method (“SPA-PM”) uses
predicted object beliefs as proposal pdf [9]. The second (yet unpublished)
method (“SPA-UT”) uses the unscented transformation to calculate an
informative proposal pdf [28]. We use $N_{\mathrm{k}}=100$ uniformly-weighted
Guassian kernels to approximate the objects’ non-Gaussian state space by GMM.
For each kernel of the new born object, we set $N_{\mathrm{p}}=10000$ for SPA-
PM, $N_{\mathrm{p}}=500$ for SPA-UT and SPA-PF; For each kernel of the legacy
object, we set $N_{\mathrm{p}}=600$ for SPA-PM, $N_{\mathrm{p}}=30$ for SPA-UT
and SPA-PF. We performed 100 simulation runs. Furthermore, since the state
space is more informative for legacy objects, it may be enough to sample from
the predicted distribution. Following this motivation, we also propose a
method “SPA-PF-Hyb” using SPA-PF for newbirth object detection with hybrid of
SPA-PM for legacy object tracking.

In the standard parameter setting, we run the four algorithms with driving
noise standard deviation $\sigma_{\bm{\mathsfbr{w}}}\\!=\\!0.1$ m/s2, mean
number of FPs $\mu_{\mathrm{fp}}\\!=\\!10$, detection probability
$p_{\text{d}}\\!=\\!0.85$ and measurement noise standard deviation
$\sigma_{\bm{\mathsfbr{v}}}\\!=\\!1\\!\\!\times\\!\\!10^{-6}\\!$ s. The
performance of the various algorithms is measured by the Euclidean distance-
based OSPA metric with cutoff parameter $C\\!=\\!50$ [52]. Figs. 2, 3, 4, and
5) show the mean  (MOSPA) error—averaged over 100 simulation runs—of all
methods versus time $k$ for different parameter settings. Generally, the SPA-
PM does not work in this challenging high-dimensional and nonlinear tracking
scenario. However, an improved proposal distribution using either UT or PF,
enables accurate MOT. All three methods yield error peaks at time steps where
the objects appear. However, it can be seen that the proposed methods, e.g.,
SPA-PF and SPA-PF-Hyb, outperform the SPA-UT at almost all time steps for both
new object detection and legacy object tracking. In particular, the proposed
SPA-PF yields a significantly reduced MOSPA error with the accumulation of
object appearances. The average computation time per time step $k$ for a
MATLAB implementation on a single core of an Intel Xeon Gold 5222 CPU was
measured as 38.16s for SPA-PM; 5.08s for SPA-UT; 43.67s for SPA-PF; and 32.25s
for SPA-PF-Hyb. Note that SPA-UT is about 7 times faster than SPA-PF if the
number of kernels and the number of particles per kernel are the same for both
methods.

We further show the influence of different parametric settings of the four
physical parameters mentioned above in Fig. 2-5, i.e., driving noise standard
deviation $\sigma_{\bm{\mathsfbr{w}}}$ in Fig. 2, measurement noise standard
deviation $\sigma_{\bm{\mathsfbr{v}}}$ in Fig. 3, detection probability
$p_{\text{d}}$ in Fig. 4, and mean number of FPs $\mu_{\mathrm{fp}}$ in Fig. 5
respectively. In summary, all subfigures show that the proposed SPA-PF and
SPA-PF-Hyb outperform the SPA-UT, while the SPA-PM always fails. It can be
observed that in Fig. 2, a smaller driving noise degrades the tracking
accuracy for SPA-UT while the SPA-PF remains unchanged. Furthermore, in Fig.
3, a smaller measurement noise improves the tracking performance of SPA-PF
while degrading the tracking performance of SPA-UT. This supports the
intuition that particle migration is especially advantageous if measurements
are informative. In Fig. 4, SPA-PF outperforms SPA-UT more significantly when
the detection probability is low. Finally, it can be seen in Fig. 5 that a
large number of FPs degrades the tracking accuracy for SPA-UT dramatically,
while the SPA-PF can still perform well.

##  VII Conclusion

We presented a graph-based Bayesian method for challenging nonlinear and high-
dimensional MOT problems that relies on particle filtering. Particle
degeneracy is avoided by performing operations on the graph using PFL. Our
numerical results demonstrate favorable detection and estimation accuracy in a
3-D MOT scenario. The introduced approach is expected to be particularly
appealing for large-scale underwater surveillance problems [53]. Possible
directions of future research include graph-based methods with embedded
stochastic PFL [54, 55] and applications including simultaneous localization
and object tracking [15], extended object tracking [56], and information-
seeking control [57].

## Acknowledgement

This material is based upon work supported by the Under Secretary of Defense
for Research and Engineering. Any opinions, findings, conclusions, or
recommendations expressed in this material are those of the author(s) and do
not necessarily reflect the views of the Under Secretary of Defense for
Research and Engineering.

The authors would like to thank Dr. Thomas Kropfreiter and Mr. Junsu Jang for
carefully reading the manuscript.

## References

  * [1] M. Zimmer, S. Dolman, and L. Weilgart, _Passive acoustic monitoring of cetaceans_. Cambridge, UK: Cambridge Univ. Press, 2011.
  * [2] K. L. Cummins and M. J. Murphy, “An overview of lightning locating systems: History, techniques, and data uses, with an in-depth look at the U.S. NLDN,” _IEEE Trans. Electromagn. Compat._ , vol. 51, no. 3, pp. 499–518, 2009.
  * [3] Y. Bar-Shalom, P. K. Willett, and X. Tian, _Tracking and Data Fusion: A Handbook of Algorithms_. Storrs, CT: Yaakov Bar-Shalom, 2011.
  * [4] D. B. Reid, “An algorithm for tracking multiple targets,” _IEEE Trans. Autom. Control_ , vol. 24, no. 6, pp. 843–854, Dec. 1979.
  * [5] R. Mahler, _Statistical Multisource-Multitarget Information Fusion_. Norwood, MA: Artech House, 2007\.
  * [6] J. L. Williams, “Marginal multi-Bernoulli filters: RFS derivation of MHT, JIPDA and association-based MeMBer,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 51, no. 3, pp. 1664–1687, Jul. 2015.
  * [7] B.-N. Vo, B.-T. Vo, and H. G. Hoang, “An efficient implementation of the generalized labeled multi-Bernoulli filter,” _IEEE Trans. Signal Process._ , vol. 65, no. 8, pp. 1975–1987, 2017.
  * [8] A. Saucan, Y. Li, and M. Coates, “Particle flow SMC delta-GLMB filter,” in _Proc. IEEE ICASSP-17_ , New Orleans, LA, USA, Mar. 2017, pp. 4381–4385.
  * [9] F. Meyer, P. Braca, P. Willett, and F. Hlawatsch, “A scalable algorithm for tracking an unknown number of targets using multiple sensors,” _IEEE Trans. Signal Process._ , vol. 65, no. 13, pp. 3478–3493, Jul. 2017.
  * [10] F. Meyer, T. Kropfreiter, J. L. Williams, R. A. Lau, F. Hlawatsch, P. Braca, and M. Z. Win, “Message passing algorithms for scalable multitarget tracking,” _Proc. IEEE_ , vol. 106, no. 2, pp. 221–259, Feb. 2018.
  * [11] A. Tesei, F. Meyer, and R. Been, “Tracking of multiple surface vessels based on passive acoustic underwater arrays,” _J. Acoust. Soc. Am._ , vol. 147, no. 2, pp. EL87–EL92, 2020.
  * [12] F. Meyer and J. L. Williams, “Scalable detection and tracking of geometric extended objects,” _IEEE Trans. Signal Process._ , vol. 69, pp. 6283–6298, 2021.
  * [13] F. R. Kschischang, B. J. Frey, and H.-A. Loeliger, “Factor graphs and the sum-product algorithm,” _IEEE Trans. Inf. Theory_ , vol. 47, no. 2, pp. 498–519, Feb. 2001.
  * [14] H. Wymeersch, J. Lien, and M. Z. Win, “Cooperative localization in wireless networks,” _Proc. IEEE_ , vol. 97, no. 2, pp. 427–450, 2009.
  * [15] F. Meyer, O. Hlinka, H. Wymeersch, E. Riegler, and F. Hlawatsch, “Distributed localization and tracking of mobile networks including noncooperative objects,” _IEEE Trans. Signal Inf. Process. Netw._ , vol. 2, no. 1, pp. 57–71, Mar. 2016.
  * [16] M. Z. Win, F. Meyer, Z. Liu, W. Dai, S. Bartoletti, and A. Conti, “Efficient multisensor localization for the internet of things: Exploring a new class of scalable localization algorithms,” _IEEE Signal Process. Mag._ , vol. 35, no. 5, pp. 153–167, 2018.
  * [17] B. Teague, Z. Liu, F. Meyer, A. Conti, and M. Z. Win, “Network localization and navigation with scalable inference and efficient operation,” _IEEE Trans. Mobile Comput._ , vol. 21, no. 6, pp. 2072–2087, 2022.
  * [18] E. Leitinger, F. Meyer, F. Hlawatsch, K. Witrisal, F. Tufvesson, and M. Z. Win, “A scalable belief propagation algorithm for radio signal based SLAM,” _IEEE Trans. Wireless Commun._ , vol. 18, no. 11, Nov. 2019.
  * [19] X. Li, E. Leitinger, A. Venus, and F. Tufvesson, “Sequential detection and estimation of multipath channel parameters using belief propagation,” _IEEE Trans. Wireless Commun._ , vol. 21, no. 10, pp. 8385–8402, 2022.
  * [20] E. Leitinger, A. Venus, B. Teague, and F. Meyer, “Data Fusion for Multipath-Based SLAM: Combing Information from Multiple Propagation Paths,” 2022, available online: https://arxiv.org/abs/2211.09241.
  * [21] F. Meyer and K. L. Gemba, “Probabilistic focalization for shallow water localization,” _J. Acoust. Soc_ , vol. 150, no. 2, pp. 1057–1066, 2021.
  * [22] N. J. Gordon, D. J. Salmond, and A. F. M. Smith, “Novel approach to nonlinear/non-Gaussian Bayesian state estimation,” _IEE Proc. F_ , vol. 140, no. 2, pp. 107–113, 1993.
  * [23] A. Doucet, N. de Freitas, and N. Gordon, _Sequential Monte Carlo Methods in Practice_. Springer, 2001.
  * [24] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking,” _IEEE Trans. Signal Process._ , vol. 50, no. 2, pp. 174–188, Feb. 2002\.
  * [25] P. Bickel, B. Li, and T. Bengtsson, “Sharp failure rates for the bootstrap particle filter in high dimensions,” in _Pushing the Limits of Contemporary Statistics: Contributions in Honor of Jayanta K. Ghosh_ , vol. 3. Beachwood, OH, USA: Inst. Math. Statist., 2008, pp. 318–329.
  * [26] C. Musso, N. Oudjane, and F. Le Gland, “Improving regularised particle filters,” in _Sequential Monte Carlo Methods in Practice_. Springer, New York, NY, 2001, pp. 247–271.
  * [27] S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” _Proc. IEEE_ , vol. 92, no. 3, pp. 401–422, Mar. 2004.
  * [28] R. van der Merwe, A. Doucet, N. de Freitas, and E. Wan, “The unscented particle filter,” in _Proc. NIPS-00_ , Denver, CO, USA, Dec. 2000, pp. 584–590.
  * [29] R. van der Merwe, A. Doucet, N. de Freitas, and E. Wang, “Four hydrophone array for acoustic three-dimensional location,” Cambridge University Engineering Department, Tech. Rep. CUED/F-INFENG/TR 380, Aug. 2000.
  * [30] F. Daum and J. Huang, “Nonlinear filters with log-homotopy,” in _Proc. SPIE-07_ , Aug. 2007, pp. 423–437.
  * [31] ——, “Nonlinear filters with particle flow induced by log-homotopy,” in _Proc. SPIE-09_ , May 2009, pp. 76–87.
  * [32] F. Daum, J. Huang, and A. Noushin, “Exact particle flow for nonlinear filters,” in _Proc. SPIE-10_ , Apr. 2010, pp. 92–110.
  * [33] F. Daum and J. Huang, “Particle flow with non-zero diffusion for nonlinear filters,” in _Proc. SPIE-13_ , May 2013, pp. 226–238.
  * [34] P. Bunch and S. Godsill, “Approximations of the optimal importance density using Gaussian particle flow importance sampling,” _J. Amer. Statist. Assoc._ , vol. 111, no. 514, pp. 748–762, Aug. 2016.
  * [35] Y. Li and M. Coates, “Particle filtering with invertible particle flow,” _IEEE Trans. Signal Process._ , vol. 65, no. 15, pp. 4102–4116, Aug. 2017\.
  * [36] S. Pal and M. Coates, “Gaussian sum particle flow filter,” in _Proc. IEEE CAMSAP-18_ , Curaçao, Dutch Antilles, Dec. 2017.
  * [37] ——, “Particle flow particle filter for gaussian mixture noise models,” in _Proc. IEEE ICASSP-18_ , Calgary, Canada, 2018, pp. 4249–4253.
  * [38] Y. Li, S. Pal, and M. J. Coates, “Invertible particle-flow-based sequential MCMC with extension to Gaussian mixture noise models,” _IEEE Trans. Signal Process._ , vol. 67, no. 9, pp. 2499–2512, 2019.
  * [39] W. Zhang and F. Meyer, “Graph-based multiobject tracking with embedded particle flow,” in _Proc. IEEE RadarConf-21_ , Atlanta, GA, USA, May 2021\.
  * [40] J. Jang, F. Meyer, E. R. Snyder, S. M. Wiggins, S. Baumann-Pickering, and J. A. Hildebrand, “Bayesian detection and tracking of odontocetes in 3-D from their echolocation clicks,” 2022, available online: https://arxiv.org/abs/2210.12318.
  * [41] Y. Bar-Shalom, T. Kirubarajan, and X.-R. Li, _Estimation with Applications to Tracking and Navigation_. New York, NY: Wiley, 2002.
  * [42] F. Daum and J. Huang, “Exact particle flow for nonlinear filters: Seventeen dubious solutions to a first order linear underdetermined PDE,” in _Proc. Asilomar-10_ , Pacific Grove, CA, USA, Nov. 2010, pp. 64–71.
  * [43] T. Ding and M. J. Coates, “Implementation of the Daum-Huang exact-flow particle filter,” in _Proc. IEEE SSP-12_ , Ann Arbor, MI, USA, 2012, pp. 257–260.
  * [44] B. Silverman, _Density Estimation for Statistics and Data Analysis_. Taylor & Francis, 1986.
  * [45] G. J. McLachlan, S. X. Lee, and S. I. Rathnayake, “Finite mixture models,” _Annu. Rev. Stat. Appl._ , vol. 6, no. 1, pp. 355–378, 2019.
  * [46] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery, _Numerical Recipes: The Art of Scientific Computing_. USA: Cambridge University Press, 2007.
  * [47] E. Leitinger, F. Meyer, F. Hlawatsch, K. Witrisal, F. Tufvesson, and M. Z. Win, “A belief propagation algorithm for multipath-based slam,” _IEEE Trans. Wireless Commun._ , vol. 18, no. 12, pp. 5613–5629, 2019.
  * [48] J. L. Williams and R. Lau, “Approximate evaluation of marginal association probabilities with belief propagation,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 50, no. 4, pp. 2942–2959, Oct. 2014.
  * [49] H. V. Poor, _An Introduction to Signal Detection and Estimation_ , 2nd ed. New York: Springer-Verlag, 1994\.
  * [50] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, _Estimation with Applications to Tracking and Navigation_. New York, NY: Wiley, 2001.
  * [51] S. M. Wiggins and J. A. Hildebrand, “High-frequency acoustic recording package (HARP) for broad-band, long-term marine mammal monitoring,” in _Proc. UT-07_ , Apr. 2007, pp. 551–557.
  * [52] D. Schuhmacher, B.-T. Vo, and B.-N. Vo, “A consistent metric for performance evaluation of multi-object filters,” _IEEE Trans. Signal Process._ , vol. 56, no. 8, pp. 3447–3457, Aug. 2008.
  * [53] G. Ferri, A. Munafò, A. Tesei, P. Braca, F. Meyer, K. Pelekanakis, R. Petroccia, J. Alves, C. Strode, and K. LePage, “Cooperative robotic networks for underwater surveillance: an overview,” _IET Radar Sonar Navig._ , vol. 11, no. 12, pp. 1740–1761, Dec. 2017.
  * [54] L. Dai and F. Daum, “Stiffness mitigation in stochastic particle flow filters,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 58, no. 4, pp. 3563–3577, 2022.
  * [55] ——, “On the design of stochastic particle flow filters,” _IEEE Trans. Aerosp. Electron. Syst._ , 2022.
  * [56] F. Meyer and M. Z. Win, “Scalable data association for extended object tracking,” _IEEE Trans. Signal Inf. Process. Netw._ , vol. 6, pp. 491–507, 2020.
  * [57] F. Meyer, H. Wymeersch, M. Fröhle, and F. Hlawatsch, “Distributed estimation with information-seeking control in agent networks,” _IEEE J. Sel. Areas Commun._ , vol. 33, no. 11, Nov. 2015.

  *[MOT]: multiobject tracking
  *[SPA]: sum-product algorithm
  *[GMM]: Gaussian mixture model
  *[PFL]: Particle flow
  *[TDOA]: time-difference-of-arrival
  *[MOU]: measurement-origin uncertainty
  *[PDA]: probabilistic data association
  *[MHT]: multi-hypothesis tracking
  *[RFS]: random finite sets
  *[pdf]: probability density function
  *[SLAM]: simultaneous localization and mapping
  *[BPF]: bootstrap particle filter
  *[PDE]: partial differential equation
  *[EDH]: exact Daum and Huang
  *[LEDH]: localized exact Duam and Huang
  *[SMCMC]: sequential Markov chain Monte Carlo
  *[pmf]: probability mass function
  *[PO]: potential object
  *[DA]: data association
  *[MMSE]: minimum mean-square error
  *[ROI]: region of interest
  *[OSPA]: optimal sub-pattern assignment
  *[]:
  *[MOSPA]: mean   ()