惯性导航作业

惯性导航作业一、数据说明：1：惯导系统为指北方位的捷连系统。

初始经度为116.344695283度、纬度为39.975172度,高度h为30米。

初速度v0=[-9.993908270;0.000000000;0.348994967]。

2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。

3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc(单位m/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为一~三行分别为X、Y、Z向信息.4: 航向角以逆时针为正。

5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat纬度);二、作业要求：1：可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。

用角增量法。

2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图；(2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）；(3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）；以上结果均要附在作业报告中。

3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。

(注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序) (学习小结按条写，不用写套话) 4：作业以纸质报告形式提交，附源程序。

惯性导航岗位职责惯性导航岗位职责：惯性导航系统（Inertial Navigation System，简称INS）是一种基于惯性原理的导航系统，能够通过测量加速度和角速度的变化来确定飞行器的位置和运动状况。

在军事和民用航空领域中广泛使用，惯性导航工程师是负责设计、开发和维护惯性导航系统的专业人士。

下面是该岗位职责的详细描述：1. 设计惯性导航系统惯性导航工程师需要根据客户需求或项目要求，设计符合要求的惯性导航系统。

首先需要进行系统需求分析，确定设计目标和技术细节。

其次需要选择合适的传感器、计算单元和接口，进行系统硬件设计。

最后需要编写软件程序，实现系统的数据处理和控制功能。

2. 进行系统测试和优化设计好惯性导航系统之后，需要进行系统测试和优化。

这包括模拟环境下的系统测试和实际场景下的验证。

工程师需要对系统进行各种测试和分析，确定系统的精度、稳定性和可靠性等指标。

对测试结果进行分析和优化，确保系统满足客户需求和项目要求。

3. 维护和升级惯性导航系统惯性导航系统随着使用时间的推移和环境变化，可能会出现各种故障和问题。

工程师需要对系统进行维护和修复，确保系统能够正常运行。

同时，随着技术的不断发展，惯性导航系统也需要不断升级和改进。

工程师需要跟踪最新技术和市场趋势，为系统升级提供技术支持和建议。

4. 提供技术支持和培训惯性导航工程师需要与客户和团队密切合作，提供技术支持和解决方案。

对于一些复杂的问题，需要深入研究和解决。

此外，还需要为客户和团队提供相关的技术培训，帮助他们更好地使用和维护惯性导航系统。

总之，惯性导航工程师是惯性导航系统开发和运营的核心人才，需要具备扎实的技术功底和对航空领域的深刻理解。

他们需要设计、测试、维护和升级惯性导航系统，为客户提供技术支持和培训，确保系统能够稳定、高效、安全地运行。

导航系统1．简述捷联惯性系统中地理系到机体系的姿态阵bg C 其含义及其功能。

答:含义:导航坐标系g g g O x y z -到机体坐标系b b b O x y z -的一组欧拉角为,,θγψ，导航坐标系经过3次转动到机体坐标系。

g g g x y z 依次沿g O z -、'b O x -、''b O y -旋转角度-ψ、θ、γ后到b b b x y z 。

姿态矩阵中包含了机体的姿态角方位角ψ、俯仰角θ和横滚角γ。

功能：机体陀螺仪输出的角速度信息经过补偿后,积分得到机体坐标系与导航坐标系的姿态信息和姿态转移矩阵。

捷联惯导系统中,加速度计与载体固连,利用姿态阵完成加速度计输出信息从机体坐标到导航坐标的转换。

转换后的加速度计信息经过积分可得到机体在导航坐标系下的速度和位置。

2．画出并用式表达速度三角形（地速、控速、风速)及航迹角、航向角与偏流角之间的关系.答：风速：空气相对于地面的运动速度;空速:飞机相对于空气运动的速度；地速：飞机相对于地面的运动速度。

=+v v v 风地空航向角：机头在水平面投影与真北方向的夹角ϕ;偏流角:空速矢量和地速矢量之间的夹角,用δ表示；航迹角:飞机速度矢量在水平面投影与真北方向的夹角。

航向角ϕ加上偏流角δ等于地速v 地的方位角α。

v 地v 空v 风3．简述惯性导航系统、卫星导航系统、多普勒导航、塔康、VOR/DME 、天文导航其各自的基本工作原理、特点及误差特性。

答:一、惯性导航系统（1）工作原理以牛顿力学定律为基础，以陀螺仪和加速度计为敏感器件进行导航参数解算。

系统根据陀螺仪的输出建立导航坐标系，根据加速度计输出解算出运载体的速度和位置，从而实现姿态和航向解算. （2）特点惯性导航系统不需要任何外来信息，也不会向外辐射任何信息，仅依靠惯性器件就能全天候，全球性的自主三维定位和三维定向,同时具备自主性、隐蔽性和信息的完备性。

（3）误差特性误差随时间积累，短时间导航精度较高。

惯性导航技术在物理实验中的应用指南导语：在物理实验中，准确测量和记录物体的位置、速度和加速度是非常重要的。

惯性导航技术作为一种先进的测量方法，广泛应用于物理实验中，可以提供高精度和高稳定性的测量结果。

本文将为您介绍惯性导航技术在物理实验中的应用指南，帮助您更好地使用和理解该技术。

一、惯性导航技术概述惯性导航技术是利用惯性传感器来测量和跟踪物体的运动状态的一种方法。

主要包括3个方面的测量参数：加速度、角速度和姿态。

其中，加速度计可以测量物体在各个方向上的加速度变化；陀螺仪可以测量物体的角速度变化；磁强计可以测量物体的姿态变化。

这些测量参数可以通过导航算法计算出物体的位置和速度。

二、物理实验中的应用1. 弹道实验在弹道实验中，惯性导航技术可以用于测量炮弹的飞行轨迹。

通过陀螺仪测量炮弹的角速度变化，可以计算出炮弹的旋转角度和飞行方向。

结合加速度计和磁强计的测量结果，可以精确测量炮弹的速度和位置。

这些测量结果对于炮弹的准确定位和击中目标至关重要。

2. 自由落体实验在自由落体实验中，惯性导航技术可以用于测量物体下落过程中的加速度和速度。

通过加速度计测量物体的加速度变化，可以计算出重力加速度和物体的下落高度。

结合陀螺仪的测量结果，可以计算出物体的初始速度和离地点的时间。

这些测量结果可以帮助我们更好地理解自由落体物理学的规律。

3. 运动轨迹分析在运动轨迹分析实验中，惯性导航技术可以用于跟踪物体的位置和姿态变化。

通过磁强计测量物体的姿态变化，可以计算出物体的角度和方向。

结合陀螺仪和加速度计的测量结果，可以计算出物体的速度和加速度。

这些测量结果可以帮助我们更好地分析物体的运动轨迹和变化规律。

三、惯性导航技术的优势和挑战惯性导航技术具有高精度和高稳定性的优势，可以提供准确的测量结果。

同时，由于惯性传感器内部不需要外部参考，不受环境条件的限制，适用于各种复杂的实验场景。

然而，惯性导航技术也存在一些挑战，如漂移、噪声和精度损失等问题。

实验三惯性导航实验小组成员：杨曦陈魁吴航杨少帅一、 实验目的1、了解惯性导航设备;2、掌握惯性导航设备的物理连接；3、掌握惯性导航信息的处理方法；4、掌握惯性导航方法并学会用编程实现惯性导航算法。

二、 实验器材YH-5000AHRS ；工业控制计算机；数据采集软件； 稳压电源；串口连接线；三、 实验原理（1） 姿态解算基于四元数法解算姿态矩阵。

p j p i p l Q +++=21 （1）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡--++----+++---+=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡b b b p b b b b p p p z y x C z y x p p p l lp p p lp p p lp p p p p p l lp p p lp p p lp p p p p p l z y x 222123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2 （2） b pbQw Q 21= （3） 上述微分方程表示成矩阵形式：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡------=⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡321321000021p p p l w w w w w w w w w w w w p p p l b pbxb pbyb pbz b pbx b pbz b pbyb pby b pbz b pbxb pbz b pbyb pbx（4） 初始四元数的确定计算如下：⎥⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡--++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡2c o s 2c o s 2s i n 2s i n 2s i n 2c o s 2c o s 2s i n 2s i n 2s i n 2c o s 2c o s 2s i n 2c o s 2s i n 2c o s 2s i n 2c o s 2s i n 2s i n 2s i n 2c o s 2c o s 2co s )0(3)0(2)0()0(0000000000000000000000001γθϕγθϕγθϕγθϕγθϕγθϕγθϕγθϕG G G G G G G G p p p l （5） 用四阶龙格库塔法解（4）的微分方程；⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=333231232221131211T T T T T T T T T C p b 由p b C 中提取γϕλ，，G231sin T -=主λ 22211tan T T G -=主ϕ)(tan 33131T T --=主γ 从而可得到：主λλ=⎪⎩⎪⎨⎧<>+>><+=0,020,002122212222T T T T T GG G G πϕϕπϕϕ主主主⎩⎨⎧<>=0,)(-0331333T T sign T πγγγ （2） 速率位置解算将加速度测量的沿坐标系轴向的比力bib a 转换成沿着导航坐标系轴向的比力p ib a ,则速度方程为:p p epp ep p ib p ep g V w a V +⨯+Ω-=)2( 展开得到：⎪⎩⎪⎨⎧-+Ω-+Ω+=+Ω+Ω-=+Ω-Ω+=gV w V w a V V w V a V V w V a V p epy p epx p x p epx p epy p y p ibz pepz p epzp epx p x p epx p z p iby p epy p epzp epy p y p epy p z p ibx p epx )2()2()2(2)2(2 由于Ω,pep w 都很小，故而速度方程简化为：⎪⎩⎪⎨⎧-===ga V a V a V p ibz pepz piby p epy p ibxp epx用一阶欧拉法解，则：⎪⎩⎪⎨⎧+-=++=++=+)(*)()()(*)()(*)(t V T g a T t V t V T a T t V t V T a T t V p epz p ibz p epzpepy p iby p epy pepx p ibx p epx 其中T 为采样时间。

惯性导航仪的工作原理引言概述：惯性导航仪是一种用于飞行器、舰船、导弹等载具的导航设备，它能够通过测量载具的加速度和角速度来确定载具的位置、速度和方向。

惯性导航仪具有高精度、独立性强等优点，被广泛应用于航空航天领域。

一、惯性导航仪的基本原理1.1 惯性导航仪的加速度测量原理惯性导航仪内置加速度计，通过测量载具的加速度来确定载具的运动状态。

当载具发生加速度变化时，加速度计会产生相应的电信号，进而计算出载具的加速度值。

1.2 惯性导航仪的角速度测量原理惯性导航仪内置陀螺仪，通过测量载具的角速度来确定载具的旋转状态。

陀螺仪会产生相应的电信号，用于计算载具的角速度值。

1.3 综合加速度和角速度信息惯性导航仪会综合加速度和角速度信息，通过积分计算出载具的位置、速度和方向，从而实现导航功能。

二、惯性导航仪的误差补偿原理2.1 零偏误差补偿惯性导航仪存在零偏误差，需要进行零偏误差补偿。

通过定期校准零偏误差，可以提高导航仪的准确性。

2.2 温度漂移补偿惯性导航仪的性能会受到温度的影响，需要进行温度漂移补偿。

通过传感器内部的温度补偿电路，可以减小温度对导航仪的影响。

2.3 震动干扰抑制惯性导航仪在运动过程中会受到震动干扰，需要进行震动干扰抑制。

通过滤波算法和信号处理技术，可以减小震动对导航仪的影响。

三、惯性导航仪的工作模式3.1 静态模式在载具停止运动时，惯性导航仪处于静态模式。

此时，导航仪主要通过加速度计和陀螺仪测量载具的姿态和位置。

3.2 动态模式在载具运动时，惯性导航仪处于动态模式。

此时，导航仪主要通过积分计算出载具的位置、速度和方向。

3.3 切换模式惯性导航仪可以根据载具的运动状态自动切换不同的工作模式，以确保导航的准确性和稳定性。

四、惯性导航仪的应用领域4.1 航空领域惯性导航仪被广泛应用于飞机、直升机等航空器上，用于实现飞行导航和飞行控制。

4.2 舰船领域惯性导航仪也被应用于舰船上，用于实现航行导航和姿态控制。

惯导作业一、填空题1.惯性导航系统是一种不依赖任何外部信息、也不向外部辐射能量的______导航系统。

答案：自主式2.不依赖外界信息，只靠对载体本身的______、来完成导航任务的技术称做惯性导航，也称为自主式导航答案：惯性测量3. 加速度计其输出一般是______、，但在积分加速度计的情况下则输出为______、。

答案：速度、加速度4. 惯性器件就是测量载体______、和______、参数的传感器。

答案：线运动、角运动5. 加速度经过一次积分可以得到______，经过二次积分得到______。

答案：运动速度、运动距离6. 描述角运动的参数有______、______。

答案：姿态角、姿态角速度7. 描述线运动的参数有______、______、______。

答案：位移、速度、加速度8. 高速旋转的自由陀螺仪，当不受外力矩作用时，其主轴将保持它在空间的______方向不变。

答案：初始9. 由表观运动所引起的陀螺______偏离当地地垂线的误差，称之为陀螺仪的“表观误差”。

答案：自转轴二、单选题1．陀螺自转轴方向相对惯性空间保持不变，以地球作为参考基础，陀螺自转轴相对地球表面的转动，为（）。

A．表观运动B．自转运动C．定轴运动D．进动运动答案：A三、多选题（每题1分）1．惯性导航系统的核心有（）A．加速度计、B．陀螺仪C．导航计算机D．GPS答案：ABC2．惯性导航系统的基本组成（）A．加速度计B．模拟某一坐标系的惯性平台C．导航计算机D．控制显示器答案：ABCD3．激光陀螺特点有哪些（）。

A．抗干扰能力弱B．启动快C．动态特性较宽D．稳定性好答案：BCD4.关于组合导航系统，下列说法正确的是（）。

A．提高导航系统的精度B．提高导航系统的可靠性C．提高导航系统的安全性D．启动快答案：ABC四、判断题1. 一个沿直线运动的载体，只要借助于加速度计测出它的加速度，那么，载体在任何时刻的速度和相对出发点的距离就可以实时地计算出来。

《惯性导航原理》第一次大作业一、 原理分析惯导系统为指北方位的平台系统，则利用比力方程以及陀螺提供的东、北、天三个比力数据，即可计算得到在每个数据采集点的平台即时速度，再通过经纬度的计算公式，就可以得到每个数据采集点平台的即时经纬度，以每个数据采集点为下一个采集点的起点，即可对速度和经纬度进行累计计算，从而得到平台在运动过程中任意时刻的速度和位置情况。

运动过程中任意时刻的速度和位置情况。

1.模型公式的推导载体相对地球运动时，载体相对地球运动时，加速度计测得的比力表达式，加速度计测得的比力表达式，加速度计测得的比力表达式，称为比力方程，称为比力方程，称为比力方程，方程如方程如下：下：g V V f epep ieep-´++=)2(vv （1）在指北方案中，平台模拟地理坐标系，将上式中平台坐标系用地理坐标系代入得：入得：t tt ett iettgV f V+´+-=)2(v v （2）系统中测量的是比力分量，将上式写成分量形式系统中测量的是比力分量，将上式写成分量形式=-+ （3） 又因为地球的自转角速率为：又因为地球的自转角速率为：(4)地理坐标系相对于地球坐标系的角速率为：地理坐标系相对于地球坐标系的角速率为：= (5)将（将（44）（5）两个式子带入（）两个式子带入（33）式，即可得到如下方程组：）式，即可得到如下方程组：（6）2.速度计算作业要求只考虑水平通道，作业要求只考虑水平通道，因此只需要计算正东、因此只需要计算正东、因此只需要计算正东、正北两个方向的速度即可。

正北两个方向的速度即可。

正北两个方向的速度即可。

理理论上计算得到t x V 、t y V 后，再积分一次可得到速度值，即后，再积分一次可得到速度值，即ïîïíì+=+=òòt t y t y t ytt x t x tx V dt V V V dt V V 000但在本次计算过程中，三个方向的速度均是从零开始在各时间节点上的累加，并不是t的函数，因此速度计算可以由以下方程组实现：（7）此方程组表示了从第i 个采集点到第（个采集点到第（i+1i+1i+1）个采集点的速度递推公式。

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.。