# A Graph-based Algorithm for Robust Sequential Localization
Exploiting Multipath for Obstructed-LOS-Bias Mitigation

Alexander Venus1,2, , Erik Leitinger1,2, , Stefan Tertinek3,
and Klaus Witrisal1,2,
1Graz University of Technology, Austria, 3NXP Semiconductors, Austria,
2Christian Doppler Laboratory for Location-aware Electronic Systems
The financial support by the Christian Doppler Research Association, the
Austrian Federal Ministry for Digital and Economic Affairs and the National
Foundation for Research, Technology and Development is gratefully
acknowledged.

###### Abstract

This paper presents a factor graph formulation and particle-based sum-product
algorithm (SPA) for robust sequential localization in multipath-prone
environments. The proposed algorithm jointly performs data association,
sequential estimation of a mobile agent position, and adapts all relevant
model parameters. We derive a novel non-uniform false alarm (FA) model that
captures the delay and amplitude statistics of the multipath radio channel.
This model enables the algorithm to indirectly exploit position-related
information contained in the multipath components for the estimation of the
agent position without using any prior information such as floorplan
information or training data. Using simulated and real measurements in
different channel conditions, we demonstrate that the algorithm can provide
high-accuracy position estimates even in fully obstructed line-of-sight (OLOS)
situations and show that the performance of our algorithm constantly attains
the posterior Cramér-Rao lower bound (P-CRLB), facilitating the additional
information contained in the presented FA model. The algorithm is shown to
provide robust estimates in both, dense multipath channels as well as channels
showing specular, resolved MPCs, significantly outperforming state-of-the-art
radio-based localization methods.

###### Index Terms:

Obstructed line-of-sight, multipath, sum-product algorithm, probabilistic data
association, message passing, belief propagation

##  I Introduction

Localization of mobile agents using radio signals in environments such as
indoor or urban territories is still a challenging task[1, 2, 3, 4]. These
environments are characterized by strong multipath propagation and frequent
obstructed line-of-sight (OLOS) situations, which can prevent the correct
extraction of the line-of-sight (LOS) component (see Fig. 1). Radio channels
resulting from multipath propagation are commonly represented as a
superposition of a finite number of specular MPCs [5, 6, 7, 8]. However,
cluttered environments with closely-spaced reflecting objects or with diffuse
scatters (such as walls covered by shelves or irregular object shapes), along
with the finite bandwidth of the measurement equipment, cause dense multipath
propagation, which cannot be resolved into specular MPCs anymore [5, 9, 10,
11].

There exist many safety- and security-critical applications, such as
autonomous driving [12], medical services [13], or keyless entry systems [14],
where robustness of the position estimate111We define robustness as the
percentage of cases in which a system can achieve its given potential
accuracy. I.e., a robust sequential localization algorithm can keep the
agent’s track in a very high percentage of cases, even in challenging
environments. is of critical importance.

Figure 1: A mobile agent is moving alongside the anchors on an example
trajectory. Due to an obstacle, the LOS to all anchors is not always
available. There occur partial and full OLOS situations. Multipath propagation
may occur, but there is no prior information about the surrounding
environment.

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

New localization and tracking approaches within the context of 5G localization
[15] that take advantage of large measurement apertures as ultra-wideband
(UWB) systems [6, 16] or mmWave systems [17] seek to mitigate the effect of
multipath propagation [18] (commonly referred to as “NLOS propagation”) and
OLOS situations [19, 8], or even take advantage of MPCs by exploiting inherent
position information, turning multipath from impairment to an asset [20, 2,
21, 22, 23]. Prominent examples of such approaches are multipath-based methods
that estimate MPCs associate them to virtual anchors representing the
locations of the mirror images of an anchor on reflecting surfaces [24]. The
locations of virtual anchors are assumed to be known a priori [25] or
estimated jointly with the position of agents using multipath-based SLAM (MP-
SLAM) [22, 21, 26]. Jointly estimating the positions of virtual anchors and
agents allows MP-SLAM to provide high-accuracy position estimates, even in
OLOS situations, or to localize the agent with only a single anchor[27].
However, it requires specular, resolved MPCs, which are consistent with the
virtual anchor model [28]. Other methods exploit cooperation among individual
agents[29, 27, 30, 4], or perform robust signal processing against multipath
propagation and clutter measurements in general. The latter comprise
heuristics [6, 31], machine learning-based approaches [19, 32, 33, 34] as well
as Bayesian methods[35, 36, 37], and hybrids thereof [38, 39, 40]. Heuristic
methods, such as searching for the first amplitude to exceed a threshold
value, are fast and easily implementable but suffer from low accuracy as well
as a high probability of outage in low signal-to-noise-ratio (SNR) regions
[6]. In recent years, machine learning methods have grown increasingly
popular. Early approaches [33, 19] extract specific features from the radio
channel applying model-agnostic supervised regression methods on these
features. While these approaches potentially provide high accuracy estimates
at low computational demand (after training), they suffer from their
dependence on a large representative measurement database and can fail in
scenarios that are not sufficiently represented by the training data. This is
why recent algorithms facilitate deep learning and auto-encoding based methods
to directly operate on the received radio signal and reduce the dependence on
training data [41, 34, 42].

Multipath-based localization [21, 43, 22, 44, 26, 36, 37], multiobject-
tracking [45, 46, 47], and parametric channel tracking [48] are applications
that pose common challenges, such as uncertainties beyond Gaussian noise, like
missed detections and clutter, an uncertain origin of measurements, and
unknown and time-varying number of objects to be localized and tracked. These
challenges can be well addressed by Bayesian inference leveraging graphical
models to perform joint detection and estimation. Since the measurement models
of these applications are nonlinear, most methods typically rely on sampling
techniques such as recursive Monte Carlo sampling or particle filtering, or
use linearized Gaussian models [49, 50]. Similarly, the probabilistic data
association (PDA) algorithm [45, 51] represents a low-complexity Bayesian
method for robust localization and tracking with extension to multiple-sensors
PDA [52] and amplitude-information probabilistic data association (AIPDA) [53,
44]. All these methods can be categorized as “two-step approaches”, in the
sense that they do not operate on the received sampled radio signal, but use
extracted measurements provided by a preprocessing step, providing a high
level of flexibility and a significant reduction of computational complexity.
In contrast, “direct positioning approaches” such as [20, 54, 55] directly
exploit the received sampled signal, which can lead to a better detectability
of low-SNR features, yet, they are computationally very demanding.

###  I-B Contributions

In this paper, we propose a particle-based SPA that sequentially estimates the
position of a mobile agent by utilizing the position-related information
contained in the LOS component as well as in MPCs222Throughout this paper,
MPCs denote all components of the received signal that are caused by the
transmit signal, except the LOS component, i.e. “ non- (NLOS) components”..
The proposed algorithm jointly performs probabilistic data association and
estimation of the mobile agent state [46, 22] together with all relevant model
parameters, employing the SPA on a factor graph [56]. Similar to other two-
step approaches, it uses signal component delays and amplitudes estimated by a
snapshot-based parametric channel estimation and detection algorithm (CEDA) as
measurements. The proposed algorithm adapts in an online manner the time-
varying component SNR [44] as well as the detection probability of the LOS
[43, 57]. To this end, we propose a novel detection probability model that
allows for both an exhaustive representation of the detection space and a
smooth estimate of the SNR. The algorithm exploits a novel non-uniform “FA
model”333Typically the FA or clutter model for delay measurements is chosen to
be a uniform distribution inside the observation region of the sensor [46,
Sec. I-C] [51, Sec. 2.5.2]. Since we do not distinguish between FAs and MPCs,
the resulting distributions of delay and amplitude measurements are non-
uniform with respect to delay.that explicitly models measurements originating
from MPCs. More specifically, the introduced model represents the non-uniform
distribution of delay measurements and corresponding delay-dependent
distribution of amplitude measurements caused by MPCs and FAs in a joint
manner. We refer to this part of the model using the terms “NLOS model” or
“NLOS measurement model” throughout the paper. The presented NLOS model is
derived from a stochastic radio signal model, which represents MPCs by their
delay power spectrum (DPS), also referred to as power delay profile in the
literature [9, 5, 58, 59]. Additionally, the model couples MPC measurements to
the LOS measurement by a jointly inferred bias state. This enables the
algorithm to utilize the position-related information contained in the MPCs
without inferring specific map information, which can increase the accuracy
and robustness of the agent’s position estimate in challenging environments,
characterized by strong multipath propagation and temporary OLOS situations.
The proposed algorithm is able to operate without any prior information (no
floorplan information or training data are needed). It is demonstrated to
provide robust estimates for specular, resolved multipath as well as dense,
non-resolvable multipath, while offering sub-second runtime444The runtimes
were determined on PC, see Sec. VII-D for details. even in environments
characterized by strong multipath propagation and, thus, a high number of
measurements. The contributions of this paper are as follows.

  * •

We derive a novel non-uniform NLOS model that is adapted to the distribution
of the MPC delays and amplitudes corresponding to a stochastic radio signal
model [9, 58, 59] and verify its potential in a numerical study.

  * •

We present a new factor graph and corresponding SPA in order to efficiently
infer the marginal posterior distributions of all state variables of the
introduced joint probabilistic model.

  * •

We show that the proposed algorithm is capable of overcoming even fully-OLOS
situations and providing Cramér-Rao lower bound (CRLB)-level position accuracy
using both synthetic and real radio signal measurements.

  * •

We analyze the influence of the individual features of our algorithm and
compare it to a particle-based variant of the multi-sensor AIPDA algorithm, to
the MP-SLAM algorithm presented in [22, 44], and to the P-CRLB [60].

This work advances over the preliminary account of our conference publication
[37] (and that of the related work [36]) by (i) applying an accurate, adaptive
model for the joint distribution of delay and amplitude measurements instead
of using heuristical models, (ii) sequentially inferring all parameters of the
NLOS model together with the agent instead of using predetermined constants,
(iii) improving the convergence behavior using a modified, “decoupled” SPA
(see Sec. VI-A), (iv) demonstrating the performance of the proposed algorithm
using simulated radio signals as well as real radio measurements obtained by
(v) applying a CEDA, (vi) comparing to the MP-SLAM algorithm [22, 44] and
(vii) providing the P-CRLB as a performance benchmark.

##  II Notations and Definitions

Column vectors and matrices are denoted by boldface lowercase and uppercase
letters. rv are displayed in san serif, upright font, e.g., $\mathsfbr{x}$ and
$\bm{\mathsfbr{x}}$ and their realizations in serif, italic font, e.g. , $x$
and $\bm{x}$; $\tilde{x}$ denotes the true value of ${x}$. The same notation
applies for stochastic processes $\mathsfbr{x}(t)$ and their realizations
$x(t)$. $f({x})$ and $p({x})$ denote, respectively, the probability density
function (PDF) or probability mass function (PMF) of a continuous or discrete
random variable (RV) $\mathsfbr{x}$. $(\cdot)^{\mathrm{T}}$, $(\cdot)^{\ast}$,
and $(\cdot)^{\text{H}}$ denote matrix transpose, complex conjugation and
Hermitian transpose, respectively. $\lVert\cdot\rVert$ is the Euclidean norm.
$|\cdot|$ represents the cardinality of a set. $\mathrm{diag}\\{\bm{x}\\}$
denotes a diagonal matrix with entries in $\bm{x}$. $\bm{I}_{[\cdot]}$ is an
identity matrix of dimension given in the subscript. $[\bm{X}]_{n,n}$ denotes
the $n$th diagonal entry of $\bm{X}$. Furthermore, ${1}_{\mathbb{A}}(\bm{x})$
denotes the indicator function that is ${1}_{\mathbb{A}}(\bm{x})=1$ if
$\bm{x}\in\mathbb{A}$ and 0 otherwise, for $\mathbb{A}$ being an arbitrary set
and $\mathbb{R}^{\text{+}}$ is the set of positive real numbers. We predefine
the following PDFs with respect to $\mathsfbr{x}$: The truncated Gaussian PDF
is

$f_{\text{TN}}(x;\mu,\sigma,\lambda)=\frac{1}{Q(\frac{\lambda-\mu}{\sigma})\sqrt{2\pi}\sigma}e^{\frac{-(x-\mu)^{2}}{2\,\sigma^{2}}}{1}_{\mathbb{R}^{\text{+}}}(x-\lambda)$
(1)

with mean $\mu$, standard deviation $\sigma$, truncation threshold $\lambda$
and $Q(\cdot)$ denoting the Q-function [61]. Accordingly, the Gaussian PDF is
$f_{\text{N}}(x;\mu,\sigma)=f_{\text{TN}}(x;\mu,\sigma,\text{-}\hskip
0.5pt\infty)$. The truncated Rician PDF is [62, Ch. 1.6.7]

$f_{\text{TRice}}(x;s,u,\lambda)=\frac{1}{Q_{1}(\frac{u}{s},\frac{\lambda}{s})}\frac{x}{s^{2}}e^{\frac{-(x^{2}+u^{2})}{2\,s^{2}}}I_{0}(\frac{x\,u}{s^{2}}){1}_{\mathbb{R}^{\text{+}}}(x-\lambda)$
(2)

with non-centrality parameter $u$, scale parameter $s$ and truncation
threshold $\lambda$. $I_{0}(\cdot)$ is the 0th-order modified first-kind
Bessel function and $Q_{1}(\cdot,\cdot)$ denotes the Marcum Q-function [61].
The truncated Rayleigh PDF is [62, Ch. 1.6.7]

$f_{\text{TRayl}}(x;s,\lambda)=\frac{x}{s^{2}}\,e^{\frac{-(x^{2}-\lambda^{2})}{2\,s^{2}}}{1}_{\mathbb{R}^{\text{+}}}(x-\lambda)$
(3)

with scale parameter $s$ and truncation threshold $\lambda$. This formula
corresponds to the so-called Swirling I model[62]. Finally, we define the
uniform PDF $f_{\mathrm{U}}(x;a,b)=1/(b-a){1}_{[a,b]}(x)$ and the uniform PMF
$f_{\mathrm{UD}}(x;\mathcal{X})=1/|\mathcal{X}|{1}_{\mathcal{X}}(x)$.

##  III Radio Signal Model

TABLE I: Summary and description of all unobserved RVs of the system model. Description | agent state | rise time |  normalized amplitude | DNR | NLOS bias | fall time |  LOS probability |  association variable
---|---|---|---|---|---|---|---|---
Symbol | $\bm{\mathsfbr{x}}_{n}$ | $\sgamma_{\text{r}\hskip 0.5ptn}$ | $\mathsfbr{u}_{n}^{(j)}$ | $\somega_{n}^{(j)\hskip 0.5pt}$ | $\mathsfbr{b}_{n}^{(j)}$ | $\sgamma_{\text{f}\hskip 0.5ptn}^{(j)}$ | $\mathsfbr{q}_{n}^{(j)}$ | $\mathsfbr{a}_{n}^{(j)}$
Type | continuous | continuous | continuous | continuous | continuous | continuous | discrete | discrete
State Transition | Markov | Markov | Markov | Markov | Markov | Markov | Markov | independent
Anchor Relation | common | common | separate | separate | separate | separate | separate | separate
Description | augmented agent state | anchor state |  |
Symbol | $\bar{\bm{\mathsfbr{x}}}_{n}=[{\bm{\mathsfbr{x}}}_{n}^{\text{T}}\;\sgamma_{\text{r}\hskip 0.5ptn}]^{\text{T}}$ |  $\bm{\mathsfbr{y}}_{n}^{(j)}=[\mathsfbr{u}_{n}^{(j)}\somega_{n}^{(j)\hskip 0.5pt}\mathsfbr{b}_{n}^{(j)}\sgamma_{\text{f}\hskip 0.5ptn}^{(j)}]^{\text{T}}$ |  |

At each discrete time $n$, the mobile agent at position $\tilde{\bm{p}}_{n}$
transmits a signal $s(t)$ and each anchor $j\in\\{1,\hskip 0.5pt...\hskip
0.5pt,J\\}$ at anchor position
$\bm{p}_{\text{A}}^{(j)}=[p_{\text{Ax}}^{(j)}\;p_{\text{Ay}}^{(j)}]^{\text{T}}$
acts as a receiver. The complex baseband signal received at the $j$th anchor
is modeled as

$\displaystyle\vspace{-10mm}\mathsfbr{r}_{n}^{(j)}(t)=\tilde{\alpha}_{n,0}^{(j)}s\big{(}t-\tilde{\tau}_{n,0}^{(j)}\big{)}+\sum_{k=1}^{\tilde{K}_{n}^{(j)}}\tilde{\alpha}_{n,k}^{(j)}s\big{(}t-\tilde{\tau}_{n,k}^{(j)}\big{)}+\mathsfbr{{w}{}}_{n}^{(j)}(t)\hskip
0.85358pt.$ (4)

