一种室内纯磁场导航的方法.pdf

上传人：狗**

文档编号：5775069

上传时间：2019-03-18

格式：PDF

页数：12

大小：2.16MB

《一种室内纯磁场导航的方法.pdf》由会员分享，可在线阅读，更多相关《一种室内纯磁场导航的方法.pdf（12页完整版）》请在专利查询网上搜索。

1、(10)申请公布号 CN 103344235 A (43)申请公布日 2013.10.09 CN 103344235 A *CN103344235A* (21)申请号 201310244459.1 (22)申请日 2013.06.18 G01C 21/00(2006.01) (71)申请人浙江大学地址 310027 浙江省杭州市西湖区浙大路 38 号 (72)发明人赵钟伟冉立新 (74)专利代理机构杭州求是专利事务所有限公司 33200 代理人林怀禹 (54) 发明名称一种室内纯磁场导航的方法 (57) 摘要本发明公开了一种室内纯磁场导航的方法。使用与导航区域一边等长的通电。

2、螺线管平行放置在室内的最长边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的导航方法来预测将要行进的下一点的坐标位置，逐步靠近目标点；当在行进至某一点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的磁场大小一致，则导航结束，导航物体到达设定的目标点。本发明公开的室内磁场导航方法不需要基于事先存储的磁场信息，不需要与任何其他设备通信，不需要依赖其他导航手段。 (51)Int.Cl. 权利要求。

3、书 2 页说明书 4 页附图 5 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请权利要求书2页说明书4页附图5页 (10)申请公布号 CN 103344235 A CN 103344235 A *CN103344235A* 1/2 页 2 1. 一种室内纯磁场导航的方法，其特征在于：使用与导航区域一边等长的通电螺线管平行放置在室内的最长边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的纯磁场导航方法。

4、来预测将要行进的下一点的坐标信息，逐步靠近目标点；当在某一目标点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的两个垂直分量的磁场大小一致，则导航结束，导航物体到达设定的目标点。 2. 根据权利要求 1 所述的一种室内纯磁场导航的方法，其特征在于：所述的基于卡尔曼滤波的纯磁场导航方法为：（1）根据目标点坐标（xe,ye）和通电螺线管长度 l、半径 a、每米的匝数 n、通电电流大小 I，计算出目标点两个垂直分量的磁场大小信息，记为 Bxe和 Bye；（2）设定导航物体所在导航区域坐标点为初始坐标点，记为（x0， y0），用双轴磁传感。

5、器测得初始位置的磁场大小信息，即位 Bx0和 By0；记初始误差均方值 P0=0，初始磁场测量值 Z0=Bx0,By0 ；（3）从初始坐标点开始，一边测量经过路径上的磁场信息一边靠近目标点，整个过程分为 k 个步进，通过在第 k 步经过点（xk， yk）磁场大小及梯度信息来预测行进下一步（第 k+1 步）即将经过点的空间坐标点位置，记第 k 步测得的磁场大小信息为 Bxk和 Byk，记 Zm(k)=Bxk,Byk，记第k步所在点测得的磁场梯度信息为Xdotk和Ydotk，记第k步Xdotk和Ydotk 与 y 轴的夹角分别为 1和 2，记下一步行进的测量前预。

6、测方向与 y 轴夹角为根据方程求出；然后求出下一步测量前的坐标预测值其中 m 为每一步测量的步长距离；求出测量前下一步磁场预测值（4）行进至测量前的坐标预测点，根据下一步测量前的坐标预测值及该点磁场测量值 Zm(k+1)，计算得到 k+1 步位置测量后预测状态值根据公式其中：初始时， P0=0 ； A=1 ； N=Zm(k) 对 Mk的导数； Q 为处理噪声均方值，设为 100,100;100,100 R 为测量噪声均方值，设为 100;100*100,100; 权利要求书 CN 103344235 A 2 2/2 页 3 得到下一步（k+1步）测。

