Kalman濾波MATLAB綜合實驗報告

1、數(shù)學實驗綜合實驗報告實驗名稱綜合實驗（Kalman濾波）2016年5月一、【實驗目的】明白濾波計算流程能夠調(diào)用相關函數(shù)進行數(shù)據(jù)處理使用循環(huán)函數(shù)和二維曲線畫圖有效的構(gòu)建仿真模型，產(chǎn)生模擬數(shù)據(jù)二、【實驗原理分析】卡爾曼濾波器是一個"optimal recursive data processing algorithm (最優(yōu)化自回歸數(shù)據(jù)處理算法)”對于解決很大部分的問題，它是最優(yōu)，效率最高甚至是最有用的。它的廣泛應用已經(jīng)超過30年，包括機器人導航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導彈追蹤等等。近來更被應用于計算機圖像處 理，例如頭臉識別，圖像分割，圖像邊緣檢測等等。設系統(tǒng)

2、可用一個線性隨機微分方程來描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系統(tǒng)的測量值：Z(k)=H X(k)+V(k)上兩式子中，X(k)是k時刻的系統(tǒng)狀態(tài)，U(k)是k時刻對系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃嚒?Z(k)是k時刻的測量值，H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設成高斯白噪聲，他們的協(xié)方差分別是Q，R (這里假設他們不隨系統(tǒng)狀態(tài)變化而變化)。對于滿足上面的條件(線性隨機微分系統(tǒng)，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處 理器。首先要利用系統(tǒng)的過程模型，來預測下一狀態(tài)

3、的系統(tǒng)。假設現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預測出現(xiàn)在狀態(tài)：X(k|k-1)=A X(k-1|k- 1)+B U(k) .(1)式(1)中，X(k|k-1)是利用上一狀態(tài)預測的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。至U現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對應于X(k|k-1)的協(xié)方差還沒更新。我們用 P表示協(xié)方差：P(k|k-1)=A P(k-1|k- 1) A ' +Q (2)式(2)中，P(k|k-1)是X(k|k-1)對應的協(xié)方差，P(k-1|k-1)是X(k-1|k-1)對應的協(xié)

4、方差，A '表示A的轉(zhuǎn)置矩 陣，Q是系統(tǒng)過程的協(xié)方差。式子1, 2就是卡爾曼濾波器 5個公式當中的前兩個，也就是對系統(tǒng)的預測?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預測值和測量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k):X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k- 1) (3)其中Kg為卡爾曼增益(Kalman Gain):Kg(k)= P(k|k- 1) H ' / (H P(k|k) H '+ R) (4)到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要令卡爾曼濾波器不斷

5、的運行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的協(xié)方差：P(k|k)= (I-Kg(k) H ) P(k|k-1) (5)其中I為1的矩陣，對于單模型單測量，1=1。當系統(tǒng)進入k+1狀態(tài)時，P(k|k)就是式子 的P(k-1|k-1)。 這樣，算法就可以自回歸的運算下去。、MATLAB中已經(jīng)給出了濾波函數(shù)，以下為直接調(diào)用方法 ：設線性系統(tǒng)為丁上 +血+血y = Cx + Du + Hw+v其調(diào)用格式為kest, L, P=kalman (sys, Qn, Rn, Nn)kest, L, P=kalma n( sys, Qn, Rn, Nn, sen sors, known)kes

6、t,L,P,M,Z=kalma n(sys,Q n,Rn,Nn)最后一種調(diào)用格式只限于離散系統(tǒng)。三、【實驗內(nèi)容及數(shù)據(jù)來源】已知離散系統(tǒng)-0.49 0.293 0412耳+l = 涼 + 現(xiàn)(2),第一式為系統(tǒng)方程，第二式為觀測方程，：表示狀態(tài)量x的第二個分量。e與v是互不相關的高斯白噪聲。10.9'假設打的真值兀1二8481，由此系統(tǒng)方程構(gòu)造出k=1,2,30的數(shù)據(jù)亦,構(gòu)造時加上系統(tǒng)噪聲干擾，再由-4.3并以此作為仿真數(shù)據(jù)。用Kalman濾波對仿真數(shù)據(jù)進行濾波處觀測方程構(gòu)造出觀測數(shù)據(jù)并加觀測噪聲干擾, 理，并與真實結(jié)果比較。四、【實驗程序】%描述% x n+1=Ax n+Bu n+Gw

7、 n% y n=Cx n+Du n+Hw n+v n%狀態(tài)轉(zhuǎn)移矩陣A=0.49 0.298 0.4120.401 -0.391 0.391-0.992 0.401 0.296;% B矩陣B=zeros(3,3);% G矩陣G=eye(3,3);% C矩陣向量C=0 1 0;D=0 0 0;H=zeros(1,3);%狀態(tài)向量初值(真值)x(:,1)=10.9 8.481 -4.3'%狀態(tài)向量初始估計值guji=20.1 21.3 20.7'%進入循環(huán)for i=2:30%c 產(chǎn)生正態(tài)分布數(shù)據(jù)w=ra ndn (3,1);v=ra ndn (1,1);%真實數(shù)據(jù)x(:,i)=A*

8、x(:,i-1);%人為制造系統(tǒng)誤差x1(:,i)=x(:,i)+w;Qn=eye(2,2);Rn=1;Nn=0;%人為制造觀測數(shù)據(jù)的誤差z0(:,i)=C*x1(:,i)+v;%建立Kalman的系統(tǒng)參數(shù)sys=ss(A,B,G,C,D,H,-1);kest,L,P,M,Z=kalma n(sys,Q n,Rn,Nn);%得到估計數(shù)據(jù)guji(:,i)=A*guji(:,i-1)+L*(z0(:,i)-C*A*guji(:,i-1);endsubplot(2,2,1)%做出真值曲線x1plot(x(1,:)hold on%做出在噪聲污染情況下的濾波估計值曲線x1plot(guji(1,:),

9、':m')hold offlege nd('real of x1','estimate of x1')gridsubplot(2,2,2)%做出真值曲線x2plot(x(2,:)hold on%做出在噪聲污染情況下的濾波估計值曲線x2plot(guji(2,:),':m')hold offlegend('real of x2','estimate of x2')gridsubplot(2,2,3:4)%做出真值曲線x2plot(x(3,:)hold on%做出在噪聲污染情況下的濾波估計值曲線x3pl

10、ot(guji(3,:),':m')hold offlege nd('real of x1','estimate of x1')grid五、【實驗結(jié)果】為了闡述kalman濾波的基本計算思路，在試驗中選擇了最簡單的一種系統(tǒng)模型，線性定常動力系統(tǒng)。在實際應用中很多時候面對的都是非線性而且時變系統(tǒng)，這有時候性質(zhì)要復雜一些。最后的結(jié)果信息我們反映在圖9.2中，這只是結(jié)果的直觀反應，更多的時候需要的是計算結(jié)果。表9.1 中給出了真實的運動軌跡。40200-20-400圖1.1 Kalman濾波處理的與真實值的比較表1.1真實的運動狀態(tài)數(shù)據(jù)時間序列第一個分

11、量第二個分量第三個分量110.98.481-4.326.096738-0.626471-8.6847193-0.777390966-0.70598303-8.8698557914-4.245685102-3.503808027-2.1374046715-4.005131216-1.1682560142.174020826-1.41495685102-0.2992273764926530241.1715184422.5114945581.8378480680.8954692850.29406328491.2885494730.501827329-1.3770193681

12、00.213601806-0.21792072-1.48460605111-0.571933183-0.40961964-0.73872259212-0.70666762-0.3580244600.376969821-0.0712707550.612040224140.0462066750.1160096940.526538396150.2741459790.1790455990.156538231160.2521808690.101132157-0.153820209170.0903320820.001438153-0.25514020918-0.060426476

13、-0.064098975-0.16455422819-0.11650681-0.063509021-0.01446867620-0.08197512-0.0275444560.0858249121-0.0130161940.0114553990.095678165220.0364551780.0277116080.045826416230.045001580.021701416-0.011486563240.0237853320.005069134-0.03933932225-0.003042386-0.007825788-0.03320676626-0.017504042-0.0111439

14、59-0.00994929727-0.015996991-0.0065520080.0099502928-0.0056915043.76E-050.016186945290.0038913910.0040320990.010452388300.0074147310.0040707810.000850519表1.2給出了三個分量的濾波處理結(jié)果。如果對比表1.1與表1.2,可以看出在序列靠后的地方真實狀態(tài)與估計狀態(tài)的絕對誤差是比較小的。表1.2 Kalman濾波處理結(jié)果時間序列第一個分量第二個分量第三個分量120.121.320.7222.72524889.494219162-6.31616921

15、6310.406516233.728682279-21.105630164-1.788182459-6.118879723-14.710902865-8.614322881-4.198556849-4.9577776266-6.763631785-4.3780776395.7870306767-2.3574079941.3648383156.6026590981.4226924661.560996264.55308832692.3926733422.27910370.224864281101.8616921010.225133574-1.436205036110.517263742-0.011

16、25328-2.11384303912-0.58022756-0.648547414-1.2212362613-0.593150559-0.7072128380.16466488714-0.5532042150.2029068090.29099303715-0.3912351910.0634052510.55915043216-0.6186255910.60012591930.2254946617-0.618625591-0.7838607191.16515757918-0.0453432221.357835159-0.66486884319-0.11295527-0.6242528310.2

17、7688772520-0.3988591910.53368178-0.19830150321-0.179721431-0.3947254910.51876082622-0.2792072370.5248242670.023365338230.448919666-0.6582977510.7137889240.4794277290.581682628-0.413558092250.032585161-0.025564549-0.472086999260.014970698-0.329369401-0.07715656327-0.8528779380.7140655-0.551591054280.

18、072338102-1.2580818171.23301402529-0.4532687511.521953811-0.53639147630-0.003343612-0.9750646480.893963618六、【實驗心得】MATLAB中有豐富的圖形處理能力，提供了繪制各種圖形、圖像數(shù)據(jù)的函數(shù)。它可以提供了一組繪制二維和三維曲線的函數(shù)，他們還可以對圖形進行旋轉(zhuǎn)、縮放等操作。MATLAB內(nèi)部還包含豐富的數(shù)學函數(shù)和數(shù)據(jù)類型，使用方便且功能非常強大。本學期通過對 MATLAB的系統(tǒng)環(huán)境，數(shù)據(jù)的各種運算，矩陣的分析和處理，程序設計，繪圖，數(shù)值計算及符號運算的學習，初步掌握了MATLAB的實用方法。

19、通過老師的講解與自己上機的操作，使我在短時間內(nèi)學會使用 MATLAB，同時，通過上機實驗，對理論知識的復習鞏固實踐，可以自己根據(jù)例題編寫設計 簡單的程序來實現(xiàn)不同的功能，繪制出比較滿意的二維三維圖形，在實踐中找到樂趣。MATLAB是一個實用性很強，操作相對容易，比較完善的工具軟件，使用起來比較方便，通過操作可以很快看到結(jié)果，能夠清晰的感覺到成功與失敗，雖然課程中也會出現(xiàn)一些小問題，但是很喜歡這門課程。鄭蕊MATLAB作為一門編程語言，其簡易程度超過了大多數(shù)的計算機高級語言，作為一名計算機專業(yè)的學生，我深深感受到了這門語言的簡潔與方便，語法和自然語言更為貼近，簡便易學，功能強大，能夠用其進行絕大多少的數(shù)學計算與繪圖。而且我們的數(shù)值計算就是用MATLAB作為我們的計算的工具，所以感覺這門課程多我的幫助很大，而且可以將其接入java等多種主流編程語言程序運行，兼容性很好。用它進行數(shù)值分析計算，大大的方便了我們的任務的進行?？傊?，通過這學期