The first and second term describe the LOS component and the sum of
$\tilde{K}_{n}^{(j)}$ specular MPCs with their corresponding complex
amplitudes $\tilde{\alpha}_{n,k}^{(j)}\in\mathbb{C}$ and delays
$\tilde{\tau}_{n,k}^{(j)}\in\mathbb{R}^{\text{+}}$, respectively. The delays
are related to respective distances via
$\tilde{\tau}_{n,k}^{(j)}=\tilde{d}_{n,k}^{(j)}/\hskip 0.5ptc$ with $c$ being
the speed of light. The third term represents an additive white Gaussian noise
(AWGN) process $\mathsfbr{{w}{}}_{n}^{(j)}(t)$ with double-sided power
spectral density $\tilde{N}_{0}^{(j)}/\hskip 0.5pt2$. The LOS distance is
geometrically related to the agent position via
$\tilde{d}_{n,0}^{(j)}\triangleq d^{(j)}_{\text{LOS}\hskip
0.5pt}(\tilde{\bm{p}}_{n})$ with $d^{(j)}_{\text{LOS}\hskip
0.5pt}(\tilde{\bm{p}}_{n})=\lVert\tilde{\bm{p}}_{n}-\bm{p}_{\text{A}}^{(j)}\rVert$.
We assume time synchronization between all anchors and the mobile agent555Note
that state-of-the art UWB ranging devices (e.g., NXP SR040/SR150 or Qorvo
DW1000/DW3000) can provide synchronized channel impulse responses (CIRs) using
a two-way ranging protocol [63].. However, our algorithm can be extended to an
unsynchronized system along the lines of [21, 2, 64].

The signal $\mathsfbr{r}_{n}^{(j)}(t)$ in (4) is uniformly sampled with
sampling frequency $f_{\text{s}}$ at corresponding sampling interval
$T_{\text{s}}=1/f_{\text{s}}$ and $N_{\text{s}}$ samples are collected,
yielding a duration $T=N_{\text{s}}\,T_{\text{s}}$. By stacking the samples,
we obtain the discrete time signal vector

$\vspace*{-2mm}\bm{\mathsfbr{r}}_{n}^{(j)}=\tilde{\alpha}_{n,0}^{(j)}\bm{s}(\tilde{\tau}_{n,0}^{(j)})+\sum_{k=1}^{\tilde{K}_{n}^{(j)}}\tilde{\alpha}_{n,k}^{(j)}\bm{s}(\tilde{\tau}_{n,k}^{(j)})+\bm{\mathsfbr{{w}{}}}_{n}^{(j)}$
(5)

where $\bm{s}(\tau)\triangleq[s(-N_{\text{s}}/2\cdot
T_{\text{s}}-\tau)\,\,\,\cdots\,\,\,s((N_{\text{s}}-1)/2\cdot
T_{\mathrm{s}}-\tau)]^{\text{T}}\in\mathbb{C}^{N_{\text{s}}\times 1}$ is the
stacked signal vector containing the samples of the transmit signal $s(t)$.
The measurement noise vector
$\bm{\mathsfbr{{w}{}}}_{n}^{(j)}\in\mathbb{C}^{N_{\text{s}}\times 1}$ is a
zero-mean, circularly-symmetric complex Gaussian random vector with covariance
matrix $\tilde{\sigma}^{(j)\hskip 0.5pt2}\bm{I}_{N_{\text{s}}}$ and noise
variance $\tilde{\sigma}^{(j)\hskip 0.5pt2}=\tilde{N}_{0}^{(j)}/T_{\text{s}}$.
The MPCs arise from reflection or scattering by unknown objects, since we
assume that no map information is available.

For a very large number of MPCs $\tilde{K}_{n}^{(j)}$ and limited bandwidth of
$s(t)$, the MPCs cannot be resolved anymore. Hence, the MPCs are described by
a zero-mean, circularly-symmetric complex Gaussian stochastic process
$\snu_{\text{D}\hskip 0.5ptn}^{(j)}(\tau)$[65, 66, 67]. The corresponding
discrete time signal vector reads

$\vspace*{0mm}\bm{\mathsfbr{r}}_{n}^{(j)}=\tilde{\alpha}_{n,0}^{(j)}\bm{s}(\tilde{\tau}_{n,0}^{(j)})+\int\bm{s}(\tau)\snu_{\text{D}\hskip
0.5ptn}^{(j)}(\tau)\,\textrm{d}\tau+\bm{\mathsfbr{{w}{}}}_{n}^{(j)}$ (6)

with the second term denoting the dense multipath component [5, 9, 58, 59].
Assuming uncorrelated scattering for $\snu_{\text{D}\hskip
0.5ptn}^{(j)}(\tau)$ [65, 9], the noise covariance matrix of
$\bm{\mathsfbr{r}}_{n}^{(j)}$ is given by

$\bm{C}_{\text{N}\hskip 0.5ptn}^{(j)}=\int{S}_{\text{D}\hskip
0.5ptn}^{(j)}(\tau)\,\bm{s}(\tau)\,\bm{s}(\tau)^{\text{H}}\,\mathrm{d}\tau+\tilde{\sigma}^{(j)\hskip
0.5pt2}\,\bm{I}_{N_{\text{s}}}$ (7)

where ${S}_{\text{D}\hskip 0.5ptn}^{(j)}(\tau)$ is the DPS. Using (7), the SNR
of the LOS component is defined as666Note that the presented SNR model takes
into account the interference between the LOS component and the dense
multipath component [59]. In the absence of the dense multipath component this
reduces to the familiar SNR
$|\tilde{\alpha}^{(j)}_{n,0}|^{2}\lVert\bm{s}(\tilde{\tau}_{n,0}^{(j)})\rVert^{2}/\tilde{\sigma}^{(j)\hskip
0.5pt2}$.
$\mathrm{SNR}^{(j)}_{n}=|\tilde{\alpha}^{(j)}_{n,0}|^{2}\bm{s}(\tilde{\tau}_{n,0})^{\text{H}}\bm{C}_{\text{N\hskip
0.5ptn}}^{(j)-1}\bm{s}(\tilde{\tau}_{n,0})$ and the according normalized
amplitude is $\tilde{u}^{(j)}_{n}\triangleq\mathrm{SNR}^{(j)\hskip
0.5pt\frac{1}{2}}_{n}$.

###  III-A Delay Power Spectrum (DPS) Model

We choose to model the DPS ${S}_{\text{D}\hskip 0.5ptn}^{(j)}(\tau)$ as [9]

$\displaystyle\tilde{S}_{\text{D}\hskip 0.5ptn}^{(j)}(\tau)$
$\displaystyle\triangleq{S}_{\text{D}}(\tau;\tilde{\bm{p}}_{n},\tilde{\Omega}_{n}^{(j)},\tilde{\bm{\zeta}}_{\text{S}\hskip
0.5ptn}^{(j)})$
$\displaystyle=\tilde{\Omega}_{n}^{(j)}\,\frac{\tilde{\gamma}_{\text{f}\hskip
0.5ptn}^{(j)}+\tilde{\gamma}_{\text{r}\hskip
0.5ptn}}{{\tilde{\gamma}_{\text{f}\hskip 0.5ptn}^{(j)\hskip
0.5pt2}}}\Big{(}1\,{-}\,e^{-\frac{\Delta_{\text{b}}(\tau;\cdot)}{\tilde{\gamma}_{\text{r}\hskip
0.5ptn}}}\Big{)}\,e^{-\frac{\Delta_{\text{b}}(\tau;\cdot)}{\tilde{\gamma}_{\text{f}\hskip
0.5ptn}^{(j)}}}\,{1}_{\mathbb{R}^{\text{+}}}(\Delta_{\text{b}}(\tau;\cdot))$
(8)

which is a double exponential function with $\tilde{\Omega}_{n}^{(j)}$ being
the DPS power. The rise time $\tilde{\gamma}_{\text{r}\hskip 0.5ptn}$ and fall
time $\tilde{\gamma}_{\text{f}\hskip 0.5ptn}^{(j)}$ are shape parameters. The
distance difference $\Delta_{\text{b}}(\tau;\cdot)$ is given by

$\Delta_{\text{b}}(\tau;\tilde{\bm{p}}_{n},\tilde{b}_{n}^{(j)})=c\,\tau-
d_{\text{LOS}\hskip 0.5pt}^{(j)}(\tilde{\bm{p}}_{n})-\tilde{b}_{n}^{(j)}$

where $\tilde{b}_{n}^{(j)}$ is the NLOS bias, which denotes the difference
between the LOS distance $d_{\text{LOS}\hskip
0.5pt}^{(j)}(\tilde{\bm{p}}_{n})$ and the “onset distance”.
$\tilde{\bm{\zeta}}_{\text{S}\hskip
0.5ptn}^{(j)}=[\tilde{b}_{n}^{(j)}\,\tilde{\gamma}_{\text{f}\hskip
0.5ptn}^{(j)}\,\tilde{\gamma}_{\text{r}\hskip 0.5ptn}]^{\text{T}}$ collects
the NLOS shape parameters for each time $n$ and anchor $j$. Experimental
evidence motivates this model: The DPS typically exhibits an exponentially
decaying tail [5, 9] and a smooth onset [9, 68]. In particular, when the LOS
power is excluded, as is done in (6). Note that
$\tilde{\gamma}_{\text{r}\hskip 0.5ptn}$ is mainly determined by the signal
bandwidth and onset-density of MPCs. For homogeneous deployment environments
the on-set density is well modeled as being invariant. Therefore,
$\tilde{\gamma}_{\text{r}\hskip 0.5ptn}$ is assumed to be the same for all
anchors.

For inference, we also define the normalized DPS
$\bar{S}_{\text{D}}(d,\tilde{\bm{p}}_{n},\tilde{\bm{\zeta}}_{\text{S}\hskip
0.5ptn}^{(j)})={S}_{\text{D}}(d/c\,;\tilde{\bm{p}}_{n},\tilde{\Omega}_{n}^{(j)},\tilde{\bm{\zeta}}_{\text{S}\hskip
0.5ptn}^{(j)})\,/\,\tilde{\Omega}_{n}^{(j)}$ and the dense-multipath-to-noise
ratio (DNR) $\tilde{\omega}_{n}^{(j)\hskip
0.5pt}=\lVert\bm{s}({\tau})\rVert\,\tilde{\Omega}_{n}^{(j)\hskip
0.5pt\frac{1}{2}}/\hskip 0.5pt\hskip 0.5pt\tilde{\sigma}^{(j)}$, where the DNR
denotes the square-root power ratio between the dense multipath component and
AWGN.

The proposed algorithm utilizes the position information contained in
${S}_{\text{D}\hskip 0.5ptn}^{(j)}(\tau)$ to improve the position estimate
without explicitly exploiting map information.

###  III-B Parametric Channel Estimation

By applying a suitable snapshot-based channel estimation and detection
algorithm (CEDA)[5, 69, 70, 71] to the observed discrete signal vector
$\bm{r}_{n}^{(j)}$, one obtains, at each time $n$ and anchor $j$, a number of
$M_{n}^{(j)}$ measurements denoted by ${\bm{z}^{(j)}_{n,m}}$ with
$m\in\mathcal{M}_{n}^{(j)}=\\{1,\,...\,,M_{n}^{(j)}\\}$. Each
${\bm{z}^{(j)}_{n,m}}=[{z_{\mathrm{d}}^{(j)}}_{n,m}~{}{z_{\mathrm{u}}^{(j)}}_{n,m}]^{\text{T}}$
contains a distance measurement
${z_{\mathrm{d}}^{(j)}}_{n,m}\in[0,~{}d_{\text{max}}]$, with maximum distance
$d_{\text{max}}=c\,T$, and a normalized amplitude measurement
${z_{\mathrm{u}}^{(j)}}_{n,m}$. The CEDA decomposes the discrete signal vector
$\bm{r}_{n}^{(j)}$ into individual, decorrelated components according to (5),
reducing the number of dimensions (as ${M}_{n}^{(j)}$ is usually much smaller
than $N_{\text{s}}$). It thus can be said to compress the information
contained in $\bm{r}_{n}^{(j)}$ into
$\bm{z}^{(j)}_{n}=[{\bm{z}^{(j)\text{T}}_{n,1}}...\,{\bm{z}^{(j)\text{T}}_{n,M_{n}^{(j)}}}]^{\text{T}}$.
See the supplementary material [72, Sec. LABEL:M-sec:app_ceda] for further
details. The stacked vector
$\bm{z}_{n}=[\bm{z}^{(1)\,\text{T}}_{n}...\,\bm{z}^{(J)\,\text{T}}_{n}]^{\text{T}}$
is used as noisy measurement by the proposed algorithm.

##  IV System Model

We consider a mobile agent to be moving along an unknown trajectory as
depicted in Fig. 1. The current state of the agent is described by the state
vector
$\bm{\mathsfbr{x}}_{n}=[\bm{\mathsfbr{p}}_{n}^{\text{T}}\;\bm{\mathsfbr{v}}_{n}^{\text{T}}]^{\text{T}}$,
which is composed of the agent’s position
$\bm{\mathsfbr{p}}_{n}=[\mathsfbr{p}_{\text{x}\hskip
0.5ptn}\;\mathsfbr{p}_{\text{y}\hskip 0.5ptn}]^{\text{T}}$ and velocity
$\bm{\mathsfbr{v}}_{n}=[\mathsfbr{v}_{\text{x}\hskip
0.5ptn}\;\mathsfbr{v}_{\text{y}\hskip 0.5ptn}]^{\text{T}}$. We also introduce
the following additional state variables, which represent all RV inferred
along with $\bm{\mathsfbr{x}}_{n}$: First, we define the augmented agent state
$\bar{\bm{\mathsfbr{x}}}_{n}=[{\bm{\mathsfbr{x}}}_{n}^{\text{T}}\;\sgamma_{\text{r}\hskip
0.5ptn}]^{\text{T}}$, which collects all RVs that are common for all anchors.
Second, we define the anchor state
$\bm{\mathsfbr{y}}_{n}^{(j)}=[\mathsfbr{u}_{n}^{(j)}\somega_{n}^{(j)\hskip
0.5pt}\mathsfbr{b}_{n}^{(j)}\sgamma_{\text{f}\hskip 0.5ptn}^{(j)}]^{\text{T}}$
collecting all continuous RVs, which are modeled separately for each anchor.
Third, there are two discrete RVs $\mathsfbr{q}_{n}^{(j)}$ and
$\mathsfbr{a}_{n}^{(j)}$, which denote the LOS probability and association
variable, respectively, and are modeled separately for all anchors. For the
sake of clarity, all RVs constituting the system model are summarized and
described in Table I.

At each time $n$ and for each anchor $j$ the CEDA provides the currently
observed measurement vector $\bm{z}_{n}^{(j)}$, with fixed ${M}^{(j)}_{n}$,
according to Sec. III-B. Before the measurements are observed, they are random
and represented by the vector
${\bm{\mathsfbr{z}}^{(j)}_{n,m}}=[{\mathsfbr{z}_{\mathrm{d}}^{(j)}}_{n,m}~{}{\mathsfbr{z}_{\mathrm{u}}^{(j)}}_{n,m}]^{\text{T}}$.
In line with Sec. III-B we define the nested random vectors
$\bm{\mathsfbr{z}}^{(j)}_{n}=[{\bm{\mathsfbr{z}}^{(j)\text{T}}_{n,1}}\,...\,{\bm{\mathsfbr{z}}^{(j)\text{T}}_{n,\mathsfbr{M}_{n}^{(j)}}}]^{\text{T}}$
and
$\bm{\mathsfbr{z}}_{n}=[\bm{\mathsfbr{z}}^{(1)\,\text{T}}_{n}...\,\bm{\mathsfbr{z}}^{(J)\,\text{T}}_{n}]^{\text{T}}$.
Also the number of measurements $\mathsfbr{M}^{(j)}_{n}$ is a RV. The vector
containing all measurement numbers is defined as
$\bm{\mathsfbr{M}}_{n}=[\mathsfbr{M}_{n}^{(1)}\,...\,\mathsfbr{M}_{n}^{(J)}]^{\text{T}}$.

Each measurement $\bm{\mathsfbr{z}}_{n,m}^{(j)}$ either originates from the
LOS or is due to an MPC. It is also possible that a measurement
$\bm{\mathsfbr{z}}_{n,m}^{(j)}$ did not originate from any physical component,
but from FAs of the CEDA. The presented model only distinguishes between “LOS
measurements” originating from the LOS and “NLOS measurements”, i.e.,
measurements due to MPCs or FAs.

###  IV-A LOS Measurement Model

The LOS likelihood function (LHF) of an individual distance measurement
${\mathsfbr{z}_{\mathrm{d}}^{(j)}}_{n,m}$ is given by

$f_{\text{L}}({z_{\mathrm{d}}^{(j)}}_{n,m}|\bm{p}_{n},u_{n}^{(j)})\triangleq
f_{\text{N}}({z_{\mathrm{d}}^{(j)}}_{n,m};\,d_{\text{LOS}}^{(j)}(\bm{p}_{n}),\,\sigma_{\text{d}}(u^{(j)}_{n}))$
(9)

