卡爾曼濾波器綜述

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

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

3、F）是針對非線性系統(tǒng)的一種改進型卡爾曼濾波器。UKF處理非線性系統(tǒng)的基本思路在于不敏變換，而不敏變換從根本上講是一種描述高斯隨機變量在非線性化變換后的概率分布情況的方法。不敏卡爾曼濾波認(rèn)為，與其將一個非線性化變換線性化、近似化，還不如將高斯隨機變量經(jīng)非線性變換后的概率分布情況用高斯分布來近似那樣簡單，因而不敏卡爾曼濾波算法沒有非線性化這一步驟。在每一定位歷元，不敏卡爾曼濾波器按照一套公式產(chǎn)生一系列樣點，每一樣點均配有一個相應(yīng)的權(quán)重，而這些帶權(quán)的樣點被用來完整地描述系統(tǒng)狀態(tài)向量估計值的分布情況，它們替代了原先卡爾曼濾波器中的狀態(tài)向量估計值及協(xié)方差。不敏卡爾曼濾器讓這些樣點一一經(jīng)歷非線性狀態(tài)方程與

4、測量方程，然后再將這些經(jīng)非線性變換后的樣點按照它們的權(quán)重而綜合出對當(dāng)前時刻的系統(tǒng)狀態(tài)向量估計值。多態(tài)自適應(yīng)（MMA）卡爾曼濾波器是一種受到廣泛關(guān)注的濾波器，它由好多個并聯(lián)、同時運行的卡爾曼濾波器組成。在這組卡爾曼濾波器中，每一個濾波器對未知的濾波參數(shù)分別做出相互不同的假設(shè)，然后各自按照自己的模型假設(shè)進行濾波計算，而多態(tài)自適應(yīng)濾波器最后將它們對系統(tǒng)狀態(tài)的各個估計值進行加權(quán)，并以此作為最優(yōu)估計值輸出。3、卡爾曼濾波器概述簡單來說，卡爾曼濾波器是一個“optimal recursive data processing algorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法）”。對于解決很大部分的問題，他是最優(yōu)

5、，效率最高甚至是最有用的。他的廣泛應(yīng)用已經(jīng)超過30年，包括機器人導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導(dǎo)彈追蹤等等。近年來更被應(yīng)用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。卡爾曼濾波器由一系列遞歸數(shù)學(xué)公式描述，它們提供了一種高效可計算的方法來估計過程的狀態(tài)，并使估計均方誤差最小?？柭鼮V波器應(yīng)用廣泛且功能強大，它可以估計信號的過去和當(dāng)前狀態(tài)，甚至能估計將來的狀態(tài)，即使并不知道模型的確切性質(zhì)。假設(shè)我們要研究的對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷，這個房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來做時間單位）。假設(shè)你對你的經(jīng)驗

