Content uploaded by Chanki Kim

Author content

All content in this area was uploaded by Chanki Kim on Jul 14, 2014

Content may be subject to copyright.

808 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

Unscented FastSLAM: A Robust and Efﬁcient

Solution to the SLAM Problem

Chanki Kim,Student Member, IEEE,Rathinasamy Sakthivel, and Wan Kyun Chung,Member, IEEE

Abstract—The Rao–Blackwellized particle ﬁlter (RBPF) and

FastSLAM have two important limitations, which are the deriva-

tion of the Jacobian matrices and the linear approximations of non-

linear functions. These can make the ﬁlter inconsistent. Another

challenge is to reduce the number of particles while maintaining the

estimation accuracy. This paper provides a robust new algorithm

based on the scaled unscented transformation called unscented

FastSLAM (UFastSLAM). It overcomes the important drawbacks

of the previous frameworks by directly using nonlinear relations.

This approach improves the ﬁlter consistency and state estima-

tion accuracy, and requires smaller number of particles than the

FastSLAM approach. Simulation results in large-scale environ-

ments and experimental results with a benchmark dataset are

presented, demonstrating the superiority of the UFastSLAM al-

gorithm.

Index Terms—FastSLAM, improved proposal distribution,

Jacobian, linearization, Rao–Blackwellized particle ﬁlter (RBPF),

scaled unscented transformation (SUT), simultaneous localization

and mapping (SLAM), unscented Kalman ﬁlter (UKF), unscented

particle ﬁlter (UPF).

I. INTRODUCTION

S

IMULTANEOUS localization and mapping (SLAM) is the

problem of determining the position and the heading of an

autonomous vehicle moving through an unknown environment,

and simultaneously, of estimating the features of the environ-

ment. In the past decade, a lot of researchers have focused on

the real-time SLAM problem (see [1] and [2], and references

therein) and also the consistency of the SLAM algorithm [3]–[9].

Also, there has been ongoing research on issues such as reso-

lution of nonlinearity, non-Gaussian distributions, data associ-

ation, and landmark characterization, all of which are vital in

achieving a practical and robust SLAM implementation.

Recently, estimation algorithms have been roughly classiﬁed

according to their underlying basic principle. The most popular

approaches to the SLAM problem are the extended Kalman ﬁlter

(EKF-SLAM) and the Rao–Blackwellized particle ﬁlter (RBPF-

Manuscript received May 21, 2007; revised October 24, 2007. This paper

wasrecommended for publication by Associate Editor L. Parker and Editor

S. Roumeliotis upon evaluation of the reviewers’ comments. This work was

supported in part by the Korea Science and Engineering Foundation (KOSEF)

under Korea Government Grant MOST R0A-2003-000-10308-0, in part by the

Korea Health 21 R&D Project, Ministry of Health and Welfare, Republic of

Korea under Grant A020603, and in part by the Agency for Defence Develop-

ment and by Unmanned Technology Research Center (UTRC), Korea Advanced

Institute of Science and Technology.

The authors are with the Department of Mechanical Engineering, Pohang Uni-

versity of Science and Technology (POSTECH), Pohang 790784, South Korea

(e-mail: minekiki@postech.ac.kr; sakthi@postech.ac.kr; wkchung@postech.

ac.kr).

Color versions of one or more of the ﬁgures in this paper are available online

at http://ieeexplore.ieee.org.

Digital Object Identiﬁer 10.1109/TRO.2008.924946

SLAM) [1]. The effectiveness of the EKF approach comes from

the fact that it estimates a fully correlated posterior over feature

maps and vehicle poses. EKF-SLAM permits linear approxima-

tions of the motion and the measurement models, and it assumes

Gaussian representations for the probability density functions.

As proved in the previous research [3], [4], [7], [10], the solu-

tion of the EKF-SLAM is inconsistent due to errors introduced

during linearization, which induces inaccurate maps with ﬁlter

divergence. Therefore, the consistency issue of the EKF-SLAM

has attracted the attention of the research community due to its

importance, and many recent research efforts have concentrated

on improving the classical algorithm [3], [9], [11], [12].

As a solution for this linearization issue, Martinez-Cantin

and Castellanos [3] introduced the unscented Kalman ﬁlter

(UKF) [13]–[16] in SLAM problems for large-scale outdoor

environments. This approach was used to avoid the analytical

linearization based on Taylor series expansion of the nonlinear

models, and improved the consistency over the EKF-based ap-

proach. Also, Merwe et al. [17] applied the UKF in a particle

ﬁlter algorithm for a novel proposal distribution in the sampling

step. The unscented ﬁlter employs a deterministic sampling ap-

proach to capture the mean and the covariance estimates with

aminimal set of sample points called sigma points.Asproved

in the previous research, the number of sigma points precisely

and accurately approximates the posterior covariance up to the

third order, whereas the EKF relies on a ﬁrst-order approxima-

tion [15], [17].

Murphy [18] introduced the RBPF as an effective approach

for learning grid maps by decoupling the state of t he vehicle

and the map. The RBPF-SLAM algorithms, with their ﬂexibil-

ity to handle nonlinearity and non-Gaussianity, have become

exceptionally popular due to their relative simplicity, computa-

tional efﬁciency, and experimental successes. One of the major

challenges for the RBPFs is to reduce the number of particles

while maintaining the estimation accuracy. Furthermore, since

aresampling process of the particle ﬁlter can eliminate the cor-

rect particles [17] and can prevent maintenance of the particle

diversity, determining a novel resampling technique is also im-

portant. Additionally, the consistency of the particle-ﬁlter-based

SLAM algorithm became an issue of recent robotics papers [8],

[19], [20].

Based on the RBPF framework, Montemerlo et al. [21], [22]

developed a SLAM framework called FastSLAM for feature-

based maps. The FastSLAM approach is summarized in Table I.

The FastSLAM1.0 algorithm uses the general particle ﬁlter to

estimate the vehicle pose, and each particle has an associated

set of independent EKF to estimate the position of each feature

in the map. FastSLAM2.0 linearizes the nonlinear model in the

1552-3098/$25.00 © 2008 IEEE

Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.

KIM et al.: UNSCENTED FastSLAM: A ROBUST AND EFFICIENT SOLUTION TO THE SLAM PROBLEM 809

TABLE I

R

AO–BLACKWELLIZED PARTICLE FILTER SLAM

same manner as the EKF-SLAM to improve the accuracy of a

proposal distribution, and it also employs the low-dimensional

EKFs for estimating feature states.

However, the FastSLAM and some EKF-based RBPF frame-

works have two important potential drawbacks, which are the

derivation of the Jacobian matrices and the linear approxima-

tions of the nonlinear functions. Calculating the Jacobian is

unwelcome effort, and inaccurate approximation to the poste-

rior covariance degenerates the estimate accuracy and the ﬁlter

consistency.

This paper proposes a new probabilistic framework called un-

scented FastSLAM (UFastSLAM) to overcome the drawbacks

caused by linearizations in the FastSLAM framework. The lin-

earization process with Jacobian calculations is removed by

applying the scaled unscented transformation (SUT) [16] to the

factorized SLAM framework. Our main contributions are as

follows.

1) UFastSLAM, unlike FastSLAM, computes the proposal

distribution by measurement updates of the unscented ﬁl-

ter in the sampling step of the particle ﬁlter. In particular,

when multiple features are observed at the same time, the

sigma points of the unscented ﬁlter are updated in every

feature update step for accurate vehicle pose estimation.

This avoids the linearized transformations of the vehicle

uncertainty and derivation of J acobian matrices.

2) UFastSLAM also updates each feature state by the un-

scented ﬁlter without accumulating the linear approxima-

tion error and without calculating the Jacobian matrix of

