# Pipeline Gaussian Particle Filter and Hardware Design for Attitude Estimation with UAV

Authors: Yali Xue; Hu Chen; Jie Chen; Jiahui Wang
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

This paper based on the Gaussian particle filter (GPF) deals with the attitude estimation of UAV. GPF algorithm has better estimation accuracy than the general nonlinear non-Gaussian state estimation and is usually used to improve the system’s real-time performance whose noise is specific such as Gaussian noise during the mini UAV positioning and navigation. The attitude estimation algorithm is implemented on FPGA to verify the effectiveness of the Gaussian particle filter. Simulation results have illustrated that the GPF algorithm is effective and has better real-time performance than that of the particle filter.


---

## Body

## 1. Introduction

Mini UAV (unmanned aerial vehicle) has got a rapid development in the military and civilian fields for their low cost, rescue, and geographic surveying [1]. UAV has a high requirement on accurate and real-time positioning and navigation because of flight safety and reliability. Fortunately, GPF algorithm has better estimation accuracy than the general nonlinear non-Gaussian state estimation and is usually used to improve the system’s real-time performance whose noise is specific such as Gaussian noise during the mini UAV positioning and navigation. The attitude estimation of UAV is usually estimated by nonlinear filtering that many scholars are committed to study such as FLANN filter [2], Volterra filter [3], neural network filter [4], and Affine projection algorithm [5]. GPF is a particle filter which is based on a Gaussian filter framework and is suitable for parallel implementation since it does not require resampling. [6]. It assumes the probability density function of the estimated state quantity as the multidimensional Gaussian distribution. Furthermore, the Gaussian particle filter is used to solve Gaussian distribution-related parameters.In the face of UAV attitude estimation problem, we can assume that the process noise and the observation noise are both Gaussian noise and can be processed by GPF. This paper applies GPF to the attitude estimation of UAV, which has the following advantages: First, GPF needs to sample particles from the Gaussian distribution so that the particles are diverse. Second, the approximate expected probability density of the GPF algorithm is Gaussian, so there is not the problem of particle depletion and degradation in the standard particle filter. Here, the resampling step is unnecessary. Third, GPF has not only the asymptotic optimality but also a simple logic structure which is easy to be implemented in hardware. All these advantages have brought more attention to GPF.


## 2. Model Equations

This section will introduce the attitude and velocity solution described by the quaternion method first. The geodetic coordinate system (east-north-sky) is used as the navigation frame, and the aircraft is taken as the research object. The measured data of the inertial measurement unit (IMU) and the initial attitude of the aircraft will be given. Real-time attitude and velocity estimation will be run on every time period. The quaternion descriptionωgff is the angular velocity of the geodetic frame to the body-fixed frame under the body-fixed frame. Together with the quaternion Λ corresponding to the rotation matrix Cgf, they can be expressed by the kinematics differential equation as(1)Λ˙=12Λ∘ωgff.Based on quaternion multiplication, equation (1) can be written as(2)QΛ˙=12MΛQωgff=12M∗ωgffQΛ,where(3)λ˙0λ˙1λ˙2λ˙3=12×0−ωgfxf−ωgfyf−ωgfzfωgfxf0ωgfzf−ωgfyfωgfyf−ωgfzf0ωgfxfωgfzfωgfyf−ωgfxf0λ0λ1λ2λ3.λ0∼λ3 are the elements of Λ. For equation (3), it can be discretized as(4)QΛk−QΛk−1=Δt2M∗ωgffk−1QΛk−1.Therefore, the updating equation of discrete state quaternion can be expressed as(5)QΛk=I4×4+Δt2M∗ωgfgk−1QΛk−1.In strapdown inertial navigation system (SINS), the specific force equation can be calculated from the following equation:(6)v˙c=fc+gc,where vc is the velocity vector based on the navigation frame fcis the accelerometer output which is transformed by Cfg, and gc is the local gravitational acceleration. Similarly, the force equation can be rewritten as the following discrete expression:(7)vkc−vk−1c=CΛk−1fgak−1f+gcΔt.


## 3. Gaussian Particle Filter

