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, i.e., 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 (Antonelli and Chiaverini, 2007). Its advantages are the low
cost of the sensors, good accuracy for short distances and compatibility
with other positioning methods. The estimation of the odometric parameters
is known in the literature as odometry calibration.
The differentialdrive 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 study by Borenstein and Freg (1996), a calibration technique called,
UMBmark has been developed to calibrate for systematic errors of a differential
drive mobile robot. Martinelli et al. (2007) and Larsen et al.
(1999) introduced an Augmented Kalman Filter (AKF) which simultaneously
estimates 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. Although it is
straightforward and simple, it has drawbacks too (Julier and Uhlmann,
2002). The Unscented Kalman Filter (UKF) is the newest revision of the
Kalman Filter, proposed to overcome these flaws. It does not need the
linearization for a nonlinear function and is more accurate and simpler
than the EKF applied to nonlinear systems (Julier and Uhlmann, 2004; Wan
and van der Merwe, 2000).
In this study, we present the local reconstruction of differentialdrive
mobile robots position and orientation with an accurate odometry calibration.
To fuse the absolute measurement`s data and the odometry from the wheels`
encoders we design the augmented extended and unscented Kalman filters
and compare their performances under the same covariance matrices and
initial conditions.
MATERIALS AND METHODS
Kinematic model: Kinematics refers to the evolution of the position
and velocity of a mechanical system, without reference to its mass and
inertia. Let us consider a twowheeled differential drive mobile robot
operating on a planar surface as shown in Fig. 1, where
x and y denote the position of the center of the axle of the robot with
respect to the global coordinate frame, θ 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 wheela caster, is placed in the front to passively roll along
while preventing the robot from falling over. The differential equations
that describe the kinematics of the robot are written as:
where, the absolute velocity of the robot body (v) and the angular speed
(ω) are assumed to be inputs.
The motion of the left and right wheels is characterized by angular velocities
ω_{L} and ω_{R}, respectively. The bodyfixed
components v and ω of the robot velocity are related to these angular
velocities by:
In which d denotes the distance between the two wheels, while γ_{R}
and γ_{L} are the radii of the right and left wheels,
respectively (Antonelli and Chiaverini, 2006).
Augmented Kalman filter: Let us define
as the robot configuration;
The distance between the wheels or the wheel radiuses may not be precisely
known, which lead to systematic errors and worsen the performance of the
Kalman filter. Making a complex model of the twowheeled differential
drive mobile robots will not reduce this problem but in fact make it worse,
as this model not only will contain the same errors, but most likely also
introduce further inaccuracies (Larsen et al., 1999).