with mean $d^{(j)}_{\text{LOS}}(\bm{\mathsfbr{p}}_{n})$ and variance
$\sigma^{2}_{\text{d}}(\mathsfbr{u}^{(j)}_{n})$. The variance is determined
based on the Fisher information given by
$\sigma_{\text{d}}^{2}(\mathsfbr{u}^{(j)}_{n})=c^{2}/(8\,\pi^{2}\,\beta_{\text{bw}}^{2}\,\mathsfbr{u}^{(j)\hskip
0.5pt2}_{n})$, where $\beta_{\text{bw}}$ is the root mean squared bandwidth
[1, 2] and $\mathsfbr{u}_{n}^{(j)}$ is the normalized amplitude at anchor $j$.
The LOS LHF of the normalized amplitude measurement
${\mathsfbr{z}_{\mathrm{u}}^{(j)}}_{n,m}$ is modeled as777The presented model
describes the distribution of the amplitude estimates of a complex baseband
signal in AWGN obtained using maximum likelihood estimation and generalized
likelihood ratio test detection [73, 61, 53]. [44, 48]

$f_{\text{L}}({z_{\mathrm{u}}^{(j)}}_{n,m}|u^{(j)}_{n})\triangleq
f_{\text{TRice}}({z_{\mathrm{u}}^{(j)}}_{n,m};\sigma_{\mathrm{u}}(u^{(j)}_{n}),u^{(j)}_{n},\gamma)$
(10)

with $f_{\text{TRice}}(\cdot)$ being a truncated Rician PDF (2). $\gamma$ is
the detection threshold of the CEDA, which is a constant to be chosen. As for
the distance LHF, the scale parameter is determined based on the Fisher
information given as
$\sigma_{\mathrm{u}}^{2}(\mathsfbr{u}^{(j)}_{n})=1/2+\mathsfbr{u}^{(j)\hskip
0.5pt2}_{n}\,1/{(4\hskip 0.5ptN_{\text{s}})}$. Note that this expression
reduces to $1/2$ if the AWGN noise variance $\sigma^{(j)\hskip 0.5pt2}$ is
assumed to be known or $N_{\text{s}}$ to grow indefinitely (see [48] for a
detailed derivation). Note that for (10) the Marcum-Q function in (2)
represents the detection probability $p_{\text{D}}(\mathsfbr{u}_{n}^{(j)})$
(see Sec. IV-D).

###  IV-B NLOS Measurement Model

The NLOS LHF of an individual normalized amplitude measurement
${\mathsfbr{z}_{\mathrm{u}}^{(j)}}_{n,m}$ is given as

$\displaystyle
f_{\text{NL}}({z_{\mathrm{u}}^{(j)}}_{n,m}|{z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},{\bm{\zeta}}_{n}^{(j)})$
$\displaystyle\hskip 48.36958pt\triangleq
f_{\text{TRayl}}({z_{\mathrm{u}}^{(j)}}_{n,m};s_{\text{u}}({z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},{\bm{\zeta}}_{n}^{(j)}),\gamma)$
(11)

where $f_{\text{TRayl}}(\cdot)$ is a truncated Rayleigh PDF (3) and

$s^{2}_{\text{u}}({z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},\bm{\zeta}_{n}^{(j)})=\frac{1}{2}(\omega_{n}^{(j)\hskip
0.5pt2}\,\bar{S}_{\text{D}}({z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},{\bm{\zeta}}_{\text{S}\hskip
0.5ptn}^{(j)})+1)$ (12)

is the NLOS scale function. We used $\bm{\szeta}_{\text{S}\hskip
0.5ptn}^{(j)}=[\mathsfbr{b}_{n}^{(j)}\;\sgamma_{\text{f}\hskip
0.5ptn}^{(j)}\;\sgamma_{\text{r}\hskip 0.5ptn}]^{\text{T}}$ and
${\bm{\szeta}}_{n}^{(j)}=[\somega_{n}^{(j)\hskip
0.5pt}\;{\bm{\szeta}}_{\text{S}\hskip 0.5ptn}^{(j)\hskip
0.5pt\text{T}}]^{\text{T}}$ for notational brevity. See the supplementary
material [72, Sec. LABEL:M-sec:app_nlos_model] for details about the
derivation of (IV-B). The shape of (IV-B) with respect to
${z_{\mathrm{u}}^{(j)}}_{n,m}$ and ${z_{\mathrm{d}}^{(j)}}_{n,m}$ is shown in
Fig. 2a. The NLOS LHF of the distance measurement
${\mathsfbr{z}_{\mathrm{d}}^{(j)}}_{n,m}$ is given by

$\displaystyle
f_{\text{NL}}({z_{\mathrm{d}}^{(j)}}_{n,m}|\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$
$\displaystyle\vspace{3mm}=Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})^{-1}\int_{\gamma}^{\infty}f_{\text{TRayl}}(u;s_{\text{u}}({z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},\bm{\zeta}_{n}^{(j)}),\gamma)\,\mathrm{d}u$
$\displaystyle=Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})^{-1}\,\text{exp}\Big{(}{-}{\frac{\gamma^{2}}{2\,s_{\text{u}}^{2}({z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},\bm{\zeta}_{n}^{(j)})}}\Big{)}$
(13)

where
$Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})=\int_{0}^{d_{\text{max}}}\text{exp}({{{-}\,\gamma^{2}/(2\,s_{\text{u}}^{2}(d,\bm{p}_{n},\bm{\zeta}_{n}^{(j)})}}))\,\mathrm{d}\hskip
0.5ptd$ is the normalization constant ensuring integration to $1$. The
exponential term in (IV-B) corresponds to the probability that at time $n$ for
anchor $j$ a NLOS measurement at distance ${z_{\mathrm{d}}^{(j)}}_{n,m}$ is
generated. The shape of (IV-B) with respect to ${z_{\mathrm{d}}^{(j)}}_{n,m}$
for different values of $\gamma$ is shown in Fig. 2b. Note that (IV-B)
approaches a uniform PDF when $\gamma$ or ${\somega_{n}^{(j)\hskip 0.5pt}}$
approach zero.

The presented NLOS measurement model is valid independently of the DPS model
chosen in (III-A). However, (III-A) is a reasonable choice as it is physically
motivated [9] and is of moderate computational complexity.

###  IV-C Data Association Model

(a)

(b)

Figure 2: Graphical representation of LABEL:sub@fig:single_nlos_ampl_like the
amplitude NLOS LHF
$f_{\text{NL}}({z_{\mathrm{u}}^{(j)}}_{n,m}|{z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$
and LABEL:sub@fig:nlos_like_gamma the distance NLOS LHF
$f_{\text{NL}}({z_{\mathrm{d}}^{(j)}}_{n,m}|\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$
for different values of the detection threshold $\gamma$ at
$\omega_{n}^{(j)\hskip 0.5pt2}=20\,\text{dB}$, $d_{\text{LOS}\hskip
0.5pt}^{(j)}(\bm{p}_{n})+b_{n}^{(j)}=7\,\text{m}$, $\gamma_{\text{f}\hskip
0.5ptn}^{(j)}=6$ m, $\gamma_{\text{r}\hskip 0.5ptn}=0.5$ m and
$d_{\text{max}}=30$ m.

At each time $n$ and for each anchor $j$, the measurements, i.e., the
components of $\bm{\mathsfbr{z}}_{n}^{(j)}$ are subject to data association
uncertainty. Thus, it is not known which measurement
$\bm{\mathsfbr{z}}_{n,m}^{(j)}$ originated from the LOS, or which one is due
to an “NLOS measurement”, i.e., measurements originating from MPCs or FAs.
Based on the concept of PDA [51], we define the association variable
$\mathsfbr{a}^{(j)}_{n}$ as

${a}^{(j)}_{n}=\begin{cases}m\in\mathcal{M}_{n}^{(j)},&\text{$\bm{z}_{n,m}^{(j)}$
is the LOS measurement in $\bm{z}_{n}^{(j)}$}\\\ 0\,,&\text{no LOS measurement
in $\bm{z}_{n}^{(j)}$}\end{cases}\,.$ (14)

Assuming the number of NLOS measurements to follow a uniform distribution (so
called “non-parametric model”), the joint PMF of $\mathsfbr{a}^{(j)}_{n}$ and
$\mathsfbr{M}_{n}^{(j)}$ can be shown to be[51]

$\displaystyle
p(a^{(j)}_{n},M_{n}^{(j)}|u_{n}^{(j)},q_{n}^{(j)})=\begin{cases}\frac{p_{\text{E}}(u_{n}^{(j)},q_{n}^{(j)})}{M_{n}^{(j)}\,M_{\text{max}}}\,,&a^{(j)}_{n}\in\mathcal{M}_{n}^{(j)}\\\\[8.53581pt]
\frac{1-p_{\text{E}}(u_{n}^{(j)},q_{n}^{(j)})}{M_{\text{max}}},&a^{(j)}_{n}=0\end{cases}$
(15)

where $p_{\text{E}}(u_{n}^{(j)},q_{n}^{(j)})$ is the probability that there is
a LOS measurement for the current set of measurements defined in Sec. IV-D and
$M_{\text{max}}$ is an irrelevant constant. Incorporating
$\mathsfbr{a}^{(j)}_{n}$ into the model, we define the overall distance LHF as

$f({z_{\mathrm{d}}^{(j)}}_{n,m}|{\bm{\zeta}}_{\text{E}\hskip
0.5ptn}^{(j)})=\begin{cases}f_{\text{L}}({z_{\mathrm{d}}^{(j)}}_{n,m}|\bm{p}_{n},u_{n}^{(j)}),&a^{(j)}_{n}=m\\\
f_{\text{NL}}({z_{\mathrm{d}}^{(j)}}_{n,m}|\bm{p}_{n},\bm{\zeta}_{n}^{(j)}),&a^{(j)}_{n}\neq
m\end{cases}\,$ (16)

where we used ${\bm{\szeta}}_{\text{E}\hskip
0.5ptn}^{(j)}=[\bm{\mathsfbr{p}}_{n}^{\text{T}}\;\mathsfbr{u}_{n}^{(j)}\;\mathsfbr{a}_{n}^{(j)}\;{\bm{\szeta}}_{n}^{(j)\hskip
0.5pt\text{T}}]^{\text{T}}$ for brevity. The shape of (16) is depicted in Fig.
3a. Further, the overall amplitude LHF is given by

$\displaystyle f({z_{\mathrm{u}}^{(j)}}_{n,m}|$
$\displaystyle{z_{\mathrm{d}}^{(j)}}_{n,m},{\bm{\zeta}}_{\text{E}\hskip
0.5ptn}^{(j)})$
$\displaystyle\vspace{0mm}=\begin{cases}f_{\text{L}}({z_{\mathrm{u}}^{(j)}}_{n,m}|u^{(j)}_{n}),&a^{(j)}_{n}=m\\\
f_{\text{NL}}({z_{\mathrm{u}}^{(j)}}_{n,m}|{z_{\mathrm{d}}^{(j)}}_{n,m},\bm{p}_{n},\bm{\zeta}_{n}^{(j)}),&a^{(j)}_{n}\neq
m\end{cases}$ (17)

which is shown in Fig. 3b. Using the common assumption of the measurements to
be independent for different values of $m$ [46], the joint LHF for all
measurements per anchor $j$ and time $n$ is

$f(\bm{z}_{n}^{(j)}|{\bm{\zeta}}_{\text{E}\hskip
0.5ptn}^{(j)})=\prod_{m=1}^{\,\,M_{n}^{(j)}}f({z_{\mathrm{u}}^{(j)}}_{n,m}|{z_{\mathrm{d}}^{(j)}}_{n,m},{\bm{\zeta}}_{\text{E}\hskip
0.5ptn}^{(j)})\,f({z_{\mathrm{d}}^{(j)}}_{n,m}|{\bm{\zeta}}_{\text{E}\hskip
0.5ptn}^{(j)})\,.$ (18)

###  IV-D LOS Existence Probability Model

We model the LOS existence probability given in (15) as
$p_{\text{E}}(\mathsfbr{u}_{n}^{(j)},\mathsfbr{q}_{n}^{(j)})=p_{\text{D}}(\mathsfbr{u}_{n}^{(j)})\,\mathsfbr{q}_{n}^{(j)}$.
The probability of detection $p_{\text{D}}(\mathsfbr{u}_{n}^{(j)})$ is the
probability that at time step $n$ and anchor $j$ the agent generates a radio
signal component whose amplitude is high enough so that it leads to an LOS
measurement. It is modeled by the counter probability of a Rician cumulative
distribution function (CDF) given as

$\displaystyle
p_{\text{D}}(\mathsfbr{u}_{n}^{(j)})=Q_{1}\Bigg{(}\frac{\mathsfbr{u}^{(j)}_{n}}{\sigma_{\mathrm{u}}(\mathsfbr{u}^{(j)}_{n})},\frac{\gamma}{\sigma_{\mathrm{u}}(\mathsfbr{u}^{(j)}_{n})}\Bigg{)}$
(19)

by assuming that the proposed algorithm is applied after a generalized
likelihood ratio test detector. $\mathsfbr{q}_{n}^{(j)}$ is the probability of
the event that the LOS is not obstructed, which is referred to as LOS
probability in the following, and acts as a prior probability to the detection
event. According to [74, 43, 57], we model $\mathsfbr{q}_{n}^{(j)}$ as
discrete RV that takes its values from a finite set
$\mathcal{Q}=\\{\lambda_{1},\,...\,,\lambda_{Q}\\}$, where
$\lambda_{i}\in(0,1]$. The LOS probabilities for different sensors $j$ are
assumed to be independent. The proposed LOS existence probability model
$p_{\text{E}}(\mathsfbr{u}_{n}^{(j)},\mathsfbr{q}_{n}^{(j)})$ correctly
incorporates the detection process into the system model via
$p_{\text{D}}(\mathsfbr{u}_{n}^{(j)})$ excluding a detection of measurements
with ${z_{\mathrm{u}}^{(j)}}_{n,m}$ below $\gamma$ and it can cope with
amplitude model mismatch by correcting the amplitude-related probability of
detection with $\mathsfbr{q}_{n}^{(j)}$. With respect to implementation (see
Sec. VI-B1) this means that our model allows for smooth sequential inference
of slow amplitude variations (e.g., due to path loss) via
$p_{\text{D}}(\mathsfbr{u}_{n}^{(j)})$, while $\mathsfbr{q}_{n}^{(j)}$ ensures
a complete representation of the probability space, covering rapid amplitude
variations (e.g., due to OLOS).

###  IV-E State Transition model

We model the evolution of $\bar{\bm{\mathsfbr{x}}}_{n}$ and
$\bm{\mathsfbr{y}}_{n}^{(j)}$ and $\mathsfbr{q}_{n}^{(j)}$ over time $n$ as
independent first-order Markov processes, which are defined by the joint state
transition PDF

$\displaystyle
f(\bar{\bm{x}}_{n},{\bm{y}}_{n},q_{n}^{(j)}|\bar{\bm{x}}_{n-1},{\bm{y}}_{n-1},q_{n-1}^{(j)})$
$\displaystyle\vspace{3mm}=f(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1})\prod_{j=1}^{J}f({\bm{y}}_{n}^{(j)}|{\bm{y}}_{n-1}^{(j)})\,p(q_{n}^{(j)}|q_{n-1}^{(j)})\,.$
(20)

with $f(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1})$ and
$f({\bm{y}}_{n}^{(j)}|{\bm{y}}_{n-1}^{(j)})$ being the respective state
transition PDFs. For the discrete RV $\mathsfbr{q}_{n}^{(j)}$ the first-order
Markov process model results in a conventional Markov chain, with
$[\bm{Q}^{(j)}]_{i,k}=p(q_{n}^{(j)}=\lambda_{i}|q_{n-1}^{(j)}=\lambda_{k})$
being the elements of the transition matrix.

(a)  (b)

Figure 3: Graphical representation of the LABEL:sub@fig:single_delay_like the
overall distance LHF
$f({z_{\mathrm{d}}^{(j)}}_{n,m}|{\bm{\zeta}}_{\text{E}\hskip 0.5ptn}^{(j)})$
and LABEL:sub@fig:nlos_like_gamma overall amplitude LHF
$f({z_{\mathrm{u}}^{(j)}}_{n,m}|{z_{\mathrm{d}}^{(j)}}_{n,m},{\bm{\zeta}}_{\text{E}\hskip
0.5ptn}^{(j)})$, all at fixed ${\bm{\zeta}}_{\text{E}\hskip 0.5ptn}^{(j)}$ in
line with Fig. 2. In LABEL:sub@fig:single_amplitude_like we also fix
${z_{\mathrm{d}}^{(j)}}_{n,m}$ to $z_{\mathrm{d}1}=0\,\mathrm{m}$ or
$z_{\mathrm{d}2}=8\,\mathrm{m}$.

##  V Problem Formulation and Factor Graph

In this section we formulate the sequential estimation problem of interest and
present the joint posterior and the factor graph underlying the proposed
algorithm.

###  V-A Problem Statement

The problem considered is the sequential estimation of the agent state
${\bm{\mathsfbr{x}}}_{n}$. This is done in a Bayesian sense by calculating the
minimum mean-squared error (MMSE) [73] of the augmented agent state

${\hat{\bar{\bm{x}}}^{\text{MMSE}}_{n}\,\triangleq\int\bar{\bm{x}}_{n}\,f(\bar{\bm{x}}_{n}|\bm{z})\,\mathrm{d}\bar{\bm{x}}_{n}}\,.$
(21)

with $\hat{\bar{\bm{x}}}^{\text{MMSE}}_{n}=[\hat{\bm{x}}^{\text{MMSE
T}}_{n}\,\hat{{\gamma}}^{\text{MMSE}}_{\text{r}\hskip 0.5ptn}]^{\text{T}}$ and
$\hat{{\bm{x}}}^{\text{MMSE}}_{n}=[\hat{\bm{p}}^{\text{MMSE T}}_{n}$
$\hat{\bm{v}}^{\text{MMSE {T}}}_{n}]$ and
$\bm{z}=[\bm{z}^{\text{T}}_{{1}}\,...\,\bm{z}^{\text{T}}_{n}]^{\text{T}}$.
Furthermore, we also calculate

$\displaystyle\hat{\bm{y}}^{(j)\hskip 0.5pt\text{MMSE}}_{n}$
$\displaystyle\,\triangleq\int\bm{y}_{n}^{(j)}\,f(\bm{y}_{n}^{(j)}|\bm{z})\,\mathrm{d}\bm{y}_{n}^{(j)},$
(22) $\displaystyle\hat{q}^{(j)\hskip 0.5pt\text{MMSE}}_{n}$
$\displaystyle\,\triangleq\sum_{\lambda_{i}\in\mathcal{Q}}\lambda_{i}\,p(q_{n}^{(j)}=\lambda_{i}|\bm{z})$
(23)

with $\hat{\bm{y}}_{n}^{(j)\hskip 0.5pt\text{MMSE}}=[\hat{u}_{n}^{(j)\hskip
0.5pt\text{MMSE}}\,\hat{\omega}_{n}^{(j)\hskip
0.5pt\text{MMSE}}\,\hat{b}_{n}^{(j)\hskip
0.5pt\text{MMSE}}\,\hat{\gamma}_{\text{f}\hskip 0.5ptn}^{(j)\hskip
0.5pt\text{MMSE}}]^{\text{T}}$. In order to obtain (21), (22), and (23), the
respective marginal posterior PDFs need to be calculated. Since direct
marginalization of the joint posterior PDF is computationally infeasible[46],
we perform message passing by means of the SPA rules on the factor graph that
represents a factorized version of the joint posterior of our statistical
model discussed in Sec. IV.

###  V-B Joint Posterior and Factor Graph

For each $n$, let $\bm{\mathsfbr{y}}_{n}=[\bm{\mathsfbr{y}}^{(1)\hskip
0.5pt\text{T}}_{n}\vspace{0.5mm}\,...\,\bm{\mathsfbr{y}}^{(J)\hskip
0.5pt\text{T}}_{n}]^{\text{T}}$,
$\bm{\mathsfbr{a}}_{n}=[\mathsfbr{a}^{(1)}_{n}\,...\,\mathsfbr{a}^{(J)}_{n}]^{\text{T}}$,
and
$\bm{\mathsfbr{q}}_{n}=[\mathsfbr{q}^{(1)}_{n}\,...\,\mathsfbr{q}^{(J)}_{n}]^{\text{T}}$.
Furthermore, let
$\bm{\mathsfbr{z}}=[\bm{\mathsfbr{z}}^{\text{T}}_{{1}}\,...\,\bm{\mathsfbr{z}}^{\text{T}}_{n}]^{\text{T}}$,
$\bar{\bm{\mathsfbr{x}}}=[\bar{\bm{\mathsfbr{x}}}^{\text{T}}_{0}\,...\,\bar{\bm{\mathsfbr{x}}}^{\text{T}}_{n}]^{\text{T}}$,
$\bm{\mathsfbr{a}}=[\bm{\mathsfbr{a}}^{\text{T}}_{1}\,...\,\bm{\mathsfbr{a}}^{\text{T}}_{n}]^{\text{T}}$,
$\bm{\mathsfbr{y}}=[\bm{\mathsfbr{y}}^{\text{T}}_{0}\,...\,\bm{\mathsfbr{y}}^{\text{T}}_{n}]^{\text{T}}$,
$\bm{\mathsfbr{q}}=[\bm{\mathsfbr{q}}^{\text{T}}_{0}\,...\,\bm{\mathsfbr{q}}^{\text{T}}_{n}]^{\text{T}}$,
and
$\bm{\mathsfbr{M}}=[\bm{\mathsfbr{M}}^{\text{T}}_{1}\,...\,\bm{\mathsfbr{M}}^{\text{T}}_{n}]^{\text{T}}$.
We now assume that the measurements $\bm{z}$ are observed and thus fixed.
Applying Bayes’ rule as well as some commonly used independence
assumptions[46, 25] the joint posterior for all states up to time $n$ and all
$J$ anchors can be derived up to a constant factor as

$\displaystyle f(\bar{\bm{x}},\bm{a},\bm{y},\bm{q},\bm{M}|\bm{z})$
$\displaystyle\propto
f(\bm{z}|\bar{\bm{x}},\bm{a},\bm{y},\bm{q})\,f(\bar{\bm{x}},\bm{a},\bm{y},\bm{q})$
$\displaystyle=f(\bm{z}|{\bar{\bm{x}}},\bm{a},\bm{y},\bm{q})\,f(\bm{a}|\bm{y},\bm{q})\,f(\bar{\bm{x}})\,p(\bm{q})\,f(\bm{y})$
$\displaystyle\propto
f(\bar{\bm{x}}_{0})\prod^{J}_{j=1}p(q_{0}^{(j)})\,f(\bm{y}_{0}^{(j)})\prod^{n}_{n^{\prime}=1}\Upsilon(\bar{\bm{x}}_{n^{\prime}}|\bar{\bm{x}}_{n^{\prime}-1})\,\Phi(\bm{y}_{n^{\prime}}^{(j)}|\bm{y}_{n^{\prime}-1}^{(j)})$
$\displaystyle~{}~{}~{}\times\Psi(q_{n^{\prime}}^{(j)}|q_{n^{\prime}-1}^{(j)})\,\bar{g}(\bm{z}_{n^{\prime}}^{(j)};\bm{p}_{n^{\prime}},\bm{y}^{(j)}_{n^{\prime}},a^{(j)}_{n^{\prime}},q^{(j)}_{n^{\prime}})$
(24)

where we introduced the state-transition functions
$\Upsilon(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1})\triangleq
f(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1})$,
$\Phi(\bm{y}_{n}^{(j)}|\bm{y}_{n-1}^{(j)})\triangleq
f(\bm{y}_{n}^{(j)}|\bm{y}_{n-1}^{(j)})$, and
$\Psi(q_{n}^{(j)}|q_{n-1}^{(j)})\triangleq p(q_{n}^{(j)}|q_{n-1}^{(j)})$. We
also introduced the pseudo likelihood function
$\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n},\bm{y}^{(j)}_{n},a^{(j)}_{n},q_{n}^{(j)})\triangleq
h(a_{n}^{(j)};\bm{y}_{n}^{(j)},q_{n}^{(j)})\,g(\bm{z}_{n}^{(j)};\bm{p}_{n},\bm{y}^{(j)}_{n},a^{(j)}_{n})$.
Finally, we define $g(\bm{z}_{n}^{(j)};\breve{\bm{\zeta}}_{n}^{(j)})\triangleq
f(\bm{z}_{n}^{(j)}|\breve{\bm{\zeta}}_{n}^{(j)})$ and

