IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

1019

A Multihypotheses Importance Density for SLAM in
Cluttered Scenarios
Ossi Kaltiokallio , Member, IEEE, Roland Hostettler , Member, IEEE, Yu Ge , Graduate Student Member, IEEE,
Hyowon Kim , Member, IEEE, Jukka Talvitie , Member, IEEE, Henk Wymeersch , Fellow, IEEE,
and Mikko Valkama , Fellow, IEEE

Abstract—One of the most fundamental problems in simultaneous localization and mapping (SLAM) is the ability to take
into account data association (DA) uncertainties. In this article,
this problem is addressed by proposing a multihypotheses sampling distribution for particle filtering-based SLAM algorithms.
By modeling the measurements and landmarks as random finite
sets, an importance density approximation that incorporates DA
uncertainties is derived. Then, a tractable Gaussian mixture model
approximation of the multihypotheses importance density is proposed, in which each mixture component represents a different DA.
Finally, an iterative method for approximating the mixture components of the sampling distribution is utilized and a partitioned
update strategy is developed. Using synthetic and experimental
data, it is demonstrated that the proposed importance density
improves the accuracy and robustness of landmark-based SLAM
in cluttered scenarios over state-of-the-art methods. At the sametime, the partitioned update strategy makes it possible to include
multiple DA hypotheses in the importance density approximation,
leading to a favorable linear complexity scaling, in terms of thenumber of landmarks in the field-of-view.
Index Terms—Importance density, particle filter (PF),probability hypotheses density, random finite set (RFS),
simultaneous localization and mapping (SLAM).

NOMENCLATURE
Major Notations of This ArticlekCurrent time index.
Pose of the robot at time k.
xk
Manuscript received 9 March 2023; revised 8 August 2023; accepted 31
October 2023. Date of publication 4 December 2023; date of current version 8
January 2024. This paper was recommended for publication by Associate Editor
M. Walter and Editor S. Behnke upon evaluation of the reviewers’ comments.
This work was supported in part by the Academy of Finland under Grant#328214, Grant #323244, Grant #319994, Grant #338224, Grant #345654, andGrant #352754, in part by the Finnish Funding Agency for Innovation underthe 6G-ISAC project, and in part by the Wallenberg AI, Autonomous Systems
and Software Program (WASP) through Knut and Alice Wallenberg Foundation,and the Chalmers Area of Advance Transport 6G-Cities project. (Corresponding
author: Ossi Kaltiokallio.)
Ossi Kaltiokallio, Jukka Talvitie, and Mikko Valkama are with the Unit ofElectrical Engineering, Tampere University, 33100 Tampere, Finland (e-mail:ossi.kaltiokallio@tuni.fi; jukka.talvitie@tuni.fi; mikko.valkama@tuni.fi).
Roland Hostettler is with the Department of Electrical Engineering, Uppsala University, 752 36 Uppsala, Sweden (e-mail: roland.hostettler@angstrom.
uu.se).
Yu Ge and Henk Wymeersch are with the Department of Electrical Engineering, Chalmers University of Technology, 412 58 Gothenburg, Sweden (e-mail:yuge@chalmers.se; henkw@chalmers.se).
Hyowon Kim is with the Department of Electronics Engineering, Chungnam National University, Daejeon 34134, South Korea (e-mail: hyowon.
kim@cnu.ac.kr).
Digital Object Identifier 10.1109/TRO.2023.3338975

uk
f (xk−1 , uk )
Qk−1
Mk
Mk
mi
Zk
Jk
zj
g(Zk |Mk , xk )g(zk |m, xk )
Rk
λc
c(z)
pD (mi |xk )
φt,i
k
xnk
wkn
N
vk (m|xn0:k )
ηkn,i
m̂n,i
k
Pn,i
k
Γ
γ̄ t
μt
Σt

Control input of the robot at time k.
Transition function of the robot.
Process noise covariance at time k − 1.
Map at time k.
Number of landmarks in Mk .
ith landmark in Mk .
Set of measurements at time k.
Number of measurements in Zk .
jth measurement in Zk .
random finite set (RFS) likelihood.
Measurement likelihood.
Measurement noise covariance.
Poisson rate.
Clutter intensity.
Detection probability.
Association variable.
nth particle of the probability hypothesis
density (PHD)-simultaneous localization andmapping (SLAM) density.
Weight of nth particle of the PHD-SLAM density.
Number of particles.
PHD conditioned on the nth trajectory.
Weight of the ith component of the Gaussian
mixture (GM)-PHD.
Mean of the ith component of the GM-PHD.
Covariance of the ith component of the GMPHD.
Number of Gaussian mixture importance density (GM-ID) components.
Weight of the tth GM-ID component.
Mean of the tth GM-ID component.
Covariance of the tth GM-ID component.
I. INTRODUCTIONOLLOWING pioneering research in autonomous robotics[1], the simultaneous localization and mapping (SLAM)
problem has gained widespread interest over the past decades,with numerous applications ranging from mobile robotics [2]to visual odometry [3]. In recursive probabilistic form, theSLAM problem requires a robot to incrementally build a map of
the unknown environment, simultaneously localize itself within
the map, and estimate the related uncertainties [4]. There areF© 2023 The Authors. This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see
https://creativecommons.org/licenses/by/4.0/

1020

numerous representations for the robot pose and map, including
landmark-based [1], [4], grid-based [5], [6], and graph-based [7],[8] approaches. Landmark-based approaches decompose thephysical environmental landmarks, such as trees and other obstacles, into parametric representations, such as a point, which thenform a map with an unknown number of landmarks at unknown
locations.
Solving the probabilistic SLAM problem requires propagating the joint posterior density of the robot trajectory and map
over time [4]. Furthermore, building the map requires the joint
estimation of both the number and location of landmarks thathave been covered by the sensor’s field-of-view (FOV). Conventional SLAM solutions comprise of a three-step approachin which the measurement to landmark association is solved
first. Then, given the found data association (DA), the joint
posterior density is estimated using Bayesian filtering. Lastly,
a separate map management routine is required to create landmarks that enter the sensor’s FOV and delete landmarks thatoriginate from false detections. Numerous works have demonstrated that such an approach works well in practice [9], [10],[11], but is sensitive to DA uncertainty [12]. To account fordata ambiguities in a theoretically sound manner requires that
DA uncertainty is treated as a part of the estimation process.
This can be done by modeling the map as a finite set and using
RFS theory for propagating the joint posterior distribution intime [13]. RFS-based methods are particularly attractive since
they enable a fully integrated Bayesian framework for SLAM
since DA uncertainty is factored in to the estimation process,uncertainties on both the number of landmarks in the map andtheir state are taken into account, there is no ordering of thelandmarks, and the map management routine is integrated into the filtering recursion. The RFS formulation for SLAM was
first proposed in [14] and a tractable first-order approximationcoined as PHD-SLAM filter soon followed [13]. Over the years,
various approximations have been used to model the landmark
map resulting in various RFS-SLAM filters including the labeled multi-Bernoulli (LMB)-SLAM filter [15], δ-generalized
LMB-SLAM filter [16], and the Poisson multi-Bernoulli mixture(PMBM)-SLAM filter [17], [18].
In terms of implementation and representation, many SLAM
algorithms take advantage of an important characteristic of the
SLAM problem: by conditioning the map to the robot’s trajectory, the landmarks are conditionally independent [19], making it
natural to apply Rao–Blackwellized particle filter (RBPF) solutions. Such a factored solution was adopted in FastSLAM, whichuses a particle filter (PF) to sample over robot paths and eachparticle possesses numerous low-dimensional extended Kalmanfilters (EKFs), one for each of the landmarks [11]. The filtering
recursion of PHD-SLAM is similar to that in FastSLAM, but
the use of RFS likelihoods affects how the particle weights arecomputed and in addition, PHD filters are used for mapping [13].
The standard PHD-SLAM algorithm [13], similar to that of
FastSLAM 1.0 [11], is a bootstrap filter in which the dynamic
model is used as the importance density. If the robot motion isaffected by large disturbances, a large number of particles are
required to adequately explore the sample space, which can becomputationally expensive.

IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

In this work, we propose an improved importance density
in which poses are sampled under consideration of both the
motion of the robot and the measurements, as well as DA
uncertainty. To account for data ambiguities, a multihypotheses
importance density (MH-ID) approximation is derived and a
Gaussian mixture model (GMM) representation of the multihypotheses importance density (MH-ID) is proposed, in which
each component of the GM represents a single DA. The proposed GM-ID enables us to incorporate DA uncertainty within
the importance density approximation, rather than assume thatthe most likely DA is correct as in [20]. Then, for each GM
component, we exploit an iterative method for approximating
parameters of the importance density, which is based on using
generalized statistical linear regression (SLR), combined with
iterated posterior linearization (IPL) [21], [22]. The proposedsolution exploits the measurement model structure and uses
partitioned updates, that is, the importance density is updated
one measurement at a time. The partitioned algorithm scales
linearly with the number of landmarks M within the FOV,
instead of O(M 3 ) when using a joint approximation approachas proposed in [20]. Using synthetic and experimental data, it isdemonstrated that the proposed GM-ID improves the accuracy
and robustness of PHD-SLAM, while the partitioned updatestrategy makes it possible to include multiple DA hypotheses in
the importance density approximation with low computational
complexity.
The contributions of this article are summarized below.
1) A novel GM-ID for probabilistic SLAM: A GM proposal
distribution is developed for PHD-SLAM. In the proposed
GM-ID, individual mixture components represent different DAs to account for data ambiguities in high clutter
scenarios. The developed GM-ID provides a new method
for taking into account DA uncertainty, which allows
developing more robust and efficient versions of existing
PHD-SLAM filters.
2) Low-complexity computation of a GM-ID approximation:
An iterative method for approximating the mixture components of the GM-ID is utilized and a partitioned updatestrategy is developed. The partitioned importance density
algorithm is computationally feasible since it scales according to O(M ).
3) Comparison against state of the art: We validate thedevelopment efforts using synthetic and experimental dataand compare the proposed algorithm to three other PHDSLAM filters. The results indicate that the proposed GMID improves PHD-SLAM performance allowing accurate,
efficient, and robust SLAM even in high clutter scenarios. We provide MATLAB code that runs PHD-SLAM
using the presented GM-ID approximation. In addition,
we provide C/C++ source codes that can be compiled to
MATLAB MEX-files to enable a highly efficient implementation of the developed algorithm.1
The rest of this article is organized as follows. In Section II,
the related work is presented. In Section III, the probabilistic

