CN105865444B - 基于仿射变换的惯性/地磁匹配迭代定位方法 - Google Patents

Abstract

本发明涉及一种基于仿射变换的惯性/地磁匹配迭代定位方法，步骤包括：从惯导系统读取待匹配位置测量值，从磁强计获得地磁场强度信息；根据惯导系统指示的位置，从地磁数据库中读取地磁场强度和地磁场强度的梯度；引入并初始化匹配位置偏移量、角度偏移量和伸缩系数；计算迭代参数；计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量；更新匹配位置偏移量、角度偏移量和伸缩系数；根据终止迭代条件判断是否更新迭代变量，进行重复迭代；计算输出匹配定位结果。本发明能同时修正惯导系统的初始位置误差、初始航向误差和初始速度误差，提高载体的定位精度；用数值迭代方法取代遍历搜索的求解手段，减小了匹配定位时间，提高了实时性。

Description

基于仿射变换的惯性/地磁匹配迭代定位方法

技术领域

本发明涉及组合导航定位技术领域，尤其涉及一种基于仿射变换的惯性/地磁匹配迭代定位方法。

背景技术

地磁场是地球的基本物理场，处在近地空间内任意一点都具有磁场强度，因此地磁场构成了一个天然的导航坐标系。以地磁场为基础的地磁导航具有无源、无辐射、隐蔽性强、误差不随时间累积等众多优点。然而由于地磁场分布存在平缓区域、地磁场容易受到外界干扰磁场影响等原因，单独使用地磁场信息导航有可能产生可靠性差等问题。若将地磁导航技术和惯导(即惯性导航的简称)系统组合，一方面可以根据地磁导航信息及时对惯导系统进行修正，消除惯导系统运行过程中的累积误差，另一方面，可以根据惯导系统的导航信息将地磁匹配区域限定在一定范围内，提高地磁导航的可靠性和精度，从而实现两种导航的优势互补。

惯性/地磁匹配定位需要根据惯导系统输出的参考轨迹形状，在预存的地磁图中进行相关性匹配，确定参考轨迹在预存的地磁图中的匹配位置，从而计算出运动载体的实时位置信息。

惯性/地磁匹配方法主要有ICCP匹配方法和轮廓线匹配方法。ICCP匹配方法求取测量轨迹与最近等值点轨迹之间的最优刚性变换，通过该变换校正测量轨迹来实现匹配的目的，然而该算法以磁场测量等值线上的最近点作为匹配目标，因此算法收敛于磁场测量等值线上的最近点而不收敛于真实轨迹，因此ICCP匹配方法的精度无法得到保证。轮廓线匹配方法要求根据惯导系统输出的参考轨迹形状，遍历有效范围内所有平行于参考轨迹的序列，并通过相关性准则确定最优的匹配结果。首先，该方法采用遍历的手段进行大范围搜索，匹配速度慢，实时性差；其次，通常该方法只用来修正惯导系统的位置误差，匹配精度差。

发明内容

为了克服现有技术下的上述缺陷，本发明的目的在于提供一种基于仿射变换的惯性/地磁匹配迭代定位方法，该方法可以同时修正惯导系统的初始位置误差、初始航向误差和初始速度误差，提高载体的定位精度，同时采用迭代算法求解匹配结果，提高载体定位的实时性。

本发明的技术方案是：

一种基于仿射变换的惯性/地磁匹配迭代定位方法，包括如下步骤：

步骤1，从惯导系统读取当前时刻以及前n-1个时刻的待匹配点的位置测量值x_i和y_i，其中x_i表示经度，y_i表示纬度，下标i表示不同时刻，i＝1…n，n为整数且n＞2，i为1表示当前时刻；通过磁强计获得当前时刻以及前n-1个时刻的地磁场强度的测量值T_r,i(即对应上述n个待匹配点的地磁场强度的测量值T_r,i)；

步骤2，根据惯导系统指示的n个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值T_m,i、地磁场强度的梯度的参考值T_x,i和T_y,i，其中T_x,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，T_y,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；

步骤3，引入并初始化匹配位置偏移量、角度偏移量和伸缩系数M：

M＝[Δx Δy α k₁ k₂]^T＝[0 0 0 1 1]^T (1)

