CN110057354A - 一种基于磁偏角修正的地磁匹配导航方法 - Google Patents

Abstract

本发明公开了一种基于磁偏角修正的地磁匹配导航方法，本发明先采集地磁三轴矢量磁场强度数据，建立一个网格型的地磁数据库；利用电子罗盘对磁偏角也进行采样，建立一个网格型的磁偏角误差数据库。然后通过广义延拓逼近的方法对采集的离散数据，生成网格尽可能细分的数据库文件；在线导航定位阶段，用地磁传感器测得一串地磁场强信息序列，同时惯性器件获得的加速度和方向角进行航位推算结算出运动轨迹，将运动轨迹与地磁场强信息序列整合；通过误差补偿修正运动轨迹方向。将修正后的航位推算定位结果与修正后的地磁特征匹配定位结果进行扩展卡尔曼滤波融合，获得最终的定位结果。本发明提升了航位推算的精度，减少了计算复杂度。

Description

一种基于磁偏角修正的地磁匹配导航方法

技术领域

本发明属于地磁导航领域，涉及到基于一种磁偏角修正的地磁特征相似性匹配导航方法。

技术背景

随着21世纪的物联网持续发展以及智能终端的普及，用户对于基于位置的服务(LBS)需求不断提升。用户急切的需要在各种地理位置场合下能获得较高定位精度的位置信息服务，目前可采用的导航方式多种多样，卫星导航系统是其中运用较为普及且非常重要的方式，如北斗导航系统、GPS导航系统、GLONASS导航系统等卫星导航系统可全天候的为导航定位服务用户提供高精度的位置信息，由于智能手机与互联网的全球性普及，在人们的日常生活中卫星导航定位服务已经深深影响了人们的生活，同时影响着社会经济的发展。但是卫星导航在某一些特殊环境里例如在深山峡谷、城市高楼大厦集中的地区、百货商店、封闭式体育馆、管廊及地下车库等区域，卫星信号受到遮挡时，卫星的定位会出现较大的误差，甚至无法定位，或者当出现信号干扰以及战争等恶劣情况时，卫星可能无法提供正常的定位服务，还可能被利用输出错误的定位信息。但是在上述卫星信号受到遮挡影响的背景环境下，却往往是用户非常希望定位得到位置服务的区域，为了考虑国防安全以及一些民用领域等需求，研究探索新型的导航定位技术成为导航定位领域的热门研究内容。

地磁导航是其中的一种研究技术。地磁导航利用地磁场的特性。地磁场是矢量场，也是空间位置的函数。当前地磁场各种全球和局部的数学模型及数据库日趋完善，高灵敏度、高可靠性、体积小、价格低廉的地磁传感器相继出现，扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)等卡尔曼滤波技术以及地磁匹配算法趋于成熟，所以可以利用在近地空间上任何一个位置均有唯一的磁场矢量相互对应的映射关系，进行地磁导航。地磁导航技术不需要大量设备成本，无辐射，成本低、隐蔽性强、误差不随时间累积的特点。已经渐渐在海、陆、空、天等领域得到应用。

由于地磁导航中的地磁匹配相较于地磁滤波易于实现，在各个导航领域的应用前景较好，通常地磁导航往往不是作为一个单一使用的导航技术，大多数是辅助惯性器件进行导航，为其提供里程信息和运动方向，所以地磁导航往往与惯性导航一起使用。但是在地磁导航中，对惯性器件的精度相比较于纯粹的惯性导航来说，精度要低很多，这样是有益于导航系统的应用普及的。

在目前的地磁匹配导航中，通常采用的两种导航方式：一种是无里程信息的地磁匹配导航技术；另一种是有里程信息的地磁匹配导航技术。前者适用于单通道，如：管廊、地面单车道等，不具备横向空间的分辨能力，在导航面积范围较大的地方，则存在计算复杂度较高的难题。后者适用于二维电子地图、室内平面图，适用的应用领域范围比前者广。但是存在地磁数据库数学建模复杂、要求在离散和在线采集的数据时，地磁信息与里程信息应严格对应。同时还存在惯性器件随时间会累积误差导致运动方向出现偏差无法修正的问题。

发明内容

本发明的目的在于提供一种基于磁偏角修正的地磁匹配导航方法，以克服现有技术中存在的缺陷。

一种基于磁偏角修正的地磁匹配导航方法，该方法基于的装置包括一个三轴地磁传感器模块、一个包含陀螺仪和加速度计的惯性器件模块、一个电子罗盘模块。

