Search in book...
Toggle Font Controls
Create new playlist

Name your new playlist

Playlist description (optional)
Sign In

Email address

Password

Forgot Password?

or

Continue with Facebook

Continue with Google
Sign Up

Full Name

Email address

Confirm Email Address

Password

or

Continue with Facebook

Continue with Google

Chapter 2

Accelerating Techniques for Particle Filter Implementations on FPGA

B.G. Sileshi¹ [email protected]; C. Ferrer¹^,² [email protected]; J. Oliver¹ [email protected] ¹ Departament de Microelectrònica i Sistemes Electrònics, Universitat Autònoma de Barcelona, Bellaterra (Barcelona), Spain
² Institut de Microelectrònica de Barcelona (CNM-CSIC), Bellaterra (Barcelona), Spain

Abstract

Particle filters (PFs) are Bayesian-based estimation algorithms with attractive theoretical properties for addressing a wide range of complex applications that are nonlinear and non-Gaussian. However, they are associated with a huge computational demand that limited their application in most real-time systems. To address this drawback in PFs, this chapter presents PF acceleration techniques based on a hardware/software codesign approach for the grid-based fast simultaneous localization and mapping (SLAM) application. With initial identification of the computationally intensive steps of the algorithm, techniques are proposed to accelerate the computational bottleneck steps. Based on the proposed PF acceleration techniques, hardware blocks are designed to speed up the computational time and interface in a central MicroBlaze soft processing core platform. The proposed hardware/software implementation resulted in an improvement in the overall execution time of the algorithm.

Keywords

Particle filters (PFs)

hardware/software codesign

field-programmable gate array (FPGA)

simultaneous localization and mapping (SLAM)

computational complexity

1 Introduction

Estimating the state of a dynamic system from a set of observations that arrive sequentially in time and are corrupted by noise has different applications. For systems where the amount of nonlinearity is limited, the parametric Kalman filters (extended or unscented versions) can be applied. However, for systems with nonlinear or non-Gaussian dynamics, the Kalman filters were found to perform poorly (Meinhold and Singpurwalla, 1989; Ristic et al., 2004). On the other hand, a nonparametric sequential Bayesian estimation technique called particle filters (PFs) present accurate estimates under such circumstances (Gordon et al., 1993; Arulampalam et al., 2002) and have been used in applications such as navigation and positioning (Gustafsson, 2010; Nordlund and Gustafsson, 2001; Atia et al., 2010, 2012; Georgy et al., 2012), tracking (Happe et al., 2011; Medeiros et al., 2008), and robotics (Grisetti et al., 2007). There has been also extensive research in recent years for the application PFs to estimate the states and parameters in biological systems. For example, several studies (Nakamura et al., 2009; Yang et al., 2007; Gokdemir and Vikalo, 2009; Liu and Niranjan, 2012) demonstrate the applicability of PFs in the state estimation of biological models by considering biological systems as dynamical, nonlinear, and generally partially observed processes.

PFs have the flexibility of efficiently representing a wide range of probability densities using sets of points called particles. Such representational power of PFs, however, comes at the cost of a great computational complexity that has so far limited their application in different types of real-life problems. It is in this sense that hardware- or hardware/software-based implementations of PFs are essential to accelerate the computational time and achieve the objective of real-time computation. With regard to this, the research literature tackles the real-time constraint of the PFs through hardware implementation platforms such as field-programmable gate arrays (FPGAs). Interesting features of FPGAs, such as their ability to introduce massive parallelization and their low power consumption, which is critical for many applications such as navigation, make them the primary choice for the implementation of PFs in real-life applications.

The hardware implementation of PFs based on FPGA platforms has been proposed by several researchers. For example, Chau et al. (2012) studied the implementation of PFs on an FPGA in a mobile robot localization problem, where the size of the particle set is adapted to reduce the run-time computational complexity of the algorithm. In Fross et al., (2010), the FPGA hardware implementation of PF for location estimation is presented and compared with the software solution running on an ARM7-based microcontroller. A recent study (Chau et al., 2013) presents a heterogeneous reconfigurable system consisting of an FPGA and a central processing unit (CPU) for a simultaneous mobile robot localization and people-tracking application. In this study, they adapt the number of particles dynamically and utilize the run-time reconfigurability of the FPGA to reduce power and energy consumption. According to their results, a speed increase of up to 7.39 times is achieved compared to the Intel Xeon X5650 CPU software implementation. Li et al. (2011) presented a hardware/software codesign approach based on a system on a chip technique that uses a NIOS II processor to calculate the weight for each particle and a hardware implementation to update the particles. They claim that their proposed method significantly improves the efficiency and provides flexibility in the design step for various applications due to the software implementation of the importance weight step. Ye and Zhang (2009) presented hardware architecture for an FPGA implementation of the sampling importance resampling (SIR) PF. The hardware architecture is simulated with ModelSim and the real-time performance of the hardware architecture is evaluated using a universal asynchronous receiver/transmitter (UART) for the input sensor data. Saha et al. (2010) presented a System-on-Chip architecture involving both hardware and software components for a class of PFs. In this chapter, a parameterized framework is used to enable the reuse of the architecture with minimal redesign for a wide range of particle filtering applications. In El-Halym et al. (2012), three different hardware architectures are proposed, and this suggests the use of a piecewise linear function instead of the classical exponential function in order to decrease the complexity of the hardware architecture in the weighting step.

With the objective to reduce the high computational requirements of the PF, this chapter presents PF acceleration techniques based on a hardware/software codesign approach with the following specific contributions:

• As the underlying computations in particle filtering vary from one application to the next, a generic approach is presented for the analysis, design, and speed-up of the PF computational steps. The same approach can be used in the development of other application-specific PF implementations.

• The embedded hardware/software implementation of the PF focuses on a grid-based fast simultaneous localization and mapping (SLAM) algorithm. In order to increase the throughput of the algorithm, two main mechanisms are introduced in the algorithm acceleration:

• Fast hardware Gaussian random number generator design and its application to PF for grid-based Fast SLAM algorithm is presented.

• The design and implementation of the custom COordinate Rotation DIgital Computer (CORDIC) hardware module for the evaluation of the complex mathematical operations involved in several steps of the PF algorithm is presented.

This chapter is an extended version of the work presented by Sileshi et al. (2014), and it is organized as follows: Section 2 first introduces the theory behind the PF algorithm, followed by a discussion on its application with the SLAM algorithm called Grid-based Fast SLAM. Section 3 provides a discussion on the identification of the computational bottlenecks of the PF algorithm and the partitioning of the different steps for hardware and software implementations. Section 4 explains the PF acceleration techniques, and section 5 is dedicated to the hardware architecture design of the proposed acceleration techniques. Section 6 discusses the proposed hardware/software architecture of the PF and Section 7 presents the implementation results. Finally, Section 8 provides a conclusion of these topics.

2 PF and SLAM algorithms

2.1 Particle filtering

The problem of nonlinear filtering is defined by a state space representation of dynamic system given by a discrete-time stochastic model of the state $x_{t} \in R^{d_{x}}$ $x_{t} \in R^{d_{x}}$ and measurement $z_{t} \in R^{d_{z}}$ $z_{t} \in R^{d_{z}}$ vectors with dimensions d_x and d_z, respectively:

$x_{t} = f_{t} (x_{t - 1}, v_{t - 1})$ $x_{t} = f_{t} (x_{t - 1}, v_{t - 1})$

(2.1)

$z_{t} = h_{t} (x_{t}, w_{t})$ $z_{t} = h_{t} (x_{t}, w_{t})$

(2.2)

where f_t and h_t are possibly nonlinear function of the state $x_{t - 1}$ $x_{t - 1}$ and the measurement z_t, respectively; $v_{t - 1}$ $v_{t - 1}$ and w_t are the process and measurement noise sequences, respectively; and t is the time index. The process and measurement noise sequences are assumed to be white, with known probability density functions and mutually independent.

In Bayesian estimation framework, the estimation to the state vector x_t at time t is obtained from a noisy measurement z_t, where the initial target state is assumed to have a known probability density function (pdf) p(x₀) and is also independent of the noise sequences. The estimate of the unknown state x_t is performed based on the sequence of all available measurements $Z_{1 : t} = \{z_{i}, i = 1, \dots, t\}$ $Z_{1 : t} = \{z_{i}, i = 1, \dots, t\}$ up to time t. The posterior distribution $p (x_{t} | Z_{t})$ $p (x_{t} | Z_{t})$ is calculated recursively based on the following two prediction [Eq. (2.3)] and update [Eq. (2.4)] equations (Arulampalam et al., 2002):

$p (x_{t} | Z_{t - 1}) = \int p (x_{t} | x_{t - 1}) p (x_{t - 1} | Z_{t - 1}) d x_{t - 1}$ $p (x_{t} | Z_{t - 1}) = \int p (x_{t} | x_{t - 1}) p (x_{t - 1} | Z_{t - 1}) d x_{t - 1}$

(2.3)

$p (x_{t} | Z_{t}) = \frac{p (z_{t} | x_{t}) p (x_{t} | Z_{t - 1})}{p (z_{t} | Z_{t - 1})}$ $p (x_{t} | Z_{t}) = \frac{p (z_{t} | x_{t}) p (x_{t} | Z_{t - 1})}{p (z_{t} | Z_{t - 1})}$

si31_e (2.4)

where the probabilistic model of the state evolution $p (x_{t} | x_{t - 1})$ $p (x_{t} | x_{t - 1})$ , is defined by the system equation [Eq. (2.1)], $p (z_{t} | x_{t})$ $p (z_{t} | x_{t})$ is defined by the measurement equation [Eq. (2.2)], and $p (z_{t} | Z_{t - 1})$ $p (z_{t} | Z_{t - 1})$ is a normalizing constant given by

$p (z_{t} | Z_{t - 1}) = \int p (z_{t} | x_{t}) p (x_{t} | Z_{t - 1}) d x_{t} .$ $p (z_{t} | Z_{t - 1}) = \int p (z_{t} | x_{t}) p (x_{t} | Z_{t - 1}) d x_{t} .$

(2.5)

While the assumption of the linear and Gaussian conditions is satisfied, the optimal analytic solution to the integrals given by Eqs. (2.3) and (2.4) is obtained by using the Kalman filtering method. However, it is often difficult to find an analytical solution to these integrals in most nonlinear/non-Gaussian estimation problems, and Sequential Monte Carlo (SMC) approximation method is used in the approximation of these integrals.

PF is an SMC-based approach that approximates a pdf by a discrete set of particles with their associated weights ${\{x_{t,}^{i} w_{t}^{i}\}}_{i = 1}^{N}$ ${\{x_{t,}^{i} w_{t}^{i}\}}_{i = 1}^{N}$ , where i and N denote the index of the particles and the total number of particles, respectively; $x_{t,}^{i} \in R^{d_{x}}$ $x_{t,}^{i} \in R^{d_{x}}$ and w_tⁱ denotes the state of the particles and their weights, respectively; and d_x is the dimension of the state. The posterior pdf $p (x_{t} | Z_{t})$ $p (x_{t} | Z_{t})$ of the state vector is approximated by (Ristic et al., 2004)