6、不是100%的相信，可能會有上下偏差幾度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前后時間是沒有關(guān)系的而且符合高斯分配（Gaussian Distribution）。另外，我們在房間里放一個溫度計，但是這個溫度計也不準(zhǔn)確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲?，F(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預(yù)測值（系統(tǒng)的預(yù)測值）和溫度計的值（測量值）。下面我們要用這兩個值結(jié)合他們各自的噪聲來估算出房間的實際溫度值。假如我們要估算時刻的是實際溫度值。首先你要根據(jù)時刻的溫度值，來預(yù)測時刻的溫度。因為你相信溫度是恒定

7、的，所以你會得到時刻的溫度預(yù)測值是跟時刻一樣的，假設(shè)是度，同時該值的高斯噪聲的偏差是度（是這樣得到的：如果時刻估算出的最優(yōu)溫度值的偏差是，你對自己預(yù)測的不確定度是度，他們平方相加再開方，就是）。然后，你從溫度計那里得到了時刻的溫度值，假設(shè)是度，同時該值的偏差是度。由于我們用于估算時刻的實際溫度有兩個溫度值，分別是度和度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們可以用他們的協(xié)方差來判斷。因為，所以，我們可以估算出時刻的實際溫度值是：度?？梢钥闯?，因為溫度計的協(xié)方差比較小（比較相信溫度計），所以估算出的最優(yōu)溫度值偏向溫度計的值?，F(xiàn)在我們已經(jīng)得到時刻的最優(yōu)溫度值了，下一

8、步就是要進入時刻，進行新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對了，在進入時刻之前，我們還要算出時刻那個最優(yōu)值（度）的偏差。算法如下：。這里的就是上面的時刻你預(yù)測的那個度溫度值的偏差，得出的就是進入時刻以后時刻估算出的最優(yōu)溫度值的偏差（對應(yīng)于上面的）。就是這樣，卡爾曼濾波器就不斷的把協(xié)方差遞歸，從而估算出最優(yōu)的溫度值。他運行的很快，而且它只保留了上一時刻的協(xié)方差。上面的，就是卡爾曼增益（Kalman Gain）。他可以隨不同的時刻而改變他自己的值。4、卡爾曼濾波器算法首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用一個線性隨機微分方程（Linear Stochastic

9、 Difference equation）來描述： (3.1)再加上系統(tǒng)的測量值： (3.2)上兩式子中，是時刻的系統(tǒng)狀態(tài)，是時刻對系統(tǒng)的控制量。和是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃嚒Ｊ菚r刻的測量值，是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，為矩陣。和分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(White Gaussian Noise)，他們的協(xié)方差分別是（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。對于滿足上面的條件(線性隨機微分系統(tǒng)，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們來用他們結(jié)合他們的協(xié)方差s 來估算系統(tǒng)的最優(yōu)化輸出。首先我們要利用系統(tǒng)的過程模型，來預(yù)測下

10、一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)： (3.3)式(3.3)中，是利用上一狀態(tài)預(yù)測的結(jié)果，是上一狀態(tài)最優(yōu)的結(jié)果，為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對應(yīng)于的協(xié)方差(協(xié)方差)還沒更新。我們用表示協(xié)方差： (3.4)式(3.4)中，是對應(yīng)的協(xié)方差，是對應(yīng)的協(xié)方差，表示的轉(zhuǎn)置矩陣，是系統(tǒng)過程的協(xié)方差。式子(3.3),(3.4)就是卡爾曼濾波器5個公式當(dāng)中的前兩個，也就是對系統(tǒng)的預(yù)測?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài)的最

11、優(yōu)化估算值： (3.5)其中為卡爾曼增益(Kalman Gain)： (3.6)到現(xiàn)在為止，我們已經(jīng)得到了狀態(tài)下最優(yōu)的估算值。但是為了要令卡爾曼濾波器不斷的運行下去直到系統(tǒng)過程結(jié)束，我們還要更新狀態(tài)下的協(xié)方差： (3.7)其中為1的矩陣，對于單模型單測量，。當(dāng)系統(tǒng)進入狀態(tài)時，就是式子(3.4)的。這樣，算法就可以自回歸的運算下去。式子（3.3），（3.4），（3.5），（3.6）和（3.7）就是卡爾曼濾波器的基本原理了。根據(jù)這5個公式，可以很容易的實現(xiàn)計算機的程序。5、卡爾曼濾波器的原理在現(xiàn)代，隨機最優(yōu)控制和隨機信號處理技術(shù)中，信號和噪聲往往是多維非平穩(wěn)隨機過程?？柭鼮V波理論采用時域上的遞推

