CN110031020A - 一种平面磁场校正方法及其装置 - Google Patents

Abstract

本发明涉及导航磁场校正技术领域，具体涉及一种可适用较大设备在任意平面进行导航磁场校正的方法及其装置，本发明一种平面磁场校正方法，包括将包括有九轴传感器的校正装置安装固定在待校正的设备上，做初始化校准；将所述设备在任一个平面旋转一圈，获取3轴地磁传感器上地磁数据；将上述步骤二所获取的地磁数据拟合圆形，输出地磁修正值；结合3轴加速传感器和3轴陀螺仪感应器记录的数据，对步骤三解析出的地磁修正值进行坐标系旋转，计算出修正后的航向角。本磁场校正方法及其装置克服现有技术的不足，可对较大型设备在任意平面磁场进行校正,校准快速简单、稳定精准、易于安装操作。

Description

一种平面磁场校正方法及其装置

技术领域

本发明涉及导航磁场校正技术领域，具体涉及一种可适用较大设备在任意平面进行导航磁场校正的方法及其装置。

背景技术

随着半导体工艺的进步和操作系统的发展，集成了越来越多传感器使智能设备功能变得强大,例如很多手机上都能实时提供航向功能。目前智能手机上具有导航功能的传感器较多使用的是九轴传感器，九轴传感器作为集成化传感器模块，减少了电路板和整体空间，更适合用在轻巧便携的电子设备和可穿戴产品中。集成化传感器的数据准确度除了器件本身的精度外，还涉及到焊接装配后的矫正，以及针对不同应用的配套算法。合适的算法可以将来自多种传感器的数据融合，弥补了单个传感器在计算准确的位置和方向时的不足，从而实现高精度的运动检测。九轴传感器是有3轴加速传感器、3轴3轴陀螺仪和3轴地磁传感器组成，三个部分作用不同，相互配合，是我们手机、平板电脑、游戏机等电子产品中常用的运动感测追踪元件，应用于各类软件、导航中的交互控制。正常情况下，3轴地磁传感器测得的磁场强度由地磁场产生，但是由于外部磁场干扰的存在，使得3轴地磁传感器测量结果出现比较大的偏差，必须对3轴地磁传感器进行实时校准才能保证导航的方向精确。

现有生活中例如手机等一些小型设备的导航时，普遍采用八字校准方法对磁场干扰进行校正。该八字校准方法需先将3轴地磁传感器安装固定好在手机上，并将3轴地磁传感器与该手机视为一个整体，将手机以“8字”环绕，以尽可能获得更多的三维空间中的磁场强度数据，该八字校准法能快速校正手机周围磁场；当安装了导航的装置为相对大体积的设备如车辆、船舶等时，磁场校正会受到“8字”环绕方法限制，很难做空间环绕，大体积设备则很难适用八字校准方法；且该八字校准方法亦无法适用于平面的磁场校准。

发明内容

本发明的目的，就是克服现有技术的不足，提供一种适用于相对大体积的设备进行快速稳定精准的任意平面磁场校正的方法及其装置。

根据本发明的一个方面，提供了一种平面磁场校正方法，包括方法包括：一种平面磁场校正方法，其特征在于：具体包括以下步骤：

步骤一：将包括有九轴传感器的校正装置安装固定在待校正的设备上，做初始化校准；

步骤二：将所述设备在任一个平面旋转一圈，获取3轴地磁传感器上地磁数据；

步骤三：将上述步骤二所获取的地磁数据拟合圆形，输出地磁修正值并校正；

步骤四：结合3轴加速传感器和3轴陀螺仪感应器记录的数据，对步骤三解析出的地磁修正值进行坐标系旋转，从而得出修正后的航向角。

所述地磁修正值X，Y的计算方法为：对于获得的地磁原始数据，记录为(x1,y1)、(x2，y2)、(x3、y3)…，x的补偿量设为offsetX，y的补偿量设为offsetY；拟合后的圆心设为(centreX，centreY)，半径为R；

X1＝∑x；Y1＝∑y；

X2＝∑x^2；Y2＝∑y^2；

X3＝∑x^3；Y3＝∑y^3；

X1Y1＝∑(x*y)；X1Y2＝∑(x*(y^2))；

X2Y1＝∑((x^2)*y)；

N＝pointX.size()；

C＝N*X2-X1*X1；

D＝N*X1Y1-X1*Y1；

