基于四元数粒子滤波的飞行器姿态估计算法研究

针对飞行器非线性姿态确定问题，提出了一种四元数粒子滤波算法。将状态向量分为线性部分和非线性部分分别进行处理，降低了粒子滤波的运算量。针对四元数加权求和规范化问题，通过构造拉格朗日代价函数的方法将四元数加权和问题转化为代价函数取极值时的四元数向量求解问题；并通过求取四元数误差方差矩阵对角线元素平方根的方法保证扰动四元数的规范化；利用乘性误差四元数表示四元数估计点与采样点之间的距离，求取四元数的协方差矩阵解决了旋转矢量方差计算问题。仿真实验表明，与传统的扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）算法相比，该算法估计精度更高，稳定性更好。

Study on Aerial Vehicle Attitude Estimation Based on Quaternion Particle Filter Algorithm

A quaternion particle filter algorithm was proposed to solve the aerial vehicle nonlinear attitude determination problem. To reduce computational burden, the state vector was divided into linear part and nonlinear part. Aiming at quaternion weighting sum normalization problem, a Lagrange cost function was derived to compute the predicted quaternion by minimizing the cost function, and the normalization of disturbed quaternion was ensured by calculating the square root of diagonal elements of the quaternion error covariance matrix, the multiplicative quaternion error was used for predicted covariance computation of the quaternion, since it represented the distance between quaternion points and the predicted mean quaternion. The simulation results show that, compared to traditional extended Kalman filter（EKF） and unscented Kalman filter（UKF）algorithms, the proposed algorithm has higher estimation precision and better stability.