基于修正Rodrigues参数和UKF的姿态估计算法

以修正Rodrigues参数作为姿态参数，利用无味Kalman滤波（UKF）设计了一种全姿态估计算法。修正Rodrigues参数在姿态估计中具有简洁高效的优点，然而，因其奇异性不能用于全姿态运动情况。利用修正Rodrigues参数与其影子参数相互切换的方法可以避免奇异现象的发生。UKF直接利用非线性模型而不需要线性化，适用于高精度的姿态估计。本文针对“陀螺＋矢量观测”这种典型的姿态确定模式，以修正Rodrigues参数与其影子参数相互切换的方法描述姿态，利用UKF设计了姿态估计器。状态协方差阵在姿态参数切换时发生突变，引起姿态估计器的滤波波动。为此，提出了在姿态参数切换时对姿态估计器进行修正的方法。最后通过仿真验证了算法的有效性。