卡爾曼濾波器介紹

1、卡爾曼濾波器介紹GregWelch1andGaryBishop2TR95-041DepartmentofComputerScienceUniversityofNorthCarolinaatChapelHill3ChapelHill,NC27599-3175翻譯：姚旭晨更新日期:2006年7月24日，星期一中文版更新日期：2007年1月8日，星期一摘要1960年，卡爾曼發(fā)表了他著名的用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文。從那以后，得益于數(shù)字計算技術(shù)的進步，卡爾曼濾波器已成為推廣研究和應用的主題，尤其是在自主或協(xié)助導航領(lǐng)域?？柭鼮V波器由一系列遞歸數(shù)學公式描述。它們提供了一種高效可計算的方法來

2、估計過程的狀態(tài)，并使估計均方誤差最小?？柭鼮V波器應用廣泛且功能強大：它可以估計信號的過去和當前狀態(tài)，甚至能估計將來的狀態(tài)，即使并不知道模型的確切性質(zhì)。這篇文章介紹了離散卡爾曼理論和實用方法，包括卡爾曼濾波器及其衍生：擴展卡爾曼濾波器的描述和討論，并給出了一個相對簡單的帶圖實例。3北卡羅來納大學教堂山分校，譯者注。1Welch&Bishop,卡爾曼濾波器介紹21離散卡爾曼濾波器1960年，卡爾曼發(fā)表了他著名的用遞歸方法解決離散數(shù)據(jù)線性濾波問題的論文Kalman60。從那以后，得益于數(shù)字計算技術(shù)的進步，卡爾曼濾波器已成為推廣研究和應用的主題，尤其是在自主或協(xié)助導航領(lǐng)域。Maybeck79的第一章

3、給出了一個非常“友好”的介紹，更全面的討論可以參考Sorenson70，后者還包含了一些非常有趣的歷史故事。更廣泛的參考包括Gelb74,Grewal93,Maybeck79,Lewis86,Brown92,Jacobs93。被估計的過程信號卡爾曼濾波器用于估計離散時間過程的狀態(tài)變量xn。這個離散時間過程由以下離散隨機差分方程描述：xk=Axk1+Buk1+wk1,定義觀測變量zm，得到量測方程：zk=Hxk+vk.(1.2)(1.1)隨機信號wk和vk分別表示過程激勵噪聲1和觀測噪聲。假設(shè)它們?yōu)橄嗷オ毩ⅲ龖B(tài)分布的白色噪聲：p(w)N(0,Q),p(v)N(0,R).(1.3)(1.4)實際

4、系統(tǒng)中，過程激勵噪聲協(xié)方差矩陣Q和觀測噪聲協(xié)方差矩陣R可能會隨每次迭代計算而變化。但在這兒我們假設(shè)它們是常數(shù)。當控制函數(shù)uk1或過程激勵噪聲wk1為零時，差分方程1.1中的nn階增益矩陣A將上一時刻k1的狀態(tài)線性映射到當前時刻k的狀態(tài)。實際中A可能隨時間變化，但在這兒假設(shè)為常數(shù)。nl階矩陣B代表可選的控制輸入ul的增益。量測方程1.2中的mn階矩陣H表示狀態(tài)變量xk對測量變量zk的增益。實際中H可能隨時間變化，但在這兒假設(shè)為常數(shù)。濾波器的計算原型n代表先驗，代表估計）為在已知第k步以前狀態(tài)情定義xk（況下第k步的先驗狀態(tài)估計。定義xkn為已知測量變量zk時第k步的后驗狀態(tài)估計。由此定義先驗估計

5、誤差和后驗估計誤差：ekxkxk,ekxkxk原文為processnoise，本該翻譯作過程噪聲，由時間序列信號模型的觀點，平穩(wěn)隨機序列可以看成是由典型噪聲源激勵線性系統(tǒng)產(chǎn)生，故譯作過程激勵噪聲。1UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹3先驗估計誤差的協(xié)方差為：Pk=Eekek,T(1.5)后驗估計誤差的協(xié)方差為：Pk=EekekT,(1.6)式1.7構(gòu)造了卡爾曼濾波器的表達式：先驗估計xk和加權(quán)的測量變量zk及其預測Hxk之差的線性組合構(gòu)成了后驗狀態(tài)估計xk。式1.7的理論解釋請參看“濾波器的概率原型”一節(jié)。xk=xk

