卡爾曼濾波matlab仿真

1、隨機(jī)信號(hào)處理上機(jī)作業(yè) 題目：假設(shè)有一個(gè)二坐標(biāo)雷達(dá)對(duì)一平面上運(yùn)動(dòng)目標(biāo)的進(jìn)行觀察，目標(biāo)在t0400秒沿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é)果，畫出目標(biāo)真實(shí)軌跡、對(duì)目標(biāo)的觀察和濾波曲線。1、 跟蹤算法 考慮利用卡爾曼濾波算法對(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)變量狀態(tài)方程： (1) 觀測(cè)方程： (2)其中， 對(duì)目標(biāo)位置和速度的同

2、時(shí)濾波與一步預(yù)測(cè)的方程組如下:預(yù)測(cè)估計(jì)方程: 預(yù)測(cè)誤差協(xié)方差： 濾波估計(jì)增益：，其中濾波估計(jì)方程：濾波誤差協(xié)方差：2. 初始化 利用目標(biāo)的前幾個(gè)測(cè)量值建立狀態(tài)的其實(shí)估計(jì),采用兩點(diǎn)起始法。 濾波誤差均值： 濾波誤差標(biāo)準(zhǔn)差：2. 仿真分析 利用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)。圖 2.2圖2.2是單次濾波和100次濾波后的數(shù)據(jù)曲線。從圖中可以看出，濾波

3、剛開始時(shí)誤差較大，之后濾波誤差逐漸降低，估計(jì)值逐步逼近真實(shí)軌跡。而隨著濾波次數(shù)增加，濾波后的結(jié)果更為接近真實(shí)軌跡。圖 2.3圖 2.4 圖2.3,圖2.4分別是x和y方向?yàn)V波估計(jì)誤差均值及誤差標(biāo)準(zhǔn)差曲線。從圖上可以看出，濾波開始時(shí)誤差較大，隨著采樣次數(shù)的增加，誤差逐漸減小，誤差的標(biāo)準(zhǔn)差也具有相同特性。另外,可以看到由于在y方向上有速度分量,因此y方向的估計(jì)誤差均值比x方向的估計(jì)誤差均值波動(dòng)要大一些。%仿真場(chǎng)景sigma=10000;T=2;t=200;Vy=-15;C=1 0 0;0 1 0;A=1 0 0;0 1 T;0 0 1;eSk(:,t)=0 0 0;eSz(:,t)=0 0 0;e

4、eSz(:,t)=0 0;N=100;%蒙特卡洛次數(shù)for i=1:Nfor j=1:tZk(:,j)=2000+wgn(1,1,40);10000+Vy*T*(j-1)+wgn(1,1,40);endfor j=1:200if j=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;elseif j2Sk1(:,j)=A*Sk(:,j-1);

5、%預(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*(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

6、)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);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(

7、2,1,2);plot(eSk(1,:),eSk(2,:);title(100次濾波數(shù)據(jù)曲線);xlabel(X(米);ylabel(Y(米);j=1:t;figure;subplot(211);plot(j,eSz(1,:);title(X濾波誤差均值曲線);xlabel(采樣次數(shù));ylabel(X(米);subplot(212);for j=1:teeSz(1,j)=sqrt(eeSz(1,j)-eSz(1,j)2);eeSz(2,j)=sqrt(eeSz(2,j)-eSz(2,j)2);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);plot(j,Sk(3