惯导轨迹发生器（产生陀螺仪与加速度计的数据）

用于产生惯导轨迹，即标定轨迹，陀螺仪的输出，加速度计的输出

%轨迹发生器
t=0:0.01:60*10;%仿真时间
f0=43*pi/180;g0=5*pi/180;p0=3*pi/180;%俯仰角、滚转角、偏航角初值
w=pi/1200;%俯仰角、滚转角、航向角角频率
f=f0*cos（w*t）;g=g0*cos（w*t）;p=p0*cos（w*t）;%f为俯仰角、g为滚转角、p为航向角
df=-f0*w*sin（w*t）;dg=-g0*w*sin（w*t）;dp=-p0*w*sin（w*t）;%dfdgdp分别为俯仰、滚转、航向角的变化率
wbtbx=cos（g）.*df-sin（g）.*cos（f）.*dp;wbtby=dg+sin（f）.*dp;wbtbz=sin（g）.*df+cos（g）.*cos（f）.*dp;%机体系相对地理系转动角速度度在机体系投影
A0=4*pi/180;A1=5*pi/180;A2=1000;w3=2*pi/720;lat0=43*pi/180;long0=123*pi/180;h0=2500;
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）;
Re=6378137;
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）;
vtetx=-sin（long）.*dx+cos（long）.*dy;%地理坐标系下的速度
vtety=-sin（lat）.*cos（long）.*dx-sin（lat）.*sin（long）.*dy+cos（lat）.*dz;
vtetz=cos（lat）.*cos（long）.*dx+cos（lat）.*sin（long）.*dy+sin（lat）.*dz;
atrx=-sin（long）.*ddx+cos（long）.*ddy;%地理坐标系下的加速度
atry=-sin（lat）.*cos（long）.*ddx-sin（lat）.*sin（long）.*ddy+cos（lat）.*ddz;
atrz=cos（lat）.*cos（long）.*ddx+cos（lat）.*sin（long）.*ddy+sin（lat）.*ddz;
atx=atrx+vtetx.*vtety.*tan（lat）./（Re+h）-vtetx.*vtetz./（Re+h）;
aty=atry-vtetx.^2.*tan（lat）./（Re+h）-vtety.*vtetz./（Re+h）;
atz=atrz+vtetx.^2./（Re+h）+vtety.^2./（Re+h）;
subplot（231）;
plot（tf*180/pi）;
title（‘俯仰角‘）;
xlabel（‘时间（秒）‘）;
ylabel（‘俯仰角（度）‘）
subplot（232）;
plot（tg*180/pi）;
axis auto;
title（‘滚转角‘）;
xlabel（‘时间（秒）‘）;
ylabel（‘滚转角（度）‘）
subplot（233）;
plot（tp*180/pi）;
axis auto;
title（‘偏航角‘）;
xlabel（‘时间（秒）‘）;
ylabel（‘偏航角（度）‘）;
subplot（234）;
plot（tlat*180/pi）;
axis auto;
title（‘纬度‘）;
xlabel（‘时间（秒）‘）;
ylabel（‘纬度（度）‘）;
subplot（235）;
plot（tlong*180/pi）;
axis auto;
title（‘经度‘）;
xlabel（‘时间（秒）‘）;
ylabel（‘经度（度）‘）;
subplot（236）;
plot（th）;
axis auto;
title（‘高度‘）;
xlabel（

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3423  2009-05-02 20:24  locus.m

----------- ---------  ---------- -----  ----

                 3423                    1