an observation model. In addition, the use of SUT removes

the impact of an inaccurate linearized transformation from

polar to Cartesian coordinates in the feature initialization.

UFastSLAM is summarized in Table I.

We observe experimentally that the proposed UFastSLAM

algorithm, even with fewer particles, yields signiﬁcantly more

accurate estimate results when compared with FastSLAM2.0.

This improvement is salient with large measurement uncertainty.

Furthermore, without using linearization for both the feature and

the vehicle estimate, the ﬁlter consistency of the UFastSLAM

is greatly improved over the FastSLAM2.0 algorithm.

The research presented in this paper is an extension of our

preliminary results presented in [23], as it further evaluates a

ﬁlter consistency and analyzes a complexity. Furthermore, we

added the importance weight equation, a formal description of

the techniques used, and provide more detailed experiments

including outdoor results as well as indoors. Especially, these

real-world experiments employ both laser range ﬁnder and sonar

to validate robustness to sensor noises.

We note that a similar approach has also been independently

developed in [24]. A major difference with our preliminary

version [23] was whether an augmented technique for feature

state was adopted to update a map, and its effect on the ﬁlter is

also analyzed in this paper.

The structure of the paper is as follows. Section II brieﬂy re-

views the original FastSLAM framework, and the necessary no-

tations are made. The main contribution of this paper is shown in

Section III, which presents the UFastSLAM algorithm to solve

the SLAM problem. This section shows how the proposal dis-

tribution is calculated using the unscented ﬁltering technique in

the RBPF framework, explains the measurement update and ini-

tialization of the feature. Section IV gives the pseudocode of the

UFastSLAM algorithm. The effectiveness of the proposed algo-

rithm is demonstrated using simulation results and experimental

results in Sections V and VI, respectively. Finally, Section VII

contains the concluding remarks with a discussion of current

shortcomings.

II. B

ACKGROUND:FASTSLAM FRAMEWORK

Before introducing FastSLAM, the SLAM problem should

be presented from a probabilistic point of view. In probabilistic

terms, the SLAM problem is a Markov process [25]. In par-

ticular, the vehicle motion is usually considered as a Markov

process. The vehicle’s pose at time t will be denoted by x

t

.

The vehicle’s environment possesses N static landmarks. Each

landmark i s denoted by θ

k

for k =1,...,N.Thesetofall

landmarks will be denoted by θ.SLAMalgorithms calculate

the posterior over the entire path along with the map

p(x

t

,θ|z

t

,u

t

,n

t

) (1)

where the path of the vehicle is denoted by x

t

= x

1

,...,x

t

,

z

t

= z

1

,...,z

t

is a sequence of measurements and u

t

=

u

1

,...,u

t

is a sequence of control inputs. The variables

n

t

= n

1

,...,n

t

are data association variables, in which each

n

t

speciﬁes the identity of the landmark observed at time t.The

SLAM problem is simultaneously inferring the location of all

landmarks θ in the environment and the path x

t

followed by the

vehicle, based on a set of measurements and inputs.

To compute the posterior (1), the evolution of poses according

to a probabilistic law is commonly referred to as a nonlinear

motion model

p(x

t

|x

t−1

,u

t

).

The current pose x

t

is a probabilistic function of the vehicle

control u

t

and the previous pose x

t−1

.Asthevehicle moves

around, it takes measurements of its environment. Moreover,

sensor measurements are governed by a probabilistic law re-

ferred to as a nonlinear measurement model

p(z

t

|x

t

,θ,n

t

).

The FastSLAM algorithm, introduced by Montemerlo et al.

[22], is an efﬁcient algorithm for the SLAM problem that is

based on a straightforward factorization. This algorithm parti-

tions the SLAM posterior into a localization problem and inde-

pendent landmark position estimation problem conditioned on

Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.

810 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

the vehicle pose estimate. The conditional independence prop-

erty of the SLAM problem enables us to estimate the posterior

(1) in the following factored form:

p(x

t

,θ|z

t

,u

t

,n

t

)=p(x

t

|z

t

,u

t

,n

t

)

N

k=1

p(θ

k

|x

t

,z

t

,u

t

,n

t

).

This factorization [18] is the fundamental idea behind

FastSLAM.

FastSLAM uses a particle ﬁlter to approximate the ideal re-

cursive Bayesian ﬁlter for estimating the vehicle pose. The

remaining posterior of feature locations is analytically calcu-

lated by using the EKF ﬁlters. So the FastSLAM algorithm

is a Rao–Blackwellized particle ﬁlter. Each particle in the

FastSLAM is of the form

X

[m]

t

=

x

t,[m]

,µ

[m]

1,t

, Σ

[m]

1,t

,...,µ

[m]

N,t

, Σ

[m]

N,t

where [m] indicates the index of the particle, x

t,[m]

is its path

estimate, and µ

[m]

k,t

and Σ

[m]

k,t

are t he mean and covariance of the

Gaussian, representing the kth landmark location that is attached

to the mth particle. In FastSLAM1.0, new poses are sampled

using the most recent motion command u

t

x

[m]

t

∼ p

x

t

|x

[m]

t−1

,u

t

.

It is important to note that this proposal distribution uses only

the motion control u

t

,but ignores the current measurement z

t

.

So the FastSLAM1.0 approach is particularly troublesome if

the observation is too accurate relative to the vehicle’s mo-

tion noise. To overcome this problem, an improved version

called FastSLAM2.0 is proposed by Montemerlo et al. [22].

In FastSLAM2.0, the vehicle poses are sampled under consid-

eration of both the control u

t

and the measurement z

t

,which is

denoted by the following sampling distribution:

x

[m]

t

∼ p(x

t

|x

t−1,[m ]

,u

t

,z

t

,n

t

)

and as a result, the FastSLAM2.0 is superior to the

FastSLAM1.0 in all aspects [8].

The weight of each sample used in the resampling step is

called the importance weight, which is denoted by w

[m]

t

.In

FastSLAM2.0, the importance weight for resampling is given by

w

[m]

t

=

target distribution

proposal distribution

=

p(x

t,[m]

|z

t

,u

t

,n

t

)

p(x

t−1,[m]

|z

t−1

,u

t−1

,n

t−1

)p(x

[m]

t

|x

t−1,[m]

,z

t

,u

t

,n

t

)

.

Foracomplete derivation of the importance weight in

FastSLAM2.0, see [21] and [22].

In the FastSLAM algorithm, each particle does not repre-

sent a single momentary vehicle pose. Rather, it represents an

entire vehicle path history and the path history is recorded in

the map estimates. The map is represented as a set of indepen-

dent Gaussians, with linear time complexity, rather than a joint

map covariance with quadratic time complexity. Moreover, if

the landmarks are represented by a binary tree, linear complex-

ity in the number of landmarks becomes the log-time. These

are key properties of the FastSLAM and are the reason for its

computational speed. In addition to the obvious reduction in

computational complexity, the technique intrinsically provides

away of maintaining multihypothesis data association.

In spite of these advantages, FastSLAM suffers from the

drawbacks of linearization. The linearized approach can pro-

duce a critical problem in the particle ﬁlter such that the in-

accurate estimation caused by the linear approximation brings

the particles to converge to a wrong pose with rapid loss of the

ﬁlter consistency after resampling. Using EKFs for the feature

estimation also contains the linearization errors, and the error is

salient with large bearing noise of a range ﬁnder sensor. This ap-

proach can prevent a successful data association and can make

an inaccurate map.

To solve these problems, we propose the UFastSLAM in the

following section.

III. U

NSCENTED FASTSLAM

Unlike FastSLAM, UFastSLAM is based on the SUT.

UFastSLAM consists of three parts: the vehicle state estima-

