THE INSTITUTE OF ELECTRONICS, IEICE ICDV 2012

INFORMATION AND COMMUNICATION ENGINEERS

Copyright 2012 by IEICE
Extended Kalman Filter in Mobile Robot and FPGA-based Implementation
T. T. Hoang, D. A. Viet, N. T. T. Van, P. D. Tuan and T. Q. Vinh
University of Engineering and Technology (UET)
Viet NamNational University, VNU
E-mail: thuanhoang@donga.edu.vn

Abstract Positioning is an important problemof mobile robot. Based on the sensor fusion method, the estimation of
uncertainty in robot perception can be improved. The probabilistic method with Extended Kalman filter (EKF) is one of good
solution for sensor fusion in mobile robot. However, the EKF requires a lot of computing time in personal computer PC due to it
has the mathematical complexity and large volume of data with operations related to multiplication and matrix. This paper
presents a multi-sensor mobile robot with the position estimation by Kalman Filter developed in personal computer at our
laboratory. In order to get the calculation speed-up, this filter was also implemented on a FPGA chip. The result shows that the
FPGA-based Kalman Filter exhibits similar accuracy to computer-based implementation but the calculation is much faster. This
research can be useful for small mobile robots which require small size and low power consumption.
Keyword Mobile Autonomous Robot, Sensor Fusion, Kalman Filter, FPGA.

1. I nt roduct i on
A mobi l e autonomous robot requi res accurate
posi ti oni ng f or i ts navi gati onal acti on. From current
readi ngs of the sensors, robot must determi ne
exactl y i ts posi ti on and ori entati on i n the
envi ronment. There are some ki nd of sensors whi ch
can be used f or robot. Each sensor of ten onl y
measure one or two envi ronmental parameters wi th a
l i mi ted accuracy. Natural l y, more used sensor, more
accuracy for determi ne posi ti on of robot. That s
reason the sensor f usi on method has been carri ed out
i n modern robots i n order to i ncrease the accuracy of
measurements. Al most of i mpl ementati on thi s
method are based on probabi l i sti c i nf erences. The
Extended Kal man Fi l ter (EKF) i s the most eff i ci ent
probabi l i sti c sol uti on to esti mate si mul taneousl y the
posi ti on of robot based on some i nterocepti ve and
exterocepti ve sensor i nf ormati on.
Mobi l e robots often use opti cal axi s encoders as
i nterocepti ve sensor for determi ni ng of posi ti on
fol l owi ng a method named as the odometry [1].
However, due to accumul ated error, the uncertai nty
of esti mated posi ti on by odometri c system i s
i ncreased ti me by ti me duri ng robot s movi ng [2]. I n
order to overcome thi s di sadvantage, by usi ng a
Kal man f i l ter wi th other measurements from one or
some exterocepti ve sensor combi ned wi th odometri c
measures, esti mated posi ti on val ue become more
accurate. That means that the esti mated traj ectory i s
more near the true traj ectory.
I n our work, one novel pl atf orm of mobi l e robot
wi th some modern sensor was desi gned and i nstal l ed.
A posi ti oni ng sof tware was devel oped whi ch i ncl ude
the Extended Kal man Fi l ter (EKF). The i nput
i nf ormati on i s from an odometri c system, a compass
sensor, and a l aser range f i nder LRF. The output
i nf ormati on i s a robot's pose (state) measure val ues
whi ch consi sts of coordi nates x, y, and headi ng u.
The experi ment shows that the esti mated output
val ues i s more near to the true ones as usi ng EK F.
The EKF has the mathemati cal compl exi ty and
l arge vol ume of data wi th operati ons rel ated to
mul ti pl i cati on, di vi si on and matri x operati ons. I t
needs a l ong ti me for computi ng by software program.
That s reason a hard-wi red f i l ter on FPGA was
devel oped and tested i n our work. Resul ts of
si mul ati on demonstrate appl yi ng capabi l i ty of FPGA
based fi l ter for mobi l e robot.
Thi s paper i s organi zed as fol l ows. I n secti on 2,
we present the structure of a mul ti -sensor mobi l e
robot desi gned i n our l aboratory. The devel opment
of a EKF for robot posi ti on esti mati on i s descri bed i n
Secti on 3 and Secti on 4 i s the report on experi mental
resul ts. I n secti on 5, the i mpl ementati on of a
FPGA- based Kal man f i l ter f or thi s robot wi th three
ki nds of sensor i s reported.
2. Pl at f or m of mul t i - sens or mobi l e robot
A mul ti -sensor mobi l e robot was desi gned and