步骤4，根据公式(2)、公式(3)和公式(4)计算迭代参数G、F和H，

G＝g(M) (2)

F＝f(M) (3)

H＝F^-1 (4)

其中，

g(M)＝[g₁(M) g₂(M) g₃(M) g₄(M) g₅(M)]^T；

x′_i＝x_i-x_n，y′_i＝y_i-y_n，T_t,i＝T_m,i-T_r,i；

步骤5，计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM：

δM＝-H×G (5)

步骤6，更新匹配位置偏移量、角度偏移量和伸缩系数M：

M＝M+δM (6)

步骤7，判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤10，否则跳到步骤8；

终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM的2范数小于设定值，即

||δM||₂＜ε (7)

其中ε为预先设定的迭代最小误差；

步骤8，根据更新后的M计算参数N和δN：

N＝g(M) (8)

δN＝N-G (9)

步骤9，根据公式(10)和公式(11)更新迭代变量H和G，并跳到步骤5，

H＝H+[(δM)-H(δN)](δM)^TH/[(δM)^TH(δN)] (10)

G＝N (11)

步骤10，根据迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M，按公式(12)计算输出匹配结果，

其中x_r,i为第i时刻匹配结果的位置经度，y_r,i为第i时刻匹配结果的位置纬度。

所述地磁场强度优选为地磁场总强度、地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量；

当所述地磁场强度为地磁场总强度时，所述磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得所述地磁场总强度，作为所述地磁场强度的测量值T_r,i；

当所述地磁场强度为地磁异常场总强度时，所述磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得地磁场总强度，并根据地球磁场模型计算出地磁异常场总强度，作为所述地磁场强度的测量值T_r,i；

当所述地磁场强度为地磁场总强度在地理坐标系下某一方向的分量时，所述磁强计采用三轴矢量磁强计，依据所述磁强计的三轴矢量测量值以及载体的姿态，计算出地磁场总强度在地理坐标系下该方向的分量，作为所述地磁场强度的测量值T_r,i。

上述任意一种所述的基于仿射变换的惯性/地磁匹配迭代定位方法还可以包括二次迭代步骤，所述二次迭代步骤的内容为：第二次重复执行所述步骤1-10，与第一次执行所不同的是，用第一次执行所得到的匹配定位结果x_r,i和y_r,i替代所述步骤1中从惯导系统读取的x_i和y_i。

本发明的有益效果为：

与轮廓线匹配方法相比，由于本方法引入了匹配位置偏移量、角度偏移量和伸缩系数，分别用于反映参考轨迹相对于真实轨迹的平移变化、旋转变化和伸缩变化，而这三种变化分别由惯导系统的初始位置误差、初始航向误差和初始速度误差导致，因此，同时引入匹配位置偏移量、角度偏移量和伸缩系数并使它们都参与到匹配定位的最优化求解过程中，并通过一系列特定的算法，使匹配结果收敛于真实轨迹，还同时修正了使惯导系统的初始位置误差、初始航向误差和初始速度误差，相比传统方法显著提高了定位精度。

本发明的方法还采用了传统定位方法所没有的迭代计算方式，与传统的遍历搜索方法相比具有明显优势，该迭代计算方式与前述同时引入多个新的变量以修正惯导系统多种初始误差的特点有机结合，不可分割，使本发明的方法具有突出的实质性特点和显著的进步。例如，如果在传统的定位方法中引入角度偏移量和伸缩系数进行搜索，由于没有从根本上改变遍历搜索方式，计算量将会大幅提高，导致无法完成在线计算，也就无法实现实时定位。

通过二次迭代计算匹配位置偏移量、角度偏移量和伸缩系数，由于二次迭代用的初值是经过第一次迭代获得的已经消除了大部分初始位置误差、大部分初始航向误差和大部分初始速度误差的值，再次通过迭代计算，其结果比第一次迭代更接近真实轨迹，使匹配定位结果精度得到进一步提高。

由于以数值迭代方法替代了轮廓线匹配方法中的遍历求解手段，明显改善了定位算法的计算效率，显著提高了定位过程的实时性，即使角度偏移量和伸缩系数同匹配位置偏移量一样都加入搜索，也不会导致计算量的大幅提高，仍然能够很好地满足在线计算的要求，为在线计算即满足实时性要求提供了前提保证，是轮廓线匹配方法所不能比拟的。