tion, the feature state estimation, and the importance weights

calculation.

A. Vehicle State Estimation: Sampling Strategy in UFastSLAM

The particle ﬁlter in the RBPF framework relies on impor-

tance sampling, so it requires the design of proposal distribu-

tions that can approximate the true posterior reasonably well.

The most common strategy is to sample from the motion model.

This strategy can fail, however, if the new measurements appear

in the tail of the proposal distribution, or if the likelihood is too

sharp in comparison to the proposal distribution.

Several researchers have introduced the most current observa-

tions into the proposal distribution and have used some heuris-

tic techniques to improve the accuracy of the proposal distri-

bution [22], [26]–[28]. However, the Jacobian and inaccurate

linear approximations still exist in the covariance-related terms.

Instead of linearinzing the nonlinear models through the ﬁrst-

order Taylor series expansion at the mean of the vehicle state,

UFastSLAM computes a more accurate mean and more precise

uncertainty of the vehicle by applying the unscented particle

ﬁlter (UPF) technique, which takes into account a linear regres-

sion of weighted points [29]. The original UPF [17], however,

assumed that an observation is obtained in each time step. How-

ever, in t he SLAM problem, a lack of an observation or multiple

observations can happen frequently, so the original UPF was

modiﬁed for the SLAM purpose.

Since an observation is not always detected, constructing the

proposal distribution and sampling from this prior have two

steps. One is the prediction step and another is the measurement

update step. At ﬁrst, the state vector is augmented with a control

input and the observation

x

a[m ]

t−1

=

x

[m]

t−1

0

0

=

x

[m]

x,t−1

x

[m]

y,t−1

x

[m]

θ,t−1

0

0

,P

a[m ]

t−1

=

P

[m]

t−1

00

0 Q

t

0

00R

t

.

(2)

Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.

KIM et al.: UNSCENTED FastSLAM: A ROBUST AND EFFICIENT SOLUTION TO THE SLAM PROBLEM 811

Here, x

a[m ]

t−1

is the augmented vector for the state and x

[m]

t−1

is the

previous mean of the vehicle. Q

t

and R

t

are the control noise

covariance and the measurement noise covariance, respectively.

The augmented covariance matrix P

a[m ]

t−1

has 7 × 7 dimensions

in 3 DOF vehicle state estimation.

UFastSLAM deterministically extracts sigma points from the

Gaussian and passes these points through the nonlinear func-

tion. A symmetric set of 2L +1sigma points χ

a[i][m ]

t−1

for the

augmented state vector with L =7can be calculated and are

given by

χ

a[0][m]

t−1

= x

a[m ]

t−1

χ

a[i][m ]

t−1

= x

a[m ]

t−1

+

(L + λ)P

a[m ]

t−1

i

(i =1,...,L)

χ

a[i][m ]

t−1

= x

a[m ]

t−1

−

(L + λ)P

a[m ]

t−1

i−L

(i = L +1,...,2L)

where subscript i means the ith column of a matrix. The λ is

computed by λ = α

2

(L + κ) − L and α (0 <α<1)should

be a small number to avoid sampling nonlocal effects when

the nonlinearities are strong (α =0.002 is appropriate). κ is

ascaling parameter that determines how far the sigma points

are separated from the mean. The speciﬁc value of κ ( κ ≥ 0

to guarantee positive semideﬁniteness of the covariance matrix)

is not critical though, so a good default choice is κ =0.Each

sigma point χ

a[i][m ]

t−1

contains the state, control, and measurement

components given by

χ

a[i][m ]

t−1

=

χ

[i][m]

t−1

χ

u[i][m]

t

χ

z[i][m ]

t

. (3)

The motion model f is characterized by a nonlinear function,

and the set of sigma points χ

a[i][m ]

t−1

is transformed by the motion

model using the current control u

[m]

t

with the added control noise

component χ

u[i][m]

t

of each sigma point

¯χ

[i][m]

t

= f

u

[m]

t

+ χ

u[i][m]

t

,χ

[i][m]

t−1

=

¯χ

[i][m]

x,t

¯χ

[i][m]

y,t

¯χ

[i][m]

θ,t

. (4)

Here, ¯χ

[i][m]

t

is the transformed sigma point of the vehicle state.

[See Section VII-A for detailed expression of (4).] The ﬁrst two

moments of the predicted vehicle state are computed by a linear

weighted regression of the transformed sigma points ¯χ

[i][m]

t

:

x

[m]

t|t−1

=

2L

i=0

w

[i]

g

¯χ

[i][m]

t

(5)

P

[m]

t|t−1

=

2L

i=0

w

[i]

c

¯χ

[i][m]

t

− x

[m]

t|t−1

¯χ

[i][m]

t

− x

[m]

t|t−1

T

(6)

where a weight w

[i]

g

is used when computing the mean, and

the weight w

[i]

c

is used when recovering the covariance of the

Gaussian. These weights are calculated by

w

[0]

g

=

λ

(L + λ)

,w

[0]

c

=

λ

(L + λ)

+(1− α

2

+ β)

w

[i]

g

= w

[i]

c

=

1

2(L + λ )

(i =1,...,2L). (7)

Here, the parameter β is used to incorporate the knowledge

of the higher order moments of the posterior distribution. For a

Gaussian prior, the optimal choice is β =2[16], [25].

As some features are observed, data association provides their

identities, and (8)–(14) are employed to update the estimated

mean x

[m]

t

and the covariance P

[m]

t

of the vehicle:

¯

N

[i][m]

t

= h

¯χ

[i][m]

t

,µ

[m]

ˆ

k,t−1

+ χ

z[i][m ]

t

(8)

ˆn

[m]

t

=

2L

i=0

w

[i]

g

¯

N

[i][m]

t

(9)

S

[m]

t

=

2L

i=0

w

[i]

c

¯

N

[i][m]

t

− ˆn

[m]

t

¯

N

[i][m]

t

− ˆn

[m]

t

T

(10)

Σ

x,n[m ]

t

=

2L

i=0

w

[i]

c

¯χ

[i][m]

t

− x

[m]

t|t−1

¯

N

[i][m]

t

− ˆn

[m]

t

T

(11)

K

[m]

t

=Σ

x,n[m ]

t

S

[m]

t

−1

. (12)

The measurement sigma points

¯

N

[i][m]

t

are calculated in (8)

using the observation model h, characterized by a nonlinear

function, with the added measurement noise component χ

z[i][m ]

t

.

Here, µ

[m]

ˆ

k,t−1

is the previously registered

ˆ

kth feature mean that is

reobserved and identiﬁed in the current time step. The difference

between the reobserved feature position and the sigma point of

the vehicle pose ¯χ

[i][m]

t

is used to calculate the measurement

sigma points

¯

N

[i][m]

t

. ˆn

[m]

t

is the predicted measurement and

S

[m]

t

is the innovation covariance. The cross-covariance Σ

x,n[m ]

t

corresponds to the Jacobian term of the FastSLAM algorithm.

K

[m]

t

is the Kalman gain in the measurement update.

Note that the measurement noise covariance R

t

is not used as

an additive term for calculating the innovation covariance S

[m]

t

.

This is because the measurement noise is already included in

the augmented covariance (2) and was already considered to

calculate the predicted measurement ˆn

[m]

t

.Alternatively, the

augmented state vector and the augmented covariance (2) could

be deﬁned using the state and control inputs, and rather than

containing the measurement term in the augmentation, the mea-

surement noise covariance could be handled as an additive term

in (10). However, it is important to note that this can lead to a

negative deﬁnite of the covariance of the vehicle P

[m]

t

for a very

small measurement noise with multiple observations.

The estimated mean and its covariance of the vehicle state at

time t are calculated by

x

[m]

