卡爾曼濾波 matlab仿真

1、隨機(jī)信號(hào)處理上機(jī)作業(yè)題目：假設(shè)有一個(gè)二坐標(biāo)雷達(dá)對(duì)一平面上運(yùn)動(dòng)目標(biāo)的進(jìn)行觀察，目標(biāo)在t=0400秒沿y軸作恒速直線運(yùn)功，運(yùn)動(dòng)速度為-15m/s,目標(biāo)的起點(diǎn)為(2000m,10000m),雷達(dá)掃描周期為2秒，x和y獨(dú)立地進(jìn)行觀察，觀察噪聲的標(biāo)準(zhǔn)差均為100m。試建立雷達(dá)對(duì)目標(biāo)的跟蹤算法，并進(jìn)行仿真分析，給出仿真結(jié)果，畫(huà)出目標(biāo)真實(shí)軌跡、對(duì)目標(biāo)的觀察和濾波曲線。一、跟蹤算法考慮利用卡爾曼濾波算法對(duì)目標(biāo)的運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)。由于目標(biāo)在二維平面內(nèi)做勻速運(yùn)動(dòng)，因此這里只考慮勻速運(yùn)動(dòng)情況。1.建立模型由于目標(biāo)沿y軸作勻速直線運(yùn)動(dòng)，取狀態(tài)變量XS=yvy狀態(tài)方程：sa+1丿二asa丿(1)觀測(cè)方程：Zk7=CSk

2、)+Vk丿其中，(2)0_00zvxV=xzvyyZ=對(duì)目標(biāo)位置和速度的同時(shí)濾波與一步預(yù)測(cè)的方程組如下:預(yù)測(cè)估計(jì)方程:S(k/k-1)=aS(k-1)預(yù)測(cè)誤差協(xié)方差：P(k/k-1)=AP(k-1)AT濾波估計(jì)增益：G2x0()CT(k/k-1)B(k)=R=Ck/k-1匕+R，其中濾波估計(jì)方程：S(k/k)=S(k/k一1)+)_Z(k)-S(/k-1)濾波誤差協(xié)方差：P(k/k)=11-B(k)Cb(/k-1)2.初始化利用目標(biāo)的前幾個(gè)測(cè)量值建立狀態(tài)的其實(shí)估計(jì),采用兩點(diǎn)起始法。S(2/2)=Z(2)xZ(2)Z(2)y-Z(1)yyTp2/2)=G2xG2yG2TG2t2G2yT濾波誤差均

3、值：-x=二芒S(k)-Si/k)i=1濾波誤差標(biāo)準(zhǔn)差：S(k)-SH/k)二.仿真分析利用MATLAB對(duì)前面建立的模型進(jìn)行仿真，結(jié)果如下。圖2.1圖2.1是目標(biāo)運(yùn)動(dòng)的真實(shí)軌跡和觀測(cè)軌跡曲線。其中，真實(shí)軌跡顯示目標(biāo)在x=2000米處沿y軸方向做勻速直線運(yùn)動(dòng)，而觀測(cè)軌跡是目標(biāo)運(yùn)動(dòng)的真實(shí)軌跡加上方差和隨機(jī)測(cè)量噪聲得到的。從圖中可以看出，觀測(cè)軌跡圍繞真實(shí)軌跡作上下浮動(dòng)。單次濾波數(shù)據(jù)曲線100次濾波數(shù)據(jù)曲線圖2.2圖2.2是單次濾波和100次濾波后的數(shù)據(jù)曲線。從圖中可以看出，濾波剛開(kāi)始時(shí)誤差較大，之后濾波誤差逐漸降低，估計(jì)值逐步逼近真實(shí)軌跡。而隨著濾波次數(shù)增加，濾波后的結(jié)果更為接近真實(shí)軌跡。圖2.3圖

