CN106323334B - 一种基于粒子群优化的磁力计校准方法 - Google Patents

Abstract

本发明提供一种基于粒子群优化的磁力计校准方法，包括采集一定时间内的磁力计所获取的数据；获取磁力计当前所处位置处的经纬度和海拔信息，以计算磁力计当前所处位置处的磁感应强度；建立磁力计校准模型；构建适应度函数；执行粒子群优化算法；将粒子群优化所获得的全局最优位置对应的参数代入磁力计校准模型，以对磁力计获取的数据进行校准；根据陀螺仪、加速计和校准后的磁力计所获取的数据进行方位估算。本发明的基于粒子群优化的磁力计校准方法通过粒子群优化算法，能够实现多种误差的校准；无需精密仪器，校准过程简易，计算复杂度相对较低，且精度较高；方向漂移大大减小，有效提高了室内定位的精度。

Description

一种基于粒子群优化的磁力计校准方法

技术领域

本发明涉及室内定位导航的技术领域，特别是涉及一种基于粒子群优化的磁力计校准方法。

背景技术

基于惯性传感器的室内定位技术是将惯性测量单元(IMU)安装在身体的合适部位，通过航位推算进行人员的定位。由于IMU具有体积小、价格便宜、携带方便、易于集成、自完备等优点，使得其显著区别于其他类别的室内定位技术。

目前，各种研究机构和商业公司在这方面的投入越来越大。但是由于传感器本身的误差以及其他误差的存在导致位置误差的积累，并且这种误差无法彻底消除，只会一直累积，以至于定位精度无法满足应用需求。而其中位置误差产生的重要因素是对行走方向推断的偏差。方向上的微小偏移往往会导致位置发生严重的偏移。

磁力计通过感知地磁场，计算分量之间的关系来计算方向。电子罗盘正是依据这一原理指引方向。在基于惯性传感器和磁力计的室内定位***中，引入磁力计来计算实时方向，以此达到消除方向误差累积的目的。

但是室内环境不同于室外环境，在室外电磁环境很干净，受外界干扰较小，磁力计计算的方向也比较准确且相对稳定。在室内，电磁环境非常复杂，例如建筑物钢筋结构、WIFI、家用电器、电线等等装置都会对磁场产生一定的影响。根据不同的误差来源，磁力计的误差可以分为软铁误差、硬铁误差、非正交误差、零漂、比例因子误差等。在这些传感器自身以及外界的干扰下，磁力计计算的方向会发生很大的偏差，所以在使用磁力计之前进行校准是完全必要的。

现有技术中，为了消除磁力计误差，通常情况下，最简便的办法是消除偏移校准法。这种方法通过分别令XYZ轴垂直于水平面放置，并绕该轴旋转采集数据，找到各轴最大最小值求均值来计算偏移量，通过减去偏移量来进行校准。然而这种方法主要用于校准硬铁漂移，无法校准其他误差。此外，遗传算法也被用来进行磁力计的校准，但是遗传算法需要通过叉积和变异来计算遗传算子，计算复杂度较高。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种基于粒子群优化的磁力计校准方法，通过粒子群优化对磁力计的硬铁误差、软铁误差、非正交误差、比例因子误差以及零漂统一进行了校准，同时融合陀螺仪、加速度计计算室内定位中的航姿，提供相对精确的航姿参考，从而减小方向误差累积，提高定位精度。

为实现上述目的及其他相关目的，本发明提供一种基于粒子群优化的磁力计校准方法，包括以下步骤：步骤S1、在远离电磁干扰的实验场地，任意转动磁力计，采集一定时间内的磁力计所获取的数据；步骤S2、获取磁力计当前所处位置处的经纬度和海拔信息；步骤S3、根据所获取的经纬度和海拔信息计算磁力计当前所处位置处的磁感应强度；步骤S4、根据磁力计误差模型建立磁力计校准模型；步骤S5、构建适应度函数；步骤S6、执行粒子群优化算法；步骤S7、将粒子群优化所获得的全局最优位置对应的参数代入磁力计校准模型，以对磁力计获取的数据进行校准；步骤S8、根据陀螺仪、加速计和校准后的磁力计所获取的数据进行方位估算。