所述的三轴地磁传感器模块，根据选定的待导航区域，对区域进行网格划分，地图上测量点的方向应该保持一致，保证地磁信息在空间的唯一性，进行采样点采样三轴地磁场强。

所述的电子罗盘模块，通过采集磁偏角信息，它对所测的待导航区域进行网格划分与三轴地磁传感器模块的网格划分位置相同，在测量的过程中，测量方向保持一致，且在同一个位置上磁偏角与三轴地磁矢量强度一一对应。

所述的包含陀螺仪和加速度计的惯性器件模块用于在模块进行初始定位的过程，以及后续模块持续运动中输出运动轨迹和航位角。

该方法具体包括以下步骤：

步骤S1：建立离散地磁数据库与磁偏角误差数据库模型；

步骤S11：根据三轴地磁传感器模块，在采集地磁数据的过程中，相邻测量点之间的间隔保持一致，同时保证在地图上测量点的三轴地磁传感器模块的测量方向保持一致，在测量点采集一组数据后，通过拉依达准则去除粗大误差，再对该测量点的数值求平均，就得到一个测量点的数据，最后通过广义延拓逼近的方法将网格中的数据点向附近邻近数据点作延伸，形成延拓域，这样能充分利用单个网格数据外的信息，使单个网格数据点内的逼近函数能充分吸收邻近网格数据点的信息，使得单个网格数据点内的逼近函数与邻近网格数据点逼近函数相协调，建立高精度的细分地磁数据库。

步骤S12：根据电子罗盘模块，在采集磁偏角误差数据的过程中，保证相邻测量点之间的网格间隔距离与三轴地磁传感器模块进行测量的网格间隔距离保持一致。同时保证测量点电子罗盘方向指向坐标系同一个方向，每个测量点采集一组数据，通过拉依达准则去除粗大误差，对该测量点的数值求平均，就得到一个测量点的磁偏角误差数据，再通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近建立高精度的磁偏角误差细分数据库。

步骤S2：对模块在地图上初始位置定位

通过包含陀螺仪和加速度计的惯性器件模块，在二维地磁图上运动一段距离，得到一段地磁数据序列(m₁,m₂,…,m_k)，以及测得一组加速度(a₁,a₂,…,a_k)和方位角数据序列(θ₁,θ₂,…,θ_k)，还包括以下步骤：

步骤S21：首先，通过模块所测的加速度和航向角数据通过航位推算方法：

解算出里程信息(d₁,d₂,…,d_k)，对应坐标为(x₁,x₂,…,x_k)、(y₁,y₂,…,y_k)，通过动态时间规整的方法将里程信息与地磁场强序列进行规整，保证规整后的序列长度与离散数据库中地磁序场强序列长度一致，在地图中将运动轨迹画出来；其中表示下一时刻x轴的位置，表示当前时刻的速度，t_n表示从模块运动开始到当前位置的时间，t_n+1表示从模块运动开始到下一个位置的时间，表示航向角的偏差，表示下一时刻y轴的位置。

步骤S22：利用Hausdorff距离的方法测得点集M与点集P双向Hausdorff距离：

H(M,P)＝max[h(M,P),h(P,M)]

其中M＝{m₁,m₂,…}代表实测得的地磁数据序列，P＝{p₁,p₂,…}代表离散数据库中的待匹配序列，H(P,M)称为双向Hausdorff距离，h(M,P)称为从点集M到点集P的单向Hausdorff距离,相应的h(P,M)称为从点集P到点集M的单向Hausdorff距离。得到一系列H值，取最小的H值，即得到模块运动初始定位位置。

步骤S3:根据步骤S2所述的模块初始定位位置，在后续运动中，根据在地图中航位推算短时的运动轨迹，对模块的航向角进行磁偏角误差补偿，修正模块的航向角。通过步骤S21将航位推算运动轨迹在地图上画出来，同时通过步骤S22的将模块的地磁匹配导航结算的位置输出出来，通过扩展卡尔曼滤波器将两者定位结果进行融合。

建立滤波模型如下,其中系统状态方程为：

式中，坐标系采用的是东北天坐标系，W＝[λE,λN,λL,λθ]^T是以位置偏差λE与λN、里程偏差λL、航向角偏差λθ组成的状态量，其中L代表里程，θ代表航向角，λw_N和λw_E代表位置噪声，λw_L代表里程的噪声，λw_θ代表航向角的噪声；G＝[ΔE,ΔN]^T为以地磁特征匹配定位与航位推算定位的两系统定位位置误差所组成的测量值。其中观测噪声为白噪声。

