Data Fusion for Multipath-Based SLAM: Combining Information From Multiple Propagation Paths

Multipath-based simultaneous localization and mapping (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 physical anchor (PA) across that surface – known as the corresponding “virtual anchor” (VA). Due to this VAs representation, each propagation path is mapped individually. Existing methods thus ignore 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 MVAs is to check the availability of single-bounce and double-bounce propagation paths at potential agent positions by means of ray-tracing (RT). This availability check is directly integrated into the statistical model as detection probabilities for propagation paths. Estimation is performed by a sum-product algorithm (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.


I. INTRODUCTION
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 radio frequency (RF) signals at flat surfaces are modeled by "virtual anchor"s (VAs) that are mirror images of base stations, also referred to as physical anchors (PAs) [1].The number of VAs and their positions are typically unknown.Multipath-based SLAM methods can detect and localize VAs and jointly estimate the time-varying agent position [1]- [5].The availability of VA location information makes it possible to leverage multiple propagation paths of RF signals for agent 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.
pa , and a single mobile agent with position and orientation, pn and ψn, respectively.The mobile agent is depicted with five individual antenna elements at positions p (h) n , defined by offset d (h) and angle ψ (h) .The reflecting surfaces can be represented by "virtual anchor"s (VAs) or master virtual anchors (MVAs), e.g., p (1) 11,va or p 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 p (1) 11,va while the unique MVA position p 1,mva remains the same.Note that u 1 and u 2 are the unit vectors perpendicular to surfaces 1 and 2, while 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 localization.It can thus significantly improve localization accuracy and robustness [6]- [9].
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 [3]- [5], [24], [25].In particular, (i) it is not known which VA was generated by which measurement, (ii) there are missed detections due to low signal-to-noise-ratio (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 VAs.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 sum-product algorithm (SPA) on the factor graph representing the underlying statistical model [3]- [5].In addition, since the models for the aforementioned measurements are nonlinear, most methods typically rely on sampling techniques [1]- [6].Recently, multipath-based SLAM was applied to data collected in indoor scenarios by radios with ultra-wide bandwidth [26] or multiple antennas [2], [4], [27].
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 [1]- [6], [28].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 [16], [17], [29] perform fusion of information provided by multiple cooperating agents.However, these methods are limited to single-bounce paths and do not perform fusion across PAs.In [30]- [33], it has been demonstrated that doublebounce 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.

B. 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 line-of-sight (LOS) and MPCs in RF signals.
To leverage the position information of MPCs, 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 VAs 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 SPAs for multipath-based SLAM in [3]- [5] 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, ray-tracing (RT) [34]- [36] 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 angle-of-arrival (AOA) measurements, which have been extracted from the RF signal in a preprocessing stage [18]- [21].The key contributions of this paper are as follows.tion 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 [38], [39] by (i) extending the MVAs 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 [3], [4], channel-SLAM [2], and multipath-based positioning assuming known VAs positions [40] 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 x and x, respectively, and a random vector and its realization by x and x, respectively.Furthermore, ∥x∥ and x T denote the Euclidean norm and the transpose of vector x, respectively, and ⟨x, y⟩ denotes the inner-product between the vectors x and y; ∝ indicates equality up to a normalization factor; f (x) denotes the probability density function (PDF) of random vector x (this is a short notation for f x (x)); f (x|y) denotes the conditional PDF of random vector x conditioned on random vector y (this is a short notation for f x|z (x|z)).The four-quadrant inverse tangent of position p = [p 1 p 2 ] T is denoted as atan2(p 2 , p 1 ).The cardinality of a set X is denoted as |X |. δ(•) denotes the Dirac delta function.Finally, δ e denotes the indicator function of the event e = 0 (i.e., δ e = 1 if e = 0 and 0 otherwise).

II. GEOMETRICAL RELATIONS
We consider a mobile agent equipped with an H-element antenna array and J PAs equipped with a single antenna at known positions p where J is assumed to be known.The agent is in an environment with S reflective surfaces indexed by s ∈ S ≜ {1, . . ., S}.At every discrete time step n, the array element locations are denoted by p (h) n , h ∈ {1, . . ., H}.The agent position p n refers to the center of gravity of the array.We also define   pa and the agent at position pn.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.
the array orientation ψ n of the agent are unknown.Each PA transmits a RF signal, and the agent acts as a receiver. 2The RF signal arrives at the receiver via the LOS path as well as via MPCs originating from the reflection of surrounding objects.We assume time synchronization between all PAs and the agent.However, our algorithm can be extended to an unsynchronized system according to [2], [6], [41].
We restrict the representation to MPCs related to singlebounce and double-bounce paths.In particular, associated with PA j there are |D S | = S single-bounce VAs [3], [34] at unknown positions p Therefore, the maximum number of VAs for PA j is given by |D| = S + S(S − 1) with D ≜ {(s, s ′ ) ∈ S × S} = D D ∪ D S .Note that subscript indices denoted as (•) ss ′ indicate variables related to a pairs (s, s ′ ) of reflecting objects.By applying the image-source model [34], [42], a VA associated with a single-bounce path is the mirror image of p (j) pa at reflective surface s ∈ S given by p (j)  ss,va = p where (s, s) ∈ D S , u s is the normal vector of reflective surface s, and e s is an arbitrary point on the considered surface.
The second term in (1) represents the normal vector w.r.t. to reflective surface s in direction u s with the length of two times the distance between PA j at position p (j) pa and the normalpoint at the reflective surface s, i.e., 2 u T s e s − u T s p (j) pa .By again applying the image-source model [34], [42] for VA at position p (j) s ′ s ′ ,va another VA that is obtained as the mirror image of p (j) 2 We assume the PAs to use orthogonal codes, i.e., there is no mutual interference between individual PAs.Note that the proposed algorithm can be easily reformulated for the case where the agent transmits a RF signal and the PAs act as receivers.
where (s, s ′ ) ∈ D D .This VA represents a double-bounce propagation path.For conveniently addressing PA-related variables and factors, we also define p  pa .An example is shown in Fig. 1.The availability of VAs at certain agent position p n is limited due to blockage of associated propagation paths or geometric constraints between reflective surfaces [34], [35].Especially, double-bounce paths and their corresponding VA positions have limited availability depending on the agent position p n (see Fig. 2b and Fig. 2c).Hence, the number of available VAs for PA j is smaller than |D| = S + S(S − 1).As discussed in Section III-D, the proposed statistical model and method performs an availability check for each VA using RT [34]- [36].In this way, for each agent position p n , it can be determine which of the potential paths in D is available.

A. MVA-Based Model of the Environment
A reflective surface is involved in multiple propagation paths and thus defines multiple VAs.To enable the consistent combination, i.e., "fusion" of map information provided by measurements of different PAs, we represent reflective surfaces by S unique MVAs at positions p s,mva ∈ R 2 , s ∈ S. The unique MVA position p s,mva ∈ R 2 is defined as the mirror image of [0 0] T on the reflective surface S. 3 By using some algebra, the transformation from MVA at p s,mva to a VA at p ss,va ) Note that the inverse transformation in (4) will be used to determine a proposal distribution for MVA states as discussed in Section V. Details of the derivation of ( 3) and ( 4) are provided in the supplementary material [37, Section I-A].For example, Fig. 2 shows three scenarios with two reflecting surfaces described by two MVAs at positions p 1,mva and p 2,mva .Fig. 2a shows a scenario with two PAs j ∈ {1, 2}, the corresponding VAs, and an agent at position p n .Each PA generates one VA associated with a single-bounce propagation path.Fig. 2b shows a scenario with one PA, the corresponding VAs associated with single-bounce and double-bounce propagation paths, and two agents positions at differnet time steps, i.e., p n1 and p n2 .Note in case surfaces are perpendicular, a different order of bounces from surfaces, i.e., surface "ssurface s ′ " or "surface s ′ -surface s", does not lead to a different VA position.Depending on the agent position p n , only one double-bounce propagation path (related to one of the two orders) is available (see also Section III-D).Fig. 2b 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 p n for which two double-bounce propagation paths are available at the same time (cf.Section III-D).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 p n (an example is given in [37, Section IV]).

III. SYSTEM MODEL
At each time n, the state of the agent is given by , where 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 [3], [43], [44], we account for the unknown number of MVAs by introducing potential MVA (PMVA) s ∈ S n ≜ {1, . . ., S n }.The number S n of PMVAs is the maximum possible number of actual MVAs, i.e., all MVAs that produced a measurement so far [3], [44] (where S n increases with time).PMVA states are denoted as y s,n = p T s,mva r s,n T .The existence/nonexistence of PMVA s is modeled by the existence variable r s,n ∈ {0, 1} in the sense that PMVA s exists if r s,n = 1.Formally, its states is considered even if PMVA s is nonexistent, i.e., if r s,n = 0.The states p T s,mva of nonexistent PMVAs are obviously irrelevant.Therefore, all PDFs defined for PMVA states, f (y s,n ) = f (p s,mva , r s,n ), are of the form f (p s,mva , 0) = f s,n f d (p s,mva ), where f d (p s,mva ) is an arbitrary "dummy PDF" and f s,n ∈ [0, 1] is a constant and can be interpreted as the probability of non-existence [3], [44].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 I.

A. Measurements and New PMVAs
The distance and AOA measurements related to the path "agent at position p n -VA at position p (j) ss ′ ,va " with (s, s ′ ) ∈ Dn are given by z d n } for PA j. (These measurements represent the MPC parameters.For details see [2], [4], [24].)Note that before measurements are acquired, the number of measurements M (j) n is random.Likelihood function for LOS paths: Using ( 5), (6), and p  Likelihood function for single-bounce paths: Using ( 5), (6), and the transformation in (3), i.e., p (j) ss,va = h va p s,mva , p (j) pa , the likelihood function related to the single-bounce propagation path "agent -surface s" with s ∈ S -PA j with MVA index-pair (s, s) ∈ D S,n reads f (z Likelihood function for double-bounce paths: Using (5), and ( 6), and the transformation in (3) twice, i.e., p , the likelihood function related to the double-bounce path "agent -surface s -surface s ′ -PA j" with MVA index-pair (s, s ′ ) ∈ D D,n can be expressed by f (z With the MVA-based measurement model, at time n, the measurements collected by all PAs j ∈ {1, . . ., J} can provide information on the same MVA and agent positions p s,mva , s ∈ S and 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 p p n , p s,mva , p s ′ ,mva .Note that the detection probability is determined by the SNR of the measurement [4] 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 z (j) m,n 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 µ fp and PDF f fp (z Newly detected MVAs, i.e., MVAs that generated a measurement for the first time, are modeled by a Poisson point process with mean µ n and PDF f n (p m,mva |p n ).Newly detected MVAs are represented by new PMVA states y (j)  n,m , m ∈ {1, . . ., M n } in our statistical model [3], [44].Each new PMVA state corresponds to a measurement z (j) m,n ; r m,n = 1 implies that measurement z (j) m,n 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 V-D.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 y (j) n ≜ y n ,n T , 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 PMVAs with a low probability of existence (see Section IV-B).

B. Legacy PMVAs and State Transition
At time n, measurements are incorporated sequentially across PAs j ∈ {1, . . ., J}.Previously detected MVAs, i.e., MVAs that have been detected either at a previous time n ′ < n or at the current time n but at a previous PA j ′ < j, are represented by legacy PMVA states y (j)  s,n .New PMVAs become legacy PMVAs 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 y The number of legacy PMVA at time n, when the measurements of the next PA j are incorporated, is updated according to S n is equal to the number of all measurements collected up to time n and PA j−1.The vector of all legacy PMVA states at time n and up to PA j can now be written as y Let us denote by y (1) , the vector of all legacy PMVA states before any measurements at time n have been incorporated.After the measurements of all PAs j ∈ {1, . . ., J} have been incorporated at time n, the total number of PMVA states is and the vector of all PMVA states at time n is given by . 4 We also define the number of VAs for each PA given as n .Legacy PMVAs states y s,n and the agent state x n are assumed to evolve independently across time according to state-transition PDFs f y s,n y s,n−1 and f (x n |x n−1 ), respectively.If PMVA k exists at time n −1, i.e., r s,n−1 = 1, it either disappears, i.e., r s,n = 0, or survives, i.e., r s,n = 1; in the latter case, it becomes a legacy PMVA at time n.The probability of survival is denoted by p s .Suppose the PMVA survives.In that case, its position remains unchanged, i.e., the state-transition pdf of the MVA positions p s,mva is given by f p s,mva p s,mva = δ p s,mva − p s,mva .Therefore, f p s,mva , r s,n p s,mva , r s,n−1 for r s,n−1 = 1 is obtained as If MVA s does not exist at time n−1, i.e., r s,n−1 = 0, it cannot exist as a legacy PMVA at time n either, thus we get For j ⩾ 2, we also define f (j) y (j) s,n y (j−1) s,n as f (j) p (j) s,mva , r (j) s,n p (j−1) s,mva , r (j−1) s,n = 0 δ p (j)  s,mva − p (j−1) s,mva , r and f (j) p (j) s,mva , r (j) s,n p (j−1) s,mva , r (j−1) where we introduced y (j−1) s,mva ≜ p s,mva , and r (j−1) s,n ≜ r s,n .It it assumed that at time n = 0 the initial prior PDF f y s,0 , s = 1, . . ., S 0 and f (x 0 ) are known.All (legacy and new) PMVA states and all agent states up to time n are denoted as

C. 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 z (j) m,n extracted at PA j originated from PA j itself (0, 0), from which MVA (s, s) ∈ D (j) S,n , or from which MVA-MVA pair (s, s ′ ) ∈ D (j) D,n associated with single-bounce and double-bounce path.Any PMVAto-measurement association (which considers associations to single PMVAs and PMVA-PMVA pairs as well as to PA j itself) is described by PMVA-oriented association variables with (s, s ′ ) ∈ D(j) n and stacked into the PMVA-oriented association vector as a n ,n T .To reduce computation complexity, following [3], [43]- [47], we use a redundant description of PMVA-measurement associations, i.e., we introduce measurement-oriented association variables if measurement m is not generated by any legacy PMVA ss ′ and stacked into the measurement-oriented association vector as a (j) n = a T .Note that any data association event that can be expressed by both a joint PMVA-oriented association vector a (j) n and measurement-oriented association vector 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 SPAs for simultaneous agent localization MVA mapping [3], [43], [44], [47].Finally, we also introduce the joint association vectors a n = a

D. Detection Probabilites and Availability of Paths
RT relies on visibility-tree techniques [34]- [36] to determine the visibility of a path in a backward manner as discussed in what follows for the double-bounce case (s, First, starting from the agent's position, p n , a straight line is drawn towards the VA position, p (j) ss ′ ,va , until it intersects with the reflective surface with index s ′ .From the resulting intersection point, another line is drawn towards the VA position, p (j) ss,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 p (j) 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 ′′ ∈ S\{j, j ′ }, the path is considered not available or "blocked".Intersections are calculated efficiently based on the fast line intersection algorithm [48].
For single-bounce and LOS paths, the procedure is simpler.In particular, for the single-bounce case (s, s) ∈ D (j) D,n , starting from the agent's position, the first of two straight lines is already drawn from the agent's position p n , towards the VA position, p (j) ss ′ ,va .For the LOS case, the first and only straight line is directly drawn from the agent's position p n to p (j) pa .For each PA j, this availability check is directly integrated into detection probabilities.In particular, the LOS path detection probability, p pa is available 0, path is not available (14) where p (j) d,00,n is the detection probability [4], [24], [44] of the available LOS path.Similarly, the single-bounce detection probability, p ss,va is available 0, path is not available (15) where p (j) d,ss,n is the detection probability of available singlebounce path.Finally, the double-bounce detection probability p ss ′ ,va is available 0, path is not available (16) where p (j) d,ss ′ ,n is again the detection probability of the available double-bounce path.

IV. 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.

A. Pre-Estimation Stage
By applying at each time n a super-resolution channel estimation algorithm [18]- [21], [24] to the observed RF signal vector one obtains, for each anchor j, a number of  (19).Short notations are used.In particular, the time index n and the functional dependencies of the factors are neglected: , q j s ≜ q S y (j) s,n , a , q j ss ′ ≜ q D y (j) s,n , y ss ′ ,n , xn; z s ′ s,n , xn; z ss ′ →m a n is usually much smaller than the number of signal samples).It thus compresses the information contained in the RF signal vector into z

B. Confirmation of MVAs and State Estimation
We aim to estimate the agent state x n using all available measurements z 1:n from all PAs up to time n.In particular, we calculate an estimate xn by using the minimum meansquare error (MMSE) estimator [49,Ch. 4] The map of the environment is represented by reflective surfaces described by PMVAs.Therefore, the positions p s,mva of the detected PMVAs s ∈ {1, . . ., S n } must be estimated.This relies on the marginal posterior existence probabilities p(r s,n = 1|z The calculation of f (x n |z 1:n ), p(r s,n = 1|z), and f (p k,mva | r s,n = 1, z 1:n ) from the joint posterior f (y 0:n , x 0:n , a 1:n , a 1:n |z 1:n ) by direct marginalization is not feasible.By performing sequential message passing using the SPA rules [3], [43], [50], [51] on the factor graph in Fig. 3, approximations ("beliefs") f x n and fs y s,n of the marginal posterior PDFs f (x n |z 1:n ), p(r s,n = 1|z 1:n ), and f (p s,mva | r s,n = 1, z 1:n ) can be obtained efficiently for the agent state as well as all legacy and new PMVAs states s ∈ S n .
f (y 0:n , x 0:n , a 1:n , a 1: Initial prior PDFs of PMVA and agent states Factors related to legacy PMVA states Prior PDFs and related parameters of new PMVA states (19) C. The Factor Graph By using common assumptions [3], [44], and for fixed (observed) measurements z 1:n , the joint posterior PDF of y 0:n , x 0:n , a 1:n , and a 1:n , conditioned on z 1:n is given by (19) as shown on top of the page, where we introduced the functions q P x n ′ , a m,n , f y s,n , and Ψ a m,n that will be discussed next.A detailed derivation of ( 19) is provided in the supplementary material [37,Section II].
The pseudo likelihood functions of PA j q P x n , a s ′ ,n , a s,n , p s ′ ,mva , r are, respectively, given for (0, 0) by for (s, s) ∈ D (j) S,n by , a s,mva ), a ss,n = 0 (21) and q S p (j) s,n = 0, a s,mva , p and q D p ( s ′ ,mva , r s ′′ ,n = 0 for s ′′ ∈ {s, s ′ }.The pseudo likelihood functions related to new PMVAs q S y (j) m,mva ,a m,n ,x n ;z (j) n is given by q S p (j) m,mva , r (j) s,n = 1, a (j) m,n , x n ; z (j) m,n = 0 (23) and q S p (j) m,mva , r s,n = 0, a m,mva , respectively.Note that the first line in ( 23) is zero because per definition, the new PMVA with index m exists (r m,n = 1) if and only if the measurement is not associated to a legacy PMVA.For a (j) m,n = 0, for each measurement z m,n is assumed to originate from a single-bounce path corresponding to exactly one PMVA s ∈ M (j) n (not a pair of PMVAs).Finally, the binary check functions that validates consistency for any pair (a m,n ) of PMVA-oriented and measurement-oriented association variable at time n, read In case the joint PMVA-oriented association vector a n and the measurement-oriented association vector a n do not describe the same association event, at least one check function in (19) is zero.Thus f (y 0:n , x 0:n , a 1:n , a 1:n |z 1:n ) is zero as well.The factor graph representing factorization (19) is shown in Fig. 3.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 [44].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.

V. PROPOSED SUM-PRODUCT ALGORITHM
Since our factor graph in Fig. 3 has cycles, we have to decide on a specific order of message computation [50], [52].We choose the order according to the following rules [3], [5], [24], [25], [43], [44], [51]: (i) messages are only sent forward in time; (ii) messages are only sent from PA j − 1 to PA j, i.e., the measurements of PA are processed serial, thus, PA j − 1 establishes new PMVAs that are acting as legacy PMVAs for PA j; (iii) iterative message passing is only performed for data association [3], [4], i.e., in particular, for the loops connecting different PMVAs, 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 [50] yield the following operations at each time step.The corresponding messages are shown in Fig. 3.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 PDFs" introduced in Section III, we consider messages φ y s,n = φ p s,mva , r s,n for non-existing PMVA states, i.e., for r s,n = 0. We define these messages by φ p s,mva , 0 ≜ φ (j) k,n (note that these messages are not PDFs and thus are not required to integrate to 1).To keep the notation concise, we also define the sets

A. Prediction Step
First, a prediction step is performed for the agent and all legacy PMVAs s ∈ S n−1 .Based on the SPA rule, the prediction message for the agent state is given by and the prediction message for the legacy PMVAs is given by s ∈ {1, . . ., S n−1 }, where the beliefs of the agent state, f (x n−1 ), and of the PMVA states, f p s,mva , r s,n−1 , were calculated at the preceding time n−1.Inserting ( 8) and ( 9) for f p s,mva , r s,n p s,mva , r s,n−1 = 1 and f p s,mva , r s,n p s,mva , r s,n−1 = 0 , respectively, we obtain α s p s,mva , 1 = p s δ p s,mva − p s,mva fs p s,mva , 1 dp s,mva (26) and α s p s,mva , 0 = α s,n f d p s,mva with We note that fs,n−1 ≜ fs p s,mva , 0 dp s,mva approximates the probability of non-existence of legacy PMVA s at the previous time step n − 1.

B. Sequential PA Update
At iteration j ∈ {1, . . ., J}, the following operations are calculated for all legacy and new PMVAs.
1) Transition of New and Legacy PMVA States Between PAs: For j = 1, the number of legacy PMVAs is S (1) n = S n−1 with the corresponding state y (1)  n ≜ y . Furthermore, the state y (j−1) is empty and the prediction message of legacy PMVAs is α s p (j) s,mva , r s,n ≜ α s p s,mva , r s,n as well as α n new PMVAs with states y (j) .For 1 ⩽ s ⩽ S (j−1) n , using (10), (11), and (25) the prediction message of former legacy PMVAs is given by s,mva , 1 = δ p (j) s,mva − p (j−1) s,mva γ p (j−1) s,mva , 1 dp (j−1) and α s p (j) s,mva , 0 = α s,n .The prediction message of former new PMVAs is α s p (j)  s,mva , r s,n ≜ ϕ y (j) s,n and α 2) Checking the Availability of Propagation Paths: The proposed SPA algorithm performs an availability check for each propagation path using RT [34]- [36] to determine whether a VA can provide map information.First, the VA positions p (j) ss ′ ,va are determined by applying (3) directly to PA j at position p ss ′ ,va .RT is performed as described in Section III-D.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 p n .Hence, the corresponding VA cannot be associated with measurements at time n.
3) Measurement Evaluation for the LOS Path: The messages β 00 a (j) 00,n passed from the factor node q P x n , a (j) 00,n ;z (j) n to the feature-oriented association variables a (j) 00,n are calculated as 4) Measurement Evaluation for Legacy PMVAs: For s = s ′ , the messages β ss a (j) ss,n passed from the factor node q S p (j) s,mva , r
For s ̸ = s ′ , the message β ss ′ a (j) ss ′ ,n passed from the factor node q D p (j) s,mva , r of PMVAs to the feature-oriented association variables a (j) ss ′ ,n are obtained as s ′ ,n .(31) 5) Measurement Evaluation for New PMVAs: For PA j, the messages ξ a (j) m,n sent from the factor node q S p (j) m,mva , r m,n to the variable nodes corresponding to the measurement-oriented association variables a (j) m,n are given by ξ a (j)  m,n = r q S p (j) m,mva , r Using the expression of q S p (j) m,mva , r m,n in Section IV-C in (23), Eq. ( 32) simplifies to ξ a n , and for a m,n are calculated iteratively according to [44], [47], for each iteration index p ∈ {1, . . ., P }.After the last iteration p = P, the messages η a (j) ss ′ ,n and ς a (j) m,n are calculated according to [44], [47].Details are provided in the supplementary material [37, Section III].
7) Measurement Update for the Agent: The message γ (j) 00 (x n ) sent from the factor node q P x n , a (j) 00,n ; z (j) n to the agent variable node is computed as to the agent variable node are given by , 1, a (j) ss,n , x n ; z (j) n × α s p (j) s,mva , 1 dp (j) s,mva + η a (j) ss,n = 0 α (j) s,n .(35) Furthermore, for s ̸ = s ′ , the messages passed from the factor node q D p (j) to the agent variable node are obtained as 8) Measurement Update for Legacy PMVAs: Similarly, for s = s ′ , the messages ρ ss ′ y (j) s sent to the legacy PMVAs variable nodes are given by , 1, a (j) ss,n , x n ; z (j) n dx n (37) ρ (j) ss ≜ ρ ss p (j) s,mva , 0 = η a (j) ss,n = 0 (38) and for s ̸ = s ′ by Based on these messages, the message sent to the next PA γ y (j) s ≜ γ p (j) s,mva , r k,n is computed as 9) Measurement Update for New PMVAs: Finally, the messages ϕ y m,n sent to the new PMVA variable nodes are obtained as ϕ p (j)  m,mva , 1 = q S p (j) m,mva , 1, a (j) m,n , x n ; z (j)

