Proceeding o/the 6th International Symposium on Mechatronics and its Applications (ISMA09), Sharjah, UAE, March 24-26,2009
DESIGN OF AUGMENTED EXTENDED KALMAN FILTER FOR REAL
TIME SIMULATION OF MOBILE ROBOTS USING SIMULINK@
Iraj Hassanzadeh
Research Laboratory of Robotics,
Control Engineering Department, Faculty of
Electrical & Computer Engineering, University of
Tabriz, Tabriz, IRAN
izadeh@tabrizu.ac.ir
ABSTRACT
This paper presents a new design of augmented extended Kalman
filter (AEKF) for real-time simulation of mobile robots. A
Simulink® model is developed for simultaneous localization and
odometry calibration of mobile robots in real time manner.
Starting from the encoders readings, and assuming an absolute
measurement available, the AEKF provides the local
reconstruction of mobile robots position and orientation with an
on-line odometry calibration. The simulation results verify the
effectiveness of the proposed method and suggest it as a
promising way for real time implementations of augmented
kalman filters.
Index Terms --- Localization, Odometry calibration, Encoder,
augmented, extended Kalman filter, real-time
1. INTRODUCTION
The problem of finding and tracking the mobile robot's pose is
called localization, and can be global or local. During the past
few years many suggestions have been made to address this
problem. One of the first methods introduced, and still used in
many projects, is odometry. It is the reconstruction of the mobile
robot configuration, Le., position and orientation, resorting to the
encoders' measurements at the wheels. Starting from a known
configuration, the current position and orientation of the robot is
obtained by time integration of the vehicle's velocity
corresponding to the commanded wheels' velocity [2]. Its
advantages are the low cost of the sensors needed, its good
accuracy for short distances, and its compatibility with other
positioning methods. The estimation of the odometric parameters
is known in the literature as odometry calibration.
The mobile robots' pose estimation is affected mainly by
odometry errors which can be separated into systematic and
nonsystematic. Regarding the systematic errors, there are two
dominant error sources, unequal wheel diameters and uncertainty
about the effective wheel base. In the work by Borenstein and
Feng [3], a calibration technique called, "UMBmark" has been
developed to calibrate for systematic errors of a differential drive
mobile robot. Martinelli [8] and Larsen [6] introduced an
augmented Kalman filter (AKF) which simultaneously estimates
ISMA09-1
978-1-4244-3481-7/09/$25.00 ©2009 IEEE
Mehdi Abedinpour Fallah
Research Laboratory of Robotics,
Control Engineering Department, Faculty of
Electrical & Computer Engineering, University of
Tabriz, Tabriz, IRAN
mabedinpour@tabrizu.ac.ir
the robot configuration and the parameters characterizing the
systematic odometry errors.
Within the significant toolbox of mathematical tools that can
be used for stochastic estimation from noisy sensor measurements,
one of the most well known and often used tools is the Kalman
filter. As an extension to the same idea, the extended Kalman
filter (EKF) is used if the dynamic of the system and/or the output
dynamic is nonlinear. It is based on linearization about the current
estimation error mean and covariance [11].
This paper presents a new design of augmented extended
Kalman filter for real-time simulation of mobile robots using
Simulink®. We design the augmented extended Kalman Filter to
fuse the absolute measurement's data and the odometry from the
wheels' encoders for real-time reconstruction of mobile robots
position and orientation with an on-line odometry calibration.
The remainder of this paper is organized as follows. First, we
present the robot model for a two-wheeled differential drive
mobile robot. Then, the AKF is introduced. In the next section,
we describe the algorithmic details of the EKF formulations and
implement the augmented version of this filter for the differential
drive mobile robots. Next, the Simulink implementation of the
system is presented. The following section shows the simulation
results and discusses their significance. And a final section
concludes the paper including some hints for future study.
2. ROBOT MODEL
A two-wheeled differential drive mobile robot operating on a
planar surface is sketched in Figure I,Where x and y denote the
position of the center of the axle of the robot with respect to the
global coordinate frame, e denotes the heading of the vehicle with
respect to the x axis in the global coordinate frame. There are two
main wheels, each of which is attached to its own motor. A third
wheel- a caster, is placed in the front to passively roll along while
preventing the robot from falling over.
Deriving a model for the whole robot's motion is a bottom-up
process. Each individual wheel contributes to the robot's motion
and, at the same time, imposes constraints on robot motion.
Kinematics refers to the evolution of the position and velocity of a
mechanical system, without reference to its mass and inertia. The
differential equations that describe the kinematics of the robot are
written as
X = v cos9
y=vsin9 (1)
e=w
 Proceeding ofthe 6th International Symposium on Mechatronics and its Applications (ISMA09), Sharjah, UAE, March 24-26,2009