1 [Online]. Available: https://github.com/okaltiok/PHD-SLAM-3.0

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOSSLAM problem is formulated, the underlying models are introduced, and the Bayesian filtering recursion of RFS-SLAM
is presented. Section IV summarizes a first-order solution to
RFS-SLAM, which utilizes an Rao-Blackwellized particle filter(RBPF) for propagating the robot posterior and a GM-PHD filter
for estimating the map. The proposed GM-ID is introducedin Section V, with Section VI presenting and discussing its
performance. Finally, Section VII concludes this article. Major
notations of this article are given in the Nomenclature.
II. RELATED WORKIn SLAM, non-Gaussian noise and unknown DA have been
the primary drivers for developing multi-hypotheses methods.
Particle filtering approaches, such as FastSLAM [11] and RFSSLAM [13], are able to approximate arbitrary probability distributions through a finite number of samples. In FastSLAM, DA
can be determined on a per-particle basis, and hence, different
particles can be associated with different landmarks, which gives
the filter the possibility of dealing with the multihypotheses DA
problem [23]. In contrast, PHD-SLAM avoids explicit DAs, andDA uncertainty is factored in to the estimation process. Morerecent works have considered multihypotheses methods in thecontext of GraphSLAM [24], [25], [26]. In [24], a max-mixture
Gaussian distribution is proposed that can be used for example in
implementing robust cost functions to handle uncertain loop closures. Respectively, the works in [25] and [26] introduce methods for probabilistic DA. The method in [25] considered probabilistic DA making use of expectation–minimization whereas thework in [26] uses a max-mixture Gaussian distribution similar
to the work in [24]. In this article, a sum-mixture of Gaussians
is used to model the sampling distribution and each componentof the GM represents a single DA.
The aforementioned filtering approaches are typically referred to as online SLAM methods since they recursively estimate the joint posterior density as new measurements become
available. On the contrary, smoothing approaches utilize the entire data to estimate the full trajectory and a common approach tosolve the problem is using GraphSLAM [7], [8], which consistsof two tasks. The front-end task constructs the graph using odometry and sensor measurements, and the back-end task solves
the optimization problem to determine the best trajectory thatsatisfies constraints of the graph. The GraphSLAM algorithms
typically exploit the sparse structure of the problem to efficiently
solve the optimization problem [27], [28], [29]. Throughout thisarticle, we focus on online SLAM methods and therefore will
only consider such approaches in the following. The interested
reader is referred to [30] for a comparison between PHD-SLAM
and GraphSLAM in which it is demonstrated that the RFS
formulation can be beneficial in scenarios in which DA is very
challenging.
One major weakness of particle filtering-based SLAM solutions is the large number of particles required to propagate
the posterior accurately over time. The culprit is utilizing the
motion model as the proposal distribution, which results in
an inefficient use of particles as most of them will have an
insignificant weight after the posterior update. The FastSLAM

1021

2.0 algorithm addressed this issue by using an improved importance density in which poses are sampled under considerationof both the motion of the robot and the measurements [31].
With known DA, the proposal distribution used in FastSLAM 2.0
is an approximation of the optimal importance density (OID),
which is optimal in terms of minimizing the variance of theincremental particle weights [32]. Similarly, most PHD-SLAM
algorithms suffer from inefficient use of particles since they use
the transition density as the proposal distribution [13], [33], [34].
Inspired by the FastSLAM 2.0 algorithm, two recent works haveproposed improved importance densities for PHD-SLAM [20],[35]. In [35], the proposal distribution is computed by conditioning the predictive distribution of the robot state to the measurements independently and thereafter, the independent conditional
distributions are merged using weighted averaging. On the other
hand, the method in [20] approximates the importance density
by finding the joint approximation of the robot and map first,
and then by conditioning on the measurements of the most
likely DA. While both methods improve the performance withrespect to the original PHD-SLAM algorithm, they still require
quite many particles to function properly. The proposal of Gao
et al. [35] was very conservative, and numerous particles areneeded to propagate the posterior accurately, whereas the workin [20] is sensitive to incorrect DA and the filter requires many
particles to handle data ambiguities. In this work, we propose
an improved importance density in which poses are sampled
under consideration of both the motion of the robot and themeasurements, as well as DA uncertainty.
III. PROBLEM FORMULATION
Consider a mobile robot exploring an unknown environment.
Its location and orientation at time k are described by state vector,
xk , and the movement is governed by motion command uk .
The robot is equipped with a sensor and it is taking relative
observations of a number of unknown landmarks while moving
in the environment. The ith landmark is described by state vector,
mi , and an observation of a landmark is denoted as zjk . Since
the robot is moving in an uncharted environment, the numberof landmarks within the sensor’s FOV, denoted as FOV(xk ), is
unknown and time-varying. Moreover, DA between the observations and landmarks is unknown, clutter measurements can cause
false alarms and landmarks can be misdetected. Formulating the
problem using RFS theory enables a fully integrated Bayesianframework for SLAM under DA uncertainty and unknown landmark number [13]. The underlying models and the RFS-SLAM
algorithm are summarized in the following.
A. Models
A Gaussian density with zero-mean additive noise is a common representation of the motion model in SLAM [4]. In addition, the state transition is assumed independent of the landmarks
and observations, and it is modeled as a Markov process in which
xk only depends on xk−1 and uk . Mathematically, the transition
model of the mobile robot can be expressed asf (xk |xk−1 , uk ) = N (xk ; f (xk−1 , uk ), Qk−1 )

(1)

1022

IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

where f (·) denotes a known transition function and Qk−1 thecovariance.
Let Mk−1 denote the explored map up to time k − 1, then the
RFS transition density of the map is given byf (Mk |Mk−1 , xk )(2)

where δMk−1 denotes a set integral [38] and p(Mk−1 , x1:k−1 )is the shorthand notation for the posterior at the previous time
step. Once observing Zk , the predicted density can be updated
using the Bayes’ rule to obtain the posterior at time k [13, eq. (8)]p(Mk , x1:k |Z1:k , u1:k , x0 )and the explored map evolves in time according to [36, eq. (7)]Mk = Mk−1 ∪ Bk (xk )(3)

in which Bk (xk ) is the birth RFS describing the set of landmarks
observed for the first time. Let Mck−1 denote the complementof Mk−1 so that the newly detected landmarks can be definedas Bk (xk ) = FOV(xk ) ∩ Mck−1 . Essentially, the RFS transition
density of the map describes how the explored map grows
monotonically as the FOV of the robot’s sensor covers moreof the unexplored environment [36].
In this article, we consider a point target measurement model,
where each landmark can create at most one observation pertime instant. Then, the RFS likelihood function is obtained bysumming over all hypotheses [37, eq. (7.2)]g(Zk |Mk , xk ) =

Γall

t=1

  j
exp (−λc )
c zk
|Zk |

 t,i
φ
i
p
(m
|x
)g
zk k |mi , xk
k

 D
i
 t,i
1 − pD (m |xk )
×
.
φk
t,i
t,i
c zk
i:φk =0i:φk >0(4)
In (4), Γall is the total number of hypotheses, φt,ik is an association
variable, λc = c(z)dz is the Poisson rate, c(z) is the clutter
intensity, pD (mi |xk ) ∈ [0, 1] is the detection probability, andg(·) is the likelihood function. In SLAM literature, the likelihood
function is typically modeled as a zero-mean Gaussian, given
by [4](5)

where g(m, xk ) is a known measurement model and Rk denotes
the covariance. The association variable is defined as
φt,i
k =

j if mi is associated with zj
0 if mi is undetected.

g(Zk |Mk , xk )p(Mk , x1:k |Z1:k−1 , u1:k , x0 )g(Zk |Z0:k−1 , x0 )where the RFS likelihood is given in (4), and the term in the
denominator is a normalization constant.
IV. RAO–BLACKWELLIZED PHD-SLAM FILTER
An RBPF implementation of the PHD-SLAM filter is summarized in this section. Fundamentally, the robot trajectory is
estimated with a PF, and a PHD filter is used for estimating eachtrajectory conditioned map.
A. Factorized RFS-SLAM Filter
Analogous to FastSLAM [11], the joint posterior RFS-SLAM
density in (7) can be factorized as [13](6)

= p(x1:k |Z1:k , u1:k , x0 )p(Mk |Z1:k , x0:k ).

(10)

The recursion for the joint RFS-SLAM density presented in
Section III-B is equivalent to jointly propagating the posterior
density of the robot trajectory, p(x1:k |Z1:k , u1:k , x0 ), and theposterior density of the map that is conditioned on the trajectory, p(Mk |Z1:k , x0:k ). The posterior of the robot trajectory is
computed as [13, eq. (16)]p(x1:k |Z1:k , u1:k , x0 ) = p(x1:k−1 |Z1:k−1 , u1:k−1 , x0 )×g(Zk |Z1:k−1 , x0:k )f (xk |xk−1 , uk ).
g(Zk |Z1:k−1 )(11)

The recursion to propagate the map posterior in time follows
the generalization of the recursive Bayesian filter applied tosets [38] for which the prediction step is given by the Chapman–
Kolmogorov equationp(Mk |Z1:k−1 , x0:k ) =f (Mk |Mk−1 , xk )× p(Mk−1 |Z1:k−1 , x0:k−1 )δMk−1(12)

B. RFS-SLAM
The objective of probabilistic SLAM is to estimate the joint
posterior density of the map and robot trajectory [4]p(Mk , x1:k |Z1:k , u1:k , x0 )(7)given the initial pose of the robot, x0 , measurements, and controls up to the current time instant k. The RFS-SLAM filter
follows the prediction and update steps of the Bayesian filtering
recursion applied with RFSs [13]. The prediction step of thefilter is given by [13, eq. (7)]p(Mk , x1:k |Z1:k−1 , u1:k , x0 ) = f (xk |xk−1 , uk )×

(9)p(Mk , x1:k |Z1:k , u1:k , x0 )

j=1

g(zk |m, xk ) = N (zk ; g(m, xk ), Rk )=f (Mk |Mk−1 , xk )p(Mk−1 , x1:k−1 )δMk−1(8)

and the update by applying the Bayes’ ruleg(Zk |Mk , xk )p(Mk |Z1:k−1 , x0:k ).
g(Zk |Z1:k−1 , x0:k )(13)
As detailed in [13], the factorized solution defined by (11)–(13)is similar to that of FastSLAM [11], [31] and the effect ofconditioning on x0:k is to render each landmark estimate conditionally independent. However, adopting RFS likelihoods affects
how (11) is evaluated. Furthermore, FastSLAM is conditioned
on DA assignments, which are essentially unknown, whereas
RFS-SLAM is not and the recursion defined by (12) and (13) isthat of an RFS-based mapping with known poses.
p(Mk |Z1:k , x0:k ) =KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS

B. Representation of the RFS-SLAM Density
We follow an RBPF approach as in [13], [36], and [33] andapproximate the posterior density of the robot trajectory using
a weighted set of N particles
p(x1:k |Z1:k , u1:k , x0 ) ≈N

wkn δ (x1:k − xn1:k )

(14)

n=1

where δ(·) is the Dirac delta function, xn1:k is the nth particle,
and wkn is the associated weight. The RFS-SLAM density isparameterized using{wkn , xn0:k , p(Mk |Z1:k , xn0:k )}N
n=1

p(Mk |Z1:k , xn0:k ) ≈v (m|xn )m∈M k n 0:k  .
exp vk (m|x0:k )dm(16)

In (16), vk (m|xn0:k ) is the shorthand notation for vk (m|Z1:k ,xn0:k ) and it denotes the PHD conditioned on the nth trajectory.
Using a GM parametrization, the PHD is given by [40]nvkn (m|xn0:k ) =

Mk

n,i
ηkn,i N (m̂n,ik , Pk )

(17)

i=1

