第７期 ２０１５年７月 组合机床与自动化加工技术 Ｍｏｄｕｌａｒ Ｍａｃｈｉｎｅ Ｔｏｏｌ＆Ａｕｔｏｍａｔｉｃ Ｍａｎｕｆａｃｔｕｒｉｎｇ Ｔｅｃｈｎｉｑｕｅ Ｎｏ．７ Ｊｕ１．２０１５ 文章编号：１００１—２２６５（２０１５）０７—０１１０—０４ ＤＯＩ：１０．１３４６２／ｊ．ｃｎｋｉ．ｍｍｔａｍｔ．２０１５．０７．０３０ 基于卡尔曼滤波的无人机姿态测量研究 袁亮，楚仕彬 ８３００４７） （大学机械工程学院，乌鲁木齐摘要：文章针对无人机姿态测量中受制于计算机存储和处理每个参数字节的，使得计算精度也 同样受到一定，舍入误差不可避免的存在于系统之中，对于卡尔曼滤波状态估计的误差可以由 测量更新过程来修正，然而对误差协方差矩阵Ｐ却没有相应的修正，随着迭代次数不断增加，将导致 Ｐ矩阵失真，打破其对角对称性。为提高无人机姿态测量的准确性，由此设计由陀螺仪、加速度计和 磁强计组成的姿态测量系统，采用ＵＤ分解扩展卡尔曼滤波方法，实现对四元数的估计，并通过实验 进行分析验证，实验结果表明该ＵＤ分解扩展卡尔曼滤波方法能够在低动态环境下准确解算出姿态 角度，并能有效克服舍入误差的影响。 关键词：卡尔曼滤波；无人机；姿态测量 中图分类号：ＴＨ１６５；ＴＧ６５９ 文献标识码：Ａ Ｒｅｓｅａｒｃｈ ｏｎ ＵＡＶ Ａｔｔｉｔｕｄｅ Ｃａｌｃｕｌａｔｉｏｎ Ｂａｓｅｄ ０１１ Ｋａｌｍａｎ Ｆｉｌｔｅｒ ＹＵＡＮ Ｌｉａｎｇ，ＣＨＵ Ｓｈｉ—ｂｉｎ （Ｍｅｃｈａｎｉｃａｌ Ｅｎｇｉｎｅｅｒｉｎｇ Ａｃａｄｅｍｙ，Ｘｉ￣ｉａｎｇ Ｕｎｉｖｅｒｓｉｔｙ，Ｕｒｕｍｑｉ ８３００００，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｓｕｂｊｅｃｔ ｔｏ ｔｈｅ ｌｉｍｉｔａｔｉｏｎｓ ｏｆ ｃｏｍｐｕｔｅｒ ｓｔｏｒａｇｅ ａｎｄ ｐｒｏｃｅｓｓｉｎｇ ｐａｒａｍｅｔｅｒｓ ｏｆ ｅａｃｈ ｂｙｔｅ，ｗｈｉｃｈ ｍａｋｅｓ ｔｈｅ ｐｒｅｃｉｓｉｏｎ ｏｆ ｃａｌｃｕｌａｔｉｏｎ ｉｓ ａｌｓｏ ｓｕｂｊｅｃｔ ｔｏ ｃｅｒｔａｉｎ ｒｅｓｔｒｉｃｔｉｏｎｓ．ｒｏｕｎｄｉｎｇ ｅｌｌ＇ｏｒｓ ｉｎｅｖｉｔａｂｌｙ ｅｘｉｓｔ ｉｎ ｈｅ ｔｓｙｓｔｅｍ，ｔｈｅ ｅｒｒｏｒｓ ｏｆ Ｋａｌｍａｎ ｆｉｌｔｅｒ ｓｔａｔｅ ｅｓｔｉｍａｔｉｏｎ ｃａｎ ｂｅ ｃｏｒｒｅｃｔｅｄ ｂｙ ｔｈｅ ｕｐｄａｔｅ ｐｒｏｃｅｓｓ ｏｆ ｍｅａｓｕｒｅｄ，ｂｕｔ ｔｈｅ ｅｒｒｏｒ ｃｏｖａｒｉａｎｃｅ ｍａｔｒｉｘ Ｐ ｃａｎ ｎｏｔ ｂｅ ｃｏｒｒｅｃｔｅｄ。ａｌｏｎｇ ｗｉｔｈ ｔｈｅ ｉｔｅｒａｔｉｏｎ ｎｕｍｂｅｒ ｉｎｃｒｅａｓｅｄ，ｔｈｅ ｃｏｖａｒｉａｎｃｅ ｍａｔｒｉｘ Ｐ ｗｉｌｌ ｂｅｃｏｍｅ ｄｉｓｔｏｒｔｉｏｎ．Ｉｔ ｉｓ ｄｉａｇｏｎａｌ ｓｙｍｍｅｔｒｙ ｗｉｌｌ ｂｒｏｋｅｎ．Ｉｎ ｏｒｄｅｒ ｔｏ ｉｍｐｒｏｖｅ ｔｈｅ ａｔｔｉｔｕｄｅ ｍｅａｓ— ｕｒｅｍｅｎｔ ａｃｃｕｒａｃｙ ｏｆ ＵＡＶ．ｔｈｕｓ ｄｅｓｉｇｎ ｔｈｅ ａｔｔｉｔｕｄｅ ｍｅａｓｕｒｅｍｅｎｔ ｓｙｓｔｅｍ ｃｏｍｐｏｓｅｄ ｏｆ ｇｙｒｏｓｃｏｐｅ ａｃｃｅｌｅｒｏｍｅ— ｔｅｒ ａｎｄ ｍａｇｎｅｔｏｍｅｔｅｒ．ｕｓｉｎｇ ｈｅ ｔｍｅｔｈｏｄ ｏｆ ＵＤ ｄｅｃｏｍｐｏｓｉｔｉｏｎ ＥＫＦ ｔｏ ｄａｔａ ｆｕｓｉｏｎ ｔｏ ｅｓｔｉｍａｔｅ Ｑｕａｔｅｒｎｉｏｎ ａｎｄ ｖｅｒｉｆｉｅｄ ｂｙ ｔｈｅ ｅｘｐｅｒｉｍｅｎｔ．Ｔｈｅ ｅｘｐｅｒｉｍｅｎｔａｌ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ｔｈｅ ＵＤ ｄｅｃｏｍｐｏｓｉｔｉｏｎ ＥＫＦ ｍｅｔｈｏｄ ｃａｎ ａｃｃｕ— ｒａｔｅｌｙ ｃａｌｃｕｌａｔｅ ｔｈｅ ａｔｔｉｔｕｄｅ ａｎｇｌｅ ｉｎ ｌｏｗ ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔ，ａｎｄ ｃａｎ ｅｆｆｅｃｔｉｖｅｌｙ ｏｖｅｒｃｏｍｅ ｔｈｅ ｉｎｆｌｕｅｎｃｅ ｏｆ ｒｏｕｎｄｉｎｇ ｅｒｒｏｒｓ． Ｋｅｙ ｗｏｒｄｓ：ｋａｌｍａｎ ｆｉｌｔｅｒ；ｕｎｍａｎｎｅｄ ａｅｒｉａ１ ｖｅｈｉｃｌｅ；ａｔｔｉｔｕｄｅ ｅｓｔｉｍａｔｅ ０ 引言 在实际应用卡尔曼滤波的时候，我们需要存储大 量的参数，并且需要进行大量的矩阵运算，在小型四旋 翼无人机的控制中，主控制器的规格一般较低，并不能 实现高精度的计算，只能采用精度较低（单精度３２ 位）的算法来执行卡尔曼滤波。这使得舍入误差不可 避免的存在于系统之中，随着迭代次数的增加，误差协 方差矩阵ＪＰ会逐渐失真，从而导致卡尔曼增益矩阵 失真。小的增益会使状态估计反应迟缓，大的增益会 产生不稳定、动荡的状态估计。极端的协方差矩阵失 真也能够导致软件系统的崩溃。因此在实际应用卡尔 曼滤波的过程中，要尽可能的减小误差协方差矩阵计 算的误差，特别是Ｐ必须保持正定。 为了消除舍人误差对滤波结果的影响，提高滤波 精度，在实际应用中可以使用高精度算法（６４位）来执 行卡尔曼滤波，文献［１］中提出采用约瑟夫式的协方 差更新，可以有效的提高计算精度，文献［１５］提出采 用平方根式的卡尔曼滤波，可以有效的提高计算精度， 并且能够保持误差协方差的对称性和正定性。 本文提出采用ＵＤ分解的方法，有效地传递 而 非Ｐ，可把动态范围减小两个数量级，这样舍入误差的 影响就小了 Ｊ。，该ＵＤ分解算法被成功应用于四旋翼 无人机的姿态测量中，有效的提高了无人机姿态测量 的精度。 ｌ 系统组成及姿态解算原理 本文所采用的姿态测量系统主要由三轴加速度 收稿日期：２０１４—０９—２６；修回日期：２０１４—１０—２７ 基金项目：国家自然科学基金（６１２６２０５９，３１４６０２４８）；优秀青年科技创新人才培养项目（２０１３７２１０１６）；自治区科技支疆项目（２０１５９１１０２） 作者简介：袁亮（１９７２一），男，乌鲁木齐人，大学副教授，博士，研究方向为机器人控制技术，计算机图像处理，（Ｅ—ｍａｉｌ）ｙｌｈａｐ＠１６３．ｃｏｍ。 ２０１５年７月 袁 亮，等：基于卡尔曼滤波的无人机姿态测量研究 冠 计、三轴陀螺仪和三轴磁强计组成。主控制器读取姿 ＝ ％２ ２ ＋ 态测量传感器的数据，通过卡尔曼滤波对传感器测量 一 数据进行融合处理，得到当前最优四元数，＋ 一 无人机的姿态角度。 ： 从而计算出 一 ％ ｇ ｇ ）、， 从载体坐标系到参考坐标系的姿态矩阵为：２ ２（ｑ１ｑ２一ｑｏｑ３） ｇ０２一ｑ ＋ｑ；一ｑ； ２（ｑ２ｑ３＋ｑｏｑ１） 姿态矩阵的方向余弦表达方式为：２ ２ ％ ｇ ｇ － ｒ ｃｏｓＯｃｏ￣ｃ０ｓ ｓｉｎ日ｓｉ 一ｃｏｓ＋ 胁 地 ／３ｓｉｎ￣ ｓｉ一 一 ｎｆｌｓｉｎｑｔ＋ｃ０ ｓｉｎＯｃｏｓ０１ ｌ ｃｏｓＯｓｉｍｐ ｃｏ￣ｃｏ￣，＋ｓｉ ｓ％ｉｎＯｓ％ ｉｎ￣ｃｏｓｌｆｓｉｎ０ｓｉｎ０一ｓｉｎｆｌｃｏｓｔ／，ｌ Ｌ—ｓｉｎＯ ｓｉｎ￣ｃｏｓＯ ））吼＋ ｇ ｃｏ￣ｃｏｓＯ Ｊ ２ ３（ １ ●●●●Ｊ １ （２） 式中 为航向角、０为俯仰角 为横滚角。 将四元数表示的姿态矩阵（１）和欧拉角表示的姿 态矩阵（２）比较，可以得到用四元数表示的欧拉角，表 达式为： …ｃｔａｎ（、 十ｑｌ 一ｇｑ０ ２—９ ／１ ０：一ａｒｃｓｉｎ（２（ｇ１ｇ３一ｇ。ｇ２）） （３） 卢…ｃｔａｎ（、 十ｑ ３ ｑ 一 ｏ ｑ 一ｇ１）２ 由欧拉法和四元数法可得欧拉角表示的四元数， 表达式如下： ｇ。＝ｃ。ｓ（等）ｃ。ｓ‘ ０）ｃ。ｓ（导）＋ｓｉｎ（２＂－￣－）ｓｉｎ（号）ｓｉｎ（导） （４） ９。＝ｃ。ｓ（ ）ｃ。ｓ ０）ｓｉｎ（譬）一ｓｉｎ（ ）ｓｉｎ‘ ０）ｃ。ｓ（譬） （５） 口：＝ｃ。ｓ（等）ｓｉｎ（号）ｃ。ｓ（Ｊ！＿）＋ｓｉｎ（２－￣－）ｃｏｓ（号）ｓｉｎ（譬） （６） ｇ３＝ｓｉｎ（２３￣－）ｃ。ｓ（号）ｃｏｓ（譬）一ｃ。ｓ（ ）ｓｉｎ（号）ｓｉｎ（譬） （７） 说明：参考坐标系定义 轴对应北、ｙ轴对应西、ｚ 轴对应天。 ２采用ＵＤ分解算法 受到处理器有效字长的，卡尔曼滤波在实际 滤波过程中存在舍人误差积累，随着卡尔曼滤波运行 次数不断增加，矩阵的失真就越大，从而出现误差协方 差矩阵Ｐ不能保持正定性，滤波出现不稳定的情 况 。 对Ｐ（ × 维）矩阵进行ＵＤ分解，即Ｐ＝ＵＤＵ ， 其中ｕ（ｎ×ｎ维）为单位上三角矩阵，且对角线元素为 １，Ｄ（ｎ×ｎ维）为对角阵，Ｐ为对阵正定矩阵。并且产 生唯一ＵＤ分解步骤如下： （１）对于第ｎ列，有 Ｄ ＝ ． （８） ＝睦…譬２… （２）对于其它列，Ｊ＝ｎ一１，ｎ一１，…，１，有 Ｊ＝ 一∑Ｄｋ，ｋ （１０） ｒ ０ １ ｉ＞Ｊ Ｕ Ｊ＝ ｌ ｋ弘１以 ＝“ ，＿ Ｌ Ｄ｜ ｉ ３ ＵＤ分解的扩展卡尔曼滤波算法 本卡尔曼滤波将四元数作为状态估计量，并且四 元数更新采用三阶毕卡算法，与此同时将加速度计和 磁强计的测量值作为观测量。滤波原理如图１所示。 图１卡尔曼滤波原理 ３．１卡尔曼滤波状态方程 目前求解四元数微分方程主要有两种方法：一种 是四阶龙格库塔法，另一种就是本文所使用的三阶毕 卡逼近法。其预测方程为： Ｘ ｃｌ，ｋｋ－１Ｘ 一１＋Ｗｋ，ｌ （１２） 由于状态估计量为四元数，则状态方程即为四元 数微分方程，其表达式如下： 哥（ ）＝— ｇ（ｔ） （１３） 采用毕卡逼近算法表示： ｒ ．ＡＯｏ １ ｉｃ０ｓ ＡＯ ４ 其中 ０ 一△ 一ＡＯｙ一△ ＡＯ＝ｆｔ２ｊ￣＇ｂｄ￡＝ △ ０ ＡＯｚ —ＡＯｙ ＡＯｙ一△ ０ △ △ ＡＯ 一ＡＯｘ ０ 表１毕卡各阶表达式 ・１１２・ 毕卡三阶算法的表达式为： 组合机床与自动化加工技术 尔曼滤波流程如下： ＵＤ分解算法的实现步骤。 （１）状态量的更新： ＝ 。 第７期 ｇ（ｎ＋１）＝｛（１＿ ），＋（÷一ａｇｏ ＡＯ｝ｑ（ ｌ７） 则状态转移矩阵 －＝。 （２８） ｛（１． ），＋（÷一等 ）（１８） （２）误差协方差矩阵的更新和它的ＵＤ分解： 根据传感器模型可知 ］： ｗ＝ ６一 （１９） 其中 是载体坐标系下角速度估计；Ｗ 是陀螺仪真实 输出；雷是陀螺仪零偏估计。Ｗ（ｔ）为白噪声，并且 Ｅ［Ｗ（ｔ）Ｗ（ｔ） ］：Ｑ。从状态转移矩阵可以看出，矩 阵 是随时间变化，并且四元数的更新过程是非线性 的。 ３．２卡尔曼滤波观测方程 将加速度计和磁强计传感器测量的值作为观测量 Ｚ（ｔ）。 Ｚ（￡）＝ＨＸ（ｔ）＋ （￡） （２０） 其中Ｖ（ｔ）为白噪声， Ｖ（ｔ）Ｖ（ ） １＝Ｒ。 由坐标系定义可知： Ｇ＝［０ ０ １】 （２１） ｈ＝［ｈ ｈ ｈ ｒ （２２） 其中Ｇ是参考坐标系下重力向量，ｈ是参考坐标下的 地磁场向量。 ， ｌｍ，］ ］…：Ｊ （２４） 其中 为 矩阵的转置矩阵，用来表示从参考坐标 系换算到载体坐标系下的姿态矩阵。ｇ是加速度计基 于载体坐标系的测量值，ｍ是磁强计基于载体坐标系 的测量值。则 ｒｇ ＝２（ｑ１ｑ３一ｑｏｑ２） ｛ｇｙ＝２（ｑ２ｑ３ ｑｏｑ１） （２５） ｔｇ ＝ｑ 一ｑ 一ｑ ｇ； ｒｍ ＝（ｇ ＋ｇ２ｌ—ｑ；一叮３２）＾ ＋２（ｑ。ｑ２＋ｑｏｑ３）＾ ＋２（ｑ，ｑ３一ｑｏｑ２）ｈ ｛ｍ ＝２（ｇｌｑ２一ｑｏｑ３）＾ ＋（ｑ 一ｇ２１＋ｑ：一ｑ；）ｈ ＋２（ｑｏｑｌ ｑ２ｑ３）ｈ ＝２（ｑ Ｊｑ３ ｑｏｑ２）＾ 十２（ｑ２ｑ３一ｑｏｑ１）ｈ 十（ｇ 一ｇ —ｑ；十ｑ；）＾： （２６） 由系统的观测过程可知，加速度计和磁强计的测 量值与四元数之间的关系也是非线性的。因此我们需 要利用雅克比矩阵将其转换成线性关系ｎ 。则： Ⅳ ３．３ 采用ＵＤ分解算法的扩展卡尔曼滤波流程 根据卡尔曼滤波的原理，基于ＵＤ分解的扩展卡 ｌ＝ １Ｐｋｌ 一ｌ＋ １＝Ｕｚ， Ｄ １ （２９） （３）求卡尔曼增益 ： ＝Ｄ ¨ Ｈ （３０） Ｇ ＝ ｌＦ （３１） Ｓ ＝日 Ｇ ＋Ｒ （３２） Ｋｋ＝Ｐｋ＇ｋｉ研（日 Ｐ 一。Ｈ ＋Ｒ ）～＝Ｇ ５ （３３） （４）计算最优估计： Ｘｋ＝ 一 ＋ （Ｚ 一 洲一１）＝ ，十Ｇ ５ （ 一ＨｋＸｒｊ ） （３４） （５）更新误差协方差矩阵ｐ： Ｐｋ＝（，一 Ｈ ）Ｐｋ／¨＝ ｌ（Ｄ 一Ｆｋ５ ） ㈦ （３５） 上述过程即为采用ＵＤ分解算法的卡尔曼滤波的 实现步骤，滤波器每次执行都按此过程循环。 （６）经过卡尔曼滤波之后，就能得到最优估计四 元数，再通过式（３）便可得到姿态角度。 ４实验 ４．１实验准备及参数设定 为验证本文的算法，使用“第七实验室”的开发板 ｃａｐｔａｉｎ，该开发板集成ＳＴＭ３２４０５ＲＧ（主控制芯片）、 ＭＰＵ６０５０（三轴陀螺仪和三轴加速度计）、ＨＭＣ５８８３ （磁强计），同时该开发板具有丰富的通讯接口，能够 将姿态数据通过ＵＡＲＴ上传至上位机进行分析。构建 验证系统如表２所示。 表２系统试验参数 表３卡尔曼滤波参数初始化 参数名 参数值 采样时间 Ｏ．Ｏ１ｓ ［（１一 ａＯｏ２ ＋（丁１一 ２）△ ］ Ｑ ［÷（０．０９） （０．０１）￣／４ ］ Ｒ 『０・【０１０ １３ ，０．００３２ ，厶 ，Ｊ １ ［ｑ０ ｑｌ ｑ２ ｑ３］ Ｐｏ ［ ］ ２ ２ ２ 一２ ０ ２ 一 ２ｑｌ ２ｑ０ ２ ２ ２ｑ。 一２ ． 一２ ２ 日 ２（口 。目 ＾，ｑ 。）２（ Ｊ＾ ｑ ｑ ｈ．）２（一ｑ ２＾ ｑｔｈ 一口 ．）２（一 ，ｑｌｈ．） ２（ｇ，＾ ｑ Ｌｈ ）２（ ＾。一ｑ，ｈ ．） ２（ｑ，ｈ ｑ ｈ２ ．） ２（ ｑ，ｈ， ．） 一２（ 一ｑ，ｈ ｑ．ｈ ） ２（ｑ，ｈ。一ｑ 一ｑ，ｈ．） ２（ｑ ＾ 一目 ） ２（ｑ．ｈ ｑ２ｈ， ｑ ）一 ２０１５年７月 袁 亮，等：基于卡尔曼滤波的无人机姿态测量研究 ・１１３・ ４．２软件实现 本软件开发平台为Ｋｅｉｌ ｕＶｉｓｉｏｎ４，姿态测量过程 分为系统初始化、姿态初始化、姿态更新三个过程，如 图２所示。 初始化陀螺仪 初始化加速度计 初始化磁强计 对加速度计、陀螺仪、 磁强计进行零偏标定 对ＰＯＲ矩阵进行初始化 利用加速度计测量值计算 横滚和俯仰角度 利用磁强计测量值判断 航向象限并计算航向角 根据欧拉角与四元数的 关系初始化四元数 利用三阶毕卡算法更新 姿态四元数并计算姿态矩阵 计算观测矩陈Ｈ 利用加速度计和磁强计的 测量值计算误差 更新误差协方差矩阵Ｐ， 并进行ｕＤ分解 计算 、Ｇｋ、 。 计算卡尔曼增益 计算最优估计四元数 计算姿态 图２姿态测量过程 根据参考坐标系定义，判断航向的象限如表４所 一一 １ ５ ０ ５ １ ５ ２ Ｓ Ｏ ５ ｎ 不。 表４航向象限的判断 ４．３实验结果 本实验采用一个能够实现匀速转动和加速转动的 转台，动态测量姿态和传感器的数据，本实验数据的采 集频率为１００Ｈｚ，采集次数６００次以上，为保证无人机 飞行过程中能够稳定、可靠的飞行，姿态变化不超过 土６０。，本实验均是在此姿态范围内进行实验。图３和 图４为静止时刻俯仰测量对比图，从图中我们可以看 出，采用ＵＤ分解算法的扩展卡尔曼滤波测量值更为 稳定和精确，能够有效的抑制舍入误差对系统稳定性 的影响；图５为在匀速转动的情况下的对比图，从图中 可以看出，在步进电机转动匀速改变姿态的过程中，采 用ＵＤ分解算法的扩展卡尔曼滤波测量值更为稳定， 在局部抖动的情况下，依然能够消除抖动，准确的测量 姿态；图６为加速运动情况下的对比图，从图中可以看 出，在加速运动的过程中，采用ＵＤ分解算法的扩展卡 尔曼滤波能够较为准确的跟踪姿态，不会因加速运动 出现剧烈的角度变化，未采用ＵＤ分解的算法虽然也 能大体跟踪姿态，但是其抖动较为明显，不适宜在无人 机上使用。 ＵＤ分解俯仰角度的测量值 ／ Ｉ３ 抽 蚴ＪｌＩＩｉＩ ‘ｌ１ ｒｆｆ１ ｌ 阳ｆ１ｌｆ １－ １『 Ｉ『Ｉｆ ｌ ｌ 一 束进行ｕ。＼ 分解俯仰角 蔓的测量值 图３静止时刻俯仰测量对比图 嘲 妇 ｒ’ ■ｌ １ Ｉ 一 ’ ！ｉ『 １ — 图４静止时刻局部放大图 图５ 匀速运动情况下的测量值 ／’ 耒进行ｕ。分解俯仰的测量值 ～ —— ／ ｌ ７ ｌ。ｌ。 ６ｌ９ ２２￣２８： ：：，； ５５ 。 １ ６４６ ’。７３ ７６’ ８２ ． ！：： 二，＿进行ｕ。分解俯仰的测量值 ／ 图６加速运动情况下的测量值 ５结论 本文研究基于ＵＤ分解算法的扩展卡尔曼滤波在 四旋翼无人机姿态测量中的应用，通过实验，发现采用 ＵＤ分解算法的扩展卡尔曼滤波能够更为稳定的测量 姿态，能够有效抑制舍入误差带来的影响，防止Ｐ矩 阵发散，提高姿态测量精度。 ［参考文献］ ［１］李涛，练军想，曹聚亮，等．ＧＮＳＳ与惯性及多传感器组合 导航系统原理［Ｍ］．北京：国防工业出版社，２０１１． ［２］冯鹏，邹世开．卡尔曼滤波器ＵＤ分解滤波算法的改进 ［Ｊ］．遥测遥控．２００５，２６（１）：１０—１４．（下转第１１７页）
