譯文--卡爾曼濾波器介紹

1、<p><b>　　卡爾曼濾波器介紹</b></p><p><b>　　摘要</b></p><p>　　在1960年，R.E.Kalman發(fā)表了關(guān)于遞歸解決線性離散數(shù)據(jù)濾波器的著名論文，從那時間起，由于在數(shù)字計算的大部分提高，Kalman濾波器已成為廣泛研究和應(yīng)用的學(xué)科，尤其是自動或輔助導(dǎo)航系統(tǒng)。</p><p&

2、gt;　　Kalman濾波器是一套數(shù)學(xué)等式，它提供了一種有效的以最小均方誤差來估計系統(tǒng)狀態(tài)的計算(遞歸的)方法。它在以下幾方面是非常強(qiáng)大的：它支持過去、現(xiàn)在、甚至將來估計，甚至在系統(tǒng)準(zhǔn)確模型也未知的情況下。</p><p>　　本文的目的是提供一種對離散的Kalman濾波器的實(shí)用介紹。這些介紹包括對基本離散kalman濾波器、起源和與之相關(guān)的簡單(有形)的帶有真實(shí)數(shù)字和結(jié)果的描述和討論。</p>&

3、lt;p>　　1、離散的kalman濾波器</p><p>　　在1960年，R.E.Kalman發(fā)表了關(guān)于遞歸解決線性離散數(shù)據(jù)濾波器的著名論文，從那時間起，由于在數(shù)字計算的大部分提高，Kalman濾波器已成為廣泛研究和應(yīng)用的學(xué)科，尤其是自動或輔助導(dǎo)航系統(tǒng)。關(guān)于kalman濾波器一般方法的友好介紹可以在〔maybeck79〕的 Chapter.1中找到，但是更完整部分的討論能在〔Sorenson70〕中發(fā)現(xiàn)

4、，它還包括許多有趣的歷史解釋。在〔Gelb74；Grewal93；Maybeck79；Lewis86；Brown92；jacobs93〕中有更多參考。</p><p><b>　　估值過程</b></p><p>　　Kalman濾波器解決估計離散時間控制過程的狀態(tài)X∈Rn的一般性問題，定義線性隨機(jī)差分方程</p><p>　　其中，測量值Z∈

5、Rm，定義為</p><p>　　隨機(jī)變量WK和VK各自表示系統(tǒng)噪聲和測量噪聲，我們假定它們?yōu)橄嗷オ?dú)立的、白噪聲且為正常概率分布</p><p>　　在實(shí)際中，系統(tǒng)噪聲協(xié)方差矩陣Q和測量噪聲協(xié)方差矩陣R可能隨過程和測量時間而改變，無論怎樣，我們在這里假定它們是常量。</p><p>　　在差分方程（1.1）中，n×n階矩陣A與前一時刻（K－1）和當(dāng)前時刻K

6、相關(guān)，這里缺少傳遞函數(shù)或系統(tǒng)噪聲。注意的是，在實(shí)際中，A可能隨各自時刻改變，但這里我們假定其為常量，n×l階矩陣R與非強(qiáng)制性輸入U∈Rl和狀態(tài)x有關(guān)，在測量公式（1.2）中，m×n階矩陣H與狀態(tài)及測量值ZK有關(guān)，在實(shí)際中，H可能隨各自過程或測量時刻而改變，這里假定它們是常數(shù)。</p><p><b>　　濾波器計算初步</b></p><p>　　

7、我們定義XK－∈Rn（注意負(fù)號）為k時刻及系統(tǒng)k時刻以前數(shù)據(jù)的priori狀態(tài)估計，定義XK－∈Rn在得到測量值ZK的k時刻的posteriori狀態(tài)估計。我們這時定義前后兩狀態(tài)的估計誤差為</p><p>　　這時priori估計協(xié)方差為</p><p>　　并且posteriori估計協(xié)方誤差為</p><p>　　在推導(dǎo)kalman濾波器方程時，我們開始找到P