根据上述的基于粒子群优化的磁力计校准方法，其中：所述步骤S3中，将经纬度和海拔信息代入国际地磁参考场模型中来计算当前位置处的磁感应强度；所采用磁感应强度的计算公式如下：

其中r表示离开地心的径向距离r＝a+h，a表示地球参考半径，h表示海拔高度，θ表示地心余纬，φ表示东经，t表示要查询的年份，和是高斯系数，表示n阶m次的施密特半格化缔合勒让德函数；L表示球谐函数展开式的最大阶数，l表示积分过程中的阶数。

根据上述的基于粒子群优化的磁力计校准方法，其中：所述步骤S4中，磁力计的误差模型为：

其中B表示传感器坐标系下的磁力计测量值；表示地磁场向量；A＝C_NC_S(C_SI+I_3×3)，C_N表示非正交矩阵；C_S表示比例因子矩阵；C_SI表示软铁矩阵；表示传感器坐标系下的硬铁误差；表示传感器坐标系下的零漂；w^s表示高斯白噪声，I_3×3表示3行3列的单位矩阵。

进一步地，根据上述的基于粒子群优化的磁力计校准方法，其中：磁力计校准模型为其中表示校准后的磁力计的输出。

根据上述的基于粒子群优化的磁力计校准方法，其中：所述步骤S5中，所述适应度函数为：

其中，B表示传感器坐标系下的磁力计测量值，argminf(T,b^s)表示使函数f取最小值的T,b^s的值，T＝A^-1，r₀表示磁力计当前所处位置的磁感应强度；A＝C_NC_S(C_SI+I_3×3)，C_N表示非正交矩阵；C_S表示比例因子矩阵；C_SI表示软铁矩阵；I_3×3表示3行3列的单位矩阵；表示传感器坐标系下的硬铁误差；表示传感器坐标系下的零漂，N表示磁力计输出的采样点数。

根据上述的基于粒子群优化的磁力计校准方法，其中：所述步骤S6包括以下步骤：

61)初始化粒子个数m、迭代次数k、粒子维数n、速度范围、位置范围、学习因子c₁,c₂以及停止条件；所述停止条件为迭代次数达到预设值或者全局最优粒子的适应度值小于预设的门限值；

62)将随机产生的m个粒子所在的位置分别设为各个粒子当前的局部最优位置，根据适应度函数计算所有粒子的适应度值，得到适应度值最小的粒子，并设其位置为全局最优位置；

63)根据速度和位置的更新方程在速度范围和位置范围内更新粒子的速度和位置；其中，速度和位置的更新方程分别为：

其中，i表示第i个粒子，j表示粒子的第j维变量，表示粒子i的局部最优位置的第j维变量对应的位置，表示粒子i第j维变量当前的位置，表示全局最优位置的第j维变量的位置，v_ij表示粒子i第j维变量当前的速度；r₁和r₂均表示范围在(0,1)之间的随机比例因子；

64)重新计算所有粒子的适应度值，将每个粒子重新计算的适应度值f_i与该粒子局部最优位置的适应度值比较，如果则设粒子的当前位置为局部最优位置，否则保持局部最优位置不变；

65)比较所有粒子的局部最优位置的适应度值，选取适应度值最小的局部最优位置，并将其适应度值值f^pbest与全局最优位置的适应度值f^gbest进行比较，如果f^pbest<f^gbest，则设f^pbest对应的位置为全局最优位置，否则保持当前全局最优位置不变；

66)持续进行迭代，直到达到预设迭代次数或者全局最优位置的适应度值小于预设的门限值，从而确定全局最优位置。

根据上述的基于粒子群优化的磁力计校准方法，其中：所述步骤S8包括以下步骤：

81)判断惯性测量单元是否处于静止状态；

82)当惯性测量单元处于静止状态时，利用加速计的三轴分量计算横滚角ψ_acc和俯仰角θ_acc，对陀螺仪获取的数据进行积分以计算实时横滚角ψ_gyro、俯仰角θ_gyro和航向角并通过互补滤波对陀螺仪和加速度计计算的横滚角和俯仰角进行融合；

