导航仿真程序Word格式.docx

1、 y通道速度 Vz（1）=0.007106781; z通道速度 重力加速度计算参数 g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;Vx=zeros（1,48001）;Vy=zeros（1,48001）;Vz=zeros（1,48001）;Lamda=zeros（1,48001）;L=zeros（1,48001）;Seita=zeros（1,48001）;Gama=zeros（1,48001）;Ksai=zeros（1,48001）; 四元素初始值 e0=cos（0.5*Ksai（1）*cos（0.5*Seita（1）*cos

2、（0.5*Gama（1）-sin（0.5*Ksai（1）*sin（0.5*Seita（1）*sin（0.5*Gama（1）;e1=-cos（0.5*Ksai（1）*sin（0.5*Seita（1）*cos（0.5*Gama（1）+sin（0.5*Ksai（1）*cos（0.5*Seita（1）*sin（0.5*Gama（1）;e2=-cos（0.5*Ksai（1）*cos（0.5*Seita（1）*sin（0.5*Gama（1）-sin（0.5*Ksai（1）*sin（0.5*Seita（1）*cos（0.5*Gama（1）;e3=cos（0.5*Ksai（1）*sin（0.5*Seita（1

3、）*sin（0.5*Gama（1）+sin（0.5*Ksai（1）*cos（0.5*Seita（1）*cos（0.5*Gama（1）;Ctb=e02+e12-e22-e32 2*（e1*e2+e0*e3） 2*（e1*e3-e0*e2）; 用四元素表示得姿态矩阵 2*（e1*e2-e0*e3） e02-e12+e22-e32 2*（e2*e3+e0*e1）;2*（e1*e3+e0*e2） 2*（e2*e3-e0*e1） e02-e12-e22+e32;E=e0 e1 e2 e3; 四元素的四个元素值 for i=1:48000 Ry（i）=Re*（1-2*e+3*e*（sin（L（i）2）;

4、计算子午圈主曲率半径 Rx（i）=Re*（1+e*（sin（L（i）2）; 计算卯酉圈主曲率半径 g=g0*（1+gk1*（sin（L（i）2）*（1-2*h/Re）/sqrt（1-gk2*（sin（L（i）2）; 重力加速度计算 Cbt=Ctbf_t=Cbt*f_INSc; 将体轴系中的比例转化到地理系 Vx（i+1）=（f_t（1,i）+2*Wie*sin（L（i）*Vy（i）+Vx（i）*Vy（i）*tan（L（i）/Rx（i）/80+Vx（i）; x通道速度计算 Vy（i+1）=（f_t（2,i）-2*Wie*sin（L（i）*Vx（i）-Vx（i）*Vx（i）*tan（L（i）/Rx

5、（i）/80+Vy（i）; y通道速度计算 Vz（i+1）=（f_t（3,i）+2*Wie*cos（L（i）*Vx（i）+Vx（i）*Vx（i）/Rx（i）+Vy（i）*Vy（i）/Ry（i）-g）/80+Vz（i）;Lamda（i+1）=Vx（i）/cos（L（i）/Rx（i）/80+Lamda（i）; 经度计算 if Lamda（i+1）pi Lamda（i+1）=Lamda（i+1）-2*pi;经度在-180度（西经）到180（东经）范围 end L（i+1）=Vy（i）/Ry（i）/80+L（i）; 纬度计算 if L（i+1）（pi/2） L（i+1）=pi-L（i+1）;纬度小于9

6、0度（北纬） Wetx_t（i）=-Vy（i）/Ry（i）;Wety_t（i）=Vx（i）/Rx（i）;Wetz_t（i）=Vx（i）*tan（L（i）/Rx（i）; 在地理坐标系的位移角速率 Wet_t=Wetx_t（i） Wety_t（i） Wetz_t（i）Wib_b=wib_INSc（1,i） wib_INSc（2,i） wib_INSc（3,i） 陀螺仪测的角速率值 Wie_t=0 Wie*cos（L（i） Wie*sin（L（i） 在地理坐标系的地球角速率 Wtb_b=Wib_b-Ctb*（Wie_t+Wet_t）; 姿态矩阵角速率 用角增量法计算四元素姿态矩阵 Mwtb=0 -W

7、tb_b（1） -Wtb_b（2） -Wtb_b（3）;Wtb_b（1） 0 Wtb_b（3） -Wtb_b（2）;Wtb_b（2） -Wtb_b（3） 0 Wtb_b（1）;Wtb_b（3） Wtb_b（2） -Wtb_b（1） 0/80;derta=sqrt（Mwtb（1,2）2+（Mwtb（1,3）2+（Mwtb（1,4）2）;E=eye（4）*（1-derta2/8+derta4/384）+（1/2-derta2/48）*Mwtb*E; E=（cos（0.5*derta）*eye（4）+Mwtb*sin（0.5*derta）/derta）*E，采用四阶近似算法 e0=E（1）;e1=E

