基于修正的Rodrigues参数的EKF算法在姿态估计中的应用

Application of extended Kalman filter algorithm in attitude estimation based on modified rodrigues parameters

以低成本航姿测量系统为研究对象,针对四元数描述姿态时存在冗余的不足,提出使用三维独立矢量的修正的Rodrigues参数进行姿态解算,利用与其影子参数相互切换的方法解决了修正的Rodrigues参数在描述姿态时存在奇异的问题,根据系统中器件的特点,以陀螺零偏和修正的Rodrigues参数为状态向量,以加速度计、磁强计输出为观测向量建立了基于扩展Kalman滤波的姿态估计算法,避免了利用四元数进行姿态估计时状态误差方差阵产生奇异的问题,并在一定程度上减小了估计算法的计算量。最后通过全姿态以及摇摆基座数据仿真验证了算法的有效性。

An all-attitude estimation algorithm for low cost attitude and heading reference system using extended Kalman filter based on modified Rodrigues parameters is designed. In view of the shortage of redundancy when using quaternion to describe attitude, the modified Rodrigues parameter, which is three-dimensional independent vector, is put forward for attitude computation. By switching between the modified Rodrigues parameters and their shadow parameters, the problem of singularity is solved. Considering the characteristics of the system sensors, the estimation algorithm is established with the gyroscope bias and modified Rodrigues parameters as state vector and the outputs of accelerometer and magnetometer as observation vectors, which avoids the state error covariance matrix being singularity. At last, an all-attitude numerical simulation and a swing movement numerical simulation are used to demonstrate the efficacy of the algorithm.