Accepted Manuscript

KLD Sampling with Gmapping Proposal for Monte Carlo Localization

of Mobile Robots

Robin Ping Guan, Branko Ristic, Liuping Wang, Jennifer L. Palmer

PII: S1566-2535(17)30330-5
DOI: https://doi.org/10.1016/j.inffus.2018.09.003
Reference: INFFUS 1013

To appear in: Information Fusion

Received date: 20 May 2017

Revised date: 2 July 2018
Accepted date: 3 September 2018

Please cite this article as: Robin Ping Guan, Branko Ristic, Liuping Wang, Jennifer L. Palmer, KLD
Sampling with Gmapping Proposal for Monte Carlo Localization of Mobile Robots, Information Fusion
(2018), doi: https://doi.org/10.1016/j.inffus.2018.09.003

This is a PDF file of an unedited manuscript that has been accepted for publication. As a service
to our customers we are providing this early version of the manuscript. The manuscript will undergo
copyediting, typesetting, and review of the resulting proof before it is published in its final form. Please
note that during the production process errors may be discovered which could affect the content, and
all legal disclaimers that apply to the journal pertain.
 ACCEPTED MANUSCRIPT

Highlights

• Proposing new algorithm that combines KLD sampling with Gmap-

ping proposal distribution.

• Reducing the number of required particles with an accurate proposal

T
distribution.

IP
• Simulation evaluation to show performance improvement over KLD
sampling.

CR
• Experimental evaluation to show greater accuracy with a smaller num-
ber of particles.

US
AN
M
ED
PT
CE
AC

1
 ACCEPTED MANUSCRIPT

KLD Sampling with Gmapping Proposal for Monte Carlo

Localization of Mobile Robots

Robin Ping Guana , Branko Ristica , Liuping Wanga,∗, Jennifer L. Palmerb

T
a
RMIT University, School of Engineering, Melbourne, Australia
b

IP
DST Group, Aerospace Division, Fishermans Bend, VIC, Australia

CR
Abstract
The paper proposes an algorithm for mobile robot navigation that integrates

US
the Gmapping proposal distribution with the Kullback-Leibler divergence
for adapting the number of particles. This results in a very effective particle
filter with adaptive sample size. The algorithm has been evaluated in both
simulation and experimental studies, using the standard KLD—sampling
AN
MCL as a benchmark. Simulation results show that the proposed algorithm
achieves higher localization accuracy with a smaller number of particles
compared to the benchmark algorithm. In a more realistic scenario using
experimental data and simulated robot odometry with drift, the proposed
M

algorithm again has greater accuracy using a lower number of particles.

Keywords: Monte Carlo localization; Mobile robots; Particle filter
ED

1. Introduction

The key prerequisite for successful deployment of a mobile robot is its

PT

ability to localize and orient itself as it moves through the environment.

Robot navigation can be cast as a sequential dynamic state-estimation prob-
lem and is typically solved within the Bayesian framework. Among the so-
CE

lutions are grid-based approaches [1], particle filter solutions, also known as
the Monte Carlo localization (MCL) methods [2, 3], multi-hypothesis track-
ing [4], and more recently particle filter in combination with a random finite
AC

set (RFS) model of measurements using a Doppler-Azimuth radar array [5].

∗
Corresponding author: L. Wang, RMIT University, School of Engineering, 124 La
Trobe Street, Melbourne, Vic 3000, Australia; email: liuping.wang@rmit.edu.au; tel: +61
3 9925 2100

Preprint submitted to Information Fusion September 20, 2018

 ACCEPTED MANUSCRIPT

MCL methods are by far the most widely used. The goal of MCL-class al-
gorithms is to represent the posterior distribution of the robot pose (i.e. its
position coordinates and the orientation angle) by a weighted set of sam-
ples or particles [6]. The assumption is that the map of the environment in
which the robot moves is known a priori. Hence the MCL methods have

T
many similarities to particle filter-based terrain navigation algorithms [7, 8].
Current research in MCL is mainly focused on maintaining a diverse particle

IP
set to represent the initially multi-modal posterior [9, 10, 11].
One important innovation in the development of MCL algorithms is the

CR
adaptation of the number of particles, based on an error estimate obtained
from the Kullback–Leibler divergence (KLD) [12]. When the uncertainty
about the robot pose is higher, it is necessary to use a larger number of
particles to cover more hypotheses. However, when the particles have con-

US
verged towards a small region in the state space, i.e. when the uncertainty is
lower, maintaining such a large sample size becomes computationally waste-
ful. KLD—sampling at each iteration involves a calculation of the required
AN
number of particles, such that, with a given probability, the error between
the true posterior probability density function (PDF) and its sample-based
approximation is less than a certain threshold value [13].
Additionally, the design of the importance (proposal) density is one of
M

the key aspects of effective particle filter implementation [14, 6, 15]. Ideally,
the proposal density should resemble the posterior distribution. Construct-
ing a useful approximation of the posterior is difficult because of the dif-
ED

fuseness of the transitional density (created from the robot motion model),
compared with the measurement likelihood. The Gmapping proposal [16]
is an innovative method of proposal distribution design, and the resulting
Gmapping-SLAM is one of the most powerful algorithms for simultaneous
PT

localization and mapping (SLAM) [17]. The main idea of Gmapping is to

use the particles sampled from the transitional density as the initial points
in maximum-likelihood (ML) optimization. The resulting ML estimates are
CE

subsequently used to form the best-fitting Gaussian PDF, which acts as the
importance density.
The key contribution of this paper is the design, analysis and evalu-
ation of a new MCL algorithm that combines KLD—sampling with the
AC

Gmapping proposal distribution (“KLD Gmapping”). The proposed local-

ization algorithm results in a very effective robot navigation algorithm, by
dramatically reducing the number of required particles combined with an
accurate proposal distribution. Integration of KLD—sampling with Gmap-
ping proposal inherits the advantages of both of these two methods and
at the same time overcomes their respective limitations, such as, the loss

3
 ACCEPTED MANUSCRIPT

of accuracy/occasional divergence of KLD—sampling (using a conventional

proposal distribution) and the large computational load of the Gmapping
proposal. The strength of the algorithm is demonstrated in simulation as
well as in an experiment (using simulated odometry data with added drift).
The remainder of this paper is organized as follows. Section 2 provides

