Hardware Architecture of the EKF Prediction Stage

applied to Mobile Robot Localization

Luis Contreras, Sérgio Cruz, J.M.S.T. Motta, Carlos H. Llanos
Graduate Program in Mechatronics Systems, Department of Mechanical Engineering,
University of Brasilia
Brası́lia, D.F., Brazil, 70910-900
Email: lcontreras@aluno.unb.br and {sergio, jmmotta, llanos}@unb.br

Abstract—This work presents an FPGA-based Hardware Ar- Prediction Stage applied to the robot localization that it uses
chitecture to implement the Prediction Stage of the Extended intensive matrix calculations in floating-point representation,
Kalman Filter (EKF) applied to the localization problem in (b) the validation of the results in terms of hardware recourse
mobile robotics. The algorithm has been implemented and run
on an Altera Cyclone IV FPGA with a Nios II processor, being consumption, performance and functionality in a real scenery.
adapted and applied to the mobile platform Pioneer 3AT (P3AT).
The prediction stage was based on a dead-reckoning system model II. T HE S EQUENTIAL EKF A LGORITHM
and its architecture was designed for floating-point representa-
tion. In this project the complete EKF was also implemented
The KF is based on a recursive method to estimate the state
considering an estimation stage hardware architecture previously variable of a linear dynamic system with process noise and
developed using a Laser Range Finder (LRF) sensor, producing measurement noise [5]. An extension of the Kalman Filter
an overall balanced implementation. Finally, it was evaluated the is the EKF algorithm (Extended Kalman Filter), which is
system performance and suitability, measuring FPGA resources used for nonlinear dynamical systems. The EKF algorithm is
consumption and comparing execution time with a software
solution.
defined by (1) to (5), where the Prediction Stage is established
by (1) and (2). Regarding to the Estimation Stage, it is laid
I. I NTRODUCTION out by the (3), (4) and (5).
In these days are being developed many works using FPGAs
Xk− = f (Xk−1
+
, uk−1 , ek−1 ) (1)
(Field Programmable Gate Arrays) to implement different kind
of algorithms that involve hard algebraic calculations, which
Pk− = F Pk−1
+
F T + W Qk−1 W T (2)
can be applied to the areas of image processing, robotics,
solving linear equations, etc. [1], [2]. This is the case of  −1
Gk = Pk− H T HPk− H T + R (3)
the probabilistic algorithms used in mobile robotics. In this
context, the Kalman Filter (KF) is one of the most useful  
Xk+ = Xk− + Gk Z − h(Xk− ) (4)
algorithms for estimating the state in dynamic systems. A
special case of KF is the Extended Kalman Filter (EKF), Pk+ = Pk− − Gk HPk− (5)
whose application is oriented to nonlinear systems [3].
This paper describes a FPGA-based implementation of the where the symbols k and k − 1 are the actual and previous
EKF aiming at solving the localization problem in mobile time respectively. A detailed description about the symbols and
robotics using a multi-sensor system, involving encoders and matrix dimensions used in this paper is presented in Table I.
laser rangefinder. To accomplish this, a Hardware Module for
TABLE I: Symbol description and matrix dimensions for EKF
the prediction stage was developed, which uses floating-point
Symbols Dimension Description
representation for the intensive matrix calculations of the Se-
X 3×1 State vector Robot position
quential EKF. This module was connected to another module u 2×1 Input vector provided by the encoders
corresponding to the estimation stage previously developed e 2×1 Input error vector
in [2], allowing the complete implementation of the EKF P 3×3 Robot position covariance
F 3×3 Robot motion Jacobian
algorithm in hardware. The algorithm has been implemented W 3×2 Robot motion noise Jacobian
on Altera Cyclone IV FPGA with Nios II Processor and tested Q 2×2 Covariance of permanent process noise
over a P3AT mobile platform [4]. For this case, it has been G 3×2 EKF Gain
H 2×3 Measurement Jacobian
considered that the mobile robot is located in an environment R 2×2 Permanent measurement noise
of known map and also that the robot is in motion using data Z 2×1 Sensor measurement
processing on line. f (.) 2×1 Process Nonlinear Function
h(.) 2×1 Measurement Nonlinear Function
The main contributions of this paper are: (a) the FPGA - - predicted value
implementation of a Hardware Architecture for the EKF + - estimated information
978-1-4799-8332-2/15/$31.00 
c 2015 IEEE
A. Prediction Stage B. Estimation Stage
The state vector (X) of the system is composed of the To design this EKF stage, it is assumed that the mobile robot
following variables: x and y robot position and its angular is located in a known environment, which has the specific form
orientation θ, i.e. X = [x y θ]T . The system can be carried out (e.g., of a wall) showed in the Fig. 1.
considering that the vehicle is a 4-wheel skid-steering mobile
Yg
robot (4-SSMR), whose steering is based on the differentially a
driving wheel pairs on each side (relative velocities of left and

En
right side) of the mobile robot. Based on the SSMR models

