Universidade Estadual de Campinas

Faculdade de Engenharia Elétrica e de Computação

IA368W: Métodos Estocásticos para Robótica Móvel

Atividade III: Localização - EKF

Extende Kalman Filter

Professor: Dr. Eleri Cardozo, PhD

Aluno: Julio Enrique Valentı́n Fajardo Hernández
RA: 209479
1 Abstract
The localization problem is the most basic perceptual task in robotics,
because a lot of tasks requires knowledge of the pose of objects that are being
manipulated, obstacles or the pose of the robot itself[1]. In this activity the
Extended Kalman Filter (EKF) localization algorithm was implemented and
tested using MobileSim, Octave, RestThru, the Pioneer 3DX robot and the
Sick LMS111. Finally, results was obtained testing the algorithm, both, in
the simulator and with the real robot with environments quite similar.

2 EKF Localization
The EKF Localization algorithm, basically, is an special case of Markov
localization, which involves the computation of a unique hypothesis on a
continuous space with an static environment, the algorithm assumes that
an accurate map of landmarks is available and relies on the assumption that
pose error is small. The algorithm is an straightforward application of the
Bayes filter to the localization problem, specifically an EKF application as
shown in Fig. 1[1, 2, 3]. The algorithm computes, first, in the Predic-
tion Step, a Gaussian probability distribution function (GPDF) using the
non-linear kinematic model (KM), where µ̄t is given by the equations of dif-
ferential drive robot model given by g(ut , µt−1 ), Gt is the Jacobian matrix of
the KM in order to linearize the system, Rt is the state transition error and
Σ̄t is estimated from the odometry error propagation model for differential
drive robots, shown in Fig. 2, then, in the Correction Step, a new GPDF
(µt and Σt ) is computed using the EKF algorithm as shown in Fig. 1, where
Kt is the Kalman Gain, zt are the poses of the Landmarks measured by
the sensor, h(µ̄t ) are the real poses of the Landmarks given the nonlinear
equations of the measurement model, Qt is the covariance of the sensor and
Ht is the Jacobian matrix of the measurement model as shown in Fig. 3.

Figure 1: The Markov Localization algorithm, the Bayes filter approach for
Localization based on multiple hypothesis.[1].

1
Figure 2: The Odometry Error Propagation Model, where f is the state of
our kinematic model state equation.[3].

Figure 3: Measurement model, where L is the real pose of a given Landmark,

xt ), yt and θt are given from the pose or the predicted pose of the robot and
Qt is given by the covariance of the sensor.[3].

3 Results
The results were obtained with Ks = 0.005, b = 165 mm with variances
for the state transition error of σx2 = σy2 = 5 and σθ2 = 3π/180, dt = 0.5s,
laser distance accuracy σld = 0.5 and laser angular accuaracy σlθ = π/1800.
The algorithm was tested using two different scenarios: Simulation shown in
Figs. 4, 5 & 6, and with the real robot in Figs. 7 & 8. In order to evaluate
the performance of the EKF in the localization problem, the Accumulated
Root Mean Square Error (RMSE) was implemented by using the measured
poses of the landmarks with their respective real poses in the environment
in Fig. 6. Each figure have captions that provides specifically information
about the test with a small explanation the of result obtained. The RMSE
is given by
v
u n
u1 X
RM SE = t (Lx − lx )2 + (Ly − ly )2 (1)
n
1

where L is the real pose of a given landmark, l is the pose measured by

the sensor and n is the number of landmarks.

2
Figure 4: EKF Robot Localization. MobileSim simulation on the left, EKF
localization visualization on the right. Blue small dots are the predicted
pose of the robot, big red square are the corrected pose. The red crosses are
the measured landmarks and the blue circles are the real landmarks.

Figure 5: EKF Robot Localization. MobileSim simulation on the left, EKF

localization visualization on the right.

Figure 6: Accumulated Root Mean Square Error of landmark pose on the

left (975 mm). RMSE graph on the right (0 − 40 mm).

3
Figure 7: EKF Robot Localization (On Pioneer 3DX). The estimated pose
is [1219 591.11]T , where x and y are in mm.

Figure 8: EKF Robot Localization (On Pioneer 3DX). The approximated

real pose of the robot is [1270 575]T , where x and y are in mm.

4
4 Conclusions
• The correction step brings more accurate pose estimation of the robot
by the using the Kalman Gain and the Innovation.

• Even though there is error in the estimated pose of the robot, it is only
5.35 cm, which shows that the EKF is an excellent estimator, despite
the systematic errors of the test, such as the initial pose of the robot,
real poses of the landmarks, etc.

• The errors of the measured poses of the landmarks influence indirectly

the estimated pose of the robot and accumulating errors over time,
however the EKF can handle this issue, providing an accurate esti-
mated pose of the robot.

References
[1] Sebastian Thrun. Probabilistic robotics. MIT press, 2005.

[2] Roland Siegwart. Introduction to Autonomous Mobile Robots. MIT

press, 2011.

[3] Eleri Cardozo. Slides of the course: IA368W - Métodos Estocásticos

para Robótica Móvel. FEEC, UNICAMP, Brazil, 2018.