where Mkn is the number of GM components at time k and, ηkn,i ,
n,i
m̂n,i
k , and Pk are the weight, mean, and covariance of landmark
i for particle n, respectively. It is to be noted that the weights
Mkn n,iηkn,i represent the number of landmarks at m̂n,ii=1 ηkk , andgives the expected number of landmarks in the estimated map. In
PHD-SLAM, the trajectory-conditioned map is estimated using
a PHD filter and the overall PHD-SLAM density at time k isrepresented by
Mkn N

n,i
,
P
.
(18)
wkn , xn0:k , ηkn,i , m̂n,i
k
k
i=1

n
n
the predicted PHD has Mk|k−1= Mk−1+ MkB,n components,
and parameters of the prior map are unchanged since the landn,in,in,i= ηk−1, m̂n,imarks are static, that is, ηk|k−1
k|k−1 = m̂k−1 , andn,iPn,ik|k−1 = Pk−1 .
Since the likelihood in (5) has a Gaussian form, it follows thatthe posterior PHD is also a GM, given by [13, eq. (23)]nvkn (m|xn0:k ) = vk|k−1(m|xn0:k ) 1 − pD (m|xnk )+n=1

C. PHD Filter for Mapping
The utilized mapping algorithm follows the GM implementation of the PHD filter [13], [40], which is summarized in
the following for the sake of completeness. If the PHD at theprevious time instant is a GM, then it follows that the predicted
PHD is also a GM, given by [13, eq. (22)]nnB(m|xn0:k ) = vk−1
(m|xn0:k−1 ) + vkB (m|Zk−1, xnk−1 )vk|k−1(19)n(·), and the PHD of theand it is the union of the prior map, vk−1
birth RFS, vkB (·), which is used to model landmarks that enter
FOV(xk ) for the first time, as discussed in Section III-A. The
birth PHD at time k is modeled as a GM with MkB,n components,
representing a subset of measurements at the previous timeB
B
⊆ Zk−1 [see definition of Zk−1
after (22)].2 Thus,
step, Zk−1
2 A similar approach has been used in [13] and [36] but using all the measureB
ments from time step k − 1, that is, Zk−1
= Zk−1 .


z∈Zk

(15)

and each particle represents a single trajectory, and a trajectory
conditioned map is associated with each of the particles.
We utilize a map representation based on the PHD by approximating the posterior RFS map using a Poisson point process(PPP) following assumed-density filtering [13], [39]1023

pD (m|xnk )g(z|m, xnk )c(z) + Λ(m , z|xn0:k )dm


(20)

where
n
Λ(m , z|xn0:k ) = pD (m |xnk )g(z|m , xnk )vk|k−1(m |xn0:k ).
(21)
In practice, parameters of the updated PHD can be estimatedusing any standard Gaussian filtering technique, such as theextended, unscented, or cubature Kalman filter (see, e.g., [41]).
In this article, a first-order Taylor series-based Gaussian approximation is used allowing EKF style updates, and the reader isreferred to [13] and [36] for further details. To reduce computational complexity of the update step, ellipsoidal gating [42]is utilized and a measurement is used to update parameters of
the GM component only if the squared Mahalanobis distance isbelow gating threshold TG . Mathematicallyn2zjk − g(m̂n,ik|k−1 , xk ))(Sn,i

k|k−1

)−1

≤ TG

(22)

n,i
where Sn,ik|k−1 = Gm Pk|k−1 Gm + Rk , Gm denotes the Jaco-bian of g with respect to mi evaluated at m̂n,ik|k−1 , and we haveused the notation e2Ω = e Ωe above. Furthermore, measurements that are not used to update any landmark constitute ZkB ,which is then used to initialize new landmarks at the next timenn
= Mk|k−1
× (|Zk | +step. The updated map has at most Mk|knn1) components if every component of vk|k−1 (m|x0:k ) is updated
by a misdetection and by every measurement, but typicallynn< Mk|k−1
× (|Zk | + 1).
Mk|k
D. Robot Trajectory
The posterior of the robot trajectory, p(x1:k |Z1:k , u1:k , x0 ),can be estimated using, for example, the sequential importancesampling PF (see, e.g., [43] and [44]), which recursively propagates the weights and support points. The recursion consistsof two steps for every particle n; the importance density is firstcomputed and sampled fromxnk ∼ q(xk |xn0:k−1 , Z1:k , u1:k )(23)

and thereafter, the weights are updated according tonwkn = wk−1

g(Zk |Z1:k−1 , xn0:k )f (xnk |xnk−1 , uk ).
q(xk |xn0:k−1 , Z1:k , u1:k )(24)

The optimal choice, in terms of minimizing the incremental
particle weights, for the importance density in (23) is given by
the OID [2], [44]1024IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024q(xk | xn0:k−1 , Z1:k , u1:k ) = p(xk | Z1:k , xn0:k−1 , u1:k )=g(Zk | Z1:k−1 , xn0:k )f (xnk | xnk−1 , uk ).
p(Zk | Z1:k−1 , xn0:k−1 , u1:k )(25)

Unfortunately, sampling directly from this distribution is impossible in the general case; it does not even possess a closed
form since we cannot express the right-hand side as a Gaussianin xk . However, an approximation of the OID can be attained
if the measurement model is approximated by a linear function
whereas the dynamic model may remain nonlinear [32]. Such an
approximation is utilized in FastSLAM 2.0 [31] and a similar one
in a recent PHD-SLAM work [20], but the solutions conditionon the selected best DA assignment, and a wrong assignment canhave significant ramifications to the approximated importancedensity. To address this problem, we propose sampling the poses
under consideration of both the motion of the robot and themeasurements, as well as DA uncertainty. The details of the
improved approximation for (25) are given in Section V.
It is to be noted that in PHD-SLAM, a common choice for(23) is the transition density [13], [33], [34], [36], [45]q(xk | xn0:k−1 , Z1:k , u1:k ) = f (xnk | xnk−1 , uk )(26)

simplifying the weight update in (24) tong(Zk |Z1:k−1 , xn0:k ).
wkn = wk−1

(27)

Using the transition density as the importance density enables
straightforward and efficient particle updates. However, if covariance of the process noise is large with respect to the measurement noise covariance, a downside of using such a proposal
is that most of the sampled particles will have an insignificant
weight after computing (24) and a large number of particles are
required to accurately approximate the posterior. In Section VI,
the performance of PHD-SLAM using different importancedensities is presented and discussed.
V. MULTIHYPOTHESES IMPORTANCE DENSITYThis section presents an improved approximation for the importance density given in (25) to better cope with DA uncertainty.
To handle data ambiguities, an MH-ID is derived and a GMM
representation is proposed, in which each component of the
GM represents a single DA. Before presenting the MH-ID, weintroduce a method that can be used to rank the DAs in theascending order of cost. Then, the Γ best DAs are used forapproximating the MH-ID. In the following, the importancedensity in (23) and weight in (24) are computed independently
for each particle and dependence of xnk and vkn (·) on particle n
is excluded from the notation for brevity.
A. Computation of Γ Best DAs
If the DA is unknown, the number of ways Mk landmarks
can be assigned to Jk measurements increases very fast as
Mk and Jk grow. Instead of exhaustively computing all thepossible assignments, Γall , the goal is to rank the candidate DA
assignments in the ascending order of cost, select the Γ bestDAs, and truncate the rest. This problem can be cast as a ranked
assignment problem and solved using Murty’s algorithm [46].

The ranked assignment problem can be formulated using the
optimal assignment problem, given by [47]minimize tr Φ L
s.t. {Φ}i,j ∈ {0, 1} ∀ i, j
M
k +Jk

{Φ}i,j = 1 ∀ i

j=1
Mk


{Φ}i,j ∈ {0, 1} ∀ j

(28)

i=1

where Φ is the assignment matrix variable, L is the cost matrix,i ∈ {1, . . . , Mk }, and j ∈ {1, . . . , Mk + Jk }. The cost matrix
is defined as

⎤
⎡ 1,1
. . . 1,Jk  1,0 . . .
0⎢.. ⎥ (29)..  ..
..
..
L = − log ⎣ ...
.
.
.  .
. ⎦Mk ,1 . . . Mk ,Jk  0 . . . Mk ,0
where the left Mk × Jk submatrix corresponds to measurementsthat are associated with a landmark and the right Mk × Mk
diagonal submatrix corresponds to misdetections. The elementsof the cost matrix are given by [48]i,j =pD (mi |x)g(zjk | mi , xk|k−1 )p(mi |x)dmi
c(z)

(30)

i,0 =(1 − pD (mi |x))p(mi |x)dmi(31)

in which xk|k−1 = f (xk−1 , uk ) denotes the predicted state,p(mi |x) = vk|k−1 (mi |x0:k ) is the shorthand notation for thepredicted density of the ith landmark, and the detection probability is approximated as pD (mi |x) ≈ pD (m̂ik|k−1 | xk|k−1 ) to
make computation of the integrals feasible.
The optimal assignment problem seeks an assignment matrix
Φ that minimizes (28) and the ranked assignment problem seeks
an enumeration of the least highest cost assignment matrices innondecreasing order [46]. The cost of assignment Φ is(32)
γk|k−1 = exp −tr(Φ L)
and Murty’s algorithm [46] is used to find Γ̃ assignment matrices
Γ̃12≤ γk|k−1≤ · · · ≤ γk|k−1. A fixed value could
such that γk|k−1
be used for Γ̃ but to conserve computational resources, Γ̃ isadaptively tuned to only account for DAs with a meaningful
cost as proposed in [49]. In Murty’s algorithm, we terminate
hypotheses generation whenever the cost of the most likely
hypotheses is below some fixed percentage of the cost of theΓ̃1/γk|k−1. We have noticed thatnext hypotheses, that is, γk|k−1
including the assignment, {ΦΓ̃+1 }i,j = 1 ∀ i ∧ j = Jk + i, increases robustness of the proposed algorithm in scenarios wherea wrong DA leads to an inaccurate importance density. In suchcircumstances, it is beneficial to treat all of the measurements as
clutter and use the transition density as the importance density.
Thus, the total number of DAs is Γ = Γ̃ + 1.
To make the treatment comprehensive, the mapping fromΦ to the association variable φt,ik in (6) is defined as follows.

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS

1025t
Let Φt denote the tth association matrix with cost γk|k−1
. The

t,Mk
association variable is defined as φtk = [φt,1] for
k , . . . , φkwhich the elements are given by

φt,i
k =

j if {Φt }i,j = 1 and j ≤ Jk
0 otherwise.

(33)

g(Zk | Z1:k−1 , x0:k )≈The proposed method to compute parameters of the importance density is based on the IPL OID approximation introduced in [22]. The proposed solution exploits the measurement
model structure and uses partitioned updates, that is, updates
the importance density approximation using one measurement
at a time, which yields an algorithm that scales according toO(|M|). This efficiency makes it possible to include multiple
DA hypotheses in the importance density approximation andstill obtain a computationally efficient algorithm.
First, recall that the optimal choice (that minimizes the incremental particle weights) for the importance density q(xk |x0:k−1 , Z1:k , u1:k ) in (23) for the PHD-SLAM filter is given
by [2], [44]q(xk | x0:k−1 , Z1:k , u1:k ) ∝ g(Zk | Z1:k−1 , x0:k )(34)

g(Zk , Mk |Z1:k−1 , x0:k )δMk

g(Zk | Mk , xk )p(Mk | Z1:k−1 , x0:k )δMk=

