1、卡爾曼濾波器綜述瞿偉軍 G100741、卡爾曼濾波的起源1960 年，匈牙利數學家卡爾曼發表了一篇關于離散數據線性濾波遞推算法 的論文，這意味著卡爾曼濾波的誕生。斯坦利 . 施密特 (Stanley Schmidt) 首次實 現了卡爾曼濾波器，卡爾曼在 NASA埃姆斯研究中心訪問時，發現他的方法對于 解決阿波羅計劃的軌道預測很有用， 后來阿波羅飛船的導航電腦使用了這種濾波 器。關于這種濾波器的論文由 Swerling (1958) 、 Kalman (1960) 與 Kalman and Bucy (1961) 發表。2、卡爾曼濾波的發展卡爾曼濾波是一種有著相當廣泛應用的濾波方法， 但它既需要

2、假定系統是線 性的，又需要認為系統中的各個噪聲與狀態變量均呈高斯分布， 而這兩條并不總 是確切的假設限制了卡爾曼濾波器在現實生活中的應用。擴展卡爾曼濾波器(EKF極大地拓寬了卡爾曼濾波的適用范圍。 EKF的基本思路是，假定卡爾曼 濾濾對當前系統狀態估計值非常接近于其真實值， 于是將非線性函數在當前狀態 估計值處進行臺勞展開并實現線性化。 另一種非線性卡爾曼濾波叫線性化卡爾曼 濾波。它與EKF的主要區別是前者將非線函數在濾波器對當前系統狀態的最優估 計值處線性化，而后者因為預先知道非線性系統的實際運行狀態大致按照所要 求、希望的軌跡變化， 所以這些非線性化函數在實際狀態處的值可以表達為在希 望的

3、軌跡處的臺勞展開式，從而完成線性化。不敏卡爾曼濾波器(UKF是針對非線性系統的一種改進型卡爾曼濾波器。UKF處理非線性系統的基本思路在于不敏變換，而不敏變換從根本上講是一種描 述高斯隨機變量在非線性化變換后的概率分布情況的方法。不敏卡爾曼濾波認 為，與其將一個非線性化變換線性化、 近似化， 還不如將高斯隨機變量經非線性 變換后的概率分布情況用高斯分布來近似那樣簡單， 因而不敏卡爾曼濾波算法沒 有非線性化這一步驟。 在每一定位歷元， 不敏卡爾曼濾波器按照一套公式產生一 系列樣點， 每一樣點均配有一個相應的權重， 而這些帶權的樣點被用來完整地描 述系統狀態向量估計值的分布情況， 它們替代了原先卡爾

4、曼濾波器中的狀態向量 估計值及協方差。不敏卡爾曼濾器讓這些樣點一一經歷非線性狀態方程與測量方 程，然后再將這些經非線性變換后的樣點按照它們的權重而綜合出對當前時刻的 系統狀態向量估計值。多態自適應（ MM）A 卡爾曼濾波器是一種受到廣泛關注的濾波器，它由好多 個并聯、同時運行的卡爾曼濾波器組成。 在這組卡爾曼濾波器中， 每一個濾波器 對未知的濾波參數分別做出相互不同的假設， 然后各自按照自己的模型假設進行 濾波計算，而多態自適應濾波器最后將它們對系統狀態的各個估計值進行加權， 并以此作為最優估計值輸出。3、卡爾曼濾波器概述簡單來說，卡爾曼濾波器是一個“ optimal recursive da

5、ta processing algorithm （最優化自回歸數據處理算法）”。對于解決很大部分的問題，他是 最優，效率最高甚至是最有用的。他的廣泛應用已經超過 30 年，包括機器人導 航，控制， 傳感器數據融合甚至在軍事方面的雷達系統以及導彈追蹤等等。 近年 來更被應用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等?？柭鼮V波器由一系列遞歸數學公式描述， 它們提供了一種高效可計算的方 法來估計過程的狀態， 并使估計均方誤差最小。 卡爾曼濾波器應用廣泛且功能強 大，它可以估計信號的過去和當前狀態， 甚至能估計將來的狀態， 即使并不知道 模型的確切性質。假設我們要研究的對象是一個房間

6、的溫度。 根據你的經驗判斷， 這個房間的 溫度是恒定的， 也就是下一分鐘的溫度等于現在這一分鐘的溫度 （假設我們用一 分鐘來做時間單位） 。假設你對你的經驗不是 100%的相信，可能會有上下偏差幾 度。我們把這些偏差看成是高斯白噪聲（ White Gaussian Noise ），也就是這些 偏差跟前后時間是沒有關系的而且符合高斯分配（ Gaussian Distribution ）。另 外，我們在房間里放一個溫度計， 但是這個溫度計也不準確的， 測量值會比實際 值偏差。我們也把這些偏差看成是高斯白噪聲?，F在對于某一分鐘我們有兩個有關于該房間的溫度值：你根據經驗的預測值（系統的預測值）和溫度計

7、的值（測量值）。下面我們要用這兩個值結合他們各 自的噪聲來估算出房間的實際溫度值。假如我們要估算k時刻的是實際溫度值。首先你要根據k"時刻的溫度值， 來預測k時刻的溫度。因為你相信溫度是恒定的，所以你會得到k時刻的溫度預測值是跟k -1時刻一樣的，假設是23度，同時該值的高斯噪聲的偏差是5度（5 是這樣得到的：如果k"時刻估算出的最優溫度值的偏差是3，你對自己預測的 不確定度是4度，他們平方相加再開方，就是5 ）。然后，你從溫度計那里得到了 k時刻的溫度值，假設是25度，同時該值的偏差是4度。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是 23度和25度。 究竟實際溫

8、度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的協方差來判斷。因為Kg 2 = 5 2 '（5 2 4 2），所以Kg二0.78，我們 可以估算出k時刻的實際溫度值是：23 978“（25 -23）=24.56度。可以看出， 因為溫度計的協方差比較?。ū容^相信溫度計），所以估算出的最優溫度值偏向 溫度計的值?，F在我們已經得到k時刻的最優溫度值了，下一步就是要進入 k-1時刻，進 行新的最優估算。至V現在為止，好像還沒看到什么自回歸的東西出現。對了，在 進入k-1時刻之前，我們還要算出k時刻那個最優值（24.56度）的偏差。算法 如下：（1-Kg）“5 2） 0.

9、5 = 2.35。這里的5就是上面的k時刻你預測的那個23度 溫度值的偏差，得出的235就是進入k 1時刻以后k時刻估算出的最優溫度值的 偏差（對應于上面的3 ）。就是這樣，卡爾曼濾波器就不斷的把協方差遞歸，從而估算出最優的溫度值。 他運行的很快，而且它只保留了上一時刻的協方差。上面的 Kg，就是卡爾曼增 益（Kalman Gain ）。他可以隨不同的時刻而改變他自己的值。4、卡爾曼濾波器算法首先，我們先要引入一個離散控制過程的系統。分方程(Lin ear Stochastic Differe nee equati on該系統可用一個線性隨機微)來描述：(3.1)X(k)二AX(k-1) BU

10、(k) W(K)再加上系統的測量值：Z(k) =HX (k) V(k)(3.2)上兩式子中，X(k)是k時刻的系統狀態，U(k)是k時刻對系統的控制量。A 和B是系統參數，對于多模型系統，他們為矩陣。Z(k)是k時刻的測量值，H是 測量系統的參數，對于多測量系統， H為矩陣。W(K)和V(k)分別表示過程和 測量的噪聲。他們被假設成高斯白噪聲(White Gaussian Noise)，他們的協方差 分別是Q, R (這里我們假設他們不隨系統狀態變化而變化)。對于滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪 聲)，卡爾曼濾波器是最優的信息處理器。下面我們來用他們結合他們的協方差 s

11、來估算系統的最優化輸出。首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的 系統狀態是k，根據系統的模型，可以基于系統的上一狀態而預測出現在狀態：X(k|k 1) = AX(k 1|k 1) + BU(k)© 可式(3.3) 中, X(k|k-)是利用上一狀態預測的結果，X(k-依-“是上一狀 態最優的結果，U(k)為現在狀態的控制量，如果沒有控制量，它可以為0。到現在為止，我們的系統結果已經更新了，可是，對應于X(k1)的協方差(協方差)還沒更新。我們用P表示協方差：P(k|k_1) = AP(k_1|k_1)A+Q(3.4)式(3.4)中，P(kI)是X(k1)對應的

12、協方差，P(1 k1是 X(k 1 k-1)對應的協方差，a表示A的轉置矩陣，Q是系統過程的協方差。式 子(3.3),(3.4) 就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測?，F在我們有了現在狀態的預測結果，然后我們再收集現在狀態的測量值。結 合預測值和測量值，我們可以得到現在狀態(k)的最優化估算值X(kk)：X(k k) =X(k|k 1) + Kg(k)(Z(k) HX(k|k1)(3.5)其中Kg為卡爾曼增益(Kalman Gain)：Kg(k) = P(k k-1)H' (HP(k k-1)H' R)(3.6)到現在為止，我們已經得到了 k狀態下最優的估算

13、值X(kk)。但是為了要令 卡爾曼濾波器不斷的運行下去直到系統過程結束，我們還要更新k狀態下X(k k)的協方差：P(k k)=(l Kg(k)H)P(k k1)©旳其中I為1的矩陣，對于單模型單測量，I刊。當系統進入k 1狀態時，P(kk)就是式子(3.4)的P(1k_1)。這樣，算法就可以自回歸的運算下去。式子(3.3)，( 3.4)，( 3.5)，( 3.6 )和(3.7 )就是卡爾曼濾波器的基本原 理了。根據這5個公式，可以很容易的實現計算機的程序。5、卡爾曼濾波器的原理在現代，隨機最優控制和隨機信號處理技術中，信號和噪聲往往是多維非平穩隨機過程?？柭鼮V波理論采用時域上的遞

14、推算法在數字計算機上進行數據濾 波處理。對于離散域線性系統：x(k) =Ax(k -1) B(u(k) w(k)(3.8)yv(k)二 Cx(k) v(k)(3.9)式中，w(k)為過程噪聲信號，v(k)為測量噪聲信號。離散卡爾曼濾波器遞推算法為：Mn(k)巳逬(3.10)CP(k)C +RP(k)二 AP(k -1)At BQBt( 3.11)P(k) =(h -Mn(k)C)P(k)(3.12)x(k)二 Ax(k -1) Mn(k)(yv(k) -CAx(k-1)(3.13)ye(k) =Cx(k)誤差的協方差為：err cov(k)二 CP(k)CT卡爾曼濾波器結構如圖所示(3.15)

15、(3.14)U卡爾曼濾波器結構6、卡爾曼濾波器的應用實例6.1、基于VDLL的GPS信號跟蹤算法積分存儲衛星位置導航濾波器圖2 VDLL的基本結構系統的狀態量為用戶狀態（位置、速度、用戶鐘差、用戶鐘漂移），觀測量為所有衛星通道的碼環鑒相器輸出組成的矢量D（X）?tX=(x y z b x y z b )D(X)=(D( A t i) D( A t 2) , D( A t n) t(3-1 )其中 x, y, z, b, x, y, z, b分別為在 ECEF(Earth Centered Earth Fixed,?地心坐標系）下的用戶接收機三維位置和速度，b, b分別為接收機鐘差和鐘差變化率。

16、預檢測積分時間T選擇為1ms即濾波器迭代的k時刻與k+1時刻間隔1ms假設用戶為勻加速運動，可寫出狀態方程如下心Xk2IQ I 4(3-2)其中wk-1為狀態擾動噪聲。導航濾波器把k- 1時刻的狀態和對應的衛星位置信息反饋給本地信號生成模塊，產生超前、即時與滯后碼序列，分別與k時刻的1ms衛星信號的同相支路與正交支路做相關運算，即圖 2中的“積分存 儲”模塊。為了去除對信號幅度變化的敏感性和對載波環路鎖定的依賴性， 碼環 鑒相器選擇歸一化的超前碼與滯后碼相關能量差(第i顆衛星)D(IE2 QE2 - IL2 -QL2IE2 QE2 IL2 QL2(3-3)該碼環鑒相器在沒有噪聲的理想情況下，當

17、超前碼與滯后碼間距為1chip(碼片)時，在土 0.5 chip 輸入誤差范圍內，鑒相器輸出等于真實的跟蹤 誤差。在實際應用中，超前滯后碼的移位通常以采樣點為單位，1個chip的長(3-4)度不一定為整數個采樣點，但鑒相器仍然與輸入誤差成線性關系，所以式可寫為D(匚石,k) = gi 匚 <i,k ' vi ,k其中9為待求的鑒相器特性在線性區的斜率，Vi, k為觀測噪聲。鑒相器輸入誤差 Ti,k是k時刻(接收機時刻)第i顆衛星1ms長度的采樣信號與本地即時碼之間的相位偏移，而本地即時碼的初始相位值等于k- 1時刻對衛星信號的相位估計。所以At i, k實質上是k時刻衛星信號的相

18、位與k - 1時刻的相位估 計之差，等于k時刻信號傳輸延遲的真實值與k- 1時刻信號傳輸延遲的估計值。 k時刻接收到第i顆衛星信號的理論測量延遲為1 2 2 2 i,k(Xk _ Xi ,k )(yk - yi,k ) ' (Zk - Zj ,k) bk(3-5 )c其中(Xk , y k , z k )為k時刻用戶的三維位置坐標,(Xi, k , y i, k , z i, k ) 為k時刻第i顆衛星的位置坐標,bk為k時刻的接收機鐘差。K-1時刻信號傳 輸延遲的估計值可由k-1時刻的用戶狀態估計X ( k- 1 |k- 1) 和衛星位置計算得到?,k 4|k 4 xi,k-4|kx

19、i, k 4 )( ?k|kxi ,k 4 ) bk J|k J(3-6)由(4) ) ( 6)式，可寫出第i顆衛星的觀測方程如下1 1 2 2 2d(xq 二gK "-乂葉)(yk yi,k) (Zk-z) bk-&井二-(3-7)cY(Xk4|k4 Xj,k4)2 + ( ?k|k 4 一 Xj ,k )2 + (?心|2 一 Xi,k 斗 f + Vi, kc其他衛星的觀測方程可以類似地寫出。6.2卡爾曼濾波器的仿真及分析6.2.1系統描述驗證卡爾曼濾波器的濾波性能對象為二階傳遞函數：133Gp(s)2 ( 3.16)s 25s取采樣時間為1ms采樣Z變換將對象離散化，

20、并描述離散狀態方程的形式:x(k 1) = Ax(k) B(u(k) w(k)(3.17)y(k) =Cx(k)(3.18)帶有測量噪聲的被控對象輸出為:yv(k)二 Cx(k) v(k)(3.19)式中,1.0000000,0.0009876A 110.0000000,0.9753099 ,B=0.0000659,11,01,101(0.13135126.2.2仿真方法一：采用 M語言進行仿真控制干擾信號w(k)和測量噪聲信號v(k)幅值均為0.10的白噪聲信號，輸 入信號幅值為1.0、頻率為1.5Hz的正圖信號。采用卡爾曼濾波器實現信號的濾 波，取Q =1,R =10仿真時間為3s，誤差協方差的變化，原始信號及帶有噪聲的 原始信號和原始信號及濾波后的信號和分別如圖1、圖2和圖3所示。圖i誤差協方差的變化II900.511.522.535X 10-5453 5 2 5 1 5 4 3 21o rrFe nna nsef o CDOnaavoo00.511.52time(s)2.532 181 O6 4 2 o o OoeRron hlr wrLangsvy;uy-0.2圖2原始信號及帶有噪聲的原始信號1.21n 0.8e 0.6a 0.4d 0.20-0.200.511.522.53time(s)圖3原始信號及濾波后的信號仿真結果