83)利用磁力计获取的数据计算方向：

其中为磁力计计算的方向，mag_x、mag_y、mag_z分别为磁力计三轴的磁感应强度，ψ_{gyro_acc}和θ_{gyro_acc}分别为融合后的横滚角和俯仰角；

84)融合陀螺仪获取的航向角和磁力计计算的方向，以获得校准后的方向。

根据权利要求7所述的基于粒子群优化的磁力计校准方法，其特征在于：步骤81)中，计算若norm_acc＝g，则惯性测量单元处于静止状态；否则惯性测量单元处于运动状态，其中g为当地重力加速度；acc_x,acc_y,acc_z分别表示加速计测量惯性测量单元所获取的X，Y，Z轴的原始数据。

进一步地，根据上述的基于粒子群优化的磁力计校准方法，其中：步骤82)中，采用以下公式对陀螺仪和加速度计计算的横滚角和俯仰角进行融合：

ψ_{gyro_acc}＝aψ_gyro+(1-a)ψ_acc

θ_{gyro_acc}＝aθ_gyro+(1-a)θ_acc

其中a是权重系数。

进一步地，根据上述的基于粒子群优化的磁力计校准方法，其中：所述步骤84)中，采用以下公式进行陀螺仪获取的航向角和磁力计计算的方向的融合：

其中b为权重系数，即为最终输出的方向，为陀螺仪获取的航向角，为磁力计计算的方向。

如上所述，本发明的基于粒子群优化的磁力计校准方法，具有以下有益效果：

(1)通过粒子群优化算法，能够实现磁力计软铁误差、硬铁误差、非正交误差、零漂、比例因子误差等多种误差的校准；

(2)无需精密仪器，校准过程简易，计算复杂度相对较低，且精度较高；

(3)方向漂移大大减小，有效提高了室内定位的精度。

附图说明

图1显示为本发明的基于粒子群优化的磁力计校准方法的流程图；

图2显示为本发明的磁力计误差模型和校准模型；

图3显示为本发明的粒子群优化算法流程图；

图4显示为本发明的航姿参考***算法流程图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

本发明的基于粒子群优化的磁力计校准方法根据磁力计误差种类，如硬铁误差、软铁误差、非正交误差、比例因子误差、零漂等，建立误差模型，对误差模型求逆得到校准模型；并根据磁感应强度不随传感器方向改变而变化的特征建立适应度函数，然后在上述基础上使用粒子群优化算法进行磁力计校准；最后结合陀螺仪和加速度计通过互补滤波融合进行方向的决策，以得到精度较高的航姿参考***，从而大大减小了方向漂移，提高了室内定位的精度。

参照图1，本发明的基于粒子群优化的磁力计校准方法包括以下步骤：

步骤S1、在远离电磁干扰的实验场地，任意转动磁力计，采集一定时间内的磁力计所获取的数据。

其中，远离电磁干扰的实验场地是指距离电子、电磁设备和铁质物体较远，相对空旷的地方。

一定时间与采样率有关，没有具体的范围，一般不少于16个点。采集的数据越多，精度越高，相应的计算效率越低。所以需要在采集数据量和计算效率之间做一个平衡。

步骤S2、获取磁力计当前所处位置处的经纬度和海拔信息。

具体地，可以利用GPS接收器来获取磁力计当前所处位置处的经纬度和海拔信息。

步骤S3、根据所获取的经纬度和海拔信息计算磁力计当前所处位置处的磁感应强度。

具体地，可通过以下两种方法来计算磁感应强度。

1)将经纬度和海拔信息代入国际地磁参考场(International GeomagneticReference Field，IGRF)模型中可以计算当前位置处的磁感应强度。所采用磁感应强度的计算公式如下：

其中r表示离开地心的径向距离r＝a+h，a＝6371.2km表示地球参考半径，h表示海拔高度，θ表示地心余纬，φ表示东经，t表示要查询的年份，和是高斯系数，表示n阶m次的施密特半格化缔合勒让德函数；L表示球谐函数展开式的最大阶数，l表示积分过程中的阶数。

