迭代扩展卡尔曼滤波IEKF

迭代扩展卡尔曼滤波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)$
• G k = ∂ g ( x k , n k ) ∂ x k ∣ x o p , k , 0 \boldsymbol{G}_{k}=\left.\frac{\partial \boldsymbol{g}\left(\boldsymbol{x}_{k}, \boldsymbol{n}_{k}\right)}{\partial \boldsymbol{x}_{k}}\right|_{\boldsymbol{x}_{\mathrm{op}, k}, \mathbf{0}} Gk​=∂xk​</