devel oped i n our l aboratory. I n thi s work, the onl y
cogni ti ve parts wi th sensors are presented. Three
ki nds of sensor to devel op a sensor f usi on method
wi th an Extended Kal man Fi l ter are used. They are a
coupl e of opti cal encoders, a compass sensor, and a
l aser range fi nder (L RF).
The quadr at ur e opt i c al e nc ode rs are used to
measure speed of two wheel s of robot fol l owi ng
odometri c method. An opti cal encoder (f i g.1) i s
basi cal l y a mechani cal l i ght chopper that produces a
certai n number of si ne or square wave pul ses f or
each shaft revol uti on. As the di ameter of wheel and
the gear rati o are known, the angul ar posi ti on and
speed of wheel can be determi ned and the robot's
posi ti on can be found by these encoders wi th the
popul ar odomet ri c met hod.

Fi gure 1. Opti cal encoder structure.
The headi ng sens or i s used to determi ne the robot
ori entati on. Thi s sensory modul e contai ns a CMPS03
compass sensor operati ng based on Hal l eff ect wi th
headi ng resol uti on of 0.1 (f i g.2). The modul e has
two axes, x and y. Each axi s reports the strength of
the magneti c f i el d component paral l el ed to i t. The
mi crocontrol l er connected to the modul e uses
synchronous seri al communi cati on to get axi s
measurements [3].

Fi gure 2. Headi ng modul e and output data
A 2D l aser r ange f i nder (LRF) wi th a range from
0.04 m to 80 m i s i nstal l ed f or robot. I t can measure
the di stance and angl e between sensor and an obj ect.
I ts operati on i s based on the ti me-of -f l i ght
measurement pri nci pl e. A si ngl e l aser pul se i s
emi tted out and refl ected by an obj ect surf ace wi thi n
the range of the sensor. The l apsed ti me between
emi ssi on and recepti on of the l aser pul se i s used to
cal cul ate the di stance between the obj ect and the
sensor. By an i ntegrated rotati ng mi rror, the l aser
pul ses sweep a radi al range i n i ts front so that a 2D
f i el d/area i s def i ned [8]. I n each envi ronment scan,
the LRF gi ves a set of di stance data to the obstacl es
at the angl es. Fi g.3 shows the vi ew f i el d of one LRF
scan. Based on thi s data, envi ronmental features can
be f ound [4].

Fi gure 3. The vi ew f i el d of a LRF scan.
3. I mpl e me nt t he Ext ended Kal man Fi l t er f or
posi t i on est i mat i on of mobi l e robot
We start wi th the ki nemati c model of our mobi l e
robot. The two wheel ed, di fferenti al -dri ve mobi l e
robot wi th non-sl i ppi ng and pure rol l i ng i s
consi dered. Fi g.4 shows the coordi nate systems and
notati ons for the robot, where (X
G
Y
G
) i s the gl obal
coordi nate, (X
R
, Y
R
) i s the l ocal coordi nate rel ati ve
to the robot chassi s. R denotes the radi us of dri ven
wheel s, and L denotes the di stance between the
wheel s.

Fi gure 4. The robot s pose and parameters
Duri ng one sampl i ng peri od At , the rotati onal
speed of the l ef t and ri ght wheel s e
L
and e
R
create
correspondi ng i ncrement di stances As
L
and As
R

travel ed by the l eft and ri ght wheel s respecti vel y:

