Position Estimation in Mobile Robot Navigation Using [629738]

Position Estimation in Mobile Robot Navigation Using
the Unscented Kalman Filter
Caius Suliman1, Florin Moldoveanu1,

1 Transilvania University of Brasov, De partment of Automaton, Eroilor. 29,
69121 Brasov, Romania
{caius.suliman, moldof}@unitbv.ro
Abstract. The Kalman filters have been widely used for mobile robot
navigation and system inte gration. So that it may operate autonomously, a
mobile robot must know where it is. Accu rate localization is a key prerequisite
for successful navigation in large-scale environments, particularly when global
models are used, such as maps, drawings, topological descriptions, and CAD
models. This paper presents the localization of a mob ile robot using two of the
variations of the Kalman filter: the extended Kalman filter (EKF) and the unscented Kalman filter (UKF). For this purpose both filters were implemented
for a known kinematic model of the robot.
Keywords: autonomous mobile r obot, extended Kalman filter, unscented
Kalman filter, position estimation
1 Introduction
The extended Kalman filter (EKF) has been used in mobile robot navigation for
decades. The EKF is a variati on of the standard Kalman filter to nonlinear systems. It
works by linearizing the nonlinear state dynamics and measurement models. This
assumption works fine were the nonlinear dynamics and measurements are adequately
approximated by a first-order Taylor series. The EKF has many limitations, including filter divergence in the presence of hi gher-order nonlinearities. Many authors
addressed the problems of the EKF and they proposed various fixes or modifications to better estimate the mean and covarian ce of the state estimates for nonlinear
systems. The EKF has been successfully a pplied to localization systems for mobile
robots [6], [2], [8]. Some authors have ev en combined the EKF w ith fuzzy logic for a
better estimation of the robot’s position [1].
Another variation of the Kalman filter is the unscented Kalman filter (UKF). This
filter is based on the unscented transform (UT) [4]. The UKF works by approximating
a Gaussian distribution, which is much easier than approximating an arbitrary
nonlinear function. The UKF is computationaly more costly than the EKF, but it
reduces estimation errors. One of the advant ages of the UKF over the EKF is that it
doesn’t need to derive the Jacobian matrices. The most important application of the UKF is in simultaneous localization and map building (SLAM). It has been applied to

ground mobile robots [3], [5], but has been successfully applied even to unmaned
aerial vehicles (UAV) [7].
2 Contributions to technological innovation
This paper presents the implementation for two of the variations of the Kalman filter
for an autonomous mobile robot based on Ackerman steering. Here we provide a
comparison of the localization accuracy be tween the EKF based implementation and
the UKF based implementation. The simulations in this paper will show the superiority of the UKF over the EKF.
3 The EKF Implementation
For the EKF implementation we have used the kinematic model of an autonomous
mobile robot based on Ackermann steering (see fig.1). The mobile robot is supposed
to move in a 2D coordinate system. The position of the robot ( x and ) and the
angl
e of orientation of the robot are assumed to be given by an overhead camera. y

The
kinematic equations for the
mobile robot:
θcosV x , (1)
θsinV y , (2)
LV tanθ . ( 3)
Fig.1 Robot representation.

Whe
re V is the velocity of the robot, represents the distance between the rear
axel
and front one, and  is the steering angle. Because of the nonlinear case that is
encountered here we have to use an EK F that linearizes the current mean and
covariance, such as: L








         


LV tV t yV t x
yx
kk
kkk
tanθsinθcos
θ111
, (4)

