currentstatisticalmodel.m

当前统计模型机动目标跟踪算法。本程序对做圆周运动的机动目标的X方向进行了跟踪仿真。

%本程序对做圆周运动的机动目标的X方向进行了跟踪仿真
clear all;
close all;
T=0.5;                %采样时间
N=200;                      %采样点数
t=0:T:(N-1)*T;
nt=2*randn(1,N);
yx=100*cos(t*0.01*2*pi);               %X方向原始轨迹
yy=100*sin(t*0.01*2*pi);               %Y方向原始轨迹
subplot(2,2,1)
plot(yx,yy)
xlabel('X');
ylabel('Y');
axis([-150 150 -150 150])
title('原始运动轨迹');
zx=(100+nt).*cos(t*0.01*2*pi);              %观测数据
zy=(100+nt).*sin(t*0.01*2*pi);
subplot(2,2,2)
plot(zx,zy)
axis([-150 150 -150 150])
xlabel('X');
ylabel('Y');
title('加噪声实际测量轨迹');
d=4;                        % 噪声的标准差值
%------------------------------------------------------------------
X_estimate=zeros(3,1);            % 用前k-1时刻的输出值估计k时刻的预测值，状态的一步预测
X_fliter=zeros(3,1);              % k时刻Kalman滤波器的输出值，状态的更新方程
Pxe=zeros(3,1);                   % 预测输出误差协方差矩阵，协方差的一步预测
Px=zeros(3,1);                    % 协方差方程
XE=zeros(1,N);                    % 得到最终的滤波输出值，距离分量
XV=zeros(1,N);                    % 得到最终的滤波输出值，速度分量
XA=zeros(1,N);                    % 得到最终的滤波输出值，加速度分量   
Y_estimate=zeros(3,1);            
Y_fliter=zeros(3,1);              
Pye=zeros(3,1);                   
Py=zeros(3,1);                   
YE=zeros(1,N);                    
YV=zeros(1,N);                  
YA=zeros(1,N);                    
%------------------------------------------------------------------定义系统的状态方程及初始化
a=1/20;   %机动时间常数的倒数，机动频率
am1=100;   %am1为最大加速度
am2=-100;  %am2为最小加速度
Pv1=4;    %Pv1为机动目标加速度方差

P1=(-1+a*T+exp(-a*T))/(a*a);
P2=(1-exp(-a*T))/a;
P3=exp(-a*T);
Fk=[1 T P1;
    0 1 P2;
    0 0 P3];                 %状态转移矩阵
Fk1=[1 T T^2/2;
     0 1 T;
     0 0 1];                 %a=0时,状态转移矩阵
 
      
gk1=(-T+a*T*T/2+(1-exp(-a*T))/a)/a;
gk2=T-(1-exp(-a*T))/a;
gk3=1-exp(-a*T);
Gk=[gk1;gk2;gk3];            %输入控制矩阵

q11=(1-exp(-2*a*T)+2*a*T+2*a^3*T^3/3-2*a^2*T^2-4*a*T*exp(-a*T))/(2*a^5);
q12=(exp(-2*a*T)+1-2*exp(-a*T)+2*a*T*exp(-a*T)-2*a*T+a^2*T^2)/(2*a^4);
q13=(1-exp(-2*a*T)-2*a*T*exp(-a*T))/(2*a^3);
q22=(4*exp(-a*T)-3-exp(-2*a*T)+2*a*T)/(2*a^3);
q23=(exp(-2*a*T)+1-2*exp(-a*T))/(2*a^2);
q33=(1-exp(-2*a*T))/(2*a);
qk=[q11 q12 q13;
    q12 q22 q23;
    q13 q23 q33];  
Qk=2*a*Pv1*qk;                %过程噪声协方差，Pv1为机动目标加速度方差,a为自相关时间常数.机动频率
Hk=[1 0 0];                   %量测矩阵
R=d*d;                        %量测噪声均方差

