IEEE Robotics & Automation Magazine - June 2014 - 32

Inputs

Filtering Actions

Prior
Measurement Set
at Time k - 1

Output

Birth Gaussians
1) For Each Prior Measurement
zk-1
z1k-1 to zk-1 do:
Per Particle GM-PHD
Birth with Jb,k = bZk-1]
Weighted Gaussians

a) Birth Gaussian mean
(i)

nb,k = (hspatial)-1 (zk-1, X k-1)

zk-1
Zk-1 = #z1k-1, z2k-1...zk-1
-

b) Birth Gaussians Covariance

Pb,k = 4hspatial anb,k, X k-1k # R
(i)

Extract Previous Pose
Estimate from Particle i
(i)
(i)
(Xk-1) $ Xk-1bk-1

Particle i
Trajectory Particles
at Time k - 1

# 94hspatial anb,k, X k-1kC
(i)

T

c) Set Constant Weight
for Gaussian
wb,k = a

b(mb(Xk)

= #nb,k, Pb,k,wb,k-j=1
(j)

end for

(j)

( j)

Jb,k

Predicted Map Gaussians
2) For Each of Jk-1bk-1 Prior Map
Components, Do:

Per Particle GM-PHD
Map Prior with Jk-1bk-1
Weighted Gaussians

Per Particle GM-PHD
Propagated Prior
with Jk-1bk-1
Weighted Gaussians

a) Predicted Gaussian Mean
(Static Map Assumed)
nkbk-1 = nk-1bk-1

vk-1bk-1amb(X k-1) k =
(i)

#nk-1bk-1, Pk-1bk-1,
(j)

k-1bk-1
wk-1bk-1- j=1

(j)

c) Predicted Gaussian Weights
(Static Map Assumed)
wkbk-1 = wk-1bk-1

#nkbk-1, Pkbk-1,
(j)

(j)

(j)

Jkbk-1
j=1

vk-1bk-1amb(X k-1)(i)k =

#nkbk-1, Pkbk-1,
(j)

( j)

wkbk-1- j=1
(j)

J

vkbk-1amb(X k-1)(i)k =
wkbk-1-

b) Predicted Gaussian Covariance
(Static Map Assumed)
Pkbk-1 = Pk-1bk-1

(j)

Per Particle GM-PHD
Predicted Map
with Jkbk-1 =
J b,k + Jk-1bk-1
Weighted
g
Gaussians

Jk-1bk-1

Figure 3. The implementation of the PHD predictor (9).

measurement and the false alarm variable c k (z). The false
alarm PHD c k (z) represents the prior knowledge of the
probability of false alarm
Pfa . In a scan in which
The PHD filter tracks only
N d feature detection
hypotheses are made
the overall feature map
(whether determined to
be detections or not), an
behavior.
average of n c = Pfa N d
false measurements will
result. Therefore, c k (z) = n c VU (z), where n c is the false
alarm rate per scan, V is the volume (or area in the 2-D
experiments presented here) corresponding to the FoV of the
sensor(s), and U (z) is a uniform distribution over the measurement space. The missed detection and weighted prediction Gaussian components are then merged to form the
J k | k -1 + (J k | k -1 # z k) Gaussians forming the updated GM(i)
PHD v k | k ^m | ^ X k -1h h .
It is important to note that, unlike in vector-based SLAM
implementations, feature initialization, termination, and
association routines are unnecessary in the PHD filter implementation. A reduced weight copy of the feature predictions
are incorporated into the final map estimate, allowing for the
32

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

June 2014

possibility that the sensor(s) may have missed them. In addition, all measurements are fused with all predictions, so that
no assumptions on the nature of the measurements is necessary. Finally, to curb the explosive growth in the number of
Gaussians formed between the prediction and update stages
of the RB-PHD-SLAM filter, Gaussian merging and pruning
can be adopted as shown in Figure 4. Note that although a
computational limit has to be set, in terms of the number of
Gaussians realizable, no feature pruning or map management heuristics were necessary in this implementation, and
the final weights of each Gaussian maintain an estimate of
the number of features they represent. The equivalent computational limit in a vector-based, MH-FastSLAM implementation would require heuristic-based curbing of measurements and map predictions, forcing each to be of the
same, computationally manageable dimensions, with no
such Bayesian estimate on the true number of features,
which have intersected the FoV of the vehicle's sensor(s).
Updating the Vehicle Trajectory
Particles-Implementation
The PHD-SLAM filter adopts a particle approximation of the
posterior vehicle trajectory, p k | k (X k) . Figure 5 shows how
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - June 2014