t

= x

[m]

t|t−1

+ K

[m]

t

z

t

− ˆn

[m]

t

(13)

P

[m]

t

= P

[m]

t|t−1

− K

[m]

t

S

[m]

t

K

[m]

t

T

. (14)

812 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

From the Gaussian distribution generated by the estimated

mean and covariance of the vehicle, the state of each particle is

sampled:

x

[m]

t

∼N

x

[m]

t

,P

[m]

t

. (15)

When there is no observation, the vehicle state is predicted

without the measurement update using (5) and (6), but if many

features are observed at the same time, (8)–(14) are repeated

for each observed feature, and the mean and the covariance of

the vehicle are updated based on the previously updated one.

In the multiple-observation case, before considering the next

observed feature, the sigma point χ

a[i][m ]

t

is refreshed by using

the updated mean (13) and covariance (14)

χ

a[i][m ]

t

=

x

a[m ]

t

,x

a[m ]

t

±

(L + λ)P

a[m ]

t

(16)

¯χ

[i][m]

t

= χ

[i][m]

t

. (17)

This approach produces accurate sigma points in each update

step and is important in estimating the vehicle state accurately.

The effectiveness of this approach to the vehicle state estima-

tion is illustrated in Section V-A, where it is called “modiﬁed

UPF aided FastSLAM.”

B. Feature State Estimation

1) Feature Update: The feature update deﬁnes the sigma

points using the previously registered mean and covariance of

the feature.

χ

[0][m ]

= µ

[m]

n

t

,t−1

χ

[i][m]

= µ

[m]

n

t

,t−1

+

(n + λ )Σ

[m]

n

t

,t−1

i

(i =1,...,n) (18)

χ

[i][m]

= µ

[m]

n

t

,t−1

−

(n + λ)Σ

[m]

n

t

,t−1

i−n

(i = n +1,...,2n)

where µ

[m]

n

t

,t−1

is the mean of the nth feature that is registered

in feature initialization step. Σ

[m]

n

t

,t−1

is the covariance matrix

of the nth feature, and it has 2×2dimensions in the RBPF

framework. n is the dimension of the feature state. If landmarks

are on a planar environment, it has only 2 DOF. In this case,

two-dimensional Gaussian ( n =2)issufﬁcient, and ﬁve sigma

points are required. λ = α

2

(n + κ) − n, and α =0.01 and

κ =0are appropriate for estimating the feature state.

The predicted measurement ˆz

[m]

t

and the Kalman gain

¯

K

[m]

t

are calculated as follows:

¯

Z

[i][m]

t

= h

χ

[i][m]

,x

[m]

t

(i =0,...,2n)

ˆz

[m]

t

=

2n

i=0

w

[i]

g

¯

Z

[i][m]

t

¯

S

[m]

t

=

2n

i=0

w

[i]

c

¯

Z

[i][m]

t

− ˆz

[m]

t

¯

Z

[i][m]

t

− ˆz

[m]

t

T

+ R

t

.

Here, h(·) is the observation model and the current vehicle

state of the mth particle x

[m]

t

is used. The transformed sigma

Fig. 1. Performance of the additive and the augmented approaches for the

measurement noise term in the feature update. Large measurement noises are

used (especially a large bearing noise of 8

◦

)toconﬁrm the difference between

the approaches.

points

¯

Z

[i][m]

t

are on the polar coordinate made by the nonlinear

transformation and are used to compute the predicted measure-

ment ˆz

[m]

t

and the innovation covariance

¯

S

[m]

t

.Theweights w

[i]

g

and w

[i]

c

are calculated as in the previous step using (7).

The measurement noise covariance R

t

is used as an addi-

tive term for calculating the innovation covariance

¯

S

[m]

t

.An

alternative way to update the feature is using the augmented

state with the measurement noise term, given in Section VII-B.

The difference between the additive approach and the aug-

mented approach was veriﬁed under the same carefully prepared

experimental conditions with large measurement noise values.

The performance of both approaches are shown in Fig. 1. As

shown in Fig. 1, the performance of each approach varies ac-

cording to the α-value, but the difference of the MSE is small

enough to be neglected. This is because the feature state has a

low-dimensional, constant size in UFastSLAM. However, the

augmented approach requires nine sigma points,whereas the

additive approach requires only ﬁve sigma points.Thus, con-

sidering the measurement noise as an additive term is more

efﬁcient for feature estimation.

¯

Σ

[m]

t

determines the cross-covariance between state and ob-

servation, which is used to compute the Kalman gain

¯

K

[m]

t

, and

is calculated by

¯

Σ

[m]

t

=

2n

i=0

w

[i]

c

χ

[i][m]

− µ

[m]

n

t

,t−1

¯

Z

[i][m]

t

− ˆz

[m]

t

T

¯

K

[m]

t

=

¯

Σ

[m]

t

¯

S

[m]

t

−1

. (19)

The Kalman gain calculated through UFastSLAM is more

accurate than the FastSLAM one. Finally, the mean µ

[m]

n

t

,t

and

the covariance Σ

[m]

n

t

,t

of the nth feature are updated by

µ

[m]

n

t

,t

= µ

[m]

n

t

,t−1

+

¯

K

[m]

t

z

t

− ˆz

[m]

t

(20)

Σ

[m]

n

t

,t

=Σ

[m]

n

t

,t−1

−

¯

K

[m]

t

¯

S

[m]

t

¯

K

[m]

t

T

(21)

where z

t

is the true measurement. The Cholesky factorization

is used in this feature update to make the algorithm more stable

numerically. Also note that the UFastSLAM does not employ

Jacobian matrices for calculating feature covariance.

KIM et al.: UNSCENTED FastSLAM: A ROBUST AND EFFICIENT SOLUTION TO THE SLAM PROBLEM 813

2) Feature Initialization: In the RBPF framework, the fea-

ture initialization is separated from the vehicle state estimator.

This means that it is possible to use the measurement noise co-

variance as an initial feature covariance [21], [22]. Therefore,

the UFastSLAM feature initialization uses the current measure-

ment z

t

and measurement noise covariance R

t

to calculate the

sigma points ψ

[i][m]

with a two-dimensional Gaussian (l =2).

Here, λ = α

2

(l + κ) − l, and α and κ can be of the same value

as the feature update. The mean µ

[m]

n

t

,t

and the covariance Σ

[m]

n

t

,t

of a new feature are calculated by

ψ

[0][m ]

= z

t

ψ

[i][m]

= z

t

+(

(l + λ)R

t

)

i

(i =1,...,l)

ψ

[i][m]

= z

t

− (

(l + λ)R

t

)

i−l

(i = l +1,...,2l)

¯

M

[i][m]

t

= h

−1

ψ

[i][m]

,x

[m]

t

(i =0,...,2l)

µ

[m]

n

t

,t

=

2l

i=0

w

[i]

g

¯

M

[i][m]

t

(22)

Σ

[m]

n

t

,t

=

2l

i=0

w

[i]

c

¯

M

[i][m]

t

−µ

[m]

n

t

,t

¯

M

[i][m]

t

−µ

[m]

n

t

,t

T

. (23)

Note that no explicit calculation of Jacobians are necessary

to implement the UFastSLAM algorithm. The UFastSLAM re-

quires computation of a matrix square root that can be imple-

mented directly using the Cholesky factorization. However, the

covariance matrices have low dimensions and can be expressed

recursively. Thus, not only does the UFastSLAM outperform

the FastSLAM in accuracy and robustness, but it also does so at

no extra computational cost.

The effectiveness of this approach to the feature state estima-

tion is illustrated in Section V-A, where it is called “UKF aided

FastSLAM2.0.”

C. Calculating Importance Weight and Resampling Strategy