L L R R
s t R s t R e e A = A A = A


These can be transl ated to the l i near i ncremental
di spl acement of the robot s center As and the robots
Ob j e c t




ori entati on angl e Au :

2
L R R L
s s s s
s
L
u
A + A A A
A = A =
The state of robot at ti me k+1 i n the gl obal
coordi nate f rame can be then updated by:
( )
( )
1
1
1
cos / 2
sin / 2

k k k
k k
k k k k k
k k k
s
x x
y y s
u u
u u
u u u
+
+
+
( A + A
( (
(
( (
= + A + A
(
( (
(
( (
A

(

(1)
I n practi ce, the system model (1) has to add a more
term rel ated to unavoi dabl e noi ses appeared i n the
system. Noi ses can be both systemati c such as the
i mperfectness of robot model and nonsystemati c
such as the sl i p of wheel s. These errors have
accumul ati ve characteri sti c so that they can aff ect to
the accuracy of posi ti on esti mated val ues i f we onl y
use the system model . Usi ng the Kal man fi l ter
al gori thm i n thi s posi ti on esti mati on l oop permi ts to
i mprove the qual i ty of measurement.
Let [ ]
T
X x y u = be the state vector. Thi s state can
be observed by some absol ute measurements, z.
These measurements are descri bed by a nonl i near
f uncti on, h, of the robot coordi nates and an
i ndependent Gaussi an noi se process, v. Denoti ng (1)
i s a nonl i near f uncti on, f , wi th an i nput vector u , the
system equati ons, i n general , are gi ven as:
State equati on:
1
( , )
k k k k
x f x u w
+
= + (2)
Output equati on: ( )
k k k
z h x v = + (3)
where the random vari abl es w
k
and v
k
represent
the process and measurement noi se respecti vel y.
They are assumed to be i ndependent to each other,
whi te, and wi th normal probabi l i ty di stri buti ons:
~ (0, ) ~ (0, ) ( ) 0
T
k k k k i j
w N Q v N R E wv =
The steps to cal cul ate the EKF are then real i zed as
bel ow [5]:
1. Predi cti on step wi th ti me update equati ons:
-
-1 -1
( , )
k k k
x f x u = (4)
-
-1 -1
T T
k k k k k k k
P A P A W Q W = + (5)
where
-

n
k
x e9 i s the pr i or i state esti mate at step k
gi ven knowl edge of the process pri or to step k.

-

k
P i s the covari ance matri x of the state-predi cti on
error.
A
k
i s the J acobi an of parti al deri vates of f to x.
W
k
i s the J acobi an of parti al deri vates of f to w.
Q
k- 1
i s the i nput- noi se covari ance matri x whi ch
depends on the standard devi ati ons of noi se of the
ri ght-wheel rotati onal speed and the l eft-wheel
rotati onal speed. They are model ed as bei ng
proporti onal to the rotati onal speed e
R , k
and e
L , k
of
these wheel s at step k. Thi s resul ts i n the vari ances
equal to
2
R
oe and
2
L
oe , where o i s a constant
determi ned by experi ments.
The i nput- noi se covari ance matri x Q
k
i s def i ned
as:
2
,
2
,
. 0
0 .
R k
k
L k
Q
o e
o e
(
= (
(

(6)
2. Correcti on step wi th measurement update
equati ons:
- - -1
( )
T T T
k k k k k k k k k
K P H H P H V R V = + (7)
- -
( - ( ,0))
k k k k k
x x K z h x = + (8)
-
( - )
k k k k
P I K H P = (9)
where
n
k
x e9 i s the post e ri ori state esti mate at step
k gi ven measurement
k
z .
K
k
i s the Kal man gai n.
V
k
i s the J acobi an of parti al deri vates of h to v.
H
k
i s the J acobi an of parti al deri vates of h to x.
R
k
i s the covari ance matri x of noi ses esti mated
from noi ses i n measurements of wheel -encoders,
compass sensor and LRF. The measures of these
sensors are col l ected i n the vector z
k
as f ol l ows:
By system equati on (1), the state parameters
(x
meas. odometr y
, y
meas. odometr y
and u
meas. odometr y
) are
recei ved i ndi rectl y by wheel -encoder's readi ng.
The robot ori entati on (u
meas.compass
) can be al so
determi ned di rectl y by an absol ute measure wi th
compass sensor.
The LRF gi ves the envi romental f eature's
measurements. I n thi s case that i s a strai ght l i ne
detected by L RF i n Fi g.5 (e.g. a wal l i n room). That
l i ne i s parameteri zed i n two parameters: the di stance
from the coordi nate ori gi n and the angl e wi th the
normal . At the f i rst scan of LRF, a gl obal map of




envi ronment i s constructed composed of a l i ne
segment descri bed by parameters of di stance and
angl e |. The l i ne equati on i n normal f orm i s:
x
G
cos | + y
G
si n | =

(10)
When the robot moves, a new scan of LRF i s
perf ormed and a new map of envi ronment, namel y
l ocal map, i s constructed whi ch al so consi sts of a
l i ne segment descri bed by the equati on:
x
R
cos

+ y
R
si n = r (11)
where

and r are the parameters of l i ne.

Fi gure 5. The l i ne parameters (, |) accordi ng to the
gl obal coordi nates, and the l i ne parameters (r, )
accordi ng to the robot coordi nates
Due to the state of the robot i s esti mated by
odometry, the parameters and | of the l i ne segment
i n the gl obal map can be transformed i nto the
esti mated parameters r and (accordi ng to the
coordi nates of the robot) whi ch wi l l be used i n
equati on:
cos( ) sin( )
r r
C x y | |

( / 2) ( 0.5 si ( ) 0.5)

r
C r
gn C | u t t
( (
=
( (
+ +

(12)
Combi ni ng wi th measures from odometry and
compass sensor, the l i ne parameters r and from
the l ocal map are col l ected i n the vector z
k
. We have
the z
k
matri x as f ol l ows:

k
x
y
z
r
u
u

| |
|
|
|
|
=
|
|
|
|
|
\ .
k, meas.odometry
k, maes.odometry
k, meas.odometry
k, meas.compass
k, meas.LRF
k, meas.LRF
(13)
and the covari ance matri x R
k
of measurement noi se
has the di agonal structure.
The noi se of wheel speed measures can be
determi ned by experi ment. The accuracy of compass
sensor and L RF measures can be recei ved f rom the
manuf acture techni cal speci f i cati on. These are
f i l l ed i n R
k
f or the correcti on step of EKF.
var( ) 0 0 0 0
0 var( ) 0 0 0
0 0 var( ) 0 0
0 0
L
R
k compass
R
e
e
u =



0 var( ) 0
0 0 0 0 var( )
LRF
LRF
r

(
(
(
(
(
(
(
(



(14)
4. Experi me nt s and eval uat i on
Experi ments are conducted to eval uate the
eff i ci ency of the f usi on al gori thm wi th EKF.
4. 1. Expe ri me nt al Set up
The robot i s a two wheel ed, di ff erenti al -dri ve
mobi l e robot. I ts wheel di ameter i s 10cm and the
di stance between two dri ve wheel s i s 60cm. The
dri ve motors are control l ed by mi croprocessor-based
el ectroni c ci rcui ts. Due to the cri ti cal requi rement
of accurate speed control duri ng one sampl i ng step,
the PI D al gori thm i s used for the stabi l i ty of motor
speed i s measured 5%.
The compass sensor has the accuracy of 0. 1
0
. The
LRF has the accuracy of 30mm i n di stance and 0. 25
0

i n def l ect angl e. The sampl i ng ti me At of the EKF i s
0. 01s.
The proporti onal factor o of the i nput- noi se
covari ance matri x Q
k
i s experi mental l y esti mated as
fol l ows. The devi ati ons between the true robot
posi ti on and the posi ti on esti mated by the ki nemati c
model when dri vi ng the robot strai ght f orwards
several ti mes (from the mi ni mum to the maxi mum
tangenti al speed of the robot) i s determi ned. The
devi ati ons between the true robot ori entati on and the
ori entati on esti mated by the ki nemati c model when
rotati ng the robot around i ts own axi s several ti mes
(from the mi ni mum to the maxi mum angul ar speed of
the robot) i s al so determi ned. Based on the error i n
posi ti on and ori entati on, the parameter o i s
cal cul ated. I n our system, the o i s esti mated to be the
val ue 0.01.
4. 2. Se ns or Fus i on Al gor i t hm Eval uat i on
I n order to eval uate the eff i ci ency of the f usi on
al gori thm, di ff erent conf i gurati ons of the EKF were
i mpl emented. Fi g.6 descri bes the traj ectori es of a




robot movement esti mated by four sensor f usi on
methods used EKF: the odometry method, f usi on
wi th compass sensor, f usi on wi th LRF, and f usi on
wi th the combi nati on of LRF and compass sensor.
The resul t shows that three traj ectori es, based on
f usi on method wi th EKF, tracked wel l the real
traj ectory.

Fi gure 6. Esti mated robot traj ectori es under
di f ferent EKF conf i gurati ons.
The devi ati ons i n X-di recti on i n f i g.7 shown the
i mprovement of posi ti oni ng usi ng EFK.

Fi gure 7. Devi ati ons i n X -di recti on i n cases.
I t i s concl uded that the EKF al gori thm i mproves
the robot l ocal i zati on and i ts combi nati on wi th L RF
and compass sensor gi ves the opti mal resul t.
5. FPGA- bas ed Ext ended Kal man Fi l t er
The posi ti oni ng system wi th EKF above can be
presented wi th i nputs and outputs as the fol l owi ng
bl ock scheme i n f i gure 8. The system programmed
wi th the l oop consi sts of the system model (pl ant)
and f i l ter. The f i l ter has i nput for measuri ng data
(rbot's state, envi ronmental features, etc...), i nput
for state esti mated data of the predi cti on step and
output f or the state esti mated data. Two i nput
parameters e
L
and e
R
are the setti ng wheel -speeds
for each sampl e ti me.

Fi gure 8. I nput and output of the EKF.
The VHDL code i s i mpl emented by Xi l i nx tool s i n
MATL AB. A desi gn fl ow based di rectl y on System
Generator wi th process as Kal man f i l ter i s compl ex.
Thats reason we used the Acc el DSP Synt hes i s Tool
enabl es System Generator to export System
Generator i ntel l ectual property (I P) based on
f l oati ng-poi nt MATAB model [6]. Fi rstl y, the
Kal man f i l ter al gori thm i s wri tten i n f l oati ng-poi nt
MATL AB M-f i l e to perf orm then these M-f i l es can be
l oaded i nto the Accel DSP tool and become the
source f or a desi gn f l ow that produces opti mi zed
i mpl ementati ons i n Xi l i nx FPGA. The desi gn f l ow
was carri ed out wi th steps si mul ati on and
veri f i cati on: the f l oati ng-poi nt model i s quanti zed to
match f i xed-poi nt model , fi xed-poi nt model i s
compi l ed and the generated code and the RTL
hardware descri pti on are deri ved. Fi nal l y, the
descri pti on i s mapped i nto a bi t stream on the
hardware of Xi l i nx chi p Vertext-4 xsv35. Fi gure 9
shows the resul t of RTL system generator of EKF.

Fi gure 9. Kal man Fi l ter exported to
System Generator.




Tabl e 5.1 shows the synthesi ze resul t of the f i l ter
i n I SE.10.1. The whol e EKF al gori thm has used 50%
of FPGA resources.

Tabl e 5.1. Resul t of RTL synthesi s of EKF.
FPGA- based Kal man f i l ter was si mul ated usi ng
Matl ab Si mul i nk envi ronment wi th the sampl e ti me
10 ms. The EKF archi tecture was synchroni zed usi ng
72.2 MHz cl ock si gnal so as the whol e EKF al gori thm
was pe r f or med onl y i n 19. 6 S whi c h i s mor e shor t er
than sampl i ng ti me of 10 ms. After perf ormi ng each
Kal man f i l ter cal cul ati on procedure, the FPGA
ci rcui t stopped the operati on and wai ted f or the next
i ncomi ng start si gnal produced by MATL AB. That
meas that ci rcui t can be used f or appl i cati ons
requi red the hi gher processi ng speed.
We tri ed to si mul ate traj ectori es wi thi n 1, 000
sampl e steps wi th two setti ng speeds of 1.8*pi (rad)
and 2*pi (rad) of the l eft-wheel and ri ght-wheel ,
respecti vel y. The resul t i n Fi g. 10 shows that the
x-di mensi on esti mated traj ectory wi th FPGA- based
EKF (the bl ue col or curve) tracked wel l to the true
traj ectory (the red col or curve) compari ng wi th the
measured traj ectory wi thout usi ng the FPGA- based
EKF (the green col or curve).

Fi gure 10. Traj ectori es of measured curve (green),
FPGA-based EKF curve (bl ue) and true curve (red).

6. Concl us i on
The desi gn and i mpl ementati on of the Extended
Kal man Fi l ter (EKF) f or posi ti oni ng esti mati on of
mobi l e robot was presented i n thi s paper. The EKF
has been desi gned to fuse i n probabi l i sti c the raw
data of sensors. Experi ments showed that thi s novel
combi nati on si gni f i cantl y i mproves the accuracy of
robot posi ti oni ng and i s suf f i ci ent to ensure the
success of robot navi gati on. Furthermore, the EKF
was i mpl emented i n a FPGA chi p i n order to i ncrease
the processi ng speed f or EFK whi ch requi red many
matri x cal cul ati ons. The resul ts showed that thi s
approach i s reasonabl e and can be used f or
appl i cati ons requi red a hi gher speed.
Thi s work has been supported by the proj ect of
Smart I ntegrated System (SI S) bel ong to the UET.
REFERENCE
[1] J . Borenstei n1, H. R. Everett2, and L . Feng3, "
Where am I ? Sensors and Methods f or Mobi l e Robot
Posi ti oni ng". The Uni versi ty of Mi chi gan, Edi ti on
1996.
[2] J ohann Borenstei n and L i qi ang Feng, Measurement
and Correcti on of Systemati c Odometry Errors i n
Mobi l e Robots. I EEE Tr a ns ac t i o n s on Robo t i c s and
Aut omat i on , Vol 12, No 5, October 1996.
[3] [ Onl i ne] http: / / www. hol ux. com
[4] Si ck AG., 2006-08- 01 Tel egrams f or Operati ng/
Conf i guri ng the L MS 2xx (Fi rmware Versi on
V2. 30/X1. 27), www. si ck. com , Germany, 2007.
[5] Greg Wel ch and Gary Bi shop, " An I ntroducti on to the
K al man Fi l ter". TR 95- 041 Uni versi ty of North
Carol i na, NC 27599- 3175. Updated: Monday, J ul y 24.
2006.
[6] Bai Xue, Wang Demi ng, and Li u Xi anxi ng, " A New
Desi gn Approach f or K al man Fi l ter System Based on
Model to Hardware". The 5th I nternati onal
Conf erence on Computer Sci ence & Educati on Hef ei ,
Chi na. August 2427, 2010.
[7] Xi l i nx, " Accel DSP Synthesi s T ool Us e r Gu i d e " .
UG634 (v11. 4) December 2, 2009
[8] T. T. Hoang, D. A. Vi et, T. Q. Vi nh, A 3D i mage
capture system usi ng a l aser range f i nder, I EI CE
Proceedi ng of the 2th i nternati onal conf erence on
I ntegrated Ci rcui t Desi gn I CDV, Vi etnam, October,
2011.