www.pudn.com > trajectory-simulation.rar > new0606.m, change:2013-06-08,size:5159b


clear all 
clc 
 
%初始参数 
f0=5*pi/180;   %俯仰角初值 
g0=5*pi/180;   %滚转角初值 
p0=5*pi/180;    %偏航角初值 
 
A0=30*pi/180; 
A1=50*pi/180; 
A2=2000; 
w3=2*pi/7200; 
lat0=45*pi/180; 
long0=120*pi/180; 
h0=3000; 
Re=6378137; 
 
w=2*pi/1000;        %俯仰角、滚转角、航向角角频率 
T=1;%采样频率 
t_stop = 5000;%仿真时间 
t=0; 
TraceData=[]; 
 
while(t<=t_stop) 
%f为俯仰角、g为滚转角、p为航向角 
f=f0*cos(w*t); 
g=g0*cos(w*t); 
p=p0*cos(w*t);     
 
 %df,dg,dp分别为俯仰、滚转、航向角的变化率 
df=-f0*w*sin(w*t); 
dg=-g0*w*sin(w*t); 
dp=-p0*w*sin(w*t);   
 
%机体系相对地理系转动角速度度在机体系投影 
wbtbx=cos(g).*df-sin(g).*cos(f).*dp; 
wbtby=dg+sin(f).*dp; 
wbtbz=sin(g).*df+cos(g).*cos(f).*dp;      
 
%纬度、经度、高度 
lat=lat0+A0*cos(w3*t); 
long=long0+A1*cos(w3*t); 
h=h0+A2*cos(w3*t); 
 
dlat=-A0*w3*sin(w3*t); 
dlong=-A1*w3*sin(w3*t); 
dh=-A2*w3*sin(w3*t); 
 
ddlat=-A0*w3^2*cos(w3*t); 
ddlong=-A1*w3^2*cos(w3*t); 
ddh=-A2*w3^2*cos(w3*t); 
 
%地球地固坐标系下的位置 
x=(Re+h).*cos(lat).*cos(long); 
y=(Re+h).*cos(lat).*sin(long); 
z=(Re+h).*sin(lat);             
 
 
%地球直角坐标系下的速度 
dx=dh.*cos(lat).*cos(long)-dlat.*(Re+h).*sin(lat).*cos(long)-dlong.*(Re+h).*cos(lat).*sin(long); 
dy=dh.*cos(lat).*sin(long)-dlat.*(Re+h).*sin(lat).*sin(long)+dlong.*(Re+h).*cos(lat).*cos(long); 
dz=dh.*sin(lat)+dlat.*(Re+h).*cos(lat); 
 
%地球直角坐标系下的加速度 
ddx=ddh.*cos(lat).*cos(long)-dh.*dlat.*sin(lat).*cos(long)-dh.*dlong.*cos(lat).*sin(long)-ddlat.*(Re+h).*sin(lat).*cos(long)-dh.*dlat.*sin(lat).*cos(long)-(Re+h).*dlat.^2.*cos(lat).*cos(long)+(Re+h).*dlat.*dlong.*sin(lat).*sin(long)-ddlong.*(Re+h).*cos(lat).*sin(long)-dh.*dlong.*cos(lat).*sin(long)+(Re+h).*dlat.*dlong.*sin(lat).*sin(long)-(Re+h).*dlong.^2.*cos(lat).*cos(long);%地球坐标系下的加速度 
ddy=ddh.*cos(lat).*sin(long)-dh.*dlat.*sin(lat).*sin(long)+dh.*dlong.*cos(lat).*cos(long)-ddlat.*(Re+h).*sin(lat).*sin(long)-dh.*dlat.*sin(lat).*sin(long)-(Re+h).*dlat.^2.*cos(lat).*sin(long)-(Re+h).*dlat.*dlong.*sin(lat).*cos(long)+ddlong.*(Re+h).*cos(lat).*cos(long)+dh.*dlong.*cos(lat).*cos(long)-(Re+h).*dlat.*dlong.*sin(lat).*cos(long)-(Re+h).*dlong.^2.*cos(lat).*sin(long); 
ddz=ddh.*sin(lat)+dh.*dlat.*cos(lat)+ddlat.*(Re+h).*cos(lat)+dh.*dlat.*cos(lat)-dlat.^2.*(Re+h).*sin(lat); 
 
%地理坐标系下的速度 
v_etx=-sin(long).*dx+cos(long).*dy; 
v_ety=-sin(lat).*cos(long).*dx-sin(lat).*sin(long).*dy+cos(lat).*dz; 
v_etz=cos(lat).*cos(long).*dx+cos(lat).*sin(long).*dy+sin(lat).*dz; 
 
%地球坐标系中相对于地球加速度在地理坐标系下的加速度投影 
a_rx=-sin(long).*ddx+cos(long).*ddy; 
a_ry=-sin(lat).*cos(long).*ddx-sin(lat).*sin(long).*ddy+cos(lat).*ddz; 
a_rz=cos(lat).*cos(long).*ddx+cos(lat).*sin(long).*ddy+sin(lat).*ddz; 
 
%当地地理坐标系下的载体相对于地球坐标系的加速度 
a_x=a_rx+v_etx.*v_ety.*tan(lat)./(Re+h)-v_etx.*v_etz./(Re+h); 
a_y=a_ry-v_etx.^2.*tan(lat)./(Re+h)-v_ety.*v_etz./(Re+h); 
a_z=a_rz+v_etx.^2./(Re+h)+v_ety.^2./(Re+h); 
 
posiN(1,1)=long;       %单位:rad 
posiN(1,2)=lat;       %单位:rad 
posiN(1,3)=h; 
   
Velocity(1,1) = v_etx; 
Velocity(1,2) = v_ety; 
Velocity(1,3) = v_etz; 
   
Accelerate(1,1) = a_x; 
Accelerate(1,2) = a_y; 
Accelerate(1,3) = a_z; 
   
Attitude(1,1)=f; 
Attitude(1,2)=g; 
Attitude(1,3)=p; 
   
AngleVelo(1,1) = wbtbx; 
AngleVelo(1,2) = wbtby; 
AngleVelo(1,3) = wbtbz; 
 
TraceData = [TraceData;t,posiN,Attitude,AngleVelo,Velocity,Accelerate]; 
t=t+T; 
end 
save TraceData; 
 
 
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(3); 
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,5));title('飞行器姿态角仿真');ylabel('俯仰角(rad)');grid; 
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,6));ylabel('滚转角(rad)');grid; 
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,7));xlabel('t(sec)');ylabel('偏航角(rad)');grid; 
 
figure(4); 
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,8));title('飞行器姿态角速度仿真');ylabel('东向角速度(rad/s)');grid; 
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,9));ylabel('北向角速度(rad/s)');grid; 
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,10));xlabel('t(sec)');ylabel('天向角速度(rad/s)');grid; 
 
figure(5); 
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,11));title('飞行器东、北、天速度仿真');ylabel('东向速度(m/s)');grid; 
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,12));ylabel('北向速度(m/s)');grid; 
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,13));xlabel('t(sec)');ylabel('天向速度(m/s)');grid; 
 
figure(6); 
subplot(3,1,1);plot(TraceData(:,1),TraceData(:,14));title('飞行器东、北、天加速度仿真');ylabel('东向加速度(m/秒平方)');grid; 
subplot(3,1,2);plot(TraceData(:,1),TraceData(:,15));ylabel('北向加速度(m/秒平方)');grid; 
subplot(3,1,3);plot(TraceData(:,1),TraceData(:,16));xlabel('t(sec)');ylabel('天向加速度(m/秒平方)');grid;