用于位姿估计的李代数无迹卡尔曼滤波

提出了一种用于矩阵李群的无迹卡尔曼滤波器，其中状态的时间传播在李代数上表述。这是通过对数的运动学微分方程完成的，其中使用了右雅可比矩阵的逆。西格玛点然后可以表示为向量形式的对数，并且西格玛点的时间传播以及均值和协方差的计算可以在李代数上完成。所得公式在很大程度上基于向量形式的对数，因此更接近于 $\mathbb{R}^n$ 中系统的 UKF。这给出了一个优雅且结构良好的公式，提供了对问题的额外洞察，并且计算高效。所提方法特别在矩阵李群 $SE(3)$ 上表述和研究。包含了关于右和左雅可比矩阵的讨论，并推导了 $SE(3)$ 上右雅可比矩阵逆的一种新颖闭式解，其给出了涉及更少矩阵运算的紧凑表示。所提方法在仿真中得到了验证。