$p (x_{t} | Z_{t}) \approx \sum_{i = 1}^{N} w_{t}^{i} δ (x_{t} - x_{t}^{i}),$ $p (x_{t} | Z_{t}) \approx \sum_{i = 1}^{N} w_{t}^{i} δ (x_{t} - x_{t}^{i}),$

(2.6)

where δ(.) is a Dirac Delta function and w_tⁱ is the importance weight, given by

$w_{t}^{i} \propto w_{t - 1}^{i} \frac{p (z_{t} | x_{t}^{i}) p (x_{t}^{i} | x_{t - 1}^{i})}{q (x_{t}^{i} | x_{t - 1}^{i}, z_{t})} .$ $w_{t}^{i} \propto w_{t - 1}^{i} \frac{p (z_{t} | x_{t}^{i}) p (x_{t}^{i} | x_{t - 1}^{i})}{q (x_{t}^{i} | x_{t - 1}^{i}, z_{t})} .$

si40_e (2.7)

In particle filtering, the particle set $S_{t} = {\{x_{t}^{i}, w_{t}^{i}\}}_{i = 1}^{N}$ $S_{t} = {\{x_{t}^{i}, w_{t}^{i}\}}_{i = 1}^{N}$ at time t is recursively propagated from the set $S_{t - 1}$ $S_{t - 1}$ at the previous time $t - 1$ $t - 1$ in such a way that they provide an approximation of the posterior distribution at each iteration. The initial particle set S₀ is obtained from samples drawn from the prior density p(x₀). In this approach, a major problem called particle degeneracy phenomenon arises. That is, after a certain number of iterations, a small number of normalized importance weights tend to value of one, while the remaining weights are negligible. This is not a desirable phenomenon, as only a few particles contribute to the approximation of the posterior density and much computation is wasted on the particles with negligible weight. A common measure of the degeneracy of the algorithm is the effective sample size N_eff, given by (Arulampalam et al., 2002)

$N_{eff} = \frac{1}{\sum_{i = 1}^{N} {(w_{t}^{i})}^{2}} .$ $N_{eff} = \frac{1}{\sum_{i = 1}^{N} {(w_{t}^{i})}^{2}} .$

si44_e (2.8)

The degeneracy problem can be reduced by using a very large value of samples N but, since that approach is impractical, other alternative techniques are commonly used. These includes the use of an optimal importance density and resampling (Arulampalam et al., 2002). The optimal choice of the importance density is commonly considered the prior density. In resampling, particles are resampled N times with replacement from the discrete approximation of the posterior density. In this process, particles with low importance weight are eliminated and particles with high importance weight are replicated. This step is performed while N_eff falls below a certain predefined threshold, N_T. The resampling step is intended to reduce the degeneracy problem, but it leads to the limitation that it parallelizes the algorithms and loses diversity among the particles. The loss of diversity among the particles may lead to the occupancy of the same point in the state space by all N particles, giving poor representation of the posterior density.

The basic steps of the PF discussed so far, which correspond to an SIR PF, is given as follows:

Input: $[{\{x_{t - 1}^{i}, w_{t - 1}^{i}\}}_{i = 1}^{N}, z_{t}]$ $[{\{x_{t - 1}^{i}, w_{t - 1}^{i}\}}_{i = 1}^{N}, z_{t}]$

Output: $[{\{x_{t}^{i}, w_{t}^{i}\}}_{i = 1}^{N}]$ $[{\{x_{t}^{i}, w_{t}^{i}\}}_{i = 1}^{N}]$

FOR $i = 1 : N$ $i = 1 : N$

Draw $x_{t}^{i} ~ q (x_{t} | x_{t - 1}, z_{t})$ $x_{t}^{i} ~ q (x_{t} | x_{t - 1}, z_{t})$

Calculate the nonnormalized importance weights ${\tilde{w}}_{t}^{i}$ ${\tilde{w}}_{t}^{i}$

${\tilde{w}}_{t}^{i} = w_{t - 1}^{i} \frac{p (z_{t} | x_{t}^{i}) p (x_{t}^{i} | x_{t - 1}^{i})}{q (x_{t}^{i} | x_{t - 1}^{i}, z_{t})}$ ${\tilde{w}}_{t}^{i} = w_{t - 1}^{i} \frac{p (z_{t} | x_{t}^{i}) p (x_{t}^{i} | x_{t - 1}^{i})}{q (x_{t}^{i} | x_{t - 1}^{i}, z_{t})}$

END FOR

Calculate sum of the weights: W = $\sum_{i = 1}^{N} {\tilde{w}}_{t}^{i}$ $\sum_{i = 1}^{N} {\tilde{w}}_{t}^{i}$

FOR $i = 1 : N$ $i = 1 : N$

Normalize: $w_{t}^{i} = W^{- 1} {\tilde{w}}_{t}^{i}$ $w_{t}^{i} = W^{- 1} {\tilde{w}}_{t}^{i}$

END FOR

Calculate N_eff using Eq. (2.8)

If $N_{eff} \leq N_{T}$ $N_{eff} \leq N_{T}$

RESAMPLE

END IF