7、量后坐标预测点后，行进至该点，重复之前的步骤，以此类推，直到行进至点时，测量到的两个垂直分量的磁场与目标点的两个垂直分量的磁场相同，即认为到达了目标点，导航方法结束。权利要求书 CN 103344235 A 3 1/4 页 4 一种室内纯磁场导航的方法技术领域 0001 本发明涉及一种磁场导航的方法，尤其是涉及一种室内纯磁场导航的方法。背景技术 0002 当今世界导航技术受到了越来越多的重视，现在市面上主要的导航手段有 GPS 导航、无线电热点导航等。但是这些导航存在很大的局限性。首先，这些导航需要强大的数据库支持，用来进行匹配。其次， GPS 导。

8、航通常需要与卫星进行通信，如果在没有网络信号的地方，导航将遇到困难。 0003 磁场导航作为新兴的导航手段有着 GPS 没有的特点。首先，磁场导航借助磁场信息，地球本身就是一个巨大的磁场，同时人工制造磁场简单，不需要复杂的成本。其次，磁场导航不需要与任何其他设备通信，在 GPS 无法触及的地区如海底、地底、室内等地方，依然可以产生效用。发明内容 0004 本发明的目的在于提供一种室内纯磁场导航的方法，可以实现导航物体从室内导航区域任意一点到达室内导航区域指定某目标点的过程。 0005 本发明采用的技术方案是： 0006 使用与导航区域最长边等长的通电螺线。

9、管平行放置在室内的一边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的纯磁场导航方法来预测将要行进的下一点的坐标信息，逐步靠近目标点；当在某一目标点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的两个垂直分量的磁场大小一致，则导航结束，导航物体到达设定的目标点。 0007 基于卡尔曼滤波的纯磁场导航方法详细描述为： 0008 （1）根据目标点坐标（xe,ye）和通电螺线管长度 l、半径 a、每米。

10、的匝数 n、通电电流大小 I，计算出目标点两个垂直分量的磁场大小信息，记为 Bxe和 Bye； 0009 （2）设定导航物体所在导航区域坐标点为初始坐标点，记为（x0， y0），用双轴磁传感器测得初始位置的磁场大小信息，即位Bx0和By0；记初始误差均方值P0=0，初始磁场测量值 Z0=Bx0,By0; 0010 （3）从初始坐标点开始，一边测量经过路径上的磁场信息一边靠近目标点，整个过程分为 k 个步进，通过在第 k 步经过点（xk， yk）磁场大小及梯度信息来预测行进下一步（第 k+1 步）即将经过点的空间坐标点位置，记第 k 步测得的磁场大小。

11、信息为 Bxk和 Byk，记 Zm(k)=Bxk,Byk，记第k步所在点测得的磁场梯度信息为Xdotk和Ydotk，记第k步Xdotk和Ydotk 与 y 轴的夹角分别为 1和 2，记下一步行进的测量前预测方向与 y 轴夹角为根据说明书 CN 103344235 A 4 2/4 页 5 方程求出；然后求出下一步测量前的坐标预测值其中 m 为每一步测量的步长距离 ; 求出测量前下一步磁场预测值 0011 0012 （4）行进至测量前的坐标预测点，根据下一步测量前的坐标预测值及该点磁场测量值 Zm(k+1)，计算得到 k+1 步位置测量后预测状态值根据公式 0013 其。

12、中： 0014 0015 初始时， P0=0 ； A=1 ； 0016 N=Zm(k) 对 Mk的导数； 0017 Q 为处理噪声均方值矩阵，设为 100,100;100,100 0018 R 为测量噪声均方值矩阵，设为 100;100*100,100; 0019 得到下一步（k+1 步）测量后坐标预测点后，行进至该点，重复之前的步骤，以此类推，直到行进至点时，测量到的两个垂直分量的磁场与目标点的两个垂直分量的磁场相同，即认为到达了目标点，导航方法结束。 0020 本发明具有的有益效果是： 0021 本发明涉及的导航方法仅依赖测量得到的磁场大小信息，不需要任何其。

