GPS卫星信号模拟器

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % 航迹发生器 % % 输入参数: % 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; %初始对准时间 elseif(t<=20) %滑跑 acceB(2,1)=4.0; elseif(t<=24) %加速拉起 atti_rate(2,1)=7.5; acceB(2,1)=5.0; elseif (t<=64) %爬高 atti_rate(2,1)=0.0; elseif (t<=68) %改平 atti_rate(2,1)=-7.5; acceB(2,1)=0.0; elseif (t<=668) %平直飞行 atti_rate(2,1)=0.0; elseif (t<=671) %倾斜预转弯 atti_rate(1,1)=10.0; elseif (t<=731) %转弯 atti_rate(3,1)=1.5; atti_rate(1,1)=0.0; elseif (t<=734) %改平 atti_rate(3,1)=0.0; atti_rate(1,1)=-10.0; elseif (t<=1334) %平直飞行 atti_rate(1,1)=0.0; elseif (t<=1342) %加速拉起 atti_rate(2,1)=7.5; acceB(2,1)=2.0; elseif (t<=1374) %加速爬高 atti_rate(2,1)=0.0; elseif (t<=1382) %改平 atti_rate(2,1)=-7.5; acceB(2,1)=0.0; elseif (t<=1862) %平直飞行 atti_rate(2,1)=0.0; elseif (t<=1902) %减速飞行 acceB(2,1)=-2.5; elseif (t<=1905) %倾斜预转弯 atti_rate(1,1)=10.0; acceB(2,1)=0.0; elseif (t<=1965) %转弯 atti_rate(3,1)=1.5; atti_rate(1,1)=0.0; elseif (t<=1968) %改平 atti_rate(3,1)=0.0; atti_rate(1,1)=-10.0; elseif (t<=2568) %平直飞行 atti_rate(1,1)=0.0; elseif (t<=2574) %低头 atti_rate(2,1)=-7.5; elseif (t<=2594) %俯冲 atti_rate(2,1)=0.0; elseif (t<=2600) %改平 atti_rate(2,1)=7.5; elseif (t<=2603) %倾斜预转弯 atti_rate(2,1)=0.0; atti_rate(1,1)=10.0; elseif (t<=2663) %转弯 atti_rate(3,1)=1.5; atti_rate(1,1)=0.0; elseif (t<=2666) %改平 atti_rate(3,1)=0.0; atti_rate(1,1)=-10.0; elseif (t<=3600) %平直飞行 atti_rate(1,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),veloN']; end figure(1); plot3(TraceData(:,2),TraceData(:,3),TraceData(:,4),'m'); 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));ylabel('纬度（^{\circ}）');grid; subplot(3,1,3);plot(TraceData(:,1),TraceData(:,4));xlabel('t(sec)');ylabel('高度（m）');grid; figure(4); 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; %%%%%%%%%%%%%存储仿真数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% save trace1.dat TraceData -ASCII; %存储航迹数据