Kalman濾波MATLAB綜合實(shí)驗(yàn)報(bào)告

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

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

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

4、方差，A '表示A的轉(zhuǎn)置矩 陣，Q是系統(tǒng)過(guò)程的協(xié)方差。式子1, 2就是卡爾曼濾波器 5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(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、的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的協(xié)方差：P(k|k)= (I-Kg(k) H ) P(k|k-1) (5)其中I為1的矩陣，對(duì)于單模型單測(cè)量，1=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子 的P(k-1|k-1)。 這樣，算法就可以自回歸的運(yùn)算下去。、MATLAB中已經(jīng)給出了濾波函數(shù)，以下為直接調(diào)用方法 ：設(shè)線性系統(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)。三、【實(shí)驗(yàn)內(nèi)容及數(shù)據(jù)來(lái)源】已知離散系統(tǒng)-0.49 0.293 0412耳+l = 涼 + 現(xiàn)(2),第一式為系統(tǒng)方程，第二式為觀測(cè)方程，：表示狀態(tài)量x的第二個(gè)分量。e與v是互不相關(guān)的高斯白噪聲。10.9'假設(shè)打的真值兀1二8481，由此系統(tǒng)方程構(gòu)造出k=1,2,30的數(shù)據(jù)亦,構(gòu)造時(shí)加上系統(tǒng)噪聲干擾，再由-4.3并以此作為仿真數(shù)據(jù)。用Kalman濾波對(duì)仿真數(shù)據(jù)進(jìn)行濾波處觀測(cè)方程構(gòu)造出觀測(cè)數(shù)據(jù)并加觀測(cè)噪聲干擾, 理，并與真實(shí)結(jié)果比較。四、【實(shí)驗(yàn)程序】%描述% 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)向量初始估計(jì)值guji=20.1 21.3 20.7'%進(jìn)入循環(huán)for i=2:30%c 產(chǎn)生正態(tài)分布數(shù)據(jù)w=ra ndn (3,1);v=ra ndn (1,1);%真實(shí)數(shù)據(jù)x(:,i)=A*

8、x(:,i-1);%人為制造系統(tǒng)誤差x1(:,i)=x(:,i)+w;Qn=eye(2,2);Rn=1;Nn=0;%人為制造觀測(cè)數(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);%得到估計(jì)數(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%做出在噪聲污染情況下的濾波估計(jì)值曲線x1plot(guji(1,:),

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

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

11、量第二個(gè)分量第三個(gè)分量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給出了三個(gè)分量的濾波處理結(jié)果。如果對(duì)比表1.1與表1.2,可以看出在序列靠后的地方真實(shí)狀態(tài)與估計(jì)狀態(tài)的絕對(duì)誤差是比較小的。表1.2 Kalman濾波處理結(jié)果時(shí)間序列第一個(gè)分量第二個(gè)分量第三個(gè)分量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六、【實(shí)驗(yàn)心得】MATLAB中有豐富的圖形處理能力，提供了繪制各種圖形、圖像數(shù)據(jù)的函數(shù)。它可以提供了一組繪制二維和三維曲線的函數(shù)，他們還可以對(duì)圖形進(jìn)行旋轉(zhuǎn)、縮放等操作。MATLAB內(nèi)部還包含豐富的數(shù)學(xué)函數(shù)和數(shù)據(jù)類型，使用方便且功能非常強(qiáng)大。本學(xué)期通過(guò)對(duì) MATLAB的系統(tǒng)環(huán)境，數(shù)據(jù)的各種運(yùn)算，矩陣的分析和處理，程序設(shè)計(jì)，繪圖，數(shù)值計(jì)算及符號(hào)運(yùn)算的學(xué)習(xí)，初步掌握了MATLAB的實(shí)用方法。

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