T
an introduction to two types of robot motion models as well as the light
detection and ranging (LIDAR) measurement model. Section 3 reviews the

IP
KLD—sampling MCL algorithm. Section 4 explains the proposed MCL
algorithm, as an integration of the KLD—sampling with the Gmapping

CR
proposal. In Section 5, simulation studies are conducted with simulated
LIDAR, and translational and rotational velocity measurements to show
that in the ideal situation, both KLD MCL and KLD Gmapping produce
converged localization estimates. To experimentally validate the proposed

US
MCL algorithm, numerical results obtained using a dataset collected with a
Hokuyo LIDAR sensor and a map of a room with obstacles are presented in
Section 6.
AN
2. Mathematical models and a problem description

The robot, carrying a ranging sensor, such as a LIDAR, moves through

M

an environment for which the map m is known. The map m is represented by

an occupancy grid, in which each cell is specified with Cartesian coordinates,
and is assigned a binary (occupancy) value. By convention, a value of 0
ED

indicates that the cell is unoccupied (and can be travelled through), while
1 indicates that the cell is occupied by an obstacle, such as a piece of a wall
or furniture. Here, we consider two-dimensional maps and robot motion.
The state of the robot at discrete-time k ∈ N is fully specified by a
PT

vector xk = [xk , yk , θk ]T , where xk and yk are the Cartesian coordinates of

the robot position and θk is its heading angle. This vector is referred to as
the robot pose. The robot, as it moves, collects two types of measurements.
CE

First is the LIDAR measurement vector, which at time k is denoted zk . It

consists of sensor pointing-angles (with respect to the robot heading) and the
associated measured ranges to any obstacles in view. The angular resolution
and the coverage are characteristics of the sensor. Let the likelihood function
AC

of a LIDAR measurement be denoted by g(zk |m, xk ).

The second type of measurement is related to robot motion and is de-
noted by uk . Depending on the context, it can be either the velocity vec-
tor applied to the robot (in order to move) [13, Sec.5.3] or the odometry
measurement [13, Sec.5.4]. This measurement is used in the probabilistic
specification of robot motion via the transitional density π(xk |xk−1 , uk ).

4
 ACCEPTED MANUSCRIPT

Adopting the Bayesian estimation framework, the problem is to estimate

sequentially the posterior distribution of the robot pose, p(xk |z1:k , u1:k ),
where z1:k := z1 , . . . , zk and u1:k := u1 , . . . , uk . Assuming the posterior at
time k − 1, p(xk |z1:k−1 , u1:k−1 ), is known, a cycle of the Bayesian recursion
can be expressed in two steps, prediction and update, as follows [6],[13]:

T
Z
p(xk |z1:k−1 , u1:k ) = π(xk |xk−1 , uk )p(xk |z1:k−1 , u1:k−1 )dxk−1 (1)

IP
g(zk |xk , m)p(xk |z1:k−1 , u1:k )
p(xk |z1:k , u1:k ) = R . (2)

CR
g(zk |xk , m)p(xk |z1:k−1 , u1:k )dxk
The rest of this section describes the adopted models of robot motion and
LIDAR measurements.

below.

2.1. Motion Model with Velocity Controls

US
Two types of robot motion models are used in the paper, as described
AN
Let the control input applied to the robot from time k − 1 to k , denoted
uk , be comprised of a translational velocity vk and a rotational velocity ωk
[13, Sec.5.3], i.e. uk = [vk , ωk ]T . The transitional density is modelled as a
Gaussian probability density function (PDF), i.e.
M

π(xk |xk−1 , uk ) = N (f (xk−1 , uk ), Q), (3)

where the covariance matrix is adopted as Q = diag[σx2 , σy2 , σθ2 ] and

ED

f (xk−1 , uk ) = [f1 (xk−1 , uk ), f2 (xk−1 , uk ), f3 (xk−1 , uk )]T , with

vk vk
f1 (xk−1 , uk ) = xk−1 − sin θk−1 + sin(θk−1 + wk−1 ∆t) (4)
wk wk
PT

vk vk
f2 (xk−1 , uk ) = yk−1 + cos θk−1 − cos(θk−1 + wk ∆t) (5)
wk wk
f3 (xk−1 , uk ) = θk−1 + wk ∆t. (6)
CE

2.2. Model with Estimated Odometry Coordinates

If the robot velocities (vk , wk ) are not available, the alternative is to
use odometry measurements in the calculation of the transitional density
AC

π(xk |xk−1 , uk ). Odometry is commonly obtained by integrating wheel en-

coder information or by twice integrating the acceleration data obtained
from an inertial measurement unit (IMU). The resulting measurements are
affected by noise from terrain, mis-actuation and wheel slippage. This is
worsened by integrating the noise, causing drift in the odometry measure-
ments. Assuming that [xod od od T
k , yk , θk ] is the odometry estimate of the robot

5
 ACCEPTED MANUSCRIPT

pose at time k, the new control input is constructed as the difference between
the odometry estimates at times k and k − 1, i.e.

ūk = [xod od od T od od od T
k , yk , θk ] − [xk−1 , yk−1 , θk−1 ] (7)

T
With this kind of control input, the transitional density is adopted as

IP
π(xk |xk−1 , ūk ) = N (xk ; xk−1 + ūk , Q̄), (8)

where the covariance matrix Q̄ will be adopted as a diagonal matrix.

CR
2.3. LIDAR Measurement Model
This section describes the model of the likelihood function for the LI-

US
DAR, g(zk |m, xk ), following [13, Sec.6.3]. A measurement along a beam d
consists of the range to the obstacle, rkd . The true distance from the robot
to an object along a beam d (referred to as a “raycast”) at time k is:
q
AN
rkd∗ = (xk − xdT )2 + (yk − yTd ), (9)

where (xdT , yTd ) are the coordinates of the point where the object is struck
by the dth beam. As proposed in [13], the likelihood function of range
M

measurements g(rk |xk , m) is modelled as a weighted sum of four densities,

which are described as the following cases.
ED

A. Laser beam successfully hits an object in the map and returns a measure-
ment. For this case, Gaussian noise with the mean equal to the true distance
to the object occurs when the laser beam successfully hits an object within
the LIDAR’s maximum range. The likelihood function is then
PT

ghit (rkd |m, xk ) = ηhit · N (rkd ; rkd∗ , σhit

2
), if 0 ≤ rkd ≤ rmax ,