E＝N*X3+N*X1Y2-(X2+Y2)*X1；

G＝N*Y2-Y1*Y1；

H＝N*X2Y1+N*Y3-(X2+Y2)*Y1；

a＝(H*D-E*G)/(C*G-D*D)；a＝x点集的最大值–x点集的最小值；

b＝(H*C-E*D)/(D*D-G*C)；b＝y点集的最大值–y点集的最小值；

c＝-(a*X1+b*Y1+X2+Y2)/N；

R^2＝(a^2+b^2-4*c)/4；

得到X的补偿量offsetX＝2*R/a；

得到Y的补偿量offsetY＝2*R/b；

centreX＝(x点集的最大值–x点集的最小值)/2；

centreY＝(y点集的最大值–y点集的最小值)/2；

则校正值：x＝x*offsetX–centreX；

y＝y*offsetY–centreY；

所述对步骤三解析出的校正值，根据3轴加速传感器记录的数据，进行坐标系旋转的方法为：设四元数为q0,q1,q2,q3，加速度的参考系旋转矩阵A为

3轴地磁传感器数据则3轴地磁传感器在物体坐标系里的表示变换到参考坐标系为：

设gx，gy,gz表示3轴陀螺仪三个轴数据；ax,ay,az表示3轴加速度感应器三个轴数据；mx,my,mz表示3轴地磁传感器三个轴数据；Ki＝0.005；Kp＝2；halfT＝0.005；

辅助量：q0q0＝q0*q0；q0q1＝q0*q1；q0q2＝q0*q2；q0q3＝q0*q3；

q1q1＝q1*q1；q1q2＝q1*q2；q1q3＝q1*q3；q2q2＝q2*q2；q3q3＝q3*q3

将加速度的原始数据，归一化，得到单位3轴加速度感应器三个轴数据：

norm^2＝ax^2+ay^2+az^2；

ax＝ax/norm；

ay＝ay/norm；

az＝az/norm；

将3轴地磁传感器的原始数据，归一化，得到单位3轴地磁传感器三个轴数据：

norm^2＝mx^2+my^2+mz^2；

mx＝mx/norm；

my＝my/norm；

mz＝mz/norm；

则参考磁通方向，正确的磁场方向矢量{bx,0,bz}：

hx＝2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2)；

hy＝2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1)；

hz＝2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2)

bx^2＝(hx*hx+hy*hy)^2

bz＝hz；

设物体的xyz轴方向上的加速度vx，vy，vz：

vx＝2*(q1q3–q0q2)；

vy＝2(q0q1+q2q3)；

vz＝q0q0-q1q1-q2q2+q3q3；

转化到正确磁场矢量{wx,wy,wz}：

wx＝2*bx*(0.5-q2q2-q3q3)+2bz(q1q3-q0q2)；

wy＝2bx(q1q2-q0q3)+2bz(q0q1+q2q3)；

wz＝2bx(q0q2)+q1q3+2bz(0.5-q1q1-q2q2)；

四元数代表了物体的一个姿态，测量数据代表另一姿态，两者误差量：

ex＝(ay*vz-az*vy)+(my*wz-mz*wy)；

ey＝(az*vx-ax*vz)+(mz*wx-mx*wz)；

ez＝(ax*vy-ay*vx)+(mx*wy-my*wx)；

将叉乘误差进行积分，则积分误差比例增益：

exInt＝ex*Ki；

eyInt＝ey*Ki；

ezInt＝ez*Ki；

调整后3轴陀螺仪数据：

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

更新四元数据:

q0＝q0+(-q1*gx–q2*gy–q3*gz)*halfT；

q1＝q1+(q0*gx+q2*gz-q3*gy)*halfT；

q2＝q2+(q0*gy-q1*gz+q3*gx)*halfT；

q3＝q3+(q0*gz+q1*gy-q2*gx)*halfT；

将四元数单位化：

norm＝(q0^2+q1^2+q2^2+q3^2)^2；

q0＝q0/norm；

q1＝q1/norm；

q2＝q2/norm；

q3＝q3/norm；

根据四元数积分误差比例增益，姿态校正补偿后获得正确的四元数，计算出修正后的航向角yaw为：

yaw＝atan2(2*q0*q3+2q1*q2,1-2q2*q2-2q3*q3)/3.14*180。