观测矩阵为：

其中状态转移矩阵为：

动态噪声矩阵为：

测量矩阵为：

通过融合模块航位推算定位结果和地磁匹配导航定位结果，得到最终模块的定位结果。

有益效果：本发明在二维的地磁匹配导航中，通过建立磁偏角误差数据库用于补偿模块运动当中航向角出现的偏差，提升了航位推算的精度，从而在地磁匹配定位过程中，减少了计算复杂度。同时克服了在路径匹配在线匹配过程中地磁信息需与里程信息严格相对应，将其转化为地磁序列之间相似性匹配。

附图说明

图1是本发明提出，针对在线测量过程中与离散过程采集地磁数据当中两个地磁序列长度不一致的问题，用动态时间规整的方法匹配寻找两序列之间物理上彼此最相似的点。

图2是本发明提出，在线匹配定位结算时，对地磁序列采用Hausdorff距离方法以滑动窗口的形式进行结算的过程。

图3是本发明提出，利用三轴地磁矢量强度组合匹配代替单一特征量匹配带来的误差，作为地磁匹配定位的结算过程。

图4是本发明提出，从离散地磁数据库与磁偏角误差数据库的数学模型建立，到最终定位结算的整体流程图。

图5是本发明提出，根据定位结算的整体流程图，画出地下车库的基于磁偏角修正的地磁匹配导航运动轨迹图。

具体实施方式

本发明设计了一种基于磁偏角修正的地磁匹配导航方法，通过建立离散地磁数据库和磁偏角误差模型，然后通过惯性器件得出的加速度和方位角，利用航位推算的原理，在二维地图上画出运动轨迹，将在线地磁序列进过时间规整之后，利用Hausdorff距离的方法对在线地磁序列进行平移和旋转，得出初始定位位置，在后续运动结算过程中，利用磁偏角误差补偿的方法，对航位推算的运动轨迹进行修正，然后再通过地磁匹配定位的方法进行定位结算，最后利用扩展卡尔曼滤波的方法将修正后的航位推算定位结果与地磁匹配定位结果融合得出最终的定位结果。

本发明的目的是克服现有的地磁匹配定位技术在二维地图上定位比较困难、计算复杂度较高面临的问题。如图1所示对在线地磁测量序列的间隔严格控制的缺点，设计一种具有高实时动态性、高精度、低复杂度的地磁匹配导航定位方法。

为了解决上述技术方面的难题，本发明采用了以下技术方案：

结合图4说明本实施方式，本实施方式的一种基于磁偏角修正的地磁匹配导航方法的具体步骤为：

步骤一、建立离散地磁数据库与磁偏角误差数据库模型

(1)、离散地磁数据库的建立：保证在采集地磁数据的过程中，测量点与测量点之间的间隔保持一致，间隔不宜过大，否则会影响到地磁数据库的精度。在建库的同时保证三轴地磁传感器模块的测量方向保持一致，在测量点采集一组数据后，通过拉依达准则去除粗大误差，再对该测量点的数值求平均，就得到一个测量点的数据，最后通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近建立高精度的细分地磁数据库。

广义延拓分片逼近方法是将每个子单元域向附近邻近单元作一些延伸，形成延拓域。这样能充分利用单元外的信息，使单元内的逼近函数能充分吸收邻近单元的信息，从而保证单元域上逼近函数能与邻近单元逼近函数相协调。

原理：设Ψ为定义域，把其区分成n个互不重叠的单元子域Ψ_e(e＝1,2,...,n)，则有：

把一定程度扩大后的延拓域记为Ψ'_e，则且有

设Ψ'_e内有m个节点，其中属于Ψ_e的节点有r个(r<m)，在延拓域Ψ'_e上构造逼近函数时，为使单元Ψ_e边界上函数值与各邻近单元函数值相一致，单元Ψ_e的m个边界点应满足边界点上插值限制因素。最后将每个延拓域中求得的逼近函数仅取单元定义域上的那部分，然后拼接起来，构成整个定义域的逼近函数。

设延拓域上Ψ'_e的逼近函数为：

其中，t为逼近函数得的项数，(h₁,h₂,…,h_p)是Ψ_e上的一组基函数；ε_i(i＝1,2,...p)为待定系数。对延拓域Ψ'_e的逼近函数实行在单元域Ψ_e的边界点上符合插值条件，而在其它点作最小二乘拟合处理，即可得单元域内的逼近函数的待定系数。