C. Belief Calculation
After the messages for all PAs, j ∈ {1, . . ., J} are computed, the belief f (x n ) of the agent state can be calculated as normalized production of all incoming messages [50], i.e., with normalization constant 45) is a valid probability distribution.Similarly, the beliefs fs y (J) s,n = fs p (J)  s,mva , r s,n of legacy PMVA s ∈ S J n , is given by fs (y with constant C s,n = γ p (J) s,mva , r Similarly, the fm y (J)  m,n = fm p (J) m,mva , r where A computationally feasible sequential particle-based message passing implementation can be obtained following [3], [43], [51].In particular, we adopted the approach in [51] using the "stacked state" comprising the agent state and the PMVA states.To avoid that the number of PMVA states grows indefinitely, PMVAs states with p(r (j) s,n = 1|z 1:n ) below a threshold p 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 max , where S max is another predefined threshold, only the S 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 material [37, Section V].

D. Implementation Aspects and Computational Complexity
When beliefs of new PMVAs (cf.( 47), ( 43), (44), and ( 23)) are introduced, contrary to [3], [43], we do not use the conditional prior PDF of newly detected MVAs, f n p (j) m,mva |p n , as a proposal PDF.We develop an alternative proposal PDF where first samples of p (j) ss,va are obtained by using the inverse transformation of ( 5) and (6).Based on the resulting samples of p (j) ss,va , samples of p (j) m,mva can then be computed by exploiting the inverse transformation in (4).
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 PMVAs-pairs for each measurement, significantly complicating the data association.
The computational complexity of the jth processing block that performs probabilistic data association can be analyzed as follows.As discussed in [3], [44], the computational complexity of such a processing block scales as 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) 2 .The computational complexity of the jth processing block thus scales as O(S (j)2 M ).If conventional probabilistic data association [53] 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 V-C, 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 PMVAs leads to a large number of corresponding VAs.A key feature of the proposed method that makes this possible is that probabilistic data association can be solved in a scalable way.