(35)
with g(Zk | Mk , xk ) and p(Mk | Z1:k−1 , x0:k ) as in (4)and (12), respectively. Then, for Γ DA hypotheses, we have (seethe Appendix for details of the derivation)g (Zk | Z1:k−1 , x0:k )=Γexp(−λc )

t=1

×

|Z|

j=1



i
ηk|k−1×c(zjk )j=1

i
ηk|k−1[1 − p̂iD ]i:φt,ik =0

iηk|k−1p̂iD
 t,iφc zk ki:φt,i
k >0



 t,i


φ
g zk k | mi , xk N mi ; m̂ik|k−1 , Pik|k−1 dmi .

×(37)
Next, recall that PHD-SLAM (as well as FastSLAM) uses
an affine approximation of the likelihood around the predicted
landmark state m̂ik|k−1 to facilitate the use of approximate Rao–Blackwellization in the PF, which makes the SLAM problem
tractable. This yields the affine approximation of the measurement likelihood function (5)g(zk k | mi , xk ) t,iφ
≈ N zk k ; g(m̂ik|k−1 , xk ) + Gm,k (mi − m̂ik|k−1 ), Rk
where Gm,k is the Jacobian of g with respect to mi evaluatedat m̂ik|k−1 . Hence, the predicted likelihood is approximated asg(Zk | Z1:k−1 , x0:k )≈Γc(zjk )exp(−λc )

t=1

×



|Z|


φt,i

where [13, eq. (24)]g(Zk | Z1:k−1 , x0:k ) =exp(−λc )t=1

B. MH-ID Approximation× f (xk | xk−1 , uk )

Γ


|Z|

j=1

c(zjk )

i
ηk|k−1[1 − p̂iD ]i:φt,ik =0

iηk|k−1p̂iD
 t,iφc zk ki:φt,ik >0

 t,i φ× N zk k ; g m̂ik|k−1 , xk , Gm,k Pik|k−1 Gm,k +Rk .


1 − pD m i | x k(38)

i:φt,ik =0
× N mi ; m̂ik|k−1 , Pik|k−1 dmi
ηik|k−1
×φt,ic zk ki:φt,ik >0Then, the MH-ID can in turn be approximated as

 t,i
 i

φ
pD m | x k g z k k | m i , x k
× N mi ; m̂ik|k−1 , Pik|k−1 dmi .

p(xk | Z1:k , x0:k−1 , u1:k )
∝
∼

(36)

Unfortunately, the detection probability makes the solution of
the remaining integrals not feasible. However, using the predicted probability of detection p̂iD ≈ pD (m̂ik|k−1 | xk|k−1 ) (obtained from the DA, Section V-A), we obtain the approximation

Γ

t=1

×

κtk|k−1 f (xk | xk−1 , uk ) t,i φ
N zk k ; g m̂ik|k−1 , xk ,i:φt,ik >0Gm,k Pik|k−1 Gm,k + Rk

(39)

1026

IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

where ∝ denotes “approximately proportional to.” This is a∼
mixture density with mixture weights|Z|κtk|k−1 ∝ exp(−λc )

×


j=1



c(zjk )iηk|k−1[1 − p̂iD ]i:φt,ik =0

iηk|k−1p̂iD
 t,i .
φc zk ki:φt,ik >0Algorithm 1: IPL Approximation for Mixture Component t.
1:
2:
3:
4:
5:

xx

Set μ̃x0 = f (xk−1 , uk ), Σ̃0 = Qk−1
for l = 1, . . . , L doxx
Set μ̃xl = f (xk−1 , uk ), Σ̃l = Qk−1t,ifor i : φk > 0 do
zz
Linearization: Calculate the moments μ̃zl , Σ̃l ,6:
Unfortunately, this cannot be sampled from and instead, we
propose to approximate the above mixture density using a GMof the formp (xk | Z1:k , x0:k−1 , u1:k ) ≈

Γ




κtk N xk ; μtk , Σtk

φt,i

zx

and Σ̃l using zk k w.r.t. the linearization densityxx
π(xk ) = N (xk ; μ̃xl−1 , Σ̃l−1 ) according to (42)
Moment matching: Calculate the moments μzl ,zxΣzzl , and Σl of the linearized measurement
model according to (43)
Measurement update for landmark i:

(40)

7:

zz −1
Kl = Σxz
l (Σl )

(41)

φt,i

t=1

instead. It is to be noted that the work in [20] uses a single
Gaussian representing one DA to approximate the importancedensity, whereas in this work, we use a GM representing Γ DAs
to approximate the importance density. As in [20], the mixture
moments μtk and Σtk are calculated using the IPL-based OID
approximation approach [22]. However, we use a partitioned
update strategy, which entails that the individual landmark measurements are used one at a time to update the proposal density,whereas the work in [20] uses a joint update strategy.
Algorithm 1 summarizes the resulting algorithm, which consists of three steps. First, the measurement likelihood is linearized, which requires calculating the moments
μ̃zl = Eπ {g(m̂ik|k−1 , xk )}

(42a)

zz
Σ̃l = Eπ {(g(m̂ik|k−1 , xk ) − μ̃zl )(g(m̂ik|k−1 , xk ) − μ̃zl )}+ Eπ {Gm,k Pik|k−1 Gm,k } + Rk

(42b)

Σ̃l = Eπ {(g(m̂ik|k−1 , xk ) − μ̃zl )(xk − μ̃xl ) }

(42c)

zx

where Eπ {·} denotes the expectation with respect to the linearization density π(xk ). Note that the resulting integrals cantypically not be solved in closed form, but Taylor series expansion or sigma points can be used to approximate them. In thisarticle, we resort to local linearization using a first-order Taylor
series expansion.
Second, based on the linearization moments (42), the moments of the linearized likelihood (linearized w.r.t. π(xk )) arecalculated according to [21], [22] xx −1zx
Bl = Σ̃l Σ̃l−1
(43a)


μzl = μ̃zl + Bl μ̃xl − μ̃xl−1
(43b)


zz
xx
xx
(43c)
Σzz
l = Σ̃l + Bl Σ̃l − Σ̃l−1 BlxxΣxzl = Σ̃l Bl .

(43d)

Third, the mean and covariance of the mixture component
are updated using the standard Kalman filter update. Note,however, that the measurements are incorporated one at a timein a partitioned update scheme, which has computational costμ̃xl = μ̃xl + Kl (zk k − μzl )
xx

Σ̃l
8:
9:
10:

xx

= Σ̃l − Kl Σzz
l Kl

end forend forxx
Set μtk = μ̃xL , Σtk = Σ̃Lof O(|Mk |), compared with a full joint update used in [20] thatscales according to O(|Mk |3 ).
The choice of the linearization density is as follows. First,
the linearization density is chosen to the dynamic model(i.e., the prior). Then, after each iteration, a new approximationof the posterior density is obtained, which is then used as
the linearization density in the next iteration. The procedure
is repeated either for a fixed number of iterations L or upon
convergence, see [22]. Once the moments have been computed,
weight of the mixture component is given byκtk ∝ exp(−λc )

|Z|

j=1

×

c(zjk )iηk|k−1p̂iD
 t,i Nφc zk ki:φt,i
k >0





i
ηk|k−1[1 − p̂iD ]i:φt,i
k =0





φt,i
zk k ; g m̂ik|k−1 , μtk , St,i
k
(44)

t
i
where St,ik = Gx,k Σk Gx,k + Gm,k Pk|k−1 Gm,k + Rk andGx,k is the Jacobian of g with respect to x evaluated at μtk .

C. Sampling and Importance Weight
After computing parameters of the GM-ID approximation in(41), a new particle xnk is drawn from the proposed importancedensity as follows. First, a component t is randomly drawn fromthe categorical distribution defined by the weights (44). Thereafter, the multivariate normal distribution with moments μtk andΣtk is sampled from, that is, xnk ∼ N (μtk , Σtk ). It is important
to note that for each particle only one mixture component issampled from the GM-ID and the multihypotheses structure is
only partially preserved over time for a single particle. Since

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS

Algorithm 2: Proposed Algorithm to Update Particle n.
1:
2:
3:

4:
5:
6:
7:
8:
9:
10:
11:

Algorithm 3: Proposed PHD-SLAM Algorithm at Time
Step k.

nn
Input: Zk , uk , {wk−1, xnk−1 , vk|k−1(m|xn0:k )}N
n=1

Procedure Compute GM-ID
Cost matrix: Propagate particle
xk|k−1 = f (xk−1 , uk ) and compute L according
to (29).
Rank DAs: Compute Γ best DAs using L and
Murty’s algorithm as presented in Section V-A.
for t = 1, . . . , Γ doCompute moments μtk and Σtk using Algorithm 1
and weight κtk according to (44).
end forend procedureSample from GM-ID: xnk ∼ Γt=1 κtk N (xk ; μtk , Σtk ).
Incremental weight: Update wkn according to (45).
Output: Updated particle {wkn , xnk } at time step k

the weights are normalized by the likelihood of the GM-ID, themultihypotheses structure is encoded in the weights of the particles. In addition, different particles, even with the same prior,can sample different mixture components from the GM-ID, and
the PHD-SLAM density in (18) is able to capture N hypotheses
overall.
Log of the importance weight in (24), w̃kn = log(wkn ), is
updated using

n
n n
n
n g(Zk |Z1:k−1 , x0:k )f (xk |xk−1 , uk )w̃k = log wk−1
q(xk |xn0:k−1 , Z1:k , u1:k )nnnn= w̃k−1+ w̃meas.
+ w̃prior− w̃prop.
.

(45)

n
In literature, several methods for computing w̃meas.
have been
proposed including an empty map update [13], single feature
update [13], and multifeature update [33]. With a PPP prior anda point object measurement model, we use the exact expressionn, computed as [34]for w̃meas.
nw̃meas.
=log c(z) + Λ(m, z|xnk )dm
(46)
z∈Zk

where Λ(·) is defined in (21). Log-likelihood of the prior isgiven byn(47)
w̃prior= log N xnk ; f (xnk−1 , uk ), Qk−1 .
Respectively, log-likelihood of the proposal is computed usingnw̃prop.
= log

Γ

t=1



κ̃tk N xnk ; μtk , Σtk

1027

(48)


where κ̃t = κtk / Γt κtk are the normalized weights. Algorithm 2 summarizes the proposed algorithm to approximate theimportance density in (23), sample from it in closed form, andupdate the incremental weights in (24). In practice however,
it is computationally more efficient to compute the incremental
weight in two phases. The weight is updated with log-likelihoodsnisof the prior and proposal during the weight update and w̃meas.
added to the importance weight during the PHD update step.

1:
2:
3:
4:
5:
6:

7:
8:

9:

10:

