A Belief Propagation Algorithm for Multipath-Based SLAMIn multipath-assisted indoor localization [5], [8], [9], [13]– [16], the relation of multipath components (MPCs) with the local

1

A Belief Propagation Algorithmfor Multipath-Based SLAM

Erik Leitinger, Member, IEEE, Florian Meyer, Member, IEEE, Franz Hlawatsch, Fellow, IEEE,Klaus Witrisal, Member, IEEE, Fredrik Tufvesson, Fellow, IEEE, and Moe Z. Win, Fellow, IEEE

Abstract—We present a simultaneous localization and map-ping (SLAM) algorithm that is based on radio signals and theassociation of specular multipath components (MPCs) with geo-metric features. Especially in indoor scenarios, robust localizationfrom radio signals is challenging due to diffuse multipath propa-gation, unknown MPC-feature association, and limited visibilityof features. In our approach, specular reflections at flat surfacesare described in terms of virtual anchors (VAs) that are mir-ror images of the physical anchors (PAs). The positions of theseVAs and possibly also of the PAs are unknown. We develop aBayesian model of the SLAM problem and represent it by afactor graph, which enables the use of belief propagation (BP)for efficient marginalization of the joint posterior distribution.The resulting BP-based SLAM algorithm detects the VAs associ-ated with the PAs and estimates jointly the time-varying positionof the mobile agent and the positions of the VAs and possiblyalso of the PAs, thereby leveraging the MPCs in the radio sig-nal for improved accuracy and robustness of agent localization.The algorithm has a low computational complexity and scaleswell in all relevant system parameters. Experimental results us-ing both synthetic measurements and real ultra-wideband radiosignals demonstrate the excellent performance of the algorithmin challenging indoor environments.

Index Terms—Simultaneous localization and mapping, SLAM,multipath channel, data association, factor graph, message pass-ing, sum-product algorithm.

I. INTRODUCTION

The goal of simultaneous localization and mapping (SLAM)[1], [2] is to estimate the time-varying pose of a mobileagent—including the agent’s position—and a map of the sur-rounding environment, from measurements provided by one ormultiple sensors. SLAM is important in many fields includingrobotics [1], autonomous driving [3], location-aware commu-nication [4], and robust indoor localization [5]–[9]. Achievinga required level of accuracy robustly is still elusive in indoorenvironments characterized by harsh multipath channel condi-tions. Therefore, most existing systems supporting multipath

E. Leitinger and K. Witrisal are with the Signal Processing and SpeechCommunication Laboratory, Graz University of Technology, Graz, Austria, (e-mail: {erik.leitinger, witrisal}@tugraz.at). F. Meyer and M. Z. Win are withthe Laboratory for Information and Decision Systems, Massachusetts Instituteof Technology, Cambridge, MA, USA (e-mail: {fmeyer, moewin}@mit.edu).F. Hlawatsch is with the Institute of Telecommunications, TU Wien, Vienna,Austria and with Brno University of Technology, Brno, Czech Republic (e-mail: [email protected]). F. Tufvesson is with the Department ofElectrical and Information Technology, Lund University, Lund, Sweden (e-mail: [email protected]). This work was supported by the AustrianScience Fund (FWF) under grants J 4027, J 3886, P 27370-N30, and P 32055-N31; by the U.S. Department of Commerce, National Institute of Standardsand Technology under Grant 70NANB17H17; and by the Czech Science Foun-dation (GACR) under grant 17-19638S. Parts of this paper were presented atIEEE ICC, ANLN Workshop 2017, Paris, France, June 2017.

channels either use sensing technologies that mitigate multi-path effects [10] or fuse multiple information sources [11],[12].

In multipath-assisted indoor localization [5], [8], [9], [13]–[16], the relation of multipath components (MPCs) with thelocal geometry potentially turns multipath propagation from animpairment into an advantage. This paper presents a SLAMalgorithm for robust indoor localization based on radio signals.The radio signals are transmitted from a mobile agent to basestations, called physical anchors (PAs). MPCs due to specularreflections are modeled by virtual anchors (VAs), which aremirror images of the PAs [17]. Our algorithm detects the VAsassociated with the PAs and estimates the VA and possiblyalso PA positions jointly with the time-varying position of themobile agent. The algorithm is designed to cope with harshmultipath channel conditions, which tend to lead to measure-ments with a high level of false alarms and missed detections.While MPCs can be generated by various propagation phe-nomena such as specular reflections, scattering, and diffrac-tion, our model focuses on PA/VA-related MPCs; all the otherMPCs are modeled as interference, even if they contain geo-metric information. We note that MPCs associated with scatterpoints are considered in the feature model used in [13].

A. Feature-based SLAM

The proposed algorithm follows the feature-based approachto SLAM [2], [18], [19]. The map is represented by an un-known number of features with unknown spatial positions,whose states are estimated in a sequential (time-recursive)way. In our model, the features are given by the PAs andVAs. Prominent feature-based SLAM algorithms are extendedKalman filter SLAM (EKF-SLAM) [18], Rao-Blackwellized(RB)-SLAM (dubbed FastSLAM) [2], [13], [20], variational-inference-based SLAM [21], [22], and set-based SLAM [19],[23], [24]. Recently, feature-based SLAM methods that ex-ploit position-related information in radio signals were intro-duced [13], [15], [16], [25]–[27]. Most of these methods oper-ate on estimated parameters related to MPCs, such as distances(which are proportional to delays), angles-of-arrival (AoAs),or angles-of-departure (AoDs) [28]–[32]. These parameters areestimated from the signal in a preprocessing stage and are con-sidered as “measurements” by the SLAM method. An impor-tant aspect is the data association (DA) between these mea-surements and the PAs or VAs.

Feature-based SLAM is closely related to multitarget track-ing (MTT), and MTT methods have been adapted to feature-

arX

iv:1

801.

0446

3v4

[cs

.IT

] 2

9 A

ug 2

019

2

based SLAM [19], [23], [33]. MTT methods that are applica-ble to SLAM include the joint probabilistic DA (JPDA) filter[34] and the joint integrated probabilistic DA (JIPDA) filter[35]. An approach similar to the JIPDA filter is taken by themethods presented in [36], [37]. More recently, the use ofbelief propagation (BP) [38], [39] was introduced for prob-abilistic DA within MTT in [40] and for multisensor MTTin [41]–[43]. In particular, the BP algorithms in [41]–[43] arebased on a factor graph representation of the multisensor MTTproblem and have a computational complexity that scales onlyquadratically in the number of objects (targets) and linearly inthe number of sensors. MTT methods that are based on ran-dom finite sets and embed a BP algorithm for probabilisticDA were presented in [41], [44], [45]. We finally note thatour approach to feature-based SLAM is also related to mul-tisensor target tracking with uncertain sensor locations usingBayesian methods [46].

B. Contributions and Organization of the Paper

Here, we propose a BP-based, Bayesian detection and esti-mation algorithm for SLAM using radio signals. Our algorithmjointly performs probabilistic DA and sequential estimation ofthe states of a mobile agent and of “potential features” (PFs)characterizing the map. We use a probabilistic model for fea-ture existence where each PF state is augmented by a binaryexistence variable and associated with a probability of exis-tence, which is also estimated. The proposed algorithm is in-spired by the BP algorithms for multisensor MTT presentedin [42], and will hence be briefly referred to as BP-SLAM al-gorithm. Probabilistic DA and state estimation are performedby running BP on a factor graph [38], [39] representing thestatistical structure of the SLAM problem. The BP approachleverages conditional statistical independencies to achieve lowcomplexity and high scalability. In fact, in contrast to conven-tional SLAM algorithms such as EKF-SLAM and RB-SLAM,the proposed BP-SLAM algorithm assumes that in each timestep the agent state and all the feature states are a priori in-dependent.

Our probabilistic model for DA and feature existence uncer-tainty allows the BP-SLAM algorithm to succeed in the par-ticularly challenging range-only SLAM problem [47], whichcannot be addressed straightforwardly by established SLAMtechniques [1], [2]. Performing SLAM only from range mea-surements is challenging because (i) when new features are ini-tialized, the probability distributions of the PAs/VAs are annu-larly shaped and thus cannot be well represented by a Gaussiandistribution; and (ii) DA is more difficult compared to classicalSLAM problems in robotics since PAs/VAs at widely differentpositions can generate almost identical range measurements.In particular, the algorithm of [47] is able to cope with a range-only measurement model, but not with DA uncertainty. To thebest of our knowledge, the proposed algorithm—along witha preliminary version presented in [33]—is the first BP algo-rithm for feature-based SLAM with probabilistic DA that isalso suitable for range-based SLAM.

Key innovative contributions of this paper include the fol-lowing:

• We establish a Bayesian model for feature-based SLAMthat uses MPC parameters extracted from radio signalsas input measurements and models probabilistically theappearance and disappearance of PAs/VAs as well as theDA uncertainty.

• Based on a factor graph representation of this model, wedevelop a scalable BP algorithm that estimates the stateof the mobile agent and the numbers and positions ofPAs/VAs.

• We evaluate the performance of the proposed algorithmon synthetic and real data. Our experimental resultsdemonstrate the algorithm’s high accuracy and robust-ness.

We apply the proposed BP-SLAM algorithm to the chal-lenging setup of a range-only measurement model. However,bearing information (AoA and AoD of MPCs) or informationderived from inertial measurement unit sensors can be easilyincorporated in the BP-SLAM algorithm, and this would leadto a significant performance gain. For simplicity, we assumetime synchronization between the PAs and the mobile agent.However, the BP-SLAM algorithm can be extended to nonsyn-chronized PA-agent links along the lines of [13] (based on thefact that the relevant geometric information is also contained inthe time differences of the MPCs [14], [48]) or to joint SLAMand synchronization along the lines of [49]. Furthermore, weassume that the probabilities with which the preliminary sig-nal analysis stage (producing measurements) detects featuresin the radio signals are known; however, an adaptive exten-sion to unknown and time-varying detection probabilities canbe obtained along the lines of [33], [43], [50].

This paper advances over our conference paper [33] in thatit replaces the heuristic used therein for determining the initialdistribution of new PFs by an improved Bayesian technique.Furthermore, the factor graph and BP algorithm of [33] areextended by the introduction of “new PFs,” i.e., features thatgenerate measurements for the first time.

The remainder of this paper is organized as follows. SectionII considers the received radio signals and the MPC param-eters. Section III describes the system model and provides astatistical formulation of the SLAM problem. The joint pos-terior distribution of the states and the corresponding factorgraph are derived in Section IV. In Section V, the proposedBP-SLAM algorithm is presented. The results of numerical ex-periments are reported in Section VI. Section VII concludesthe paper.

II. RADIO SIGNAL AND MPC PARAMETERS

Radio signal based SLAM [5], [13], [25], [26] associatesspecular MPC parameters estimated from the received signalswith “geometrically expected” parameters, such as distances(related to delays), AoAs, and AoDs. These parameters aremodeled in terms of the positions of the mobile agent and ofthe PAs or VAs. The VA positions are mirror images of thePA positions that are induced by reflections at flat surfaces—typically walls—and thus depend on the surrounding environ-ment (floor plan) [17]. For each reflection path, the length from

3

15

x in m

yin

m

a(1)1

a(2)1

p1

p350

trajectory of mobile agentstarting position of mobile agent

first-order VAs associated with PA 1first-order VAs associated with PA 2second-order VA associated with PA 1

PA 1PA 2

current position of mobile agent

path to first-order VA associated with PA 1path to first-order VA associated with PA 2path to second-order VA associated with PA 1

0

0

5

5−5

10

10

Fig. 1: Example of an environment map (floor plan). The PAs atfixed positions a

(1)1 (PA 1) and a

(2)1 (PA 2) are indicated by, respec-

tively, a red bullet and a blue box within the floor plan. The magentadashed-dotted line represents the trajectory of the mobile agent. Thestarting position of the mobile agent, p1, and the current positionp350 (at discrete time n=350) are indicated by a black and a cyanbullet, respectively. The red circles and blue squares outside the floorplan indicate some of the geometrically expected first-order VAs as-sociated with PA 1 and PA 2, respectively. The green circle outsidethe floor plan indicates one second-order VA associated with PA 1.Two exemplary first-order reflection paths between the mobile agentat position p350 and two first-order VAs associated with the two PAsare shown by red and blue lines. One exemplary second-order reflec-tion path between the mobile agent at position p350 and a second-order VA associated with PA 1 is shown by a green line.

the PA (via the flat surface) to the mobile agent is equal to thelength from the VA to the mobile agent. Even though the mo-bile agent moves, the VAs remain static as long as the PAs andthe flat surfaces (floor plan) are static. Note that the VA posi-tions are unknown because the floor plan is unknown. As anexample, Fig. 1 depicts a floor plan with two PAs and some ofthe corresponding first-order VAs as well as one second-orderVA belonging to some larger flat surfaces. Also shown arethree reflection paths (for the two PAs) for mobile agent posi-tion p350. The construction of higher- order VAs is describedin [51, Ch. 2].

We consider a mobile agent with unknown time-varyingposition pn∈R2 and J PAs with possibly unknown positionsa

(j)1 ∈ R2, j = 1, . . . , J , where J is assumed to be known.

Associated with the jth PA, there are L(j)n −1 VAs at unknown

positions a(j)l ∈ R2, l = 2, . . . , L

(j)n . The PAs and VAs will

also be referred to as features. The number of features, L(j)n ,

is unknown and time-varying, and it depends on the agentposition pn. In each discrete time slot n, the mobile agent

transmits a radio signal s(t) and the PAs act as receivers.However, the proposed algorithm can be easily reformulatedfor the case where the PAs act as transmitters and the mobileagent acts as a receiver. The baseband signal received by thejth PA is modeled as [14]

r(j)n (t) =

L(j)n∑l=1

w(j)l,n s

(t− τ (j)

l,n

)+ d(j)

n (t) + nAWGN(t) . (1)

