Iterated Kalman Filter(IKF/IEKF)总结

写在前面

本篇内容主要介绍一下IKF的推导过程，IKF作为KF的一种变体，和EKF一样，主要是为了解决非线性问题。两者相比较而言，在精度上，IKF要比EKF好一些，但是花费的计算代价要稍大一些，当IKF仅仅进行一次迭代的话相当于EKF。
这里看到还有ROVIO等一些文章中使用IEKF来优化位姿，但是看公式和原理上和IKF是一样的（如有大神知道不同点还请指出）。

 

---

更新阶段的问题表示

由于IKF的主要贡献在于更新阶段，因此KF中的预测阶段这里直接跳过了，后面可以看到，算法把预测阶段的状态估计值作为了当前阶段的观测值。
在更新阶段，主要涉及到如下几个变量：

• 当前状态$x$，可以认为就是$f(x)$中的x；
• 当前状态的估计$\overline{x}$，对应的协方差矩阵$P$；
• 当前时刻的观测量$z$，对应的噪声协方差为R；

观测与状态之间的无噪声模型为$h(x)$，所以该阶段概率模型可表示为：
$$z～N(h(x),R),\overline{x}～N(x,P)$$
更新阶段最重要的目标就是根据给定的$z,\overline{x},R,P$的状态下找到更好的估计${\overline{x}}^{+},P^{+}$。
 

---

KF方法的优化目标

这部分主要是为了引入最小化问题，比较清晰的朋友可以直接跳过了

为了方便表示，我们把观测$z$和当前状态的估计$\overline{x}$看做为一个观测向量 ，同时对观测方程也进行重写可得下式：
$$Z=\left[\begin{array}{c}z\\ \overline{x}\end{array}\right],g(x)=\left[\begin{array}{c}h(x)\\ x\end{array}\right],G=g^{\prime }(x)=\left[\begin{array}{c}H(x)\\ I\end{array}\right]$$
因此我们把它们重新写为概率分布的形式：
$$Z～N(g(x),Q),Q=\left[\begin{array}{cc}R& 0\\ 0& P\end{array}\right]$$
有了上面的概率分布，可以很容易的得到似然函数：
$$L(x)=\alpha \ast exp(-0.5(Z-g(x){)}^T{Q}^{-1}(Z-g(x)))\text{\hspace{1em}(1)}$$
对公式（1）关于x取最大，就可以得到最大似然的解，即：
$$x^{+}=argmax(L(x))$$
通常，对于公式（1），我们经常取负对数将最大似然问题转换为求最小值的问题，简化后我们需要最小化的函数设为$q(x)$，其表示如下：
$$q(x)=0.5(Z-g(x){)}^T{Q}^{-1}(Z-g(x))\text{\hspace{1em}(2)}$$
将公式（2）求导并等于0就可以得到最优的状态量$x^{+}$所满足的条件：
$$0=g^{\prime }(x^{+}{)}^T{Q}^{-1}(Z-g(x^{+}))\text{\hspace{1em}(3)}$$
现假设$V=(Z-g(x))～N(0,Q)$，则$Z=V+g(x)$，公式（3）可以继续写作：
$$0=g^{\prime }(x^{+}{)}^T{Q}^{-1}(V+g(x)-g(x^{+}))=g^{\prime }(x^{+}{)}^T{Q}^{-1}(V+g^{\prime }(x^{+})(x-x^{+}))\text{\hspace{1em}(4)}$$
公式（3）到公式（4）的化简中，有一个比较强的假设就是当前状态量$x$与最优的状态量$x^{+}$非常的接近，这个假设是完全合理的，之后使用了$g(x)$在$x^{+}$点的一阶泰勒展开$g(x)=g(x^{+})+g^{\prime }(x^{+})(x-x^{+})$，至此，设$G=g^{\prime }(x^{+})$，上述经过符号化简可得：
$$0=G^T{Q}^{-1}(G(x-x^{+})+V)=>x^{+}-x=(G^T{Q}^{-1}G{)}^{-1}{G}^T{Q}^{-1}V\text{\hspace{1em}(5)}$$
根据方差的定义，下面计算当前状态的协方差：
$$P^{+}=E((x^{+}-x)(x^{+}-x{)}^T)=(G^T{Q}^{-1}G{)}^{-1}{G}^T{Q}^{-1}E(V{V}^T)G(G^T{Q}^{-1}G{)}^{-1}=(G^T{Q}^{-1}G{)}^{-1}\text{\hspace{1em}(6)}$$
这里由于整个公式中仅有$V$是变量，因此期望直接由$E(AV{V}^{T}B)$变为$AE(V{V}^T)B$
对于公式（6），把G和Q的符号进行替换，同时使用矩阵逆引理（Matrix Inversion Lemma）
$$(A+BCD{)}^{-1}=A^{-1}-A^{-1}B(D{A}^{-1}B+C^{-1}{)}^{-1}D{A}^{-1}$$
可得：
$$\begin{gathered} P^{+}=P-(H^T{R}^{-1}H+P^{-1}{)}^{-1}{H}^T{R}^{-1}HP=(I-KH)P \\ whereK=(H^T{R}^{-1}H+P^{-1}{)}^{-1}{H}^T{R}^{-1}=P{H}^T(HP{H}^T+R{)}^{-1} \end{gathered}$$
到此，我们看到最优状态的协方差矩阵的更新与三个量有关：观测的状态量$\overline{x}$的协方差P，观测方程的一阶导数H以及观测量$z$的噪声协方差R有关。
 