8、osteriori狀態(tài)估計XK與priori估計XK－和實(shí)際測量值ZK與預(yù)測值Hxk－之差的加權(quán)的線性組合的公式，如式（1.7）。對于（1.7）的一些調(diào)整在下面的“濾波器的概率初步”中給出。</p><p>　　式（1.7）中（ZK－Hxk）的差叫測量協(xié)方差或叫余數(shù)，這余數(shù)反映的是預(yù)測值Hxk與實(shí)際值Zk的不合。一個零余數(shù)意味著這兩個數(shù)完全一致。</p><p>　　式（1.7）中n

9、15;m階矩陣選擇Posteriori協(xié)方誤差的最小增益或混合因子，這最小值可以獲得：首先代式（1.7）到上面定義的ek ，代入到（1.6）中，得到期望值，然后然后推導(dǎo)期望結(jié)果K的跡，并設(shè)其為0，最后解得K。對于 更詳細(xì)的看〔Maybeck79；Brown92；Jacobs93〕。最小化式（1.6）的結(jié)果K的一種形式如下</p><p>　　從（1.8）中，我們可以看到測量均方誤差R趨于0時，增益K加權(quán)余數(shù)會越大

10、，尤其</p><p>　　另一方面，當(dāng)Priori估計協(xié)方誤差PK－趨于0時，增益k加權(quán)余數(shù)越小，尤其</p><p>　　考慮加權(quán)K的另一種方法：當(dāng)測量協(xié)方誤差R趨于0時，真實(shí)測量值ZK越來越真實(shí)，這時，預(yù)測值Hxk－越來越不真實(shí)，另一方面，當(dāng)Priori估計協(xié)方誤差PK－趨于0時，真實(shí)測量值Zk越來越不真實(shí)，預(yù)測值Hxk－越來越不真實(shí)。</p><p><

11、;b>　　濾波器概率初步</b></p><p>　　式（1.7）的調(diào)整來源制約于在先前測量值ZK（Bayes準(zhǔn)則）上Priori估計XK－的概率。此時，我們足夠指出：Kalman濾波器保持了分布狀態(tài)的一、二階矩。</p><p>　　式（1.7）的Posteriori狀態(tài)估計反映了分布狀態(tài)的均值（一階矩）——這是在條件（1.3）和（1.4）同時滿足的自然分布。Poste

12、riori估計協(xié)方誤差（1.6）反映分布狀態(tài)的變化（二階非中心矩），換之，</p><p>　　對于Kalman濾波器的更詳細(xì)的概率初步，可以參考〔Maybeck79；Brown92；Jacobs93〕。</p><p>　　離散Kalman濾波器算法</p><p>　　我們從大體概述了一種包含離散Kalman濾波器形式的高級算法來開始這部分（看以前腳注）。在描述

13、完它的高級目的之后，我們將在濾波器的本文集中到特定的公式和應(yīng)用。</p><p>　　Kalman濾波器是用反饋控制的形式來估計過程：在當(dāng)時濾波器估計過程狀態(tài)，然后在噪聲測量值時獲得反饋。比如，Kalman濾波器的等式有兩組：time update等式和measurement update等式。這time update等式是當(dāng)前狀態(tài)之前的過程和獲得下一個時刻的Priori狀態(tài)的估計協(xié)方誤差。這measuremen

14、t update等式反映的是反饋。如伴有新測量值的Priori狀態(tài)估計和獲得提高的Posteriori估計的組合。</p><p>　　當(dāng)measurement update被作為修正方程時，time update也被作為原始等式。確實(shí)，最后的估計算法與解決數(shù)字問題的預(yù)測－修正算法相似，如下Figure 1-1所示</p><p>　　Figure 1-1 不間斷離散Kalman濾波器循環(huán)

15、，Time update適時計算當(dāng)前狀態(tài)估計。Measurement update在那時通過真實(shí)測量值來調(diào)整設(shè)計估計。</p><p>　　在Table 1-1和Table 1-2表示暫態(tài)和穩(wěn)態(tài)方程</p><p>　　再次注意，在Table 1-1計劃中，無論Time update方程如何，狀態(tài)和協(xié)方差估計從K-1狀態(tài)到K狀態(tài)。當(dāng)Q來自式（1.3）是，A和B來自式（1.1）。濾波器的內(nèi)部

