卡爾曼濾波器介紹

1、卡爾曼濾波器介紹摘要在1960年，R.E.Kalman發(fā)表了關于遞歸解決線性離散數(shù)據(jù)濾波器的著名論文，從那時間起，由于在數(shù)字計算的大部分提高，Kalman濾波器已成為廣泛研究和應用的學科，尤其是自動或輔助導航系統(tǒng)。Kalman濾波器是一套數(shù)學等式，它提供了一種有效的以最小均方誤差來估計系統(tǒng)狀態(tài)的計算（遞歸的）方法。它在以下幾方面是非常強大的：它支持過去、現(xiàn)在、甚至將來估計，甚至在系統(tǒng)準確模型也未知的情況下。本文的目的是提供一種對離散的Kalman濾波器的實用介紹。這些介紹包括對基本離散kalman濾波器、起源和與之相關的簡單（有形）的帶有真實數(shù)字和結果的描述和討論。1、離散的kalman濾波器

2、在I960年，R.E.Kalman發(fā)表了關于遞歸解決線性離散數(shù)據(jù)濾波器的著名論文，從那時間起，由于在數(shù)字計算的大部分提高，Kalman濾波器已成為廣泛研究和應用的學科，尤其是自動或輔助導航系統(tǒng)。關于kalman濾波器一般方法的友好介紹可以在maybeck79的Chapter.1中找到，但是更完整部分的討論能在Sorenson70中發(fā)現(xiàn)，它還包括許多有趣的歷史解釋。在Gelb74;Grewal93;Maybeck79;Lewis86;Brown92;jacobs93中有更多爹苦。估值過程Kalman濾波器解決估計離散時間控制過程的狀態(tài)X玳”的一般性問題，定義線性隨機差分方程4=出*1+h，a-i

3、,（I/）其中，測量值ZCRm,定義為4+以,（1.2）隨機變量Wk和Vk各自表示系統(tǒng)噪聲和測量噪聲，我們假定它們?yōu)橄嗷オ毩⒌?、白噪聲且為正常概率分布Q）,（13）-K）.（1.4）在實際中，系統(tǒng)噪聲協(xié)方差矩陣Q和測量噪聲協(xié)方差矩陣R可能隨過程和測量時間而改變，無論怎樣，我們在這里假定它們是常量。在差分方程（1.1）中，n沏階矩陣A與前一時刻（K-1）和當前時刻K相關，這里缺少傳遞函數(shù)或系統(tǒng)噪聲。注意的是，在實際中，A可能隨各自時刻改變，但這里我們假定其為常量，nM階矩陣R與非強制性輸入U貝1和狀態(tài)x有關，在測量公式（1.2）中，mxn階矩陣H與狀態(tài)及測量值Zk有關，在實際中，H可能隨各自過程

4、或測量時刻而改變，這里假定它們是常數(shù)。濾波器計算初步我們定義XCRn（注意負號）為k時刻及系統(tǒng)k時刻以前數(shù)據(jù)的priori狀態(tài)估計，定義Xk-CRn在得到測量值Zk的k時刻的posteriori狀態(tài)估計。我們這時定義前后兩狀態(tài)的估計誤差為ck=and-L4一維這時priori估計協(xié)方差為Pk-£丞門，（1.5）并且posteriori估計協(xié)方誤差為4二EVekek'（L6）在推導kalman濾波器方程時，我們開始找到Posteriori狀態(tài)估計Xk與priori估計Xk-和實際測量值Zk與預測值Hx之差的加權的線性組合的公式，如式（1.7）。對于（1.7）的一些調整在下面的“

5、濾波器的概率初步”中給出。xk=短十長仁無一臬）（1.7）式（1.7）中（ZkHxk）的差叫測量協(xié)方差或叫余數(shù)，這余數(shù)反映的是預測值Hxk與實際值Zk的不合。一個零余數(shù)意味著這兩個數(shù)完全一致。式（1.7）中nxm階矩陣選擇Posteriori協(xié)方誤差的最小增益或混合因子，這最小值可以獲得：首先代式（1.7）到上面定義的ek，代入到（1.6）中，得到期望值，然后然后推導期望結果K的跡，并設其為0,最后解得Ko對于更詳細的看Maybeck79;Brown92;Jacobs93。最小化式（1.6）的結果K的一種形式如下PHtHPHT+RA.從（1.8）中，我們可以看到測量均方誤差R趨于0時，增益K加