6、+K(zkHxk)(1.7)式1.7中測量變量及其預測之差(zkHxk)被稱為測量過程的革新或殘余。殘余反映了預測值和實際值之間的不一致程度。殘余為零表明二者完全吻合。式1.7中nm階矩陣K叫做殘余的增益或混合因數(shù)，作用是使1.6式中的后驗估計誤差協(xié)方差最小?？梢酝ㄟ^以下步驟計算K：首先將1.7式代入ek的定義式，再將ek代入1.6式中，求得期望后，將1.6式中的Pk對K求導。并使一階導數(shù)為零從而解得K值。詳細推導清參照Maybeck79,Brown92,Jacobs93。K的一種表示形式為：TTKk=PkH(HPkH+R)1TPkH=.THPkH+R(1.8)由1.8式可知，觀測噪聲協(xié)方差R

7、越小，殘余的增益越大K越大。特別地，R趨向于零時，有：Rk0limKk=H1.越小，殘余的增益K越小。特別另一方面，先驗估計誤差協(xié)方差Pk地，Pk趨向于零時，有：Pk0limKk=0.增益K的另一種解釋是隨著測量噪聲協(xié)方差R趨于零，測量變量zk的權(quán)重越來越大，而zk的預測Hxk的權(quán)重越來越小。另一方面，隨著先驗估計誤差協(xié)方差Pk趨于零，測量變量zk的權(quán)重越來越小，而zk的預測Hxk的權(quán)重越來越大。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹4濾波器的概率原型解釋1.7式的解釋來源于貝葉斯規(guī)則：xk的更新取決于在已知先前的測量變

8、量zk的情況下xk的先驗估計xk的概率分布?？柭鼮V波器表達式中包含了狀態(tài)分布的前二階矩。Exk=xkE(xkxk)(xkxk)T=Pk.p(xk|zk)N(Exk,E(xkxk)(xkxk)T)=N(xk,Pk).有關(guān)卡爾曼濾波器的概率原型的更多討論，請參考Maybeck79,Brown92,Jacobs93。離散卡爾曼濾波器算法我們先給出卡爾曼濾波器的總體性概述，然后討論方程式的具體細節(jié)及其作用?？柭鼮V波器用反饋控制的方法估計過程狀態(tài)：濾波器估計過程某一時刻的狀態(tài)，然后以（含噪聲的）測量變量的方式獲得反饋。因此卡爾曼濾波器可分為兩個部分：時間更新方程和測量更新方程。時間更新方程負責及時向

9、前推算當前狀態(tài)變量和誤差協(xié)方差估計的值，以便為下一個時間狀態(tài)構(gòu)造先驗估計。測量更新方程負責反饋也就是說，它將先驗估計和新的測量變量結(jié)合以構(gòu)造改進的后驗估計。時間更新方程也可視為預估方程，測量更新方程可視為校正方程。最后的估計算法成為一種具有數(shù)值解的預估校正算法，如圖1-1所示。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹5圖1-1:離散卡爾曼濾波器循環(huán)更新圖。時間更新方程將當前狀態(tài)變量作為先驗估計及時地向前投射到測量更新方程，測量更新方程校正先驗估計以獲得狀態(tài)的后驗估計。表1-1和表1-2分別給出了時間更新方程和測量更新方程的

10、具體形式。表1-1:離散卡爾曼濾波器時間更新方程xk1+Buk1k=AxPk=APk1AT+Q(1.9)(1.10)請再次注意表1-1中的時間更新方程怎樣將狀態(tài)估計xk和協(xié)方差估計Pk從k1時刻向前推算到k時刻。A和B來自式1.1，Q來自式1.3，濾波器的初始條件在早先的引用中討論過。表1-2:離散卡爾曼濾波器狀態(tài)更新方程TTKk=PkH(HPkH+R)1(1.11)(1.12)(1.13)xk=xk+Kk(zkHxk)Pk=(IKkH)Pk計算完時間更新方程和測量更新方程，整個過程再次重復。上一次計UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop

11、,卡爾曼濾波器介紹6算得到的后驗估計被作為下一次計算的先驗估計2。這種遞歸推算是卡爾曼濾波器最吸引人的特性之一它比其它濾波器更容易實現(xiàn)：例如維納濾波器Brown92，每次估計必須直接計算全部數(shù)據(jù)，而卡爾曼濾波器每次只根據(jù)以前的測量變量遞歸計算當前的狀態(tài)估計。圖1-2將表1-1和表1-2結(jié)合顯示了濾波器的整個操作流程。圖1-2:卡爾曼濾波器工作原理圖，由圖1-1和表1-1及表1-2結(jié)合得到。濾波器系數(shù)及調(diào)整濾波器實際實現(xiàn)時，測量噪聲協(xié)方差R一般可以觀測得到，是濾波器的已知條件。觀測測量噪聲協(xié)方差R一般是可實現(xiàn)的（可能的），畢竟我們要觀測整個系統(tǒng)過程。因此通常我們離線獲取一些系統(tǒng)觀測值以計算測量噪

12、聲協(xié)方差。通常更難確定過程激勵噪聲協(xié)方差的Q值，因為我們無法直接觀測到過程信號xk。有時可以通過Q的選擇給過程信號“注入”足夠的不確定性來建立一個簡單的（差的）過程模型而產(chǎn)生可以接受的結(jié)果。當然在這種情況下人們希望信號觀測值是可信的。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹7通常離線進行，并經(jīng)常與另一個（確定無誤的）在線濾波器對比，這個過程稱為系統(tǒng)識別。在討論的結(jié)尾，我們指出在Q和R都是常數(shù)的條件下，過程估計誤差協(xié)方差R和卡爾曼增益Kk都會快速收斂并保持為常量（參照圖1-2中的更新方程）。若實際情況也如此，那么濾波器系數(shù)便

13、可以通過預先離線運行濾波器計算，或者，比如說，用Grewal93中的方法計算Pk的穩(wěn)定值。實際中，觀測誤差R尤其不易保持不變。例如，用我們的光電跟蹤儀觀察掛在房間頂棚面板上的信號燈時，較近的信號燈會比較遠的信號燈具有較小的觀測噪聲。不僅是觀測噪聲會變化，有時過程激勵噪聲協(xié)方差Q也會隨著濾波器運行而動態(tài)變化這樣Q變成了Qk來適應不同的動態(tài)狀態(tài)。例如，在跟蹤三維虛擬環(huán)境中用戶頭部位置時，如果用戶頭部緩慢移動，我們會減小Qk的幅度，如果移動開始快速變化，則增加幅度。在這些情況下，Qk的幅度要根據(jù)用戶的移動方向和模型的不確定性來選擇。2擴展卡爾曼濾波器被估計的過程信號如第一節(jié)所述，卡爾曼濾波器估計一個

14、用線性隨機差分方程描述的離散時間過程的狀態(tài)變量xn。但如果被估計的過程和（或）觀測變量與過程的關(guān)系是非線性的，那應怎么辦？一些最著名和有趣的卡爾曼濾波應用就是處理這些情況的。將期望和方差線性化的卡爾曼濾波器稱作擴展卡爾曼濾波器（ExtendedKalmanFilter），簡稱EKF。示。假設(shè)過程仍具有狀態(tài)向量xn，但其狀態(tài)方程已變?yōu)榉蔷€性隨機差分方程的形式。xk=f(xk1,wk1),觀測變量zm為：zk=h(xk,vk),(2.2)(2.1)隨機變量wk和vk仍代表過程激勵噪聲和觀測噪聲。差分方程式2.1中的非線性函數(shù)f將上一時刻k1的狀態(tài)映射到當前時刻k的狀態(tài)。量測方程2.2中的驅(qū)動函數(shù)u

15、k和零均值過程噪聲wk是它的參數(shù)。非線性函數(shù)h反映了狀態(tài)變量xk和觀測變量zk的關(guān)系。實際中我們顯然不知道每一時刻噪聲wk和vk各自的值。但是，我們可以將它們假設(shè)為零，從而估計狀態(tài)向量和觀測向量為：xk=f(xk1,uk1,0)和UNC-ChapelHill,TR95-041,July24,2006(2.3)Welch&Bishop,卡爾曼濾波器介紹8zk=h(xk,0),(2.4)其中，xk是過程相對前一時刻k的后驗估計。有一點非常重要，那就是擴展卡爾曼濾波器的一個基本缺陷：離散隨機變量的分布（或連續(xù)隨機變量的密度）在經(jīng)過非線性系統(tǒng)轉(zhuǎn)化后不再是正態(tài)的了。擴展卡爾曼濾波器其實就是一個通過線性化