vir
(W me
on
al nt
l ) lin
proposed in [6] and by using the Euler integration method one
can obtain the discrete-time system model for (1), i.e. the 1st

e
EKF equation, which can be written as (6). V O(l,m)
m
Xk− = −
f (Xk−1 ) = +
Xk−1 + Mk−1 , Ys vi Ȗi
ȕi U
ui
Si Xs
ysi xsi ៭
⎡ ⎤ ⎡ ⎤ ⎡ + ⎤ y
R
x−k x+k−1 ΔT.(α. VR +V
2
L
). cos(θk−1 )
⎣y − ⎦ = + ⎦
⎣ yk−1 + ⎣ ΔT.(α. VR +VL ). sin(θ+ ) ⎦ (6)
k 2 k−1
θk− 3×1 +
θk−1 ΔT.(α. VRD −VL
)
Ͷͷᤪ Xg
0 x l a

where ΔT is the sample time and uk = [VL , VR ]T is the Fig. 1: Mobile robot positioned on a specific environment.
input vector (provided by the encoders). The parameter D is
the effective spacing between wheels. The function f (.) was where Xg-Yg are the Global Coordinate System (GCS). Xs-Ys
+ are the Robot Coordinate System (RCS). xsi and ysi are the
decomposed in an addition of the vectors Xk−1 and Mk−1 .
The error for the linear tread speeds of VL and VR can be Sensor i Position into RCS. U-V are the Sensor Si Coordinate
defined such as described in (7). System (SiCS). γi is the distance between the detected object
O and the sensor Si. βi is the angle of measurement; ui and
 vi are the O-object Position into SiCS. And, l and m are the
VLe
VL = VLc + VLe , VR = VRc + VRe , ek = (7) O-object Position into GCS.
VRe 2×1 Then, two arrays can be defined as in (11):
where VLc and VRc are the correct linear speeds of each   2
u σ 0
wheels and the error vector e represents the uncertainties of Z= i , R = ui (11)
vi 2×1 0 σv2i 2×2
the odometric model, assuming that is Gaussian noise with
zero mean. where Z is the measurement vector, whose parameters ui and
To construct a model equation from (2), i.e. the 2nd EKF vi are used as inputs at this stage to update the mobile state.
equation, it is firstly described the Jacobian matrices F and Besides, R represents the measurement covariance matrix of
W , whose respective representations are described by (8) and the parameters (ui vi ). During each measurement, the O-object
(9). global position (coordinates l in Xg and m in Yg) is input to
the EKF algorithm. By using analytic geometry the parameters
⎡ ⎤ (l,m) can be found and described such as in (12) and (13):
1 0 −ΔT.(α. VR +V L
). sin(θk+ )
∂f () 2
F = = ⎣0 1 ΔT.(α. VR +V L
). cos(θk+ ) ⎦ (8)
∂Xk+ 2
(x− − − −
0 0 1 k + xsi . cos(θk ) − ysi . sin(θk )). tan(θk + βi )
3×3 lk = − +
tan(θk + βi ) + 1
⎡ ΔT ⎤
( 2 ). cos(θk+ ) ( ΔT + a − (yk− + xsi . sin(θk− ) + ysi . cos(θk− ))
∂f () 2 ). cos(θk ) + (12)
W = = α. ⎣ ( ΔT +
2 ). sin(θk ) ( ΔT +
2 ). sin(θk )
⎦ (9) tan(θk− + βi ) + 1
∂ek
− ΔTD
ΔT
D 3×2
mk = a − l k (13)
Then, the 2nd EKF equation can be described as in (10):
where a is related to the point (a,0) in GCS, corresponding to
2nd EKF Equation : Pk− = F.Pk−1
+
.F T + W.Q.W T , the intersection of the environment line with the Xg-axis.
The other array necessary for this stage is h(.), and it can
⎡ ⎤ be obtained from a coordinate transform of the feature O from
σx2 σxy σxθ  2
⎣ ⎦ σ 0 GCS to the Sensor Si Coordinate System (SiCS), yielding (14)
Pk = σyx σy2 σyθ , Q = VL (10)
0 σV2 R
σθx σθy σθ2 3×3 2×2   
Cθ Sθ lk x− xsi
h(Xk− ) = · − k −
where P is the error covariance matrix of robot pose, and Q −Sθ Cθ mk yk− ysi
represents the covariance matrix of permanent process noise. (14)
where, Cθ is cos(θk− ) and Sθ is sin(θk− ). Steps: 2nd, 3rd, 4th,
START
5th
From (14), the Jacobian matrix H can be calculated as in WAITING