$\displaystyle h(a^{(j)}_{n};\bm{y}_{n}^{(j)},q_{n}^{(j)})$
$\displaystyle\propto p(a^{(j)}_{n},M_{n}^{(j)}|u_{n}^{(j)},q_{n}^{(j)})$
$\displaystyle=\begin{cases}\frac{p_{\text{E}}(u_{n}^{(j)},q_{n}^{(j)})}{M_{n}^{(j)}}\,,&a^{(j)}_{n}\in\mathcal{M}_{n}^{(j)}\\\\[8.53581pt]
{1-p_{\text{E}}(u_{n}^{(j)},q_{n}^{(j)})},&a^{(j)}_{n}=0\end{cases}$ (25)

neglecting the constant terms in (15). Note that $\bm{M}$ vanishes in (V-B) as
it is fixed and thus constant, being implicitly defined by the measurements
$\bm{z}$. Furthermore note that unlike [51, 53, 46, 52, 22, 44, 47, 48] in our
model the NLOS LHFs (IV-B) and (IV-B) are both functions of RVs and, thus,
cannot be neglected.

The joint posterior PDF in (V-B) is represented by the factor graph shown in
Fig. 4.

##  VI Sum-Product Algorithm

###  VI-A Marginal Posterior and Sum-Product Algorithm (SPA)

The marginal posterior can be calculated efficiently by passing messages on
the factor graph according to the SPA[56]. For the proposed algorithm, we
specify not to send messages backward in time. This makes the factor graph in
Fig. 4 an acyclic graph. For acyclic graphs the SPA yields exact results for
the marginal posteriors [56]. At time $n$, the following calculations are
performed for all $J$ anchors. The prediction messages are given as

$\displaystyle\vspace{-1mm}\eta(\bar{\bm{x}}_{n})$
$\displaystyle=\int{\Upsilon}(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1})\,\breve{f}_{\text{{x}}}(\bar{\bm{x}}_{n-1})\,\mathrm{d}\bar{\bm{x}}_{n-1}$
(26) $\displaystyle\phi(\bm{y}_{n}^{(j)})$
$\displaystyle=\int\Phi(\bm{y}_{n}^{(j)}|\bm{y}_{n-1}^{(j)})\,\breve{f}_{\text{{y}}}(\bm{y}_{n-1}^{(j)})\,\mathrm{d}\bm{y}_{n-1}^{(j)}$
(27) $\displaystyle\psi(q_{n}^{(j)})$
$\displaystyle=\sum_{q_{n-1}^{(j)}=1}^{N_{q}}\Psi(q_{n}^{(j)}|q_{n-1}^{(j)})\,\breve{p}_{\text{q}}(q_{n-1}^{(j)})$
(28)

where $\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{n-1})$,
$\breve{f}_{\text{{y}}}(\bm{y}_{n-1}^{(j)})$ and
$\breve{p}_{\text{q}}(q_{n-1}^{(j)})$ are messages of the previous time $n-1$.
The measurement update messages are given by

$\displaystyle\xi^{(j)}(\bar{\bm{x}}_{n})$
$\displaystyle=\int\phi(\bm{y}_{n}^{(j)})\sum_{q_{n}^{(j)}=1}^{N_{q}}\psi(q_{n}^{(j)})$
$\displaystyle\hskip
14.22636pt\times\sum_{a_{n}^{(j)}=0}^{M_{n}^{(j)}}\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n},\bm{y}^{(j)}_{n},a^{(j)}_{n},q_{n}^{(j)})\,\mathrm{d}\bm{y}_{n}^{(j)}$
(29) $\displaystyle\chi^{(j)}(\bar{\bm{x}}_{n})$
$\displaystyle=\eta(\bar{\bm{x}}_{n})\prod_{j^{\prime}=1}^{J}\xi^{(j^{\prime})}(\bar{\bm{x}}_{n})/\xi^{(j)}(\bar{\bm{x}}_{n})$
(30) $\displaystyle\nu(\bm{y}_{n}^{(j)})$
$\displaystyle=\sum_{q_{n}^{(j)}=1}^{N_{q}}\psi(q_{n}^{(j)})\int\chi^{(j)}(\bar{\bm{x}}_{n})$
$\displaystyle\hskip
14.22636pt\times\sum_{a_{n}^{(j)}=0}^{M_{n}^{(j)}}\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n},\bm{y}^{(j)}_{n},a^{(j)}_{n},q_{n}^{(j)})\,\mathrm{d}\bar{\bm{x}}_{n}$
(31) $\displaystyle\beta(q_{n}^{(j)})$
$\displaystyle=\int\int\phi(\bm{y}_{n}^{(j)})\chi^{(j)}(\bar{\bm{x}}_{n})$
$\displaystyle\hskip
14.22636pt\times\sum_{a_{n}^{(j)}=0}^{M_{n}^{(j)}}\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n},\bm{y}^{(j)}_{n},a^{(j)}_{n},q_{n}^{(j)})\,\mathrm{d}\bar{\bm{x}}_{n}\,\mathrm{d}\bm{y}_{n}^{(j)}.$
(32)

Figure 4: Factor graph representing the factorization of the joint posterior
PDF in (V-B) as well as the respective messages according to the SPA (see Sec.
VI-A). The following short notations are used:
$\eta_{n}\triangleq\eta(\bar{\bm{x}}_{n})$,
$\phi_{n}^{(j)}\triangleq\phi(\bm{y}_{n}^{(j)})$,
$\psi_{n}^{(j)}\triangleq\psi(q_{n}^{(j)})$,
$\xi^{(j)}_{n}\triangleq\xi^{(j)}(\bar{\bm{x}}_{n})$,
$\chi^{(j)}_{n}\triangleq\chi^{(j)}(\bar{\bm{x}}_{n})$,
$\nu_{n}^{(j)}\triangleq\nu(\bm{y}_{n}^{(j)})$,
$\beta_{n}^{(j)}\triangleq\beta(q_{n}^{(j)})$,
$\chi^{(j)}_{n}\triangleq\chi^{(j)}(\bar{\bm{x}}_{n})$,
$\breve{f}_{\bar{\text{{x}}}\hskip
0.5ptn}\triangleq\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{n})$,
$\breve{f}_{\text{{y}}\hskip
0.5ptn}^{(j)}\triangleq\breve{f}_{\text{{y}}}(\bm{y}_{n}^{(j)})$,
$\breve{p}_{\text{q}\hskip
0.5ptn}^{(j)}\triangleq\breve{p}_{\text{q}}(q_{n}^{(j)})$.

Finally, we calculate the marginal posteriors as
$f(\bar{\bm{x}}_{n}|\bm{z})\propto\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{n})=\eta(\bar{\bm{x}}_{n})\prod_{j=1}^{J}\xi^{(j)}(\bar{\bm{x}}_{n})$,
$f(\bm{y}_{n}^{(j)}|\bm{z})\propto\breve{f}_{\text{{y}}}(\bm{y}_{n}^{(j)})=\phi(\bm{y}_{n}^{(j)})\,\nu(\bm{y}_{n}^{(j)})$
and
$p(q_{n}^{(j)}|\bm{z})\propto\breve{p}_{\text{q}}(q_{n}^{(j)})=\psi(q_{n}^{(j)})\,\beta(q_{n}^{(j)})\vspace*{1mm}$.

We additionally compare the performance of the above optimum SPA to that of a
suboptimal message passing algorithm, which we refer to as “decoupled SPA”.
Inspired by [43], we replace (30) by
$\chi^{(j)}(\bar{\bm{x}}_{n})=\eta(\bar{\bm{x}}_{n})$ neglecting the mutual
dependency of the uncertainties of individual anchor states
$\bm{\mathsfbr{y}}_{n}^{(j)}$. We demonstrate this modified algorithm to lead
to improved numerical stability for a low number of particles. Hence, the
particle-based implementation discussed in section VI-B1 addresses the
decoupled variant of the presented SPA.

###  VI-B Implementation Aspects

####  VI-B1 Particle-based Implementation

Since the integrals involved in the calculations of the messages and beliefs
(26)-(VI-A) cannot be obtained analytically, we use a computationally
efficient sequential particle-based message passing implementation that
provides approximate computation. Our implementation uses a “stacked state”
[75], comprising the augmented agent state as well as the anchor states of all
anchors $\mathcal{J}=\\{1,...,J\\}$.

  1. i)