附图说明

图1是本发明的定位方法的流程图；

图2是采用本发明的定位方法的经度误差曲线图举例；

图3是采用本发明的定位方法的纬度误差曲线图举例。

具体实施方式

如图1所示，本发明公开了一种基于仿射变换的惯性/地磁匹配迭代定位方法，包括如下步骤：

步骤1，从惯导系统读取当前时刻以及前n-1个时刻的待匹配点的位置测量值x_i和y_i，其中x_i表示经度，y_i表示纬度，下标i表示不同时刻，i＝1…n，n为整数且n＞2，i为1表示当前时刻；通过磁强计获得当前时刻以及前n-1个时刻的地磁场强度的测量值T_r,i；

步骤2，根据惯导系统指示的n个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值T_m,i、地磁场强度的梯度的参考值T_x,i和T_y,i，其中T_x,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，T_y,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；

步骤3，引入并初始化匹配位置偏移量、角度偏移量和伸缩系数M：

M＝[Δx Δy α k₁ k₂]^T＝[0 0 0 1 1]^T (1)

T为转置，Δx、Δy对应于匹配位置偏移量，α对应于角度偏移量，k₁、k₂对应于伸缩系数，k₁和k₂大于1表示真实轨迹比参考轨迹长；小于1表示真实轨迹比参考轨迹短，等于1表示真实轨迹和参考轨迹等长。

步骤4，根据公式(2)、公式(3)和公式(4)计算迭代参数G、F和H，

G＝g(M) (2)

F＝f(M) (3)

H＝F^-1 (4)

其中，

g(M)＝[g₁(M) g₂(M) g₃(M) g₄(M) g₅(M)]^T；

x′_i＝x_i-x_n，y′_i＝y_i-y_n，T_t,i＝T_m,i-T_r,i；

步骤5，计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM：

δM＝-H×G (5)

步骤6，更新匹配位置偏移量、角度偏移量和伸缩系数M：

M＝M+δM (6)

步骤7，判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤10，否则跳到步骤8；

终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM的2范数小于设定值，即

||δM||₂＜ε (7)

其中ε为预先设定的迭代最小误差；

步骤8，根据更新后的M计算参数N和δN：

N＝g(M) (8)

δN＝N-G (9)

步骤9，根据公式(10)和公式(11)更新迭代变量H和G，并跳到步骤5，

H＝H+[(δM)-H(δN)](δM)^TH/[(δM)^TH(δN)] (10)

G＝N (11)

步骤10，根据迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M，按公式(12)计算输出匹配结果，

其中x_r,i为第i时刻匹配结果的位置经度，y_r,i为第i时刻匹配结果的位置纬度。

所述地磁场强度优选为地磁场总强度、地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量；

当所述地磁场强度为地磁场总强度时，所述磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得所述地磁场总强度，作为所述地磁场强度的测量值T_r,i；相应地，预先存储的地磁场强度和地磁场强度的梯度应为地磁场总强度和地磁场总强度的梯度。

当所述地磁场强度为地磁异常场总强度时，所述磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得地磁场总强度，并根据地球磁场模型计算出地磁异常场总强度，作为所述地磁场强度的测量值T_r,i；相应地，预先存储的地磁场强度和地磁场强度的梯度应为地磁异常场总强度和地磁异常场总强度的梯度。

当所述地磁场强度为地磁场总强度在地理坐标系下某一方向的分量时，所述磁强计采用三轴矢量磁强计，依据所述磁强计的三轴矢量测量值以及载体的姿态，计算出地磁场总强度在地理坐标系下该方向的分量，作为所述地磁场强度的测量值T_r,i；相应地，预先存储的地磁场强度和地磁场强度的梯度应为地磁场总强度在地理坐标系下该方向的分量及该分量的梯度。

