統(tǒng)計(jì)信號(hào)處理實(shí)驗(yàn)三

1、統(tǒng)計(jì)信號(hào)處理實(shí)驗(yàn)三目的：掌握卡爾曼濾波濾波器的原理；內(nèi)容：用雷達(dá)跟蹤目標(biāo)，目標(biāo)的運(yùn)動(dòng)可以看成是在徑向和橫向內(nèi)的二維運(yùn)動(dòng)，其運(yùn)動(dòng)方程和觀測(cè)方程分別為：、和分別為徑向距離、速度和觀測(cè)值，而、和分別為橫向距離、速度和觀測(cè)值。和是狀態(tài)噪聲，是目標(biāo)速度的波動(dòng)；和是觀測(cè)噪聲；四種噪聲的均值都為0，呈高斯分布，互不相關(guān)。T是雷達(dá)掃描一次的時(shí)間，此處設(shè)為1.0秒。假設(shè)目標(biāo)距離雷達(dá)約160Km左右，徑向初速度設(shè)為300 m/s，并且在向雷達(dá)靠近，橫向初速度設(shè)為0 m/s。這樣它的徑向速度波動(dòng)大，而橫向速度波動(dòng)小，所以我們假設(shè)的方差為300m/s，的方差為m/s。鑒于雷達(dá)的觀測(cè)誤差，我們假設(shè)觀測(cè)噪聲和的方差和均為

2、1.0Km。其中，和的初始值不是最佳的，學(xué)生完全可自己修改以上參數(shù)，并觀察計(jì)算結(jié)果的變化，給出最好的濾波效果。任務(wù)：1） 試用濾波法對(duì)信號(hào)進(jìn)行處理，并通過計(jì)算機(jī)模擬對(duì)其跟蹤過程進(jìn)行驗(yàn)證；2） 試求其Kalman濾波方程，并通過計(jì)算機(jī)模擬對(duì)其跟蹤過程進(jìn)行驗(yàn)證；3） 假設(shè)目標(biāo)在運(yùn)動(dòng)過程中發(fā)生了機(jī)動(dòng)（速度在某個(gè)時(shí)刻突然發(fā)生了改變），試觀測(cè)此時(shí)的濾波和Kalman濾波結(jié)果，并對(duì)結(jié)果進(jìn)行解釋。要求：1）設(shè)計(jì)仿真計(jì)算的Matlab程序，給出軟件清單；2）完成實(shí)驗(yàn)報(bào)告，給出實(shí)驗(yàn)結(jié)果，并對(duì)實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析。解題思路：一、任務(wù)1：濾波狀態(tài)方程和輸出方程：k時(shí)刻對(duì)k+1時(shí)刻的狀態(tài)估計(jì)為：根據(jù)恒增益濾波思想，二者按

3、照一定比例疊加，得到恒增益濾波的濾波公式：其中的是一個(gè)4行2列的矩陣，需要自己推導(dǎo)。本題中，,距離新息：速度新息：按照新息修正的思想，對(duì)距離新息的加權(quán)系數(shù)取，對(duì)速度新息的加權(quán)系數(shù)取，直接對(duì)狀態(tài)變量預(yù)測(cè)估值進(jìn)行修正：即求得。二、任務(wù)2：Kalman濾波我們要想完成計(jì)算，就必須知道A、C和Q、R，觀測(cè)數(shù)據(jù)是必須提供的數(shù)據(jù)。A、C已經(jīng)知道，現(xiàn)在尋找Q、R的確定值。Q是狀態(tài)噪聲協(xié)方差矩陣：R是觀測(cè)噪聲協(xié)方差矩陣：下一步就是確定初始值和，作為遞推的初始數(shù)據(jù)。是預(yù)測(cè)誤差協(xié)方差矩陣：其中：，所以：在推導(dǎo)上式時(shí)，和是隨機(jī)過程中不同時(shí)刻的兩個(gè)隨機(jī)變量，我們認(rèn)為這兩個(gè)隨機(jī)變量統(tǒng)計(jì)獨(dú)立，而且是平穩(wěn)隨機(jī)過程，其不同時(shí)

4、刻的方差相同。兩個(gè)時(shí)刻的時(shí)差T為雷達(dá)掃描一圈的時(shí)間。這樣我們就有了初始值和，可以開始進(jìn)行遞推計(jì)算，首先計(jì)算k3時(shí)的狀態(tài)變量。狀態(tài)預(yù)測(cè)方程：狀態(tài)預(yù)測(cè)誤差 協(xié)方差矩陣：最佳增益方程：濾波估值方程：濾波估值誤差 協(xié)方差方程：然后計(jì)算k4時(shí)的狀態(tài)變量，如此遞推，直到波形估計(jì)結(jié)束。三、任務(wù)3：機(jī)動(dòng)目標(biāo)跟蹤在目標(biāo)機(jī)動(dòng)的情況下，即目標(biāo)運(yùn)動(dòng)軌跡突然變化，比較濾波器和卡爾曼濾波效果的差別。四、任務(wù)4（選作）：非平穩(wěn)噪聲下的目標(biāo)跟蹤自己建立仿真的、序列，控制好它們的方差Q（K）和R（K），并用于程序中。在Q（K）和R（K）改變的情況下，觀測(cè)恒增益濾波的結(jié)果和卡爾曼濾波的結(jié)果，比較并解釋。實(shí)驗(yàn)結(jié)果：1、 濾波取al