一种平面磁场校正装置，其特征在于，包括：九轴传感器、电源、存储有可被执行用于实现如权利要求1至3中任一项所述方法的程序的存储器以及用于执行如权利要求1至 3中任一项所述方法的程序的数据处理器；

所述九轴传感器包括：

3轴陀螺仪，用于检测物体相对于地理坐标系的角速度信号并输出；

3轴加速度感应器，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

3轴地磁传感器，用于检测地磁场的强度和方向信息并输出。

本发明所达到的有益效果：

本发明提供的方法只需将设备在一个平面旋转一圈，结合3轴加速度感应器和3轴地磁传感器的数据即可完成任意平面磁场校正，克服现有技术的不足，可对任意平面磁场校正,装置校准快速稳定精准、易安装使用。

附图说明

为了易于说明，本发明由下述的较佳实施例及附图作以详细描述。

图1是本发明平面磁场校正装置的结构示意图；

图2是本发明平面磁场校正方法的流程图。

具体实施方式

下面通过具体实施方式结合附图对本发明作进一步详细说明。在某些情况下，本申请相关的一些操作并没有在说明书中显示或者描述，这是为了避免本申请的核心部分被过多的描述所淹没，而对于本领域技术人员而言，详细描述这些相关操作并不是必要的，他们根据说明书中的描述以及本领域的一般技术知识即可完整了解相关操作。

另外，说明书中所描述的特点、操作或者特征可以以任意适当的方式结合形成各种实施方式。同时，方法描述中的各步骤或者动作也可以按照本领域技术人员所能显而易见的方式进行顺序调换或调整。

航向角(英文Bearing)，是一个导航术语，指车船等的前进方向与真北夹角。导航系统都会输出自己的航向角，各个导航系统都会有误差，需校正，这里的校正值，就是校正后的航向角的意思yaw，在本发明实施例中，先将获取到的地磁数据拟合圆形，计算出3 轴地磁传感器13(x,y)的补偿量offsetX，offsetY，拟合后的圆心设为(centreX，centreY)，半径为R；对3轴地磁传感器13(x,y)的补偿量offsetX，offsetY，根据3轴加速度感应器 12数据，进行坐标系旋转,得到加速度的参考系旋转矩阵A,3轴地磁传感器13数据为M_b, 则3轴地磁传感器13在物体坐标系里的表示变换到参考坐标系为：A×M_b,设gx，gy,gz表示陀螺仪三个轴数据；ax,ay,az表示3轴加速度感应器12三个轴数据；mx,my,mz表示3轴地磁传感器13三个轴数据；Ki＝0.005；Kp＝2；halfT＝0.005；3轴加速度感应器12三个轴数据、3 轴地磁传感器13三个轴数据数据单位化，得出参考磁通方向和汽车的xyz轴方向上的加速度，通过四元数姿态与测量数据姿态的误差得到调整后的陀螺仪数据，更新四元数据并将四元数单位化，计算出修正后的航向角yaw。；本方法具体实施例中的设备为相对于手机而言体积较大的汽车。

实施例一：

图1为本发明一种实施例中平面磁场校正装置的结构示意图，如图1所示，该装置包括：九轴传感器1、电源23、存储器22以及数据处理器21；所述九轴传感器1包括：3轴陀螺仪 11，用于检测物体相对于地理坐标系的角速度信号并输出；3轴加速度感应器12，用于检测物体在载体坐标系中三个轴的加速度信号并输出；3轴地磁传感器13，用于检测地磁场的强度和方向信息并输出。

在一具体实施例中，图2示出了使用上述装置进行平面磁场校正方法的流程图，如图 2所示，该方法包括如下步骤：

步骤一：将包括有九轴传感器1的校正装置安装固定在待校正的汽车上，做初始化校准；

步骤二：将所述汽车在任一个平面旋转一圈，确保航向角变化超过360度即可，不用严格保持圆形，比如两次掉头即可，获取3轴地磁传感器13上地磁数据；

步骤三：将上述步骤二所获取的地磁数据拟合圆形，输出地磁修正值并校正；

步骤四：结合3轴加速传感器12和3轴陀螺仪感应器11记录的数据，对步骤三解析出的地磁修正值进行坐标系旋转，从而得出修正后的航向角。