The basic theory of GPF is not given in this section and has been discussed in [7]. The algorithm pseudocode of the generic Gaussian particle filter is shown in Pseudocode 1.Pseudocode 1: Generic Gaussian particle filter algorithm.
Input: observationyk, previous estimation μk−1, and Cholesky decomposition Ck−1 of covariance Σk−1;Initial: meanμ0 and Σ0 from initial estimationMethod:(1) Drawing conditioning particles fromNxk−1;μk−1,Σk−1 to obtain xk−1ii=1M(2) Drawing particles: draw particles frompxkxk=xk−1i to obtain xkii=1M(3) (a) Weight calculationw˜ni=pykxki(b)Weight standardizationwki=w˜ki/∑i=1Nw˜ki(4) Updating mean and covariance by(a)μk=∑i=1Nwkixki(b)Σk=∑i=1Nwkixki−μkxki−μkT(5) Cholesky decompositionCk−1 calculation Σk=CkCkTFor the particle filter-based attitude estimation algorithm proposed in this section, the state of theith particle is selected as(8)xki=qki,vkig,where qki is the attitude described by the quaternion and vkig is the velocity vector under geodetic frame. In order to obtain a suitable approximation of the target distribution, the following conditions are necessary:(i)
The probability distribution of the initial state of the systempx0(ii)
A proposal distributionqxkxk−1,yk,uk for sampling particles(iii)
For a given statexk, a method for calculating observations ykFor the designed particle filter, it is mainly divided into two steps: prediction and update.


### 3.1. Prediction Step of Particle Filter

Supposepx0 is known, then an approximate distribution of the target can be obtained by sampling the particles as follows:(9)xki∼qxkxk−1i,yk,uk,i=1,…,N,where the proposal distribution is(10)qxkxk−1i,yk,uk=pxkxk−1i.Equations (6) and (8) already give out the quaternion-based attitude and velocity solution for discrete equations, use it as sampling strategy, and are rewritten for easy understanding:(11)qki=I4×4+Δt2Ωωk−1ifqk−1i+wqi,vkig=vk−1ig+Cqk−1ifgak−1f+wai+gΔt+wvi,where I4×4 is the unit matrix and Cqk−1ifg, Ωωk−1if is the matrix as(12)Ωωk−1if=0−ωxk−1if−ωyk−1if−ωzk−1ifωxk−1if0ωzk−1if−ωyk−1ifωyk−1if−ωzk−1i0ωxk−1ifωzk−1ifωyk−1if−ωxk−1if0,where ωk−1if=ωk−1f+wωi and •× represents matrix multiplication. The filter inputs ωk−1f and ak−1f are the triaxial angular velocity under the body-fixed frame and specific force, respectively, and wωi, wqi, wai, and wvi are obtained from the proposal distribution to express the uncertainty in the prediction step. The uncertainty of the prediction step usually includes the following two cases:(a)
The uncertainty of filter input:ωk−1f and ak−1fare always with noises, and the noises belong to wai∼N0,σa2 and wωi∼N0,σω2.(b)
The numerical integration error and the small angle approximation error: the simplification and approximation to the quaternion updating equation cause some errors in equations (5) and (7), and the error belongs to wqi∼N0,σq2 and wvi∼N0,σv2.


### 3.2. Updating Step of the Velocity Weight

In this updating step, the GPS velocity output is used to update the velocity weight. Assume that the discrete approximationpxky1:k,u1:k is only relevant to the current moment of the measurement yk. By using the following equation, the updating weight can be recured:(13)wki=wk−1ipykxki.Here, the observation yk is obtained by GPS velocity vector v˜kg. The likelihood function is selected as(14)pv˜kgvkig=1σgv2πexp−12v˜kg−vkigσgv2,where σgv is the standard deviation of v˜kg.


### 3.3. State Estimation for the UAV’s Attitude and Velocity

The state estimationx^k=q^k,v^kg is obtained from weighted average to particle sets. We choose the number of particles as N:(15)v^kg=∑i=1Nwkivkig,q^k=1∑i=1Nwkiqki∑i=1Nwkiqki.It is worth noting that the quaternion rotation cannot be averaged because it is not in the same vector space. Furthermore, Gramkow has proved that the quaternion mean from calculating the minimum variance exactly corresponds to the weighted average given by equation (16). And when the rotation angle is less than 40 degrees, the relative error is less than 1% [8].


