用IEKF(迭代Kalman滤波器)处理姿态估计的非线性

问题提出

在之前ESKF或者EKF算法中采用四元数对姿态进行估计时，由于采用的是六轴传感器，Yaw轴的数据仅来源于Kalman的预测过程。在最后更新状态一步由于姿态融合的非线性，在加速度计数据通过线性融合进入四元数时会出现Yaw轴的额外偏移，使得最后积分结果偏离了角速度计积分出的Yaw轴结果。具体表现为当加速度计与角速度计数据较为不匹配时（例如受到冲击），Yaw会出现较大的偏移。

为了解决非线性带来的影响，除了消除线性更新中对于Yaw轴旋转的分量外，还需要进一步使用迭代Kalman滤波器(Iterated Extended Kalman Filter)的方法改进原有算法。

原理

在EKF算法更新环节中中，我们在工作点附近用线性过程替代实现线性的融合，而迭代Kalman滤波器的思路是不改变模型采用数值迭代的方法隐式地逼近状态估计值，即通过某种非线性的方法调整状态使得观测值和预测值两者与实际值之间的误差最小化。在数学上，令[1]

如果假设Z的分布符合，则在最大似然准则下可以取令Z的似然函数最大化时的状态值作为非线性的观测结果,也即最小化二次型

取其导数为0的点作为结果，带入后验方差的计算中可推导出与传统Kalman滤波器一样的方差更新公式。而对于状态值的更新则采用高斯-牛顿法求解，若令则

根据高斯-牛顿法的迭代过程，状态值的迭代式为

代入经过推导可以得到迭代式

另外如果结合之前的误差状态Kalman滤波器其迭代过程依然类似，最主要的区别仅在于与后面两项之和是在流形上相加[2]，当然也有为了进一步理想化非线性环节将一项也改写为流形上操作的[3]

实现过程

在实现上来讲，在传统EKF的基础上将状态更新和后验方差更新修改为可迭代的过程，即

这里以改进之前的ESKF作为例子，根据[3]以及上述推导迭代过程改写为

当然为了避免更新过程对Yaw产生的额外旋转，在计算迭代过程中的时，可直接减去Yaw轴旋转分量

实际测试中一般迭代两次即可达到较高精度，特殊情况下例如震动等加速度测量的姿态变化较大时迭代次数会显著增多，但是实际上减少迭代次数可以在这种情况下达到类似低通滤波的效果，反而是对测量结果的稳定性有利。

参考

[1]BELL B M, CATHEY F W. The iterated Kalman filter update as a Gauss-Newton method[J/OL]. IEEE Transactions on Automatic Control, 1993, 38(2): 294-297. DOI:10.1109/9.250476.
[2]GAO X, XIAO T, BAI C, 等. Anderson Acceleration for on-Manifold Iterated Error State Kalman Filters[J/OL]. IEEE Robotics and Automation Letters, 2022, 7(4): 12243-12250. DOI:10.1109/LRA.2022.3214056.
[3]HUAI J, GAO X. A Quick Guide for the Iterated Extended Kalman Filter on Manifolds[M/OL]. arXiv, 2023[2024-02-10]. http://arxiv.org/abs/2307.09237.