(15): Steps:
1st , 2nd, Steps: MULT1
3rd, 4th, 1st , 6th
Steps:

5th, 6th
2nd, 3rd,
−Cθ −Sθ −(lk − x−
k) · Sθ + (mk − yk− ) · Cθ 4th, 5th

H=
Sθ −Cθ −(lk − x−
k) · Cθ − (mk − yk− ) · Sθ 2×3 ADD Steps: 4th, 5th MULT2

(15)
MULTADD Steps: 2nd, 3rd
Steps: 2nd, 3rd

III. FPGA I MPLEMENTATION USING I NDIVIDUAL

M ODULES FOR EACH S TAGE Fig. 2: FSM to perform the EKF Prediction Stage.

This section presents the hardware architecture that was

used to implement the EKF Prediction Stage using a specific
data path and a global Finite State Machine (FSM). This
approach reduces the required hardware resources in terms
x Register
of multipliers and adders. This occurs because it is possible Entrance
Matrices
Exit
Matrices
Bank Bank
to share the same data path for implementing the 1st and 2nd (EMB) Element
Selector +
(XMB)

EKF equations, reducing the number of floating-pointing op- or

+
x -
erations and, consequently, the FPGA resources consumption. or
-
For implementing the EKF Prediction Stage was necessary to Element
Selector
Element
Element
Selector

divide in six steps as shown in (16) to (21). Intermediate

Matrices
Selector Intermediate
Matrices
Bank x Bank
(IMB-1) (IMB-2)

1st : Xk− = Xk−1

+
+ Mk−1 (16) Element
Selector

1 +
2nd : O3×3 = Pk−1 · FT
WAITING MULT1 MULT2 MULTADD ADD

(17)
Fig. 3: DPB for computing the EKF Prediction Stage.
2 1
3rd : O2×3 = F · O3×3 (18)
3
4th : O2×3 = Q · WT (19)
4 3
5th : O3×3 = W · O2×3 (20) FPGA Design
Clock Extended Kalman Filter

6th : Pk− = 2
O3×3 + 4
O3×3 (21) Reset Nios II Hardware Module ± EKF Prediction Stage
Processor EMB Datapath 1
Datapath 2
where the 1st EKF equation is represented by (16). So, (17) to Keyboard
IMB FSM

(21) are the corresponding steps to compute the 2nd EKF equa- PIOs 1 XMB Datapath 9

tion. The six steps of this stage include matrices operations of PS2 PIOs2 Hardware Module ± EKF Estimation Stage
LEDs
multiplication and addition. To implement those operations, PIOs 3
LCD
Keys PIOs 4 UART 0
a general Finite State Machine (FSM) was designed based Rx/Tx 0
Timers UART 1
floating-point arithmetic operators [7]. The FSM structure is Switches
Avalon Bus

Rx/Tx 1
SDRAM Controller
shown in Fig. 2 and is composed of five states: waiting, SDRAM
CORDIC
mult1, mult2, multadd and add. The Fig. 3 depicts the general
Data Path Block (DPB) used to calculate each defined matrix
element. The DPB was based on the FSM structure. Also Fig. 4: Diagram of the overall architecture for the project
in this DPB, the EMB unit is composed of the input array
+ +
elements Xk−1 , Mk−1 , Pk−1 , F, Q and W ; the XMB unit is
composed of the output array elements Xk− and Pk− , and the
IMB unit is composed of temporary array elements O1 , O2 , O3 Fig. 4 presents the diagram of the overall architecture used
and O4 . for this project. In this case, there are nine Data Path Blocks
For instance, it can be seen that (17), which is the first step (such as that depicted in Fig. 3) because of the maximum
to compute the 2nd EKF equation, is a matrix multiplication matrix dimension in the algorithm computation is 3×3. The
+
(Pk−1 · F T ). The result of the multiplication is a new matrix architecture of the EKF was designed using: (a) the hardware
O1 with dimension of 3×3, whose first element is represented module for the Prediction Stage developed in this work, and
by (22). (b) a hardware module for the Estimation Stage developed
previously [2]. On the other hand, the calculation of the
o111 = p+ + + Jacobian matrices F , W and H; the nonlinear function h(.)
11 · F11 + p12 · F21 + p13 · F31
T T T
(22)
and the vector M is performed previously in the Nios II
In this case the FSM was designed to perform the three processor and then their results are addressed to the EKF
multiplications and two additions in floating-point. hardware architecture.
 80

IV. R ESULTS 70

A. Synthesis results and execution time 60

Angle ( grades )
50

The prediction stage architecture has been described in 40

VHDL using the Quartus 12.1. Table II shows the synthesis 30

results for EP4CE115F29C7 FPGA Device (Family Cyclone 20

IV E). 10

