ls.m

卡尔曼的滤波跟踪程序

function [x,y,zx,zy,XE,YE,XERB,YERB,XSTD,YSTD]=LS(T,M,d)
%输入参数：观测噪声方差d,采样间隔T,蒙特卡洛次数M
totalTime=800;  
%真实航迹产生
[x,y]=realTrack(T,totalTime);
%随机初始化
randn('state',sum(100*clock));
D=d*d;
N=ceil(totalTime/T);
for mont=1:M
    %观测数据产生
    for i=1:N
        zx(i)=x(i)+d*randn(1);
        zy(i)=y(i)+d*randn(1);
    end
    %开始滤波
    t=T:T:N*T;
    %X方向
    XXX2=[zx(1)*t(2)-zx(2)*t(1)  zx(2)-zx(1)]'/(t(2)-t(1));
    XG(2)=XXX2(1);
    VXG(2)=XXX2(2);
    XG(1)=XG(2);
    VXG(1)=VXG(2);
    for k=2:(N-1)
        K=[-2/(k+1) 6/((k+1)*(k+2)*T)]';
        H=[1 t(k+1)];
        XXX2=XXX2+K*(zx(k+1)-H*XXX2);
        XG(k+1)=XXX2(1);
        VXG(k+1)=XXX2(2);
    end
    %Y方向
    YYY2=[zy(1)*t(2)-zy(2)*t(1)  zy(2)-zy(1)]'/(t(2)-t(1));
    YG(2)=YYY2(1);
    VYG(2)=YYY2(2);
    YG(1)=YG(2);
    VYG(1)=VYG(2);
    for k=2:(N-1)
        K=[-2/(k+1) 6/((k+1)*(k+2)*T)]';
        H=[1 t(k+1)];
        YYY2=YYY2+K*(zy(k+1)-H*YYY2);
        YG(k+1)=YYY2(1);
        VYG(k+1)=YYY2(2);
    end
    %计算位置估计
    for k=1:N
        XE(k)=XG(k)+VXG(k)*t(k);
    end
    for k=1:N
        YE(k)=YG(k)+VYG(k)*t(k);
    end
    %误差矩阵
    XER(1:N,mont)=x(1:N)-(XE(1:N))';
    YER(1:N,mont)=y(1:N)-(YE(1:N))';
end  %end for mont
%滤波误差的均值
XERB=mean(XER,2);
YERB=mean(YER,2);
%滤波误差的标准差
XSTD=std(XER,1,2);
YSTD=std(YER,1,2);
%作图 
% figure(1) 
% plot(zx,zy,'g',x,y,'red',XE,YE,'b')
% axis([1500 4500 -10000 11000]); 
% figure(2) 
% subplot(2,2,1)
% plot(XERB) 
% xlabel('观测次数') 
% ylabel('X方向滤波误差均值') 
% subplot(2,2,2) 
% plot(YERB)
% xlabel('观测次数') 
% ylabel('Y方向滤波误差均值') 
% subplot(2,2,3) 
% plot(XSTD)
% xlabel('观测次数') 
% ylabel('X方向滤波误差标准值') 
% subplot(2,2,4) 
% plot(YSTD)
% xlabel('观测次数') 
% ylabel('Y方向滤波误差标准值')