VI. EVALUATION
The performance of the proposed MVA-based SLAM algorithm is validated and compared with the multipath-based SLAM algorithm from [3] that has been extended to use AOA measurements described here and in [4], the channel SLAM algorithm from [2], and the multipath assisted positioning algorithm from [40] using synthetic measurements as well as real RF measurements.Additional simulation results can be found in the supplementary material [37, Section IV].

A. Common Setup and Performance Metrics
The agent's state-transition pdf f (x n |x n−1 ), with T , is defined by a linear, near constant-velocity motion model [54, Sec.6.3.2],i.e., x n = Ax n−1 + Bw n .Here, A ∈ R 4×4 and B ∈ R 4×2 are as defined in [54, Sec.6.3.2](with sampling period ∆T = 1s), and the driving process w n is iid across n, zero-mean, and Gaussian with covariance matrix σ 2 w I 2 , where I 2 denotes the 2 × 2 identity matrix and σ w denotes the acceleration noise standard deviation.For the sake of numerical stability, we introduced a small regularization noise to the PMVA state p s,mva at each time n, i.e., p s,mva = p s,mva + ω s , where ω s is iid across s, zeromean, and Gaussian with covariance matrix σ 2 a I 2 .The particles for the initial agent state are drawn from a 4-D uniform distribution with center x 0 = [p T 0 0 0] T , where 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 m, 0.5 m] and of each velocity component is given by [−0.1 m/s, 0.1 m/s].At time n = 0, the number of PMVAs is S 0 = 0, i.e., no prior map information is available.The prior distribution for new PMVA states f n (y m,n ) is uniform on the square region given by [−15 m, 15 m] × [−15 m, 15 m] around the center of the floor plan shown in Fig. 4a and the mean number of new PMVA is µ n = 0.05.The probability of survival is p s = 0.999, the confirmation and pruning thresholds are respectively p cf = 0.5 and p pr = 10 −3 .We performed 500 simulation runs.The performance of the different methods discussed is measured in terms of the root mean-square error (RMSE) of the agent position, as well as the optimal subpattern assignment (OSPA) error [55] of VAs and MVAs.Since the proposed method estimates MVAs, we first map MVA estimates to VA estimates following equation (3), before we compute OSPA errors of VAs.OSPA is a multiobject 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, RMSEs of each unknown variable are obtained by averaging over all converged simulation runs.We declare a simulation run to be converged if {∀n : || xn − x n || < 5 m}.
For synthetic measurements (Experiment 1-3), we use the following common parameters.The detection probability of all paths is p d,ss ′ ,n = p d = 0.95 for (s, s) ∈ D S and (s, s ′ ) ∈ DD , respectively.In addition, a mean number µ fp = 1 of false positive measurements z