16、條件在早先的參考書中已經(jīng)討論了。</p><p>　　在Measurement update期間，最初任務(wù)是計算Kalman濾波器的增益Kk。注意的是，當(dāng)式（1.11）和（1.8）相同時，等式已經(jīng)給出。下一步是根據(jù)真實(shí)計算過程來獲得Zk。然后通過式（1.12）合并測量值來生成Posteriori狀態(tài)估計。式（1.12）在這里是式（1.7）的完全重復(fù)。最后通過式（1.13）來獲得Posteriori估計協(xié)方誤差。&

17、lt;/p><p>　　每次Time update和Measurement update成對后，系統(tǒng)重復(fù)用以前的Posteriori估計過去計劃或預(yù)測的新的Priori估值。這遞歸的本質(zhì)是Kalman濾波器的一大特色——它的實(shí)際應(yīng)用比設(shè)計每次操作直接數(shù)據(jù)的Wiener濾波器的應(yīng)用更為有效〔Brown92〕。在過去所有過去測量值的基礎(chǔ)上Kalman濾波器遞歸的代替當(dāng)前估計。下面的Figure 1-2提供了濾波器操作的完

18、整圖片，從Table 1-1和Table 1-2組合成前面圖表Figure 1-1。</p><p><b>　　濾波器參數(shù)和調(diào)整</b></p><p>　　在濾波器的實(shí)際應(yīng)用中，測量噪聲協(xié)方差R通常先于濾波器操作之前測量。測量值協(xié)方誤差R一般是實(shí)際的（可能的）因?yàn)槲覀兡軌驕y量過程，無論如何（當(dāng)運(yùn)行濾波器）為了決定測量噪聲的變化我們一般能夠得到離線例子測量值。<

19、;/p><p>　　系統(tǒng)噪聲協(xié)方誤差Q的測定一般是很困難的，因?yàn)槲覀儾荒苤苯拥玫接^測估計過程。有時候相關(guān)簡單的系統(tǒng)模型能產(chǎn)生可能的結(jié)果，如果通過選擇Q它注入足夠不確定進(jìn)入過程。的確，在這種情況下，我們希望系統(tǒng)測量值是可信的。</p><p>　　在另一種情況，無論我們是否選擇一個有理數(shù)參數(shù)，時間前級濾波器參數(shù)（統(tǒng)計說）通過調(diào)整濾波器參數(shù)Q和R便能得到。這個調(diào)整經(jīng)常離線操作，通常的在系統(tǒng)中，另一

20、種（明顯的）Kalman濾波器一般參考系統(tǒng)鑒定。</p><p>　　Figure 1-2 Kalman濾波器操作的完整圖片，Table 1-1和Table 1-2組合成前面圖表Figure 1-1。</p><p>　　在結(jié)束時，我們注意在Q和R是常數(shù)的條件下，估計協(xié)方誤差PK和Kalman增益KK將快速穩(wěn)定，然后保持常量（看Figure 1-2濾波器修正公式）。如果這種場合，這些參數(shù)

21、能在〔Grewal93〕中通過離線運(yùn)行濾波器或決定PK的穩(wěn)態(tài)值來提前計算。</p><p>　　測量協(xié)方誤差（特別的）不能保持常數(shù)是通常情況。例如，當(dāng)在我們的光電跟蹤面板看到信號是，在靠近信號的測量值比遠(yuǎn)離信號的測量噪聲將更小。同樣，系統(tǒng)噪聲Q在濾波操作——變成Qk——期間為了調(diào)整動態(tài)差有時候也會動態(tài)的改變。例如，在跟蹤虛擬環(huán)境的使用過程情況下，如果目標(biāo)移動慢，我們能夠減小QK的量值，如果動態(tài)變化快，我們增加量值

22、。在這種情況下，QK能夠選擇計算不確定的用戶目的和用戶模型。</p><p>　　2、擴(kuò)展的Kalman濾波器（EKF）</p><p><b>　　估值系統(tǒng)</b></p><p>　　正如上一節(jié)的描述，Kalman濾波器解決估計離散時間控制過程的狀態(tài)X∈Rn的一般性問題，定義線性隨機(jī)差分方程。但是如果被估值系統(tǒng)或系統(tǒng)的測量值關(guān)系是非線性的，