SIR is one of the most commonly used variants of the basic particle filtering known as sequential importance sampling (SIS) PF, where the prior density $p (x_{t}^{i} | x_{t - 1}^{i})$ $p (x_{t}^{i} | x_{t - 1}^{i})$ is used as the importance density $q (x_{t}^{i} | x_{t - 1}^{i}, z_{t})$ $q (x_{t}^{i} | x_{t - 1}^{i}, z_{t})$ and the weights of the particles is given by $w_{t}^{i} \propto p (z_{t} | x_{t}^{i})$ $w_{t}^{i} \propto p (z_{t} | x_{t}^{i})$ .

2.2 Application of PF to SLAM

SLAM involves the problem of localizing and building a map of a given environment simultaneously (Chen et al., 2007). It is usually described with a joint posterior probability density distribution [Eq. (2.9)] of the map (m) and the robot states (x_t) at time t, given the observations (z_0 : t) and control inputs (u_0 : t) up to and including time t, together with the initial state of the robot (x₀):

$p (x_{t}, m | z_{0 : t}, u_{0 : t}, x_{0}) .$ $p (x_{t}, m | z_{0 : t}, u_{0 : t}, x_{0}) .$

(2.9)

For the computation of the conditional probability given by Eq. (2.9) for all times t, a recursive solution based on the Bayes theorem is used in SLAM by starting with an estimate for the distribution $p (x_{t - 1}, m | z_{0 : t - 1}, u_{0 : t - 1})$ $p (x_{t - 1}, m | z_{0 : t - 1}, u_{0 : t - 1})$ at time $t - 1$ $t - 1$ and following a control u_t and observation z_t. Therefore, the SLAM algorithm is implemented with the two standard steps of the recursive prediction and measurement update process. The recursion is a function of the robot motion model $p (x_{t} | x_{t - 1}, u_{t})$ $p (x_{t} | x_{t - 1}, u_{t})$ and an observation model $p (z_{t} | x_{t}, m)$ $p (z_{t} | x_{t}, m)$ .

The prediction is obtained from the following equation:

$p (x_{t}, m | z_{0 : t - 1}, u_{0 : t - 1}, x_{0}) = \int p (x_{t} | x_{t - 1}, u_{t}) p (x_{t - 1}, m | z_{0 : t - 1}, u_{0 : t - 1}, x_{0}) d x_{t - 1}$ $p (x_{t}, m | z_{0 : t - 1}, u_{0 : t - 1}, x_{0}) = \int p (x_{t} | x_{t - 1}, u_{t}) p (x_{t - 1}, m | z_{0 : t - 1}, u_{0 : t - 1}, x_{0}) d x_{t - 1}$

(2.10)

and the measurement update is calculated using

$p (x_{t}, m | z_{0 : t}, u_{0 : t}, x_{0}) = \frac{p (z_{t} | x_{t}, m) p (x_{t}, m | z_{0 : t - 1}, u_{0 : t - 1}, x_{0})}{p (z_{k} | z_{t - 1}, U_{0 : t})} .$ $p (x_{t}, m | z_{0 : t}, u_{0 : t}, x_{0}) = \frac{p (z_{t} | x_{t}, m) p (x_{t}, m | z_{0 : t - 1}, u_{0 : t - 1}, x_{0})}{p (z_{k} | z_{t - 1}, U_{0 : t})} .$

si64_e (2.11)

The solution to a probabilistic SLAM problem involves an efficient and consistent computation of the prediction and update equations. Based on different estimation frameworks for performing these computations, there are different variants of the SLAM algorithm (Thrun, 2002; Chen et al., 2007; Aulinas et al., 2008). The Grid-based Fast SLAM algorithm is a variant that applies particle filtering methods to solve the SLAM problem (Thrun et al., 2005). Grid-based Fast SLAM incrementally processes the observations and odometry readings as they are available and updates the set of samples representing the posterior about the map and trajectory of the robot. The steps of the Fast SLAM based on PF are summarized as follows:

Initialization: At time $t = 0$ $t = 0$ , sample ${\{x_{0}^{i}\}}_{i = 1}^{N}$ ${\{x_{0}^{i}\}}_{i = 1}^{N}$ from the initial density function p(x₀).

Sampling: Based of the state of the system at time $t - 1$ $t - 1$ represented by ${\{x_{t - 1}^{i}, w_{t - 1}^{i}\}}_{i = 1}^{N}$ ${\{x_{t - 1}^{i}, w_{t - 1}^{i}\}}_{i = 1}^{N}$ and the observation at z_t, propose a set of new particles ${\{x_{t}^{i}\}}_{i = 1}^{N}$ ${\{x_{t}^{i}\}}_{i = 1}^{N}$ from the proposal function $q (x_{t} | x_{t - 1}, z_{t})$ $q (x_{t} | x_{t - 1}, z_{t})$ . The motion model of the robot is often used as the proposal.

Importance weight: An individual weight w_tⁱ is assigned for each proposed particle ${\{x_{t}^{i}\}}_{i = 1}^{N}$ ${\{x_{t}^{i}\}}_{i = 1}^{N}$ . The weight of each particle is proportional to the likelihood $p (z_{t} | x_{t}, m)$ $p (z_{t} | x_{t}, m)$ of the most recent observation given the map m associated with the particle and the corresponding pose x_t.