8、（2）;e2=E（3）;e3=E（4）; 姿态角计算 Seita（i+1）=asin（Ctb（2,3）; 俯仰角计算 Gama（i+1）=atan（-Ctb（1,3）/Ctb（3,3）; 横滚角计算 if abs（Ctb（3,3）eps if Ctb（3,3）0 Gama（i+1）=Gama（i+1）;elseif -Ctb（1,3） 0 Gama（i+1）=Gama（i+1）+pi;else Gama（i+1）=Gama（i+1）-pi;Gama（i+1）=pi/2;else Gama（i+1）=-pi/2;Ksai（i+1）=atan（Ctb（2,1）/Ctb（2,2）; 航向角计算 if

9、 abs（Ctb（2,2）if Ctb（2,2）Ksai（i+1）=Ksai（i+1）;elseif Ctb（2,1）Ksai（i+1）=Ksai（i+1）+pi;else Ksai（i+1）=Ksai（i+1）-pi;Ksai（i+1）=pi/2;else Ksai（i+1）=-pi/2; 将弧度换算为角度 Seita=Seita*180/pi;Gama=Gama*180/pi;Ksai=Ksai*180/pi;L=L*180/pi;Lamda=Lamda*180/pi;t=0:1/80:600; 绘制曲线图 figure（1）;plot（t,Lamda）% 绘制经度变化曲线图 grid o

10、n Xlabel（时间/秒）;Ylabel（经度Lamda/度title（经度变化曲线图figure（2）;plot（t,L） % 绘制纬度变化曲线图 纬度L/度纬度变化曲线图figure（3）;plot（Lamda,L）% 绘制经-纬度变化曲线图 经纬度坐标曲线图figure（4）;plot（t,Seita）% 绘制俯仰角变化曲线图 俯仰角Seita/度俯仰角变化曲线图figure（5）;plot（t,Gama）% 绘制横滚角变化曲线图 横滚角Gama/度横滚角变化曲线图figure（6）;plot（t,Ksai） % 绘制航向角变化曲线 航向角Ksai/度航向角变化曲线图figure（7）

11、;plot（t,Vx）% 绘制东向速度变化曲线 东向速度Vx 米/秒东向速度变化曲线图figure（8）;plot（t,Vy）% 绘制北向速度变化曲线 北向速度Vy 米/秒北向速度变化曲线图figure（9）;plot（t,Vz）% 绘制垂直速度变化曲线 垂直速度Vz 米/秒垂直速度变化曲线图65行有错误,不知道能否更正 缺少了注释号修改如下：这里我有一分钟的数据，是自己模拟的载机S机动，现将其传上来，顺便将这程序进行了一点点的改变，给出的结果并不理想，大家可以拿去跑一下，反正跟我真实的轨迹差别很大此为基于四元素法，角增量法的捷连惯导系统程序算法 飞行器飞行过程中飞行高度不变 航向角以逆时针为

12、正 以地理系为导航坐标系 运行程序时需导入比力信息及陀螺议角速率信息clcclearclose allData = load（Track_S.txtf_INS = Data（:,1:3）;% 加载加表数据wib_INS = Data（:,4:6）;% 加载陀螺数据L0 = size（Data,1）;Wie = 7.292115147e-5; 地球自传角速度Re = 6378245; 地球椭球长半径h = 30; 飞行高度e = 1/298.3; 初始经纬度Lamda（1） = 116.344695283*pi/180; 初始经度（弧度）L（1） = 39.975172*pi/180; 初始纬度

13、（弧度） 初始姿态角Seita（1） = 0.120992605*pi/180; 俯仰角（弧度）Gama（1） = 0.010445947*pi/180; 横滚角（弧度）Ksai（1） = 91.637207*pi/180; 航向角（弧度） 初始速度Vx（1） = 0.000048637; x通道速度Vy（1） = 0.000206947; y通道速度Vz（1） = 0.007106781; z通道速度 重力加速度计算参数g0 = 9.7803267714;gk1 = 0.00193185138639;gk2 = 0.00669437999013;Vx = zeros（1,L0）;Vy = z

14、eros（1,L0）;Vz = zeros（1,L0）;Lamda = zeros（1,L0）;L = zeros（1,L0）;Seita = zeros（1,L0）;Gama = zeros（1,L0）;Ksai = zeros（1,L0）; 四元素初始值e0 = cos（0.5*Ksai（1）*cos（0.5*Seita（1）*cos（0.5*Gama（1）-sin（0.5*Ksai（1）*sin（0.5*Seita（1）*sin（0.5*Gama（1）;e1 = -cos（0.5*Ksai（1）*sin（0.5*Seita（1）*cos（0.5*Gama（1）+sin（0.5*Ksai（

