trace5.m

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%                    航迹发生器
%
%  输入参数:
%  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;    %存储航迹数据