ArticlePDF Available

Unscented FastSLAM: A robust and efficient solution to the SLAM problem

Authors:

Abstract and Figures

The Rao-Blackwellized particle filter (RBPF) and FastSLAM have two important limitations, which are the derivation of the Jacobian matrices and the linear approximations of nonlinear functions. These can make the filter 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 filter consistency and state estimation accuracy, and requires smaller number of particles than the FastSLAM approach. Simulation results in large-scale environments and experimental results with a benchmark dataset are presented, demonstrating the superiority of the UFastSLAM algorithm.
Content may be subject to copyright.
808 IEEE TRANSACTIONS ON ROBOTICS, VOL. 24, NO. 4, AUGUST 2008
Unscented FastSLAM: A Robust and Efficient
Solution to the SLAM Problem
Chanki Kim,Student Member, IEEE,Rathinasamy Sakthivel, and Wan Kyun Chung,Member, IEEE
Abstract—The Rao–Blackwellized particle filter (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 filter 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 filter 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 filter (RBPF),
scaled unscented transformation (SUT), simultaneous localization
and mapping (SLAM), unscented Kalman filter (UKF), unscented
particle filter (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 classified
according to their underlying basic principle. The most popular
approaches to the SLAM problem are the extended Kalman filter
(EKF-SLAM) and the Rao–Blackwellized particle filter (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 figures in this paper are available online
at http://ieeexplore.ieee.org.
Digital Object Identifier 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 filter
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 filter
(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
filter algorithm for a novel proposal distribution in the sampling
step. The unscented filter 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 first-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 flexibil-
ity to handle nonlinearity and non-Gaussianity, have become
exceptionally popular due to their relative simplicity, computa-
tional efficiency, 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 filter 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-filter-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 filter 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 filter
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 fil-
ter in the sampling step of the particle filter. In particular,
when multiple features are observed at the same time, the
sigma points of the unscented filter 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 filter 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 significantly 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 filter 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
filter 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 finder 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 filter is
also analyzed in this paper.
The structure of the paper is as follows. Section II briefly 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 filtering 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
specifies 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
t1
,u
t
).
The current pose x
t
is a probabilistic function of the vehicle
control u
t
and the previous pose x
t1
.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 efficient 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 filter to approximate the ideal re-
cursive Bayesian filter for estimating the vehicle pose. The
remaining posterior of feature locations is analytically calcu-
lated by using the EKF filters. So the FastSLAM algorithm
is a Rao–Blackwellized particle filter. 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]
t1
,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
t1,[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
t1,[m]
|z
t1
,u
t1
,n
t1
)p(x
[m]
t
|x
t1,[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 filter 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
filter 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 finder 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 filter 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 first-
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
filter (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
modified 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 first, the state vector is augmented with a control
input and the observation
x
a[m ]
t1
=
x
[m]
t1
0
0
=
x
[m]
x,t1
x
[m]
y,t1
x
[m]
θ,t1
0
0
,P
a[m ]
t1
=
P
[m]
t1
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 ]
t1
is the augmented vector for the state and x
[m]
t1
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 ]
t1
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 ]
t1
for the
augmented state vector with L =7can be calculated and are
given by
χ
a[0][m]
t1
= x
a[m ]
t1
χ
a[i][m ]
t1
= x
a[m ]
t1
+
(L + λ)P
a[m ]
t1
i
(i =1,...,L)
χ
a[i][m ]
t1
= x
a[m ]
t1
(L + λ)P
a[m ]
t1
iL
(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 specific value of κ ( κ 0
to guarantee positive semidefiniteness of the covariance matrix)
is not critical though, so a good default choice is κ =0.Each
sigma point χ
a[i][m ]
t1
contains the state, control, and measurement
components given by
χ
a[i][m ]
t1
=
χ
[i][m]
t1
χ
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 ]
t1
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]
t1
=
¯χ
[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 first 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|t1
=
2L
i=0
w
[i]
g
¯χ
[i][m]
t
(5)
P
[m]
t|t1
=
2L
i=0
w
[i]
c
¯χ
[i][m]
t
x
[m]
t|t1

¯χ
[i][m]
t
x
[m]
t|t1
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,t1
+ χ
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|t1

¯
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,t1
is the previously registered
ˆ
kth feature mean that is
reobserved and identified 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 defined 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 definite 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|t1
+ K
[m]
t
z
t
ˆn
[m]
t
(13)
P
[m]
t
= P
[m]
t|t1
K
[m]
t
S
[m]
t
K
[m]
t
T
. (14)
Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.
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 “modified
UPF aided FastSLAM.”
B. Feature State Estimation
1) Feature Update: The feature update defines the sigma
points using the previously registered mean and covariance of
the feature.
χ
[0][m ]
= µ
[m]
n
t
,t1
χ
[i][m]
= µ
[m]
n
t
,t1
+
(n + λ
[m]
n
t
,t1
i
(i =1,...,n) (18)
χ
[i][m]
= µ
[m]
n
t
,t1
(n + λ
[m]
n
t
,t1
in
(i = n +1,...,2n)
where µ
[m]
n
t
,t1
is the mean of the nth feature that is registered
in feature initialization step. Σ
[m]
n
t
,t1
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)issufficient, and five 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
)toconfirm 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 verified 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 five sigma points.Thus, con-
sidering the measurement noise as an additive term is more
efficient 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
,t1

¯
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
,t1
+
¯
K
[m]
t
z
t
ˆz
[m]
t
(20)
Σ
[m]
n
t
,t
[m]
n
t
,t1
¯
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.
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 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
)
il
(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 influences on the performance of the par-
ticle filter 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
eff
=
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
eff
decreases. When N
eff
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 filter. 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
t1
,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
t1
Predict mean and covariance of the vehicle (5) and (6)
for all observations
ˆ
k = compatibility test
z
t
, µ
[m]
k,t1
, Σ
[m]
k,t1
)
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,t1
Σ
[m]
k,t
[m]
k,t1
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
Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.
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-
fied 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, modified UPF-aided FastSLAM consists
of the unscented-filter-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
field-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 filter 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 filter approach
is the sampling process. The FastSLAM framework is also a
particle-filter-based estimator, and as a result, the novel sam-
pling technique affects the performance of the estimator directly.
The modified 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.
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 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 five 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 first 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 five 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 filter 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-fifth 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 field-of-view. The environ-
ment used in this experiment is shown in Fig. 5 with the estimate
results of both algorithms.
Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.
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 filter.
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 finder with a 180
frontal field-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.
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 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 filter. In the experiment, ten
particles were used for both algorithms. Each algorithm was run
many times to confirm the variance of the estimate error, and the
comparison results of the accuracy and the loop closure ability
of each filter 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
filter 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 figures, 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.
Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.
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 final 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 filter 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 filter 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
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 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]
t1
, consists
of the three components
χ
[i][m]
t1
=
χ
[i][m]
x,t1
χ
[i][m]
y,t1
χ
[i][m]
θ,t1
. (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]
t1
of the sigma points are passed
through the Ackerman model:
¯χ
[i][m]
t
= χ
[i][m]
t1
+
V
n
t cos
G
n
+ χ
[i][m]
θ,t1
V
n
t sin
G
n
+ χ
[i][m]
θ,t1
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
,t1
=
µ
[m]
n
t
,t1
0
, Σ
a[m ]
n
t
,t1
=
Σ
[m]
n
t
,t1
0
0 R
t
(31)
where µ
[m]
n
t
,t1
is the mean of the nth feature that is registered
in feature initialization step. Σ
[m]
n
t
,t1
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 defined 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 filter 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 filters 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 filtering 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 filter,” 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
filtering SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2007, pp. 2433–
2438.
Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.
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 filters,” 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, efficient 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 filters 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.
Authorized licensed use limited to: IEEE Xplore. Downloaded on December 23, 2008 at 04:03 from IEEE Xplore. Restrictions apply.
... Kim, together with his coworkers, proposed the unscented FastSLAM (UFastSLAM) algorithm. Based on the FastSLAM algorithm, they used an unscented Kalman filter (UKF) instead of an extended Kalman filter (EKF) for robot pose estimation, which improved the filtering accuracy of the robot [17]. However, they did not solve the particle degradation problem. ...
... algorithm, where the mobile robot positioning map accuracy caused serious impact. In the literature [17][18][19][20][21][22][23][24][25][26][27][28][29], researchers proposed different improvement strategies. One was to apply the global optimal value algorithm to change the particle set to avoid falling into the local optimal. ...
Article
This paper improves the accuracy of a mine robot's positioning and mapping for rapid rescue. Specifically, we improved the FastSLAM algorithm inspired by the lion swarm optimization method. Through the division of labor between different individuals in the lion swarm optimization algorithm, the optimized particle set distribution after importance sampling in the FastSLAM algorithm is realized. The particles are distributed in a high likelihood area, thereby solving the problem of particle weight degradation. Meanwhile, the diversity of particles is increased since the foraging methods between individuals in the lion swarm algorithm are different so that improving the accuracy of the robot's positioning and mapping. The experimental results confirmed the improvement of the algorithm and the accuracy of the robot.
... Different works [18]- [20] have considered the use of PFs in low-cost GNSS/INS systems for high-accuracy positioning and navigation in land-vehicle applications. Among them, a hybrid PF-based implementation -the Unscented Particle Filter (UPF) [21], [22] -targets to improve the Importance Sampling (IS) process by operating an upstream Unscented Kalman Filter (UKF) [23], [24]. The latter is meant to deliver an accurate enough approximation of the optimal importance density, matching with the posterior state density, to feed the underlying PF stage. ...
... A simple block diagram illustrating the main stages of the UPF routine is shown in Fig. 2. At every estimation epoch, the UKF posterior state mean and covariance estimates from measurement updates are exploited to define a new importance density that, correspondingly, drives the particles sampling. After computing their importance weights, the posterior state mean and covariance estimates are updated and further refined [21], [22], [28], [49]. For ease of comprehension, the interested reader can refer to [21] which collects few details and proposes a formalization for each building block reported in Fig. 2. ...
Article
Full-text available
Tight integration algorithms fusing Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) have become popular in many high-accuracy positioning and navigation applications. Despite their reliability, common integration architectures can still run into accuracy drops under challenging navigation settings. The growing computational power of low-cost, embedded systems has allowed for the exploitation of several advanced Bayesian state estimation algorithms, such as the Particle Filter (PF) and its hybrid variants, e.g. Unscented Particle Filter (UPF). Although sophisticated, these architectures are not immune from multipath scattering and Non-Line-of-Sight (NLOS) signal receptions, which frequently corrupt satellite measurements and jeopardise GNSS/INS solutions. Hence, a certain level of modelling adaptivity should be granted to avoid severe drifts in the estimated states. Given these premises, the paper presents a novel Adaptive Unscented Particle Filter (AUPF) architecture leveraging two cascading stages to cope with disruptive, biased GNSS input observables in harsh conditions. A INS-based signal processing block is implemented upstream of a Redundant Measurement Noise Covariance Estimation (RMNCE) stage to strengthen the adaptation of observables’ statistics and improve the state estimation. An experimental assessment is provided for the proposed robust AUPF that demonstrates a 10 % average reduction of the horizontal position error above the 75-th percentile. In addition, a comparative analysis both with previous adaptive architectures and a plain UPF is carried out to highlight the improved performance of the proposed methodology.
... The FastSLAM algorithm based on EKF and particle filtering [10] is well known. The uFastSLAM based on UKF and particle filtering [11] showed clearly the better properties of UKF [12], [13]. A purely particle filter (PF)-based application would give the more accurate results but it is hard to manage the necessary computational complexity in realtime applications. ...
Article
Advanced robotics and autonomous vehicles rely on filtering and sensor fusion techniques to a large extent. These mobile applications need to handle the computations onboard at high rates while the computing capacities are limited. Therefore, any improvement that lowers the CPU time of the filtering leads to more accurate control or longer battery operation. This article introduces a generic computational relaxation for the unscented transformation (UT) that is the key operation of the Unscented Kalman filter-based applications. The central idea behind the relaxation is to pull out the linear part of the filtering model and avoid the calculations for the kernel of the nonlinear part. The practical merit of the proposed relaxation is demonstrated through a simultaneous localization and mapping (SLAM) implementation that underpins the superior performance of the algorithm in the practically relevant cases, where the nonlinear dependencies influence only an affine subspace of the image space. The numerical examples show that the computational demand can be mitigated below 50% without decreasing the accuracy of the approximation. The method described in this article is implemented and published as an open-source C ++ library RelaxedUnscentedTransformation on GitHub.
... Kalman methods, turns out to be useful in a variety of GNSS applications since it offers the chance of finely tuning the modelling of input measurements z k depending on the characteristics of the external environment. Among the various PF variants proposed in literature [3,30,31], this research is concerned with a hybrid implementation -the UPF [18,32] -which globally improves the Importance Sampling (IS) process by employing an upstream UKF stage and, accordingly, reducing the occurrence rate of particles degeneracy phenomenon [33]. The UPF architecture consists of a baseline PF stage which exploits a feeding UKF module to construct an importance (or proposal) distribution π (x k |z k ). ...
Conference Paper
Several navigation filters have been developed since the early steps of Global Navigation Satellite Systems (GNSS) to provide high-accuracy Positioning, Navigation and Timing (PNT), and many solutions are available in the literature to support a plethora of applications. In the context of vehicular navigation and positioning, advanced state estimation and sensors fusion techniques cannot cope by themselves with strong multipath effects in dense urban areas. Therefore, these solutions require more robust approaches typically involving an additional processing effort, especially in low-cost Inertial Navigation System (INS)/GNSS Tightly-Coupled (TC) integration scheme. This work analyzes state-of-the-art covariance matrix estimation methods and proposes an INS-based pre-processing stage to mitigate the impact of undesired, multipath-related bias injections without inhibiting the Inertial Navigation System (INS)/GNSS integration. The proposed adaptive solution improves the overall stability and estimation accuracy of a set of Bayesian filters, i.e., Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Unscented Particle Filter (UPF). Results are presented about a real dataset in which an advanced, state-of-the-art Adaptive UPF (AUPF) TC scheme, applied to a low-cost integrated setup, occasionally failed to track the navigation solution due to poorly conditioned GNSS measurements.
... Regional path planning is designed to avoid obstacles on the map and new paths must be planned. We apply Dynamic Window Approach (DWA) [18] to achieve this goal. In Fig. 4, we can use Eqs. ...
Article
Full-text available
Multifunctional composites can accomplish multiple tasks such as shape morphing, sensing, and load bearing using a single structure. Smart materials including liquid crystal elastomers (LCE) and shape memory polymers (SMP) have long been used as the primary components of multifunctional composites because of their shape and property changes in response to external stimuli. However, LCEs can generate rapid and reversible shape changes but are soft and require a constant temperature to retain their deformed shape; SMPs have favorable mechanical properties but few can achieve reversible actuations. Moreover, both LCEs and SMPs have limited capability for tunable shape morphing. Multi‐material 3D printing of smart materials, also known as 4D printing, has seen significant advances enabling the fabrication of composites with novel functionality. In this work, 4D printing is leveraged to create an LCE‐SMP composite that can achieve not only rapid and reversible shape changes, but also cooling‐rate regulated tunable shape morphing. The latter is achieved by harnessing the distinct time‐dependent thermomechanical properties of LCEs and SMPs. Furthermore, the composite has a high stiffness at low temperature to support heavy loads. The LCE‐SMP composite hence offers a novel approach to achieve tunable shape morphing for future engineering applications.
Article
Full-text available
The emerging field of soft robotics presents a new paradigm for robot design in which “precision through rigidity” is replaced by “cognition through compliance.” Lightweight and flexible, soft robots have vast potential to interact with fragile objects and navigate unstructured environments. Like octopuses and worms in nature, soft robots’ flexible bodies conform to hard objects and reconfigure for different tasks, delegating the burden of control from brain to body through embodied cognition. However, because of the lack of efficient modeling and simulation tools, soft robots are primarily designed by hand. Typically, hard components from rigid robots or living creatures are heuristically substituted for comparable soft ones. Autonomous design and manufacturing methodologies are urgently required to produce bespoke, high‐performing robots. Currently, design methodologies exist between simple but realistic parametric optimizations, and evolutionary algorithms which simulate morphology and control coevolution. To find high‐performing designs, novel high‐fidelity simulators and high‐throughput manufacturing and testing processes are required to explore the complex soft material, morphology and control landscape, blending simulation, and experimental data. This article reviews the state of the art in autonomous soft robotic design. Existing manual and automated designs are surveyed and future directions to automate soft robot design and manufacturing are presented. By using soft and functional materials to deform around objects and adapt to new environments, soft robotics has the potential to revolutionize material handling and terrain navigation. But in the absence of accurate modeling tools, they are still laboriously designed manually. This article reviews progress toward autonomous modeling, simulation, and design.
Article
Full-text available
Setae, fibrils located on a gecko's feet, have been an inspiration of synthetic dry microfibrillar adhesives in the last two decades for a wide range of applications due to unique properties: residue‐free, repeatable, tunable, controllable and silent adhesion; self‐cleaning; and breathability. However, designing dry fibrillar adhesives is limited by a template‐based‐design‐approach using a pre‐determined bioinspired T‐ or wedge‐shaped mushroom tip. Here, a machine learning‐based computational approach to optimize designs of adhesive fibrils is shown, exploring a much broader design space. A combination of Bayesian optimization and finite element methods creates novel optimal designs of adhesive fibrils, which are fabricated by two‐photon‐polymerization‐based 3D microprinting and double‐molding‐based replication out of polydimethylsiloxane. Such optimal elastomeric fibril designs outperform previously proposed designs by maximum 77% in the experiments of dry adhesion performance on smooth surfaces. Furthermore, finite‐element‐analyses reveal that the adhesion of the fibrils is sensitive to the 3D fibril stem shape, tensile deformation, and fibril microfabrication limits, which contrast with the previous assumptions that mostly neglect the deformation of the fibril tip and stem, and focus only on the fibril tip geometry. The proposed computational fibril design could help design future optimal fibrils with less help from human intuition. Current optimal fibril adhesive designs are mainly limited to bioinspired mushroom tip designs. Here, a machine learning‐based design method that can discover new 3D fibril tip and stem shapes simultaneously with improved adhesion performance is presented. This method could be used to create optimal fibril designs automatically for adhesive applications in robotics, transfer printing, closures, consumer products, and medical devices.
Article
Full-text available
The space environment offers unique challenges for robotic grippers and controllable attachment mechanisms. Applications include satellite and orbital debris capture, perching for free-floating robots inside the International Space Station, climbing rocky surfaces in planetary exploration tasks, and grasping asteroids and other rough substrates for drilling and sample collection operations. Many traditional adhesive technologies, such as pressure-sensitive adhesives, suction, and electromagnetism, are not tenable in the space environment due to the lack of an atmosphere (e.g., pressure-sensitive adhesives outgas and suction requires a pressure differential) or suitable substrates (e.g., materials for space applications are rarely ferromagnetic). Instead, a number of other technologies have shown promise for space, including bio-inspired fibrillar dry adhesives that offer controllable adhesion on both flat and curved smooth surfaces, electrostatic adhesives that work on smooth surfaces but also rougher surfaces and fabrics, and microspines for rocky and rough substrates. Herein, a review of these technologies, focusing on their operation and providing examples of the technologies applied to space and other extraterrestrial missions is presented.
Article
The using of an autonomous wheeled mobile robot (AWMR) that perform diverse processes in a numerous number of applications without human’s interposition in an unknown environment is thriving, nowadays. An AWMR can search the environment, create an adequate map, and localizing itself into this map, by interpreting the environment, autonomously. The FastSLAM is a structure for simultaneous localization and mapping (SLAM) for an AWMR. The correctness and efficiency of the estimation of the FastSLAM often depend on the accurate a previous knowledge of the control and measurement noise covariance matrices. Also, inaccurate previous knowledge may seriously degrade their efficiency. One of the major causes of losing particle manifold is sample impoverishment in the FastSLAM. These cases of the most main problems. This paper presents a robust new method to solve these problems as called Hybrid filter SLAM. In this method, for learning the measurement and control noise co-variance matrices for increasing correctness and consistency are utilized Intuitionistic Fuzzy Logic System (IFLS). In order to optimize efficiency of sampling from Cuckoo Search (CS). The results of the simulation and experimental shown that the Hybrid filter SLAM is efficient than the FastSLAM that has less number of computations and good performance for the larger environment.
Conference Paper
Full-text available
Acquiring models of the environment belongs to the fundamental tasks of mobile robots. Approaches addressing the problem of simultaneous localization and mapping (SLAM) typically process the perceived sensor data and do not influence the motion of the mobile robot. In this paper, we present an approach to actively closing loops during exploration. It applies a Rao-Blackwellized particle filter to maintain multiple hypotheses about potential trajectories of the robot and corresponding maps. To prevent the particle filter from becoming overly confident, we present a technique to recover the particle diversity after successfully closing a loop. This way the particle depletion problem is avoided. The combination of our approach with the active loop closing strategy allows to deal with multiple nested loops. Experimental results presented in this paper illustrate the advantage of our method over pervious approaches to mapping with Rao-Blackwellized particle filters.
Conference Paper
Full-text available
Rao-Blackwellized particle filter and FastSLAM have become popular tools to solve the simultaneous localization and mapping (SLAM) problem. However, the above techniques have two important potential limitations, which are the derivation of the Jacobian matrices and the linear approximations to the nonlinear functions. Also, one of the major challenges of both Rao-Blackwellized particle filter and FastSLAM is to reduce the number of particles while maintaining the estimation accuracy. This paper proposes a new algorithm based on unscented transformation called Unscented FastSLAM (UFastSLAM) that overcomes important drawbacks of the Rao-Blackwellized particle filter frameworks by directly using nonlinear relations. Experimental results in large scale environments are presented that demonstrate the effectiveness of the UFastSLAM algorithm over the previous approaches.
Book
The purpose of this chapter is to present state estimation techniques than can “adapt” themselves to certain types of uncertainties beyond those treated in earlier chapters—adaptive estimation algorithms. One type of uncertainty to be considered is the case of unknown inputs into the system, which typifies maneuvering targets. The other type will be a combination of system parameter uncertainties with unknown inputs where the system parameters (are assumed to) take values in a discrete set. The input estimation with state estimate correction technique is presented. The technique of estimating the input and, when “statistically significant,” augmenting the state with it (which leads to variable state dimension), is detailed. These two algorithms and the noise level switching technique are later compared. The design of an IMM estimator for air traffic control (ATC) is discussed in detail. Guidelines are also developed for when an adaptive estimator is really needed, i.e., when a (single model based) Kalman filter is not adequate. The chapter concludes with a brief presentation of the use of the extended Kalman filter for state and system parameter estimation. A problem solving section appears at the end of the chapter.
Book
"Estimation with Applications to Tracking and Navigation treats the estimation of various quantities from inherently inaccurate remote observations. It explains state estimator design using a balanced combination of linear systems, probability, and statistics." "The authors provide a review of the necessary background mathematical techniques and offer an overview of the basic concepts in estimation. They then provide detailed treatments of all the major issues in estimation with a focus on applying these techniques to real systems." "Suitable for graduate engineering students and engineers working in remote sensors and tracking, Estimation with Applications to Tracking and Navigation provides expert coverage of this important area."--BOOK JACKET.
Article
This paper analyzes the consistency of the classical extended Kalman filter (EKF) solution to the simultaneous localization and map building (SLAM) problem. Our results show that in large environments the map quickly becomes inconsistent due to linearization errors. We propose a new EKF-based SLAM algorithm, robocentric mapping, that greatly reduces linearization errors, improv- ing map consistency. We also present results showing that large-scale mapping methods based on building local maps with a local uncertainty representation (Tardos et al., 2002) have better consistency than methods that work with global uncertainties.
Book
From the Publisher: "Estimation with Applications to Tracking and Navigation treats the estimation of various quantities from inherently inaccurate remote observations. It explains state estimator design using a balanced combination of linear systems, probability, and statistics." "The authors provide a review of the necessary background mathematical techniques and offer an overview of the basic concepts in estimation. They then provide detailed treatments of all the major issues in estimation with a focus on applying these techniques to real systems." "Suitable for graduate engineering students and engineers working in remote sensors and tracking, Estimation with Applications to Tracking and Navigation provides expert coverage of this important area."--BOOK JACKET.
Conference Paper
We describe two new sampling strategies for Rao-Blackwellized particle filtering SLAM. The strategies, called fixed-lag roughening and the block proposal distribution, both exploit "future" information, when it becomes available, to improve the filter's estimation for previous time steps. Fixed-lag roughening perturbs trajectory samples over a fixed lag time according to a Markov chain-Monte Carlo kernel. The block proposal distribution directly samples poses over a fixed lag from their fully joint distribution conditioned on all the available data. Our experimental results indicate that the proposed strategies, especially the block proposal, yield significant improvements in filter consistency and a reduction in particle degeneracies compared to standard sampling techniques such as the improved proposal distribution of FastSLAM 2.
Conference Paper
This paper describes a technique to estimate the 3D motion of a vehicle using odometric sensors and a stereo camera. The algorithm falls into the category of simultaneous localization and mapping as a large database of visual landmarks is created. The algorithm has been field tested online on a rover traversing loose terrain in the presence of obstacles. The resulting position estimation errors are between 0.5% and 4% of distance travelled, a significant improvement over odometry alone.
Conference Paper
The paper analyzes the properties of the full covariance simultaneous localization and map building problem (SLAM). We prove that, even for the special case of a stationary vehicle (with no process noise) which uses a range-bearing sensor and has non-zero angular uncertainty, the full covariance SLAM algorithm always yields an inconsistent map. We also show, through simulations, that these conclusions appear to extend to a moving vehicle with process noise. However, these inconsistencies only become apparent after several hundred beacon updates.