基于Rodrigues参数的姿态估计算法

摘要

以Rodrigues参数作为姿态描述参数提出一种快速的全姿态估计算法.Rodrigues参数具有简洁高效的特点,但它具有奇异性,不适用于大角度情况.首先将序贯旋转方法和Rodrigues参数相结合,提出了一种简洁、无奇异的姿态描述方法.然后应用该姿态描述方法,针对"矢量观测+陀螺"这种典型的姿态测量方案,讨论飞行器的姿态估计问题.在姿态估计算法设计中,给出了陀螺模型和观测矢量模型,推导了状态和状态误差协方差阵的预测方程和量测更新方程,并提出了预测方程的一种高效积分方法.仿真结果表明在估计精度等同的条件下,该姿态估计算法比四元数算法效率大约提高10%.