### 3.4. Way to Calculate Covariance

Markley has proved that the covariance matrix can also be calculated without loss of generality for nonstandardized quaternions [9]. Since the real quaternion cannot be obtained, the quaternion covariance matrix is represented by the quaternion state of the single particle and the estimated quaternion as shown in equation (17):(16)Σk=∑k=1Mwkmqki−q^kqki−q^kT.The attitude estimation algorithm based on GPF is given as shown in Pseudocode2.Pseudocode 2: An attitude estimation algorithm based on pipeline GPF.
Input: particlexk−1ij=1N, IMU output ωk−1if and ak−1f GPS, out v˜kg, and Cholesky decomposition Ck−1 of covariance Σk−1:Σk−1=Ck−1Ck−1TOutput: attitude and velocityq^k and covariance v˜kg(1) Sampling conditioning particles:qk−1i=q^k−1+Ck−1wci(2) Sampling particles:qki=I4×4+Δt/2Ωωk−1ifqk−1i+wqivkig=vk−1ig+Cqk−1ifgak−1f+wai+gΔt+wvi(3) (a) Weight calculation:wki=wk−1i/σgv2πexp−1/2v˜kg−vkig/σgv2(b) Weight standardization:wki=wki/Wk(4) (a) State estimation:v^kg=∑i=1Nwkivkigq^k=1/∑i=1Nwkiqki∑i=1Nwkiqki(b) Covariance calculation:Σk=∑k=1Mwkiqki−q^kqki−q^kT(5) Cholesky decompositionCk calculation: Σk=CkCkT


## 3.1. Prediction Step of Particle Filter

Supposepx0 is known, then an approximate distribution of the target can be obtained by sampling the particles as follows:(9)xki∼qxkxk−1i,yk,uk,i=1,…,N,where the proposal distribution is(10)qxkxk−1i,yk,uk=pxkxk−1i.Equations (6) and (8) already give out the quaternion-based attitude and velocity solution for discrete equations, use it as sampling strategy, and are rewritten for easy understanding:(11)qki=I4×4+Δt2Ωωk−1ifqk−1i+wqi,vkig=vk−1ig+Cqk−1ifgak−1f+wai+gΔt+wvi,where I4×4 is the unit matrix and Cqk−1ifg, Ωωk−1if is the matrix as(12)Ωωk−1if=0−ωxk−1if−ωyk−1if−ωzk−1ifωxk−1if0ωzk−1if−ωyk−1ifωyk−1if−ωzk−1i0ωxk−1ifωzk−1ifωyk−1if−ωxk−1if0,where ωk−1if=ωk−1f+wωi and •× represents matrix multiplication. The filter inputs ωk−1f and ak−1f are the triaxial angular velocity under the body-fixed frame and specific force, respectively, and wωi, wqi, wai, and wvi are obtained from the proposal distribution to express the uncertainty in the prediction step. The uncertainty of the prediction step usually includes the following two cases:(a)
The uncertainty of filter input:ωk−1f and ak−1fare always with noises, and the noises belong to wai∼N0,σa2 and wωi∼N0,σω2.(b)
The numerical integration error and the small angle approximation error: the simplification and approximation to the quaternion updating equation cause some errors in equations (5) and (7), and the error belongs to wqi∼N0,σq2 and wvi∼N0,σv2.


## 3.2. Updating Step of the Velocity Weight

In this updating step, the GPS velocity output is used to update the velocity weight. Assume that the discrete approximationpxky1:k,u1:k is only relevant to the current moment of the measurement yk. By using the following equation, the updating weight can be recured:(13)wki=wk−1ipykxki.Here, the observation yk is obtained by GPS velocity vector v˜kg. The likelihood function is selected as(14)pv˜kgvkig=1σgv2πexp−12v˜kg−vkigσgv2,where σgv is the standard deviation of v˜kg.