otherwise, it is zero. The Gaussian probability density function (PDF) has

CE

been truncated to fit within the LIDAR’s range limits, hence the normal-
Rr

2 ) dr d −1 .
ization constant is ηhit = 0 max N (rkd ; rkd∗ , σhit k
AC

B. Laser beam was blocked by an unexpected object. Exponentially distributed

noise is used to model the measurement noise occurring when an object, not
forming part of the map (e.g. a person) has blocked the beam and produced
an abnormally short measurement. The probability of this event occurring
is higher for low ranges because unexpected objects far away are less likely

6
 ACCEPTED MANUSCRIPT

to be struck by a LIDAR beam and create a laser return. This is captured

in the properties of the exponential distribution:
d
gshort (rkd |m, xk ) = ηshort · λshort e−λshort rk , if 0 ≤ rkd ≤ rkd∗ ,
otherwise, it is zero. The normalization constant is then:

T
Z rd∗ !−1
k d 1

IP
ηshort = λshort e−λshort rk drkd = .
−λshort rkd∗
0 1−e
C. Obstacles were out of range. Some sensors return the value of rmax when

CR
objects are out of sensor range. The same effect can occur if the emitted
beam strikes an object, but the reflection is not received. This can for
example happen if the object is glass or its surface is a good light absorber,

d
gmax (rk |m, xk ) =
US
or simply, in bright lighting conditions. In any case, the measurement noise
model will incorporate a point-mass distribution at rmax :
(
1 if zkd = zmax
.
AN
0 otherwise
D. Laser returns were random. Finally, an indiscriminate source of noise is
added to account for entirely unexplainable, random clutter in the measure-
ments. This noise is uniformly distributed:
M

(
1
if 0 ≤ rkd < rmax
grandom (rkd |m, xk ) = rmax .
0 otherwise
ED

By choosing four weighting coefficients, whit , wshort , wmax and wrandom

to reflect the relative likelihood of occurrence of each type of noise, the
weighted mixture of these four sources of noise forms the likelihood of mea-
PT

surement noise:
 T  
whit ghit (rkd |m, xk )
 wshort   gshort (rd |m, xk ) 
g(rkd |m, xk ) =    k
 wmax  ·  gmax (rd |m, xk )  ,
 (10)
CE

k
wrandom grandom (rkd |m, xk )
where whit + wshort + wmax + wrandom = 1.
AC

Furthermore, it is assumed the measurements from LIDAR beams are

independent, hence:
D
Y
g(zk |m, xk ) = g(rkd |m, xk ), (11)
d=1

where D is the total number of beams.

7
 ACCEPTED MANUSCRIPT

3. KLD MCL

This section begins with a brief background on particle filtering, followed

by a description of the KLD—sampling MCL algorithm, which are essential
in the development of the KLD MCL algorithm with the Gmapping proposal.

T
3.1. Particle Filtering

IP
Particle filters approximate the posterior distribution p(xk |z1:k , u1:k ) by
[i] [i]
a set of N weighted samples (particles), i.e. {wk , xk }N i=1 , where the weights

CR
[i]
wk are positive and sum to 1:
N
X [i] [i]
p(xk |z1:k , u1:k ) ≈ wk δ(xk − xk ). (12)

US i=1

Propagation of particles and associated weights over time is the essence of

particle filtering. The basic steps of a generic particle filter are described
AN
[i] [i]
in [14], [6]. Assuming {wk−1 , xk−1 }N i=1 is available, the following steps are
repeated for i = 1, · · · , N :
1. The ith article of the robot pose is drawn from a proposal (importance)
distribution:
M

[i] [i]
xk ∼ p(xk |xk−1 , zk , uk ).
2. The weight of ith particle is computed as:
ED

[i] [i] [i]

[i] [i] g(zk |m, xk )π(xk |xk−1 , uk )
wk ∝ wk−1 [i] [i]
.
p(xk |xk−1 , zk , uk )
PT

The weights are subsequently normalized (in order to sum to 1). A particle
filter with the steps above would degenerate over time; that is, all but one
particle would eventually be assigned a weight of zero. Hence, a resampling
CE

step is necessary when the variation in particle weights is too high.

Point estimates can be easily computed from the particle filter approx-
imation of the posterior. For example, the minimum mean squared error
(MMSE) estimator can be computed as follows ([19],[20]):
AC

N
X [i] [i]
x̂k = wk x k .
i=1

The Sampling Importance Resampling (SIR) particle filter, the most

widely used particle filter, has the following two characteristics [14], [6]:

8
 ACCEPTED MANUSCRIPT

[i]
1. The proposal density p(xk |xk−1 , z1:k , uk ) is chosen to be the transi-
[i]
tional density π(xk |xk−1 , uk ).

2. Resampling is carried out after every time index, resulting in a simple

T
computation of weights
[i] [i]

IP
wk ∝ g(zk |m, xk ).

SIR particle filters have been used in many robotics applications to represent

CR
the posterior PDF of a mobile robot’s pose within a known map.

3.2. KLD—MCL Algorithm

SIR particle filters assume a fixed number of particles throughout the ap-

US
plication. Kullback–Leibler (KL) divergence, when applied to Monte Carlo
localization (MCL) [12] allows the number of particles to vary with time by
adjusting to the level of uncertainty in estimation. The end result is a lower
AN
average number of particles required for sequential estimation.
The KL-distance is a standard measure for the difference between prob-
ability distributions. It is shown in [12] that the number of samples that
guarantees with probability 1−δ that the distance between the sample-based
M

maximum likelihood (ML) estimate and the true posterior will not exceed a
pre-specified threshold  is given by:
1 2
ED

nKLD = χ . (13)
2 c−1,1−δ
Here c is the count of bins with support in the estimated posterior. A prac-
tical implementation of KLD sampling thus requires the state space to be
PT

divided into bins (in our case in three-dimensions, position x, y, and heading
θ), to determine if a particle has fallen into a bin with or without existing
support. The number of samples nKLD is hence updated sequentially over
CE

(discrete) time k, as well as the array of bins in three dimensions (x, y, θ)

and the count c.
Equation (13) can be approximated in terms of the standard normal
distribution by means of the Wilson-Hilferty transformation [18]:
AC

( s )3
c−1 2 2
nKLD ≈ 1− + z1−δ . (14)
2 9(c − 1) 9(c − 1)

Here z1−δ is the upper 1 − δ quantile of the standard normal distribution

[12].

9
 ACCEPTED MANUSCRIPT

The KLD MCL algorithm is summarized in Algorithm 1. For complete-

ness, see [12],[13, pp. 263-267]. The minimum and maximum numbers of
particles, Nmin and Nmax , respectively, are user- defined parameters. Ini-
tially the particles are distributed uniformly across the state space to solve
the global localization problem. Variable n counts the particles and c is the

T
number of occupied bins. At the first time instant, the weights are initial-
ized using the likelihood function in Equation (11) formed by the process

IP
described in Section 2.3. Note from line 7 in Algorithm 1 that the KLD—
sampling MCL uses the transitional density as the proposal.

CR
Algorithm 1 KLD MCL
n oNk−1
[i] [i]
1: Input: xk−1 , wk−1 , uk , zk
i=1
2:
3:
4:
5:
n = 0; c = 0; nKLD = Nmin

while (n ≤ nKLD ) do
n=n+1
US
Form a bin array in the state-space; set all bins to empty

. Particle counter
AN
[i]
6: Draw i with probability ∝ wk−1
[n] [i]
7: xk ∼ π(xk |xk−1 , uk )
[n] [n]
8: w̃k = g(zk |m, xk )
[n]
9: if xk falls into an empty bin b then
M

10: Set b to non-empty

11: c=c+1 . Counter of occupied bins.
12: If c > 1,  n q o3 
ED

13: nKLD = min Nmax , max{Nmin , c−1

2 1− 2
9(c−1) + 2
9(c−1) z1−δ }
14: end if
15: end while
16: Nk = n
PT

[i]
[i] w̃k
17: For i = 1, . . . , Nk , wk = PNk [j] . Normalise weights
j=1 w̃k
n oNk
[i] [i]
18: Output: xk , wk
i=1
CE

4. The proposed MCL algorithm

AC

4.1. Gmapping Proposal Distribution

Gmapping is a technique proposed in [16] in which the current LIDAR
measurement is used to build an efficient proposal distribution. This is based
on the assumption that the current LIDAR measurement is on average more
accurate than odometry data. Gmapping has been shown to improve the
estimation quality and also, to reduce the number of required particles,

10
 ACCEPTED MANUSCRIPT

because each particle is placed in the region of the state space that has non-
zero likelihood and hence has a good chance of surviving the resampling
step. For mapping applications, each particle carries a stochastic realization
of the map. Thus, reducing the number of particles significantly reduces
the memory required. Gmapping comes at a cost of more computations per

T
particle.
Descriptively, Gmapping can be summarized as follows. For all particles

IP
i = 1, . . . , N , at discrete-time k:
[i] [i]
1. Sample a pose from transitional density, i.e. x̃k ∼ π(xk |xk−1 , uk ). See

CR
Equation (3) and (8) respectively, for the transitional density used in
simulation and with experimental data, respectively.
2. Find a pose in the vicinity of this sample that maximizes the likelihood

[i]
US
of having received the current measurement, i.e.

x̂k = arg max g(zk |xk , m),

xk
AN
[i]
where the search for the maximum is initialised with x̃k (obtained in
step 1). Optimization can be carried out using, for example, MAT-
LAB’s f mincon function. The search for the maximum/minimum
is bounded by an interval for which the optimization is conducted,
M

α = ` · [σx , σy , σθ ]T , where ` is a user-defined parameter.

3. Uniformly sample U particles in the vicinity of the “maximum like-
[i] [i]
lihood” pose. xj ∼ U(x̂k − ∆, x̂k + ∆), where j = 1, . . . , U and
ED

∆ = [∆x , ∆y , ∆θ ]T defines the vicinity. ∆ is a user-defined parameter.

4. Construct a Gaussian distribution based on the statistics of the uni-
form sample. This is the Gmapping proposal distribution. The Gaus-
[i] [i]
sian distribution has the mean µk and covariance Σk computed as
PT

follows: P
[i] [i]
(a) µk = η1[i] · U j=1 xj · g(zk |m, xj ) · π(xj |xk−1 , uk )
[i] P [i] [i] [i] T
(b) Σk = η1[i] · U j=1 g(zk |m, xj )·π(xj |xk−1 , uk )·(xj −µk )·(xj −µk )
CE

P [i]
(c) The normalization factor: η [i] = U j=1 g(zk |m, xj )·π(xj |xk−1 , uk ).
5. Sample a pose from the Gaussian Gmapping proposal distribution.
6. Update importance weights:
AC

U
X
[i] [i] [i] [i]
wk ≈ wk−1 · g(zk |m, xj ) · π(xj |xk−1 , uk ) = wk−1 · η [i] .
j=1

7. Resample if the effective sample size is smaller than a threshold, and

repeat.

11
 ACCEPTED MANUSCRIPT

4.2. KLD MCL with Gmapping Proposal Distribution

The basis of the KLD MCL algorithm is independent of the proposal dis-
tribution. It is therefore possible to superimpose the KLD MCL algorithm
on top of the Gmapping proposal distribution. KLD—sampling provides
computational efficiency by having an adaptive sample size in comparison

T
to a fixed-sample Gmapping particle filter. The Gmapping proposal distri-

IP
bution provides a more accurate particle filter when compared to KLD with
the standard proposal distribution. It should be noted, however, that the
computational requirements for KLD MCL with Gmapping Proposal (“KLD

CR
Gmapping”) is greater than for KLD MCL on a per-particle basis.
To apply KLD–Sampling, particles must first be sampled according to a
prior probability distribution. Then to apply the Gmapping proposal distri-
bution, a movement of the robot must have occurred. At the initial sample

US
k = 1, the robot has not yet moved and the prior probability distribution is
initialized according to the standard LIDAR measurement model in Section
2.3. From here onwards, KLD Gmapping can be used without interruption.
AN
[i]
Thus, all that is required is to sample pose xk from the Gmapping proposal
distribution (Section 4.1), use this sample in the KLD framework (Section
3), and then update the weights according to the Gmapping weighting (Sec-
tion 4.1, Step 6). The proposed KLD Gmapping algorithm is summarized
M

by Algorithm 2. User-defined parameters are , δ, Nmin , Nmax , ∆, α and U .

In line 8 we compute the ML pose in the vicinity of the sampled particle
of the previous step, using measurement zk . In line 9, we sample uniformly
ED

in the vicinity of the ML pose. Lines 10-20 compute the mean and the
covariance matrix of the Gaussian proposal. The nth particle and its un-
normalised weight are computed in lines 21 and 22. The remaining lines of
the algorithm are the same as in Algorithm 1.
PT
CE
AC

12
 ACCEPTED MANUSCRIPT

Algorithm 2 KLD MCL with Gmapping Proposal Distribution

n oNk−1
[i] [i]
1: Input: xk−1 , wk−1 , uk , zk
i=1
2: n = 0; c = 0; nKLD = Nmin
3: Form a bin array in the state-space; set all bins to empty
while (n ≤ nKLD ) do

T
4:
5: n=n+1 . Particle counter
[i]

IP
6: Draw i with probability ∝ wk−1
[i]
7: x∗k ∼ π(xk |xk−1 , uk )
8: x̂∗k = arg maxxk g(zk |m, xk ), subject to x̂∗k ∈ {x∗k − α, x∗k + α}

CR
9: For j = 1, · · · , U , draw xj ∼ U(x̂∗k − ∆, x̂∗k + ∆)
[n]
10: µk = [0, 0, 0]T ; η [n] = 0
11: for j = 1, . . . , U do
[n] [n] [i]
12: µk = µk + xj · g(zk |m, xj ) · π(xj |xk−1 , uk )
13:
14:
15:
end for
[n]

[n]
[n]
µk = µk /η [n]
US [i]
η [n] = η [n] + g(zk |m, xj ) · π(xj |xk−1 , uk )

. Mean of Gaussian proposal

AN
16: Σk = 0(3×3)
17: for j = 1 : U do
[n] [n] [n] [n] [i]
18: Σk = Σk + (xj − µk )(xj − µk )T · g(zk |xj , m) · π(xj |xk−1 , uk )
19: end for
[n] [n]
20: Σk = Σk /η [n] . Covariance of Gaussian proposal
M

[n] [n] [n]

21: xk ∼ N (µk , Σk )
[n]
22: w̃k = η [n] . Importance weight
[n]
23: if xk falls into an empty bin b then
ED

24: Set b to non-empty

25: c=c+1 . Count number of occupied bins.
26: If c > 1,  n q o3 
c−1 2 2
nKLD = min Nmax , max{Nmin , 2 1 − 9(c−1) + 9(c−1) z1−δ }
PT

27:
28: end if
29: end while
30: Nk = n
CE

[i]
[i] w̃k
31: For i = 1, . . . , Nk , wk = PNk [j] . Normalise weights
j=1 w̃k
n oNk
[i] [i]
32: Output: xk , wk
i=1
AC

5. Simulation Studies

This section presents a numerical assessment of the accuracy in pose

estimation using the proposed KLD Gmapping MCL algorithm. The results

13
 ACCEPTED MANUSCRIPT

are obtained through simulation, assuming the translational and rotational

velocity measurements are available, as in the motion models given in (4)-(6).
Monte-Carlo simulations are used to evaluate the average estimation errors.
The root mean squared error (RMSE) of each pose vector component,
s

T
PM [s] [s] 2
s=1 (β̂k − βk )
RMSEβ (k) = , (15)

IP
M

for β ∈ {x, y, θ}, is computed from M Monte Carlo trials.

CR
5.1. Simulation Parameters
In simulations, two scenarios have been considered, as shown in Figure
1. This figure displays the ground truth, as well as a typical output of the

US
KLD MCL and the KLD Gmapping algorithms. The number of particles
used to initialize the particle filter and the maximum number of particles was
set to Nmax = 100, 000. These particles were initially distributed uniformly
AN
across the map to solve the global localization problem. The same set of
Nmax particles were used as the starting set of particles for both the KLD
and KLD Gmapping algorithms. At the first time step, the weights were
initialized using the approach described in Section 2.3. This process was
M

repeated for all M = 30 Monte Carlo simulations. The sampling interval

was set to ∆t = 1 second.
ED
PT
CE
AC

14
 ACCEPTED MANUSCRIPT

1000

Scenario 2: Start
900
Scenario 2: Finish

T
800

KLD MMSE Estimate

IP
700 KLD Gmapping MMSE Estimate
Ground Truth

600

CR
y (cm)

500

400 Scenario 1: Finish

300

200
US
AN
100
Scenario 1: Start

100 200 300 400 500 600 700 800 900 1000
M

x (cm)

Figure 1: A single realization of KLD and KLD Gmapping Monte Carlo localization
ED

algorithms

The robot starts at pose: [200, 50, π2 ] for Scenario One and at [800, 900, π]
for Scenario Two (the units are: cm for x and y; rad for θ) for each sim-
PT

ulation run. The true robot trajectory is stochastic and different on each
Monte Carlo run, due to process noise. We consider 20 time-steps, i.e.
k = 1, 2, . . . , 20. The covariance matrix of process noise for the robot mo-
4π 2
tion model was set to Q = diag([62 , 62 , ( 180
CE

) ]).
The LIDAR is assumed to have a maximum range zmax = 450cm and
π
has D = 24 beams spread out every φ = 15 180 radians for a full 2π radians
field of view.
AC

The probabilities of receiving a hit, a short reading, a maximum reading,

15
 ACCEPTED MANUSCRIPT

or a uniformly distributed, random reading are, respectively:

   
whit 0.928
 wshort   0.04 
   .
 wmax  =  0.002 

T
wrandom 0.03

IP
When an object is hit, the noise is distributed with zero mean and variance
2 = 102 . The rate of the exponential distribution when a short reading
of σhit
obtained is λshort = 0.05. The ML estimated posterior should have KL

CR
distance less than  = 0.05 with probability 1 − δ, where δ = 0.05. The
minimum number of particles was set to Nmin = 20.
The Gmapping proposal distribution had an optimization solution con-

US
straint determined by α = 1.5 · [σx , σy , σθ ]T when finding an ML pose. The
number of particles uniformly distributed around each ML pose was set to
U = 50. These uniformly distributed particles were distributed in a region
determined by:
AN
4π T
∆ = [5, 5, ] .
180
For both MCL algorithms, the map was divided into bins with the bin
size in x, y, and θ equal to 50 cm, 50 cm and 20π 180 rad, respectively.
M

5.2. Comparative Simulation Results

The RMSE, calculated using (15) across M = 30 Monte Carlo runs, are
ED

compared in Figures 2 and 3 (a)-(c) for Scenarios One and Two, respectively.
The RMSE for KLD MCL and KLD Gmapping share the same initial error
because they are initiated from exactly the same particle set. It can be
seen that the KLD Gmapping algorithm generally has a higher estimation
PT

accuracy than the KLD MCL algorithm. The angular estimation for KLD
Gmapping can sometimes be volatile. This could be exacerbated by sharp
rotations in the robot pose combined with the optimization step in Gmap-
CE

ping, which can lead to particles with unusual poses. However, based on
the authors’ experience with other simulations and this example, the angu-
lar estimation deterioration is transient. KLD Gmapping is able to provide
AC

superior long-term estimation quality compared to the KLD MCL.

16
 ACCEPTED MANUSCRIPT

14
12 KLD Gmapping 6 KLD Gmapping
KLD KLD
10
8 5
RMSE: x (cm)

RMSE: y (cm)
6
4

T
4

IP
2
5 10 15 20 5 10 15 20
Sample: k Sample: k

CR
(a) RMSE in the estimate of x (b) RMSE in the estimate of y

KLD Gmapping
0.04
KLD
0.03

US
(rad)

0.02
RMSE:

AN
0.01

5 10 15 20
Sample: k
M

(c) RMSE in the estimate of θ

Figure 2: Comparisons of RMSEs between KLD and KLD Gmapping for Scenario One
ED
PT
CE
AC

17
 ACCEPTED MANUSCRIPT

14
KLD Gmapping KLD Gmapping
12
KLD KLD
10
101
RMSE: x (cm)

RMSE: y (cm)
8

T
4

IP
100
5 10 15 20 5 10 15 20
Sample: k Sample: k

CR
(a) RMSE in the estimate of x (b) RMSE in the estimate of y

10-1
KLD Gmapping
KLD

US
(rad)
RMSE:

AN
10-2

5 10 15 20
Sample: k
M

(c) RMSE in the estimate of θ

Figure 3: Comparisons of RMSEs between KLD and KLD Gmapping for Scenario Two
ED

Number of particles. Demonstrated in Figures 4 and 5 for Scenario

One and Two, respectively, is the average number of particles required by
each algorithm over the Monte Carlo simulation. KLD Gmapping requires
PT

on average, approximately 20 fewer particles than KLD MCL, despite using

the same KLD parameters, as described above. This is due to KLD Gmap-
ping placing particles in a more concentrated manner and thus occupying
fewer bins than KLD MCL, leading to a lower nKLD value.
CE
AC

18
 ACCEPTED MANUSCRIPT

105

Average Number of Particles

KLD Gmapping
KLD
104

T
103

IP
102

CR
101
5 10 15 20
Sample: k

US
Figure 4: Number of particles for KLD and KLD Gmapping for Scenario One

105
AN
Average Number of Particles

KLD Gmapping
KLD
104
M

103

102
ED

101
5 10 15 20
Sample: k
PT

Figure 5: Number of particles for KLD and KLD Gmapping for Scenario Two
CE

6. Experimental Validation

As described in [21], an experimental dataset was collected by pushing

a trolley carrying a Hokuyo LIDAR sensor through a room with obstacles.
AC

A map of the room and obstacles is available as a priori knowledge. The

objective is to determine the position of the trolley within the map using
the KLD MCL and the KLD Gmapping MCL algorithms.
To establish the ground-truth, an OptiTrack motion- capture system was
used to determine the true position of the moving trolley. This will be used

19
 ACCEPTED MANUSCRIPT

as the ground-truth in the evaluations of the RMSE and also, to generate a

simulated odometry signal.

6.1. Reconstruction of Odometry Estimate

IMU-based odometry data was not available from the experiment. How-

T
ever, the measurement set can be realistically simulated by adding biased
noise to the actual position provided by the OptiTrack system. The odome-

IP
try signal is assumed to have the generous benefit of knowing the true initial
position at sampling time k = 1.

CR
[xod od od T T
1 , y1 , θ1 ] = [x1 , y1 , θ1 ] .

Then, the odometry data set is reconstructed using the following equation,

[xod od od T od od

USod T x y θ T
k , yk , θk ] = [xk−1 , yk−1 , θk−1 ] + [∆k , ∆k , ∆k ] + ēk , (16)

where [∆xk , ∆yk , ∆θk ]T = [xk , yk , θk ]T −[xk−1 , yk−1 , θk−1 ]T , and ēk ∼ N (B, Q̄).
AN
B is a noise bias encountered that is not typically modelled. It is therefore
unknown to the particle filter which has its particles moved by the proposal
distribution in equations (7) and (8). This creates a realistic drift between
the simulated odometry estimate and the true robot position.
M

While the odometry estimate is assumed to be initialized at the ground-

truth initial pose, the benefit of this information has not been incorporated
in the KLD MCL, nor the KLD Gmapping algorithms, to test an additional
ED

layer of robustness for the particle filters.

6.2. Choice of Parameters

The number of particles used to initialize the particle filter and the max-
PT

imum number of particles were set to Nmax = 15, 000. These particles were
distributed uniformly across the map to solve the global localization prob-
lem. The same set of Nmax particles were used as the starting set of parti-
CE

cles for both the KLD and KLD Gmapping algorithms. At time k = 1, the
weights were initialized using the approach in Section 2.3. This was repeated
for M = 30 runs, each run lasts for 40 time-steps, i.e. k = 1, 2, . . . , 40 . The
output on each run was different due to randomness inherent in the particle
AC

filter and the random nature of the odometry data.

The noise levels for the robot motion model were selected as: Q̄ =
2π 2
diag([0.12 , 0.12 , ( 180 ) ]) and B = [0.012, −0.012, 0.6π T
180 ] .
The LIDAR is assumed to have a maximum range rmax = 6.4 m and
covers π radians field of view. Of the original 512 LIDAR measurements per
sample, all measurements with no returning signal were removed, as it was

20
 ACCEPTED MANUSCRIPT

concluded that the substantial majority of these readings were due to sensor
failure. Furthermore, measurements below 0.5 m were also removed, as
they were erroneous readings resulting from the experiment apparatus ([21]).
From the remaining valid LIDAR measurements, only 50 measurements per
LIDAR scan were retained and the rest of measurements were discarded to

T
reduce the computational load.

IP
k = 10

CR
Ground Truth
3

1 US Finish
AN
Y (m)

0 Start

-1
M

-2
ED

-3

-5 -4 -3 -2 -1 0 1 2 3 4 5
PT

X (m)

Figure 6: Actual measurements received (green lines), at k = 10 from the ground-truth

trajectory (blue) during the experiment
CE

Figure 6 shows a plot of the data received from the LIDAR used in the
experiment at k = 10. The vertical bar on the far right-hand side of the map
AC

in Figure 9 was not detected by the LIDAR because it was a transparent

obstacle. Section 6.3 will demonstrate that the proposed KLD Gmapping
algorithm is capable of robust localization, even in the presence of such
undetectable objects. The probabilities of receiving a hit, a short reading, a
maximum reading and a uniformly distributed, random reading have been
loosely estimated by viewing plots of the received LIDAR measurements

21
 ACCEPTED MANUSCRIPT

against the true robot pose and true map. The probability of short, and
maximum readings is 0 because those readings were mostly erroneous and
removed from the dataset. The estimated probabilities of receiving a hit,
a short reading, a maximum reading and a uniformly distributed, random
reading are chosen respectively as,

T
   
zhit 0.85

IP
 zshort   0 
   
 zmax  =  0  .

CR
zrand 0.15

Figure 7 shows an example PDF of a single LIDAR measurement applied

to the experimental data. When an object is hit, the noise is distributed
2 = 0.62 .
with a mean of 0 and variance of: σhit

8
10-3
US
AN
r dk * = 3.61

6
M
g(rdk |m,xk)

4
ED

2
PT

0
0 2 4 6 8
CE

rdk
AC

Figure 7: Example PDF of a single LIDAR measurement applied to the experimental

measurements.

The ML estimated posterior should have KL distance less than  = 0.25

with probability 1−δ, where δ = 0.25 for KLD Gmapping with the minimum
number of particles set to Nmin = 10. For KLD MCL:  = 0.03, δ = 0.03,
Nmin = 20. A larger number of particles for KLD MCL was set relative

22
 ACCEPTED MANUSCRIPT

to KLD Gmapping as the estimation accuracy was unacceptably poor with

fewer particles. The maximum number of particles was set to the initial
number of particles: Nmax = 15000 for both particle filters.
The Gmapping proposal distribution had an optimization solution con-
straint determined by: α = 3 · [σx , σy , σθ ]T when finding an ML pose. The

T
number of particles uniformly distributed around each ML pose was set to
U = 20. These uniformly distributed particles were distributed in a region

IP
defined by:
20π T
∆ = [0.8, 0.8, ] .

CR
180
The map was divided into bins with the bin size in x, y, and θ equal to 0.50
m, 0.50 m, and 15π
180 rad, respectively.

US
6.3. Comparative Experimental Results
Figures 8 (a)-(c) show the RMSEs for the estimations using the recon-
structed odometry data, the KLD–sampling MCL estimator, and the KLD
AN
Gmapping MCL estimator. It is evident that the reconstructed odometry
data contain drift components as the RMSEs with respect to x, y, and
θ increase with time k. This drift is reflective of realistic challenges at-
tempting to localize using dead reckoning. Under these assumptions, the
M

KLD–sampling MCL algorithm sometimes fails to converge, and on aver-

age, it produces the estimation results similar to the estimation obtained
from using the odometry data. The unmodelled bias in combination with
ED

odometry noise can shift the proposal distribution too far away from the
true pose which is difficult to recover from. In contrast, the proposed KLD
Gmapping proposal algorithm always converges, causing a RMSE value that
substantially outperforms KLD MCL and Odometry. To visually illustrate
PT

these conclusions, Figure 9 shows the map, together with a single realization
of the localization results using odometry, KLD sampling, and KLD Gmap-
ping proposal algorithms. KLD Gmapping is much more robust to odometry
CE

drift due to the ML optimization (and subsequent uniform sampling around

the ML pose) step based on the LIDAR measurement, which can compen-
sate for drift in odometry. A Monte Carlo run is defined as a “failure” if the
particle filter estimation is greater than or equal to two meters in distance
AC

from the ground-truth for any of the final ten samples of that run. In Figure
8, of the 30 Monte Carlo runs, KLD MCL and KLD Gmapping both failed
zero times.

23
 ACCEPTED MANUSCRIPT

T
IP
CR
0.8 3
KLD Gmapping KLD Gmapping
KLD KLD
2.5
Odometry Estimate Odometry Estimate
0.6
RMSE: x (m)

RMSE: y (m)
2

US
0.4 1.5

1
0.2
0.5

0 0
AN
5 10 15 20 25 30 35 40 5 10 15 20 25 30 35 40
Sample: k Sample: k

(a) RMSE in the estimate of x (b) RMSE in the estimate of y

1
M

KLD Gmapping
KLD
0.8 Odometry Estimate
(rad)

0.6
ED RMSE:

0.4

0.2

0
PT

5 10 15 20 25 30 35 40
Sample: k

(c) RMSE in the estimate of θ

CE

Figure 8: Comparisons between the RMSEs resulted from KLD MCL and KLD Gmapping
AC

24
 ACCEPTED MANUSCRIPT

KLD Gmapping MMSE Estimate

T
2 KLD MMSE Estimate
Ground Truth
Odometry Estimate

IP
1

CR
Y (m)

0 Finish
Start

-1

-2
US
AN
-3

-5 -4 -3 -2 -1 0 1 2 3 4 5
X (m)
M

Figure 9: A single realization for odometry data, KLD, and KLD Gmapping applied to
experimental results.
ED

Figure 10 shows the number of particles required in KLD and KLD

Gmapping proposal algorithms. The KLD Gmapping algorithm uses ap-
proximately ten times fewer particles. However, despite having a larger
number of particles, KLD MCL overall required less computational time
PT

than KLD Gmapping as shown in Table 1 which displays the average com-
putation time (in seconds) required to complete one trial. This is because
the computational demand is much higher on a per-particle basis when using
CE

the Gmapping proposal. The reduction in the number of particles has utility
in mapping applications where each particle stores a stochastic realization
of the map, therefore requiring memory. The computations were completed
AC

using MATLAB, on a PC with an Intel Core i5 6600 3.30 GHz processor,

and 32 GB of RAM.

25
 ACCEPTED MANUSCRIPT

Application: KLD MCL KLD Gmapping

Simulation: Scenario One 87 184
Simulation: Scenario Two 83 167
Experimental Results 47 192

T
Table 1: Computation time (in seconds) for the different applications of KLD and KLD
Gmapping

IP
105
KLD Gmapping

CR
Average Particles Required

KLD

104

103

102

101
US
AN
5 10 15 20 25 30 35 40
Sample: k

Figure 10: Number of particles for KLD and KLD Gmapping

M

7. Conclusion
ED

This paper presented an algorithm that incorporates the Gmapping pro-

posal distribution into KLD Monte Carlo localization for the purpose of
mobile robot localization in a known, grid-based map. KLD–sampling adap-
tively adjusts the number of particles required at a given time to adaptively
PT

minimize computation. The Gmapping proposal is much more robust to

odometry noise, increases the estimation accuracy, and reduces the number
of particles required (which saves memory for mapping applications). How-
CE

ever, as demonstrated, the total computational time for KLD Gmapping is

still greater compared to KLD MCL, due to the Gmapping proposal having
a higher computational load per particle. These two techniques have been
successfully combined to create KLD MCL with Gmapping proposal dis-
AC

tribution “KLD Gmapping”. This algorithm displays desirable properties

from its constituent algorithms, and has been validated in simulation and
experimental results.
Acknowledgment The authors wish to thank Mr. Ricardo Cannizzaro
from DST Group and Mr. Lance Fang from RMIT University for their help
with experimental data.

26
 ACCEPTED MANUSCRIPT

References
[1] Wolfram Burgard, Dieter Fox, Daniel Hennig, and Timo Schmidt. Es-
timating the absolute position of a mobile robot using position prob-
ability grids. In Proceedings of the National Conference on Artificial

T
Intelligence, pages 896–901. Citeseer, 1996.

IP
[2] Dieter Fox, Wolfram Burgard, Frank Dellaert, and Sebastian Thrun.
Monte Carlo localization: Efficient position estimation for mobile
robots. AAAI/IAAI, 1999(343-349):2–2, 1999.

CR
[3] Sebastian Thrun, Dieter Fox, Wolfram Burgard, and Frank Dellaert.
Robust Monte Carlo localization for mobile robots. Artificial intelli-
gence, 128(1-2):99–141, 2001.

US
[4] Patric Jensfelt and Steen Kristensen. Active global localization for a
mobile robot using multiple hypothesis tracking. IEEE Transactions
on Robotics and Automation, 17(5):748–760, 2001.
AN
[5] Guan, Robin P, Ristic, Branko, Wang, Liuping and Evans, Rob. Monte
Carlo Localization of a Mobile Robot Using a Doppler-Azimuth Radar.
Automatica, to appear, 2018.
M

[6] Branko Ristic, Sanjeev Arulampalam, and Neil Gordon. Beyond the
Kalman Filter: Particle Filters for Tracking Applications. Artech
House, Norwood, MA, 2004.
ED

[7] Fredrik Gustafsson, Fredrik Gunnarsson, Niclas Bergman, Urban Fors-

sell, Jonas Jansson, Rickard Karlsson, and P-J Nordlund. Particle filters
for positioning, navigation, and tracking. IEEE Transactions on Signal
PT

Processing, 50(2):425–437, 2002.

[8] Francisco Curado Teixeira, João Quintas, Pramod Maurya, and
António Pascoal. Robust particle filter formulations with application
CE

to terrain-aided navigation. International Journal of Adaptive Control

and Signal Processing, 2016.
[9] Zhibin Liu, Zongying Shi, Mingguo Zhao, and Wenli Xu. Adaptive
AC

dynamic clustered particle filtering for mobile robots global localization.

Journal of Intelligent and Robotic Systems, 53(1):57–85, 2008.
[10] Gert Kootstra and Bart De Boer. Tackling the premature convergence
problem in Monte-Carlo localization. Robotics and Autonomous Sys-
tems, 57(11):1107–1118, 2009.

27
 ACCEPTED MANUSCRIPT

[11] Chiang-Heng Chien, Wei-Yen Wang, and Chen-Chien Hsu. Multi-

objective evolutionary approach to prevent premature convergence in
Monte Carlo localization. Applied Soft Computing, 50:260–279, 2017.

[12] Dieter Fox. Adapting the sample size in particle filters through KLD-

T
sampling. The International Journal of Robotics, 22(12):985–1003, De-
cember 2003.

IP
[13] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic
Robotics. MIT Press, Cambridge, MA, 2006.

CR
[14] M. Sanjeev Arulampalam, Simon Maskell, Neil Gordon, and Tim Clapp.
A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian
tracking. IEEE Transactions in Signal Processing, 50(2), 2002.

US
[15] Olivier Cappé, Simon J Godsill, and Eric Moulines. An overview of
existing methods and recent advances in sequential Monte Carlo. Pro-
ceedings of the IEEE, 95(5):899–924, 2007.
AN
[16] Giorgio Grisetti, Cyrill Stachniss, and Wolfram Burgard. Improved
techniques for grid mapping with Rao-Blackwellized particle filters.
IEEE Transactions on Robotics, 23(1):34–46, February 2007.
M

[17] Joao Machado Santos, David Portugal, and Rui P Rocha. An evaluation
of 2D SLAM techniques available in robot operating system. In Safety,
ED

Security, and Rescue Robotics (SSRR), 2013 IEEE International Sym-

posium on, pages 1–6. IEEE, 2013.

[18] Norman L. Johnson, Samuel Kotz, and N. Balakrishnan. Continuous

Univariate Distributions, volume 1. Wiley, New York, 1994.
PT

[19] Simon Godsill, Arnaud Doucet, and Mike West. Maximum a posteriori
sequence estimation using Monte Carlo particle filters, 2000.
CE

[20] Yaakov Bar-Shalom, X. Rong Li, and Thiagalingam Kirubarajan. Es-

timation with Applications to Tracking and Navigation: Theory Algo-
rithms and Software. Wiley, 2001.
AC

[21] Lance Fang, Alex Fisher, Stefan Kiss, James Kennedy, Chatura Na-
gahawatte, Reece Clothier, and J Palmer. Comparative evaluation of
time-of-flight depth-imaging sensors for mapping and slam applications.
In Australasian Conference on Robotics and Automation 2016, pages 1–
7. Australian Robotics and Automation Association, 2016.

28