目标跟踪问题的应用背景是雷达数据处理，即雷达在搜索到目标并记录目标的位置数据，对测量到的目标位置数据（称为点迹）进行处理，自动形成航迹，并对目标在下一时

刻的位置进行预测。本文简要讨论了用Kalman滤波方法对单个目标航迹进行预测，并借助于Matlab仿真工具，对实验的效果进行评估。

clear all;clc;%================================仿真场景===================================Ts=2;%雷达扫描周期d=100;%噪声的标准差值Pv=10000;%噪声的功率sigma=10;% 加速度方向的的扰动offtime=900;%仿真的截止时间Num=50;%蒙特卡洛仿真次数N=ceil(offtime/Ts); % 采样点数%描述系统运动轨迹x=zeros(offtime,1);%x轴方向的运动轨迹y=zeros(offtime,1);%y轴方向的运动轨迹X=zeros(N,1);%x轴方向的雷达观测数据Y=zeros(N,1);%y轴方向的雷达观测数据
%t=0:400s,速度vx,vy为沿x和y轴的速度分量(m/s),目标在这段时间做匀速运动x0=2000;%起始点坐标y0=10000;vx=0;vy=-15; % 沿-y方向
for t=1:400    x(t)=x0+vx*t;    y(t)=y0+vy*t;end
% t=400:600s，ax,ay为沿x和y轴的加速度分量(m/s/s)ax=0.075;ay=0.075;for t=0:200    x(t+401)=x(400)+vx*t+ax*t*t/2;    y(t+401)=y(400)+vy*t+ay*t*t/2;endvx=vx+ax*200; % 第一次机动转弯结束时的速度vy=vy+ay*200;
%t=600:610s匀速运动for t=0:10;    x(t+601)=x(601)+vx*t;    y(t+601)=y(601)+vy*t;end
% t=610:660s，第二次转弯ax=-0.3;%第二次转弯的加速度ay=0.3;for t=0:50    x(t+611)=x(611)+vx*t+ax*t*t/2;    y(t+611)=y(611)+vy*t+ay*t*t/2;end
vx=vx+ax*(660-610);% 第二次机动转弯结束时的速度vy=vy+ay*(660-610);
% 660s以后匀速运动，一直到截止时间for t=0:(offtime-660)    x(t+661)=x(661)+vx*t;    y(t+661)=y(661)+vy*t;end
% 得到雷达的观测数据for n=0:Ts:offtime-1    X(n/Ts+1)=x(n+1);    Y(n/Ts+1)=y(n+1);end
%定义系统的状态方程Phi=[1,Ts;0,1];Gamma=[Ts*Ts/2;Ts];C=[1 0];R=Pv;Q=sigma^2;WX=[];WY=[];ax=[zeros(1,400) 0.075*ones(1,200) zeros(1,10) -0.3*ones(1,50) zeros(1,offtime-660)]; % 对x方向不同时段的加速度进行描述ay=[zeros(1,400) 0.075*ones(1,200) zeros(1,10)  0.3*ones(1,50) zeros(1,offtime-660)]; % 对y方向不同时段的加速度进行描述
%x轴方向的各类矩阵定义Xe_x=zeros(2,1); % 用前k-1时刻的输出值估计k时刻的预测值Xf_x=zeros(2,1); % k时刻Kalman滤波器的输出值Xes_x=zeros(2,1); % 预测输出误差Xef_x=zeros(2,1); % 滤波后输出的误差Pxe_x=zeros(2,1); % 预测输出误差均方差矩阵 Px_x=zeros(2,1);  % 滤波输出误差均方差矩阵XE=zeros(1,N); % 得到最终的滤波输出值，仅仅考虑距离分量
%y轴方向的各类矩阵定义Xe_y=zeros(2,1); % 用前k-1时刻的输出值估计k时刻的预测值Xf_y=zeros(2,1); % k时刻Kalman滤波器的输出值Xes_y=zeros(2,1); % 预测输出误差Xef_y=zeros(2,1); % 滤波后输出的误差Pxe_y=zeros(2,1); % 预测输出误差均方差矩阵 Px_y=zeros(2,1);  % 滤波输出误差均方差矩阵YE=zeros(1,N); % 得到最终的滤波输出值，仅仅考虑距离分量
   for i=1:Num    randn('state',sum(100*clock)); % 设置随机数发生器    for n=0:Ts:offtime-1    WX(n/Ts+1)=ax(n+1)+sigma*randn(1,1);    WY(n/Ts+1)=ay(n+1)+sigma*randn(1,1);    end     for j=1:N        Vx(j)=d*randn(1);% 观测噪声，两者独立        Vy(j)=d*randn(1);        zx(j)=X(j)+Vx(j); % 实际观测值        zy(j)=Y(j)+Vy(j);    end                    Xf_x=[zx(2) (zx(2)-zx(1))/Ts]'; %利用前两个观测值来对初始条件进行估计        Xf_y=[zy(2) (zy(2)-zy(1))/Ts]'; %利用前两个观测值来对初始条件进行估计        Xef_x=[-Vx(2) Ts*WX(1)/2+(Vx(1)-Vx(2))/Ts]';        Xef_y=[-Vy(2) Ts*WY(1)/2+(Vy(1)-Vy(2))/Ts]';        Px_x=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts+Ts*Ts*Q/4];        Px_y=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts+Ts*Ts*Q/4];     for k=3:N          Xe_x=Phi*Xf_x; % 更新该时刻的预测值          Xe_y=Phi*Xf_y; % 更新该时刻的预测值          Xes_x=Phi*Xef_x+Gamma*WX(k-1); % 预测输出误差          Xes_y=Phi*Xef_y+Gamma*WY(k-1); % 预测输出误差          Pxe_x=Phi*Px_x*Phi'+Gamma*Q*Gamma'; % 预测误差的协方差阵          Pxe_y=Phi*Px_y*Phi'+Gamma*Q*Gamma'; % 预测误差的协方差阵          K_x=Pxe_x*C'*inv(C*Pxe_x*C'+R); % x轴Kalman滤波增益          K_y=Pxe_y*C'*inv(C*Pxe_y*C'+R); %y轴 Kalman滤波增益              Xf_x=Xe_x+K_x*(zx(k)-C*Xe_x);           Xf_y=Xe_y+K_y*(zy(k)-C*Xe_y);           Xef_x=(eye(2)-K_x*C)*Xes_x-K_x*Vx(k);          Xef_y=(eye(2)-K_y*C)*Xes_y-K_y*Vy(k);          Px_x=(eye(2)-K_x*C)*Pxe_x;          Px_y=(eye(2)-K_y*C)*Pxe_y;                  XE(k)=Xf_x(1,1);          YE(k)=Xf_y(1,1);     end                 XE(1)=zx(1);XE(2)=zx(2);         YE(1)=zy(1);YE(2)=zy(2);     %误差矩阵         XER(1:N,i)=X(1:N)-(XE(1:N))';         YER(1:N,i)=Y(1:N)-(YE(1:N))';end%滤波误差的均值XERB=mean(XER,2);YERB=mean(YER,2);
