% %  子程序
% %  同步處理
% %  時間  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);