Here, the first term on the right-hand side describes the con-tribution of L(j)

n specular MPCs with complex amplitudesw

(j)l,n and delays τ (j)

l,n , where l ∈ L(j)n ,

{1, . . . , L

(j)n

}. These

MPCs correspond to features (PAs or VAs). The delays τ (j)l,n

are proportional to the distances (ranges) between the agentand either the jth PA (for l = 1) or the associated VAs (forl ∈ {2, . . . , L(j)

n }). That is, τ (j)l,n =

∥∥pn−a(j)l

∥∥/c, where c is thespeed of light. The second term in (1), d(j)

n (t), represents thediffuse multipath component, which interferes with the spec-ular MPC term. The third term in (1), nAWGN(t), is additivewhite Gaussian noise. We note that expression (1) presupposesa common time reference at the mobile agent and at the PAs.However, our algorithm can be extended to an unsynchronizedsystem along the lines of [13], exploiting the fact that the rele-vant geometric information is preserved in the time differencesof the MPC delays [14], [48]. Furthermore, our algorithm canalso be extended to the case where the MPC parameters in-clude AoAs and/or AoDs in addition to the delays τ (j)

l,n . Anextension that uses the complex MPC amplitudes w(j)

l,n to di-rectly estimate the detection probability of each feature hasbeen proposed in [50].

In each time slot n and for each PA j ∈ {1, . . . , J}, a para-metric radio channel estimator [28]–[32], [52] processes theradio signals r(j)

n (t) and produces M (j)n MPC parameter es-

timates along with estimates of the corresponding complexamplitudes, with m ∈M(j)

n , {1, . . . ,M (j)n }. The set of esti-

mated MPC parameters M(j)n is related to the set of specular

MPCs L(j)n as follows. It is possible that some specular MPCs

are not “detected” by the radio channel estimator and thusdo not produce an MPC parameter estimate, and it is alsopossible that some estimates do not correspond to MPCs. Ac-cordingly, M (j)

n =∣∣M(j)

n

∣∣ may be smaller than, equal to, orlarger than L(j)

n =∣∣L(j)n

∣∣. (Here, | · | denotes the cardinality ofa set.) Note also that M (j)

n depends on the agent position pnand on the environment. The amplitude estimates are used tocalculate estimates of the MPC parameter variances, if suchestimates are not provided by the channel estimator (see Sec-tion VI-C). We denote by z

(j)m,n with m∈M(j)

n the estimatedparameters of the mth MPC of PA j. The stacked vectorsz

(j)n ,

[z

(j)T1,n · · · z

(j)T

M(j)n ,n

]Tare used as noisy “measurements”

by the proposed BP-SLAM algorithm.

III. SYSTEM MODEL AND STATISTICAL FORMULATION

A. Agent State and PF States

The state of the mobile agent at time n is xn, [pTn vT

n]T,where vn is the agent’s velocity. The PFs are indexed by the tu-ple (j, k), where j ∈ {1, . . . , J} and k ∈K(j)

n , {1, . . . ,K(j)n }

4

(which implies that there are K(j)n PFs for each PA j). Note

that the set of PFs also includes the PA itself. The number ofPAs J is known, but the number of PFs K(j)

n (for PA j) isunknown and random. The existence of the (j, k)th PF as anactual feature is indicated by the variable r(j)

k,n∈ {0, 1}, wherer

(j)k,n= 0 (r(j)

k,n=1) means that the PF does not exist (exists) attime n. The state of PF (j, k) is the PF’s position a

(j)k,n , and

the augmented state is y(j)k,n,

[a

(j)Tk,n r

(j)k,n

]T[42]. We also de-

fine y(j)n ,

[y

(j)T1,n · · · y

(j)T

K(j)n ,n

]Tand yn ,

[y

(1)Tn · · · y(J)T

n

]T.

We will formally consider PF states also for the nonexistingPFs (case r

(j)k,n = 0); however, their values are obviously ir-

relevant. Therefore, the probability density function (pdf) ofan augmented state, f

(y

(j)k,n

)= f

(a

(j)k,n , r

(j)k,n

), is such that

for r(j)k,n= 0, f

(a

(j)k,n , 0

)= f

(j)k,nfD

(a

(j)k,n

), where fD

(a

(j)k,n

)is

an arbitrary “dummy pdf” and f(j)k,n > 0 can be interpreted

as the probability of PF nonexistence [42]. We note that thejoint augmented PF state, described here by the random vectory

(j)n , can also be modeled by a multi-Bernoulli random finite

set [44]. However, the unordered nature of random finite setswould complicate the development of a factor graph and a BPalgorithm.

At any time n, each PF is either a legacy PF, which wasalready established in the past, or a new PF, which is estab-lished for the first time. The augmented states of legacy PFsand new PFs for PA j will be denoted by y

(j)k,n,

[a

(j)Tk,n r

(j)k,n

]T,

k ∈K(j)n−1 and y

(j)m,n,

[a

(j)Tm,n r

(j)m,n

]T, m∈M(j)

n , respectively.Thus, the number of new PFs equals the number of measure-ments, M (j)

n . The set and number of legacy PFs are updatedaccording to

K(j)n = K(j)

n−1∪M(j)n , K(j)

n = K(j)n−1 +M (j)

n , (2)

where the first relation is understood to include a suitablereindexing of the elements of M(j)

n . (The number of PFsdoes not actually grow by M

(j)n because the set of PFs is

pruned, i.e., PFs with small existence probability are dis-carded, as explained in Section V-A.) We also define thefollowing state-related vectors. For the legacy PFs for PAj, a

(j)n ,

[a

(j)T1,n · · · a

(j)T

K(j)n−1,n

]T, r

(j)n ,

[r

(j)1,n · · · r

(j)

K(j)n−1,n

]T,

and y(j)n ,

[y

(j)T1,n · · · y

(j)T

K(j)n−1,n

]T. For the new PFs for PA j,

a(j)n ,

[a

(j)T1,n · · · a

(j)T

M(j)n ,n

]T, r

(j)n ,

[r

(j)1,n · · · r

(j)

M(j)n ,n

]T, and

y(j)n ,

[y

(j)T1,n · · · y

(j)T

M(j)n ,n

]T. For the combination of legacy

PFs and new PFs for PA j, y(j)n ,

[y

(j)Tn y

(j)Tn

]T; note that

the vector entries (subvectors) of y(j)n are given by y

(j)k,n

for k ∈ K(j)n = K(j)

n−1 ∪ M(j)n . For all the legacy PFs,

yn ,[y

(1)Tn · · · y(J)T

n

]T, and for all the new PFs, yn ,[

y(1)Tn · · · y(J)T

n

]T.

The number of new PFs at time n is known only after thecurrent measurements have been observed. Features that areobserved for the first time will be referred to as newly detectedfeatures. Before the current measurements are observed, onlyprior information about the newly detected features is available(namely, their prior distribution and mean number). After the

current measurements are observed, newly detected featuresare represented by new PFs.

The agent state xn and the augmented states of the legacyPFs, y(j)

k,n, are assumed to evolve independently according toMarkovian state dynamics, i.e.,

f(xn, yn|xn−1,yn−1

)= f(xn|xn−1)f(yn|yn−1)

= f(xn|xn−1)

×J∏j=1

K(j)n−1∏k=1

f(y

(j)k,n

∣∣y(j)k,n−1

), (3)

where f(xn|xn−1) and f(y

(j)k,n

∣∣y(j)k,n−1

)= f

(a

(j)k,n, r

(j)k,n

∣∣a

(j)k,n−1, r

(j)k,n−1

)are the state-transition pdfs of the agent and

of legacy PF (j, k), respectively. If PF (j, k) exists at timen−1, i.e., r(j)

k,n−1 =1, it either dies, i.e., r(j)k,n= 0, or survives,

i.e., r(j)k,n=1; in the latter case, it becomes a legacy PF at time

n. The probability of survival is denoted by Ps. If the PF sur-vives, its new state a

(j)k,n is distributed according to the state-

transition pdf f(a

(j)k,n

∣∣a(j)k,n−1

). Therefore, f

(y

(j)k,n

∣∣y(j)k,n−1

)in

(3) is given for r(j)k,n−1 = 1 by

f(a

(j)k,n, r

(j)k,n

∣∣a(j)k,n−1, r

(j)k,n−1 =1

)=

