% %  融合
% %  同步后數據的融合
% %  時間  2003 6 18
% %  秦玉亮
% % %-----------------------------------------------------------------------------------------------
function y=fusion_asyn;
clear;
clc;
[ZTrue]=source;


Tf=0.020;%融合周期
TimesOut=fix(10/Tf);
 %-------------誤差計算-----------------------------------
 ErrSquareSita=zeros(TimesOut,1)    %方位角誤差平方和變量
 ErrSquareSita_w=zeros(TimesOut,1);    %方位角速度誤差平方和變量
 ErrSquareBeta=zeros(TimesOut,1)    %俯仰角誤差平方和變量
 ErrSquareBeta_w=zeros(TimesOut,1);    %俯仰角速度誤差平方和變量
 
  ErrSquareSita1=zeros(TimesOut,1)    %方位角誤差平方和變量
 ErrSquareSita_w1=zeros(TimesOut,1);    %方位角速度誤差平方和變量
 ErrSquareBeta1=zeros(TimesOut,1)    %俯仰角誤差平方和變量
 ErrSquareBeta_w1=zeros(TimesOut,1);    %俯仰角速度誤差平方和變量

  ErrSquareSita2=zeros(TimesOut,1)    %方位角誤差平方和變量
 ErrSquareSita_w2=zeros(TimesOut,1);    %方位角速度誤差平方和變量
 ErrSquareBeta2=zeros(TimesOut,1)    %俯仰角誤差平方和變量
 ErrSquareBeta_w2=zeros(TimesOut,1);    %俯仰角速度誤差平方和變量
 
 mont=10;
 for m=1:mont
 [Xasyn_Radar,Zasyn_Radar,Pasyn_Radar]=Asynchronism_Radar;
[Xasyn_IR,Zasyn_IR,Pasyn_IR]=Asynchronism_IR;
Xfusion=zeros(6,1,TimesOut);
for i=1:TimesOut
    for j=1:6
       Xfusion(j,1,i)=(Pasyn_IR(j,j,i)*Xasyn_Radar(j,1,i)+Pasyn_Radar(j,j,i)*Xasyn_IR(j,1,i))/(Pasyn_Radar(j,j,i)+Pasyn_IR(j,j,i));
    end
end

Zfusion=zeros(4,1,TimesOut);

%-------------轉換為極坐標下值---------------------------------------------------------------
 for i=1:TimesOut
    %模型觀測距離
   
%     range=(X2(1,1,i)^2+X2(3,1,i)^2+X2(5,1,i)^2)^0.5;

      
    %模擬觀測方位角度 
    azimuth=atan(Xfusion(1,1,i)/Xfusion(3,1,i));      %%%%%   這里采用的是孫忠康的雷達數據處理中323頁的公式  注意其方位角的公式與一般不同!!!!!!!      