n
n
Input: Zk , uk , {wk−1, xnk−1 , vk−1
(m|xn0:k−1 )}Nn=1
for n = 1, . . . , N don(m|xn0:k ) using (19).
PHD predict: Compute vk|k−1
Particle update: Use Algorithm 2 to obtain{wkn , xnk }.
PHD update: Compute vkn (m|xn0:k ) using (20).
nGMHypotheses reduction: Prune and merge Mk|k
PHD components to obtain a reduced number ofn, as presented in [40].
components, Mkn ≤ Mk|kend forEstimate: Find j = arg maxN {wk }N
n=1 and compute
j,i
:
η
≥
T
,
i
= 1, . . . , Mkj }the map M̂k = {m̂j,iηkj
and robot x̂k = xk estimates.
Resample: Normalize
the weights
nw,
compute the effective sample
wkn = wkn / N
n=1
 k n
size ESS = 1/ Nw
n=1 k . and resample if
ESS ≤ TESS .
Output: M̂k , x̂k , {wkn , xnk , vkn (m|xn0:k )}N
n=1 .

VI. EXPERIMENTAL RESULTS
The development efforts of the article are evaluated with synthetic data [35] and the experimental Victoria Park dataset [50].
The experiments focus on vehicles operating in planar environments so that pose can be represented by the 2-D location (x, y)and heading θ. Furthermore, the vehicle is controlled by speed v
and steering ω commands. Thus, the state and control input of thevehicle at time k are xk = [xk , yk , θk ] and uk = [vk , ωk ] ,in respective order. The landmarks in the environment are static,
and location of the ith landmark is mi = [xi , y i ] .
We compare the developed filter only with other PHD-SLAM
filters [13], [20], [35], since it has already been demonstrated
that they outperform more conventional SLAM filters (e.g., [11],[31], and [51]) in high clutter scenarios [13], [20], [33], [35]. We
refer to the original PHD-SLAM filter [13] as PHD-SLAM 1.0
and the other two filters as PHD-SLAM 2.0a [35] and PHDSLAM 2.0b [20] since both approximate the importance density
considering both the motion of the robot and the measurements.
This naming convention follows the FastSLAM algorithms,
since the original algorithm that used the motion model as theimportance density was coined as FastSLAM 1.0 [11] and thefilter that used the improved sampling distribution was named
as FastSLAM 2.0 [31]. Honoring this naming convention, thealgorithm implemented in this article is coined as PHD-SLAM
3.0, since the presented MH-ID offers a notable advantage incluttered scenarios where data ambiguities are common.
Overall, 100 Monte Carlo simulations (MCSs) are performed
and the results are obtained by averaging over the independent
simulations. The accuracy of the state estimates is evaluatedusing the root mean squared error (RMSE) and mapping accuracy is evaluated with the generalized optimal subpatternassignment (GOSPA) metric, which captures the localization
error and penalizes for missed and false landmarks. Let Mk =

1028

IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

|M|

|M̂|

{m1k , . . . , mk } and M̂k = {m̂1k , . . . , m̂k } denote the map
and its estimate at time k, respectively. Now, GOSPA is definedas [52]
dck,p (Mk , M̂k )⎡= ⎣minξ∈Ξcmik − m̂jk p +(i,j)∈ξ

p

2



⎤1

p

|Mk |+|M̂k |−2|γ| ⎦in which ξ is the assignment set between M and M̂, Ξ is theset of all possible assignment sets ξ,  ·  denotes the Euclideannorm, and c defines the maximum allowable localization error.
Unless otherwise stated, GOSPA is computed only for the lasttime instant to capture the localization and cardinality errorsof the final map. Parameters used in computing GOSPA are:c = 20 m and p = 2.
As with all PHD-SLAM filters, pruning and merging operations are required to limit the exponential growth of Gaussiancomponents in the PHD. These operations are carried out asin [40, Table II] using a pruning threshold of log(10−6 ) anda merging threshold of 50. In addition, ellipsoidal gating withthreshold TG = 41.4465 is used to lower computational complexity of the PHD update step in (20), as well as, computing
the cost matrix in (29). Weight of newly created landmarks
is initialized to η = log(10−6 ). The map estimate threshold isTη = (1 − pD )2 to allow two consecutive misdetections before
the landmark is not considered as an estimate. The maximum
number of ranked assignments computed by Murty’s algorithm
is limited to Γ̃ = 50 and the threshold to terminate hypotheses
generation is Tγ = log(10−3 ). The maximum number of IPL
iterations is L = 5 and the convergence threshold is = 10−3
[22, eq. (11)]. The effective sample size (ESS) threshold toperform resampling is set to TESS = 0.2 × N . The developed
PHD-SLAM filter is summarized in Algorithm 3.
The PHD-SLAM filters are implemented using MATLAB
and the core functions are written in C/C++ and compiled to
MATLAB MEX-files to enable a highly efficient implementation of the algorithms. The simulations and experiments are run
on a Lenovo ThinkPad P1 Gen 2, with a 2.6 GHz 6-Core Intel
i7-9850H CPU and 64 GB of memory. Multithreading is not
exploited and the simulations are run on a single CPU core.
A. Synthetic Dataset
The simulation scenario is illustrated in Fig. 1 in which a
robot is exploring a 1 km2 area that contains 160 landmarks
along the robot trajectory. Kinematics of the robot is describedby a velocity motion model [2]f (xk−1 , uk )⎤⎡xk−1 − ωvkk sin(θk−1 ) + ωvkk sin(θk−1 + ωk T )= ⎣yk−1 + ωvkk cos(θk−1 ) − ωvkk cos(θk−1 + ωk T )⎦ (49)θk−1 + ωk T
and disturbances enter the system via a noisy control input thatis corrupted by zero-mean independent identically distributed(i.i.d.) Gaussian noise ϑk ∼ N (0, Q), with covariance Q =Fig. 1. Simulation scenario in which the landmarks are illustrated using ( ),the robot trajectory with (), and measurements as observed from the groundtruth poses using ( ). Trajectory estimate only using odometry information ( )and the trajectory () and map ( ) estimates of PHD-SLAM 3.0 using one
particle. In the example, rms positioning error is 1.30 m, rms heading error is
0.27◦ , and GOSPA of the final map is 21.94 m.

diag(σv2 , σω2 ). The robot is equipped with a sensor that measures
the range and bearing of the landmark relative to the robot’s local
coordinate frame. The measurement model for the ith landmark
is given by [2]g(mi , xk ) = (xi − xk )2 + (y i − yk )2
atan2(y i − yk , xi − xk ) − θk(50)

and the measurements are corrupted by zero-mean i.i.d. Gaussian noise, εk ∼ N (0, R), with covariance R = diag(σr2 , σφ2 ).
In the simulations, the robot speed is 1 m/s on average andthe maximum angular velocity is approximately 1 ◦ /s. Standard deviations of the control noises are σv = 0.8 m/s andσω = 0.5π
180 rad/s. The range-bearing sensor outputs a measurement every T = 1 s, and the sensor has a limited FOV witha ± 90◦ scanning angle and a maximum range of 150 m. The
detection probability is pD = 0.95 within the FOV, zero outside
the FOV, and the detection probability is approximated using
E{pD (m | xk )} in which the expectation is evaluated with respect to the landmark density N (m̂, P). Standard deviationsof measurement noise are σr = 0.8 m and σφ = 0.3π
180 rad. The
clutter intensity inside the FOV is λ = 5 so that the expected
number of false measurements is five per time epoch. The
simulation parameters are the same as used in [35].
1) Particle Degeneracy: We begin the evaluation by examining sample degeneracy of the filters. PFs utilize resampling
to algorithmically limit degeneracy of the particles [32] but asa drawback, diversity in the sample set is decreased as someof the particles are neglected while others are duplicated. After
resampling has been performed enough times, all of the particles
will share a single mutual ancestor and diversity in the sample set
beyond this point is lost. The distance to the common ancestor
is crucial to the performance of RBPF-based SLAM algorithms,
since it defines how large a loop can effectively be closed [53].

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS1029

TABLE IPOSITIONING AND MAPPING ACCURACY OF PHD-SLAM WITHOUTRESAMPLING

Fig. 2. Evaluation metrics as a function of time for the different filters, which
are illustrated using: PHD 2.0a (), PHD 2.0b (), and PHD 3.0 () forwhich the estimated trajectory is shown in Fig. 1. On the bottom, the number
of GM components used in the PHD 3.0 GM-ID approximation, and as shown,data ambiguity increases the number of GM components, which enables the
proposed filter to account for DA uncertainty. Wrong DA can cause a filter todiverge as portrayed by PHD 2.0b at time instance k = 689.

Hence, resampling is disabled in the following analysis to emphasize sample degeneracy. Results for PHD-SLAM 1.0 are
partly omitted in the following analysis since the filter exhibits
severe particle degeneracy and requires very frequent resampling. For brevity, we refer to the filters simply as PHD X.0x
from now on.
The evaluation metrics of an exemplar MCS and the number
of GM components used by the proposed GM-ID approximation
are illustrated in Fig. 2. In the example, the filters only use
one particle and as visualized, the proposed filter is able to
track the robot and map the environment accurately (see also
Fig. 1). On the contrary, PHD 2.0a and PHD 2.0b diverge as
indicated by the evaluation metrics. The importance densityof PHD 2.0a is computed by conditioning the predicted robot
state to the measurements independently to obtain |Mk | × |Zk |independent conditional distributions. Thereafter, the GMM isapproximated as a single Gaussian using weighted averaging
to obtain the mean and covariance of the proposal density. The
method is suboptimal since the dependencies between the robot
and landmarks are mostly ignored. PHD 2.0b approximates theimportance density by first finding the joint approximation of therobot and map, and then by conditioning on the measurementsof a single DA. Although this approach typically yields a moreaccurate approximation of the importance density, it is sensitive to data ambiguities, which can cause the filter to diverge,
as illustrated in Fig. 2. The proposal of PHD 3.0 takes data
ambiguity into account by computing the importance density
approximation for multiple DAs resulting in a GMM proposal,

which is then used for sampling. As illustrated in Fig. 2, theproposed filter is able to handle data ambiguities, at for example
sample k = 689, resulting in good performance even when using
just one particle.
In general, the accuracy of RBPF-based SLAM solutions
can be improved using more particles, and Fig. 3 illustrates
the trajectories of 100 independent particles of an exemplar
MCS. As illustrated, PHD 3.0 is able to constrain uncertaintyof the posterior approximation at a much lower level than thebenchmark solutions, which is advantageous when closing large
loops. The proposal used in PHD 2.0a is overly conservative
and the filter is unable to propagate the posterior approximation
accurately over time leading to satisfactory performance. The
proposal of PHD 2.0b utilizes an importance density approximation similar to the OID improving the performance with respect
to PHD 2.0a. However, wrong DAs increase dispersion of theparticles and the filter requires a high number of particles toaccount for data ambiguities. On the contrary, PHD 3.0 is able
to achieve high accuracy already with one particle since theposterior can be accurately approximated over extended periodsof time and under DA uncertainty. The results with different
number of particles are summarized in Table I and the results
imply that PHD 3.0 has notable benefits with respect to thebenchmark solutions.
The DAs used by PHD 2.0b and PHD 3.0 for approximating the importance density are explicitly defined so that we
can quantify the accuracy of DA for these two filters. Assigning measurements to correct landmarks is measured using thetrue positive ratio (TPR), assigning measurements to clutter
is measured using the true negative ratio (TNR), and valueof one corresponds to perfect DA. For PHD 2.0b, TPR =0.9223 and TNR = 0.9980, and for PHD 3.0, TPR = 0.9829
and TNR = 0.9984. It is important to note that for PHD 3.0,
the DA assignments are ranked in nondecreasing order of cost,12
Γ≤ γk|k−1≤ · · · ≤ γk|k−1. PHD 2.0b always usesthat is, γk|k−1
1
for approximating thethe optimal assignment with cost γk|k−1
importance density. On the other hand, PHD 3.0 samples fromthe GMM according to the mixture weights κtk and it is possible
1
does not result tothat the optimal assignment with cost γk|k−1
the highest weight so that another mixture component is sampledtand κtk is theinstead. The main difference of computing γk|k−1tused prior, γk|k−1
is computed using the predicted vehicle pose,twhereas κk is computed using the IPL approximation. Thanks
to this subtle but important difference, PHD 3.0 is able to makecorrect DAs more frequently, validating the research premisethat the GM-ID is more suited for modeling DA uncertainty.