Like FastSLAM2.0, the importance weight of UFastSLAM

should be computed by considering the most recent observa-

tions, and it is given by the following equation:

w

[m]

t

=

2πL

[m]

t

−(1/2)

e

−(1/2)

z

t

−ˆz

[m ]

t

T

L

[m ]

t

−1

z

t

−ˆz

[m ]

t

(24)

where

L

[m]

t

=

Σ

x,n[m ]

t

T

P

[m]

t

−1

Σ

x,n[m ]

t

+

¯

S

[m]

t

. (25)

One of the major inﬂuences on the performance of the par-

ticle ﬁlter is the resampling process. The particles with a low

importance weight are replaced by samples with a high weight

during the resampling process. The resampling technique used

the effective number of particles [30] as a criteria. The effective

number of particles is calculated by

N

eﬀ

=

1

M

m =1

(ˆw

[m]

)

2

(26)

where M is the total number of particles and ˆw

[m]

is the normal-

ized weight of the mth particle. If the variance of the importance

weights increases, the N

eﬀ

decreases. When N

eﬀ

drops below

athreshold of 50%ofthetotal number of particles, resampling

occurs. This can handle the degeneracy caused by the variance

increase.

The performance and effect of the aforementioned three parts

will be shown in Section V.

D. Complexity of UFastSLAM

This section describes the complexity of UFastSLAM. Since

the sigma point and linear regression calculations have constant

complexities, the number of particles M determines the com-

plexity of the ﬁlter. In UFastSLAM, computing the proposal

distribution, updating the landmarks, and calculating the impor-

tance weights have a complexity of O(M).Onthe other hand,

resampling requires a complexity of O(MN) [or O(M log N),

if the landmarks are represented by a binary tree]. To conclude,

UFastSLAM has the same complexity as FastSLAM.

IV. UF

ASTSLAM OVERVIEW

The pseudocode of UFastSLAM is as follows.

Unscented FastSLAM (z

t

,u

t

, X

t−1

,Q

t

,R

t

):

n = feature dimension; L = vehicle dimension

Set SUT parameters (α, β, κ, λ)

Calculate SUT weights (w

[i]

g

,w

[i]

c

,i=0∼ 2n(or 2L))

X

t

= X

aux

= ∅

for all particles

Retrieve X

t−1

Predict mean and covariance of the vehicle (5) and (6)

for all observations

ˆ

k = compatibility test

z

t

, µ

[m]

k,t−1

, Σ

[m]

k,t−1

)

end for

for

ˆ

k = known feature

Update mean and covariance of the vehicle (13) and (14)

Refresh sigma points (16) and (17)

w

[m]

t

= calculate importance weight (24)

end for

Sample from updated posterior (15)

if

ˆ

k = new feature

Calculate new feature mean and covariance (22) and (23)

else

Update mean and covariance of feature (20) and (21)

end if

for unobserved features

µ

[m]

k,t

= µ

[m]

k,t−1

Σ

[m]

k,t

=Σ

[m]

k,t−1

end for

add

x

[m]

t

,N

[m]

t

,

µ

[m]

N

[m ]

t

,t

, Σ

[m]

N

[m ]

t

,t

to X

aux

end for

Resample from X

aux

with probability ∝ w

[m]

t

Add new particles to X

t

Return X

t

814 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

Fig. 2. Estimated and true vehicle paths with estimated and true landmarks. (a)

Odometry path. (b) FastSLAM2.0. (c) UFastSLAM. The black line and green

stars denote the true path and landmark positions, respectively. The red line is

the mean estimate of vehicle position and the red dots are estimated landmarks.

The measurement noises are relatively large (σ

r

= 0.2 m, σ

φ

= 8

◦

)andonly

three particles were used.

V. S IMULATION RESULTS

Bailey et al. developed the SLAM simulator and opened

it to the public on a Web site [31]. This simulator made the

comparison of different SLAM algorithms possible, and Bailey

et al. [8] discussed the consistency of the FastSLAM2.0 using

this simulator. UFastSLAM was developed and compared with

the FastSLAM2.0 code for the same environment.

Before comparing UFastSLAM with FastSLAM2.0, results

of two important versions of the improved FastSLAM algo-

rithms are shown: the UKF-aided FastSLAM2.0 and the modi-

ﬁed UPF-aided FastSLAM. UKF-aided FastSLAM2.0 employs

the same sampling technique as FastSLAM2.0 but feature states

are updated using the UKF approach and initialized by the SUT.

On the other hand, modiﬁed UPF-aided FastSLAM consists

of the unscented-ﬁlter-based sampling technique and the stan-

dard EKF for the feature estimation. These approaches were

described in Section III-A and III-B.

Fig. 3. Comparison of the developed algorithms with FastSLAM2.0.

TABLE II

P

RELIMINARY EXPERIMENTS

A. Preliminary Experiments

The preliminary experiment was executed on rectangular

shaped trajectories of 100 m × 20 m, as shown in Fig. 2. The

vehicle has a 0.26-m wheel base and is equipped with a range-

bearing sensor with a maximum range of 20 m and a 180

◦

frontal

ﬁeld-of-view. Gaussian noise covariances are generated for both

the measurement and the motion. The control frequency was

40 Hz and observation scans were obtained every 5 Hz. Data as-

sociation is assumed to be known. The measurement noise was

0.2 m in range and 8

◦

in bearing. We used three particles to ob-

serve the estimate accuracy of each particle. The feature and the

vehicle states were predicted and updated by unscented transfor-

mation in UFastSLAM, which outperforms the FastSLAM2.0,

as shown in Fig. 2.

Each bar in Fig. 3 represents the mean of the estimated error

of the vehicle pose. The mean and variance of the MSE are

calculated over ten independent runs for each algorithm.

In Fig. 3 and Table II, the estimation accuracy of the

UKF-aided FastSLAM2.0 (B in Fig. 3) is better than that of

FastSLAM2.0 (A in Fig. 3). This is because a more accurate

transformation in the estimator produces better estimate results

than the linear approximation of the nonlinear function. How-

ever, the reduction of the MSE is only 1 m. This means that the

effect of the unscented ﬁlter in the UKF-aided FastSLAM2.0 (B

in Fig. 3) is not so large. This is because the feature covariance

has a low dimension in the RBPF framework.

The most important factor of the particle ﬁlter approach

is the sampling process. The FastSLAM framework is also a

particle-ﬁlter-based estimator, and as a result, the novel sam-

pling technique affects the performance of the estimator directly.

The modiﬁed UPF-aided FastSLAM (C in Fig. 3) had greatly

increased performance when compared with the UKF-aided

FastSLAM2.0 (B in Fig. 3).

Finally, the proposed algorithm UFastSLAM (D in Fig. 3)

had the most accurate result.

KIM et al.: UNSCENTED FastSLAM: A ROBUST AND EFFICIENT SOLUTION TO THE SLAM PROBLEM 815

Fig. 4. Effectiveness of UFastSLAM for large measurement noises. Each bar

represents the mean of the estimated vehicle pose. The mean and variance

of the MSE were calculated over ﬁve independent runs for each SLAM al-

gorithm. Ten particles were used in a small environment (20 m × 25 m). The

measurement noises of each experiment are as follows. (a) σ

r

=0.1,σ

φ

=3

◦

.

(b) σ

r

=0.2,σ

φ

=6

◦

.(c)σ

r

=0.2,σ

φ

=8

◦

.(d)σ

r

=0.3,σ

φ

=10

◦

.(e)

σ

r

=0.3,σ

φ

=14

◦

.The loop closure with known data association and the

long range of an observation (20 m) induced small MSEs in all experiments.

B. Performance Comparison Between UFastSLAM

and FastSLAM2.0