求出每个单元域内的逼近函数后，拼接可得整个定义域的逼近函数：

(2)、磁偏角误差数据库的建立：在采集磁偏角误差数据的过程中，保证测量点与测量点之间的网格间隔距离与离散地磁数据库的网格间隔距离保持一次。同时保证测量点磁罗盘方向指向坐标系同一个方向，每个测量点采集一组数据，通过拉依达准则去除粗大误差，对该测量点的数值求平均，就得到一个测量点的磁偏角误差数据，再通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近建立高精度的磁偏角误差细分数据库。

步骤二、在二维地磁图上运动一段距离，得到一段地磁数据序列(m₁,m₂,…,m_k)，以及用惯性器件模块测得一组加速度(a₁,a₂,…,a_k)和方位角数据序列(θ₁,θ₂,…,θ_k)。通过航位推算将运动轨迹在地磁图中画出来。

航位推算原理：假设：已知t_n时刻位置和当前时刻运动速度及航向角的情况下，可以通过航位推算求t_n+1时刻位置其中计算公式为：

步骤三、利用动态时间规整算法将运动轨迹上里程信息对应的数据库中地磁序列与在线所测得的地磁序列进行整合，如图1所示，保证两地磁序列长度相同。采用Hausdorff距离方法将在线所测得的地磁序列进行旋转、平移去跟二维地磁图中的离散地磁数据进行匹配，当匹配得到的值H最小的时候，两地磁序列匹配程度最高，便可以对模块运动位置进行定位。

假设离散数据库中待匹配的地磁序列为A＝[a₁,a₂,...,a_n]，在线测量得到的地磁数量为B＝[b₁,b₂,...,b_n]，他们的长度分别为n和m。

若n≠m,采用动态规划的方法，为使得这两个序列对齐，构造n×m的矩阵网络矩阵(i,j)元素为a_i和b_j两点间的距离d(a_i,b_j)，(即序列A的每一个点和B的每一个点之间的相似度关系，距离越小则相似度越高)，一般采用欧式距离，即d(a_i,b_j)＝(a_i-b_j)²。该方法主要寻找一条通过此网格中若干格点的路径，路径通过的格点即为两个序列进行计算的对齐的点。

首先定义一条路径为规整路径W，规整路径W的物理含义为：W_k＝(i,j)为两序列A与B的映射，有：

W＝W₁,W₂,…,W_o,…,W_k (6)

不过，在解算此最佳规整路径时，规整路径W需要满足以下条件限制：

1)边界条件限制：

离线阶段与在线阶段采集地磁的速度有所不同，但地磁特征排列的先后顺序不会改变。所利用边界条件可以限定所选的规整路径的起点是由矩阵网格的左下角出发，终点在矩阵网格的右上角结束。

2)单调性条件限制：

若W_o-1＝(r',c')，则路径下一点W_o＝(r,c)需满足：

此单调性条件限制了规整路径W是随着时间单调前行。

3)连续性条件限制：

若W_o-1＝(r',c')，则规整路径下一个点W_o＝(r,c)需满足：

此条件限制了规整路径W在匹配时只能与自己相邻的点对齐。保证序列A与B的每个坐标都在W中出现。

综合连续性和单调性约束，每一个格点的路径就只有三个方向。如果设路径已经通过了格点(i,j),那么下一个通过的格点只可能是下列三种情况之一：

(i+1,j),(i,j+1),(i+1,j+1)。

在动态时间规整算法中，满足上述三个约束限制的路径有数条，DTW所求的是其中规整代价最小的路径：

式中K为补偿系数，对不同长度的规整路径W做补偿。DTW算法的目的是求得将两序列规整后距离度量最小的规整路径，因此为求得此路径，定义累加距离：从(1,1)点开始匹配两序列A与B，规整路径每经过一点，之前所经过的每点的计算距离都会累加，至终点(n,m)时，累加距离即为所有匹配点的总距离之和，亦即序列A与B的相似度：

L(i,j)＝d(a_i,b_j)+min{L(i-1,j-1),L(i-1,j),L(i,j-1)} (11)

式中L＝(i,j)为点a_i与b_j的欧氏距离到达该点的最小的邻近元素的累积距离之和。求得最小累积距离之和即求得了最小规整代价的规整路径D，从而完成对离线阶段与在线阶段地磁数据的动态时间规整，进而求得两序列的相似性，完成地磁特征相关性匹配解算。

Hausdorff距离的定义是给定欧式空间中的两点集Q＝{q₁,q₂,…}，P＝{p₁,p₂,…}，Hausdorff距离就是用来衡量这两个点集间的距离。