1030

IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

Fig. 3. In total, 100 particle trajectories estimated using PHD 2.0a, PHD 2.0b, and PHD 3.0 without resampling. In the figures, the landmarks are illustratedusing ( ) and the ground truth trajectory with (). The independent particle trajectories shown with (), the most likely hypotheses illustrated using () and
its map estimate portrayed with ( ). (a) PHD 2.0a. (b) PHD 2.0b. (c) PHD 3.0.

TABLE IIPERFORMANCE SUMMARY USING SYNTHETIC DATA

Comprehensive understanding of the underlying uncertainties
supports inference, which in turn can be used to improve theoverall performance of PHD-SLAM as we have demonstrated
above.
2) Filter Performance: Resampling is a crucial step of mostPFs to limit degeneracy of the algorithm [32], and as summarized in Table II, the performance of all filters improves whenresampling is enabled. Most of the particles sampled from the
proposals of PHD 1.0 and PHD 2.0a have an insignificant weight
as indicated by the low ESS and as a result, these filters rely on
resampling at nearly every time step to avoid sample degeneracy.
ESS of PHD 2.0b and PHD 3.0 is significantly higher, and resampling is performed only at a fraction of times, which increases
the probability to close larger loops effectively. In addition,
more efficient use of particles enables the proposed system toachieve comparative performance and a reduced computational
overhead, or higher accuracy and a comparative run time withrespect to the benchmark PHD-SLAM filters.
To investigate RBPF-based SLAM performance with perfectDAs, we implement an Oracle RBPF algorithm that knows thecorrespondences between measurements and landmarks. The
implemented algorithm is similar to FastSLAM 2.0 [31] with thedifference that the used sampling distribution is replaced with
the IPL-based OID approximation approach [22]. The reason

why we implement FastSLAM 2.0 is that PHD-SLAM does notoffer any advantage over FastSLAM 2.0 if the correspondences
are known since clutter, detections, misdetections, and DAs canbe discarded. The results of the Oracle RBPF are tabulated inTable II and interestingly, PHD 3.0 nearly achieves the same
performance and the difference between the two diminishes as
N grows. Already with 100 particles, PHD 3.0 and Oracle RBPF
yield comparative tracking accuracy but the mapping accuracyof PHD 3.0 is worse. The reason is that PHD 3.0 occasionallyhas a missed or false landmark in the final map and the GOSPA
metric penalizes heavily for such cardinality errors. For Oracle
RBPF, the cardinality error is always zero since the DAs are
known. Since PHD 3.0 has comparative accuracy to the Oracle
RBPF, it can be concluded that the PHD-SLAM filter combinedwith the proposed GM-ID fulfills the objectives of probabilistic
SLAM, that is, accurately estimate the joint posterior density ofthe map and robot trajectory.
3) Computational Complexity: Let M = |MFOV | denote thenumber of landmarks within the FOV and let us assume
each landmark is associated with a measurement for simplicity. The joint importance density approximation utilized by
PHD 2.0b has complexity O(M 3 ), whereas the partitioned update proposed in this article scales linearly as O(M ). Moreover,
the joint approximation requires (dim(x) + dim(m) × M )2 =

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS

