寫在前面

本篇內容主要介紹一下IKF的推導過程，IKF作為KF的一種變體，和EKF一樣，主要是為了解決非線性問題。兩者相比較而言，在精度上，IKF要比EKF好一些，但是花費的計算代價要稍大一些，當IKF僅僅進行一次迭代的話相當於EKF。

這裡看到還有ROVIO等一些文章中使用IEKF來優化位姿，但是看公式和原理上和IKF是一樣的（如有大神知道不同點還請指出）。

更新階段(update)的問題表示

由於IKF的主要貢獻在於更新階段，因此KF中的預測階段這裡直接跳過了，後面可以看到，演算法把預測階段(prediction)的狀態估計值作為了當前階段的觀測值。

在更新階段，主要涉及到如下幾個變數：
  • 當前狀態xx，可以認為就是f(x)中的x；
  • 當前狀態的估計 overline{x} ，對應的協方差矩陣P；
  • 當前時刻的觀測量z，對應的雜訊協方差為R；

觀測與狀態之間的無雜訊模型為 h(x) ，所以該階段概率模型可表示為：

z ～ N(h(x), R), quad overline{x} ～ N(x, P)

更新階段最重要的目標就是根據給定的 z,overline{x}, R, P 的狀態下找到更好的估計 x^+,P^+ 。

KF方法的優化目標

PS: 這部分主要是為了引入最小化問題，比較清晰的朋友可以直接跳過了

為了方便表示，我們把觀測 z 和當前狀態的估計 overline{x} 看做為一個觀測向量 ，同時對觀測方程也進行重寫可得下式：

Z=egin{bmatrix}z \ overline{x} end{bmatrix} , quad g(x)=egin{bmatrix}h(x) \ x end{bmatrix} , quad G=g(x)=egin{bmatrix}H(x) \ I end{bmatrix} 因此我們把它們重新寫為概率分布的形式： \ Z～N(g(x), Q) quad , quad Q=egin{bmatrix}R & 0 \ 0 & P end{bmatrix} 有了上面的概率分布，可以很容易的得到似然函數： \ L(x) = alpha*exp(-0.5(Z-g(x))^TQ^{-1}(Z-g(x))) ag{1}

對公式（1）關於x取最大，就可以得到最大似然的解，即：

egin{equation} x^+ = argmax(L(x)) end{equation} 通常，對於公式（1），我們經常取負對數將最大似然問題轉換為求最小值的問題，簡化後我們需要最小化的函數設為q(x)，其表示如下： q(x) = 0.5(Z-g(x))^TQ^{-1}(Z-g(x)) ag{2} 將公式（2）求導並等於0就可以得到最優的狀態量 x^+ 所滿足的條件： \ 0 = g(x^+)^T Q^{-1}(Z-g(x^+)) ag{3} 現假設 V = (Z-g(x)) ～ N(0, Q) ，則 Z=V+g(x) ，公式（3）可以繼續寫作： egin{align} 0 =& g(x^+)^T Q^{-1}(V+g(x)-g(x^+)) \ =& g(x^+)^T Q^{-1}(V+g(x^+)(x-x^+)) ag{4} end{align} 公式（3）到公式（4）的化簡中，有一個比較強的假設就是當前狀態量x與最優的狀態量 x^+ 非常的接近，這個假設是完全合理的，之後使用了 g(x) 在 x^+ 點的一階泰勒展開 g(x)=g(x^+)+g(x^+)(x-x^+) ，至此，設 G=g(x^+) ，上述經過符號化簡可得： egin{align} 0=&G^TQ^{-1}(G(x-x^+)+V) \ x^+-x =& (G^TQ^{-1}G)^{-1}G^T Q^{-1}V ag{5} end{align}

根據方差的定義，下面計算當前狀態的協方差：

egin{align} 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} ag{6} end{align} 這裡由於整個公式中僅有V是變數，因此期望直接由 E(AVV^TB) 變為 AE(VV^T)B 。對於公式（6），把G和Q的符號進行替換，同時使用矩陣逆引理（Matrix Inversion Lemma） (A+BCD)^{-1}=A^{-1}-A^{-1}B(DA^{-1}B+C^{-1})^{-1}DA^{-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} 到此，我們看到最優狀態的協方差矩陣的更新與三個量有關：觀測的狀態量 overline{x} 的協方差P，觀測方程的一階導數H以及觀測量 z 的雜訊協方差R有關。

使用GN方法優化目標函數

根據上面的推導，公式（2）就是整個問題的優化目標函數，下面的內容主要是通過使用Gauss-Newton方法對目標函數進行優化，推導得到IKF（IEKF）的結論。

對於目標函數： q(x) = frac{1}{2}(Z-g(x))^TQ^{-1}(Z-g(x)) ag{2}

可以重寫為如下形式，其中 r(x)=S*(Z-g(x)) , S滿足 S*S^T=Q^{-1} :

q(x)=frac{1}{2}||r(x)||^2 對於上述形式，使用Guass-Newton法（可以參考這裡）進行求解，可以得到變數每次的迭代公式為： x_{i+1}=x_i-(r(x_i)^Tr(x_i))^{-1}(r(x_i)^Tr(x_i)) 把 r(x)=S*(Z-g(x)) , r(x)=S*g(x) 帶入上述公式中，不難得到如下公式，這裡 G=g(x_i) ： x_{i+1}=(G^TQ^{-1}G)^{-1}G^TQ^{-1}(Z-g(x_i)+G_ix_i) 同樣把狀態量g(x)和協方差Q展開就可得到狀態量的更新公式： egin{align} 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)) ag{7} end{align} 所以，從公式（7）可以看到，狀態量的迭代更新與6個變數有關：觀測值z及其協方差R，觀測狀態 overline{x} 及其協方差P（這裡P是從predict階段得來的）以及觀測方程 h(x) 及其一階導數 H(x)=h(x) 。喜大普奔的是，每次的迭代過程並不用更新每個中間狀態 x_i 的協方差矩陣，也算是稍微節省了一些算力吧。

Ps: 對於公式（7）或者說整個狀態變數的迭代過程，如果僅僅進行一次，即 x_0=overline{x} ，則IKF就變化為了EKF。

IKF推導小結

對於以上流程而言，簡而言之就是：

  1. 使用最大似然的方法得到優化的目標函數（即公式（2））；
  2. 隨後使用Gauss-Newton方法進行迭代優化，不斷的更新狀態變數x；
  3. 對於演算法而言，當x的改變數很小的時候，就可以停止迭代返回結果了（參考2中對迭代方法進行了更多的討論，感興趣的可以看看）。

reference

[1] The Iterated Kalman Filter Update as a Gauss-Newton Method.

[2] Performance evaluation of iterated extended Kalman filter with variable step-length.
推薦閱讀：
相关文章