This section considers three types of simulation experiments

to demonstrate the effectiveness of the proposed algorithm,

UFastSLAM.

The ﬁrst set of experiments has been designed to validate the

effect of the measurement uncertainty. In this experiment, ten

particles were used with various measurement noise levels in a

relatively small environment (20 m × 25 m). The mean and vari-

ance of the MSE are calculated over ﬁve independent runs for

each algorithm. As the measurement noise was increased, the

estimation errors of the FastSLAM2.0 and the UFastSLAM al-

gorithms increased, as shown in Fig. 4. However, the estimation

error of UFastSLAM increased much more slowly than that of

FastSLAM2.0, and the variance of the MSE of the UFastSLAM

is smaller than that of FastSLAM2.0. Thus, UFastSLAM is more

robust to high sensor noise than FastSLAM2.0.

The sensor returns polar information (range and bearing),

which is converted to estimate Cartesian coordinates. In this

process, the large bearing uncertainty can lead to violations of

the assumption of local linearity. Therefore, the EKF’s mean

is biased, and even if there is no bias, the transformation of

uncertainties can become inconsistent. Due to this limitation,

the MSE of the FastSLAM2.0 is rapidly increased with large

error variances. On the other hand, the unscented ﬁlter is able t o

estimate the mean and the covariance to a higher order of accu-

racy than the EKF. Even if there is a large bearing uncertainty,

higher order information about the state distribution can be cap-

tured using a small number of sigma points,which produces the

robustness of UFastSLAM to the sensor noise.

The second experiment is concerned with how many parti-

cles are needed in UFastSLAM to obtain the same estimation

accuracy as FastSLAM2.0. The size of the environment was

about 100 m × 100 m, and the vehicle traveled about 500 m.

Table III presents the MSE and the number of particles.

Although UFastSLAM used one-ﬁfth of the particles of

FastSLAM2.0, the estimated error is almost the same.

TABLE III

S

ECOND EXPERIMENT

Fig. 5. (a) FastSLAM2.0 and ( b) UFastSLAM. The black line and green

(bright) asterisks denote the true and landmark positions, respectively. The red

(bright) line is the mean estimate of the vehicle position and the red (bright)

dots, near the asterisks, are estimated landmarks. These cases used 500 particles.

C. Evaluation of Filter Consistency

In this section, the consistency of both the UFastSLAM and

the FastSLAM2.0 is considered with same resampling strategy.

To verify the consistency of both algorithms, average nor-

malized estimation error squared (NEES) is used as a measure.

Thirty Monte Carlo simulations were performed with the two-

sided 95% probability concentration region. The acceptance

interval of this condition is in [2.19, 3.93].

The vehicle used in this experiment had a 0.26 m wheelbase

with 0.6 m/s velocity. The control and sensor frequencies were

40 and 5 Hz, respectively. The control noises were 0.3 m/s in

velocity and 3

◦

in steering angle, and the measurement noises

were 0.1 m in range and 1

◦

in bearing. The maximum sensor

range was 5 m, with a 180

◦

frontal ﬁeld-of-view. The environ-

ment used in this experiment is shown in Fig. 5 with the estimate

results of both algorithms.

816 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

Fig. 6. (a) FastSLAM2.0 and (b) UFastSLAM. The black thick line and green

(bright) asterisk denote the true path and landmark position, respectively. The

red (bright) dots, near the true vehicle pose, are the estimated vehicle states

and the red (bright) dots, near the green asterisk, are the estimated landmark

location. Here, 100 particles were used for both algorithms.

Estimating an accurate state and its uncertainty is an impor-

tant problem for improving the consistency of the SLAM ﬁlter.

Consider the two situations depicted in Fig. 6. The particle di-

versity of both cases is similar, because both algorithms use the

same resampling strategy. Actually, both algorithms resampled

similar numbers of times. However, since an inaccurate state

estimation induced a convergence of the particles to a wrong

pose, the estimated covariance of Fig. 6(a) does not contain the

true vehicle pose and the consistency of Fig. 6(a) is broken.

Fig. 6(a) has the possibility of an optimistic estimate. This is a

general case of FastSLAM2.0.

On the other hand, although the diversity of Fig. 6(b) is similar

to that of Fig. 6(a), the mean and covariance of Fig. 6(b) are

estimated well. As a r esult, the uncertainty of Fig. 6(b) contains

the true vehicle state. Since a more accurate estimate without

accumulating linearization error is achieved in UFastSLAM,

many particles can have accurate information repetitively. This

induces the possibility for maintaining the consistency. Fig. 7

shows that the consistency of UFastSLAM with 100 particles is

prolonged longer than that of FastSLAM2.0 with 500 particles.

VI. E

XPERIMENTAL RESULTS

A. Outdoor SLAM Result With Laser Scanner

UFastSLAM was compared to FastSLAM2.0 using the

Sydney Victoria Park dataset, a popular dataset in the SLAM

community. Fig. 8(a) shows Victoria Park with GPS data that

is represented by an intermittent yellow line. The vehicle had

a2.83 m wheel base and was equipped with the SICK laser

range ﬁnder with a 180

◦

frontal ﬁeld-of-view. The vehicle was

driven around the park for about 30 min, covering a path of over

3.5 km. The velocity and the steering angle were measured with

Fig. 7. (a) Consistency of FastSLAM2.0 with 100 particles. (b) Consistency

of FastSLAM2.0 with 500 particles. (c) Consistency of UFastSLAM with 100

particles. (d) Consistency of UFastSLAM with 500 particles. The red bounds

are the two-sided 95% probability concentration region with 30 Monte Carlo

runs. The peaks at 18 s originated from large heading errors because of a rapid

turn.

KIM et al.: UNSCENTED FastSLAM: A ROBUST AND EFFICIENT SOLUTION TO THE SLAM PROBLEM 817

Fig. 8. (a) Victoria Park, Sydney, Australia, with the GPS path. The yellow

(bright), intermittent path on the park is the GPS path of the vehicle. (b) Odom-

etry of the vehicle.

encoders but uneven terrain induced additional nonsystematic

errors because of wheel slippage and vehicle attitude. Conse-

quently, the odometry information from the encoder is poor,

as shown in Fig. 8(b). In the park, nearby trees were used as

point landmarks, and there were many spurious observations

including dynamic objects. Although the vehicle was equipped

with the GPS, the sensor gave intermittent information due to

limited satellite availability. Nevertheless, the ground true posi-

tion of the vehicle from the GPS was good enough to validate

the estimated vehicle state of the ﬁlter. In the experiment, ten

particles were used for both algorithms. Each algorithm was run

many times to conﬁrm the variance of the estimate error, and the

comparison results of the accuracy and the loop closure ability

of each ﬁlter are shown in Fig. 9.

The results show that for both ten- and one-particle cases, the

performance of UFastSLAM is better than that of FastSLAM2.0.

The estimated paths of the UFastSLAM coincide very well with

the GPS paths. Since the Kalman gains in the proposal distri-

bution and the feature update are calculated from the unscented

ﬁlter without using the Jacobian and the linearization of the

nonlinear model, the uncertainties are propagated well and the

accuracy of the state estimation has been improved over the

FastSLAM approach. Furthermore, we observed from many

runs of each algorithm that, similar to the simulation results, the

UFastSLAM showed a smaller variance in the estimation error.

In other words, the UFastSLAM almost always had the same

estimate results, which are shown in Fig. 9(c) and (d). However,

FastSLAM2.0 had a relatively large variance in the estimate

results, with larger estimation errors than UFastSLAM.

Fig. 9. (a) FastSLAM2.0 with ten particles. (b) FastSLAM2.0 with one par-

ticle. (c) UFastSLAM with ten particles. (d) UFastSLAM with one particle. In

