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.
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 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 , they can be expressed by the kinematics differential equation as
Based on quaternion multiplication, equation (1) can be written aswhere
are the elements of . For equation (3), it can be discretized as
Therefore, the updating equation of discrete state quaternion can be expressed as
In strapdown inertial navigation system (SINS), the specific force equation can be calculated from the following equation:where is the velocity vector based on the navigation frame is the accelerometer output which is transformed by , and is the local gravitational acceleration. Similarly, the force equation can be rewritten as the following discrete expression:
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.
|
For the particle filter-based attitude estimation algorithm proposed in this section, the state of the th particle is selected aswhere is the attitude described by the quaternion and 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 system (ii)A proposal distribution for sampling particles(iii)For a given state , a method for calculating observations
For the designed particle filter, it is mainly divided into two steps: prediction and update.
3.1. Prediction Step of Particle Filter
Suppose is known, then an approximate distribution of the target can be obtained by sampling the particles as follows:where the proposal distribution is
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:where is the unit matrix and , is the matrix aswhere and represents matrix multiplication. The filter inputs and are the triaxial angular velocity under the body-fixed frame and specific force, respectively, and , , , and 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: and are always with noises, and the noises belong to and .(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 and .
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 approximation is only relevant to the current moment of the measurement . By using the following equation, the updating weight can be recured:Here, the observation is obtained by GPS velocity vector . The likelihood function is selected aswhere is the standard deviation of .
3.3. State Estimation for the UAV’s Attitude and Velocity
The state estimation is obtained from weighted average to particle sets. We choose the number of particles as N:
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):
The attitude estimation algorithm based on GPF is given as shown in Pseudocode 2.
|
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 value 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]:
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.
4.1. Sampling Conditioning Particles Module
Here, the and 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 mean and the covariance matrix . multipliers are enough for the mean and covariance calculation, where is the dimension of the state equation as shown in Table 1. For quaternion attitude estimation, and 14 multipliers are required too. Since all outputs require their accumulations, 14 accumulations are also required at the same time.
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 Table 2 shows the resource utilization information.
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 Table 3. 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.
The above simulation results (Figures 3 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.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported in part by the National Science Foundation of China (Grant no. 61922042).