{(1−Ps)fD

(a

(j)k,n

), r

(j)k,n= 0

Psf(a

(j)k,n

∣∣a(j)k,n−1

), r

(j)k,n=1.

(4)

If PF (j, k) does not exist at time n−1, i.e., r(j)k,n−1 = 0, it

cannot exist as a legacy PF at time n either. Therefore,

f(a

(j)k,n, r

(j)k,n

∣∣a(j)k,n−1, r

(j)k,n−1 = 0

)=

{fD(a

(j)k,n

), r

(j)k,n= 0

0, r(j)k,n=1.

(5)

B. Association Vectors

For each PA j, the measurements (MPC parameter esti-mates) z

(j)m,n , m ∈ M(j)

n described in Section II are subjectto a measurement origin uncertainty, also known as DA un-certainty. That is, it is not known which measurement z(j)

m,n

is associated with which PF k ∈ K(j)n , or if a measurement

z(j)m,n did not originate from any PF (this is known as a false

alarm or clutter), or if a PF did not give rise to any measure-ment (this is known as a missed detection). The probabilitythat a PF is “detected” in the sense that it generates a mea-surement z(j)

m,n in the MPC parameter estimation stage is de-noted by P (j)

d

(xn,a

(j)k,n

). The distribution of false alarm mea-

surements is described by the pdf fFA(z

(j)m,n

). The functions

P(j)d

(xn,a

(j)k,n

)∈ (0, 1] and fFA big(z

(j)m,n

)≥ 0 are supposed

known. Following [34], we assume that at any time n, each PFcan generate at most one measurement, and each measurementcan be generated by at most one PF.

The associations between the measurements m ∈ M(j)n

and the legacy PF states (j, k), k ∈ K(j)n−1 at time n can

be described by the K(j)n−1-dimensional feature-oriented DA

5

vector c(j)n =

[c(j)1,n · · · c

(j)

K(j)n−1,n

]T, whose kth entry is de-

fined to be c(j)k,n , m ∈ M(j)

n if legacy PF (j, k) gener-ates measurement z

(j)m,n, and c

(j)k,n , 0 if it does not gener-

ate any measurement. In addition, following [40], [42], weconsider the M (j)

n -dimensional measurement-oriented DA vec-tor b

(j)n =

[b(j)1,n · · · b

(j)

M(j)n ,n

]T, whose mth entry is defined

to be b(j)m,n , k ∈ K(j)

n−1 if measurement z(j)m,n is generated

by legacy PF (j, k), and b(j)m,n , 0 if it is not generated by

any legacy PF. We also define cn ,[c

(1)Tn · · · c(J)T

n

]Tand

bn ,[b

(1)Tn · · · b(J)T

n

]T. The two DA vectors cn and bn are

unknown and modeled as random. They are equivalent sinceone can be determined from the other. However, the redun-dant formulation of DA uncertainty in terms of both cn andbn is key to obtaining the scalability properties of the BP al-gorithm to be presented in Section V-B. Furthermore, as willbe discussed in Section IV, it facilitates the establishment ofa factor graph for the problem of jointly inferring the agentstate and the states of the legacy PFs and new PFs.

C. Prior Distributions

For a given PA j, there are M(j)n = |M(j)

n | new PFs attime n. The number of false alarms and the number of newlydetected features are assumed Poisson distributed with meanµ

(j)FA and µ

(j)n,n, respectively [34], [53]. Then, one can derive

the following expression of the joint conditional prior proba-bility mass function (pmf) of the DA vector c

(j)n , the vector

of existence indicators of new PFs, r(j)n , and the number of

measurements or equivalently new PFs, M (j)n , given the state

of the mobile agent, xn, and the vector of augmented statesof legacy PFs, y(j)

n (cf. [42])

p(c(j)n , r(j)

n ,M (j)n

∣∣xn, y(j)n

)= χ

c(j)n , r

(j)n ,M

(j)n

Ψ(c(j)n

)( ∏m∈N

r(j)n

Γc

(j)n

(r(j)m,n

))

×

( ∏k∈D

c(j)n ,r

(j)n

P(j)d

(xn,a

(j)k,n

))

×∏

k′∈Dc(j)n ,r

(j)n

[1(c(j)k′,n

)− r(j)

k′,nP(j)d

(xn, a

(j)k′,n

)], (6)

with χc

(j)n , r

(j)n ,M

(j)n

, e−(µ(j)FA +µ(j)

n,n) (µ(j)n,n)

∣∣Nr(j)n

∣∣×(µ

(j)FA )

M(j)n −

∣∣Dc(j)n ,r

(j)n

∣∣−∣∣Nr(j)n

∣∣/M

(j)n !. Here N

r(j)n

denotes the

set of existing new PFs, i.e., Nr

(j)n

,{m ∈M(j)

n : r(j)m,n=1

};

Dc

(j)n ,r

(j)n

denotes the set of existing legacy PFs for PA j, i.e.,

Dc

(j)n ,r

(j)n

,{k ∈ K(j)

n−1 : r(j)k,n = 1, c

(j)k,n 6= 0

}; and D

c(j)n ,r

(j)n

, K(j)n−1

∖Dc

(j)n ,r

(j)n

. Furthermore, Ψ(c

(j)n

)is defined to be 0 if

there exist k, k′∈K(j)n−1 with k 6=k′ such that c(j)k,n= c

(j)k′,n 6= 0,

and to be 1 otherwise; and Γc

(j)n

(r

(j)m,n

)is defined to be 0

if r(j)m,n = 1 and there exists k ∈ K(j)

n−1 such that c(j)k,n = m,and to be 1 otherwise. Finally, 1(c) denotes the indicatorfunction of the event c = 0 (i.e., 1(c) = 1 if c = 0 and 0

otherwise). The functions Ψ(c

(j)n

)and Γ

c(j)n

(r

(j)m,n

)enforce

our DA assumption from Section III-B, i.e., Ψ(c

(j)n

)enforces

p(c

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)= 0 if any measurement is associ-

ated with more than one legacy PF, and Γc

(j)n

(r

(j)m,n

)enforces

p(c

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)= 0 if a new PF is associated with

a measurement m that is also associated with a legacy PF.At time n = 1, prior information on PAs and VAs can beincorporated by introducing legacy PFs. Alternatively, thereare no legacy PFs at n= 1, i.e., y(j)

1 is an empty vector, andthus p

(c

(j)1 , r

(j)1 ,M

(j)1

∣∣x1, y(j)1

)= p

(c

(j)1 , r

(j)1 ,M

(j)1

∣∣x1

). An

expression of this pmf can be obtained by replacing in (6)(for n = 1) all factors involving y

(j)1 (or, equivalently, a

(j)1

and r(j)1 ) by 1.

The states of newly detected features are assumed a prioriindependent and identically distributed (iid) according to somepdf fn,n

(a

(j)m,n

∣∣xn), to be discussed in Section VI-A3. Theprior pdf of the states of new PFs for PA j, a(j)

n , conditionedon xn, r(j)

n , and M (j)n is then obtained as

f(a(j)n

∣∣xn, r(j)n ,M (j)

n

)=

( ∏m∈N

r(j)n

fn,n(a(j)m,n

∣∣xn))

×∏

m′∈Nr(j)n

fD(a

(j)m′,n

), (7)

where Nr

(j)n

, M(j)n

∖N

r(j)n

. Note that before the measure-

ments are obtained, M (j)n and, thus, the length of the vectors

a(j)n and r

(j)n (which is M (j)

n ) is random.We assume that for the agent state at time n=1, x1, an in-

formative prior pdf f(x1) is available. We also assume that thenew PF state vector a(j)

n and the DA vector c(j)n are condition-

ally independent given the legacy PF state vector a(j)n . Let us

next consider xn′ , yn′ , cn′ , and mn′ ,[M

(1)n′ · · ·M

(J)n′

]Tfor

all time steps n′= 1, . . . , n, and accordingly define the vectorx1:n,

[xT

1 · · · xTn

]Tand similarly y1:n , c1:n , and m1:n . We

then obtain the joint prior pdf as

f(x1:n,y1:n, c1:n,m1:n

)= f(x1)

(J∏

j′=1

f(y

(j′)1 , c

(j′)1 ,M

(j′)1

∣∣x1

)) n∏n′=2

f(xn′ |xn′−1)

×J∏j=1

(K(j)n−1∏k=1

f(y

(j)k,n′

∣∣y(j)k,n′−1

))f(a

(j)n′

∣∣xn′ , r(j)n′ ,M

(j)n′

)× p(c

(j)n′ , r

(j)n′ ,M

(j)n′

∣∣xn′ , y(j)n′

), (8)

where f(y

(j)1 , c

(j)1 ,M

(j)1

∣∣x1

)= p

(c

(j)1 , r

(j)1 ,M

(j)1

∣∣x1

)×f(a

(j)1

∣∣x1, r(j)1 ,M

(j)1

)(see (6) and (7)), and where the last

three factors in (8) are given by (4) and (5); (7); and (6),respectively.

D. Likelihood FunctionThe conditional pdf f

(z

(j)m,n

∣∣xn,a(j)k,n

)characterizing the

statistical relation between the measurements z(j)m,n and

the states xn and a(j)k,n depends on the concrete mea-

surement model; an example will be considered in Sec-tion VI-A2. This pdf is a central element in the conditional

6

pdf of the total measurement vector zn ,[z

(1)Tn · · · z(J)T

n

]T

given xn, yn, yn, cn, and mn. Assuming that the z(j)n

are conditionally independent across j given xn, yn, yn,cn, and mn [34], we obtain f(zn|xn, yn, yn, cn,mn) =∏Jj=1 f

(z

(j)n

∣∣xn, y(j)n , y

(j)n , c

(j)n ,M

(j)n

), where [34]

f(z(j)n

∣∣xn, y(j)n , y(j)

n , c(j)n ,M (j)

n

)=

(M(j)n∏

m=1

fFA(z(j)m,n

)) ∏k∈D

c(j)n ,r

(j)n

f(z

(j)

c(j)k,n,n

∣∣∣xn, a(j)k,n

)fFA

(z

(j)

c(j)k,n,n

)

×∏

m′∈Nr(j)n

f(z

(j)m′,n

∣∣xn, a(j)m′,n

)fFA(z

(j)m′,n

) . (9)

In particular, at n = 1, f(z

(j)1

∣∣x1, y(j)1 , y

(j)1 , c

(j)1 ,M

(j)1

)=

f(z

(j)1

∣∣x1, y(j)1 , c

(j)1 ,M

(j)1

). An expression of this pdf can be

obtained by replacing in (9) (for n = 1) all factors involvingy

(j)1 (or, equivalently, a(j)

1 and r(j)1 ) by 1.

Let us consider f(z

(j)n

∣∣xn, y(j)n , y

(j)n , c

(j)n ,M

(j)n

)as a likeli-

hood function, i.e., a function of xn, y(j)n , y(j)

n , c(j)n , and M (j)

n ,for observed z

(j)n . If z(j)

n is observed and therefore fixed, alsoM

(j)n is fixed, and we can rewrite (9), up to a constant factor,

asf(z(j)n

∣∣xn, y(j)n , y(j)

n , c(j)n ,M (j)

n

)∝

(K(j)n∏

k=1

g1

(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n; z(j)

n

))

×∏

m∈Nr(j)n

f(z

(j)m,n

∣∣xn, a(j)m,n

)fFA(z

(j)m,n

) . (10)

Here, g1

(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n; z

(j)n

)is defined as

g1

(xn, a

(j)k,n, 1, c

(j)k,n; z(j)

n

),

f(z

(j)m,n

∣∣xn, a(j)k,n

)fFA(z

(j)m,n

) , c(j)k,n=m ∈M(j)

n

1 , c(j)k,n= 0

(11)

and g1

(xn, a

(j)k,n, 0, c

(j)k,n; z

(j)n

), 1. Finally, the likelihood

function for z1:n,[zT

1 · · · zTn

]T, involving the measurements

z(j)m,n′ of all PAs j = 1, . . . , J and all time steps n′=1, . . . , n,

can be derived similarly to (8); one obtains

f(z1:n|x1:n,y1:n,c1:n,m1:n)

∝J∏j=1

( ∏m∈N

r(j)1

f(z

(j)m,1

∣∣x1, a(j)m,1

)fFA(z

(j)m,1

) )

×n∏

n′=2

( ∏m′∈N

r(j)n

f(z

(j)m′,n′

∣∣xn′ , a(j)m′,n′

)fFA(z

(j)m′,n′

) )

×K

(j)

n′∏k=1

g1

(xn′ , a

(j)k,n′ , r

(j)k,n′ , c

(j)k,n′ ; z

(j)n′

). (12)

IV. JOINT POSTERIOR PDF AND FACTOR GRAPH

A. Redundant Formulation of the Exclusion Constraint

The proposed BP-SLAM algorithm relies on a redundantformulation of probabilistic DA in terms of c(j)

n and b(j)n [40],

[42]. To obtain a probabilistic description and, in turn, a fac-tor graph, we formally replace the exclusion constraint factorΨ(c

(j)n

)involved in the prior pmf in (6) by

Ψ(c(j)n , b(j)

n

),

K(j)n∏

k=1

M(j)n∏

m=1

ψ(c(j)k,n , b

(j)m,n

),

where ψ(c(j)k,n , b

(j)m,n

)is defined to be 0 if either c(j)k,n=m and

b(j)m,n 6= k or b(j)m,n= k and c(j)k,n 6=m, and 1 otherwise. The re-

sulting modified prior pmf p(c

(j)n , b

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)is

related to the original prior pmf p(c

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)in (6) acording to p

(c

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)=∑

b(j)np(c

(j)n , b

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

). Here, the summation

is over all b(j)n ∈

{0, 1, . . . ,K

(j)n

}M(j)n , where {•}M(j)

n = {•}×{•} × . . .×{•} denotes the M (j)

n -fold Cartesian product.Let us now consider the product of the likelihood function

f(z

(j)n

∣∣xn, y(j)n , y

(j)n , c

(j)n ,M

(j)n

)in (10), the prior pdf of the

new PF states f(a

(j)n

∣∣xn, r(j)n ,M

(j)n

)in (7), and the modified

prior pmf p(c

(j)n , b

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)(cf. (6)). We obtain

f(z(j)n

∣∣xn, y(j)n , y(j)

n , c(j)n ,M (j)

n

)f(a(j)n

∣∣xn, r(j)n ,M (j)

n )

× p(c(j)n , b(j)

n , r(j)n ,M (j)

n

∣∣xn, y(j)n

)∝ Ψ

(c(j)n , b(j)

n

)(K(j)n∏

k=1

g(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n; z(j)

n

))

×

( ∏m∈N

r(j)n

µ(j)n,nfn,n

(a

(j)m,n

∣∣xn)Γc(j)n

(r

(j)m,n

)f(z

(j)m,n

∣∣xn, a(j)m,n

)fFA(z

(j)m,n

) )

×

( ∏m′∈N

r(j)n

fD(a

(j)m′,n

)). (13)

Here, g(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n; z

(j)n

), g1

(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n;

z(j)n

)g2

(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n;M

(j)n

), where g1

(xn, a

(j)k,n, r

(j)k,n,

c(j)k,n; z

(j)n

)was defined in and below (11) and g2

(xn, a

(j)k,n,

r(j)k,n, c

(j)k,n;M

(j)n

)is defined as

g2

(xn, a

(j)k,n, 1, c

(j)k,n;M (j)

n

)

,

P

(j)d

(xn, a

(j)k,n

)µ

(j)FA

, c(j)k,n∈M

(j)n

1−P (j)d

(xn, a

(j)k,n

), c

(j)k,n= 0

and g2

(xn, a

(j)k,n, 0, c

(j)k,n;M

(j)n

), 1

(c(j)k,n

). One thus obtains

for g(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n; z

(j)n

)g(xn, a

(j)k,n, 1, c

(j)k,n; z(j)

n

)

=

P

(j)d

(xn, a

(j)k,n

)f(z

(j)m,n

∣∣xn, a(j)k,n

)µ

(j)FA fFA

(z

(j)m,n

) , c(j)k,n=m ∈M(j)

n

1−P (j)d

(xn, a

(j)k,n

), c

(j)k,n= 0

7

and g(xn, a

(j)k,n, 0, c

(j)k,n; z

(j)n

)= 1

(c(j)k,n

). For a choice

of c(j)n and b

(j)n that is valid in the sense that

p(c

(j)n , b

(j)n , r

(j)n ,M

(j)n

∣∣xn, y(j)n

)6= 0, the exclusion constraint

expressed by Γc

(j)n

(r

(j)m,n

)is satisfied, i.e., Γ

c(j)n

(r

(j)m,n

)= 1, if

and only if both b(j)m,n = 0 and r

(j)m,n = 1. Therefore, in (13),

we can summarize the products over all m∈Nr

(j)n

and over all

m′∈Nr

(j)n

by a product over all m ∈M(j)n = {1, . . . ,M (j)

n }.More specifically, we can rewrite (13) as

f(z(j)n

∣∣xn, y(j)n , y(j)

n , c(j)n ,M (j)

n

)f(a(j)n

∣∣xn, r(j)n ,M (j)

n )

× p(c(j)n , b(j)

n , r(j)n ,M (j)

n

∣∣xn, y(j)n

)∝ Ψ

(c(j)n , b(j)

n

)(K(j)n−1∏k=1

g(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n; z(j)

n

))

×M(j)

n∏m=1

h(xn, a

(j)m,n, r

(j)m,n, b

(j)m,n; z(j)

n

), (14)

where h(xn, a

(j)m,n, r

(j)m,n, b

(j)m,n; z

(j)n

)is defined as

h(xn, a

(j)m,n, 1, b

(j)m,n; z(j)

n

),

0 , b

(j)m,n∈K(j)

n−1

µ(j)n,nfn,n

(a

(j)m,n

∣∣xn)f(z(j)m,n

∣∣xn, a(j)m,n

)µ

(j)FA fFA

(z

(j)m,n

) , b(j)m,n = 0

and h(xn, a

(j)m,n, 0, b

(j)m,n; z

(j)n

), fD

(a

(j)m,n

). The mean num-

ber µ(j)n,n and the conditional pdf fn,n

(a

(j)m,n

∣∣xn) of newly de-tected features can either be pre-specified (e.g., constant withrespect to time n and with a spatial distribution that is uni-form in a

(j)m,n) or inferred online by means of a probability

hypothesis density (PHD) filter, as described in [37].

B. Joint Posterior pdf

Using Bayes’ rule and independence assumptions related tothe state-transition pdfs (see Section III-A), the prior pdfs (seeSection III-C), and the likelihood model (see Section III-D),the joint posterior pdf of xn′ , yn′ , yn′ , cn′ , bn′ , and mn′ forall n′ = 1, . . . , n is obtained as (by using (3), (12), and (8))

f(x1:n,y1:n, c1:n, b1:n,m1:n|z1:n)

∝ f(z1:n|x1:n,y1:n, c1:n,m1:n)f(x1:n,y1:n, c1:n,m1:n)

= f(x1)

(J∏

j′=1

f(z

(j′)1

∣∣x1, y(j′)1 , c

(j′)1 ,M

(j′)1

)× f

(y

(j′)1 , c

(j′)1 ,M

(j′)1

∣∣x1

)) n∏n′=2

f(xn′ |xn′−1)

×J∏j=1

(K(j)n−1∏k=1

f(y

(j)k,n′

∣∣y(j)k,n′−1

))× f

(z

(j)n′

∣∣xn′ , y(j)n′ , y

(j)n′ , c

(j)n′ ,M

(j)n′

)f(a

(j)n′

∣∣xn′ , r(j)n′ ,M

(j)n′

)× p(c

(j)n′ , b

(j)n′ , r

(j)n′ ,M

(j)n′

∣∣xn′ , y(j)n′

). (15)

After inserting expression (14) and performing some simplemanipulations, Eq. (15) can be reformulated as

f(x1:n,y1:n, c1:n, b1:n,m1:n|z1:n)

∝ f(x1)

(J∏

j′=1

M(j′)1∏

m′=1

h(x1, a

(j′)m′,1, r

(j′)m′,1, b

(j′)m′,1; z

(j′)1

))

×n∏

n′=2

f(xn′ |xn′−1)

J∏j=1

Ψ(c

(j)n′ , b

(j)n′

)

×

(K(j)

n′−1∏k=1

f(y

(j)k,n′

∣∣y(j)k,n′−1

)g(xn′ , a

(j)k,n′ , r

(j)k,n′ , c

(j)k,n′ ; z

(j)n′

))

×M

(j)

n′∏m=1

h(xn′ , a

(j)m,n′ , r

(j)m,n′ , b

(j)m,n′ ; z

(j)n′

). (16)

This factorization is represented by the factor graph [38], [39]shown in Fig. 2.

V. THE BP-SLAM ALGORITHM

The proposed BP-SLAM algorithm performs Bayesian de-tection and estimation of the relevant states. The required pos-terior distributions are calculated in an efficient, time-recursivemanner by using BP message passing [39] on the factor graphin Fig. 2.

A. Detection and Estimation

Our goal is to estimate the agent state xn and to detectand estimate the PF states a

(j)k,n from the total measurement

vector z1:n. For estimating xn, we will develop an approxi-mate calculation of the minimum mean-square error (MMSE)estimator [54]

xMMSEn ,

∫xnf(xn|z1:n)dxn . (17)

This estimator involves the posterior pdf f(xn|z1:n). Fur-thermore, detecting (i.e., determining the existence of) PFk ∈ K(j)

n at time n is based on the posterior existenceprobability p

(r

(j)k,n = 1

∣∣z1:n

). This probability can be ob-

tained from the posterior pdf of the augmented PF state,f(y

(j)k,n

∣∣z1:n

)= f(a

(j)k,n , r

(j)k,n

∣∣z1:n

), by a marginalization, i.e.,

p(r

(j)k,n=1

∣∣z1:n

)=

∫f(a

(j)k,n , r

(j)k,n=1

∣∣z1:n

)da

(j)k,n . (18)

Then PF k is defined to be detected at time n if p(r

(j)k,n =

1∣∣z1:n

)>Pdet, where Pdet is a detection threshold. The states

a(j)k,n of the detected PFs are finally estimated as

a(j)MMSEk,n ,

∫a

(j)k,n f

(a

(j)k,n

∣∣r(j)k,n=1, z1:n

)da

(j)k,n , (19)

where

f(a

(j)k,n

∣∣r(j)k,n=1, z1:n

)=f(a

(j)k,n , r

(j)k,n=1

∣∣z1:n

)p(r

(j)k,n=1

∣∣z1:n

) . (20)

The posterior existence probabilities p(r

(j)k,n = 1

∣∣z1:n

)in

(18) are also used in a different context. To prevent an indef-inite increase of the total number of PFs for PA j due to (2),i.e., K(j)

n =K(j)n−1 +M

(j)n , a pruning of the PFs is employed.

More specifically, PF k is retained only if p(r

(j)k,n = 1

∣∣z1:n

)exceeds a suitably chosen pruning threshold Pprun.

8

time n j = J

j = 1

x

y1

y1

yK

y1

y1

yM

f

f1

f1

fK

q− qq

q1

q1

qM

q−1

q−1

q1

q1

q−K

q−K

qK

α

α1

α1

αK

g1

gK

h1

h1

hM

γ1

γ1

γK

φ1

φ1

φM

γ11

γJ1

γ1K

γJK

β1

βK

η1

ηK

ξ1

ξ1

ξM

ς1

ς1

ςM

c1

cK

b1

bM

ψ1,1

ψ1,M

ψK,1

ψK,M

ν1,1

νM,1

ν1,K

ζ1,1

ζ1,M

ζK,1

νM,K ζK,M

legacy PF

new PF

loopy BP DA

Fig. 2: Factor graph representing the factorization of the joint posterior pdf in (16). The subgraphs corresponding to individual PAs are indicatedby boxes with light yellow background. All the factor nodes, variable nodes, and messages related to the agent state are represented in redcolor, those related to the legacy PF states are represented by the blue parts contained in dashed boxes, those related to the new PF states arerepresented by the magenta parts contained in dotted boxes, and those related to loopy BP DA are represented by the green parts contained indashed-dotted boxes. The following short notations are used: K ,K

(j)n , M ,M

(j)n , x, xn, yk , y

(j)k,n, ym , y

(j)m,n, bm , b

(j)m,n, ck , c

(j)k,n,

ψk,m , ψ(c(j)k,n, b

(j)m,n

), gk , g

(xn, a

(j)k,n, r

(j)k,n, c

(j)k,n;z

(j)n

), hm , h

(xn, a

(j)m,n, r

(j)m,n, b

(j)m,n;z

(j)n

), f , f(xn|xn−1), fk , f

(y(j)k,n

∣∣y(j)k,n−1

),

α , α(xn), αk , αk

(a(j)k,n, r

(j)k,n

), q− , q(xn−1), q , q(xn), q−k , q

(a(j)k,n−1, r

(j)k,n−1

), qk , q

(a(j)k,n, r

(j)k,n

), qm , q

(a(j)m,n, r

(j)m,n

),

βk , β(c(j)k,n

), ξm , ξ

(b(j)m,n

), ηk , η

(c(j)k,n

), ςm , ς

(b(j)m,n

), νm,k , ν

(p)m→k

(c(j)k,n

), ζk,m , ζ

(p)k→m

(b(j)m,n

), γj

k , γ(j)k (xn), γk , γ

(a(j)k,n, r

(j)k,n

),

and φm , φ(a(j)m,n, r

(j)m,n

).

B. Message Passing Algorithm

The posterior pdfs f(xn|z1:n) and f(a

(j)k,n , r

(j)k,n

∣∣z1:n

)in-

volved in (17)–(20) are marginal pdfs of the joint posteriorpdf f(x1:n,y1:n, c1:n, b1:n,m1:n|z1:n) in (16). Because a di-rect marginalization is infeasible, we use loopy (iterative) BPmessage passing [39] on the factor graph in Fig. 2. Since thefactor graph contains loops, the resulting beliefs are only ap-proximations of the respective posterior pdfs, and there is nocanonical order in which the messages should be computed[39]. In our method, we choose the order according to the fol-lowing rules: (i) Messages are not passed backward in time;(ii) iterative message passing is only performed for DA, andonly for each time step and for each PA separately (i.e., inparticular, for the loops connecting different PAs, we only per-form a single message passing iteration); (iii) along an edgeconnecting an agent state variable node and a new PF statevariable node, messages are only sent from the former to thelatter. The resulting BP message passing algorithm is presentedin what follows; cf. also the underlying factor graph in Fig. 2.We note that similarly to the “dummy pdfs” introduced in Sec-tion III-A, we consider BP messages ϕ

(y

(j)k,n

)=ϕ

(a

(j)k,n , r

(j)k,n

)also for the non-existing PF states, i.e., for r(j)

k,n= 0. We definethese messages by setting ϕ

(a

(j)k,n , 0

)= ϕ

(j)k,n (note that these

messages are not pdfs and thus are not required to integrate

to 1).First, a prediction step is performed. The prediction message

for the agent state is given by

α(xn) =

∫f(xn|xn−1)q(xn−1)dxn−1 , (21)

and the prediction message for the legacy PFs is given by

αk(a

(j)k,n , r

(j)k,n

)=

∑r(j)k,n−1∈{0,1}

∫f(a

(j)k,n , r

(j)k,n

∣∣a(j)k,n−1 , r

(j)k,n−1

)× q(a

(j)k,n−1 , r

(j)k,n−1

)da

(j)k,n−1 , (22)

k∈K(j)k−1, where the beliefs of the mobile agent state, q(xn−1),

and of the PF states, q(a

(j)k,n−1 , r

(j)k,n−1

), were calculated

at the preceding time n − 1. Inserting (4) and (5) forf(a

(j)k,n , r

(j)k,n

∣∣a(j)k,n−1 , r

(j)k,n−1 = 1

)and f

(a

(j)k,n , r

(j)k,n

∣∣a(j)k,n−1 ,

r(j)k,n−1 = 0

), respectively, we obtain for r(j)

k,n=1

αk(a

(j)k,n , 1

)= Ps

∫f(a

(j)k,n

∣∣a(j)k,n−1

)q(a

(j)k,n−1 , 1

)da

(j)k,n−1 ,

(23)and for r(j)

k,n= 0

α(j)k,n = (1−Ps

)∫q(a

(j)k,n−1 , 1

)da

(j)k,n−1 + q

(j)k,n−1 , (24)

9

where α(j)k,n ,

∫αk(a

(j)k,n , 0

)da

(j)k,n and q

(j)k,n−1 ,∫

q(a

(j)k,n−1 , 0

)da

(j)k,n−1 .

After the prediction step, the following calculations are per-formed for all legacy PFs k ∈ K(j)

n−1 and for all new PFsm∈M(j)

n , for all PAs j ∈ {1, . . . , J} in parallel:

1) Measurement evaluation for legacy PFs: The messagesβ(c(j)k,n

)passed to the variable nodes corresponding to

the feature-oriented DA variables c(j)k,n (cf. Fig. 2) are

calculated as

β(c(j)k,n

)=

∫∫αk(a

(j)k,n, 1

)α(xn)g

(xn, a

(j)k,n, 1, c

(j)k,n; z(j)

n

)× dxnda

(j)k,n + 1

(c(j)k,n

)α

(j)k,n . (25)

2) Measurement evaluation for new PFs: The messagesξ(b(j)m,n

)passed to the variable nodes corresponding to