Resampling: This step involves sampling new set of particles from the set ${\{x_{t}^{i}, w_{t}^{i}\}}_{i = 1}^{N}$ ${\{x_{t}^{i}, w_{t}^{i}\}}_{i = 1}^{N}$ according to the weights of the particles; i.e., particles with larger weights are replicated, while lighter particles are discarded. The resulting particle set is then used in the next time step to predict the posterior probability. Resampling helps to improve the quality of the estimation by reducing the variance of the particles. In this step, different resampling algorithms (Douc and Cappe, 2005) can be used, as they are not application-specific algorithms. The specific resampling method adopted in this study is the Independent Metropolis Hasting (IMHA) resampling algorithm. This algorithm has the lowest computational complexity of most conventional resampling schemes like systematic resampling (Sileshi et al., 2013). As a result of these interesting characteristics, it is preferred for this work.

Map estimation: For each particle, the corresponding map estimate is updated based on the observation and the pose represented by that particle.

3 Computational bottleneck identification and hardware/software partitioning

In tackling the speed-up of the overall computation in PFs, a preliminary study of the identification of the critical bottlenecks of the algorithm is crucial for the design of hardware modules in order to accelerate the computational bottleneck steps. The study is based on the Grid-based Fast SLAM algorithm discussed in section 2. For an embedded implementation of the Grid-based Fast SLAM algorithm on the FPGA platform, an accurate analysis of the timing required in each step of the PF is obtained in order to identify the computational bottlenecks in each step of the PF.

Figure 2.1 summarizes the critical computational bottlenecks obtained from such preliminary study. In the sampling step, the computation of sine and cosine functions and Gaussian random number generator (GRNG) accounts to 45.91% and 53.62% of its execution time. In the importance weight step, the computation of sine and cosine, and exponential functions contribute 75.34% and 7.65% of the execution time, respectively. For the resampling step, the generation of uniform random numbers accounts for most of the execution time (i.e., 60.71%). For each step of the PF computations shown in Figure 2.1, “others” corresponds to other related computations involved in each step.

f02-01-9780128025086 — Figure 2.1 Computational bottlenecks identifications in the sampling (s), importance weight (I), and resampling (R) steps of the PF in the Grid-based Fast SLAM application.

The profiling information of the PF given in Figure 2.1 can used in the hardware/software partitioning; i.e., which parts of the algorithm should be implemented in hardware and which ones can be kept in software (running on the FPGA’s embedded processor). It is clear from Figure 2.1 that the sine and cosine functions and random number generation are the critical bottlenecks of the Grid-based Fast SLAM algorithm, thus requiring acceleration with a hardware implementation.

The techniques used in the speed-up of the computational bottleneck steps of the PF in the Grid-based Fast SLAM application are based on the use of the CORDIC algorithm (Volder, 1959) for the evaluations of the sine, cosine, and exponential functions, and the Ziggurat (Marsaglia and Tsang, 2000) and Tausworthe (Ecuyer, 1996) algorithms for generation of Gaussian and uniform random numbers, respectively. The details of these techniques are provided in section 4, and the respective hardware designs are given in section 5.

4 PF acceleration techniques

4.1 CORDIC acceleration technique

CORDIC is an iterative algorithm capable of evaluating many basic arithmetic operations and mathematical functions (Aulinas et al., 2008). It involves the rotation of a vector $v = (x, y)$ $v = (x, y)$ with an angle z in circular, linear, and hyperbolic coordinate systems depending on the functions to be evaluated. Using shift and add operations, it performs a rotation iteratively using a series of specific incremental rotation angles. It can operate in one of three configurations (circular, linear, or hyperbolic) and two different modes (rotation or vectoring). In rotation mode, it performs a general rotation by a given angle z, and in vectoring mode, it computes the unknown angle z of a vector by performing a finite number of microrotations.

The unified CORDIC iteration equations for evaluation of a wide range of functions are given by Eq. (2.12) (Walther, 1971; Lakshmi and Dhar, 2010):

$\begin{array}{l} x_{i + 1} = x_{i} - μ d_{i} y_{i} 2^{- i} \\ y_{i + 1} = y_{i} + d_{i} x_{i} 2^{- i} \\ z_{i + 1} = z_{i} - d_{i} e_{i,} \end{array}$ $\begin{array}{l} x_{i + 1} = x_{i} - μ d_{i} y_{i} 2^{- i} \\ y_{i + 1} = y_{i} + d_{i} x_{i} 2^{- i} \\ z_{i + 1} = z_{i} - d_{i} e_{i,} \end{array}$

si75_e (2.12)

where (x, y) defines a vector with angle z; μ defines a circular ( $μ = 1$ $μ = 1$ ), linear ( $μ = 0$ $μ = 0$ ), or hyperbolic ( $μ = - 1$ $μ = - 1$ ) coordinate system; d_i represents either a clockwise or counterclockwise direction of rotation in rotation mode $d_{i} = sing (z_{i})$ $d_{i} = sing (z_{i})$ and vectoring mode $d_{i} = - sing (y_{i})$ $d_{i} = - sing (y_{i})$ ; and e_i defines a table of constant values in circular ( $e_{i} = {tan}^{- 1} 2^{- i}$ $e_{i} = {tan}^{- 1} 2^{- i}$ ), linear ( $e_{i} = 2^{- i}$ $e_{i} = 2^{- i}$ ), and hyperbolic ( $e_{i} = {tanh}^{- 1} 2^{- i}$ $e_{i} = {tanh}^{- 1} 2^{- i}$ ) configurations. With the unified CORDIC iteration equations, a wide range of functions can be evaluated. However, the discussion is focused to the evaluation of the different functions involved in the PF steps for Grid-based Fast SLAM application and the Ziggurat Gaussian random-number generator algorithm. These include the sine, cosine, exponential, and natural logarithm functions. The configurations to the variables x, y, and z, and the set of equations that the unified CORDIC iteration equations converges after i iterations in rotation and vectoring modes is given in Table 2.1, where, in circular-rotation mode of configuration, the functions f₁ and f₂ correspond to the sine () and cosine () functions, respectively; and for hyperbolic-rotation configuration, f₁ and f₂ correspond to the sinh () and cosh () functions. In vectoring-hyperbolic configuration, the function f₃ corresponds to $\tan h^{- 1}$ $\tan h^{- 1}$ .

