Hardware and Software Co-design for the EKF Applied to the Mobile Robotics Localization Problem

Abstract

Abstract-This paper describes a Hardware/Software Co-design approach for the Extended Kalman Filter (EKF) applied to the localization problem in mobile robotics. The EKF algorithm has been implemented and run on an Altera Cyclone IV FPGA with a Nios II embedded processor jointly with specific hardware modules, being adapted and applied to the mobile platform Pioneer 3AT (P3AT). In order to achieve this, we developed both the model of the mobile robot and its measurement systems previously to obtain the respective EKF equations. In the prediction step of the EKF algorithm, a system model based on concepts of dead-reckoning has been used and its implementation was achieved in software, using the Nios II processor. Conversely, in the estimation step of the EKF algorithm the respective equations have been implemented directly in hardware, producing an overall balanced implementation.Index Terms-Dead-reckoning, extended Kalman filter, FPGA, hardware/software co-design, Pioneer 3AT. I. INTRODUCTIONThere are several works describing the implementation of probabilistic algorithm in FPGAs such as described in [1]- [3]. Among them, the Kalman Filter (KF) is one of the most useful algorithms for estimating the state of a dynamic system [3]. A special case of KF is the Extended Kalman Filter, which application is oriented to nonlinear systems [4].This work presents a hardware/software co-design approach in FPGA of the Extended Kalman Filter Algorithm aiming at solving the Localization Problem in Mobile Robotics. The algorithm implementation was achieved considering the following approach: 1) a software implementation in the Nios II processor of the prediction stage (namely, 1 st and 2 nd EKF algorithm equations) and 2) a hardware implementation in Altera Cyclone IV FPGA of the estimation stage (namely 3 rd , 4 th and 5 th EKF algorithm equations). For this case, it has been considered that the mobile robot is located within an environment with a known map and it moves using data processing on line. In this case, the mobile platform used in this project was the Pioneer 3-AT [5].In this context, the contributions of this paper include: 1) the implementation of a sequential EKF algorithm in FPGA based on an approach of hardware/software co-design for the stages of prediction and estimation, using floating-point representation, and 2) the validation of the results in terms of hardware recourse consumption, performance and functionality in a real environment using the robot platform. II. THE EKF ALGORITHMThe KF is based on a recursive method to estimate the state variable (x K ) of a linear dynamic system with the process noise (w K ) and measurement noise (v K ) [6]. An extension of the Kalman Filter is the EKF algorithm (Extended Kalman Filter), which is used for nonlinear dynamical systems. The EKF tries to linearize the system around the current state. For this case, we will consider a nonlinear system represented by:where f(.) is the nonlinear function of the process system, h(.) is the nonlinear function of the measurement...