H(Q,P)＝max[h(Q,P),h(P,Q)] (12)

其中H(P,Q)称为双向Hausdorff距离，h(Q,P)称为从点集Q到点集P的单向Hausdorff距离。相应的h(P,Q)称为从点集P到点集Q的单向Hausdorff距离。因为Hausdorff距离衡量的是两条序列之间的最大不匹配程度，因此当结算出来的H值越小时，两条序列匹配程度高；反之，得到的H值越大时，两条序列的匹配程度差。

采用如图2所示滑动窗口的形式，利用Hausdorff距离的方法对离散地磁序列和在线地磁序列进行相关性结算。在对地磁序列进行地磁匹配导航时，采用三轴矢量强度的方法，如图3所示进行层层筛选的方法，最终得到地磁匹配导航的定位结果，其中阈值λ_X和λ_Y的确定至关重要，太大或太小都影响匹配结果，甚至可能产生虚假结果。地磁日变具有在子夜附近几乎不变的特点，因此，阈值的设定参考子夜均值附近的日变大小。

步骤四、通过步骤三对整体模块运动初始位置定位，由于惯性器件在短时间内有较高的精度，在后续的地磁匹配定位中，在运动一段距离d后，根据运动路径中所对应的离散数据库中的磁偏角误差数据对航位推算的方位角进行补偿，修正运动轨迹。

步骤五、将修正后的运动轨迹所对应的离散地磁数据库的地磁序列与在线所测得的地磁序列进行动态时间规整，重复步骤三的过程，将在线测得的地磁序列与运动轨迹上的地磁数据进行对应，通过采用hausdorff距离的方法对运动轨迹上对应的离散数据库中地磁序列在附近小范围内进行平移和旋转，当匹配结算得到的值H最小的时候，地磁序列匹配程度最高，这样便得到地磁匹配的定位结果。

步骤六、将地磁匹配定位的结果与航位推算得出的定位结果通过扩展卡尔曼滤波算法进行滤波处理，滤除误匹配造成的定位误差，得到一个稳定、连续、高精度的定位结果。

建立滤波模型如下,其中系统状态方程为：

式中，W＝[λE,λN,λL,λθ]^T是以位置偏差λE与λN、里程偏差λL、航向角偏差λθ组成的状态量；G＝[ΔE,ΔN]^T为以地磁特征匹配定位与航位推算定位的两系统定位位置误差所组成的测量值。其中观测噪声为白噪声。

观测矩阵为：

其中状态转移矩阵为：

动态噪声矩阵为：

测量矩阵为：

使用上面的模型通过扩展卡尔曼滤波(EKF)对地磁匹配定位结果与航位推算定位结果进行融合处理，可以在磁匹配定位应用场景下保证了定位结果的可靠性、连续性、稳定性。

实施案例2：

结合图2、图3、图4来说明本发明，图5是基于磁偏角修正的地磁匹配导航方法，经过实际测量之后，最后经过仿真软件仿真，基于地下车库二维平面图画出来的运动轨迹图。在实验中，建立离散地磁数据库和磁偏角误差数据库时，均为间隔为1米的网格采样，一个采样点采集50个数据，通过拉依达准则去除粗大误差，对该测量点的数值求平均，然后将数据保存在数据库中。然后通过运动一段距离，进行地磁匹配定位，得出初始定位点。然后通过磁偏角误差补偿对后面的运动轨迹进行修正航位推算定位轨迹，通过修正后的定位轨迹做地磁匹配定位结算，最后将地磁匹配定位结果与航位推算定位结果融合得出最终定位结果，在基于磁偏角修正的地磁匹配导航方法的定位过程中，定位结算的误差始终保持在2.5米以内。

以上是本发明的实施例，并不应用于限制本发明，凡依本发明技术方案所做的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，所属领域的普通技术人员应当理解；依然可以对本发明的方案进行修改或者是部分技术的替换，凡非用于限制本发明的专利保护范围，本发明保护范围依然以权利要求书为准；凡是采用本发明附图内容或者是说明书所做的类似结构变动，同理均应该包含在该专利的保护范围内。

Claims (1)

1.一种基于磁偏角修正的地磁匹配导航方法，其特征在于：该方法基于的装置包括一个三轴地磁传感器模块、一个包含陀螺仪和加速度计的惯性器件模块、一个电子罗盘模块；

所述的三轴地磁传感器模块，根据选定的待导航区域，对区域进行网格划分，地图上测量点的方向应该保持一致，保证地磁信息在空间的唯一性，进行采样点采样三轴地磁场强；

