advanced robotics-24 s6
Post on 06-Apr-2018
224 Views
Preview:
TRANSCRIPT
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 120
Advanced Robotics 24 (2010) 585ndash604brillnlar
Full paper
Analysis of Rank-Based Resampling Based on Particle
Diversity in the RaondashBlackwellized Particle Filter for
Simultaneous Localization and Mapping
Nosan Kwak alowast Kazuhito Yokoi a and Beom-Hee Lee b
a
Humanoid Research GroupJoint Robotics Laboratory UMI3218CRT National Institute of Advanced Industrial Science and Technology Central 2 AIST Umezono 1-1-1 Tsukuba
Ibaraki 305-8568 Japanb
School of Electrical Engineering and Computer Science Seoul National University Gwanak 599
Gwanak-gu Seoul 151-742 South Korea
Received 6 March 2009 accepted 10 June 2009
Abstract
In order to solve the simultaneous localization and mapping (SLAM) problem of mobile robots the Raondash
Blackwellized particle filter (RBPF) has been intensively employed However it suffers from particle
depletion problem ie the number of distinct particles becomes smaller during the SLAM process As
a result the particles optimistically estimate the SLAM posterior meaning that particles tend to underesti-
mate their own uncertainty and the filter quickly becomes inconsistent The main reason of loss of particle
diversity is the resampling process of RBPF-SLAM Standard resampling algorithms for RBPF-SLAM can-
not preserve particle diversity due to the behavior of their removing and replicating particles Thus we
propose rank-based resampling (RBR) which assigns selection probabilities to resample particles based on
the rankings of particles In addition we provide an extensive analysis on the performance of RBR includ-
ing scheduling of resampling Through the simulation results we show that the estimation capability of
RBPF-SLAM by RBR outperforms that by standard resampling algorithms More importantly RBR pre-
serves particle diversity much longer so it can prevent a certain particle from dominating the particle set
and reduce the estimation errors In addition through consistency tests it is shown that RBPF-SLAM by the
standard resampling algorithms is optimistically inconsistent but RBPF-SLAM by RBR is so pessimisti-
cally inconsistent that it gives a chance to reduce the estimation errors
copy Koninklijke Brill NV Leiden and The Robotics Society of Japan 2010
Keywords
SLAM particle diversity resampling ranking consistency
To whom correspondence should be addressed E-mail nosan-kwakaistgojp
copy Koninklijke Brill NV Leiden and The Robotics Society of Japan 2010 DOI101163016918610X487126
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 220
586 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
1 Introduction
The RaondashBlackwellized particle filter (RBPF) was introduced about a decade ago
as an effective means to solve the simultaneous localization and mapping (SLAM)
problem by Murphy [1] and Doucet et al [2] This solution to SLAM is RBPF-
SLAM which is also known as FastSLAM [3] The SLAM posterior is normally
the joint estimation of a robotrsquos path and a map However RBPF-SLAM factors the
joint estimation using RaondashBlackwellization which factors a state into a sampled
part (path) and an analytical part (map) In RBPF-SLAM the robotrsquos path is esti-
mated by a particle filter and the map by low-dimensional extended Kalman filters
(EKFs)
RBPF-SLAM has two major advantages compared to other approaches (i) By
factoring the SLAM posteriors RBPF-SLAM has linear time complexity (ii) Un-like EKF-SLAM RBPF-SLAM allows each particle to perform its own data asso-
ciation which implements multi-hypothesis data association [3] The ability to si-
multaneously pursues multi-hypothesis data association makes RBPF-SLAM more
robust to data association problems than algorithms based on incremental maxi-
mum likelihood data association such as EKF-SLAM This special characteristic
is dependent on particle diversity The bigger the number of distinct particles the
more chance to close a loop because new observations can affect the locations of
the landmark [4] Thus it is crucial for RBPF-SLAM to maintain particle diversityas long as possible
However the innate disadvantage of RBPF-SLAM is that past pose estimation
errors of a robot are not forgotten which means that they are recorded in the feature
estimates Whenever the resampling process is conducted the entire robot paths
and feature estimates of rejected particles are lost forever As a result the number
of particles representing past paths and feature estimates severely decreases This
is called the particle depletion problem [5] In other words as the particle set loses
its diversity it becomes over-confident which means it tends to underestimate itsown uncertainty According to the work by Bailey et al [4] the particle diversity is
drastically attenuated when a robot closes a large traverse loop
Another critical issue in RBPF-SLAM is the consistency which is the ability of
the filter to accurately estimate uncertainty A filter can be inconsistent in either
an optimistic or a pessimistic way According to Ref [6] a filter is optimistic or
over-confident if there is significant bias in the estimates the errors are too large
compared to the filter-calculated covariance or the covariance is too small On the
other hand a filter is pessimistic or conservative if the covariance is too large It hasbeen shown in simulation works [4 7] that the current RBPF-SLAM algorithm is in-
consistent in an optimistic way and Stachniss et al [8] showed in their experiments
that RBPF-SLAM is consistent only in a local area (or in the short term) According
to Ref [4] RBPF-SLAM in its current form cannot produce consistent estimates
in the long term although it stays reasonably consistent for a few tens of seconds
after starting Beevers et al [7] improved the consistency by applying block pro-
posal distribution in the sampling process which exploits future information Their
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 587
modified strategies however cannot guarantee a consistent filter in the long term
The consistency of a filter is closely related to particle depletion which is directly
related to the resampling process in RBPF-SLAM Thus we extensively conducted
analysis on standard resampling algorithms to investigate the relationship between
particle diversity and resampling algorithms in the previous work [9] According toour results all resampling algorithms cannot preserve particle diversity
The existing results show that the loss of particle diversity causes critical prob-
lems such as poor data association and inconsistent estimates Even though RBPF-
SLAM thanks to accurate sensors is applicable to practical problems it is desirable
to guarantee its performance over the long term One-particle RBPF-SLAM some-
times shows results as good as one with 100 particles This can be interpreted that
the single particle estimates the SLAM posterior with the help of accurate sensors
However if the only particle has large estimation errors then there is no way to
compensate for the errors Besides according to our previous work [10] perfor-
mance by taking the mean of particles is better than that by the most weighted
particle Thus the only way for consistent estimates over the long term is to keep
particle diversity as long as possible
To preserve particle diversity in this work we propose rank-based resampling
(RBR) which is an indirect resampling algorithm since it uses the ranking of a par-
ticle for resampling Actually we briefly introduced the RBR in our previous work [9] but we underestimated its effectiveness for keeping particle diversity In this
work we thoroughly analyze RBR in terms of particle diversity and consistency
of RBPF-SLAM and emphasize its capabilities For the organization of this work
a brief introduction to RBPF-SLAM including its particle diversity and consistency
will be presented in Section 2 RBR will be described in Section 3 and its per-
formance will be investigated in Section 4 with estimation errors particle diversity
and consistency of RBPF-SLAM Finally concluding remarks on the capability of
RBR will be given in Section 5
2 RBPF-SLAM
The structure of SLAM enables particle filters to be applicable since the SLAM
problem is characterized by a conditional independence between any two disjoint
sets of landmarks in the map given the robotrsquos pose [11] It means if the robotrsquos true
path was given locations of all landmarks would be estimated independently Thisspecial particle filter is known as RBPF In this section the algorithm of SLAM
using RBPF (RBPF-SLAM) is briefly introduced in terms of particle diversity and
consistency of RBPF-SLAM
21 RBPF-SLAM Algorithm
RBPF-SLAM enables us to factor the SLAM posterior into a product of simpler
terms The key mathematical insight of RBPF-SLAM pertains to the fact that the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 420
588 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
full SLAM posterior can be factored as [11]
p(x1t M |z1t u1t c1t )= p(x1t |z1t u1t c1t )
N f n=1
p(mn|x1t z1t c1t ) (1)
where x1t is the robot path up to time t M (mn is nth landmark and there are N f
landmarks) is the map and z1t u1t and c1t are the measurements controls and
correspondences up to time t respectively RBPF-SLAM uses a particle filter to
estimate the robotrsquos pose and EKFs to estimate the robotrsquos map More specifically
the mapping problem can be factored into separate low-dimensional EKFs using
the conditional independence among landmarks [3]
A particle at time t Y [k]t is denoted by
Y [k]t =
x
[k]1t μ
[k]1t
[k]1t μ
[k]N f t
[k]N f t
(2)
where the [k] indicates the index of the particle and x[k]1t is the robot path estimate
of the kth particle at time t μ[k]nt and
[k]nt are mean and covariance of the Gaussian
distribution representing the nth feature location relative to the kth particle respec-
tively Altogether these elements form the kth particle Y [k]t and there are a total of
N p particles and N f features in a particle set The RBPF-SLAM algorithm consists
of four steps as follows [11]
(i) Sampling x[k]t sim p(xt |x
[k]t minus1 z1t u1t c1t )
(ii) Measurement update For each observed feature zit identify the correspon-
dence j for the measurement zit and incorporate the measurement zit into the
corresponding EKF by updating the mean μ[k]jt and covariance
[k]jt
(iii) Importance weight Calculate the importance weight w[k] for the new particle
(iv) Resampling Sample N p particles with replacement where each particle is
sampled with a probability proportional to w[k]
22 Resampling Algorithms
In common particle filtering resampling is used to reduce the particle degener-
acy which occurs because particles or samples have negligible weights over time
Through resampling (removing particles with low weights and replicating more
particles in more probable regions) a particle set can better reflect the true poste-rior of SLAM However resampling makes particles with high weights be selected
more and more often As a result after a few iterations of the algorithm the par-
ticles with high weights dominate the particle set Thus a particle cannot perform
its own function because it is a copy of the dominant particle at a certain point
The most commonly used resampling algorithms in SLAM are different variants
of stratified sampling such as residual resampling (RR) and systematic resampling
(SR) SR is the most commonly used since it is the fastest resampling algorithm
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 589
Table 1
RSR algorithm
Algorithm RSR(wN in N out)
Input A set of normalized weights w
and number of inputs N in and outputs N out
Output A set of numbers to replicate each particle N R
Generate a random number U 0 sim U([0 1N out
])
for i = 1 to N in
N [i]R = [(w[i] minusU iminus1) middotN out] + 1
U i =U iminus1 +N [i]R N out minusw[i]
end
for computer simulations Bolic et al [12] proposed the residual systematic re-
sampling (RSR) which produces an identical resampling result as SR with fewer
operations and less memory access In the previous work [9] we confirmed that
RSR for RBPF-SLAM shows the best performance among the variants of stratified
resampling approaches Thus we will compare our resampling algorithm with RSR
The algorithm of RSR is presented in Table 1 where RSR draws the first uniform
random number U 0 =U 0 and updates it by U i =U iminus1 +N [i]R N out minusw
[i]n
The output N R is an array of indices which means how many times each particle is
replicated for the next particle set In the RSR algorithm the updated uniform ran-
dom number is formed in a different fashion compared to the standard SR That is
it requires only one iteration loop In addition in RSR resampling is performed in
fixed time whereas in SR it is not performed in fixed time because the number of
replicated particles is random which makes an unspecified number of operations
23 Particle Diversity
In the resampling step particles are resampled based on their importance weights
which are computed by the ratio of the target or posterior distribution and the pro-
posal distribution for sampling as [11]
w[k]t =
target distribtuion
proposal distribution
= p(x[k]
1t |u1t z1t c1t )p(x
[k]1t minus1|u1t minus1 z1t minus1 c1t minus1)p(x
[k]t |x
[k]1t minus1 u1t z1t c1t )
(3)
where it is assumed that paths in x[k]1t minus1 have been generated according to the tar-
get distribution one step earlier p(x[k]1t minus1|u1t minus1 z1t minus1 c1t minus1) Note that the most
recent measurement zt is used to construct the proposal distribution from which
particles are sampled If the sensor is very accurate relative to the motion model
the target distribution will be sharply peaked relative to a flat proposal distribution
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 620
590 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 1 Number of distinct particles over time
After resampling a small percentage of particles are assigned non-negligible im-
portance weights causing significant duplication of a few dominant particles Once
the particles are removed in the set particle diversity cannot be recovered because
particles share the robot path and feature estimates at some point This is the parti-cle depletion problem Over time particle depletion could result in particles drifting
away from the true state [13]
A measure for the rate of loss of particle diversity is obtained by recording the
number of distinct particles having different estimates for a landmark in the set
Once a landmark goes out of the robotrsquos sight resampling causes some particles to
be rejected and others to be replicated At first all of the particles are distinct which
means they have different feature estimates about a landmark As time passes only
particles with high weights survive and particles with low weights disappear to-gether with their feature estimates Thus the number of distinct estimates of the
landmark becomes smaller The number of distinct particles is counted after every
resampling process and its transition (the result is obtained from simulations in the
environment of Fig 6 with the condition in Section 41) in the case of using RSR
is shown in Fig 1 Soon after closing a loop the ratio of distinct particles becomes
smaller than 3 of the initial distinct particles As is seen by this example parti-
cle depletion often occurs and due to this particle depletion RBPF-SLAM might
produce very inaccurate estimates
24 Consistency of RBPF-SLAM
The χ2 distribution is often used to check state estimators for consistency ie
whether their actual errors are consistent with the variances calculated by the esti-
mator [6] For the RBPF-SLAM algorithm to measure if a filter is consistent one
would compare its estimate with the probability density function (PDF) obtained
from an ideal Bayesian filter The PDF is however not available for the RBPF-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 220
586 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
1 Introduction
The RaondashBlackwellized particle filter (RBPF) was introduced about a decade ago
as an effective means to solve the simultaneous localization and mapping (SLAM)
problem by Murphy [1] and Doucet et al [2] This solution to SLAM is RBPF-
SLAM which is also known as FastSLAM [3] The SLAM posterior is normally
the joint estimation of a robotrsquos path and a map However RBPF-SLAM factors the
joint estimation using RaondashBlackwellization which factors a state into a sampled
part (path) and an analytical part (map) In RBPF-SLAM the robotrsquos path is esti-
mated by a particle filter and the map by low-dimensional extended Kalman filters
(EKFs)
RBPF-SLAM has two major advantages compared to other approaches (i) By
factoring the SLAM posteriors RBPF-SLAM has linear time complexity (ii) Un-like EKF-SLAM RBPF-SLAM allows each particle to perform its own data asso-
ciation which implements multi-hypothesis data association [3] The ability to si-
multaneously pursues multi-hypothesis data association makes RBPF-SLAM more
robust to data association problems than algorithms based on incremental maxi-
mum likelihood data association such as EKF-SLAM This special characteristic
is dependent on particle diversity The bigger the number of distinct particles the
more chance to close a loop because new observations can affect the locations of
the landmark [4] Thus it is crucial for RBPF-SLAM to maintain particle diversityas long as possible
However the innate disadvantage of RBPF-SLAM is that past pose estimation
errors of a robot are not forgotten which means that they are recorded in the feature
estimates Whenever the resampling process is conducted the entire robot paths
and feature estimates of rejected particles are lost forever As a result the number
of particles representing past paths and feature estimates severely decreases This
is called the particle depletion problem [5] In other words as the particle set loses
its diversity it becomes over-confident which means it tends to underestimate itsown uncertainty According to the work by Bailey et al [4] the particle diversity is
drastically attenuated when a robot closes a large traverse loop
Another critical issue in RBPF-SLAM is the consistency which is the ability of
the filter to accurately estimate uncertainty A filter can be inconsistent in either
an optimistic or a pessimistic way According to Ref [6] a filter is optimistic or
over-confident if there is significant bias in the estimates the errors are too large
compared to the filter-calculated covariance or the covariance is too small On the
other hand a filter is pessimistic or conservative if the covariance is too large It hasbeen shown in simulation works [4 7] that the current RBPF-SLAM algorithm is in-
consistent in an optimistic way and Stachniss et al [8] showed in their experiments
that RBPF-SLAM is consistent only in a local area (or in the short term) According
to Ref [4] RBPF-SLAM in its current form cannot produce consistent estimates
in the long term although it stays reasonably consistent for a few tens of seconds
after starting Beevers et al [7] improved the consistency by applying block pro-
posal distribution in the sampling process which exploits future information Their
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 587
modified strategies however cannot guarantee a consistent filter in the long term
The consistency of a filter is closely related to particle depletion which is directly
related to the resampling process in RBPF-SLAM Thus we extensively conducted
analysis on standard resampling algorithms to investigate the relationship between
particle diversity and resampling algorithms in the previous work [9] According toour results all resampling algorithms cannot preserve particle diversity
The existing results show that the loss of particle diversity causes critical prob-
lems such as poor data association and inconsistent estimates Even though RBPF-
SLAM thanks to accurate sensors is applicable to practical problems it is desirable
to guarantee its performance over the long term One-particle RBPF-SLAM some-
times shows results as good as one with 100 particles This can be interpreted that
the single particle estimates the SLAM posterior with the help of accurate sensors
However if the only particle has large estimation errors then there is no way to
compensate for the errors Besides according to our previous work [10] perfor-
mance by taking the mean of particles is better than that by the most weighted
particle Thus the only way for consistent estimates over the long term is to keep
particle diversity as long as possible
To preserve particle diversity in this work we propose rank-based resampling
(RBR) which is an indirect resampling algorithm since it uses the ranking of a par-
ticle for resampling Actually we briefly introduced the RBR in our previous work [9] but we underestimated its effectiveness for keeping particle diversity In this
work we thoroughly analyze RBR in terms of particle diversity and consistency
of RBPF-SLAM and emphasize its capabilities For the organization of this work
a brief introduction to RBPF-SLAM including its particle diversity and consistency
will be presented in Section 2 RBR will be described in Section 3 and its per-
formance will be investigated in Section 4 with estimation errors particle diversity
and consistency of RBPF-SLAM Finally concluding remarks on the capability of
RBR will be given in Section 5
2 RBPF-SLAM
The structure of SLAM enables particle filters to be applicable since the SLAM
problem is characterized by a conditional independence between any two disjoint
sets of landmarks in the map given the robotrsquos pose [11] It means if the robotrsquos true
path was given locations of all landmarks would be estimated independently Thisspecial particle filter is known as RBPF In this section the algorithm of SLAM
using RBPF (RBPF-SLAM) is briefly introduced in terms of particle diversity and
consistency of RBPF-SLAM
21 RBPF-SLAM Algorithm
RBPF-SLAM enables us to factor the SLAM posterior into a product of simpler
terms The key mathematical insight of RBPF-SLAM pertains to the fact that the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 420
588 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
full SLAM posterior can be factored as [11]
p(x1t M |z1t u1t c1t )= p(x1t |z1t u1t c1t )
N f n=1
p(mn|x1t z1t c1t ) (1)
where x1t is the robot path up to time t M (mn is nth landmark and there are N f
landmarks) is the map and z1t u1t and c1t are the measurements controls and
correspondences up to time t respectively RBPF-SLAM uses a particle filter to
estimate the robotrsquos pose and EKFs to estimate the robotrsquos map More specifically
the mapping problem can be factored into separate low-dimensional EKFs using
the conditional independence among landmarks [3]
A particle at time t Y [k]t is denoted by
Y [k]t =
x
[k]1t μ
[k]1t
[k]1t μ
[k]N f t
[k]N f t
(2)
where the [k] indicates the index of the particle and x[k]1t is the robot path estimate
of the kth particle at time t μ[k]nt and
[k]nt are mean and covariance of the Gaussian
distribution representing the nth feature location relative to the kth particle respec-
tively Altogether these elements form the kth particle Y [k]t and there are a total of
N p particles and N f features in a particle set The RBPF-SLAM algorithm consists
of four steps as follows [11]
(i) Sampling x[k]t sim p(xt |x
[k]t minus1 z1t u1t c1t )
(ii) Measurement update For each observed feature zit identify the correspon-
dence j for the measurement zit and incorporate the measurement zit into the
corresponding EKF by updating the mean μ[k]jt and covariance
[k]jt
(iii) Importance weight Calculate the importance weight w[k] for the new particle
(iv) Resampling Sample N p particles with replacement where each particle is
sampled with a probability proportional to w[k]
22 Resampling Algorithms
In common particle filtering resampling is used to reduce the particle degener-
acy which occurs because particles or samples have negligible weights over time
Through resampling (removing particles with low weights and replicating more
particles in more probable regions) a particle set can better reflect the true poste-rior of SLAM However resampling makes particles with high weights be selected
more and more often As a result after a few iterations of the algorithm the par-
ticles with high weights dominate the particle set Thus a particle cannot perform
its own function because it is a copy of the dominant particle at a certain point
The most commonly used resampling algorithms in SLAM are different variants
of stratified sampling such as residual resampling (RR) and systematic resampling
(SR) SR is the most commonly used since it is the fastest resampling algorithm
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 589
Table 1
RSR algorithm
Algorithm RSR(wN in N out)
Input A set of normalized weights w
and number of inputs N in and outputs N out
Output A set of numbers to replicate each particle N R
Generate a random number U 0 sim U([0 1N out
])
for i = 1 to N in
N [i]R = [(w[i] minusU iminus1) middotN out] + 1
U i =U iminus1 +N [i]R N out minusw[i]
end
for computer simulations Bolic et al [12] proposed the residual systematic re-
sampling (RSR) which produces an identical resampling result as SR with fewer
operations and less memory access In the previous work [9] we confirmed that
RSR for RBPF-SLAM shows the best performance among the variants of stratified
resampling approaches Thus we will compare our resampling algorithm with RSR
The algorithm of RSR is presented in Table 1 where RSR draws the first uniform
random number U 0 =U 0 and updates it by U i =U iminus1 +N [i]R N out minusw
[i]n
The output N R is an array of indices which means how many times each particle is
replicated for the next particle set In the RSR algorithm the updated uniform ran-
dom number is formed in a different fashion compared to the standard SR That is
it requires only one iteration loop In addition in RSR resampling is performed in
fixed time whereas in SR it is not performed in fixed time because the number of
replicated particles is random which makes an unspecified number of operations
23 Particle Diversity
In the resampling step particles are resampled based on their importance weights
which are computed by the ratio of the target or posterior distribution and the pro-
posal distribution for sampling as [11]
w[k]t =
target distribtuion
proposal distribution
= p(x[k]
1t |u1t z1t c1t )p(x
[k]1t minus1|u1t minus1 z1t minus1 c1t minus1)p(x
[k]t |x
[k]1t minus1 u1t z1t c1t )
(3)
where it is assumed that paths in x[k]1t minus1 have been generated according to the tar-
get distribution one step earlier p(x[k]1t minus1|u1t minus1 z1t minus1 c1t minus1) Note that the most
recent measurement zt is used to construct the proposal distribution from which
particles are sampled If the sensor is very accurate relative to the motion model
the target distribution will be sharply peaked relative to a flat proposal distribution
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 620
590 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 1 Number of distinct particles over time
After resampling a small percentage of particles are assigned non-negligible im-
portance weights causing significant duplication of a few dominant particles Once
the particles are removed in the set particle diversity cannot be recovered because
particles share the robot path and feature estimates at some point This is the parti-cle depletion problem Over time particle depletion could result in particles drifting
away from the true state [13]
A measure for the rate of loss of particle diversity is obtained by recording the
number of distinct particles having different estimates for a landmark in the set
Once a landmark goes out of the robotrsquos sight resampling causes some particles to
be rejected and others to be replicated At first all of the particles are distinct which
means they have different feature estimates about a landmark As time passes only
particles with high weights survive and particles with low weights disappear to-gether with their feature estimates Thus the number of distinct estimates of the
landmark becomes smaller The number of distinct particles is counted after every
resampling process and its transition (the result is obtained from simulations in the
environment of Fig 6 with the condition in Section 41) in the case of using RSR
is shown in Fig 1 Soon after closing a loop the ratio of distinct particles becomes
smaller than 3 of the initial distinct particles As is seen by this example parti-
cle depletion often occurs and due to this particle depletion RBPF-SLAM might
produce very inaccurate estimates
24 Consistency of RBPF-SLAM
The χ2 distribution is often used to check state estimators for consistency ie
whether their actual errors are consistent with the variances calculated by the esti-
mator [6] For the RBPF-SLAM algorithm to measure if a filter is consistent one
would compare its estimate with the probability density function (PDF) obtained
from an ideal Bayesian filter The PDF is however not available for the RBPF-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 587
modified strategies however cannot guarantee a consistent filter in the long term
The consistency of a filter is closely related to particle depletion which is directly
related to the resampling process in RBPF-SLAM Thus we extensively conducted
analysis on standard resampling algorithms to investigate the relationship between
particle diversity and resampling algorithms in the previous work [9] According toour results all resampling algorithms cannot preserve particle diversity
The existing results show that the loss of particle diversity causes critical prob-
lems such as poor data association and inconsistent estimates Even though RBPF-
SLAM thanks to accurate sensors is applicable to practical problems it is desirable
to guarantee its performance over the long term One-particle RBPF-SLAM some-
times shows results as good as one with 100 particles This can be interpreted that
the single particle estimates the SLAM posterior with the help of accurate sensors
However if the only particle has large estimation errors then there is no way to
compensate for the errors Besides according to our previous work [10] perfor-
mance by taking the mean of particles is better than that by the most weighted
particle Thus the only way for consistent estimates over the long term is to keep
particle diversity as long as possible
To preserve particle diversity in this work we propose rank-based resampling
(RBR) which is an indirect resampling algorithm since it uses the ranking of a par-
ticle for resampling Actually we briefly introduced the RBR in our previous work [9] but we underestimated its effectiveness for keeping particle diversity In this
work we thoroughly analyze RBR in terms of particle diversity and consistency
of RBPF-SLAM and emphasize its capabilities For the organization of this work
a brief introduction to RBPF-SLAM including its particle diversity and consistency
will be presented in Section 2 RBR will be described in Section 3 and its per-
formance will be investigated in Section 4 with estimation errors particle diversity
and consistency of RBPF-SLAM Finally concluding remarks on the capability of
RBR will be given in Section 5
2 RBPF-SLAM
The structure of SLAM enables particle filters to be applicable since the SLAM
problem is characterized by a conditional independence between any two disjoint
sets of landmarks in the map given the robotrsquos pose [11] It means if the robotrsquos true
path was given locations of all landmarks would be estimated independently Thisspecial particle filter is known as RBPF In this section the algorithm of SLAM
using RBPF (RBPF-SLAM) is briefly introduced in terms of particle diversity and
consistency of RBPF-SLAM
21 RBPF-SLAM Algorithm
RBPF-SLAM enables us to factor the SLAM posterior into a product of simpler
terms The key mathematical insight of RBPF-SLAM pertains to the fact that the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 420
588 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
full SLAM posterior can be factored as [11]
p(x1t M |z1t u1t c1t )= p(x1t |z1t u1t c1t )
N f n=1
p(mn|x1t z1t c1t ) (1)
where x1t is the robot path up to time t M (mn is nth landmark and there are N f
landmarks) is the map and z1t u1t and c1t are the measurements controls and
correspondences up to time t respectively RBPF-SLAM uses a particle filter to
estimate the robotrsquos pose and EKFs to estimate the robotrsquos map More specifically
the mapping problem can be factored into separate low-dimensional EKFs using
the conditional independence among landmarks [3]
A particle at time t Y [k]t is denoted by
Y [k]t =
x
[k]1t μ
[k]1t
[k]1t μ
[k]N f t
[k]N f t
(2)
where the [k] indicates the index of the particle and x[k]1t is the robot path estimate
of the kth particle at time t μ[k]nt and
[k]nt are mean and covariance of the Gaussian
distribution representing the nth feature location relative to the kth particle respec-
tively Altogether these elements form the kth particle Y [k]t and there are a total of
N p particles and N f features in a particle set The RBPF-SLAM algorithm consists
of four steps as follows [11]
(i) Sampling x[k]t sim p(xt |x
[k]t minus1 z1t u1t c1t )
(ii) Measurement update For each observed feature zit identify the correspon-
dence j for the measurement zit and incorporate the measurement zit into the
corresponding EKF by updating the mean μ[k]jt and covariance
[k]jt
(iii) Importance weight Calculate the importance weight w[k] for the new particle
(iv) Resampling Sample N p particles with replacement where each particle is
sampled with a probability proportional to w[k]
22 Resampling Algorithms
In common particle filtering resampling is used to reduce the particle degener-
acy which occurs because particles or samples have negligible weights over time
Through resampling (removing particles with low weights and replicating more
particles in more probable regions) a particle set can better reflect the true poste-rior of SLAM However resampling makes particles with high weights be selected
more and more often As a result after a few iterations of the algorithm the par-
ticles with high weights dominate the particle set Thus a particle cannot perform
its own function because it is a copy of the dominant particle at a certain point
The most commonly used resampling algorithms in SLAM are different variants
of stratified sampling such as residual resampling (RR) and systematic resampling
(SR) SR is the most commonly used since it is the fastest resampling algorithm
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 589
Table 1
RSR algorithm
Algorithm RSR(wN in N out)
Input A set of normalized weights w
and number of inputs N in and outputs N out
Output A set of numbers to replicate each particle N R
Generate a random number U 0 sim U([0 1N out
])
for i = 1 to N in
N [i]R = [(w[i] minusU iminus1) middotN out] + 1
U i =U iminus1 +N [i]R N out minusw[i]
end
for computer simulations Bolic et al [12] proposed the residual systematic re-
sampling (RSR) which produces an identical resampling result as SR with fewer
operations and less memory access In the previous work [9] we confirmed that
RSR for RBPF-SLAM shows the best performance among the variants of stratified
resampling approaches Thus we will compare our resampling algorithm with RSR
The algorithm of RSR is presented in Table 1 where RSR draws the first uniform
random number U 0 =U 0 and updates it by U i =U iminus1 +N [i]R N out minusw
[i]n
The output N R is an array of indices which means how many times each particle is
replicated for the next particle set In the RSR algorithm the updated uniform ran-
dom number is formed in a different fashion compared to the standard SR That is
it requires only one iteration loop In addition in RSR resampling is performed in
fixed time whereas in SR it is not performed in fixed time because the number of
replicated particles is random which makes an unspecified number of operations
23 Particle Diversity
In the resampling step particles are resampled based on their importance weights
which are computed by the ratio of the target or posterior distribution and the pro-
posal distribution for sampling as [11]
w[k]t =
target distribtuion
proposal distribution
= p(x[k]
1t |u1t z1t c1t )p(x
[k]1t minus1|u1t minus1 z1t minus1 c1t minus1)p(x
[k]t |x
[k]1t minus1 u1t z1t c1t )
(3)
where it is assumed that paths in x[k]1t minus1 have been generated according to the tar-
get distribution one step earlier p(x[k]1t minus1|u1t minus1 z1t minus1 c1t minus1) Note that the most
recent measurement zt is used to construct the proposal distribution from which
particles are sampled If the sensor is very accurate relative to the motion model
the target distribution will be sharply peaked relative to a flat proposal distribution
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 620
590 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 1 Number of distinct particles over time
After resampling a small percentage of particles are assigned non-negligible im-
portance weights causing significant duplication of a few dominant particles Once
the particles are removed in the set particle diversity cannot be recovered because
particles share the robot path and feature estimates at some point This is the parti-cle depletion problem Over time particle depletion could result in particles drifting
away from the true state [13]
A measure for the rate of loss of particle diversity is obtained by recording the
number of distinct particles having different estimates for a landmark in the set
Once a landmark goes out of the robotrsquos sight resampling causes some particles to
be rejected and others to be replicated At first all of the particles are distinct which
means they have different feature estimates about a landmark As time passes only
particles with high weights survive and particles with low weights disappear to-gether with their feature estimates Thus the number of distinct estimates of the
landmark becomes smaller The number of distinct particles is counted after every
resampling process and its transition (the result is obtained from simulations in the
environment of Fig 6 with the condition in Section 41) in the case of using RSR
is shown in Fig 1 Soon after closing a loop the ratio of distinct particles becomes
smaller than 3 of the initial distinct particles As is seen by this example parti-
cle depletion often occurs and due to this particle depletion RBPF-SLAM might
produce very inaccurate estimates
24 Consistency of RBPF-SLAM
The χ2 distribution is often used to check state estimators for consistency ie
whether their actual errors are consistent with the variances calculated by the esti-
mator [6] For the RBPF-SLAM algorithm to measure if a filter is consistent one
would compare its estimate with the probability density function (PDF) obtained
from an ideal Bayesian filter The PDF is however not available for the RBPF-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 420
588 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
full SLAM posterior can be factored as [11]
p(x1t M |z1t u1t c1t )= p(x1t |z1t u1t c1t )
N f n=1
p(mn|x1t z1t c1t ) (1)
where x1t is the robot path up to time t M (mn is nth landmark and there are N f
landmarks) is the map and z1t u1t and c1t are the measurements controls and
correspondences up to time t respectively RBPF-SLAM uses a particle filter to
estimate the robotrsquos pose and EKFs to estimate the robotrsquos map More specifically
the mapping problem can be factored into separate low-dimensional EKFs using
the conditional independence among landmarks [3]
A particle at time t Y [k]t is denoted by
Y [k]t =
x
[k]1t μ
[k]1t
[k]1t μ
[k]N f t
[k]N f t
(2)
where the [k] indicates the index of the particle and x[k]1t is the robot path estimate
of the kth particle at time t μ[k]nt and
[k]nt are mean and covariance of the Gaussian
distribution representing the nth feature location relative to the kth particle respec-
tively Altogether these elements form the kth particle Y [k]t and there are a total of
N p particles and N f features in a particle set The RBPF-SLAM algorithm consists
of four steps as follows [11]
(i) Sampling x[k]t sim p(xt |x
[k]t minus1 z1t u1t c1t )
(ii) Measurement update For each observed feature zit identify the correspon-
dence j for the measurement zit and incorporate the measurement zit into the
corresponding EKF by updating the mean μ[k]jt and covariance
[k]jt
(iii) Importance weight Calculate the importance weight w[k] for the new particle
(iv) Resampling Sample N p particles with replacement where each particle is
sampled with a probability proportional to w[k]
22 Resampling Algorithms
In common particle filtering resampling is used to reduce the particle degener-
acy which occurs because particles or samples have negligible weights over time
Through resampling (removing particles with low weights and replicating more
particles in more probable regions) a particle set can better reflect the true poste-rior of SLAM However resampling makes particles with high weights be selected
more and more often As a result after a few iterations of the algorithm the par-
ticles with high weights dominate the particle set Thus a particle cannot perform
its own function because it is a copy of the dominant particle at a certain point
The most commonly used resampling algorithms in SLAM are different variants
of stratified sampling such as residual resampling (RR) and systematic resampling
(SR) SR is the most commonly used since it is the fastest resampling algorithm
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 589
Table 1
RSR algorithm
Algorithm RSR(wN in N out)
Input A set of normalized weights w
and number of inputs N in and outputs N out
Output A set of numbers to replicate each particle N R
Generate a random number U 0 sim U([0 1N out
])
for i = 1 to N in
N [i]R = [(w[i] minusU iminus1) middotN out] + 1
U i =U iminus1 +N [i]R N out minusw[i]
end
for computer simulations Bolic et al [12] proposed the residual systematic re-
sampling (RSR) which produces an identical resampling result as SR with fewer
operations and less memory access In the previous work [9] we confirmed that
RSR for RBPF-SLAM shows the best performance among the variants of stratified
resampling approaches Thus we will compare our resampling algorithm with RSR
The algorithm of RSR is presented in Table 1 where RSR draws the first uniform
random number U 0 =U 0 and updates it by U i =U iminus1 +N [i]R N out minusw
[i]n
The output N R is an array of indices which means how many times each particle is
replicated for the next particle set In the RSR algorithm the updated uniform ran-
dom number is formed in a different fashion compared to the standard SR That is
it requires only one iteration loop In addition in RSR resampling is performed in
fixed time whereas in SR it is not performed in fixed time because the number of
replicated particles is random which makes an unspecified number of operations
23 Particle Diversity
In the resampling step particles are resampled based on their importance weights
which are computed by the ratio of the target or posterior distribution and the pro-
posal distribution for sampling as [11]
w[k]t =
target distribtuion
proposal distribution
= p(x[k]
1t |u1t z1t c1t )p(x
[k]1t minus1|u1t minus1 z1t minus1 c1t minus1)p(x
[k]t |x
[k]1t minus1 u1t z1t c1t )
(3)
where it is assumed that paths in x[k]1t minus1 have been generated according to the tar-
get distribution one step earlier p(x[k]1t minus1|u1t minus1 z1t minus1 c1t minus1) Note that the most
recent measurement zt is used to construct the proposal distribution from which
particles are sampled If the sensor is very accurate relative to the motion model
the target distribution will be sharply peaked relative to a flat proposal distribution
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 620
590 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 1 Number of distinct particles over time
After resampling a small percentage of particles are assigned non-negligible im-
portance weights causing significant duplication of a few dominant particles Once
the particles are removed in the set particle diversity cannot be recovered because
particles share the robot path and feature estimates at some point This is the parti-cle depletion problem Over time particle depletion could result in particles drifting
away from the true state [13]
A measure for the rate of loss of particle diversity is obtained by recording the
number of distinct particles having different estimates for a landmark in the set
Once a landmark goes out of the robotrsquos sight resampling causes some particles to
be rejected and others to be replicated At first all of the particles are distinct which
means they have different feature estimates about a landmark As time passes only
particles with high weights survive and particles with low weights disappear to-gether with their feature estimates Thus the number of distinct estimates of the
landmark becomes smaller The number of distinct particles is counted after every
resampling process and its transition (the result is obtained from simulations in the
environment of Fig 6 with the condition in Section 41) in the case of using RSR
is shown in Fig 1 Soon after closing a loop the ratio of distinct particles becomes
smaller than 3 of the initial distinct particles As is seen by this example parti-
cle depletion often occurs and due to this particle depletion RBPF-SLAM might
produce very inaccurate estimates
24 Consistency of RBPF-SLAM
The χ2 distribution is often used to check state estimators for consistency ie
whether their actual errors are consistent with the variances calculated by the esti-
mator [6] For the RBPF-SLAM algorithm to measure if a filter is consistent one
would compare its estimate with the probability density function (PDF) obtained
from an ideal Bayesian filter The PDF is however not available for the RBPF-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 589
Table 1
RSR algorithm
Algorithm RSR(wN in N out)
Input A set of normalized weights w
and number of inputs N in and outputs N out
Output A set of numbers to replicate each particle N R
Generate a random number U 0 sim U([0 1N out
])
for i = 1 to N in
N [i]R = [(w[i] minusU iminus1) middotN out] + 1
U i =U iminus1 +N [i]R N out minusw[i]
end
for computer simulations Bolic et al [12] proposed the residual systematic re-
sampling (RSR) which produces an identical resampling result as SR with fewer
operations and less memory access In the previous work [9] we confirmed that
RSR for RBPF-SLAM shows the best performance among the variants of stratified
resampling approaches Thus we will compare our resampling algorithm with RSR
The algorithm of RSR is presented in Table 1 where RSR draws the first uniform
random number U 0 =U 0 and updates it by U i =U iminus1 +N [i]R N out minusw
[i]n
The output N R is an array of indices which means how many times each particle is
replicated for the next particle set In the RSR algorithm the updated uniform ran-
dom number is formed in a different fashion compared to the standard SR That is
it requires only one iteration loop In addition in RSR resampling is performed in
fixed time whereas in SR it is not performed in fixed time because the number of
replicated particles is random which makes an unspecified number of operations
23 Particle Diversity
In the resampling step particles are resampled based on their importance weights
which are computed by the ratio of the target or posterior distribution and the pro-
posal distribution for sampling as [11]
w[k]t =
target distribtuion
proposal distribution
= p(x[k]
1t |u1t z1t c1t )p(x
[k]1t minus1|u1t minus1 z1t minus1 c1t minus1)p(x
[k]t |x
[k]1t minus1 u1t z1t c1t )
(3)
where it is assumed that paths in x[k]1t minus1 have been generated according to the tar-
get distribution one step earlier p(x[k]1t minus1|u1t minus1 z1t minus1 c1t minus1) Note that the most
recent measurement zt is used to construct the proposal distribution from which
particles are sampled If the sensor is very accurate relative to the motion model
the target distribution will be sharply peaked relative to a flat proposal distribution
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 620
590 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 1 Number of distinct particles over time
After resampling a small percentage of particles are assigned non-negligible im-
portance weights causing significant duplication of a few dominant particles Once
the particles are removed in the set particle diversity cannot be recovered because
particles share the robot path and feature estimates at some point This is the parti-cle depletion problem Over time particle depletion could result in particles drifting
away from the true state [13]
A measure for the rate of loss of particle diversity is obtained by recording the
number of distinct particles having different estimates for a landmark in the set
Once a landmark goes out of the robotrsquos sight resampling causes some particles to
be rejected and others to be replicated At first all of the particles are distinct which
means they have different feature estimates about a landmark As time passes only
particles with high weights survive and particles with low weights disappear to-gether with their feature estimates Thus the number of distinct estimates of the
landmark becomes smaller The number of distinct particles is counted after every
resampling process and its transition (the result is obtained from simulations in the
environment of Fig 6 with the condition in Section 41) in the case of using RSR
is shown in Fig 1 Soon after closing a loop the ratio of distinct particles becomes
smaller than 3 of the initial distinct particles As is seen by this example parti-
cle depletion often occurs and due to this particle depletion RBPF-SLAM might
produce very inaccurate estimates
24 Consistency of RBPF-SLAM
The χ2 distribution is often used to check state estimators for consistency ie
whether their actual errors are consistent with the variances calculated by the esti-
mator [6] For the RBPF-SLAM algorithm to measure if a filter is consistent one
would compare its estimate with the probability density function (PDF) obtained
from an ideal Bayesian filter The PDF is however not available for the RBPF-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 620
590 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 1 Number of distinct particles over time
After resampling a small percentage of particles are assigned non-negligible im-
portance weights causing significant duplication of a few dominant particles Once
the particles are removed in the set particle diversity cannot be recovered because
particles share the robot path and feature estimates at some point This is the parti-cle depletion problem Over time particle depletion could result in particles drifting
away from the true state [13]
A measure for the rate of loss of particle diversity is obtained by recording the
number of distinct particles having different estimates for a landmark in the set
Once a landmark goes out of the robotrsquos sight resampling causes some particles to
be rejected and others to be replicated At first all of the particles are distinct which
means they have different feature estimates about a landmark As time passes only
particles with high weights survive and particles with low weights disappear to-gether with their feature estimates Thus the number of distinct estimates of the
landmark becomes smaller The number of distinct particles is counted after every
resampling process and its transition (the result is obtained from simulations in the
environment of Fig 6 with the condition in Section 41) in the case of using RSR
is shown in Fig 1 Soon after closing a loop the ratio of distinct particles becomes
smaller than 3 of the initial distinct particles As is seen by this example parti-
cle depletion often occurs and due to this particle depletion RBPF-SLAM might
produce very inaccurate estimates
24 Consistency of RBPF-SLAM
The χ2 distribution is often used to check state estimators for consistency ie
whether their actual errors are consistent with the variances calculated by the esti-
mator [6] For the RBPF-SLAM algorithm to measure if a filter is consistent one
would compare its estimate with the probability density function (PDF) obtained
from an ideal Bayesian filter The PDF is however not available for the RBPF-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 591
SLAM algorithm Instead the true pose of the robot can be known in simulations
but not in real experiments With this information the normalized estimation error
squared (NEES) defined in (4) can be used to investigate the consistency of a filter
[4 6 7] NEES is defined as
εt = (xt minus xt )TP minus1t (xt minus xt ) (4)
where xt P t are the estimated mean and covariance of particles at time t A mea-
sure of filter consistency is obtained by examination of the average NEES over N
Monte-Carlo runs of the filter Under the assumptions that the filter is consistent
and is approximately linear Gaussian εt is χ2 distributed with dim(xt ) degrees
of freedom The consistency of RBPF-SLAM is evaluated by conducting several
Monte-Carlo runs and computing the average NEES Given N runs the average
NEES is obtained as
εt =1
N
N i=1
εit (5)
Given the hypothesis of a consistent linear Gaussian filter N εt has a χ2 density
with N dim(xt ) degrees of freedom Thus in case of three-dimensional robot pose
with N = 50 the 95 probability concentration region for εt is bounded by the in-
terval [236372] [6] If εt rises significantly higher than the upper bound the fileris optimistic or over-confident If it tends below the lower bound the filter is pes-
simistic or conservative The average NEES of the current RBPF-SLAM framework
presented in Fig 2 shows that the filter is not consistent More precisely at first the
filter is pessimistic but after about 3000 time steps it suddenly becomes optimistic
Figure 2 Average NEES of the standard RBPF-SLAM algorithm over 50 Monte-Carlo runs Two
horizontal red lines indicate the upper and the lower bounds of χ2 which are obtained with the
assumption that the filter is consistent
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 820
592 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
3 RBR
31 Ranking for Selection
Direct resampling with importance weights causes loss of particle diversity and
as a result RBPF-SLAM becomes inconsistent in an optimistic way If the opti-mistic estimation becomes erroneous there would be no way to correct the error
in RBPF-SLAM Thus for any particle filters including RBPF preserving particle
diversity is of paramount importance since each distinct particle represents a dif-
ferent hypothesis of the SLAM posterior In other words as the number of distinct
particles becomes smaller several probable regions of the posterior cannot be esti-
mated because the particles that are assigned in the regions have been removed in
the resampling process
The diversity issue also occurs in a genetic algorithm [14] which is a search
technique used in computing to find exact or approximate solutions to optimization
and search problems In a genetic algorithm each gene that represents a solution to
a problem is reproduced using its own fitness The mechanism of the reproduction
process is very similar to the resampling process in particle filtering Most of the
schemes in genetic algorithms cannot overcome premature convergence or diversity
better than the rank-based reproduction [15] A ranking is used as a transformation
function that assigns a new value to a gene based on its fitness By using not a fitness
but a ranking it is possible to slow down the premature convergence Furthermore
it is possible to control the speed of the convergence with varying the ranking func-
tion To take advantage of the rank-based reproduction scheme it is modified in
this work and used as RBR In addition we thoroughly analyze RBR in terms of
particle diversity and consistency of RBPF-SLAM
32 RBR for RBPF-SLAM
In order to employ the rank-based reproduction scheme in the RBPF-SLAM frame-work RBR is described in this section RBR consists of two parts The first part is
assigning a selection probability of a particle using a ranking function The ranking
can be easily obtained by sorting the particles by the magnitude of their importance
weights The second part is standard resampling with the selection probability of
each particle This ranking approach seems to discard information of importance
weights but it actually discards the information about the magnitude of importance
weights and assigns relative magnitude instead Therefore RBR can be called an
indirect resampling algorithm In the current RBPF-SLAM framework the impor-tance weight is the only measure to evaluate the performance of a particle However
when an accurate sensor such as a laser range finder is used the differences of im-
portance weights between the most weighted particle and the others having slight
pose differences are so large that only the most weighted particle dominates the
particle set As a result several particles are suddenly rejected in the particle set
With RBR these ill-balanced performance measures are linearly re-assigned using
the ranking function
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 593
With the selection probabilities of all the particles the RBR performs the stan-
dard resampling RSR In this work a linear ranking function is used to assign the
selection probability of a particle When the ranking function is linear the mean
of the selection probabilities will correspond to the median rank in the particle set
[15] One can think of a non-linear ranking function but it will shift the mean to-ward the top of the set This is not desirable to preserve particle diversity as long
as possible because of larger differences of selection probabilities than in the linear
ranking function Thus the non-linear ranking function is not considered in this
work The slopes of linear functions are adjusted to control the selection pressure
which is the ratio of the best particlersquos selection probability over the average selec-
tion probability of all particles in the set The following linear equation is used as
the ranking function for the selection probability of the kth particle p[k]s
p[k]s =
1
N p
ηmax minus (ηmax minus ηmin)
(rank (k)minus 1)
N p minus 1
(6)
where N p is the number of particles ηmaxN p is the maximum selection probabil-
ity of the highest weight and ηminN p is the minimum selection probability of the
lowest weight The particle at the first ranking gets the highest selection probability
whereas the particle at the last ranking gets the lowest selection probability When
the number of particles is fixed ηmin = 2 minus ηmax 0 should be satisfied and ηmax
usually has a value between 1 and 2 [16] Depending on ηmax the selection proba-bility varies The larger ηmax which means the selection pressure is getting larger
the larger the differences between selection probabilities The relation between se-
lection probabilities and rankings of particles with varying ηmax is plotted in Fig 3
in the case of six particles in the set This ranking approach is inserted into the RBR
Figure 3 Selection probabilities over rankings of particles with varying ηmax
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1020
594 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 2
RBR algorithm for resampling in RBPF-SLAM
Algorithm RBR (w N in N out)
Input A set of normalized weights w and N in and N out
Output A set of numbers to replicate each particle N R
1 Set a value between [12] to ηmax
2 ηmin larr 2 minus ηmax
3 [wsorted I sorted] larr Sort w in a descending order
4 for i = 1 to N in
5 k larr I sorted(i) i is the ranking
5 p[k]s larr (ηmax minus (ηmax minus ηmin) lowast (i minus 1)(N in))N in
6 end for7 N R larr Call RSR( ps N in N out)
algorithm shown in Table 2 where I sorted stores indices of particles in a descending
order ie the first element of I sorted has the highest ranking
33 A Biased Resampler
It is worth noting that RBR has a big difference from the standard resampling algo-rithms such as RSR since RBR uses the indirect information of importance weights
Particles drawn from RBR construct a different distribution from the true posterior
due to the indirect usage of the importance weights In this sense adding new ran-
dom particles also distorts the particle distribution This kind of resampler is called
a biased resampler which violates the lsquounbiasednessrsquo or lsquoproper weightingrsquo condi-
tion defined as follows A random variable X drawn from a proposal distribution q
is said to be properly weighted [17] by a weighting function w(X) with respect to
the target distribution π if for any integrable function hEqh(X)w(X) = Eπ h(X) (7)
A set of random samples and weights (x[k]w[k]) is said to be properly weighted
with respect to π if
limN prarrinfin
N pk=1h(x
[k])w[k]
N pk=1w
[k]=Eπ h(X) (8)
The effect of RBR seems like that of regularization which attempts to create a morediverse posterior density approximation by relocating the particles to a more con-
tinuous distribution [13] However RBR does not draw a new particle Instead it
selects particles taking into account the indirect information of the posterior the
ranking Figure 4 shows the normalized importance weights of all the particles in
case of the RSR In Fig 4 few particles have very high weights whereas most of the
particles have negligible weights even though the weights are normalized There-
fore after RSR only the particles with high weights survive and are replicated
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1120
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 595
Figure 4 Importance weights of all the particles just before the loop-closure when applying RSR
Table 3
Number of replicas of the dominant particles
Particle index 1 2 3 21 46 49 83 87 88 96 100
No replicas 29 1 5 1 2 9 1 1 1 42 8
as shown in Table 3 As shown in the above example peaked weight distribution
severely damages particle diversity and particle depletion often occurs in RBPF-
SLAM since the measurement of an accurate sensor is used to build the samplingdistribution (proposal distribution) It is known that the standard resampling satis-
fying the proper weighting condition cannot resolve the particle depletion problem
[4 9] According to our previous works [10] in the current RBPF-SLAM frame-
work keeping particle diversity is very important because all the particles drawn
from the proposal distribution are valuable When particle diversity is preserved we
showed that mean particle data gives the better estimation results It is worth testing
how RBPF is biased using RBR instead of the unbiased RSR in the perspective
of unbiasedness of an estimator Generally means of particle poses and variancesare unbiased and asymptotically unbiased respectively [6] The unbiasedness of an
estimator is defined as
E[x] = 0 (9)
where x is the estimation error A simulation is conducted for the bias test and the
result is provided in Fig 5 where means of particle paths and features are pre-
sented with the true path and landmarks According to the simulation result the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1220
596 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 5 Path mean and feature mean of all the particles for checking the bias of RBR
particle mean estimates the path and the landmarks correctly The estimation per-
formance of RBR is usually better than that of RSR thanks to the particle diversity
In this sense RBR can be a solution to keep particle diversity even though it does
not satisfy the proper weighting condition In addition results in Refs [18 19] in-
dicate that the proper weighting condition is unnecessary to obtain convergence
results [5]
In this paper strategies that reallocate particles such as artificial evolution [20]
are not considered since the particle filter is used for SLAM which has to deal
with robot pose and the map at the same time Perturbation to the particles cannot
influence the map data that each particle stores
4 Simulation Results
In this work we only conducted simulations of RBPF-SLAM with a mobile robot
since comparing the estimation error of the filters requires the true values of poseand features In the simulation it was assumed that a mobile robot detects point fea-
tures using a laser range scanner that produces the range and the bearing to a feature
Also it was assumed that data association between measurements and features is
known in order to effectively investigate the performance of the filter The simula-
tion works were focused on the consistency and particle diversity of RBPF-SLAM
For this purpose NEES particle diversity and rms estimation errors including
scheduling of RBR and ranking functions were analyzed in this work
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1320
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 597
Figure 6 Sparse environment 54 landmarks The point C is the only loop closing point
41 Simulation Set-up
RBPF-SLAM simulations were performed on a sparse environment shown in Fig 6
(and Fig 9) Simulations were run with 100 particles In Fig 6 there is only one
outer loop-closure the point C In every simulation the mobile robot closed the
large loop twice Note that the resampling process is not conducted at every iter-ation of RBPF-SLAM It is conducted only when the effective sample size [21]
falls below a threshold to keep particle diversity as long as possible We conducted
simulations to compare the performance of several thresholds In addition we also
conducted simulations with varying ηmax in (6) The weights of all the particles are
initialized with the same weight after every resampling The motion noise and the
observation noise of the robot were set to (03 ms 3 s) and (01 m 1) respec-
tively Control and observation times were set to 25 and 200 ms respectively Every
result in the following simulations was obtained by averaging over 50 Monte-Carloruns since results of RBPF-SLAM are different at each run
42 Estimation Errors
In order to compare the localization and mapping performance of RBR we mea-
sured estimation results with varying ηmax of (6) Estimation errors with different
ηmax in the environment of Fig 6 are summarized in Table 4 where rms position
and orientation of the robot pose and feature errors are denoted RMSE P RMSE O
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1420
598 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Table 4
Summary of estimation errors with different ηmax
ηmax RMSE P (m) RMSE O (rad) RMSE F (m)
11 01239 00554 01239
13 01267 00543 01201
20 01637 00491 01529
Table 5
Comparison of estimation errors between RSR and RBR in the environment of Fig 6
Resampling RMSE P (m) RMSE O (rad) RMSE F (m)
RSR 03099 00468 03590
RBR 01267 00543 01201
Remarks minus59 16 minus66
and RMSE F respectively The estimation errors over 50 Monte-Carlo runs were
collected after the robot closed the large loop twice These results were obtained bytaking the mean of all the particles since we confirmed that the mean of particles
produces the best results of RBPF-SLAM [10] According to the results of Table 4
the case of ηmax = 13 showed the least errors overall even though its position er-
ror was slightly larger than the case of ηmax = 11 From now on every result for
RBR was from the simulations with ηmax = 13 In order to compare the estima-
tion performance of RBR with that of RSR simulation results are summarized in
Table 5 Note that again these results are obtained by taking the mean of all the
particles since for instance the error variance of feature errors are much smaller(00033 m) when taking the mean of particles than when taking the most weighted
particle (00381 m) In Table 5 estimation errors of RBPF-SLAM by RBR were
much reduced compared to those of RBPF-SLAM by RSR The bigger error in
the orientation by 16 (=04286) is small compared to the improvements in the
position and the feature errors These estimation improvements come from parti-
cle diversity The estimation improvements in RMSE P and RMSE F were about 59
and 66 respectively In addition the standard deviation of the estimation errors
over 50 runs were much reduced The standard deviations by RBR were 00537( RMSE P) and 00574 m ( RMSE F) while those by RSR were 01049 ( RMSE P) and
01774 m ( RMSE F)
43 Particle Diversity
Comparison of the loss of particle diversity between RBPF-SLAM by RSR and
RBPF-SLAM by RBR is shown in Fig 7 where the time step is presented when
the robot closed the large loop The rate of the loss of particle diversity by RBR is
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1520
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 599
Figure 7 Particle diversity of RBPF-SLAM by RBR decreases almost linearly which is significantly
different from the result of RBPF-SLAM by RSR In both resampling approaches no resampling
occurred after the first loop-closure which is presented as the time step
Table 6
Comparison of estimation errors varying a threshold for resampling
Threshold () RMSE P (m) RMSE O (rad) RMSE F (m)
25 01267 00543 01201
50 01312 00537 01324
75 01513 00534 01563
almost linear whereas that by RSR is exponential After the loop-closure the num-
ber of distinct particles by RBR was more than 50 of the particle size The reason
why the graphs keep the constant value after the loop-closure is because no resam-
pling was conducted after the first loop-closure We confirmed that resampling after
the large loop-closure is not effective for RBPF-SLAM performance Even though
the loss of particle diversity by RBR cannot be prevented RBR makes more than
half of the particle size survive after the robot closes the large loop Related to par-
ticle diversity we also conducted simulations for the time to instigate the RBR TheRBR was conducted when the effective sample size falls below the thresholds as
shown in Table 6 where estimation errors are presented A threshold of 25 for
instance means that the RBR was conducted whenever the ratio of the effective
sample size falls below 25 of the particle size According to the results in Table 6
the case of the 25 threshold showed the most accurate results overall Also note
that the lower the threshold the less computational cost since the lower threshold
means that the resampling occurs less often
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1620
600 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 8 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Two horizon-
tal red lines indicate the upper and the lower bounds of χ2 which are obtained with the assumption
that the filter is consistent Both approaches show that they are not consistent but that by RBR is
inconsistent in a pessimistic way and that by RSR is inconsistent in an optimistic way
44 ConsistencyWe measured the average NEES over 50 Monte-Carlo runs to test whether RBPF-
SLAM by RBR is consistent over the long term and compared the results with the
average NEES of RBPF-SLAM by RSR which are shown in Fig 8 According to
the average NEES of RBPF-SLAM by RBR although it is not consistent (the NEES
is not always inside the two bounds red lines) RBR produces a very different graph
from that of RSR More specifically RBPF-SLAM by RSR is inconsistent in an
optimistic way meaning that the estimated uncertainty is smaller than the true un-
certainty On the other hand RBPF-SLAM by RBR is inconsistent in a pessimisticway meaning that the uncertainty of particles is larger than the true uncertainty
Several SLAM literature [4 7] only report inconsistency of RBPF-SLAM as op-
timistic estimates No instance of pessimistic estimates of RBPF-SLAM has been
reported In our previous work [10] we confirmed that after several loop-closures
one can obtain an accurate map and path by taking the mean of particles when the
particle diversity is preserved even though RBPF-SLAM is pessimistically incon-
sistent When RBPF-SLAM is optimistically inconsistent however there is no way
to induce the better map and path than those of the most weighted particle since the
uncertainty of particles is too small to keep the particle diversity
45 Analysis in a Large Environment
We also analyzed the performance of RBR in a large environment 240 m times 240 m
as shown in Fig 9 The resulting data were also obtained by averaging over 50
Monte-Carlo runs In Table 7 the resulting data such as estimation errors and num-
ber of distinct particles are compared with those of RSR RBR produced about 50
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1720
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 601
Figure 9 Large sparse environment 35 landmarks 240 m times 240 m
Table 7
Summary of simulation results in the large environment
Resampling RMSE P RMSE O RMSE F No distinct
(m) (rad) (m) particles ()
RSR 44857 00696 44575 29
RBR 19060 00873 19306 243
Remarks minus57 25 minus57 84
less rms errors and about 8 times higher particle diversity than RSR and RMSE Oslightly increased The degradation in RMSE O by 25 (=10115) is small com-
pared to the improvement in RMSE P and RMSE F In addition the average NEES
graphs of both RBR and RSR are presented in Fig 10 to examine the consistency
of RBPF-SLAM The average NEES of RBPF-SLAM by RBR is similar to that in
the smaller environment of Fig 8 The RBPF-SLAM by RBR is pessimistically in-
consistent during the SLAM process but its SLAM estimation is much better than
that of when RSR is used for the resampling process for RBPF-SLAM
5 Conclusions
RBPF has been employed in several robotic problems such as SLAM thanks to the
robust data association and the lower computational complexity However it suffers
from the particle depletion problem ie the number of distinct particles becomes
smaller over the RBPF-SLAM process As a result the particle set to estimate the
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1820
602 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
Figure 10 Average NEES over 50 Monte-Carlo runs of RBPF-SLAM by RSR and RBR Also in this
environment that by RBR is inconsistent in a pessimistic way and that by RSR is inconsistent in an
optimistic way
SLAM posterior becomes over-confident which means it tends to underestimate its
own uncertaintyEven though RBPF-SLAM thanks to accurate sensors is applicable to practical
problems it is desirable to guarantee its performance as long as possible However
the standard resampling algorithms of RBPF-SLAM cannot preserve particle diver-
sity after a large loop-closure Thus we analyzed on RBR which assigns selection
probabilities to resample particles based on the rankings of importance weights
The estimation capability of RBPF-SLAM by RBR outperformed that by RSR
which is the commonly used resampling algorithm More specifically RBR pre-
serves particle diversity much longer than RSR so it can prevent some particlesfrom dominating the particle set Through our results we confirmed that when
particle diversity is preserved the estimation performance is almost always better
than the particle depletion case In addition through the consistency test although
RBPF-SLAM by RBR is not consistent the average NEES of RBPF-SLAM by
RBR is bounded during the RBPF-SLAM process whereas that of RBPF-SLAM
by RSR quickly diverges just after the loop-closure
Consequently RBPF-SLAM by RBR can preserve particle diversity much longer
than usual while reducing the estimation errors Although RBR cannot guaranteethe consistency of RBPF-SLAM it can make the filter pessimistically inconsistent
which prevents the filter from diverging
Acknowledgements
This work has been supported by a JSPS Postdoctoral Fellowship for Foreign Re-
searchers (20-08610) and the Brain Korea 21 Project Korea Science and Engineer-
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 1920
N Kwak et al Advanced Robotics 24 (2010) 585ndash604 603
ing Foundation (KOSEF) NRL Program grant funded by the Korean government
(MEST) (R0A-2008-000-20004-0) and the Growth Engine Technology Develop-
ment Program funded by the Ministry of Knowledge Economy
References
1 K Murphy Bayesian Map Learning in Dynamic Environments (Advances in Neural Information
Processing Systems) MIT Press Cambridge MA (1999)
2 A Doucet N de Freitas K Murphy and S Russell RaondashBlackwellized particle filtering for
dynamic Bayesian networks in Proc Conf on Uncertainty in Artificial Intelligence Stanford
CA pp 176ndash183 (2000)
3 M Montemerlo FastSLAM a factored solution to the simultaneous localization and mapping
problem with unknown data association PhD Dissertation Carnegie Mellon University (2003)
4 T Bailey J Nieto and E Nebot Consistency of the FastSLAM algorithm in Proc IEEE Int
Conf on Robotics and Automation Orlando FL pp 424ndash429 (2006)
5 R Merwe A Doucet N de Freitas and E Wan The unscented particle filter Technical Report
CUEDF INFENGTR380 Cambridge University Engineering Department (2000)
6 Y Bar-Shalom X R Li and T Kirubarajan Estimation with Applications to Tracking and Navi-
gation Wiley New York NY (2001)
7 K R Beevers and W H Huang Fixed-lag sampling strategies for particle filtering SLAM in
Proc IEEE Int Conf on Robotics and Automation Rome pp 2433ndash2438 (2007)
8 C Stachniss D Hahnel W Burgard and G Grisetti On actively closing loops in grid-basedFastSLAM Adv Robotics 19 1059ndash1080 (2005)
9 N Kwak G W Kim and B H Lee A new compensation technique based on analysis of resam-
pling process in FastSLAM Robotica 26 205ndash217 (2008)
10 N Kwak B H Lee and K Yokoi Result representation of RaondashBlackwellized particle filter for
Mobile Robot SLAM J Korea Robotics Soc 3 308ndash314 (2008) (in Korean)
11 S Thrun W Burgard and D Fox Probabilistic Robotics MIT Press Cambridge MA (2005)
12 M Bolic P M Djuric and S J Hong Resampling algorithms for particle filters a computational
complexity perspective Eurasip J Appl Signal Process 15 2267ndash2277 (2004)
13 A D Anderson Recovering sample diversity in RaondashBlackwellized particle filters for simultane-
ous localization and mapping Master Thesis MIT Cambridge MA (2006)
14 D E Goldberg Genetic Algorithms in Search Optimization and Machine Learning Kluwer
Boston MA (1989)
15 D Whitley The GENITOR algorithm and selection pressure why rank-based allocation of repro-
ductive trials is best in Proc 3rd Int Conf on Genetic Algorithms Fairfax pp 116ndash121 (1989)
16 K-K Jin Genetic Algorithms and Their Applications Kyo Woo Sa Seoul (2002) (in Korean)
17 J S Liu and R Chen Sequential Monte-Carlo methods for dynamic systems J Am Stat Assoc
93 1032ndash1044 (1998)18 G Kitagawa Monte-Carlo filter and smoother for non-Gaussian nonlinear state space models
J Computat Graph Stat 5 1ndash25 (1996)
19 D Crisan P Moral and T Lyons Discrete filtering using branching and interacting particle sys-
tems Markov Process Related Fields 5 293ndash318 (1999)
20 A Doucet N de Freitas and N Gordon Sequetial Monte-Carlo Methods in Practice Springer
Berlin (2000)
21 J S Liu and R Chen Blind deconvolution via sequential imputations J Am Stat Ass 90
567ndash576 (1995)
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
832019 Advanced Robotics-24 s6
httpslidepdfcomreaderfulladvanced-robotics-24-s6 2020
604 N Kwak et al Advanced Robotics 24 (2010) 585ndash604
About the Authors
Nosan Kwak received the PhD degree (MS and PhD integrated course) in Elec-
trical Engineering and Computer Science from Seoul National University South
Korea in 2008 and the BE in Mechanical Engineering from Ajou University
South Korea in 2001 He is currently a Japan Society for the Promotion of Sci-ence (JSPS) Postdoctoral Fellow at Humanoid Research Group National Institute
of Advanced Industrial Science and Technology (AIST) at Tsukuba Japan He
was a postdoctoral researcher at AIST until September 2008 He also belongs to
the Joint Robotics Laboratory where he researches Visual SLAM for a humanoid
robots using particle filters His major research fields are SLAM particle filtering game theory and
altruistic behaviors Overall his goal in robotics is to develop pragmatic robots in order to use them
in daily life
Kazuhito Yokoi received his BE degree in Mechanical Engineering from NagoyaInstitute of Technology in 1984 and the ME and PhD degrees in Mechanical
Engineering Science from Tokyo Institute of Technology in 1986 and 1994 re-
spectively In 1986 he joined the Mechanical Engineering Laboratory Ministry
of International Trade and Industry He was the Co-director of the ISAIST-CNRS
Joint Robotics Laboratory He is currently the Deputy Director of the Intelli-
gent Systems Research Institute National Institute of Advanced Industrial Science
and Technology at Tsukuba and is the Group Leader of the Humanoid Research
Group He is also an Adjunctive Professor of the Cooperative Graduate School at University Tsukuba
From November 1994 to October 1995 he was a Visiting Scholar at the Robotics Laboratory Com-
puter Science Department Stanford University His research interests include humanoids human-
centered robotics and intelligent robot systems He is a Member of the IEEE Robotics and Automation
Society
Beom-Hee Lee received the BS and MS degrees in Electronics Engineering from
Seoul National University in 1978 and 1980 respectively and the PhD degree in
Computer Information and Control Engineering from the University of Michigan
Ann Arbor MI USA in 1985 Since then he was associated with the School of
Electrical Engineering at Purdue University as an Assistant Professor until 1987
He is now with the School of Electrical Engineering and Computer Science as a
Professor and with the Office of Information Systems as Dean at Seoul National
University His major research interests include motion planning and control of
robot manipulators multi-robot operation sensor fusion applications and factory automation He has
been the President of the Korea Robotics Society since December 2008
top related