Data Fusion for Multipath-Based SLAM:
Combining Information from Multiple Propagation Paths
Erik Leitinger Member, IEEE,
Alexander Venus Student Member, IEEE,

Bryan Teague Member, IEEE,
Florian Meyer Member, IEEE

E. Leitinger and A. Venus are with the Signal Processing and Speech Communication Laboratory, Graz University of Technology, Graz, Austria, and the Christian Doppler Laboratory for Location-aware Electronic Systems (e-mail: (erik.leitinger,a.venus)@tugraz.at).
B. Teague is with the MIT Lincoln Laboratory, Lexington, MA, USA (bryan.teague@ll.mit.edu).
F. Meyer is with the Department of Electrical and Computer Engineering and Scripps Institution of Oceanography, University of California San Diego, San Diego, CA, USA (e-mail: fmeyer@ucsd.edu).
This material is based upon work supported by the Under Secretary of Defense for Research and Engineering under Air Force Contract No. FA8702-15-D-0001 and the TU Graz.

Multipath-based slam is an emerging paradigm for accurate indoor localization constrained by limited navigation resources. The goal of multipath-based slam is to support the estimation of time-varying positions of mobile agents by detecting and localizing radio-reflective surfaces in the environment. In existing Bayesian methods, a propagation surface is represented by the mirror image of each pa across that surface – known as the corresponding va. Due to this va representation, each propagation path is mapped individually. Existing methods thus neglect inherent geometrical constraints across different paths that interact with the same surface, which limits accuracy and speed. In this paper, we introduce an improved statistical model and estimation method that enables data fusion in multipath-based slam. By directly representing each surface with a mva, geometrical constraints across propagation paths are also modeled statistically. A key aspect of the proposed method based on mva is to check the availability of single-bounce and double-bounce propagation paths at potential agent positions by means of rt. This availability check is directly integrated into the statistical model as detection probabilities for propagation paths. Estimation is performed by a spa derived based on the factor graph that represents the new statistical model. Numerical results based on simulated and real data demonstrate significant improvements in estimation accuracy compared to state-of-the-art multipath-based slam methods.

§ INTRODUCTION

A multipath-based slam scenario at time step $n$, involving two reflective surfaces, a single pa with position $\V{p}^{(1)}_{\text{pa}}\rmv\rmv$, and a single mobile agent with position and orientation, $\V{p}_n$ and $\psi_n$, respectively. The mobile agent is depicted with five individual antenna elements at positions $\V{p}^{(h)}_{n}\rmv\rmv$, defined by offset $d^{(h)}$ and angle $\psi^{(h)}$. The reflecting surfaces can be represented by va or mva, e.g., $\V{p}^{(1)}_{\text{11,va}}$ or $\V{p}_{\text{1,mva}}$, respectively. A single-bounce va represents the location of the mirror image of a pa on the corresponding reflective surface. A mva represents the location of the mirror image of a common origin on the corresponding reflective surface, thus creating a unique mva for each reflecting surface. For a second pa or double-bounce propagation path involving surface 1 (not shown),va positions are different than $\V{p}^{(1)}_{\text{11,va}}$ while the unique mva position $\V{p}_{\text{1,mva}}$ remains the same. Note that $\V{u}_1$ and  $\V{u}_2$ are the unit vectors perpendicular to surfaces 1 and 2, while $\V{e}_2$ is an arbitrary vector from the origin to surface 2. Distance and angle for pa and va at surface 1, i.e., $(d_{00,n}^{(1)}, \varphi_{00,n}^{(1)})$ and $(d_{11,n}^{(1)}, \varphi_{11,n}^{(1)})$, are also shown.

Emerging sensing technologies and innovative signal processing methods exploiting multipath propagation will lead to new capabilities for autonomous navigation, asset tracking, and situational awareness in future communication networks. Multipath-based slam is a promising approach in wireless networks for obtaining position information of transmitters and receivers as well as information on their propagation environments. In multipath-based slam, specular reflections of rf signals at flat surfaces are modeled by va that are mirror images of base stations, also referred to as pa <cit.>. The number of va and their positions are typically unknown. Multipath-based slam methods can detect and localize va and jointly estimate the time-varying agent position <cit.>. The availability of va location information makes it possible to leverage multiple propagation paths of rf signals for agent localization. It can thus significantly improve localization accuracy and robustness <cit.>.

§.§ State of the Art

Multipath-based slam falls under the umbrella of feature-based SLAM approaches that focus on detecting and mapping distinct features in the environment <cit.>. In existing multipath-based slam methods, the distinct features of interest are the va<cit.> while “measurements” are obtained by extracting parameters from the mpc of rf signals in preprocessing stage <cit.>. Measurements can be noisy distances, angles-of-arrival (AoAs), or angles-of-departure (AoDs) <cit.>.

As typical for feature-based slam, a complicating factor in multipath-based slam is measurement origin uncertainty, i.e., the unknown association of measurements with features <cit.>. In particular, (i) it is not known which va was generated by which measurement, (ii) there are missed detections due to low  snr or occlusion of features, and (iii) there are false positive measurements due to clutter. Thus, an important aspect of multipath-based SLAM is data association between measurements and va. Probabilistic data association can increase the robustness and accuracy of multipath-based slam but introduces association variables as additional unknown parameters. To avoid the curse of dimensionality related to the high-dimensional parameters space, state-of-the-art methods for multipath-based slam perform the spa on the factor graph representing the underlying statistical model <cit.>. In addition, since the models for the aforementioned measurements are nonlinear, most methods typically rely on sampling techniques <cit.>. Recently, multipath-based slam was applied to data collected in indoor scenarios by radios with ultra-wide bandwidth <cit.> or multiple antennas <cit.>.

In existing methods for multipath-based slam, each va represents a single propagation path from an agent to a pa. In particular, even if a reflective surface takes part in multiple propagation paths, each path is represented by a va, and mapped individually <cit.>. Existing methods thus neglect inherent geometrical constraints across different paths that interact with the same surface, which limits accuracy and speed. Note that the multipath-based slam methods proposed in <cit.> perform fusion of information provided by multiple cooperating agents. However, these methods are limited to single-bounce paths and do not perform fusion across pa. In <cit.>, it has been demonstrated that double-bounce paths of rf signals exhibit considerable signal strength and should thus be considered in real-world multipath-based slam scenarios. This provides compelling evidence that incorporating information from double-bounce paths can lead to an improved multipath-based slam performance.

§.§ Contributions and Notations

The problem studied in this paper can be summarized as follows.

Estimate the time-varying position of a mobile agent by making use of the los and mpc in rf  signals. To leverage the position information of mpc, the unknown number and positions of reflective surfaces in the environment are estimated during runtime.

We introduce a new statistical model for multipath-based slam that considers single-bounce and double-bounce propagation paths and enables data fusion across multiple paths. In existing multipath-based slam models, paths represented by va are the SLAM features. In the proposed model, however, every reflective surface is directly represented by a slam feature referred to as mva. The mva representation makes it possible to model inherent geometrical constraints across paths statistically. Following the new model, a factor graph is established, and an extension of the spa for multipath-based slam in <cit.> is developed. The resulting spa can infer reflective surfaces by combining information across multiple propagation paths. Particularly appealing is the ability to fuse paths related to different pa. Within the spa, rt <cit.> is performed to determine the availability of each single-bounce and double-bounce paths at potential agent positions. The availability check is directly integrated into the statistical model as detection probabilities for propagation paths. The resulting multipath-based slam method can provide fast and accurate estimates in scenarios with many reflective surfaces. Note that the proposed method uses distance and aoa measurements, which have been extracted from the rf signal in a preprocessing stage <cit.>.

The key contributions of this paper are as follows.

* We introduce a statistical model and factor graph for multipath-based slam that facilitates data fusion across propagation paths.

* We integrate rt into our statistical model to determine the availability of individual single-bounce and double-bounce propagation path.

* We extend the spa for multipath-based slam <cit.> based on the introduced factor graph to establish data fusion for multipath-based slam[We also provide a pseudocode for the particle-based implementation in the supplementary materialSectionsec:pdeudocode.].

* We demonstrate significant improvements in estimation performance compared to existing multipath-based slam methods based on both simulated and real data.

This paper advances beyond the preliminary account of our method provided in the conference publications <cit.> by (i) extending the mva model to double-bounce propagation paths, (ii) introducing rt to determine the availability of paths and integrating it into the statistical model,
(iii) presenting a detailed derivation of the factor graph, and (iv) demonstrating performance advantages compared to reference methods. As reference methods, classical multipath-based slam <cit.>, channel-slam <cit.>, and multipath-based positioning assuming known va positions <cit.> are considered.

Notation: Random variables are displayed in sans serif, upright fonts; their realizations in serif, italic fonts. Vectors and matrices are denoted by bold lowercase and uppercase letters, respectively. For example, a random variable and its realization are denoted by $\rv x$ and $x$, respectively, and a random vector and its realization by $\RV x$ and $\V x$, respectively. Furthermore, $\|\V{x}\|$ and ${\V{x}}^{\text T}$ denote the Euclidean norm and the transpose of vector $\V x$, respectively, and $\langle \V{x}, \V{y} \rangle$ denotes the inner-product between the vectors $\V{x}$ and $\V{y}$; $\propto$ indicates equality up to a normalization factor; $f(\V x)$ denotes the pdf of random vector $\RV x$ (this is a short notation for  $f_{\RV x}(\V x)$); $f(\V x | \V y)$ denotes the conditional pdf of random vector $\RV x$ conditioned on random vector  $\RV y$  (this is a short notation for  $f_{\RV x | \RV z}(\V x | \V z)$). The four-quadrant inverse tangent of position $\V{p} = [\ist p_1 \ist\ist p_2 \ist]^{\mathrm{T}}$ is denoted as $ \atantwo(p_2, p_1)$. The cardinality of a set $\Set{X}$ is denoted as $ \vert\Set{X}\vert $. $\delta(\cdot)$ denotes the Dirac delta function. Finally, $ \delta_e$ denotes the indicator function of the event $ {e} \rmv=\rmv {0} $ (i.e., $ \delta_e \rmv=\rmv 1 $ if $ e \rmv=\rmv {0} $ and ${0}$ otherwise).

§ GEOMETRICAL RELATIONS

Three multipath-based slam scenarios with two reflective surfaces illustrate single- and double-bounce paths in three different geometries. The positions of the mobile agents, pa, va, and mva are shown. (a) depicts a single time step $n$ of a scenario with perpendicular reflective surfaces, two pa at positions $\V{p}_\text{pa}^{(1)}$ and $\V{p}_\text{pa}^{(2)}$, and the agent at position $\V{p}_n$; only single-bounce paths are illustrated. (b) shows perpendicular reflective surfaces, one pa at position $\V{p}_\text{pa}^{(1)}$, and agent positions $\V{p}_{n_1}$ and $\V{p}_{n_2}$ at two different time steps $n_1$ and $n_2$. Only double-bounce paths are shown. (c) depicts a single time step $n$ of a scenario with reflective surfaces at an acute angle, one pa at position $\V{p}_\text{pa}^{(1)}$ and the agent at position $\V{p}_n$. Again, only double-bounce paths are illustrated. Note that in (b), due to perpendicular reflective surfaces, both double-bounce paths have equal lengths, and there is a single double-bounce VA. In (b), however, due to reflective surfaces at an acute angle, the two double-bounce paths have different lengths, and there are two different double-bounce VAs.

We consider a mobile agent equipped with an $H$-element antenna array and $J$ pa equipped with a single antenna at known positions $\V{p}_{\mathrm{pa}}^{(j)} = \big[ {p}_{1,\mathrm{pa}}^{(j)} \ist\ist\ist {p}_{2,\mathrm{pa}}^{(j)} \big]^{\mathrm{T}} \rmv\!\in\rmv \mathbb{R}^2\rmv$, $j \rmv\in\rmv \{1,\ldots,J\}$, where $J$ is assumed to be known. The agent is in an environment with $S$ reflective surfaces indexed by $s \in \Set{S}\triangleq\rmv \{1,\ldots,S\}$. At every discrete time step $n$, the array element locations are denoted by $\V{p}_n^{(h)}$, $h \in \{1,\dots,H\}$. The agent position $\V{p}_n$ refers to the center of gravity of the array. We also define $d^{(h)} = \|\V{p}_n^{(h)} - \V{p}_n\|$ and $\psi^{(h)} = \atantwo\big({p}_{2,n}^{(h)}-{p}_{2,n}, {p}_{1,n}^{(h)}-{p}_{1,n}\big)-\psi_n$, the distance from the reference location $\V{p}_n$ and the orientation, respectively, of the $h$-th element as shown in Fig. <ref>. At each discrete time slot $n$, the position $\V{p}_n \rmv\!\in\rmv \mathbb{R}^2$ and the array orientation $\psi_n$  of the agent are unknown. Each pa transmits a rf signal, and the agent acts as a receiver.[We assume the pa to use orthogonal codes, i.e., there is no mutual interference between individual pa. Note that the proposed algorithm can be easily reformulated for the case where the agent transmits a rf signal and the pa act as receivers.]
The rf signal arrives at the receiver via the los path as well as via mpc originating from the reflection of surrounding objects.

We assume time synchronization between all pa and the agent. However, our algorithm can be extended to an unsynchronized system according to <cit.>.

