INS-Toolbox2003

Inertial Navigation System TOOL BOX 惯性导航工具箱 Inertial Navigation System TOOLBOX For Matlab For Windows 用户手册 目 录 31.安装说明 42 指南 验证指南下载验证指南下载验证指南下载星度指南下载审查指南PDF 41.介绍 5航迹/飞行器动力学仿真 5导航用坐标系 6简化的3D惯性导航 7地球体-加速度漂移 8旋转地球体-科氏漂移和加速度计漂移 9陀螺仪漂移 12初始化速度误差 14初始化姿态角 16陀螺仪噪声 18地球大圆轨迹产生与显示 19地球大圆轨迹的惯性导航 22游动方位角系统 26参考资料： 273. 参 考 27介 绍 29bodupdat 30coriolis 31crafrate 33dcm2eulr 33dcm2llw 34dcm2qua 35dcmelgen 36dcmnbgen 36earthrot 37enu2xyz 38eulr2dcm 38eulr2qua 39extrapol 39gendelcr 40gendthet 41gendv 42gendvcor 43gendverr 43gentherr 44genvelpr 45gravity 46greatcir 47interpol 47lclevupd 48llh2xyz 49llw2dcm 50navupdat 50pathplot 51plotwrld 51progen 53propitch 53proroll 54prostrai 55proturn 56qua2dcm 56quamult 57quaupdat 58radicurv 58skewsymm 59velupdat 60xyz2enu 61xyz2llh 1.安装说明 对matlat5.x： 本手册附带的磁碟包含参考和应用文件。建议在目录c:\matlat\toolbox下新建一个ins的子目录。然后将磁碟上所有内容拷贝到ins目录下。 要完成安装，将文件pathdef.m备份，然后编译文件，将ins子目录添加到Matlab路径下。这能让你在不改变当前目录的情况下访问INS工具箱。保存更新过的pathdef.m以后，再启动MATLAB时，INS工具箱就有效了。 因计算量大，使用旧的CPU会导致很多程序运行缓慢，所以建议使用133MHZ或更好的CUP。 安装完成后，继续参阅指南部分。 2指南 1.介绍 在20世纪50年代，卫星导航正处于 发展初期，而惯性导航很快发展成熟起来。随着陀螺仪和加速度计的发展，惯性引导下的远程导航开始在军事方面得到广泛应用，并最终发展到民用航天领域。 惯性系统到今天仍在不断发展。捷联技术是惯性的最常见表现形式，小型惯性测量元件是当今测量和发展的热门话 题 快递公司问题件快递公司问题件货款处理关于圆的周长面积重点题型关于解方程组的题及答案关于南海问题 。卫星定位导航并不像之前人们所预测的那样，将惯性取而代之，而是和其成为一个整体形成了一个更加实用的系统。这也就是现今这里研究的的惯性导航系统。 对于任何一个学过中学物理的人，惯性定位的最基本概念都是很简单的。对加速度求两次积分就可以得到位移。然而要真正让一个实际系统运作起来，事情就远远不是那么简单了。即使有陀螺仪和加速计，用来处理真实测量数据的运算法则并不能解决什么问题。 有一些很好的解释这些算法的文章（参见本指南末的资料部分）。但是，我们不能仅仅通过书本中就获得对惯导的一个全面的认识。实际应用惯性仪器并处理数据是个很好的途径，但是也存在问题。因为一般使用设备和处理大量数据会比研究和练习 公式 小学单位换算公式大全免费下载公式下载行测公式大全下载excel公式下载逻辑回归公式下载 耗费更多的时间。显然在台式电脑上模拟其过程（包括测试）就很有用了。 该惯性导航系统工具箱（INS）就是专门为这个目的 设计 领导形象设计圆作业设计ao工艺污水处理厂设计附属工程施工组织设计清扫机器人结构设计 的。这个工具箱可以用来模拟仿真载体动力学，处理原始测量值（加速计上的delta-V数据和陀螺仪上的delte-theta数据）和数据。 需要注意，本工具书里INS为是捷联类型。 航迹/飞行器动力学仿真 首先我们先来看看一个简单的飞行路径和其相应的飞行动力学是如何模拟的。 上图是演示程序insdem01产生的,称为产生一条飞行轨迹的progen功能，能同时产生位置、速度、加速度和物体角度。你可以运行m文件dem01plt来看相应产生的速度、加速度和欧拉角曲线。使用progen可以指定直线、爬升、降落和转弯部分。该功能默认了一个坐标系为x轴：东；y轴：北；z轴：上。即为ENU。 导航用坐标系 先要指出INS工具箱中用到的一些不同的系统。 惯性坐标系——Inertial Frame(I-frame) 惯性坐标系是一个以地心为坐标原点，北极轴线方向为z轴的笛卡儿坐标系。x轴和y轴位于赤道平面。但是它们并不旋转，而是依据太阳和星系来定位。在某些书里惯性坐标系就简单的以字母“i”来表示。 地球坐标系——Earth Frame(E-frame) 地球坐标系原点为地球中心，x轴穿过地心及赤道和零子午线的交点，z轴指向北极。在某些书里地球坐标系以字母“e”表示。 载体坐标系——Body Frame（b系） 载体坐标系原点为载体之质心，X轴指向载体前端，Y轴沿载体的纵轴方向，Z轴指向载体纵轴的右侧。绕这些轴正转会分别产生正滚动、俯仰和偏航。 导航坐标系——Navigation Frame（n系） 导航坐标系是以载体为中心的笛卡儿坐标系，但并不固定在载体内。x轴和y轴通常在本地飞机里，z轴沿本地垂直方向。x轴和y轴的确切方向取决于要求的惯性机械运算法则。在north-slaved一例中，x轴指北，y轴指东。通常称此为东北下（NED）。另外，注意载体坐标系有时记为nav-frame，有些文章里记为‘N’或‘n’。 本地坐标系——Local-Level Frame（L系） 本地系和导航系基本相同，只是导航系指北东地，而本地系指东北天。有时记为ENU。另外，注意本地系有时简记为‘L’或‘l’。 简化的3D惯性导航 回顾一下，我们要对飞行轨迹做一个简单的INS仿真演示。第一步是用该轨迹产生系列的加速度计信号（delta-V's）和一系列陀螺仪信号(delta-theta's)。一旦数据产生后，惯性处理法则就可以应用了。（参见演示程序insdemo02） 为了开始简单点，我们可以假设地球是平的且不自转。另外还要忽略重力（注意：在之后的仿真中会再来考虑这些因素的）。加速计和陀螺仪只测量主体（如飞行器载体）运动。gendv和gendthet用来分别产生delta-V和delta-theta数据。 如在惯性导航中经常要求的，所需参数（位置、速度和姿态）必须初始化。虽然在实际中这些是参数是估计值，在这里我们用真值对它们进行初始化。 在本文中，使用陀螺仪是为了确定飞行姿势，同时确定相对于惯性坐标系的载体坐标系中的方位。载体系到惯性系的方向余弦矩阵（DCMbn）的更新是根据先前矩阵的数值和当前delta-theta的测量值，通过函数bodupdat来实现的。 更新载体运动姿态后，当前的delta-V测量值能从载体坐标系转换到惯性坐标系（通过矩阵DCMbn），然后再通过矩阵C转换到本地系。得到现在本地坐标系里的delta-V's后，可以很容易得到当前速度，由速度可以积分进行位置更新。 运行程序得到的结果看起来象产生的原始轨迹。这是在给定理想初值同时没有模拟传感器误差的情况下可以得到的。在实际中因为不理想的数据处理尤其是不精确积分，使得上面两者存在轻微的不同。由于这些不同不明显，根据上面图形显示是可以忽略不计的。这些误差可以通过更密集的采样来减小。（见例insdem03） 四元数也可以用来进行载体运动姿势的方向余弦矩阵的更新。（运行insdem04可以得到相应演示） 地球体-加速度漂移 insdem05演示提出了考虑重力的地球体。这是对当存在加速度计漂移而得到的舒勒振动的有效单信道仿真。在该仿真中，物体处于静态且位于北纬45度，东经45度。模拟得到一个100μg北加速度计漂移。 根据理论[3]，以加速度计漂移值为参数的速度误差是一个峰值为每μg 0.026 f/s的正弦曲线。 对于每100μg，最大速度误差应大约为2.6f/s（即0.8m/s）。在图中观察得到此情况。另外，根据原理位置误差应成呈现从零到最大值为每μg0.007海里的正弦曲线。在这里峰值应为0.7海里，即约为42角秒。同样，在图中观察得到此情况。 旋转地球体-科氏漂移和加速度计漂移 在insdem06中，之前的演示扩展来可得到以下情况：1）地球自转；2）速度更新应包括科里奥利修正。功能earthrot是用来产生与地球速度相关的陀螺测量值（delta-theta）部分(根据给定的纬度、经度、测量时间间隔和载体姿态计算理想的地球转动测量值)。函数lclevup用来说明当地水平坐标系在一个更新时间间隔内考虑了地球转动和载体转动的运动。另外，需要一个静态物体位于北纬45度、东经45度和一个100μg北部加速度漂移。结果和insdem05演示的相似，但是捷联结果仍需注意： 除了位置/速度为，误差也可以在计算出的姿势中观察出来。俯仰误差最关键，因为飞行器指北时的北加速度漂移的要求。但是，该误差也出现在另外两个轴。 陀螺仪漂移 在insdem07中，模拟了一个0.001度/小时的东陀螺仪漂移。位置误差上的舒勒振动除了有漂移之外是实时的。正如可能预计到的，这也是偏航误差的情况。位置误差可通过与理论推测[2,page379]进行比较得到校验： 其中的POSerr由一系列角度（如纬度误差）给出，由此引出的φ是陀螺漂移或由角速度引起的漂移，t是INS开始导航后的运行时间，ωs是舒勒频率。演示结果显示了在一段时间位移后纬度误差上的舒勒振动。四小时后纬度误差约为12角度。在同样的条件下，由上述的公式得到误差应约为14角度。这两个结果非常接近，有区别是因为该理论等式是对单线情况导出的，不适于轴间交叉耦合。 垂直陀螺漂移效果在insdem08中可以显示出来： 初始化速度误差 insdem09是对初始化速度误差的仿真。同样，可以明显的观察到舒勒振动。初始化速度误差的理论结果显示一种舒勒振动位置误差，其大小是初始速度误差和舒勒振动[2]的比率。演示说明了一个0.1m/s的惯性误差，因此根据定理，位置误差大小应约为北纬45度80m或2.6角秒。从演示图可以看出情况确实如此。 初始化姿态角 insdem10是对初始化姿态角的仿真。根据定理论，位置误差是一个最小值为零，最大值为角度误差两倍的舒勒正弦曲线。仿真结果一致，为角度误差在10角秒，经度误差范围为0到27角秒： 陀螺仪噪声 insdem11是对陀螺仪噪声的仿真。在本例中噪声只在东陀螺仪上进行仿真。显然姿态误差位于俯仰存在噪声（飞行器指北，所以东陀螺仪影响俯仰），但滚动和偏航误差平滑的地方。这是噪声间接耦合和附加综合的结果。 地球大圆轨迹产生与显示 在示范了INS工具箱对静态惯性系统仿真（包括测量、数据处理和误差）的效用后，接下来是对运动载体进行仿真。在前面演示过的一个简单例子中旋转地球体的重力影响被忽略了。在开始完整仿真前，我们需要一个能生成长路径的函数（回忆一下progen是用来产生短时间本地路径的）。 greatcir功能是在地球上指定两点间产生地球大圆轨迹路径。功能/演示insdem12在地球上划出了地球大圆轨迹。 当在大小合适的监控器上用彩色显示时结果更好。但灰度显示图就能清晰显示从纽约到伊斯坦布尔的一条路径。用Matlab的view功能使用户能够改变视角。注意insdem12也能被用来在任意的用户指定点间画地球大圆轨迹。通过insdem12帮助可以看如何使用该功能。 地球大圆轨迹的惯性导航 insdem13给出了该路径的INS仿真（无传感器误差）。科氏效果通过函数gendvcor被作用到加速度计测量值（delta-V）上。飞行器速度通过函数gendelcr作用到陀螺测量值（delta-theta）上。 上图显示了greatcir产生的经纬轨迹图和INS计算出的位置。因为没有对传感器误差进行模拟，两者如预计的非常接近。 如果我们从测量值减去真实值，会得到按第几十度到第几百度顺序排列的误差。这是大圆轨迹复杂性带来的结果。在实际中，一个大圆轨迹包括一个在其整个路径上不断变化的方向或过程。但对于一个仿真过程，我们该满足于有限的位置点和之间不变的方位。 有人可能会怀疑在重要的计算误差已经存在的情况下，怎么可能延伸仿真去解决传感器误差。答案是建立一条基线。例如，我们在所有误差值设为零值的情况，保存计算出的经度值和纬度值，然后以传感器误差形式存在的经纬误差能通过对两次仿真结果的微分计算出来，得到一个带传感器误差，一个没有。 在已知的例子中，纽约到伊斯坦布尔的无误差仿真保存到了jkf_ist0.mat中。这在下一个仿真insdem04，会用来计算在大圆轨迹上因传感器误差带来的位置/速度/姿态误差。该仿真能容易的延伸来合并速度误差初值和初始化姿态角的影响。 尽管为了指导目的,传感器误差被 ‘人为’的加到静态载体中，我们能用函数gendverr和gentherr来分别使加速度计和陀螺仪误差自动产生。 该仿真中，有如下被模拟的传感器误差： - 40μg x加速度计漂移 - 50μg y加速度计漂移 - 0.01度/小时 x陀螺仪漂移 - 0.015度/小时 y陀螺仪漂移 - 0.009deg/root-hour x陀螺仪白噪声背离 标准 excel标准偏差excel标准偏差函数exl标准差函数国标检验抽样标准表免费下载红头文件格式标准下载 - 0.005 deg/root-hour y陀螺仪白噪声背离标准 当已经模拟导航级传感器的情况下，能得到最终结果近似为1海里/小时的系统。 游动方位角系统 到现在,仿真用到了坐标轴指向东北天(ENU)的本地坐标系。对于中纬的情况而言，这很合适。但对极点附近的导航，这种所谓的指北配置就不合适了。注意当你位于北极点上时，是不可能让轴指东或指北的(所有方向均为南)。使用游动方位角配置就是为了解决这个问题。在地球坐标系和本地坐标系之间进行转变时游动方位角构成使用了一个附加的有效自由角。在指北一例中，本地坐标系绕垂直轴旋转来保证其水平轴指东指北。在游动方位角系统中，同样有一个附加的绕垂直轴的旋转。这个附加的旋转被称为'游动角'。 三个共同的游动方位角配置分别为[2]：1）自由方位；2）傅科方位；和3）单极方位。在自由方位中，载体速度垂直分量设为零。在傅科方位中，空间速度(即载体速度和地球速度的总和)设为零。在单极方位中，游动角用来作为经度速度的简单参数（这样产生理想的垂直飞行器速度和空间速度）。 在纽约到伊斯坦布尔的路径中，关于上述三个配置的示范见insdem15，insdem16和insdem17。这些演示和指北一例(insdem13)的唯一不同是vertmech(垂直配置标记)被改变，从而crafrate能执行正确的游动方位角飞行速度。另外，因为速度在本地坐标系里被更新(不再是ENU)，一些附加计算被用来将本地坐标系里速度转化到ENU。 对于自由方位角的例子，垂直飞行速度设为零，因此偏航角度(计算出的方位角)恒定。方位是由偏航减去游动角得到的。每一个游动方位角配置都有其自身的优缺点，其中某些发展中用到的基本原理参见[2,page371]。 参考资料： 下列参考书目极力推荐给那些在惯导方面需要额外理论/资料的读者： [1] Brockstein, A. and J. Kouba, “Derivation of Free Inertial, General Wander Azimuth Mechanization Equations,” Litton Systems, Inc., Guidance and Control Systems Division, Woodland Hills, California, June 1969, Revised June 1981. [2] Kayton, M. and W. Fried, AVONICS NAVIGATION SYSTEMS, 2nd edition, John Wiley & Sons, New York, 1997. [3] Savage, P, Introductin to Strapdown Inertial Navegation Systems, 8th printing, Strapdown Associates, Inc., Maple Plain, MN, 1997. [4] Savage, P., “Strapdown Inertial Navigation Integration Algorithm Design Part1: Attitude Algorithms,” AIAA Journal of Guidance, Control and Dynamics, Vol.21, No.1, January-February 1998, p.23. [5] Savage, P., “Strapdown Inertial Navigation Integration Algorithm Design Part2: Velocity and Position Algorithms,” AIAA Journal of Guidance, Control and Dynamics, Vol. 21, No.2 March-April 1998, p.209. [6] Siouris, G., AEROSPACE AVIONICS SYSTEMS-A MODERN SYNTHESIS, Academic Press, Inc., San Diego, 1993. [7] Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, London, 1997, pp.295-297. 3. 参 考 介 绍 该部分包含一系列惯导系统工具箱里所有的M文件。关于路径是如何产生的例子中提供了输入输出参数的详情。注意该资料中大部分也可以在网上帮助工具中获得。 姿态更新 bodupdat 更新计算载体相对于惯性系的方向余弦矩阵 crafrate 计算飞行器速率 lclevupd 计算当地水平坐标系在一个更新时间间隔内的转动余弦矩阵,考虑了载体相对于地球的运动和地球相对于惯性系的运动 位置和速度更新 navupdat 更新计算和当地水平坐标系以及地球系相关的方向余弦矩阵 velupdat 执行速度更新 本地路径产生及INS仿真 progen 产生一条飞行轨迹(本地坐标系) propitch 产生前后俯仰和水平直线飞行之间的过度路径 proroll 产生倾斜转弯和水平直线飞行之间的过度路径 prostrai 产生一条速度或加速度恒定的直线路径 proturn 产生一条等高且加速度恒定的转动路径 insdem01 progen使用演示 insdem02 简化的3-dINS仿真;更新方向余弦矩阵 insdem03 缩短时间间距修改后的insdem02 insdem04 简化的3-dINS仿真;更新四元数 大圆路径产生及INS仿真 dcmelgen 产生和当地水平坐标系以及地球系相关的方向余弦矩阵 dcmnbgen 产生和当地惯性坐标系以及载体坐标系相关的方向余弦矩阵 genvelpr 产生速度轨迹 greatcir 产生大圆轨迹 insdem12 greatcir,pathplot和plotwrld使用演示 insdem13 大圆轨迹的INS仿真(传感器误差设为零) insdem14 大圆轨迹的INS仿真(考虑模拟传感器误差) pathplot 在球体上绘制系列路线点的函数 plotwrld 加载地形数据;为球体设置色彩图 四元数处理 dcm2qua 将和当地惯性坐标系以及载体坐标系相关的方向余弦矩阵转换为四元数 qua2dcm 将四元数转换为和载体坐标系以及当地惯性坐标系相关的方向余弦矩阵 quamult 进行四元数乘法运算 quaupdat 执行四元数更新 姿态表现转换 dcm2eulr 将和当地惯性坐标系以及载体坐标系相关的方向余弦矩阵转换为欧拉角 eulr2dcm 将欧拉角转换为和当地惯性坐标系以及载体坐标系相关的方向余弦矩阵 eulr2qua 将欧拉角转换为四元数 qua2dcm 将四元数转换为和当地惯性坐标系以及载体坐标系相关的方向余弦矩阵 坐标系统转换 dcm211w 将和当地惯性坐标系以及载体坐标系相关的方向余弦矩阵转换为经纬游动角 enu2xyz 从本地东北上坐标转换到地心WGS-84笛卡儿坐标系 llh2xyz 从经纬高度转化为地心WGS-84笛卡儿坐标系 llw2dcm 从经纬游动角转化为本地方向余弦矩阵 xyz2enu 从地心WGS-84笛卡儿坐标系转化为本地东北上坐标 xyz2llh 从地心WGS-84笛卡儿坐标系转化为经纬高 静态物体仿真 insdem05 用加速度计漂移的单线(有效)仿真 insdem06 用加速度计漂移的全仿真 insdem07 用单线陀螺仪漂移的仿真 insdem08 用垂直陀螺仪漂移的仿真 insdem09 用初始速度误差的仿真 insdem10 用初始化姿态角的仿真 insdem11 用陀螺仪噪声的仿真 原始测量数据和误差产生 earhrot 产生delta-theta的地球级部分 gendelcr 产生delta-theta的飞行器级部分 gendthet 产生陀螺测量值（delta-theta）的载体级部分(在本地系内) gendv 产生与以地球为参照物的速度变化相应的加速度计测量值(delta-V) gendvcor 产生一系列加速度计测量值，考虑哥氏加速度和重力加速度 gendverr 产生加速度计测量值（delta-V）误差(漂移,比例尺和噪声) gentherr 产生陀螺测量值（delta-theta）误差(漂移,比例尺和噪声) 游动方位角仿真 insdem15 一条从纽约到伊斯坦布尔飞行路径自由方位角配置的INS仿真 insdem16 一条从纽约到伊斯坦布尔的飞行路径的傅科配置INS仿真 insdem17 一条从纽约到伊斯坦布尔飞行路径Unipolar配置INS仿真 其他 coriolis 计算哥氏加速度引起的速度偏差的修正值 caftrate 计算craft(又名传送器)速度 extrapol 外推两个标量 interpol 插入两个标量 radicurv 计算曲率的子午线的和主要的半径 skewsymm将一向量转化进其相应的歪斜的相均衡的矩阵构成 bodupdat 功能： 用于载体运动(相对于惯性环境)载体系到导航系方向余弦矩阵的更新。该函数作用于捷联式陀螺仪输出。 格式： DCMbn_new=bodupdat(DCMbn_old,ang_vect) 或 [DCMbn_new,DCMbb]=bodupdat(DCMbn_old,ang_vect) 输入： DCMbn_old=提供从载体到在导航坐标系转换的当前方向余弦矩阵 ang_vect=载体角度方向矢量的增量积分；若没有coning，这就是速度积分陀螺仪的输出值 ang_vect(1)=x-部分(滚转型运动) ang_vect(2)=y-部分(俯仰型运动) ang_vect(3)=z-部分(偏航型运动) 输出： DCMbn_new=与当前载体坐标系和之前导航坐标系相关的更新了的方向余弦矩阵。注意导航坐标系的更新(与本地坐标系相关联)是由函数lclevupd完成的。 DCMbb=能使在k+1时刻的载体坐标系转换到k时刻载体坐标系的方向余弦矩阵 算法： C表示方向余弦矩阵，当前的和之前的时间指数用k+1和k表示，则更新计算如下： 参见：lclevupd,navupdat 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the institution of Electrical Engineers, London, 1997, pp. 295-297. Savage, P., “Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms,” AIAA Journal of Guidanc, Control and Dynamics, Vol. 21, No.1, January-February 1998, p.23. coriolis 功能： 计算与参照地球的速度值和惯性速度值的科氏修正。注意对原始测量值使用该函数。 格式： vel_cor=coriolis(vel_extr_L,omega_en_n,DCMbn,...DCMel,outflg,deltat) 输入： vel_extrp_L=载体速度矢量(在本地坐标系了表示的参照地球的速度值)外推，故其在当前更新的时间段内正确 omega_en_n=在导航坐标系内表示的传送速度矢量 DCMbn=完成从载体坐标系到导航坐标系转化的方向余弦矩阵 DCMel=完成从地球坐标系到本地坐标系转化的方向余弦矩阵 outflg=输出数据标记(0=在导航坐标系里输出；1=在载体坐标系里输出) deltat=更新计算时间间隔(计以秒) 输出： vel_cor=哥氏加速度引起的速度偏差修正值 (载体或导航系下的地球参考速度) vel_cor(1,1)=载体或导航坐标系里x方向速度(米/秒) vel_cor(2,1)=载体或导航坐标系里y方向速度(米/秒) vel_cor(3,1)=载体或导航坐标系里z方向速度(米/秒) 算法： 规定如下： vel_cor={vX(2Ω+ρ)}* Δt 其中：v是物体速度(外推使其在更新的时间段内正确)；X表示叉乘算子；Ω是地球球转动矢量(在修正时间间隔上取均值)；ρ是载体转动速率矢量(在修正时间间隔上取均值)；Δt是修正时间间隔。 参见：velupdat 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. On behalf of the Institution of Electrical Engineers, London, 1997, pp.35,51,316. Savage, P., “Strapdown Inertial Navigation Integration Algorithm Design Part2: Velocity and Position Algorithms,” AIAA Journal of Guidance, Control and Dynamics, Vol. 21, No.2 March-April 1998, p.209. crafrate 功能： 计算飞行器(又名：传输器)速度。 格式： rho= crafrate(lat,vx,vy,height,DCMel,earthflg,vertmech) 输入： lat=测量出的纬度的弧度值 vx=本地坐标系里x方向速度(米/秒) vy=本地坐标系里y方向速度(米/秒) height=参照椭圆体上的载体高度(米) DCMel=与地球坐标系和本地坐标系相关的方向余弦矩阵 earthflg=地形标记 0=半径为6378137米的地球球体 1=地球椭圆体(WGS-84) vertmech=任意指定纵向配置 0=指北(缺省值) 1=自由方位角(纵向飞行器速度设为零值) 2=傅科(空间速度的纵向分量设为零) 3=单极(游动角速度设为+/-经度速度;在北半球-,在南半球+) 输出： rho=飞行器速度矢量 rot(1,1)=本地坐标系里x方向速度角(弧度/秒) rot(2,1)=本地坐标系里y方向速度角(弧度/秒) rot(3,1)=本地坐标系里z方向速度角(弧度/秒) 在指北一例中，rho(1)是载体东速度，rho(2)是载体北速度。对于所有情况，rho(3)是载体上(正值)。 算法： 对于所有指北例中,用到的运算法则是： rho(1,1)=-Vn/(rm+height); rho(2,1)=Ve/(rp+height); rho(3,1)=Ve*ran(lan)/(rp+height); 其中，Vn是速度的北方向部分，Ve是速度的东方向部分，height是参照椭圆体上的载体高度，lat是物体纬度，rm和rp分别是曲率子午线方向和常规方向的半径。 参见： radicurv 参考书目： Kayton, M. and W. Fried, AVONICS NAVIGATION SYSTEMS, 2nd edition, John Wiley & Sons, New York, 1997. Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. On behalf of the Institution of Electrical Engineers, London, 1997. Brockstein, A. and J. Kouba, “Derivation of Free Inertial, General Wander Azimuth Mechanization Equations,” Litton Systems, Inc., Guidance and Control Systems Division, Woodland Hills, California, June 1969, Revised June 1981. dcm2eulr 功能： 用来计算在载体系到导航系方向余弦矩阵中计算欧拉角的函数 格式： eul_vect=dem2eulr(DCMbn) 输入： DCMbn=能实现载体坐标系到导航坐标系转化的3*3方向余弦矩阵 输出： eul_vect(1)=滚转弧度角 eul_vect(2)=俯仰弧度角 eul_vect(3)=偏航弧度角 注意： 如果俯仰角是难以察觉的接近+/-π/2，eul_vect中的元素将被NaN填充 参见： dcm2qua,eulr2dcm,eulr2qua 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, London, 1997. dcm2llw 功能： 用于从地球系到本地系方向余弦矩阵计算纬度、经度和游动角的函数。 格式： llw_vect=dcm2llw(DCMel) 输入： DCMel=能实现地球坐标系到本地坐标系转化的3*3方向余弦矩阵 输出： llw_vect(1)=纬度弧度值 llw_vect(2)=经度弧度值 llw_vect(3)=游动角弧度值 算法： lat=asin(DCMel(3,3)); long=atan2(DCMel(3,2),DCMel(3,1)); alpha=atan2(DCMel(1,3),DCMel(2,3)); 参见： llw2dcm,dcm2eulr,dcm2qua,eulr2dcm,eulr2qua,qua2dcm,xyz2enu,xyz2llh,llh2xyz 参考书目： Kayton, M. and W. Fried, AVONICS NAVIGATION SYSTEMS, 2nd edition, John Wiley & Sons, New York, 1997. dcm2qua 功能： 将载体系到导航系方向余弦矩阵转化为四元数。 格式： qua_vec=dcm2qua(DCMbn) 输入： DCMbn=能实现载体坐标系到导航坐标系转化的3*3方向余弦矩阵 输出： qua_vec=四元数矢量 =[a b c d] 其中：a=cos(MU/2) b=(MUx/MU)*sin(MU/2) c=(MUy/MU)*sin(MU/2) d=(MUz/MU)*sin(MU/2) 这里，MUx,MUy,MUz是角矢量的部分，MU是角矢量的大小。 角矢量是四元数的中心部分。认为从一个坐标系(例如本例中的载体坐标系)到到另一个坐标系(例如本例中的导航坐标系)的转化，能被角矢量大小影响的绕其转动的一个角度所影响。 算法： 运算法则如下，只有对小的角度位移才有效(即对那些小的三角近似值有效的角度)： a=0.5*sqrt(1+DCMbn(1,1)+DCMbn(2,2)+DCMbn(3,3)); tmp=inv(4*a); b=tmp*(DCMbn(3,2)-DCMbn(2,3)); c=tmp*(DCMbn(1,3)-DCMbn(3,1)); d=tmp*(DCMbn(2,1)-DCMbn(1,2)); qua_vec=[a b c d]; 参见： qua2dcm,eulr2qua,eulr2dcm 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, London, 1997. dcmelgen 功能： 根据由greatcir生成的一条路径生成一个方向余弦矩阵（地球坐标系到本地坐标系）的函数。 格式： DCMel_prof=dcmelgen(lat_prof,lon_prof,tc_prof) 输入： lat_prof=每一个路径点的纬度矢量（以弧度表示） lon_prof=每一个路径点的经度矢量（以弧度表示） tc_prof=从在大圆路径上每一个路径点得出的真实运动过程（弧度相对于北；东是实际方向） 输出： DCMel_prof（i，1，9）=载体姿态（地球到本地系）方向余弦矩阵（DCM）的要素；1=DCM（1，1），2=DCM（1，2）；3=DCM（1，3）；4=DCM（2，1），等等。 算法： 飞行路径（即轨迹图）上的每一个点用纬度、经度和真实运动方向表示。对于每一个点，llw2dcm用来计算地球系到本地系方向余弦矩阵。该法则在指北配置中受限制。 参见： greatcir，progen dcmnbgen 功能： 根据恒定高度、匀速的飞行路径生成一个方向余弦矩阵（惯性系到载体系）的函数。 格式： DCMnb_prof=dcmnbgen(tc_prof) 输入： tc_prof=从在大圆路径上每一个路径点得出的真实运动过程（弧度相对于北；东是实际方向） 输出： DCMeb_prof（i，1，9）=载体姿态（地球到本地系）方向余弦矩阵（DCM）的要素；1=DCM（1，1），2=DCM（1，2）；3=DCM（1，3）；4=DCM（2，1），等等。 虽然模拟的飞行路径其实是水平的，但DCMnb说明了在大圆路径上每一个路径点方向的变化（由tc_prof给出）。 算法： 沿飞行路径（即轨迹图）上每个点的载体姿态由俯仰、旋转和偏航定义。由于路径平直，俯仰和旋转值为零。另外，考虑风向为零，偏航就为方向，即真实路线。对每一个点，eulr2dcm用来计算导航系到载体系的方向余弦矩阵。 参见： greatcir earthrot 功能： 以载体经度、纬度、测量时间间隔和载体姿态为参量，用来产生理想的地球旋转测量值。 格式： deltaer=earthrot(timevec,DCMel_prof,DCMnb_prof) 输入： timevec=在LAT_PROF和LONG_PROF中指定的与载体经纬度联系的时间点矢量（计以秒） DCMel_prof=地球系到本地系（ENU）路径一段时间上的方向余弦元素。 DCMel_prof（i，1：9）=第i个方向余弦矩阵（DCM）； 1=DCM（1，1），2=DCM（1，2）；3=DCM（1，3）；4=DCM（2，1），等等。 DCMnb_prof=一段时间上的惯性系（NED）到载体系的方向余弦元素 DCMnb_prof（i，1：9）=载体姿态的第i个方向余弦矩阵（DCM）； 1=DCM（1，1），2=DCM（1，2）；3=DCM（1，3）；4=DCM（2，1），等等。 输出： deltaer=理想的地球体旋转测量值矢量（计以弧度）；如果输入矢量有N%点长，DELTAER就是N-1点长。 DELTAER(1,:)对应于由TIMEVEC(2)指定的时间。 DELTAER(2,:)对应于TIMEVEC(3)，以此类推。 对于一个给定的数‘I’，DELTAE(I,1:3)是载体坐标系内地球旋转矢量的部分。 算法： 梯形积分用来计算在输入路径中指定的时间点间发生的地球旋转量。 参见： velupdat enu2xyz 功能： 从本地切线矩形坐标系（东-北-天）转化到WGS-84ECEF笛卡儿坐标系。 格式： xyz=enu2xyz(enu,orgxyz) 输入： enu(1)=相对于本地原点的'东'向坐标(米) enu(2)=相对于本地原点的'西'向坐标(米) enu(3)=相对于本地原点的'上'向坐标(米) org_xyz(1)=相对于本地原点的ECEFx坐标(米) org_xyz(2)=相对于本地原点的ECEFy坐标(米) org_xyz(3)=相对于本地原点的ECEFz坐标(米) 输出： xyz(1)=ECEFx坐标轴 xyz(2)=ECEFy坐标轴 xyz(3)=ECEFz坐标轴 注意：ENU坐标是笛卡儿本地切线坐标系的一个部分。在本地系中，x轴从本地原点指东；y轴从本地系指北，等等。由于其中的'东'向沿一条纬线(实际上是一个圆)分布,本地笛卡儿坐标系近似沿低纬和中纬的测量方向。当在地球两极操作时,'东'向不很理想的近似为一条直线。 参考书目： Alfred Leick, GPS Satellite Surveying, 2nd ed., Wiley-Interscience, John Wiley & Sons, New York, 1995. eulr2dcm 功能： 将欧拉角转换为导航系到载体系方向余弦矩阵。 格式： DCMnb_eulr2dcm(eul_vect) 输入： eul_vect(1)=转动角(弧度) eul_vect(2)=俯仰角(弧度) eul_vect(3)=偏航角(弧度) 输出： DCMnb=能从惯性系转化到载体系的3*3方向余弦矩阵 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, London, 1997, p.44. eulr2qua 功能： 用于欧拉角到四元数的转化。 格式： qua_vec=eulr2qua(eul_vect) 输入： eul_vect(1)=转动角(弧度) eul_vect(2)=俯仰角(弧度) eul_vect(3)=偏航角(弧度) 输出： qua_vec=4元素矢量 =[a b c d] 其中：a=cos(MU/2) b=(MUx/MU)*sin(MU/2) C=(MUy/MU)*sin(MU/2) D=(MUz/MU)*sin(MU/2) 其中：MUx,MUy,MUz是角度矢量的构成部分。MU是角度矢量的大小。 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, London, 1997, p.50. extrapol 功能： 执行两个标量的外推。 格式： v_ex=extrapol(v1,v2,td12,tdex) 描述： v1和v2值在时间上以秒分隔td12。假设v1对应于时间t1，则v2对应于时间t2(t2>t1)。该函数线形地外推这些值到一个时间点，即t2几秒后的tdex。 gendelcr 功能： 对greatcir生成的一条路径产生与飞行器速度相关的陀螺测量值（delta-theta）的理想部分的函数。 格式： deltacr=gendelcr(lat_prof,tc_prof,... totvel_prof,height_prof,time_prof,... DCMnb_prof,DCMel_prof,earthflg) 输入： lat_prof=路径上每个点的纬度矢量（弧度） tc_prof=从大圆上每个路径点的得出的真实路线（弧度值相对于北；东向为绝对方向） totvel_prof=对路径每一段上载体速度（海里/小时）的总速度矢量 height_prof=在路径每一短上，对于参照椭圆体上的载体高度矢量（米） time_prof=时间矢量（秒） DCMnb_prof=一段时间内导航结构（NED）到载体结构的方向余弦元素 DCMnb_prof(i,1:9)=对于载体姿态的第i个方向余弦矩阵（DCM）元素；1=DCM(1,1),2=DCM(1,2) 3=DCM(1,3),4=DCM(2,1)等等。 DCMel_prof=一段时间内地球frame到本地frame的方向余弦元素。 DCMnb_prof(i,1:9)=对于载体位置的第i个方向余弦矩阵（DCM）元素；1=DCM(1,1),2=DCM(1,2) 3=DCM(1,3),4=DCM(2,1)等等。 earthflg=地球外形标记；0=地球球体；1=WGS-84地球椭球体（参见crafrate）。 输出： deltacr(i,1:3)=对第i部分路径，craft rate的陀螺测量值（delta-theta）的三部分(在载体系内)。 算法： 用梯形积分来计算在输入路径中指定的时间点间发生的craft-rate的旋转量。 参见： crafrate gendthet 功能： 产生一系列陀螺测量值（delta-theta）。只有本地分量，即不不考虑地球自转和载体转动。(地球转动和载体转动参见earthrot和crafrate)。 格式： deltheta=gendthet(DCMnb_prof) 输入： DCMnb_prof=仿真时间段内导航系到载体系的载体姿态方向余弦序列 DCMnb_prof(i,1:9)= 第 i 个载体姿态方向余弦矩阵的元素；1=DCM(1,1),2=DCM(1,2) 3=DCM(1,3),4=DCM(2,1) 依次类推。 输出： deltheta=一段时间内三个正交的陀螺仪输出值（只对载体运动）对第i个飞行路径, deltheta(i,1) =x陀螺输出(前向)，deltheta(i,2)=y陀螺输出(右翼)，deltheta(i,3)=z陀螺输出(下向)。 算法： 得自参考中的关键部分可概括如下： DCMbn(t+δt)=DCMbn(t)*A(t) 其中，δt是一个小的时间增量，*表示矩阵乘法，A(t)是如下给定的一个矩阵： A(t)=[i+δΨ] 其中I是一个3*3恒等矩阵，δΨ是一个矩阵，从中有： δΨ(2,1)=δψ，偏航轴的转动 δΨ(2,1)=δθ，俯仰轴的转动 δΨ(2,1)=δφ，滚转轴的转动 该函数因此用来为解决A(t)的一系列连续的方向余弦矩阵，然后从δΨ矩阵中析取出要求的转动量。 应特别注意法则认为转动量很小。在理想情况中，转动应是无穷小，但这样就要求无限的速度采样和一个拥有无限字长的无限快的计算机。从实际考虑，时间间隔必须足够短，这样三角角度近似 (如：sinα=α)有效。 参见： gendelcr,gendv,gendvcor,gentherr 参考书目： Titterton, D. and J. Weston, STRAPDOWN INERTIAL NAVIGATION TECHNOLOGY, Peter Peregrinus Ltd. on behalf of the Institution of Electrical Engineers, London, 1997, pp.42-43. gendv 功能： 产生相应于参照地球的速度变化的加速度计测量值（delta-V）。不考虑哥氏加速度和重力加速度(参见gendvcor)。 格式： deltav_b=gendv(vel_prof_L,DCMnb_prof) 输入： vel_prof_L=仿真时间段内的速度序列 vel_prof_L(i,1:3)= 当地水平坐标系下第 i 个速度向量的元素。 DCMnb_prof=仿真时间段内导航系到载体系的载体姿态方向余弦序列 DCMnb_prof(i,1:9)= 第 i 个载体姿态方向余弦矩阵的元素； 1=DCM(1,1)，2=DCM(1,2)，3=DCM(1,3)，4=DCM(2,1) 依次类推。 输出： deltav_b=载体系下的加速度计测量值序列 (前向-右翼-下向) 算法： vel_prof_L中的元素微分后再从本地