地磁辅助惯性导航系统的数据融合算法

成为广泛使用的滤波模式，但线性化的过程不可避免产生处理
误差，如果局部线性的假设不成立时，这种非线性近似误差可 能会造成滤波器发散。为了解决这个问题，国外学者提出了采 样卡尔曼滤波（ｕＫＦ）理论，并在多种试验中证明了它的优越性。
图１地磁辅助惯性导航方案
Ｆｉｇ．１ Ｓｃｈｅｍｅ ｏｆ ｇｅｏｍａｇｎｅｔｉｃ ａｉｄ ＩＮＳ
将扩展卡尔曼滤波中统计特性传播方式的线性化近似用Ｕ变换方法替换，就得到ｕＫＦ滤波器。其具体步骤如下： １）初始化状态向量及其方差
Ｋ＝研Ｋ】，只＝取（Ｋ—Ｋ）（Ｋ—Ｋ）１】
（５）
２）时间更新
①给出ｓｉｇｍａ点和相应的加权因子：
对于七∈｛０，１，…，∞｝，计算ｓｉｇｍａ点：
石，ｔ＝［】‘ｊ气＋√（力＋后）忍】≮一√（门＋Ｊｉ｝）丑】
在ｕ变换算法中，最重要的是确定ｓｉｇｍａ点采样策略，也就是确定使用Ｓｉｇｍａ点的个数、位置以及相应权值。Ｓｉｇｍａ点 的选择应确保其抓住输入变量的最重要的特征。
２．２ ＵＫＦ设计 Ｊｕｌｉｅｒ和Ｕｈｌｍ锄ｎ根据逼近状态的条件分布比逼近它的非线性函数更容易的思想，提出了基于ＵＴ的采样卡尔曼滤波方
法，也就是在卡尔曼滤波算法中，对于一步预测方程，使用ｕＴ变换来处理均值和协方差的非线性传递，就成为ｕＫＦ算法。
目前，非常规潜艇一般采用以惯性导航为核心的导航系统，但惯性导航系统的误差随时间积累，如不定期修正，就会限 制潜艇的应用。虽可采用ＧＰｓ、无线电和天文导航等信息对惯性导航系统进行校正，但这些方法增加了潜艇被发现的危 险性。随着无源导航技术的发展，地磁辅助惯性导航技术的出现为实现这一目标提供了新的技术途径。由于地磁场在全 球范围内各点值都不相同，理论上和经纬度是一一对应的，同时某些地区磁场特征也很明显，因此地磁辅助导航成为目 前研究的一个热点。利用地磁导航结果对惯导系统进行定时重调，就可以实现潜艇在水下长期隐蔽的目的。地磁导航技 术在获取地磁信息时对外无能量辐射，具有良好的隐蔽性，因此可以很好地辅助潜艇惯导系统实现长期高精度水下定位。
基于非线性状态方程的更新。仿真表明，特别是当系统非线性较大时，采样卡尔曼器性能明显优于扩展卡尔曼
滤波器。因此，从某种程度上说，采样卡尔曼滤波可以替代扩展卡尔曼滤波在地磁辅助导航系统中使用。
关键词：惯性导航；地磁导航；卡尔曼滤波；匹配算法
中图分类号：ｕ６６６．１
文献标识码：Ａ
Ｄａｔａ ｆｕｓｉｎｇ ａｌｇｏ“ｔｈｍ ｉｎ ｇｅｏｍａｇｎｅｔｉｃ ａｉｄｅｄ ＩＮＳ
收稿日期：２００６—０９—２６；修回日期：２００６—１２—２０ 基金项目：“十一五”武器装备预研项目 作者简介：杨功流（１９６７一），男，研究员，主要从事惯性导航系统、综合导航系统及无源辅助导航研究。
万方数据
２００７年２月
中国惯性技术学报
４８
航。根据对惯性导航系统、地磁模块的分析，组合导航系统的总体方案如图ｌ所示。
ｕＫＦ的基础是ｕ变换。ｕ变换是指在满足某些约束方程（约束方程与状态变量统计特性相关）的条件下，给出一组加 权ｓｉｇｍａ点（采样点），并对每一个ｓｉｇｍａ点进行非线性变换，以变换点的均值和方差来逼近状态变量非线性变换的均值和 方差。通过使用ＵＴ方法，状态向量的分布得到近似，而不是对采用类似于ＥＫＦ方法对状态方程和测量方程进行近似。
系统的量测方程为：
ｚ（，）＝【互ｚ２】１＝椴Ｏ）＋ｙ（，）
（１２）
其中，Ｚ、ｚ＇分别为量测到的系统经度误差、纬度误差：在此是由匹配位置信息与惯导位置信息之差所确定：日为２×１３ 维观测矩阵，Ⅳ（１，４）＝ｌ，Ⅳ（２，５）＝１，其余元素都等于零；ｙ（，）为量测噪声矩阵。有了状态方程和观测方程，不需要 线性化观测方程，利用上面所述的ＵＫＦ方法，就可得到导航状态的最优估计。
万方数据
２００７年２月
中国惯性技术学报
仿真运动为匀速直线运动，每２０ ｍｉｎ利用匹
配位置信息滤波一次，匹配位置信息由相关匹配
算法得到，地磁匹配精度受磁探测器误差、地磁 ０ ｕ
０ Ｖ
图误差以及匹配算法误差的影响。每隔３ｈ对惯导 求 蹬
３仿真结果分析 下面用仿真对上一节设计的ＵＫＦ进行验证。 仿真条件如下·陀螺常值漂移：气＝％＝％＝０．０１（。）／｝ｌ；陀螺随机漂移：三个陀螺均取盯，２０．００１（。）／Ｉｌ、相关时
间为ｌ ｈ的一阶马尔可夫过程；系统的初始方差阵为对角矩阵，其值为状态可能达到的误差平方；系统的噪声方差阵为：
Ｑ＝ｄｉａｇ｛２成群，２岛ｔ，２屈一｝，其相关时间展＝辟＝屈＝ｌ／３６００ ｓ；测量噪声方差阵为：胄＝ｄｉａｇ｛（２０ｍ）２，（２０ｍ）２｝。
ｆｉｌｔｅｒｉｎｇ ｂａＬｓｅｄ ｏｎ ｔｈｅ ｎｏｎｌｉｎｅａｒ ｓｔａｔｅ ｓｐａｃｅ ｅｑｕａｔｉｏｎ ｗａｓ ｒｅａｌｉｚｅｄ ｂｙ ｄｅｓｉｇｎｉｎｇ ａ ｆｅｗ ｓｉｇｍａ ｐｏｉｍｓ锄ｄ ｃａｌｃｕｌａｔｉｒｌｇ ｎｌｅ ｍｍｓｆｏｍａｔｉｏｎ ｏｆ ｔｈｅｓｅ ｓｉｇｍａ ｐｏｉｍｓ Ｖｉａ ｎｏｎ－ｌｍｅａｒ ｆｕｎｃｔｉｏｎ．Ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｓｈｏｗｓ廿ｌａｔ ｉｔ ｊｓ ｍｏｒｅ ｐｒｅｃｉｓｅ锄ｄ ｍｏｒｅ
状态均值周围的分布，通常取一个很小的正值；七是尺度参数，对于状态估计通常设定为Ｏ，对于系统参数估计则设为３一，ｚ，
，ｌ为系统状态维数；（√（＂＋旯）匕），是矩阵平方根的第ｆ列。矩阵平方根通过下三角ｃｈｏｌｅｓ时分解等算法获得。
②通过非线性变换，得到变换后采样向量点
鬈＝Ｆ（置）用惯导无阻尼误差方程，并将系统的陀螺漂移扩充为状态作为系统的状态方程。其状态变量Ｘ为１３维列矢量，
ｘ＝（口∥，６五６≯６■６■占。占。，占。占。￡，，占。）１
（１１）
其中，口是惯性平台水平误差角；ｙ是航向误差角；６旯、６矽是经度、纬度误差；６圪、６圪为东向、北向速度误差；
％、‰、气为东向、北向、方位陀螺的常值漂移；％、～、％为东向、北向、方位陀螺的随机漂移。
测量一步预测：
％２日［砧】，磊２互硝ｚｏ，Ｐｗ ２互哆（乇一彳）（ｚＩ—互）１＋胄”， ％＝∑２ｑＨ （死一Ｋ）．（ｚ＆—磊＾）１一，也＝％巧１，
Ｘ：＝ｘ：＋Ｋ ｋｑｋ—Ｚ：、，Ｐ：＝Ｐ；一ＫｔＰｗＫ：
㈧
２．３惯性／地磁组合导航系统的ＵＫＦ设计
系统的状态方程为：
ＸＯ）＝，（ｆ）·Ｘ（ｆ）＋ＧＯ）·∥Ｏ）
２地磁辅助惯性导航系统的融合算法设计 采样卡尔曼滤波器也是卡尔曼滤波器的扩展，但不同于ＥＫＦ，它不对系统方程进行线性化。ｕＫＦ依赖于ｕＴ方法，这
是一种进行非线性转换求任意变量统计量（均值与容差）的新方法。其具有以下几个特点：①对非线性函数的概率密度 分布进行近似，而不是对非线性函数进行近似，因此不需要知道非线性函数的显式表达式；②不需要求导计算Ｊａｃｏｂｉａｌｌ矩 阵，可以处理非可导的非线性函数；③可处理非加性噪声情况以及离散系统，扩展了应用范围；④由于采用确定性采样 策略，而非粒子滤波器的随机采样，避免了粒子衰退问题；⑤算量与ＥＫＦ同阶次。
１ 地磁辅助惯性导航系统的基本原理
地磁导航是指利用地磁图特征与当地即时测量磁特征信息进行匹配导航的过程。地磁场为矢量场，在地球近地空间 内任意一点的磁场矢量都不同于其它地点的矢量，且与该点的经纬度存在对应关系。因此，理论上只要精确确定该地点 的地磁场矢量即可实现全球定位。
惯性／地磁匹配组合导航是由惯性导航系统、地磁传感器、地磁图以及解算计算机等组成，其本质就是在载体运动过 程中，传感器获得地磁数据，同时根据惯性导航系统所输出的位置信息，从数字化的地磁图中某一区域内查找相应的数 据信息，利用匹配算法进行匹配以获得最佳匹配位置，并利用位置信息校正惯性导航系统。也就是说，将地磁图匹配结 果信息作为卡尔曼滤波的观测量对组合导航系统进行滤波，这样就可对惯性导航系统的误差进行抑制从而实现高精度导
ｐｅ面ｒｍａｎｃｅ ｃｏｎｖｅｎｉｅｎｔ ｉｎ ｃｏｍｐｕｔｉｎｇ，ａＩｌｄ ｔｈｅ
ｏｆ ｕｎｓｃｅｎｔｃｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ ｉｓ ｓｕｐｅｒｉｏｒ ｔｏ ｍａｔ ｏｆ ｅｘｔｅｎｄｅｄ ｏｎｅ，ｓｏ ｉｔ ｃａｎ
ｂｅ ｕｓｅｄ ｉｎ Ｇｅｏｍａｇｎｅｔｉｃ ａｊｄｅｄ ＩＮＳ．
Ｋｅｙ ｗｏｒｄｓ：ｉｎｅｎｉａｌ ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ；ｇｅｏｍａｇｎｃｔｉｃ ｎａｖ追ａｔｉｏｎ Ｓｙｓｔｃｍ；Ｋａｌｍ柚ｆｉｌｔｅｒｉｎｇ；ｍａｔｃｈｉｎｇ ａｌｇｏｒｉ廿１ｍ
Ａｂｓｔｒａｃｔ：Ｔｈｅ ｔｈｅｏｒｙ ｏｆ ｔｌｌｅ ｇｅｏｍａｇｎｅｔｉｃ ａｉｄｅｄ ｉｎｅｒｔｉａｌ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｃｍ（ＩＮＳ）ｗａｓ蚰ａｌｙｚｅｄ．Ｔｈｅ ｕｎｓｃｅｎｔｃｄ ＫａＩｍ觚 ｆｉｌｔｃｒｉｎｇ ｗａＳ ｉｎｔｒｏｄｕｃｅｄ ａ１１ｄ ａｐｐｌｉｅｄ ｉｎｔｏ ｍｅ ｉｌｌｔｃｇｒａｔｅｄ ｓｙｓｔｅｍ．Ｉｎ ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒｉｎ岛ｔ１１ｅ ｕｐｄａｔｉｎｇ ｏｆ ｔＩｌｅ
２．１ Ｕ变换简介
设已知竹维随机变量ｘ的均值ｘ和方差匕，ｘ经过非线性变换ｙ＝Ｆ（ｘ），到ｒ维随机变量Ｉ，，其均值ｙ和方差 Ｐｗ估计如下：
①计算２门＋１个采样点
ｌ‰＝ｘ
｛石＝ｘ＋（√（行＋兄），：）ｆ ｆ＝ｌ，２，…，”
（１）
Ｉ石＝ｘ一（√（胛＋五），：）ｆ一。 ｆ＝刀＋１，…，２刀
上式构成一个由２九＋１个向量石（称为仃点）组成的矩阵Ｚ，其中旯＝口２∽＋七）一”是一个尺度参数；口确定采样点在
（２）
③计算均值和协方差估计
一
２月
２月
一
一一
ｙ＝∑卵鬈； 匕＝∑研（髟一ｙ）（鬈一ｙ）１
（３）
，２０
Ｊ＝０
式中，矿、硝分别是均值和方差的权重；硝＝兄／０＋五），瞄＝旯／０＋旯）＋（１一口２＋∥），硝＝娣＝１／［２（门＋允）］， ｆ＝ｌ＇２’…，２门，参数∥包含ｘ分布的先验信息（对于高斯分布，∥取２最合适），如不采用比例修正，则可取矿＝衅＝哆。