IEKF&IESKF：卡尔曼滤波器如何迭代？

卡尔曼滤波器和误差状态卡尔曼滤波器都可以在每轮更新内部对进行迭代，这种迭代能够大幅提升算法稳定性。本文详细分析迭代扩展卡尔曼滤波器和迭代误差状态扩展卡尔曼滤波器，并给出算法步骤和等价最优化方程。卡尔曼滤波器和误差状态卡尔曼滤波器相关知识可以参考本人先前的文章卡尔曼滤波器推导和误差状态卡尔曼滤波器推导

1、迭代扩展卡尔曼滤波器

在扩展卡尔曼滤波器中，因为观测方程展开点固定，在该展开点处进行线性化可能无法很好地近似函数整体形式，因此可以不断迭代以寻找更好的展开点。确定展开点后，即可在该处进行展开并求解系统状态的后验分布，即采用迭代扩展卡尔曼滤波器的方法估计系统状态。该过程也可看做先验后验不断交替的过程。

首先写出系统状态的先验分布
$$p(\mathbf{x}|\mathrm{Before})=\mathcal{N}({\check{\mathbf{x}}}^{\kappa -1},\check{\mathbf{P}})$$
其中$\mathrm{Before}$表示从到当前时刻的当前迭代次数为止，系统所经历过的调整。

将观测方程在$({\check{\mathbf{x}}}^{\kappa -1},0)$处展开
$$\mathbf{z}=\mathbf{h}(\mathbf{x},\mathbf{v})\approx \mathbf{h}({\mathbf{x}}^{\kappa },0)+{\mathbf{H}}^{\kappa }(\mathbf{x}-{\check{\mathbf{x}}}^{\kappa -1})+{{\mathbf{v}}^{\kappa }}^{\prime }$$
其中${\mathbf{H}}^{\kappa }={\frac{\partial \mathbf{h}(\mathbf{x},\mathbf{v})}{\partial \mathbf{x}}|}_{{\check{\mathbf{x}}}^{\kappa -1},0}$，${{\mathbf{v}}^{\kappa }}^{\prime }={\frac{\partial \mathbf{h}(\mathbf{x},\mathbf{v})}{\partial \mathbf{v}}|}_{{\check{\mathbf{x}}}^{\kappa -1},0}\mathbf{v}\sim \mathcal{N}(0,{{\mathbf{Q}}^{\kappa }}^{\prime })$

根据观测方程的展开方式和$(A4)$，可得
$$\begin{array}{r}p(\mathbf{x},\mathbf{z}|\mathrm{Before})=\mathcal{N}\left(\left[\begin{array}{c}{\check{\mathbf{x}}}^{\kappa -1}\\ \mathbf{h}({\check{\mathbf{x}}}^{\kappa -1},0)\end{array}\right],\left[\begin{array}{cc}\check{\mathbf{P}}& \check{\mathbf{P}}{{\mathbf{H}}^{\kappa }}^T\\ {\mathbf{H}}^{\kappa }\check{\mathbf{P}}& {\mathbf{H}}^{\kappa }\check{\mathbf{P}}{{\mathbf{H}}^{\kappa }}^T+{{\mathbf{Q}}^{\kappa }}^{\prime }\end{array}\right]\right)\end{array}$$
因此由高斯推断$(A5)$可得
$$\begin{array}{rl}p(\mathbf{x}|\mathbf{z},\mathrm{Before})=\mathcal{N}({\hat{\mathbf{x}}}^{\kappa },{\hat{\mathbf{P}}}^{\kappa })=\mathcal{N}(& {\check{\mathbf{x}}}^{\kappa -1}+\check{\mathbf{P}}{{\mathbf{H}}^{\kappa }}^T({\mathbf{H}}^{\kappa }\check{\mathbf{P}}{{\mathbf{H}}^{\kappa }}^T+{{\mathbf{Q}}^{\kappa }}^{\prime })(\mathbf{z}-\mathbf{h}({\check{\mathbf{x}}}^{\kappa -1},0)),(\mathbf{I}-\check{\mathbf{P}}{{\mathbf{H}}^{\kappa }}^T({\mathbf{H}}^{\kappa }\end{array}$$