all ﬁgures, the thick blue path is the GPS data and the solid red path is the

estimated path. The black asterisks are the estimated positions of the landmark.

The control noises in the experiment are σ

V

=0.8m/s,σ

γ

=1.8

◦

,andthe

measurement noises are σ

r

=1.5m,σ

φ

=2.8

◦

.For the unknown data asso-

ciation, the individual compatibility nearest neighbor test with 2σ acceptance

region is used.

818 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

Fig. 10. (a) Indoor environment for the experiment. Here, the robot is at (5.5

m, 0.5 m) in Fig. 11(b). (b) Our indoor mobile robot. In this experiment, the

only sensor was a ring of 12 sonars.

Fig. 11. Estimated maps and robot trajectories. (a) FastSLAM2.0. (b) UFast-

SLAM. The red thick lines and the black asterisks are the sonar line and the point

features, respectively. The blue thin lines are the estimated robot trajectories.

B. Indoor SLAM Result With Sonar Sensors

To validate the robustness of UFastSLAM to high sensor

noise, many experiments were carried out in indoor environ-

ments using sonar features (line and point [32]), one of which

Fig. 12. Average runtime of the indoor experiment. The circles are UFast-

SLAM and the asterisks are FastSLAM2.0. In this experiment, the binary tree

representation of the landmarks is not considered.

is described in this section. In the home environment shown in

Fig. 10(a), the robot explored the environment following the

right side obstacle, and after closing the loop, it traveled the

environment using the same navigation s trategy one more time.

Asonar ring consisting of 12 sonars was attached to a Active-

Media Pioneer 3 DX, as shown in Fig. 10(b) and a 2-GHz laptop

was used.

Fig. 11 shows the ﬁnal estimated map and the trajectory of the

robot. FastSLAM2.0 failed to close the loop and it generated an

inconsistent map as shown in Fig. 11(a), since the uncertainty of

sonar features is large (0.1–0.2 m in range, 5.5–6

◦

in bearing).

On the other hand, UFastSLAM estimated the robot trajectory

and the map correctly, as shown in Fig. 11(b). In this experiment,

20 particles were used to represent the posterior.

To validate the complexity and the computational cost, we

analyzed the execution time of the experiment. Fig. 12 describes

the average execution time of both algorithms. The complexity

of both algorithms is linear, since we did not use the binary tree

technique for the landmark representation in this experiment.

The computational cost of UFastSLAM is slightly larger than

FastSLAM2.0.

VII. C

ONCLUSION

This paper proposed the UFastSLAM algorithm as a robust

and effective SLAM solution. The main advantage of this al-

gorithm is that it does not use the derivation of t he Jacobian

matrices and the linear approximations to the nonlinear func-

tions in the RBPF framework.

The proposed algorithm updates the mean and the covari-

ance of the f eature state by using the unscented ﬁlter to avoid

linearization errors and Jacobian calculations in the feature es-

timation. New features are also registered by using an accurate

transformation, SUT. In the localization process, the SUT is

implemented in the prediction step of the vehicle state, and the

unscented ﬁlter provides better proposal distribution without

accumulating linearization errors and without calculating the

Jacobian matrices in the measurement update step.

Due to this, the accuracy of the state estimation has been

improved over the previous approaches. This means that the

proposed algorithm has the additional advantage of reducing the

number of particles while maintaining the estimation accuracy.

From the consistency viewpoint, since the vehicle and the

feature states are estimated without accumulating linearization

errors in UFastSLAM, many particles can have accurate i n-

formation repetitively. Thus, although the particle diversity is

KIM et al.: UNSCENTED FastSLAM: A ROBUST AND EFFICIENT SOLUTION TO THE SLAM PROBLEM 819

similar to the previous algorithm, more accurate estimation can

be achieved consistently for longer time periods.

We conclude from the results in large-scale environments

with various sensor noise uncertainties that the proposed

UFastSLAM algorithm, even with fewer particles, yields signif-

icantly more accurate and robust estimation results compared

with the previous linearization approaches. Additionally, we

conclude that the consistency of UFastSLAM, even with fewer

particles, is prolonged longer than that of FastSLAM2.0.

However, since the UFastSLAM also employs the resampling

process, the reduction of the impact of the resampling should

be treated more thoroughly to avoid an underestimate to the

uncertainty .

A

PPENDIX

A. Detailed Equations for Passing the Sigma Points Through

the Motion Model

To simplify the exposition, we introduce the following nota-

tions. Each ith sigma points state component, χ

[i][m]

t−1

, consists

of the three components

χ

[i][m]

t−1

=

χ

[i][m]

x,t−1

χ

[i][m]

y,t−1

χ

[i][m]

θ,t−1

. (27)

Let us suppose the Ackerman model. The control inputs

[V

t

G

t

]

T

,with the added control noise component χ

u[i][m]

t

,are

as follows:

V

n

G

n

=

V

t

+ χ

u[i][m]

V,t

G

t

+ χ

u[i][m]

G,t

(28)

where

χ

u[i][m]

t

=

χ

u[i][m]

V,t

χ

u[i][m]

G,t

. (29)

Then, the state components χ

[i][m]

t−1

of the sigma points are passed

through the Ackerman model:

¯χ

[i][m]

t

= χ

[i][m]

t−1

+

V

n

∆t cos

G

n

+ χ

[i][m]

θ,t−1

V

n

∆t sin

G

n

+ χ

[i][m]

θ,t−1

V

n

∆t (sin(G

n

)/wheelbase)

. (30)

B. Augmented Approach for the Feature Update

Forthe feature update, the augmented state of the feature is

constructed using the previously registered mean and covariance

of the feature [24]:

µ

a[m ]

n

t

,t−1

=

µ

[m]

n

t

,t−1

0

, Σ

a[m ]

n

t

,t−1

=

Σ

[m]

n

t

,t−1

0

0 R

t

(31)

where µ

[m]

n

t

,t−1

is the mean of the nth feature that is registered

in feature initialization step. Σ

[m]

n

t

,t−1

is the covariance matrix

of the nth feature, and it has 2 × 2dimensions in the RBPF

framework. R

t

is the measurement noise covariance.

The sigma points are deﬁned using these augmented states,

and the Kalman gain

¯

K

[m]

t

is calculated by (19) without using

R

t

as an additive term in the innovation covariance

¯

S

[m]

t

.

A

CKNOWLEDGMENT

The authors would like to thank T. Bailey for providing the

FastSLAM code, K. Lee for supporting the navigation program,

and the University of Sydney for the use of the Victoria Park

dataset.

R

EFERENCES

[1] H. Durrant-Whyte and T. Bailey, “Simultaneous localization and mapping:

Part I,” IEEE Robot. Autom. Mag.,vol. 13, no. 2, pp. 99–108, Jun.

2006.

[2] T. Bailey and H. Durrant-Whyte, “Simultaneous localization and mapping:

Part II,” IEEE Robot. Autom. Mag.,vol. 13, no. 3, pp. 108–117, Sep. 2006.

[3] R. Martinez-Cantin and J. A. Castellanos, “Unscented SLAM for large-

scale outdoor environments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots

Syst., 2005, pp. 328–333.

[4] S. Huang and G. Dissanayake, “Convergence and consistency analysis for

extended Kalman ﬁlter based SLAM,” IEEE Trans. Robot.,vol.23, no. 5,

pp. 1036–1049, Oct. 2007.

[5] R. Martinez-Cantin and J. A. Castellanos, “Bounding uncertainty in EKF-

SLAM: The robocentric local approach,” in Proc. IEEE Int. Conf. Robot.

Autom., 2006, pp. 430–435.