2)通过在如下网站在线计算获得磁力计当前所处位置处的磁感应强度：

http://www.ngdc.noaa.gov/geomag-web/？model＝igrf#igrfwmm

步骤S4、根据磁力计误差模型建立校准模型。

如图2所示，根据磁力计误差模型的逆来建立磁力计校准模型。具体包括以下步骤：

51)考虑到硬铁误差、软铁误差、比例因子误差、零漂以及非正交误差的情况，磁力计的误差模型为：

其中，B表示传感器坐标系下的磁力计测量值；C_N表示非正交矩阵；C_S表示比例因子矩阵；表示从地球坐标系到传感器坐标系的旋转矩阵；表示地球坐标系下的地磁场值；表示传感器坐标系下的硬铁误差；表示传感器坐标系下的软铁误差；表示传感器坐标系下的零漂；w^s表示高斯白噪声。

大多数情况下，软铁误差可以用一个线性模型表示：因此，磁力计误差模型可以表示为其中A＝C_NC_S(C_SI+I_3×3)，C_SI表示软铁矩阵，表示地磁场向量，I_3×3表示3行3列的单位矩阵，即

52)对磁力计误差模块进行求逆，得到磁力计校准模型为其中表示校准后的磁力计的输出。

步骤S5、构建适应度函数。

具体地，根据磁感应强度模值不会随磁力计的转动而改变的原理，令适应度函数为：

其中，argminf(T,b^s)表示使函数f取最小值的T,b^s的值。其中T＝A^-1，r₀表示磁力计当前所处位置的磁感应强度；N表示磁力计输出的采样点数。

步骤S6、执行粒子群优化算法。

如图3所示，粒子群优化算法包括以下步骤：

61)初始化粒子个数m、迭代次数k、粒子维数n、速度范围[-V_max,V_max]、位置范围、学习因子c₁,c₂以及停止条件。

具体地，停止条件为迭代次数达到预设值或者全局最优粒子的适应度值小于预设的门限值。

62)将随机产生的m个粒子所在的位置分别设为各个粒子当前的局部最优位置，根据适应度函数计算所有粒子的适应度值，得到适应度值最小的粒子，并设其位置为全局最优位置。

63)根据速度和位置的更新方程在速度范围和位置范围内更新粒子的速度和位置。

其中，速度和位置的更新方程分别为：

其中，i表示第i个粒子，i∈[1，m]；j表示粒子的第j维变量，j∈[1，n]；表示粒子i的局部最优位置的第j维变量对应的位置，表示粒子i第j维变量当前的位置，表示全局最优位置的第j维变量的位置，v_ij表示粒子i第j维变量当前的速度；r₁和r₂均表示范围在(0,1)之间的随机比例因子。

64)重新计算所有粒子的适应度值，将每个粒子重新计算的适应度值f_i与该粒子局部最优位置的适应度值比较，如果则设粒子的当前位置为局部最优位置，否则保持局部最优位置不变。

65)比较所有粒子的局部最优位置的适应度值，选取适应度值最小的局部最优位置，并将其适应度值值f^pbest与全局最优位置的适应度值f^gbest进行比较，如果f^pbest<f^gbest，则设f^pbest对应的位置为全局最优位置，否则保持当前全局最优位置不变。

66)持续进行迭代，直到达到停止条件，即达到预设迭代次数或者全局最优粒子的适应度值小于预设的门限值th，即f^gbest<th，从而确定全局最优位置。

步骤S7、将粒子群优化所获得的全局最优位置的参数T,b^s代入磁力计校准模型，以对磁力计获取的数据进行校准。

步骤S8、根据陀螺仪、加速计和校准后的磁力计所获取的数据进行方位估算。

如图4所示，步骤S8具体包括以下步骤：

81)判断惯性测量单元是否处于静止状态。

其中，判断方法为：计算若norm_acc＝g，则惯性测量单元处于静止状态；否则惯性测量单元处于运动状态，其中g为当地重力加速度。

其中，acc_x,acc_y,acc_z分别表示加速计测量惯性测量单元所获取的X，Y，Z轴的原始数据。