12、算法在數(shù)字計算機上進行數(shù)據(jù)濾波處理。對于離散域線性系統(tǒng)： （3.8） （3.9）式中，為過程噪聲信號， 為測量噪聲信號。離散卡爾曼濾波器遞推算法為： （3.10） （3.11） （3.12） （3.13） （3.14）誤差的協(xié)方差為： （3.15）卡爾曼濾波器結(jié)構(gòu)如圖所示 卡爾曼濾波器結(jié)構(gòu)6、卡爾曼濾波器的應(yīng)用實例6.1、基于VDLL的GPS信號跟蹤算法圖2 VDLL的基本結(jié)構(gòu)系統(tǒng)的狀態(tài)量為用戶狀態(tài)( 位置、速度、用戶鐘差、用戶鐘漂移) ，觀測量為所有衛(wèi)星通道的碼環(huán)鑒相器輸出組成的矢量D(X) X=(x y z b )TD(X)=(D(1) D(2) D(N)T （3-1）其中x，y，z，b，

13、分別為在ECEF( Earth Centered Earth Fixed, 地心坐標(biāo)系) 下的用戶接收機三維位置和速度， b, 分別為接收機鐘差和鐘差變化率。預(yù)檢測積分時間T 選擇為1ms，即濾波器迭代的k時刻與k+1時刻間隔1ms。假設(shè)用戶為勻加速運動，可寫出狀態(tài)方程如下 （3-2）其中wk-1 為狀態(tài)擾動噪聲。 導(dǎo)航濾波器把k- 1 時刻的狀態(tài)和對應(yīng)的衛(wèi)星位置信息反饋給本地信號生成模塊，產(chǎn)生超前、即時與滯后碼序列，分別與k時刻的1ms 衛(wèi)星信號的同相支路與正交支路做相關(guān)運算，即圖2 中的“積分存儲”模塊。為了去除對信號幅度變化的敏感性和對載波環(huán)路鎖定的依賴性，碼環(huán)鑒相器選擇歸一化的超前碼與

14、滯后碼相關(guān)能量差( 第i 顆衛(wèi)星) （3-3）該碼環(huán)鑒相器在沒有噪聲的理想情況下，當(dāng)超前碼與滯后碼間距為1 chip( 碼片) 時，在0.5 chip 輸入誤差范圍內(nèi)，鑒相器輸出等于真實的跟蹤誤差。在實際應(yīng)用中，超前滯后碼的移位通常以采樣點為單位，1 個chip 的長度不一定為整數(shù)個采樣點，但鑒相器仍然與輸入誤差成線性關(guān)系，所以式可寫為 （3-4）其中g(shù)i 為待求的鑒相器特性在線性區(qū)的斜率， vi, k為觀測噪聲。鑒相器輸入誤差是k時刻( 接收機時刻) 第i顆衛(wèi)星1ms 長度的采樣信號與本地即時碼之間的相位偏移，而本地即時碼的初始相位值等于k- 1時刻對衛(wèi)星信號的相位估計。所以i, k實質(zhì)上是

15、k時刻衛(wèi)星信號的相位與k - 1時刻的相位估計之差，等于k 時刻信號傳輸延遲的真實值與k- 1 時刻信號傳輸延遲的估計值。 k 時刻接收到第i顆衛(wèi)星信號的理論測量延遲為 （3-5）其中( xk , yk , zk ) 為k 時刻用戶的三維位置坐標(biāo),( xi, k , yi, k , zi, k ) 為k 時刻第i 顆衛(wèi)星的位置坐標(biāo),bk 為k 時刻的接收機鐘差。K-1 時刻信號傳輸延遲的估計值可由k-1 時刻的用戶狀態(tài)估計 ( k- 1 |k- 1) 和衛(wèi)星位置計算得到 （3-6）由( 4) ) ( 6) 式，可寫出第i 顆衛(wèi)星的觀測方程如下 （3-7）其他衛(wèi)星的觀測方程可以類似地寫出。6.2

16、 卡爾曼濾波器的仿真及分析6.2.1 系統(tǒng)描述驗證卡爾曼濾波器的濾波性能對象為二階傳遞函數(shù)： （3.16）取采樣時間為1ms，采樣Z變換將對象離散化，并描述離散狀態(tài)方程的形式： （3.17） （3.18）帶有測量噪聲的被控對象輸出為： （3.19）式中，6.2.2 仿真方法一：采用M語言進行仿真控制干擾信號和測量噪聲信號 幅值均為0.10的白噪聲信號，輸入信號幅值為1.0、頻率為1.5Hz的正圖信號。采用卡爾曼濾波器實現(xiàn)信號的濾波，取。仿真時間為3s，誤差協(xié)方差的變化，原始信號及帶有噪聲的原始信號和原始信號及濾波后的信號和分別如圖1、圖2和圖3所示。圖1誤差協(xié)方差的變化 圖2原始信號及帶有噪聲

17、的原始信號圖3原始信號及濾波后的信號仿真結(jié)果表明，該濾波器對控制干擾和測量噪聲具有良好的濾波作用。6.2.3 仿真方法二 ：采用Simulink進行仿真卡爾曼算法由M函數(shù)實現(xiàn)?？刂聘蓴_信號和測量噪聲信號幅值均為0.10的白噪聲信號，輸入信號幅值為1.0、頻率為0.5Hz的正弦信號采用卡爾曼濾波器實現(xiàn)信號的濾波，取。仿真時間為10s，仿真結(jié)果如下圖4和圖5所示 圖4 原始信號（紅色線）及濾波后的信號（黃色線）圖5原始信號及帶有噪聲的原始信號仿真程序如下圖所示基于Kalman濾波器的Simulink仿真6.3 基于卡爾曼濾波器的 PID 控制算法及仿真6.3.1 基于卡爾曼濾波器 PID 控制的原理結(jié)合卡爾曼濾波器與經(jīng)典PID控制得到一種基于卡爾曼濾波器的PID控制方法。其控制系統(tǒng)結(jié)構(gòu)如圖：PID被控對象卡爾曼 濾波器rin + + _+ 控制干擾 測量噪聲 yout基