currentmodel.m

this code is an example for kalman filter

function XE=currentmodel(T,Flag,N,zx,zy,d)
% Kalman_filter             采用Kalman滤波方法，从观测数值中得到航迹的估计
% T                        采样时间，即雷达工作周期
% N                       采样点数。
% zx,zy                    观测数据
% d                         噪声的标准差值
% Flag                      判断计算x轴或y轴数据,'0'--x,'1'--y
if nargin>6
    error('输入的变量过多，请检查');
end





randn('state',sum(100*clock)); % 设置随机数发生器
Xest=zeros(3,1); % 用前k-1时刻的输出值估计k时刻的预测值
Xfli=zeros(3,1); % k时刻Kalman滤波器的输出值
Xes=zeros(3,1); % 预测输出误差
Xef=zeros(3,1); % 滤波后输出的误差
Pxe=zeros(3,1); % 预测输出误差均方差矩阵 
Px=zeros(3,1);  % 滤波输出误差均方差矩阵
XE=zeros(1,N); % 得到最终的滤波输出值，仅仅考虑距离分量
%###################################################

 %###################################################   
 
% $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
% 定义系统的状态方程及初始化
a=1/20;am2=-100;am1=100;Pv1=4;%am2为最小加速度,am1为最大加速度.
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];
gk1=(-T+a*T*T/2+(1-exp(-a*T))/2)/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;
%$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
%滤波递推,XY轴各自独立进行滤波.
switch Flag
    case 0
           Xfli=[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
        
            
              Xest=Fk1*Xfli; 
               % 更新该时刻的预测值

            % Xes=Phi*Xef+Gamma*W(k-1); % 预测输出误差
             Pxe=Fk*Px*Fk'+2*a*Pv1*qk; % 预测误差的协方差阵
            K=Pxe*Hk'*inv(Hk*Pxe*Hk'+R*randn(1)); % Kalman滤波增益
     if(Xfli(3,1)<0)
                 Pv1=((4-pi)*(am2+Xest(3,1))^2)/pi;
             
               else
                Pv1=((4-pi)*(am1-Xest(3,1))^2)/pi;
               
               end
           Xfli=Xest+K*(zx(k)-Hk*Xest); 
          %  Xef=(eye(3)-K*C)*Xes-K*vx(k);
          Px=(eye(3)-K*Hk)*Pxe;
         
        
           XE(k)=Xfli(1,1);

          end
          XE(1)=zx(1);XE(2)=zx(2);XE(3)=zx(3);
     case 1
        Pv1=4;
         Xfli=[zy(3) (zy(2)-zy(1))/T (zy(3)-2*zy(2)+zy(1))/T^2]'; %利用前两个观测值来对初始条件进行估计
         Px=[Pv1,Pv1/T,Pv1/(T^2);Pv1/T,2*Pv1/(T^2),3*Pv1/(T^3);2*Pv1/(T^2),3*Pv1/(T^3),6*Pv1/(T^4)];

         for k=4:N
        
            
            Xest=Fk1*Xfli; 
                  
       % 更新该时刻的预测值
              
       % Xes=Phi*Xef+Gamma*W(k-1); % 预测输出误差
        Pxe=Fk*Px*Fk'+2*a*Pv1*qk; % 预测误差的协方差阵
        K=Pxe*Hk'*inv(Hk*Pxe*Hk'+R*randn(1)); % Kalman滤波增益
    if(Xfli(3,1)<0)
            Pv1=((4-pi)*(am2+Xest(3,1))^2)/pi;
            else
           Pv1=((4-pi)*(am1-Xest(3,1))^2)/pi;
            end
        Xfli=Xest+K*(zy(k)-Hk*Xest); 
      %  Xef=(eye(3)-K*C)*Xes-K*vx(k);
        Px=(eye(3)-K*Hk)*Pxe;
        
        
        XE(k)=Xfli(1,1);
   end
        XE(1)=zy(1);XE(2)=zy(2);XE(3)=zy(3);
      otherwise
        error('False iuput nargin');
end
%&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&