the measurement-oriented DA variables b(j)m,n are calcu-lated as

ξ(b(j)m,n

)=

∑r(j)m,n∈{0,1}

∫∫h(xn, a

(j)m,n, r

(j)m,n, b

(j)m,n; z(j)

n

)× α(xn) dxnda(j)

m,n . (26)

Using the expression of h(xn, a

(j)m,n, r

(j)m,n, b

(j)m,n; z

(j)n

)stated in Section IV-A, Eq. (26) is easily seen to sim-plify to ξ

(b(j)m,n

)= 1 for b(j)m,n ∈K(j)

n−1, and for b(j)m,n = 0it becomes

ξ(b(j)m,n

)= 1 +

µ(j)n,n

µ(j)FA fFA

(z

(j)m,n

) ∫∫ α(xn)fn,n(a(j)m,n

∣∣xn)× f

(z(j)m,n

∣∣xn, a(j)m,n

)dxnda(j)

m,n . (27)

3) Iterative data association: Next, from β(c(j)k,n

)and

ξ(b(j)m,n

), messages η

(c(j)k,n

)and ς

(b(j)m,n

)are obtained by

means of loopy (iterative) BP. First, for each measurementm∈M(j)

n , messages ν(p)m→k

(c(j)k,n

)and ζ

(p)k→m

(b(j)m,n

)are

calculated iteratively according to [40], [42]

ν(p)m→k

(c(j)k,n

)=

K(j)n−1∑

b(j)m,n=0

ξ(b(j)m,n

)ψ(c(j)k,n, b

(j)m,n

)×

∏k′∈K(j)

n−1\{k}

ζ(p−1)k′→m

(b(j)m,n

)(28)

ζ(p)k→m

(b(j)m,n

)=

M(j)n∑

c(j)k,n=0

β(c(j)k,n

)ψ(c(j)k,n, b

(j)m,n

)×

∏m′∈M(j)

n \{m}

ν(p)m′→k

(c(j)k,n

), (29)

for k ∈K(j)n−1, m∈M(j)

n , and iteration index p= 1, . . . ,P . The recursion defined by (28) and (29) is initial-ized (for p = 0) by ζ

(0)k→m

(b(j)m,n

)=∑M(j)

n

c(j)k,n=0

β(c(j)k,n

)

×ψ(c(j)k,n, b

(j)m,n

). Then, after the last iteration p= P, the

messages η(c(j)k,n

)and ς

(b(j)m,n

)are calculated as

η(c(j)k,n

)=

∏m∈M(j)

n

ν(P )m→k

(c(j)k,n

)(30)

ς(b(j)m,n

)=

∏k∈K(j)

n−1

ζ(P )k→m

(b(j)m,n

). (31)

4) Measurement update for the agent: From η(c(j)k,n

),

αk(a

(j)k,n, 1

), and α

(j)k,n , the message γ(j)

k (xn) related tothe agent is calculated as

γ(j)k (xn) =

M(j)n∑

c(j)k,n=0

η(c(j)k,n

)∫g(xn, a

(j)k,n, 1, c

(j)k,n; z(j)

n

)× αk

(a

(j)k,n, 1

)da

(j)k,n + η

(c(j)k,n=0

)α

(j)k,n . (32)

5) Measurement update for legacy PFs: Similarly, the mes-sages γ

(a

(j)k,n, r

(j)k,n

)related to the legacy PFs are calcu-

lated as

γ(a

(j)k,n, 1

)=

M(j)n∑

c(j)k,n=0

η(c(j)k,n

)∫g(xn, a

(j)k,n, 1, c

(j)k,n; z(j)

n

)× α(xn)dxn (33)

γ(j)k,n , γ

(a

(j)k,n, 0

)= η

(c(j)k,n=0

). (34)

6) Measurement update for new PFs: Finally, the messagesφ(a

(j)m,n, r

(j)m,n

)related to the new PFs are calculated as

φ(a(j)m,n, 1

)= ς

