imm.m

使用卡尔曼滤波进行目标追踪的matlab源代码

function [x,y,zx,zy,XXE,YYE,XERB,YERB,XSTD,YSTD]=IMM(T,mont,d)
%输入参数：观测噪声方差d,采样间隔T,蒙特卡洛次数mont
%返回值：
totalTime=800;  
%真实航迹产生
[x,y]=realTrack(T,totalTime);
%随机初始化
randn('state',sum(100*clock));
D=d*d;
N=ceil(totalTime/T);
for n=1:mont;
    %观测数据产生
    for i=1:N
        zx(i)=x(i)+d*randn(1);
        zy(i)=y(i)+d*randn(1);
    end
    %滤波(IMM)
    [XXE,YYE]=immkf(zx,zy,T,D);   
    XXE(1)=zx(1);XXE(2)=zx(1);
    YYE(1)=zy(1);YYE(2)=zy(1);
    %误差矩阵
    XER(1:N,n)=x(1:N)-(XXE(1:N))';
    YER(1:N,n)=y(1:N)-(YYE(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(x,y,'r');hold on;
% plot(zx,zy,'g');hold on;
% plot(XXE,YYE,'b');
% axis([1500 5000 -5000 11000]);
% title('IMM算法仿真');
% legend('真实轨迹','观测数据','滤波估计');
% 
% figure(2)
% subplot(2,2,1)
% plot(XERB)
% axis([0 500 -50 50])
% xlabel('观测次数')
% ylabel('X方向滤波误差均值')
% subplot(2,2,2)
% plot(YERB)
% axis([0 500 -50 50])
% xlabel('观测次数')
% ylabel('Y方向滤波误差均值')
% subplot(2,2,3)
% plot(XSTD)
% axis([0 500 0 150])
% xlabel('观测次数')
% ylabel('X方向滤波误差标准值')
% subplot(2,2,4)
% plot(YSTD)
% axis([0 500 0 150])
% xlabel('观测次数')
% ylabel('Y方向滤波误差标准值')