www.pudn.com > data_fusion.rar > 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);