trace4.m

本程序设计了5条典型的航迹发生器

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                    航迹发生器
%
%  输入参数:
%  t          仿真时间
%  T          仿真步长
%  atti       横滚、俯仰、航向（单位：度）
%  atti_rate  横滚速率、俯仰速率、航向速率（单位：度/秒）
%  veloB      飞机运动速度——X右翼、Y机头、Z天向（单位：米/秒）
%  acceB      飞机运动加速度——X右翼、Y机头、Z天向（单位：米/秒/秒）
%  posi       航迹发生器初始位置经度、纬度、高度（单位：度、度、米）
%
%        
%                           程序设计：吴玲  日期：2007/12/05
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%仿真时间设置%%%%%%
t=0;
T=1;
TraceData=[];
t_stop=1445.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<=10)
      acceB(2,1)=2.0;
   elseif(t<=50)
      acceB(2,1)=0.0;
   elseif(t<=55)
      acceB(2,1)=-2.0;
   elseif(t<=60)
      acceB(2,1)=0.0;
      atti_rate(3,1)=-18.0;
   elseif(t<=65)
      acceB(2,1)=2.0;
      atti_rate(3,1)=0.0;
   elseif(t<=105)
      acceB(2,1)=0.0;
  elseif(t<=110)
      acceB(2,1)=-2.0;
  elseif(t<=115)
      acceB(2,1)=0.0;
      atti_rate(3,1)=-18.0;
  elseif(t<=120)
      acceB(2,1)=2.0;
      atti_rate(3,1)=0.0; 
  elseif (t<=125)                  
      atti_rate(1,1)=9.0;
   elseif (t<=240)                  
      atti_rate(1,1)=0.0;
   elseif (t<=245)                  
      atti_rate(1,1)=-9.0;
   elseif (t<=360)                  
      atti_rate(1,1)=0.0;
   elseif(t<=365)
      atti_rate(1,1)=-9.0;
   elseif(t<=480)
      atti_rate(1,1)=0.0;
   elseif(t<=485)
      atti_rate(1,1)=9.0;
   else
      atti_rate(1,1)=0.0;
   end
     %横滚变化

   if(t<=600)                  
      atti_rate(2,1)=0.0;
   elseif (t<=605)                  
      atti_rate(2,1)=9.0;
   elseif (t<=720)                  
      atti_rate(2,1)=0.0;
   elseif (t<=725)                  
      atti_rate(2,1)=-9.0;
   elseif (t<=840)                  
      atti_rate(2,1)=0.0;
   elseif(t<=845)
      atti_rate(2,1)=-9.0;
   elseif(t<=960)
      atti_rate(2,1)=0.0;
   elseif(t<=965)
      atti_rate(2,1)=9.0;
   else
      atti_rate(2,1)=0.0;
   end
     %俯仰变化
 
   if(t<=1080)                  
      atti_rate(3,1)=0.0;
   elseif (t<=1085)                  
      atti_rate(3,1)=9.0;
   elseif (t<=1200)                  
      atti_rate(3,1)=0.0;
   elseif (t<=1205)                  
      atti_rate(3,1)=-9.0;
   elseif (t<=1320)                  
      atti_rate(3,1)=0.0;
   elseif(t<=1325)
      atti_rate(3,1)=-9.0;
   elseif(t<=1440)
      atti_rate(3,1)=0.0;
   elseif(t<=1445)
      atti_rate(3,1)=9.0;
   else
      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');
grid;
figure(2);
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,2));grid;
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,3));grid;
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,4));grid;
xlabel('飞行航迹仿真（经度（度），纬度（度），高度（米））');

%%%%%%%%%%%%%存储仿真数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
save trace4.dat TraceData -ASCII;    %存储航迹数据