所述地磁修正值X，Y的计算方法为：因汽车只在平面行驶，所以3轴地磁传感器13获取到的地磁数据可忽略z轴垂直方向数据，对于获得的地磁原始数据，记录为(x1,y1)、(x2，y2)、(x3、y3)…，x的补偿量设为offsetX，y的补偿量设为offsetY；拟合后的圆心设为(centreX，centreY)，半径为R；

X1＝∑x；Y1＝∑y；

X2＝∑x^2；Y2＝∑y^2；

X3＝∑x^3；Y3＝∑y^3；

X1Y1＝∑(x*y)；X1Y2＝∑(x*(y^2))；

X2Y1＝∑((x^2)*y)；

设N＝pointX.size()；

设C＝N*X2-X1*X1；

设D＝N*X1Y1-X1*Y1；

设E＝N*X3+N*X1Y2-(X2+Y2)*X1；

设G＝N*Y2-Y1*Y1；

设H＝N*X2Y1+N*Y3-(X2+Y2)*Y1；

a＝(H*D-E*G)/(C*G-D*D)；a＝x点集的最大值–x点集的最小值；

b＝(H*C-E*D)/(D*D-G*C)；b＝y点集的最大值–y点集的最小值；

c＝-(a*X1+b*Y1+X2+Y2)/N；

R^2＝(a^2+b^2-4*c)/4；

得到X的补偿量offsetX＝2*R/a；

得到Y的补偿量offsetY＝2*R/b；

centreX＝(x点集的最大值–x点集的最小值)/2；

centreY＝(y点集的最大值–y点集的最小值)/2；

则校正值：x＝x*offsetX–centreX；

y＝y*offsetY–centreY；

步骤四具体还包括：结合3轴加速传感器12和3轴陀螺仪感应器11记录的数据，对步骤三解析出的地磁修正值进行坐标系旋转，从而得出修正后的航向角；

设四元数为q0,q1,q2,q3，加速度的参考系旋转矩阵A为

3轴地磁传感器13数据则3轴地磁传感器13在物体坐标系里的表示变换到参考坐标系为：

设gx，gy,gz表示3轴陀螺仪11三个轴数据；ax,ay,az表示3轴加速度感应器12三个轴数据；mx,my,mz表示3轴地磁传感器13三个轴数据；Ki＝0.005；Kp＝2；halfT＝0.005；其中Kp，Ki为控制3轴加速度感应器修正陀螺仪积分姿态的速度，halfT为姿态解算时间的一半；

辅助量：q0q0＝q0*q0；q0q1＝q0*q1；q0q2＝q0*q2；q0q3＝q0*q3；

q1q1＝q1*q1；q1q2＝q1*q2；q1q3＝q1*q3；q2q2＝q2*q2；q3q3＝q3*q3

将加速度的原始数据，归一化，得到单位3轴加速度感应器12三个轴数据：

norm^2＝ax^2+ay^2+az^2；

ax＝ax/norm；

ay＝ay/norm；

az＝az/norm；

将3轴地磁传感器13的原始数据，归一化，得到单位3轴地磁传感器13三个轴数据：

norm^2＝mx^2+my^2+mz^2；

mx＝mx/norm；

my＝my/norm；

mz＝mz/norm；

则参考磁通方向，正确的磁场方向矢量{bx,0,bz}：

hx＝2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2)；

hy＝2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1)；

hz＝2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2)

bx^2＝(hx*hx+hy*hy)^2

bz＝hz；

设物体的xyz轴方向上的加速度vx，vy，vz：

vx＝2*(q1q3–q0q2)；

vy＝2(q0q1+q2q3)；

vz＝q0q0-q1q1-q2q2+q3q3；

转化到正确磁场矢量{wx,wy,wz}：

wx＝2*bx*(0.5-q2q2-q3q3)+2bz(q1q3-q0q2)；

wy＝2bx(q1q2-q0q3)+2bz(q0q1+q2q3)；

wz＝2bx(q0q2)+q1q3+2bz(0.5-q1q1-q2q2)；

四元数代表了物体的一个姿态，测量数据代表另一姿态，两者误差量

由于3轴陀螺仪11在汽车运行过程中会受震动影响，数据噪音在长时间的积分下会出现漂移的现象，因此需要将用3轴加速度感应器12求得的姿态来矫正陀螺仪积分姿态的漂移：

ex＝(ay*vz-az*vy)+(my*wz-mz*wy)；

ey＝(az*vx-ax*vz)+(mz*wx-mx*wz)；