23、會發(fā)生什么變化呢？許多Kalman濾波器重要的或成功的應(yīng)用已用于這種情況。線性Kalman濾波器的當(dāng)前均值和協(xié)方差可以作為EKF的參考。</p><p>　　在類似Taylor級數(shù)的時候，即使是非線性關(guān)系時我們也能圍繞當(dāng)前估計，通過系統(tǒng)的部分推導(dǎo)公式和測量公式計算估計來把估值線性化。為了如此，我們在本部分必須修改一些重要描述。我們再次假定系統(tǒng)有一個狀態(tài)矢量X∈Rn，但是，這個系統(tǒng)現(xiàn)在被定義為非線性隨機(jī)差分方程。&

24、lt;/p><p>　　其中，測量值Z∈Rm，定義為</p><p>　　這里，隨機(jī)變量WK和VK再次表示系統(tǒng)噪聲和測量噪聲。正如式（1.3）和（1.4）一樣。在這種情況下，在差分方程式（2.1）中，線性函數(shù)f與上時刻狀態(tài)K-1和當(dāng)前時刻狀態(tài)K有關(guān)。它包括驅(qū)動函數(shù)UK-1和零均值系統(tǒng)噪聲Wk的參數(shù)。在測量等式（2.2）中，非線性函數(shù)h與狀態(tài)XK和測量值ZK有關(guān)。</p><

25、p>　　在實(shí)際過程中，我們不知道每個時刻的WK和VK的獨(dú)立值，然而，我們可以在沒有WK和VK的狀態(tài)下近似狀態(tài)矢量和測量矢量，如下</p><p>　　這里，Xk是Posteriori估計狀態(tài)（從上一個時刻K開始）。</p><p>　　重點(diǎn)注意：EKF和基本缺陷是在遭到各自非線性變換后，不同的隨機(jī)變量的分布（連續(xù)情況下的密度）不再正常。在EKF是簡單的接近線性最佳Bayes公式的特殊

26、狀態(tài)估值。 Julier et al.已經(jīng)通過用非線性變換來優(yōu)質(zhì)正常分布來了發(fā)展了EKF變量〔Julier96〕</p><p><b>　　濾波器計算初步</b></p><p>　　為了估計非線性系統(tǒng)差分值和測量值的關(guān)系，我們重新寫線性估計式（2.3）和(2.4)方程的控制方程,</p><p><b>　　這里</b&

27、gt;</p><p>　　XK和ZK是真實(shí)狀態(tài)和測量矢量,</p><p>　　XK和ZK是由式(2.3)和(2.4)而得到近似狀態(tài)和測量值矢量,</p><p>　　XK是K時刻的Posteriori估計狀態(tài),</p><p>　　隨機(jī)變量WK和VK表示在(1.3)和的(1.4)的系統(tǒng)噪聲和測量噪聲,</p><p&g

28、t;　　A是關(guān)于X的由 f 部分派生的Jacobian矩陣,定義為</p><p>　　W是關(guān)于 w 的由 f 部分派生的Jacobian矩陣,定義為</p><p>　　H是關(guān)于X的由 h 部分派生的Jacobian矩陣,定義為</p><p>　　V是關(guān)于 v 的由 h 部分派生的Jacobian矩陣,定義為</p><p>　　在這種情

29、況下,簡單注意,我們不能用Jacobians的A，W，H，的時間下標(biāo)，即使在各自時刻真正不同。</p><p>　　現(xiàn)在我們?yōu)轭A(yù)測誤差定義一個新符號，</p><p><b>　　和測量余數(shù)，</b></p><p>　　記得，在實(shí)際中，式2.7不能接近Ｘk，它便是真實(shí)狀態(tài)矢量，例如，要估計的量。另一方面，式2.8不能接近Ｘk，它是用Xk估計真

30、實(shí)測量值。用式（2.7）和（2.8）我們能寫系統(tǒng)誤差的控制方程，如下</p><p>　　這里，εk和ηk表示新的有零均值和協(xié)方差WQWT和VRVT并同帶有Q和R的式（1.3）和式（1.4）一樣的獨(dú)立隨機(jī)變量。</p><p>　　注意的是等式（2.9）和等式（2.10）是線性的，從離散Kalman濾波器我們得到真得得到類似的差分方程和測量等式（1.1）和（1.2）。這種激勵在式（2.8）