15、1）*cos（0.5*Seita（1）*sin（0.5*Gama（1）;e2 = -cos（0.5*Ksai（1）*cos（0.5*Seita（1）*sin（0.5*Gama（1）-sin（0.5*Ksai（1）*sin（0.5*Seita（1）*cos（0.5*Gama（1）;e3 = cos（0.5*Ksai（1）*sin（0.5*Seita（1）*sin（0.5*Gama（1）+sin（0.5*Ksai（1）*cos（0.5*Seita（1）*cos（0.5*Gama（1）;Ctb = e02+e12-e22-e32 2*（e1*e2+e0*e3） 2*（e1*e3-e0*e2）; 用四

16、元素表示得姿态矩阵E = e0 e1 e2 e3 四元素的四个元素值for i = 1:L0f_INSc = f_INS（i,:）wib_INSc = wib_INS（i,:Ry（i） = Re*（1-2*e+3*e*（sin（L（i）2）; 计算子午圈主曲率半径Rx（i） = Re*（1+e*（sin（L（i）2）; 计算卯酉圈主曲率半径g = g0*（1+gk1*（sin（L（i）2）*（1-2*h/Re）/sqrt（1-gk2*（sin（L（i）2）; 重力加速度计算Cbt = Ctbf_t = Cbt*f_INSc; 将体轴系中的比例转化到地理系Vx（i） = （f_t（1）+2*Wi

17、e*sin（L（i）*Vy（i）+Vx（i）*Vy（i）*tan（L（i）/Rx（i）/80+Vx（i）; x通道速度计算Vy（i） = （f_t（2）-2*Wie*sin（L（i）*Vx（i）-Vx（i）*Vx（i）*tan（L（i）/Rx（i）/80+Vy（i）; y通道速度计算Vz（i） = （f_t（3）+2*Wie*cos（L（i）*Vx（i）+Vx（i）*Vx（i）/Rx（i）+Vy（i）*Vy（i）/Ry（i）-g）/80+Vz（i）;Lamda（i） = Vx（i）/cos（L（i）/Rx（i）/80+Lamda（i）; 经度计算if Lamda（i）piLamda（i） =

18、Lamda（i）-2*pi;经度在-180度（西经）到180（东经）范围endL（i） = Vy（i）/Ry（i）/80+L（i）; 纬度计算if L（i）（pi/2）L（i） = pi-L（i）;纬度小于90度（北纬）Wetx_t（i） = -Vy（i）/Ry（i）;Wety_t（i） = Vx（i）/Rx（i）;Wetz_t（i） = Vx（i）*tan（L（i）/Rx（i）; 在地理坐标系的位移角速率Wet_t = Wetx_t（i） Wety_t（i） Wetz_t（i）Wib_b = wib_INSc（1） wib_INSc（2） wib_INSc（3） 陀螺仪测的角速率值Wie_t

19、 = 0 Wie*cos（L（i） Wie*sin（L（i） 在地理坐标系的地球角速率Wtb_b = Wib_b-Ctb*（Wie_t+Wet_t）; 姿态矩阵角速率 用角增量法计算四元素姿态矩阵Mwtb = 0 -Wtb_b（1） -Wtb_b（2） -Wtb_b（3）;derta = sqrt（Mwtb（1,2）2+（Mwtb（1,3）2+（Mwtb（1,4）2）;E = eye（4）*（1-derta2/8+derta4/384）+（1/2-derta2/48）*Mwtb*E; E = （cos（0.5*derta）*eye（4）+Mwtb*sin（0.5*derta）/derta）*E

20、，采用四阶近似算法e0 = E（1）;e1 = E（2）;e2 = E（3）;e3 = E（4）; 姿态角计算Seita（i） = asin（Ctb（2,3）; 俯仰角计算Gama（i） = atan（-Ctb（1,3）/Ctb（3,3）; 横滚角计算epsGama（i） = Gama（i）; 0Gama（i） = Gama（i）+pi;else Gama（i） = Gama（i）-pi;Gama（i） = pi/2;else Gama（i） = -pi/2;Ksai（i） = atan（Ctb（2,1）/Ctb（2,2）; 航向角计算Ksai（i） = Ksai（i）;Ksai（i） = K

21、sai（i）+pi;else Ksai（i） = Ksai（i）-pi;Ksai（i） = pi/2;else Ksai（i） = -pi/2; 将弧度换算为角度Seita = Seita*180/pi;Gama = Gama*180/pi;Ksai = Ksai*180/pi;L = L*180/pi;Lamda = Lamda*180/pi;t = 0.01:0.01:L0*0.01; 绘制曲线图figureplot（L,Lamda）% 绘制经度变化曲线图grid on经度维度经维度变化曲线图 绘制俯仰角变化曲线图 绘制横滚角变化曲线图 绘制航向角变化曲线 绘制东向速度变化曲线 绘制北向速度变化曲线