y 4. EXTENDED KALMAN FILTER

The Kalman filter addresses the general problem of trying to

castor estimate the state x € Rn of a discrete-time controlled process that

-
is governed by a linear stochastic difference equation. As an

/1-~/
extension to the same idea, the extended Kalman filter (EKF) is
used if the dynamic of the system and/or the output dynamic is
nonlinear. EKF is based on linearization about the current
----------------------
y ~-----------
estimation error mean and covariance [11].

4.1. Definitions

Let us assume that the process has a state vector x € Rn and a

'-----------'----------~x
control vector U and is governed by the non-linear stochastic
x difference equation
Figure 1. Schematic model ofa differential-drive mobile robot Xk = !(Xk-l, Uk' Wk-l) (5)
with a measurement z € Rm that is
zk = h(Xk' Vk) (6)
where the absolute velocity of the robot body v and the angular the random variables Wk and Vk represent the process and
speed ro are assumed to be inputs. measurement noise, respectively. They are assumed to be
The robot has independent velocity control of each wheel. The independent of each other, white, and with normal probability
motion of the left and right wheels are characterized by angular distributions with covariance matrices Q and R.
velocities WL and WR, respectively. The body-fixed components v Defining Xk as the a posteriori estimate of the state (from
and ro of the robot velocity are related to these angular velocities previous time step k) one can approximate the state and
by measurement vector without the noise effects as
rR rL
V = -ZWR + -ZWL Xk =!(Xk-l,Uk'O) (7)
rR rL (2) and
W = d WR - d'WL =
Zk h(Xk' 0) (8)
in which d denotes the distance between the two wheels, while To estimate the non-linear process a first order Taylor expansion
rRand rL are the radii of the right and left wheels, respectively [1]. is performed
The main feature of this model for wheeled mobile robots is the Xk ~ Xk + A(Xk-l - Xk-l) + WWk-l
(9)
presence of non-holonomic constraints, due to the rolling without zk ~ Zk + H(Xk - Xk) + VVk
slipping condition between the wheels and the ground. The non- where Xk and Zk are the actual state and measurement vectors,
holonomic constraints impose that the generalized system xkis an estimate of the state at step k, variables Wk and Vk are
velocities, Le., X, y and 8, cannot assume independent values. It representing the process and measurement noise and xkand Zk are
can be observed that the kinematic model in equation 1 does not the approximate state and measurement vectors. It can be shown
include the dynamic effects of the robot body and actuators. that the time update equations of EKF is
xl; = [(xl;-1' Uk' 0)
(10)
3. AUGMENTED KALMAN FILTER Pk = A k Pk- 1A'k + Wk Qk-1 W[
where xl; is the a priori state estimate [11]. These time update
x
Let us define as the robot configuration equations project the state and covariance estimate (Pk ) from the
X = [x, y, 9]T (3) previous time step k - 1 to the current time step k. And the
The distance between the wheels or the wheel radiuses may not be measurement update equations of the EKF are
precisely known, which lead to systematic errors and worsen the Kk = PkH[ (HkPkH[ + Vk RkV[)-1
performance of the Kalman filter. Making a complex model of the Xk = xl; + Kk (Zk - h(XI;,O)) (11)
two-wheeled differential drive mobile robots will not reduce this Pk = (I - KkH k ) Pk
problem but in fact make it worse, as this model not only will where A, W, H and V are Jacobian matrices and K is the correction
contain the same errors, but most likely also introduce further gain vector. These measurement update equations correct the state
inaccuracies [6]. and covariance estimate with the measurement Zk. The design
To remedy this problem, we adopt the same augmented kalman process of this filter is explained next.
filter (AKF) introduced by Martinelli [8] and Larsen [6]. This
filter estimates the augmented state containing the robot
configuration and the systematic parameters as
~ = [x, y, 9, rL, rR, d]T (4) 4.2. Implementation
In the following chapter, we implement the so-called augmented The discrete model of the system can be derived by converting
extended kalman filter to simultaneously localize the vehicle and the differential equation 1 to their corresponding backward
estimate the vehicle odometry. difference equations. So, the dynamical equation for the
augmented state ~ is given by:

ISMA09-2
 Proceeding ofthe 6th International Symposium on Mechatronics and its Applications (ISMA09), Sharjah, UAE, March 24-26,2009

x x T Vk cos(8 k + T Wk/ 2) 5. SIMULINK IMPLEMENTATION

Y Y T Vk sin(8k + T Wk/ 2 )
8 8 + T Wk (12) The algorithm has been implemented and simulated in
rL rL 0 Simulink®. The block diagram used in simulation of the AEKF for
~ ~ 0 the differential drive mobile robot is depicted in Figure 2. Figures
d k+l d k 0 3-5 show the subsystems illustrated in Figure 2. The kinematics
where the subscript k denotes the k-th time sample and T is the equations of the robot are inserted into the Simulink using an S-
sampling period. Function block, Le. "robot". Also, a MATLAB Fcn block is added
The Jacobian matrix at each iteration can be derived as to perform the AEKF algorithm; and a zero-order hold (ZOH) is
inserted into the filter algorithm to model the digital to analog
1 0 A 13 A 14 A 15 0 conversion.
0 1 A 23 A 24 A 25 0
0 0 1 A 34 A 35 A 36
A= (13)
0 0 0 1 0 0
0 0 0 0 1 0
0 0 0 0 0 1
With

A 13 =
aA = -21
ae T(rLWL + rRWR)
.
Sine Mobile
robot
afl 1 omega _c ,....--_-----,
A 14 = arL = 2T WL cos 8
xhat
afl 1
A 15 = arR = 2T WR cos 8

A 23 = ae = 21
af2
T(rLWL + rRWR) cos 8
af2 1 .
A 24 = arL = 2T WL sin 8 Plot

af2 1 . Figure 2. Block diagram ofsimulation model

A 25 = arR = 2T WR sin 8
af3 WL
A 34 =-= - T -
arL d
af3 WR
A 35 = - = T -
arR d
a f3 rLwL - rRwR
A 36 = 7id = T d2
Suppose that the position and orientation of the robot are
measured directly. Hence Random
Zk = HXk + vk (14) Number

Where Vk represents the measurement noise and the output matrix

H is constant and defined as
H = [/3 03] (15)
Random
where /3is the 3 x 3 Identity matrix and 03is a null 3 x 3 matrix. Number 1
Due to the recursive nature of the EKF algorithm, the state vector
needs to be initialized in startup. The initial robot configuration is Figure 3. Block diagram of "Mobile robot" as a subsystem
taken as the first measured value. Also, the nominal values
of d, rRand rL are chosen as their initial estimations. Here, the
following initial conditions are selected randomly for the state
vector:
y
Xinitial = [1.5 1.25 0 0.8 0.8 5.333]T (16)

We add uncertainty to the initial condition by selecting

Po = /6 (17)
Zero -Order
and we choose the process noise and measurement noise as
Hold

Q = diag [0.01, 0.02] Figure 4. Block diagram of "AEKF" as a subsystem

(18)
R = diag[0.05 0.05 5 * 1:0]
Thus we developed all the necessary elements of the augmented
EKF for the two-wheeled differential drive mobile robot.

ISMA09-3
 Proceeding ofthe 6th International Symposium on Mechatronics and its Applications (ISMA09), Sharjah, UAE, March 24-26,2009

2.1

1.9

1.8
E
y ;; 1.7
c
1 0
:;: 1.6
.~

a. 1.5

x 1.4

1.3
1.2 ",,",--~~""""""'~~---.I'----~""""""'~~--...-I'---'~~-"""-"_--
o 50 100 150 200 250 300
time(s)
Figure 7. Illustration ofusing AEKF to estimate the position x
Scope D

Scope 1 D
2.6
1 --Posy
1 1 1
Scope 2 -L-- AEKF estimate
2.4 - - - - -t - - - - -1- - - - - t- - - ~

1 1 1 1
1 1 1 1
____ ..1 1 .L _ _____ L_
Figure 5. Block diagram of "Camera" as a subsystem 2.2 1 1 1

g
1 1 1 1
1 1 1 1
2 - - - - "1 - - - - -1- - - - - T - - - - ----1-----
>- 1 1 1
c 1

6. SIMULATION RESULTS
0
:;:
·00 1.8
0
a.
---- -----:- ----J rf'---
~
1 1 1
-----1-----
1

This section presents simulation results by Simulink®. Figure 6 1.6 - - - - ~ - - - -

1

1
-:-
1

~
1

~ - - - -
1
-----'-----
1
1

1
shows the trajectory estimation of AEKF for the mobile robot. To 1 1 1 1
1.4 - -1- - - - - r - - - - -----r----
evaluate the performance of AEKF, we plotted the estimated and 1 1 1

actual states for the available measurements in Figures 7-9. Also, 1 1 1

Figure 10 shows the estimation error of AEKF. These figures o 50 100 150 200 250 300
verify the performance of the designed AEKF using Simulink. time(s)
In addition, Figures 11-13 illustrate the values of estimated Figure 8. Illustration ofusing AEKF to estimate the position y
parameters via the augmented filter.

2.6 3.5
1 - true Trajectory 1 --theta
1
____ .L ~ _ AEKF estimate 1 1 AEKF estimate
2.4 3 ---------------I---L--.-_ _ ~-~~
1
1
1
1 1 1 1 1
1 1 1 1
2.2 - -1- - - - ..., - - - - r - - - -1- - - - 2.5 - - - - -+ - - - - -1- - - - - +- - - - - -l - - - - -
1 1 1 1 1
1 1 1 1

g
1 1 1 1 1
1 1 1 1
____ .L ...J _ L S - - - - ...! - - - - _1- .!... _ _ _ _ _ 1 _
2
1 ~ 1 _
2
>- Jg, 1 1 1 1
c : : : ~ : <tS
1 1 1 1

---- ~ ----:- --- ~ ---- \1- --:- ---

0 1 1 1 1 1
:;: Q5
1.8 1.5 - - - - "1 - - - - -1- - - - - - - - - "1 - - - - - 1- - - - -
.~ £
a. I 1 1 1 1

-:- -:-
1 1 1 1

~- -)(- -
1 1 1 1 1

1.6 - - - - ~- - - - - - - - --
____ ..1 _ _ _ _
1 1
_ L
1
...J
1
1

1
_

1 1 1 1 1
~: 1 : :
0.5
1 1 1 1
r - - - - ..., - - - - -
1
1.4 ~ T - - - -1- - -, - - - - r - - - -1- - - -
- - - - -1- - - - - 1- - - - -
1 1 1 1
rV
~ I I I
1 1 1 1
1 1 1
O'-"-------'-----L-------'-----'--------'--------'
1.5 1.6 1.7 1.8 1.9 2 2.1 2.2 o 50 100 150 200 250 300
position x (m) time(s)

Figure 6. Trajectory estimation ofAEKF Figure 9. Illustration ofusing AEKF to estimate the orientation

ISMA09-4
Proceeding ofthe 6th International Symposium on Mechatronics and its Applications (ISMA09), Sharjah, UAE, March 24-26,2009

6
0.5 ,.....----~--___.___--~-___r_------_____, 1 - - estimated d
- - Estimation Error 1
1
x ____ l l J L _
5.8
& 1
1
1
1 1 1 1 1
150 200 250 300 5.6 - - - - -+ - - - - -1- - - - - +- - - - - -l - - - - - I- - - - -
1 1 1 1 1
E 1 1
.8- 1
>- 0.5 : : : :
&
:1: ] "C 1 1 1 1 1
0 - 1 -I - 1 1 5.4 - "T - - - - -1- - - - - T - - - - --, - - - - - r - - - -
1 1 1 1 1
1 1 1 1
-0.5 . l . . . - -_ _ ....1....-_ _ ----L..-_ _- - - - ' - ' - - -_ _
1 1 1 1 1

o
1 1 1 1 1
~ 100 1~ ~ 250 300 1
- - - - I" - - - -
1 1
T- - - -
1 1
5.2 -1- - - - - -I - - - - - 1- - - - -
0.5 1 1 1 1 1
1 1 1 1 1
1 1 1 1 1
~ 0 - 1 _ 1 1 1 __ 1 __ 1 1 1 1 1

£ 1 1 1 1 1
5'-----....1......------1...-------1...-------1---...1.....-----1
1 1 1 1 1 o 50 100 150 200 250 300
-0.5 time(s)
o 50 100 150 200 250 300
time(s) Figure 13. The estimated d by AEKF
Figure 10. Estimation error ofAEKF

7. CONCLUSIONS
2
1 - - estimated rL In this research, a new design of augmented extended kalman
1 1 1
1.8 - - - - -t - - - - - 1- - - - - + - - - - -I - - - - - I- - - - - filter for real-time simulation of mobile robots is presented. A
1
1
1
1
1
1
1
1
Simulink® model is developed to localize mobile robots while
____ ..1 L _
1.6 1
1
1_ _ _ _ _ _
1
1
1
estimating a proper set of odometric parameters. The simulation
1
1 1 1 results verify the effectiveness of the new design. In the future we
1
1 1 1
1.4 - - - - "1 - - - - -1- - - - - - - - - -1- - - - - I - - - - will conduct some experiments using the Khepera II mobile robot,
E 1 1 1 1
.8- 1 1 1 1 manufactured by K-Team, to verify the simulations of this
,-....J
1.2 - - -I - - - -1- - --
research. Furthermore, utilizing the AEKF in real time manner for
1 1

- - - - _I
1
~ _
the simultaneous localization and mapping (SLAM) problem
1 would be our future challenging task, where on-line odometry
1
1 1 1 1 calibration could playa very important role especially in solving
0.8 - - - , - - - - - 1- - - - - T - - - - -I - - - - - r - - - -
1 1 1 1 1 the data association problem.
1 1 1 1 1
0.6'-----....1....-------L.-------'----'-----....L....------'
o 50 100 150 200 250 300
time(s) 8. REFERENCES
Figure 11. The estimated rL by AEKF
[1] Antonelli, G. and S. Chiaverini, 2006. Linear estimation of
the odometric parameters for differential-drive mobile robots.
Proceedings of the 2006 IEEE/RSJ. International Conference
on Intelligent Robots and Systems. 9 - 15 October 2006,
2 Beijing, China. PRC. pp: 3287-3292.
1 - - estimated rR
____ l
1
L J _ [2] Antonelli, G. and S. Chiaverini, 2007. Linear estimation of
1.8 1 the physical odometric parameters for differential-drive
1
1 1 1
r - - - - --, - - - - -
1 1 mobile robots. Springer Netherlands, Auton. Robots, 23(1):
1.6 - - - - "1 - - - - -1- - - - - 1- - - - -
1 1 1 1 1 59-68, 2007.
1.4 - - - - -t -
1
- - -
1
-1- - - - -
1
+- - - - -
1
-l - - - -
1
- 1- - - - - [3] Borenstein, 1. and L. Feng, 1996.Measurement and
E 1 1 1 1 1
Correction of Systematic Odometry Errors in Mobile Robots.
.8- 1 1 1 1 1
,-CI:.
1.2
_ _ _ _ ..l 1 L ...J 1 _
IEEE Trans. Robot. Automat., 12(6): 869-880, 1996.
1 1 1 1 1
1 1 1 1 1 [4] Doh, N.L., H. Choset, and W.K. Chung, 2006. Relative
1 1 1 1
"'.r"'I"V""",,,I·V" '\. - - - -I localization using path odometry information. Springer
1 1 1
1
1 1
Netherlands. Auton. Robots, 21(2): 143-154, September
0.8 - - - - "1 - - - - -1- - - - - t" - - - - ""1 - - - - -1- - - - -
2006.
1 1 1 1 1
1
0.6 '---_ _....L....-_ _---1...-_ _----'-_ _-----1
1 1 1 1
-'---_-----'
[5] Grewal, M.S. and A.P. Andrews (2001). Kalman Filtering:
o 50 100 150 200 250 300 Theory and Practice Using Matlab. 2nd Edition, John Wiley
time(s) & Sons Inc., New York.
Figure 12. The estimated rR by AEKF [6] Larsen, T.D, K.L. Hansen, N.A. Andersen and O. Ravn,
1999. Design of Kalman filters for mobile robots; evaluation
of the kinematic and odometric approach. IEEE. Int. Conf.
Control Applications, 2: 1021-1026.

ISMA09-5
 Proceeding o/the 6th International Symposium on Mechatronics and its Applications (ISMA09), Sharjah, UAE, March 24-26,2009

[7] LaValle, S.M. (2006). Planning Algorithms, First Edition,

Cambridge University Press, New York. Also downloadable
at http://planning.cs.uiuc.edu.
[8] Martinelli, A., N. Tomatis, and R. Siegwart, 2007.
Simultaneous localization and odometry self calibration for
mobile robot. Springer Netherlands. Auton. Robots, 22(1):
75-85, January 2007.
[9] Siegwart, R. and I.R. Nourbakhsh (2004). Introduction to
Autonomous Mobile Robots, First Edition, MIT Press,
Cambridge, MA, USA.
[10] Tongue, B.H. and S.D. Sheppard (2005). Dynamics: Analysis
and Design of Systems in Motion. First Edition, John Wiley
and Sons Inc., New York.
[11] Welch, G. and G. Bishop, 2001. An introduction to the
Kalman filter, SIGGRAPH 2001 course 8 .In Computer
Graphics, Annual Conference on Computer Graphics &
Interactive Techniques, August 2001, Los Angeles, CA.

ISMA09-6