所述的电子罗盘模块，通过采集磁偏角信息，它对所测的待导航区域进行网格划分与三轴地磁传感器模块的网格划分位置相同，在测量的过程中，测量方向保持一致，且在同一个位置上磁偏角与三轴地磁矢量强度一一对应；

所述的包含陀螺仪和加速度计的惯性器件模块用于在模块进行初始定位的过程，以及后续模块持续运动中输出运动轨迹和航位角；

该方法具体包括以下步骤：

步骤S1：建立离散地磁数据库与磁偏角误差数据库模型；

步骤S11：根据三轴地磁传感器模块，在采集地磁数据的过程中，相邻测量点之间的间隔保持一致，同时保证在地图上测量点的三轴地磁传感器模块的测量方向保持一致，在测量点采集一组数据后，通过拉依达准则去除粗大误差，再对该测量点的数值求平均，就得到一个测量点的数据，最后通过广义延拓逼近的方法将网格中的数据点向附近邻近数据点作延伸，形成延拓域，建立高精度的细分地磁数据库；

步骤S12：根据电子罗盘模块，在采集磁偏角误差数据的过程中，保证相邻测量点之间的网格间隔距离与三轴地磁传感器模块进行测量的网格间隔距离保持一致；同时保证测量点电子罗盘方向指向坐标系同一个方向，每个测量点采集一组数据，通过拉依达准则去除粗大误差，对该测量点的数值求平均，就得到一个测量点的磁偏角误差数据，再通过广义延拓逼近的方法将网格中的数据点进行分片最佳逼近建立高精度的磁偏角误差细分数据库；

步骤S2：对模块在地图上初始位置定位

通过包含陀螺仪和加速度计的惯性器件模块，在二维地磁图上运动一段距离，得到一段地磁数据序列(m₁,m₂,…,m_k)，以及测得一组加速度(a₁,a₂,…,a_k)和方位角数据序列(θ₁,θ₂,…,θ_k)，还包括以下步骤：

步骤S21：首先，通过模块所测的加速度和航向角数据通过航位推算方法：

解算出里程信息(d₁,d₂,…,d_k)，对应坐标为(x₁,x₂,…,x_k)、(y₁,y₂,…,y_k)，通过动态时间规整的方法将里程信息与地磁场强序列进行规整，保证规整后的序列长度与离散数据库中地磁序场强序列长度一致，在地图中将运动轨迹画出来；其中表示下一时刻x轴的位置，表示当前时刻的速度，t_n表示从模块运动开始到当前位置的时间，t_n+1表示从模块运动开始到下一个位置的时间，表示航向角的偏差，表示下一时刻y轴的位置；

步骤S22：利用Hausdorff距离的方法测得点集M与点集P双向Hausdorff距离：

H(M,P)＝max[h(M,P),h(P,M)]

其中M＝{m₁,m₂,…}代表实测得的地磁数据序列，P＝{p₁,p₂,…}代表离散数据库中的待匹配序列，H(P,M)称为双向Hausdorff距离，h(M,P)称为从点集M到点集P的单向Hausdorff距离,相应的h(P,M)称为从点集P到点集M的单向Hausdorff距离；得到一系列H值，取最小的H值，即得到模块运动初始定位位置；

步骤S3:根据步骤S2所述的模块初始定位位置，在后续运动中，根据在地图中航位推算短时的运动轨迹，对模块的航向角进行磁偏角误差补偿，修正模块的航向角；通过步骤S21将航位推算运动轨迹在地图上画出来，同时通过步骤S22的将模块的地磁匹配导航结算的位置输出出来，通过扩展卡尔曼滤波器将两者定位结果进行融合；

建立滤波模型如下,其中系统状态方程为：

式中，坐标系采用的是东北天坐标系，W＝[λE,λN,λL,λθ]^T是以位置偏差λE与λN、里程偏差λL、航向角偏差λθ组成的状态量，其中L代表里程，θ代表航向角，λw_N和λw_E代表位置噪声，λw_L代表里程的噪声，λw_θ代表航向角的噪声；G＝[ΔE,ΔN]^T为以地磁特征匹配定位与航位推算定位的两系统定位位置误差所组成的测量值；其中观测噪声为白噪声；

观测矩阵为：

其中状态转移矩阵为：

动态噪声矩阵为：

测量矩阵为：

通过融合模块航位推算定位结果和地磁匹配导航定位结果，得到最终模块的定位结果。