13、他导航手段辅助，不需要任何事先存储的数据来进行匹配，在深海、地底等区域都能生效。附图说明 0022 图 1 是本发明通电螺线管在室内导航的放置位置。 0023 图 2 是本发明通电螺线管放置方式产生的 Bx 磁场分布图。 0024 图 3 是本发明通电螺线管放置方式产生的 By 磁场分布图。 0025 图 4 是本发明室内磁场导航方法的流程图。 0026 图 5 是本发明一个导航实例路径图。具体实施方式 0027 下面结合附图和实施例对本发明作进一步说明。 0028 如图 1 所示，使用与导航区域一边等长的通电螺线管平行放置在室内的最长边，通上电流产生空间磁场；设定室内导航。

14、区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过说明书 CN 103344235 A 5 3/4 页 6 点的磁场大小信息及基于卡尔曼滤波的纯磁场导航方法来预测将要行进的下一点的坐标信息，逐步靠近目标点；当在某一目标点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的两个垂直分量的磁场大小一致，则导航结束，导航物体到达设定的目标点。 0029 如图1所示，本发明要实现在40m*60m的室内区域进行导航，使用与导航区域一边等长的通电螺线管平行放置在室内的一边，通上电流产生空。

15、间磁场。通电螺线管半径是1m，长度为 40m，单位每米的匝数为 1000 匝，通电电流为 1A。由于靠近通电螺线管处磁场过大，将远离通电螺线管 15m 处的区域作为导航区域。由此产生的空间磁场两个垂直分量的值记为 Bx、 By，设与通电螺线管平行的场为 Bx，与通电螺线管垂直的场为 By。Bx， By 的分布图如图 2、图 3 所示。单位是 nT。 0030 这样产生的磁场环境满足以下三个条件： 0031 （1）在室内导航区域内任意地方 Bx、 By 能唯一确定一个坐标点。 0032 （2）磁场的特点是梯度变化大于 100nT/m，能够被磁传感器分辨。 0033 （。

16、3）可以根据某一点的坐标计算出该点的两个垂直分量磁场大小信息。 0034 导航物体从室内导航区域任意一点到达室内导航区域指定某目标点的过程。具体实施过程例子如下： 0035 （1）根据目标点坐标（xe,ye）和通电螺线管长度 l、半径 a、每米的匝数 n、通电电流大小 I，计算出目标点两个垂直分量的磁场大小信息，记为 Bxe和 Bye；如图 5 所示的导航实例中，目标点坐标为（5,25），如图中用三角形所示； 0036 （2）设定导航物体所在导航区域坐标点为初始坐标点，记为（x0， y0），用双轴磁传感器测得初始位置的磁场大小信息，即位Bx。

17、0和By0；记初始误差均方值P0=0，初始磁场测量值 Z0=Bx0,By0 ；如图 5 所示的导航实施例中，初始坐标为（5,55），如小圆圈 1 所示。 0037 （3）从初始坐标点开始，一边测量经过路径上的磁场信息一边靠近目标点，整个过程分为 k 个步进，每个步进经过的点如图 5 中的小圆圈所示。通过在第 k 步经过点（xk， yk）磁场大小及梯度信息来预测行进下一步（第 k+1 步）即将经过点的空间坐标点位置。记第 k 步测得的磁场大小信息为 Bxk和 Byk，记 Zm(k)=Bxk,Byk，记第 k 步所在点测得的磁场梯度信息为 Xdotk和 Ydo。

18、tk，记第 k 步 Xdotk和 Ydotk与 x 轴的夹角分别为 1和 2，记下一步行进的测量前预测方向与 y 轴夹角为根据方程求出。然后求出下一步测量前的坐标预测值其中m 为每一步测量的步长距离 ; 求出测量前下一步磁场预测值 0038 0039 （4）行进至测量前的坐标预测点，根据下一步测量前的坐标预测值及该点磁场测量值 Zm(k+1)，计算得到 k+1 步位置测量后预测状态值根据公式说明书 CN 103344235 A 6 4/4 页 7 0040 其中： 0041 0042 初始时， P0=0 ； A=1 ； 0043 N=Zm(k) 对 Mk的导数； 00。