(b(j)m,n=0

)∫h(xn, a

(j)m,n, 1, 0; z(j)

n

)× α(xn)dxn (35)

φ(j)m,n , φ

(a

(j)k,n, 0

)=

K(j)n−1∑

b(j)m,n=0

ς(b(j)m,n

). (36)

Once these messages are available, the beliefs approximat-ing the desired marginal posterior pdfs are obtained. The belieffor the agent state is given, up to a normalization factor, by

q(xn) ∝ α(xn)

J∏j=1

∏k∈K(j)

n−1

γ(j)k (xn) . (37)

The belief q(xn) provides an approximation of the marginalposterior pdf f(xn|z1:n), and it is used instead of f(xn|z1:n)

in (17). Furthermore, the beliefs q(a

(j)k,n, r

(j)k,n

)for the aug-

mented states of the legacy PFs, y(j)k,n =

[a

(j)Tk,n r

(j)k,n

]T, are

calculated as

q(a

(j)k,n, 1

)∝ αk

(a

(j)k,n, 1

)γ(a

(j)k,n, 1

)(38)

q(j)k,n , q

(a

(j)k,n, 0

)∝ α

(j)k,nγ

(j)k,n , (39)

and the beliefs q(a

(j)m,n, r

(j)m,n

)for the augmented states of the

new PFs, y(j)m,n =

[a

(j)Tm,n r

(j)m,n

]T, as

q(a(j)m,n, 1

)∝ φ

(a(j)m,n, 1

)(40)

10

q(j)m,n , q

(a(j)m,n, 0

)∝ φ(j)

m,n . (41)

In particular, q(a

(j)k,n, 1

)and q

(a

(j)m,n, 1

)approximate the

marginal posterior pdf f(a

(j)k′,n, r

(j)k′,n = 1

∣∣z1:n

), where k′ ∈

K(j)n−1 ∪ M

(j)n (assuming an appropriate index mapping be-

tween k, m on the one hand and k′ on the other), and theyare used in (18)–(20).

The BP-SLAM algorithm is summarized in Algorithm 1.A flowchart is available online at https://www2.spsc.tugraz.at/people/eriklei/BP-SLAM/ and also at https://gitlab.com/erikleitinger/BP-Multipath-basedSLAM. A computationallyfeasible sequential Monte Carlo (particle-based) implementa-tion can be obtained via the approach in [42], [55]. In our case,the sequential Monte Carlo implementation uses a “stackedstate” [55] comprising the agent state and the PF states. Theresulting complexity scales only linearly in the number of par-ticles. MATLAB code for this particle-based implementationis available online at the addresses indicated above.

ALGORITHM 1: BP-SLAM (RECURSION FROM n−1 TO n)

Step 1—Prediction:Calculate α(xn) from q(xn−1) according to (21), and calculate

αk

(a(j)k,n , 1

)and α(j)

k,n from q(a(j)k,n−1, r

(j)k,n−1

), k ∈ K(j)

n−1 , j ∈ {1,. . . , J} according to (23), (24).

Step 2—Measurement evaluation:For legacy PFs: Calculate β

(c(j)k,n

), k ∈ K(j)

n−1 , j ∈ {1, . . . , J}according to (25).

For new PFs: Calculate ξ(b(j)m,n

), m ∈M(j)

n , j ∈ {1, . . . , J} ac-cording to (26), (27).

Step 3—Iterative data association:Calculate η

(c(j)k,n

), k ∈ K(j)

n−1 , j ∈ {1, . . . , J} and ς(b(j)m,n

),

m∈M(j)n , j ∈ {1, . . . , J} according to (28)–(31).

Step 4—Measurement update:For the mobile agent: Calculate γ(j)

k (xn), k ∈K(j)n−1 , j ∈ {1, . . . ,

J} according to (32).For legacy PFs: Calculate γ

(a(j)k,n, 1

)and γ(j)

k,n, k ∈K(j)n−1 , j ∈ {1,

. . . , J} according to (33), (34).For new PFs: Calculate φ

(a(j)m,n, 1

)and φ(j)

m,n, m∈M(j)n , j ∈ {1,

. . . , J} according to (35), (36).

Step 5—Belief calculation:For the mobile agent: Calculate q(xn) according to (37).For legacy PFs: Calculate q

(a(j)k,n, 1

)and q(j)k,n, k ∈K(j)

n−1, j ∈ {1,. . . , J} according to (38), (39).

For new PFs: Calculate q(a(j)m,n, 1

)and q(j)m,n, m∈M(j)

n , j ∈ {1,. . . , J} according to (40), (41).

Step 6—Pruning:Determine the set K(j)

n = K(j)n−1 ∪M

(j)n .

For all j ∈ {1, . . . , J}, reinterpret/reindex the beliefs q(a(j)

k′,n, 1)

and q(j)

k′,n, k′ ∈ K(j)n−1 and the beliefs q

(a(j)m,n, 1

)and q

(j)m,n,

m∈M(j)n as beliefs q

(a(j)k,n, 1

)and q(j)k,n of the PFs k ∈ K(j)

n .

For all j ∈ {1, . . . , J}, calculate estimates p(r(j)k,n=1

∣∣z1:n

)of the

existence probabilities p(r(j)k,n= 1

∣∣z1:n

), k ∈ K(j)

n according to (18)with f

(a(j)k,n , r

(j)k,n=1

∣∣z1:n

)replaced by q

(a(j)k,n, 1

).

For all j ∈ {1, . . . , J}, determine the set K(j)n of PFs k for which

p(r(j)k,n=1

∣∣z1:n

)>Pprun.

Step 7—Detection and estimation:Calculate an agent state estimate xn according to (17) with

f(xn|z1:n) replaced by q(xn).For all j ∈ {1, . . . , J}, determine the set K(j)

n of PFs k ∈ K(j)n

for which p(r(j)k,n= 1

∣∣z1:n

)> Pdet, where Pdet is chosen larger than

Pprun.For all j ∈ {1, . . . , J} and k ∈ K(j)

n , calculate a PF state esti-mate a

(j)k,n according to (19) and (20) with f

(a(j)k,n , r

(j)k,n = 1

∣∣z1:n

)replaced by q

(a(j)k,n, 1

).

Initialization:The recursive algorithm is initialized at time n=1 by f(x1) andK(j)

1 = ∅, j ∈ {1, . . . , J}.

VI. EXPERIMENTAL RESULTS

To analyze the performance of the proposed BP-SLAM al-gorithm, we apply it to synthetic and real measurement datawithin two-dimensional (2-D) scenarios. The parameters in-volved in the algorithm and those used to generate the syn-thetic measurements are listed in Table I.

A. Analysis Setup

1) State-Evolution Model: The agent’s state-transition pdff(xn|xn−1), with xn = [pT

n vTn]T, is defined by a linear,

near constant-velocity motion model [56, Sec. 6.3.2], i.e.,xn = Axn−1 +Bwn. Here, A∈R4×4 and B ∈R4×2 are asdefined in [56, Sec. 6.3.2] (with sampling period ∆T = 1s),and the driving process wn is iid across n, zero-mean, andGaussian with covariance matrix σ2

wI2, where I2 denotes the2×2 identity matrix. The PFs are static, i.e., the state-transitionpdfs are given by f

(a

(j)k,n

∣∣a(j)k,n−1

)= δ(a

(j)k,n−a

(j)k,n−1

), where

δ(·) is the Dirac delta function. However, in our implementa-tion of the BP-SLAM algorithm, we introduced a small driv-ing process in the PF state-evolution model for the sake ofnumerical stability. Accordingly, the state evolution is mod-eled as a

(j)k,n=a

(j)k,n−1 +ω

(j)k,n, where ω

(j)k,n is iid across k, n,

and j, zero-mean, and Gaussian with covariance matrix σ2aI2.

2) Measurement Model: In contrast to usual SLAM setups[1], our measurement model is solely based on MPC ranges.The scalar range measurements z(j)

m,n are modeled as

z(j)m,n =

∥∥pn−a(j)k,n

∥∥+ ν(j)m,n , (42)

where the measurement noise ν(j)m,n is iid across m, n, and j,

zero-mean, and Gaussian with variance σ(j)2m,n. The measure-

ment model (42) determines the likelihood function factorsf(z

(j)m,n

∣∣xn, a(j)k,n

)and f

(z

(j)m,n

∣∣xn, a(j)m,n

)in (9). However, we

emphasize that the BP-SLAM algorithm can be extended tomeasurement models involving bearing measurements (AoAsand/or AoDs) or measurements from inertial measurement unitsensors. Such an extension would further increase the robust-ness of our approach.

11

TABLE I: Simulation parameters.

Parameters involved in the BP-SLAM algorithm

σw σa σa,1 σm µ(j)FA µ

(j)b µ

(j)n,1 Ps Pd Pdet Pprun #particles #simulation runs

0.01m/s2 10−4 m, 0.5·10−2 m 10−3 m 0.15m 1, 2 10−4 6 0.999 0.95, 0.5, 0.6 0.5 10−4 105, 3·104 100, 30

Parameters used to generate the synthetic measurements

µ(j)FA Pd σ

(j)m,n

1, 2 0.95, 0.5 0.1m

3) Common Simulation Parameters: The following param-eters are used for both synthetic and real measurements, seealso Table I. We use the floor plan, agent trajectory, and twostatic PAs at positions a

(1)1 and a

(2)1 as shown in Fig. 1. The

false alarm pdf fFA(z

(j)m,n

)is uniform on [0m, 30m] with mean

number µ(j)FA . The conditional pdf fn,n

(a

(j)m,n

∣∣xn) and meannumber µ(j)

n,n of newly detected features are inferred onlineby a PHD filter using a birth pdf fb,n

(a

(j)m,n

∣∣xn) and a meannumber of newborn features µ(j)

b (see [37]). The birth pdf isuniform on the region of interest (ROI), which is a circulardisk of radius 30m around the center of the floor plan shownin Fig. 1. At time n= 1, newly detected features are initial-ized by setting µ(j)

n,1 = 6 and using an initial pdf fn,1(a

(j)m,1

∣∣x1

)that is uniform on the ROI. The detection probability is a con-stant value for all PFs and PAs, i.e. P (j)

d

(xn,a

(j)k,n

)=Pd. The

values of these and further parameters are given in Table I.Our implementation of the BP-SLAM algorithm uses a par-

ticle representation of messages and beliefs similarly to [42],[55]. The particles for the initial PA states are drawn fromthe 2-D Gaussian distributions N

(a

(j)1,1, σ

2a,1I2

), where a

(j)1,1 is

the position of PA j∈{1, 2} and σa,1 = 10−3 m. This impliesthat the initial PA positions are effectively known. On the otherhand, we do not use any prior information about the VA states.The particles for the initial agent state are drawn from a 4-Duniform distribution with center x1 = [pT

1 0 0]T, where p1 isthe starting position of the actual agent trajectory, and the sup-port of each component about the respective center is given by[−λ, λ]. Here, λ is 0.5 except for our simulations in SectionVI-B2, where it is 0.1; the physical dimension of λ is m (posi-tion) or m/s (velocity). We note that the BP-SLAM algorithmperforms well even without any prior information about theinitial states of the mobile agent and the PAs. However, be-cause we use only range measurements, the estimated featuremap and agent trajectory would contain an arbitrary translationand rotation relative to the true positions. Finally, the num-ber P of message passing iterations for DA is limited by thetermination condition

[∑k∈K(j)

n−1

∑m∈M(j)

n

(ν

(p)m→k(c

(j)k,n) −

ν(p−1)m→k (c

(j)k,n)

)2]1/2< 10−7 (cf. (28)) or by the maximum num-

ber Pmax =1000.

B. Results for Synthetic Measurements

For our simulations based on synthetic measurements, weused the common simulation parameters described above.The range measurements were generated with noise stan-dard deviation σ

(j)m,n = 0.1m. However, for numerical robust-

ness, the BP-SLAM algorithm used noise standard deviation

σm = 1.5 · σ(j)m,n. The standard deviation of the driving pro-

cess in the PF state-evolution model was σa = 10−4 m. Weperformed 100 simulation runs, each using the floor plan andagent trajectory shown in Fig. 1. In each run, we generatedwith detection probability Pd noisy ranges z(j)

m,n according to(42). Evaluation of (42) was based on the agent positionsalong the agent trajectory, the fixed PA positions a

(1)1 , a(2)

1 ,and the fixed positions of the first-order VAs, i.e., a(j)

l ∈ R2,l = 2, . . . , L

(j)n for j = 1, 2 (all shown in Fig. 1), where

L(1)n = 6 and L

(2)n = 5 are the numbers of features (PA plus

associated VAs). In addition, false alarm measurements z(j)m,n

were generated as described below.1) Comparison of Different Parameter Settings: We con-

sidered three different parameter settings dubbed SLAM 1,SLAM 2, and SLAM 3. In SLAM 1 and SLAM 2, we used de-tection probabilityPd = 0.95 and mean number of false alarmsµ

(j)FA = 1. The posterior pdfs of the agent state, of the legacy

PF states, and of the new PF states were each represented by100.000 (SLAM 1) or 30.000 (SLAM 2) particles. (The num-ber of particles could be reduced if, e.g., the MPCs’ AoAswere used in addition to the range measurements since thiswould decrease the effective support regions of the posteriorpdfs, or by adaptively adjusting the detection probability Pd

as in [33].) In SLAM 3, we used Pd = 0.5 and µ(j)FA =2 to an-

alyze the robustness of the BP-SLAM algorithm to extremelypoor radio signal conditions, i.e., to scenarios characterizedby a high probability that existing MPCs are not detected ornonexisting MPCs are detected; furthermore, the agent stateand the PF states were each represented by 100.000 particles.For one exemplary simulation run, Fig. 3 illustrates the con-vergence of the posterior pdfs of the PF positions to the truefeature (PA and VA) positions by displaying the respective par-ticles at times n=30, 90, 300, 900. These results demonstratethat the BP-SLAM algorithm is able to cope with highly mul-timodal distributions and with measurements conveying onlyvery limited information at each time step (since only rangemeasurements are used).