6、權余數(shù)會越大，尤其limKl=H_1.此一0另一方面，當Priori估計協(xié)方誤差P-趨于。時，增益k加權余數(shù)越小，尤其limKk=0.o考慮加權K的另一種方法：當測量協(xié)方誤差R趨于0時，真實測量值Zk越來越真實，這時，預測值Hx1越來越不真實，另一方面，當Priori估計協(xié)方誤差Pl趨于0時，真實測量值Zk越來越不真實，預測值Hxj越來越不真實。濾波器概率初步式（1.7）的調整來源制約于在先前測量值Zk（Bayes準則）上Priori估計X的概率。此時，我們足夠指出：Kalman濾波器保持了分布狀態(tài)的一、二階矩。£鼻|=xk研每-）（4-針=Pf式（1.7）的Posteriori狀態(tài)

7、估計反映了分布狀態(tài)的均值（一階矩）這是在條件（1.3）和（1.4）同時滿足的自然分布。Posteriori估計協(xié)方誤差（1.6）反映分布狀態(tài)的變化（二階非中心矩），換之，p（x&E£）N（£|x/E（xk-xk）（xk-xk）TI）對于Kalman濾波器的更詳細的概率初步，可以參考Maybeck79;Brown92;Jacobs93。離散Kalman濾波器算法我們從大體概述了一種包含離散Kalman濾波器形式的高級算法來開始這部分（看以前腳注）。在描述完它的高級目的之后，我們將在濾波器的本文集中到特定的公式和應用。Kalman濾波器是用反饋控制的形式來估計過程：在當

8、時濾波器估計過程狀態(tài)，然后在噪聲測量值時獲得反饋。比如，Kalman濾波器的等式有兩組：timeupdate等式和measurementupdate等式。這timeupdate等式是當前狀態(tài)之前的過程和獲得下一個時刻的Priori狀態(tài)的估計協(xié)方誤差。這measurementupdat外式反映的是反饋。如伴有新測量值的Priori狀態(tài)估計和獲得提高的Posteriori估計的組合。當measurementupdate被作為修正方程時，timeupdate也被作為原始等式。確實，最后的估計算法與解決數(shù)字問題的預測修正算法相似，如下Figure1-1所示Figure1-1不間斷離散Kalman濾波器

9、循環(huán)，Timeupdate適時計算當前狀態(tài)估計。Measurementupdate那時通過真實測量值來調整設計估計。在Table1-1和Table1-2表示暫態(tài)和穩(wěn)態(tài)方程Table1-1:DiscreteKalmanfiltertimeupdateequations.,=Axk_1十Buk_j(1.9)Pk=APk_AT+Q(1.10)再次注意，在Table1-1計劃中，無論Timeupdate方程如何，狀態(tài)和協(xié)方差估計從K-1狀態(tài)到K狀態(tài)。當Q來自式(1.3)是，A和B來自式(1.1)。濾波器的內部條件在早先的參考書中已經(jīng)討論了。Table1-2:)iscrctcKalmanfiltertn

