IEEE Robotics & Automation Magazine - June 2014 - 54

landmark detection uncertainty, and false measurements (clutter)
in a single filtering step. Using this RFS SLAM framework as the
basis, Moratuwage et al. proposed two RFS CMSLAM solutions
in [6] and further improved their original CMSLAM solution in
[7] to address the CMSLAM with the moving object tracking
problem. Moreover, in [8], an RFS-based hierarchical solution to
the CMSLAM was proposed to reduce the communication
bandwidth requirements.
In this article, we evaluate the performance of this RFS
CMSLAM [6] solution under dynamic high-clutter conditions. A series of simulations are carried out under varying
clutter conditions, and the results are benchmarked against a
state-of-the-art CMSLAM solution [9]. Next, a comprehensive
field experiment is conducted in an actual environment containing high measurement clutter to evaluate the performance
of the RFS CMSLAM solution in adverse field conditions.
RFS Multivehicle SLAM Problem
In this article, we summarize the RFS CMSLAM solution for
a two-vehicle scenario where two vehicles collaboratively perform CMSLAM and build a single global map and estimate
the vehicle trajectories with respect to this map.
The landmark map at time k is denoted by the set
M k = {m k,1, m k,2, ..., m k, lk}, where l k denotes the number of
landmarks present in the map. Let the time sequence of the
(r)
pose history of each vehicle be denoted by X 1: k =
(r)
(r) T
(r)
(r)
6X 1 , X 2 , ..., X k @ , where X k denotes the pose of the vehicle,
r, and the time, k. Let the time sequence of sets of range measurements obtained using range and bearing sensors mounted
(r)
(r)
on each vehicle be denoted by Z 1: k =6Z (1r), Z (2r), ..., Z k @, where
(r)
(r)
(r) (r)
Z k = $ z k,1, z k,2, ..., z k, n(kr) . denotes the measurement set
(r)
received from vehicle r at time k, while n k denotes the num(r)
(r) T
(r)
ber of measurements. Let U 1: k = 6U 1 , U (2r), ..., U k @ denote
the time sequence of control commands applied to each vehi(r)
cle r (r = 1, 2) up to time k, where U k denotes the control
command applied at time k. Using this information, we are
required to evaluate the posterior probability distribution
(1)

(2)

(1)

(2)

(1)

(2)

(2)
p k | k (M k, X 1: k, X 1: k | Z 1: k, Z 1: k, U 1: k, U 1: k, X (1)
0 , X0 )

(1)

(2)
to solve the CMSLAM problem. In (1), X (1)
0 and X 0 , respectively, denote the initial poses of the first and second vehicles.

RFS Multivehicle SLAM Solution
The CMSLAM posterior (1) is propagated by factorizing as a
product of the joint vehicle trajectories posterior and the
landmark map posterior conditioned on the vehicle trajectories as follows:
(1)
1: k

p k | k (M k, X , X

(2)
1: k

(1)
(2)
(1)
(2)
(1)
0
1: k
1: k
1: k
1: k
(1)
(2)
(1)
(2)
k
1: k
1: k
0: k
0: k
(1)
(2)
(1)
(2)
( 1)
1: k
1: k
1: k
1: k
1: k

(2)
0

| Z ,Z ,U ,U ,X ,X )

= p k | k (M | Z , Z , X , X )
# p k | k (X , X

(2)
| Z , Z , U , U 1: k, X 0(1), X 0(2)) . (2)

Now the joint vehicle trajectories posterior is evaluated using
a Rao-Blackwellized particle filter, and the landmark map
posterior is propagated using a PHD filter.
54

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

June 2014

Map Transition Model
Using the landmark map at time k-1, M k-1, the landmark
map transition model at time k is represented by

'

M k = C k (X k , X k ) , ;
(1)

(2)

gk - 1 ! Mk - 1

(1)

Y (g k - 1)E,

(3)

(2)

where the RFS C k (X k , X k ) denotes the newly appearing
landmarks in the joint sensor field of view (FOV), while the
Bernoulli RFS Y (g k - 1) denotes the predicted state of the
landmark g k - 1 ! M k .
Measurement Model
Let M k denote the predicted landmark map at time k. Then,
the measurement set received from exteroceptive sensor
mounted on the rth vehicle at time k is represented as a RFS
as follows:
Zk = Ck , ;
(r)

(r)

'

H k (g k)E,
(r)

gk ! Mk

(4)

(r)