Fig. 4 shows the root mean square error (RMSE) of the es-timated time-varying agent position, the average numbers ofdetected PFs for the two PAs, and the mean optimal subpat-tern assignment (MOSPA)1 errors [57] for the two PAs andthe associated VAs, all versus time n. These results were ob-tained by averaging over the 100 simulation runs. The MOSPA

1We remark that an alternative to the “classical” OSPA metric [57] is pro-vided by the generalized OSPA metric proposed in [58], which does not nor-malize the OSPA error by the cardinality of the larger set and penalizes thecardinality error differently.

12

x in m

yin

m

0

0

5

5

−5

−5

10

10

15

15

(a)

x in my

inm

0

0

5

5

−5

−5

10

10

15

15

(b)

x in m

yin

m

0

0

5

5

−5

−5

10

10

15

15

(c)

x in m

yin

m

0

0

5

5

−5

−5

10

10

15

15

(d)

Fig. 3: Particle convergence. Particles representing the posterior pdfs of the state of the mobile agent (gray) and of the states of the detectedPFs (red for PA 1, blue for PA 2) are shown at (a) n= 30, (b) n= 90, (c) n= 300, and (d) n= 900. The positions of PA 1 and PA 2 areindicated by a red bullet and a blue box, respectively, and the corresponding geometrically expected VA positions by red circles and bluesquares. The green line represents the MMSE estimates of the past mobile agent positions and the green cross the MMSE estimate of thecurrent mobile agent position. The black crosses indicate the MMSE estimates of the positions of the detected PFs.

100 300 500 700 900

known PAs/VAs

known PAs, only PAs

SLAM 1SLAM 2SLAM 3

n

RM

SEin

m

00

0.05

0.10

0.15

0.20

0.25

0.30

(a)

00 100 300 500 700 900

known agent positionSLAM 1SLAM 2SLAM 3

n

Av.

no.o

fde

tect

edPF

s

2

4

6

(b)

00 100 300 500 700 900


n

Av.

no.o

fde

tect

edPF

s

2

4

6

(c)

00 100 300 500 700 900


n

MO

SPA

erro

rin

m

0.5

1.0

1.5

2.0

(d)

00 100 300 500 700 900


n

MO

SPA

erro

rin

m

0.5

1.0

1.5

2.0

(e)

Fig. 4: Results for synthetic measurements: (a) Agent position RMSE, (b) and (c) average number of detected PFs associated with PA 1and 2, respectively, (d) MOSPA error for PA 1 and the associated VAs, and (e) MOSPA error for PA 2 and the associated VAs. In (a), thecurve labeled “known PAs/VAs” shows the comparison to the benchmark algorithm that assumes knowledge of the feature map, i.e., of thePA and VA positions, and the curve labeled “known PAs, only PAs” shows the comparison to the benchmark algorithm using only the twoPA positions, i.e., no VA positions. In (b)–(e), the curves labeled “known agent position” show the case where the agent position is known.In (b) and (c), the curves labeled “known agent position,” “SLAM 1,” and “SLAM 2” coincide. The vertical dash-dotted lines indicate thetimes around which the mobile agent performs turns.

errors are based on the Euclidean metric and use cutoff param-eter 5m and order 1 [57]. It can be seen in Fig. 4(a) that theagent position RMSE is mostly below 0.12m for all parame-ter settings and below 0.08m for SLAM 1 and SLAM 2. Theaverage numbers of detected PFs in Figs. 4(b), (c) are effec-tively equal to the respective true numbers of features L(1)

n = 6

and L(2)n = 5 for SLAM 1 and SLAM 2. The MOSPA errors

are shown in Figs. 4(d), (e). For SLAM 1, they decrease un-til they are ultimately below 0.11m for both PAs. In general,the agent position RMSE and MOSPA errors for SLAM 2 andSLAM 3 are slightly larger. However, the performance of our

algorithm is still good, which suggests a high robustness. Wenote that the agent position RMSE converged for all simula-tion runs and all parameter settings. Furthermore, we observedthat the position RMSEs of the individual PFs (not shown inFig. 4) are at most 0.25m and in many cases around 0.05m.

As a performance benchmark for the accuracy of agent lo-calization, we also plot in Fig. 4(a) the agent position RMSEobtained for SLAM 1 with the algorithm from [9] (referredto as “benchmark algorithm”). This algorithm uses BP-basedprobabilistic DA just as our BP-SLAM algorithm but assumesknowledge of the feature map, i.e., of the PA and VA posi-

13

tions. It is seen that its agent position RMSE (labeled “knownPAs/VAs” in Fig. 4(a)) is significantly lower than that ofthe BP-SLAM algorithm, which demonstrates the impact ofmap uncertainty on the performance of SLAM. In addition,Fig. 4(a) shows the agent position RMSE obtained for SLAM1 with the benchmark algorithm using only the two PA posi-tions, i.e., no VA positions (labeled “known PAs, only PAs”in Fig. 4(a)). Here, the measurement vector z

(j)n was prepro-

cessed such that only the measurement with the shortest rangewas detected and used by the algorithm. Since for the chosenPA positions the mobile agent position cannot be estimated un-ambiguously, it is not possible to determine the agent positionRMSE without a significant bias. Therefore, to be able to de-termine the agent position RMSE along the true trajectory, weapplied genie-aided k-means clustering of the particles. Onecan see that the agent position RMSE of the benchmark algo-rithm now tends to be considerably higher, especially aroundthe second turn of the trajectory where the agent crosses theline between the PAs (cf. Fig. 1).

In addition, as a performance benchmark for the accuracy offeature map estimation, we compare in Figs. 4(b)–(e) the av-erage number of detected PFs and the MOSPA errors to thosethat would be obtained for the SLAM 1 parameter setting ifthe agent position was known at all times. These two bench-marks provide bounds on the two main performance aspectsof SLAM, i.e., accuracy in localization and mapping.

Finally, we considered the use of measurement gating,which is commonly employed in practical implementations toreduce computational complexity [34, Sec. 2.3.2]. With mea-surement gating, DA is performed only on those measurementsthat fall into given “gates” around predicted measurement val-ues. We chose the gating threshold as γ = 6.635, which im-plies that the probability that feature-originated measurementsare outside their corresponding gate is 10−2 [34, Table 2.3.2-1]. For SLAM 2, we measured the average runtime per timestep n, averaged over the 900 time steps and 100 simulationruns, as 0.0929s without measurement gating and 0.0735s withmeasurement gating. Thus, measurement gating yielded a re-duction of the average runtime by 20.1%, and this did notcome at the cost of a noticeable performance loss in terms ofagent position RMSE or MOSPA error. These results were ob-tained using a MATLAB implementation on an Intel i5-4690CPU. We note that besides measurement gating, another po-tential means of reducing complexity is the use of geometricdata structures such as kd-trees [59]. For example, kd- treeswere used in the RB-SLAM (FastSLAM) algorithm reportedin [20]. However, in the particle-based implementation of theproposed BP-SLAM algorithm, due to the necessity of per-forming probabilistic DA, the weights representing the agentstate and the feature states are updated by a weighted sum ofmeasurements instead of a single measurement (as is done inestablished particle-based RB-SLAM algorithms, even whenMonte Carlo-based DA is used [1], [20]). Due to this differ-ence, to the best of our knowledge, geometric data structuressuch as kd-trees cannot be directly applied to SLAM withprobabilistic DA.

2) Comparison with Rao-Blackwellized SLAM: Next, wecompare the accuracy and complexity (runtime) of our BP-

SLAM algorithm with those of RB-SLAM [2], [13], [20],which is an important state-of-the-art method. We note thatRB-SLAM can be motivated by the idea of stacking the agentstate and all the feature states into one high-dimensional jointstate and using a particle filter to track the joint state. A directimplementation of this idea is typically infeasible due to thecurse of dimensionality [1], which is further exacerbated byour range-only measurement model. Indeed, even the 100.000particles used in simulation setup SLAM 1 would be totallyinsufficient to represent the joint state vector. The RB-SLAMalgorithm avoids the curse of dimensionality by exploiting theconditional independencies of the feature states (conditionallyon the agent state) and applying a Rao-Blackwellization to thejoint state [1, Sec. 13], [20].

Because our measurement model is highly nonlinear, weadopt the RB-SLAM implementation proposed in [13], whichemploys particle filters instead of EKFs to estimate the featurestates. (Note that we only implemented the SLAM algorithmproposed in [13], not the full two-stage method that includesa channel estimator/tracker.) Since the measurement-featureassociation is unknown, we perform Monte Carlo-based DAfor each particle separately, as it is commonly done in classi-cal RB-SLAM [2], [20]. The classical RB-SLAM algorithm isvery sensitive to missed detections and clutter measurements,especially for our range-only measurement model, which im-plies a strong DA uncertainty. Therefore, to avoid the risk ofdivergence in RB-SLAM, we assumed in our simulation thatthere are no missed detections or false alarms, i.e., we gener-ated the data using Pd =1 and µ(j)

FA =0.Fig. 5 shows the RMSE of the estimated agent position and

the MOSPA errors for the two PAs and the associated VAs. ForBP-SLAM, we considered three different parameter settings,dubbed BP-SLAM A–C, in which each posterior state pdf (forthe agent state, each legacy PF state, and each new PF state)was represented by 50.000 (BP-SLAM A), 10.000 (BP-SLAMB), or 5.000 (BP-SLAM C) particles. For RB-SLAM, we con-sidered five different parameter settings, dubbed RB-SLAM 1–5, in which the posterior pdf of the agent state and the posteriorpdf of each feature state were represented by, respectively, 50and 10.000 (RB-SLAM 1), 100 and 5.000 (RB-SLAM 2), 100and 10.000 (RB-SLAM 3), 500 and 1.000 (RB-SLAM 4), or1.000 and 1.000 (RB-SLAM 5) particles. In Fig. 5(a), it can beseen that the agent position RMSEs obtained with BP-SLAMA–C and with RB-SLAM 1 are generally quite similar. On theother hand, Figs. 5(b) and (c) show that, as may be expected,the MOSPA error of BP-SLAM is smaller for a larger num-ber of particles. The MOSPA error of RB-SLAM 1 is seen tobe only slightly smaller than that of BP-SLAM B (which is agood basis for a comparison, because also RB-SLAM 1 uses10.000 particles for each feature state). This is remarkable,in view of the fact that BP-SLAM assumes a priori indepen-dence of the agent state and all the feature states. BP-SLAMA even has a significantly smaller MOSPA error than RB-SLAM. Figs. 5(d)–(f) compare the RMSE and MOSPA errorresults for the different RB-SLAM parameter settings; theseresults will be discussed presently.

Table II shows the average runtimes per time step n (aver-aged over 900 time steps and 100 simulation runs) of our MAT-

14

100 300 500 700 900

BP-SLAM ABP-SLAM BBP-SLAM CRB-SLAM 1

n

RM

SEin

m

00

0.02

0.04

0.06

0.08

0.10

(a)

100 300 500 700 900


n

MO

SPA

erro

rin

m

0.5

0.4

0.3

0.2

0.1

00

(b)

100 300 500 700 900


n

MO

SPA

erro

rin

m

0.5

0.4

0.3

0.2

0.1

00

(c)

100 300 500 700 900

RB-SLAM 1RB-SLAM 2RB-SLAM 3RB-SLAM 4RB-SLAM 5

n

RM

SEin

m

00

0.05

0.10

0.15

0.20

(d)

100 300 500 700 900


n

MO

SPA

erro

rin

m

0.5

0.4

0.3

0.2

0.1

00

(e)

100 300 500 700 900


n

MO

SPA

erro

rin

m

0.5

0.4

0.3

0.2

0.1

00

(f)

Fig. 5: Results for synthetic measurements; comparison between BP-SLAM and RB-SLAM [2], [13], [20]: (a) Agent position RMSE, (b)MOSPA error for PA 1 and the associated VAs, and (c) MOSPA error for PA 2 and the associated VAs, all for BP-SLAM A–C and RB-SLAM1; (d)–(f) comparison of agent position RMSE and PA/VA MOSPA error results for RB-SLAM 1–5. The vertical dash-dotted lines indicatethe times around which the mobile agent performs turns.

TABLE II: Average runtimes of BP-SLAM A–C and RB-SLAM 1–5per time step.

BP-SLAM A BP-SLAM B BP-SLAM C0.146s 0.037s 0.028s

RB-SLAM 1 RB-SLAM 2 RB-SLAM 3 RB-SLAM 4 RB-SLAM 50.47s 0.62s 1.26s 0.65s 1.61s

LAB implementations of the algorithms on an Intel i5-4690CPU. It is seen that BP-SLAM is significantly less complexthan RB-SLAM 1; in particular, the runtime of BP-SLAM B—i.e., using the same number of particles as RB-SLAM 1, lead-ing to only slightly poorer performance as observed above—issmaller by a factor of more than 10. This is because RB-SLAMcalculates the distances of each agent state particle to each fea-ture state particle. Consequently, the complexity of RB-SLAMscales linearly in the product of the number of agent parti-cles and the total number of feature particles; by contrast, thecomplexity of BP-SLAM scales linearly in the number of allparticles. One can conclude that the slightly better accuracy ofRB-SLAM relative to BP-SLAM B comes at the expense of amuch highercomplexity. We note that the complexity of RB-SLAM algorithms that use particles to represent the featurestates can be reduced by the adaptive resampling algorithmpresented in [27].

From these runtime results and from Figs. 5(d)–(f), the fol-lowing further conclusions can be drawn: (i) Increasing thenumber of agent particles in RB-SLAM increases the accu-racy of agent state and VA position estimation only slightlybut increases the computational complexity significantly. (ii)Decreasing the number of VA particles in RB-SLAM decreasesthe accuracy of agent state and VA position estimation signif-icantly and may even lead to a divergence of the agent state

