擴展卡爾曼濾波ekf

1、第三章擴展卡爾曼濾波EKF3.1擴展Kalman濾波原理Kalman濾波能夠在線性高斯模型的條件下，可以對目標的狀態(tài)做出最優(yōu)的估計，得到較好的跟蹤效果。對非線性濾波問題常用的處理方法是利用線性化技巧將其轉(zhuǎn)化為一個近似的線性濾波問題。因此，可以利用非線性函數(shù)的局部性特性，將非線性模型局部化，再利用Kalman濾波算法完成濾波跟蹤。擴展Kalman濾波就是基于這樣的思想，將系統(tǒng)的非線性函數(shù)做一階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??這里為了便于數(shù)學處理，假定沒有控制量的輸入，并假定過程噪聲是均值為零的高斯白噪聲，且噪聲分布矩陣是已知的。其中，觀測噪聲也是加性(k)G(k)V均值為零的高斯白噪聲。假定過程噪聲和觀測噪聲序列是彼此獨立的，并且有初始狀態(tài)估計和協(xié)方差矩陣。和線性系統(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)轉(zhuǎn)移和量測矩陣是由和(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)轉(zhuǎn)移矩陣(k)?（325）2222.5[1X(k|k1)](k)0.5[1X(k|k1)]fX??????????第五

5、步：一階線性化觀測方程，求解觀測矩陣(k)H（326）(k|k1)(k)10hXHX?????第六步：求協(xié)方差矩陣預(yù)測(k|k1)P?（327）(k|k1)(k)P(k1|k1)(k)PQ???????????這里需要說明的是，當噪聲驅(qū)動矩陣不存在的時候，或系統(tǒng)狀態(tài)方程中，在前沒有任何驅(qū)動矩陣，這時候，必然和狀態(tài)的維數(shù)一樣的方陣，可將(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?????第九步：協(xié)方差更新（3210）(k)(IK(k)H(k))P(k|k1)nP???以上九步為擴展卡爾曼年濾波的一個計算周期，如此循環(huán)下去就是各個時刻EKF對非線性系統(tǒng)的處理過程。其他參數(shù)設(shè)置請查看源程序，方陣以上系統(tǒng)得到狀態(tài)濾波結(jié)果，如圖321所示。