IEEE Robotics & Automation Magazine - June 2014 - 31
this filter is opposite to that of conventional vector-based
approaches, which require external methods to fix the number of map features and then attempt to optimize their location estimates. The PHD filter tracks only the overall feature
map behavior and then attempts to detect and track individual features as new measurements are made.
A PHD function v k must have the following two properties:
● The mass (integral of the density over the volume) of the
PHD within a given spatial region S gives the expected
number of features in S.
● As a consequence, the peaks of the PHD indicate locations
with high probability of feature existence.
RFS SLAM with the PHD Filter
The aforementioned RB SLAM implementation, which uses
a PHD approximation for the set-based map, is adopted,
conditioned on the vehicle trajectory. A weighted sum-ofGaussians is used as the PHD function, and the mapping
recursion is approximated by a GM-PHD filter. The trajectory recursion adopts a particle filter [3]. This is referred to
as RB-PHD-SLAM.
The map is predicted with a GM form of the PHD predictor, the implementation of which will be explained in the
next section.
v k | k -1 (m | X k) = v k -1 | k -1 (m | X k -1) + b (m | X k)
1 444 2 4 44 3
1 4444 2 4444 3
1 44 2 44 3 ,
Predicted PHD
Prior PHD
Birth PHD
(9)
where v k -1 | k -1 (m | X k -1) is the previous GM estimate of
the PHD, v k | k -1 (m | X k) is its prediction at time k, and
b (m | X k) is the GM-PHD of the birth RFS, used to model
the new features predicted to enter the FoV of the vehicle's
sensor(s), i.e., the bracketed term on the right hand side
(RHS) of (7). Here, b (m | X k) is similar to the proposal function used in particle filters and is used to give some a priori
information to the filter about where features are likely to
appear in the map. In SLAM, with no a priori information,
b (m | X k) may be uniformly distributed in a noninformative
manner about the space of features. However, in this article,
the feature birth proposal at time k is chosen to be a GM
containing J b, k Gaussian components, representing the set of
measurements at time k - 1, Z k -1 [7].
The PHD corrector equation is [2]
where v k | k (m | X k) is the new GM estimate of the PHD at
time k, K (m | X k) = PD (m | X k) g k (z | m, X k), and
PD (m | X k) = the probability of detecting a land
mark at m, from vehicle pose X k .
c k (z) = PHD of the clutter RFS C k in
(4) at time k.
Implementing the RB-PHD-SLAM Filter
The PHD-SLAM density at time k - 1 can be represented by
a set of N particles, each accompanied by their own GMPHDs representing their belief of the map. The RB-PHDSLAM filter then proceeds to approximate the posterior density by a new set of weighted particles according to the block
diagrams in Figures 3-5.
Per Particle PHD Mapping-Implementation
Prediction-Implementing (9)
Figure 3 implements the predictor (9) on a per-trajectory particle basis. First, the J b, k birth Gaussians replicate the spatial locations of the | Z k -1 | prior measurements (i.e., J b, k =| Z k -1 |h,
-1
using the inverse spatial measurement model ^h spatialh ($),
and are each assigned equal weight. Second, each prior map
Gaussian is predicted forward in time yielding J k -1 | k -1 propagated Gaussians. For a static map (assumed here), these propagated Gaussians simply equal the prior, in terms of their means,
covariances, and weights. Any knowledge of dynamic map
behavior would be incorporated at this point. Finally, the J b, k
birth and J k -1 | k -1 propagated Gaussians are added to form the
J k | k -1 = J b, k + J k -1 | k -1 predicted Gaussians on the RHS of
Figure 3, thus implementing (9).
Correction-Implementing (10)
Based on the predicted GM-PHD, if the measurement likelihood g k (z | m, X k) is also of Gaussian form, it follows from
(i)
(i)
(10) that the posterior map PHD, v k | k (m | X k ) is also a GM.
Figure 4 shows the per trajectory particle update implementation procedure of (10).
Note that in the filtering actions block update GM-PHD
missed detection components, the means and covariance of
all the J k | k -1 predicted Gaussians are simply copied into the
posterior GM-PHD map estimate but with their weights
reduced by the probability of missed detection 1 (i)
PD ` m | X k | k - 1 j . This takes into account the possibility that
they may not be observed in the new measurement set Z k .
v k | k (m | X k) =
v k | k -1 (m | X k) (1 - PD (m | X k))
1 44 2 44 3
1 4444444 2 4444444 3
This represents the first term on the RHS of (10).
Posterior
All predicted features weighted
To implement the second term on the RHS of (10), each
PHD
by their probabilities of missed detection of the J k | k -1 predicted Gaussian component's spatial means
and covariances are corrected by each of the z k measureK
m
X
(
|
)
k
+ v k | k -1 (m | X k) /
, ments. This can be achieved by the standard extended Kalz ! Z k c k (z) + #
K (p | X k) v k | k -1 (p | X k) dp man filter equations, as described in Figure 4. The weights of
Mk
1 44444444444444 2
44444444444444 3 each of these J k | k -1 # z k new Gaussian components are
All predicted features, updated by the spatial locations
updated based on the probability of detection of each preof all the new measurements, and their probabilities
dicted Gaussian, the Mahalanobis distance between that
of detection
(10) component's predicted spatial measurement and each actual
June 2014
*
IEEE ROBOTICS & AUTOMATION MAGAZINE
*
31
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