Measurements are taken from an overhead camera, and thus x, and can be
measured di
rectly: yθ
.
θ) , (
θ




  
vv yv x
z v x h z
ky kx k
k k k k (5)
where is a vector containing the state variables, is a vector containing the
measurem
ent variables and is the observation noise. kxkz
kv
The E
KF calculation steps are described below:
I. The prediction step:
)
00 , , ˆ( ˆ1 1


kk k k
wu x f x , (6)
T
k k kT
k k k k W Q W F P F P1 1    , (7)
where is the Jacobian matrix of partial derivates of with respect to and
is th
e Jacobian matrix of partial derivates of with respect to . kW fkwkF
fkx
Now
we need to calculate the Jacobians and : kFkW




 

1 0 0θcos 1 0θsin 0 1
vv
Fk, .





0 0 00 0 00 0 0
kW (8)
II. Observation and update:
In this step we need to compute the Kalman gain : kK
1) (   T
k k kT
k k kT
k k k V R V J P J J P K . (9)
Now, we need to compute the Jacobians and : kJkV





1 0 00 1 00 0 1 kJ , ,





1 0 00 1 00 0 1 kV (10)
where is the Jacobian matrix of partial derivates of with respect to and
is is th
e Jacobian matrix of partial derivates of h with respect to . kJ hkxkV
kv
The m
easurement update equations are:
) ( ˆ ˆ H z K x xk k k k  , (11)

 k k k k P J K I P) (. (12)
4 The UKF Implementation
The UKF has been applied to the same robo t model as the one used for the EKF case.
The UKF is an straightforward extention of the unscented transform (UT) to recursive
estimation. The UT is a method for computing the statistics of a random variable
which goes through a nonlinear transformation. The dimensional random variable L
x with mean x and covariance , is approximated by xP 1 2L weighted points:
x X0 , 0i , (13)
i x i P L x X) )λ ( (   , L i…, , 1 , ( 14)
L i x i P L x X   ) )λ ( ( , L L i2 …, , 1 . ( 15)
Then the sigma points are propagate d through the nonlinear function:
) (i iX f y , L i2 …, , 0 . (16)
from which the mean and covariance can be approximated:

L
iim
iy W y2
0, (17)
   
L
iT
i ic
i y y y y y W P2
0) )( (, (18)
with weights : iW
λλ
0LWm, (19)
where  is a scaling parameter.
)β α 1 (λλ 2
0   LWc, (20)
)λ ( 5 , 0  L W Wc
im
i , L i2 …, , 1 . (21)

In the first step of UKF algorithm we ne ed to initialize the augmented states and
covariance:






00ˆ
ˆ0
0T
ax
x and . T
a
RQP
P





0 00 00 00
0 (22)
The steps of UKF are:
I. The computing of the sigma points:

       a
ka
ka
ka
ka
ka
k P x P x x X1 1 1 1 1 1ˆ ˆ ˆ   , (23)
II. Time update:
) , , (1 1 1 1    kw
kx
kx
k ku X X f X , (24)

L
ix
k k ic
i kX W x2
01 ,ˆ , (25)
    


L
iT
kx
k k i kx
k k ic
i k x X x X W P2
01 , 1 ,ˆ ˆ , (26)
v
kx
k k k kX X h z1 1 1,  , (27)

L
ik k im
i kz W z2
01 ,ˆ . (28)
III. Measurement update:
    


L
iT
k k k i k k k ic
i zz z z z z W P2
01 , 1 ,ˆ ˆ , (29)
    


L
iT
k k k i k k k ic
i xz z z x X W P2
01 , 1 ,ˆ ˆ , (30)
1zz xz kP P K, (31)
)ˆ ( ˆ ˆ    k k k k kz z K x x, (32)
T
k zz k k kK P K P P  . (33)

5 Simulation results
In this section of the paper we compare the results obtained by simulating the EKF
case, with the ones obtained from the UKF case. For this purpose both filters were
implemented in Matlab. We have considered a very simple case: a robot that follows a
predefined path. In the figure s it is presented the estimated path of the robot compared
to the real path. In fig. 2 it is shown the pa th estimated with the help of the EKF, and
in fig. 3 is presented the path es timated with the help of the UKF.

Fig. 2 . Trajectory estimation with the EKF.

Fig. 3 . Trajectory estimation with the UKF.

Both filters performed very well, but it can be seen that the UKF has performed
better and as a result the estimated path with the help of this filter is closer to the real
path. To better evaluate the performance of the two filters, we ploted the actual states
and the estimated ones for a small part of the simulation. For this purpose we have
ploted the estimated data for the X axis and copared it to the real data (see fig. 4).

Fig. 4 . Illustration of using the EKF and UKF to estimate the position on the X axis.

Fig. 5 . Illustration of using the EKF and UKF to estimate the position on the Y axis.

We also ploted the estimated data for the Y axis (see fig. 5). In both plots it can be
seen clearly that the UKF (blu e line) predicted path is almost on top of the real path
(green line). At first the EKF (red line) perf ormed almost as well as the UKF, but as
the simulation continued the er rors of the EKF were bigger than the ones of the UKF.
The difference between the pe rformance of the two filters can be seen above.
6 Conclusions
In this paper two of the main variations of the Kalman filter used for the position
estimation of an autonomous mobile robot based on Ackermann steering. Comparing
the simulation results show that the performance of the UKF is consistently better
than the EKF. In the future we will tr y to combine the fuzzy logic with one of the
filters presented in this paper for a better position estimation. Furthermore, we will

use the UKF in a visual based SLAM syst em, where camera calibration and corect
feature detection play a vital role in solving the data association problem.
Acknowledgement. This paper is supported by the Sectoral Operational Programme
Human Resources Development (SOP HRD), financed from the European Social
Fund and by the Romanian Government under the contract number
POSDRU/6/1.5/S/6.
References
1. Aguirre, E., González, A., Muñoz, R.: Mobile Robot Ma p-Based Localization using
approximate locations and the Extended Kalman Filter. In: 10th International Conference on
Information Processing and Ma nagement of Uncertainty in Knowledge-Based Systems –
IPMU, pp. 191–198, Perugia (2004).
2. Andrade-Cetto, J., Sanfeliu, A.: Topologica l map learning for a mobile robot in indoor
environments. In: Proceedings of 9th Spanish Symposium on Pattern Recognition and Image
Analysis, pp. 22–226, Benicasim (2001).
3. Holmes, S., Klein, G., Murray, D. W.: A S quare Root Unscented Kalman Filter for Visual
MonoSLAM. In: IEEE International Conferen ce on Robotics and Automation – ICRA, pp.
3710–3716, Pasadena (2008).
4. Julier, S., Ulhmann, J. K.: A New Extension of the Kalman Filter to Nonlinear Systems. In:
Proceedings of Areosense: 11th International Symposium Aerospace/Defense Sensing, Simulation, and Controls, vol. 3068, pp. 182–193, Orlando (1997).
5. Ko, S., Choi, J., Kim, B. : Indoor Mobile Localization System and Stabilization of
Localization Performance using Pre-filtering. In: International Journal of Control, Automation, and Systems, vol. 6, pp. 204–213, ICROS & KIEE (2008).
6. Santana, A. M., Sousa, A. S. S., Britto, R. S., Alsina, P. J., Medeiros, A. A. D.: Localization
Of A Mobile Robot Based In Odometry A nd Natural Landmarks Using Extended Kalman
Filter. In: Proceedings of 5th International Conference on Informatics in Control, Automation & Robotics – ICINCO, Funchal-Madeira-Portugal (2008).
7. Sünderhauf, N., Lange, S., Protzel, P.: Us ing the Unscented Kalman Filter in Mono-SLAM
with Inverse Depth Parametrization for Autonomous Airship Control. In: IEEE International
Workshop on Safety, Security and Resc ue Robotics, pp. 1–6, Rome (2007).
8. Xu, H., Wang, C.: Position Estimation for Intelligent vehicles Using an Unscented Kalman
Filter. In: International Journal of Vehicle Autonomous Sy
stems , vol. 6, pp. 186–194,
Inders
cience (2008).