所述基于仿射变换的惯性/地磁匹配迭代定位方法还可以包括二次迭代步骤，所述二次迭代步骤的内容为：第二次执行所述步骤1-10，与第一次执行(即一次迭代)所不同的是，用第一次执行所得到的匹配定位结果x_r,i和y_r,i替代所述步骤1中从惯导系统读取的x_i和y_i(如式(13)所示)，即将一次迭代得到的修正后的待匹配点位置值作为二次迭代的初始值。由于二次迭代用的初值是经过第一次迭代获得的已经消除了大部分初始位置误差、大部分初始航向误差和大部分初始速度误差的值，再次通过迭代计算，其结果比第一次迭代更接近真实轨迹，使匹配定位结果精度得到进一步提高。

本发明的原理为：引入匹配位置偏移量、角度偏移量和伸缩系数三个变量，按照匹配位置偏移量、角度偏移量和伸缩系数对惯性导航系统的指示轨迹进行平移、旋转和伸缩变换以构建匹配轨迹，将指示轨迹上各点位置值和匹配轨迹上对应点的位置值代入由指示轨迹上各点的地磁场特征量测量值与匹配轨迹上相对应点的地磁场特征量参考值共同构建的误差指标函数(即最优化求解的目标函数)，其中匹配轨迹上对应点的位置值由指示轨迹上各点位置值与匹配位置偏移量、角度偏移量和伸缩系数决定，通过迭代计算得到最优的匹配位置偏移量、角度偏移量和伸缩系数，即进行最优化求解，得到使所述误差指标函数最小的匹配位置偏移量、角度偏移量和伸缩系数，就相当于得到了最优匹配轨迹。

为了便于数值计算，本发明的所述误差指标函数是经过简化处理的。例如可以以指示轨迹上多点序列中每一点的地磁场特征量的测量值与匹配轨迹上对应点的地磁场特征量的参考值之差的平方再各点取和作为所述误差指标函数。因匹配轨迹相应的地磁场特征量的参考值随匹配轨迹位置的变化而变化，而匹配轨迹位置又是匹配位置偏移量、角度偏移量和伸缩系数的函数，因此，可通过多项式展开(如泰勒展开)并省略二阶以上项、离散化等类似处理，将目标函数简化为仅有匹配位置偏移量、角度偏移量和伸缩系数变量的多变量表达式，形成计算中实际使用的目标函数，再利用使该目标函数对上述各变量的一阶偏导为零，将地磁匹配问题转化为含上述多个变量的非线性方程组的求解问题。经过上述变换处理后，迭代计算过程更为简单明了。作为本发明的一种实施例，经过上述变换处理后，对所述非线性方程组迭代求解可以得到公式(2)-(5)。在利用公式(2)-(5)进行本方法的迭代计算时不再需要列目标函数，不需要求取偏微分，只需要进行多次的代数运算即可得到最终匹配结果。

所述迭代计算以惯导系统指示轨迹上各个待匹配点对应的位置测量值、地磁场特征量测量值、来自地磁数据库的地磁场特征量参考值为已知参数，计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量，用所述增量对应更新所述匹配位置偏移量、角度偏移量和伸缩系数，将更新后的所述匹配位置偏移量、角度偏移量和伸缩系数投入到迭代计算中的下一循环，直至满足当次迭代计算的终止条件，用最后得到的所述匹配位置偏移量、角度偏移量和伸缩系数对各个待匹配点的位置进行修正，将该修正后的待匹配点位置值作为对应于当次迭代计算的匹配定位结果。修正后的待匹配点位置值即为最优匹配轨迹上的点的位置值。所述更新优选为线性更新。

迭代计算中所述匹配位置偏移量和角度偏移量的初始值优选为0，所述伸缩系数的初始值优选为1。

所述地磁场特征量可以包括地磁场总强度、地磁异常场总强度、水平磁场强度、东向分量、北向分量、垂直分量、磁偏角及磁倾角中的一种或多种。所述地磁场特征量参考值至少可以包括地磁场强度参考值和地磁场强度的梯度参考值。所述迭代计算的终止条件可以是以下任意一种或两种条件的组合：条件(1)，迭代次数达到预设次数；条件(2)，所述匹配位置偏移量的增量和角度偏移量的增量小于预先设定值。

依照图1，以某实验跑车为例实施本发明的一种基于仿射变换的惯性/地磁匹配迭代方法的过程如下。