Fig. 1: 
Schematic diagram of a twowheeled differential drive
mobile robot 
To remedy this problem, we adopt the same Augmented Kalman Filter (AKF)
introduced by Martinelli et al. (2007) and Larsen et al.
(1999). This filter estimates the augmented state containing the robot
configuration and the systematic parameters as:
Extended Kalman filter: The Kalman filter addresses the general
problem of trying to estimate the state x ε R^{n} of a
discretetime controlled process that is governed by a linear stochastic
difference equation. As an extension to the same idea, the EKF is used
if the dynamic of the system and/or the output dynamic is nonlinear. EKF
is based on linearization about the current estimation error mean and
covariance.
Definitions: Let us assume that the process has a state vector
x ε R^{n} and a control vector u and is governed by
the nonlinear stochastic difference equation:
with a measurement z ε R^{m} that is
the random variables w_{k} and v_{k} represent
the process and measurement noise, respectively. They are assumed to be
independent of each other, white and with normal probability distributions
with covariance matrices Q and R. It can be shown that the time update
equations of EKF is:
where,
is the a priori state estimate. These time update equations project the
state and covariance estimate (P_{k}) from the previous time step
k–1 to the current time step k. And the measurement update equations
of the EKF are:
where, A, W, H and V are Jacobian matrices and K is the correction gain
vector. These measurement update equations correct the state and covariance
estimate with the measurement z_{k}. The design process of this
filter is explained next.
EKF implementation: The discrete model of the system can be derived
by converting the differential Eq. 1 to their corresponding
backward difference equations. So, the dynamical equation for the augmented
state
is given by:
where, the subscript k denotes the kth time sample and T is the sampling
period.
The Jacobian matrix at each iteration can be derived as:
With
Suppose that the position and orientation of the robot are measured directly.
Hence,
where, v_{k} represents the measurement noise and the output
matrix H is constant and defined as:
where, I_{3} is the 3x3 identity matrix and O_{3} is
a null 3x3 matrix.
Due to the recursive nature of the EKF algorithm, the state vector needs
to be initialized in startup. The initial robot configuration is taken
as the first measured value. Also, the nominal values of d, γ_{R}
and γ_{L} are chosen as their initial estimations.
Here, the following initial conditions are selected randomly for the state
vector:
We add uncertainty to the initial conditions by selecting
and it choose the process noise and measurement noise as:
Thus, it is developed all the necessary elements of the augmented EKF
for the twowheeled differential drive mobile robot.
Unscented Kalman filter: The basic premise behind the UKF is employing
a Gaussian distribution approximation of a nonlinear function rather than
the approximation. Instead of linearizing using Jacobian matrices, the
UKF uses a deterministic sampling approach to capture the mean and covariance
estimates with a minimal set of sample points and it has 3rd order (Taylor
series expansion) accuracy for Gaussian error distribution for any nonlinear
system (Wan and Van der Merwe, 2000), while EKF uses linearizing Jacobian
matrix, which is a first order approximation. The UKF is claimed to have
obvious advantages over EKF (Julier and Uhlmann, 2004). A brief overview
of the UKF algorithm here.
Definitions: The Unscented Transformation (UT) is a method
for calculating the statistics of a random variable which undergoes a
nonlinear transformation. The L dimensional random variable x with mean
and covariance P_{x} is approximated by 2L+1 weighted points given
by:
These sigma points are propagated through the nonlinear function,
from which the mean and covariance of the transformed probability can
be approximated
with weights W_{i} given by:
where, λ = α^{2}(L+κ)–L is a scaling parameter.
The constant α determines the spread of the sigma points around
and is usually set to small positive values less than one (typically in
the range 0.001 to 1) whereas, κ is the secondary scaling parameter
usually set to zero or 3–L and the constant
is used to incorporate prior knowledge of the distribution of x (for Gaussian
distributions, β = 2 is optimal). The scale parameters may be tuned
to match the specific problem; some guidelines to choose them are provided
in (Julier and Uhlmann, 2004).
The UKF can be implemented using UT by expanding the state space
to include the noise component: .
The UKF algorithm can be summarized as follows:
Initialization
Iteration for each time step
Where:
UKF implementation: We initialize it in the same way as the EKF,
using the same randomly selected values for the augmented state vector
and covariance matrices. It is also need to set the tuning parameters
α, β and κ. The optimum values for coefficients α
and β are chosen as 1 and 1.8, respectively. Also, κ is set
to zero. These optimum values are chosen such that they provide the best
estimates for robot position and orientation for all experiments (Wan
and Van der Merwe, 2000).
RESULTS AND DISCUSSION
Here, presents simulation results using our developed package by MATLAB^{®}.
Figure 2 shows the trajectory estimations of augmented EKF and UKF for
the twowheeled differential drive mobile robot.

Fig. 2: 
Trajectory estimations of augmented EKF and UKF 

Fig. 3: 
Illustration of using AEUKF to estimate the position
x 
To evaluate the performances of AEUKF, we plotted the estimated and actual
states for the available measurements in Fig. 35.
Figure 35 reveal that UKF outperforms the EKF. So, the claim
in (Julier and Uhlmann, 2004) and (Wan and Van der Merwe, 2000) that the UKF
is better than the EKF is verified in this specific application. Figure
6 shows the error comparison for these filters.
Also, Fig. 7 shows the values of estimated parameters
via the augmented filters. It can be concluded that the AUKF has less
variations around the nominal values than the AEKF and can be trusted
more.

Fig. 4: 
Illustration of using AEUKF to estimate the position
y 

Fig. 5: 
Illustration of using AEUKF to estimate the orientation 

Fig. 6: 
Error comparison for the augmented EKF and UKF 

Fig. 7: 
The estimated and by the augmented filters 
CONCLUSIONS
In this study, AEKF and AUKF are proposed to localize a differentialdrive
mobile robot while estimating a proper set of odometric parameters. Comparing
the simulations results in different scenarios show the performance of
the AUKF is consistently better than the AEKF. Moreover, both filters
provide estimations for the systematic parameters around their nominal
values. In the future we will conduct some experiments using the Khepera
II mobile robot, manufactured by KTeam, to verify the simulations of
this study. Furthermore, utilizing the AEUKF in the simultaneous localization
and mapping (SLAM) problem would be our future challenging task, where
the odometry calibration could play a vital role especially in solving
the data association problem.
ACKNOWLEDGMENT
This research is part of a research supported by a research grant of
the University of Tabriz.