_Prediction_ : The beliefs $\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{n-1})$
and $\breve{f}_{\text{{y}}}(\bm{y}_{n-1}^{(j)})$ for all $j\in\mathcal{J}$
calculated at the previous time step $n-1$, respectively, are represented by
$I$ particles and corresponding weights, i.e.,
$\\{\bar{\bm{\mathsfbr{x}}}_{n-1}^{[i]},{{}{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n-1}^{[i]}\\}_{i=1}^{I}$
and $\\{{\bm{\mathsfbr{y}}}_{n-1}^{(j)\hskip
0.5pt[i]},{{}{w}_{{\mathsf{\mathbf{y}}}}}{}_{n-1}^{(j)\hskip
0.5pt[i]}\\}_{i=1}^{I}$ for all $j\in\mathcal{J}$. Weighted particles
$\\{\bar{\bm{\mathsfbr{x}}}_{n}^{\prime\hskip 0.5pt[i]},{{}{w}^{\hskip
0.5pt\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}\\}_{i=1}^{I}$ and
$\\{{\bm{\mathsfbr{y}}}_{n}^{\prime\hskip 0.5pt(j)\hskip
0.5pt[i]},{{}{w}^{\hskip
0.5pt\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip 0.5pt[i]}\\}_{i=1}^{I}$
for all $j\in\mathcal{J}$, representing the messages $\eta(\bar{\bm{x}}_{n})$
and $\phi(\bm{y}_{n}^{(j)})$ in (26) and (27) are determined as follows: For
each particle $\bar{\bm{\mathsfbr{x}}}_{n-1}^{[i]}$ and
${\bm{\mathsfbr{y}}}_{n-1}^{(j)\hskip 0.5pt[i]}$ with $i\in\\{1,\dots,I\\}$,
one particle $\bar{\bm{\mathsfbr{x}}}_{n}^{\prime\hskip 0.5pt[i]}$ and
${\bm{\mathsfbr{y}}}_{n}^{\prime\hskip 0.5pt(j)\hskip 0.5pt[i]}$ with
corresponding weights ${{}{w}^{\hskip
0.5pt\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}={{}{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n-1}^{[i]}$
and ${{}{w}^{\hskip 0.5pt\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}={{}{w}_{{\mathsf{\mathbf{y}}}}}{}_{n-1}^{(j)\hskip 0.5pt[i]}$ is
drawn from $f(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1}^{[i]})$ and
$f({\bm{y}}_{n}^{(j)}|{\bm{y}}_{n-1}^{(j)\hskip 0.5pt[i]})$ for all
$j\in\mathcal{J}$.

  2. ii)

_Measurement Update_ : The non-normalized weights representing the messages
$\xi^{(j)}(\bar{\bm{x}}_{n})$ and $\nu(\bm{y}_{n}^{(j)})$ in (29) and (31) are
calculated by

$\displaystyle{{}{w}^{\hskip
0.5pt\prime\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{(j)\hskip 0.5pt[i]}$
$\displaystyle={{}{w}^{\hskip
0.5pt\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}\sum_{q_{n}^{(j)}=1}^{N_{q}}\psi(q_{n}^{(j)})$ $\displaystyle\hskip
14.22636pt\times\sum_{a_{n}^{(j)}=0}^{M_{n}^{(j)}}\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n}^{\prime\hskip
0.5pt[i]},\bm{y}^{\prime\hskip 0.5pt(j)\hskip
0.5pt[i]}_{n},a^{(j)}_{n},q_{n}^{(j)})$ (33) $\displaystyle{{}{w}^{\hskip
0.5pt\prime\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip 0.5pt[i]}$
$\displaystyle={{}{w}^{\hskip
0.5pt\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}\sum_{q_{n}^{(j)}=1}^{N_{q}}\psi(q_{n}^{(j)})$ $\displaystyle\hskip
14.22636pt\times\sum_{a_{n}^{(j)}=0}^{M_{n}^{(j)}}\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n}^{\prime\hskip
0.5pt[i]},\bm{y}^{\prime\hskip 0.5pt(j)\hskip
0.5pt[i]}_{n},a^{(j)}_{n},q_{n}^{(j)})\,.$ (34)

An approximation of the message $\beta(q_{n}^{(j)})$ in (VI-A) is given as

$\displaystyle\beta(q_{n}^{(j)})$
$\displaystyle\approx\sum_{i=1}^{I}{{}{w}^{\hskip
0.5pt\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}{{}{w}^{\hskip
0.5pt\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip 0.5pt[i]}$
$\displaystyle\hskip
14.22636pt\times\sum_{a_{n}^{(j)}=0}^{M_{n}^{(j)}}\bar{g}(\bm{z}_{n}^{(j)};\bm{p}_{n}^{\prime\hskip
0.5pt[i]},\bm{y}^{\prime\hskip 0.5pt(j)\hskip
0.5pt[i]}_{n},a^{(j)}_{n},q_{n}^{(j)})\,.$ (35)

  3. iii)

_Belief Calculation and State Estimation_ : The above approximate messages are
further used for calculating the non-normalized weights corresponding to the
beliefs $\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{n})$ and
$\breve{f}_{\text{{y}}}(\bm{y}_{n}^{(j)})$ for all $j\in\mathcal{J}$ as
${\hat{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}={{}{w}^{\hskip
0.5pt\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}\prod_{j=1}^{J}{{}{w}^{\hskip
0.5pt\prime\prime}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}$and ${\hat{w}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}={{}{w}^{\hskip
0.5pt\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip 0.5pt[i]}{{}{w}^{\hskip
0.5pt\prime\prime}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip 0.5pt[i]}$
respectively.

After normalization, i.e.,
${{}\bar{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}={\hat{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}/\sum_{i=1}^{I}{\hat{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n}^{[i]}$
and ${{}\bar{w}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}={\hat{w}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}/\sum_{i=1}^{I}{\hat{w}_{{\mathsf{\mathbf{y}}}}}{}_{n}^{(j)\hskip
0.5pt[i]}$, an approximation of the MMSE state estimates
$\hat{\bar{\bm{x}}}^{\text{MMSE}}_{n}$ and $\hat{\bm{y}}^{(j)\hskip
0.5pt\text{MMSE}}_{n}$ in (21), (22) and (23) is given as
$\hat{\bar{\bm{x}}}^{\text{MMSE}}_{n}\approx\sum_{i=1}^{I}\bar{\bm{x}}_{n}^{\prime\hskip
0.5pt[i]}\,{{}\bar{w}_{{\bar{\mathsf{\mathbf{x}}}}}}^{[i]}$ and
$\hat{\bm{y}}^{(j)\hskip
0.5pt\text{MMSE}}_{n}\approx\sum_{i=1}^{I}{\bm{y}}_{n}^{\prime\hskip
0.5pt(j)\hskip 0.5pt[i]}\,{{}\bar{w}_{{\mathsf{\mathbf{y}}}}}^{(j)\hskip
0.5pt[i]}$.

To avoid particle degeneracy [49], a resampling step888We suggest to use
“systematic” resampling for efficiency [49]. is performed as a preparation for
the next time step $n+1$ leading to equally weighted particles
$\\{\bar{\bm{\mathsfbr{x}}}_{n-1}^{[i]},{{}{w}_{{\bar{\mathsf{\mathbf{x}}}}}}{}_{n-1}^{[i]}=1/I\\}_{i=1}^{I}$
and $\\{{\bm{\mathsfbr{y}}}_{n-1}^{(j)\hskip
0.5pt[i]},{{}{w}_{{\mathsf{\mathbf{y}}}}}{}_{n-1}^{(j)\hskip
0.5pt[i]}=1/I\\}_{i=1}^{I}$ for all $j\in\mathcal{J}$ representing the beliefs
$\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{n})$ and
$\breve{f}_{\text{{y}}}(\bm{y}_{n}^{(j)})$.

The resulting problem complexity scales only linearly in the number of
particles $I$ and in the number of measurements $M_{n}^{(j)}$. For
computational efficiency of the particle-based implementation the LOS LHF of
the normalized amplitude measurement (10) is approximated by a truncated
Gaussian PDF, i.e.,

$f_{\text{L}}({z_{\mathrm{u}}^{(j)}}_{n,m}|u^{(j)}_{n})=f_{\text{TN}}({z_{\mathrm{u}}^{(j)}}_{n,m};\sigma_{\mathrm{u}}(u^{(j)}_{n}),u^{(j)}_{n},\gamma).$

####  VI-B2 Initial State Distributions

We assume the initial state distributions to factorize as
$\breve{f}_{\bar{\text{{x}}}}(\bar{\bm{x}}_{0})=\breve{f}_{{\text{{p}}}}({\bm{p}}_{0})\breve{f}_{{\text{{v}}}}({\bm{v}}_{0})\breve{f}_{\mathrm{\gamma}_{\text{r}}}(\gamma_{\text{r}\hskip
0.5pt0})$ and
$\breve{f}_{\text{{y}}}(\bm{y}_{0}^{(j)})=\breve{f}_{\text{u}}(u_{0}^{(j)})\breve{f}_{\mathrm{\omega}}(\omega_{0}^{(j)})\breve{f}_{\text{b}}(b_{0}^{(j)})\breve{f}_{\mathrm{\gamma}_{\text{f}}}(\gamma_{\text{f}\hskip
0.5pt0}^{(j)})$. We propose to initialize the NLOS shape parameters as
$\breve{f}_{\mathrm{\gamma}_{\text{r}}}(\gamma_{\text{r}\hskip
0.5pt0})=f_{\text{U}}(\gamma_{\text{r}\hskip 0.5pt0},0,d_{\text{max}})$,
$\breve{f}_{\text{b}}(b_{0}^{(j)})=f_{\text{U}}(b_{0}^{(j)},0,d_{\text{max}})$
and $\breve{f}_{\mathrm{\gamma}_{\text{f}}}(\gamma_{\text{f}\hskip
0.5pt0}^{(j)})=f_{\text{U}}(\gamma_{\text{f}\hskip
0.5pt0}^{(j)},0,d_{\text{max}})$. The LOS PMFs are initialized as a discrete
uniform PMF $\tilde{f}_{\text{q}\hskip
0.5pt0}^{(j)}(q_{0}^{(j)})=f_{\text{UD}}(q_{0}^{(j)},\mathcal{Q})$ taking all
values of $\mathcal{Q}$ with equal probability. We assume the velocity vector
${\bm{v}}_{0}$ to be zero mean, Gaussian, with covariance matrix
$\sigma_{\text{v}}^{2}\,\bm{I}_{2}$ and $\sigma_{\text{v}}=6\,\text{m/s}$, as
we do not know in which direction we are moving.

The remainder of the states are initialized heuristically, by assuming an
initial measurement vector $\bm{z}_{0}$ containing $M^{(j)}_{0}$ measurements
to be available. The normalized amplitude PDFs are initialized as
$\tilde{f}_{\mathrm{u}\hskip
0.5pt0}^{(j)}(u_{0}^{(j)})=f_{\text{TRayl}}(u_{0}^{(j)};\,{{z}_{\mathrm{u}}^{(j)}}_{0,\text{max}},$
$\,0.05\,{{z}_{\mathrm{u}}^{(j)}}_{0,\text{max}},\gamma)$ where
${{z}_{\mathrm{u}}^{(j)}}_{0,\text{max}}$ is the maximum normalized amplitude
measurement in $\bm{z}_{0}^{(j)}$. The position state is initialized as
$f(\bm{p}_{0})\sim\prod_{j=1}^{J}\prod_{m=1}^{M^{(j)}_{0}}f_{\text{L}}({z_{\mathrm{d}}^{(j)}}_{0,m}|\bm{p}_{\hskip
0.5pt\text{init}},{{z}_{\mathrm{u}}^{(j)}}_{0,\text{max}})\,f(\bm{p}_{\hskip
0.5pt\text{init}})$, where the proposal distribution $f(\bm{p}_{\hskip
0.5pt\text{init}})$ is drawn uniformly on two-dimensional discs around each
anchor $j$, which are bounded by the maximum possible distance
$d_{\text{max}}$ and a sample is drawn from each of the $J$ discs with equal
probability. The DNR PDFs are initialized as
$\breve{f}_{\mathrm{\omega}}(\omega_{0}^{(j)})=f_{\text{TRayl}}(\omega_{0}^{(j)};\,\omega_{\mathrm{init}}^{(j)},\,0.05\,\omega_{\mathrm{init}}^{(j)},\gamma)$,
where $\omega_{\mathrm{init}}^{(j)}$ is determined as described in the
supplementary material [72, Sec. LABEL:M-sec:app_inr_init].

####  VI-B3 Normalization of the NLOS Distance Likelihood

As discussed in Sec. IV-B, the NLOS LHF in (IV-B) must be normalized by
$Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$. However,
$Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$ cannot be determined analytically
and, being a function of $\bm{p}_{n}$ and $\bm{\zeta}_{n}^{(j)}$, it needs to
be calculated for each individual particle (see Sec. VI-B1). Thus, we need an
efficient numerical approximation. For details see the supplementary material
[72, Sec. LABEL:M-sec:app_numerical].

##  VII Results

We validate the proposed model and analyze the performance gain caused by the
features of the proposed algorithm using both synthetic data obtained using
numerical simulation and real radio measurements. The performance is compared
with the P-CRLB and that of the AIPDA. For synthetic measurements with
geometry related multipath999Note that for measurements involving stochastic
multipath as in Sec. VII-B1, the system model of the MP-SLAM algorithm is not
suited, leading to divergence of the track., we also compare to the MP-SLAM
algorithm presented in [22, 44].

###  VII-A Common Analysis Setup

The following setup and parameters are commonly used for all analyses
presented.

The PDF of the joint agent state $\bar{\bm{\mathsfbr{x}}}_{n}$ is factorized
as
$f(\bar{\bm{x}}_{n}|\bar{\bm{x}}_{n-1})=f({\bm{x}}_{n}|{\bm{x}}_{n-1})\,f(\gamma_{\text{r}\hskip
0.5ptn}|\gamma_{\text{r}\hskip 0.5ptn-1})$, where the agent motion, i.e. the
state transition PDF $f(\bm{x}_{n}|\bm{x}_{n-1})$ of the agent state
$\bm{\mathsfbr{x}}_{n}$, is described by a linear, constant velocity and
stochastic acceleration model[62, p. 273], given as
$\bm{\mathsfbr{x}}_{n}=\bm{A}\,\bm{\mathsfbr{x}}_{n-1}+\bm{B}\,\bm{\mathsfbr{w}}_{n}$,
with the acceleration process $\mathsfbr{w}_{n}$ being i.i.d. across $n$, zero
mean, and Gaussian with covariance matrix
${\sigma_{\text{a}}^{2}}\,\bm{I}_{2}$, ${\sigma_{\text{a}}}$ is the
acceleration standard deviation, and $\bm{A}\in\mathbb{R}^{\text{4x4}}$ and
$\bm{B}\in\mathbb{R}^{\text{4x2}}$ are defined according to [62, p. 273], with
sampling period $\Delta T$. The state transition of the rise distance
$\sgamma_{\text{r}\hskip 0.5ptn}$, i.e., the state transition PDF
$f(\gamma_{\text{r}\hskip 0.5ptn}|\gamma_{\text{r}\hskip 0.5ptn-1})$, is
$\sgamma_{\text{r}\hskip 0.5ptn}=\sgamma_{\text{r}\hskip
0.5ptn-1}+\sepsilon_{\gamma_{\text{r}\hskip 0.5ptn}}$, where the noise
$\sepsilon_{\gamma_{\text{r}\hskip 0.5ptn}}$ is i.i.d. across $n$, zero mean,
Gaussian, with variance $\sigma^{2}_{\mathrm{\gamma}_{\text{r}}}$. Similarly,
the state transition model of the joint anchor state
$\bm{\mathsfbr{y}}_{n}^{(j)}$, i.e. the state transition PDF
$f(\bm{y}_{n}|\bm{y}_{n-1})$, is chosen as
$\bm{\mathsfbr{y}}_{n}^{(j)}=\bm{\mathsfbr{y}}_{n-1}^{(j)}+\bm{\sepsilon}_{\text{{y}}\hskip
0.5ptn}^{(j)}$ , where the noise vector $\bm{\sepsilon}_{\text{{y}}\hskip
0.5ptn}^{(j)}$ is i.i.d. across $n$ and $j$, zero mean, jointly Gaussian, with
covariance matrix
$\mathrm{diag}\\{[\sigma^{2}_{\text{u}}\,\sigma^{2}_{\mathrm{\omega}}\,\sigma^{2}_{\text{b}}\,\sigma^{2}_{\mathrm{\gamma}_{\text{f}}}]\\}$
and the individual state-transition variances (STV) $\sigma^{2}_{\text{u}}$,
$\sigma^{2}_{\mathrm{\omega}}$, $\sigma^{2}_{\text{b}}$ and
$\sigma^{2}_{\mathrm{\gamma}_{\text{f}}}$. Unless noted differently the STV
are set as $\sigma_{a}=2~{}\mathrm{m/s^{2}}$,
$\sigma_{\text{u}}=0.05\,\hat{u}_{n-1}^{(j)\hskip 0.5pt\text{MMSE}}$,
$\sigma_{\mathrm{\omega}}=0.05\,\hat{\omega}_{n-1}^{(j)\hskip
0.5pt\text{MMSE}}$, $\sigma_{\text{b}}=0.05\,\hat{b}_{n-1}^{(j)\hskip
0.5pt\text{MMSE}}$,
$\sigma_{\mathrm{\gamma}_{\text{f}}}=0.05\,\hat{\gamma}_{\text{f}\hskip
0.5pt{n-1}}^{(j)\hskip 0.5pt\text{MMSE}}$,
$\sigma_{\mathrm{\gamma}_{\text{r}}}=0.5\,\hat{\gamma}_{\text{r}\hskip
0.5pt{n-1}}^{\text{MMSE}}$. While $\sigma_{a}$ is set according to the maximum
agent acceleration [62], for the STV of all other parameters we use values
relative to the root mean squared error (RMSE) estimate of the previous time
step $n-1$ as a heuristic. Note that this choice allows no tuning of the STV
to be required for all experiments presented, even though the propagation
environments are considerably different. We used $5\cdot 10^{4}$ particles
before the first resampling operation and $5000$ particles for inference
during the track. We set the detection threshold as low as $\gamma=1.77$
($5\,\mathrm{dB}$) for all simulations, which allows the algorithm to
facilitate low-energy MPCs (this choice is further discussed in Sec. VII-B).
The set of possible LOS probabilities is chosen as
$\mathcal{Q}=\\{0.01,0.33,0.66,1\\}$. The state transition matrix
$\bm{Q}^{(j)}\triangleq\bm{Q}$ is set as follows: $[\bm{Q}]_{1,1}=0.9$,
$[\bm{Q}]_{4,4}=0.95$, $[\bm{Q}]_{2,1}=0.1$ and $[\bm{Q}]_{3,4}=0.05$. For
$2\leqslant k\leqslant 3$, $[\bm{Q}]_{k,k}=0.85$, $[\bm{Q}]_{k-1,k}=0.05$ and
$[\bm{Q}]_{k+1,k}=0.1$. For all other tuples $\\{i,k\\}$, $[\bm{Q}]_{i,k}=0$
in order to encourage high LOS probabilities [57]. For the numerical
approximation of $Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$ as discussed in Sec.
VI-B3, we used $K_{\text{T}}=30$. The results are shown in terms of the RMSE
of the estimated agent position
$e_{n}^{\text{RMSE}}~{}=~{}\sqrt{\mathbb{E}\left\\{\lVert\hat{\bm{p}}^{\text{MMSE}}_{n}-\bm{p}_{n}\rVert^{2}\right\\}}$,
evaluated using a numerical simulation with 500 realizations. For each of the
scenarios investigated, we consistently analyze the influence of the
individual features of our algorithm according to Table. II. It shows the
algorithm variants implemented and the corresponding features that are enabled
for an algorithm (x) or not ( ).

TABLE II: Algorithm variants investigated for different scenarios | AL1 | AL2 | AL3 | AL4 | AL5 | AL6 |  AL4′ |  AL5′
---|---|---|---|---|---|---|---|---
|  |  |  |  |  |  |  |
$q_{n}^{(j)}$ tracking |  |  | x | x | x |  | x | x
Non-uniform $f_{\text{NL}}$ |  | x |  | x | x |  | x | x
Decoupled SPA |  | x | x |  | x |  |  | x
$5\cdot 10^{4}$ particles |  |  |  |  |  | x | x | x
MP-SLAM |  |  |  |  |  | x |  |
Shown in Figs. |  8c, 8d, 10 |  10a, 10b |  8c, 8d

When “$q_{n}^{(j)}$ tracking” is deactivated, we set $q_{n}^{(j)}=0.999$ for
all $n$, $j$. When we use “decoupled SPA”, the suboptimal message passing
scheme presented in Sec. VI-A is used. Not applying the “non-uniform
$f_{\text{NL}}$” means (12) is replaced by $s^{2}_{\text{u}}\triangleq 1/2$,
and for AL4′ and AL5′ we use $5\cdot 10^{4}$ particles instead of $5000$. Note
that AL1 represents a multi-sensor variant of the conventional AIPDA. The MP-
SLAM algorithm is implemented according to [44, 22] using the measurements
$\bm{z}_{m,n}^{(j)}$, i.e., distance and amplitude measurements, as an input.
For consistency, the state transition PDFs and initial state distributions of
the agent state and normalized amplitude state are set as described in Sec.
VII-A and VI-B2. For convergence, we had to use $5\cdot 10^{4}$ particles and
an anchor driving noise of $\sigma_{\text{An}}=0.02~{}\mathrm{m}$, other
parameters are $P_{\text{s}}=0.999$, $\mu_{\text{n,1}}=0.05$. The mean number
of false alarms was approximated as
$\mu_{\text{FA}}=N_{\text{s}}\,e^{-\gamma^{2}}$ (see [44] for definitions).
For stability we increased the delay measurement variances of all virtual
anchors (not the physical anchors) by a factor of $2$ with respect to the
Fisher information-based value.

Figure 5: Simulated trajectory and anchor positions for all synthetic
experiments (Sec. VII-B) and environment setup (walls and resulting
obstructions) for Geometry-related synthetic experiments (Sec. VII-B2).

Figure 6: MMSE estimates (determined using fully synthetic measurements and
CEDA-based measurements) and true values of all state variables v.s. discrete
time $n$ for the experiment described in Sec. VII-B1. We show the mean MMSE
estimate and the corresponding range from minimum to maximum value. Anchor
state variables are only shown for anchor A1. Different shades of gray
represent different numbers of anchors in OLOS according to Fig. 5.

As a performance benchmark, we provide the CRLB on the position error variance
considering all visible LOS measurements of a single time step $n$, which we
refer to as the snapshot-based positioning CRLB (SP-CRLB) [76, 7, 59, 77].
Furthermore, we provide the corresponding posterior Cramér-Rao lower bound
(P-CRLB) [60] that additionally considers the dynamic model of the agent state
and the “P-CRLB-LOS”, which is the P-CRLB assuming the LOS component to all
anchors is always available and, thus, provides a lower bound for the proposed
estimator. See the supplementary material [72, Sec. LABEL:M-sec:app_crlb] for
further details.

###  VII-B Analysis on Synthetic Measurements

For the synthetic setup, we investigate the scenario shown in Fig. 5. The
agent moves along a trajectory, with two distinct direction changes, where the
agent velocity is set to vary around a magnitude of $0.8\,\mathrm{m/s}$. It is
observed at $N=190$ discrete time steps $n\in\\{1,\,...\,,N\\}$ at a constant
observation rate of $\Delta T=100\,\mathrm{ms}$, resulting in a continuous
observation time of $19\,\mathrm{s}$. We simulate three anchors, A1-A3, which
are placed in close vicinity to each other. The limited directional diversity
of the anchors (corresponding to a poor geometric dilution of precision (GDOP)
[78]), poses a challenging setup for delay measurement-based position
estimation. Note that the environment setup shown in Fig. 5, i.e., walls and
resulting obstructions, are only used in Sec. VII-B2. For all synthetic radio
measurements involving the proposed CEDA (see [72, Sec.
LABEL:M-sec:app_ceda]), we choose the transmitted complex baseband signal
$s(t)$ to be of root-raised-cosine shape with a roll-off factor of $0.6$ and a
duration of $2\,\mathrm{ns}$ (bandwidth of $500\,\mathrm{MHz}$). The signal is
critically sampled, i.e., $T_{\text{s}}=1.25\,\mathrm{ns}$, with a total
number of $N_{\text{s}}=161$ samples, amounting to a maximum distance of
$d_{\text{max}}=60\,\mathrm{m}$.

####  VII-B1 Synthetic Measurements with Stochastic Multipath

In this section we present results using synthetic measurements generated by
simulating the MPCs as zero mean stochastic process. More specifically, we
compare results obtained by simulating the radio signal according to (6) and
applying the CEDA to results obtained using fully synthetic measurements,
which are generated according to Sec. IV without involving the CEDA. For fully
synthetic measurements the average number of NLOS measurements per time $n$
and anchor $j$ prior to the simulated detection process was approximated as
$N_{\text{s}}$. Detection further reduces the prior number of NLOS events by
the mean NLOS detection probability. We simulate two OLOS situations clearly
separated in time, a partial one at $n\in[75,104]$, where only the LOS to
anchor $A2$ is blocked, and a full one at $n\in[115,144]$, where the LOS to
all anchors is blocked. The following true system parameters are used, which
are set constant for all time steps $n$ and anchors $j$: The normalized
amplitude is set to
$\tilde{\bm{u}}_{n}=[\sqrt{19.5}\,\mathrm{dB}~{}\sqrt{20.0}\,\mathrm{dB}~{}\sqrt{20.5}\,\mathrm{dB}]^{\text{T}}$
and the parameters of the DPS are set to $\tilde{\omega}_{n}^{(j)\hskip
0.5pt2}=\sqrt{25}\,\mathrm{dB}$, $\tilde{\gamma}_{\text{r}\hskip
0.5ptn}=0.7\,\mathrm{m}$, $\tilde{\gamma}_{\mathrm{f}\hskip
0.5ptn}^{(j)}=6\,\mathrm{m}$, $\tilde{b}_{n}^{(j)}=0.7\,\mathrm{m}$.

Figure 7: npe of (a) the Rayleigh scale parameter of the amplitude
measurements and (b) the relative frequency of the distance measurements
compared to (a) the NLOS scale function and (b) the NLOS distance LHF. All
values are shown as a function of the difference of the distance measurement
and the corresponding LOS component distance, given as
${z_{\mathrm{d}}^{(1)}}_{n,m}-d_{\text{LOS}\hskip
0.5pt}^{(1)}(\tilde{\bm{p}}_{200})$ for anchor A1.

(a)

(b)

(c)

(d)

Figure 8: Performance of different algorithm variants in terms of the RMSE of
the estimated agent position (a), (c) as a function of the discrete
observation time $n$, and (b), (d) as the cumulative frequency in inverse
logarithmic scale, determined from numerical simulation of stochastic
multipath according to Sec. VII-B1. Different shades of gray represent
different numbers of anchors in OLOS according to Fig. 5.

We start by validating the system model presented in Sec. IV. For this
experiment the relatively defined STV are set with respect to the true values
instead of the RMSE values, given as
$\sigma_{\text{u}}=0.05\,\tilde{u}_{n}^{(j)}$,
$\sigma_{\mathrm{\omega}}=0.05\,\tilde{\omega}_{n}^{(j)}$,
$\sigma_{\text{b}}=0.05\,\tilde{b}_{n}^{(j)}$,
$\sigma_{\mathrm{\gamma}_{\text{f}}}=0.05\,\tilde{\gamma}_{\text{f}\hskip
0.5ptn}^{(j)}$,
$\sigma_{\mathrm{\gamma}_{\text{r}}}=0.5\,\tilde{\gamma}_{\text{r}\hskip
0.5ptn}$. Fig. 6, 7, 8a and 8b show the results of the performed numerical
simulations. Fig. 6 shows MMSE estimates of all state variables as a function
of time $t^{\prime}$ and compares to the respective true values. The MMSE
estimates are determined according to (21)-(23) using both fully synthetic
measurements and CEDA-based measurements. Fig. 7 compares distance-model-
agnostic, bin-based estimates (BBEs) of scale parameter and relative
measurement frequency with the presented model functions, i.e., with the NLOS
scale function (12) and the NLOS distance LHF (IV-B). Each of the functions is
determined both ways, using the MMSE estimates of $\bm{\szeta}_{200}^{(1)}$ of
the last time step, given as $\hat{\bm{\zeta}}_{200}^{(1)\text{MMSE}}$ and
using the respective true values used for simulation
$\tilde{\bm{\zeta}}^{(1)}$. The bin-based estimates are determined using all
NLOS measurements (the LOS measurements are removed) of the last 20 time
steps, given as
$\\{\bm{z}_{n,m}^{(1)}\,|\,{m\in\mathcal{M}_{n}^{(1)}\setminus\tilde{a}_{n}^{1},n\in\\{180,\,...\,,200\\}}\\}$.
For details about the BBEs see the supplementary material [72, Sec.
LABEL:M-sec:app_nonparametric]. This analysis is complemented by Figs. 8a and
8b which show the position RMSE $e_{n}^{\mathrm{RMSE}}$ in two ways. First, as
a function of the discrete observation time $n$ and, second, as the cumulative
frequency of the RMSE evaluated over the whole time span. Fig. 6 demonstrates
that using CEDA-based measurements the MMSE estimates of the parameters of the
NLOS LHF (i.e., the MMSE estimates corresponding to $\bm{\szeta}_{n}^{(1)}$ )
are slightly biased, in particular the DNR estimate
$\hat{\omega}_{n}^{(1)\hskip 0.5pt\text{MMSE}}$. This effect is a consequence
of the asymptotic bandwidth assumption used in the derivation of the NLOS
likelihood model (see [72, Sec. LABEL:M-sec:app_nlos_model]). However, as in
Fig. 6 the model functions parameterized with the MMSE values accurately fit
the BBEs, the MMSE estimate of the agent position
$\hat{{\bm{p}}}^{\text{MMSE}}_{n}$ in Fig. 6 remains unbiased and, thus, the
positioning performance in Figs. 8a and 8b using “CEDA-based measurements” is
identical to the performance using “fully synthetic measurements” up to random
deviations.

In addition, in Figs. 8a and 8b we compare to fully synthetic measurements
with (i) known initial state distributions, slightly lowering the RMSE around
$n=0$, and (ii) assuming the parameters of $\bm{\szeta}_{n}^{(j)}$ to be known
constants, leading to a significant increase of performance at the end of the
full OLOS situation as the bias information does not vanish over discrete time
$n$. With CEDA-based measurements we also compare to results where (i) we
calculate the relatively defined STV using the RMSE values of the respective
last time step $n-1$ according to Sec. VII-A and where (ii) we use a uniform
delay intensity function
$f_{\text{NL}}({z_{\mathrm{d}}^{(j)}}_{n,m})=1/d_{\text{max}}$ showing no
significant degradation of performance. The latter result suggests that for
low values of $\gamma$, the information provided in (IV-B) is insignificant
(c.f. Fig. 2b). Therefore, in what follows, we keep the uniform delay
intensity function leading to a considerable reduction of runtime since
$Q_{0}(\bm{p}_{n},\bm{\zeta}_{n}^{(j)})$ does not need to be calculated (see
also Sec. VI-B3 and Sec. VII-D). Next, we investigate the influence of the
individual features of our algorithm as described in Sec. VII-A and Table II.
Figs. 8c and 8d show the RMSE of this experiment as a function of $t^{\prime}$
as well as the cumulative frequency of the RMSE.

Figure 9: A single measurement realization and the respective MMSE estimates
using the proposed algorithm (AL5) for geometry-related synthetic measurements
(see Sec. VII-B2). We show MMSE estimates in (a) distance domain
$\hat{d}_{n,0}^{(1)\hskip 0.5pt\text{MMSE}}$, (c) amplitude domain
($\hat{u}_{n}^{(1)\hskip 0.5pt\text{MMSE}\hskip 0.5pt2}$,
$\hat{\omega{}}_{n}^{(1)\hskip 0.5pt\text{MMSE}\hskip 0.5pt2}$), (d) LOS
probability domain $\hat{q}_{n}^{(1)\hskip 0.5pt\text{MMSE}}$, and (b)
amplitude as a function of distance domain for $n\in\\{130,\,...\,,136\\}$.
The measurements in (b) correspond to the upper and in (c) to the left axis.
Note that in (a) and (c) only measurements for every third time $n$ are shown
(i,e., $n\in\\{1,4,7,...\\}$).

The RMSE of the multi-sensor AIPDA (AL1) mostly attains the P-CRLB during LOS
and partial OLOS situations. A reason for that is that the angle, which the
remaining anchors A1 and A3 span with respect to the agent is sufficiently
large to provide a reasonable position estimate. However, AL1 shows a slightly
increased RMSE around $n=80$ due to the agent direction change and
significantly deviates from the very beginning of the full OLOS situation,
losing the track in every single realization. Comparing the curves of AL2-AL5,
one can conclude that every single algorithm feature investigated lowers the
RMSE significantly when activated. The RMSE of the proposed algorithm AL5
constantly attains the P-CRLB, which indicates no lost track, even falling
below the P-CRLB in full OLOS situations. This is possible as it leverages the
additional position information contained inside the MPCs via the non-uniform
NLOS LHF, which is not considered by the P-CRLB model. In contrast, AL2 loses
a large percentage of tracks after the full OLOS situation, because NLOS
measurements significantly contribute to the LOS based position hypotheses due
to the insufficient representation of the existence probability by the
amplitude state particles (see Sec. IV-D). While AL3 constantly attains the
P-CRLB during the LOS situation as well the partial OLOS situation, it loses
the track for every realization in full OLOS. After a short amount of time in
which AL3 can maintain the agent position through the agent state transition
model and the decreasing LOS probability, it identifies MPCs as the LOS
component due to their coherent appearance and large amplitude, which is not
covered by the uniform NLOS model, and loses the track. AL4 shows a seemingly
random performance degradation, which is due to the insufficient
representation of the high dimensional joint state by the particle filter and
some resulting lost tracks, which AL5 overcomes by decoupling the anchor
states (see Sec. VI). However, the discrepancy between AL4 and AL5 can be
dissolved by using a sufficiently high number of particles (see AL4′ and
AL5′), at the cost of significantly increasing the runtime (see Sec. VII-D).

####  VII-B2 Synthetic Measurements with Geometry-related Multipath

In this section, we discuss results using synthetic measurements based on the
simple floorplan shown in Fig. 5. The measurements are obtained by simulating
a radio signal according to (5), consisting of the LOS component and specular
MPCs, and using the proposed CEDA. The MPC delays are calculated out of the
floorplan (i.e. W1-W5) using the mirror images (virtual anchors) up to the
third order [24]. The SNR of the LOS component as well as the MPCs [48] are
set to ${20}\,\mathrm{dB}$ at a distance of $1\,\mathrm{m}$ and are assumed to
follow free-space path loss. The SNR of the individual MPCs are additionally
attenuated by $3$ dB after each reflection (e.g., $6$ dB for a second-order
reflection). As depicted in Fig. 5, for this experiment the anchors are
obstructed by an obstacle (W5), which leads to partial and full OLOS
situations in the center of the investigated trajectory.

(a)

(b)

(c)

(d)

Figure 10: Performance of all algorithm variants of Table II in terms of the
RMSE of the estimated agent position (a), (c) as a function of the discrete
observation time $n$, and (b), (d) as the cumulative frequency in inverse
logarithmic scale, determined from numerical simulation of specular MPCs
according to Sec. VII-B2 in (a), (b) and using real radio measurements
according to Sec. VII-C in (c), (d). Different shades of gray represent
different numbers of anchors in OLOS according to Fig. 5.

Figs. 9, 10a, and 10b show results of the performed numerical simulation. Fig.
9 provides a graphical representation of the measurement space, showing a
single measurement realization $\bm{z}$ together with the corresponding MMSE
estimates of the proposed algorithm (AL5). The MMSE estimates are determined
according to (21)-(23). In particular, Fig. 9a shows that (i) the MMSE
estimate of the LOS delay $\hat{d}^{(j)\hskip
0.5pt\text{MMSE}}_{n,0}=d^{(j)}_{\text{LOS}}(\hat{\bm{p}}^{\text{MMSE}}_{n})$
remains stable over the whole OLOS situation and that (ii) the maximum of the
NLOS LHF follows the first MPC available. We determine the shape of the NLOS
LHF using the respective MMSE estimates of all RVs on which (IV-B) depends.
Fig. 9c shows that the DNR estimate $\hat{\omega}_{n}^{(j)\hskip
0.5pt\text{MMSE}}$ accurately represents the dynamic behavior of the multipath
energy, deceasing rapidly when the strongest, first MPC is covered, while the
SNR estimate $\hat{u}_{n}^{(j)\hskip 0.5pt\text{MMSE}}$ remains stable. For
visualization, Fig. 9b shows the NLOS scale function at time $n=133$
parametrized with the respective MMSE estimates of all NLOS function
parameters. Fig. 9d shows the LOS existence probability $q_{n}^{(1)}$ well
representing the OLOS situation. Figs. 10a and 10b show the RMSE as a function
of the discrete observation time $n$ as well as the cumulative frequency of
the RMSE. Again, we investigate the influence of the individual features of
our algorithm according to Sec. VII-A and Table II. Comparing the presented
curves, we again observe AL5 to significantly outperform AL1-AL5, with the
qualitative performance differences being almost identical to those of Sec.
VII-B1. The only significant dissimilarity is the seemingly smaller deviation
between AL4 and AL5. This is because AL4 does not lose any tracks during
initialization, as the average energy and distance to the LOS component of the
measurements of the first time step $n=0$ are significantly lower in this
scenario, leading to a better coverage of the state space by the particle
filter. Thus, we only observe a slightly more unstable local behavior of AL4.
The MP-SLAM algorithm (AL6) achieves a significantly reduced RMSE during the
first part of the OLOS situation, due to geometric information provided by the
specular MPCs, outperforming the proposed method (AL5). However, the
investigated scenario is geometrically ambiguous as there is little
directional change in the agent movement [28]. Also there are many low-SNR
components, which disappear and reappear, due to the obstacle (W5). This is
why AL6 follows ambiguous paths for many realizations (i.e., it loses the
track), leading to a significantly reduced performance after the full OLOS
situation. We additionally added AL6∗, which represents the numerical results
after removing $20.6\%$ (103 realizations) of diverged tracks. This result
demonstrates the dramatically increased accuracy that can be obtained using
MP-SLAM.

###  VII-C Performance for Real Radio Measurements

For further validation of the proposed algorithm, we use real radio
measurements collected in a laboratory hall of NXP Semiconductors, Gratkorn,
Austria. The hall, shown in Fig. 11a, features a wide, open space and includes
a demonstration car (Lancia Thema 2011), furniture, and metallic surfaces,
thereby representing a typical multipath-prone industrial environment. An
agent is assumed to move along a pseudo-random trajectory (selected out of a
grid of agent positions), obtained in a static measurement setup. We selected
$N=195$ measurements, assuming an observation rate of $\Delta
T=170\,\mathrm{ms}$. The agent velocity is set to vary around a magnitude of
$0.35\,\mathrm{m/s}$. This leads to a corresponding continuous observation
time $33.15\,\mathrm{s}$. At each selected position, a radio signal was
transmitted from the assumed agent position, which was received by 4 anchors.
The agent was represented by a polystyrene build, while the anchor antennas
were mounted on the demonstration car. The agent as well as the anchors were
equipped with a dipole antenna with an approximately uniform radiation pattern
in the azimuth plane and zeros in the floor and ceiling directions. The radio
signal was recorded by an M-sequence correlative channel sounder with
frequency range $3-10\,\mathrm{GHz}$. Within the measured band, the actual
signal band was selected by a filter with root-raised-cosine impulse response
$s(t)$, with a roll-off factor of $0.6$, a two-sided 3-dB bandwidth of
$B=499.2\,\mathrm{MHz}$ and a center frequency of $7.9872\,\mathrm{GHz}$
(corresponding to channel 9 of IEEE 802.15.4a), and critically sampled with
$T_{\text{s}}=1/(1.6\,B)$. We used $N_{\text{s}}=161$ samples, amounting to a
maximum distance of $d_{\text{max}}=60\,\mathrm{m}$ for the CEDA. We created
two full OLOS situations at $n\in[80,92]$ and $n\in[159,170]$ using an
obstacle consisting of a metal plate covered with attenuators as shown in Fig.
11b. A floor plan showing the track, the environment (i.e, the car, other
reflecting objects and walls), the antenna positions, and OLOS conditions with
respect to all antennas is shown in Fig. 11c. The metal surface of the car
strongly reflected the radio signal, leading to a radiation pattern of
$270^{\circ}$ for A1 and A2 and $180^{\circ}$ for A3 and A4. Thus, during
large parts of the trajectory the LOS of 2 or 3 out of 4 anchors is not
available. Moreover, the pulse reflected by the car surface strongly
interferes with the LOS pulse, leading to significant fluctuations of the
amplitudes. In addition, this leads to the channel estimator being prone to
produce a high SNR component just after the LOS component. As this violates
our signal model, we processed the CEDA measurements attenuating all
components, where
${z_{\mathrm{d}}^{(j)}}_{n,m}\in\tilde{d}_{n,0}^{(j)}+\text{[}0,\;2\,c\,T_{\text{p}}]$,
except for the highest component. As only two antennas (A1 and A2) are visible
at the track starting point, the position estimate obtained by trilateration
is ambiguous. In the scenario presented, the relative antenna position with
respect to the car can be assumed to be known. Thus, for this experiment, we
used the antenna pattern as prior information for initialization of the
position state. For the numerical evaluation presented, we added AWGN to the
real radio signal obtained. We set
$\lVert\bar{\bm{r}}_{\mathrm{raw}}^{(j)}\rVert^{2}/\sigma^{(j)\hskip
0.5pt2}=20\,\mathrm{dB}$, where
$\lVert\bar{\bm{r}}_{\mathrm{raw}}^{(j)}\rVert^{2}$ is the average energy of
the real measured signal per anchor $j$. Figs. 10c and 10d show the RMSE as a
function of the discrete observation time $n$ as well as the cumulative
frequency of the RMSE. Again, we analyze the influence of the individual
features of our algorithm according to Sec. VII-A and Table II and observe AL5
to significantly outperform the other algorithm variants. Different to Sec.
VII-B2 all presented algorithms fail to reach the P-CRLB over parts of the
track. The exact consistency in progression of the RMSE curves suggests
unmodeled effects (e.g. diffraction at the vehicle body) as well as
inaccuracies in the reference as a probable reason.

###  VII-D Runtime

Table III shows the average runtime of the proposed algorithm (A5) and
compares it to the runtime of the multi-sensor AIPDA (AL1) and that of the MP-
SLAM algorithm (AL6). All runtimes are estimated using Matlab implementations
executed on an AMD Ryzen Threadripper 1900X 8-Core Processor with up to
$4\,\text{GHz}$ for all scenarios investigated. We also show the average
number of measurements (over all anchors and time steps) ${M}_{\text{mean}}$,
the number of anchors $J$ and the number of particles, which determine the
algorithm complexity per time step. The runtime of our algorithm (AL5) is of
the same order of magnitude than that of the multi-sensor AIPDA (AL1), which
is in the range of tens of milliseconds for all scenarios investigated. In
contrast, the runtime of the MP-SLAM algorithm (AL6) is significantly higher,
since it requires joint data association between all map features [22] and a
higher number of particles for numerical stability.

TABLE III: Algorithm runtimes and characteristic values of all investigated scenarios. |  particles $I$ | Sec. VII-B1 | Sec. VII-B2 | Sec. VII-C
---|---|---|---|---
proposed (AL5) | 5000 | $53\,\mathrm{ms}$ | $34\,\mathrm{ms}$ | $30\,\mathrm{ms}$
AIPDA (AL1) | 5000 | $40\,\mathrm{ms}$ | $27\,\mathrm{ms}$ | $23\,\mathrm{ms}$
MP-SLAM (AL6) | 30000 | n.a. | $1.6\,\mathrm{s}$ | n.a.
${M}_{\text{mean}}\times J$ |  | $28\times 3$ | $12\times 3$ | $7\times 4$

(a)  (b)  (c)

Figure 11: Measurement setup for real radio-signal experiments described in
Sec. VII-C. We show pictures of (a) the overall scenario and (b) the OLOS
setup used, as well as (c) the abstracted floorplan and trajectory.

##  VIII Conclusion

We have presented a particle-based sum-product algorithm (SPA) that
sequentially estimates the position of a mobile agent using range and
amplitude measurements provided by a snapshot-based channel estimation and
detection algorithm (CEDA). We introduced a novel non- (NLOS) model that is
adapted to the delay power spectrum (DPS) of the multipath radio channel. We
analyzed the performance of the proposed algorithm using both numerically
simulated and real measurements in different channel conditions and showed
that the additional information provided by the NLOS model can support the
estimation of the agent position. Our algorithm significantly outperformed the
conventional AIPDA filter and consistently attained the P-CRLB in partial OLOS
situations (i.e., no lost tracks). While multipath-based SLAM (MP-SLAM) can
naturally outperform our algorithm in channels showing resolved, specular
MPCs, we demonstrate the proposed algorithm to offer a significantly smaller
number of lost tracks at reduced execution time in a geometrically ambiguous
scenario.

A possible direction for future research includes extending the model to
multiple biases with respect to several MPCs using joint probabilistic data
association and dynamic MPC initialization [46, 48] or to several MPC clusters
by using data association with extended objects [47].

## References

  * [1] K. Witrisal, P. Meissner _et al._ , “High-accuracy localization for assisted living: 5G systems will turn multipath channels from foe to friend,” _IEEE Signal Process. Mag._ , vol. 33, no. 2, pp. 59–70, Mar. 2016.
  * [2] E. Leitinger, P. Meissner, C. Ruedisser, G. Dumphart, and K. Witrisal, “Evaluation of position-related information in multipath components for indoor positioning,” _IEEE J. Sel. Areas Commun._ , vol. 33, no. 11, pp. 2313–2328, Nov. 2015.
  * [3] R. Mendrzik, H. Wymeersch, G. Bauch, and Z. Abu-Shaban, “Harnessing NLOS Components for Position and Orientation Estimation in 5G Millimeter Wave MIMO,” _IEEE Trans. Wireless Commun._ , vol. 18, no. 1, pp. 93–107, 2019.
  * [4] Y. Wang, K. Gu, Y. Wu, W. Dai, and Y. Shen, “NLOS effect mitigation via spatial geometry exploitation in cooperative localization,” _IEEE Trans. Wireless Commun._ , vol. 19, no. 9, pp. 6037–6049, 2020.
  * [5] A. Richter, “Estimation of Radio Channel Parameters: Models and Algorithms,” Ph.D. dissertation, Ilmenau University of Technology, 2005.
  * [6] D. Dardari, A. Conti, U. Ferner, A. Giorgetti, and M. Z. Win, “Ranging with ultrawide bandwidth signals in multipath environments,” _Proc. IEEE_ , vol. 97, no. 2, pp. 404–426, Feb. 2009.
  * [7] Y. Shen and M. Z. Win, “Fundamental limits of wideband localizationpart i: A general framework,” _IEEE Trans. Inf. Theory_ , vol. 56, no. 10, pp. 4956–4980, 2010.
  * [8] S. Aditya, A. F. Molisch, and H. M. Behairy, “A survey on the impact of multipath on wideband time-of-arrival based localization,” _Proc. IEEE_ , vol. 106, no. 7, pp. 1183–1203, 2018.
  * [9] J. Karedal, S. Wyne, P. Almers, F. Tufvesson, and A. Molisch, “A measurement-based statistical model for industrial ultra-wideband channels,” _IEEE Trans. Wireless Commun._ , vol. 6, no. 8, pp. 3028–3037, Aug. 2007\.
  * [10] J. Kulmer, F. Wen, N. Garcia, H. Wymeersch, and K. Witrisal, “Impact of rough surface scattering on stochastic multipath component models,” in _Proc. IEEE PIMRC 2018_ , Bologna, Italy, Dec. 2018, pp. 1410–1416.
  * [11] S. Jiang, W. Wang, Y. Miao, W. Fan, and A. F. Molisch, “A survey of dense multipath and its impact on wireless systems,” _IEEE Open J. Antennas Propag._ , vol. 3, pp. 435–460, 2022.
  * [12] R. Karlsson and F. Gustafsson, “The future of automotive localization algorithms: Available, reliable, and scalable localization: Anywhere and anytime,” _IEEE Signal Process. Mag._ , vol. 34, no. 2, pp. 60–69, 2017\.
  * [13] J. Ko, T. Gao, R. Rothman, and A. Terzis, “Wireless sensing systems in clinical environments: Improving the efficiency of the patient monitoring process,” _IEEE Eng. Med. Biol. Mag._ , vol. 29, pp. 103–9, 05 2010.
  * [14] A. Kalyanaraman, Y. Zeng, S. Rakshit, and V. Jain, “CaraoKey : Car states sensing via the ultra-wideband keyless infrastructure,” in _Proc. IEEE SECON-20_ , 2020, pp. 1–9.
  * [15] A. Conti, F. Morselli, Z. Liu, S. Bartoletti, S. Mazuelas, W. C. Lindsey, and M. Z. Win, “Location awareness in beyond 5G networks,” _IEEE Commun. Mag._ , vol. 59, no. 11, pp. 22–27, 2021.
  * [16] L. Taponecco, A. D’Amico, and U. Mengali, “Joint TOA and AOA estimation for UWB localization applications,” _IEEE Trans. Wireless Commun._ , vol. 10, no. 7, pp. 2207–2217, 2011.
  * [17] F. Rusek, D. Persson, B. K. Lau, E. G. Larsson, T. L. Marzetta, O. Edfors, and F. Tufvesson, “Scaling up MIMO: Opportunities and challenges with very large arrays,” _IEEE Signal Process. Mag._ , vol. 30, no. 1, pp. 40–60, Jan. 2013.
  * [18] W. M. Gifford, D. Dardari, and M. Z. Win, “The impact of multipath information on time-of-arrival estimation,” _IEEE Trans. Signal Process._ , vol. 70, pp. 31–46, 2022.
  * [19] H. Wymeersch, S. Maranò, W. M. Gifford, and M. Z. Win, “A machine learning approach to ranging error mitigation for UWB localization,” _IEEE Trans. Wireless Commun._ , vol. 60, no. 6, pp. 1719–1728, 2012.
  * [20] E. Leitinger, M. Fröhle, P. Meissner, and K. Witrisal, “Multipath-assisted maximum-likelihood indoor positioning using UWB signals,” in _Proc. IEEE ICCW-14_ , Sydney, NSW, Australia, Jun. 2014, pp. 170–175.
  * [21] C. Gentner, T. Jost, W. Wang, S. Zhang, A. Dammann, and U. C. Fiebig, “Multipath assisted positioning with simultaneous localization and mapping,” _IEEE Trans. Wireless Commun._ , vol. 15, no. 9, pp. 6104–6117, Sep. 2016.
  * [22] 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.
  * [23] A. Shahmansoori, G. E. Garcia, G. Destino, G. Seco-Granados, and H. Wymeersch, “Position and orientation estimation through mm Wave MIMO in 5G systems,” _IEEE Trans. Wireless Commun._ , vol. 17, no. 3, pp. 1822–1835, Mar. 2018.
  * [24] T. Pedersen, “Modeling of path arrival rate for in-room radio channels with directive antennas,” _IEEE Trans. Antennas Propag._ , vol. 66, no. 9, pp. 4791–4805, 2018.
  * [25] E. Leitinger, F. Meyer, P. Meissner, K. Witrisal, and F. Hlawatsch, “Belief propagation based joint probabilistic data association for multipath-assisted indoor navigation and tracking,” in _Proc. ICL-GNSS-16_ , Barcelona, Spain, June 2016, pp. 1–6.
  * [26] H. Kim, K. Granström, L. Gao, G. Battistelli, S. Kim, and H. Wymeersch, “5G mmWave cooperative positioning and mapping using multi-model PHD filter and map fusion,” _IEEE Trans. Wireless Commun._ , vol. 19, no. 6, pp. 3782–3795, Mar. 2020.
  * [27] J. Kulmer, E. Leitinger, S. Grebien, and K. Witrisal, “Anchorless cooperative tracking using multipath channel information,” _IEEE Trans. Wireless Commun._ , vol. 17, no. 4, pp. 2262–2275, Apr. 2018.
  * [28] M. Krekovic, I. Dokmanic, and M. Vetterli, “Shapes from echoes: Uniqueness from point-to-plane distance matrices,” _IEEE Trans. Signal Process._ , vol. 68, pp. 2480–2498, 2020.
  * [29] H. Wymeersch, J. Lien, and M. Z. Win, “Cooperative localization in wireless networks,” _Proc. IEEE_ , vol. 97, no. 2, pp. 427 –450, Feb. 2009.
  * [30] P. Sharma, A.-A. Saucan, D. J. Bucci, and P. K. Varshney, “Decentralized gaussian filters for cooperative self-localization and multi-target tracking,” _IEEE Trans. Signal Process._ , vol. 67, no. 22, pp. 5896–5911, 2019.
  * [31] M. Chiani, A. Giorgetti, and E. Paolini, “Sensor radar for object tracking,” _Proc. IEEE_ , vol. 106, no. 6, pp. 1022–1041, 2018.
  * [32] T. Wang, K. Hu, Z. Li, K. Lin, J. Wang, and Y. Shen, “A semi-supervised learning approach for UWB ranging error mitigation,” _IEEE Wireless Commun. Lett._ , vol. 10, no. 3, pp. 688–691, 2021.
  * [33] S. Marano and, W. Gifford, H. Wymeersch, and M. Win, “NLOS identification and mitigation for localization based on UWB experimental data,” _IEEE J. Sel. Areas Commun._ , vol. 28, no. 7, pp. 1026 –1035, Sept. 2010\.
  * [34] M. Stahlke, S. Kram, F. Ott, T. Feigl, and C. Mutschler, “Estimating TOA reliability with variational autoencoders,” _IEEE Sensors J._ , pp. 1–1, 2021.
  * [35] F. Meyer, Z. Liu, and M. Z. Win, “Network localization and navigation using measurements with uncertain origin,” in _Proc. FUSION-18_ , July 2018, pp. 1–7.
  * [36] Z. Yu, Z. Liu, F. Meyer, A. Conti, and M. Z. Win, “Localization based on channel impulse response estimates,” in _Proc. IEEE/ION PLANS-20_ , 2020, pp. 1014–1021.
  * [37] A. Venus, E. Leitinger, S. Tertinek, and K. Witrisal, “A message passing based adaptive PDA algorithm for robust radio-based localization and tracking,” in _2021 Proc. IEEE RadarConf-21_ , 2021, pp. 1–6.
  * [38] S. Bartoletti, A. Giorgetti, M. Win, and A. Conti, “Blind Selection of Representative Observations for Sensor Radar Networks,” _IEEE Trans. Vehicular Technology_ , vol. 64, no. 4, pp. 1388–1400, April 2015.
  * [39] S. Mazuelas, A. Conti, J. C. Allen, and M. Z. Win, “Soft range information for network localization,” _IEEE Trans. Signal Process._ , vol. 66, no. 12, pp. 3155–3168, June 2018.
  * [40] A. Conti, S. Mazuelas, S. Bartoletti, W. C. Lindsey, and M. Z. Win, “Soft information for localization-of-things,” _Proc. IEEE_ , vol. 107, no. 11, pp. 2240–2264, Nov. 2019.
  * [41] Y. Li, S. Mazuelas, and Y. Shen, “A semi-supervised learning approach for ranging error mitigation based on UWB waveform,” in _Proc. IEEE MILCOM-21_ , 2021, pp. 533–537.
  * [42] Y. Huang, S. Mazuelas, F. Ge, and Y. Shen, “Indoor localization system with NLOS mitigation based on self-training,” _IEEE Trans. Mobile Comput._ , pp. 1–1, 2022.
  * [43] E. Leitinger, F. Meyer, F. Tufvesson, and K. Witrisal, “Factor graph based simultaneous localization and mapping using multipath channel information,” in _Proc. IEEE ICCW-17_ , Paris, France, May 2017, pp. 652–658.
  * [44] E. Leitinger, S. Grebien, and K. Witrisal, “Multipath-based SLAM exploiting AoA and amplitude information,” in _Proc. IEEE ICCW-19_ , Shanghai, China, May 2019, pp. 1–7.
  * [45] Y. Bar-Shalom, F. Daum, and J. Huang, “The probabilistic data association filter,” _IEEE Control Syst. Mag._ , vol. 29, no. 6, pp. 82–100, Dec 2009\.
  * [46] F. Meyer, T. Kropfreiter, J. L. Williams, R. 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.
  * [47] F. Meyer and J. L. Williams, “Scalable detection and tracking of geometric extended objects,” _IEEE Trans. Signal Process._ , vol. 69, pp. 6283–6298, Oct. 2021.
  * [48] X. Li, E. Leitinger, A. Venus, and F. Tufvesson, “Sequential detection and estimation of multipath channel parameters using belief propagation,” _IEEE Trans. Wireless Commun._ , pp. 1–1, 2022.
  * [49] 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\.
  * [50] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping: Part I,” _IEEE Robot. Autom. Mag._ , vol. 13, no. 2, pp. 99–110, Jun. 2006.
  * [51] Y. Bar-Shalom and X.-R. Li, _Multitarget-Multisensor Tracking: Principles and Techniques_. Storrs, CT, USA: Yaakov Bar-Shalom, 1995.
  * [52] S. Jeong and J. Tugnait, “Multisensor tracking of a maneuvering target in clutter using IMMPDA filtering with simultaneous measurement update,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 41, no. 3, pp. 1122–1131, Nov. 2005.
  * [53] D. Lerro and Y. Bar-Shalom, “Automated tracking with target amplitude information,” in _1990 American Control Conference_ , May 1990, pp. 2875–2880.
  * [54] S. Zhang, E. Staudinger, T. Jost, W. Wang, C. Gentner, A. Dammann, H. Wymeersch, and P. A. Hoeher, “Distributed direct localization suitable for dense networks,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 56, no. 2, pp. 1209–1227, July 2020.
  * [55] T. Kropfreiter, J. L. Williams, and F. Meyer, “A scalable track-before-detect method with poisson/multi-bernoulli model,” in _Proc. IEEE FUSION-21_ , 2021\.
  * [56] F. Kschischang, B. 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.
  * [57] G. Soldi, F. Meyer, P. Braca, and F. Hlawatsch, “Self-tuning algorithms for multisensor-multitarget tracking using belief propagation,” _IEEE Trans. Signal Process._ , vol. 67, no. 15, pp. 3922–3937, Aug. 2019\.
  * [58] G. Steinboeck, T. Pedersen, B. H. Fleury, W. Wang, and R. Raulefs, “Distance dependent model for the delay power spectrum of in-room radio channels,” _IEEE Trans. Antennas Propag._ , vol. 61, no. 8, pp. 4327–4340, 2013.
  * [59] K. Witrisal, E. Leitinger, S. Hinteregger, and P. Meissner, “Bandwidth scaling and diversity gain for ranging and positioning in dense multipath channels,” _IEEE Wireless Commun. Lett._ , vol. 5, no. 4, pp. 396–399, Aug. 2016.
  * [60] P. Tichavsky, C. Muravchik, and A. Nehorai, “Posterior Cramer-Rao bounds for discrete-time nonlinear filtering,” _IEEE Trans. Signal Process._ , vol. 46, no. 5, pp. 1386–1396, May 1998.
  * [61] S. Kay, _Fundamentals of Statistical Signal Processing: Detection Theory_. Upper Saddle River, NJ, USA: Prentice Hall, 1998.
  * [62] Y. Bar-Shalom, T. Kirubarajan, and X.-R. Li, _Estimation with Applications to Tracking and Navigation_. New York, NY, USA: John Wiley & Sons, Inc., 2002.
  * [63] J. Kulmer, S. Hinteregger, B. Großwindhager, M. Rath, M. S. Bakr, E. Leitinger, and K. Witrisal, “Using DecaWave UWB transceivers for high-accuracy multipath-assisted indoor positioning,” in _Proc. IEEE ICCW-17_ , Paris, France, May 2017, pp. 1239–1245.
  * [64] B. Etzlinger, F. Meyer, F. Hlawatsch, A. Springer, and H. Wymeersch, “Cooperative simultaneous localization and synchronization in mobile agent networks,” _IEEE Trans. Signal Process._ , vol. 65, no. 14, pp. 3587–3602, July 2017.
  * [65] P. Bello, “Characterization of randomly time-variant linear channels,” _IEEE Trans. Commun. Syst._ , vol. 11, no. 4, pp. 360–393, Dec. 1963.
  * [66] B. H. Fleury, “First- and second-order characterization of direction dispersion and space selectivity in the radio channel,” _IEEE Trans. Inf. Theory_ , vol. 46, no. 6, pp. 2027–2044, Sept. 2000.
  * [67] E. Leitinger, S. Grebien, B. H. Fleury, and K. Witrisal, “Detection and estimation of a spectral line in MIMO systems,” in _Proc. Asilomar-20_ , Pacifc Grove, CA, USA, Oct. 2020, pp. 1090–1095.
  * [68] M. L. Jakobsen, T. Pedersen, and B. H. Fleury, “Analysis of stochastic radio channels with temporal birth-death dynamics: A marked spatial point process perspective,” _IEEE Trans. Antennas Propag._ , vol. 62, no. 7, pp. 3761–3775, Apr. 2014.
  * [69] D. Shutin, W. Wang, and T. Jost, “Incremental sparse Bayesian learning for parameter estimation of superimposed signals,” in _Proc. SAMPTA-2013_ , no. 1, Sept. 2013, pp. 6–9.
  * [70] M. A. Badiu, T. L. Hansen, and B. H. Fleury, “Variational Bayesian inference of line spectra,” _IEEE Trans. Signal Process._ , vol. 65, no. 9, pp. 2247–2261, May 2017.
  * [71] T. L. Hansen, B. H. Fleury, and B. D. Rao, “Superfast line spectral estimation,” _IEEE Trans. Signal Process._ , vol. PP, no. 99, pp. 1–1, Feb. 2018.
  * [72] A. Venus, , S. Tertinek, and K. Witrisal, “A graph-based algorithm for robust sequential localization exploiting multipath for obstructed-LOS-bias mitigation: Supplementary material,” _ArXiv e-prints_ , vol. abs/2207.08646, 2022. [Online]. Available: http://arxiv.org/abs/2207.08646
  * [73] S. Kay, _Fundamentals of Statistical Signal Processing: Estimation Theory_. Upper Saddle River, NJ, USA: Prentice Hall, 1993.
  * [74] G. Papa, P. Braca, S. Horn, S. Marano, V. Matta, and P. Willett, “Adaptive Bayesian tracking with unknown time-varying sensor network performance,” in _Proc. IEEE ICASSP-15_ , 2015, pp. 2534–2538.
  * [75] 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, 2016.
  * [76] D. B. Jourdan, D. Dardari, and M. Z. Win, “Position error bound for UWB localization in dense cluttered environments,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 44, no. 2, pp. 613–628, 2008.
  * [77] M. Z. Win, Y. Shen, and W. Dai, “A theoretical foundation of network localization and navigation,” _Proc. IEEE_ , vol. 106, no. 7, pp. 1136–1165, 2018.
  * [78] H. Godrich, A. Haimovich, and R. Blum, “Target localization accuracy gain in MIMO radar-based systems,” _IEEE Trans. Inf. Theory_ , vol. 56, no. 6, pp. 2783 –2803, June 2010.

|  Alexander Venus (S’20) received his B.Sc. and Dipl.-Ing. (M.Sc. ) degrees
(with highest honors) in biomedical engineering and information and
communication engineering from Graz University of Technology, Austria in 2012
and 2015, respectively. He was a research and development engineer at Anton
Paar GmbH, Graz from 2014 to 2019. He is currently a project assistant at Graz
University of Technology, where he is pursuing his Ph.D. degree. His research
interests include radio-based localization and navigation, statistical signal
processing, estimation/detection theory, machine learning and error bounds.
---|---
|  Erik Leitinger (S’12–M’16) received his Dipl.-Ing. (M.Sc. ) and Ph.D.
degrees (with highest honors) in electrical engineering from Graz University
of Technology, Austria in 2012 and 2016, respectively. He was postdoctoral
researcher at the department of Electrical and Information Technology at Lund
University from 2016 to 2018. He is currently a University Assistant at Graz
University of Technology. Dr. Leitinger served as co-chair of the special
session ”Synergistic Radar Signal Processing and Tracking” at the IEEE Radar
Conference in 2021. He is co-organizer of the special issue ”Graph-Based
Localization and Tracking” in the Journal of Advances in Information Fusion
(JAIF). Dr. Leitinger received an Award of Excellence from the Federal
Ministry of Science, Research and Economy (BMWFW) for his Ph.D. Thesis. He is
an Erwin Schrödinger Fellow. His research interests include inference on
graphs, localization and navigation, multiagent systems, stochastic modeling
and estimation of radio channels, and estimation/detection theory.
---|---
|  Stefan Tertinek received the Dipl.-Ing. degree in electrical engineering
from Graz University of Technology, Graz, Austria, in 2007, and the Ph.D.
degree in electrical engineering from University College Dublin, Dublin,
Ireland, in 2011. From 2011 to 2018 he was with Danube Mobile Communications
Engineering GmbH & Co KG (majority owned by Intel Austria GmbH), Linz,
Austria, as a RF System Engineer involved in research and product development
of multiple generations of cellular RF transceiver and modem platforms. In
2018 he joined NXP Semiconductors Austria GmbH & Co KG as a RF System
Architect in the Product Line Secure Car Access, where he works on ultra-
wideband (UWB) and Bluetooth radio technologies with a focus on localization,
radar and machine learning.
---|---
|  Klaus Witrisal (S’98–M’03) received the Ph.D. degree (cum laude) from Delft
University of Technology, Delft, The Netherlands, in 2002, and the
Habilitation from Graz University of Technology in 2009. He is currently an
Associate Professor at the Signal Processing and Speech Communication
Laboratory (SPSC) of Graz University of Technology and head of the Christian
Doppler Laboratory for Location-aware Electronic Systems. His research
interests are in signal processing for wireless communications, propagation
channel modeling, and positioning. Klaus Witrisal served as an associate
editor of IEEE Communications Letters, co-chair of the TWG “Indoor” of the
COST Action IC1004, cochair of the EWG “Localisation and Tracking” of the COST
Action CA15104, leading chair of the IEEE Workshop on Advances in Network
Localization and Navigation (ANLN), and TPC (co)-chair of the Workshop on
Positioning, Navigation and Communication (WPNC).
---|---
  *[SPA]: sum-product algorithm
  *[FA]: false alarm
  *[P-CRLB]: posterior Cramér-Rao lower bound
  *[MPCs]: multipath component
  *[LOS]: line-of-sight
  *[OLOS]: obstructed
  *[UWB]: ultra-wideband
  *[MP-SLAM]: multipath-based SLAM
  *[SNR]: signal-to-noise-ratio
  *[PDA]: probabilistic data association
  *[AIPDA]: amplitude-information probabilistic data association
  *[NLOS]: non-
  *[CEDA]: channel estimation and detection algorithm
  *[FAs]: false alarm
  *[DPS]: delay power spectrum
  *[MPC]: multipath component
  *[CRLB]: Cramér-Rao lower bound
  *[PDF]: probability density function
  *[PMF]: probability mass function
  *[RV]: random variable
  *[PDFs]: probability density function
  *[RVs]: random variable
  *[AWGN]: additive white Gaussian noise
  *[DNR]: dense-multipath-to-noise ratio
  *[LHF]: likelihood function
  *[MMSE]: minimum mean-squared error
  *[LHFs]: likelihood function
  *[PMFs]: probability mass function
  *[STV]: state-transition variances
  *[RMSE]: root mean squared error
  *[BBEs]: bin-based estimate