%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                    航跡發生器
%
%  輸入參數:
%  t          仿真時間
%  T          仿真步長
%  atti       橫滾、俯仰、航向（單位：度）
%  atti_rate  橫滾速率、俯仰速率、航向速率（單位：度/秒）
%  veloB      飛機運動速度——X右翼、Y機頭、Z天向（單位：米/秒）
%  acceB      飛機運動加速度——X右翼、Y機頭、Z天向（單位：米/秒/秒）
%  posi       航跡發生器初始位置經度、緯度、高度（單位：度、度、米）
%  veloN      飛機運動速度——X東向、Y北向、Z天向（單位：米/秒）
%  posiN      經度、緯度、高度（單位：度、度、米）        
%                           程序設計：吳玲  日期：2007/12/05
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%仿真時間設置%%%%%%
t=0;
T=1;
TraceData=[];
t_stop=3600.0;

%%%%%%%%%%%%%%%%航跡發生器%%%%%%%%%%%%%%%%%
atti=zeros(3,1);     %橫滾、俯仰、航向（單位：度）
atti_rate=zeros(3,1);%橫滾速率、俯仰速率、航向速率（單位：度/秒）
veloB=zeros(3,1);    %飛機運動速度——X右翼、Y機頭、Z天向（單位：米/秒）
acceB=zeros(3,1);    %飛機運動加速度——X右翼、Y機頭、Z天向（單位：米/秒/秒）
posi=zeros(3,1);     %航跡發生器初始位置經度、緯度、高度（單位：度、度、米）
posi=[118;32;300.0];  

Re=6378137.0;  %地球半徑（米） 
f=1/298.257;   %地球的橢圓率
posiN=posi;    %初始位置與航跡位置一致
long=posiN(1,1)*pi/180.0;lati=posiN(2,1)*pi/180.0;heig=posiN(3,1);
    %飛行器位置

Rm=Re*(1-2*f+3*f*sin(lati)*sin(lati));
Rn=Re*(1+f*sin(lati)*sin(lati));
    %地球曲率半徑求解
atti(1,1)=0.0;
atti(2,1)=0.0;
atti(3,1)=90.0;  %初始航向角度（單位：度）

veloB(2,1)=0.0; %飛機初始運動速度——機頭（單位：米/秒）
while t<=t_stop
    %
    if(t==0 )
      acceB(2,1)=0.0;               %初始對準時間
   elseif(t<=3600)
      acceB(2,1)=2.0;
%    elseif(t<=3600)
%       acceB(2,1)=0.0;
%    elseif(t<=105)
%       acceB(2,1)=-2.0;
%    elseif(t<=110)
%       acceB(2,1)=0.0;
%       atti_rate(3,1)=-18.0;
%    elseif(t<=115)
%       acceB(2,1)=2.0;
%       atti_rate(3,1)=0.0;
%    elseif(t<=205)
%       acceB(2,1)=0.0;
%   elseif(t<=210)
%       acceB(2,1)=-2.0;
%   elseif(t<=215)
%       acceB(2,1)=0.0;
%       atti_rate(3,1)=-18.0;
%   elseif(t<=220)
%       acceB(2,1)=2.0;
%       atti_rate(3,1)=0.0;      
%   elseif(t<=310)
%       acceB(2,1)=0.0;
%   elseif(t<=315)
%       acceB(2,1)=-2.0;
%   elseif(t<=320)
%       acceB(2,1)=0.0;
%       atti_rate(3,1)=-18.0;
%   elseif(t<=325)
%       acceB(2,1)=2.0;
%       atti_rate(3,1)=0.0;
%   elseif(t<=415)
%       acceB(2,1)=0.0;
%   elseif(t<=420)
%       acceB(2,1)=-2.0;
%   elseif(t<=425)
%       acceB(2,1)=0.0;
%       atti_rate(3,1)=-18.0;
%   elseif(t<=450)
%       atti_rate(3,1)=0.0;
   end
   %
   t=t+T;
   t
   
   veloB(2,1)=veloB(2,1)+acceB(2,1)*T;    

   atti(1,1)=atti(1,1)+atti_rate(1,1)*T;
   atti(2,1)=atti(2,1)+atti_rate(2,1)*T;
   atti(3,1)=atti(3,1)+atti_rate(3,1)*T;
  
   roll=atti(1,1)*pi/180.0;pitch=atti(2,1)*pi/180.0;head=atti(3,1)*pi/180.0;

  %坐標系N-->B
  Cbn=[cos(roll)*cos(head)+sin(roll)*sin(pitch)*sin(head), -cos(roll)*sin(head)+sin(roll)*sin(pitch)*cos(head), -sin(roll)*cos(pitch);
       cos(pitch)*sin(head),                               cos(pitch)*cos(head),                                sin(pitch);
       sin(roll)*cos(head)-cos(roll)*sin(pitch)*sin(head), -sin(roll)*sin(head)-cos(roll)*sin(pitch)*cos(head), cos(roll)*cos(pitch)];

  veloN=Cbn'*veloB;
  Ve=veloN(1,1);Vn=veloN(2,1);Vu=veloN(3,1);

  heig=heig+T*Vu;
  lati=lati+T*(Vn/(Rm+heig));
  long=long+T*(Ve/((Rn+heig)*cos(lati)));

  posiN(1,1)=long*180.0/pi;       %單位：度
  posiN(2,1)=lati*180.0/pi;       %單位：度
  posiN(3,1)=heig;

  
  
  TraceData=[TraceData;t,posiN',veloB(2,1),atti'];
end
figure(1);
plot3(TraceData(:,2),TraceData(:,3),TraceData(:,4),'m');
ylim([30,35]);
title('飛行航跡仿真');
xlabel('經度（^{\circ}）');ylabel('緯度（^{\circ}）');zlabel('高度（m）');
grid;
figure(2);
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,2));title('飛行器經、緯、高度仿真');ylabel('經度（^{\circ}）');grid;
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,3));ylim([30,35]);ylabel('緯度（^{\circ}）');grid;
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,4));xlabel('t(sec)');ylabel('高度（m）');grid;

%%%%%%%%%%%%%存儲仿真數據%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
save trace5.dat TraceData -ASCII;    %存儲航跡數據