Fig. 4. Filter performance using one particle with the benchmark Victoria Park dataset. The GPS coordinates illustrated using ((), and the map estimate portrayed with ( ). (a) PHD-SLAM 2.0a. (b) PHD-SLAM 2.0b. (c) PHD-SLAM 3.0.

(3 + 2 M )2 of memory, which must be allocated dynamically,
whereas the partitioned update only requires 32 + 22 of static
memory. The aforementioned memory requirements are only forthe covariance matrix in (42) and similar memory requirements
also hold for other parameters involved in the importance density
approximation (see Algorithm 1). These beneficial features ofthe partitioned IPL importance density approximation yield toa notable reduction in computational complexity and on average, the partitioned approximation can be computed in 14.0
μs for a single particle and GM component, whereas the joint
approximation takes 173.4 μs. As a result, even though theproposed approach considers multiple DAs in the GM-ID, theoverall filtering algorithm is computationally more efficient than
PHD 2.0b, as tabulated in Table II. With respect to PHD 1.0 and
PHD 2.0a, the higher computational cost of PHD 3.0 can beeasily justified since the number of particles can be reduced
notably resulting in a much more efficient algorithm.
B. Victoria Park Dataset
The algorithms are also tested on a benchmark SLAM dataset
collected with an instrumented vehicle covering a distance ofover 4 km in Victoria Park, Sydney [10]. The vehicle is equippedwith a laser rangefinder, encoders, and GPS. The laser scans areused to obtain range and bearing measurements to nearby trees
and the encoders are used to measure velocity and steering angleof the vehicle. The GPS is used for evaluation purposes only
and it provides ground truth for the vehicle trajectory, while no
ground truth data are available for the locations of the landmarks.
The reader is referred to [10] and [50] for further details on thedataset and used models. To make the scenario more challenging,artificial clutter is added to the original dataset to make thedetection conditions less ideal as in [33]. The clutter measurements follow a Poisson distribution with clutter intensity λ = 5
and the false measurements are uniformly distributed inside theFOV. In the experiment, the FOV is limited to ± 85◦ scanning
angle and 50 m scanning range, and the detection probability
is approximated using pD = 0.7(1 − m̂ − xk /50) within the
4π 2) [rad/s]2 ]) and
FOV. The filters use Q = diag([12 [m/s]2 , ( 180
1π 2
2
2
2
R = diag([1 [m] , ( 180 ) [rad] ]) for the control and measurement covariances, respectively. To demonstrate the capability of1031), the estimated trajectory withTABLE IIIPERFORMANCE SUMMARY USING THE VICTORIA PARK DATASETPHD 2.0b and PHD 3.0 to close large loops, the ESS thresholdto perform resampling is set to TESS = 1 for these two filters inthe Victoria Park experiments.
Fig. 4 illustrates a typical estimation result of the filters, andTable III summarizes performance of the algorithms. The results
are inline with the simulations, PHD 1.0 and PHD 2.0a3 requirea high number of particles to obtain satisfactory performance,
and PHD 2.0b outperforms the two but is prone to diverge if
a low number of particles are used. PHD 3.0 is robust to data
ambiguities and results to the best overall performance. It is
to be noted that correct DA is very challenging in the considered scenario and PHD 3.0 confronts this difficulty by using
more components in the GM-ID approximation, which increases
computational overhead of the algorithm. However, the small
number of particles required by PHD-SLAM 3.0 translates to ahighly efficient algorithm. While the data acquisition phase in
the Victoria Park dataset required 1549 s, the proposed algorithmwith one particle only takes 2.90 s to run, which is only 0.19%
of the experiment duration.

3 The reference implementation of PHD2.0a, which is available at [54], does
not sample the particles according to [35, eq. (51)] but uses the mean of theimportance density instead. In this article, we have implemented PHD2.0a aspresented in [35] and how PFs typically operate, that is, sampling is performed bydrawing random samples from the approximated importance density. In addition,
we have added artificial clutter to the measurements and therefore, the presented
results for PHD 2.0a differ significantly from the ones presented in [35].

1032

Fig. 5. Sensitivity analysis using the synthetic () and Victoria Park ()
datasets. (a) RMSE as a function of detection probability. (b) RMSE as a functionof clutter intensity. In (b), the large changes in RMSE for λ ≥ 5 are caused bya single MCS, which has much higher RMSE with some specific λ values.

C. Discussion
The proposed PHD filter has numerous tuning parameters thatprovide a tradeoff between accuracy and computational complexity. These tuning parameters include the gating threshold,hypotheses pruning parameters, and IPL parameters. We have set
the parameters such that the values provide a good compromise
between accuracy and computational complexity. For example,
increasing Γ̃ improves the accuracy up to some limit, but at thesame time, the computational complexity increases. We have
set the value such that it provides fairly low computational
overhead and still high enough so that the performance cannot
be improved using an even higher value (Γ̃ > 50). The othertuning parameters behave in the same manner. Either increasingor decreasing the parameter value improves the accuracy up tosome limit but it also increases the computational overhead. The
values of the other parameters were adjusted to achieve good
filter performance and moderate computational overhead.
There are also models and parameters that are not precisely
known in real-world experiments. As an example, the detection
statistics are unknown for the Victoria Park dataset since it isa function of the used sensor as well as the environment. In
this work, the detection statistics are approximated using theprobability of detection and clutter intensity parameters. Since
these two parameters are also very central for the PHD filter, wehave analyzed sensitivity of the proposed algorithm to changes
in these parameters for which the results are illustrated in Fig. 5.
As shown, the algorithm is not very sensitive to changes in eitherof the parameters and good accuracy is achieved when pD ≥ 0.4
and λ ≥ 5. It is to be noted that in the Victoria Park experiment,
the used model for the detection probability is sufficient toobtain good accuracy but it has certain limitations since it does
not account for obstructions created by other landmarks andvegetation. Using more accurate detection statistics, which isable to take occlusions into account [55], it is possible to further
improve the filter performance, which we will explore in future
research.

IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

The most significant weakness of the proposed algorithm
is inconsistency of the filter, which is mostly caused by linearization errors combined with resampling. The problem is thatwhenever resampling is performed, an entire trajectory and map
hypotheses are lost permanently for those particles that are notselected. This depletes the number of samples representing pastposes and consequently erodes the statistics of the landmark
estimates conditioned on these past poses. After resampling has
been performed enough times, all of the particles will sharea single mutual ancestor and estimate diversity beyond thispoint is lost forever. As a consequence, the loss of particle
diversity can prevent a consistent long-term estimate of the joint
RFS-SLAM density. The very same problem is encounteredby FastSLAM [56] and, even though not reported in literature,other PHD-SLAM filters. To prolong the time-period over which
RBPF-SLAM solutions are reasonably consistent, it is necessary
to reduce the impact of resampling [56], [57]. In this regard,
the proposed algorithm’s ability to constrain uncertainty of theposterior approximation at a much lower level and the ability toachieve good performance even without resampling can be seen
as a major advantage.
Most filtering-based SLAM methods have been shown to beinconsistent [56], [58]. Fundamentally, inconsistency is causedby the filter’s inability to reflect the unobservable degrees of
freedom of SLAM and the filter tends to erroneously acquire
information along the directions spanned by these unobservablestates [59]. These erroneous updates cannot be undone in thefiltering framework and therefore, more robust SLAM solutions consider the smoothing version of the SLAM problem
instead [7], [8], [27], [28], [29]. A smoothing approach to
SLAM involves not just the most current robot location, but
the entire robot trajectory up to the current time. A significant
advantage of the smoothing approach is that the problem canbe relinearized and wrong DAs can be corrected in the SLAM
front-end. An interesting comparison between PHD-SLAM andGraphSLAM is provided in [30] in which it is shown that
PHD-SLAM can outperform GraphSLAM in scenarios in which
DA is very challenging. Moreover, the authors also present thesmoothing version of PHD-SLAM called Loopy PHD-SLAM
that combines beneficial properties of both SLAM methods. As
the authors discuss in [30, Ch. 3.3], the PHD-SLAM method is
not very efficient and it would benefit from a better importancedensity approximation and the GM-ID proposed in this article
is a good alternative. Moreover, in the GraphSLAM front-end
one needs to solve the DA problem [8] and the method proposedin this article presents one alternative to model DA uncertainty
and for solving the DA problem.
VII. CONCLUSION
This article presents an MH-ID approximation to improvethe efficiency and robustness of landmark-based SLAM. By
modeling the measurements and landmarks as RFSs, an MH-ID
that incorporates DA uncertainties is derived and a tractable
GM-ID approximation is presented that can be sampled fromin closed form. In the proposed GM-ID, each mixture component represents a single DA and the GM representation allows

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS

incorporating multiple DAs into the importance density approximation. An iterative method based on using generalized SLR
combined with IPL is used to compute parameters of the GM
components and a partitioned update strategy that exploits themeasurement model structure is developed.
Analysis was carried out both in a simulated environment
through MCS and an experimental outdoor Victoria Park SLAM
dataset. Results demonstrated the accuracy and robustness ofthe proposed method, as well as, the efficiency of the developed
PHD-SLAM filter. In fact, it is demonstrated that a single particle
is sufficient to generate an accurate map of the environment
even in high clutter scenarios. The newly proposed GM-ID
approximation admits numerous possibilities of future research
into other RBPF-based SLAM approaches or enhancements via
other RFS-SLAM filters. For example, the PMBM filter is expected to improve the robustness to clutter and data ambiguities,
improving the overall performance even more. Moreover, theproposed GM-ID approximation could be directly or with small
modifications used in any particle filtering SLAM approach thatutilize an importance density approximation similar to the oneused in FastSLAM 1.0 and 2.0. As an example, Pareja [30] discussed that the PHD filter for visual SLAM is not very efficientand the method would benefit using a better importance density.
In future work, the proposed method will be evaluated with otherexperimental datasets and the algorithm will be applied to other
SLAM approaches, such as visual SLAM.

g(Zk | Z1:k−1 , x0:k )
=

Γ




i
× ηk|k−1
N mi ; m̂ik|k−1 , Pik|k−1 dm1 . . . dmMk .
Noting that any landmark mi is either detected (φt,ik > 0) or
misdetected (φt,i
=
0),
the
integralovertheproducts
can bekwritten asg(Zk | Z1:k−1 , x0:k )
=

|Z|

j=1



 

c(zjk )



1 − pD m i | x k

i:φt,ik =0φt,izk k | mi , x k

pD m i | x k g
 t,i
×
φ
t,i
c zk ki:φk >0


×

Mk

i=1



Γ


exp(−λc )

t=1

|Z|

j=1





c(zjk )1 − pD m i | x k

i:φt,ik =0i
N (mi ; m̂ik|k−1 , Pik|k−1 ) dmi
× ηk|k−1
 t,i
 i

φ
m
g
zk k | m i , x k
|
x
p
D
k

 t,i
×
φ
t,i
c zk ki:φk >0i× ηk|k−1
N mi ; m̂ik|k−1 , Pik|k−1 dmi .
Finally, rearranging the integrals yieldsg(Zk | Z1:k−1 , x0:k )g(Zk | Z1:k−1 , x0:k )

t=1



1 − pD m i | x ki× ηk|k−1
N mi ; m̂ik|k−1 , Pik|k−1
 t,i
 i

φ
m
g
zk k | m i , x k
p
|
x
D
k

 t,i
×
φ
t,i
c zk ki:φk >0g (Zk | Mk , xk ) p (Mk | Z1:k−1 , x0:k ) δMk .

exp(−λc )c(zjk )

i:φt,ik =0

g (Zk | Z1:k−1 , x0:k )

=

 

×

Next, we plug in (4) for g(Zk | Mk , xk ), p(Mk | Z1:k−1 , x0:k )is approximated using (16) and (19), and since the cardinality is
known for each DA, we get

|Z|

j=1

×

This Appendix provides a more detailed derivation of the approximation of g(Zk | Z1:k−1 , x0:k ) used in the approximationof the MH-ID. First, recall thatΓ

exp(−λc )t=1

APPENDIX

=

1033



i
ηk|k−1
N mi ; m̂ik|k−1 , Pik|k−1 dm1 . . . dmMk .

Regrouping the landmarks mi from the prior with the mi fromthe likelihood and moving terms independent of any mi out of
the integral yields=Γexp(−λc )

t=1

×

|Z|

j=1



i
ηk|k−1c(zjk )
1 − pD m i | x k

i:φt,ik =0
× N mi ; m̂ik|k−1 , Pik|k−1 dmi
×ηik|k−1φt,ic zk ki:φt,i
k >0


 t,i


φ
pD m i | x k g z k k | m i , x k
× N mi ; m̂ik|k−1 , Pik|k−1 dmi
which is (36).

1034IEEE TRANSACTIONS ON ROBOTICS, VOL. 40, 2024

REFERENCES[1] R. Smith, M. Self, and P. Cheeseman, Estimating Uncertain Spatial
Relationships in Robotics. New York, NY, USA: Springer, 1990.
[2] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics (Intelligent
Robotics and Autonomous Agents). Cambridge, MA, USA: MIT Press,
2005.
[3] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “MonoSLAM:
Real-time single camera SLAM,” IEEE Trans. Pattern Anal. Mach. Intell.,vol. 29, no. 6, pp. 1052–1067, Jun. 2007.
[4] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping:Part I,” IEEE Robot. Autom. Mag., vol. 13, no. 2, pp. 99–110, Jun. 2006.
[5] C. Stachniss, D. Hahnel, and W. Burgard, “Exploration with active loopclosing for FastSLAM,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst.,
2004, pp. 1505–1510.
[6] G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid
mapping with Rao-Blackwellized particle filters,” IEEE Trans. Robot.,vol. 23, no. 1, pp. 34–46, Feb. 2007.
[7] S. Thrun and M. Montemerlo, “The Graph SLAM algorithm with applications to large-scale mapping of urban structures,” Int. J. Robot. Res.,
vol. 25, no. 5–6, pp. 403–429, 2006.
[8] G. Grisetti, R. Kümmerle, C. Stachniss, and W. Burgard, “A tutorial
on graph-based SLAM,” IEEE Intell. Transp. Syst. Mag., vol. 2, no. 4,
pp. 31–43, Feb. 2011.
[9] M. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba,“A solution to the simultaneous localization and map building (SLAM)
problem,” IEEE Trans. Robot. Autom., vol. 17, no. 3, pp. 229–241,
Jun. 2001.
[10] J. Guivant and E. Nebot, “Optimization of the simultaneous localizationand map-building algorithm for real-time implementation,” IEEE Trans.
Robot. Autom., vol. 17, no. 3, pp. 242–257, Jun. 2001.
[11] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “FastSLAM: A
factored solution to the simultaneous localization and mapping problem,”in Proc. 18th Nat. Conf. Artif. Intell., 2002, pp. 593–598.
[12] J. Neira and J. Tardos, “Data association in stochastic mapping using
the joint compatibility test,” IEEE Trans. Robot. Autom., vol. 17, no. 6,
pp. 890–897, Dec. 2001.
[13] J. Mullane, B.-N. Vo, M. D. Adams, and B.-T. Vo, “A random-finiteset approach to Bayesian SLAM,” IEEE Trans. Robot., vol. 27, no. 2,
pp. 268–282, Apr. 2011.
[14] J. Mullane, B.-N. Vo, M. D. Adams, and W. S. Wijesoma, “A random
set formulation for Bayesian SLAM,” in Proc. IEEE/RSJ Int. Conf. Intell.
Robots Syst., 2008, pp. 1043–1049.
[15] H. Deusch, S. Reuter, and K. Dietmayer, “The labeled multi-Bernoulli
SLAM filter,” IEEE Signal Process. Lett., vol. 22, no. 10, pp. 1561–1565,
Oct. 2015.
[16] D. Moratuwage, M. Adams, and F. Inostroza, “δ-generalised labelled
multi-Bernoulli simultaneous localisation and mapping,” in Proc. Int.
Conf. Control Autom. Inf. Sci., 2018, pp. 175–182.
[17] Y. Ge, H. Kim, F. Wen, L. Svensson, S. Kim, and H. Wymeersch, “Exploiting diffuse multipath in 5G SLAM,” in Proc. IEEE Glob. Commun.
Conf., 2020, pp. 1–6.
[18] H. Kim, K. Granström, L. Svensson, S. Kim, and H. Wymeersch, “PMBMbased SLAM filters in 5G mmWave vehicular networks,” IEEE Trans. Veh.
Technol., vol. 71, no. 8, pp. 8646–8661, Aug. 2022.
[19] K. P. Murphy, “Bayesian map learning in dynamic environments,” in Proc.
12th Int. Conf. Neural Inf. Process. Syst., 1999, pp. 1015–1021.
[20] O. Kaltiokallio et al., “Towards real-time Radio-SLAM via optimal importance sampling,” in Proc. IEEE 23rd Int. Workshop Signal Process. Adv.
Wireless Commun., 2022, pp. 1–5.
[21] A. F. García-Fernández, L. Svensson, M. R. Morelande, and S. Särkkä,“Posterior linearization filter: Principles and implementation using sigma
points,” IEEE Trans. Signal Process., vol. 63, no. 20, pp. 5561–5573,
Oct. 2015.
[22] R. Hostettler, F. Tronarp, A. F. García-Fernández, and S. Särkkä, “Importance densities for particle filtering using iterated conditional expectations,” IEEE Signal Process. Lett., vol. 27, pp. 211–215, Dec. 2020.
[23] J. Nieto, J. Guivant, E. Nebot, and S. Thrun, “Real time data association for
FastSLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2003, pp. 412–418.
[24] E. Olson and P. Agarwal, “Inference on networks of mixtures for robust
robot mapping,” Int. J. Robot. Res., vol. 32, no. 7, pp. 826–840, 2013.
[25] S. L. Bowman, N. Atanasov, K. Daniilidis, and G. J. Pappas, “Probabilistic
data association for semantic SLAM,” in Proc. IEEE Int. Conf. Robot.
Autom., 2017, pp. 1722–1729.

[26] K. J. Doherty, D. P. Baxter, E. Schneeweiss, and J. J. Leonard, “Probabilistic data association via mixture models for robust semantic SLAM,”
in Proc. IEEE Int. Conf. Robot. Autom., 2020, pp. 1098–1104.
[27] F. Dellaert and M. Kaess, “Square root SAM: Simultaneous localizationand mapping via square root information smoothing,” Int. J. Robot. Res.,
vol. 25, no. 12, pp. 1181–1203, 2006.
[28] D. Koller and N. Friedman, Probabilistic Graphical Models: Principles
and Techniques - Adaptive Computation and Machine Learning. Cambridge, MA, USA: MIT Press, 2009.
[29] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert,“iSAM2: Incremental smoothing and mapping with fluid relinearizationand incremental variable reordering,” in Proc. IEEE Int. Conf. Robot.
Autom., 2011, pp. 3281–3288.
[30] A. Falchetti Pareja, “Random finite sets in visual SLAM,” Master’s thesis,
Elect. Eng. Dept., Universidad de Chile, Santiago, Chile, 2017.
[31] M. Montemerlo, S. Thrun, D. Roller, and B. Wegbreit, “FastSLAM 2.0:
An improved particle filtering algorithm for simultaneous localization andmapping that provably converges,” in Proc. 18th Int. Joint Conf. Artif.
Intell., 2003, pp. 1151–1156.
[32] A. Doucet, S. Godsill, and C. Andrieu, “On sequential Monte Carlosampling methods for Bayesian filtering,” Statist. Comput., vol. 10, no. 3,
pp. 197–2008, 2000.
[33] K. Y. K. Leung, F. Inostroza, and M. Adams, “Multifeature-based importance weighting for the PHD SLAM filter,” IEEE Trans. Aerosp. Electron.
Syst., vol. 52, no. 6, pp. 2697–2714, Dec. 2016.
[34] H. Kim, K. Granström, L. Gao, G. Battistelli, S. Kim, and H. Wymeersch,“5G mmWave cooperative positioning and mapping using multi-model
PHD filter and map fusion,” IEEE Trans. Wireless Commun., vol. 19, no. 6,
pp. 3782–3795, Jun. 2020.
[35] L. Gao, G. Battistelli, and L. Chisci, “PHD-SLAM 2.0: Efficient SLAM in
the presence of missdetections and clutter,” IEEE Trans. Robot., vol. 37,
no. 5, pp. 1834–1843, Oct. 2021.
[36] M. Adams, B.-N. Vo, R. Mahler, and J. Mullane, “SLAM gets a PHD:
New concepts in map estimation,” IEEE Robot. Automat. Mag., vol. 21,
no. 2, pp. 26–37, Jun. 2014.
[37] R. P. Mahler, Advances in Statistical Multisource-Multitarget Information
Fusion. Norwood, MA, USA: Artech House, 2014.
[38] R. P. Mahler, “Multitarget Bayes filtering via first-order multitarget moments,” IEEE Trans. Aerosp. Electron. Syst., vol. 39, no. 4, pp. 1152–1178,
Oct. 2003.
[39] T. P. Minka, “A family of algorithms for approximate Bayesian inference,”
Ph.D. dissertation, Dept. Elect. Eng. Comp. Sci., Massachusetts Inst.
Technol., Cambridge, MA, USA, 2001.
[40] B.-N. Vo and W.-K. Ma, “The Gaussian mixture probability hypothesis
density filter,” IEEE Trans. Signal Process., vol. 54, no. 11, pp. 4091–4104,
Nov. 2006.
[41] S. Särkkä, Bayesian Filtering and Smoothing. Cambridge, U.K.: Cambridge Univ. Press, 2013.
[42] Y. Bar-Shalom, Multitarget-Multisensor Tracking: Advanced Applications. Norwood, MA, USA: Artech House, 1990.
[43] M. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial
on particle filters for online nonlinear/non-Gaussian Bayesian tracking,”
IEEE Trans. Signal Process., vol. 50, no. 2, pp. 174–188, Feb. 2002.
[44] A. Doucet and A. M. Johansen, “A tutorial on particle filtering and smoothing: Fifteen years later,” in Handbook of Nonlinear Filtering, vol. 12, D.
Crisan and B. Rozovskii, Eds., Oxford, U.K.: Oxford Univ. Press, 2011,
pp. 656–704.
[45] C. S. Lee, D. E. Clark, and J. Salvi, “SLAM with dynamic targets via
single-cluster PHD filtering,” IEEE J. Sel. Topics Signal Process., vol. 7,
no. 3, pp. 543–552, Jun. 2013.
[46] K. G. Murty, “An algorithm for ranking all the assignments in order ofincreasing cost,” Operations Res., vol. 16, no. 3, pp. 682–687, 1968.
[47] H. W. Kuhn, “The Hungarian method for the assignment problem,” Nav.
Res. Logistics Quart., vol. 2, no. 1–2, pp. 83–97, 1955.
[48] B.-N. Vo, B.-T. Vo, and D. Phung, “Labeled random finite sets and theBayes multi-target tracking filter,” IEEE Trans. Signal Process., vol. 62,
no. 24, pp. 6554–6567, Dec. 2014.
[49] I. Cox and M. Miller, “On finding ranked assignments with application
to multitarget tracking and motion correspondence,” IEEE Trans. Aerosp.
Electron. Syst., vol. 31, no. 1, pp. 486–489, Jan. 1995.
[50] “Victoria park SLAM data set,” J. Guivant, Australian centre for field
robotics - The University of Sydney. Accessed: Aug. 11, 2022. [Online]. Available: http://www-personal.acfr.usyd.edu.au/nebot/victoria_
park.htm

KALTIOKALLIO et al.: MULTIHYPOTHESES IMPORTANCE DENSITY FOR SLAM IN CLUTTERED SCENARIOS[51] C. Kim, R. Sakthivel, and W. K. Chung, “Unscented FastSLAM: A robustand efficient solution to the SLAM problem,” IEEE Trans. Robot., vol. 24,
no. 4, pp. 808–820, Aug. 2008.
[52] A. S. Rahmathullah, A. F. García-Fernández, and L. Svensson, “Generalized optimal sub-pattern assignment metric,” in Proc. IEEE 20th Int. Conf.
Inf. Fusion, 2017, pp. 1–8.
[53] M. Montemerlo, “FastSLAM: A factored solution to the simultaneous
localization and mapping problem with unknown data association,” Ph.D.
dissertation, Robot. Inst., Carnegie Mellon Univ., Pittsburgh, PA, Jul. 2003.
[54] “Matlab implementation of PHD-SLAM 2.0,” Lin Gao, School of Information and Communication Engineering, Univ. Electron. Sci. Technol.
China, Chengdu, China. Accessed: Jun. 27, 2023. [Online]. Available:
https://github.com/gao12345ab/PHD-SLAM-2.0[55] F. Inostroza, M. Adams, and K. Leung, “Modeling detection statistics infeature-based robotic navigation for range sensors,” Navigation, vol. 65,
no. 3, pp. 297–318, 2018.
[56] T. Bailey, J. Nieto, and E. Nebot, “Consistency of the FastSLAM algorithm,” in Proc. IEEE Int. Conf. Robot. Autom., 2006, pp. 424–429.
[57] L. Zhang, X. jiong Meng, and Y. wu Chen, “Convergence and consistency analysis for FastSLAM,” in Proc. IEEE Intell. Veh. Symp., 2009,
pp. 447–452.
[58] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, “Consistency of
the EKF-SLAM algorithm,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots
Syst., 2006, pp. 3562–3568.
[59] M. Brossard, A. Barrau, and S. Bonnabel, “Exploiting symmetries todesign EKFs with consistency properties for navigation and SLAM,” IEEE
Sensors J., vol. 19, no. 4, pp. 1572–1579, Feb. 2019.

Ossi Kaltiokallio (Member, IEEE) received the
Ph.D. degree in electrical engineering from Aalto
University, Espoo, Finland, in 2017.
He has held Postdoctoral Fellow position with the
University of Utah, Aalto University, and Tampere
University. He is currently a Senior Research Fellow with the Electrical Engineering Unit, Tampere
University, Tampere, Finland. His current researchinterests include the intersection of statistical signal
processing and wireless networking for improving
radio-based positioning and sensing technologies.
Roland Hostettler (Member, IEEE) received theDipl. Ing. degree in electrical and communicationengineering from the Bern University of Applied
Sciences, Bern, Switzerland, in 2007, and the M.Sc.
degree in electrical engineering and the Ph.D. degreein automatic control from the Luleå University ofTechnology, Luleå, Sweden, in 2009 and 2014, respectively.
He has held Postdoctoral Researcher positions with
the Luleå University of Technology, Sweden, andAalto University, Finland. He is currently an Associate Professor with the Department of Electrical Engineering, Uppsala University, Uppsala, Sweden. His research interests include statistical signal processing
and sensor fusion and their applications.
Dr. Hostettler is a Member of the IEEE Machine Learning for Signal Processing Technical Committee.
Yu Ge (Graduate Student Member, IEEE) received
the B.E. degree from Zhejiang University, Hangzhou,
China, in 2017, and the M.Sc. degree from the KTH
Royal Institute of Technology, Stockholm, Sweden,in 2019, both in electrical engineering. He is currently
working toward the Ph.D. degree in electrical engineering with the Department of Electrical and Engineering, Chalmers University of Technology, Gothenburg, Sweden.
His research interests include integrated communication and sensing, wireless positioning systems,
simultaneous localization and mapping, and multi-object tracking, particularly
in 5G and beyond 5G scenarios.

1035

Hyowon Kim (Member, IEEE) received the Ph.D.
degree in department of electronics engineering fromthe Department of Electronic Engineering, Hanyang
University, Seoul, South Korea, in 2021.
From 2019 to 2020, he was a Visiting Researcher with the Department of Electrical Engineering, Chalmers University of Technology, Sweden, where from 2021 to 2023, he was a Marie
Skłodowska-Curie Fellow/Postdoctoral Researcher.
He is currently an Assistant Professor with the Department of Electronics Engineering, Chungnam National University, Daejeon, South Korea. His main research interests include
wireless communications and integrated sensing/localization/communications,in 5G and beyond 5G communication systems.

Jukka Talvitie (Member, IEEE) received the M.Sc.
degree in automation engineering and the Ph.D. degree in computing and electrical engineering fromthe Tampere University of Technology, Tampere, Finland, in 2008 and 2016, respectively.
He is currently a University Lecturer with the Unit
of Electrical Engineering, Tampere University, Tampere. His research interests include signal processing
for wireless communications, radio-based positioning and sensing, radio link waveform design, andradio system design, particularly concerning 5G andbeyond mobile technologies.

Henk Wymeersch (Fellow, IEEE) received the Ph.D.
degree in electrical engineering/applied sciencesfrom Ghent University, Ghent, Belgium, in 2005.
He is currently a Professor of communication systems with the Department of Electrical Engineering, Chalmers University of Technology, Gothenburg,
Sweden. His current research focuses on the convergence of communication and sensing, in a 5G andbeyond 5G context.
Dr. Wymeersch is currently a Senior Member of theIEEE Signal Processing Magazine Editorial Board.
During 2019–2021, he was an IEEE Distinguished Lecturer with the VehicularTechnology Society.

Mikko Valkama (Fellow, IEEE) received the M.Sc.
(Tech.) and D.Sc. (Tech.) degrees (both with honors)
in electrical engineering from the Tampere Universityof Technology, Tampere, Finland, in 2000 and 2001,
respectively.
In 2003, he was with the Communications Systems
and Signal Processing Institute, SDSU, San Diego,
CA, as a Visiting Research Fellow. He is currently a
Full Professor and the Head of the Unit of ElectricalEngineering with the newly formed Tampere University, Tampere. His research interests include radio
communications, radio localization, and radio-based sensing, with particularemphasis on 5G and 6G mobile radio networks.