## 3.3. State Estimation for the UAV’s Attitude and Velocity

The state estimationx^k=q^k,v^kg is obtained from weighted average to particle sets. We choose the number of particles as N:(15)v^kg=∑i=1Nwkivkig,q^k=1∑i=1Nwkiqki∑i=1Nwkiqki.It is worth noting that the quaternion rotation cannot be averaged because it is not in the same vector space. Furthermore, Gramkow has proved that the quaternion mean from calculating the minimum variance exactly corresponds to the weighted average given by equation (16). And when the rotation angle is less than 40 degrees, the relative error is less than 1% [8].


## 3.4. Way to Calculate Covariance

Markley has proved that the covariance matrix can also be calculated without loss of generality for nonstandardized quaternions [9]. Since the real quaternion cannot be obtained, the quaternion covariance matrix is represented by the quaternion state of the single particle and the estimated quaternion as shown in equation (17):(16)Σk=∑k=1Mwkmqki−q^kqki−q^kT.The attitude estimation algorithm based on GPF is given as shown in Pseudocode2.Pseudocode 2: An attitude estimation algorithm based on pipeline GPF.
Input: particlexk−1ij=1N, IMU output ωk−1if and ak−1f GPS, out v˜kg, and Cholesky decomposition Ck−1 of covariance Σk−1:Σk−1=Ck−1Ck−1TOutput: attitude and velocityq^k and covariance v˜kg(1) Sampling conditioning particles:qk−1i=q^k−1+Ck−1wci(2) Sampling particles:qki=I4×4+Δt/2Ωωk−1ifqk−1i+wqivkig=vk−1ig+Cqk−1ifgak−1f+wai+gΔt+wvi(3) (a) Weight calculation:wki=wk−1i/σgv2πexp−1/2v˜kg−vkig/σgv2(b) Weight standardization:wki=wki/Wk(4) (a) State estimation:v^kg=∑i=1Nwkivkigq^k=1/∑i=1Nwkiqki∑i=1Nwkiqki(b) Covariance calculation:Σk=∑k=1Mwkiqki−q^kqki−q^kT(5) Cholesky decompositionCk calculation: Σk=CkCkT


## 4. Hardware Implementation

In this section, the attitude estimation algorithm based on the pipeline Gaussian particle filter will be verified on the system generator and finally implemented on ZYNQ-7000 platform.The valueq^k is necessary for covariance calculation, and it can be obtained until the N particles finish the state estimation 4(a) step. In order to combine the covariance matrix calculation with the above steps, the covariance matrix can be modified such as in [10]:(17)Σk=1Wk∑k=1MwkmqkiqkiT−q^kq^kT.The Gaussian particle filter is modularly decomposed into the following five modules: control module, sampling conditioning particles module, sampling particles module, weight calculation module, mean covariance calculation module, and Cholesky decomposition module.The following mainly introduces the design of the sampling conditioning particle module, the mean and covariance calculation module, and the Cholesky decomposition module. Others have been realized in [11]. The particle filter modular flow diagram is shown in Figure 1, and the flowing timing diagram is given in Figure 2.Figure 1
Pipeline Gaussian particle filter algorithm for attitude estimation.Figure 2
Particle filter modular flowing timing diagram.


### 4.1. Sampling Conditioning Particles Module

Here, theq^k and Ck obtained from the mean and covariance calculation module are used and all multipliers are pipelined. These multipliers generate M particles in a pipeline of M + L1 clock cycles. This module requires 4 Gaussian random number generators while there are 16 delays in this module.


### 4.2. Mean and Covariance Calculation Module

The job of this module is to calculate the meanq^k and the covariance matrix ∑k. Ns2+3Ns/2 multipliers are enough for the mean and covariance calculation, where Ns is the dimension of the state equation as shown in Table 1. For quaternion attitude estimation, Ns=4 and 14 multipliers are required too. Since all outputs require their accumulations, 14 accumulations are also required at the same time.Table 1
Multipliers and accumulations on mean and covariance calculation.
Attitude mean calculationCovariance calculationTotalNsNs2+Ns/2Ns2+3Ns/2


