%--------------------------------------------------------------------------
%XTrueZ 目標真實軌跡
%Z0 目標觀測值
%T 采樣周期
%Q 系統誤差陣
%DeltaR 距離誤差
%DeltaSita 方位角誤差
%DeltaBeta 俯仰角誤差
%totaltime 運動時間
%montimes 蒙特卡羅仿真次數
function [XTrue,Z,Z0,T,Q,DeltaR,DeltaSita,DeltaBeta,totaltime,montimes]=MYInit;
% function [XTrue,Z, Z2,T,Q,DeltaR,DeltaSita,DeltaBeta,DeltaR2,DeltaSita2,DeltaBeta2,totaltime,montimes]=MYInit;
%設置初始值
montimes=50;      %蒙特卡羅仿真次數
 T1=0.05;                %采樣周期 2
 T=0.05;
%totaltime=2400;         %勻速運動階段時間
totaltime=10;         %勻速運動階段時間
%初始距離r0,sita0,beta0
r0=120000;            %目標初始斜距
sita0=pi/4;          %初始方位角
beta0=5*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方向速度


%采樣率為1/3HZ的雷達的初始誤差
DeltaR=3000;               %觀測距離誤差標準差
DeltaSita=1*pi/180;    %觀測方位角角度誤差標準差    此例對應紅外
DeltaBeta=1*pi/180;    %觀測俯仰角角度誤差標準差   

% DeltaSita=1*pi/180;    %觀測方位角角度誤差標準差    此例對應雷達
% DeltaBeta=1*pi/180;    %觀測俯仰角角度誤差標準差   


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




% for i=1:times
% %    xt(i)=Z(1,1,i)*cos(Z(2,1,i))*cos(Z(3,1,i))-XTrue(1,1,i);
% %    yt(i)=Z(1,1,i)*sin(Z(2,1,i))*cos(Z(3,1,i))-XTrue(3,1,i);
% %    zt(i)=Z(1,1,i)*sin(Z(3,1,i))-XTrue(5,1,i);
%    xt(i)=Z(1,1,i)*cos(Z(2,1,i))*cos(Z(3,1,i));
%    yt(i)=Z(1,1,i)*sin(Z(2,1,i))*cos(Z(3,1,i));
%    zt(i)=Z(1,1,i)*sin(Z(3,1,i));
% end;
% plot(xt,zt);    

%－－－－－－－－計算目標在雷達2(采樣間隔為40HZ)中的真實軌跡,初始化XTrue2－－－－－－－－－－－－－－－－－－－－
times2 = totaltime/T1+1;          %采樣點數

XTrue2=zeros(6,1,times2);


XTrue=zeros(6,1,times2);       %%%%%
%真實軌跡作直線運動
for ii=1:times2
    XTrue2(:,:,ii)=[Xstart + vx*(ii-1)*T1;
                      vx;
                  Ystart + vy*(ii-1)*T1;
                      vy;
                  Zstart + vz*(ii-1)*T1;
                      vz;];
                XTrue(:,:,ii)= XTrue2(:,:,ii) ;  
end;



%－－－－－－－－極坐標下的模擬觀測值，觀測噪聲距離方向上標準差DeltaR米，角度方位差為DeltaSita,DeltaBeta－－－－－－－－－－
Z=zeros(3,1,times2);
for ii=1:times2
    %模型觀測距離
    range=(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2+XTrue2(5,1,ii)^2)^0.5+randn(1)*DeltaR;
    %模擬觀測方位角度 
    azimuth=atan2(XTrue2(3,1,ii),XTrue2(1,1,ii))+randn(1)*DeltaSita;
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %觀測俯仰角度
    pitching=atan(XTrue2(5,1,ii)/(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2)^0.5)+randn(1)*DeltaBeta;
    
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z(:,:,ii)=[range;
              azimuth;
              pitching];
end;

% %－－－－－－－－計算目標在雷達1的真實軌跡,初始化XTrue－－－－－－－－－－－－－－－－－－－－
Z0=zeros(3,1,times2);
for ii=1:times2
    %模型觀測距離
    range=(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2+XTrue2(5,1,ii)^2)^0.5;
    %模擬觀測方位角度 
    azimuth=atan2(XTrue2(3,1,ii),XTrue2(1,1,ii));
    if azimuth>2*pi
        azimuth=azimuth-2*pi;
    else if azimuth < 0
            azimuth=azimuth+2*pi;
        end;
    end;
    %觀測俯仰角度
    pitching=atan(XTrue2(5,1,ii)/(XTrue2(1,1,ii)^2+XTrue2(3,1,ii)^2)^0.5);
    
    if pitching>pi/2
        pitching=pitching-pi;
    else if pitching<-pi/2
            pitching=pitching+pi;
        end;
    end;
    
    Z0(:,:,ii)=[range;
              azimuth;
              pitching];
end;