zuijinlinyu.m

本程序通过卡尔曼滤波。对航迹进行估计

function [X,Y]=Kalman_filter(Ts,offtime)
% Kalman_filter             采用Kalman滤波方法，从观测数值中得到航迹的估计
% Ts                        采样时间，即雷达工作周期
% offtime                   仿真截止时间

x0=0;%起始点坐标
y0=10;
vx=10;
vy=0; % 沿-y方向

for t=1:offtime
    x(t)=x0+vx*t;
    y(t)=y0+vy*t;
end


w=0.2;%s  过程噪声标准差值
d=0.6;%d 量测噪声的标准差值

Pv=d*d; % 噪声的功率
N=offtime/Ts; % 采样点数

% 定义系统的状态方程
Phi=[1,Ts;0,1];
Gamma=[Ts*Ts/2;Ts];
C=[1 0];
R=Pv;%量测噪声协方差阵
Q=w^2;

randn('state',sum(100*clock)); % 设置随机数发生器
for i=1:N
   vx(i)=d*randn(1); % 观测噪声，两者独立
   vy(i)=d*randn(1);
   zx(i)=x(i)+vx(i); % 实际观测值
   zy(i)=y(i)+vy(i);
end
 
        Xfli=[zx(2) (zx(2)-zx(1))/Ts]'; %利用前两个观测值来对初始条件进行估计
        Px=[d,d/Ts;d/Ts,2*d/Ts];
        for k=3:N+1
        Xest=Phi*Xfli; % 状态一步预测值
        Xzst=C*Xest;%量测预测
        XX(k)=Xzst;
        Pxe=Phi*Px*Phi'+Gamma*Q*Gamma'; % 预测协方差阵
        Xs=C*Pxe*C'+R;%新息协方差
        Kx=Pxe*C'*inv(Xs); % Kalman滤波增益
        Px=Pxe-Kx*Xs*Kx';%协方差更新
           if(k==N+1) break;
            end;
        Xv=zx(k)-C*Xest;%新息
        Xfli=Xest+Kx*(zx(k)-C*Xest); %滤波
        XE(k)=Xfli(1,1);
        end
        XE(1)=zx(1);XE(2)=zx(2);
        
        
        Yfli=[zy(2) (zy(2)-zy(1))/Ts]'; %利用前两个观测值来对初始条件进行估计
        Py=[d,d/Ts;d/Ts,2*d/Ts];
        for k=3:N+1
        Yest=Phi*Yfli; % 状态一步预测值
        Yzst=C*Yest;%量测预测
        YY(k)=Yzst;
        Pye=Phi*Py*Phi'+Gamma*Q*Gamma'; % 预测误差的协方差阵
        Ys=C*Pye*C'+R;%新息协方差
        Ky=Pye*C'*inv(Ys);% Kalman滤波增益
        Py=Pye-Ky*Ys*Ky';%协方差更新
        if(k==N+1) break;
        end;
        Yv=zy(k)-C*Yest;%新息
        Yfli=Yest+Ky*(zy(k)-C*Yest); 
        YE(k)=Yfli(1,1);
        end
        YE(1)=zy(1);YE(2)=zy(2); 
        
        A=XX(100);
        B=YY(100);

%画波门
x1=A-abs(Xs)
x2=A+abs(Xs)
y1=B-abs(Ys)
y2=B+abs(Ys)

i=0:1000;
Y=y1+((y2-y1)/1000)*i;
X=x1+((x2-x1)/1000)*i;
%subplot(1,2,1),plot(x,y,'b',XE,YE,'r',A,B,'*');title('滤波');axis([0 1100 -10 30]);
%subplot(1,2,2),plot(A,B,'*',x1,Y,'r',X,y1,'r',x2,Y,'r',X,y2,'r');title('波门');


%产生回波
randn('state',sum(100*clock)); % 设置随机数发生器
for j=1:5
   m(j)=A+randn(1);
   n(j)=B+randn(1); 
end
subplot(1,2,1),plot(x,y,'b',XE,YE,'r',A,B,'*');title('滤波');axis([0 1100 -10 30]);
subplot(1,2,2),plot(A,B,'*',x1,Y,'r',X,y1,'r',x2,Y,'r',X,y2,'r',m(1),n(1),'.',m(2),n(2),'.',m(3),n(3),'.',m(4),n(5),'.',m(5),n(5),'.');title('波门');

%最近邻域算法