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



%%%%%%%%仿真時間設(shè)置%%%%%%
t=0;
T=1;
TraceData=[];
t_stop=1850.0;

%%%%%%%%%%%%%%%%航跡發(fā)生器%%%%%%%%%%%%%%%%%
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);     %航跡發(fā)生器初始位置經(jīng)度、緯度、高度（單位：度、度、米）
posi=[118.78;32.05;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;                 %初始對準時間    
   elseif(t<=20)                     %加速滑跑
       acceB(2,1)=4.0;
   elseif(t<=24)                     %抬頭
       atti_rate(2,1)=7.5;
       acceB(2,1)=5.0;            
   elseif (t<=84)                    %爬高
       atti_rate(2,1)=0.0;
   elseif (t<=88)                    %改平
       atti_rate(2,1)=-7.5;
       acceB(2,1)=0.0;
   elseif (t<=108)                   %加速飛行
       atti_rate(2,1)=0.0;
       acceB(2,1)=5.0;
   elseif (t<=111)                   %傾斜預(yù)轉(zhuǎn)彎
       acceB(2,1)=0.0;
       atti_rate(1,1)=10.0;
   elseif (t<=141)                   %轉(zhuǎn)彎
       atti_rate(1,1)=0.0;
       atti_rate(3,1)=-2.6;
   elseif (t<=144)                   %改平
       atti_rate(1,1)=-10.0;
       atti_rate(3,1)=0.0;
   elseif (t<=1744)                  %平直飛行
       att_rate(1,1)=0.0;
   elseif (t<=1764)                  %減速飛行
       acceB(2,1)=-5.0;
   elseif (t<=1770)                  %低頭
       atti_rate(2,1)=-7.5;
   elseif (t<=1803)                  %俯沖
       atti_rate(2,1)=0.0;
   elseif (t<=1809)                  %改平
       atti_rate(2,1)=7.5;
   elseif (t<=1844)                  %減速滑行至停止
       atti_rate(2,1)=0.0;
       acceB(2,1)=-5.0;
   elseif (t<=1850)                  %靜止
       acceB(2,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);
  V=sqrt(veloN(1,1)^2+veloN(2,1)^2+veloN(3,1)^2);

  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',V,veloN'];
end
fg_1=figure('Name','Trace','NumberTitle','off');
plot3(TraceData(:,2),TraceData(:,3),TraceData(:,4),'m',TraceData(1,2),TraceData(1,3),TraceData(1,4),'o',TraceData(1825,2),TraceData(1825,3),TraceData(1825,4),'o');
title('飛行航跡仿真(南京－>北京)');
text(TraceData(1,2),TraceData(1,3),TraceData(1,4),'南京');
text(TraceData(1825,2),TraceData(1825,3),TraceData(1825,4),'北京');
xlabel('經(jīng)度（^{\circ}）');ylabel('緯度（^{\circ}）');zlabel('高度（m）');
grid;
fg_2=figure('Name','trace','NumberTitle','off');
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,2));title('飛行器經(jīng)、緯、高度仿真');ylabel('經(jīng)度（^{\circ}）');grid;
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,3));ylabel('緯度（^{\circ}）');grid;
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,4));xlabel('t(sec)');ylabel('高度（m）');grid;
fg_3=figure('Name','Velo','NumberTitle','off');
plot(TraceData(:,1),TraceData(:,5));
title('飛行速度仿真');
xlabel('t(sec)');ylabel('速度（m/s）');grid;
fg_4=figure('Name','velo','NumberTitle','off');
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,6));title('飛行器東、北、天速度仿真');ylabel('東向速度(m/s)');grid;
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,7));ylabel('北向速度(m/s)');grid;
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,8));xlabel('t(sec)');ylabel('天向速度(m/s)');grid;
%%%%%%%%%%%%%存儲仿真數(shù)據(jù)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
save trace0.dat TraceData -ASCII;    %存儲航跡數(shù)據(jù)