since the VA particles converge to wrong positions. (Here,we consider a simulation run to be divergent if the errorrises above 30 cm and continues to increase. Note that theagent position RMSE and the PA/VA MOSPA error plotted inFigs. 5(d)–(f) were calculated using only the converged simu-lation runs for the respective parameter setting. For parametersettings RB-SLAM 1–3, all simulation runs converged. Forparameter settings RB-SLAM 4 and RB-SLAM 5, only 73 %and 87 %, respectively, of the simulation runs converged.)

C. Results for Real Measurements

For an evaluation of the performance of the proposedBP-SLAM algorithm using real measurements, we choseP

(j)d

(xn,a

(j)k,n

)= Pd = 0.6 and µ

(j)FA = 2. This accounts for

the lower detection probability and higher false alarm proba-bility exhibited by the preliminary channel estimation due tothe diffuse multipath existing in indoor environments.

The measurements were taken from the seminar room sce-nario previously used in [33], [60]. They correspond to fiveclosely spaced parallel trajectories each consisting of 900agent positions with a spacing of 0.01m along each trajectoryand a spacing of 0.01m between the trajectories, resulting ina total number of 4500 agent positions. The magenta line inFig. 1 represents one of the five trajectories. More details aboutthe measurements are provided in [60]; in particular, a close-upof the five trajectories is shown in [60, Fig. 1]. At each agentposition, the agent transmitted an ultra-wideband signal, whichwas received by the two static PAs. This signal was measuredusing an M-sequence correlative channel sounder with fre-quency range 3–10 GHz and antennas with an approximatelyuniform radiation pattern in the azimuth plane and zeros inthe floor and ceiling directions. Within the measured band,

15

RMSEs for individual trajectoriesaverage RMSE

n

RM

SEin

m

00 100 300 500 700 900

0.05

0.15

0.10

(a)

RMSEs for individual trajectoriesRMSE for all trajectories

RMSE in m

CD

F

00 0.05 0.150.10 0.20 0.25

0.2

0.4

0.6

0.8

1.0

(b)

Fig. 6: Results for real measurements: (a) Agent position RMSEsfor the five individual trajectories and average RMSE (averaged overthe five trajectories), (b) empirical CDFs of the RMSEs for the fiveindividual trajectories and empirical CDF of the five RMSEs takentogether.

the actual signal band was selected by a filter with raised-cosine impulse response s(t) with a roll-off factor of 0.5, atwo-sided 3-dB bandwidth of 2 GHz, and a center frequencyof 7 GHz. From the measured signals, the range measurementsz

(j)m,n = cτ

(j)m,n constituting the input to the proposed algorithm

were derived by means of a snapshot-based SISO SAGE al-gorithm [28] that provides estimates τ (j)

m,n of the delays of theMPCs and estimates of the associated complex amplitudes (cf.(1)). In this method, the number of estimated MPCs for eachPA j was fixed to M (j)

n =20. Estimates of the range variancesσ

(j)2m,n (cf. (42)) were determined from the estimated complex

amplitudes as described in [33], [60]. The standard deviationof the driving process in the PF state-evolution model was cho-sen as σa = 0.5 · 10−2 m. We performed 30 simulation runs.The pdfs of the states were represented by 30.000 particleseach. Each individual trajectory was processed independently,i.e., the estimated PF positions of a trajectory were not usedas prior knowledge for another trajectory.

Fig. 6(a) shows the agent position RMSEs of BP-SLAMobtained individually for the five trajectories versus time n,along with the overall RMSE averaged over the five trajec-tories. Fig. 6(b) shows the empirical cumulative distributionfunction (CDF) of the individual RMSEs and the empiricalCDF of the five RMSEs taken together. It can be seen that theindividual CDFs are very close to 1 already at RMSE equal to0.12m or even less. The maximum of all the individual RM-SEs is below 0.2m in all cases and below 0.083m in 90% ofall cases.2

For an exemplary simulation run, Fig. 7 depicts the particlesrepresenting the posterior pdfs of the states of the detected

2In [33], lower RMSE values (below 0.072m in all cases and below 0.035min 90% of all cases) were obtained by using an adaptive adjustment of thedetection probability Pd in the SLAM algorithm.

x in m

yin

m

−5

0

0

5

5

10

10

15

Fig. 7: Results for real measurements: Particles representing the pos-terior pdfs of the states of the detected PFs (red for PA 1, blue forPA 2). The positions of PA 1 and PA 2 are indicated by a red bul-let and a blue box, respectively, and the corresponding geometricallyexpected VA positions by red circles and blue squares. The greenline represents the MMSE estimates of the mobile agent positions,the last of which is indicated by a green cross. The black crosses in-dicate the MMSE estimates of the positions of the detected PFs. Thedashed circles indicate PF positions that cannot be associated withgeometrically expected VA positions.

PFs as well as the MMSE estimates of the positions of thedetected PFs at the current time instant for the five trajectories.Furthermore, Fig. 7 depicts the MMSE estimates of the pastand current mobile agent positions. Almost all the estimatedPF positions can be associated with geometrically expectedVA positions. This shows that the BP-SLAM algorithm is ableto leverage position-related information contained in the radiosignals for accurate and robust localization.

VII. CONCLUSIONS AND FUTURE PERSPECTIVES

We proposed a radio signal based SLAM algorithm withprobabilistic DA. The underlying system model describesspecular MPCs in terms of VAs with unknown and possiblytime-varying positions. To tackle the DA problem, i.e., theunknown association of MPCs with VAs, we modeled the en-tire SLAM problem including probabilistic DA in a Bayesianframework. We then represented the factorization of the jointposterior distribution by a factor graph and applied BP for ap-proximate marginalization of the joint posterior distribution.This approach allowed the incorporation of an efficient BP al-gorithm for probabilistic DA that was originally proposed forMTT [40], [42]. Our factor graph extends that of [33] by thestates of new potential features.

Simulation results using synthetic data showed that the pro-posed BP-SLAM algorithm estimates the time-varying agentposition and the feature map with high accuracy and robust-ness, even in conditions of strong clutter and low probability ofdetection. Moreover, an experimental analysis using real ultra-wideband radio signals in an indoor environment showed thatthe BP-SLAM algorithm performs similarly well in real-worldscenarios; the agent position error was observed to be below

16

0.2m for 100% and below 0.083m for 90% of all measure-ments.

Promising directions for future research are to exploit fur-ther MPC parameters such as AoAs and AoDs, to include ad-ditional types of features such as scatter points, and to redefinethe features to be extended objects. Finally, studying opera-tion in an unsynchronized sensor network and a distributed(decentralized) mode of operation would be interesting.

REFERENCES

[1] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge,MA, USA: MIT Press, 2006.

[2] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and map-ping: Part I,” IEEE Robot. Autom. Mag., vol. 13, no. 2, pp. 99–110, Jun.2006.

[3] G. Bresson, Z. Alsayed, L. Yu, and S. Glaser, “Simultaneous localizationand mapping: A survey of current trends in autonomous driving,” IEEETrans. Intell. Veh., vol. 2, no. 3, pp. 194–220, Sep. 2017.

[4] R. Di Taranto, S. Muppirisetty, R. Raulefs, D. Slock, T. Svensson, andH. Wymeersch, “Location-aware communications for 5G networks: Howlocation information can improve scalability, latency, and robustness of5G,” IEEE Signal Process. Mag., vol. 31, no. 6, pp. 102–112, Nov. 2014.

[5] K. Witrisal, P. Meissner, E. Leitinger, Y. Shen, C. Gustafson, F. Tufves-son, K. Haneda, D. Dardari, A. F. Molisch, A. Conti, and M. Z. Win,“High-accuracy localization for assisted living: 5G systems will turnmultipath channels from foe to friend,” IEEE Signal Process. Mag.,vol. 33, no. 2, pp. 59–70, Mar. 2016.

[6] S. Bartoletti, A. Conti, A. Giorgetti, and M. Z. Win, “Sensor radar net-works for indoor tracking,” IEEE Wireless Commun. Letters, vol. 3,no. 2, pp. 157–160, Apr. 2014.

[7] D. Dardari, P. Closas, and P. M. Djuric, “Indoor tracking: Theory, meth-ods, and technologies,” IEEE Trans. Veh. Technol., vol. 64, no. 4, pp.1263–1278, Apr. 2015.

[8] F. Guidi, A. Guerra, and D. Dardari, “Personal mobile radars withmillimeter-wave massive arrays for indoor mapping,” IEEE Trans. Mo-bile Comput., vol. 15, no. 6, pp. 1471–1484, Jun. 2016.

[9] E. Leitinger, F. Meyer, P. Meissner, K. Witrisal, and F. Hlawatsch, “Be-lief propagation based joint probabilistic data association for multipath-assisted indoor navigation and tracking,” in Proc. ICL-GNSS-16,Barcelona, Spain, Jun. 2016, pp. 1–6.

[10] H. Wymeersch, S. Marano, W. M. Gifford, and M. Z. Win, “A machinelearning approach to ranging error mitigation for UWB localization,”IEEE Trans. Commun., vol. 60, no. 6, pp. 1719–1728, Jun. 2012.

[11] Y. Shen, S. Mazuelas, and M. Z. Win, “Network navigation: Theoryand interpretation,” IEEE J. Sel. Areas Commun., vol. 30, no. 9, pp.1823–1834, Oct. 2012.

[12] M. Z. Win, Y. Shen, and W. Dai, “A theoretical foundation of networklocalization and navigation,” Proc. IEEE, vol. 106, no. 7, pp. 1136–1165, Jul. 2018, special issue on Foundations and Trends in LocalizationTechnologies.

[13] C. Gentner, T. Jost, W. Wang, S. Zhang, A. Dammann, and U. C. Fiebig,“Multipath assisted positioning with simultaneous localization and map-ping,” IEEE Trans. Wireless Commun., vol. 15, no. 9, pp. 6104–6117,Sep. 2016.

[14] E. Leitinger, P. Meissner, C. Rudisser, G. Dumphart, and K. Witrisal,“Evaluation of position-related information in multipath components forindoor positioning,” IEEE J. Sel. Areas Commun., vol. 33, no. 11, pp.2313–2328, Nov. 2015.

[15] A. Guerra, F. Guidi, J. Dall’Ara, and D. Dardari, “Occupancy grid map-ping for personal radar applications,” in Proc. IEEE SSP-18, Freiburg,Germany, June 2018, pp. 766–770.

[16] H. Zhang and S. Y. Tan, “TOA based indoor localization and tracking viasingle-cluster PHD filtering,” in Proc. IEEE GLOBECOM-17, Singapore,Dec. 2017, pp. 1–6.

[17] J. Borish, “Extension of the image model to arbitrary polyhedra,” JASA,vol. 75, no. 6, pp. 1827–1836, Mar. 1984.

[18] M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, andM. Csorba, “A solution to the simultaneous localization and map build-ing (SLAM) problem,” IEEE Trans. Robot. Autom., vol. 17, no. 3, pp.229–241, Jun. 2001.

[19] J. Mullane, B.-N. Vo, M. Adams, and B.-T. Vo, “A random-finite-setapproach to Bayesian SLAM,” IEEE Trans. Robot., vol. 27, no. 2, pp.268–282, Apr. 2011.

[20] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM: Afactored solution to the simultaneous localization and mapping problem,”in Proc. AAAI-02, Edmonton, Canda, Jul. 2002, pp. 593–598.

[21] M. Lundgren, L. Svensson, and L. Hammarstrand, “Variational Bayesianexpectation maximization for radar map estimation,” IEEE Trans. SignalProcess., vol. 64, no. 6, pp. 1391–1404, Mar. 2016.

[22] M. Fatemi, L. Svensson, L. Hammarstrand, and M. Lundgren, “Varia-tional Bayesian EM for SLAM,” in Proc. IEEE CAMSAP-15, Cancun,Mexico, Dec. 2015, pp. 501–504.

[23] H. Deusch, S. Reuter, and K. Dietmayer, “The labeled multi-BernoulliSLAM filter,” IEEE Signal Process. Lett., vol. 22, no. 10, pp. 1561–1565, Oct. 2015.

[24] M. Fatemi, K. Granstrom, L. Svensson, F. J. R. Ruiz, and L. Ham-marstrand, “Poisson multi-Bernoulli mapping using Gibbs sampling,”IEEE Trans. Signal Process., vol. 65, no. 11, pp. 2814–2827, Jun. 2017.

[25] E. Leitinger, P. Meissner, M. Lafer, and K. Witrisal, “Simultaneous lo-calization and mapping using multipath channel information,” in Proc.IEEE ICCW-15, London, UK, Jun. 2015, pp. 754–760.

[26] M. Zhu, J. Vieira, Y. Kuang, K. Astrom, A. F. Molisch, and F. Tufvesson,“Tracking and positioning using phase information from estimated multi-path components,” in Proc. IEEE ICCW-15, London, UK, Jun. 2015, pp.712–717.

[27] C. Gentner, R. Pohlmann, M. Ulmschneider, T. Jost, and S. Zhang, “Po-sitioning using terrestrial multipath signals and inertial sensors,” MobileInformation Systems, vol. 2017, p. 18, Oct. 2017.

[28] B. H. Fleury, M. Tschudin, R. Heddergott, D. Dahlhaus, and K. I. Peder-sen, “Channel parameter estimation in mobile radio environments usingthe SAGE algorithm,” IEEE J. Sel. Areas Commun., vol. 17, no. 3, pp.434–450, Mar. 1999.

[29] A. Richter, “Estimation of Radio Channel Parameters,” Ph.D. disserta-tion, Ilmenau University of Technology, 2005.

[30] D. Shutin and B. H. Fleury, “Sparse variational Bayesian SAGE algo-rithm with application to the estimation of multipath wireless channels,”IEEE Trans. Signal Process., vol. 59, no. 8, pp. 3609–3623, Aug. 2011.

