扩展卡尔曼滤波在地磁导航中的应用分析

１Ｏ　・北京测绘・　２０１　１年第１期　扩展卡尔曼滤波在地磁导航中的应用分析　孙中豪　牛艳艳　王永收　（１．６３８８３部队，河南洛阳，４７１０００；２．郑州交通技师学院，河南郑州，４５００１６）　［摘要１研究基于地磁场的自主导航，建立以卫星轨道动力学方程为基础的系统状态方程，并详细推导以　地磁场大小和地磁场矢量为观测量时的观测方程，通过引入ＥＫＦ解决系统的非线性问题。最后．用ｍａｔｌａｂ对地　磁导航系统进行仿真，仿真结果表明ＥＫＦ有很好的收敛性和稳定性，以地磁场矢量为观测量的导航精度要远高　于以地磁场大小为观测量的导航精度，初值误差对导航精度影响很大　ｆ关键词１地磁导航；扩展卡尔曼滤波（ＥＫＦ）；地磁场模型；稳定性；收敛性　【中图分类号】Ｐ２３７　『文献标识码］Ｂ　［文章编号］１００７—３０００（２０１　１）０１—４　地磁场是地球的罔有资源，利用地球磁场卒问分　布的地磁导航技术具有简便高效、性能可靠、抗十扰　是地磁场矢量，其基本原理都是在卫星轨道动力学方　程和地磁场强度观测的基础上，运用一定的滤波技术　的优点，因而得到了各军事强　的重视，逐渐成为导　航领域的一个新的研究热点。　得到卫旱的位置和速度，它们的不同之处在于利用地　磁场的观测信息及相应的观测方程不一样。　地磁导航的精度主要还停留存巾等精度以下，如　德国的Ｂｒｅｍｅｎ大学对ＢＲＥＭ—ＳＡＴ卫早的星载磁强　２　ＥＫＦ滤波算法　卫星定轨是一个非线性系统，其在理论上很难找　计数据，利用ＥＫＦ估计了卫星的位置和速度，精度约　为１０ｋｍ左右；美国Ｃｏｒｎｅｌｌ大学利用磁强计和太．５闩敏　感信息进行组合卫星定轨的精度约为１．５ｋｉｎ左右［２］。　国内有关地磁导航的研究还主要集中在仿真和预研　阶段，如西北工业大学晏登洋和任建新等人在２００７　到一种严格的递推滤波算法，通常采用近似的方法来　进行处理，一个重要的方法就是非线性系统的线性化　［５１。对非线性模型进行“线性化”的近似，通常采用两种　方法：一种同绕标称状态进行线性化，另一种围绕最　优状态估值进行线性化。第二种线性化的滤波方法即　ＥＫＦ。　年利用地磁导航校正惯性导航的仿真实验得剑了比　较理想的导航精度。本义通过对ＥＫＦ在地磁导航中　的应用分析，确认该方法可有效提高导航精度。　这里设系统的状态方程和观测方程为：　Ｘ（ｔ）＿厂（Ｘ（ｔ），ｔ）＋　（ｔ）　ＺＳ，（Ｘ　）＋　，　＝１，２，…　（１）　（２）　１地磁导航原理　低轨卫星运行的轨道处有丰富的地磁场资源ｎｆ　以利用，而地磁场的强度和方向都足位置的　数，【大Ｉ　此从理论上讲可以采用磁测的方法进行　星定轨。　Ｗ（ｔ）＝Ⅳ（０，Ｑ（ｔ）），Ｖ　＝Ｎ（０，Ｒ　），　（０）＝Ｎ（Ｘ。，Ｐｏ）　（３）　式巾，　（ｆ）表示系统的状态向量，Ｗ（ｔ）表示系统的　过程噪声，假设为零均值高斯白噪声过程，Ｑ（ｆ）表示协　方差矩阵，　表示初值的数学期望，　表示初值的协　通过长期对地磁场模型的研究，发现包含存地磁　场强度中的信息是类似于高度的位置信息，每个给定　的地磁场强度都处在一条椭圆形的等高线』二，卫星的　位置随着时间在不断的变化，不会始终处存　一条等　高线上，冈此通过对卫星所处空间位置的地磁场强进　方差矩阵，　。、　均假设为已知的。　表示测量噪声，　假设为离散的零均值高斯白噪声过程，　表示测量方　差矩阵。并且假设对任意ｔ和ｋ，Ｅ（Ｗ（ｔ），　）　０。　行观测，就有可能获得卫星的位置信息　。通过对地磁　场强度的不断观测，与我们通过地磁场模型计箅得到　的地磁场强进行比较，经过滤波处理，就叮以对卫星　的轨道进行自主确定。　地磁导航中的观测量可以是地磁场大小，也可以　３　ＥＫＦ在地磁导航中的应用　３．１地心固联坐标系下卫星轨道动力学方程　地磁导航系统的状态方程是卫星轨道动力学方　程。在这里把地同系中的卫星轨道动力学方程作为状　态方程。假设地球引力是作用在卫星上的唯一力，地　【收稿日期】２０１０—０７—２Ｏ　［作者简介】孙中豪（１９８２），男，汉族，河南邓州人，Ｔ程师，从事大地测量１　作。　２０１１年第１期　同联坐标系下卫星的轨道动力学方程为：　・北京测绘・　ｈ（　）＝ＬＢ：（（ｒ，　，　），ｔ　）＋曰ｉ（（ｒ，　，　），ｔ＾）　＋　（（ｒ，０，　），ｔ　）Ｊ　＋　（ｋ）　（８）　ｆ　　】ｆ鱼：　．　式中，ｖ（ｋ）表示测量噪声，曰　、　、　分别表示地　磁场强度矢量在地心同联坐标系下的分量。　对式ｆ１５）进行线性化可以得到测量矩阵：　盟出亟　监以　（９）　丝　丝机监帆　１　ｄｃｔ　ｔ，　√　ｒ　ｃ　事　５　卅　’　　｝ｄｔ＝（一　［卜　（　）（７．５亨－１．５）１　）ｙ＋２，Ｑｚ　胙　１）一－　Ｏｈ　　（ｊｋ／ｋＩ　丝　丝机监　豇　丝　监　式中，ｈ表示地磁场强度的大小，雎　＝（　，　，　Ｂｚ），　＝（ｒ，　，　）表示卫星位置矢量住地心同联坐标系　ｄｖｚ＝　ｆｌ一　（等）（７．５等一４．５）］　３．２状态向量和状态方程　这里选取状态向量为：　＝Ｉ　，ｙ，　，＿Ｌ，　，　，　Ｉ　（５）　式中　Ｙ　，　，　分别表示卫星位置和速度在　地心固联坐标系下的分量。　令　：［　，　，　，　，　，　，］，则卫星　在地心固联坐标系下的状态方程可写为：　（ｔ）　，ｔ）十　（ｔ）　（６）　联系式（４）　，ｔ）中各分量为：　，Ａ＝ｖｚ　＝　＝　＝争＝　，　ｒ（　ｚ　，　一　一　把状态方程式ｆ６）进行线性离散化得：　一　～　坌　！　Ｉ　ＯＸ　ｒｋ、　’　（＾）＝　（　）　（）　盟　盟　丝　丝　（７）　篮　监　３．３观测向量及观测方程　在地磁导航中观测量有两种：一是采用地磁场强　度的模，即地磁场大小；二是采用地磁场强度的矢量　作为观测量。下面给出两种观测量对应的观测方程。　３．３．１地磁场大小为观测量的观测方程　通过测量可以得到地磁场强度在卫星星体坐标　系中的三个分量，利用国际参考地磁场模型（ＩＧＲＦ）可　以求得地磁场强度在东北天坐标系中的分量。直接利　用地磁场强度的模作为观测量，就可以省去复杂的坐　标转换，计算量也会极大的减少，这对于星上计算非　常有利。　以地磁场大小作为观测量，对应的观测方程为：　下的球坐标表示，　为地心纬度，　表示状态向量，　＝　（ｋ／ｋ一１）为ｋ时刻卫星在地心同联坐标系中的状　态预测值，。　３．３．２地磁场矢量为观测量的观测方程　以地磁场矢量作为观测量有助于提高定轨精度，　对应的观测方程为：　ｈ（ｋ）＝ｌ　，Ｂｒ，　ｊ　（　）　（１０）　式中，鼠、日　、　。的意义同式（１２），ｎ（　）＝【ｎｘ，ｎ　，ｎｚ】　表示量测噪声。　式ｆ】０）线性化就可以测量矩阵：　Ｉ　（ｋ／ｋＩ）－－　熹　（１１）　３．３３测量矩阵的求解　测量矩阵求解过程中，必须将所有的矢量统一在　一个坐标系下。这里的状态方程和测量方程均在地心　同联坐标系下表示，参考坐标系足地心固联坐标系。　这里以地磁场偶极子模型为例，此处用地心纬度　‘Ｄ来代替地心余纬　，则有：　：擘Ｉ　ｓｉｎ　＋（　ＣＯＳ６＋６１　ｓｉｎ西）ｃ０ｓ　Ｊ　（】２）　把上式在　个方向上求偏导数就可以得到三个　方向上的分量：　Ｂ　：　（ｇｉ　ｓｉｎ　＋　ｃ。ｓ　）　日　：譬（　ｃｏｓ￣＋（ｇｌ　ｃｏｓ（ｂ＋ｈｌ　ｓｉ　（ｂ）ｓｉｎ　）　（１３）　Ｂｒ＝　拿（　ｓｉｎ　＋（　ｃｏｓｑ５＋ｈｌ　ｓｉ　）ｃｏｓ（１ｆ）　再通过坐标转换就可以得到地磁场矢量在地心　固联坐标系的各分量为：　＝（　ｃｏｓ　—Ｂ【。ｓｉｎ　）ｃｏｓ０５一　ｉｎ（＝ｂ　Ｂｙ＝Ｂ：＝ＢＭｎｑ￣十Ｂ　ｏｓ　（Ｂｆｏｓ￣一Ｂ　ｉｎ　）ｓｉｎ６＋Ｂ，ｃｏｓ￣ｂ　＝曰ｒｓｉｎ　＋曰　０ｓ　（１　４）　以地磁场强度振幅作为观测量时，其观测量ｈ可　表示为：　ｈ＝Ｎ／Ｂ．２＋Ｂｆ＋Ｂｚ２　（１５）　由于其观测量是一个标量，测量矩阵矢量可以表　１２　・北京测绘・　２０１１年第１期　图１地固坐标系与东北天坐标系的转换　示为：　玎一Ｏｈ一一　一『ｌ　盟’ｏｙ’Ｏｚ’０ｖ盟　盟　’０ｖ盟　’　盟］Ｊ　　＝【　，０，０　０　Ｊ　（１６）　其中：　肚熹＝一　一［【　，　’　，　ｙ’　］出Ｊ　ＯＢ　ａＢ　ｃ）Ｂ　Ｖ　（　１　ＬＢ８Ｂ　８８　ＯＢ　：　ｘ，　，　Ｊ　（１７）　凡　０　阳　ａＢ，　ａＢ　Ｖ　以地磁场强度矢量作为观测量时，其观测量ｈ可　表示为：　＝ｔＢ　，Ｂ　，Ｂ：１Ｔ　ｔ　１８１　观测量是一个３ｘｌ矢量，测量矩阵　为３ｘ６矢　量。即表示为：　１　０Ｂｘ　ＯＢ　０ｔ７　Ｉ　以　肚　一亟　０、　ｏｚ　（１９）　ＯＢ　ＯＢｚ　８Ｂ　　Ｉ如　４仿真计算　４．１仿真软件设计　地磁导航系统的仿真有两部分组成：第一部分为　实际系统的仿真模型，第二部分为导航滤波器没计。　实际系统的仿真模型是对卫星运行的实际环境进行　模拟，包括卫星的轨道运动方程、空间环境的影响以　及测量系统的性能等，用于基本数据准备。导航滤波　器模型则用于对采用的导航方法进行仿真分析。　导航系统的仿真如图２所示：　４．２仿真结果及分析　为了模拟卫星在轨的真实运动情况，存计算卫旱　精确的地磁场　　ｉｌ　蔓堂垡型ｌ实际系统　Ｉ　参攀厂　Ｌｉ嘎网星塾重垫　堂广—Ｔ］　谥　氟匠　竺　兰　……　西　：：：：：　估计轨道　＿Ｔ、——／　＋　ｌ［——　ｌｍａｎ导航矗基艘器模型　图２地磁导航系统仿真软件组成　轨道时，考虑如下摄动因素：地球非球形引力；太阳引　力；月球引力；太阳光压；大气阻力。　仿真使用的轨道数据设置如下：　１１坐标系：地心同联坐标系。标称轨道参数：半长　轴ａ＝６７９９．４００ｋｍ，偏心率ｅ＝１．３４ｘ１０。，轨道倾角ｉ＝６５。，　升交点赤经ｎ＝３０．００。，近升角距（１）＝３０．００。。　２）卫星初始轨道参数为：　（０）＝Ｉ　４３７０．５７　ｍ　４１　８３．４１ｋｉｎ　３０８３．０６ｋｍ　一４．７２８　＂　０．５０８ｋｍ／ｓ　６．Ｏ１４ｋｒＭｓ　ｌ　３）各个滤波器的滤波初值均取：　（０１０）：　（０）＋【６０ｋｉｎ　３０ｋｍ　２０ｋｉｎ　０．３ｋｍ／ｓ　０．２ｋｍ／ｓ　０．１ｋｍ／ｓ　ｉ　４）系统噪声方差为：　Ｑ　＝ｄｉａｇ｛１．０（ｒｒｄｓ）　１．０（ｍ／ｓ）　１．０（ｍ／ｓ）　１．０（ｍ／ｓ　）　１．０（ｒＴＪｓ　）　１．０（ｒｒｄｓ　）　Ｊ　５）以地磁场强度幅值作观测量时，观测噪声方差　取：Ｒｋ＝（１５０ｎＴ）　６）１２￣地磁场强度矢量作观测量时，观测噪声方差　取：Ｒ庐（１５０ｎＴ）　ＸＩ３×３　这里分别以地磁场强度幅值和地磁场矢量作为　观测量，进行仿真分析。假设卫星三轴姿态对地稳定，　状态方程采用式（１７），滤波周期Ｔ＝Ｉ　Ｓ，仿真时间为　１５０００ｓ，表１和２各给出了其中５次的计算结果，盯　为计算结果误差的均方差。　表１幅值为观测模型的定轨误差　ｏ－￣（ｋｍ）ｏｓ（ｋｍ）ｏ－．（ｋｍ）　（ｋｍ／ｓ）ｏ＇．￣ｋｍ／ｓ）Ｏｖ＝（ｋｍ／ｓ）　结果表明：以地磁场强度为观测量时，滤波效果　较差；以地磁场矢量为观测量时，收敛精度比较高，滤　波效果有了显著提高。　下面针对ＥＫＦ滤波算法下滤波初值误差对导航　２０１　１年第１期　・北京测绘・　１３　表２矢量为观测模型的定轨误差　精度的影响进行仿真分析。以地磁场强度振幅为观测　模型，滤波周期Ｔ＝ｌｓ，测量噪声方差Ｒｋ＝（１５０ｎＴ）　。　滤波初值误差为［（６０，３０，２０）ｋｉｎ，（０．３，０．２，０．１　，　］，　结果如图３和４所示：　滤波初值误差为【（１００，８０，６０）ｋｍ，　（０．３，０．２，０．１）ｋｍ／ｓ『，结果如图５和６所示。　８Ｏ　Ｘ　ｄｉｒｅｃｔｉｏｎ　６０　Ｙｄｉｒｅｃｔｉｏｎ　Ｚ　ｄｉｒｅｃｔｉＯｉｌ　４０　姗瓣　Ｉ　　５０００　ｔｉｍｅ（ｓ）１０００　ｌ５０００　图３初值误差为【６０　３０…】时卫星位置误差　Ｘｄｉｒｅｃｔｉｏｎ　Ｙｄｉｒｅｃｔｉｏｎ　ｚ　ｄｉｒｅｃｔｉｎｎ　嚣　＾峥　施　图４初值误差为［６０　３０…】时卫星速度误差　将图５、６分别与图３、４进行比较，可以看　当将　滤波初值误差从增大到时，定轨的位置误差和速度误　差分别由１０ｋｍ、１４　ｋｍ、１０ｋｍ、２０ｍ／ｓ、１８　ｍ／ｓ、１２　ｍ／ｓ变　为３２ｋｍ、３６　ｋｍ、１８ｋｍ、３８ｍ／ｓ、４７　ｍ／ｓ、１９　ｍ／ｓ。定轨误　差有显著的增大，其中三轴位置误差的最大值均超过　了５０ｋｉｎ，Ｚ方向的位置误差甚至超过了１００ｋｍ。　５结论　ＥＫＦ算法有较好的收敛性和稳定性，可以有效地　）】宴ｏｇ甍ｏｄ　一鼍　《暑０ｇ皇呈　∞。　埘　图５初值误差为【１００　８０…】时卫星位置误差　Ｘ　ｄｉｒｅｎｔｉｏｎ　Ｙｄｉｒｅｃｔｉｏｎ　Ｚｄｉｒｅｃｆｉｏｎ　、　　’＼＾口　９Ｈ８　０１　　．　．叭　。　叭　舵　Ｖ　５００Ｏ　（ｓ）ｌ０Ｏｏ０　图６初值误差为［１００　８０…］ｗｔ卫星速度误差　对地磁导航系统的状态进行估计，为卫星实时提供导　航数据，将地磁场大小作为观测模型时的定轨精度在　１０ｋｍ左右，将地磁场矢量作为观测模型时的定轨精度　为３ｋｍ左右，可以满足卫星中等精度的定轨需求。以　地磁场矢量作为观测模型时，由于可观测量的增多，对　于系统状态模型的修正会更好，因此导航精度比较好。　滤波初值误差对ＥＫＦ算法的影响非常大，在初值误差　较小的情况下，ＥＫＦ算法可以得到中等精度的定轨结　果；当初值误差很大时，定轨精度会变得很差。　参考文献　『１】李豫泽，石志勇，杨云涛．地磁辅助惯性导航　配算法　研究ｌＪＪ．武器装备自动化，２００８，２７（６）：２５—２９．　『２　晏登洋．２】惯性／地磁组合导航技术研究［Ｄ］．西北Ｔ、【　大　学硕士沦义，２００７．３　ｆ３ｌ周军，葛致磊，施桂国，刘玉霞．地磁导航发展与关键　技术ｌ　ＪＩ．宇航学报，２００８，２９（５）：１４６７—１４７６　［４］杨功流，李士心等地磁辅助惯性导航系统的数据融　合算法　中同惯性技术学报，第１５卷第１期，　２００７．２：４７—５２　［５】秦永元，张洪钺，汪叔华．卡尔曼滤波与自主导航原理　ｆＭ】西安：西北丁业大学出版社．１９９８　ｆ下转第８７页）　一∞２０１１年第ｌ期　贝０△５＝（ｋ２＋　＋２ｋ）ｘＳ　・北京｝贝０绘・　（５）　二为根据表一计算的坐标转换四参数，表三为在　５４坐标系和８０坐标系下分别计算的的宗地面积　及按（６）式推算的理论面积差值，由实例计算可　知，按（６）　由于　、ｋ都是微小量，略去平方项后可得　５算例分析　表一是某市围土局土地调查的控制点数据，表　表１控制点数据　表２四参数计算结果　产生变形的主要原因是由于尺度因子引起的，其比例　关系基本上约等于２。因此，在以后进行坐标转换的　时候，当要求面积变形较小的情况下，在选择转换控　表３　５４、８０坐标系下面积及变形值　制点时需选择控制点几何分布合理，能使值较小的一　组控制点来求坐标转换参数。　参考文献：　边少锋．大地坐标系与大地基准［Ｍ］．北京：国防工业　出版社．２００５．　６结论　根据以上分析和计算可以得知，四参数坐标转换　成英燕，吴秀娟等．不同坐标系图斑理论面积　［２］　党亚民，计算研究ｌ　Ｊｌ棚０绘科学，２００５，３０（６）．　Ｔｈｅ　ａｎａｌｙｓｉｓ　ｏｆ　ａｒｅａ　ｅｒｒｏｒ　ｃａｕｓｅｄ　ｏｆ　ｃｏｏｒｄｉｎａｔｅ　ｔｒａｎｓｆｏｒｍａｔｉｏｎ　ＸＩＥ　Ｚｈｏｎｇ－ｌｉａｎｇ　（ＳｈａｎＸｉ　Ｒａｉｌｗａｙ　Ｅｎｇｉｎｅｅｒｉｎｇ　Ｉｎｓｔｉｔｕｔｅ，ＷｅｉＮａｎ，Ｓｈａｎｘｉ　１７４０００，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ｔｈｉｓ　ｐａｐｅｒ　ｄｉｓｃｕｓｓｅｄ　ｔｈｅ　ｍｅｔｈｏｄ　ｏｆ　ｃｏｏｒｄｉｎａｔｅ　ｔｒａｎｓｆｏｒｍａｔｉｏｎ　ｂｅｔｗｅｅｎ　ＢＪ——５４　ｓｙｓｔｅｍ　ａｎｄ　Ｘｉ＇ａｎ　ｓｙｓｔｅｍ　ｏｆ　１９８０．Ｂｙ　ｔｈｅ　ｍｅａｎｓ　ｏｆ　ｉｎｓｔａｎｃｅ　ｏｂｔａｉｎｓ　ｔｈｅ　ｍａｉｎ　ｉｎｆａｃｔｏｒ　ｉｓ　ｓｃａｌｅ　ｆａｃｔｏｒ　ｏｆ　ｃｏｏｒｄｉｎａｔｅ　ｔｒａｎｓｆｏｒｍａｔｉｏ．　Ｋｅｙ　ｗｏｒｄｓ：ＢＪ一５４　ｓｙｓｔｅｍ；Ｘｉ　ａｎ　ｓｙｓｔｅｍ　ｏｆ　１９８０；ｃｏｏｒｄｉｎａｔｅ　ｔｒａｎｓｆｏｒｍａｔｉｏｎ；ａｒｅａ　ｅｒｒｏｒ　（上接第１３页１　Ｔｈｅ　Ａｐｐｌｉｃａｔｉｏｎ　Ａｎａｌｙｓｉｓ　ｏｆ　ＥＫＦ　ｉｎ　Ｇｅｏｍａｇｎｅｔｉｃ　Ａｕｔｏｎｏｍｏｕｓ　Ｎａｖｉｇａｔｉｏｎ　ＳＵＮ　Ｚｈｏｎｇ－ｈａｏ　ＮＩＵ　Ｙａｎ－ｙａｎ　ＷＡＮＧ　Ｙｏｎｇ—ｓｈｏｕ’　ｆ１．６３８８３　Ｔｒｏｏｐｓ，Ｌｕｏｙａｎｇ，Ｈｅｎａｎ　Ｐｒｏｖｉｎｃｅ，４７１０００；２．Ｚｈｅｎｇｚｈｏｕ　Ｉｎｓｔｉｔｕｔｅ　ｏｆ　Ｔｒａｉｃｆ　Ｔｅｃｈｎｉｃｉａｎ，Ｈｅｎａｎ　Ｐｒｏｖｉｎｃｅ，４５００１６）　Ａｂｓｔｒａｃｔ：　Ａｕｔｏｎｏｍｏｕｓ　ｎａｖｉｇａｔｉｏｎ　ｕｓｉｎｇ　ｇｅｏｍａｇｎｅｔｉｃ　ｆｉｅｌｄ　ｉｓ　ｓｔｕｄｉｅｄ　ｉｎ　ｔｈｅ　ｐａｐｅｒ．Ｔｈｅ　ｓｙｓｔｅｍ　ｓｔａｔｅ　ｅｑｕａｔｉｏｎｓ　ｂａｓｅｄ　ｏｎ　ｔｈｅ　ｄｙｎａｍｉｃ　ｅｑｕａｔｉｏｎ　ｏｆ　ｓａｔｅｌｌｉｔｅ，ｏｒｂｉｔ　ｗｅｒｅ　ｂｕｉｌｔ，ａｎｄ　ｔｈｅ　ｏｂｓｅｒｖａｔｉｏｎ　ｅｑｕａｔｉｏｎｓ　ｗｉｔｈ　ｇｅｏｍａｇｎｅｔｉｃ　ｆｉｅｌｄ　ｖｅｃｔｏｒ　ａｓ　ｔｈｅ　ｏｂｓｅｒｖａｂｌｅ　ｗｅｒｅ　ｄｅｒｉｖｅｄ　ｉｎ　ｄｅｔａｉｌ，ｔｈｅ　ＵＫＦ　ｍｅｔｈｏｄ　ｆｏｒ　ｎｏｎｌｉｎｅａｒ　ｓｙｓｔｅｍ　ｗａｓ　ｉｎｔｒｏｄｕｃｅｄ　ｔｏ　ｓｏｌｖｅ　ｔｈｅ　ｎｏｎｌｉｎｅａｒ　ｐｒｏｂｌｅｍ　ｏｆ　ｔｈｅ　ｓｙｓｔｅｍ．Ａｔ　ｔｈｅ　ｅｎｄ　ｓｉｍｕｌａｔｉｏｎｓ　ｗｅｒｅ　ｍａｄｅ　ｏｎ　ｔｈｅ　ｇｅｏｍａｇｎｅｔｉｃ　ａｕｔｏｎｏｍｏｕｓ　ｎａｖｉｇａｔｉｏｎ　ｓｙｓｔｅｍ　ｂｙ　ｕｓｉｎｇ　Ｍａｔｌａｂ，Ｔｈｅ　ｒｅｓｕｌｔｓ　ｓｈｏｗ　ｔｈａｔ　ｔｈｅ　ＥＫＦ　ｈａｓ　ｇｏｏｄ　ｐｅｒｆｏｒｍａｎｃｅ　ｉｎ　ｓｔａｂｉｌｉｔｙ　ａｎｄ　ｅｏｎｖｅｒｇｅｎｅｅ．Ｔｈｅ　ｏｂｓｅｒｖａｔｉｏｎ　ｗｉｔｈ　Ｇｅｏｍａｇｎｅｔｉｃ　ｖｅｃｔｏｒ　ｈａｓ　ｈｉｇｈｅｒ　ｎａｖｉｇａｔｉｏｎ　ｐｒｅｃｉｓｉｏｎ　ｒｅｌａｔｉｖｅｓ　ｔｏ　ｔｈｅ　ｏｂｓｅｒｖａｔｉｏｎ　ｗｉｔｈ　Ｇｅｏｍａｇｎｅｔｉｃ　ｓｉｚｅ，ｔｈｅ　ｉｎｉｔｉａｌ　ｖａｌｕｅｓ　ｅｒｒｏｒ　ｈａｖｅ　ａ　ｌａｒｇｅ　ｉｎｆｌｕｅｎｃｅ　ｔｏ　ｎａｖｉｇａｔｉｏｎ　ｐｒｅｃｉｓｉｏｎ．　Ｋｅｙ　ｗｏｒｄｓ：ｇｅｏｍａｇｎｅｔｉｃ　ｎａｖｉｇａｔｉｏｎ；Ｅｘｔｅｎｄｅｄ　Ｋａｌｍａｎ　Ｆｉｈｅｒ（ＥＫＦ）；ｇｅｏｍａｇｎｅｔｉｃ　ｆｉｅｌｄ　ｍｏｄｅｌ；ｓｔａｂｉｌｉｔｙ；ｃｏｎｖｅｒｇｅｎｃｅ