实验条件：地磁场选用地磁异常场总强度。选用质子磁力仪实时测量磁场信息，质子磁力仪的主要性能指标如下：分辨率：0.01nT，精度：±0.2nT。惯导系统的主要性能指标如下：陀螺零偏不稳定性：0.008°/h，陀螺随机游走：加速度计零偏不稳定性：10μg，加速度计随机游走：

将经度范围107.5°到109.5°和纬度范围39.3°到41.3°区间的地磁异常场总强度数据存入机载计算机，采用前向差分方法计算地磁异常场总强度的梯度信息并存入机载计算机；选取待匹配点个数为10个，即n＝10。

采用一种基于仿射变换的惯性/地磁匹配迭代定位方法，步骤如下：

步骤1：从惯导系统读取当前时刻以及前9个时刻的待匹配点的位置测量值x_i和y_i，如表1所示；根据质子磁力仪的测量值和地球磁场模型，得到当前时刻以及前9个时刻的磁场强度测量信息T_r,i，如表2所示。

表1惯导系统测量位置坐标

表2磁场强度测量信息

步骤2：根据惯导系统指示的10个位置，从预先存储的地磁数据库中分别读取相应位置的地磁场强度信息T_m,i，以及该位置的地磁场强度的梯度信息T_x,i和T_y,i，如表3所示。

表3地磁数据库中的地磁场强度和梯度信息

步骤3：初始化匹配位置偏移量、角度偏移量和伸缩系数M：

M＝[Δx Δy α k₁ k₂]^T＝[0 0 0 1 1]^T；

步骤4：根据公式(2)、公式(3)和公式(4)计算迭代参数G、F和H，

步骤5至步骤9：选取ε＝0.001，预设迭代次数为50次。执行公式(5)、公式(6)、公式(8)至公式(11)，实施迭代算法。并根据公式(7)判断迭代终止条件，可知当迭代次数为11时迭代终止。迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M为

M＝[-0.0038 0.0050 -0.0244 0.9143 1.0362]^T

步骤10：根据迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M，计算匹配结果如表4所示。

表4匹配结果位置坐标

上述迭代计算在计算机中耗时为16.9毫秒。

为了验证实验结果，在实验跑车上安装GPS导航定位系统，从而可以得到10个时刻的真实位置坐标，如表5所示。

表5跑车真实位置坐标

采用二次迭代来提高匹配定位方法的精度。二次迭代时，将步骤1中从惯导系统读取的待匹配点的位置测量值x_i和y_i由第一次迭代所得到的匹配定位结果x_r,i和y_r,i来替代。二次迭代步骤如下：

步骤1：用第一次的迭代结果(见表4)替代从惯导系统读取的10个时刻的待匹配点的位置测量值x_i和y_i，如表6所示；根据质子磁力仪的测量值和地球磁场模型，得到当前时刻以及前9个时刻的磁场强度测量信息T_r,i，如表2所示。

表6二次迭代时的待匹配点位置坐标初值

步骤2：根据惯导系统指示的10个位置(对应表6中的10个位置)，从预先存储的地磁数据库中分别读取这10个位置的地磁场强度T_m,i，以及该位置的地磁场强度的梯度T_x,i和T_y,i，如表7所示。

表7地磁数据库中的地磁场强度和梯度信息

步骤3：初始化匹配位置偏移量、角度偏移量和伸缩系数M：

M＝[Δx Δy α k₁ k₂]^T＝[0 0 0 1 1]^T；

步骤4：根据公式(2)、公式(3)和公式(4)计算迭代参数G、F和H，

步骤5至步骤9：选取ε＝0.001，预设迭代次数为50次。执行公式(5)、公式(6)、公式(8)至公式(11)，实施迭代算法。并根据公式(7)判断迭代终止条件，可知当迭代次数为17时迭代终止。迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M为

M＝[0.0000 -0.0014 -0.0426 0.9532 0.9890]^T

步骤10：根据迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M，计算匹配结果如表8所示。

表8匹配结果位置坐标

两次迭代计算在计算机中总耗时为19.2毫秒。

根据GPS导航定位系统的定位结果(表5)，可以绘出10个时刻的惯导系统测量误差曲线、第一次迭代结果的误差曲线和第二次迭代结果的误差曲线，如图2和图3所示，其中图2给出了经度误差曲线，图3给出了纬度误差曲线。根据图2和图3可以看出，所提出的一种基于仿射变换的惯性/地磁匹配迭代定位方法具有较高的定位精度，而且重复迭代后，定位精度会进一步提高。