B. Reference Methods
In the following sections, we compare the proposed MVAbased SLAM algorithm (PROP) to four different reference methods as described in the following: 1) MP-SLAM: The multipath-based SLAM algorithm from [3].
The method considered here is a combination of [3] and [4] since in [4] statistical model of [3] is extended to AOA measurements of MPCs.However, in contrast to [4], we do not use the component SNRs estimates of MPCs.Contrary to the method proposed in this paper, the reference method does rely on a much simpler statistical model without MVAs and ray-tracing.It thus cannot fuse data across propagation paths and VAs. 2) CH-SLAM: The channel SLAM algorithm from [2].We only implemented the channel SLAM algorithm proposed in [2], 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 [56, Section 13], [10], [12].3) MINT: The multipath-based positioning algorithm from [40], which assumes known map features (i.e., the VA positions are known).4) LOS-MINT: A reduced version of MINT, where we only consider the PAs (i.e., no VAs).
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 VAs.To analyze the performance gain due to exploiting double-bounce reflections, we generate two different datasets: (Setup-I) the full setup considering all VAs corresponding to single-bounce and double-bounce paths and (Setup-II) a reduced setup considering only VAs corresponding to single-bounce paths.If not stated differently, measurements are generated according to Setup-I.

C. 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 4a.We chose the scenario to be identical to [3] for easy comparison.The scenario consists of four reflective surfaces, i.  5a shows the MOSPA error for the two PAs and all associated VAs, Fig. 5b shows the MOSPA error for all MVAs, and Fig. 5c shows the RMSE of the agent's position all versus time n.Fig. 5d 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.The MOSPA error of PROP related to VAs and PAs is shown in Fig. 5a.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. 5b shows the MOSPA error of the MVA positions, which confirms the results seen in Fig. 5a.The RMSE of the agent position in 5c 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 5d, facilitating the statistical dependencies across multiple paths and PAs.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   n | j ∈ J, n ∈ {1, ... , N } in a multiplicative way.Base values are set as the measurement standard deviations of Experiment 1.While f std,all affects measurements corresponding to PAs and MVAs, f std,los affects only measurement standard deviations corresponding to PAs (i.e., LOS measurements).The scenario is identical to experiment 1 of Section VI-C.Note that for all algorithms none of the 500 simulation runs diverged.
Fig. 6a to 6f 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 std,all or f std,los .In particular, in Fig. 6a, 6c and 6e we varied f std,all and kept f std,los ≜ 1 fixed, while in Fig. 6b, 6d and 6f we varied f std,los and kept f std,all ≜ 4 fixed.The VAs MOSPA errors in Figs.6a to 6d emphasize the observations of Experiment 1 (Section VI-C).All error values increase with increasing f 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 PAs and different propagation paths.In contrast, the agent RMSE in Fig. 6e 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 std,los in Fig. 6f, the RMSE of MP-SLAM significantly increases.In contrast, the agent RMSE of PROP remains approximately constant due to the increased map stability.