where C k denotes the RFS of the measurement clutter
(r)
received from the exteroceptive sensor and H k (g k) is a Bernoulli RFS representing the measurement corresponding to
the observation of landmark g k ! M k . Due to the limited
(r)
(r)
FOV of the sensor, H k (g k) takes a value of the form " z k ,
(r)
(r)
with probability of detection P D (g k | X k ) or 4 with a proba(r)
bility of 1 - P D(r) (g k | X k ). The probability of detecting a landmark that is located outside of the sensor FOV is zero.
Landmark Map Posterior Estimation
The landmark map posterior is propagated via a recursive
prediction/update procedure using a PHD filter [10]. This
allows for the dynamic estimation of the landmark map in the
presence of measurement clutter with data association uncertainty and detection uncertainty.
Landmark Map Prediction
The PHD of the predicted landmark map posterior is given as
(1)

(2)

(1)

(2)

D k | k -1 (g k | Z 1: k -1, Z 1: k -1, X 0: k, X 0: k)
(1)

(2)

= b k (g k | X k , X k )
(1)

(2)

(1)

(2)

+ D k -1 | k -1 (g k | Z 1: k -1, Z 1: k -1, X 0: k -1, X 0: k -1) dg k -1,
(5)
(1)

(2)

where b k (g k | X k , X k ) denotes the intensity (expected number of landmarks per unit area) of the newly appearing landmarks in the joint sensor FOV.
Landmark Map Update
Assuming that the number of false measurements produced
by each vehicle is Poisson distributed at an average of m (r) and
(r)
their physical distribution given by c k (z (r)), we obtain the



Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2014

