哈工大导航原理大作业剖析
哈工大自动控制原理大作业
自动控制原理大作业1.题目在通常情况下，自动导航小车（AGV ）是一种用来搬运物品的自动化设备。
大多数AGV 都需要有某种形式的导轨，但迄今为止，还没有完全解决导航系统的驾驶稳定性问题。
因此，自动导航小车在行驶过程中有时会出现轻微的“蛇行”现象，这表明导航系统还不稳定。
大多数的AGV 在说明书中都声明其最大行驶速度可以达到1m/s ，但实际速度通常只有0.5m/s ,只有在干扰较小的实验室中，才能达到最高速度。
随着速度的增加，要保证小车得稳定和平稳运行将变得越来越困难。
AGV 的导航系统框图如图9所示，其中12=40ms =21ms ττ， 。
为使系统响应斜坡输入的稳态误差仅为1%，要求系统的稳态速度误差系数为100。
试设计合适的滞后校正网络，试系统的相位裕度达到50 ，并估计校正后系统的超调量及峰值时间。
()R s ()Y s2.分析与校正主要过程2.1确定开环放大倍数K100)1021.0)(104.0(lim )(lim =++==s s s sK s sG K v （s →0） 解得K=100)1021.0)(104.0(100++=s s s G s 2.2分析未校正系统的频域特性根据Bode 图：穿越频率s rad c /2.49=ω相位裕度︒---=⨯-⨯--=99.18)2.49021.0(arctan )2.4904.0(arctan 9018011γ 未校正系统频率特性曲线由图可知实际穿越频率为s rad c /5.34=ω2.3根据相角裕度的要求选择校正后的穿越频率1c ω现在进行计算：︒︒︒--=+=---55550)021.0(arctan )04.0(arctan 901801111c c ωω则取s rad c /101=ω可满足要求2.4确定滞后校正网络的校正函数 由于11201~101c ωω）（=因此取s rad c /110111==ωω）（，则由Bode 图可以列出 40)1lg(20)1lg(40)110lg(2022+=+ωω 解得s rad /1.02=ω于是1.0=β 则滞后网络传递函数为1101)(++=s s s G c ，10=T 2.5验证已校正系统的相位裕度已校正系统的开环传递函数为：)110)(1021.0)(104.0()1(100)()(++++=s s s s s s G s G c 相位裕度︒----=-⨯-⨯-+-=2.51)100(arctan )10021.0(arctan )1004.0(arctan )10(arctan 901801111γ校正后的相位裕度大于50°，满足设计要求。
哈工大导航原理大作业
《导航原理》作业(惯性导航部分)一、题目要求 A fighter equipped with SINS is initially at the position of ︒35 NL ︒122X G Y G Z G ,and three accelerometers, X A ,Y A ,Z A are installed along the axes b X ,b Y ,b Z of the body frame respectively.Case 1:stationary onboard testThe body frame of the fighter initially coincides with the geographical frame, as shown in the figure, with its pitching axis b X pointing to the east,rolling axis b Y to the north, and azimuth axis b Z upward. Then the body of the fighter is made to rotate step by step relative to the geographical frame.(1) ︒10around b X(2) ︒30around b Y(3) ︒50-around b ZAfter that, the body of the fighter stops rotating. You are required to compute the final output of the three accelerometers on the fighter, using both DCM and quaternion respectively,and ignoring the device errors. It is known that the magnitude of gravity acceleration is 2/8.9g s m =. Case 2:flight navigationInitially, the fighter is stationary on the motionless carrier with its board 25m above the sea level. Its pitching and rolling axes are both in the local horizon, and its rolling axis is ︒45on the north by east, parallel with the runway onboard. Then the fighter accelerate along the runway and take off from the carrier.The output of the gyros and accelerometers are both pulse numbers,Each gyro pulse is an angular increment of sec arc 1.0-,and each accelerometer pulse is g 6e 1-,with 2/8.9g s m =.The gyro output frequency is 10 Hz,andthe accelerometer’s is 1Hz. The output of gyros and accelerometers within 5400s are stored in MATLAB data files named gout.mat and aout.mat, containing matrices gm of35400⨯ and am of 35400⨯ respectively. The format of data as shown in the tables, with 10 rows of each matrix selected. Each row represents the out of the type of sensors at each sample time.The Earth can beseen as an idealsphere, with radius6368.00km andspinning rate rad/s 10292.75-⨯, Theerrors of sensors areignored, so is theeffect of height onthe magnitude ofgravity. The outputof the gyros are to integrated every 0.1s. The rotation of geographical frame is to be updated every 1s, so are the velocities and position of the figure. You are required to:(1)Compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the fighter.(2)Compute the total horizontal distance traveled by the fighter.(3)Draw the latitude-versus-longitude trajectory of the fighter, with horizontal longitude axis.(4)Draw the curve of the height of fighter, with horizontal time axis.二、Case1解答2.1 方向余弦阵法（1） 绕Xb 轴转过ψ︒=10ϕ ⎪⎪⎪⎭⎫ ⎝⎛︒︒-︒︒=⎪⎪⎪⎭⎫ ⎝⎛-=10cos 10sin 010sin 10cos 0001cos sin 0sin cos 0001ϕϕϕϕϕC（2） 绕Yb 轴转过︒=30θ ⎪⎪⎪⎭⎫ ⎝⎛︒︒︒-︒=⎪⎪⎪⎭⎫ ⎝⎛-=30cos 030sin 01030sin 030cos cos 0sin 010sin 0cos θθθθθC（3） 绕Zb 轴转过︒-=50ψ所以变换后的坐标由于初始时刻有 所以计算得( Z Y X )=(3581.86027.2-4054.4-)三个加速度计的输出分别是计算程序见附录一2.2 四元数法（1）绕Xb（2）绕Yb（3）绕Zb 则合成四元数合成四元数的逆由公式计算得 (Z Y X )=(3581.86027.2-4054.4-)三个加速度计的输出分别是由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。
L1-导航原理(哈工大导航原理、惯性技术课件)讲解学习
陶瓷 壳体
球形 转子
球形电极 自转轴
钛离子泵
缺陷：结构复杂、昂贵
Lecture 1 -- Introduction
23
5.2 低成本、小型化
环形激光陀螺 (Ring laser gyro -- RLG) 1960s 早期开始研制, 1970s 后期进入实用
光纤陀螺 (Fiber Optical Gyro – FOG) 1970s 开始研制, 1980s 早期进入实用
15
4.2 历史: 陀螺罗经
陀螺仪被寄予希望, 但面临着自动寻北 的挑战
1908年, Anschutz (德国) 发明了陀螺罗经 (gyro compass)
1909年, Sperry (美国) 也独 立研制出陀螺罗经.
—— 陀螺罗经的出现标志着陀螺仪技术 的现代应用的发端
Lecture 1 -- Introduction
Lecture 1 -- Introduction
14
4.2 历史: 在航海的应用
磁罗盘 (Magnetic compass) 被用于早期的航海
19世纪后期，大量的木质 轮船被钢铁材质的轮船取代， 使磁罗盘的效能受到影响.
磁罗盘的使用在地球两极 附近受到限制 寻找替代的方向指示装置
Lecture 1 -- Introduction
Exam: Close book, close note Contact: 15204694662
Lecture 1 -- Introduction
28
此课件下载可自行编辑修改，仅供参考！ 感谢您的支持，我们努力做得更好！谢谢
t
S
S0
Vdt
0
惯性导航的特点: 自主 (Autonomous, self-contained) 只依赖于对载体的惯性测量 (借助加速度计、陀螺仪) 连续稳定的输出
“导航原理”实验教学课程思政探索与实践
2024.1黑龙江教育·理论与实践一、引言科技发展急需人才，人才培养取决于教育。
当前，随着我国社会改革的不断深化，社会思潮多样并存，各种思想交相融合，多元文化冲突更加频繁。
而学生正处于知识体系、思维方式和价值观念的形成时期，极易受到各种现象、观点、言论的影响。
极端个人主义、拜金主义、享乐主义等不良思潮给学生带来了消极影响，导致部分学生理想信念迷失、道德行为欠缺[1]。
如何培养富有社会责任感的创新人才，是近年来高等教育特别关注和不断探讨的课题。
专业课程教学融入思政元素，正是解决上述问题的一种尝试，也是加强和改进学生素质教育的一种探索[2]。
这种方式符合高等教育与时俱进的发展需要，不仅能够克服人才培养中的种种弊端，也极大地推动了高等教育的改革和创新，具有十分重要的意义[3]。
然而，课程思政在我国高校实施的时间并不长，还有待进一步健全和完善，尚需在理论上深入研究，在实践中总结经验。
文章结合“导航原理”实验教学的特点，将课程思政融入实验教学中，探索与实践该种教学方式对当代学生素质教育的提升效果。
二、“导航原理”实验教学特点“导航原理”作为高等工科院校控制科学与工程学科或航空宇航科学与技术学科的一门专业课程，是学习后续专业课程，如“飞行器控制与制导”“航天器控制”“无人机控制”“最优导航与滤波”等的基础。
“导航原理”实验教学不仅要帮助学生建立起惯性空间的概念，使学生加深对惯性器件结构特点、工作原理和基本特性的了解，实现对理论知识的验证，更重要的是通过实验使学生领悟惯性导航原理的应用规律，提高学生的动手能力、工程实践能力、设计能力和创新能力。
惯性导航系统是导弹和火箭的“眼睛”和“大脑”，提高惯性导航系统的精度是精确打击的关键。
惯性导航器件和系统的设计与制造需要精益求精的工匠精神和创新精神。
“导航原理”实验教学以培育学生科研实践能力和创新精神为目标，融合了国家战略、人才培养内涵式建设、学生个性化发展等多方面的内容[4]，具有深厚的课程思政资源和基础。
哈工大机械原理大作业一连杆运动分析（02）
哈⼯⼤机械原理⼤作业⼀连杆运动分析（02）⼀．设计题⽬⼆. 结构分析与基本杆组划分1.机构的结构分析机构各构件都在同⼀平⾯内运动，活动构件数n=3 P L=4 P H=0则机构的⾃由度为: F = 3n -2P L –P H = 3×3-2×4 = 12.基本杆组划分（1）去除虚约束和局部⾃由度本机构中⽆虚约束或局部⾃由度，此步骤跳过。
（2）拆杆组。
从远离原动件（即杆1）进⾏拆分，就可以得到由杆2,3组成的RRRⅡ级杆组和Ⅰ级机构杆1。
如下图：（3）确定机构的级别由（2）知，机构为Ⅱ级机构。
三. 运动分析数学模型以A为原点建⽴坐标系，如图：原动件AB的转⾓：φ1=0--2π运动副A的位置坐标：x A=0 y A=0 运动副D的位置坐标：x D=d y D=0 则运动副B的位置坐标：x B = acosφ1 y B = asinφ1其中：t=0:0.001:2*pi;a=60;b=90;c=120;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd)曲柄a=50,55,60,65红蓝绿黄b=90,c=120,d=100 t=0:0.001:2*pi; a=50;b=90;c=120;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'r'); hold on;grid on;t=0:0.001:2*pi;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'b'); hold on;t=0:0.001:2*pi;a=60;b=90;c=120;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'g'); hold on;t=0:0.001:2*pi;a=65;b=90;c=120;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'y');a=50,55,60,65红蓝绿黄2.摇杆c=105,115,125,135红蓝绿黄a=50,b=90,d=100 t=0:0.001:2*pi; a=50;b=90;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'r'); hold on;grid on;t=0:0.001:2*pi;a=50;b=90;c=115;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'b');t=0:0.001:2*pi;a=50;b=90;c=125;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'g'); hold on;t=0:0.001:2*pi;a=50;b=90;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'y');c=105,115,125,135红蓝绿黄3.连杆b=80,90,100,110红蓝绿黄a=50,c=120,d=100 t=0:0.001:2*pi; a=50;b=80;c=120;d=100;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'r'); hold on;grid on;t=0:0.001:2*pi;a=50;b=90;c=120;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'b'); hold on;t=0:0.001:2*pi;a=50;b=100;c=120;d=100;xa=0;ya=0;xd=d;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'g'); hold on;t=0:0.001:2*pi;a=50;b=110;c=120;d=100;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'y');b=80,90,100,110红蓝绿黄4.机架d=85,95,105,115 红蓝绿黄a=50,b=90,c=120 t=0:0.001:2*pi; a=50;b=90;c=120;d=85;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'r'); hold on;grid on;t=0:0.001:2*pi;a=50;b=90;c=120;d=95;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'b'); hold on;t=0:0.001:2*pi;d=105;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'g'); hold on;t=0:0.001:2*pi;a=50;b=90;c=120;d=115;xa=0;ya=0;xd=d;yd=0;xb=a.*cos(t);yb=a.*sin(t);m=xd-xb;n=yd-yb;lbd=(m.^2+n.^2).^(1/2);a0=2*b.*(xd-xb);b0=2*b.*(yd-yb);c0=b.^2+lbd.^2-c.^2;dd=2*atan((b0+(a0.^2+b0.^2-c0.^2).^(1/2))./(a0+c0)); plot(dd,'y');d=85,95,105,115 红蓝绿黄。
导航原理 大作业 哈工大
2. 程序设计说明及代码
2.1 仿真需要的两个子程序 （1）四元数求逆子函数 %四元数求逆函数 function [ qni ] = qiuni( q ) q(1)=q(1); q(2)=-q(2); q(3)=-q(3); q(4)=-q(4); qni=q; end （2）四元数相乘子程序 %四元数相乘计算函数 function [q]=quml(q1,q2); lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4); q=[lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm]*q2; end 2.2 第一种情形：正对导弹进行地面静态测试（导弹质心相对地面静止） （1）用方向余弦矩阵计算，MATLAB 程序如下： function dcm g0=[0;0;9.8];%重力加速度在地里坐标系中的分量表示 wx=15/180*pi; wy=20/180*pi; wz=-10/180*pi; w=sqrt(wx^2+wy^2+wz^2); %四阶近似 I=eye(3); S=1-w^2/6; C=1/2-w^2/24; W=[0,-wz,wy;wz,0,-wx;-wy,wx,0]; c=I+S*W+C*W^2;%载体坐标系到初始坐标系的方向余弦阵 c=inv(c)%初始坐标系到载体坐标系的方向余弦阵 g=c*g0%重力加速度在载体坐标系中的分量 end 在MATLAB命令窗口输入dcm，即得到如下结果： c = 0.9253 0.2129 0.3139 -0.1233 0.9515 -0.2821 -0.3587 0.2223 0.9067
g = -3.5160 2.1791 8.8842 2.3 第二种情形：导弹正在飞行中 MATLAB 程序如下： %主程序
哈工大卫星定位导航原理实验报告
卫星定位导航原理实验专业：班级：学号：姓名：日期：实验一实时卫星位置解算及结果分析一、实验原理实时卫星位置解算在整个GPS接收机导航解算过程中占有重要的位置。
卫星位置的解算是接收机导航解算（即解出本地接收机的纬度、经度、高度的三维位置）的基础。
需要同时解算出至少四颗卫星的实时位置，才能最终确定接收机的三维位置。
对某一颗卫星进行实时位置的解算需要已知这颗卫星的星历和GPS时间。
而星历和GPS时间包含在速率为50比特/秒的导航电文中。
导航电文与测距码（C/A码）共同调制L1载频后，由卫星发出。
本地接收机相关接收到卫星发送的数据后，将导航电文解码得到导航数据。
后续导航解算单元根据导航数据中提供的相应参数进行卫星位置解算、各种实时误差的消除、本地接收机位置解算以及定位精度因子（DOP）的计算等工作。
关于各种实时误差的消除、本地接收机位置解算以及定位精度因子（DOP）的计算将在后续实验中陆续接触，这里不再赘述。
卫星的额定轨道周期是半个恒星日，或者说11小时58分钟2.05秒；各轨道接近于圆形，轨道半径（即从地球质心到卫星的额定距离）大约为26560km。
由此可得卫星的平均角速度ω和平均的切向速度v s为：ω=2π/(11*3600+58*60+2.05)≈0.0001458rad/s (1.1)v s=rs*ω≈26560km*0.0001458≈3874m/s (1.2) 因此，卫星是在高速运动中的，根据GPS时间的不同以及卫星星历的不同（每颗卫星的星历两小时更新一次）可以解算出卫星的实时位置。
本实验同时给出了根据当前星历推算出的卫星在11小时58分钟后的预测位置，以此来验证卫星的额定轨道周期。
本实验另一个重要的实验内容是对卫星进行相隔时间为1s的多点测量（本实验给出了三点），根据多个点的测量值，可以估计Doppler频移。
由于卫星与接收机有相对的径向运动，因此会产生Doppler效应，而出现频率偏移。
哈工大惯性技术(导航原理)大作业
Assignment ofInertial Technology《惯性技术》作业My Chinese NameMy Student ID 15S004001Autumn 2015Assignment 1: 2-DOF response simulationA 2-DOF gyro is subjected to a sinusoidal torque with amplitude of 4 g.cm and frequency of 10 Hz along its outer ring axis. The angular moment of its rotor is 10000 g.cm/s , and the angular inertias in its equatorial plane are both 4 g.cm/s 2. Please simulate the response of the gyro within 1 second, and present whatever you can discover or confirm from the result.In this simulation, we are going to discuss the response of a 2-DOF gyro to sinusoidal torque input. According to the transfer function of the 2-DOF gyro, the outputs can be expressed as:12222()()()()yx y x y x y J Hs M s M s J J s H s J J s H α=-++12222()()()()x yx x y x y J Hs M s M s J J s H s J J s H β=+++ The original system transfer function is a 2-input, 2-output coupling system. But the given input only exists one input, we can treat the system as 2 separate FIFO systemAs a consequence, we can establish the block diagram of the system in simulink in Fig 1.1. Substitute the parameters into the system and input, then we have the input signals as follow: 0,4sin(20)y ox M M t π==Then the inverse Laplace transform of the output equals the response of the gyro in timedomain as follows:0222200020222200()sin sin ()()()cos cos ()()ox a oxa x a x a ox ox a ox a a a a a M M t t t J J M M M t t t H H H ωαωωωωωωωωωβωωωωωωωω=-+--=+---Fig 1.1 The block diagram of the system in simulinkAnd the simulation results in time domain within 1 second are shown in the follwing pictures. Fig1.2 is the the output of outer ring ()t α. Fig1.3 is the output of inner ring ()t β. Fig1.4 is the trajectory of 2-DOF gyro ’s response to sinusoidal input. As we can see from the Fig1.3,there are obvious sawtooth wave in the output of the inner ring. It ’s a unexpected phenomenon in my original theoretical analysis.Fig1.2 The output of inner ring ()t βFig 1.3 The output of outer ring ()t αI believe the sawtooth wave is caused by the nutation. For the frequency of the nutation is obtained as010*******/3974e H rad s Hz J ω===≈, which is far higher than the frequencyof the applied sinusoidal torque , namely0a ωω .Fig 1.4 Trajectory of 2-DOF gyro ’s response to sinusoidal inputThe trajectory of 2-DOF gyro ’s response to sinusoidal input are shown in Fig1.4. As we can see, it ’s coupling of X and Y channel scope output. The overall shape is an ellipse, which is not perfect for there are so many sawteeth on the top of it.Note that the major axis of ellipse is in the direction of the forced procession, amplitude of which is 0ox M H ω, whereas the minor axis is in the direction of the torsion spring effects,with amplitude ox aM H ω.The nutation components are much smaller than that of the forced vibration, which can be eliminated to get the clear static response.22002220()sin sin ()cos (1cos )()ox oxaa x a ox ox oxa a a a a aM M t t t J H M M M t t t H H H αωωωωωωβωωωωωωω≈≈-≈-=--To prove it, we eliminate the effects of the nutation namely the quadratic term in the denominator and get Fig 1.5, which is a perfect ellipse. We can conclude that when input to the 2-DOF gyro is sinusoidal torque, the gyro will do an ellipse conical pendulum as a static response, including procession and the torsion spring effects, together with a high-frequency vibration as the dynamic response.Fig 1.5 Trajectory of the gyro’s response without nutationAssignment 2: Single-axis INS simulation2.1 description of the problemA magnetic levitation train is being tested along a track running north-south. It first accelerates and then cruises at a constant speed. Onboard is a single-axis platform INS, working in the way described by the courseware of Unit 5: Basic problems of INS. The motion informationand Earth parameters are shown in table 2-1, and the possible error sources are shown in Table2-2.Fig2.1 The sketch map of the single-axis INS problemYou are asked to simulate the operation of the INS within 10,000 seconds, and investigate,first one by one and then altogether, the impact of these error sources on the performance of theINS.The block diagram in the courseware might be of some help. However, there lurks aninconspicuous error, which you have to correct before you can obtain reasonable results.There are one core relevant formula, to get the specific form of its solution, we should substitute the unknown parameters.(1)()c N a y A K yga =∆++-Firstly, the input signal is accelerometer of the platform, and the velocity of the platform is the integration of the acceleration.0/p y y ydt yR ω=+=-⎰The acceleration along Yp may contains two parts:cos gsin g yp f y yααα=-≈- When accelerometer errors are concerned, the output of accelerometer will be:(1)N a yp N a K f A =++When gyro errors concerned:'(1)p g p K ωωε=++Onlyαis unknown:000[(1)]tg p t tp t K dt dtααωεϕϕω=+++-∆∆=⎰⎰And the reference block diagram and simulink block diagram are as follows in Fig2.2, Fig2.3. There is a small fault in the reference block, which is that the sign of the marked add operation should be positive instead of negative.The results of the simulation are shown in Fig 2.4 to Fig 2.13.Fig2.2 The reference block diagram in the courseware(unrectified)Fig2.3 The simulink block diagram for Assignment 22.2 results and analysis of the problemFig.2.4 real acceleration,velocity and displacement output without error sourcesKFig2.5 position bias when only accelerometer scale factor error exists0.0001aFig2.6 position bias output when only gyro scale factor error exists 0.0001g K =Fig2.7 position bias when only acceleromete bias error exists 20.0002/N A m s ∆=Fig2.8 position bias output when only initial velocity error exists 200.01/ym s ∆=Fig2.9 position bias output when only initial position error exists 010y m ∆=Fig2.10 position bias output bias when only gyro bias error exists 0.000000024240.00681/3/h rad s ε=︒=Fig2.11 position bias output when only initial platform misalignment angle exists0.000012120''345radα==Fig2.12 output considering all the error sourcesFig2.13 position bias output considering all error sourcesAs we can see in the above simulation results, if there is no error we can navigate the train ’s motion correctly, which comes from north to the south as shown in Fig2.4, beginning with an constant acceleration within 60 seconds then cruises at a constant speed, approximately 65 m/s. However, the situation will change a lot when different errors put into the simulation. The initial position error 010y m ∆= effects least as Fig.2.8, for this error doesn ’t enter into the closed loop and it won ’t influence the iterative process. The position bias is constant and can be negligible.In the second case, when the accelerometer scale factor error exists, 0.0001a K =, as shown in Fig2.5, the result are stable and almost accurate, the position bias is a sinusoidal output. So it is with the accelerometer bias error situation, 20.0002/N A m s ∆=, in Fig2.7, the initialvelocity error, 200.01/ym s ∆= in Fig2.9, and the initial platform misalignment angle, 05''α=, in Fig2.11. However, the influence degrees of the different factors are not in the samemagnitude. The accelerometer scale factor influences the least with magnitude of 5, then the initial velocity larger magnitude of 8, and the accelerometer bias magnitude of 25. The influence of the initial platform misalignment angle is much more significant with a magnitude of 150. All the navigation bias in the second kind case is sinusoidal, which means they ’re limited and negligible as time passes by.In the third case, such as the gyro scale factor error situation, 0.0001g K =, in Fig2.6, and the gyro bias error,0.01/h ε=︒, results in Fig2.10, effects the most significant, the trajectory of the navigation disvergence accumulated as time goes. The position bias is a combination of sinusoidal signal and ramp signal. They also show that the longitudinal and distance errors resulted from gyro drifts are not convergent in time. It means the errors in the gyroscope do most harm to our navigation. And due to the significant influence of the gyro bias errors and the gyroscope scale factor error, results considering all the error sources disverge, and the navigationposition of the motion will be away from the real motion after a enough long time, as shown in Fig2.10. The gyro bias error is the most significant effect factor of all errors. By the time of 10000s, it has reaches 1600m, and it’s nearly the quantity of the position bias considering all error sources. Through contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias.Impression of the Whole simulation experimentThrough contrasting all the results, We can conclude that the gyro bias error is the main component of the whole position bias, and the the gyro bias or the drift error do most harm to our navigation. So it is a must for us to weaken or eliminate it anyway. In spite of all the disadvantages discussed above, the INS still shows us a relatively accurate results of single-axis navigation.Assignment 3: SINS simulation3.1 Task descriptionA missile equipped with SINS is initially at the position of 46o NL and 123 o EL, stationary on a launch pad. Three gyros, GX, GY, GZ, and three accelerometers, AX, AY, AZ, are installed along the axes Xb, Yb, Zb of its body frame respectively.Case 1: stationary testThe body frame of the missile initially coincides with the geographical frame, as shown in the figure, with its pitching axis Xb pointing to the east, rolling axis Yb to the north, and azimuth axis Zb upward. Then the body of the missile is made to rotate in 3 steps:(1) -22o around Xb(2) 78o around Yb(3) –16o around ZbFig 3.1 Introduction to assignment 3After that, the body of the missile stops rotating. You are required to compute the final outputs of the three accelerometers on the missile, using quaternion and ignoring the device errors. It is known that the magnitude of gravity acceleration is g = 9.8m/s2.Case 2: flight tes tInitially, the missile is stationary on the launch pad, 400m above the sea level. Its rolling axis is vertical up,and its pitching axis is to the east. Then the missile is fired up. The outputs of the gyros and accelerometers are both pulse numbers. Each gyro pulse is an angular increment of 0.01arc-sec, and each accelerometer pulse is 1e-7g, with g =9.8m/s2. The gyro output frequency is 200Hz, and the accelerometer’s is 10Hz. The outputs of the gyros and accelerometers within 1315s are stored in a MA TLAB data file named imu.mat, containing matrices gm of 263000× 3 and am of 13150× 3 respectively. The format of the data is as shown in the tables, with 10 rows of each matrix selected. Each row represents the outputs of the type of sensors at each sampling time. The Earth can be seen as an ideal sphere, with radius 6371.00km and spinning rate 7.292× 10-5 rad/s, The errors of the sensors are ignored, so is the effect of height on the magnitude of gravity. The outputs of the gyros are to be integrated every 0.005s. The rotation of the geographical frame is to be updated every 0.1s, so are the velocities and positions of the missile.You are required to:(1) compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the missile.(2) compute the total horizontal distance traveled by the missile.(3) draw the latitude-versus-longitude trajectory of the missile, with horizontal longitude axis.(4) draw the curve of the height of the missile, with horizontal time axis.Fig 3.2 simplified navigation algorithm for SINS 3.2Procedure code3.2.1 Sub function code：quaternion multiply code:function [q1]=quml(q1,q2);lm=q1(1);p1=q1(2);p2=q1(3);p3=q1(4);q1=[lm -p1 -p2 -p3;p1 lm -p3 p2;p2 p3 lm -p1;p3 -p2 p1 lm]*q2;endquaternion inversion code:function [qni] =qinv(q)q(1)=q(1);q(2)=-q(2);q(3)=-q(3);q(4)=-q(4);qni=q;end3.2.2case1 DCM algorithm：function ans11cz=[cos(-22/180*pi) sin(-22/180*pi) 0 ;-sin(-22/180*pi) cos(-22/180*pi) 0;0 0 1]; %The third rotation DCMcx=[1 0 0 ;0 cos(78/180*pi) sin(78/180*pi);0 -sin(78/180*pi) cos(78/180*pi)]; %The first rotation DCMcy=[cos(-16/180*pi) 0 -sin(-16/180*pi);0 1 0;sin(-16/180*pi) 0 cos(-16/180*pi)]; %The second rotation DCM A=cz*cy*cx*[0;0;-9.8]end3.2.3case1 quaternion algorithm：function ans12g=[0;0;0;-9.8];q1=[cos(-11/180*pi);sin(-11/180*pi);0;0]; %The first rotation quaternionq2=[cos(39/180*pi);0;sin(39/180*pi);0]; %The second rotation quaternionq3=[cos(-8/180*pi);0;0;-sin(-8/180*pi)]; %The third rotation quaternionr=quml(q1,q2); %call the quaternion multiplication subfunction q=quml(r,q3);P1=[q(1) q(2) q(3) q(4);-q(2) q(1) q(4) -q(3);-q(3) -q(4) q(1) q(2);-q(4) q(3) -q(2) q(1)];P2=[q(1) -q(2) -q(3) -q(4);q(2) q(1) q(4) -q(3);q(3) -q(4) q(1) q(2);q(4) q(3) -q(2) q(1)];P=P1*P2;gn=P*g;gn=gn(2:4)end3.2.4 case2 SINS quaternion algorithm code：function ans2clc;clear;%parameters initializing：T=0.005;K=13150;R=6371000; %radius of earthwE=7.292*10^(-5); %spinning rate of earthQ=zeros(4, 263001) ;%quaternion matrix initializinglongitude=zeros(1,13151);latitude=zeros(1,13151);H=zeros(1,13151); %altitude matrixQ(:,1)=[cos(45/180*pi);-sin(45/180*pi);0;0]; % initial quaternion longitude(1)=123; %initial longitudelatitude(1)=46; % initial latitudeH(1)=400; %initial altitudelength=0;g=9.8;vE = zeros(1,13151); %eastern velocityvN = zeros(1,13151); %northern velocityvH = zeros(1,13151); %upward velocityvE(1)=0;vN(1)=0;vH(1)=0;load imu.mat %data loading%main calculation sectionfor N=1:Kq1=zeros(4,11);q1(:,1)=Q(:,N);for n=1:20 % Attitude iteration wx=0.01/(3600*180)*pi*gm((N-1)*10+n,1); % Angle incrementwy=0.01/(3600*180)*pi*gm((N-1)*10+n,2);wz=0.01/(3600*180)*pi*gm((N-1)*10+n,3);w=[wx,wy,wz]';normw=norm(w); % Norm calculationW=[0,-w(1),-w(2),-w(3);w(1),0,w(3),-w(2);w(2),-w(3),0,w(1);w(3),w(2),-w(1),0];I=eye(4);S=1/2-normw^2/48;C=1-normw^2/8;q1(:,n+1)=(C*I+S*W)*q1(:,n);Q(:,N+1)=q1(:,n+1);endWE=-vN(N)/R; % rotational angular velocity component of a geographic coordinate systemWN=vE(N)/R+wE*cos(latitude(N)/180*pi);WH=vE(N)/R*tan(latitude(N)/180*pi)+wE*sin(latitude(N)/180*pi);attitude=[WE,WN,WH]'*T; %correction of the quaternion by updating the rotation of geographic coordinatenormattitude=norm(attitude);e=attitude/normattitude;QG=[cos(normattitude/2);sin(normattitude/2)*e];Q(:,N+1)=quml(qinv(QG),Q(:,N+1));fx=1e-7*9.8*am(N,1); %specific force measured by accelerometerfy=1e-7*9.8*am(N,2);fz=1e-7*9.8*am(N,3);Fb=[fx fy fz]';F=quml(Q(:,N+1),quml([0;Fb],qinv(Q(:,N+1)))); %The specific force is decomposed into geographic coordinate system.FE(N)=F(2);FN(N)=F(3);FU(N)=F(4);%calculate the velocity of the vehicle:VED(N)=FE(N)+vE(N)*vN(N)/R*tan(latitude(N)/180*pi)-(vE(N)/R+2*wE*cos(latitude(N)/ 180*pi))*vH(N)+2*vN(N)*wE*sin(latitude(N)/180*pi);VND(N)=FN(N)-2*vE(N)*wE*sin(latitude(N)/180*pi)-vE(N)*vE(N)/R*tan(latitude(N)/180 *pi)-vN(N)*vH(N)/R;VUD(N)=FU(N)+2*vE(N)*wE*cos(latitude(N)/180*pi)+(vE(N)^2+vN(N)^2)/R-g;%integration and get the relative velocity of vehicle:vE(N+1)=VED(N)*T+vE(N);vN(N+1)=VND(N)*T+vN(N);vH(N+1)=VUD(N)*T+vH(N);% integration and get the position of vehicle:longitude(N+1)=vE(N)/(R*cos(latitude(N)/180*pi))*T/pi*180+longitude(N);latitude(N+1)=vN(N)/R*T/pi*180+latitude(N);H(N+1)=vH(N)*T+H(N);length=sqrt((vE(N))^2+(vN(N))^2)+length;endfigure(1) %picture the longitude-latitude curve of the motion within 1315 secondstitle(' longitude-latitude ');hold ongrid onplot(longitude,latitude);figure(2) % picture the altitude curve of the motion within 1315 secondstitle('altitude');hold ongrid onplot(0:13150,H);3.3Simulation outputs and results analysisIn case 1, the DCM algorithm and quaternion results are the same as follow:A =7.53165.9788-1.8892And the results suggest the solution of DCM and quaternion is equivalence in this problem.In case 2, the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis) and the altitude curve of the missile are shown as follows in Fig 3.3 and Fig 3.4.Fig3.3 the latitude-versus-longitude trajectory of the missile(with horizontal longitude axis)Fig3.4the altitude curve of the missileImpression of the Assignment 3In this assignment, we simulate the missile navigation situation in a real problem, as the problem we have done before. I did this based on my own original code, but to my surprise, it’s harder than I have expected. For the detailed thoughts for my program I have forgotten. I have to recheck the code line by line, But I am still troubled in correcting the initial parameters for many times. It has taught me a lesson, which is never to be egotistical for your ability to memory and thework you have done or understood.。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
《导航原理》作业(惯性导航部分)一、题目要求A fighter equipped with SINS is initially at the position of ︒35 NL and ︒122 EL,stationary on a motionless carrier. Three gyros X G ,Y G ,Z G ,and three accelerometers, X A ,Y A ,Z A are installed along the axes b X ,b Y ,b Z of the body frame respectively.Case 1:stationary onboard testThe body frame of the fighter initially coincides with the geographical frame, as shown in the figure, with its pitching axis b X pointing to the east,rolling axis b Y to the north, and azimuth axis b Z upward. Then the body of the fighter is made to rotate step by step relative to the geographical frame.(1) ︒10around b X (2) ︒30around b Y (3) ︒50-around b ZAfter that, the body of the fighter stops rotating. You are required to compute the final output of the three accelerometers on the fighter, using both DCM and quaternion respectively,and ignoring the device errors. It is known that the magnitude of gravity acceleration is 2/8.9g s m =.Case 2:flight navigationInitially, the fighter is stationary on the motionless carrier with its board 25m above the sea level. Its pitching and rolling axes are both in the local horizon, and its rolling axis is ︒45on the north by east, parallel with the runway onboard. Then the fighter accelerate along the runway and take off from the carrier.The output of the gyros and accelerometers are both pulse numbers,Each gyro pulse is an angular increment of sec arc 1.0-,and each accelerometer pulse isg 6e 1-,with 2/8.9g s m =.The gyro output frequency is 10 Hz,and the accelerometer’sis 1Hz. The output of gyros and accelerometers within 5400s are stored in MATLAB data files named gout.mat and aout.mat, containing matrices gm of 35400⨯ and am of 35400⨯ respectively. The format of data as shown in the tables, with 10 rows of each matrix selected. Each row represents the out of the type of sensors at each sample time.The Earth can be seen as an ideal sphere, with radius 6368.00km and spinning rate rad/s 10292.75-⨯, The errors of sensors are ignored, so is the effect of height on the magnitude of gravity. The output of the gyros are to integrated every 0.1s. The rotation ofgeographical frame is to be updated every 1s, so are the velocities and position of the figure. You are required to:(1)Compute the final attitude quaternion, longitude, latitude, height, and east, north, vertical velocities of the fighter.(2)Compute the total horizontal distance traveled by the fighter.(3)Draw the latitude -versus -longitude trajectory of the fighter, with horizontal longitude axis.(4)Draw the curve of the height of fighter, with horizontal time axis.二、Case1解答2.1 方向余弦阵法（1） 绕Xb 轴转过ψ︒=10ϕ⎪⎪⎪⎭⎫ ⎝⎛︒︒-︒︒=⎪⎪⎪⎭⎫ ⎝⎛-=10cos 10sin 010sin 10cos 0001cos sin 0sin cos 0001ϕϕϕϕϕC（2） 绕Yb 轴转过︒=30θ⎪⎪⎪⎭⎫ ⎝⎛︒︒︒-︒=⎪⎪⎪⎭⎫ ⎝⎛-=30cos 030sin 01030sin 030cos cos 0sin 010sin 0cos θθθθθC（3） 绕Zb 轴转过︒-=50ψ⎪⎪⎪⎭⎫ ⎝⎛︒-︒-︒-︒-=⎪⎪⎪⎭⎫ ⎝⎛=1000）50（cos ）50（sin -0）50（sin ）50（cos 1000cos sin -0sin cos ψψψψψC 所以变换后的坐标X E Y C C C N Z ϕθψζ⎛⎫⎛⎫ ⎪⎪= ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭由于初始时刻有009.8E N ζ⎛⎫⎛⎫ ⎪ ⎪= ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭所以计算得(Z Y X )=( 3581.86027.2-4054.4-)三个加速度计的输出分别是 ,/4504.42x s m A -=,/6027.22y s m A -=s m A /3581.8z =2计算程序见附录一2.2 四元数法（1）绕Xb 轴转过ψ︒=10ϕ i 2sin2cos q ϕϕϕ+=（2）绕Yb 轴转过︒=30θj 2sin2cosq θθθ+=（3）绕Zb 轴转过︒-=50ψk 2sin2cosq ψψψ+=则合成四元数)2sin 2(cos )2sin 2(cos )2sin 2(cosk j i q q q q ⋅+⋅+⋅+==ψψθθϕϕψθϕ合成四元数的逆1123q p i p j p k λ-=---由公式 q N E qZ YX ⎪⎪⎪⎭⎫ ⎝⎛=⎪⎪⎪⎭⎫⎝⎛-ξ1计算得 (Z Y X )=(3581.86027.2-4054.4-)三个加速度计的输出分别是 ,/4504.42x s m A -=,/6027.22y s m A -=s m A /3581.8z =2由两种计算方法的计算结果可以看出，方向余弦阵法和四元数法的计算结果是一致的。
计算程序见附录二三、Case2解答3.1程序流程图3.2 源程序详见附录三 3.3 仿真及结果3.3.1 运行程序可得：（1）战斗机相对当地地理坐标系的姿态四元数为：[]4841.00023.00054.08750.0----=Q（2）经度= ︒8757.122纬度=︒35.0694高度=︒8061.24（3）战斗机的东部，北部和垂直速度s m V /7716.393e -=s m V /5612.267n = s m V /1583.3u -=（4）战斗机总水平距离为：m 103822.26⨯3.3.2运行程序，绘制纬度-经度战斗机轨迹（以经度为横轴，纬度为纵轴）；如图：3.3.3 运行程序，绘制战斗机的高度的曲线（以时间为横轴，高度为纵轴），如图：四、心得体会通过本次的大作业，我对比力、四元数等惯性导航技术中涉及的基本概念有了更深的理解，同时再次复习方向余弦矩阵法和四元数法求解捷联惯导系统，更加熟练地掌握其算法。
除此之外，本次大作业中所有程序均要涉及Matlab编程，在编写程序时，四元数的初始化非常重要，如果四元数的初始化错误，会导致所有的运算结果错误。
在编程过程中，每当遇到问题，我就会上网查一些资料，并与周边同学进行深入讨论，学习并解决问题。
这进一步提高了我的自学能力和对知识综合应用的能力。
五、参考/协助确认本文是根据本人研究成果，并参考相关文献资料整合而成，非本人完全独立研究完成，编程部分很大程度上参考了百度百科及周边同学的研究成果。
另外，报告形式上借鉴了上届学长学姐的形式。
本人对作业中的某些结论不负完全责任，特此声明。
C1=[1,0,00,cos(10/180*pi),sin(10/180*pi)0,-sin(10/180*pi),cos(10/180*pi)]; %第一次转动方向余弦阵C2=[ cos(30/180*pi),0,-sin(30/180*pi)0,1,0sin(30/180*pi),0,cos(30/180*pi)];%第二次转动方向余弦阵C3=[ cos(-50/180*pi),sin(-50/180*pi),0-sin(-50/180*pi),cos(-50/180*pi),00,0,1];%第三次转动方向余弦阵A=C3*C2*C1*[0;0;9.8] %计算转动后加速度的输出norm(A) %检验输出是否正确附录二q1=[cos(10/360*pi);0;0;sin(10/360*pi)];%第一次转动四元数q2=[cos(30/360*pi);sin(30/360*pi);0;0];%第二次转动四元数q3=[cos(-50/360*pi);0;sin(-50/360*pi);0];%第三次转动四元数r=quml(q1,q2);%计算四元数乘积q=quml(r,q3);A1=[q(1) q(2) q(3) q(4)-q(2) q(1) q(4) -q(3)-q(3) -q(4) q(1) q(2)-q(4) q(3) -q(2) q(1)]A2=[q(1) -q(2) -q(3) -q(4)q(2) q(1) q(4) -q(3)q(3) -q(4) q(1) q(2)q(4) q(3) -q(2) q(1)]A=A1*A2;%G=A*[0;0;0;9.8] %计算转动后的弹体加速度计的输出附录三%已知参数k=10;T=1;Gyro_pulse=0.1/3600/180*pi;Acc_pulse=9.8/1000000;R=6368000;g=9.8;W_earth=0.00007292;%初始导航参数和地球参数Q=zeros(5401,4); %定义变换四元数矩阵Q(1,:)=[cos((-45/2)/180*pi),0,0,sin((-45/2)/180*pi)]; Longitude=zeros(1,5401); %定义长度为5400的经度数组Latitude=zeros(1,5401); %定义长度为5400的纬度数组Height=zeros(1,5401); %定义长度为5400的高度数组Longitude(1)=122; %初始化经度，纬度，高度Latitude(1)=35;Height(1)=25;%定义速度矩阵，供画图用Ve=zeros(1,5401); %东向速度Vn=zeros(1,5401); %北向速度Vu=zeros(1,5401); %天向速度X=zeros(1,5401);%定义加速度矩阵fe=zeros(1,5400);fn=zeros(1,5400);fu=zeros(1,5400);FE=zeros(1,5400);FN=zeros(1,5400);FU=zeros(1,5400);load gout.mat;load aout.mat;for N=1:K %位置、速度迭代q=zeros(11,4);q(1,:)=Q(N,:);for n=1:k %姿态迭代w=Gyro_pulse*gm((N-1)*10+n,1:3);w_mod=quatmod([w,0]);W=[0,-w(1),-w(2),-w(3);w(1),0,w(3),-w(2); w(2),-w(3),0,w(1); w(3),w(2),-w(1),0];I=eye(4);S=1/2-w_mod^2/48;C=1-w_mod^2/8+w_mod^4/384;q(n+1,:)=((C*I+S*W)*q(n,:)')';endQ(N+1,:)=q(n+1,:);%地理坐标系的转动角速度分量WE=-Vn(N)/R;WN=Ve(N)/R+W_earth*cos(Latitude(N)/180*pi);WU=Ve(N)/R*tan(Latitude(N)/180*pi)+W_earth*sin(Latitude(N)/180*pi );Gama=[WE,WN,WU]*T; %用地理坐标系的转动四元数Gama_mod=quatmod([WE,WN,WU,0]);n=Gama/Gama_mod;Qg=[cos(Gama_mod/2),sin(Gama_mod/2)*n];Q(N+1,:)=quatmultiply(quatinv(Qg),Q(N+1,:));%加速度计测得的比力fb=Acc_pulse*am(N,1:3);f1=quatmultiply(Q(N+1,:),[0,fb]);fg=quatmultiply(f1,quatinv(Q(N+1,:)));fe(N)=fg(2);fn(N)=fg(3);fu(N)=fg(4);%计算载体的相对加速度FE(N)=fe(N)+Ve(N)*Vn(N)/R*tan(Latitude(N)/180*pi)-(Ve(N)/R+2*W_ea rth*cos(Latitude(N)/180*pi))*Vu(N)+2*Vn(N)*W_earth*sin(Latitude(N )/180*pi);FN(N)=fn(N)-2*Ve(N)*W_earth*sin(Latitude(N)/180*pi)-Ve(N)^2/R*tan (Latitude(N)/180*pi)-Vn(N)*Vu(N)/R;FU(N)=fu(N)+2*Ve(N)*W_earth*cos(Latitude(N)/180*pi)+(Ve(N)^2+Vn(N )^2)/R-g;%积分计算载体的相对速度Ve(N+1)=FE(N)*T+Ve(N);Vn(N+1)=FN(N)*T+Vn(N);Vu(N+1)=FU(N)*T+Vu(N);%积分计算载体的位置Latitude(N+1)=(Vn(N)+Vn(N+1))/2*T/R/pi*180+Latitude(N);Longitude(N+1)=(Ve(N)+Ve(N+1))/2*T/(R*cos(Latitude(N)/180*pi))/pi *180+Longitude(N);Height(N+1)=(Vu(N)+Vu(N+1))/2*T+Height(N);%computing the distance of horizonXe=(Ve(N)+Ve(N+1))/2*T;Xn=(Vn(N)+Vn(N+1))/2*T;X(N+1)=X(N)+sqrt(Xe^2+Xn^2);End%绘制经、纬度变化曲线（以经度为横轴，纬度为纵轴）；figure(1);xlabel('经度/度');ylabel('纬度/度');grid on;hold on;plot(Longitude,Latitude);%绘制高度变化曲线（以时间为横轴，高度为纵轴)figure(2);xlabel('时间/秒');ylabel('高度/米');grid on;hold on;plot((0:5400),Height);%绘制总水平位移曲线figure(3);xlabel('时间/秒');ylabel('航程/米');grid on;hold on;plot((0:5400),X)。