---

使用GN方法优化目标函数

根据上面的推导，公式（2）就是整个问题的优化目标函数，下面的内容主要是通过使用Gauss-Newton方法对目标函数进行优化，推导得到IKF（IEKF）的结论。
对于目标函数：
$$q(x)=\frac{1}{2}(Z-g(x){)}^T{Q}^{-1}(Z-g(x))\text{\hspace{1em}(2)}$$
可以重写为如下形式，其中$r(x)=S\ast (Z-g(x))$, S满足$S\ast S^T=Q^{-1}$:
$$q(x)=\frac{1}{2}||r(x)|{|}^2$$
对于上述形式，使用Guass-Newton法（可以参考这里）进行求解，可以得到变量每次的迭代公式为：
$$x_{i+1}=x_i-(r^{\prime }(x_i{)}^T{r}^{\prime }(x_i){)}^{-1}(r^{\prime }(x_i{)}^{T}r(x_i))$$
把$r(x)=S\ast (Z-g(x)),r^{\prime }(x)=S\ast g^{\prime }(x)$带入之后，不难得到如下公式，这里$G=g^{\prime }(x_i)$：
$$x_{i+1}=(G^T{Q}^{-1}G{)}^{-1}{G}^T{Q}^{-1}(Z-g(x_i)+G_i{x}_i)$$
同样把状态量$g(x)$和协方差$Q$展开就可得到状态量的更新公式：
$$x_{i+1}=x_i+(H_i^T{R}^{-1}{H}_i+P^{-1}{)}^{-1}{H}_i^T{R}^{-1}(z-h(x_i)-H_i(\overline{x}-x))=x_i+K_i(z-h(x_i)-H_i(\overline{x}-x_i))\text{\hspace{1em}(7)}$$
所以，从公式（7）可以看到，状态量的迭代更新与6个变量有关：观测值$z$及其协方差，观测状态$\overline{x}$及其协方差以及观测方程$h(x)$及其一阶导数$H(x)$。喜大普奔的是，每次的迭代过程并不用更新每个中间状态$x_i$的协方差矩阵，也算是稍微节省了一些算力吧。

Ps:
对于公式（7）或者说整个状态变量的迭代过程，如果仅仅进行一次，即$x_0=\overline{x}$，则IKF就变化为了EKF

 

---

IKF推导小结

对于以上流程而言，简而言之就是：

1. 使用最大似然的方法得到优化的目标函数（即公式（2））；
2. 随后使用Gauss-Newton方法进行迭代优化，不断的更新状态变量x；
3. 对于算法而言，当x的改变量很小的时候，就可以停止迭代返回结果了。

 

---

reference

[1] The Iterated Kalman Filter Update as a Gauss-Newton Method.
[2] Performance evaluation of iterated extended Kalman filter with variable step-length.
.