%      
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %觀測俯仰角度
    pitching=atan(Xfusion(5,1,i)/(Xfusion(1,1,i)^2+Xfusion(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=(Xfusion(3,1,i)*Xfusion(2,1,i)-Xfusion(1,1,i)*Xfusion(4,1,i))/(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2)  ;
    
    %觀測俯仰角角速度
    R_Rs=(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2)^0.5;
    R_R=(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2+Xfusion(5,1,i)^2)^0.5;
    R_V=(Xfusion(1,1,i)*Xfusion(2,1,i)+Xfusion(3,1,i)*Xfusion(4,1,i)+Xfusion(5,1,i)*Xfusion(6,1,i) )/R_R;
    
   pitching_w=( Xfusion(6,1,i)*R_R-Xfusion(5,1,i)*R_V)/(R_R*R_Rs) ;
    
    Zfusion(:,:,i)=[%range;
              azimuth;
              pitching;
              azimuth_w;
              pitching_w];
  end; 
 %-------------轉換為極坐標系下值結束-------------


 for i=1:TimesOut
    ErrSquareSita(i)=(ZTrue(1,1,i)-Zfusion(1,1,i)).^2+ErrSquareSita(i);
    ErrSquareBeta(i)=(ZTrue(2,1,i)-Zfusion(2,1,i)).^2+ErrSquareBeta(i);
    ErrSquareSita_w(i)=(ZTrue(3,1,i)-Zfusion(3,1,i)).^2+ErrSquareSita_w(i);
    ErrSquareBeta_w(i)=(ZTrue(4,1,i)-Zfusion(4,1,i)).^2+ErrSquareBeta_w(i);

    ErrSquareSita1(i)=(ZTrue(1,1,i)-Zasyn_Radar(1,1,i)).^2+ErrSquareSita1(i);
    ErrSquareBeta1(i)=(ZTrue(2,1,i)-Zasyn_Radar(2,1,i)).^2+ErrSquareBeta1(i);
    ErrSquareSita_w1(i)=(ZTrue(3,1,i)-Zasyn_Radar(3,1,i)).^2+ErrSquareSita_w1(i);
    ErrSquareBeta_w1(i)=(ZTrue(4,1,i)-Zasyn_Radar(4,1,i)).^2+ErrSquareBeta_w1(i);

    ErrSquareSita2(i)=(ZTrue(1,1,i)-Zasyn_IR(1,1,i)).^2+ErrSquareSita2(i);
    ErrSquareBeta2(i)=(ZTrue(2,1,i)-Zasyn_IR(2,1,i)).^2+ErrSquareBeta2(i);
    ErrSquareSita_w2(i)=(ZTrue(3,1,i)-Zasyn_IR(3,1,i)).^2+ErrSquareSita_w2(i);
    ErrSquareBeta_w2(i)=(ZTrue(4,1,i)-Zasyn_IR(4,1,i)).^2+ErrSquareBeta_w2(i);

end
end
% for i=2:TimesOut
%     A(i)=sqrt(ErrSquareSita(i)/mont);
%     B(i)=sqrt(ErrSquareBeta(i)/mont);
%     C(i)=sqrt(ErrSquareSita_w(i)/mont);
%     D(i)=sqrt(ErrSquareBeta_w(i)/mont);
% 
%     A1(i)=sqrt(ErrSquareSita1(i)/mont);
%     B1(i)=sqrt(ErrSquareBeta1(i)/mont);
%     C1(i)=sqrt(ErrSquareSita_w1(i)/mont);
%     D1(i)=sqrt(ErrSquareBeta_w1(i)/mont);
%     
%     A2(i)=sqrt(ErrSquareSita2(i)/mont);
%     B2(i)=sqrt(ErrSquareBeta2(i)/mont);
%     C2(i)=sqrt(ErrSquareSita_w2(i)/mont);
%     D2(i)=sqrt(ErrSquareBeta_w2(i)/mont);
% 
% end
for i=2:TimesOut
    A(i-1)=sqrt(ErrSquareSita(i)/mont);
    B(i-1)=sqrt(ErrSquareBeta(i)/mont);
    C(i-1)=sqrt(ErrSquareSita_w(i)/mont);
    D(i-1)=sqrt(ErrSquareBeta_w(i)/mont);

    A1(i-1)=sqrt(ErrSquareSita1(i)/mont);
    B1(i-1)=sqrt(ErrSquareBeta1(i)/mont);
    C1(i-1)=sqrt(ErrSquareSita_w1(i)/mont);
    D1(i-1)=sqrt(ErrSquareBeta_w1(i)/mont);
    
    A2(i-1)=sqrt(ErrSquareSita2(i)/mont);
    B2(i-1)=sqrt(ErrSquareBeta2(i)/mont);
    C2(i-1)=sqrt(ErrSquareSita_w2(i)/mont);
    D2(i-1)=sqrt(ErrSquareBeta_w2(i)/mont);

end
  %-------------誤差計算結束------------------------------

p=2:TimesOut;

plot(p,A,'r',p,A1,p,A2);
title('方位角誤差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合結果','雷達濾波值','紅外濾波值');
figure;
plot(p,B,'r',p,B1,p,B2);
title('俯仰角誤差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合結果','雷達濾波值','紅外濾波值');
figure;
plot(p,C,'r',p,C1,p,C2);
title('方位角角速率誤差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合結果','雷達濾波值','紅外濾波值');
figure;
plot(p,D,'r',p,D1,p,D2);
title('俯仰角角速率誤差均方根');
xlabel('t');
ylabel('error /rad');
legend('融合結果','雷達濾波值','紅外濾波值');



% for i=1:TimesOut
%     A(i)=Zasyn_Radar(1,1,i);
%     B(i)=ZTrue(1,1,i);
%     C(i)=Zasyn_IR(1,1,i);
% end
% plot(p,A,p,B,'r',p,C);