1、【精品文檔】如有侵權，請聯系網站刪除，僅供學習與交流卡爾曼濾波器綜述.精品文檔.卡爾曼濾波器綜述瞿偉軍 G100741、卡爾曼濾波的起源1960年，匈牙利數學家卡爾曼發表了一篇關于離散數據線性濾波遞推算法的論文，這意味著卡爾曼濾波的誕生。斯坦利.施密特(Stanley Schmidt)首次實現了卡爾曼濾波器，卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對于解決阿波羅計劃的軌道預測很有用，后來阿波羅飛船的導航電腦使用了這種濾波器。關于這種濾波器的論文由Swerling (1958)、Kalman (1960)與 Kalman and Bucy (1961)發表。2、卡爾曼濾波的發展卡爾曼濾

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

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

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

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

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

7、的是實際溫度值。首先你要根據時刻的溫度值，來預測時刻的溫度。因為你相信溫度是恒定的，所以你會得到時刻的溫度預測值是跟時刻一樣的，假設是度，同時該值的高斯噪聲的偏差是度（是這樣得到的：如果時刻估算出的最優溫度值的偏差是，你對自己預測的不確定度是度，他們平方相加再開方，就是）。然后，你從溫度計那里得到了時刻的溫度值，假設是度，同時該值的偏差是度。由于我們用于估算時刻的實際溫度有兩個溫度值，分別是度和度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的協方差來判斷。因為，所以，我們可以估算出時刻的實際溫度值是：度。可以看出，因為溫度計的協方差比較小（比較相信溫度計）

8、，所以估算出的最優溫度值偏向溫度計的值。現在我們已經得到時刻的最優溫度值了，下一步就是要進入時刻，進行新的最優估算。到現在為止，好像還沒看到什么自回歸的東西出現。對了，在進入時刻之前，我們還要算出時刻那個最優值（度）的偏差。算法如下：。這里的就是上面的時刻你預測的那個度溫度值的偏差，得出的就是進入時刻以后時刻估算出的最優溫度值的偏差（對應于上面的）。就是這樣，卡爾曼濾波器就不斷的把協方差遞歸，從而估算出最優的溫度值。他運行的很快，而且它只保留了上一時刻的協方差。上面的，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值。4、卡爾曼濾波器算法首先，我們先要引入一個離散控