Table 2.1

Configuration to CORDIC for Evaluation of Different Functions

	Circular	Hyperbolic
Rotation $x = K (x f_{1} z - y f_{2} z)$ $x = K (x f_{1} z - y f_{2} z)$ $y = K (y f_{1} z - x f_{2} z)$ $y = K (y f_{1} z - x f_{2} z)$ $z = 0$ $z = 0$	Sine/Cosine $x = 1 / K, y = 0$ $x = 1 / K, y = 0$ and z as input, $K = 1.646$ $K = 1.646$	Exponential $x = 1 / K, y = 0$ $x = 1 / K, y = 0$ and z as input $K = 0.828159$ $K = 0.828159$
Vectoring $x = K \sqrt{x^{2} - y^{2}}$ $x = K \sqrt{x^{2} - y^{2}}$ $y = 0$ $y = 0$ $z = z + f_{3} (\frac{y}{x})$ $z = z + f_{3} (\frac{y}{x})$	-	Logarithmic $x = 1, z = 0$ $x = 1, z = 0$ and y as input

The evaluation of the sine and cosine functions is obtained directly by applying the properties given in Table 2.1; however, for the evaluation of exponential and natural logarithm functions, indirect properties are used. The evaluation of the exponential function is obtained indirectly by applying the following property:

$\exp (z) = \sinh (z) + \cosh (z) .$ $\exp (z) = \sinh (z) + \cosh (z) .$

(2.13)

Similarly, in the case of the natural logarithm function, the property given by the following equation is used for its indirect evaluation:

$\ln w = 2 \tan h^{- 1} (\frac{y}{x}),$ $\ln w = 2 \tan h^{- 1} (\frac{y}{x}),$

(2.14)

where, $x = w + 1$ $x = w + 1$ and $y = w - 1$ $y = w - 1$ .

As the CORDIC algorithm works for a limited range of the input arguments for the evaluation of the elementary functions, it is required to extend the range of the inputs for each mode of operation by applying proper prescaling identities. This is achieved by dividing the original input arguments to the CORDIC algorithm by a constant to obtain a quotient Q and remainder D (Boudabous et al., 2004). In the case of the sine and cosine functions, the constant value corresponds to $\frac{π}{2}$ $\frac{π}{2}$ , and log_e2 for the exponential and logarithmic functions. The prescaling identities for all the required functions are given in Table 2.2.

Table 2.2

Prescaling Identities for Function Evaluations

Identity	Domain
$sin (Q \frac{π}{2} + D) = \{\begin{array}{c} sin D if Q \mod 4 = 0 \\ cos D if Q \mod 4 = 1 \\ - sin D if Q \mod 4 = 2 \\ - cos D if Q \mod 4 = 3 \end{array}\}$ $sin (Q \frac{π}{2} + D) = \{\begin{array}{c} sin D if Q \mod 4 = 0 \\ cos D if Q \mod 4 = 1 \\ - sin D if Q \mod 4 = 2 \\ - cos D if Q \mod 4 = 3 \end{array}\}$	$\|D\| < \frac{π}{2}$ $\|D\| < \frac{π}{2}$
$cos (Q \frac{π}{2} + D) = \{\begin{array}{c} cos D if Q \mod 4 = 0 \\ - sin D if Q \mod 4 = 1 \\ - cos D if Q \mod 4 = 2 \\ sin D if Q \mod 4 = 3 \end{array}\}$ $cos (Q \frac{π}{2} + D) = \{\begin{array}{c} cos D if Q \mod 4 = 0 \\ - sin D if Q \mod 4 = 1 \\ - cos D if Q \mod 4 = 2 \\ sin D if Q \mod 4 = 3 \end{array}\}$	$\|D\| < \frac{π}{2}$ $\|D\| < \frac{π}{2}$
$exp (Q l o g_{e} 2 + D) = 2^{Q} (cosh D + sinh D)$ $exp (Q l o g_{e} 2 + D) = 2^{Q} (cosh D + sinh D)$	$\|D\| < l o g_{e} 2$ $\|D\| < l o g_{e} 2$
$l o g_{e} (M 2^{M}) = l o g_{e} (M) + E l o g_{e} 2$ $l o g_{e} (M 2^{M}) = l o g_{e} (M) + E l o g_{e} 2$	$0.5 \leq M < 1.0$ $0.5 \leq M < 1.0$

Source: Walther (1971); Lakshmi and Dhar, 2010; Boudabous et al. (2004).

4.2 Ziggurat acceleration technique

The sampling step of the PF for the Grid-based Fast SLAM application requires the generation of normal random numbers. For fast generation of such random numbers in this study, a Ziggurat algorithm was used. The Ziggurat algorithm is an efficient method for generating normally distributed random variates from an arbitrary decreasing probability density function $y = f (x)$ $y = f (x)$ by applying the acceptance-rejection method. This method involves conditionality in accepting correct random values that are part of the target distribution and rejecting incorrect ones.

