imm_ukf.m

无迹卡尔曼滤波算法,用无迹卡尔曼滤波算法实现了跟踪中的误差估计

clc;
clear;
T=1;
mont=1;
d=100;
N=90;

% [x,y,zx,zy,XXE,YYE,XERB,YERB,XSTD,YSTD]=LS(T,M,d);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
totalTime=N;  
%真实航迹产生

%[x,y]=realTrack(T,totalTime);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a=100;
a2=50;
v=300;
theta=pi/2;
theta2=pi*3/4;
t1=100;



w=0.1;


x=zeros(totalTime,1);
y=zeros(totalTime,1);

X=[];%观测数据
X_ideal=[];%理想数据
var_rx=400;
var_ry=400;


%for i=1:totalTime
 %  [rx,ry]=track2(i*T,t1,theta,theta2,a,a2,v);
   % [rx,ry]=track(i*T,t1,theta,a,v);
   % X_ideal=[X_ideal,[rx;ry]];
   % rx=rx+var_rx*randn(1,1);
   % ry=ry+var_ry*rand(1,1);
   % X=[X,[rx;ry]];
    %end

%x=X_ideal(1,:)';
%y=X_ideal(2,:)';
[x0,y0,z0]=track3;
x=x0';
y=y0';
z=z0';




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%随机初始化 
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);
        %zr1(i)=sqrt(x(i).^2+y(i).^2)+100*randn(1);
        zr(i)=sqrt(x(i).^2+y(i).^2+z(i).^2)+100*randn(1);
        ztheta(i)=atan(y(i)/x(i))+0.01*randn(1);
        zphi(i)=atan(z(i)/sqrt(x(i).^2+y(i).^2))+0.001*randn(1);
      
        zx(i)=zr(i)*cos(zphi(i))*cos(ztheta(i));
        zy(i)=zr(i)*cos(zphi(i))*sin(ztheta(i));
        zz(i)=zr(i)*sin(zphi(i));
    end
   
    
   % for i=1:N/5
   %     zx2(i)=zx(5*(i-1)+1);
   %     zy2(i)=zy(5*(i-1)+1);
   %     zr2(i)=zr(5*(i-1)+1);
   %     ztheta2(i)=ztheta(5*(i-1)+1);
   % end
    
   % zxf=zx
   % zyf=zy
   % Tf=T
   % Df=D
    
    %滤波(IMM)
%    [XXE,YYE,ZZE]=immukf3(zr,ztheta,zphi,T,D,w);
  %[XXE,YYE,ZZE]=immcmkf(zr,ztheta,zphi,T,D,w);
   [XXE,YYE,ZZE]=immekf(zr,ztheta,zphi,T,D,w);
  %[XXE,YYE,ZZE]=immkf(zx,zy,zz,T,D*100,w);
 
  [XXEkf,YYEkf,ZZEkf]=immcmkf(zr,ztheta,zphi,T,D,w);
 %[XXEkf,YYEkf,ZZEkf]=immukf3(zr,ztheta,zphi,T,D,w);
 %[XXEkf,YYEkf,ZZEkf]=immekf(zr,ztheta,zphi,T,D,w);
 %[XXEkf,YYEkf,ZZEkf]=immkf(zx,zy,zz,T,D*100,w);
    
    XXE(1)=zx(1);XXE(2)=zx(2);
    YYE(1)=zy(1);YYE(2)=zy(2);
    ZZE(1)=zz(1);ZZE(2)=zz(2);
    
    XXEkf(1)=zx(1);XXEkf(2)=zx(2);
    YYEkf(1)=zy(1);YYEkf(2)=zy(2);
    ZZEkf(1)=zz(1);ZZEkf(2)=zz(2);
    
    
    
    xf=x;
    yf=y;
    
    %误差矩阵
    
    XER(1:N,n)=x(1:1:N)-(XXE(1:N))';
    YER(1:N,n)=y(1:1:N)-(YYE(1:N))';
    ZER(1:N,n)=z(1:1:N)-(ZZE(1:N))';
    
    XERkf(1:N,n)=x(1:1:N)-(XXEkf(1:N))';
    YERkf(1:N,n)=y(1:1:N)-(YYEkf(1:N))';
    ZERkf(1:N,n)=z(1:1:N)-(ZZEkf(1:N))';
    
    XMER(1:N,n)=x(1:1:N)-zx(1:N)';
    YMER(1:N,n)=y(1:1:N)-zy(1:N)';
    ZMER(1:N,n)=z(1:1:N)-zz(1:N)';
    
    
   % XVE(1:N,n)=XVE(1:N)';
   % YVE(1:N,n)=YVE(1:N)';
