擴展卡爾曼濾波ekf

1、第三章擴展卡爾曼濾波EKF3.1擴展Kalman濾波原理Kalman濾波能夠在線性高斯模型的條件下，可以對目標的狀態(tài)做出最優(yōu)的估計，得到較好的跟蹤效果。對非線性濾波問題常用的處理方法是利用線性化技巧將其轉化為一個近似的線性濾波問題。因此，可以利用非線性函數的局部性特性，將非線性模型局部化，再利用Kalman濾波算法完成濾波跟蹤。擴展Kalman濾波就是基于這樣的思想，將系統(tǒng)的非線性函數做一階Tayl展開，得到線性化的系統(tǒng)方程從而完成對目

2、標的濾波估計等處理。非線性系統(tǒng)離散動態(tài)方程可以表示為（311）(k1)f[kX(k)]G(k)W(k)X???（312）(k)h[kX(k)]V(k)Z??這里為了便于數學處理，假定沒有控制量的輸入，并假定過程噪聲是均值為零的高斯白噪聲，且噪聲分布矩陣是已知的。其中，觀測噪聲也是加性(k)G(k)V均值為零的高斯白噪聲。假定過程噪聲和觀測噪聲序列是彼此獨立的，并且有初始狀態(tài)估計和協方差矩陣。和線性系統(tǒng)的情況一樣，我們可以?(0|0)X(

3、0|0)P得到擴展Kalman濾波算法如下（313）??(k|k1)f(X(k|k))X??（314）(k1|k)(k1|k)P(k|k)(k1|k)Q(k1)P?????????（315）1(k1)P(k1|)H(k1)[H(k1)P(k1|k)H(k1)R(k1)]Kk????????????（316）???(K1|k1)X(K1|k)K(k1)[Z(k1)h(X(K1|k))]X?????????（317）(k1)[IK(k1)H

4、(k1)]P(k1|k)P??????這里需要重要說明的是，狀態(tài)轉移和量測矩陣是由和(k1|)k??(k1)H?f的雅克比矩陣代替的。其雅克比矩陣的求法如下：h假如狀態(tài)變量有n維，即，則對狀態(tài)方程對各維求偏12[xx...x]nX?導，（324）2(k|k1)(k|k1)20XY???第四步：一階線性化狀態(tài)方程，求解狀態(tài)轉移矩陣(k)?（325）2222.5[1X(k|k1)](k)0.5[1X(k|k1)]fX??????????第五

5、步：一階線性化觀測方程，求解觀測矩陣(k)H（326）(k|k1)(k)10hXHX?????第六步：求協方差矩陣預測(k|k1)P?（327）(k|k1)(k)P(k1|k1)(k)PQ???????????這里需要說明的是，當噪聲驅動矩陣不存在的時候，或系統(tǒng)狀態(tài)方程中，在前沒有任何驅動矩陣，這時候，必然和狀態(tài)的維數一樣的方陣，可將(k)wQ式（327）直接寫為。(k|k1)(k)P(k1|k1)(k)QP????????第七步：求K

6、alman增益（328）(k)P(k|k1)H(k)(H(k)P(k|k1)H(k)R)K??????第八步：求狀態(tài)更新（329）(k)X(k|k1)K(Y(k)Y(k|k1))X?????第九步：協方差更新（3210）(k)(IK(k)H(k))P(k|k1)nP???以上九步為擴展卡爾曼年濾波的一個計算周期，如此循環(huán)下去就是各個時刻EKF對非線性系統(tǒng)的處理過程。其他參數設置請查看源程序，方陣以上系統(tǒng)得到狀態(tài)濾波結果，如圖321所示。