%滤波误差的标准差XSTD=std(XER,1,2); % 计算有偏的估计值YSTD=std(YER,1,2);
%===========================绘图====================================%真实轨迹figureplot(X,Y,'r'),hold onaxis([1500 5000 1000 10000]),grid on;title('目标真实轨迹')%x轴方向的真实轨迹和滤波估计的轨迹figureplot(X(k),'r'),hold onplot(XE,'b'),hold ongrid onlegend('真实轨迹','滤波估计航迹');%y轴方向的真实轨迹和滤波估计的轨迹figureplot(Y,'r'),hold onplot(YE,'b'),hold ongrid on;legend('真实航迹','滤波估计航迹');%目标真实轨迹和观测轨迹figureplot(X,Y,'r'),hold on;plot(zx,zy,'g'),hold on;axis([1500 5000 1000 10000]),grid on;legend('理论航迹','观测数据');%目标真实轨迹和滤波估计的轨迹figureplot(X,Y,'r'),hold on;plot(XE,YE,'b'),hold off;axis([1500 5000 1000 10000]),grid on;legend('真实轨迹','滤波估计');
%估计误差曲线figuresubplot(2,1,1)plot(XERB)axis([0 500 -50 50])xlabel('观测次数')ylabel('X方向滤波误差均值'),grid on;subplot(2,1,2)plot(YERB)axis([0 500 -50 50])xlabel('观测次数')ylabel('Y方向滤波误差均值'),grid on;%估计误差标准曲线figuresubplot(2,1,1)plot(XSTD)axis([0 500 0 150])xlabel('观测次数')ylabel('X方向滤波误差标准值'),grid on;subplot(2,1,2)plot(YSTD)axis([0 500 0 150])xlabel('观测次数')ylabel('Y方向滤波误差标准值'),grid on;

往期回顾>>>>>>

【模式识别】Matlab指纹识别

【图像处理】LSB水印技术

matlab自动识别银行卡号

【基础教程】MATLAB导出高清晰图片

无参考图像质量评价之基于多特征的增强图像质量评价

更多资料请关注！！