測(cè)控技術(shù)與系統(tǒng)

1、測(cè)控技術(shù)與系統(tǒng)測(cè)控技術(shù)與系統(tǒng)l姓名：邵 帥l老師：馮榮達(dá)l班級(jí)：12測(cè)控A1目錄目錄一、摘要二、卡爾曼濾波的介紹三、卡爾曼濾波的應(yīng)用四、卡爾曼濾波的算法五、卡爾曼濾波的應(yīng)用例子六、總結(jié)一、摘要一、摘要l在濾波器的發(fā)展過程中，早期的維納濾波器涉及到對(duì)不隨時(shí)間變化的統(tǒng)計(jì)特性的處理，即靜態(tài)處理。在這種信號(hào)處理過程中，有用信號(hào)和無(wú)用噪聲的統(tǒng)計(jì)特性可與它們的頻率特性聯(lián)系起來(lái)，因此與經(jīng)典濾波器在概念上還有一定的聯(lián)系。由于軍事上的需要，維納濾波器在第二次世界大戰(zhàn)期間得到了廣泛的應(yīng)用。但是，維納濾波器有如下不足之處：第一，必須利用全部的歷史觀測(cè)數(shù)據(jù)，存儲(chǔ)量和計(jì)算量都很大；第二，當(dāng)獲得新的觀測(cè)數(shù)據(jù)時(shí)，沒有合適的

2、遞推算法，必須進(jìn)行重新計(jì)算；第三，很難用于非平穩(wěn)過程的濾波。為了克服維納濾波器的上述不足之處，卡爾曼等人在維納濾波的基礎(chǔ)上，與60年代初提出了一種遞推濾波方法，稱為卡爾曼濾波。與維納濾波不同，卡爾曼濾波是對(duì)時(shí)變統(tǒng)計(jì)特性進(jìn)行處理。他不是從頻域，而是從時(shí)域的角度出發(fā)來(lái)考慮問題。二、卡爾曼濾波器的介紹二、卡爾曼濾波器的介紹l 1.卡爾曼濾波器的基本思想l （1）過程方程l （2）測(cè)量方程l2.新息過程l3.利用新息過程進(jìn)行狀態(tài)估計(jì)l4.Kalman濾波抗野值三、卡爾曼濾波器的應(yīng)用三、卡爾曼濾波器的應(yīng)用l卡爾曼濾波器（ Kalman Filter ）是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法（ optimal r

3、ecursive dataprocessing algorithm ），它的廣泛應(yīng)用已經(jīng)超過 30 年，包括航空器軌道修正、機(jī)器人系統(tǒng)控制、雷達(dá)系統(tǒng)與導(dǎo)彈追蹤等。近年來(lái)更被應(yīng)用于組合導(dǎo)航與動(dòng)態(tài)定位，傳感器數(shù)據(jù)融合、微觀經(jīng)濟(jì)學(xué)等應(yīng)用研究領(lǐng)域。特別是在圖像處理領(lǐng)域如頭臉識(shí)別、圖像分割、圖像邊緣檢測(cè)等當(dāng)前熱門研究領(lǐng)域占有重要地位?？柭鼮V波作為一種數(shù)值估計(jì)優(yōu)化方法，與應(yīng)用領(lǐng)域的背景結(jié)合性很強(qiáng)。 因此在應(yīng)用卡爾曼濾波解決實(shí)際問題時(shí)，重要的不僅僅是算法的實(shí)現(xiàn)與優(yōu)化問題，更重要的是利用獲取的領(lǐng)域知識(shí)對(duì)被認(rèn)識(shí)系統(tǒng)進(jìn)行形式化描述， 建立起精確的數(shù)學(xué)模型，再?gòu)倪@個(gè)模型出發(fā)，進(jìn)行濾波器的設(shè)計(jì)與實(shí)現(xiàn)工作。濾波器實(shí)際

4、實(shí)現(xiàn)時(shí)，測(cè)量噪聲協(xié)方差 R 一般可以觀測(cè)得到，是濾波器的已知條件。 它可以通過離線獲取一些系統(tǒng)觀測(cè)值計(jì)算出來(lái)。 通常，難確定的是過程激勵(lì)噪聲協(xié)方差的 Q 值，因?yàn)槲覀儫o(wú)法直接觀測(cè)到過程信號(hào)。一種方法是通過設(shè)定一個(gè)合適的 Q ，給過程信號(hào)“注入”足夠的不確定性來(lái)建立一個(gè)簡(jiǎn)單的可以產(chǎn)生可接受結(jié)果的過程模型。 為了提高濾波器的性能，通常要按一定標(biāo)準(zhǔn)進(jìn)行系數(shù)的選擇與調(diào)整。四、卡爾曼濾波器算法四、卡爾曼濾波器算法l 在這一部分，我們就來(lái)描述源于Dr Kalman 的卡爾曼濾波器。下面的描述，會(huì)涉及一些基本的概念知識(shí)，包括概率（Probability），隨 即 變 量 （ R a n d o m V a