The Ziggurat technique partitions the standard normal density function $(x) = exp (- 0.5 x^{2}),$ $(x) = exp (- 0.5 x^{2}),$ $x > 0$ $x > 0$ , into n horizontal rectangular blocks R_i, where $i = 0, 1, 2, \dots, (n - 1)$ $i = 0, 1, 2, \dots, (n - 1)$ , extending horizontally from $x = 0$ $x = 0$ to x_i and vertically from f(x_i) to $f (x_{i - 1})$ $f (x_{i - 1})$ . The bottom of the block consists of a rectangular area joined with the remainder of the density starting from a point r (Figure 2.2). All the rectangular blocks have equal area v, given by

$v = x_{i} [f (x_{i - 1}) - f (x_{i})] = r f (r) + \int_{r}^{\infty} f (x) d x .$ $v = x_{i} [f (x_{i - 1}) - f (x_{i})] = r f (r) + \int_{r}^{\infty} f (x) d x .$

si96_e (2.15)

f02-02-9780128025086 — Figure 2.2 Partitioning of a Gaussian distribution with the Ziggurat method.

The description of the Ziggurat algorithm based on Doornik (2005) for generating normally distributed random variates by partitioning of the normal density distribution, and with the application of the acceptance-rejection method, is given by

Input: ${\{x_{i}\}}_{i = 0}^{n - 1}$ ${\{x_{i}\}}_{i = 0}^{n - 1}$

Output: normal random number x.

Draw an index i of a rectangle ( $0 \leq i \leq n - 1$ $0 \leq i \leq n - 1$ ) at random with probability 1/n.

Draw a random number x from the rectangle i as $x = U_{0} x_{i}$ $x = U_{0} x_{i}$ , where U₀ is a uniform random number drawn from a uniform distribution U(0, 1).

Rectangular:

IF $i \geq 1$ $i \geq 1$ and $x < x_{i - 1}$ $x < x_{i - 1}$ accept x.

Tail:

IF $i = 0$ $i = 0$ , accept x from the tail.

Wedge:

ELSE IF $i > 0$ $i > 0$ and $U_{1} |f (x_{i - 1}) - f (x_{i})| < f (x) - f (x_{i})$ $U_{1} |f (x_{i - 1}) - f (x_{i})| < f (x) - f (x_{i})$ accept x, where U₁ is a uniform random number.

RETURN to step 1

where, in line 4, the method to generate the normal random numbers from the tail of the distribution is obtained as follows:

Generate i.i.d. uniform (0, 1) variates u₀ and u₁

$x \leftarrow - ln (u_{0}) / r$ $x \leftarrow - ln (u_{0}) / r$ , $y \leftarrow - ln (u_{1})$ $y \leftarrow - ln (u_{1})$

WHILE ( $y + y) < x^{2}$ $y + y) < x^{2}$

RETURN $x > 0 ? (r + x) : - (r + x)$ $x > 0 ? (r + x) : - (r + x)$

The Ziggurat algorithm requires a table of x_i points and their corresponding function values f_i. The number of rectangular blocks n is normally considered as a power of 2 (64, 128, 256, …), and for $n = 128$ $n = 128$ , a value of $r = 3.442619855899$ $r = 3.442619855899$ is used to determine the x_i points that are required in the hardware realization of the algorithm (Marsaglia and Tsang, 2000). The uniform random numbers U₀ and U₁ required in the Ziggurat algorithm are generated with a Tausworthe Uniform Random Number Generator (URNG). For evaluating the exponential and natural logarithmic functions in the wedge (line 5) and tail regions (line 4), respectively, a CORDIC algorithm was used.

5 Hardware implementation

This section describes the hardware designs for the PF acceleration techniques listed in section 4 for Grid-based Fast SLAM applications. The hardware design for the CORDIC acceleration technique is based on a CORDIC hardware module (shown in Figure 2.3) composed of three hardware computational submodules: CORDIC core, prescaling, and postscaling. The implementation of these submodules is based on the unified CORDIC iteration equations given in section 4 for the CORDIC-core submodule, and the identity equations given in Table 2.2 for the prescaling and postscaling submodules. The identity equations are used by the prescaling and postscaling modules to extend the range of input arguments while evaluating the different functions. The evaluation of the different functions by a single CORDIC module is required to avoid the use of multiple instances of the CORDIC module and consequently save the additional resource requirements. This is achieved by providing a runtime configuration to the CORDIC module through its Config port to configure the module to a specific function evaluation.

f02-03-9780128025086 — Figure 2.3 Architecture of the CORDIC module.

A serial CORDIC algorithm with 24-bit accuracy is implemented in the CORDIC core shown in Figure 2.3. In the input/output interfaces for the CORDIC module shown in Figure 2.3, one of the two possible inputs U₀ and U₁ is evaluated for a specific function, one at a time. The configuration to a specific function evaluations is provided by the configuration bits at the Config input port, where, depending on these bits, a specific function is evaluated and the result is provided to the output interface f (). The functions f () that can be obtained at the output are the sin (), cos (), exp () and ln () functions.

The hardware architecture for the generation of both uniform and Gaussian random numbers is given in Figure 2.4. The three hardware submodules involved in Figure 2.4 are the Ziggurat NRNG submodule for generation of normally distributed random numbers; the Tausworthe submodule for the generation of two uniform random numbers (U₀ and U₁) that are used in the Ziggurat NRNG submodule and resampling step of the PF; and the CORDIC submodule for evaluating the complex functions required by the Ziggurat NRNG submodule. The CORDIC submodule implements the architecture presented in Figure 2.3. In the hardware architecture of the Ziggurat NRNG submodule, independently working individual hardware blocks for the generation of normal random variates from the rectangular, tail, and wedge regions are implemented. The computation of the exponential and natural logarithmic functions while normal variates are generated from the wedge and tail regions of the distribution, respectively, is achieved with the CORDIC algorithm.