19、44 Q 为处理噪声均方值矩阵，设为 100,100;100,100 0045 R 为测量噪声均方值矩阵，设为 100;100*100,100; 0046 得到下一步（k+1 步）测量后坐标预测点后，行进至该点，重复之前的步骤，以此类推，直到行进至点时，测量到的两个垂直分量的磁场与目标点的两个垂直分量的磁场相同，即认为到达了目标点，导航成功结束。整个方法的流程图如图 4 所示。说明书 CN 103344235 A 7 1/5 页 8 图 1 说明书附图 CN 103344235 A 8 2/5 页 9 图 2 说明书附图 CN 103344235 A 9 3/5 页 10 图 3 说明书附图 CN 103344235 A 10 4/5 页 11 图 4 说明书附图 CN 103344235 A 11 5/5 页 12 图 5 说明书附图 CN 103344235 A 12 。

摘要
申请专利号：	CN201310244459.1	申请日：	2013.06.18
公开号：	CN103344235A	公开日：	2013.10.09
当前法律状态：	授权	有效性：	有权
法律详情：	授权\|\|\|实质审查的生效IPC(主分类):G01C 21/00申请日:20130618\|\|\|公开
IPC分类号：	G01C21/00	主分类号：	G01C21/00
申请人：	浙江大学
发明人：	赵钟伟; 冉立新
地址：	310027 浙江省杭州市西湖区浙大路38号
优先权：
专利代理机构：	杭州求是专利事务所有限公司 33200	代理人：	林怀禹
PDF完整版下载：	PDF下载

内容摘要

本发明公开了一种室内纯磁场导航的方法。使用与导航区域一边等长的通电螺线管平行放置在室内的最长边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的导航方法来预测将要行进的下一点的坐标位置，逐步靠近目标点；当在行进至某一点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的磁场大小一致，则导航结束，导航物体到达设定的目标点。本发明公开的室内磁场导航方法不需要基于事先存储的磁场信息，不需要与任何其他设备通信，不需要依赖其他导航手段。

权利要求书

权利要求书
1. 一种室内纯磁场导航的方法，其特征在于：使用与导航区域一边等长的通电螺线管平行放置在室内的最长边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的纯磁场导航方法来预测将要行进的下一点的坐标信息，逐步靠近目标点；当在某一目标点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的两个垂直分量的磁场大小一致，则导航结束，导航物体到达设定的目标点。

2. 根据权利要求1所述的一种室内纯磁场导航的方法，其特征在于：所述的基于卡尔曼滤波的纯磁场导航方法为：
（1）根据目标点坐标（xe,ye）和通电螺线管长度l、半径a、每米的匝数n、通电电流大小I，计算出目标点两个垂直分量的磁场大小信息，记为Bxe和Bye；
（2）设定导航物体所在导航区域坐标点为初始坐标点，记为（x0，y0），用双轴磁传感器测得初始位置的磁场大小信息，即位Bx0和By0；记初始误差均方值P0=0，初始磁场测量值Z0=[Bx0,By0]；
（3）从初始坐标点开始，一边测量经过路径上的磁场信息一边靠近目标点，整个过程分为k个步进，通过在第k步经过点（xk，yk）磁场大小及梯度信息来预测行进下一步（第k+1步）即将经过点的空间坐标点位置，记第k步测得的磁场大小信息为Bxk和Byk，记Zm(k)=[Bxk,Byk]，记第k步所在点测得的磁场梯度信息为Xdotk和Ydotk，记第k步Xdotk和Ydotk与y轴的夹角分别为θ1和θ2，记下一步行进的测量前预测方向与y轴夹角为根据方程求出；然后求出下一步测量前的坐标预测值其中m为每一步测量的步长距离；求出测量前下一步磁场预测值

（4）行进至测量前的坐标预测点，根据下一步测量前的坐标预测值及该点磁场测量值Zm(k+1)，计算得到k+1步位置测量后预测状态值根据公式M^k+1=M^k+1-+Kk+1·(Zm(k+1)-Zf(k+1)),]]>
其中：
Kk=Pk-NT(NPk-NT+R)-1,]]>
Pk-=APk-1AT+Q,]]>初始时，P0=0；A=1；
N=Zm(k)对Mk的导数；
Q为处理噪声均方值，设为[100,100;100,100]
R为测量噪声均方值，设为[100;100]*[100,100];
得到下一步（k+1步）测量后坐标预测点后，行进至该点，重复之前的步骤，以此类推，直到行进至点时，测量到的两个垂直分量的磁场与目标点的两个垂直分量的磁场相同，即认为到达了目标点，导航方法结束。