10、casurciiieniupdateequations.K.=PiHT(HPHT+R/(1J1)l.fl1%.xk=Wq-(1J2)Pk=(1-KJ/)月(1.13)在Measurementupdate期間，最初任務是計算Kalman濾波器的增益Kk。注意的是，當式Zk。然后通1.11）和（1.8）相同時，等式已經(jīng)給出。下一步是根據(jù)真實計算過程來獲得過式（1.12）合并測量值來生成Posteriori狀態(tài)估計。式（1.12）在這里是式（1.7）的完全重復。最后通過式（1.13）來獲得Posteriori估計協(xié)方誤差。每次Timeupdate和Measurementupdate成對后，系統(tǒng)重復用

11、以前的Posteriori估計過去計劃或預測的新的Priori估值。這遞歸的本質是Kalman濾波器的一大特色它的實際應用比設計每次操作直接數(shù)據(jù)的Wiener濾波器的應用更為有效Brown92。在過去所有過去測量值的基礎上Kalman濾波器遞歸的代替當前估計。下面的Figure1-2提供了濾波器操作的完整圖片，從Table1-1和Table1-2組合成前面圖表Figure1-1。濾波器參數(shù)和調整在濾波器的實際應用中，測量噪聲協(xié)方差R通常先于濾波器操作之前測量。測量值協(xié)方誤差R一般是實際的（可能的）因為我們能夠測量過程，無論如何（當運行濾波器）為了決定測量噪聲的變化我們一般能夠得到離線例子測量值

12、。系統(tǒng)噪聲協(xié)方誤差Q的測定一般是很困難的，因為我們不能直接得到觀測估計過程。有時候相關簡單的系統(tǒng)模型能產生可能的結果，如果通過選擇Q它注入足夠不確定進入過程。的確，在這種情況下，我們希望系統(tǒng)測量值是可信的。在另一種情況，無論我們是否選擇一個有理數(shù)參數(shù)，時間前級濾波器參數(shù)（統(tǒng)計說）通過調整濾波器參數(shù)Q和R便能得到。這個調整經(jīng)常離線操作，通常的在系統(tǒng)中，另一種（明顯的）Kalman濾波器一般參考系統(tǒng)鑒定。18Measiircniait Update'(【mtect'1(1)CiimputvtheKulmaiigiin(1) PiK>ivU the slate uhcaM果=八

13、般一 i + "年-IA；=4“7（"幾"丁+長尸(2) Updult L：stimiUt: with metisurcmvtU 二4 篩=% 十七(%一耳)(3) Upilalc I he ci rt)r covuri;mcu(2) Pnycct the error ixwsuriaiKe ahedInitial estimates for 工上_】and 4r iFigure 1-2 Kalman濾波器操作的完整圖片，Table 1-1和Table 1-2組合成 前面圖表Figure 1-1。在結束時，我們注意在Q和R是常數(shù)的條件下，估計協(xié)方誤差Pk和Kalm

14、an增益Kk將快速穩(wěn)定，然后保才I常量（看Figure1-2濾波器修正公式）。如果這種場合，這些參數(shù)能在Grewal93中通過離線運行濾波器或決定Pk的穩(wěn)態(tài)值來提前計算。測量協(xié)方誤差（特別的）不能保持常數(shù)是通常情況。例如，當在我們的光電跟蹤面板看到信號是，在靠近信號的測量值比遠離信號的測量噪聲將更小。同樣，系統(tǒng)噪聲Q在濾波操作一一變成Qk期間為了調整動態(tài)差有時候也會動態(tài)的改變。例如，在跟蹤虛擬環(huán)境的使用過程情況下，如果目標移動慢，我們能夠減小Qk的量值，如果動態(tài)變化快，我們增加量值。在這種情況下，Qk能夠選擇計算不確定的用戶目的和用戶模型。2、擴展的Kalman濾波器（EKF）估值系統(tǒng)正如上一

15、節(jié)的描述，Kalman濾波器解決估計離散時間控制過程的狀態(tài)X頁n的一般性問題，定義線性隨機差分方程。但是如果被估值系統(tǒng)或系統(tǒng)的測量值關系是非線性的，會發(fā)生什么變化呢？許多Kalman濾波器重要的或成功的應用已用于這種情況。線性Kalman濾波器的當前均值和協(xié)方差可以作為EKF的參考。在類似Taylor級數(shù)的時候，即使是非線性關系時我們也能圍繞當前估計，通過系統(tǒng)的部分推導公式和測量公式計算估計來把估值線性化。為了如此，我們在本部分必須修改一些重要描述。我們再次假定系統(tǒng)有一個狀態(tài)矢量XCRn,但是，這個系統(tǒng)現(xiàn)在被定義為非線性隨機差分方程。4=/（/_1，以_，心_1）（2,1）其中，測量值ZCRm

16、,定義為二力（/，va），（22）這里，隨機變量Wk和Vk再次表示系統(tǒng)噪聲和測量噪聲。正如式（1.3）和（1.4）一樣。在這種情況下，在差分方程式（2.1）中，線性函數(shù)f與上時刻狀態(tài)K-1和當前時刻狀態(tài)K有關。它包括驅動函數(shù)Uk-i和零均值系統(tǒng)噪聲Wk的參數(shù)。在測量等式（2.2）中，非線性函數(shù)h與狀態(tài)Xk和測量值Zk有關。在實際過程中，我們不知道每個時刻的Wk和Vk的獨立值，然而，我們可以在沒有Wk和Vk的狀態(tài)下近似狀態(tài)矢量和測量矢量，如下kA£-I，ki，°）（23）andZk=M%。）.（2.4）這里，Xk是Posteriori估計狀態(tài)（從上一個時刻K開始）。重點注意：

17、EKF和基本缺陷是在遭到各自非線性變換后，不同的隨機變量的分布（連續(xù)情況下的密度）不再正常。在EKF是簡單的接近線性最佳Bayes公式的特殊狀態(tài)估值。Julieretal.已經(jīng)通過用非線性變換來優(yōu)質正常分布來了發(fā)展了EKF變量Julier962.3）和（2.4）方（2.6）濾波器計算初步為了估計非線性系統(tǒng)差分值和測量值的關系，我們重新寫線性估計式（程的控制方程，xkxk+A（xk_-xk_x）+Wwk_,“0:#+一晨）+vk-這里Xk和Zk是真實狀態(tài)和測量矢量,Xk和Zk是由式（2.3）和（2.4）而得到近似狀態(tài)和測量值矢量,Xk是K時刻的Posteriori估計犬態(tài),隨機變量Wk和Vk表示

18、在（1.3）和的（1.4）的系統(tǒng)噪聲和測量噪聲A是關于X的由f部分派生的Jacobian矩陣,定義為W是關于w的由f部分派生的Jacobian矩陣,定義為a/R".力"工用一，以一I，°）dHjH是關于X的由h部分派生的Jacobian矩陣,定義為（果,0）,V是關于v的由h部分派生的Jacobian矩陣，定義為二”在這種，f#況下，簡單注意，我們不能用Jacobians的A,W,H,的時間下標，即使在各自時刻真正不同?，F(xiàn)在我們?yōu)轭A測誤差定義一個新符號和測量余數(shù),記得，在實際中，式2.7不能接近Xk,它便是真實狀態(tài)矢量，例如，要估計的量。另一方面，式2.8不能接近

19、Xk,它是用Xk估計真實測量值。用式（2.7）和（2.8）我們能寫系統(tǒng)誤差的控制方程，如下=八（4_臬（2-9）=Hex+“火，（2.10）這里，ek和Yk表示新的有零均值和協(xié)方差WQWT和VRVT并同帶有Q和R的式（1.3）和式（1.4）一樣的獨立隨機變量。注意的是等式（2.9）和等式（2.10）是線性的，從離散Kalman濾波器我們得到真得得到類似的差分方程和測量等式（1.1）和（1.2）。這種激勵在式（2.8）用真實測量值余數(shù)Ezk和第二（假定的）Kalman濾波器來估計預測誤差Exk由式（2.9）給出，然后這叫Ek測量能連同式（2.7）被用來獲得原始非線性系統(tǒng)的Posteriori狀態(tài)

20、估計，如下xk=+0.（2JI）式（2.9）和（2.10）的隨機變量有近似的下面可能的分布（看以前腳注）：p（J）N（0,Eeej（以）MSW。/*）（山）N（0,%yT）給定一些ek的近似值和預測值為0,用來估計ek的Kalman濾波器等式是ek二KkeZk.（2.12）把式（2.12）代回（2.11）,利用（2.8）,我們可以看到，實際不用兩個Kalman濾波器。（2J3）用M院+式（2.13）在擴展的Kalman濾波器中用作Measurementupdate,其中Xk和Zk來源于式（2.3）和式（2.4）,Kalman增益Kk來自帶有測量協(xié)方差的特有代替式（1.11）。EKF完整等式如下

21、Talble2-1和Table2-2所示。注意，我們用Xk一代替Xk,并且保持了與以前上標負號的一致?，F(xiàn)在我們給JacobiansA,W,H,V,附加下標k來標注他們在各個時刻的不同。Table2-1:EKFtimeupdateequations.戈支二，f（X#_I，"A_I，。）（2.14P"（2.15）如同基本的離散Kalman濾波器，在Table2-1中的Timeupdate等式計算從前一時刻K-1到當前時刻K的估計狀態(tài)和協(xié)方差。此外，式（2.14）的f來源于式（2.3）,Ak和Wk是K時刻的系統(tǒng)Jacobians,Qk是K時刻的系統(tǒng)噪聲協(xié)方差。luhlc2-2:E

22、KF（tKasiircn）entupdatecquauons.初八"心以十y*丸=窺+如一雙耳。）（2J7）Pt=（2J8）如同基本離散Kalman濾波器，Table2-2中Measurementupdate等式修正了測量值Zk的估計狀態(tài)和協(xié)方差。此外，式（2.17）的h來自式（2.4）,Hk和V是K時刻的測量值Jacobians,Rk是測量噪聲協(xié)方差（注意，現(xiàn)在R的下標允許隨每個測量值而改變）。EKF的基本算法同線性離散Kalman濾波器Figure1-1所示的一樣，下面的合并了前面表格Figure1-1和Table2-1和Table2-2等式的Figure2-1提供了EKF算法的

23、完整描述。Tinn- I ptUtc rlVwlkt'4)(1 I Projetl the iLuIr JkikI(I) Compute ihe KJ man gin臬= 皂_ 1,O)(2) theerrnr «natidrte desd8 -+叫。*7啤42 lPpddite csliTTLiilc wi ih mcjiMJircmenl .% = £+ 馬(q-雙耳。)(3) Updjrie die coot covariance尸* =。-勺HJP上Figure 2-1合并了高級表格Figure1-1和Table2-1和Table2-2等式的EKF的完整描述

24、EKF的重要特征是正常增大或放大相關測量數(shù)據(jù)的Kalman增益Kk等式中的Jacobians例如，如果測量值Zk和測量狀態(tài)通過h不是一對一的映射，JacobianHk將影響Kalman增益，以致于精僅僅放大了影響因素的XK-h(XK,0)余數(shù)的部分。當然，如果測量值Zk和測量狀態(tài)通過h都不是一對一的映射關系，你可以很快預測到濾波器是發(fā)散的，這種情況是不可觀測的。3、Kalman濾波器的應用：估計隨機常量在前兩節(jié)中，我們描述了離散Kalman和擴展Kalman濾波器的基本形式，為了更好的了解濾波器的運算和性能，我們在這里舉一個簡單的例子。系統(tǒng)模型在這個簡單例子中，我們估計一個隨機常標量，例如，電

25、壓。假設，我們能夠獲得測量常數(shù)，但是測量值是被均方根為0.1的白噪聲破壞(例如，從模擬到數(shù)字轉換是不準確的)。在這個例子中，系統(tǒng)為線性差分方程=兒以-1+8%-】:叫="1+股'其中，測量值ZkCR1,并定義:*=H“+Vk=4+Aflb在種狀態(tài)不隨時刻而變化，因此A=0。這里沒有控制輸入，因此u=0。噪聲測量值為直接狀態(tài)，于是H=1。（注意，我們在許多地方?jīng)]有考慮下標，這是因為在簡單模型中，各參數(shù)均為常數(shù)）濾波器等式和參數(shù)Timeupdate等式為%=和Measurementupdate等式為勺=尸R（3.1）露=+4二U-號吠假設一個很小的系統(tǒng)變化，我們使Q=1e-5。（

26、我們能夠確定Q=0,但是為了更好的調整濾波器，假定一個很小但又不為0的值，下面我們會給出證明）。根據(jù)經(jīng)驗知道，隨機常量的真實值有標準自然概率分布，于是我們定義濾波器常量為0,換句話說，工作前，我們使Xk-i=0。類似的，我們需要選擇Pk-i的初始值，如果我們完全確定初始化狀態(tài)估計Xo=0是正確的，那么Po=0o然而，初始估計Xo是不確定的，選擇Po=0能起濾波器初始化和使Xk=0o于是證明，二者的選擇是臨界的，我們能夠選擇任何PoR,最終，濾波器是收斂的，我們以Po=1開始。仿真開始，我們隨機選擇一個標量Z=0.37727。（Z不是hat'；因為它表示真實值）。然后,我們模擬50個不同

27、的標準偏差為0.1的零自然誤差分布的測量值Zko（記得，我們假設測量值被均方根為0.1的白噪聲破壞）。我們只有在同一準確測量情況下的一系列的50個仿真值能在濾波器循環(huán)內得到單獨的測量值（例如，相同的測量噪聲）。于是在不同參數(shù)的模擬的比較是很有用的。在第一次仿真時，我們確定了在R=（0.1）2=0.01時的測量協(xié)方差。因為這是真實測量協(xié)在第二次和第三次仿真中，將有更x= 0.37727的真實值已經(jīng)在實方誤差，我們根據(jù)平衡響應和估計方差來預測最優(yōu)特性。多的證據(jù)。Figure3-1描述了第一次仿真的結果。隨機常量線上給定了，噪聲值為“+”標記，濾波器估計仍保持曲線。tlj1020304050Iter

28、ationFigure3-1第一次仿真：R=（0.1）2=0.01。隨機常量x=0.37727的真實值已經(jīng)在實線上給定了，噪聲值為標記，濾波器估計仍保持曲線。當考慮到上面選擇P0時，我們提到：這選擇在P0卻時候不是臨界的，因為濾波器最終會收斂。下面Figure3-2中，我們畫出了相對于重復的Pk的值。通過第50個重復，它解決了從1到近似0.0002的最初選擇。Figure 3-2 50個重復后，我們最初協(xié)方誤差 Pk從1到0.0002的調整。在“濾波器參數(shù)和調整”那節(jié)中，我們簡要討論了為了獲得不同濾波器特性而改變或調整參數(shù)Q和R。在下面Figure3-3和Figure3-4中，我們能看到當R各

29、自以100的因子增加或減小時，濾波器是如何改變的。在Figure3-3中，濾波器告訴測量方差是大100次（例如,R=1）。于是假定測量值為慢的。Figure3-3第二次仿真：R=1。濾波器響應測量較慢，導致減小估計方差在Figure 3-4中，濾波器說明：測量方差小于100次（例如：R=0.0001 ）于是假定噪聲測量值是非?？臁igure 3-4第三次仿真：R=0.0001。濾波器快速響應測量值，增加估計方差雖然，常數(shù)的估計是相對直接的。它明顯的證明Kalman濾波的運轉。特另LFigure3-3中，Kalman濾波是明顯的，因為估計出現(xiàn)比噪聲測量明顯平滑。RefercneesBrukii

30、92BrinMi,R.G.andP.Y.C.H*ung-H為？.hitreKlucfitmit)RifiuitniiSignalsandAppliedKalmanFiltering,SecondEdhwntJohnWiley&SonstInc.Gclb74Grewal93GdbtA.1974-AppliedOptimalEstimation,MITGess,Cambridge.MA.Grewal,MohinderS.tandAngusP.Andrews(1993).KalmanFilteringTheoryandJlacticc.LppcrSaddleRier.NJUSAPrcnlieeHidLJacobs93Jaeobji.O.L.R.1993.hiutKluciiontoControlllicoiy.2ndEdition.O.xfordUtiiversilyPress.Julicr%Ijlkr.SinumundJeflrcyIhliiiiiii-bAGcutralhieIhndufAp