5、r i a b l e ） ， 高 斯 或 正 態(tài) 分 配（Gaussian Distribution）還有State-space Model等等。但對(duì)于卡爾曼濾波器的詳細(xì)證明，這里不能一一描述。 首先，我們先要引入一個(gè)離散控制過程的系統(tǒng)。該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程來(lái)描述：lX(k)=A X(k-1)+B U(k)+W(k) l再加上系統(tǒng)的測(cè)量值： Z(k)=H X(k)+V(k) l上兩式子中，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)

6、分別表示過程和測(cè)量的噪聲。l下面我們來(lái)用他們結(jié)合他們的covariances 來(lái)估算系統(tǒng)的最優(yōu)化輸出。 l首先我們要利用系統(tǒng)的過程模型，來(lái)預(yù)測(cè)下一狀態(tài)的系統(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)=AX(k-1|k-1)+BU(k).(1)l式(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)的控制量，如果沒有控制量，它可以為0。到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1)的covariance還沒更新。l我們用P表示covariance：P(k|k-1

7、)=AP(k-1|k-1)A+Q(2)l式(2)中，P(k|k-1)是X(k|k-1)對(duì)應(yīng)的covariance，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的covariance，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的covariance。式子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è)量值。l結(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)-HX(k|k-1)(3)其中Kg為卡爾曼增益(KalmanGain)：Kg(k)=P(k|k-1)

8、H/(HP(k|k-1)H+R)(4)l到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。l但是為了要另卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的covariance：lP(k|k)=（I-Kg(k)H）P(k|k-1)(5)l其中I為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去?？柭鼮V波器的原理基本描述了，式子1，2，3，4和5就是他的5個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易的實(shí)現(xiàn)計(jì)算機(jī)的程序。五、卡爾曼濾波器的簡(jiǎn)單應(yīng)用例子五、卡爾曼濾波器的簡(jiǎn)單應(yīng)

9、用例子l我們要研究的對(duì)象是一個(gè)房間的溫度。根據(jù)你的經(jīng)驗(yàn)判斷，這個(gè)房間的溫度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度（假設(shè)我們用一分鐘來(lái)做時(shí)間單位）。l把房間看成一個(gè)系統(tǒng)，然后對(duì)這個(gè)系統(tǒng)建模。當(dāng)然，我們見的模型不需要非常地精確。我們所知道的這個(gè)房間的溫度是跟前一時(shí)刻的溫度相同的，所以A=1。沒有控制量，所以U(k)=0。因此得出： X(k|k-1)=X(k-1|k-1) . (6) 式子（2）可以改成： P(k|k-1)=P(k-1|k-1) +Q (7) 因?yàn)闇y(cè)量的值是溫度計(jì)的，跟溫度直接對(duì)應(yīng)，所以H=1。式子3，4，5可以改成以下： lX ( k | k ) = X ( k |

10、k - 1 ) + K g ( k ) ( Z ( k ) - X ( k | k -1)(8)Kg(k)=P(k|k-1)/(P(k|k-1)+R)(9)P(k|k)=（1-Kg(k)）P(k|k-1)(10)l 現(xiàn)在我們模擬一組測(cè)量值作為輸入。假設(shè)房間的真實(shí)溫度為25度，我模擬了200個(gè)測(cè)量值，這些測(cè)量值的平均值為25度，但是加入了標(biāo)準(zhǔn)偏差為幾度的高斯白噪聲（在圖中為藍(lán)線）。為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個(gè)零時(shí)刻的初始值，是X(0|0)和P(0|0)。他們的值不用太在意，隨便給一個(gè)就可以了，因?yàn)殡S著卡爾曼的工作，X會(huì)逐漸的收斂。但是對(duì)于P，一般不要取0，因?yàn)檫@樣可能會(huì)令卡

11、爾曼完全相信你給定的X(0|0)是系統(tǒng)最優(yōu)的，從而使算法不能收斂。我選了X(0|0)=1度，P(0|0)=10。該系統(tǒng)的真實(shí)溫度為25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的最優(yōu)化結(jié)果（該結(jié)果在算法中設(shè)置了Q=1e-6，R=1e-1）。lclearl N=200; %隨機(jī)序列長(zhǎng)度l w(1)=0; %初始化操作l w=randn(1,N) %產(chǎn)生隨機(jī)序列l(wèi) x(1)=0; la=1;l for k=2:N;l x(k)=a*x(k-1)+w(k-1); %產(chǎn)生隨機(jī)序列x（n）l endl subplot(311)l t=1:N;l plot(t,x,b) %繪制原始輸入信號(hào)圖形x(n)