%-----------------------------滤波递推  
%------------------------X方向
              Pv1=4;
              X_fliter=[zx(3) (zx(2)-zx(1))/T (zx(3)-2*zx(2)+zx(1))/(T^2)]'; %利用前两个观测值来对初始条件进行估计
              Px=[Pv1,       Pv1/T,       Pv1/(T^2);
                  Pv1/T,    2*Pv1/(T^2),3*Pv1/(T^3);        %协方差初值
                  Pv1/(T^2),3*Pv1/(T^3),6*Pv1/(T^4)];
             for k=4:N
                X_estimate=Fk*X_fliter+Gk*X_estimate(3,1);  %状态估计,X_estimate(3,1)即加速度
                % 更新该时刻的预测值
                Pxe=Fk*Px*Fk'+Qk;                     % 预测误差的协方差阵
                K=Pxe*Hk'*inv(Hk*Pxe*Hk'+R);          % 滤波增益K
              if(X_estimate(3,1)<0)
                Pv1=((4-pi)*(am2+X_estimate(3,1))^2)/pi;    %利用Pv1和X_estimate之间的关系
              else
                Pv1=((4-pi)*(am1-X_estimate(3,1))^2)/pi;    %利用Pv1和X_estimate之间的关系
              end    
                X_fliter=X_estimate+K*(zx(k)-Hk*X_estimate); %更新观测值

                Px=(eye(3)-K*Hk)*Pxe;                    %eye单位矩阵，Px为协方差更新方程
                XE(k)=X_fliter(1,1);                     %得到最终的滤波输出值，距离分量
                XV(k)=X_fliter(2,1);                     %得到最终的滤波输出值，速度分量
                XA(k)=X_fliter(3,1);                     %得到最终的滤波输出值，加速度分量  
             end
             XE(1)=zx(1);XE(2)=zx(2);XE(3)=zx(3);
             XV(1)=(zx(2)-zx(1))/T; XV(2)=XV(1); XV(3)=XV(1);
             XA(1)=(zx(3)-2*zx(2)+zx(1))/(T^2);XA(2)=XA(1);XA(3)=XA(1);
%------------------------Y方向
              Pv1=4;
              Y_fliter=[zy(3) (zy(2)-zy(1))/T (zy(3)-2*zy(2)+zy(1))/(T^2)]'; %利用前两个观测值来对初始条件进行估计
              Py=[Pv1,       Pv1/T,       Pv1/(T^2);
                  Pv1/T,    2*Pv1/(T^2),3*Pv1/(T^3);        %协方差初值
                  Pv1/(T^2),3*Pv1/(T^3),6*Pv1/(T^4)];
             for k=4:N
                Y_estimate=Fk*Y_fliter+Gk*Y_estimate(3,1);  %状态估计,Y_estimate(3,1)即加速度
                % 更新该时刻的预测值
                Pye=Fk*Py*Fk'+Qk;                     % 预测误差的协方差阵
                K=Pye*Hk'*inv(Hk*Pye*Hk'+R);          % 滤波增益K
              if(Y_estimate(3,1)<0)
                Pv1=((4-pi)*(am2+Y_estimate(3,1))^2)/pi;    %利用Pv1和Y_estimate之间的关系
              else
                Pv1=((4-pi)*(am1-Y_estimate(3,1))^2)/pi;    %利用Pv1和Y_estimate之间的关系
              end    
                Y_fliter=Y_estimate+K*(zy(k)-Hk*Y_estimate); %更新观测值

                Py=(eye(3)-K*Hk)*Pye;                    %eye单位矩阵，Py为协方差更新方程
                YE(k)=Y_fliter(1,1);                     %得到最终的滤波输出值，距离分量
                YV(k)=Y_fliter(2,1);                     %得到最终的滤波输出值，速度分量
                YA(k)=Y_fliter(3,1);                     %得到最终的滤波输出值，加速度分量  
             end
             YE(1)=zy(1);YE(2)=zy(2);YE(3)=zy(3);
             YV(1)=(zy(2)-zy(1))/T; YV(2)=YV(1); YV(3)=YV(1);
             YA(1)=(zy(3)-2*zy(2)+zy(1))/(T^2);YA(2)=YA(1);YA(3)=YA(1);
%--------------------------------
             subplot(2,2,3)
             plot(XE,YE,zx,zy,':')                               %此处只画出位置分量
             axis([-150 150 -150 150])
             xlabel('X');
             ylabel('Y');
             title('滤波预测运动轨迹');

             subplot(2,2,4)
             plot(zx-XE)  
             axis([0 N -5 5])
             xlabel('采样次数');
             ylabel('位置');
             title('X方向滤波值与加噪声量测值之差');