说明书

说明书一种室内纯磁场导航的方法
技术领域
本发明涉及一种磁场导航的方法，尤其是涉及一种室内纯磁场导航的方法。
背景技术
当今世界导航技术受到了越来越多的重视，现在市面上主要的导航手段有GPS导航、无线电热点导航等。但是这些导航存在很大的局限性。首先，这些导航需要强大的数据库支持，用来进行匹配。其次，GPS导航通常需要与卫星进行通信，如果在没有网络信号的地方，导航将遇到困难。
磁场导航作为新兴的导航手段有着GPS没有的特点。首先，磁场导航借助磁场信息，地球本身就是一个巨大的磁场，同时人工制造磁场简单，不需要复杂的成本。其次，磁场导航不需要与任何其他设备通信，在GPS无法触及的地区如海底、地底、室内等地方，依然可以产生效用。
发明内容
本发明的目的在于提供一种室内纯磁场导航的方法，可以实现导航物体从室内导航区域任意一点到达室内导航区域指定某目标点的过程。
本发明采用的技术方案是：
使用与导航区域最长边等长的通电螺线管平行放置在室内的一边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的纯磁场导航方法来预测将要行进的下一点的坐标信息，逐步靠近目标点；当在某一目标点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的两个垂直分量的磁场大小一致，则导航结束，导航物体到达设定的目标点。
基于卡尔曼滤波的纯磁场导航方法详细描述为：
（1）根据目标点坐标（xe,ye）和通电螺线管长度l、半径a、每米的匝数n、通电电流大小I，计算出目标点两个垂直分量的磁场大小信息，记为Bxe和Bye；
（2）设定导航物体所在导航区域坐标点为初始坐标点，记为（x0，y0），用双轴磁传感器测得初始位置的磁场大小信息，即位Bx0和By0；记初始误差均方值P0=0，初始磁场测量值Z0=[Bx0,By0];
（3）从初始坐标点开始，一边测量经过路径上的磁场信息一边靠近目标点，整个过程分为k个步进，通过在第k步经过点（xk，yk）磁场大小及梯度信息来预测行进下一步（第k+1步）即将经过点的空间坐标点位置，记第k步测得的磁场大小信息为Bxk和Byk，记Zm(k)=[Bxk,Byk]，记第k步所在点测得的磁场梯度信息为Xdotk和Ydotk，记第k步Xdotk和Ydotk与y轴的夹角分别为θ1和θ2，记下一步行进的测量前预测方向与y轴夹角为根据方程求出；然后求出下一步测量前的坐标预测值其中m为每一步测量的步长距离;求出测量前下一步磁场预测值