ez＝(ax*vy-ay*vx)+(mx*wy-my*wx)；

其中ex、ey、ez为两个重力向量的叉积；

将叉乘误差进行积分，则积分误差比例增益：

exInt＝ex*Ki；

eyInt＝ey*Ki；

ezInt＝ez*Ki；

调整后3轴陀螺仪数据11：

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

更新四元数据:

q0＝q0+(-q1*gx–q2*gy–q3*gz)*halfT；

q1＝q1+(q0*gx+q2*gz-q3*gy)*halfT；

q2＝q2+(q0*gy-q1*gz+q3*gx)*halfT；

q3＝q3+(q0*gz+q1*gy-q2*gx)*halfT；

将四元数单位化：

范数norm＝(q0^2+q1^2+q2^2+q3^2)^2；

q0＝q0/norm；

q1＝q1/norm；

q2＝q2/norm；

q3＝q3/norm；

根据四元数积分误差比例增益，姿态校正补偿后获得正确的四元数，计算出修正后的航向角yaw为：

yaw＝atan2(2*q0*q3+2q1*q2,1-2q2*q2-2q3*q3)/3.14*180。

本发明还包括一种平面磁场校正装置，其特征在于，包括：九轴传感器、电源、存储有可被执行用于实现如权利要求1至3中任一项所述方法的程序的存储器以及用于执行如权利要求1至3中任一项所述方法的程序的数据处理器；

所述九轴传感器包括：

3轴陀螺仪，用于检测物体相对于地理坐标系的角速度信号并输出；

3轴加速度感应器，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

3轴地磁传感器，用于检测地磁场的强度和方向信息并输出。

在本发明实施例中，对3轴地磁传感器数据13进行校正，并根据校正的数据得到过程姿态以及观测航向，进而将过程姿态与观测姿态进行有机融合、互补滤波，从而得到较为精确的导航姿态信息，并输出。相对于现有技术的八字校准法，本发明提供的导航姿态信息输出方法中对于数据不断进行校正，从而保证了导航姿态信息的精确性，避免了因误差累积导致导航精度的下降。并且，不需要使用GPS等其他外部辅助参考，达到快速稳定精准、易安装使用的目的。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到本发明可借助，软件加必需的硬件平台的方式来实现，当然也可以全部通过硬件来实施，但很多情况下前者是更佳的实施方式。以上对本发明进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (3)

1.一种平面磁场校正方法，其特征在于：具体包括以下步骤：

步骤一：将包括有九轴传感器的校正装置安装固定在待校正的设备上，做初始化校准；

步骤二：将所述设备在任一个平面旋转一圈，获取3轴地磁传感器上地磁数据；

步骤三：将上述步骤二所获取的地磁数据拟合圆形，输出地磁修正值；

步骤四：结合3轴加速传感器和3轴陀螺仪感应器记录的数据，对步骤三解析出的地磁修正值进行坐标系旋转，计算出修正后的航向角。

2.根据权利要求1所述的一种平面磁场校正方法，其特征在于：所述地磁修正值X，Y的计算方法为：对于获得的地磁原始数据，记录为(x1,y1)、(x2，y2)、(x3、y3)…，x的补偿量设为offsetX，y的补偿量设为offsetY；拟合后的圆心设为(centreX，centreY)，半径为R；

X1＝∑x；Y1＝∑y；

X2＝∑x^2；Y2＝∑y^2；

X3＝∑x^3；Y3＝∑y^3；

X1Y1＝∑(x*y)；X1Y2＝∑(x*(y^2))；

X2Y1＝∑((x^2)*y)；

N＝pointX.size()；

C＝N*X2-X1*X1；

D＝N*X1Y1-X1*Y1；

E＝N*X3+N*X1Y2-(X2+Y2)*X1；

G＝N*Y2-Y1*Y1；

H＝N*X2Y1+N*Y3-(X2+Y2)*Y1；

a＝(H*D-E*G)/(C*G-D*D)；a＝x点集的最大值–x点集的最小值；

b＝(H*C-E*D)/(D*D-G*C)；b＝y点集的最大值–y点集的最小值；

c＝-(a*X1+b*Y1+X2+Y2)/N；

R^2＝(a^2+b^2-4*c)/4；

得到X的补偿量offsetX＝2*R/a；

得到Y的补偿量offsetY＝2*R/b；

centreX＝(x点集的最大值–x点集的最小值)/2；