We restrict the representation to mpc related to single-bounce and double-bounce paths. In particular, associated with pa $j$ there are $|\Set{D}_\text{S}| = S$ single-bounce va <cit.> at unknown positions $\V{p}_{ss,\mathrm{va}}^{(j)} \!\rmv\in\rmv \mathbb{R}^2\rmv$ with index-pair $(s,s) \in  \Set{D}_\text{S} \rmv\triangleq\rmv \{ (s,s) \rmv\in\rmv \Set{S} \rmv\times\rmv \Set{S}\}$ and $|\Set{D}_\text{D}| = S(S-1)$ double-bounce va at unknown positions $\V{p}_{ss',\mathrm{va}}^{(j)} \!\rmv\in\rmv \mathbb{R}^2\rmv$ with index-pair $(s,s') \in  \Set{D}_\text{D} \rmv\triangleq\rmv \{ (s,s') \rmv\in\rmv \Set{S} \rmv\times\rmv \Set{S}| \ist s \neq s' \}$. Therefore, the maximum number of va for pa $j$ is given by $|\mathcal{D}| = S+S(S-1)$ with $\Set{D} \rmv\triangleq\rmv \{ (s,s') \rmv\in\rmv \Set{S} \rmv\times\rmv \Set{S}\} = \Set{D}_\text{D} \cup \Set{D}_\text{S}$. Note that subscript indices denoted as $(\cdot)_{ss'}$ indicate variables related to a pairs $(s,s')$ of reflecting objects. By applying the image-source model <cit.>, a va associated with a single-bounce path is the mirror image of $\V{p}_{\mathrm{pa}}^{(j)}$ at reflective surface $s \in \mathcal{S}$ given by

\begin{align}
\V{p}^{(j)}_{ss,\mathrm{va}} &= \V{p}^{(j)}_{\mathrm{pa}} + 2\big( \V{u}_s^{\T}\V{e}_s - \V{u}_s^{\T}\V{p}^{(j)}_{\mathrm{pa}}\big)\V{u}_s \label{eq:VA1BPAequation}
\\[-6mm]
\nn
\end{align}

where $(s,s) \in \Set{D}_\text{S}$, $\V{u}_s$ is the normal vector of reflective surface $s$, and $\V{e}_s$ is an arbitrary point on the considered surface. The second term in (<ref>) represents the normal vector w.r.t. to reflective surface $s$ in direction $\V{u}_s$ with the length of two times the distance between pa $j$ at position $\V{p}^{(j)}_{\mathrm{pa}}$ and the normal-point at the reflective surface $s$, i.e., $2\big( \V{u}_s^{\T}\V{e}_s - \V{u}_s^{\T}\V{p}^{(j)}_{\mathrm{pa}}\big)$. By again applying the image-source model <cit.> for va at position $\V{p}^{(j)}_{s's',\mathrm{va}}$ another va
that is obtained as the mirror image of $\V{p}^{(j)}_{s's',\mathrm{va}}$ at surface $s$, i.e.,

\begin{align}
\V{p}^{(j)}_{ss',\mathrm{va}} &= \V{p}^{(j)}_{s's',\mathrm{va}} + 2\big( \V{u}_s^{\T}\V{e}_s - \V{u}_s^{\T}\V{p}^{(j)}_{s's',\mathrm{va}}\big)\V{u}_s\\[-6mm]
\nn
\end{align}

where $(s,s') \in \Set{D}_\text{D}$. This va represents a double-bounce propagation path. For conveniently addressing pa-related variables and factors, we also define $\V{p}^{(j)}_{00,\mathrm{va}} \triangleq \V{p}^{(j)}_{\mathrm{pa}}$ and ${\tilde{\mathcal{D}}} = (0,0) \cup {\mathcal{D}}$. The distances and aoa related to the propagation paths at the agent position $\V{p}_n$ represented by $\V{p}_{ss',\mathrm{va}}^{(j)}$ with $(s,s') \in {\tilde{\mathcal{D}}}$ are modeled by $d_{ss',n}^{(j)} = \big\|  \V{p}_n - \V{p}^{(j)}_{ss',\mathrm{va}} \big\|$ and $\varphi_{ss',n}^{(j)} =  \atantwo \big ( {p}_{2,n} - {p}^{(j)}_{1,ss',\text{va}}, {p}_{1,n} - {p}^{(j)}_{1,ss',\text{va}} \big ) - \psi_n$. Note that the distance $d_{00,n}^{(j)} $ and the aoa $\varphi_{00,n}^{(j)}$  are the parameters related to the los path between agent at position $\V{p}_n$ and pa $j$ at position $\V{p}^{(j)}_{\mathrm{pa}}$. An example is shown in Fig. <ref>.

The availability of va at certain agent position $\V{p}_n$ is limited due to blockage of associated propagation paths or geometric constraints between reflective surfaces <cit.>.
Especially, double-bounce paths and their corresponding va positions have limited availability depending on the agent position $\V{p}_n$ (see Fig. <ref> and Fig. <ref>). Hence, the number of available va for pa $j$ is smaller than $|\mathcal{D}| = S+S(S-1)$. As discussed in Section <ref>, the proposed statistical model and method performs an availability check for each VA using rt <cit.>. In this way, for each agent position $\V{p}_n$, it can be determine which of the potential paths in $\mathcal{D}$ is available.

Notations and definitions of important quantities.

Notation    Definition
Notation     Definition

\(\V{p}_{\mathrm{pa}}^{(j)}\)     2D position of physical anchor \(j\)
\(\V{p}_n\)     2D position of mobile agent at time $n$

\(\psi_n\)     Orientation of mobile agent at time $n$
\(\V{v}_n\)     2D velocity of mobile agent at time $n$

\(S\)     Total number of reflecting surfaces (mva)
\(\rv{M}^{(j)}_n\)     Number of measurements with pa \(j\) at time $n$

\(\V{p}_{ss,\mathrm{va}}^{(j)}\)     Single-bounce virtual anchor position
\(\V{p}_{ss',\mathrm{va}}^{(j)}\)     Double-bounce virtual anchor position

\(\V{p}_{s,\mathrm{mva}}\)     Position of mva corresponding to pa \(s\)
\(S_n\)     Number of pmva at time $n$

\({\rv{z}_\text{d}}_{m,n}^{(j)}\)     Distance measurement \(m\) with pa \(j\) at time $n$
\({\rv{z}_\varphi}_{m,n}^{(j)}\)     aoa measurement \(m\) with pa \(j\) at time $n$

\({\rv{\nu}_\mathrm{\tau}}_{m,n}^{(j)}\)     Distance measurement noise
\({\nu_{\mathrm{\varphi}}}_{m,n}^{(j)}\)     aoa measurement noise

\(\rv{r}_{s,n}\)     Binary existence variable related to pmva \(s\) at time $n$
\( p_{\mathrm{d},ss',n}^{(j)} \)     detection probability of available path

\(\mu_{\mathrm{fp}}\)     False alarm Poisson point process mean
\(f_{\mathrm{fp}}(\RV{z}_{m,n}^{(j)})\)     False positive point process pdf

\(p_\mathrm{s}\)     Probability of survival of legacy pmva
\(p_{\mathrm{cf}}\)     Confirmation threshold for pmva

\(p_{\text{pr}}\)     Pruning threshold to remove pmva
\(\Delta T\)     Simulation sampling period

\(\V{\omega}_{n}\)     Process noise of the agents' motion model
\(\)

§.§ MVA-Based Model of the Environment

A reflective surface is involved in multiple propagation paths and thus defines multiple va. To enable the consistent combination, i.e., “fusion” of map information provided by measurements of different pa, we represent reflective surfaces by $S$ unique mva at positions $\V{p}_{s,\mathrm{mva}} \!\rmv\in\rmv \mathbb{R}^2\rmv$, $s \in \Set{S}$. The unique mva position $\V{p}_{s,\mathrm{mva}} \!\rmv\in\rmv \mathbb{R}^2\rmv$ is defined as the mirror image of $[0 \hspace{.6mm} 0]^{\T}$ on the reflective surface $S$.[Note that any point can be used, not only the coordinate center. And that the mva-model can be extended by the length of the corresponding walls.] By using some algebra, the transformation from mva at $\V{p}_{s,\mathrm{mva}}$ to a va at $\V{p}^{(j)}_{ss,\mathrm{va}}$, i.e., $\V{p}^{(j)}_{ss,\mathrm{va}} = h_\text{va}\big( \V{p}_{s,\mathrm{mva}}, \V{p}^{(j)}_{\mathrm{pa}} \big)$ can be obtained as

\begin{align}
\V{p}^{(j)}_{ss,\mathrm{va}} = -\bigg(\frac{ 2 \big\langle \V{p}_{s,\mathrm{mva}},\V{p}^{(j)}_{\mathrm{pa}} \big\rangle}{ \big\|\V{p}_{s,\mathrm{mva}}\big\|^2 } - 1 \bigg) \ist \V{p}_{s,\mathrm{mva}} + \ist \V{p}^{(j)}_{\mathrm{pa}}\ist. \label{eq:nonLinearTransformation}\\[-6mm]\nn
\end{align}

The transformation from mva at $\V{p}_{s,\mathrm{mva}}$ and $\V{p}_{s',\mathrm{mva}}$ to a double-bounce va at $\V{p}^{(j)}_{ss',\mathrm{va}}$ can be obtained by applying (<ref>) twice, i.e,
$\V{p}^{(j)}_{ss',\text{va}} = h_\text{va}\big( \V{p}_{s,\mathrm{mva}}, h_\text{va}\big( \V{p}_{s',\mathrm{mva}}, \V{p}^{(j)}_{\mathrm{pa}} \big)\big)$. The inverse transformation from a va to a mva is given by

\begin{align}
\V{p}_{s,\mathrm{mva}} &= h_\text{mva}\big( \V{p}^{(j)}_{ss,\mathrm{va}}, \V{p}^{(j)}_{\mathrm{pa}} \big) \nn\\[1mm]
&= \frac{ \big\|\V{p}^{(j)}_{\mathrm{pa}}\big\|^2 - \big\|\V{p}^{(j)}_{ss,\mathrm{va}}\big\|^2}{ \big\|(\V{p}^{(j)}_{\mathrm{pa}} - \V{p}^{(j)}_{ss,\mathrm{va}})\big\|^2} \big(\V{p}^{(j)}_{\mathrm{pa}} - \V{p}^{(j)}_{ss,\mathrm{va}}\big)\ist. \label{eq:nonLinearTransformationMVA}\\[-6mm]\nn
\end{align}

Note that the inverse transformation in (<ref>) will be used to determine a proposal distribution for mva states as discussed in Section <ref>. Details of the derivation of (<ref>) and  (<ref>) are provided in the supplementary materialSectionsec:app_mva_model. For example, Fig. <ref> shows three scenarios with two reflecting surfaces described by two mva at positions $\V{p}_{1,\mathrm{mva}}$ and $\V{p}_{2,\mathrm{mva}}$. Fig. <ref> shows a scenario with two pa $j \in \{1,2\}$, the corresponding va, and an agent at position $\V{p}_n$. Each pa generates one va associated with a single-bounce propagation path. Fig. <ref> shows a scenario with one pa, the corresponding va associated with single-bounce and double-bounce propagation paths, and two agents positions at differnet time steps, i.e., $\V{p}_{n_1}$ and $\V{p}_{n_2}$. Note in case surfaces are perpendicular, a different order of bounces from surfaces, i.e., surface “$s$ – surface $s'$” or “surface $s'$ – surface $s$”, does not lead to a different va position. Depending on the agent position $\V{p}_{n}$, only one double-bounce propagation path (related to one of the two orders) is available (see also Section <ref>). Fig. <ref> shows a scenario with non-perpendicular surfaces. In this case, different “bounce orders” lead to different va-positions. In particular, if there is an acute angle between a pair of reflecting surfaces, there exist regions of agent positions $\V{p}_{n}$ for which two double-bounce propagation paths are available at the same time (cf. Section <ref>). These regions depend on the PA position as well as the angle between the two surfaces. Note that when the angle between a pair is obtuse, only one of the two double-bounce propagation paths is available for all positions $\V{p}_{n}$ (an example is given in Sectionsec:app_results).

§ SYSTEM MODEL

At each time $n$, the state of the agent is given by $\RV{x}_{n} = [\RV{p}_{n}^\T \iist \RV{v}_{n}^\T]$, where  $\RV{v}_{n}$ is the agent velocity vector. We assume that the array is rigidly coupled with the movement direction, i.e., array orientation is determined by the direction of the agent velocity vector. As in <cit.>, we account for the unknown number of mva by introducing pmva $s \in \Set{S}_n \rmv\triangleq\rmv \{ 1,\dots, \rv{S}_n \}$. The number $S_n$ of pmva is the maximum possible number of actual mva, i.e., all mva that produced a measurement so far <cit.> (where $S_n$ increases with time). pmva states are denoted as $\RV{y}_{s,n} \rmv\rmv=\rmv\rmv \big[\RV{p}^{\T}_{s,\mathrm{mva}} \; \rv{r}_{s,n} \big ]^\T\rmv\rmv\rmv$. The existence/nonexistence of pmva $s$ is modeled by the existence variable $\rv{r}_{s,n} \rmv\rmv\in\rmv\rmv \{0,1\}$ in the sense that pmva $s$ exists if $r_{s,n}\rmv\rmv=\rmv\rmv 1$. Formally,  its states is considered even if pmva $s$ is nonexistent, i.e., if $r_{s,n} \rmv\rmv=\rmv\rmv 0$. The states $\RV{p}^{\T}_{s,\mathrm{mva}}$ of nonexistent pmva are obviously irrelevant. Therefore, all pdf defined for pmva states, $f(\RV{y}_{s,n}) \rmv\rmv=\rmv\rmv f(\RV{p}_{s,\mathrm{mva}}, \rv{r}_{s,n})$, are of the form $f(\RV{p}_{s,\mathrm{mva}}, 0 )\rmv\rmv=\rmv\rmv f_{s,n} f_{\text{d}}(\RV{p}_{s,\mathrm{mva}})$, where $f_{\text{d}}(\RV{p}_{s,\mathrm{mva}})$ is an arbitrary “dummy pdf” and $f_{s,n} \!\rmv\in [0,1]$ is a constant and can be interpreted as the probability of non-existence <cit.>. A summary of all variables related to agent and PMVA states as well as other variables of the proposed statistical model can be found in Table <ref>.

§.§ Measurements and New pmva

The distance and aoa measurements  related to the path “agent at position $\RV{p}_n$ – va at position $\RV{p}^{(j)}_{ss',\text{va}}$” with $(s,s') \in \tilde{\Set{D}}_n$ are given by

\begin{align}
{\rv{z}_\text{d}}_{m,n}^{(j)} &= \big\|  \RV{p}_n - \V{p}^{(j)}_{ss',\mathrm{va}} \big\| + {\rv{\nu}_{\mathrm{d}}}_{m,n}^{(j)} \label{eq:VADistmeas} \\
{\rv{z}_\varphi}_{m,n}^{(j)}&=  \atantwo \big ( {p}_{2,n} - {p}^{(j)}_{2,ss',\mathrm{va}}, {p}_{1,n} - {p}^{(j)}_{1,ss',\mathrm{va}} \big )\nn \\
&\hspace{5mm} {- \atantwo \big ( {v}_{2,n}, {v}_{1,n} \big )}  + {\rv{\nu}_{\mathrm{\varphi}}}_{m,n}^{(j)} \label{eq:VAAnglemeas}\\[-7.5mm]\nn
\end{align}

where ${\rv{\nu}_\mathrm{d}}_{m,n}^{(j)}$ and ${\rv{\nu}_{\mathrm{\varphi}}}_{m,n}^{(j)}\vspace*{-0.8mm}$ are, respectively, zero-mean Gaussian measurement noise with standard deviations ${\sigma_{\mathrm{d}}}^{(j)}_{m,n}\vspace*{0.2mm}$ and ${\sigma_{\mathrm{\varphi}}}^{(j)}_{m,n}$. The measurements are combined in the vector $\RV{z}_{m,n}^{(j)} = [{\rv{z}_\text{d}}_{m,n}^{(j)} \iist {\rv{z}_\varphi}_{m,n}^{(j)}]^\T$ with $m \rmv\in\rmv \Set{M}_n^{(j)} \rmv\triangleq\rmv \{1,$ $\dots,\rv{M}^{(j)}_n\}$ for pa $j$. (These measurements represent the mpc parameters. For details see <cit.>.) Note that before measurements are acquired, the number of measurements $\rv{M}^{(j)}_n \rmv$ is random.

Likelihood function for los paths: Using (<ref>), (<ref>), and $\V{p}^{(j)}_{00,\text{va}} = \V{p}^{(j)}_{\mathrm{pa}}$, we can directly obtain the likelihood function for the los path as $f\big(\V{z}_{m,n}^{(j)}\big|\V{p}_{n}\big)\vspace{.8mm}$.

Likelihood function for single-bounce paths: Using (<ref>), (<ref>), and the transformation in (<ref>), i.e., $\RV{p}^{(j)}_{ss,\text{va}} = {h_\text{va}}\big( \RV{p}_{s,\mathrm{mva}}, \V{p}^{(j)}_{\mathrm{pa}} \big)$, the likelihood function related to the single-bounce propagation path “agent – surface $s$” with $s \in \Set{S}$ – pa $j$ with mva index-pair $(s,s) \in \mathcal{D}_{\text{S},n}$ reads $f(\V{z}_{m,n}^{(j)}|\V{p}_{n},\V{p}_{s,\mathrm{mva}})\vspace{.8mm}$.

Likelihood function for double-bounce paths: Using (<ref>), and (<ref>), and the transformation in (<ref>) twice
, i.e., $\RV{p}^{(j)}_{ss',\text{va}} = {h_\text{va}}\big( \RV{p}_{s,\mathrm{mva}}, {h_\text{va}}\big( \RV{p}_{s',\mathrm{mva}}, \V{p}^{(j)}_{\mathrm{pa}} \big)\big)$, the likelihood function related to the double-bounce path “agent – surface $s$ – surface $s'$ – pa $j$” with mva index-pair $(s,s') \in \mathcal{D}_{\text{D},n}$ can be expressed by $f(\V{z}_{m,n}^{(j)}|,\V{p}_{n},\V{p}_{s,\mathrm{mva}},\V{p}_{s',\mathrm{mva}})\vspace{.8mm}$.

With the mva-based measurement model, at time $n$, the measurements collected by all pa $j \rmv\in\rmv \{1,\dots,J\}$ can provide information on the same mva and agent positions $\RV{p}_{s,\mathrm{mva}}, s \in \Set{S}$ and $\RV{p}_{n}$, respectively. It is assumed that each va (related to a specific pa, mva or mva-mva pair) generates at most one measurement and that a
measurement originates from at most one va.
pa $j$ at position $\V{p}^{(j)}_{00,\text{va}} = \V{p}^{(j)}_{\mathrm{pa}}$ with $(0,0) \in \tilde{\Set{D}}_n$ generates a measurements $\RV{z}_{m,n}^{(j)}$ with detection probability $p^{(j)}_{\mathrm{d}}\big (\RV{p}_n\big)$. If mva $s$ exists ($r_{s,n} \rmv=\rmv 1$), the corresponding single-bounce path $(s,s) \in \Set{D}_{\text{S},n}$ generates a mva-originated measurements $\RV{z}_{m,n}^{(j)}$ with detection probability $p^{(j)}_{\mathrm{d}}\big (\RV{p}_n,\RV{p}_{s,\text{mva}} \big)$. The same holds for the double-bounce path $(s,s') \in \Set{D}_{\text{D},n}$ with detection probability $p^{(j)}_{\mathrm{d}}\big (\RV{p}_n,\RV{p}_{s,\text{mva}},\RV{p}_{s',\text{mva}}\big)$. Note that the detection probability is determined by the snr of the measurement  <cit.> as well as the availability check performed by rt. In particular, if a path is unavailable, its detection probability is set to zero. A measurement $\RV{z}_{m,n}^{(j)}$ may also not originate from any mva. This type of measurement is referred to as a false positive and is modeled as a Poisson point process with mean $\mu_{\mathrm{fp}}$ and pdf $f_{\mathrm{fp}}(\RV{z}_{m,n}^{(j)})$.

Newly detected mva, i.e., mva that generated a measurement for the first time, are modeled by a Poisson point process with mean $\mu_{\mathrm{n}}$ and pdf $f_{\mathrm{n}}(\V{p}_{m,\mathrm{mva}}|\V{p}_n)$. Newly detected mva are represented by new pmva states $\overline{\RV{y}}^{(j)}_{n,m}$, $m \rmv\rmv \in \{1,\dots, \rv{M}^{(j)}_n \}$ in our statistical model <cit.>. Each new pmva state corresponds to a measurement $\RV{z}_{m,n}^{(j)}$; $\overline{r}_{m,n} \!=\! 1$ implies that measurement $\RV{z}_{m,n}^{(j)}$ was generated by a newly detected mva. All new pmva states are introduced, assuming the corresponding measurements originate from a single-bounce path. This assumption leads to a simpler statistical model and reduced computational complexity as further discussed in Section <ref>. This assumption is well motivated by the fact that, due to the lower snr of double-bounce paths, new surfaces are typically detected first via a single-bounce measurement. However, the assumption also implies that any reflecting surface can only be mapped if it originates at least one single-bounce measurement.

We denote by $\overline{\RV{y}}^{(j)}_n \triangleq \big [ \ist \overline{\RV{y}}^{(j)\ist\T}_{1,n} \rmv\cdots\ist \overline{\RV{y}}^{(j)\ist \T}_{\rv{M}^{(j)}_n \rmv\rmv\rmv,n} \ist \big]^{\T}\rmv\rmv\rmv$, the joint vector of all new pmva states. Introducing new pmva for each measurement leads to a number of pmva states that grows with time $n$. Thus, to keep the proposed slam algorithm feasible, a sub-optimum pruning step is performed, removing pmva with a low probability of existence (see Section <ref>).

§.§ Legacy pmva  and State Transition

At time $n$, measurements are incorporated sequentially across pa $j \rmv\in\rmv \{1,\dots,J\}$. Previously detected mva, i.e., mva that have been detected either at a previous time $n' \!<\rmv n$ or at the current time $n$ but at a previous PA $j' \!<\rmv  j$, are represented by legacy pmva states $\underline{\RV{y}}_{s,n}^{(j)}$. New pmva become legacy pmva when the next measurements—either of the next pa or at the next time instance—are taken into account. In particular, the mva represented by the new mva state $\overline{\RV{y}}^{(j')}_{m',n'}$ introduced due to measurement $m'$ of pa $j'$ at time $n' \leq n$ is represented by the legacy pmva
state $\underline{\RV{y}}^{(j)}_{s,n}$ at time $n$, with $s = \rv{S}_{n'-1} + \sum^{j' - 1}_{j'' = 1} \rv{M}^{(j'')}_{n} + m' \vspace{.5mm}$. The number of legacy pmva at time $n$, when the measurements of the next pa $j$ are incorporated, is updated according to $\rv{S}^{(j)}_{n} = \rv{S}^{(j-1)}_{n} + \rv{M}^{(j-1)}_{n}$, where $\rv{S}^{(1)}_{n} \rmv=\rmv \rv{S}_{n-1}$. Here, $\rv{S}^{(j)}_{n}$ is equal to the number of all measurements collected up to time $n$ and PA $j \rmv-\rmv 1$.
The vector of all legacy pmva states at time $n$ and up to pa $j$ can now be written as $\underline{\RV{y}}^{(j)}_{n} = \big[\underline{\RV{y}}^{(j-1) \T}_{n} \ist\ist\ist \overline{\RV{y}}^{(j-1) \T}_{n} \big]^{\T}\rmv\rmv$.

Let us denote by $\underline{\RV{y}}^{(1)}_n \rmv\triangleq\rmv \big[ \underline{\RV{y}}^{\T}_{1,n} \rmv\cdots\ist \underline{\RV{y}}^{T}_{S_{n\rmv-\rmv1},n} \big]^{\T}\rmv\rmv$, the vector of all legacy pmva states before any measurements at time $n$ have been incorporated. After the measurements of all pa $j \in \{1,\dots,J\}$ have been incorporated at time $n$, the total number of pmva states is

\begin{align}
\rv{S}_n = \rv{S}_{n-1} + \sum_{j=1}^{J} \rv{M}^{(j)}_n = \rv{S}^{(J)}_{n} + \rv{M}^{(J)}_n \label{eq:NrPMVAtotal-update}\\[-8mm]\nn
\end{align}

and the vector of all PMVA states at time $n$ is given by $\RV{y}_{n} \!=\rmv \big[\underline{\RV{y}}^{(J) \T}_{n} \ist\ist \overline{\RV{y}}^{(J) \T}_{n} \big]^{\T}\rmv\rmv$.[Note that this sequential incorporation of new pmva states is based on the multisensor multitarget tracking approach introduced in <cit.>.] We also define the number of va for each pa given as $|\mathcal{D}^{(j)}_n| = \rv{S}^{(j)}_n+\rv{S}^{(j)}_n(\rv{S}^{(j)}_n-1)$ with $\mathcal{D}^{(j)}_n \in \{ (s,s') \rmv\in\rmv \Set{S}_n \rmv\times\rmv \Set{S}_n \} = \Set{D}_{\text{S},n}^{(j)} \cup \Set{D}_{\text{D},n}^{(j)}$ and $\tilde{\mathcal{D}}^{(j)}_n = (0,0) \cup \mathcal{D}^{(j)}_n $.

Legacy pmva states $\underline{\RV{y}}_{s,n}$ and the agent state $\RV{x}_n$ are assumed to evolve independently across time according to state-transition pdf $f\big(\underline{\V{y}}_{s,n} \big| \V{y}_{s, n-1}\big)$ and  $f(\V{x}_{n}|\V{x}_{n-1})$, respectively. If pmva $k$ exists at time $n \rmv-\! 1$, i.e., $r_{s,n-1} \!=\! 1$, it either disappears, i.e., $\overline{r}_{s,n} \!=\rmv 0$, or survives, i.e., $\overline{r}_{s,n} \!=\! 1$; in the latter case, it becomes a legacy pmva at time $n$. The probability of survival is denoted by $p_\mathrm{s}$. Suppose the pmva survives. In that case, its position remains unchanged, i.e., the state-transition pdf of the mva positions $\underline{\RV{p}}_{s,\mathrm{mva}}$ is given by $f\big(\underline{\V{p}}_{s,\mathrm{mva}}  \ist \big| \ist \V{p}_{s,\mathrm{mva}} \big) = \delta \big(\underline{\V{p}}_{s,\mathrm{mva}} \rmv - \ist \V{p}_{s,\mathrm{mva}} \big)$.
Therefore, $f\big(\underline{\V{p}}_{s,\mathrm{mva}}\rmv,\underline{r}_{s,n} \ist \big| \ist \V{p}_{s,\mathrm{mva}} , r_{s,n-1} \big) $ for $r_{s,n-1} \rmv=\rmv 1$ is obtained as

\begin{align}
&f\big(\underline{\V{p}}_{s,\mathrm{mva}},\underline{r}_{s,n} \ist \big| \ist \V{p}_{s,\mathrm{mva}} , r_{s,n-1} = 1\big) \nn\\[0mm]
(1 \!-\rmv p_\mathrm{s}) \ist f_\text{d}\big(\underline{\V{p}}_{s,\mathrm{mva}} \big) , &\!\!\! \underline{r}_{s,n} \!=\rmv 0 \\[0mm]
p_\mathrm{s} \ist\ist \delta \big(\underline{\V{p}}_{s,\mathrm{mva}} \rmv - \ist \V{p}_{s,\mathrm{mva}} \big) , &\!\!\! \underline{r}_{s,n} \!=\! 1.
\end{cases}\label{eq:stmpmvarone}\\[-7mm]\nn
\end{align}

If mva $s$ does not exist at time $n \rmv-\! 1$, i.e., $r_{s,n-1} \!=\! 0$, it cannot exist as a legacy pmva at time $n$
either, thus we get

\begin{align}
&f\big(\underline{\V{p}}_{s,\mathrm{mva}},\underline{r}_{s,n} \big| \V{p}_{s,\mathrm{mva}} , r_{s,n-1} \rmv\rmv= \rmv\rmv 0\big) \nn\\
f_\text{d}\big( \underline{\V{p}}_{s,\mathrm{mva}} \big) , &\!\!\! \underline{r}_{s,n} \!=\rmv 0 \\[0mm]
0 , &\!\!\! \underline{r}_{s,n} \!=\! 1.
\end{cases}\label{eq:stmpmvarzero}\\[-5mm]\nn
\end{align}

For $j \geq 2$, we also define $f^{(j)}\big(\underline{\V{y}}^{(j)}_{s,n} \big| \underline{\V{y}}^{(j-1)}_{s, n}\big)$ as

\begin{align}
&f^{(j)}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},\underline{r}^{(j)}_{s,n} \ist \big| {\ist \underline{\V{p}}^{(j-1)}_{s,\mathrm{mva}} , \underline{r}^{(j-1)}_{s,n} = 1}\big) \nn\\[0mm]
f_\text{d}\big(\underline{\V{p}}^{(j-1)}_{s,\mathrm{mva}} \big) , &\!\!\! \underline{r}^{(j)}_{s,n} \!=\rmv 0 \\[0mm]
\delta \big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}} \rmv - \ist \underline{\V{p}}^{(j-1)}_{s,\mathrm{mva}} \big) , &\!\!\! \underline{r}^{(j)}_{s,n} \!=\! 1
\end{cases}\label{eq:stmpmvaonepas}\\[-7mm]\nn
\end{align}

\begin{align}
&f^{(j)}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},\underline{r}^{(j)}_{s,n} \big| {\underline{\V{p}}^{(j-1)}_{s,\mathrm{mva}} , \underline{r}^{(j-1)}_{s,n} \rmv\rmv= \rmv\rmv 0}\big) \nn\\
f_\text{d}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}} \big) , &\!\!\! \underline{r}^{(j)}_{s,n} \!=\rmv 0 \\[0mm]
0 , &\!\!\! \underline{r}^{(j)}_{s,n} \!=\! 1.
\end{cases}\label{eq:stmpmvazeropas}
\end{align}