-10
TABLE II: Hardware synthesis results and comparison to other architectures 0 10 20 30 40 50 60
Time ( s )
Architecture LEs DSPs Clk (MHz)
Fig. 6: Estimation of the robot angular orientation θ. The blue line and red
EKF Prediction 15843 (14%) 63 (12%) 49,86
squares represent the software and hardware solution respectively.

Regarding the time execution, it was used the ModelSim-

Altera Start Edition tool for the behavioral simulation of the V. C ONCLUSION
proposed architecture. It was observed that this stage requires
This work has proposed an FPGA-based hardware ar-
1 μs (20 ns × 50 clock cycles) to be executed using a 50MHz
chitecture of EKF Prediction Stage applied to the Mobile
clock. However, taking into account the delay of data commu-
Robotics Localization Problem. The resulting system runs on
nication between NIOS and the hardware architecture this time
an Altera Cyclone IV FPGA with a Nios II processor, adapted
increases to 31 μs. Additionally, the EKF Prediction stage was
to the mobile platform Pioneer 3AT. The synthesis results
implemented completely in the NIOS II processor (50MHz)
presented few hardware resources consumption demonstrating
and Matlab (using an Intel
TM
R R
Core i5-2410M Processor
the suitability of the proposed architecture. The execution
2.90 GHz, 35W) in order to calculate the performance of a
time results point out that the hardware solution has a speed
software implementation, obtaining that the required time was
up factor greater than 2 in comparison with the embedded
470μs and 76 μs respectively.
software implementations. Moreover, the validation results
B. Architecture Validation showed the adaptability of the proposed architecture to be used
in an EKF implementation, obtaining estimations (position
After the implementation of EKF algorithm (using indi-
and orientation) very similar to an implementation purely in
vidual modules for each EKF stage) in the Altera DE2-115
software. As future works, it will develop a unique architecture
board [8] the experimental validation was realized using the
for the overall EKF algorithm (Prediction and Estimation).
Pioneer 3-AT mobile platform. The test environment map was
designed to have the parameter a = 137cm and the EKF was VI. ACKNOWLEDGMENT
set with initial values as X0+ = [5mm, 5mm, 0o ] . Also, the
T
The authors would like to thank the Brazilian institution
LRF sensor was located on the Xs- axis, i.e. the LRF laser CAPES (Coordination for Higher Education Staff Develop-
beam was oriented to the robot front (xsi = 17cm, ysi = 0 ment) for its financial support and to the Altera University
and βi = 0o ). In this case, it was done a comparison between Program.
a hardware implementation (written in VHDL language) and
a software implementation (written in C language) using the R EFERENCES
NIOS II processor. The results of the robot motion estimation [1] J. Arias-Garcia, C. Llanos, M. Ayala-Rincon, and R. Jacobi, “A fast and
(x,y) are shown in Fig. 5, where the blue starts represent the low cost architecture developed in fpgas for solving systems of linear
equations,” in Circuits and Systems (LASCAS), 2012 IEEE Third Latin
EKF software solution and the red squares correspond to the American Symposium on, Feb 2012, pp. 1–4.
hardware solution, for the same data adquisition. [2] S. Cruz, D. Munoz, M. Conde, C. Llanos, and G. Borges, “A hardware
approach for solving the robot localization problem using a sequential
35 ekf,” in Parallel and Distributed Processing Symposium Workshops PhD
30 Forum (IPDPSW), 2013 IEEE 27th International, May 2013, pp. 306–
25
313.
[3] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. Cambridge,
20
MA, USA: The MIT Press, 2005.
y ( cm )

15 [4] AdeptMobilerobots. (2012) ”p3-at datasheet”. [Online]. Available:

10
http://www.mobilerobots.com/Libraries/Downloads
[5] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E.
5
Kavraki, and S. Thrun, Principles of Robot Motion: Theory, Algorithms,
0 and Implementations. Cambridge, MA, USA: The MIT Press, 2005.
-5 [6] K. Kozlowski and D. Pazderski, “Modeling and control of a 4-wheel skid-
-5 0 5 10 15 20 25 30 35 40 45
x ( cm ) steering mobile robot,” International Journal of Applied Mathematics and
Fig. 5: Results of the robot pose estimation in the plane XgYg. Computer Science, vol. 14, no. 4, pp. 477–496, 2004.
[7] D. M. Munoz, D. Sanchez, C. Llanos, and M. Ayala-Rincon, “Tradeoff of
FPGA design of a floating-point library for arithmetic operators,” Journal
Otherwise Fig. 6 presents results of the angular orientation of Integrated Circuits and Systems, vol. 5, no. 1, pp. 42–52, 2010.
estimation (θ) along the time. It can be observed that both [8] Terasic. (2012) ”altera de-115 development and education board”.
[Online]. Available: http://www.terasic.com.tw
hardware and software estimations are very similar during the
test session.