[6] J. A. Castellanos, J. Neira, and J. D. Tardos, “Limits to the consistency

of EKF-based SLAM,” in Proc. 5th IFAC Symp. Intell. Auton. Vehicles,

Lisbon, Portugal, Jul. 2004, pp. 1244–1249.

[7] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, “Consistency of

the EKF-SLAM algorithm,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots

Syst., 2006, pp. 3562–3568.

[8] T. Bailey, J. Nieto, and E. Nebot, “Consistency of the FastSLAM algo-

rithm,” in Proc. IEEE Int. Conf. Robot. Autom., 2006, pp. 424–429.

[9] J. A. Castellanos, R. Martinez-Cantin, J. D. Tardos, and J. Neira, “Robo-

centric map joining: Improving the consistency of EKF-SLAM,” Robot.

Auton. Syst.,vol. 55, pp. 21–29, 2007.

[10] Y. Bar Sahlom, X. R. Li, and T. Kirubarajan, Estimation With Applications

to Tracking and Navigation.New Yo rk: Wiley, 2001.

[11] J. D. Tardos, J. Neira, P. M. Newman, and J. J. Leonard, “Robust mapping

and localization in indoor environments using sonar data,” Int. J. Robot.

Res.,vol. 21, no. 4, pp. 311–330, 2002.

[12] C. Estrada, J. Neira, and J. D. Tardos, “Hierarchical SLAM: Real-time

accurate mapping of large environments,” IEEE Trans. Robot.,vol.21,

no. 4, pp. 588–596, Aug. 2005.

[13] S. Julier, J. Uhlmann, and H. F. Durrant-Whyte, “A new method for the

nonlinear transformation of means and covariances in ﬁlters and estima-

tors,” IEEE Trans. Autom. Control,vol. 45, no. 3, pp. 477–482, Mar.

2000.

[14] S. Julier and J. K. Uhlmann, “A counter example to the theory of simul-

taneous localization and map building,” in Proc. IEEE Int. Conf. Robot.

Autom., 2001, pp. 4238–4243.

[15] S. Julier and J. K. Uhlmann, “Unscented ﬁltering and nonlinear estima-

tion,” Proc. IEEE,vol. 92, no. 3, pp. 401–422, Mar. 2004.

[16] S. Julier, “The scaled unscented transformation,” in Proc. Amer. Control

Conf., 2002, pp. 4555–4559.

[17] R. Merwe, A. Doucet, N. Freitas, and E. Wan, “The unscented parti-

cle ﬁlter,” Cambridge Univ., Eng. Dep., Cambridge, U.K., Tech. Rep.

CUED/F-INFENG/TR 380. 2000.

[18] K. Murphy, “Bayesian map learning in dynamic environments,” in Neural

Information Proceedings System (NIPS).Cambridge, MA: MIT Press,

1999.

[19] R. Martinez-Cantin, N. de Freitas, and J. A. Castellanos, “Analysis of

particle methods for simultaneous robot localization and mapping and a

new algorithm: Marginal-SLAM,” in Proc. IEEE Int. Conf. Robot. Autom.,

2007, pp. 2415–2420.

[20] K. R. Beevers and W. H. Huang, “Fixed-lag sampling strategies for particle

ﬁltering SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2007, pp. 2433–

2438.

820 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008

[21] M. Montemerlo and S. Thrun, “Simultaneous localization and mapping

with unknown data association using FastSLAM,” in Proc. IEEE Int. Conf.

Robot. Autom., 2003, pp. 1985–1991.

[22] M. Montemerlo, “FastSLAM: A factored solution to the simultaneous

localization and mapping problem with unknown data association,” Ph.D.

dissertation, Carnegie Mellon Univ., Pittsburgh, PA, 2003,

[23] C. Kim, R. Sakthivel, and W. K. Chung, “Unscented FastSLAM: A robust

algorithm for the simultaneous localization and mapping problem,” in

Proc. IEEE Int. Conf. Robot. Autom., 2007, pp. 2439–2445.

[24] X. Wang and H. Zhang, “A UPF-UKF framework for SLAM,” in Proc.

IEEE Int. Conf. Robot. Autom., 2007, pp. 1664–1669.

[25] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics.Cambridge,

MA: MIT Press, 2005.

[26] G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid

mapping with Rao–Blackwellized particle ﬁlters,” IEEE Trans. Robot.,

vol. 23, no. 1, pp. 34–46, Feb. 2007.

[27] P. Beeson, A. Murarka, and B. Kuipers, “Adapting proposal distributions

for accurate, efﬁcient mobile robot localization,” in Proc. IEEE Int. Conf.

Robot. Autom., 2006, pp. 49–55.

[28] T. D. Barfoot, “Online visual motion estimation using FastSLAM with

SIFT features,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syt., 2005,

pp. 579–585.

[29] R. Merwe, “Sigma-point Kalman ﬁlters for probabilistic inference in dy-

namic state-space models,” Ph.D. dissertation, OGI School of Sci. & Eng.,

Oregon Health and Sci. Univ., Portland, OR, Apr. 2004.

[30] A. Doucet, N. de Freitas, and N. Gordan, Sequential Monte Carlo Methods

in Practice.New York: Springer-Verlag, 2001.

[31] (2008, Jun.). [Online]. Available: http://www-personal.acfr.usyd.edu.au/

tbailey/software/index.html

[32] J. Choi, S. Ahn, M. Choi, and W. K. Chung, “Metric SLAM in home

environment with visual objects and sonar features,” in Proc. IEEE/RSJ

Int. Conf. Intell. Robots Syt., 2006, pp. 4048–4053.

Chanki Kim (S’07) received the B.S. degree in

mechanical engineering from Hanyang University,

Seoul, Korea, in 2003, and the M.S. degree in me-

chanical engineering, in 2005 from Pohang Uni-

versity of Science and Technology (POSTECH),

Pohang, Korea, where, he is currently working to-

ward the Ph.D. degree.

His current research interests include mobile robot

simultaneous localization and mapping (SLAM),

path planning, and integrated exploration.

Rathinasamy Sakthivel received the B.Sc., M.Sc.,

and Ph.D. degrees in mathematics from Bharathiar

University, Coimbatore, India, in 1992, 1994, and

1999, respectively.

He is currently a Postdoctoral Researcher in

the Department of Mechanical Engineering, Pohang

University of Science and Technology (POSTECH),

Pohang, Korea. From 2003 to 2005, he was a JSPS

(Japan Society for the Promotion of Science) Fellow

in the Department of Systems Innovation and Infor-

matics, Kyushu Institute of Technology, Japan. His

current research interests include autonomous mobile robots, simultaneous lo-

calization and mapping (SLAM) problems, and robust control for nonlinear

systems.

WanKyun Chung (M’84) received the B.S. degree

in mechanical design from Seoul National University,

Seoul, Korea, in 1981, the M.S. degree in mechanical

engineering and the Ph.D. degree in production en-

gineering from Korea Advanced Institute of Science

and Technology (KAIST), Daejeon, Korea, in 1983,

1987, respectively.

In 1987, he was a Professor in the School of

Mechanical Engineering, Pohang University of Sci-

ence and Technology (POSTECH), Pohang, Korea.

In 1988, he was a Visiting Professor at the Robotics

Institute Carnegie-Mellon University, Pittsburgh, PA. In 1995, he was a Visiting

Scholar at the University of California, Berkeley. He is currently working as the

Director of the National Research Laboratory for Intelligent Mobile Robot Nav-

igation. He is also on the International Editorial Board for Advanced Robotics.

His current research interests include localization and navigation o f mobile

robots, underwater robots, and development of robust controllers for precision

motion control.

Dr. Chung is an Associate Editor for IEEE T

RANSACTION ON ROBOTICS.