%產生紅外數據，考慮異步，即紅外數據到達時間與融合時間之間不同
% 2003,6,16   秦玉亮
function [Z0,ZIR,XTrue,totaltime,TIR,Tao0_IR,t_IR]=sourceRadar;
TIR=0.010;               %采樣周期 
totaltime=10;         %勻速運動階段時間
Tao0_IR=0.001;          %紅外第一個數據的延時為0.001ms
%初始距離r0,sita0,beta0
r0=12000;            %目標初始斜距
sita0=pi/4;          %初始方位角
beta0=10*pi/180;          %初始俯仰角

Xstart=r0*cos(beta0)*cos(sita0);       %目標起始X坐標
Ystart=r0*cos(beta0)*sin(sita0);      %目標起始Y坐標
Zstart=r0*sin(beta0);                %目標起始Z坐標

vx=-800;             %X方向速度
vy=-230;          %Y方向速度
vz=0;            %Z方向速度

%采樣率為2HZ的雷達的初始誤差
DeltaR=3000;               %觀測距離誤差標準差
DeltaSita=0.1*pi/180;    %觀測方位角角度誤差標準差    此例中方位角和俯仰角的標準差應相等
DeltaBeta=0.1*pi/180;    %觀測俯仰角角度誤差標準差   


DeltaSita_w=.2*pi/180;    %觀測方位角角度誤差標準差    此例中方位角和俯仰角的標準差應相等
DeltaBeta_w=.2*pi/180;    %觀測俯仰角角度誤差標準差   


q=0.01;                  % q是系統噪聲標準差
Q=q^2*eye(3);              %系統各方向的狀態噪聲方差

%－－－－－－－－計算目標在雷達1(采樣間隔為10HZ)中的真實軌跡,初始化XTrue1－－－－－－－－－－－－－－－－－－－－
times = totaltime/TIR+1;          %采樣點數

XTrue=zeros(6,1,times);
t_IR=zeros(1,1,times);
%真實軌跡作直線運動
for i=1:times
    XTrue(:,:,i)=[Xstart + vx*((i-1)*TIR+Tao0_IR);
                      vx;
                  Ystart + vy*((i-1)*TIR+Tao0_IR);
                      vy;
                  Zstart + vz*((i-1)*TIR+Tao0_IR);
                      vz;];                 
   t_IR(1,1,i)=(i-1)*TIR+Tao0_IR;
end;


%－－－－－－－－極坐標下的模擬觀測值，觀測噪聲距離方向上標準差DeltaR米，角度方位差為DeltaSita,DeltaBeta－－－－－－－－－－
ZIR=zeros(4,1,times);
for i=1:times

    
    azimuth=atan(XTrue(1,1,i)/XTrue(3,1,i))+randn(1)*DeltaSita;

     
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %觀測俯仰角度
    pitching=atan(XTrue(5,1,i)/(XTrue(1,1,i)^2+XTrue(3,1,i)^2)^0.5)+randn(1)*DeltaBeta;
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    %觀測方位角速度
    azimuth_w=(XTrue(3,1,i)*XTrue(2,1,i)-XTrue(1,1,i)*XTrue(4,1,i))/(XTrue(1,1,i)^2+XTrue(3,1,i)^2)  + randn(1)*DeltaSita_w;
    
    %觀測俯仰角角速度
    R_Rs=(XTrue(1,1,i)^2+XTrue(3,1,i)^2)^0.5;
    R_R=(XTrue(1,1,i)^2+XTrue(3,1,i)^2+XTrue(5,1,i)^2)^0.5;
    R_V=( XTrue(1,1,i)*XTrue(2,1,i)+XTrue(3,1,i)*XTrue(4,1,i)+XTrue(5,1,i)*XTrue(6,1,i) )/R_R;
    
    pitching_w=( XTrue(6,1,i)*R_R-XTrue(5,1,i)*R_V)/(R_R*R_Rs)  + randn(1)*DeltaBeta_w;
    ZIR(:,:,i)=[ azimuth;
              pitching;
              azimuth_w;
              pitching_w];
          
          

    
end;
%－－－－－－－－極坐標下的模擬觀測值理想狀態下的值－－－－－－－－－－
Z0=zeros(4,1,times);
for i=1:times

        
    azimuth=atan(XTrue(1,1,i)/XTrue(3,1,i));

   
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %觀測俯仰角度
    pitching=atan(XTrue(5,1,i)/(XTrue(1,1,i)^2+XTrue(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=(XTrue(3,1,i)*XTrue(2,1,i)-XTrue(1,1,i)*XTrue(4,1,i))/(XTrue(1,1,i)^2+XTrue(3,1,i)^2)  ;
    
    %觀測俯仰角角速度
    R_Rs=(XTrue(1,1,i)^2+XTrue(3,1,i)^2)^0.5;
    R_R=(XTrue(1,1,i)^2+XTrue(3,1,i)^2+XTrue(5,1,i)^2)^0.5;
    R_V=( XTrue(1,1,i)*XTrue(2,1,i)+XTrue(3,1,i)*XTrue(4,1,i)+XTrue(5,1,i)*XTrue(6,1,i) )/R_R;
    
    pitching_w=( XTrue(6,1,i)*R_R-XTrue(5,1,i)*R_V )/(R_R*R_Rs) ;
    
    Z0(:,:,i)=[azimuth;
              pitching;
              azimuth_w;
              pitching_w];

    
end;