target_track.m

自己写的卡尔曼滤波源程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                         致谢!
%   感谢张锴同学对此程序中模型误差给出的极具价值的启发
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Author: 邢世其
%Date  : 2007 6.8
%Fisrst Edition : fail
%Revised Edition : 2007 6.9 Ok
clear;clc;close all;
%----------------------------
%        -参数设置
%---------------------------
T=2;%雷达扫描间隔
a=20;%向心加速度1a2=5;%向心加速度2
X0=-20000;%雷达初始x位置
Vcx=300;%雷达初始x方向速度
sigma=100; %观测噪声均方差.假定噪声为高斯分布,且白色
%----------------------------
t_const=-X0/Vcx/T;%匀速运动时间(归一化后)
R=Vcx^2/a;%圆周运动半径
t_round=pi*R/Vcx/T%圆周运动时间(归一化后)
nSample=floor((-2*X0+pi*R)/Vcx/T);%采样点数
nMonCarl=100;%蒙特卡罗仿真次数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------------------First step: 计算真实轨迹
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:nSample
  if(k<t_const) %匀速直线运动
      x(k)=X0+Vcx*k*T;
      y(k)=R;
  elseif(k<t_const+t_round)
      theta=(k-t_const)*T*Vcx/R;%目标转过的角度
      x(k)=sin(theta)*R;
      y(k)=cos(theta)*R;
  elseif(k>t_const+t_round)
      x(k)=-(k-t_const-t_round)*T*Vcx;
      y(k)=-R;
  end
end
%  plot(x,y,'b')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                 卡尔曼滤波模型
%                X(k)=A*X(k-1)+w(k)
%                E[w(k)*w(k)']=Q;
%                Z(k)=C*X(k)+V(k);
%                E(V(k)*V(k)']=R;
%                状态变量X(k)=[x,y,vx,vy,ax,ay]'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=[1,0,T,0,1/2*T^2,0;%转移矩阵
   0,1,0,T,0,1/2*T^2;
   0,0,1,0,T,0;
   0,0,0,1,0,T;
   0 0 0 0 1 0 ;
   0 0 0 0 0 1 ];
Q=diag([40 40 40 40 20 20]);%此处用以修正模型误差,感谢张锴同学建言

C=[1 0 0 0 0 0 ;
   0 1 0 0 0 0];

R=[sigma^2 0;
    0 sigma^2];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------卡尔曼滤波
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for ncount=1:nMonCarl%主循环
    
    %------------------模拟观测量
    randn('state',sum(100*clock));
    Zx=x+sigma*randn(1,nSample);
    Zy=y+sigma*randn(1,nSample);
    %---------------------------
    %  hold on 
    %  plot(Zx,Zy,'r')
    
    Z=[Zx;Zy];
    x_k_k=[Zx(1),Zy(1),Vcx,0,0,0]';%滤波初值
    P_k_k=diag([sigma^2,sigma^2,40,40,20,20]);%滤波初值
    
    for k=2:nSample
        x_k_k1=A*x_k_k;%一步预测
        P_k_k1=A*P_k_k*A'+Q;%计算预测误差协方差;
        
        K=P_k_k1*C'*inv(C*P_k_k1*C'+R);%计算卡尔曼增益
        
        x_k_k=x_k_k1+K*(Z(:,k)-C*x_k_k1);%滤波
        
        P_k_k=(eye(6)-K*C)*P_k_k1;%计算滤波误差协方差
        result(ncount,:,k)=x_k_k;  
    end
    result(ncount,1:2,1:2)=Z(:,1:2);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%结果分析，可视化
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
result=result(:,1:2,:);
merror=reshape(sum(result,1),2,nSample);
merror=merror/nMonCarl-[x;y];%计算平均误差
for i=1:nMonCarl
   temp(i,:,:)=reshape(result(i,:,:),2,nSample)-[x;y];
   temp(i,:,:)=temp(i,:,:).^2;
end
msigma_error=reshape(sum(temp,1)/(nMonCarl-1),2,nSample);
msigma_error=sqrt(msigma_error-merror.^2);%计算平均误差标准差
%---------------------------可视化
subplot(211)
plot(1:nSample,merror)
axis([0,nSample,min(min(merror)),max(max(merror))])
legend('x坐标滤波平均误差','y坐标滤波平均误差')
title('平均滤波误差')
subplot(212)
plot(1:nSample,msigma_error)
axis([0,nSample,min(min(msigma_error)),max(max(msigma_error))])
legend('x坐标滤波平均误差标准差','y坐标滤波平均误差标准差')
title('滤波平均误差标准差')
%--------------------------------------------------------------------
%% hold on 
% plot(x,y,'r*')
figure
plot(x,y,'b');
hold on
plot(Zx,Zy,'r');
plot(reshape(result(1,1,:),nSample,1),reshape(result(1,2,:),nSample,1),'g')
axis('equal')