E. Experiment 3: Low Information and Obstructed LOS
In this experiment, we analyze the performance of PROP in the scenario shown in Figure 4b.It contains only two reflective walls (K = 2) while one short wall obstructs the LOS path, i.e., the path between PAs and the agent as well as the paths between VAs and agent [57].The obstructing wall does not cause any VAs due to the geometric constellation.We compare MP-SLAM, MINT, and LOS-MINT.The scenario contains two PAs.We generate the distance and the AOA of the individual MPC parameters according to a RF signal model [24].We assume the agent has a 3×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 MHz centered at 6 GHz resulting in a sampling time of T s = 1/(1.6B).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 [4], [7], [24].The acceleration noise standard deviation is σ w = 0.02 m/s 2 .Fig. 7a shows the MOSPA error for all MVAs, Fig. 7b shows the RMSE of the agent's position for converged simulation runs, all versus time n.Finally, Fig. 7c 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 7b illustrates the benefits of PROP with respect to MP-SLAM, as it systematically leverages both PAs 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 7c 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 MVAs 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 7a.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 doublebounce propagation paths, comes close to approaching this lower bound.Note that, in Figure 7b, 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 [8], [9].

F. Experiment 4: Validation Using Measured Radio Signals
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. 8 at TU Graz, Austria.More details about the measurement environment and VA calculations can be found in [3], [6], [58].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 × 3, 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. 4c using an M-sequence correlative channel sounder with frequency range 3.1-10.6GHz.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 GHz centered at 6 GHz.The received signal is critically sampled with T s = 1/(1.6B) and artificial AWGN is added such that the output signal-tonoise-ratio is SNR = 30 dB.We apply a variational sparse Bayesian parametric channel estimation algorithm [18] to acquire the M m,n of MPCs.The corresponding noise standard deviations are calculated based on the Fisher information [4], [7], [24].Compared to the synthetic setup, we changed the mean number of false alarm measurements to µ fp = 3, the detection probability to p d = 0.7, regularization noise standard deviation to σ a = 2 • 10 −3 m, and the acceleration noise standard deviation to σ w = 0.0114 m/s 2 .Note that for all algorithms, none of the 500 simulation runs diverged.Fig. 4c depicts for one simulation run the posterior PDFs 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 MPCs estimates, i.e., measurements, PROP robustly estimates the main walls. 6Fig. 9 compares PROP and MP-SLAM in terms of the agent RMSE.Fig. 9a shows the RMSE of the agent's position, and Fig. 9b shows the RMSE of the agent's orientation for simulation runs, all versus time n.The comparison of the position RMSEs shows a similar behavior as for synthetic measurements (see Fig. 5), i.e., PROP outperforms MP-SLAM.The mapping capability and low agent RMSEs of PROP, when applied to real RF signals, demonstrate the high potential of PROP for accurate and robust RF-based localization.