end
%end for mont

%滤波误差的均值
XERB=mean(XER,2);
YERB=mean(YER,2);
ZERB=mean(ZER,2);



XERBkf=mean(XERkf,2);
YERBkf=mean(YERkf,2);
ZERBkf=mean(ZERkf,2);

%滤波误差的标准差
for i=1:N
    XSTD(i)=sqrt(1/mont*dot(XER(i,:)',XER(i,:)'));
    YSTD(i)=sqrt(1/mont*dot(YER(i,:)',YER(i,:)'));
    ZSTD(i)=sqrt(1/mont*dot(ZER(i,:)',ZER(i,:)'));
    
    XSTDkf(i)=sqrt(1/mont*dot(XERkf(i,:)',XERkf(i,:)'));
    YSTDkf(i)=sqrt(1/mont*dot(YERkf(i,:)',YERkf(i,:)'));
    ZSTDkf(i)=sqrt(1/mont*dot(ZERkf(i,:)',ZERkf(i,:)'));
    
    XMSTD(i)=sqrt(1/mont*dot(XMER(i,:)',XMER(i,:)'));
    YMSTD(i)=sqrt(1/mont*dot(YMER(i,:)',YMER(i,:)'));
    ZMSTD(i)=sqrt(1/mont*dot(ZMER(i,:)',ZMER(i,:)'));
 end
%XSTD=std(XER,1,2);
%YSTD=std(YER,1,2);

%XSTDkf=std(XERkf,1,2);
%YSTDkf=std(YERkf,1,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i=1:N
    xaxis(i)=i;
end

figure(2)
 plot(zx,zy,'b',x,y,'g',XXE,YYE,'r',XXEkf,YYEkf,'k');

%axis([-25000 10000 -10000 2000]);
title('imm,kalman仿真');
%legend('观测数据','真实轨迹','imm估计','kalman估计');

figure(3)
subplot(2,1,1)
plot(xaxis,XERB,'r',xaxis,XERBkf,'k')
%axis([0 300 -10000 500])
xlabel('观测次数n')
title('E(X-Xls)');
subplot(2,1,2)
plot(xaxis,XSTD,'r',xaxis,XSTDkf,'k',xaxis,XMSTD,'g')
%axis([0 300 -10000 500])
xlabel('观测次数n')
title('std(X-Xls)');


figure(4)
subplot(2,1,1)
plot(xaxis,YERB,'r',xaxis,YERBkf,'k')
%axis([0 300 -5000 5000])
xlabel('观测次数n')
title('E(Y-Yls)')
subplot(2,1,2)
plot(xaxis,YSTD,'r',xaxis,YSTDkf,'k',xaxis,YMSTD,'g')
%axis([0 300 -10000 500])
xlabel('观测次数n')
title('std(Y-Yls)');

figure(5)
subplot(2,1,1)
%plot(xaxis,ZERB,'r',xaxis,ZERBkf,'k')
plot(xaxis,ZERBkf,'k',xaxis,ZERB,'r')
subplot(2,1,2)
plot(xaxis,ZSTD,'r',xaxis,ZSTDkf,'k',xaxis,ZMSTD,'g')
%plot(xaxis,ZSTDkf,'k',xaxis,ZMSTD,'g')
figure(6)
polar(ztheta,zr);