5、pha=0.8，beta=0.2，濾波結(jié)果如下：（紅色曲線代表濾波后的波形）改變alpha、beta的取值，使alpha=0.6，beta=0.1，濾波結(jié)果如下：從圖中可以看出，濾波之后波形變得更平滑了，在觀測(cè)信號(hào)尖峰的位置比較明顯。而且適當(dāng)減小alpha、后濾波效果更好了。2、 Kalman濾波從圖中可以看出，濾波之后波形變得更平滑了，在觀測(cè)信號(hào)尖峰的位置比較明顯。3、 機(jī)動(dòng)目標(biāo)跟蹤在t=50時(shí)刻，設(shè)置速度反向，t=56時(shí)刻，再次反向，得到的濾波和kalman濾波的波形分別如下：減小alpha，后，從圖像中可以看出，當(dāng)目標(biāo)狀態(tài)突變時(shí)，濾波和kalman濾波都能較好的跟蹤目標(biāo)機(jī)動(dòng)，即具有自適應(yīng)

6、性，而卡爾曼濾波的自適應(yīng)性更好。程序清單：%alpha_beta濾波alpha=0.8;beta=0.2;x0=10000 300 0 0'y0=10000 0'K=alpha 0; beta 0; 0 alpha; 0 beta;C=1 0 0 0; 0 0 1 0;A=1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1;X=zeros(4,100);Y=zeros(2,100);X(:,1)=x0;Y(:,1)=y0; wt=random('norm',0,sqrt(500),1,100);u1t=random('norm'

7、,0,sqrt(100),1,100);u2t=random('norm',0,sqrt(0.12),1,100);for i=1:99 v1t=300+u1t(1,i); v2t=u2t(1,i); Y(:,i+1)=10000-v1t*i+wt(1,i) v2t*i+wt(1,i)' x1(:,i+1)=A*X(:,i); %x(k+1|k) temp1=C*x1(:,i+1); temp2(1,1)=Y(1,i+1)-temp1(1,1); temp2(2,1)=Y(2,i+1)-temp1(2,1); X(:,i+1)=x1(:,i+1)+K*temp2; %x

8、(k+1)endfigure(1);i=1:100;plot(i,Y(1,:),i,X(1,:),'r');grid;title('alpha_beta濾波');%卡爾曼濾波C=1 0 0 0; 0 0 1 0;A=1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1;Q=0 0 0 0; 0 300 0 0; 0 0 0 0; 0 0 0 0.12;R=1000 0; 0 1000;I=1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1;v1t=300; X=zeros(4,100);Y=zeros(2,100);wt=ran

9、dom('norm',0,sqrt(1000),1,100);u1t=random('norm',0,sqrt(300),1,100);u2t=random('norm',0,sqrt(0.12),1,100);Y(:,1)=16000+wt(1,1) wt(1,1)'Y(:,2)=16000-v1t+wt(1,2) wt(1,2)'X(:,2)=Y(1,1) Y(1,2)-Y(1,1) Y(2,2) Y(2,2)-Y(2,1)'P2=1000 1000 0 0; 1000 2300 0 0; 0 0 1000 1000;

10、 0 0 1000 2000.12; Pi=P2;for i=3:100 v1t=300+u1t(1,i); v2t=u2t(1,i); Y(:,i)=16000-v1t*(i-1)+wt(1,i) v2t*i+wt(1,i)' x1(:,i)=A*X(:,i-1); %狀態(tài)預(yù)測(cè)估值 pi=A*Pi*A'+Q; %狀態(tài)預(yù)測(cè)誤差 協(xié)方差矩陣 Ki=pi*C'*inv(C*pi*C'+R); %最佳增益方程 temp1=C*x1(:,i); temp2(1,1)=Y(1,i)-temp1(1,1); temp2(2,1)=Y(2,i)-temp1(2,1); X(:

11、,i)=x1(:,i)+Ki*temp2; %濾波估值方程 Pi=(I-Ki*C)*pi; %濾波估值誤差 協(xié)方差方程endfigure(1);i=1:100;plot(i,Y(1,:),i,X(1,:),'r');grid;title('kalman濾波');%機(jī)動(dòng)情況下alpha_beta濾波alpha=0.8;beta=0.2;x0=10000 300 0 0'y0=10000 0'K=alpha 0; beta 0; 0 alpha; 0 beta;C=1 0 0 0; 0 0 1 0;A=1 1 0 0; 0 1 0 0; 0 0 1