f02-04-9780128025086 — Figure 2.4 Hardware architecture for Gaussian and uniform random number generators

To further speed up the normal random number generation process in the Ziggurat NRNG submodule, an effective mechanism for the simultaneous access to the coefficients x_i and f_i is required. This is achieved by dividing the random index i in to even and odd values, and storing the respective x_i and f_i in separate memories (Figure 2.5). While the generated index i is an odd value, x_i and $x_{i - 1}$ $x_{i - 1}$ are read from the odd and even memories at the same memory index positions simultaneously. However, if the generated index is with an even value, then x_i is read from the even memory and $x_{i - 1}$ $x_{i - 1}$ is read from the odd memory at the next memory position in parallel. As a result, parallel access of the coefficients is achieved.

f02-05-9780128025086 — Figure 2.5 Illustration of the storage of the x_i and f_i coefficients in memory (MEM) and the parallel access of the x_i, $x_{i - 1}$ $x_{i - 1}$ , f_i and $f_{i - 1}$ $f_{i - 1}$ for n = 128.

6 Hardware/software Architecture

The global architecture of the proposed system for the PF in a grid based Fast SLAM algorithm is given in Figure 2.6. It comprises of an embedded Microblaze processor responsible for the execution of software functions, the PF hardware accelerator (PF HW accelerator) and, timer and UART cores with the purpose to help the analysis and verification of the system. The PF HW accelerator contains the CORDIC and the random number generator cores explained in section V, and they are connected to the Microblaze soft-core processor through a dedicated one-to- one communication bus (Fast simplex Link) for fast streaming of data.

f02-06-9780128025086 — Figure 2.6 Global architecture of the hardware/software co-design system for PF.

To test the Grid-based Fast SLAM algorithm, real-time odometry and laser data collected from a mobile robot platform interfaces with random access memory (RAM) in order to synchronize data for processing. The odometry data are used in the sampling step to generate new particle instances, and the laser data are used in the importance weight step for the evaluation of particle weights. The particles and their associated weights are buffered to Block RAM for fast accessing. Individual CORDIC hardware modules are assigned to evaluate the functions in the sampling and importance weight steps. Uniform and Gaussian random numbers are provided to the resampling and sampling steps of the PF, respectively, by the random number generator module.

7 Results and discussion

The implementation of the architecture shown in Figure 2.6 is performed on a Xilinx Kintex-7 KC705 FPGA device running at 100 MHz. The design of the hardware modules is written in VHSIC Hardware Description Language (VHDL). For the implementation of the Ziggurat NRNG module, a value of $n = 128$ $n = 128$ is used and, the x_i and f_i coefficients are represented as fixed-point numbers with Q_8 : 24 format (i.e., 8 bits for the integer part and 24 bits for the fractional part). The same fixed-point format is used for the representation of the data in the CORDIC module design. For the different variables in the software part of the algorithm, a 32-bit floating-point representation is used by enabling the floating-point unit (FPU) of the MicroBlaze processor.

Figure 2.7 summarizes the execution time given by the number of clock cycles for each step of the PF in the Grid-based Fast SLAM algorithm. The results show that the hardware acceleration leads to greater speed in the execution time of the PF in all three steps of the algorithm. In particular, a significant speed-up is achieved in the sampling step, which can be attributed to the fast generation of Gaussian random numbers by the random number generator hardware module. Compared to the other steps, the importance weight step shows a relatively higher clock cycle, as shown in Figure 2.7. This is because for the evaluation of the weight of each particle in the importance weight step, first it is required to transform every laser scan measurement point of data (range and bearing angle) from a robot frame of reference to a global frame of reference, where normally more than hundreds of laser scan points have to be evaluated from the sensor. This requires the evaluation of the sine and cosine functions for every scan point. After this step comes a search for the closest point between every laser scan end point and occupied points in a map (which is a computationally intensive step). Though at this stage, this step is implemented in software in the embedded MicroBlaze processor, it is intended to speed up this process in hardware. Furthermore, the computation of an exponential function is required in the calculation of the weight of each particle for every laser scan points. These are the main reasons for the relatively large clock cycles obtained in the importance weight step.

f02-07-9780128025086 — Figure 2.7 Comparison on execution times for the sampling (S), importance weight (I), and resampling (R) steps for embedded software implementation (SW) versus hardware/software (HW/SW)–embedded implementation and the corresponding increase in execution time.

8 Conclusions

This chapter presented techniques aimed to minimize the huge computational demand in PFs applied to a Grid-based Fast SLAM application. With initial identification of the computational bottlenecks in each step of the algorithm, techniques are proposed in order to tackle the computational bottlenecks of the algorithm. Based on the proposed techniques, and with a hardware/software codesign approach, hardware acceleration blocks are designed and implemented to speed up the computational time. The presented approach leads to an improvement in the speedup of 140 ×, 14.87 ×, and 19.36 × in the sampling, importance weight and resampling steps, respectively (Figure 2.7). Due to the flexibility in the hardware/software codesign and the presented approach, the acceleration in the computational time of other real-time particle filtering applications can be easily adopted following the approach presented here.

..................Content has been hidden....................

You can't read the all page of ebook, please click here login for view all page.

Table of Contents for Chapter 2: Accelerating Techniques for Particle Filter Implementations on FPGA

Create new playlist

Sign In

Sign Up