基于四元数平方根容积卡尔曼滤波的姿态估计  

Attitude estimation based on quaternion square-root cubature Kalman filter

作　　者：钱华明[1] 黄蔚[1] 葛磊[1] 张广拓[2] 

机构地区：[1]哈尔滨工程大学自动化学院,哈尔滨150001 [2]天津航海仪器研究所,天津300131

出　　处：《北京航空航天大学学报》2013年第5期645-649,共5页Journal of Beijing University of Aeronautics and Astronautics

基　　金：国家自然科学基金资助项目(61104036)

摘　　要：针对飞行器姿态确定中乘性扩展卡尔曼滤波(MEKF,Multiplicative Extend-ed Kalman Filter)在较大初始姿态误差角情况下存在估计精度低及收敛速度慢的问题,提出了一种四元数平方根容积卡尔曼滤波(QSCKF,Quaternion Square-root Cubature Kalman Filter)算法.在推导姿态确定系统四元数非线性误差模型的基础上,采用容积数值积分理论来计算非线性函数的均值与方差,同时使用平方根的形式来提高数值稳定性;针对四元数规范化问题,采用拉格朗日代价函数法求解四元数加权均值.仿真结果表明:在初始姿态误差较大的情况下,该算法相比较于MEKF以及无迹四元数估计法(USQUE,Unscented Quaternion Estimator),估计精度高且收敛速度快,滤波稳定性好,同时估计时间比USQUE缩短了1/3.Considering that a muhiplicative extended Kalman filter （MEKF） existed lower estimation ac- curacy and slower convergence rate for large initialization errors, a quaternion square-root cubature Kalman fil- ter （QSCKF） was proposed. A quaternion nonlinear error model was derived for attitude determination sys- tem, and then the cubature numerical integration theory was used to calculate the mean and variance of the nonlinear function. The square-root forms were used to improve numerical stability. To avoid the quaternion normalization problem, a Lagrange loss function was derived to compute the average quaternion. The simula- tion results indicate that the proposed algorithm provides higher estimation precision with faster convergence rate and better filter＇s stability than MEKF and unscented quaternion estimator （USQUE） for large initializa- tion errors. And, the estimated time of this algorithm is shortened by 1/3 compared to USQUE.

关 键 词：平方根容积卡尔曼滤波 四元数 拉格朗日代价函数法 姿态估计 

分 类 号：U666.12[交通运输工程—船舶及航道工程]