VII. CONCLUSIONS
In this paper, we introduced data fusion for multipathbased simultaneous localization and mapping (SLAM).A key novelty of our approach is to represent each reflective surface in the propagation environment by a single master virtual anchor (MVA).In this way, we address a key limitation of existing multipath-based SLAM methods, which represent every propagation path by a "virtual anchor" (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 advanof 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 ray-tracing (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 stateof-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 MVAs due to floor and ceiling surfaces.Promising directions for future research also include an extension to multiple-measurement-to-feature data association [25], [59] and an advanced MVA model, where the length and shape of reflective surfaces [16] are also taken into account.Another future research venue aims at incorporating amplitude information to make detection probabilities and measurement variances adaptive [4], [24], [57].

Fig. 1 .
Fig. 1.A multipath-based simultaneous localization and mapping (SLAM) scenario at time step n, involving two reflective surfaces, a single physical anchor (PA) with position p the distance from the reference location p n and the orientation, respectively, of the h-th element as shown in Fig.1.At each discrete time slot n, the position p n ∈ R 2 and pn (

Fig. 2 .
Fig. 2. Three multipath-based SLAMs scenarios with two reflective surfaces illustrate single-and double-bounce paths in three different geometries.The positions of the mobile agents, PAs, VAs, and MVAs are shown.(a) depicts a single time step n of a scenario with perpendicular reflective surfaces, two PAs at positions p (1) pa and p (2) pa , and the agent at position pn; only single-bounce paths are illustrated.(b) shows perpendicular reflective surfaces, one PA at position p (1) pa , and agent positions pn 1 and pn 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 p (1) ∈ R 2 with index-pair (s, s) ∈ D S ≜ {(s, s) ∈ S × S} and |D D | = S(S − 1) double-bounce VAs at unknown positions p (j) 0) ∪ D. The distances and AOAs related to the propagation paths at the agent position p n represented by p (j) ss ′ ,va with (s, s ′ ) ∈ D are modeled by d (j) ss ′ ,n = p n − p (j) ss ′ ,va and φ (j) ss ′ ,n = atan2 p 2,n − p (j) 1,ss ′ ,va , p 1,n − p (j) 1,ss ′ ,va − ψ n .Note that the distance d (j) 00,n and the AOAs φ are the parameters related to the LOS path between agent at position p n and PA j at position p (j)

2 − 1
ss,va , i.e., p (j) ss,va = h va p s,mva , p (j) pa can be obtained as p(j)  ss,va = − 2 p s,mva , p (j) pa p s,mva p s,mva + p (j) pa .(3) are, respectively, zero-mean Gaussian measurement noise with standard deviations σ d (j) m,n and σ φ (j) m,n .The measurements are combined in the vector z can directly obtain the likelihood function for the LOS path as f z (j) m,n p n .
with (0, 0) ∈ Dn generates a measurements z (j) m,n with detection probability p (j) d p n .If MVA s exists (r s,n = 1), the corresponding single-bounce path (s, s) ∈ D S,n generates a MVA-originated measurements z (j) m,n with detection probability p (j) d p n , p s,mva .The same holds for the double-bounce path (s, s ′ ) ∈ D D,n with detection probability p (j) d p n , p s,mva , (s, s) ∈ D (j) S,n , reads p (j) d p n , p s,mva ≜ ,n , path from agent at p n to VA at p (j)

Fig. 3 .
Fig.3.Factor graph representation of the joint posterior PDF(19).Short notations are used.In particular, the time index n and the functional dependencies

.
, ρj ss ′ = ρ ss ′ y (j) s ρ s ′ s y(j)   For the numbers of MVAs, the short notations reads S 1 ≜ S n−1 , S j ≜ S (j) n , and (•) S jj ≜ (•) The dashed lines with arrows indicate messages representing the agent and PMVAs beliefs of time n−1, n + 1 or of anchors j−1, j+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). n ] T representing a potential MPC parameter estimate contains a distance measurement z d (j) m,n ∈ [0, d max ] and an AOA measurement z φ (j) m,n ∈ [−π, π).The channel estimator decomposes the RF signal vector into individual, decorrelated components, reducing the number of dimensions (as M (j) PMVAs related to singlebounce paths q S y (j) (j) ss,n , x n ; z (j) n ≜ δ a (j) ss,n as well as for (s, s ′ ) ∈ D (j) D,n by ≜ α s,n .For j > 1, we have S pa to get single-bounce-related VAs at positions p (j) ss,va .Next, (3) is also applied to this single-bouncerelated VAs to get double-bounce-related VAs at positions p (j) ss,n , x n ; z (j) n of single PMVAs to the feature-oriented association variables a (j) ss,n given by β ss a (j) ss,n

) 6 )
Iterative Data Association: Next, from β a (j) ss ′ ,n and ξ a (j) m,n , messages η a (j) ss ′ ,n and ς a (j) m,n are obtained using loopy (iterative) BP.First, for each measurement, m ∈ M ss ′ ,n and, then, for ss ′ ∈ D