（4）行进至测量前的坐标预测点，根据下一步测量前的坐标预测值及该点磁场测量值Zm(k+1)，计算得到k+1步位置测量后预测状态值根据公式M^k+1=M^k+1-+Kk+1·(Zm(k+1)-Zf(k+1)),]]>
其中：
Kk=Pk-NT(NPk-NT+R)-1,]]>
Pk-=APk-1AT+Q,]]>初始时，P0=0；A=1；
N=Zm(k)对Mk的导数；
Q为处理噪声均方值矩阵，设为[100,100;100,100]
R为测量噪声均方值矩阵，设为[100;100]*[100,100];
得到下一步（k+1步）测量后坐标预测点后，行进至该点，重复之前的步骤，以此类推，直到行进至点时，测量到的两个垂直分量的磁场与目标点的两个垂直分量的磁场相同，即认为到达了目标点，导航方法结束。
本发明具有的有益效果是：
本发明涉及的导航方法仅依赖测量得到的磁场大小信息，不需要任何其他导航手段辅助，不需要任何事先存储的数据来进行匹配，在深海、地底等区域都能生效。
附图说明
图1是本发明通电螺线管在室内导航的放置位置。
图2是本发明通电螺线管放置方式产生的Bx磁场分布图。
图3是本发明通电螺线管放置方式产生的By磁场分布图。
图4是本发明室内磁场导航方法的流程图。
图5是本发明一个导航实例路径图。
具体实施方式
下面结合附图和实施例对本发明作进一步说明。
如图1所示，使用与导航区域一边等长的通电螺线管平行放置在室内的最长边，通上电流产生空间磁场；设定室内导航区域中目标的空间坐标点，计算得到空间目标点两个垂直分量的磁场大小；设定导航物体所在导航区域坐标点为初始坐标点；通过测量经过点的磁场大小信息及基于卡尔曼滤波的纯磁场导航方法来预测将要行进的下一点的坐标信息，逐步靠近目标点；当在某一目标点测得的两个垂直分量磁场大小信息与之前设定的目标点计算得到的两个垂直分量的磁场大小一致，则导航结束，导航物体到达设定的目标点。
如图1所示，本发明要实现在40m*60m的室内区域进行导航，使用与导航区域一边等长的通电螺线管平行放置在室内的一边，通上电流产生空间磁场。通电螺线管半径是1m，长度为40m，单位每米的匝数为1000匝，通电电流为1A。由于靠近通电螺线管处磁场过大，将远离通电螺线管15m处的区域作为导航区域。由此产生的空间磁场两个垂直分量的值记为Bx、By，设与通电螺线管平行的场为Bx，与通电螺线管垂直的场为By。Bx，By的分布图如图2、图3所示。单位是nT。
这样产生的磁场环境满足以下三个条件：
（1）在室内导航区域内任意地方Bx、By能唯一确定一个坐标点。
（2）磁场的特点是梯度变化大于100nT/m，能够被磁传感器分辨。
（3）可以根据某一点的坐标计算出该点的两个垂直分量磁场大小信息。
导航物体从室内导航区域任意一点到达室内导航区域指定某目标点的过程。具体实施过程例子如下：
（1）根据目标点坐标（xe,ye）和通电螺线管长度l、半径a、每米的匝数n、通电电流大小I，计算出目标点两个垂直分量的磁场大小信息，记为Bxe和Bye；如图5所示的导航实例中，目标点坐标为（5,25），如图中用三角形所示；
（2）设定导航物体所在导航区域坐标点为初始坐标点，记为（x0，y0），用双轴磁传感器测得初始位置的磁场大小信息，即位Bx0和By0；记初始误差均方值P0=0，初始磁场测量值Z0=[Bx0,By0]；如图5所示的导航实施例中，初始坐标为（5,55），如小圆圈1所示。
（3）从初始坐标点开始，一边测量经过路径上的磁场信息一边靠近目标点，整个过程分为k个步进，每个步进经过的点如图5中的小圆圈所示。通过在第k步经过点（xk，yk）磁场大小及梯度信息来预测行进下一步（第k+1步）即将经过点的空间坐标点位置。记第k步测得的磁场大小信息为Bxk和Byk，记Zm(k)=[Bxk,Byk]，记第k步所在点测得的磁场梯度信息为Xdotk和Ydotk，记第k步Xdotk和Ydotk与x轴的夹角分别为θ1和θ2，记下一步行进的测量前预测方向与y轴夹角为根据方程求出。然后求出下一步测量前的坐标预测值其中m为每一步测量的步长距离;求出测量前下一步磁场预测值

（4）行进至测量前的坐标预测点，根据下一步测量前的坐标预测值及该点磁场测量值Zm(k+1)，计算得到k+1步位置测量后预测状态值根据公式M^k+1=M^k+1-+Kk+1·(Zm(k+1)-Zf(k+1)),]]>
其中：
Kk=Pk-NT(NPk-NT+R)-1,]]>
Pk-=APk-1AT+Q,]]>初始时，P0=0；A=1；
N=Zm(k)对Mk的导数；
Q为处理噪声均方值矩阵，设为[100,100;100,100]
R为测量噪声均方值矩阵，设为[100;100]*[100,100];
得到下一步（k+1步）测量后坐标预测点后，行进至该点，重复之前的步骤，以此类推，直到行进至点时，测量到的两个垂直分量的磁场与目标点的两个垂直分量的磁场相同，即认为到达了目标点，导航成功结束。整个方法的流程图如图4所示。