IEEE Robotics & Automation Magazine - June 2014 - Cover1
IEEE Robotics & Automation Magazine - June 2014 - Cover2
IEEE Robotics & Automation Magazine - June 2014 - 1
IEEE Robotics & Automation Magazine - June 2014 - 2
IEEE Robotics & Automation Magazine - June 2014 - 3
IEEE Robotics & Automation Magazine - June 2014 - 4
IEEE Robotics & Automation Magazine - June 2014 - 5
IEEE Robotics & Automation Magazine - June 2014 - 6
IEEE Robotics & Automation Magazine - June 2014 - 7
IEEE Robotics & Automation Magazine - June 2014 - 8
IEEE Robotics & Automation Magazine - June 2014 - 9
IEEE Robotics & Automation Magazine - June 2014 - 10
IEEE Robotics & Automation Magazine - June 2014 - 11
IEEE Robotics & Automation Magazine - June 2014 - 12
IEEE Robotics & Automation Magazine - June 2014 - 13
IEEE Robotics & Automation Magazine - June 2014 - 14
IEEE Robotics & Automation Magazine - June 2014 - 15
IEEE Robotics & Automation Magazine - June 2014 - 16
IEEE Robotics & Automation Magazine - June 2014 - 17
IEEE Robotics & Automation Magazine - June 2014 - 18
IEEE Robotics & Automation Magazine - June 2014 - 19
IEEE Robotics & Automation Magazine - June 2014 - 20
IEEE Robotics & Automation Magazine - June 2014 - 21
IEEE Robotics & Automation Magazine - June 2014 - 22
IEEE Robotics & Automation Magazine - June 2014 - 23
IEEE Robotics & Automation Magazine - June 2014 - 24
IEEE Robotics & Automation Magazine - June 2014 - 25
IEEE Robotics & Automation Magazine - June 2014 - 26
IEEE Robotics & Automation Magazine - June 2014 - 27
IEEE Robotics & Automation Magazine - June 2014 - 28
IEEE Robotics & Automation Magazine - June 2014 - 29
IEEE Robotics & Automation Magazine - June 2014 - 30
IEEE Robotics & Automation Magazine - June 2014 - 31
IEEE Robotics & Automation Magazine - June 2014 - 32
IEEE Robotics & Automation Magazine - June 2014 - 33
IEEE Robotics & Automation Magazine - June 2014 - 34
IEEE Robotics & Automation Magazine - June 2014 - 35
IEEE Robotics & Automation Magazine - June 2014 - 36
IEEE Robotics & Automation Magazine - June 2014 - 37
IEEE Robotics & Automation Magazine - June 2014 - 38
IEEE Robotics & Automation Magazine - June 2014 - 39
IEEE Robotics & Automation Magazine - June 2014 - 40
IEEE Robotics & Automation Magazine - June 2014 - 41
IEEE Robotics & Automation Magazine - June 2014 - 42
IEEE Robotics & Automation Magazine - June 2014 - 43
IEEE Robotics & Automation Magazine - June 2014 - 44
IEEE Robotics & Automation Magazine - June 2014 - 45
IEEE Robotics & Automation Magazine - June 2014 - 46
IEEE Robotics & Automation Magazine - June 2014 - 47
IEEE Robotics & Automation Magazine - June 2014 - 48
IEEE Robotics & Automation Magazine - June 2014 - 49
IEEE Robotics & Automation Magazine - June 2014 - 50
IEEE Robotics & Automation Magazine - June 2014 - 51
IEEE Robotics & Automation Magazine - June 2014 - 52
IEEE Robotics & Automation Magazine - June 2014 - 53
IEEE Robotics & Automation Magazine - June 2014 - 54
IEEE Robotics & Automation Magazine - June 2014 - 55
IEEE Robotics & Automation Magazine - June 2014 - 56
IEEE Robotics & Automation Magazine - June 2014 - 57
IEEE Robotics & Automation Magazine - June 2014 - 58
IEEE Robotics & Automation Magazine - June 2014 - 59
IEEE Robotics & Automation Magazine - June 2014 - 60
IEEE Robotics & Automation Magazine - June 2014 - 61
IEEE Robotics & Automation Magazine - June 2014 - 62
IEEE Robotics & Automation Magazine - June 2014 - 63
IEEE Robotics & Automation Magazine - June 2014 - 64
IEEE Robotics & Automation Magazine - June 2014 - 65
IEEE Robotics & Automation Magazine - June 2014 - 66
IEEE Robotics & Automation Magazine - June 2014 - 67
IEEE Robotics & Automation Magazine - June 2014 - 68
IEEE Robotics & Automation Magazine - June 2014 - 69
IEEE Robotics & Automation Magazine - June 2014 - 70
IEEE Robotics & Automation Magazine - June 2014 - 71
IEEE Robotics & Automation Magazine - June 2014 - 72
IEEE Robotics & Automation Magazine - June 2014 - 73
IEEE Robotics & Automation Magazine - June 2014 - 74
IEEE Robotics & Automation Magazine - June 2014 - 75
IEEE Robotics & Automation Magazine - June 2014 - 76
IEEE Robotics & Automation Magazine - June 2014 - 77
IEEE Robotics & Automation Magazine - June 2014 - 78
IEEE Robotics & Automation Magazine - June 2014 - 79
IEEE Robotics & Automation Magazine - June 2014 - 80
IEEE Robotics & Automation Magazine - June 2014 - 81
IEEE Robotics & Automation Magazine - June 2014 - 82
IEEE Robotics & Automation Magazine - June 2014 - 83
IEEE Robotics & Automation Magazine - June 2014 - 84
IEEE Robotics & Automation Magazine - June 2014 - 85
IEEE Robotics & Automation Magazine - June 2014 - 86
IEEE Robotics & Automation Magazine - June 2014 - 87
IEEE Robotics & Automation Magazine - June 2014 - 88
IEEE Robotics & Automation Magazine - June 2014 - 89
IEEE Robotics & Automation Magazine - June 2014 - 90
IEEE Robotics & Automation Magazine - June 2014 - 91
IEEE Robotics & Automation Magazine - June 2014 - 92
IEEE Robotics & Automation Magazine - June 2014 - 93
IEEE Robotics & Automation Magazine - June 2014 - 94
IEEE Robotics & Automation Magazine - June 2014 - 95
IEEE Robotics & Automation Magazine - June 2014 - 96
IEEE Robotics & Automation Magazine - June 2014 - 97
IEEE Robotics & Automation Magazine - June 2014 - 98
IEEE Robotics & Automation Magazine - June 2014 - 99
IEEE Robotics & Automation Magazine - June 2014 - 100
IEEE Robotics & Automation Magazine - June 2014 - 101
IEEE Robotics & Automation Magazine - June 2014 - 102
IEEE Robotics & Automation Magazine - June 2014 - 103
IEEE Robotics & Automation Magazine - June 2014 - 104
IEEE Robotics & Automation Magazine - June 2014 - Cover3
IEEE Robotics & Automation Magazine - June 2014 - Cover4
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2023
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2023
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2023
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2023
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2022
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2022
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2022
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2022
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2021
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2021
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2021
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2021
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2020
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2020
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2020
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2020
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2019
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2019
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2019
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2019
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2018
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2018
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2018
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2018
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2017
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2017
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2017
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2017
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2016
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2016
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2016
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2016
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2015
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2015
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2015
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2015
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2014
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2014
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2014
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2014
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2013
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2013
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2013
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2013
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2012
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2012
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2012
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2012
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2011
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2011
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_june2011
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_march2011
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_december2010
https://www.nxtbook.com/nxtbooks/ieee/roboticsautomation_september2010
https://www.nxtbookmedia.com