4、2.4圖2.3,圖2.4分別是x和y方向?yàn)V波估計(jì)誤差均值及誤差標(biāo)準(zhǔn)差曲線。從圖上可以看出，濾波開(kāi)始時(shí)誤差較大，隨著采樣次數(shù)的增加，誤差逐漸減小，誤差的標(biāo)準(zhǔn)差也具有相同特性另外，可以看到由于在y方向上有速度分量，因此y方向的估計(jì)誤差均值比x方向的估計(jì)誤差均值波動(dòng)要大一些。%仿真場(chǎng)景sigma=10000;T=2;t=200;Vy=-15;C=100;010;A=100;01T;001;eSk(:,t)=000;eSz(:,t)=000;eeSz(:,t)=00;N=100;%蒙特卡洛次數(shù)fori=1:Nforj=1:tZk(:,j)=2000+wgn(1,1,40);10000+Vy*T*(j-

5、1)+wgn(1,1,40);endforj=1:200ifj=1Sk(:,1)=Zk(1,1),Zk(2,1),0;Sk1(:,1)=Sk(:,1);Sk(:,2)=Zk(1,2),Zk(2,2),(Zk(2,2)-Zk(2,1)/T;Sk1(:,2)=Sk(:,2);Pk=sigma,0,0;0,sigma,sigma/T;0,sigma/T,2*sigma/T;elseifj2Sk1(:,j)=A*Sk(:,j-1);%預(yù)測(cè)Pk1=A*Pk*A;%預(yù)測(cè)誤差協(xié)方差Bk=Pk1*C*inv(C*Pk1*C+sigma*eye(2);%kalman增益Sk(:,j)=Sk1(:,j)+Bk*(

6、Zk(:,j)-C*Sk1(:,j);%濾波Pk=(eye(3)-Bk*C)*Pk1;%濾波協(xié)方差endend%1000次求平均eSk(:,j)=eSk(:,j)+Sk(:,j)/N;%濾波eSz(:,j)=eSz(:,j)+(2000;10000+Vy*(j-1)*T;0-Sk(:,j)/N;%濾波誤差均值eeSz(:,j)=eeSz(:,j)+(2000-Sk(1,j)人2;(10000+Vy*(j-1)*T-Sk(2,j)人2/N;%濾波誤差標(biāo)準(zhǔn)差endend%繪圖%真實(shí)軌跡和測(cè)量軌跡subplot(2,1,1);j=0:0.1:t;plot(2000,10000+Vy*(j-1)*T)

7、;title(目標(biāo)真實(shí)軌跡);xlabel(X(米);ylabel(Y(米)；subplot(2,1,2);plot(Zk(1,:),Zk(2,:);title(測(cè)量軌跡);xlabel(X(米);ylabel(Y(米)；%濾波單次仿真和蒙特卡洛仿真figure;subplot(2,1,1);plot(Sk(1,:),Sk(2,:);title(單次濾波數(shù)據(jù)曲線);xlabel(X(米);ylabel(Y(米)；subplot(2,1,2);plot(eSk(l,:),eSk(2,:);title(100次濾波數(shù)據(jù)曲線)；xlabel(X(米);ylabel(Y(米)；j=1:t;figure

8、;subplot(211);plot(j,eSz(1,:);title(X濾波誤差均值曲線);xlabel(采樣次數(shù));ylabel(X(米)；subplot(212);forj=1:teeSz(1,j)=sqrt(eeSz(1,j)-eSz(1,j)A2);eeSz(2,j)=sqrt(eeSz(2,j)-eSz(2,j)A2);endj=1:t;plot(j,eeSz(1,:);title(x濾波誤差標(biāo)準(zhǔn)差曲線)；xlabel(采樣次數(shù));ylabel(X(米)；figure;subplot(211);plot(j,eSz(2,:);title(y濾波誤差均值曲線);xlabel(采樣次數(shù));ylabel(Y(米)；subplot(212);plot(j,eeSz(2,:);title(y濾波誤差標(biāo)準(zhǔn)差曲線)；xlabel(采樣次數(shù));ylabel(Y(米/秒)；figure;subplot(211);