迭代扩展卡尔曼滤波IEKF

预测

迭代扩展卡尔曼滤波（IEKF）的预测部分和扩展卡尔曼滤波基本相同。直接给出结论：

$${\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)$$

$${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{Q}}_k^{\prime }$$

更新

非线性观测模型为：

$${\mathbf{y}}_k=g\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)$$

对其中任意一个点 ${\mathbf{x}}_{\mathrm{op},k}$ 进行线性化，可得：

$$\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)\approx {\mathbf{y}}_{\mathrm{op},k}+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\mathbf{x}}_{\mathrm{op},k}\right)+{\mathbf{n}}_k^{\prime }$$

其中：

• ${\mathbf{y}}_{\mathrm{op},k}=\mathbf{g}\left({\mathbf{x}}_{\mathrm{op},k},0\right)$
• ${\mathbf{G}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\mathbf{x}}_{\mathrm{op},k},0}$
• ${\mathbf{n}}_k^{\prime }={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\mathbf{x}}_{\mathrm{op},k},0}{\mathbf{n}}_k$

任意一个点 ${\mathbf{x}}_{\mathrm{op},k}$ 进行线性化，可得：

需要注意的是，观测模型和雅可比矩阵均在 ${\mathbf{x}}_{\mathrm{op},k}$ 处计算。（在EKF中， ${\mathbf{x}}_{\mathrm{op},k}={\check{\mathbf{x}}}_k$）

使用上面的线性化模型，我们可以将时刻 $k$ 处的状态和测量的联合概率近似为高斯分布，即：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k,{\mathbf{y}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\approx \mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_{x,k}\\ {\mathbf{\mu }}_{y,k}\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx,k}& {\mathbf{\Sigma }}_{xy,k}\\ {\mathbf{\Sigma }}_{yx,k}& {\mathbf{\Sigma }}_{yy,k}\end{array}\right]\right)\\ =& \mathcal{N}\left(\left[\begin{array}{c}{\check{\mathbf{x}}}_k\\ {\mathbf{y}}_{\mathrm{op},k}+{\mathbf{G}}_k\left({\check{\mathbf{x}}}_k-{\mathbf{x}}_{\mathrm{op},k}\right)\end{array}\right],\left[\begin{array}{cc}{\check{\mathbf{P}}}_k& {\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}\\ {\mathbf{G}}_k{\check{\mathbf{P}}}_k& {\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{R}}_k^{\prime }\end{array}\right]\right)\end{array}$$

如果测量值 ${\mathbf{y}}_k$ 已知，我们可以利用高斯推断得到 ${\mathbf{x}}_k$ 的条件概率密度（即后验）：

$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)\\ =& \mathcal{N}(\underset{{\hat{\mathbf{x}}}_k}{\underbrace{{\mathbf{\mu }}_{x,k}+{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}\left({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right)}},\underset{{\hat{\mathbf{P}}}_k}{\underbrace{{\mathbf{\Sigma }}_{xx,k}-{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}{\mathbf{\Sigma }}_{yx,k}}})\end{array}$$

令 ${\mathbf{K}}_k={\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{R}}_k^{\prime }\right)}^{-1}$（卡尔曼增益），则得到：

$${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k$$

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{y}}_{\mathrm{op},k}-{\mathbf{G}}_k\left({\check{\mathbf{x}}}_k-{\mathbf{x}}_{\mathrm{op},k}\right)\right)$$

可以看出，IEKF中的卡尔曼增益和更新方程与EKF非常相似，唯一的区别在于线性化的工作点。如果将线性化的工作点设置为预测先验的均值（即 ${\mathbf{x}}_{\mathrm{op},k}={\check{\mathbf{x}}}_k$），那么IEKF就等价于EKF。

然而，如果我们迭代的重新计算 ${\hat{\mathbf{x}}}_k$ ， 并且在每一次迭代中将工作点设置为上一次迭代的后验均值（即令 ${\mathbf{x}}_{\mathrm{op},k}={\hat{\mathbf{x}}}_k$ ），将得到更好的结果。

在第一次迭代中, 我们令 ${\mathbf{x}}_{\mathrm{op},k}={\check{\mathbf{x}}}_k$，这使得我们能够对更好的估计进行线性化, 从而改进每次迭代的近似程度。在迭代的过程中，若 ${\mathbf{x}}_{\mathrm{op},k}$ 的改变足够小就终止迭代。

注意，卡尔曼增益方程和更新方程收敛之后，协方差方程只需要计算一次。