Fig. 4 .
Fig. 4. Scenarios used for numerical evaluation: (a) shows the scenario for Experiments 1 and 2 in Sections VI-C and VI-D.This scenario consists of a rectangular room with two PAs, 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 VI-E which consists of two PAs, 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 VI-F.This scenario consists of a classroom with two PAs 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 II.
were generated according to the pdf f fp (z(j) m,n ) that is uniformly distributed on [0 m,30m] for distance measurements and uniformly distributed on [−π, π] for AOA measurements.In each simulation run, we generated noisy distance and AOA measurements according to (5) and (6) stacked into the vector z (j) m,n .

e., K = 4
MVAs and two PAs.The noise standard deviations for the LOS path are σ d (j) m,n = 0.05 m and σ φ (j) m,n = 10 • , for single-bounce path are σ d (j) m,n = 0.10 m and σ φ (j) m,n = 15 • , and for double-bounce path are σ d (j) m,n = 0.15 m and σ φ (j) m,n = 25 • .The acceleration noise standard deviation is σ w = 9 • 10 −3 m/s 2 .As an example, Fig. 4a depicts for one simulation run the posterior PDFs represented by particles of the MVA positions and corresponding reflective surfaces as well as estimated agent tracks.Fig.

Fig. 5 .
Fig. 5. Performance results for Experiment 1 in Section VI-C: (a) MOSPA errors of the VAs of each PA, (b) MOSPA errors versus time of the MVAs, (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.

Fig. 6 .
Fig. 6.Performance results for Experiment 2 in Section VI-D 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.CPUs) were measured 4 s for PROP, 1.2 s for MP-SLAM, and 11 s for CH-SLAM.5

Fig. 7 .
Fig. 7. Performance results for Experiment 3 in Section VI-E: (a) MOSPA errors versus time of MVAs, (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).

Fig. 8 .
Fig. 8. Picture of the classroom used for data collection.

Fig. 9 .
Performance results for Experiment 4 in Sec VI-F: (a) RMSEs versus time of the agent position and (b) RMSEs versus time of the agent orientation.
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 coorganizer 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.

TABLE I NOTATIONS
AND DEFINITIONS OF IMPORTANT QUANTITIES.ss ′ ,va = h va p s,mva , h va p s ′ ,mva , p [49,number Ŝn of PMVA states that are considered to exist is the estimate of the total number S of MVAs.To avoid that the number of PMVAs states grows indefinitely, PMVAs states with p(r s,n = 1|z 1:n ) < p pr are removed from the state space ("pruned").For existing PMVAs, an estimate of it's position p s,mva can again be calculated by the MMSE[49, Ch. 4] s,mva and the marginal posterior PDFs f (p s,mva |r s,n = 1, z 1:n ) = f (p s,mva , r s,n = 1|z 1:n )/p(r s,n = 1|z 1:n ).A PMVA s is declared to exist p(r s,n = 1|z 1:n ) > p cf , where p cf is a confirmation threshold.