31、用真實(shí)測量值余數(shù)Ezk和第二（假定的）Kalman濾波器來估計預(yù)測誤差Exk由式（2.9）給出，然后這叫EK測量能連同式（2.7）被用來獲得原始非線性系統(tǒng)的Posteriori狀態(tài)估計，如下</p><p>　　式（2.9）和（2.10）的隨機(jī)變量有近似的下面可能的分布（看以前腳注）：</p><p>　　給定一些ek的近似值和預(yù)測值為0，用來估計ek的Kalman濾波器等式是</p

32、><p>　　把式（2.12）代回（2.11），利用（2.8），我們可以看到，實(shí)際不用兩個Kalman濾波器。</p><p>　　式（2.13）在擴(kuò)展的Kalman濾波器中用作Measurement update，其中XK和ZK來源于式（2.3）和式（2.4），Kalman增益KK來自帶有測量協(xié)方差的特有代替式（1.11）。</p><p>　　EKF完整等式如下Ta

33、lble 2-1和Table 2-2所示。注意，我們用Xk－代替Xk，并且保持了與以前上標(biāo)負(fù)號的一致?，F(xiàn)在我們給Jacobians A，W，H，V，附加下標(biāo)k來標(biāo)注他們在各個時刻的不同。</p><p>　　如同基本的離散Kalman濾波器，在Table2-1中的Time update等式計算從前一時刻K-1到當(dāng)前時刻K的估計狀態(tài)和協(xié)方差。此外，式（2.14）的f來源于式（2.3），Ak和Wk是K時刻的系統(tǒng)Jac

34、obians，QK是K時刻的系統(tǒng)噪聲協(xié)方差。</p><p>　　如同基本離散Kalman濾波器，Table 2-2中Measurement update等式修正了測量值Zk的估計狀態(tài)和協(xié)方差。此外，式（2.17）的h來自式（2.4），Hk和V是K時刻的測量值Jacobians，Rk是測量噪聲協(xié)方差（注意，現(xiàn)在R的下標(biāo)允許隨每個測量值而改變）。</p><p>　　EKF的基本算法同線性離

35、散Kalman濾波器Figure 1-1所示的一樣，下面的合并了前面表格Figure 1-1和Table 2-1和Table 2-2等式的Figure 2-1提供了EKF算法的完整描述。</p><p>　　Figure 2-1 合并了高級表格Figure1-1和Table2-1和Table2-2等式的EKF的完整描述</p><p>　　EKF的重要特征是正常增大或放大相關(guān)測量數(shù)據(jù)的K

36、alman增益Kk等式中的Jacobians。例如，如果測量值Zk和測量狀態(tài)通過h不是一對一的映射，JacobianHk將影響Kalman增益，以致于糨僅僅放大了影響因素的XK-h(XK,0)余數(shù)的部分。當(dāng)然，如果測量值Zk和測量狀態(tài)通過h都不是一對一的映射關(guān)系，你可以很快預(yù)測到濾波器是發(fā)散的，這種情況是不可觀測的。</p><p>　　3、Kalman濾波器的應(yīng)用：估計隨機(jī)常量</p><p

37、>　　在前兩節(jié)中，我們描述了離散Kalman和擴(kuò)展Kalman濾波器的基本形式，為了更好的了解濾波器的運(yùn)算和性能，我們在這里舉一個簡單的例子。</p><p><b>　　系統(tǒng)模型</b></p><p>　　在這個簡單例子中，我們估計一個隨機(jī)常標(biāo)量，例如，電壓。假設(shè)，我們能夠獲得測量常數(shù)，但是測量值是被均方根為0.1的白噪聲破壞（例如，從模擬到數(shù)字轉(zhuǎn)換是不準(zhǔn)確

38、的）。在這個例子中，系統(tǒng)為線性差分方程</p><p>　　其中，測量值ZK∈Rl，并定義：</p><p>　　在種狀態(tài)不隨時刻而變化，因此A=0。這里沒有控制輸入，因此u＝0。噪聲測量值為直接狀態(tài)，于是H=1。（注意，我們在許多地方?jīng)]有考慮下標(biāo)，這是因?yàn)樵诤唵文Ｐ椭校鲄?shù)均為常數(shù)）</p><p><b>　　濾波器等式和參數(shù)</b>&l