9、制過程的系統。該系統可用一個線性隨機微分方程（Linear Stochastic Difference equation）來描述： (3.1)再加上系統的測量值： (3.2)上兩式子中，是時刻的系統狀態，是時刻對系統的控制量。和是系統參數，對于多模型系統，他們為矩陣。是時刻的測量值，是測量系統的參數，對于多測量系統，為矩陣。和分別表示過程和測量的噪聲。他們被假設成高斯白噪聲(White Gaussian Noise)，他們的協方差分別是（這里我們假設他們不隨系統狀態變化而變化）。對于滿足上面的條件(線性隨機微分系統，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優的信息處理器。下面我們來用他們結

10、合他們的協方差s 來估算系統的最優化輸出。首先我們要利用系統的過程模型，來預測下一狀態的系統。假設現在的系統狀態是，根據系統的模型，可以基于系統的上一狀態而預測出現在狀態： (3.3)式(3.3)中，是利用上一狀態預測的結果，是上一狀態最優的結果，為現在狀態的控制量，如果沒有控制量，它可以為0。到現在為止，我們的系統結果已經更新了，可是，對應于的協方差(協方差)還沒更新。我們用表示協方差： (3.4)式(3.4)中，是對應的協方差，是對應的協方差，表示的轉置矩陣，是系統過程的協方差。式子(3.3),(3.4)就是卡爾曼濾波器5個公式當中的前兩個，也就是對系統的預測。現在我們有了現在狀態的預測結

11、果，然后我們再收集現在狀態的測量值。結合預測值和測量值，我們可以得到現在狀態的最優化估算值： (3.5)其中為卡爾曼增益(Kalman Gain)： (3.6)到現在為止，我們已經得到了狀態下最優的估算值。但是為了要令卡爾曼濾波器不斷的運行下去直到系統過程結束，我們還要更新狀態下的協方差： (3.7)其中為1的矩陣，對于單模型單測量，。當系統進入狀態時，就是式子(3.4)的。這樣，算法就可以自回歸的運算下去。式子（3.3），（3.4），（3.5），（3.6）和（3.7）就是卡爾曼濾波器的基本原理了。根據這5個公式，可以很容易的實現計算機的程序。5、卡爾曼濾波器的原理在現代，隨機最優控制和隨機信

12、號處理技術中，信號和噪聲往往是多維非平穩隨機過程。卡爾曼濾波理論采用時域上的遞推算法在數字計算機上進行數據濾波處理。對于離散域線性系統： （3.8） （3.9）式中，為過程噪聲信號， 為測量噪聲信號。離散卡爾曼濾波器遞推算法為： （3.10） （3.11） （3.12） （3.13） （3.14）誤差的協方差為： （3.15）卡爾曼濾波器結構如圖所示卡爾曼濾波器結構6、卡爾曼濾波器的應用實例6.1、基于VDLL的GPS信號跟蹤算法圖2 VDLL的基本結構系統的狀態量為用戶狀態( 位置、速度、用戶鐘差、用戶鐘漂移) ，觀測量為所有衛星通道的碼環鑒相器輸出組成的矢量D(X) X=(x y z b

13、)TD(X)=(D(1) D(2) D(N)T （3-1）其中x，y，z，b，分別為在ECEF( Earth Centered Earth Fixed, 地心坐標系) 下的用戶接收機三維位置和速度， b, 分別為接收機鐘差和鐘差變化率。預檢測積分時間T 選擇為1ms，即濾波器迭代的k時刻與k+1時刻間隔1ms。假設用戶為勻加速運動，可寫出狀態方程如下 （3-2）其中wk-1 為狀態擾動噪聲。 導航濾波器把k- 1 時刻的狀態和對應的衛星位置信息反饋給本地信號生成模塊，產生超前、即時與滯后碼序列，分別與k時刻的1ms 衛星信號的同相支路與正交支路做相關運算，即圖2 中的“積分存儲”模塊。為了去除

14、對信號幅度變化的敏感性和對載波環路鎖定的依賴性，碼環鑒相器選擇歸一化的超前碼與滯后碼相關能量差( 第i 顆衛星) （3-3）該碼環鑒相器在沒有噪聲的理想情況下，當超前碼與滯后碼間距為1 chip( 碼片) 時，在0.5 chip 輸入誤差范圍內，鑒相器輸出等于真實的跟蹤誤差。在實際應用中，超前滯后碼的移位通常以采樣點為單位，1 個chip 的長度不一定為整數個采樣點，但鑒相器仍然與輸入誤差成線性關系，所以式可寫為 （3-4）其中gi 為待求的鑒相器特性在線性區的斜率， vi, k為觀測噪聲。鑒相器輸入誤差是k時刻( 接收機時刻) 第i顆衛星1ms 長度的采樣信號與本地即時碼之間的相位偏移，而本

15、地即時碼的初始相位值等于k- 1時刻對衛星信號的相位估計。所以i, k實質上是k時刻衛星信號的相位與k - 1時刻的相位估計之差，等于k 時刻信號傳輸延遲的真實值與k- 1 時刻信號傳輸延遲的估計值。 k 時刻接收到第i顆衛星信號的理論測量延遲為 （3-5）其中( xk , yk , zk ) 為k 時刻用戶的三維位置坐標,( xi, k , yi, k , zi, k ) 為k 時刻第i 顆衛星的位置坐標,bk 為k 時刻的接收機鐘差。K-1 時刻信號傳輸延遲的估計值可由k-1 時刻的用戶狀態估計 ( k- 1 |k- 1) 和衛星位置計算得到 （3-6）由( 4) ) ( 6) 式，可寫出

16、第i 顆衛星的觀測方程如下 （3-7）其他衛星的觀測方程可以類似地寫出。6.2 卡爾曼濾波器的仿真及分析6.2.1 系統描述驗證卡爾曼濾波器的濾波性能對象為二階傳遞函數： （3.16）取采樣時間為1ms，采樣Z變換將對象離散化，并描述離散狀態方程的形式： （3.17） （3.18）帶有測量噪聲的被控對象輸出為： （3.19）式中，6.2.2 仿真方法一：采用M語言進行仿真控制干擾信號和測量噪聲信號 幅值均為0.10的白噪聲信號，輸入信號幅值為1.0、頻率為1.5Hz的正圖信號。采用卡爾曼濾波器實現信號的濾波，取。仿真時間為3s，誤差協方差的變化，原始信號及帶有噪聲的原始信號和原始信號及濾波后的

17、信號和分別如圖1、圖2和圖3所示。圖1誤差協方差的變化 圖2原始信號及帶有噪聲的原始信號圖3原始信號及濾波后的信號仿真結果表明，該濾波器對控制干擾和測量噪聲具有良好的濾波作用。6.2.3 仿真方法二 ：采用Simulink進行仿真卡爾曼算法由M函數實現。控制干擾信號和測量噪聲信號幅值均為0.10的白噪聲信號，輸入信號幅值為1.0、頻率為0.5Hz的正弦信號采用卡爾曼濾波器實現信號的濾波，取。仿真時間為10s，仿真結果如下圖4和圖5所示圖4 原始信號（紅色線）及濾波后的信號（黃色線）圖5原始信號及帶有噪聲的原始信號仿真程序如下圖所示基于Kalman濾波器的Simulink仿真6.3 基于卡爾曼濾波器的 PID 控制算法及仿真6.3.1 基于卡爾曼濾波器 PID 控制的原理結合卡爾曼濾波器與經典PID控制得到一種基于卡爾曼濾波器的PID控制方法。其控制系統結構如圖：PID被控對象卡爾曼 濾波器rin + + _+ 控制干擾 測量噪聲 yo