Iterated Kalman Filter(IKF/IEKF)总结

写在前面

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

更新阶段的问题表示

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

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

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

KF方法的优化目标

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

为了方便表示，我们把观测 z z z和当前状态的估计 x ‾ \overline{x} x看做为一个观测向量 ，同时对观测方程也进行重写可得下式：
Z = [ z x ‾ ] , g ( x ) = [ h ( x ) x ] , G = g ′ ( x ) = [ H ( x ) I ] Z=\begin{bmatrix}z \\ \overline{x} \end{bmatrix} , \quad g(x)=\begin{bmatrix}h(x) \\ x \end{bmatrix} , \quad G=g'(x)=\begin{bmatrix}H(x) \\ I \end{bmatrix} Z=[zx],g(x)=[h(x)x],G=g′(x)=[H(x)I]
因此我们把它们重新写为概率分布的形式：
Z ～ N ( g ( x ) , Q ) , Q = [ R 0 0 P ] Z～N(g(x), Q) \quad , \quad Q=\begin{bmatrix}R & 0 \\ 0 & P \end{bmatrix} Z～N(g(x),Q),Q=[R00P]
有了上面的概率分布，可以很容易的得到似然函数：
(1) L ( x ) = α ∗ e x p ( − 0.5 ( Z − g ( x ) ) T Q − 1 ( Z − g ( x ) ) ) L(x) = \alpha*exp(-0.5(Z-g(x))^TQ^{-1}(Z-g(x))) \tag{1} L(x)=α∗exp(−0.5(Z−g(x))TQ−1(Z−g(x)))(1)
对公式（1）关于x取最大，就可以得到最大似然的解，即：
x + = a r g m a x ( L ( x ) ) x^+ = argmax(L(x)) x+=argmax(L(x))
通常，对于公式（1），我们经常取负对数将最大似然问题转换为求最小值的问题，简化后我们需要最小化的函数设为 q ( x ) q(x) q(x)，其表示如下：
(2) q ( x ) = 0.5 ( Z − g ( x ) ) T Q − 1 ( Z − g ( x ) ) q(x) = 0.5(Z-g(x))^TQ^{-1}(Z-g(x)) \tag{2} q(x)=0.5(Z−g(x))TQ−1(Z−g(x))(2)
将公式（2）求导并等于0就可以得到最优的状态量 x + x^+ x+所满足的条件：
(3) 0 = g ′ ( x + ) T Q − 1 ( Z − g ( x + ) ) 0 = g'(x^+)^T Q^{-1}(Z-g(x^+)) \tag{3} 0=g′(x+)TQ−1(Z−g(x+))(3)
现假设 V = ( Z − g ( x ) ) ～ N ( 0 , Q ) V = (Z-g(x)) ～ N(0, Q) V=(Z−g(x))～N(0,Q)，则 Z = V + g ( x ) Z=V+g(x) Z=V+g(x)，公式（3）可以继续写作：
(4) 0 = g ′ ( x + ) T Q − 1 ( V + g ( x ) − g ( x + ) ) = g ′ ( x + ) T Q − 1 ( V + g ′ ( x + ) ( x − x + ) ) 0 = g'(x^+)^T Q^{-1}(V+g(x)-g(x^+)) = g'(x^+)^T Q^{-1}(V+g'(x^+)(x-x^+)) \tag{4} 0=g′(x+)TQ−1(V+g(x)−g(x+))=g′(x+)TQ−1(V+g′(x+)(x−x+))(4)
公式（3）到公式（4）的化简中，有一个比较强的假设就是当前状态量 x x x与最优的状态量 x + x^+ x+非常的接近，这个假设是完全合理的，之后使用了 g ( x ) g(x) g(x)在 x + x^+ x+点的一阶泰勒展开 g ( x ) = g ( x + ) + g ′ ( x + ) ( x − x + ) g(x)=g(x^+)+g'(x^+)(x-x^+) g(x)=g(x+)+g′(x+)(x−x+)，至此，设 G = g ′ ( x + ) G=g'(x^+) G=g′(x+)，上述经过符号化简可得：
(5) 0 = G T Q − 1 ( G ( x − x + ) + V ) = > x + − x = ( G T Q − 1 G ) − 1 G T Q − 1 V 0=G^TQ^{-1}(G(x-x^+)+V) => x^+-x = (G^TQ^{-1}G)^{-1}G^T Q^{-1}V \tag{5} 0=GTQ−1(G(x−x+)+V)=>x+−x=(GTQ−1G)−1GTQ−1V(5)
根据方差的定义，下面计算当前状态的协方差：
(6) 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 P^+ = E((x^+-x)(x^+-x)^T)=(G^TQ^{-1}G)^{-1}G^TQ^{-1}E(VV^T)G(G^TQ^{-1}G)^{-1}=(G^TQ^{-1}G)^{-1} \tag{6} P+=E((x+−x)(x+−x)T)=(GTQ−1G)−1GTQ−1E(VVT)G(GTQ−1G)−1=(GTQ−1G)−1(6)
这里由于整个公式中仅有 V V V是变量，因此期望直接由 E ( A V V T B ) E(AVV^TB) E(AVVTB)变为 A E ( V V T ) B AE(VV^T)B AE(VVT)B
对于公式（6），把G和Q的符号进行替换，同时使用矩阵逆引理（Matrix Inversion Lemma）
( A + B C D ) − 1 = A − 1 − A − 1 B ( D A − 1 B + C − 1 ) − 1 D A − 1 (A+BCD)^{-1}=A^{-1}-A^{-1}B(DA^{-1}B+C^{-1})^{-1}DA^{-1} (A+BCD)−1=A−1−A−1B(DA−1B+C−1)−1DA−1
可得：
P + = P − ( H T R − 1 H + P − 1 ) − 1 H T R − 1 H P = ( I − K H ) P w h e r e K = ( H T R − 1 H + P − 1 ) − 1 H T R − 1 = P H T ( H P H T + R ) − 1 P^+ = P-(H^TR^{-1}H+P^{-1})^{-1}H^TR^{-1}HP = (I-KH)P \\ where \quad K=(H^TR^{-1}H+P^{-1})^{-1}H^TR^{-1} =PH^T(HPH^T+R)^{-1} P+=P−(HTR−1H+P−1)−1HTR−1HP=(I−KH)PwhereK=(HTR−1H+P−1)−1HTR−1=PHT(HPHT+R)−1
到此，我们看到最优状态的协方差矩阵的更新与三个量有关：观测的状态量 x ‾ \overline{x} x的协方差P，观测方程的一阶导数H以及观测量 z z z的噪声协方差R有关。

使用GN方法优化目标函数

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

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

IKF推导小结

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

使用最大似然的方法得到优化的目标函数（即公式（2））；
随后使用Gauss-Newton方法进行迭代优化，不断的更新状态变量x；
对于算法而言，当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.
.