kalman波形估计在雷达数据处理中的应用

%kalman波形估计在雷达数据处理中的应用 clc; clear; close all; T=2;%雷达扫描周期 num=50;%滤波次数 %*****************产生真实的轨迹************************ N1=round(66.7/T);N2=round(255.1/T);N3=round(320/T); x=zeros(N3,1); y=zeros(N3,1); vx=zeros(N3,1); vy=zeros(N3,1); x(1)=-20000;%目标初始位置 y(1)=18000; vx(1)=300;%目标初始速度 vy(1)=0; ax=0;%目标初始加速度 ay=0; var=100;%观测噪声方差 for i=1:N3-1 if(i>N1-1&i<=N2-1) %这一阶段是横向和纵向均有加速度的时段 sita=pi/(N2-N1)*(i-N1+1); ax=-5*sin(sita); ay=-5*cos(sita); vx(i+1)=vx(i)+ax*T; vy(i+1)=vy(i)+ay*T; elseif(i>N2-1&i<=N3-1) ax=0; ay=0; vx(i+1)=vx(i); vy(i+1)=0; else ax=0; ay=0; vx(i+1)=vx(i); vy(i+1)=vy(i); end x(i+1)=x(i)+vx(i)*T+0.5*ax*T^2; y(i+1)=y(i)+vy(i)*T+0.5*ay*T^2; end %*****************产生真实轨迹结束******************** rex=num:N3; %形成两个矩阵用于存储50次结果 rey=num:N3; %滤波50次 for m=1:num; %产生噪声当作测量噪声 nx=randn(N3,1);%产生正态分布的噪声 ny=randn(N3,1); %加入噪声 zx=x+nx; zy=y+ny; %kalman滤波初始化 rex(m,1)=-20000;%初始化初始位置 rey(m,1)=3000; ki=0; low=1;high=0; u=0;ua=0; e=0.8; %加权衰减因子 z=2:1; xks(1)=zx(1); yks(1)=zy(1); xks(2)=zx(2); yks(2)=zy(2); o=4:4;g=4:2;h=2:4;q=2:2;xk=4:1;perr=4:4; o=[1,T,0,0; 0,1,0,0; 0,0,1,T;0,0,0,1]; h=[1 0 0 0;0 0 1 0]; g=[T/2,0;T/2,0;0,T/2;0,T/2]; q=[var^2 0;0 var^2];%测量噪声方差阵 perr=[var^2 var^2/T 0 0 %误差协方差矩阵 var^2/T 2*var^2/(T^2) 0 0 0 0 var^2 var^2/T 0 0 var^2/T 2*var^2/(T^2)]; vx=(zx(2)-zx(1))/T;%对速度进行初始化 vy=(zy(2)-zy(1))/T;%雷达扫描周期为T秒 xk=[zx(1);vx;zy(1);vy];%得到初始滤波值 %kalman滤波开始 for r=3:N3; if(u<=30)%非机动模型，赋初值 30 if(low==0) [o,g,h,q,perr,xk]=lmode_initial(T,r,zx,zy,vkxs,vkys,perr2); z=2:1; high=0; low=1; ua=0; end z=[zx(r);zy(r)]; xk1=o*xk;%预测 perr1=o*perr*o';%perr是误差协方差,perr1是预测误差协方差 k=perr1*h'*inv(h*perr1*h'+q);%kalman增益 xk=xk1+k*(z-h*xk1);%xk是滤波值,xk1是预测值，z-h*xk1;即是新息 perr=(eye(4)-k*h)*perr1;% xks(r)=xk(1,1);%滤波得到值进行存储 yks(r)=xk(3,1); vkxs(r)=xk(2,1); vkys(r)=xk(4,1); xk1s(r)=xk1(1,1);%预测位置值存储save yk1s(r)=xk1(3,1); perr11(r)=perr(1,1);%滤波后误差协方差进行存储 perr12(r)=perr(1,2); perr22(r)=perr(2,2); if(r>=20) %r取值;vd算法最先开始是采用非机动模型,然后根据新息计算调整 v=z-h*xk1;%这就是所谓的新息序列 w=h*perr*h'+q; p=v'*inv(w)*v; u=e*u+p; end elseif(u>30)%启动机动检测 if(high==0) [o,g,h,q,perr,xk]=hmode_initial(T,r,e,zx,zy,xk1s,yk1s,vkxs,vkys,perr11,perr12,perr22,var); high=1;low=0; for i=r-5:r-1 z=[zx(i);zy(i)]; xk1=o*xk; perr1=o*perr*o'; k=perr1*h'*inv(h*perr1*h'+q);%注意inv必须是方阵 xk=xk1+k*(z-h*xk1); perr=(eye(6)-k*h)*perr1; xks(r)=xk(1,1); yks(r)=xk(3,1); vkxs(r)=xk(2,1); vkys(r)=xk(4,1); xk1s(r)=xk1(1,1); yk1s(r)=xk1(3,1); end end z=[zx(r);zy(r)]; xk1=o*xk; perr1=o*perr*o'; k=perr1*h'*inv(h*perr1*h'+q); xk=xk1+k*(z-h*xk1); perr=(eye(6)-k*h)*perr1; xks(r)=xk(1,1); yks(r)=xk(3,1); vkxs(r)=xk(2,1); vkys(r)=xk(4,1); xk1s(r)=xk(1,1); yk1s(r)=xk1(3,1); ag=[xk(5,1);xk(6,1)]; perr2=perr; ki=ki+1; pm=[perr(5,5) perr(5,6);perr(6,5) perr(6,6)]; pa=ag'*inv(pm)*ag; sa(r)=pa; if(ki>5)%退出机动判断10 u1=sa(r-4)+sa(r-3)+sa(r-2)+sa(r-1)+sa(r); sb(r)=u1; if(u1<1e4)%^_^最终把这个门限改正确是很关键的！！ 1e4 u=0; end end end rex(m,r)=xks(r); rey(m,r)=yks(r); end%结束一次滤波 end ex=0;ey=0; eqx=0;eqy=0; ey1=0; ex1=N3:1;ey1=N3:1; qx=N3:1; qy=N3:1; %计算滤波的均值以及均方值 for i=1:N3 for j=1:num ex=ex+x(i)-rex(j,i); ey=ey+y(i)-rey(j,i); eqx=eqx+(x(i)-rex(j,i))^2; eqy=eqy+(y(i)-rey(j,i))^2; end ex1(i)=ex/num; ey1(i)=ex/num; qx(i)=(eqx/num)^0.5; qy(i)=(eqy/num)^0.5; ex=0;eqx=0;ey=0;eqy=0; end %绘图 %******************************************* figure(1); plot(x,y,'b-',zx,zy,'c:',xks,yks,'m-.'); legend('真实轨迹','观测样本','估计轨迹'); %******************************************* figure(2); %subplot(211); plot(zx,zy,'g:'); title('观测样本'); %subplot(212); figure(3); plot(xks,yks,'r-.'); title('估计轨迹'); n=T.*[0:N3-1]; %******************************************* figure(4); subplot(211); plot(n,ex1);axis([0,400,-500,500]); legend('x方向平均误差'); subplot(212); plot(n,qx);axis([0,400,-500,500]); legend('x方向滤波曲线的标准差曲线'); figure(5); subplot(211); plot(n,ey1);axis([0,400,-500,500]); legend('y方向平均误差'); subplot(212); plot(n,qy);axis([0,400,-500,500]); legend('y方向滤波曲线的标准差曲线');