asynchronism_radar.m

扩展卡尔曼滤波 信息同步 数据融合算法实现

% %  子程序
% %  同步处理
% %  时间  2003 6 18
% %  秦玉亮
% % %-----------------------------------------------------------------------------------------------
function [Xasyn,Zasyn,Pasyn]=Asynchronism_Radar;
clc;
%丛SourceRadar.m中调用所有用到的参数
[ZTrueRadar,ZRadar,XTrueRadar,totaltimeRadar,T0Radar,Tao0_Radar,t_Radar]=sourceRadar;
[X2,Zfilt,P2,ZRadar]=EKF_Radar;
Tf=0.020;%融合周期
times=totaltimeRadar/T0Radar+1;
TimesOut=fix(totaltimeRadar/Tf);
T=T0Radar;
Xasyn=zeros(6,1,TimesOut);
Zasyn=zeros(4,1,TimesOut);
Pasyn=zeros(6,6,TimesOut);

nCount=0;
j=0;

q=0.01;                     % q是系统噪声标准差
Q=q^2*eye(3);              % 系统各方向的状态噪声方差

for i=1:times  
%-------------同步-------------
     Ks=fix((Tf-Tao0_Radar)/T0Radar)+1;
     nCount=nCount+1;
     if nCount>=Ks
        nCount=0;
        j=j+1;
        delt_t=Tf-(Ks-1)*T-Tao0_Radar;%计算融合同步时间差
        phi_t=[1 delt_t 0 0 0 0;
               0 1 0 0 0 0;
               0 0 1 delt_t 0 0;
               0 0 0 1 0 0;
               0 0 0 0 1 delt_t;
               0 0 0 0 0 1];
       Xasyn(:,:,j)=phi_t*X2(:,:,i);%预测(次优滤波)
      % Xasyn(:,:,j)=phi_t*XTrueRadar(:,:,i);%预测(次优滤波)
       G=[delt_t^2/2  0   0;
          delt_t    0   0;
           0  delt_t^2/2 0;
           0    delt_t   0;
           0    0  delt_t^2/2;
           0    0   delt_t];
         Q1=G*Q*G';
         Pasyn(:,:,j)=phi_t*P2(:,:,i)*phi_t'+Q1';%预测（次优滤波）误差阵
         Tao0_Radar=Ks*T+Tao0_Radar-Tf;%计算下一个融合周期内的时标差
     end
 end
 %-------------同步结束-------------
 %-------------转换为极坐标下值---------------------------------------------------------------
 for i=1:TimesOut
    %模型观测距离
   
%     range=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;

      
    %模拟观测方位角度 
    azimuth=atan(Xasyn(1,1,i)/Xasyn(3,1,i));      %%%%%   这里采用的是孙忠康的雷达数据处理中323页的公式  注意其方位角的公式与一般不同!!!!!!!      

%      
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %观测俯仰角度
    pitching=atan(Xasyn(5,1,i)/(Xasyn(1,1,i)^2+Xasyn(3,1,i)^2)^0.5);
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    %观测方位角速度
    %观测方位角速度
    azimuth_w=(Xasyn(3,1,i)*Xasyn(2,1,i)-Xasyn(1,1,i)*Xasyn(4,1,i))/(Xasyn(1,1,i)^2+Xasyn(3,1,i)^2)  ;
    
    %观测俯仰角角速度
    R_Rs=(Xasyn(1,1,i)^2+Xasyn(3,1,i)^2)^0.5;
    R_R=(Xasyn(1,1,i)^2+Xasyn(3,1,i)^2+Xasyn(5,1,i)^2)^0.5;
    R_V=( Xasyn(1,1,i)*Xasyn(2,1,i)+Xasyn(3,1,i)*Xasyn(4,1,i)+Xasyn(5,1,i)*Xasyn(6,1,i) )/R_R;
    
    pitching_w=( Xasyn(6,1,i)*R_R-Xasyn(5,1,i)*R_V )/(R_R*R_Rs) ;
    
    
    Zasyn(:,:,i)=[azimuth;
              pitching;
              azimuth_w;
              pitching_w];
  end; 
 %-------------转换为极坐标系下值结束-------------

% p=1:TimesOut;
% for i=1:TimesOut
%     A(i)=Zasyn(1,1,i);
% end
% plot(p,A);