82)当惯性测量单元处于静止状态时，利用加速计的三轴分量计算横滚角ψ_acc和俯仰角θ_acc，对陀螺仪获取的数据进行积分以计算实时横滚角ψ_gyro、俯仰角θ_gyro和航向角并通过互补滤波对陀螺仪和加速度计计算的横滚角和俯仰角进行融合。

由于陀螺仪计算的角度存在低频误差，加速计算的角度存在高频误差，所以通过互补滤波对陀螺仪和加速度计计算的横滚角和俯仰角进行融合。具体如下：

ψ_{gyro_acc}＝aψ_gyro+(1-a)ψ_acc

θ_{gyro_acc}＝aθ_gyro+(1-a)θ_acc

其中a是权重系数，具体根据经验设定。

83)利用磁力计获取的数据计算方向：

其中为磁力计计算的方向，mag_x、mag_y、mag_z分别为磁力计三轴的磁感应强度。

84)融合陀螺仪获取的航向角和磁力计计算的方向，以获得校准后的方向。

由于磁力计易受到外界电磁场的干扰，在干扰较大时计算的方向会出现一些偏差。所以通过磁场扰动探测的方法来确定磁力计输出的方向的可信度。当扰动较大时，将可信度设置为较小的值，当扰动小时，将可信度设置为较大的值。融合公式如下：

其中b与磁场扰动大小成正比，为权重系数，即为最终输出的方向。

需要说明的是，步骤S1和步骤S2-S3的执行顺序并不是固定的，可根据实际情况随机执行。

综上所述，本发明的基于粒子群优化的磁力计校准方法通过粒子群优化算法，能够实现磁力计软铁误差、硬铁误差、非正交误差、零漂、比例因子误差等多种误差的校准；无需精密仪器，校准过程简易，计算复杂度相对较低，且精度较高；方向漂移大大减小，有效提高了室内定位的精度。所以，本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (8)

1.一种基于粒子群优化的磁力计校准方法，其特征在于：包括以下步骤：

步骤S1、在远离电磁干扰的实验场地，任意转动磁力计，采集一定时间内的磁力计所获取的数据；

步骤S2、获取磁力计当前所处位置处的经纬度和海拔信息；

步骤S3、根据所获取的经纬度和海拔信息计算磁力计当前所处位置处的磁感应强度；

步骤S4、根据磁力计误差模型建立磁力计校准模型；

步骤S5、构建适应度函数；

步骤S6、执行粒子群优化算法；

步骤S7、将粒子群优化所获得的全局最优位置对应的参数代入磁力计校准模型，以对磁力计获取的数据进行校准；

步骤S8、根据陀螺仪、加速计和校准后的磁力计所获取的数据进行方位估算；

所述步骤S5中，所述适应度函数为：

其中，B表示传感器坐标系下的磁力计测量值，argminf(T,b^s)表示使函数f取最小值的T,b^s的值，T＝A^-1，r₀表示磁力计当前所处位置的磁感应强度；A＝C_NC_S(C_SI+I_3×3)，C_N表示非正交矩阵；C_S表示比例因子矩阵；C_SI表示软铁矩阵；I_3×3表示3行3列的单位矩阵；表示传感器坐标系下的硬铁误差；表示传感器坐标系下的零漂，N表示磁力计输出的采样点数；

所述步骤S6包括以下步骤：

61)初始化粒子个数m、迭代次数k、粒子维数n、速度范围、位置范围、学习因子c₁,c₂以及停止条件；所述停止条件为迭代次数达到预设值或者全局最优粒子的适应度值小于预设的门限值；

62)将随机产生的m个粒子所在的位置分别设为各个粒子当前的局部最优位置，根据适应度函数计算所有粒子的适应度值，得到适应度值最小的粒子，并设其位置为全局最优位置；

63)根据速度和位置的更新方程在速度范围和位置范围内更新粒子的速度和位置；

其中，速度和位置的更新方程分别为：

其中，i表示第i个粒子，j表示粒子的第j维变量，表示粒子i的局部最优位置的第j维变量对应的位置，表示粒子i第j维变量当前的位置，表示全局最优位置的第j维变量的位置，v_ij表示粒子i第j维变量当前的速度；r₁和r₂均表示范围在(0,1)之间的随机比例因子；

