基于流水线高斯粒子滤波的无人机姿态估计算法及FPGA实现

UAV Attitude Estimation Algorithm and Its FPGA Implementation Based on Pipeline Gaussian Particle Filter

采用高斯粒子滤波算法进行姿态估计算法设计,将四元数离散方程作为状态方程。算法由采样调节粒子、采样粒子、权值计算、均值协方差计算和Cholesky 5个模块组成。通过采用非标准化权值计算四元数"平均"值和协方差阵,并且改写协方差阵计算公式,实现流水线高斯粒子滤波算法。同时提出了并行化设计方案,利用FPGA剩余资源进一步优化运行速率。给出的简化粒子滤波算法与高斯粒子滤波算法设计不仅可用于无人机姿态估计,对于其他非线性估计问题及应用亦适用。仿真结果表明了本设计的可行性和有效性。

An attitude estimation algorithm is designed by using Gaussian Particle Filtering( GPF) algorithm.The quaternion discrete equation is taken as the state equation. The algorithm consists of five modules:sampling conditioning particle, sampling particle, weight calculation, mean covariance calculation and Cholesky. The "average " value and covariance matrix of the quaternion are calculated by using nonstandardized weights, and the calculation formula of the covariance matrix is rewritten. In this way, the pipeline Gaussian particle filter algorithm is implemented. At the same time, a parallel design scheme is proposed, which can further optimize the running speed by using the remaining resources of FPGA. The simplified particle filter algorithm and Gaussian particle filter algorithm presented here can not only be used in UAV attitude estimation, but also be applied to other nonlinear estimation problems and applications. The simulation results have proved the feasibility and effectiveness of this design.