16、而達到漸進最優(yōu)貝葉斯決策的特殊狀態(tài)估計器。Julier96中描述了一項有趣的研究，Julieretal.設(shè)計了擴展卡爾曼濾波器的一種變體，使得通過非線性轉(zhuǎn)換后的隨機變量仍具有正態(tài)分布特性。濾波器的計算原型xkxk+A(xk1xk1)+Wwk1,zkzk+H(xkxk)Vvk.其中，xk和zk是狀態(tài)向量和觀測向量的真值，A是f對x的偏導的雅可比矩陣：Ai,jfi=(xk1,uk1,0),xj(2.5)(2.6)W是f對w的偏導的雅可比矩陣：Wi,j=fi(xk1,uk1,0),wjH是h對x的偏導的雅可比矩陣：Hi,j=hi(xk,0),xjUNC-ChapelHill,TR95-041,Jul

17、y24,2006Welch&Bishop,卡爾曼濾波器介紹9V是h對v的偏導的雅可比矩陣：Vi,j=hi(xk,0).vj簡單起見，我們并沒有在A,W,H,V中加入下標k，但它們實際上是隨時間變化的?，F(xiàn)在我們定義一個新的預測誤差的表達式：exkxkxk,和觀測變量的殘余，ezkzkzk,(2.8)(2.7)exkA(xk1xk1)+k,ezkHexk+k,(2.9)(2.10)xk=xk+ek.(2.11)p(exk)N(0,EexkeTxk)p(k)N(0,WQkWT)p(k)N(0,VRkVT)令ek的估計值為零，由以上近似，可以寫出估計ek的卡爾曼濾波器表達式：ek=Kkezk.(2.1

18、2)UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹10xk=xk+Kkezk=xk+Kk(zkzk)(2.13)表2-1和表2-2給出了擴展卡爾曼濾波器的全部表達式。注意我們用xk替換了xk來表達先驗概率的意思，并且雅可比矩陣A,W,H,V也被加上了下標，顯式地表明了它們在不同的時刻具有變化的值，每次需要被重復計算。表2-1:擴展卡爾曼濾波器時間更新方程xxk1,uk1,0)k=f(2.14)(2.15)TPk=AkPk1ATk+WkQk1Wk表2-2:擴展卡爾曼濾波器狀態(tài)更新方程TTKk=PkHk(HkPkHk+VkRkVkT

19、)1(2.16)(2.17)(2.18)xk=xxk+Kk(zkh(k,0)Pk=(IKkHk)Pk擴展卡爾曼濾波器的基本運行流程與圖1-1中的線性離散卡爾曼濾波器相同。圖2-1將圖1-1與表2-1和表2-2中的表達式結(jié)合。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹11圖2-1:擴展卡爾曼濾波器工作原理圖，由圖1-1和表2-1及表2-2結(jié)合得到。擴展卡爾曼濾波器的一個重要特性是卡爾曼增益Kk的表達式中的雅可比矩陣Hk能夠正確地傳遞或“加權(quán)”觀測信息中的有用部分。例如，如果通過h觀測變量zk和狀態(tài)變量沒有一一對應的關(guān)系，雅可比

20、矩陣Hk便通過改變卡爾曼增益從而使得殘余zkh(xk,0)中真正作用于狀態(tài)變量的部分被加權(quán)。當然，如果整個觀測中觀測變量zk和狀態(tài)變量通過h都沒有一個一一對應的關(guān)系，那么濾波器很快就會發(fā)散。這種情況下過程是不可觀測的。3卡爾曼濾波器實踐：估計隨機常數(shù)過程模型我們可以測量這個常數(shù)的幅值，但觀測幅值中摻入了幅值均方根（Root-UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹12Mean-Square，RMS）為0.1伏的白噪聲（比如在模數(shù)轉(zhuǎn)換器不是很準確的情況下）。下面的線性差分方程描述了整個過程：xk=Axk1+Buk1+wk=x

21、k1+wk觀測變量z1為：zk=Hxk+vk=xk+vk.,過程的狀態(tài)不隨時間變化，所以A=1；沒有控制輸入，所以u=0；包含噪聲的觀測值是狀態(tài)變量的直接體現(xiàn)，所以H=1。（注意有些地方我們忽略了下標k，因為對應的系數(shù)在這個例子中為常數(shù)。）濾波器方程和參數(shù)時間更新方程為：xk1,k=xPk=Pk1+Q.測量更新方程為：Kk=Pk(Pk+R)1Pk=Pk+R,(3.1)xk=xk+Kk(zkxk),Pk=(IKk)Pk.假設(shè)過程激勵噪聲方差Q非常小，Q=105（也可以令Q=0，但是一個小的非零常數(shù)可以方便地調(diào)整濾波器參數(shù)，下面將會證明）。再假設(shè)由經(jīng)驗我們知道隨機常數(shù)的真值具有標準正態(tài)分布，因此我

22、們令濾波器的初始條件為零，即xk1=0。類似地，我們要選擇Pk1的初值P0。如果確定初始狀態(tài)估計x0=0，可以令P0=0。但因為初始狀態(tài)估計x0并不確定，令P0=0可能會使濾波器一直產(chǎn)生xk=0的結(jié)果。就像實驗驗證的那樣，P0的選擇并不關(guān)鍵，幾乎任何P0=0都會使濾波器最終收斂。在這里我們令P0=1。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹13模擬實驗首先我們令常標量x=0.37727（因為x是真值所以并沒有符號）。然后產(chǎn)生50個不同的觀測值zk，其誤差為正態(tài)分布，期望為0，標準偏移為0.1（先前我們假設(shè)觀測值摻進了幅值均

23、方根為0.1伏的白噪聲）。我們本可以在濾波器運行時產(chǎn)生這些觀測值，但預先準備好這些觀測值然后再使用在幾組不同的模擬中可以讓對照更有意義。第一組實驗中我們固定測量方差為R=(0.1)2=0.01。因為這正好是預先產(chǎn)生的觀測誤差的方差的真值，所以在響應速度和估計方差方面這組實驗應該具有最好的性能。這在與第二、三組實驗的對比中更能顯現(xiàn)出來。圖3-1畫出了第一組實驗的結(jié)果。實線代表隨機變量的真值x=0.37727，加號代表預先產(chǎn)生的觀測噪聲，剩下的曲線是濾波器的估計結(jié)果。圖3-1:第一組實驗：R=(0.1)2=0.01。實線代表隨機變量的真值x=0.37727，加號代表預先產(chǎn)生的觀測噪聲，剩下的曲線是

24、濾波器的估計結(jié)果。前面討論P0的選擇時我們提到過只要P0=0其取值并不是特別關(guān)鍵，因為最終濾波器總要收斂。下面的圖3-2給出了每次重復迭代后的Pk值。第50次迭代后它已從最初的1下降到0.0002（平方伏特）了。UNC-ChapelHill,TR95-041,July24,2006圖3-2:第50次迭代后初始誤差協(xié)方差Pk已從最初的1下降到0.0002（平方伏特）了。第一節(jié)“濾波器系數(shù)和調(diào)制”中我們簡單地討論了改變或調(diào)整系數(shù)Q和R對性能的影響。圖3-3和圖3-4顯示了將R增大或減少100倍的結(jié)果。圖3-3中觀測方差擴大到100倍（也就是R=1），因此濾波器收斂地更慢。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹15圖3-3:第二次實驗：R=1。濾波器對觀測值的反應變慢，導致估計方差減小。圖3-4中觀測方差減小了100倍（也就是R=0.0001），因此濾波器收斂地更快。UNC-ChapelHill,TR95-041,July24,2006Welch&Bishop,卡爾曼濾波器介紹16圖3-4:第三次實驗：R=0.0001。濾波器對觀測值的反應變快，導致估計方差增大。對常數(shù)的估計相對比較直接，這樣便清晰地顯示了卡爾曼濾波器的工作性能。特別是在圖3-3中，估計值要