MOBILE ROBOT LOCALIZATION USING LINEAR SYSTEM
MODEL
Xu Zezhong
a,b
, Liu Jilin
a
a,Department of Information Science and Electronic Engineering,Zhejiang University, 310027, Hangzhou, China
b, Department of Computer Science and Engineering,Hangzhou Institute of Electronic Engineering, Hangzhou, China
Keywords: Mobile robot localization, Nonlinear state estimation, Extended Kalman filter, Linearization error.
Abstract: Localization is a fundamental problem for mobile robot autonomous navigation. EKF is an efficient tool for
position estimation, but it suffers from linearization errors due to linear approximation of nonlinear system
equations. In this paper we describe a position estimation method for mobile robot. Process and
measurement equations are linear by appropriately constructing the state vector and system models. The
position of mobile robot is estimated recursively based on optimal KF. It avoids linear approximation of
nonlinear system equations and is free of linearization error. All these techniques have been implemented on
our mobile robot ATRVII equipped with 2D laser rangefinder SICK.
1 INTRODUCTION
The mobile robot localization problem, especially
for pose tracking, is one fundamental problem in
mobile robotics. Dead reckoning technique is a
solution to estimate the position of mobile robot.
However, due to the uncertainties on odometer
modeling errors and occasional slipping errors, it
generally suffers from cumulative errors that
increase without bounds (Borenstein et al. 1995). In
order to overcome this disadvantage, probabilistic
localization method (Olson 2000; Thrun 2000) is
proposed by taking into account various
uncertainties on system state and sensor
measurement. In this way, the mobile robot
localization problem can be described as state
filtering or state estimation problem.
The optimal minimum mean square error
(MMSE) estimation is conditional expectation
conditioned on all prior observations. If system
equations are linear, the optimal state estimation
solution is the Kalman filter (KF) algorithm
(Kalman 1960). Unfortunately, system equations are
generally nonlinear in mobile robotics. System state
estimation requires a complete description of the
conditional probability density with unbounded
number of parameters. To estimate position of robot,
various suboptimal solutions for nonlinear
estimation problem have been proposed based on
different approximation techniques.
There are three types of approximation
techniques. The first is to approximate probability
distribution over state space. Grid-based Markov
Localization (Fox et al. 1998) is a discrete
approximation of probability density distribution
over all possible position and Monte Carlo
Localization (Fox et al. 1999) approximates the
posterior probability distribution over the state space
with a set of particles. The second is to approximate
nonlinear system equation with linear function.
Extended Kalman filter (EKF) (Kalman and Bucy
1961) and various variants apply the optimal
Kalman filter to deal with nonlinear estimation
problems by replacing nonlinear equations with
linear approximation. The third is to compute
conditional expectation with efficient numerical
approximation method. Gaussian filter (Ito et al.
2000) and Gaussian sum filter (Alspach 1972) resort
to numerical integration techniques to compute
conditional mean and conditional covariance.
The Kalman filter has brought revolutionary
improvement in stochastic estimation problem since
its introduction in 1960. The linear KF was
developed to provide optimal state estimation of
linear system with noisy measurement. The EKF
works by approximating the nonlinear process and
measurement equations about the current state
estimation. Classical EKF is a first order
approximation filter. It approximates nonlinear
function with first order Taylor series expansion
evaluated at current state estimation. However, EKF
243
Zezhong X. and Jilin L. (2004).
MOBILE ROBOT LOCALIZATION USING LINEAR SYSTEM MODEL.
In Proceedings of the First International Conference on Informatics in Control, Automation and Robotics, pages 243-248
DOI: 10.5220/0001136202430248
Copyright
c
SciTePress
has two drawbacks. One is that this approximation is
not accurate if area around current estimation is very
nonlinear. Another is Jacobian matrix computation.
Some nonlinear functions may not be differentiable,
even if it is differential, computing the derivatives
may still be hard.
In order to improve the performance of state
estimation, various modified EKF methods are
proposed. The second-order filter (Nam et al. 1999)
is known to achieve better precision than the first-
order filter at the price of complex computation.
Higher order filter (Kelly 1994) is proposed by
taking into account higher order terms in the Taylor
series expansion, but the more complex
computations are involved. Iterated EKF (Bellaire et
al. 1995) is designed to improve the precision of
approximation by approximating measurement
equation about the corrected posterior estimation
instead of the predicted prior estimation. A range-
direction-cosine EKF (Mehra 1971) transforms
nonlinear measurement equation into linear function
by choosing a suitable coordinates system. A
modified EKF (Wall et al. 1997) algorithm offers
better performance by approximating nonlinear
equations about state vector computed from
deterministic equation rather than state vector
estimated from stochastic equation. Interlaced EKF
(Glielmo et al. 1999) partitions the state vector into
several parts. Each filter works independently and
considers the other parts of state vector as known
parameters. A modified EKF (Chui et al. 1990)
improves the filtering performance by modifying the
centre of Taylor series approximation. The linear
regression Kalman filter (Lefebvre et al. 2002)
approximates the process and measurement
functions by statistical linear regression of the
function with some sampling points. A typical
LRKF is unscented Kalman filter (UKF) (Julier et al.
1995; Wan et al. 2000). It approximates nonlinear
process and measurement function with sigma
points. A NMSKF (Lefebvre et al. 2002) linearize
process and measurement equations in a higher-
dimensional state space. It is applicable to state
estimation for static system and for a limited class of
dynamic systems.
In this paper, we apply KF to estimate
recursively the position of robot. The process and
measurement equation are linear by constructing
appropriately the system state and system models.
With linear system equations, the position estimation
of robot is optimal.
An outline of this paper is as follows. In Section
2 we describe the coordinate system. In Section 3 we
describe the system state space and system models.
Process equation and measurement equation are
defined. In Section 4 an efficient filter algorithm
with linear process equation and linear measurement
equation is described. Experimental results are
presented in Section 5. Finally we conclude in
Section 6.
2 COORDINATE SYSTEM
There are four coordinate systems. The first is world
coordinate system denoted with X
W
O
W
Y
W
. The
second is odometer coordinate system denoted with
X
O
O
O
Y
O
. The third is scanner coordinate system
denoted with X
S
O
S
Y
S
. The fourth is the local
coordinate system of mobile robot, denoted with
X
L
O
L
Y
L
. Robot local coordinate system has the
same orientation and origin as the odometer
coordinate system. Scanner coordinate system has
the same orientation as robot local coordinate
system. They are shown in Fig.1.
Robot local coordinate system, sensor coordinate
system and odometer coordinate system are defined
according to DIN70000. All angle are in the range of
–180
O
…180
O
. They are shown in Fig. 2.
In this paper, only world coordinate system and
robot local coordinate system are considered. All
values represented with scanner and odometer
Figure 1: coordinate system.
Fi
g
ure 2: DIN70000
ICINCO 2004 - ROBOTICS AND AUTOMATION
244
coordinate system are transformed into robot local
coordinate system.
3 STATE SPACE AND SYSTEM
MODEL
Mobile robot moves in an indoor environment.
Robot is equipped with an odometer and a laser
rangefinder. The data from the odometer is used to
predict the position of robot. Laser rangefinder scans
environment information and extracts natural
landmark, corner, to correct the predicted position of
robot. Data from sensor odometer and rangefinder
are all with uncertainty. The Kalman filter has
proven to be a valuable tool for mobile robot
position estimation given that the initial state and
covariance are known. The position of robot, [x, y,
θ]
T
, is considered as system state. Process equation
is based on odometer model and the world
coordinate of landmark is considered as observation
information. The position of robot is recursively
estimated as it evolves through time.
3.1 State space
We assume that environment is a 2D plane and
represent this 2D plane with a world coordinates
system X
W
O
W
Y
W
. Another is robot’s local
coordinates system X
L
O
L
Y
L
. The configuration of
robot is represented with a three-dimensional state
vector [x, y, θ]
T
. (x, y) represents the position of
robot. θ represents the orientation or heading of the
robot. It is defined in the counterclockwise. The
value of θ is from –π to π. It is shown in Fig.1.
The position prediction of robot is according to
the motion model. The observation prediction is
according to the observation model and the predicted
position of robot is corrected according to updated
rules.
3.2 Motion model
It is assumed that the robot moves along a circular
arc at each step. The position transition is based on
odometer information. Date from odometer is U
k
=
(D
k
, γ
k
) at step k. D
k
is the distance traveled along
the arc and γ
k
is the change in motion direction. R
k
=
D
k
/γ
k
is the radius of arc. According to motion
model shown in Fig.3, the deterministic process
equation is:
3.3 Observation model
Laser rangefinder scans environmental information
and extracts landmark. A landmark is a typical
feature of environment. In this paper the corner is
considered as natural landmark. A range scan is
segmented and merged. The corner is extracted from
current laser scan and is represented with (x
L
, y
L
) in
local coordinates system. The corner is represented
with (x
W
, y
W
) in world coordinates system. We use
the position (x
W
, y
W
) of a corner as the observation
value. The position (x
L
, y
L
) of corner is computed in
local coordinates system. It is translated into world
coordinates system as observation prediction based
on current estimated position of robot. It is a two-
dimensional observation vector. According to
observation model shown in Fig.4, the deterministic
measurement equation is:
4 LOCALIZATION WITH KF
EKF approximates nonlinear system equations with
first-order terms of Taylor series and induces
linearization errors. If the initial position is assumed
as Gaussian distribution, the result position is not
Gaussian distribution after a nonlinear
transformation. So, it is incomplete that only
position expectation and position covariance are
recursively estimated. It is not an optimal estimation
solution for mobile robot localization with nonlinear
system equations.
()()()
()()()
kkk
kkk
k
k
kk
kkk
k
k
kk
D
yy
D
xx
γθθ
θγθ
γ
θγθ
γ
+=
++=
++=
+
+
+
1
1
1
sinsin
coscos
() ()
() ()
kLkLkW
kLkLkW
yxyy
yxxx
θθ
θθ
sincos
cossin
+=
=
Figure 3: Motion model.
MOBILE ROBOT LOCALIZATION USING LINEAR SYSTEM MODEL
245
We apply optimal linear Kalman filter to deal
with mobile robot localization by constructing
appropriately system state vector and system
models. In this way, the process equation and
measurement equation are linear. So, we estimate
the position of robot directly with a linear Kalman
filter. This estimation solution is optimal in MMSE
sense. State vector X
k
, observation vector Z
k
and
input vector U
k
are defined as:
We extend system state to four-dimensional
vector [x
k
, y
k
, s
k
, c
k
]
T
. (x
k
, y
k
) is the position of
robot. (s
k
, c
k
) is the sin and cosine of orientation θ of
robot. System state is estimated recursively
predicted and updated. The position (x
k
, y
k
, θ
k
) of
robot and its variance are computed from mean and
variance of system state [x
k
, y
k
, s
k
, c
k
]
T
. Observation
vector Z=[x
W
, y
W
]
T
is the position of corner in world
coordinates system. By replacing sin(θ
k
) and cos(θ
k
)
in system models with s
k
and c
k
, we get linear
process equation and linear measurement equation.
The process equation is:
Where ω
k
is noise vector representing
uncertainty on odometer modeling and slipping. Its
mean is zero and variance is Q.
The measurement equation is:
Where υ
k
is noise vector representing uncertainty
on sensor measurement. Its mean is zero and
variance is R.
Matrix F
k
only depends on the input vector and is
uncorrelated with current position estimation. Matrix
H
k
only depends on the position of landmark and is
uncorrelated with current position prediction. So,
process and measurement equations are linear. The
position of mobile robot is predicted and corrected
according to following update rules:
From step k to step k+1, system state mean and
covariance are predicted and corrected recursively.
The resulting position estimation of robot is an
optimal estimation. If the initial position of robot is
assumed as Gaussian distribution, the probability
distribution of position after each step is still
Gaussian. It is reasonable that only position mean
and variance are recursively computed.
5 EXPERIMENTAL RESULT
In this section, we demonstrate the experimental
result of proposed position estimation method. By
extending the state space of robot to four-
dimensional state vector, the system process and
measurement equations are linear. Localization
=
=
=
k
k
k
W
W
k
k
k
k
k
k
D
U
y
x
Z
c
s
y
x
X
γ
(
)
() ()
()
() ()
()
() ()
() ()
() ()()
()() ()
() ()
() ()
k
kk
k
k
kk
kk
kk
kk
kk
kk
k
k
k
k
k
k
k
k
k
k
k
k
k
k
kk
k
kk
k
kk
k
kk
kk
k
kk
k
kk
k
k
kk
kk
k
kk
k
kk
k
k
kk
kk
kkkk
XF
c
s
y
x
DD
DD
sc
cs
scs
D
y
csc
D
x
UXfX
ω
ω
γγ
γγ
γ
γ
γ
γ
γ
γ
γ
γ
ω
γγ
γγ
γγ
γ
γγ
γ
ω
+=
+
=
+
+
++
+
=
+=
+
cossin
sincos
sincos
cossin
sincos
sincos
sincos
sincos
,
00
00
110
101
1
(
)
k
kk
k
k
kk
kk
kk
kk
LL
LL
k
kk
L
kk
L
kk
kk
L
kk
L
kk
k
kk
k
XH
c
s
y
x
xy
yx
sycxy
cysxx
XhZ
υ
υ
υ
υ
+=
+
=
+
+
=
+=
+
+
+
+
+
+++
+++
+
1
1
1
1
1
111
111
1
10
01
1
1
1
111
1111
1
1
+
+
+++
+
+
++
+
+
=
+=
=
+=
+=
=
k
T
k
kk
k
T
k
kk
kk
T
kkk
kkkk
kk
kkk
kkkk
T
k
kk
k
kk
kk
k
kk
SHPK
RHPHS
KSKPP
XHZKXX
QFPFP
XFX
Figure 4: Observation model.
ICINCO 2004 - ROBOTICS AND AUTOMATION
246
based on linear system model avoids linearization
error due to linear approximation of nonlinear
system equations.
In this section, we demonstrate the experimental
results of proposed position estimation method with
our mobile robot ATRVII. Fig. 5 is robot equipped
with laser range rangefinder. Robot moves around
our lab, scans landmark features and estimate its
position and heading.
Fig. 6 shows the estimated position errors with
one sigma confidence limit. Fig. 7 shows innovation
sequence and variance. Experimental results show
that the filter proposed here is consistent and
convergent.
With the same parameter, the position of robot is
estimated with classical EKF. Fig. 8 shows
difference of estimation results between KF and
EKF. Experimental results show that the Linear
filter gives more conservative estimation result than
nonlinear filter.
KF is an optimal state estimation for linear
system. Position estimation based on EKF induces
linearization error and brings additional uncertainty.
linearization error due to linear approximation of
nonlinear process equation affects mainly the long-
term position estimation. linearization error due to
linear approximation of nonlinear measurement
equation affects mainly the short-term position
estimation. By constructing appropriately state
vector and system models, we use optimal Kalman
filter for mobile robot position estimation. It is free
of linearization errors.
6 CONCLUSION
The extended Kalman filter has been widely used as
a position estimation method for mobile robot
localization and simultaneous localization and map
building (SLAM) problem. However, The classical
extended Kalman filter for this application suffers
from a fundamental flaw. Linear approximation of
nonlinear system equations with first-order Taylor
Figure 6: Estimated position error with KF.
Figure 7: Innovation and innovation variance
with KF.
Figure 5: Mobile robot.
Figure 8: Position estimation with KF and EKF.
solid line for KF and dashed line for EKF.
MOBILE ROBOT LOCALIZATION USING LINEAR SYSTEM MODEL
247
series induces linearization error. In this paper, a
position estimation method with linear process and
measurement equations is developed. Process and
measurement equations are linear by appropriately
constructing state vector and system models. With
linear process and measurement function, we apply
linear Kalman filter to estimate optimally the
position of robot. It avoids linear approximation of
nonlinear system equations and is free of
linearization error. The filter is consistent and
convergent. Comparing with EKF, it gives more
conservative estimation result.
REFERENCES
Alspach, D. and Sorenson, H. 1972. Nonlinear Bayesian
estimation using Gaussian sum approximations. IEEE
Transactions on Automatic Control, vol. AC-17, pp.
439-448.
Bellaire, R.L., Kamen E.W. and Zabin, S.M. 1995. A new
nonlinear iterated filter with applications to target
tracking. In SPIE Proceedings on Signal and Data
Processing of Small Targets, vol. 2561, pp. 240-251.
Borenstein, J. and Feng, L. 1995. Correction of Systematic
Odometer Errors in Mobile Robots. In Proceedings of
the 1995 International Conference on Intelligent
Robots and Systems (lROS'95), Pittsburgh,
Pennsylvania, pp. 569-574.
Chui, C.K., Chen, G. and Chui, H. 1990. Modified
extended Kalman filtering and parallel system
parameter identification, IEEE Transactions on
Automatic Control, 35 (1): 100-104.
Fox, D., Burgard, W., Thrun, S. and Cremers, A.B. 1998.
Position Estimation for Mobile Robots in Dynamic
Environments. AAAI/IAAI 1998: 983-988.
Fox, D., Burgard, W., Dellaert F. and Thrun, S. 1999.
Monte Carlo Localization: Efficient Position
Estimation for Mobile Robots. AAAI/IAAI 1999: 343-
349.
Glielmo, L., Setola R. and Vasca, F. 1999. An Interlaced
Extended Kalman Filter, IEEE Transactions on
Automatic Control, vol. 44, no. 8, pp. 1546-1549.
Ito, K. and Xiong, K. 2000. Gaussian Filters for Nonlinear
Filtering Problems. IEEE Transactions on Automatic
Control, 45(5): 910-927.
Julier, S., Uhlmann J. and Durrant-Whyte, H. 1995. A new
approach for filtering nonlinear systems. In
Proceedings of the American Control Conference,
pp.1628-1632.
Kalman, R.E. 1960. A New Approach to Linear Filtering
and Prediction Problems. Journal of Basic Engineering
Transactions of the ASME, pp.35-45.
Kalman, R.E. and Bucy, R. 1961. New results in linear
filtering and prediction theory. Journal of Basic
Engineering Transactions of the ASME, vol. 83, pp.
95-108.
Kelly, A.J. 1994. A 3D State Space Formulation of a
Navigation Kalman Filter for Autonomous Vehicles,
CMU Robotics Institute Technical Report CMU-RI-
TR-94-19.
Lefebvre, T., Bruyninckx, H. and De Schutter, J. 2002.
Comment on “A New Method for the Nonlinear
Transformation of Means and Covariances in Filters
and Estimators”. IEEE Transactions On Automatic
Control, Vol. 47, no. 8, pp.1406-1408.
Lefebvre, T., Bruyninckx, H. and De Schutter, J. 2003.
Non-linear Autonomous Compliant Motion with a
Non-minimal State Kalman Filter. In International
Conference on advanced Robotics, pp.136-141.
Mehra, R.K. 1971. A Comparison of Several Nonlinear
Filters for Reentry Vehicle Tracking. IEEE
Transactions on Automatic Control, AC-16(4): 307-
319.
Nam, K.H. and Tahk, M.J. 1999. A second-order
stochastic filter involving coordinate transformation.
IEEE Transactions on Automatic Control
, Vol. 44,
No.3, pp. 603-608.
Olson, C.F. 2000. Probabilistic self-localization for mobile
robots. IEEE Transactions on Robotics and
Automation, vol. 16(1), pp. 55-66.
Thrun, S. 2000. Probabilistic algorithms in robotics, AI
Magazine, 21(4): 93-109.
Wall, D.S. and Gaston, F.M.F. 1997. Modified extended
kalman filtering. In Proceedings of International
Conference on Digital Signal Processing, pp.703-706.
Wan, E.A. and van der Merwe, R. 2000. The unscented
Kalman filter for nonlinear estimation. In Proceedings
of Conf. Adaptive Systems for Signal Processing,
Communication and Control, Canada.
ICINCO 2004 - ROBOTICS AND AUTOMATION
248