迭代扩展卡尔曼滤波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$ 处的状态和测量的联合概率近似为高斯分布，即：

p ( x k , y k ∣ x ˇ 0 , v 1 : k , y 0 : k − 1 ) ≈ N ( [ μ x , k μ y , k ] , [ Σ x x , k Σ x y , k Σ y x , k Σ y y , k ] ) = N ( [ x ˇ k y o p , k + G k ( x ˇ k − x o p , k ) ] , [ P ˇ k P ˇ k G k T G k P ˇ k G k P ˇ k G k T + R k ′ ] )

=​p(xk​,yk​∣xˇ0​,v1:k​,y0:k−1​)≈N([μx,k​μy,k​​],[Σxx,k​Σyx,k​​Σxy,k​Σyy,k​​])N([xˇk​yop,k​+Gk​(xˇk​−xop,k​)​],[Pˇk​Gk​Pˇk​​Pˇk​GkT​Gk​Pˇk​GkT​+Rk′​​])​

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

p ( x k ∣ x ˇ 0 , v 1 : k , y 0 : k ) = N ( μ x , k + Σ x y , k Σ y y , k − 1 ( y k − μ y , k ) ⏟ x ^ k , Σ x x , k − Σ x y , k Σ y y , k − 1 Σ y x , k ⏟ P ^ k )

=​p(xk​∣xˇ0​,v1:k​,y0:k​)N(x^k​ μx,k​+Σxy,k​Σyy,k−1​(yk​−μy,k​)​​,P^k​ Σxx,k​−Σxy,k​Σyy,k−1​Σyx,k​​​)​

令 ${\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}$ 的改变足够小就终止迭代。

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