### 4.3. Cholesky Decomposition Module

The square root and division operation are used to realize the Cholesky decomposition here. This module contains 4 square operations and 6 division operations. Their operations are all implemented by the CORDIC algorithm.After the above introduction, the final design can be obtained, and Table2 shows the resource utilization information.Table 2
ZYNQ-7000 resource utilization of attitude estimation algorithm based on GPF.
Hardware: Zynq-7000 (7z020clg484-1)ResourceUtilizationTotalUtilization rate %Slice LUTs107645320020.23Slice registers1676710640015.75Block RAMs61404.29DSPs8922040.45


## 4.1. Sampling Conditioning Particles Module

Here, theq^k and Ck obtained from the mean and covariance calculation module are used and all multipliers are pipelined. These multipliers generate M particles in a pipeline of M + L1 clock cycles. This module requires 4 Gaussian random number generators while there are 16 delays in this module.


## 4.2. Mean and Covariance Calculation Module

The job of this module is to calculate the meanq^k and the covariance matrix ∑k. Ns2+3Ns/2 multipliers are enough for the mean and covariance calculation, where Ns is the dimension of the state equation as shown in Table 1. For quaternion attitude estimation, Ns=4 and 14 multipliers are required too. Since all outputs require their accumulations, 14 accumulations are also required at the same time.Table 1
Multipliers and accumulations on mean and covariance calculation.
Attitude mean calculationCovariance calculationTotalNsNs2+Ns/2Ns2+3Ns/2


## 4.3. Cholesky Decomposition Module

The square root and division operation are used to realize the Cholesky decomposition here. This module contains 4 square operations and 6 division operations. Their operations are all implemented by the CORDIC algorithm.After the above introduction, the final design can be obtained, and Table2 shows the resource utilization information.Table 2
ZYNQ-7000 resource utilization of attitude estimation algorithm based on GPF.
Hardware: Zynq-7000 (7z020clg484-1)ResourceUtilizationTotalUtilization rate %Slice LUTs107645320020.23Slice registers1676710640015.75Block RAMs61404.29DSPs8922040.45


## 5. Simulation Analysis

In order to verify the effectiveness of the Gaussian particle filter, the attitude estimation algorithm is designed on the FPGA. And an existing UAV track model is adopted for simulation. Numerical values are represented by 24-bit Fixed-Point numbers. The quaternion consists of 1 sign bit, 3 integer bits, and 20 decimal bits, and it is recorded as FIX24_20. Similarly, the geography velocity is recorded as FIX24_13 and the weight is UFIX24_23. Filter parameter settings are shown in Table3. Simulation results represent the attitude output, attitude output error, velocity, and velocity error under geodetic frame based on FPGA platform, respectively, in Figures 3 and 4.Table 3
Gaussian particle filter parameter settings.
Filter parametersNσw,deg/sσa,m/s2σv,m/sNumeric20480.0080.070.02Figure 3
UAV attitude output and output error using Gaussian particle filter based on FPGA.Figure 4
UAV velocity output and velocity output error of GPF based on FPGA.The above simulation results (Figures3 and 4) show that the UAV attitude estimation algorithm of GPF based on FPGA is feasible and effective. While due to the setting of the fixed-point number and the approximate solution on the FPGA platform, the hardware simulation result is with certain errors. The errors can be only improved by a lot of board-level debugging work.


## 6. Conclusion

The resampling algorithm is very complex and not difficult for parallel implementation. So it is considered to apply the Gaussian particle filter for attitude estimation algorithm in this paper. The quaternion discrete equation is regarded as the state equation here. This algorithm consists of sample adjustment particles, sampled particles, weight calculation, mean covariance calculation, Cholesky decomposition five modules, and the quaternion “average” value and covariance matrix which are calculated by nonstandardized weights. An improved pipeline Gaussian particle filtering algorithm is deduced through rewriting the covariance matrix calculation formula. The final simulation shows the result effectiveness. The Gaussian particle filter presented in this paper is not limited to the application of UAV attitude estimation but also in providing the reference value and practical applications on other nonlinear estimation problems.



---

*Source: 7092526-2020-05-11.xml*