centreY＝(y点集的最大值–y点集的最小值)/2；

则地磁修正值：x＝x*offsetX–centreX；

y＝y*offsetY–centreY；

根据权利要求2所述的一种平面磁场校正方法，其特征在于：结合3轴加速传感器和3轴陀螺仪感应器记录的数据，对所述地磁修正值进行坐标系旋转，设四元数为q0,q1,q2,q3，加速度的参考系旋转矩阵A为

3轴地磁传感器数据则3轴地磁传感器在物体坐标系里的表示变换到参考坐标系为：

设gx，gy,gz表示3轴陀螺仪三个轴数据；ax,ay,az表示3轴加速度感应器三个轴数据；mx,my,mz表示3轴地磁传感器三个轴数据；Ki＝0.005；Kp＝2；halfT＝0.005；

辅助量：q0q0＝q0*q0；q0q1＝q0*q1；q0q2＝q0*q2；q0q3＝q0*q3；

q1q1＝q1*q1；q1q2＝q1*q2；q1q3＝q1*q3；q2q2＝q2*q2；q3q3＝q3*q3

将加速度的原始数据，归一化，得到单位3轴加速度感应器三个轴数据：

norm^2＝ax^2+ay^2+az^2；

ax＝ax/norm；

ay＝ay/norm；

az＝az/norm；

将3轴地磁传感器的原始数据，归一化，得到单位3轴地磁传感器三个轴数据：

norm^2＝mx^2+my^2+mz^2；

mx＝mx/norm；

my＝my/norm；

mz＝mz/norm；

则参考磁通方向，正确的磁场方向矢量{bx,0,bz}：：

hx＝2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2)；

hy＝2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3-q0q1)；

hz＝2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2)

bx^2＝(hx*hx+hy*hy)^2

bz＝hz；

设物体的xyz轴方向上的加速度vx，vy，vz：

vx＝2*(q1q3–q0q2)；

vy＝2(q0q1+q2q3)；

vz＝q0q0-q1q1-q2q2+q3q3；

转化到正确磁场矢量{wx,wy,wz}：

wx＝2*bx*(0.5-q2q2-q3q3)+2bz(q1q3-q0q2)；

wy＝2bx(q1q2-q0q3)+2bz(q0q1+q2q3)；

wz＝2bx(q0q2)+q1q3+2bz(0.5-q1q1-q2q2)；

四元数代表了物体的一个姿态，测量数据代表另一姿态，两者误差量：

ex＝(ay*vz-az*vy)+(my*wz-mz*wy)；

ey＝(az*vx-ax*vz)+(mz*wx-mx*wz)；

ez＝(ax*vy-ay*vx)+(mx*wy-my*wx)；

将叉乘误差进行积分，则积分误差比例增益：

exInt＝ex*Ki；

eyInt＝ey*Ki；

ezInt＝ez*Ki；

调整后3轴陀螺仪数据：

gx＝gx+Kp*ex+exInt；

gy＝gy+Kp*ey+eyInt；

gz＝gz+Kp*ez+ezInt；

更新四元数据:

q0＝q0+(-q1*gx–q2*gy–q3*gz)*halfT；

q1＝q1+(q0*gx+q2*gz-q3*gy)*halfT；

q2＝q2+(q0*gy-q1*gz+q3*gx)*halfT；

q3＝q3+(q0*gz+q1*gy-q2*gx)*halfT；

将四元数单位化：

norm＝(q0^2+q1^2+q2^2+q3^2)^2；

q0＝q0/norm；

q1＝q1/norm；

q2＝q2/norm；

q3＝q3/norm；

根据四元数积分误差比例增益，姿态校正补偿后获得正确的四元数，计算出修正后的航向角yaw为：

yaw＝atan2(2*q0*q3+2q1*q2,1-2q2*q2-2q3*q3)/3.14*180。

3.一种平面磁场校正装置，其特征在于，包括：九轴传感器、电源、存储有可被执行用于实现如权利要求1至3中任一项所述方法的程序的存储器以及用于执行如权利要求1至3中任一项所述方法的程序的数据处理器；

所述九轴传感器包括：

3轴陀螺仪，用于检测物体相对于地理坐标系的角速度信号并输出；

3轴加速度感应器，用于检测物体在载体坐标系中三个轴的加速度信号并输出；

3轴地磁传感器，用于检测地磁场的强度和方向信息并输出。