基于SE(3)的扩展卡尔曼滤波姿态估计算法

利用李群理论对姿态估计问题开展研究,提出一种基于SE(3)扩展卡尔曼滤波姿态估计算法。通过将姿态信息和陀螺仪漂移构造为特殊欧式群SE(3)的元素,推导出了基于SE(3)扩展卡尔曼滤波的姿态估计算法SE(3)-EKF。通过对比分析,指出SE(3)-EKF与近年来提出的几何扩展卡尔曼滤波算法是等价的。在坐标系一致性的基础上,所提出的结论为几何扩展卡尔曼滤波算法提供了另外一种解释。仿真实验结果表明提出的SE(3)-EKF与几何扩展卡尔曼滤波算法精度基本一致,且在失准角较大时估计精度显著优于传统的乘性扩展卡尔曼滤波算法,从而从一定程度上放松了线性卡尔曼滤波姿态估计算法对初始姿态精度的要求。