捷联式惯性导航积分算法设计上下完整篇.doc

捷联式惯性导航积分算法设计上篇：姿态算法Paul G. Savage Strapdown Associates, Inc., Maple Plain, Minnesota 55359摘要：本论文分上下两篇，用于给现代捷联惯导系统的主要软件算法设计提供一个严密的综合方法：将角速率积分成姿态角，将加速度变换或积分成速度以及将速度积分成位置。该算法是用两速修正法构成的，而两速修正法是具有一定创新程度的新颖算法，是为姿态修正而开发出来的，在姿态修正中，以中速运用精密解析方程去校正积分参数(姿态、速度或位置)，其输入是由在参数修正(姿态锥化修正、速度摇橹修正以及高分辨率位置螺旋修正)时间间隔内计算运动角速度和加速度的高速算法提供的。该设计方法考虑了通过捷联系统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态基准和矢量速度积分的导航系旋转问题。本论文上篇定义了捷联惯导积分函数的总体设计要求，并开发出了用于姿态修正算法的方向余弦法和四元数法；下篇着重讨论速度和位置积分算法的设计。尽管上下两篇讨论中常常涉及到基本的惯性导航概念，然而，本论文是为那些已对基础惯导概念很熟悉的实际工作者而写的。专门用语：=任意坐标系 =将矢量从坐标系投影到坐标系的方向余弦矩阵I =单位矩阵 =从坐标系投影到坐标系的旋转矢量所构成的姿态变化四元数 =的共轭四元数，它的第1项与的首项相同，余下的24项与 的互为相反数 =单位四元数，它的第1项为1，其余3项为0 =无具体坐标系定义的矢量 =列向量，它的各项元素等于矢量在坐标系A的各轴上的投影 =向量的反对称（或交叉积）形式，代表如下矩阵： 其中：，是的分量，与A系矢量的矩阵乘积等于与该矢量的叉积 =与等量的四元数矢量， =坐标系相对于坐标系的角速率，当为惯性系（I系）时，是由安装在坐标系上的角速率传感器所测到的角速率1.概论 惯性导航是通过对速度积分得到位置并对总加速度积分得到速度的过程。总加速度是指由重力加速度和被施加的非重力产生的加速度（亦即比力加速度）之和。惯性导航系统( INS)包括：用于积分的导航计算机；用于给积分运算定时的精密时钟，测量比力加速度用的加速度计组台；用于作为所算位置的一个函数而进行的重力加速度计算而留于导航计算机中的重力模型软件，以及为了定义作为速度计算一部分的加速度计三元组的角度方向所用的姿态基准。在现代INS中，姿态基准是由驻留于INS计算机中的软件积分函数提供的，其输入来自一个有三轴的惯性角速度传感器。角速度传感器和加速度计三元组安装在一个公用的牢固构架上，该构架装在INS的底盘上，以保证每个惯性传感器之间的精确对准，这样的一种布置称之为捷联INS。因为惯性传感器牢固地固定在底盘内，所以也就牢固地固定于安装INS的飞行器上。 INS计算机中的基本函数有将角速率变换为姿态的积分函数(称之为姿态积分)使用姿态数据将测得的加速度值转换到适当的导航坐标系中，再将它积分成矢量速度的函数(称之为矢量速度积分)，还有将导航系矢量速度积分成位置的函数(称之为位置积分)。这样就有了三个积分函数，姿态函数、矢量速度函数及位置函数，每个函数的精度要求很高，以确保函数误差极小，符台惯性传感器精度的要求。回眸历史，因为早在50年代，基础捷联惯导概念就开始形成，所以多年来捷联分析师将精力主要集中于姿态积分函数算法的设计上。而种种算法的设计方法总是受到当时的飞行计算机技术的能力和局限性的影响。50年代后期和60年代，各种研究机构的捷联工作者们采取两种用于姿态积分函数运算的方法，即用一阶数字算法进行高速姿态修正运算，如1020kHz和用高阶算法进行的低速姿态修正运算，如50100HZ。如果要精确考虑高频角速率分量时，就得考虑用高速算法，这样可以调整为系统的三维姿态变化。然而，那个时期的计算机工艺技术只能对姿态修正算法进行一些简化的一阶方程运算，精度有限。相反，高阶算法提倡者极力吹捧较一阶算法提高了解析精度的高阶算法；但是，由于每个姿态修正循环可执行运算次数的连带增加导致必须使姿态修正速率减缓以满足当时的计算机吞吐量的限制，从而使提高了的精度被降低。由于姿态四元数可以当作对所算姿态参数的解析形，这一优点使之成为一种更可取的算法(与传统方向余弦矩阵姿态表示法相比)所以它的出现使上述两种算法的优点黯然失色。就那一时期研究过的算法而言，四元数法在高频角速率环境下表现出的运算精度最优。1966年，作者提出一个新的两速姿态积分函数运算方法，在该方法中，姿态修正运算被分成两部分，即把简单高速一阶算法部分与更复杂的中速高阶算法部分结合起来使用，后一部分的输入由高速算法提供。简化了的高速部分用于考虑姿态修正循环内的高频角振荡，这能调整系统的姿态建立(传统上称之为锥化)。合在一起，两速方法的组合精度等于以高速速率进行的高阶运算(为了提高精度)；然而 由于高速算法简化之故，组合后对计算机的吞吐量的要求丝毫也不比原来的高速一阶姿态修正算法的吞吐量大。参考文献【6】采用的两速算法设计受到其基本解析公式的限制，该公式是一个皮卡德类型（Picard-type）的连续型的姿态速率微分方程的递归积分，在该方程中，同时产生中速和高速算法。解析递归积分设计过程的复杂性限制了高阶中速算法的扩展(在参考文献【6 】中仅扩展到2阶，这在当时被认为是可以接受的)。1969年，焦尔敦（Jordan）在一个不相关的设计活动中提出一个用于捷联姿态修正函数的两速法，在此方法中，解析公式的开头建立在两个单独定义的计算上：一个建立在输入姿态变化基础上的中速、经典封闭型、准确高阶姿态修正算法和一个测量中速算法姿态变化输入的简化高速二阶积分算法。1971年，鲍尔兹（Bortz）将焦尔敦的概念扩展到基于微分方程的高速计算，积分时将测量的准确姿态变化输入给准确的姿态修正算法。准确的中速姿态算法可以通过两个三角系数舍位构建成任何精度要求的特定阶次。实际上，鲍尔兹的姿态变化微分力程的简化型式现被用于高速函数计算。因此，参考文献【7】和【8】提供了一个更一般的两速姿态修正算法，在此算法中，中速高阶算法和高速简化算法可以独立地进行改编以满足特定的应用要求。（让人感兴趣的是，参考文献【8】提出了一个简化型高速算法的模拟执行程序。)参考文献【7】 和【8】提出的两速法(主张在准确中速姿态修正运算中采用方向余弦法)的第二个好处是中速部分也可以用一个解析的精确的封闭形式的四元数修正算法来作为公式，该算法采用同用于方向余弦修正算法一样的高速输入。这样一来，新的两速法无论对于方向余弦法还是对四元数修正法都有同样的精度，而这两种方法都是由解析的精确封闭式方程推导出来的(这里假定对泰勒级数的三角系数的展开进行到比较准确的量级)。大多数飞机使用的现代捷联INS采用以两速法为基础的姿态修正算法。中速算法部分的重复速率一般是基于最大角速率来设计（如50200Hz ）),以尽量减少中、高速算法中幂级数舍位误差的影响。高速算法的重复速率是在预计的捷联惯性传感器组合振动环境的基础上精确考虑了引起振荡的锥化效应而进行设计的。（如用于1n mile/h有50的径向位置误差速率的飞机的INS的重复速率为14kHz。）连续进行的两速姿态算法的开发工作一直集中于高速积分函数的变化上。原来设想的简单一阶的种种现有高速姿态算法利用现代计算机日益增加的吞吐量，演变为各种高阶算法，而且经度得到提高，(参见参考文献【9】【11】和【12】的第7-1节)。对于姿态修正函数的演化，截止目前，有关用于加速度变换或矢量速度积分和位置积分的捷联INS姊妹算法的开发方面的其他并驾齐驱式著述很少见到发表。本论文上篇定义了捷联惯导积分函数的总体设计要求并描述了开发基于两速法的姿态积分算法的综合设计过程。文章中所提供的材料是参考文献【12】第7-1节(它是参考文献【9】中材料的扩展)的浓缩型。这些材料着重于提供更严密的解析公式，同时，在可能的情况下，考虑到便于生成计算机软件的文件及有效性(这与现代飞行计算机技术也是一致的)，这些材料使用准确的封闭式方程。在姿态算法没计过程中包括了考虑到姿态修正时间周期内导航坐标系旋转的种种严密处理方法。本论文的第二节提供了有关坐标系和所采用参数的背景材料。第三节以连续微分方程形式提供了一整套典型的捷联惯导姿态、矢量速度和位置方程，这构成了等量算法设计过程的框架。第四节探讨以高速部分的普通形式进行两速姿态积分运算的算法(适用于在导航系旋转影响下的方向余弦法和四元数法的公式)，并描述了一个特殊的形式，说明在古典高速二阶锥化计算算法中一种算法的设计，第五节给出有关姿态积分算法的表格参考数据，第六节就选择特殊应用算法和建立运算运行速率之后的过程进行了一般化讨论，第七节给出结论性评述。最后，认识到下述观点很重要：尽管两速法的初衷是克服早期（l9651975年)计算机技术的吞吐量的局限性，然而随着现代高速计算机的快速发展进步，这一局限性正在变得无足轻重。这就促使工程技术人员想回到一个更简单的单速算法结构上来，采用这种结构时，所有计算以一个重复速率进行，速率很高，足以准确地将多轴高频角速率和加速度调整影响考虑进去。本论文上，下两篇提供的两速结构是和压缩成在对算法进行公式化的专门章节里解释的那种单速型是相兼容的。2坐标系和姿态方向的关系本节定义了本篇论文中使用的坐标系，并一般性地说明方向余弦矩阵、姿态四元数和用于代表两坐标系之间的角度关系的旋转矢量姿态参数等等的性质。2.1 坐标系定义 坐标系是根据右手法则对彼此互相垂直的三个连续编了号(或标了字符)的单位矢量加以定义的解析抽象。它可以想像为三个垂直线(轴)穿过一个共同的点（原点），各单位矢量从原点沿三条垂直线向外发散。在本论文中，坐标系原点的实际位置是任意的。在特定坐标系中，一个矢量的分量(或投影)等于矢量与该坐标系各个单位矢量的点乘。本论文中采用的矢量为自由矢量，因此，在对它们进行解析描述的坐标系中，其位置不存在优劣性。坐标系定义如下：1.E系是用于位置定位因而定义的地球固联坐标系。它的典型定义是它有一个轴与地球极轴平行，其他两轴固连于地球并与赤道平面平行；2.N系是一个导航坐标系，Z轴与当地地球表面参考位置的垂直向上方向平行，N系用于将加速度积分为速度并定义E系内当地垂直角方向；3.L系是当地水平坐标系，与N系平行，但Z轴垂直向下，而X和Y轴沿N系的Y和X轴。L系用于描述捷联传感器坐标系方向的基准；4.B系是捷联惯性传感器坐标系(机体系)，其轴与标准右手正交传感器输入轴相平行；5.I系是一个非旋转惯性坐标系，用作角旋转测量基准。为I系选择的特殊方向在其方向与解析运算有关的章节中进行讨论。 2.2姿态参数定义 方向余弦矩阵定义为一个方阵，其各列是单位矢量的正交组，每列等于沿系的一个轴方向上的单位矢量在系各轴上的投影 ： (1)式中是沿系轴i方向上的单位矢量在坐标系各轴上的投影。从这一基本定义出发，可以验证出：的i行、j列的元素等于系轴i和系轴j之间的夹角的余弦值，的转置矩阵等于其逆矩阵，转置矩阵的各列等于系轴中单位矢量在系各轴上的投影，且与一个投影于系各轴上的向量的积等于此向量在系各轴上的投影分量(而对的转置矩阵则相反)。 (2)(2)式可以用于导出方向余弦矩阵链式规律： (3)旋转矢量定义旋转轴和绕轴的旋转幅度。可以想像系通过绕此旋转矢量旋转而从它的起始姿态转到一个新的姿态，绕旋转矢量旋转的角度等于旋转矢量的大小。现在，称系为系的新姿态，通过给系的这一定义，一个任意定义的旋转矢量就可唯一地确定系姿态与原始系姿态二者的关系。反之，如果给定系和系的姿态关系，就可以定义一个与此姿态一致的旋转矢量。这样一来，旋转矢量可以用来定义系与系相关的姿态。从解析式上看(见参考文献【4】，【9】和【12】的第3.2.2.1节)旋转矢量和方向余弦矩阵之间的关系可以由下式给出： (4)式中和是旋转矢量及其幅度。旋转矢量的一个独特特性是在系和系中有相同的分量(见参考文献：【12】的第3.2.2.1节)；因此 (4)式中的代表或.姿态四元数是一个四矢量，即有四个分量，定义为旋转矢量的一个函数(见参考文献【4】，【9】和【12】的第3.2.4节和【14】的第7376页) （5）由(5)式可以看出，各元素的平方和是单位1。坐标变换方程可以用四元数代数方程表示(见参考文献【4】、【9】和【12】的第3.2.4.1节)。 ， （6）(6)式可以用来推导出姿态四元数的链式规律： （7）2.3 姿态参数速率方程 第22节姿态参数的变化速率(见参考文献【4】、【8】、【9】和【12】的第3.3节)由下式给出： （8） （9） （10） 3 连续型捷联惯导方程定义捷联惯导系统中进行的典型基本运算的微分方程(见参考文献 【9】、【l2】的第4章和【15】的第77103页以及156177页)给出如下：姿态速率： (11) 或代之以： (12)当地水平系旋转速率： (13) (14) (15)加速度变换: (16)或代之以： (17) (18)矢量速度速率: (19) (20)位置速率： (21) (22)式中： R =从地球中心到INS的位置矢量； =相对于地球的速度矢量(位置变化速率)，解析式中定义为E系中R的时间导数； h =地面高度，定义为从INS到地球表面的距离，沿从INS到地球参考重力平面上的切线平面的垂线方向测量； =曲率矩阵(3×3)是位置的一个函数，其元素3、i和i、3等于零，而其余元素绕对角线成对称形。对于球形地球模型而言，除对角线上外其余元素是零，且对角线上数值是从地球中心到INS的径向距离的倒数。对于扁地球模型而言，其余的项代表投影到INS高度线上的地球表面上的本地曲率(见参考文献【12】第5.3节有关封闭型的表述部分)； =沿重力平面垂直方向(N系的Z轴)上的单位矢量； =的垂直分量; 选取的值取决于所采用的N系的类型。如：游动方位系统或自由方位系统的设计保证对所有地球定位而言是非奇异的(见参考文献【l2】的第4.6节和【15】的88-99页)； =比力加速度，定义为由施加的非重力力产生的相对于非旋转惯性空间的加速度，它由加速度计测得； =质量吸引重力加速度或重力(R的一个函数)； =铅垂重力或重力，对于稳定的INS而言，它沿铅垂线方向； 的解析模型可以在参考文献【16】；【17】的第4.4节和【18】的6.3节找到。关于在N系的分量，请参见参考文献【12】第5.4.1节。 在运行捷联惯导函数过程中，捷联INS计算机用适当的积分算法对后面的姿态速率、矢量速度速率以及位置速率方程进行积分运算。关于后面的导航方程形式，下面几点值得注意。方向余弦和四元数姿态都是以机体姿态速率或加速度变换运算的形式表示。无论哪一个部可以用于实践，而其结果实质上是一致的。矢量速度是相对于地球(E系)定义的，矢量速度速率方程是以当地水平定义的N系写出来的（用于将它积分成矢量速度)。对于像飞机的INS之类的许多地形导航用途而言，这很典型。也可以选择其它坐标系用于矢量速度定义和矢量速度速率方程，如用于战术和战略导弹制导。位置速率方程将位置定义为高度和N系相对于E系的角度方向。从中可以提取出经度和纬度，并可以计算出R值(见参考文献【12】第4.5.1和4.5.3以及【15】的88，89页)。对于位置速率方程而言，位置也可以定义为简单的R(从中可以计算出和h,见参考文献【12】的第4.5.4节)。高度速率方程(22)显得价值不大，但当你考虑旋转扁地球模型、地球上的一个旋转N系和固定的高度定义时，这就不一定了。参考文献【12】的4.4节和5.5节表明对于旋转的扁地球模型而言，方程(22)是准确的。如果决定引入垂直通道重力或发散控制来防止不稳定垂直通道误差呈指数增加，那么方程（20)和(22)务必包括一个额外增加的垂直控制项(参考文献 【12】第4.4.1节，【15】第102-103页以及【18】的10.3节)。4 姿态即时修正算法 本节探讨适合用数字计算机进行积分的方向余弦矩阵速率方程(11)和姿态四元数速率方程(12的算法形式，这些方程将用现在的传统两速法来建立，传统两速法中的解析的精确的封闭形式的方程可以用于基本姿态修正函数的运算。该函数的输入是由一个为了在基本姿态修正周期中测出姿态变化而设计的高速算法所提供的。4.1 姿态方向余弦矩阵 方向余弦矩阵的修正算法是为实现下述目的而设计的：在姿态即时修正时间内所得的数字结果跟在相同时间间隔对方程(11)中表达式的正规连续积分得出的结果相同。该算法的思路是以理想的机体B系和当地水平L系在数字即时修正区间内确定的变化(方程11中由和引的)，作为每一即时修正时刻相对非旋转惯性空间(I系)的连续离散点上的当前构造值。考虑到完全一般化的情形，我们还允许由于L系角运动的修正运算不一定是在当B系运动而对修正的同一时刻瞬时运行，例如在一个多速率数字计算回路结构中，以高速率进行修正是由于B系旋转而不是由于L系旋转。如果对计算机吞吐量的最小化感兴趣的话，从软件结构上说，可以让L系修正比B系修正慢510倍。我们用来描述坐标系方向随时间变化的专门术语如下： =当计算机修正时间为时，机体B系在非旋转惯性空间I中的离散方向； =因B系角运动对进行即时修正的计算机循环拍次； =当地水平L系在计算机修正时间在非旋转惯性空间I中的离散方向； =因L系角运动对进行即时修正的计算机循环拍次；有了这些定义，采用(3)式方向余弦矩阵递推计算规律便能建立起对即时修正算法如下： （23） （24）其中：=时刻B系与时刻L系的转换关系矩阵；=时刻B系与时刻L系的转换关系矩阵；=考虑到B系相对惯性空间从时刻点到时刻点的旋转的方向余弦矩阵；=考虑到L系相对惯性空间从时刻点到时刻点的旋转的方向余弦矩阵； 在(23)和(24)式描述的算法把各个时间的机体B系和当地水平L系的方向联系起来，并为B系与L系提供了以不同修正速率对进行的惯性角运动修正。与B系(它能以200-300度/秒的速度进行动态旋转)相比，当地水平L系的惯性角速率一般来说较小，它等于地球旋转速率加上L系相对地球的角速率(传递速率，它一般不会大过地球速率的几倍)。因此，L系即时修正一般可以以低于B系修正的速率完成，而且有相当的精度。应当注意，对B系和L系运动修正速率的要求，在某种程度上，是通过和测量姿态变化的近似高速算法作比较，将误差降至最小的基础上确立的(见本文4.1.1和4.1.2节)。对的B系和L系的运动修正是由(23)和(24)式的项和项完成的，其算法分别推导如下：4.1.1 机体系旋转 (23)式用来考虑捷联传感器(机体)B系相对于非旋转空间的角速率来修正姿态方向余弦矩阵，的正式定义为： （25）式中B(t)是到时间间隔内任意时间的机体B系姿态。 矩阵还可以用定义的系姿态相对于系的旋转矢量来表示。对(4)式进行泰勒级数展开，可得系数项： (26) 式中, 定义为在时刻系姿态相对系的旋转矢量。旋转矢量可以通过把当作定义为大于时刻的普通的B系姿态相对于系普通旋转矢量来对待的办法计算出来。然后，是通过对普通的方程从时刻进行积分得来，而(26)式中用到的就是在时刻的积分结果。把用于定义的系当作非旋转惯性基准系来处理，一般可以得到下列表达式，其方法是将(10)式中的坐标系由机体系取代，而坐标系由惯性系取代，得到用角速率描述的下式： （27） 式中定义为在大于时刻的系姿态相对于系的旋转矢量。通常称之为鲍尔兹方程的(27)式使机体系的姿态的变化与系相对于惯性系的角速度产生联系，而可以通过捷联角速率传感器测得。 于是(26)式的姿态旋转矢量便可以通过从时刻到时刻对(27)式进行积分求得： ， （28）式中是运行中的积分时间变量。要减少(27)式中求算值的计算次数，应加入简化设计。例如，通过幂级数扩展，可以将(27)式中的项的放大系数近似为： （29）因此，(27)式的可以由下式给出： （30）通过仿真和分析(在假定的解析定义角运动条件下进行解析扩展)，可以得到保证值的二阶精度的表达式： （31）式中： （32）(31)式很重要，因为它能使(27)式简化到二阶精度，即的误差为三阶，只保持-阶项。这样一来，(27)式就变成二阶精度： （33）代入方程(33)，(28)式则变为下式： （34）最后，用(32)式可得： （35）其中： ， （36）式中是从到的圆锥姿态运动。项被锥化成锥化项目，因为它受到测量的圆锥运动分量的影响。圆锥运动定义为角速度矢量本身在旋转的状态。由于表现为纯圆锥运动(的幅度虽然恒定不变，但其矢量在旋转)，则系的一个固定轴近似垂直于矢量的旋转平面，该固定轴将随着角速度的运动而形成一个锥面(因此用术语圆锥运动来描述这个运动)。在圆锥角运动条件下，与垂直的系轴表现为振荡(这与非圆锥运动或自转角运动相反，在后一种情况下，于垂直的轴绕旋转)。对不发生旋转的情况而言，从(36)式可以容易看出，将与平行，因此中的积分函数的叉乘将是零，也将是零。这种情况下，即当不发生旋转时，(34)式将变成如下简化形式： （37）注意：（37）式也适用于当不旋转时，（27）和(28)式中的精确求解，即不存在近似问题。这点可以通过观察(27)式很容易地得到验证，因为在(27)式中，在积分开始时就与对齐，并将与保持平行，这是由于在表达式中，与叉乘将保持为零。在这种条件下，(27)式和(28)式也将化简为(37)式。 下面讨论积分角速率和圆锥增量算法。关于(36)式中的积分速率和的锥化表述的离散化数字算法可以通过这样的方法开发出来，即考虑是一般函数在=时刻的值，由（36）式得： （38）现在考虑将(38)式的积分分成在到时间间隔内到达时刻的时间和时刻之后的时间两部分，这样一来，(38)式等于： ， （39）式中：是时值，而是时的计算机的循环拍次。需注意，根据上述定义，循环拍次快于循环拍次。现在，我们再定义一个在到的时间间隔内循环时间点，于是在时间点，包扩初始条件在内的（39）式就变为： ， 当， （40） 通过类似过程，（40）式的表达式可以通过（36）式的计算求得： ， ， （41） 当，根据(41)式和(42)式得： ， （42） 当， (41)和(42)式构成数字递归算法，该算法以作为计算机循环速率计算和锥化项，作为从到的整个时间间隔内和的变化之和，(42)式中积分项的数字当量仍尚待确定。姿态算法开发方面的后续工作集中在估算锥化方程(42)中积分项的数字算法设计上，一一般而言，所采用的方法都是对从到时间间隔内的角速率模型假定一个一般的解析式，如一个在时间上截尾的一般多项式。于是(42)式的积分项就可以以解析的形式作为一般速率模型系数的一个函数，如多项式系数来表示。最后，角速率模型系数是通过匹配连续积分计算出的角速率增量的测量值来计算出来的。就随后的举例而言，角速率模型被近似为一个常数加一个实时线性累积值，其常数和梯度是由现有的和以前的值计算出来的。此算法的一个更为复杂的形式可能是在假设的角速率模型中包含有一个时变平方项，用现有的、过去的以及过去之过去的值判定系数。近来在这一领域进行的工作是通过利用到时间间隔内角速率传感器的测量值来计算角速率模型系数（参见文献【19】提出的一个用于强化单速算法的技术扩展），从而将第三个循环速率加到整个姿态修正过程的结构中：姿态()修正和锥化()修正(正如至此所作的讨论)以及锥化修正的传感器采样。后一种技术在改进时采用一个一般的角速率模型，该角速率模型是依据其对(42)式中积分项的影响直接定义为到时间间隔内连续积分角速率增量传感器采样值之间的加权叉乘之和(类似于文献【19】介绍的到时间间隔内的方法)。 在后一种情况下的加权系数于是得到优化并可以获取纯锥化环境下的最佳平均性能，亦即在幅度上为常数，但在旋转。之后提到的种种设计方法中的每一种都是以根据假定的角速率模型形状使用曲面拟合技术为基础的。每一种的最终算法在不是为某个速率环境而设计时和存在有角速率传感器量化噪声的情况下会表现不同。更为可取的算法的选择，务必包括仿真分析以确认运行速率环境下和传感器噪声特性状态下是否可以接受。现通过算法举倒对这一部分进行总结。这儿举出的是(42)式积分项算法的例子，它的机理是通过对幂级数进行截尾展开，使机体速率项被近似到一阶：， 和=常数 （43）参考文献【9】-【11】和【12】的第7.1.1.1.1表明对于时间段到的(43)式运动有： （44）将(44)式代入(42)式，可得： （45）(45)式从类别上可归为求的二阶算法，这是因为它在方程中包含有现在的和过去的循环乘积。从导出(44)式的分析可以看出，中的和乘积项即项是由到时间间隔内线性斜坡角速率的近似化得来的。如果角速率近似化成时间的二次曲线变化函数，那么，势必导致包含，和的乘积的三阶算法，如果角速率被近似成到整个时间间隔内的一个常数，那么，(45)式中的项势必导致求的一阶算法。最后，如果角速率正在慢慢变化，那么，我们可以将近似为等于零。换言之(或更准确地说)，我们可以让循环速率等于循环速率这使等于用(45)式在时刻计算出来的 注意根据(41)式初始状态定义为零，后一种算法在参考文献【4】得到了改进。须注意要设定和速率相等，还可以通过增加速率使之与速率相匹配的办法实现。结果得到一个单一的高速高阶算法，该算法附带有一个比两速算法更为简单的软件结构，但是，需要更大的吞吐量，现代计算机速度的不断提高使这一算法在未来成为更为可取的算法。 (35)式和的全部数字算法是由(41)、(42)和(45)式合并后得出的结果所决定的： ， ，当时 （46） ， ， 当时 （47）式中：微积分角速率增量，即：捷联角速率传感器给出的脉冲输出的解析表达式，即=由角速率传感器给出的积分角速率输出增量之和。4.1.2 当地水平系的旋转 （24）式采用式来修正姿态方向余弦矩阵，以考虑本地水平坐标系系相对于非旋转空间的角速率.的推导与第4.1.1节的推导非常类似。的正式定义是： （48）式中，是到时间间隔内任意时刻的系姿态。矩阵也可以用定义的系的姿态相对于系的旋转矢量来表示。对（4）式进行泰勒级数展开可得系数项： （49） 式中是定义时刻系的姿态相对于时刻系的旋转矢量。注意：在(49)式中，项的符号与(26)式表达式的同类项的符号相反，是负的。这是因为与（或(4)式）的指向相反，的矢量变换是从到，而的矢量变换是从到。这样，(49)式中的形式便是(26)式中表达式的转置。因为到的修正周期相对较短，在数值上将会很小。又因为小且在整个到的典型修正周期内变化缓慢(由于该时间周期内的矢量速度和位置变化小)，所以系速率矢量可以近似为非旋转。结果可以像旋转矢量方程(1O)的简化形式一样计算(49)，这里的叉乘项被忽略不计： （50）顺便指出，鉴于是个小量，如已讨论过的那样，用于的(49)式亦可简化。例如，可从（49）式导出一个二阶形式： （51） 对于现行的现代计算机技术而言，采用解算的(49)式简化形式如(51)式的计算机在内存或吞吐量方面的优点与增加软件有效性或文件复杂性以及精确度上损失相比较，是微不足道的。精确度损失在导航过程中一般来说是很小的；然而，在初始对准运算过程中(惯性导航启动之前)这种精度损失不可忽视，初始对准运算时矩阵常被用于对进行倾斜修正(参考文献【12】第6.1.2节和第15节l20121页)。对的初始倾斜对准修正量可能会很大，如0.1-1度，如果对(49)式采用过于简化的形式，在初始对准过程中就可能产生不希望的误差。初始对准运算的闭环伺服运动最终会修正由产生的结果性姿态误差；然而，它可能会将冗余正交误差留在行(和列)里面。其结果一定是需要包括一个正交或归一化修正算法(见第4.1.3节)作为修正过程的外层循环。(50)式积分的离散数字算法可以通过下述方法建立起来，即第一步先将(13)式和(15)式合并，求出被积函数，然后进行如下近似计算： (52)式中，下标是到的中途()的值。将(52)式代入(5O)式，然后可得： （53）用(14)式估算且 （54）式中，是计算机循环修正周期，而是在到的计算机修正循环内循环的循环次数。 (53)式下标为的项都是位置的函数，它(见下篇参考文献【13】)在以循环速率进行的姿态修正之后得到更新。因此，要计算(52)式中的这些项，必须在以前计算的（）参数的基础上，采用一个近似外推公式。例如，采用最后两次（）的计算值的线性外推公式，将变为： （55）在下篇(参考文献【13】)中，我们发现速度更新紧跟着位置更新。因此，现在的和过去的循环的数值可用于估算(54)式中的积分。对(54)式进行梯形积分可得： （56）式中