Claims (3)

1.一种基于仿射变换的惯性/地磁匹配迭代定位方法，其特征在于包括如下步骤：

步骤1，从惯导系统读取当前时刻以及前n-1个时刻的待匹配点的位置测量值x_i和y_i，其中x_i表示经度，y_i表示纬度，下标i表示不同时刻，i＝1…n，n为整数且n>2，i为1表示当前时刻；通过磁强计获得当前时刻以及前n-1个时刻的地磁场强度的测量值T_r,i；

步骤2，根据惯导系统指示的n个待匹配点的位置，从预先存储的地磁数据库中分别读取相应的地磁场强度的参考值T_m,i、地磁场强度的梯度的参考值T_x,i和T_y,i，其中T_x,i表示地磁场强度在经度方向的梯度在第i点位置上的取值，T_y,i表示地磁场强度在纬度方向的梯度在第i点位置上的取值；

步骤3，引入并初始化匹配位置偏移量、角度偏移量和伸缩系数M：

M＝[Δx Δy α k₁ k₂]^T＝[0 0 0 1 1]^T (1)

步骤4，根据公式(2)、公式(3)和公式(4)计算迭代参数G、F和H，

G＝g(M) (2)

F＝f(M) (3)

H＝F^-1 (4)

其中，

g(M)＝[g₁(M) g₂(M) g₃(M) g₄(M) g₅(M)]^T；

x′_i＝x_i-x_n，y′_i＝y_i-y_n，T_t,i＝T_m,i-T_r,i；

步骤5，计算匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM：

δM＝-H×G (5)

步骤6，更新匹配位置偏移量、角度偏移量和伸缩系数M：

M＝M+δM (6)

步骤7，判断是否满足终止迭代条件，若满足则停止迭代并跳到步骤10，否则跳到步骤8；

终止迭代条件为①、②中任意一个或两个：①迭代次数达到预设次数；②匹配位置偏移量的增量、角度偏移量的增量和伸缩系数的增量δM的2范数小于设定值，即

||δM||₂<ε (7)

其中ε为预先设定的迭代最小误差；

步骤8，根据更新后的M计算参数N和δN：

N＝g(M) (8)

δN＝N-G (9)

步骤9，根据公式(10)和公式(11)更新迭代变量H和G，并跳到步骤5，

H＝H+[(δM)-H(δN)](δM)^TH/[(δM)^TH(δN)] (10)

G＝N (11)

步骤10，根据迭代计算得到的匹配位置偏移量、角度偏移量和伸缩系数M，按公式(12)计算输出匹配结果，

其中x_r,i为第i时刻匹配结果的位置经度，y_r,i为第i时刻匹配结果的位置纬度。

2.如权利要求1所述的基于仿射变换的惯性/地磁匹配迭代定位方法，其特征在于所述地磁场强度为地磁场总强度、地磁异常场总强度或者地磁场总强度在地理坐标系下某一方向的分量；

当所述地磁场强度为地磁场总强度时，所述磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得所述地磁场总强度，作为所述地磁场强度的测量值T_r,i；

当所述地磁场强度为地磁异常场总强度时，所述磁强计采用标量磁强计或者三轴矢量磁强计，从所述磁强计直接获得地磁场总强度，并根据地球磁场模型计算出地磁异常场总强度，作为所述地磁场强度的测量值T_r,i；

当所述地磁场强度为地磁场总强度在地理坐标系下某一方向的分量时，所述磁强计采用三轴矢量磁强计，依据所述磁强计的三轴矢量测量值以及载体的姿态，计算出地磁场总强度在地理坐标系下该方向的分量，作为所述地磁场强度的测量值T_r,i。

3.如权利要求1或2所述的基于仿射变换的惯性/地磁匹配迭代定位方法，其特征在于还包括二次迭代步骤，所述二次迭代步骤的内容为：第二次重复执行所述步骤1-10，与第一次执行所不同的是，用第一次执行所得到的匹配定位结果x_r,i和y_r,i替代所述步骤1中从惯导系统读取的x_i和y_i。