12、1; 0 0 0 1;X=zeros(4,100);Y=zeros(2,100);X(:,1)=x0;Y(:,1)=y0; wt=random('norm',0,sqrt(500),1,100);u1t=random('norm',0,sqrt(100),1,100);u2t=random('norm',0,sqrt(0.12),1,100);for i=1:49 v1t=300+u1t(1,i); v2t=u2t(1,i); Y(:,i+1)=10000-v1t*i+wt(1,i) v2t*i+wt(1,i)' x1(:,i+1)=A*

13、X(:,i); %x(k+1|k) temp1=C*x1(:,i+1); temp2(1,1)=Y(1,i+1)-temp1(1,1); temp2(2,1)=Y(2,i+1)-temp1(2,1); X(:,i+1)=x1(:,i+1)+K*temp2; %x(k+1)end for i=50:60 v1t=150+u1t(1,i); v2t=u2t(1,i); Y(:,i+1)=10000+v1t*i+wt(1,i) v2t*i+wt(1,i)' x1(:,i+1)=A*X(:,i); %x(k+1|k) temp1=C*x1(:,i+1); temp2(1,1)=Y(1,i+1)

14、-temp1(1,1); temp2(2,1)=Y(2,i+1)-temp1(2,1); X(:,i+1)=x1(:,i+1)+K*temp2; %x(k+1)endfor i=61:99 v1t=150+u1t(1,i); v2t=u2t(1,i); Y(:,i+1)=10000-v1t*i+wt(1,i) v2t*i+wt(1,i)' x1(:,i+1)=A*X(:,i); %x(k+1|k) temp1=C*x1(:,i+1); temp2(1,1)=Y(1,i+1)-temp1(1,1); temp2(2,1)=Y(2,i+1)-temp1(2,1); X(:,i+1)=x1(

15、:,i+1)+K*temp2; %x(k+1)endfigure(1);i=1:100;plot(i,Y(1,:),i,X(1,:),'r');grid;title('alpha_beta濾波');%機(jī)動(dòng)情況下卡爾曼濾波C=1 0 0 0; 0 0 1 0;A=1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1;Q=0 0 0 0; 0 300 0 0; 0 0 0 0; 0 0 0 0.12;R=1000 0; 0 1000;I=1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1;v1t=300; X=zeros(4,100

16、);Y=zeros(2,100);wt=random('norm',0,sqrt(1000),1,100);u1t=random('norm',0,sqrt(300),1,100);u2t=random('norm',0,sqrt(0.12),1,100);Y(:,1)=16000+wt(1,1) wt(1,1)'Y(:,2)=16000-v1t+wt(1,2) wt(1,2)'X(:,2)=Y(1,1) Y(1,2)-Y(1,1) Y(2,2) Y(2,2)-Y(2,1)'P2=1000 1000 0 0; 1000 2

17、300 0 0; 0 0 1000 1000; 0 0 1000 2000.12; Pi=P2;for i=3:49 v1t=300+u1t(1,i); v2t=u2t(1,i); Y(:,i)=16000-v1t*(i-1)+wt(1,i) v2t*i+wt(1,i)' x1(:,i)=A*X(:,i-1); %狀態(tài)預(yù)測(cè)估值 pi=A*Pi*A'+Q; %狀態(tài)預(yù)測(cè)誤差 協(xié)方差矩陣 Ki=pi*C'*inv(C*pi*C'+R); %最佳增益方程 temp1=C*x1(:,i); temp2(1,1)=Y(1,i)-temp1(1,1); temp2(2,1)=

18、Y(2,i)-temp1(2,1); X(:,i)=x1(:,i)+Ki*temp2; %濾波估值方程 Pi=(I-Ki*C)*pi; %濾波估值誤差 協(xié)方差方程endfor i=50:60 v1t=150+u1t(1,i); v2t=u2t(1,i); Y(:,i)=16000+v1t*(i-1)+wt(1,i) v2t*i+wt(1,i)' x1(:,i)=A*X(:,i-1); %狀態(tài)預(yù)測(cè)估值 pi=A*Pi*A'+Q; %狀態(tài)預(yù)測(cè)誤差 協(xié)方差矩陣 Ki=pi*C'*inv(C*pi*C'+R); %最佳增益方程 temp1=C*x1(:,i); temp2(1,1)=Y(1,i)-temp1(1,1); temp2(2,1)=Y(2,i)-temp1(2,1); X(:,i)=x1(:,i)+Ki*temp2; %濾波估值方程 Pi=(I-Ki*C)*pi; %濾波估值誤差 協(xié)方差方程endfor i=61:99 v1t=150+u1t(1,i); v2t=u2t(1,i); Y(:,i)=16000-v1t*(i-1)+wt(1,i) v2t*i+wt(1,i)' x1(:,i)=A*X(:,i-1); %狀態(tài)預(yù)測(cè)估值 pi=