39、t;/p><p>　　Time update等式為</p><p>　　和Measurement update等式為</p><p>　　假設(shè)一個很小的系統(tǒng)變化，我們使Q=1e-5。（我們能夠確定Q=0，但是為了更好的調(diào)整濾波器，假定一個很小但又不為0的值，下面我們會給出證明）。根據(jù)經(jīng)驗(yàn)知道，隨機(jī)常量的真實(shí)值有標(biāo)準(zhǔn)自然概率分布，于是我們定義濾波器常量為0，換句話說，工作

40、前，我們使Xk-1=0。</p><p>　　類似的，我們需要選擇Pk-1的初始值，如果我們完全確定初始化狀態(tài)估計X0＝0是正確的，那么P0=0。然而，初始估計X0是不確定的，選擇P0=0能起濾波器初始化和使Xk=0。于是證明，二者的選擇是臨界的，我們能夠選擇任何P0≠0，最終，濾波器是收斂的，我們以P0=1開始。</p><p><b>　　仿真</b></p

41、><p>　　開始，我們隨機(jī)選擇一個標(biāo)量Z＝－0.37727。（Z不是“hat”，因?yàn)樗硎菊鎸?shí)值）。然后，我們模擬50個不同的標(biāo)準(zhǔn)偏差為0.1的零自然誤差分布的測量值Zk。（記得，我們假設(shè)測量值被均方根為0.1的白噪聲破壞）。我們只有在同一準(zhǔn)確測量情況下的一系列的50個仿真值能在濾波器循環(huán)內(nèi)得到單獨(dú)的測量值（例如，相同的測量噪聲）。于是在不同參數(shù)的模擬的比較是很有用的。</p><p>　　

42、在第一次仿真時，我們確定了在R=(0.1)2=0.01時的測量協(xié)方差。因?yàn)檫@是真實(shí)測量協(xié)方誤差，我們根據(jù)平衡響應(yīng)和估計方差來預(yù)測最優(yōu)特性。在第二次和第三次仿真中，將有更多的證據(jù)。Figure3-1描述了第一次仿真的結(jié)果。隨機(jī)常量x＝－0.37727的真實(shí)值已經(jīng)在實(shí)線上給定了，噪聲值為“＋”標(biāo)記，濾波器估計仍保持曲線。</p><p>　　Figure3-1 第一次仿真：R=(0.1)2=0.01。隨機(jī)常量x＝－

43、0.37727的真實(shí)值已經(jīng)在實(shí)線上給定了，噪聲值為“＋”標(biāo)記，濾波器估計仍保持曲線。</p><p>　　當(dāng)考慮到上面選擇P0時，我們提到：這選擇在P0≠0時候不是臨界的，因?yàn)闉V波器最終會收斂。下面Figure 3-2中，我們畫出了相對于重復(fù)的PK的值。通過第50個重復(fù)，它解決了從1到近似0.0002的最初選擇。</p><p>　　Figure 3-2 50個重復(fù)后，我們最初協(xié)方誤差P

44、K從1到0.0002的調(diào)整。</p><p>　　在“濾波器參數(shù)和調(diào)整”那節(jié)中，我們簡要討論了為了獲得不同濾波器特性而改變或調(diào)整參數(shù)Q和R。在下面Figure 3-3和Figure 3-4 中，我們能看到當(dāng)R各自以100的因子增加或減小時，濾波器是如何改變的。在Figure 3-3中，濾波器告訴測量方差是大100次（例如，R=1）。于是假定測量值為慢的。</p><p>　　Figure

45、3-3 第二次仿真：R=1。濾波器響應(yīng)測量較慢，導(dǎo)致減小估計方差。</p><p>　　在Figure 3-4中，濾波器說明：測量方差小于100次（例如：R=0.0001）于是假定噪聲測量值是非常快。</p><p>　　Figure 3-4 第三次仿真：R=0.0001。濾波器快速響應(yīng)測量值，增加估計方差。</p><p>　　雖然，常數(shù)的估計是相對直接的。它明顯