64)重新计算所有粒子的适应度值，将每个粒子重新计算的适应度值f_i与该粒子局部最优位置的适应度值f_i ^pbest比较，如果f_i<f_i ^pbest，则设粒子的当前位置为局部最优位置，否则保持局部最优位置不变；

65)比较所有粒子的局部最优位置的适应度值，选取适应度值最小的局部最优位置，并将其适应度值f^pbest与全局最优位置的适应度值f^gbest进行比较，如果f^pbest<f^gbest，则设f^pbest对应的位置为全局最优位置，否则保持当前全局最优位置不变；

66)持续进行迭代，直到达到预设迭代次数或者全局最优位置的适应度值小于预设的门限值，从而确定全局最优位置。

2.根据权利要求1所述的基于粒子群优化的磁力计校准方法，其特征在于：所述步骤S3中，将经纬度和海拔信息代入国际地磁参考场模型中来计算当前位置处的磁感应强度；所采用磁感应强度的计算公式如下：

其中r表示离开地心的径向距离r＝a+h，a表示地球参考半径，h表示海拔高度，θ表示地心余纬，φ表示东经，t表示要查询的年份，和是高斯系数，表示n'阶m'次的施密特半格化缔合勒让德函数；L表示球谐函数展开式的最大阶数，l表示积分过程中的阶数。

3.根据权利要求1所述的基于粒子群优化的磁力计校准方法，其特征在于：所述步骤S4中，磁力计的误差模型为：

其中B表示传感器坐标系下的磁力计测量值；表示地磁场向量；A＝C_NC_S(C_SI+I_3×3)，C_N表示非正交矩阵；C_S表示比例因子矩阵；C_SI表示软铁矩阵；表示传感器坐标系下的硬铁误差；表示传感器坐标系下的零漂；w^s表示高斯白噪声，I_3×3表示3行3列的单位矩阵。

4.根据权利要求3所述的基于粒子群优化的磁力计校准方法，其特征在于：磁力计校准模型为其中表示校准后的磁力计的输出。

5.根据权利要求1所述的基于粒子群优化的磁力计校准方法，其特征在于：所述步骤S8包括以下步骤：

81)判断惯性测量单元是否处于静止状态；

82)当惯性测量单元处于静止状态时，利用加速计的三轴分量计算横滚角ψ_acc和俯仰角θ_acc，对陀螺仪获取的数据进行积分以计算实时横滚角ψ_gyro、俯仰角θ_gyro和航向角并通过互补滤波对陀螺仪和加速度计计算的横滚角和俯仰角进行融合；

83)利用磁力计获取的数据计算方向：

其中为磁力计计算的方向，mag_x、mag_y、mag_z分别为磁力计三轴的磁感应强度，ψ_{gyro_acc}和θ_{gyro_acc}分别为融合后的横滚角和俯仰角；

84)融合陀螺仪获取的航向角和磁力计计算的方向，以获得校准后的方向。

6.根据权利要求5所述的基于粒子群优化的磁力计校准方法，其特征在于：步骤81)中，计算若norm_acc＝g，则惯性测量单元处于静止状态；否则惯性测量单元处于运动状态，其中g为当地重力加速度；acc_x,acc_y,acc_z分别表示加速计测量惯性测量单元所获取的X，Y，Z轴的原始数据。

7.根据权利要求5所述的基于粒子群优化的磁力计校准方法，其特征在于：步骤82)中，采用以下公式对陀螺仪和加速度计计算的横滚角和俯仰角进行融合：

ψ_{gyro_acc}＝aψ_gyro+(1-a)ψ_acc

θ_{gyro_acc}＝aθ_gyro+(1-a)θ_acc

其中a是权重系数。

8.根据权利要求5所述的基于粒子群优化的磁力计校准方法，其特征在于：所述步骤84)中，采用以下公式进行陀螺仪获取的航向角和磁力计计算的方向的融合：

其中b为权重系数，即为最终输出的方向，为陀螺仪获取的航向角，为磁力计计算的方向。