where we introduced $ \underline{\V{y}}^{(j-1)}_{s, n} \triangleq  \underline{\V{y}}_{s, n}$, $\V{p}^{(j-1)}_{s,\mathrm{mva}} \triangleq \V{p}_{s,\mathrm{mva}}$, and $ \underline{r}^{(j-1)}_{s, n} \triangleq  \underline{r}_{s, n}$. It it assumed that at time $n \rmv=\rmv 0$ the initial prior pdf $f\big(\V{y}_{s,0} \big)$, $s = \big\{1,\dots,S_0\big\}$ and  $f(\V{x}_{0})$ are known. All (legacy
and new) pmva states and all agent states up to time $n$ are denoted as $\RV{y}_{0:n} \triangleq \big[\RV{y}^{\T}_{0} \cdots\ist \RV{y}^{\T}_{n} \big]^{\T}\!$ and $\RV{x}_{0:n} \triangleq \big[\RV{x}^{\T}_{0} \cdots\ist \RV{x}^{\T}_{n} \big]^{\T}\!$, respectively.

§.§ Data Association Uncertainty

Mapping of reflective surfaces modeled by mva is complicated by the data association uncertainty: at time $n$ it is unknown which measurement $\RV{z}_{m,n}^{(j)}$ extracted at pa $j$ originated from pa $j$ itself $(0,0)$, from which mva $(s,s) \in \mathcal{D}^{(j)}_{\text{S},n}$, or from which mva-mva pair $(s,s') \in \mathcal{D}^{(j)}_{\text{D},n}$ associated with single-bounce and double-bounce path. Any pmva-to-measurement association (which considers associations to single pmva and pmva-pmva pairs as well as to pa $j$ itself) is described by pmva-oriented association

\begin{equation}
\hspace*{-2mm}\rv{\underline{a}}^{(j)}_{ss',n} \ist\triangleq \begin{cases}
m \rmv\in\rmv \Set{M}_n^{(j)}\ist, & \begin{minipage}[t]{45mm}if legacy \ac{pmva} $ss'$ generates measurement $m$\\[-2.5mm]\end{minipage}\\[4mm]
0 \ist, & \begin{minipage}[t]{45mm}if legacy \ac{pmva} $ss'$ does not generate any measurement\\[-4.5mm]\end{minipage}
\end{cases}\label{eq:pmvaorientedDAV}
\end{equation}

with $(s,s') \in \tilde{\mathcal{D}}^{(j)}_n$ and stacked into the pmva-oriented association vector as $\vspace*{0.5mm}\underline{\RV{a}}^{(j)}_n \rmv= \big[\rv{\underline{a}}^{(j)}_{00,n} \iist \rv{\underline{a}}^{(j)}_{11,n} \cdots \rv{\underline{a}}^{(j)}_{\rv{S}_n^{(j)}\rv{S}_n^{(j)}\rmv\rmv\rmv,n} \big]^{\T}\rmv\rmv$.
To reduce computation complexity, following <cit.>, we use a redundant description of pmva-measurement associations, i.e., we introduce measurement-oriented association variables

\begin{equation}
\rv{\overline{a}}^{(j)}_{m,n} \rmv\rmv\triangleq\rmv\rmv \begin{cases}
(s,\rmv s') \rmv\rmv\in\rmv\rmv \tilde{\mathcal{D}}^{(j)}_n \rmv\rmv,\rmv\rmv\rmv\rmv\rmv& \begin{minipage}[t]{45mm}if measurement $m$ is originated by legacy \ac{pmva} $ss'$\end{minipage}\\[2mm]
0,\rmv\rmv\rmv\rmv\rmv& \begin{minipage}[t]{45mm}if measurement $m$ is not generated by any legacy \ac{pmva} $ss'$\end{minipage}
\end{cases}\rmv\rmv\label{eq:measorientedDAV}
\end{equation}

and stacked into the measurement-oriented association vector as $\overline{\RV{a}}^{(j)}_n \rmv= \big[\rv{\overline{a}}^{(j)}_{1,n} \cdots \rv{\overline{a}}^{(j)}_{M^{(j)}_n \rmv\rmv\rmv,n} \big]^{\T}\rmv\vspace*{0.4mm}$. Note that any data association event that can be expressed by both a joint pmva-oriented association vector $\underline{\RV{a}}^{(j)}_n$ and measurement-oriented association vector $\overline{\RV{a}}^{(j)}_n$  is a valid event in the sense that an pmva generates at most one measurement. A measurement is originated by at most one pmva. This hybrid representation of data association makes it possible to develop scalable spa for simultaneous agent localization mva mapping <cit.>. Finally, we also introduce the joint association
vectors $\underline{\RV{a}}_n \rmv= \big[\underline{\RV{a}}^{(1) \T}_{n} \cdots \underline{\RV{a}}^{(J) \T}_{n} \big]^{\T}\rmv\rmv$, $\overline{\RV{a}}_n \rmv= \big[\overline{\RV{a}}^{(1)\T}_{n} \cdots \overline{\RV{a}}^{(J)\T}_{n} \big]^{\T}\rmv\rmv$, $\underline{\RV{a}}_{1:n} \rmv= \big[\underline{\RV{a}}^{\T}_{1} \cdots \underline{\RV{a}}^{\T}_{n} \big]^{\T}\rmv\rmv$, and $\overline{\RV{a}}_{1:n} \rmv= \big[\overline{\RV{a}}^{\T}_{1} \cdots \overline{\RV{a}}^{\T}_{n} \big]^{\T}\rmv\rmv$.

§.§ Detection Probabilites and Availability of Paths

rt relies on visibility-tree techniques  <cit.> to determine the visibility of a path in a backward manner as discussed in what follows for the double-bounce case $(s,s') \in \Set{D}_{\text{D},n}^{(j)}$. First, starting from the agent's position, $\V{p}_n$, a straight line is drawn towards the va position, $\V{p}^{(j)}_{ss',\mathrm{va}} $, until it intersects with the reflective surface with index $s'$. From the resulting intersection point, another line is drawn towards the  va position, $\V{p}^{(j)}_{ss,\mathrm{va}} $, until it intersects with the reflective surface with index $s$. From this second intersection point, a line is finally drawn towards physical anchor position $\V{p}^{(j)}_{\mathrm{pa}} $. If this procedure fails, because (i) there is no intersect first with surface $s'$ and then with surface $s$, or (ii) along the path, there is an intersection with another surface $s'' \in \Set{S} \backslash \{j,j'\}$, the path is considered not available or “blocked”. Intersections are calculated efficiently based on the fast line intersection algorithm <cit.>.

For single-bounce and LOS paths, the procedure is simpler. In particular, for the single-bounce case  $(s,s) \in \Set{D}_{\text{D},n}^{(j)}$, starting from the agent's position, the first of two straight lines is already drawn from the agent's position $\V{p}_n$, towards the va position, $\V{p}^{(j)}_{ss',\mathrm{va}} $. For the LOS case, the first and only straight line is directly drawn from the agent's position $\V{p}_n$ to $\V{p}^{(j)}_{\mathrm{pa}} $.
For each pa $j$, this availability check is directly integrated into  detection probabilities. In particular, the los path detection probability, $p^{(j)}_{\mathrm{d}}\big (\RV{p}_n\big)$, is given by

\begin{align}
p^{(j)}_{\mathrm{d}}\big (\RV{p}_n\big) \ist\triangleq \begin{cases}
p_{\mathrm{d},00,n}^{(j)}\ist, & \begin{minipage}[t]{35mm}path from agent at $\RV{p}_n$ to \ac{va} at $\RV{p}^{(j)}_{\text{pa}}$ is available\\[-2.5mm]\end{minipage}\\[4mm]
0 \ist, & \begin{minipage}[t]{35mm}path is not available\\[-2.5mm]\end{minipage}
\end{cases}\label{eq:visibilityPA}\\[-6mm]\nn
\end{align}

where $p_{\mathrm{d},00,n}^{(j)}$ is the detection probability <cit.> of the available los path. Similarly, the single-bounce detection probability, $p^{(j)}_{\mathrm{d}}\big (\RV{p}_n,\RV{p}_{s,\text{mva}}\big)$, $(s,s) \in \Set{D}_{\text{S},n}^{(j)}$, reads

\begin{align}
\hspace*{-1mm} p ^{(j)}_{\mathrm{d}}\big (\RV{p}_n,\RV{p}_{s,\text{mva}}\big) \ist\triangleq \begin{cases}
p_{\mathrm{d},ss,n}^{(j)}\ist, & \hspace*{-0.5mm}\begin{minipage}[t]{35mm}path from agent at $\RV{p}_n$ to \ac{va} at $\RV{p}^{(j)}_{ss,\text{va}}$ is  available\\[-2.5mm]\end{minipage}\\[4mm]
0 \ist, & \hspace*{-0.5mm}\begin{minipage}[t]{35mm}path is not available\\[-2.5mm]\end{minipage}
\end{cases}\label{eq:visibilitySB}\\[-7mm]\nn
\end{align}

where $p_{\mathrm{d},ss,n}^{(j)}$ is the detection probability of available single-bounce path. Finally, the double-bounce detection probability $p^{(j)}_{\mathrm{d}}\big (\RV{p}_n,\RV{p}_{s,\text{mva}},\RV{p}_{s',\text{mva}}\big)$, $(s,s') \in \Set{D}_{\text{D},n}^{(j)}$, can be obtained as

\begin{align}
&p^{(j)}_{\mathrm{d}}\big (\RV{p}_n,\RV{p}_{s,\text{mva}},\RV{p}_{s',\text{mva}}\big) \nn\\[1.5mm]
&\hspace{10mm}\triangleq \begin{cases}
p_{\mathrm{d},ss',n}^{(j)}\ist, & \begin{minipage}[t]{43mm}path from agent at $\RV{p}_n$ to \ac{pmva} at $\RV{p}^{(j)}_{ss',\text{va}}$ is  available\\[-2.5mm]\end{minipage}\\[4mm]
0 \ist, & \begin{minipage}[t]{43mm}path is not available\\[-2.5mm]\end{minipage}
\end{cases} \label{eq:visibilityDB}\\[-7mm]\nn
\end{align}

where $p_{\mathrm{d},ss',n}^{(j)}$ is again the detection probability of the available double-bounce path.

Factor graph representation of the joint posterior pdf (<ref>). Short notations are used.
In particular, the time index $n$ and the functional dependencies of the factors are neglected:
$\V{x} \rmv\triangleq\rmv \V{x}_n $,
$M^j \rmv\triangleq\rmv M_n^{(j)}$,
$\underline{\V{y}}^j_s \rmv\triangleq\rmv \underline{\V{y}}_{s,n}^{(j)}$,
$\overline{\V{y}}^j_m \rmv\triangleq\rmv \overline{\V{y}}_{m,n}^{(j)}$,
$\underline{q}^j_0 \rmv\triangleq\rmv \underline{q}_{\mathrm{P}}\big(  \V{x}_{n} , \underline{a}^{(j)}_{00,n}; \V{z}^{(j)}_{n} \big)$,
$\underline{q}^j_s \rmv\triangleq\rmv \underline{q}_{\mathrm{S}}\big( \underline{\V{y}}_{s,n}^{(j)},  \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$,
$\underline{q}^j_{ss'} \rmv\triangleq\rmv \underline{q}_\text{D}\big( \underline{\V{y}}_{s,n}^{(j)}, \underline{\V{y}}_{s',n}^{(j)},  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)
{\, \underline{q}_\text{D}\big( \underline{\V{y}}_{s',n}^{(j)}, \underline{\V{y}}_{s,n}^{(j)},  \underline{a}^{(j)}_{s's,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)}$,
$\overline{q}^j_m \rmv\triangleq\rmv \overline{q}_{\mathrm{S}}\big( \overline{\V{y}}^{(j)}_{m,n}, \overline{a}^{(j)}_{m,n},\V{x}_{n}; z^{(j)}_{m,n} \big)$,
$f \rmv\triangleq\rmv f(\V{x}_n | \V{x}_{n-1})$,
$f^1_s \rmv\triangleq\rmv f\big(\underline{\V{y}}_{s,n} | \V{y}_{s,n-1}\big)$,
$f^j_s \rmv\triangleq\rmv f^{(j)}\big(\underline{\V{y}}^{(j)}_{s,n} \big| \underline{\V{y}}^{(j-1)}_{s,n}\big)$,
$\alpha_n = \alpha( \V{x}_n)$,
$\alpha_s^j = \alpha_s\big( \underline{\V{y}}^{(j)}_{s,n}\big)$,
$\beta_{ss'}^j = \beta_{ss'}\big(\underline{a}_{ss',n}^{(j)} \big)  {\,  \beta_{s's}\big(\underline{a}_{s's,n}^{(j)} \big) }$,
$\gamma_{ss'}^j = \gamma^{(j)}_{ss'}(\V{x}_n)  {\,  \gamma^{(j)}_{s's}(\V{x}_n) }$,
$\xi_m^j  = \xi \big(\overline{a}^{(j)}_{m,n}\big)$,
$\eta_{ss'}^j = \eta(\underline{a}_{ss',n}^{(j)} )   {\,   \eta(\underline{a}_{s's,n}^{(j)} )  }$,
$\nu_{ss'}^j = \nu_{m\rightarrow ss'}^{(p)}(\underline{a}_{ss',n} ^{(j)}) {\,  \nu_{m\rightarrow s's}^{(p)}(\underline{a}_{s's,n} ^{(j)}) }$,
$\varsigma_{m}^j = \varsigma\big( \overline{a}_{m,n}^{(j)} \big)$,
$\zeta_{m}^j = \zeta_{ss' \rightarrow m}^{(p)}\big(\overline{a}_{m,n}^{(j)}\big) {\, \zeta_{s's \rightarrow m}^{(p)}\big(\overline{a}_{m,n}^{(j)}\big) }$,
$\tilde{\rho}_{ss'}^j = \rho_{ss'}\big(\underline{\V{y}}^{(j)}_{s}\big) {\,  \rho_{s's}\big(\underline{\V{y}}^{(j)}_{s}\big)  }$,
$\phi_{m}^j = \phi(\overline{\V{y}}_{s}^{(j)})$.  For the numbers of mva, the short notations reads $S^{1} \rmv\triangleq\rmv S_{n-1}$,
$S^j \rmv\triangleq\rmv S_n^{(j)}$, and
$(\cdot)_{S^{jj}} \rmv\triangleq\rmv (\cdot)_{S_n^{(j)}S_n^{(j)}}$.
The dashed lines with arrows indicate messages representing the agent and pmva beliefs of time $n\rmv\rmv-\rmv\rmv 1$, $n+1$ or of anchors $j\rmv\rmv-\rmv\rmv 1$, $j\rmv\rmv+\rmv\rmv 1$. These messages are either only sent to the next time step (e.g., from $n-1$ to $n$) or only to the next anchor (e.g., from $j-1$ to $j$).

§ PROBLEM FORMULATION AND PROPOSED METHOD

In this section, we formulate the estimation problem of interest and present the joint posterior pdf and factor graph underlying the proposed slam method.

§.§ Pre-Estimation Stage

By applying at each time $ n $ a super-resolution channel estimation algorithm <cit.> to the observed rf signal vector one obtains, for each anchor $j$, a number of $M_n^{(j)}$ measurements denoted by ${\V{z}^{(j)}_{m,n}}$ with $m \in  \Set{M}_n^{(j)} \triangleq \{1,\,\dots\,,M_n^{(j)}\} $.
Each  $\V{z}^{(j)}_{m,n} = [{z^{(j)}_\text{d}}_{\rmv\rmv\rmv\rmv\rmv\rmv\rmv m,n} ~ {z^{(j)}_\varphi}_{\rmv\rmv\rmv\rmv\rmv m,n}]^\text{T}$ representing a potential mpc parameter estimate contains a distance measurement ${z_\text{d}}_{m,n}^{(j)} \rmv\rmv\in [0, d_\text{max}]$ and an aoa measurement ${z_\varphi}_{m,n}^{(j)} \rmv\rmv\in\rmv\rmv [-\pi, \pi )$. The channel estimator decomposes the rf signal vector into individual, decorrelated components, reducing the number of dimensions (as ${M}_n^{(j)}$ is usually much smaller than the number of signal samples). It thus compresses the information contained in the rf signal vector into $\vspace*{-0.2mm}\V{z}^{(j)}_{n} = [{\bm{z}^{(j)\text{T}}_{1,n}}  \rmv \cdots  {\V{z}^{(j)\text{T}}_{M_n^{(j)},n}}]^\text{T}$. We also define
$\V{z}_n \triangleq [ \V{z}_{n}^{(1)\ist \T}  \dots \V{z}_n^{(J)\ist \T} ]^{\T}\rmv\rmv\vspace*{-0.6mm}$ and  $\V{z}_{1:n} \triangleq \big[\V{z}^{\T}_{0} \cdots\ist \V{z}^{\T}_{n} \big]^{\T}$.

§.§ Confirmation of mva and State Estimation

We aim to estimate the agent state $\RV{x}_n$ using all available measurements $\V{z}_{1:n}\rmv$ from all pa up to time $n$. In particular, we calculate an estimate $\hat{\V{x}}_n$ by using the mmse estimator

\begin{equation}
\hat{\V{x}}_n \ist\triangleq\, \int \rmv \V{x}_n \ist f( \V{x}_n|\V{z}_{1:n}) \ist \mathrm{d} \V{x}_n \rmv. \label{eq:MMSEagent}
\vspace*{-1.5mm}
\end{equation}

The map of the environment is represented by reflective surfaces described by pmva. Therefore, the positions $\RV{p}_{s,\mathrm{mva}}$ of the detected pmva $s \!\in\! \{ 1,\dots,S_n \}$ must be estimated. This relies on the marginal posterior existence probabilities $p(r_{s,n} \!=\! 1|\V{z}_{1:n}) = \int f(\V{p}_{s,\text{mva}} , r_{s,n} \!=\! 1| \V{z}_{1:n} ) \mathrm{d}\V{p}_{s,\text{mva}}$ and the marginal posterior pdf $f(\V{p}_{s,\text{mva}} | r_{s,n} \!=\! 1, \V{z}_{1:n} ) \rmv\rmv=\rmv\rmv f(\V{p}_{s,\text{mva}} , r_{s,n} \!=\! 1| \V{z}_{1:n} )/p(r_{s,n} \!=\! 1|\V{z}_{1:n})$. A pmva $s$ is declared to exist $p(r_{s,n} \!=\! 1|\V{z}_{1:n}) > p_{\mathrm{cf}}$, where $p_{\mathrm{cf}}$ is a confirmation threshold. The number $\hat{S}_n$ of pmva states that are considered to exist is the estimate of the total number $S$ of mva. To avoid that the number of pmva states grows indefinitely, pmva states with $p(r_{s,n} \!=\! 1|\V{z}_{1:n}) < p_{\text{pr}}$ are removed from the state space (“pruned”). For existing pmva, an estimate of it's position $\RV{p}_{s,\mathrm{mva}}$ can again be calculated by the mmse <cit.>

\begin{align}
\hat{\V{p}}_{k,\mathrm{mva}} \,\triangleq \int \rmv \V{p}_{k,\mathrm{mva}}  \ist\ist f(\V{p}_{k,\mathrm{mva}} \ist | \ist r_{s,n} \!=\! 1, \V{z}_{1:n}) \ist\ist \mathrm{d} \V{p}_{k,\mathrm{mva}} \ist. \label{eq:MMSEpmva}
\vspace*{-4mm}
\end{align}

The calculation of $f( \V{x}_n|\V{z}_{1:n})$, $p(r_{s,n} \!=\! 1 |\V{z})$, and $f(\V{p}_{k,\mathrm{mva}} |$ $r_{s,n} \rmv=\rmv 1, \V{z}_{1:n})$ from the joint posterior $f( \V{y}_{0:n}, \V{x}_{0:n},\underline{\V{a}}_{1:n},\overline{\V{a}}_{1:n} $ $| \ist \V{z}_{1:n}) $ by direct marginalization is not feasible. By performing sequential message passing using the SPA rules <cit.> on the factor graph in Fig. <ref>, approximations (“beliefs”) $\tilde{f}\big(\V{x}_{n} \big)$ and $\tilde{f}_s\big(\V{y}_{s,n} \big)$ of the marginal posterior pdf $f( \V{x}_n|\V{z}_{1:n})$, $p(r_{s,n} \!=\! 1 |\V{z}_{1:n})$, and $f(\V{p}_{s,\mathrm{mva}} |$ $r_{s,n} \rmv=\rmv 1, \V{z}_{1:n})$ can be obtained efficiently for the agent state as well as all legacy and new pmva states $s \in \Set{S}_n$.

\begin{align}
&f( \V{y}_{0:n}, \V{x}_{0:n}, \underline{\V{a}}_{1:n},\overline{\V{a}}_{1:n} \ist | \ist \V{z}_{1:n})\nn\\[-.5mm]
&\hspace{0mm} \propto \hspace{-3mm} \underbrace{\bigg( \ist f(\V{x}_{0}) \ist \prod^{S_0}_{s = 1} \ist f(\V{y}_{s,0})  \bigg)}_{\footnotesize \text{Initial prior \acp{pdf} of \ac{pmva} and agent states}} \hspace{-5mm}\prod^{n}_{n' = 1}  \underbrace{f\big(\V{x}_{n'} | \V{x}_{n'-1}\big)}_{\footnotesize \text{Agent state transition}} \hspace{.8mm} \underbrace{\bigg(\prod^{J}_{j = 1} \ist \underline{q}_{\mathrm{P}}\big( \V{x}_{n'}, \underline{a}^{(j)}_{00,n'}; \V{z}^{(j)}_{n'} \big) \prod^{M^{(j)}_{n'}}_{m' = 1} \rmv\rmv \Psi\big(\underline{a}^{(j)}_{00,n'},\overline{a}^{(j)}_{m',n'} \big) \bigg)}_{\footnotesize \text{Factors related to \acp{pa}}} \underbrace{\bigg(  \prod^{S_{n'\rmv-\rmv1}}_{s' = 1} f \big(\underline{\V{y}}_{s'\rmv\rmv,n'} | \V{y}_{s'\rmv\rmv,n'-1}\big) \rmv \bigg)}_{\footnotesize \text{Legacy \ac{pmva} state transition}}\nn\\[-1mm]
&\hspace{1mm}\times \underbrace{\bigg( \prod^{J}_{j' = 2} \ist\ist \bigg(\prod^{S^{(j')}_{n'}}_{s' = 1}\ist f^{(j)}\big(\underline{\V{y}}^{(j')}_{s',n'} \big| \underline{\V{y}}^{(j'-1)}_{s', n'}\big)\bigg)\bigg) \prod^{J}_{j = 1} \bigg( \ist \prod^{S^{(j)}_{n'}}_{s = 1} \underline{q}_{\mathrm{S}}\big( \underline{\V{y}}^{(j)}_{s,n'}\rmv, \underline{a}^{(j)}_{ss,n'},  \V{x}_{n'} ; \V{z}^{(j)}_{n'} \big) \bigg(\prod^{M^{(j)}_{n'}}_{m' = 1} \rmv\rmv \Psi\big(\underline{a}^{(j)}_{ss,n'},\overline{a}^{(j)}_{m',n'} \big) \rmv\rmv \bigg)}_{\footnotesize  \text{Factors related to legacy \ac{pmva} states}} \nn\\[-1mm]
& \hspace{1mm} \times \rmv\rmv\rmv\underbrace{\prod^{S^{(j)}_{n'}}_{s' = 1,s' \neq s}  \underline{q}_{\mathrm{D}}\big(\underline{\V{y}}^{(j)}_{s,n'}, \underline{\V{y}}^{(j)}_{s',n'},\underline{a}^{(j)}_{ss',n'},  \V{x}_{n'} ; \V{z}^{(j)}_{n'} \big) \rmv\rmv\prod^{M^{(j)}_{n'}}_{m' = 1} \rmv\rmv \Psi\big(\underline{a}^{(j)}_{ss',n'},\overline{a}^{(j)}_{m',n'} \big) \bigg)}_{\footnotesize \text{Factors related to legacy \ac{pmva} states}}  \underbrace{\bigg( \prod^{M^{(j)}_{n'}}_{m = 1} \overline{q}_{\mathrm{S}}\big( \overline{\V{y}}^{(j)}_{m,n'}, \overline{a}^{(j)}_{m,n'}, \V{x}_{n'} ; \V{z}^{(j)}_{n'} \big) \rmv \bigg)}_{\footnotesize  \text{Prior \acp{pdf} and related parameters of new \ac{pmva} states}}
\label{eq:factorGraph}\\[-9mm] \nn
\end{align}

§.§ The Factor Graph

By using common assumptions <cit.>, and for fixed (observed) measurements $\V{z}_{1:n}$,
the joint posterior pdf of $\RV{y}_{0:n}$, $\RV{x}_{0:n}$, $\underline{\RV{a}}_{1:n}$, and $\overline{\RV{a}}_{1:n}$, conditioned on $\V{z}_{1:n}$ is given by (<ref>) as shown on top of the page, where we introduced the
functions $\underline{q}_{\mathrm{P}}\big( \V{x}_{n'}, \underline{a}^{(j)}_{00,n'}; \V{z}^{(j)}_{n'} \big)$, $\underline{q}_{\mathrm{S}}\big( \underline{\V{y}}^{(j)}_{s,n}\rmv, \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$, $\underline{q}_{\mathrm{D}}\big( \underline{\V{y}}^{(j)}_{s,n}\rmv,\underline{\V{y}}^{(j)}_{s',n}\rmv, \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$, $\overline{q}_{\mathrm{S}}\big( \overline{\V{y}}^{(j)}_{m,n}, \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; z^{(j)}_{m,n} \big) $, $\bar{f}\big(\overline{\V{y}}_{s,n}\big)$, and
$\Psi\big(a^{(j)}_{s,n},\overline{a}^{(j)}_{m,n} \big) $ that will be discussed next. A detailed derivation of (<ref>) is provided in the supplementary material Sectionsec:app_statistical_model.

The pseudo likelihood functions of pa $j$ $\underline{q}_{\mathrm{P}}\big( \V{x}_{n}, \underline{a}^{(j)}_{00,n}; \V{z}^{(j)}_{n} \big)$, of legacy pmva related to single-bounce paths $\underline{q}_{\mathrm{S}}\big( \underline{\V{y}}^{(j)}_{s,n}\rmv,\underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) = \underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv,\underline{r}^{(j)}_{s,n}\rmv,$ $\underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$ and to double-bounce paths $\underline{q}_{\mathrm{D}}\big( \underline{\V{y}}^{(j)}_{s,n}\rmv,\underline{\V{y}}^{(j)}_{s',n}\rmv, \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) = \underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n}\rmv,\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv,$ $\underline{r}^{(j)}_{s',n}\rmv, \underline{a}^{(j)}_{ss',n}, \V{x}_{n} ; \V{z}^{(j)}_{n} \big)\vspace*{0.4mm}$ are, respectively, given for $(0,0)$ by

\begin{align}
&\underline{q}_{\mathrm{P}}\big( \V{x}_{n}, \underline{a}^{(j)}_{00,n}; \V{z}^{(j)}_{n} \big)  \nn\\
&\hspace{4mm} \triangleq \begin{cases}
\displaystyle \ist \frac{ p^{(j)}_{\mathrm{d}} (\V{p}_n)  f\big( \V{z}_{m,n}^{(j)} \big|\ist \V{p}_n \big)}{ \mu_{\mathrm{fp}} \ist f_{\mathrm{fp}}\big( \V{z}_{m,n}^{(j)} \big)} \ist,
& \!\!\rmv \underline{a}^{(j)}_{00,n} \!=\rmv m \rmv\in\rmv \Set{M}_n^{(j)}\\[3.5mm]
1 \!-\rmv p^{(j)}_{\mathrm{d}} (\V{p}_n) \ist, & \!\!\rmv \underline{a}^{(j)}_{00,n} \!=\rmv 0
\end{cases} \label{eq:factorqP}\\[-5mm]\nn
\end{align}

for $(s,s) \in \mathcal{D}^{(j)}_{\text{S},n}$ by

\begin{align}
&\underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv,  \underline{r}^{(j)}_{s,n} \rmv=\rmv1, \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)  \nn\\
&\hspace{0mm} \triangleq \begin{cases}
\displaystyle \ist \frac{ 	p^{(j)}_{\mathrm{d}}(\V{p}_n,\V{p}^{(j)}_{s,\text{mva}})  f\big( \V{z}_{m,n}^{(j)} \big|\ist \V{p}_n, \V{p}_{s,\text{mva}}^{(j)} \big)}{ \mu_{\mathrm{fp}} \ist f_{\mathrm{fp}}\big( \V{z}_{m,n}^{(j)} \big)} \ist,
& \!\!\rmv \underline{a}^{(j)}_{ss,n} \!=\rmv m \rmv\in\rmv \Set{M}_n^{(j)}\\[3.5mm]
1 \!-\rmv 	p^{(j)}_{\mathrm{d}}(\V{p}_n,\V{p}^{(j)}_{s,\text{mva}}) \ist, & \!\!\rmv \underline{a}^{(j)}_{ss,n} \!=\rmv 0
\end{cases} \label{eq:factorqS}
\end{align}

and $\underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n} \rmv=\rmv 0, \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)   \rmv\triangleq\rmv {\delta_{\underline{a}^{(j)}_{ss,n}}}$ as well as for $(s,s') \in \mathcal{D}^{(j)}_{\text{D},n}$ by

\begin{align}
&\underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n}\rmv=\rmv1\rmv,\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s',n}\rmv=\rmv1\rmv,  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)\nn\\
&\hspace{0mm}\triangleq \begin{cases}
\displaystyle \ist \frac{ p^{(j)}_{\mathrm{d}} (\V{p}_n,\V{p}^{(j)}_{s,\text{mva}},\V{p}^{(j)}_{s',\text{mva}})}{\mu_{\mathrm{fp}}} \\[3mm]
\hspace{4mm}\times \frac{ f( \V{z}_{m,n}^{(j)} |\ist \V{p}_n, \underline{\V{p}}^{(j)}_{s,\mathrm{mva}},  \underline{\V{p}}^{(j)}_{s',\mathrm{mva}})}{ f_{\mathrm{fp}}( \V{z}_{m,n}^{(j)} )}
\ist, \iist
& \!\!\rmv a^{(j)}_{ss'} \!=\rmv m \rmv\in\rmv \Set{M}_n^{(j)} \\[3.5mm]
1 \!-\rmv p^{(j)}_{\mathrm{d}} (\V{p}_n,\V{p}^{(j)}_{s,\text{mva}},\V{p}^{(j)}_{s',\text{mva}})  \ist, \iist & \!\!\rmv a^{(j)}_{ss'} \!=\rmv 0
\end{cases} \label{eq:factorqD}\\[-5mm]\nn
\end{align}

and $\underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n},\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s',n},  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)   \rmv\triangleq\rmv \delta_{\underline{a}^{(j)}_{ss',n}}$ if any $\underline{r}^{(j)}_{s'',n} \rmv=\rmv 0$ for $s'' \in \{s,s'\}$.

The pseudo likelihood functions related to new pmva $\overline{q}_{\mathrm{S}}\big(  \overline{\V{y}}^{(j)}_{m,\mathrm{mva}},\rmv \overline{a}^{(j)}_{m,n},\rmv \V{x}_{n} ;\rmv \V{z}^{(j)}_{n} \big) \rmv\rmv=\rmv\rmv \overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}},\rmv \overline{r}^{(j)}_{s,n},\rmv \overline{a}^{(j)}_{m,n},\rmv \V{x}_{n};\rmv \V{z}^{(j)}_{n} \big)$ is given by

\begin{align}
&\overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}}, \overline{r}^{(j)}_{s,n} \rmv=\rmv1, \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; \V{z}^{(j)}_{n} \big) \nn\\[1.5mm]
&\hspace{0mm} \triangleq \begin{cases}
0    \ist,
& \hspace{-1mm} \overline{a}^{(j)}_{m,n}  \rmv\rmv\in\rmv \tilde{\Set{D}}_{n}^{(j)} \\[1mm] %
\frac{ \mu_{\mathrm{n}}f_{\mathrm{n}}(\overline{\V{p}}^{(j)}_{m,\mathrm{mva}}\ist | \ist\V{p}_{n}) f(\V{z}_{m,n}^{(j)}\ist | \V{p}_{n}, \overline{\V{p}}^{(j)}_{m,\mathrm{mva}})}{\mu_{\mathrm{fp}}  f_{\mathrm{fp}}(\V{z}_{m,n}^{(j)})} \ist,  & \hspace{-1mm} \overline{a}^{(j)}_{m,n} \rmv=\rmv 0
\end{cases}  \label{eq:factorvNewPMVAs}\\[-6mm]\nn
\end{align}

and $\overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}}, \overline{r}^{(j)}_{s,n} \rmv=\rmv 0,  \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; z^{(j)}_{m,n} \big) \rmv\triangleq\rmv 	f_\text{d}\big(\overline{\V{p}}^{(j)}_{m,\mathrm{mva}}\big)$, respectively. Note that the first line in (<ref>) is zero because per definition, the new pmva with index $m$ exists ($\overline{r}_{m,n} \rmv=\rmv 1$) if and only if the measurement is not associated to a legacy PMVA. For $\overline{a}^{(j)}_{m,n} = 0$, for each measurement $z^{(j)}_{m,n}$ a new pmva is introduced, implying that each measurement $\V{z}_{m,n}^{(j)}$ is assumed to originate from a single-bounce path corresponding to exactly one pmva $s \in \Set{M}_{n}^{(j)}$ (not a pair of pmva).

Finally, the binary check functions that validates consistency for any pair $(\rv{\underline{a}}^{(j)}_{ss',n},\overline{\rv{a}}^{(j)}_{m,n})$ of pmva-oriented and measurement-oriented association variable at time $n$,

\begin{align}
&\Psi(\underline{a}^{(j)}_{ss',n},\overline{a}^{(j)}_{m,n}) %
\hspace{2mm}\triangleq \begin{cases}
0, & \begin{minipage}{5cm}
$\underline{a}^{(j)}_{ss',n} =  m, \overline{a}^{(j)}_{m,n} \neq (s,s') \text{ or }\\ \underline{a}^{(j)}_{ss',n} \neq  m, \overline{a}^{(j)}_{m,n} = (s,s')$
\end{minipage} \\[.1mm]
1 \ist, & \text{otherwise}.
\end{cases} \nn \\[-6mm]
\nn
\end{align}

In case the joint pmva-oriented association vector $\underline{\RV{a}}_n$ and the measurement-oriented association vector $\overline{\RV{a}}_n$ do not describe the same association event, at least one check function in (<ref>) is zero. Thus $f( \V{y}_{0:n}, \V{x}_{0:n}, \underline{\V{a}}_{1:n},\overline{\V{a}}_{1:n} \ist | \ist \V{z}_{1:n})$ is zero as well. The factor graph representing factorization (<ref>) is shown in Fig. <ref>. A detailed derivation of related factorization structures developed in the context of multitarget tracking and their relationship to the TOMB/P filter is presented in <cit.>. The problem and resulting factorization structure considered in this paper are more complicated than multisensor multitarget tracking because (i) there is also an unknown agent state and (ii) measurements can originate from two different types of propagation paths.

§ PROPOSED SUM-PRODUCT ALGORITHM

Since our factor graph in Fig. <ref> has cycles, we have to decide on a specific order of message computation <cit.>. We choose the order according to the following rules <cit.>: (i) messages are only sent forward in time; (ii) messages are only sent from pa $j\rmv-\rmv 1$ to pa $j$, i.e., the measurements of pa are processed serial, thus, pa $j\rmv-\rmv 1$ establishes new pmva that are acting as legacy pmva for pa $j$;
(iii) iterative message passing is only performed for data association <cit.>, i.e., in particular, for the loops connecting different pmva, we only perform a single message passing iteration; and (iv) along an edge connecting an agent state variable node and a new pmva state variable node, messages are only sent from the former to the latter.
With these rules, the message passing equations of the SPA <cit.> yield the following operations at each time step. The corresponding messages are shown in Fig. <ref>. Note that this message passing order has been developed for real-time processing. Sending messages also backward in time, referred to as  “smoothing,” will improve post-processing performance but lead to increased computational complexity.

We note that similarly to the “dummy pdf” introduced in Section <ref>, we consider messages $\varphi\big(\V{y}_{s,n}\big) \rmv\rmv=\rmv\rmv \varphi\big(\V{p}_{s,\mathrm{mva}}\ist, r_{s,n}\big)$ for non-existing pmva states, i.e., for $r_{s,n} \!=\rmv 0$. We define these messages by $\varphi\big( \V{p}_{s,\mathrm{mva}} \ist, 0\big) \triangleq \varphi^{(j)}_{k,n}$ (note that these messages are not pdf and thus are not required to integrate to $1$). To keep the notation concise, we also define the sets $\Set{M}_{0,n}^{(j)} \triangleq \Set{M}_n^{(j)} \cup \{0\}$ and $ \tilde{\Set{D}}^{(j)}_{0,n} \in \tilde{\Set{D}}^{(j)}_{n} \cup \{0\}$.

§.§ Prediction Step

First, a prediction step is performed for the agent and all legacy pmva $s \in S_{n-1}$. Based on the spa rule, the prediction message for the agent state is given by

\begin{align}
\alpha( \V{x}_n) =\rmv \int \rmv f(\V{x}_n|\V{x}_{n-1}) \ist \tilde{f}(\V{x}_{n-1}) \ist \mathrm{d}\V{x}_{n-1}
\label{eq:stateTransitionMessageAgent}\\[-7mm]\nn
\end{align}

and the prediction message for the legacy pmva is given by

\begin{align}
\hspace*{-0.5mm}\alpha_s\big( \underline{\V{p}}_{s,\text{mva}} \ist, \underline{r}_{s,n}\big) &=\rmv\rmv\rmv\rmv   \sum_{r_{k,n-1} \in \{0,1\}}\rmv\int \rmv\rmv f\big(\underline{\V{p}}_{s,\text{mva}}\ist, \underline{r}_{s,n} \big| \V{p}_{s,\text{mva}}\ist, r_{s,n-1} \big) \nn \\
&\hspace{5mm}\times\tilde{f}\big(\V{p}_{s,\text{mva}}\ist, r_{s,n-1} \big) \ist \mathrm{d}\V{p}_{s,\text{mva}}
\label{eq:stateTransitionMessageFeature} \\[-7mm]\nn
\end{align}

$s \rmv\rmv\in\rmv\rmv \{1,\dots,S_{n-1}\}$, where the beliefs of the agent state, $\tilde{f}(\V{x}_{n-1})$, and of the pmva states, $\tilde{f}\big(\V{p}_{s,\text{mva}}\ist, r_{s,n-1} \big)$, were calculated at the preceding time $n \rmv-\rmv 1$. Inserting (<ref>) and (<ref>) for $f\big(\underline{\V{p}}_{s,\text{mva}}\ist, \underline{r}_{s,n} \big| \V{p}_{s,\text{mva}} \ist, r_{s,n-1}\rmv\rmv=\rmv\rmv 1\big)$ and $f\big(\underline{\V{p}}_{s,\text{mva}}\ist, \underline{r}_{s,n} \big|\V{p}_{s,\text{mva}}\ist,$$ r_{s,n-1}\rmv=\rmv 0\big)$, respectively, we obtain

\begin{align}
\alpha_s\big(\underline{\V{p}}_{s,\text{mva}}\ist, 1\big)
=\ist p_\text{s} \rmv \int \rmv \delta\big(\underline{\V{p}}_{s,\text{mva}} - \V{p}_{s,\text{mva}}\big) \ist \tilde{f}_s\big(\V{p}_{s,\text{mva}}\ist, 1 \big) \ist \mathrm{d}\V{p}_{s,\text{mva}}
\label{eq:stateTransitionMessageFeature_r1}\\[-7mm]\nn
\end{align}

and $\alpha_s\big(\underline{\V{p}}_{s,\text{mva}}\ist, 0\big)  = \alpha_{s,n} f_\text{d}\big( \V{p}_{s,\mathrm{mva}} \big)$ with

\begin{align}
\alpha_{s,n} = (1 \!-\rmv p_\text{s}\big) \rmv\int\rmv \tilde{f}_s\big(\V{p}_{s,\text{mva}}\ist, 1\big) \ist \mathrm{d} \V{p}_{s,\text{mva}} +\ist 	\tilde{f}_{s,n-1}\ist.
\label{eq:stateTransitionMessageFeature_r0}\\[-7mm]\nn
\end{align}

We note that $\tilde{f}_{s,n-1} \triangleq \int \rmv \tilde{f}_s\big( \V{p}_{s,\text{mva}}\ist, 0\big) \ist \mathrm{d}\V{p}_{s,\text{mva}}$ approximates the probability of non-existence of legacy pmva $s$ at the previous time step $n\rmv-\rmv1$.

§.§ Sequential pa Update

At iteration $j \in \{1,\dots,J\}$, the following operations are calculated for all legacy and new pmva.

§.§.§ Transition of New and Legacy pmva States Between pa
For $j = 1$, the number of legacy pmva is $S^{(1)}_{n} = S_{n-1}$ with the corresponding state $\underline{\V{y}}^{(1)}_n \rmv\triangleq\rmv \big[ \underline{\V{y}}^{\T}_{1,n} \rmv\cdots\ist \underline{\V{y}}^{T}_{S_{n\rmv-\rmv1},n} \big]^{\T}\rmv\rmv$. Furthermore, the state $\overline{\V{y}}^{(j-1)}$ is empty and the prediction message of legacy pmva is $\alpha_s\big( \underline{\V{p}}^{(j)}_{s,\text{mva}} \ist, \underline{r}_{s,n}\big) \triangleq \alpha_s\big( \underline{\V{p}}_{s,\text{mva}} \ist, \underline{r}_{s,n}\big)$ as well as $\alpha^{(j)}_{s,n} \triangleq \alpha_{s,n}$. For $j > 1$, we have $S^{(j)}_{n} = S^{(j-1)}_{n} + M^{(j-1)}_{n}$ legacy pmva with states $\underline{\V{y}}^{(j)}_{n} = \big[\underline{\V{y}}^{(j-1) \T}_{n} \ist\ist\ist \overline{\V{y}}^{(j-1) \T}_{n} \big]^{\T}$ and $M^{(j)}_{n}$ new pmva with states $\overline{\V{y}}^{(j)}$. For $1 \leq s \leq S^{(j-1)}_{n}$, using (<ref>), (<ref>), and (<ref>) the prediction message of former legacy pmva is given by

\begin{equation}
\alpha_s\big(\underline{\V{p}}^{(j)}_{s,\text{mva}}\ist, 1\big)
=\rmv\rmv \int \rmv\rmv \delta\big(\underline{\V{p}}^{(j)}_{s,\text{mva}} - \V{p}^{(j-1)}_{s,\text{mva}}\big) \ist \gamma\big(\underline{\V{p}}^{(j-1)}_{s,\mathrm{mva}},1\big) \ist \mathrm{d}\V{p}^{(j-1)}_{s,\text{mva}}
\vspace{1.2mm}
\label{eq:statePATransitionMessageFeature_r1}
\end{equation}

and $\alpha_s\big(\underline{\V{p}}^{(j)}_{s,\text{mva}}\ist, 0\big)  = \alpha^{(j)}_{s,n} f_\text{d}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}} \big)$ with $\alpha^{(j)}_{s,n} =  \gamma_{s,n}^{(j-1)}$.
The prediction message of former new pmva is $\alpha_s\big( \underline{\V{p}}^{(j)}_{s,\text{mva}} \ist, \underline{r}_{s,n}\big) \triangleq \phi\big(\overline{\V{y}}^{(j)}_{s,n}\big)$ and $\alpha^{(j)}_{s,n} \triangleq \phi^{(j)}_{s,n}$.

§.§.§ Checking the Availability of Propagation Paths

The proposed SPA algorithm performs an availability check for each propagation path using rt <cit.> to determine whether a va can provide map information. First, the va positions $\V{p}^{(j)}_{ss',\text{va}}$ are determined by applying (<ref>) directly to pa $j$ at position $\V{p}^{(j)}_{\text{pa}}$ to get single-bounce-related va at positions $\V{p}^{(j)}_{ss,\text{va}}$. Next, (<ref>) is also applied to this single-bounce-related va to get double-bounce-related va at positions $\V{p}^{(j)}_{ss',\text{va}}$. rt is performed as described in Section <ref>.

In case the path between the agent position and a va position or between two va positions (double-bounce path) intersects with a reflective surface (e.g., it is blocked), the corresponding path is not available at  $\V{p}_n$. Hence, the corresponding va cannot be associated with measurements at time $n$.

§.§.§ Measurement Evaluation for the los Path
The messages $\beta_{00}\big(\rmv \underline{a}^{(j)}_{00,n} \big)$ passed from the factor node $\rmv\underline{q}_{\mathrm{P}}\rmv\big(\rmv \V{x}_{n}, \underline{a}^{(j)}_{00,n}; \rmv \V{z}^{(j)}_{n} \rmv\big)\rmv\rmv$ to the feature-oriented association variables $ \underline{a}^{(j)}_{00,n}\rmv\rmv$ are calculated as

\begin{align}
\beta_{00}\big( \underline{a}^{(j)}_{00,n} \big) &\rmv\rmv\rmv=\rmv\rmv\rmv \int \rmv\rmv\rmv \alpha(\V{x}_n) \ist \underline{q}_{\mathrm{P}}\big(  \V{x}_{n}, \underline{a}^{(j)}_{00,n} ; \V{z}^{(j)}_{n} \big) \mathrm{d}\V{x}_{n}\ist.
\label{eq:bp_measevalutionPA}
\\[-7mm]\nn
\end{align}

§.§.§ Measurement Evaluation for Legacy pmva
For $s=s'$, the messages $\beta_{ss}\big(\underline{a}_{ss,n}^{(j)} \big)$ passed from the factor node $\underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n}\rmv,  \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$ of single pmva to the feature-oriented association variables $a_{ss,n}^{(j)}$ given by

\begin{align}
&\beta_{ss}\big(\underline{a}_{ss,n}^{(j)} \big) \nn\\
&=\int\!\!\!\int \rmv\rmv\rmv \alpha_s\big( \underline{\V{p}}_{s,\text{mva}}^{(j)},1\big) \ist \alpha(\V{x}_n) \underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 	\underline{r}^{(j)}_{s,n}\rmv,  \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) \nn \\
&\rmv\rmv\rmv \hspace{5mm} \times \mathrm{d}\V{x}_{n} \ist \mathrm{d}\underline{\V{p}}_{s,\text{mva}}^{(j)}  \ist+\ist 1\big(\underline{a}_{ss,n}^{(j)}\big)\ist\alpha_{s,n}^{(j)} \ist.
\label{eq:bp_measevalutionLPMVA1}
\\[-7mm]\nn
\end{align}

For $s\neq s'$, the message $\beta_{ss'}\big(\underline{a}_{ss',n}^{(j)} \big)$ passed from the factor node $\underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n},\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s',n},  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$ of pairs of pmva to the feature-oriented association variables $a_{ss',n}^{(j)}$ are obtained as

\begin{align}
\hspace*{-2mm}\beta_{ss'}\big(\underline{a}_{ss',n}^{(j)} \big) &\rmv\rmv=\rmv\rmv\rmv \int\!\!\!\int\!\!\!\int \rmv\rmv\rmv \alpha_s\big( \underline{\V{p}}_{s,\text{mva}}^{(j)},1\big) \ist\alpha_s\big( \underline{\V{p}}_{s',\text{mva}}^{(j)},1\big)  \alpha(\V{x}_n) \nn \\
&\hspace{-1mm}\times \underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s,n},\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, \underline{r}^{(j)}_{s',n},  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) \nn \\
&\hspace{-1mm}\times \mathrm{d}\V{x}_{n} \ist \mathrm{d}\underline{\V{p}}_{s,\text{mva}}^{(j)} \mathrm{d}\underline{\V{p}}_{s',\text{mva}}^{(j)}\rmv\rmv+\rmv\rmv 1\big(\underline{a}_{ss',n}^{(j)}\big)\ist\alpha_{s,n}^{(j)}\alpha_{s',n}^{(j)} \ist.
\label{eq:bp_measevalutionLPMVA2}
\\[-7mm]\nn
\end{align}

§.§.§ Measurement Evaluation for New pmva
For pa $j$, the messages $\xi\big(\overline{a}^{(j)}_{m,n}\big)$ sent from the factor node $\overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}}, \overline{r}^{(j)}_{k,\mathrm{mva}}, \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; z^{(j)}_{m,n} \big)$ to the variable nodes corresponding to the measurement-oriented  association variables $\overline{a}^{(j)}_{m,n}$ are given by

\begin{align}
\xi\big(\overline{a}^{(j)}_{m,n}\big) &=\rmv\rmv \sum_{\overline{r}^{(j)}_{m,n} \in \{0,1\}} \int\!\!\!\int \rmv  \overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}}, \overline{r}^{(j)}_{k,\mathrm{mva}}, \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; z^{(j)}_{m,n} \big) \nn \\
&\hspace{5mm}\times  \alpha(\V{x}_n) \, \mathrm{d}\V{x}_{n} \ist \mathrm{d}\overline{\V{p}}^{(j)}_{m,\mathrm{mva}} \ist.
\label{eq:bp_measevalutionNF1}\\[-6.5mm]
\nn
\end{align}

Using the expression of $ \overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}}, \overline{r}^{(j)}_{k,\mathrm{mva}}, \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; z^{(j)}_{m,n} \big)$ in Section <ref> in (<ref>), Eq. (<ref>) simplifies to $\xi\big(\overline{a}^{(j)}_{m,n}\big) \!=\! 1$
for $\overline{a}^{(j)}_{m,n} \!\in\! \Set{D}^{(j)}_{n}$, and for $\overline{a}^{(j)}_{m,n} \!=\rmv 0$ it becomes

\begin{align}
\xi\big(\overline{a}^{(j)}_{m,n}\big) &= 1 + \frac{ \mu_{\text{n}} }{\mu_\text{fp} \ist f_{\text{fp}}\big( \V{z}^{(j)}_{m,n} \big)}
\int\!\!\!\int\rmv \alpha(\V{x}_n) \ist f_{\text{n}}\big(\overline{\V{p}}^{(j)}_{m,\mathrm{mva}}\big| \V{x}_n \big) \nn \\[1.5mm]
&\hspace{5mm}\times  f\big( \V{z}^{(j)}_{m,n} \big|\V{x}_{n}, \overline{\V{p}}^{(j)}_{m,\mathrm{mva}} \big) \, \mathrm{d}\V{x}_{n} \ist \mathrm{d}\overline{\V{p}}^{(j)}_{m,\mathrm{mva}}\ist.
\label{eq:bp_measevalutionNF}
\end{align}

§.§.§ Iterative Data Association
Next, from $\beta\big(\underline{a}_{ss',n}^{(j)} \big)$ and $\xi\big(\overline{a}^{(j)}_{m,n}\big)$, messages
$\eta\big(\underline{a}_{ss',n}^{(j)} \big)$ and $\varsigma\big( \overline{a}_{m,n}^{(j)} \big)$ are obtained using loopy (iterative) BP.
First, for each measurement, $m \!\in\! \Set{M}_n^{(j)}$, messages $\nu_{m\rightarrow s}^{(p)}\big(\underline{a}_{ss',n}^{(j)}\big)$
and, then, for $ss' \rmv\in\rmv \tilde{\mathcal{D}}_{n}^{(j)}$ messages $\zeta_{s \rightarrow m}^{(p)}\big(\overline{a}_{m,n}^{(j)}\big)$ are calculated iteratively according to <cit.>, for each iteration index $p \in \{ 1,\ldots, P\}$. After the last iteration $p \rmv=\rmv P\rmv$, the messages $\eta\big(\underline{a}_{ss',n}^{(j)} \big)$ and $\varsigma\big( \overline{a}_{m,n}^{(j)} \big)$ are calculated according to <cit.>. Details are provided in the supplementary materialSectionsec:DA.

§.§.§ Measurement Update for the Agent
The message $\gamma^{(j)}_{00}(\V{x}_n)$ sent from the factor node $\underline{q}_{\mathrm{P}}\big( \V{x}_{n},  \underline{a}^{(j)}_{00,n}  ; \V{z}^{(j)}_{n} \big)$ to the agent variable node is computed as

\begin{align}
\gamma^{(j)}_{00}(\V{x}_n) &=\! \sum_{\underline{a}^{(j)}_{00,n} \in \Set{M}_{0,n}^{(j)}} \!\! \eta\big( \underline{a}^{(j)}_{00,n} \big)
\underline{q}_{\mathrm{P}}\big( \V{x}_{n},  \underline{a}^{(j)}_{00,n}  ; \V{z}^{(j)}_{n} \big)\ist.
\label{eq:measurementUpdateAgentPA}\\[-6.5mm]
\nn
\end{align}

For $s = s'$, the messages $\gamma^{(j)}_{ss}(\V{x}_n)$ sent from the factor node $\underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 1,  \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$ to the agent variable node are given by

\begin{align}
\gamma^{(j)}_{ss}(\V{x}_n) &=\hspace{-4mm} \sum_{a_{ss,n}^{(j)} \in \Set{M}_{0,n}^{(j)}} \!\!  \eta\big( \underline{a}_{ss,n}^{(j)} \big)
\rmv\int \!\underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 1,  \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) \nn \\
&\hspace{2mm} \times \alpha_s\big(\underline{\V{p}}_{s,\text{mva}}^{(j)} , 1\big) \mathrm{d} \underline{\V{p}}_{s,\text{mva}}^{(j)} \ist+\ist \eta\big( \underline{a}_{ss,n}^{(j)} \!\rmv=\! 0\big) \ist\alpha_{s,n}^{(j)}.
\label{eq:measurementUpdateAgent1}
\end{align}

Furthermore, for $s \neq s'$, the messages passed from the factor node $\underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 1,\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, 1,$ $\underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)$ to the agent variable node are obtained as

\begin{align}
\hspace{-2mm}\gamma^{(j)}_{ss'}(\V{x}_n) &=\!\!\!\! \sum_{\underline{a}_{ss',n}^{(j)} \in \Set{M}_{0,n}^{(j)}} \!\!\!\!\! \eta\big( \underline{a}_{ss',n}^{(j)} \big) \int\!\!\!\!\int\!\! \alpha_s\big(\underline{\V{p}}_{s,\text{mva}}^{(j)} , 1\big) \alpha_{s'}\big(\underline{\V{p}}_{s',\text{mva}}^{(j)} , 1\big) \nn \\
&\hspace{2mm}\times \rmv \!\underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 1,\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, 1,  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) \nn \\
&\hspace{2mm}\times \mathrm{d} \underline{\V{p}}_{s,\text{mva}}^{(j)}\mathrm{d} \underline{\V{p}}_{s',\text{mva}}^{(j)} \ist+\ist \eta\big( \underline{a}_{ss',n}^{(j)} \!\rmv=\! 0\big) \ist\alpha_{s,n}^{(j)} \ist\alpha_{s',n}^{(j)}\ist.
\label{eq:measurementUpdateAgent1} \\[-5mm]
\nn
\end{align}

§.§.§ Measurement Update for Legacy pmva
Similarly, for $s = s'$, the messages  $\rho_{ss'}\big(\underline{\V{y}}^{(j)}_{s}\big)$ sent to the legacy pmva variable nodes are given by

\begin{align}
\rho_{ss}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},1\big)  &=\rmv\rmv\rmv \sum_{\underline{a}_{ss,n}^{(j)} \in \Set{M}_{0,n}^{(j)}} \rmv\rmv\int\!\! \eta\big( \underline{a}_{ss,n}^{(j)} \big) \alpha(\V{x}_n)\nn \\
&\hspace{2mm}\times \!\underline{q}_{\mathrm{S}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 1,  \underline{a}^{(j)}_{ss,n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big)  \mathrm{d} \V{x}_n\\[1.5mm]
\rho_{ss}^{(j)} &\triangleq \rho_{ss}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},0\big) = \eta\big( \underline{a}_{ss,n}^{(j)} \!\rmv=\! 0\big)  \\[-6mm]
\nn
\end{align}

and for $s\neq s'$ by

\begin{align}
\rho_{ss'}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},1\big)  &=\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv \sum_{\underline{a}_{ss',n}^{(j)} \in \Set{M}_{0,n}^{(j)}} \rmv\int\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\rmv\int\rmv\rmv \eta\big( \underline{a}_{ss',n}^{(j)} \big)  \alpha(\V{x}_n) \alpha_{s'}\big(\underline{\V{p}}_{s',\text{mva}}^{(j)} , 1\big) \nn \\
&\hspace{0mm}\times  \!\underline{q}_{\mathrm{D}}\big( \underline{\V{p}}^{(j)}_{s,\mathrm{mva}}\rmv, 1,\underline{\V{p}}^{(j)}_{s',\mathrm{mva}}\rmv, 1,  \underline{a}^{(j)}_{ss',n},  \V{x}_{n} ; \V{z}^{(j)}_{n} \big) \nn \\
&\hspace{0mm}\times \mathrm{d}\V{x}_n \mathrm{d} \underline{\V{p}}_{s',\text{mva}}^{(j)}\\[1.5mm]
\rho_{ss'}^{(j)} &\triangleq \rho_{ss'}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},0\big) = \eta\big( \underline{a}_{ss',n}^{(j)} \!\rmv=\! 0\big) \ist.\\[-7mm]\nn
\end{align}

Based on these messages, the message sent to the next pa $\gamma\big(\underline{\V{y}}_{s}^{(j)}\big) \triangleq \gamma\big(\underline{\V{p}}_{s,\text{mva}}^{(j)}, \underline{r}_{k,n}^{(j)}\big)$ is computed as

\begin{align}
\label{eq:measurementUpdateLegacy1}
\hspace{-2mm}\gamma\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},1\big)  &=\! \rho_{ss}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},1\big) \rmv\rmv+\rmv\rmv \prod^{S_n^{(j)}}_{s' = 1, s\neq s'} \rho_{ss'}\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},1\big)\\[-1mm]
\gamma_{s,n}^{(j)} &\triangleq\ist \gamma\big(\underline{\V{p}}^{(j)}_{s,\mathrm{mva}},0\big) = \rho_{ss}^{(j)} + \prod^{S_n^{(j)}}_{s' = 1, s\neq s'} \rho_{ss'}^{(j)} \ist.
\label{eq:measurementUpdateLegacy2}\\[-7mm]\nn
\end{align}

§.§.§ Measurement Update for New pmva
Finally, the messages $\phi\big(\overline{\V{y}}_{m}^{(j)}\big) \triangleq \phi\big(\overline{\V{p}}_{m,\text{mva}}^{(j)},\overline{r}_{m,n}^{(j)}\big)$ sent to the new pmva variable nodes are obtained as

\begin{align}
\label{eq:measurementUpdateNew1}
\phi\big(\overline{\V{p}}_{m,\text{mva}}^{(j)} , 1\big) &=\ist\int\! \overline{q}_{\mathrm{S}}\big(  \overline{\V{p}}^{(j)}_{m,\mathrm{mva}}, 1, \overline{a}^{(j)}_{m,n}, \V{x}_{n} ; z^{(j)}_{m,n} \big)  \, \nn \\
\ist \alpha(\V{x}_n) \mathrm{d}\V{x}_n \varsigma\big( \overline{a}_{m,n}^{(j)} \!\rmv=\! 0\big) \\[.5mm]
\phi_{m,n}^{(j)} &\triangleq\ist \phi\big(\overline{\V{p}}_{m,\text{mva}}^{(j)},0\big) =\rmv \sum_{\overline{a}_{m,n}^{(j)} \in  \tilde{\Set{D}}^{(j)}_{0,n}} \!\! \varsigma\big( \overline{a}_{m,n}^{(j)}\big) \ist.
\label{eq:measurementUpdateNew2}\\[-10mm]\nn
\end{align}

§.§ Belief Calculation
After the messages for all pa, $j \in \{1,\dots,J\}$ are computed, the belief $\tilde{f}(\V{x}_n)$ of the agent state can be calculated as normalized production of all incoming messages <cit.>, i.e.,

\begin{align}
\tilde{f}(\V{x}_n) \ist=\ist C_n\alpha( \V{x}_n)  \prod_{(s,s') \in \tilde{\Set{D}}_{n}^{(J)}}\!\!\rmv\gamma^{(J)}_{ss'}(\V{x}_n)
\label{eq:f_x}\\[-7mm]\nn
\end{align}

with normalization constant $C_n = \big(\int \alpha( \V{x}_n) \prod_{(s,s') \in \tilde{\Set{D}}_{n}^{(J)}}$ $\gamma^{(J)}_{ss'}( \V{x}_n) \mathrm{d}\V{x}_n\big)^{-1}$ that guarantees that (<ref>) is a valid probability distribution. Similarly, the beliefs $ \tilde{f}_s\big(\underline{\V{y}}^{(J)}_{s,n}\big) = \tilde{f}_s\big(\underline{\V{p}}^{(J)}_{s,\mathrm{mva}}, \underline{r}_{s,n}^{(J)}\big)\vspace*{0.5mm}$ of legacy pmva $s \in S_n^{J}$, is given by

\begin{align}
\tilde{f}_s(\underline{\V{y}}^{(J)}_{s,n}) \ist=\ist \underline{C}_{s,n}\gamma\big(\underline{\V{y}}^{(J)}_{s}\big)
\label{eq:fs_legacy} \\[-6mm]\nn
\end{align}

with constant $\underline{C}_{s,n} \rmv\rmv=\rmv\rmv \big(\int \gamma\big(\underline{\V{p}}^{(J)}_{s,\mathrm{mva}}, \underline{r}_{s,n}^{(J)} \rmv= \rmv1\big) \mathrm{d}\underline{\V{p}}^{(J)}_{s,\mathrm{mva}}\rmv\rmv+ \gamma_{s,n}^{(J)}\big)^{-1}$. Similarly, the $ \tilde{f}_m\big(\overline{\V{y}}^{(J)}_{m,n}\big) = \tilde{f}_m\big(\overline{\V{p}}^{(J)}_{m,\mathrm{mva}}, \overline{r}_{m,n}^{(J)}\big) $ of new pmva $m \in M_n^{J}$, is obtained as

\begin{align}
\tilde{f}_m\big(\overline{\V{y}}^{(J)}_{m,n}\big) \ist=\ist \overline{C}_{m,n} \phi\big(\overline{\V{y}}^{(J)}_{m}\big)
\label{eq:fs_new}\\[-5mm]\nn
\end{align}

where $\overline{C}_{m,n} \rmv\rmv\rmv\rmv = \rmv \rmv\rmv \big(\int \phi\big(\overline{\V{y}}^{(J)}_{m}\big) \mathrm{d}\overline{\V{y}}^{(J)}_{m} \rmv +\rmv  \phi^{(J)}_{m,n}\big)^{-1}$ is again a constant.

A computationally feasible sequential particle-based message passing implementation can be obtained following <cit.>. In particular, we adopted the approach in <cit.> using the “stacked state” comprising the agent state and the pmva states. To avoid that the number of pmva states grows indefinitely, pmva states with $p(r^{(j)}_{s,n} \!=\! 1 |\V{z}_{1:n})$ below a threshold $p_{\text{pr}}$ are removed from the state space (“pruned”) after processing the measurements of each pa $j$. To limit computation complexity, one might limit the maximum number of pmva states, i.e., in case $S_n > S_{\mathrm{max}}$, where $S_{\mathrm{max}}$ is another predefined threshold, only the $S_{\mathrm{max}}$ pmva states with the highest existence probability are considered when the measurement of the next pa is processed. Pseudocode for the particle-based implementation is provided in the supplementary materialSectionsec:pdeudocode.

§.§ Implementation Aspects and Computational Complexity

When beliefs of new PMVAs (cf. (<ref>), (<ref>), (<ref>), and (<ref>)) are introduced, contrary to <cit.>,  we do not use the conditional prior pdf of newly detected MVAs, $f_{\mathrm{n}}\big(\V{p}^{(j)}_{m,\mathrm{mva}}|\V{p}_n\big)$, as a proposal pdf. We develop an alternative proposal pdf where first samples of $\V{p}^{(j)}_{ss,\mathrm{va}}$ are obtained by using the inverse transformation of (<ref>) and (<ref>). Based on the resulting samples of $\V{p}^{(j)}_{ss,\mathrm{va}}$, samples of $\V{p}^{(j)}_{m,\mathrm{mva}}$ can then be computed by exploiting the inverse transformation in (<ref>).

New pmva states are introduced based on the assumption that the measurements come from a single-bounce path. However, if a measurement from a double-bounce path is used, the corresponding pmva will be pruned after a few time steps. This is because, due to the assumption that the measurement is from a single bounce path, the spatial distribution of the corresponding new pmva has high probability mass at incorrect locations. As a result, the probability of its existence will vanish. Dropping this assumption would require the introduction of new pmva-pairs for each measurement, significantly complicating the data association.

The computational complexity of the $j$th processing block that performs probabilistic data association can be analyzed as follows. As discussed in <cit.>, the computational complexity of such a processing block scales as $\mathcal{O}(L^{(j)}M)$, where $L^{(j)}$ is the number of PMVA-oriented association variables, and $M$ is the number of measurement-oriented association variables. It can easily be verified that in the proposed model, $L^{(j)}$ is upper bounded by $S^{(j)\ist2}$. The computational complexity of the $j$th processing block thus scales as $\mathcal{O}(S^{(j) 2} \hspace{.1mm} M)$. If conventional probabilistic data association <cit.> would be used, i.e., if the graph structure related to PMVA-oriented association variables and measurement-oriented association variables were not exploited, the computational complexity would scale exponentially in the number of PMVAs and the number of measurements. It is straightforward to see that after appropriate pruning, as discussed in the previous Section <ref>, the computation complexity is linear in the number of processing blocks and, thus, in the number of PAs. Note that even a moderate number of pmva leads to a large number of corresponding va. A key feature of the proposed method that makes this possible is that probabilistic data association can be solved in a scalable way.

Scenarios used for numerical evaluation: (a) shows the scenario for Experiments $1$ and $2$ in Sections <ref> and <ref>. This scenario consists of a rectangular room with two pa, i.e., there are four reflective surfaces. MVAs and VAs corresponding to single-bounce paths are shown. (b) shows the scenario for Experiment $3$ in Section <ref> which consists of two pa, two reflective surfaces and an obstructing wall segment. MVAs and VAs corresponding to single-bounce paths are shown. (c) shows the scenario for Experiment 4 in Section <ref>. This scenario consists of a classroom with two pa and four main reflective surfaces. Particle representations of the beliefs of MVA positions are shown as blue crosses. A line representing a reflecting surface is computed and shown as a dashed blue line for each particle. The geometric relations used to calculate a line from an MVA position have been presented in Section <ref>.

§ EVALUATION

The performance of the proposed mva-based slam algorithm is validated and compared with the multipath-based slam algorithm from <cit.> that has been extended to use aoa measurements described here and in <cit.>, the channel slam algorithm from <cit.>, and the multipath assisted positioning algorithm from <cit.> using synthetic measurements as well as real rf measurements. Additional simulation results can be found in the supplementary materialSectionsec:app_results.

§.§ Common Setup and Performance Metrics

The agent's state-transition pdf $f(\V{x}_n|\V{x}_{n-1})$, with $\V{x}_n \rmv=\rmv  [\V{p}_n^{\T} \; \V{v}_n^{\T} ]^{\T}\rmv$, is defined by a linear, near constant-velocity motion model
<cit.>, i.e., $\V{x}_n =  \V{A} \V{x}_{n-1} + \V{B} \V{w}_n$.
Here, $\V{A} \rmv\in\rmv \mathbb{R}^{4 \times 4}$ and $\V{B} \rmv\in\rmv \mathbb{R}^{4 \times 2}$ are as defined in <cit.>
(with sampling period $\vu{\Delta T \rmv=\rmv 1\rmv}{s}$), and the driving process $\V{w}_n$ is iid across $n$, zero-mean, and Gaussian with covariance matrix $\sigma_w^2\bold{I}_2$, where $\bold{I}_2$ denotes the $2 \rmv\times\rmv 2$ identity matrix and $\sigma_w$ denotes the acceleration noise standard deviation. For the sake of numerical stability, we introduced a small regularization noise to the PMVA state $\V{p}_{s,\mathrm{mva}}$ at each time $n$, i.e., $\underline{\V{p}}_{s,\mathrm{mva}} \rmv\rmv=\rmv\rmv \V{p}_{s,\mathrm{mva}} \rmv+\rmv \V{\omega}_{s}$, where $\V{\omega}_{s}$ is iid across $s$, zero-mean, and Gaussian with covariance matrix $\sigma_a^2\bold{I}_2$. The particles for the initial agent state are drawn from a 4-D uniform distribution with center $\V{x}_0 = [\V{p}_{0}^{\T}\;0\;\, 0]^{\T}\rmv$, where $\V{p}_{0}$ is the starting position of the actual agent trajectory, and the support of each position component about the respective center is given by $[-0.5\,\text{m}, 0.5\,\text{m}]$ and of each velocity component is given by $[-0.1\,\text{m/s}, 0.1\,\text{m/s}]$. At time $n \rmv=\rmv 0$, the number of pmva is $S_0 = 0$, i.e., no prior map information is available. The prior distribution for new pmva states $f_\mathrm{n}(\overline{\V{y}}_{m,n})$ is uniform on the square region given by $[-15\,\text{m},\ist 15\,\text{m}]\ist\times\ist[-15\,\text{m},\ist 15\,\text{m}]$ around the center of the floor plan shown in Fig. <ref> and the mean number of new pmva is $\mu_n = 0.05$. The probability of survival is $p_{\mathrm{s}} = 0.999$, the confirmation and pruning thresholds are respectively ${p_{\mathrm{cf}}} = 0.5$ and $p_{\mathrm{pr}} = 10^{-3}$. We performed $500$ simulation runs. The performance of the different methods discussed is measured in terms of the rmse of the agent position, as well as the ospa error <cit.> of va and mva. Since the proposed method estimates MVAs, we first map MVA estimates to VA estimates following equation (<ref>), before we compute ospa errors of va. ospa is a multi-object tracking metric that combines a localization error and a cardinality error into a single scalar score. As a result, it penalizes both state estimation inaccuracy and incorrect numbers of estimated features. We calculate the ospa errors based on the Euclidean metric with cutoff parameter $c = 5\,$m and order $p = 1$. The mean ospa (MOSPA) errors, rmse of each unknown variable are obtained by averaging over all converged simulation runs. We declare a simulation run to be converged if $\{\forall n: ||\hat{\V{x}}_n - \V{x}_n|| < 5\,\text{m} \}$.

For synthetic measurements (Experiment $1$–$3$), we use the following common parameters. The detection probability of all paths is $\vspace{0.2mm} p_{\mathrm{d},ss,n}^{(j)} = p_{\mathrm{d},ss',n}^{(j)} = p_\mathrm{d} = 0.95$ for $(s,s) \in \mathcal{D}_\text{S}$ and $(s,s') \in \tilde{\mathcal{D}}_\text{D}$, respectively. In addition, a mean number $\mu_{\mathrm{fp}} = 1$ of false positive measurements $\V{z}_{m,n}^{(j)}$ were generated according to the pdf $f_{\mathrm{fp}}(\V{z}_{m,n}^{(j)})$ that is uniformly distributed on $[0\,\text{m},30\,\text{m}]$ for distance measurements and uniformly distributed on $[-\pi, \pi]$ for aoa measurements.  In each simulation run, we generated noisy distance and aoa measurements according to (<ref>) and (<ref>) stacked into the vector $\V{z}_{m,n}^{(j)}$.

§.§ Reference Methods

In the following sections, we compare the proposed mva-based slam algorithm (PROP) to four different reference methods as described in the following:

* MP-SLAM: The multipath-based slam algorithm from <cit.>. The method considered here is a combination of <cit.> and <cit.> since in <cit.> statistical model of <cit.> is extended to aoa measurements of mpc. However, in contrast to <cit.>, we do not use the component snr estimates of mpc. Contrary to the method proposed in this paper, the reference method does rely on a much simpler statistical model without mva and ray-tracing. It thus cannot fuse data across propagation paths and VAs.

* CH-SLAM: The channel slam algorithm from <cit.>. We only implemented the channel slam algorithm proposed in <cit.>, not the full two-stage method that includes a channel estimator/tracker. Since the measurement-feature association is unknown, we perform Monte-Carlo data association for each particle separately, as it is commonly done in classical Rao-Blackwellized slam <cit.>, <cit.>.

* MINT: The multipath-based positioning algorithm from <cit.>, which assumes known map features (i.e., the va positions are known).

* LOS-MINT: A reduced version of MINT, where we only consider the pa (i.e., no va).

We used $50000$ particles for PROP, MP-SLAM, MINT, and LOS-MINT. For CH-SLAM, we used $2000$ particles for the agent and $1000$ for the va. To analyze the performance gain due to exploiting double-bounce reflections, we generate two different datasets: (Setup-I) the full setup considering all va corresponding to single-bounce and double-bounce paths and (Setup-II) a reduced setup considering only va corresponding to single-bounce paths. If not stated differently, measurements are generated according to Setup-I.

§.§ Experiment 1: Comparison with Reference Methods

In this experiment, we compare PROP to MP-SLAM and CH-SLAM. Furthermore, we compare PROP with measurements generated without false positive measurements and missed detections termed ground-truth (GT) for Setup-I and Setup-II. We consider the indoor scenario shown in Figure <ref>. We chose the scenario to be identical to <cit.> for easy comparison. The scenario consists of four reflective surfaces, i.e., $K=4$ mva and two pa. The noise standard deviations for the los path are ${\sigma_{\mathrm{d}}}^{(j)}_{m,n} = 0.05\,$m and ${\sigma_{\mathrm{\varphi}}}^{(j)}_{m,n} = 10^{\circ}$, for single-bounce path are ${\sigma_{\mathrm{d}}}^{(j)}_{m,n} = 0.10\,$m and ${\sigma_{\mathrm{\varphi}}}^{(j)}_{m,n} = 15^{\circ}$, and for double-bounce path are ${\sigma_{\mathrm{d}}}^{(j)}_{m,n} = 0.15\,$m and ${\sigma_{\mathrm{\varphi}}}^{(j)}_{m,n} = 25^{\circ}$. The acceleration noise standard deviation is $\sigma_w = 9\cdot10^{-3}\,\text{m}/\text{s}^2$. As an example, Fig. <ref> depicts for one simulation run the posterior pdf represented by particles of the mva positions and corresponding reflective surfaces as well as estimated agent tracks. Fig. <ref> shows the mospa error for the two pa and all associated va, Fig. <ref> shows the mospa error for all mva, and Fig. <ref> shows the rmse of the agent's position all versus time $n$. Fig. <ref> shows the cumulative frequency of the agent errors (not excluding the diverged runs). Note that for all algorithms, none of the $500$ simulation runs diverged.

Performance results for Experiment 1 in Section <ref>: (a) MOSPA errors of the va of each PA, (b) MOSPA errors versus time of the mva, (c) RMSEs of the agent position versus time, (d) RMSEs versus time of the agent orientation, and (e) cumulative frequency of the RMSEs of the agent position.

The MOSPA error of PROP related to VAs and PAs is shown in Fig. <ref>. It can be seen that the MOSPA drops significantly after only a few time steps. In contrast, MP-SLAM only converges rather slowly to a small mapping error. Furthermore, the MOSPA error of PROP converges along the agent track to a smaller value, i.e., to a smaller mapping error. This shows that PROP efficiently exploits all measurements for the map features provided by the PAs. Fig. <ref> shows the MOSPA error of the mva positions, which confirms the results seen in Fig. <ref>. The RMSE of the agent position in <ref> of PROP is considerably smaller than that of MP-SLAM along the whole agent track. Moreover, the RMSE of the agent position of PROP consistently decreases over time $n$, while that of MP-SLAM increases slightly during changes in the agent's direction.

PROP consistently demonstrates a statistically significant improvement in accuracy across all metrics, as illustrated in Figure <ref>, facilitating the statistical dependencies across multiple paths and pa. Furthermore, the results comparing Setup I and II using GT measurements illustrate that leveraging double-bounce paths systematically improves the performance of PROP. The average runtimes per time step for MATLAB implementations on a single core of Intel i7 CPUs (computer cluster with different versions of CPUs) were measured $4\,$s for PROP, $1.2\,$s for MP-SLAM, and $11\,$s for CH-SLAM.[Rao-Blackwellized slam explicitly considers the dependency of map features on the agent state in its posterior representation. Thus, the particle-based representation in CH-SLAM <cit.> is very computationally demanding.]

§.§ Experiment 2: Varying Measurement Uncertainties

In this experiment, we analyze the performance of PROP with varying measurement noise standard deviations and compare it to MP-SLAM.
In particular, we introduce factors $f_\text{std, all}$ and $f_\text{std,los}$. Both factors equally increase the base values of all measurement standard deviations in distance ${\sigma_{\mathrm{d}}}^{(j)}_{m,n}$ and aoa ${\sigma_{\mathrm{\varphi}}}^{(j)}_{m,n}$ for all $m \in M_n^{(j)} \, | \, j\in J,\, n \in \{1,\,...\,, N\}$ in a multiplicative way. Base values are set as the measurement standard deviations of Experiment 1. While $f_\text{std,all}$ affects measurements corresponding to pa and MVAs, $f_\text{std,los}$ affects only measurement standard deviations corresponding to PAs (i.e., LOS measurements). The scenario is identical to experiment 1 of Section <ref>. Note that for all algorithms none of the $500$ simulation runs diverged.

Performance results for Experiment 2 in Section <ref> for different noise standard deviations. Errors are averaged across time steps. Different noise standard deviations are used for all VAs and PAs:  (a) MOSPA errors of VAs of each PA, (c) MOSPA errors of MVAs, and (e) RMSEs of the agent positions. Different noise standard deviations are used for all PAs: (b) MOSPA errors of VAs of each PA, (d) MOSPA errors of MVAs, and (f) RMSEs of the agent position.

Fig. <ref> to <ref> show the mean MOSPA of VAs and PAs, the mean MOSPA of MVAs or the mean agent RMSE, respectively, over all time steps $n$ as a function of $f_\text{std,all}$ or $f_\text{std,los}$. In particular, in Fig. <ref>, <ref> and <ref> we varied $f_\text{std,all}$ and kept $f_\text{std,los} \triangleq 1$ fixed, while in Fig. <ref>, <ref> and <ref> we varied $f_\text{std,los}$ and kept $f_\text{std,all} \triangleq 4$ fixed. The va MOSPA errors in Figs. <ref> to <ref> emphasize the observations of Experiment 1 (Section <ref>). All error values increase with increasing $f_\text{std,all}$. However, the MOSPA errors of the proposed method increases much slower. This is because the proposed method can fuse information provided by different pa and different propagation paths. In contrast, the agent RMSE in Fig. <ref> remains constant for both methods as there is still enough information available for proper localization, mainly provided by the PAs. Thus, when additionally increasing $f_\text{std,los}$ in Fig. <ref>, the RMSE of MP-SLAM significantly increases. In contrast, the agent RMSE of PROP remains approximately constant due to the increased map stability.

§.§ Experiment 3: Low Information and Obstructed LOS

Performance results for Experiment 3 in Section <ref>: (a) MOSPA errors versus time of mva, (b) RMSEs versus time of the agent position, (c) RMSEs versus time of the agent orientation, (d) cumulative frequency of the RMSEs of the agent position (with outliers).

In this experiment, we analyze the performance of PROP in the scenario shown in Figure <ref>. It contains only two reflective walls ($K=2$) while one short wall obstructs the los path, i.e., the path between pa and the agent as well as the paths between va and agent<cit.>. The obstructing wall does not cause any va due to the geometric constellation. We compare MP-SLAM, MINT, and LOS-MINT. The scenario contains two pa. We generate the distance and the aoa of the individual mpc parameters according to a rf signal model <cit.>. We assume the agent has a $ 3 \times 3 $ uniform rectangular array ($H=9$) with an inter-element spacing of $ 2 $ cm. The transmit signal spectrum has a root-raised-cosine shape, with a roll-off factor of $0.6$ and a $3$-dB bandwidth of $B=500\,\mathrm{MHz}$ centered at $6\,\mathrm{GHz}$ resulting in a sampling time of $T_\text{s} = 1/(1.6 \, B)$. The amplitude of each mpc is assumed to follow free-space path loss and is attenuated by $ 3 $ dB after each reflection. The snr output at $1$ m distance to the agent is assumed to be $38$ dB. The measurement noise standard deviations are calculated based on the Fisher information <cit.>. The acceleration noise standard deviation is $\sigma_w = 0.02\,\text{m}/\text{s}^2$.

Fig. <ref> shows the MOSPA error for all mva, Fig. <ref> shows the rmse of the agent's position for converged simulation runs, all versus time $n$. Finally, Fig. <ref> shows the cumulative frequency of all agent's position errors (not excluding the diverged runs). We show results for all investigated algorithms, where solid lines correspond to Setup-I and dashed lines correspond to Setup-II as described above. For PROP and MP-SLAM, none of the $500$ simulation runs diverged, but $30\,$% of the simulation runs diverged for LOS-MINT. LOS-MINT performs poorly, as in the central part of the track ($n=93$ to $n=107$), the los to all anchors is obstructed. This leads to LOS-MINT tending to choose an mpc as the los hypothesis as the agent state gradually becomes more uncertain. Figure <ref> illustrates the benefits of PROP with respect to MP-SLAM, as it systematically leverages both pa as well as both single-bounce and double-bounce propagation paths to infer the map features leading to a reduction in the rmse of the agent's position. Furthermore, Figure <ref> statistically shows that this fusion results in fewer instances of large agent errors and that PROP consistent outperforms MP-SLAM. A possible explanation is the increased presence of mva and their corresponding reflective surfaces, which are more likely to exist due to the additional double-bounce measurement update.Although the single-bounce paths may not be visible, their absence is compensated by the fact that the proposed method performs data fusion across multiple propagation paths, as observed in Figure <ref>. In contrast, MP-SLAM independently estimates each va, thus lacking this advantageous feature. MINT has perfect (prior) knowledge of the va positions and, thus, provides a lower bound for va-based slam algorithms. PROP, which exploits double-bounce propagation paths, comes close to approaching this lower bound. Note that, in Figure <ref>, between $n=0$ and $n=80$, LOS-MINT shows a slightly higher positioning accuracy compared to the proposed method. This difference is due to the uncertainty in mva positions. A theoretical analysis on how uncertainty of map information affects the positioning accuracy of the agent is provided in <cit.>.

§.§ Experiment 4: Validation Using Measured Radio Signals

Picture of the classroom used for data collection.

To validate the applicability of the proposed mva-based slam algorithm to real rf measurements, we use data collected in a classroom shown in Fig. <ref> at TU Graz, Austria. More details about the measurement environment and va calculations can be found in <cit.>. On the pa side, a dipole-like antenna with an approximately uniform radiation pattern in the azimuth plane and zeros in the floor and ceiling directions was used. At each agent position, the same antenna was deployed multiple times on a $3\times3$, 2-D grid to yield a virtual uniform rectangular array with an inter-element spacing of $2$ cm. The UWB signals are measured at $ 180 $ agent positions along a trajectory with position spacing of approx. $5$ cm as shown in Fig. <ref> using an M-sequence correlative channel sounder with frequency range $3.1–10.6\,$GHz. Within the measured frequency-band, the actual signal spectrum was selected by a filter with root-raised-cosine shape, with a roll-off factor of $0.6$ and a $3$-dB bandwidth of $B=1\,\mathrm{GHz}$ centered at $6\,\mathrm{GHz}$. The received signal is critically sampled with $T_\text{s} = 1/(1.6 \, B)$ and artificial AWGN is added such that the output signal-to-noise-ratio is $\ac{snr}=30$ dB. We apply a variational sparse Bayesian parametric channel estimation algorithm <cit.> to acquire the $M_n^{(j)}$ distance estimates ${z_\text{d}}_{m,n}^{(j)}$ and aoa estimates ${z_\varphi}_{m,n}^{(j)}$ of mpc. The corresponding noise standard deviations are calculated based on the Fisher information <cit.>. Compared to the synthetic setup, we changed the mean number of false alarm measurements to $\mu_\mathrm{fp}=3$, the detection probability to $p_\text{d} = 0.7$, regularization noise standard deviation to $\sigma_a = 2 \cdot 10^{-3}\,\text{m}$, and the acceleration noise standard deviation to $\sigma_w = 0.0114\,\text{m}/\text{s}^2$. Note that for all algorithms, none of the $500$ simulation runs diverged.

Fig. <ref> depicts for one simulation run the posterior pdf represented by particles of the mva positions and corresponding reflective surfaces as well as estimated agent tracks. PROP can identify the main reflective surfaces of the room (The lower wall is only visible at the beginning of the agent track since the reflection coefficient is very low). Although the walls have a rich geometric structure (windows, doors, etc.) and generate many mpc estimates, i.e., measurements, PROP robustly estimates the main walls.[Note that <cit.> shows results using real measured radio signals in the same environment. This figure shows the presence of single-bounce and multiple-bounce propagation paths. For instance, in the case of the pa indicated in blue, the double-bounce path related to left and top VA is clearly visible. It is particularly noteworthy that single-bounce and double-bounce paths are consistently observable along a significant portion of the agent track.] Fig. <ref> compares PROP and MP-SLAM in terms of the agent rmse. Fig. <ref> shows the rmse of the agent's position, and Fig. <ref> shows the rmse of the agent's orientation for simulation runs, all versus time $n$. The comparison of the position rmse shows a similar behavior as for synthetic measurements (see Fig. <ref>), i.e., PROP outperforms MP-SLAM. The mapping capability and low agent rmse of PROP, when applied to real rf signals, demonstrate the high potential of PROP for accurate and robust rf-based localization.

Performance results for Experiment 4 in Sec <ref>: (a) RMSEs versus time of the agent position and (b) RMSEs versus time of the agent orientation.

§ CONCLUSIONS

In this paper, we introduced data fusion for multipath-based slam. A key novelty of our approach is to represent each reflective surface in the propagation environment by a single mva. In this way, we address a key limitation of existing multipath-based slam methods, which represent every propagation path by a va and thus neglect inherent geometrical constraints across different paths that interact with the same reflective surface. As a result, the accuracy and speed of existing multipath-based slam methods are limited. A key aspect in leveraging the advantages of the introduced mva-based model was to check the availability of single-bounce and double-bounce propagation paths at potential agent positions by means of rt. Availability checks were directly integrated into the statistical model as detection probabilities of paths. Our numerical simulation results demonstrated significant improvements in estimation accuracy and mapping speed compared to state-of-the-art multipath-based slam methods. Looking forward, we expect to extend our approach to large-scale scenarios and more realistic 3-D environments. We expect such an extension to yield significantly increased computational complexity due to an increased dimensionality of the states to be estimated and an increased number of mva due to floor and ceiling surfaces. Promising directions for future research also include an extension to multiple-measurement-to-feature data association <cit.> and an advanced mva model, where the length and shape of reflective surfaces <cit.> are also taken into account. Another future research venue aims at incorporating amplitude information to make detection probabilities and measurement variances adaptive <cit.>.

§ ACKNOWLEDGEMENT

DISTRIBUTION STATEMENT A: Approved for public release. This material is based upon work supported by the Under Secretary of Defense for Research and Engineering under Air Force Contract No. FA8702-15-D-0001. Any opinions, findings, conclusions, or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the Under Secretary of Defense for Research and Engineering.

[]Erik Leitinger (Member, IEEE) received his MSc and PhD 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 PhD Thesis. He is an Erwin Schrödinger Fellow. His research interests include inference on graphs, localization and navigation, machine learning, multiagent systems, stochastic modeling and estimation of radio channels, and estimation/detection theory.

[]Alexander Venus (Student Member, IEEE) received his BSc and  MSc 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.

[]Bryan Teague (Member, IEEE) is a technical staff member at MIT Lincoln Laboratory. He received an MS degree in aerospace engineering from the Massachusetts Institute of Technology (MIT), Cambridge, Massachusetts, in 2017, and a BS (with highest honors) degree in engineering from Harvey Mudd College, Claremont, California, in 2010. He was a member of Wireless Information and Network Sciences Laboratory, Massachusetts Institute of Technology (MIT) from 2015 to 2017. He is a winner of a 2018 R&D100 innovation award. His research interests include probabilistic inference, optimal control, radio frequency technologies, and decentralized intelligence.

[]Florian Meyer (Member, IEEE) received the MSc and PhD degrees (with highest honors) in electrical engineering from TU Wien, Vienna, Austria in 2011 and 2015, respectively. He is an Assistant Professor with the University of California San Diego, La Jolla, CA, jointly between the Scripps Institution of Oceanography and the Electrical and Computer Engineering Department. From 2017 to 2019 he was a Postdoctoral Fellow and Associate with the Laboratory for Information & Decision Systems at the Massachusetts Institute of Technology, Cambridge, MA, and from 2016 to 2017 he was a Research Scientist with the NATO Centre for Maritime Research and Experimentation, La Spezia, Italy. Prof. Meyer is the recipient of the 2021 ISIF Young Investigator Award, a 2022 NSF CAREER Award, a 2022 DARPA Young Faculty Award, and a 2023 ONR Young Investigator Award. He is an Associate Editor with the IEEE Transactions on Aerospace and Electronic Systems and the ISIF Journal of Advances in Information Fusion and was a keynote speaker at the IEEE Aerospace Conference in 2020.  His research interests include statistical signal processing, high-dimensional and nonlinear estimation, inference on graphs, machine perception, and graph neural networks.