[31] J. Salmi, A. Richter, and V. Koivunen, “Detection and tracking of MIMOpropagation path parameters using state-space approach,” IEEE Trans.Signal Process., vol. 57, no. 4, pp. 1538–1550, Apr. 2009.

[32] M. A. Badiu, T. L. Hansen, and B. H. Fleury, “Variational Bayesianinference of line spectra,” IEEE Trans. Signal Process., vol. 65, no. 9,pp. 2247–2261, May 2017.

[33] E. Leitinger, F. Meyer, F. Tufvesson, and K. Witrisal, “Factor graphbased simultaneous localization and mapping using multipath channelinformation,” in Proc. IEEE ICC-17, Paris, France, Jun. 2017, pp. 652–658.

[34] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion: AHandbook of Algorithms. Storrs, CT, USA: Yaakov Bar-Shalom, 2011.

[35] D. Musicki and R. Evans, “Joint integrated probabilistic data association:JIPDA,” IEEE Trans. Aerosp. Electron. Syst., vol. 40, no. 3, pp. 1093–1099, Jul. 2004.

[36] P. Horridge and S. Maskell, “Searching for, initiating and tracking mul-tiple targets using existence probabilities,” in Proc. FUSION-09, Seattle,WA, USA, Jul. 2009, pp. 611–617.

[37] ——, “Using a probabilistic hypothesis density filter to confirm tracksin a multi-target environment,” in Proc. INFORMATIK-11, Berlin, Ger-many, Jul. 2011, pp. 1–12.

[38] H.-A. Loeliger, “An introduction to factor graphs,” IEEE Signal Process.Mag., vol. 21, no. 1, pp. 28–41, Jan. 2004.

[39] F. Kschischang, B. Frey, and H.-A. Loeliger, “Factor graphs and thesum-product algorithm,” IEEE Trans. Inf. Theory, vol. 47, no. 2, pp.498–519, Feb. 2001.

[40] J. L. Williams and R. A. Lau, “Approximate evaluation of marginalassociation probabilities with belief propagation,” IEEE Trans. Aerosp.Electron. Syst., vol. 50, no. 4, pp. 2942–2959, Oct. 2014.

[41] F. Meyer, T. Kropfreiter, J. L. Williams, R. A. Lau, F. Hlawatsch,P. Braca, and M. Z. Win, “Message passing algorithms for scalablemultitarget tracking,” Proc. IEEE, vol. 106, no. 2, pp. 221–259, Feb.2018.

[42] F. Meyer, P. Braca, P. Willett, and F. Hlawatsch, “A scalable algorithmfor tracking an unknown number of targets using multiple sensors,” IEEETrans. Signal Process., vol. 65, no. 13, pp. 3478–3493, Jul. 2017.

[43] G. Soldi, F. Meyer, P. Braca, and F. Hlawatsch, “Self-tuning algo-rithms for multisensor-multitarget tracking using belief propagation,”IEEE Trans. Signal Process., vol. 67, no. 15, pp. 3922–3937, Aug. 2019.

[44] J. L. Williams, “Marginal multi-Bernoulli filters: RFS derivation ofMHT, JIPDA, and association-based MeMBer,” IEEE Trans. Aerosp.Electron. Syst., vol. 51, no. 3, pp. 1664–1687, Jul. 2015.

17

[45] T. Kropfreiter, F. Meyer, and F. Hlawatsch, “Sequential Monte Carloimplementation of the track-oriented marginal multi-Bernoulli/Poissonfilter,” in Proc. FUSION-16, Heidelberg, Germany, Jul. 2016, pp. 972–979.

[46] A. D. Marrs, “Asynchronous multi-sensor tracking in clutter with uncer-tain sensor locations using Bayesian sequential Monte Carlo methods,”in Proc. IEEE Aerosp. Conf., vol. 5, Big Sky, MT, USA, Mar. 2001, pp.2171–2178.

[47] J. Blanco, J. Gonzalez, and J. Fernandez-Madrigal, “A pure probabilisticapproach to range-only SLAM,” in Proc. IEEE ICRA-08, Pasadena, CA,USA, May 2008, pp. 1436–1441.

[48] C. Gentner and T. Jost, “Indoor positioning using time difference ofarrival between multipath components,” in Proc. IPIN-13, Montbeliard-Belfort, France, Oct. 2013, pp. 1–10.

[49] B. Etzlinger, F. Meyer, F. Hlawatsch, A. Springer, and H. Wymeersch,“Cooperative simultaneous localization and synchronization in mobileagent networks,” IEEE Trans. Signal Process., vol. 65, no. 14, pp. 3587–3602, Jul. 2017.

[50] E. Leitinger, S. Grebien, X. Li, F. Tufvesson, and K. Witrisal, “On theuse of MPC amplitude information in radio signal based SLAM,” inProc. IEEE SSP-18, Freiburg, Germany, Jun. 2018, pp. 633–637.

[51] P. Meissner, “Multipath-Assisted Indoor Positioning,” Ph.D. dissertation,Graz University of Technology, 2014.

[52] T. Jost, W. Wang, U. Fiebig, and F. Perez-Fontan, “Detection andtracking of mobile propagation channel paths,” IEEE Trans. AntennasPropag., vol. 60, no. 10, pp. 4875–4883, Oct. 2012.

[53] J. Vermaak, S. J. Godsill, and P. Perez, “Monte Carlo filtering for multitarget tracking and data association,” IEEE Trans. Aerosp. Electron.Syst., vol. 41, no. 1, pp. 309–332, Jan. 2005.

[54] S. Kay, Fundamentals of Statistical Signal Processing: Estimation The-ory. Upper Saddle River, NJ, USA: Prentice Hall, 1993.

[55] F. Meyer, O. Hlinka, H. Wymeersch, E. Riegler, and F. Hlawatsch, “Dis-tributed localization and tracking of mobile networks including nonco-operative objects,” IEEE Trans. Signal Inf. Process. Netw., vol. 2, no. 1,pp. 57–71, Mar. 2016.

[56] Y. Bar-Shalom, T. Kirubarajan, and X.-R. Li, Estimation with Applica-tions to Tracking and Navigation. New York, NY, USA: Wiley, 2002.

[57] D. Schuhmacher, B.-T. Vo, and B.-N. Vo, “A consistent metric for perfor-mance evaluation of multi-object filters,” IEEE Trans. Signal Process.,vol. 56, no. 8, pp. 3447–3457, Aug. 2008.

[58] A. S. Rahmathullah, A. F. Garcıa-Fernandez, and L. Svensson, “Gen-eralized optimal sub-pattern assignment metric,” in Proc. FUSION-17,Xi’an, China, Jul. 2017, pp. 1–8.

[59] A. W. Moore, “Very fast EM-based mixture model clustering using mul-tiresolution kd-trees,” in Proc. NIPS-98, Denver, CO, USA, Dec. 1998,pp. 543–549.

[60] P. Meissner, E. Leitinger, and K. Witrisal, “UWB for robust indoor track-ing: Weighting of multipath components for efficient estimation,” IEEEWireless Comm. Lett., vol. 3, no. 5, pp. 501–504, Oct. 2014.

Erik Leitinger (S’12–M’16) received his Dipl.-Ing.(M.Sc. ) and Ph.D. degrees (with highest honors) inelectrical engineering from Graz University of Tech-nology, Austria in 2012 and 2016, respectively. Heis currently a Postdoctoral Associate at Graz Univer-sity of Technology. He was Postdoctoral Associate atthe department of Electrical and Information Tech-nology at Lund University from 2016 to 2018. Heis an Erwin Schrodinger Fellow.

Erik Leitinger received the outstanding award ofexcellence of the Federal Ministry of Science, Re-

search and Economy (BMWFW) for his Ph.D. Thesis. Dr. Leitinger servedas reviewer for several IEEE journals and as a TPC member in IEEE con-ferences. His research interests include localization and navigation, stochasticmodeling and estimation of radio channels, factor graphs and iterative mes-sage passing algorithms, and estimation/detection theory.

Florian Meyer (S’12–M’15) received the Dipl.-Ing.(M.Sc. ) and Ph.D. degrees in electrical engineer-ing from TU Wien, Vienna, Austria in 2011 and2015, respectively. He is currently a postdoctoralresearcher with the Laboratory for Information andDecision Systems, Massachusetts Institute of Tech-nology (MIT). He was a visiting researcher with theDepartment of Signals and Systems, Chalmers Uni-versity of Technology, Gothenburg, Sweden in 2013and with the NATO Centre for Maritime Researchand Experimentation (CMRE), La Spezia, Italy in

2014 and 2015. In 2016, he joined CMRE as a research scientist. Dr. Meyeris an Associate Editor of the Journal of Advances in Information Fusion andserved as a co-chair of the IEEE ANLN Workshop at IEEE ICC 2018, KansasCity, MO, USA and at IEEE ICC 2019, Shanghai, China. His research inter-ests include multiobject tracking, applied ocean sciences, network localizationand navigation, and multiagent systems. He is an Erwin Schrodinger Fellow.

Franz Hlawatsch (S’85–M’88–SM’00–F’12) re-ceived the Diplom-Ingenieur, Dr. techn. and Univ.-Dozent (habilitation) degrees in electrical engineer-ing/signal processing from TU Wien, Vienna, Aus-tria in 1983, 1988, and 1996, respectively. Since1983, he has been with the Institute of Telecommu-nications, TU Wien, where he is currently an Asso-ciate Professor. During 1991–1992, as a recipient ofan Erwin Schrodinger Fellowship, he spent a sab-batical year with the Department of Electrical Engi-neering, University of Rhode Island, Kingston, RI,

USA. In 1999, 2000, and 2001, he held one-month Visiting Professor posi-tions with INP/ENSEEIHT, Toulouse, France and IRCCyN, Nantes, France.He (co)authored a book, three review papers that appeared in the IEEE SIG-NAL PROCESSING MAGAZINE, about 200 refereed scientific papers and bookchapters, and three patents. He coedited three books. His research interestsinclude statistical and compressive signal processing methods and their appli-cation to inference and learning problems.

Dr. Hlawatsch was a member of the IEEE SPCOM Technical Commit-tee from 2004 to 2009. He was a Technical Program Co-Chair of EUSIPCO2004 and served on the technical committees of numerous IEEE conferences.He was an Associate Editor for the IEEE TRANSACTIONS ON SIGNAL PRO-CESSING from 2003 to 2007, the IEEE TRANSACTIONS ON INFORMATIONTHEORY from 2008 to 2011, and the IEEE TRANSACTIONS ON SIGNAL ANDINFORMATION PROCESSING OVER NETWORKS from 2014 to 2017. He coau-thored papers that won an IEEE Signal Processing Society Young Author BestPaper Award and a Best Student Paper Award at IEEE ICASSP 2011. He isa EURASIP Fellow.

Fredrik Tufvesson (S’97–M’04–SM’07–F’17) re-ceived his Ph.D. in 2000 from Lund University inSweden. After two years at a startup company, hejoined the department of Electrical and InformationTechnology at Lund University, where he is now pro-fessor of radio systems. His main research interest isthe interplay between the radio channel and the restof the communication system with various applica-tions in 5G systems such as massive MIMO, mmwave communication, vehicular communication andradio based positioning.

Fredrik has authored around 90 journal papers and 140 conference papers,he is fellow of the IEEE and recently he got the Neal Shepherd MemorialAward for the best propagation paper in IEEE Transactions on Vehicular Tech-nology and the IEEE Communications Society best tutorial paper award.

18

Klaus Witrisal (S’98–M’03) received the Ph.D. de-gree from the Delft University of Technology, TheNetherlands, in 2002 and the Habilitation degreefrom the Graz University of Technology, Austria,in 2009, where he is currently an Associate Profes-sor. His research interests are in signal processing forwireless communications, propagation channel mod-eling, and positioning.

He has been an Associate Editor of the IEEECOMMUNICATIONS LETTERS, a Co-Chair of theTWG “Indoor” of the COST Action IC1004 and the

EWG “Localisation and Tracking” of the COST Action CA15104, a leadingChair of the IEEE Workshop on Advances in Network Localization and Nav-igation, and TPC (Co)-Chair of the Workshop on Positioning, Navigation andCommunication.

Moe Z. Win (S’85-M’87-SM’97-F’04) is a Pro-fessor at the Massachusetts Institute of Technology(MIT) and the founding director of the Wireless In-formation and Network Sciences Laboratory. Priorto joining MIT, he was with AT&T Research Labo-ratories and NASA Jet Propulsion Laboratory.

His research encompasses fundamental theories,algorithm design, and network experimentation fora broad range of real-world problems. His current re-search topics include network localization and navi-gation, network interference exploitation, and quan-

tum information science. He has served the IEEE Communications Society asan elected Member-at-Large on the Board of Governors, as elected Chair ofthe Radio Communications Committee, and as an IEEE Distinguished Lec-turer. Over the last two decades, he held various editorial posts for IEEEjournals and organized numerous international conferences. Currently, he isserving on the SIAM Diversity Advisory Committee.

Dr. Win is an elected Fellow of the AAAS, the IEEE, and the IET. He washonored with two IEEE Technical Field Awards: the IEEE Kiyo TomiyasuAward (2011) and the IEEE Eric E. Sumner Award (2006, jointly with R. A.Scholtz). His publications, co-authored with students and colleagues, have re-ceived several awards. Other recognitions include the IEEE CommunicationsSociety Edwin H. Armstrong Achievement Award (2016), the InternationalPrize for Communications Cristoforo Colombo (2013), the Copernicus Fel-lowship (2011) and the Laurea Honoris Causa (2008) from the Universitadegli Studi di Ferrara, and the U.S. Presidential Early Career Award for Sci-entists and Engineers (2004). He is an ISI Highly Cited Researcher.

Documents

A Belief Propagation Algorithm for Multipath-Based SLAMIn multipath-assisted indoor localization [5], [8], [9], [13]– [16], the relation of multipath components (MPCs) with the local