基于奇异值分解的改进TCKF姿态估计算法

摘要

针对微惯性测量单元精度较低及正交变换容积卡尔曼滤波(Transformed Cubature Kalman Filter,TCKF)协方差矩阵非正定导致滤波终止的问题,提出了一种基于奇异值分解的改进TCKF(Attitude Singular Value Decomposition Transformed Cubature Kalman Filter,ASVDTCKF)姿态估计算法.该算法以陀螺仪、加速度计和磁力计解算的姿态角作为量测,以姿态四元数作为状态,采用奇异值分解代替Cholesky分解的ASVDTCKF进行数据融合,实时估计姿态角.实验结果表明,在静态和动态条件下,ASVDTCKF能够有效解决TCKF误差协方差矩阵非正定造成的滤波终止问题,与平方根TCKF相比,姿态估计精度和运算效率平均提高了10.6％和19.7％,更适用于矿山等复杂环境的载体姿态测量.