12、l title(原始輸入信號(hào)圖像)l xlabel(x軸單位：t/s,color,b)l ylabel(y軸單位：f/HZ,color,b)l V=randn(1,N);l q1=std(V); %計(jì)算隨機(jī)列序V的標(biāo)準(zhǔn)差 lRvv=q1.2; %將標(biāo)準(zhǔn)差q1的平方賦予Rvvl q2=std(w); %計(jì)算隨機(jī)序列w的標(biāo)準(zhǔn)差l Rww=q2.2; %將標(biāo)準(zhǔn)差q2的平方賦予Rwwl c=0.2;l y=c*x+V; %產(chǎn)生隨機(jī)序列Yl subplot(312)l t=1:N;l plot(t,y,g) %繪制加噪后輸入信號(hào)圖像l title(加噪后輸入信號(hào)圖像)l xlabel(x軸單位：t/s,

13、color,b)l ylabel(y軸單位：f/HZ,color,b)l p(1)=0; %初始化操作l s(1)=0; %初始化操作l for t=2:N;l p1(t)=a.2*p(t-1)+Rww;l b(t)=c*p1(t)/(c.2*p1(t)+Rvv);l s(t)=a*s(t-1)+b(t)*(y(t)-a*c*s(t-1); %輸出預(yù)測(cè)值估計(jì)公式l p(t)=p1(t)-c*b(t)*p1(t);l Endlsubplot(313)l t=1:N;l plot(t,s,r) %繪制經(jīng)過卡爾曼濾波后圖像l title(經(jīng)過卡爾曼濾波后圖像)l xlabel(x軸單位：t/s,co

14、lor,b)l ylabel(y軸單位：f/HZ,color,b)l圖020406080100120140160180200-50050原 始 輸 入 信 號(hào) 圖 像x軸 單 位 ： t/sy軸單位：f/HZ020406080100120140160180200-10010加 噪 后 輸 入 信 號(hào) 圖 像x軸 單 位 ： t/sy軸單位：f/HZ020406080100120140160180200-50050經(jīng) 過 卡 爾 曼 濾 波 后 圖 像x軸 單 位 ： t/sy軸單位：f/HZ卡爾曼濾波卡爾曼濾波lclearlN=800;lw(1)=0;lw=randn(1,N) %系統(tǒng)預(yù)測(cè)的隨

15、機(jī)白噪聲lx(1)=0;la=1;lfor k=2:N;lx(k)=a*x(k-1)+w(k-1); %系統(tǒng)的預(yù)測(cè)值lendlV=randn(1,N); %測(cè)量值的隨機(jī)白噪聲lq1=std(V);lRvv=q1.2;lq2=std(x);lRxx=q2.2;lq3=std(w);lRww=q3.2;lc=0.2;lY=c*x+V; %測(cè)量值lp(1)=0;ls(1)=0;lfor t=2:N;lp1(t)=a.2*p(t-1)+Rww; %前一時(shí)刻X的相關(guān)系數(shù)lb(t)=c*p1(t)/(c.2*p1(t)+Rvv); %卡爾曼增益ls(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s

16、(t-1); %經(jīng)過濾波后的信號(hào)lp(t)=p1(t)-c*b(t)*p1(t);%t狀態(tài)下x(t|t)的相關(guān)系數(shù)lendlfigure(1)lplot(x)ltitle(系統(tǒng)的預(yù)測(cè)值)lfigure(2)lplot(Y)ltitle(測(cè)量值)lfigure(3)lplot(s)ltitle(濾波后的信號(hào))l圖10100200300400500600700800-30-20-100102030系 統(tǒng) 的 預(yù) 測(cè) 值l圖20100200300400500600700800-8-6-4-202468測(cè) 量 值l圖30100200300400500600700800-30-20-100102030濾

17、 波 后 的 信 號(hào)lclearlN=200;lw(1)=0;lw=randn(1,N) lx(1)=0;la=1;lfork=2:N;lx(k)=a*x(k-1)+w(k-1);lendlV=randn(1,N);lq1=std(V);lRvv=q1.2;lq2=std(x);lRxx=q2.2;lq3=std(w);lRww=q3.2;lc=0.2;lY=c*x+V;lp(1)=0;ls(1)=0;fort=2:N;lp1(t)=a.2*p(t-1)+Rww;lb(t)=c*p1(t)/(c.2*p1(t)+Rvv);ls(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1);lp(t)=p1(t)-c*b(t)*p1(t);lendlt