CN103153790A - 使用运动传感器和附接至装置的磁力计的测量数据估计该装置在重力参照系中的偏航角的设备和方法 - Google Patents

Abstract

提供了使用运动传感器和附接至装置的磁力计来估计该装置的物体参照系相对于重力参照系的偏航角的方法。方法包括：（A）从运动传感器和磁力计接收测量数据；（B）基于所接收的测量数据，确定物体参照系中测量的3D磁场、装置的滚动角、俯仰角和偏航角的概估；（C）从测量的3D磁场提取局部3D磁场；以及（D）基于所提取的局部3D磁场、滚动角、俯仰角和偏航角的概估，使用至少两种不同的方法计算重力参照系中物体参照系的偏航角，其中，对于不同的方法，滚动角的估计误差、俯仰角的估计误差、以及提取的局部3D磁场对偏航角的误差具有不同影响。

Description

使用运动传感器和附接至装置的磁力计的测量数据估计该装置在重力参照系中的偏航角的设备和方法

相关申请

本申请涉及并要求于2011年10月1日提交的题为“Magnetometer-Based Sensing（基于磁力计的感测）”的序列号为61/388,865的美国临时专利申请、于2011年11月17日提交的题为“Magnetometer Alignement Calibration Without Prior Knowledge ofInclination Angle and Initial Yaw Angle（无需预先知晓倾斜角和初始偏航角的磁力计对准校正）”的序列号为61/414,560号美国临时专利申请、于2011年11月17日提交的题为“Magnetometer AttitudeIndependent Parameter Calibration in Closed Form（封闭形式的磁力计与姿态无关的参数校正）”的序列号为61/414,570号美国临时专利申请以及于2011年11月17日提交的题为“Dynamic Magnetic Near FieldTracking and Compensation（动态磁近场追踪与补偿）”的序列号为61/414,582号美国临时专利申请的优先权，这些临时申请的内容通过引用并入本文。

技术领域

本发明一般涉及通过使用磁力计和其它运动传感器的测量数据来估计重力参照系中装置的偏航角和/或确定用于提取修正动态近场的静态磁场的参数的设备和方法。更具体地，通过使用并发测量数据，至少部分解析地提取用于将磁力计获取的信号转换成用于修正磁力计偏移、标度和交叉耦合/斜交、硬铁效应和软铁效应、以及对准偏差的局部磁场的参数。重力参照系中装置的偏航角可通过使用局部静态磁场（即，去除了已被追踪的近场的局部磁场）以及基于并发测量数据提取的当前滚动和俯仰来实时地估计。

背景技术

日益流行和广泛使用的移动装置常常包括所谓的九轴传感器，该名称来源于3轴陀螺仪、3D加速计和3D磁力计。3D陀螺仪测量角速度。3D加速度计测量线性加速度。磁力计测量局部磁场向量（或其偏差）。尽管这些装置已经比较普及，但是这些九轴传感器可预测的能力并未完全开发，原因在于难以校正并难以从磁力计测量数据去除不希望的影响，以及实践中不能仅使用陀螺仪和加速计来可靠地估计偏航角。

刚性主体（即，指定磁力计和运动传感器所附接的任何装置的刚性主体）相对于地固重力正交参照系的3维角度位置被唯一地限定。当使用磁力计和加速计时，方便地将重力参照系定义为具有沿重力的正Z轴、指向磁北的正X轴和指向东的正Y轴。加速计感测重力，虽然根据磁力计的测量数据，可从地球的指向北的磁场推测出重力（尽管已知地球的磁场与重力之间的角度可能不同于90°）。定义重力参照系的轴的这种方式不是用于限制。正交右手参照系的其它限定可基于两个已知的方向（重力和磁北）而获得。

附接至3D主体的运动传感器测量其在相对于三维主体限定的主体正交参照系中的位置（或其变化）。例如，如用于飞机的图1所示，不失一般性地，物体参照系具有沿飞机的纵轴指向前的正X轴、沿右翼定向的正Y轴以及通过考虑右手正交参照系（右手法则）确定的正Z轴。如果飞机水平地飞行，则正Z轴沿重力方向与重力系统的Z轴对准。虽然可使用3D加速计和附接至主体的2D或3D旋转传感器并基于重力的已知方向来确定重力参照系中滚动和俯仰（例如，参见自由专利-美国专利第7,158,118号、第7,262,760号和第7,414,611号），但是重力参照系中的偏航角更难以精确地估计，使得其更多地增加从磁力计测量数据得到的地球的磁场（或者更准确地来说为方位）的读数。

基于欧拉定理，物体参照系和重力参照系（如两个正交的右手坐标系）可与绕坐标轴的旋转序列（不多于三个）关联，其中，顺次旋转绕不同的轴进行。这种旋转序列被认为是欧拉角-轴序列。这种参照旋转序列在图2中示出。这些旋转的角度为装置在重力参照系中的角度位置。

3D磁力计测量3D磁场，3D磁场表示3D静态磁场（例如，地球的磁场）、硬铁效应和软铁效应、以及因外部时间相关电磁场而产生的3D动态近场的重叠。所测量的磁场依赖于磁力计的实际方位。如果硬铁效应、软铁效应和动态近场为零，则所测量的磁场的轨迹（当磁力计以不同的方向定位时）将为半径等于地球的磁场大小的球体。非零值的硬铁效应和软铁效应将所测量的磁场的轨迹从最初的形状偏移成椭圆体。

硬铁效应由展示与地球的磁场重叠的恒定磁场的材料产生，从而产生所测量的磁场分量的恒定偏移。只要由于硬铁效应而导致磁场的方位和位置相对于磁力计是不变的，则相应的偏移也是不变的。

不同于使磁场与地球磁场重叠的硬铁效应，软铁效应为影响、歪曲磁场（例如，铁或镍）的材料的结果，但不一定生成磁场本身。因此，软铁效应为所测量的磁场根据引起相对于磁力计和相对于地球磁场的效应的材料的位置和特性而产生变形。因此，软铁效应不能通过简单的偏移补偿，需要更复杂的过程。

磁近场为所测量的磁场由于时间相关磁场而产生的动态变形。在缺少对三轴加速计和三轴旋转传感器的偏航的可靠估计的情况下（例如，因没有观察到绝对偏航角测量数据而引起的偏航角漂移问题），磁近场补偿的磁力计测量数据可提供使其能够修正偏航角漂移的重要参照。

通常，使用多个磁场测量数据来修正硬铁和软铁效应。该方法耗时且消耗内存。此外，考虑到因硬铁和软铁效应导致的变形的动态本质，多个磁测量数据的差异也可能反映局部磁场在时间上的改变，使得当前测量过度修正或者修正不足。

因此，期望提供能够实时、可靠地使用磁力计以及附接至装置的运动传感器来确定该装置的方位（即，包括偏航角的角度位置），并同时避免前述问题和缺陷的装置、系统和方法。

发明内容

使用来自包括磁力计的传感器组合的并发测量数据获得局部3D静态磁场值，然后获得3D主体的偏航角的改进值的装置、系统和方法。

根据一个示例性实施方式，提供了使用运动传感器和附接至装置的磁力计来估计该装置的物体参照系相对于重力参照系的偏航角的方法。方法包括：（A）从运动传感器和磁力计接收测量数据；（B）基于所接收的测量数据，确定物体参照系中测量的3D磁场、装置的滚动角、俯仰角和偏航角的概估；（C）从测量的3D磁场提取局部3D磁场；以及（D）基于所提取的局部3D磁场、滚动角、俯仰角和偏航角的概估，使用至少两种不同的方法计算重力参照系中物体参照系的偏航角，其中，对于不同的方法，滚动角的估计误差、俯仰角的估计误差、以及提取的局部3D磁场对偏航角的误差具有不同影响。

根据另一示例性实施方式，提供了一种设备，其包括（A）具有刚性主体的装置；（B）3D磁力计，安装在装置上并且被配置为生成与局部磁场对应的测量数据；（C）运动传感器，安装在装置上并且被配置为生成与刚性主体的方位对应的测量数据；以及（D）至少一个处理单元。至少一个处理单元被配置为（1）从运动传感器和磁力计接收测量数据；（2）基于所接收的测量数据，确定物体参照系中测量的3D磁场、装置的滚动角、俯仰角和偏航角的概估；（3）从测量的3D磁场提取局部3D磁场；以及（4）基于所提取的局部3D磁场、滚动角、俯仰角和偏航角的概估，使用至少两种不同的方法计算重力参照系中装置的物体参照系的倾斜的补偿偏航角，其中，对于至少两种不同的方法，滚动角的估计误差、俯仰角的估计误差、以及提取的局部3D磁场的误差对倾斜的补偿偏航角的误差具有不同影响。

根据另一示例性实施方式，提供了一种被配置为非瞬时地存储可执行代码的计算机可读存储介质，当可执行代码在计算机上执行时使计算机执行使用运动传感器和附接至装置的磁力计来估计装置的物体参照系相对于重力参照系的偏航角的方法。该方法包括：（A）从运动传感器和磁力计接收测量数据；（B）基于所接收的测量数据，确定物体参照系中测量的3D磁场、装置的滚动角、俯仰角和偏航角的概估；（C）从测量的3D磁场提取局部3D磁场；以及（D）基于所提取的局部3D磁场、滚动角、俯仰角和偏航角的概估，使用至少两种不同的方法计算重力参照系中物体参照系的偏航角，其中，对于不同的方法，滚动角的估计误差、俯仰角的估计误差、以及提取的局部3D磁场对偏航角的误差具有不同影响。

附图说明

在并入且构成说明书的一部分的附图中示出了一个或多个实施方式，附图连同描述阐述了这些实施方式。在附图中：

图1是3D物体参照系的示意图；

图2是从重力参照系转换为物体参照系的示意图；

图3是根据示例性实施方式的传感单元的框图；

图4是根据示例性实施方式的使用倾斜的补偿滚动角和俯仰角计算偏航角的方法300的框图；

图5示出了地球的磁场相对于重力的定向；

图6是根据示例性实施方式的用于校正与姿态无关的参数的方法的框图；

图7是根据示例性实施方式的为采集待用于校正与姿态无关的参数的数据所使用的系统的框图；

图8是根据示例性实施方式的用于将3D磁力计与地固重力参照对准的方法的框图；

图9是根据示例性实施方式的用于将3D磁力计在九轴系统中对准的方法的框图；

图10是根据示例性实施方式的用于追踪并补偿磁近场的方法的框图；

图11是根据示例性实施方式的用于追踪并补偿磁近场的方法的框图；

图12是根据示例性实施方式的用于融合偏航角估计以获得最佳偏航角估计的方法的框图；

图13是根据示例性实施方式的使用运动传感器和附接至装置的磁力计来估计该装置的物体参照系相对于重力参照系的偏航角的方法的流程图；以及

图14是根据示例性实施方式的使用运动传感器和附接至装置的磁力计的并发测量数据校正磁力计的方法的流程图。

具体实施方式

下面参照附图对示例性实施方式进行描述。不同附图中的相同参考标号指示相同或相似的元件。下面的详细描述不限制本发明。相反地，本发明的范围由所附权利要求来限定。为了简化，以下实施方式讨论了关于包括运动传感器和附接至刚性3D主体（“装置”）的磁力计的传感单元的术语和结构。然而，接下来将要讨论的实施方式不限于这些系统，而可用于包括具有相同性能的磁力计或其他传感器的其他系统中。

在整个说明书中提及的“一个实施方式”或“实施方式”指的是关于实施方式所描述的特殊性能、结构或特性被包含在本发明的至少一个实施方式中。因此，在整个说明书的各个位置出现的短语“在一个实施方式中”或“在实施方式中”并不全指同一实施方式。而且，特殊的性能、结构或特性能够以任何适当的方式组合在一个或多个实施方式中。

根据图3所示的示例性实施方式，可附接至装置以便监控该装置的方位的传感单元100包括运动传感器110和磁力计120，磁力计120附接至该装置的刚性主体101。通过运动传感器110和磁力计120进行的并发测量数据获得信号，该信号经由接口140被发送至数据处理单元130。在图3中，数据处理单元130位于刚性主体101上。然而，在替代的实施方式中，数据处理单元可以是远程的，通过位于装置上的发送器从磁力计和运动传感器发送信号至数据处理单元。数据处理单元130包括至少一个处理器，并使用校正参数进行计算以将所接收的信号转换成包括磁场的测量数据。

可相对于装置的主体101定义物体坐标系统（例如参见图1）。被固定地附接至刚性主体101的运动传感器110和磁力计120产生与物体参照系中的可观察量（例如，磁场、角速度或线性加速度）有关的信号。然而，例如为了在参照系中独立于装置确定主体的方位，必须能够将这些测量数据关联至观察者参照系。可将观察者的参照系考虑为惯性参照系，而将物体参照系考虑为非惯性参照系。对于位于地球上的观察者，重力提供一个参照方向，磁北提供另一个参照方向。观察者的参照系可相对于这些方向来定义。例如，重力参照系可定义为具有沿重力方向的z轴、在包括重力和磁北方向的平面上的y轴、以及使用右手法则朝向东指向的x轴。然而，该特殊定义并不限制本发明的实施方式。在以下的描述中，术语“重力参照系”被用于描述使用重力和磁北而定义的参照系。

信号反映在物体参照系中测量的量。在物体参照系中的这些测量数据进一步通过数据处理单元130处理，从而被转换为与重力参照系对应的量。例如，使用旋转传感器和3D加速计，可推断出物体参照系到重力正交参照系的滚动和俯仰。为了在重力正交参照系中精确地估计装置的偏航角，通过物体的参照系中所测量的磁场确定地球的磁场的方位是必要的。

为了通过物体参照系中所测量的磁场确定地球的磁场的方位，数据处理单元130以预定的操作顺序使用多种参数针对硬铁效应、软铁效应、偏差和近场修正测量的3D磁场（其已使用校正参数通过磁力计信号理想地计算出）。一旦数据处理单元130完成所有这些修正，所得到的磁场可合理地认为是与地球磁场对应的局部静态磁场。通过被称为“倾角（dip angle）”的公知角度，地球磁场自然地指向北，稍微高于或低于与重力垂直的平面。

下面描述可在系统100中进行的方法的工具包。数据处理单元130可连接至存储可执行代码的计算机可读介质135，当可执行代码执行时使得系统100执行一种或多种方法。

根据示例性实施方式，工具包可包括（将在本公开的单独章节中描述以下方法类型中的每一种）：

（1）用于计算倾斜补偿偏航角的方法，

（2）用于确定（校正）诸如偏置、标度和斜交（交叉耦合）的与姿态无关的磁力计参数的方法，

（3）用于确定（校正）包括因周围软铁导致的等同效应的与姿态相关的磁力计对准参数的方法，

（4）用于追踪并补偿动态近场的方法，以及

（5）用于融合不同的偏航角估计以获得最佳偏航角估计的方法。

除了磁力计数据之外，这些方法中的若干方法还使用重力参照系中的滚动角和俯仰角、以及经受重力参照系中的初始未知偏移的装置的相对偏航角。重力参照系中的滚动角和俯仰角例如可通过上面在自由专利中描述的3D加速计和3D旋转传感器来确定。然而，方法（1）-方法（5）不限于在重力参照系中获得滚动角和俯仰角的方式和具体的运动传感器。

方法（2）-方法（4）用于校正并补偿由磁力计测量的磁场值的无意干扰。方法（1）和方法（5）聚焦于获得偏航角的值。校正和补偿地越好，通过方法（1）或方法（5）所获得的偏航角的值越精确。方法（1）和/或方法（5）可针对从磁力计和运动传感器所接收的并发测量数据的每个数据集来进行。方法（2）、（3）和（4）也可针对从磁力计和运动传感器所接收的并发测量数据的每个数据集来进行，但无需针对每个数据集执行方法（2）、（3）和（4）中的一个、多个或全部。根据外部条件或用户需求的改变，可针对并发测量数据的数据集执行一个、若干、全部方法或者不执行任何方法。

用于计算倾斜补偿偏航角的方法

提供了使用通过考虑到倾斜的角度信息所校正的磁力计测量数据来计算任何3D角位置（方位）的偏航角的方法。该方法在一些情况下比传统方法更精确，并且在所有条件下精度更高。

根据示例性实施方式，图4是使用滚动角和俯仰角测量数据和偏航角的粗略估计来计算倾斜的补偿偏航角的方法300的框图。由磁力计和运动传感器进行的并发测量数据允许提供3D校正的磁力计测量数据310以及滚动、俯仰角倾斜的修正测量数据与偏航角的粗略估计320作为这些方法的输入。算法330计算并输出偏航角340的值和偏航角340的估计误差350。倾斜为物体参照系的z轴相对于重力参照系的Z轴（重力）的倾斜。倾斜可通过将主体的线性加速度与重力进行比较来估算。

使用解释磁力计制造特性、硬铁和软铁效应、对准和动态近场的多个参数通过从磁力计所接收的原始信号来获得3D校正测力计测量数据310。因此，3D校正磁力计测量数据是物体参照系中的静态局部3D磁场。

以下数学表达式所指的地固参照系xyz被定义为正的z轴以地球为中心地指向（向下），在与重力垂直的平面中的x轴和y轴分别朝向磁北和东指向。

下面的表1为阐述与方法300有关的算法所使用的符号列表。

表1

参见图5，将地固重力参照系变为当前装置物体参照系的旋转矩阵

为包括三个旋转的欧拉角序列，其由下式给出

      式1

如图5所示，地固重力参照系中的磁场可表示为

^EH₀=|^EH₀|·[sinα 0 -cosα]^T       式2

其中，α为向量^EH₀与[0 0 -1]^T之间的角度，其与倾角β之间的关系为

$\mathrm{\α}=\frac{\mathrm{\π}}{2}+\mathrm{\β}$                式3

3D校正的测力计测量数据310可表示为

              式4

其中，^DB_n为

$B_n^D=R_n^E{}_D\×H_0^E$           式5

以及W_n为具有联合概率密度函数为 $N({\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T,\left[\begin{array}{ccc}{\mathrm{\σ}}_x^2& 0& 0\\ 0& {\mathrm{\σ}}_y^2& 0\\ 0& 0& {\mathrm{\σ}}_z^2\end{array}\right])$ 的高斯白测量噪声。

通过将等式1和等式2代入等式5，实际磁场（无噪声）为

               式6

然后，标准化的

为

       式7

标准化的为平行于重力的分量

           式8

与垂直于重力的分量

           式9

之和。

应该注意：（1）平行于重力的分量

没有携带关于偏航角

的信息；（2）角α为^DB与负的平行标准化的分量之间的角度。因此，提供修正后的输入倾斜角

和

             式10

然后，使用其与校正后的磁力计输入

一起计算

                 式11

使用估计的并将式（10-11）代入式7获得以下关系式

               式12

基于式12，此处提出与传统方法不同的三种方法来计算偏航角。为了简化以下的式子，定义

             式13

通过从

与

的Z分量之积减去

与的Y分量之积，获得

         式14

类似地，通过将

与

的Y分量之积与

与的Z分量之积相加，得到

           式15

的X分量为

                式16

在计算偏航角的第一种方法中，将式14乘以并除以式15，从而获得

       式17

在计算偏航角的第二种方法中，将式14乘以

并除以式16，从而获得

          式18

在计算偏航角

的第三种方法中，将式14-16组合得到

    式19

在一个实施方式中，由于三种方法的误差为关于沿每个信道的磁力计噪声与输入滚动角和俯仰角的误差的不同函数，所以算法动态地选择以上三种方法中对于最终的

具有最高精度的一种方法（一些方法较多地受到一些误差源的影响，而较少地受到其它误差源的影响，例如，方法1不受磁力计的x轴测量的影响，方法2为的误差的函数，因而当俯仰角接近0度时其对俯仰的误差不会太敏感）。在一个实施方式中，方法可动态地进行如下选择：（1）如果俯仰角的绝对值在[0,π/4]之间，则使用第二种方法；（2）如果俯仰角的绝对值在[π/3-π/2]之间，则使用第一种方法；（3）否则，使用第三种方法。该方式可得到更稳定的偏航角，该偏航角在每个单独区域中对装置的方位均不会太敏感。应该注意，该相同的基本方式可通过单个等式来实现，在该单个等式中基于等式中的各元素的所需精度合并多个估计。而且应该注意，该同一方式可在使用磁力计测量数据对俯仰和滚动进行计算中使用。

以供参考，传统方法使用以下公式来计算

      式20

该传统计算无差别地受到所有误差源的影响（即，滚动角的误差、俯仰角的误差、对于三个轴中的每个轴的磁力计测量数据的误差）。在一个实施方式中，该传统方法可以在第一种方法、第二种方法和第三种方法中的一个或多个方法之外使用。

因此，在第一种方法、第二种方法和第三种方法中使用偏航角的最佳估计（具有最小估计误差）获得的精度高于传统方法。

用于校正与姿态无关的参数的方法

根据一些实施方式，提供了用于校正三轴磁力计的与姿态无关的参数（标度、非正交性/斜交/交叉耦合、偏移）的方法。这些与姿态无关的参数被获得作为数学封闭形式的解析解，同时不考虑发散问题或收敛于局部最小值。此外，无需进行迭代计算，而方法可实时执行。参数的估计精度可用来决定是否需要在相同方位或不同方位对磁力计的另一测量数据重复校正，或者确定当前参数值是否满足所需的精度标准。

图6是根据示例性实施方式的用于校正与姿态无关的参数的方法400的框图。方法400具有输入410，即来自3D磁力计的原始测量数据。使用该输入，算法420输出与姿态无关的参数集430以及使用与姿态无关的参数430计算出的当前测量的3D磁场440的值。

在图7中示出了系统500，系统500用于修正待用来校正与姿态无关的参数的数据。系统500由四部分构成：感测元件510、数据采集引擎520、参数确定单元530和精度估计单元540。

传感器元件510输出表征所感测的磁场值的噪声和失真信号。数据采集块520通过逐个样本地积累传感器数据为参数确定做准备。参数确定单元530计算与姿态无关的参数以校正传感器元件，从而提供恒定幅度的测量数据。精度估计单元540计算计算出的与姿态无关的参数的误差，该误差指示是否获得预定的期望精度。

下表2为用于解释与校正与姿态无关的参数的方法相关的算法的符号表。

表2

通过磁力计的感测元件检测的信号因其附近的铁磁元件的存在而失真。例如，信号因磁场与周围的安装材料之间的干扰、因局部永久磁化的材料、因传感器自身的标度、交叉耦合、偏置以及因传感器的技术局限等而失真。磁场失真和感测误差的类型和效应例如在W.Denne的Magnetic Compass Deviation and Correction（磁罗盘偏差和修正），3rd ed.Sheridan House Inc，1979的多个公开的、可获得的参考文献中描述过。

在J.F.Vasconcelos等人的参考文献“A Geometric Approach toStrapdown Magnetometer Calibration in Sensor Frame（传感器框架中捷联式磁力计校正的几何方法）”中已经将三轴磁力计读数（即，3D测量的磁场）建模为

${\stackrel{\→}{B}}_i=S_M\×C_{\mathrm{NO}}\×(C_{\mathrm{SI}}\×R_i^E{}_B\×\stackrel{\→}{H}+{\stackrel{\→}{b}}_{\mathrm{HI}}_E)+{\stackrel{\→}{b}}_M+{\stackrel{\→}{n}}_{\mathrm{Mi}}$           式21

在Journal of the Astronautical Sciences，50(4):477-490，2002年10月-11月由R.Alonso和M.D.Shuster所著的文献“Complete linearattitude-independent magnetometer calibration（完整的线性与姿态无关的磁力计校正）”中的更实际且不失一般性的等式为

B_k=(I_3x3+D)^-1×(O×A_k×H+b+n_k)         式22

其中，D将来自传感器分布和软铁效应的标度和斜交组合，O为组合软铁效应和传感器相对于地固重力参照系的内部对准误差的偏移矩阵，b为因硬铁效应和传感器的本质分布而导致的偏置，n为具有零均值和恒定标准差σ的转换后的传感器测量数据噪声向量。

由于O和A_K仅改变向量的方向，所以O×A_k×H的大小为磁力计的方位相对于地固物体参照系的恒量。假设点O×A_k×H被限制为球体，则磁力计读数B_K处于椭球体上。

对于B_K的任何集合，即，椭球体的任何部分，提供了同时、解析地通过数学封闭形式确定D和b的方法。式22写为

(I_3x3+D)×B_k-b=O×A_k×H+n_k       式23

在式23两边的平方大小也是相等的，这得到

|(I_3x3+D)×B_k-b|²=|O×A_k×H|²+|n_k|²+2·（O×A_k×H)^T·n_k    式24

由于|O×A_k×H|²=|H|²,所以式24可写为

|(I_3x3+D)×B_k-b|²-|H|²=|n_k|²+2·(O×A_k×H)^T×n_k     式25

式25的右边为噪声项，式25的解可以为|(I_3x3+D)×B_k-b|²到|H|²的最小二乘拟合，即

$\underset{(D,\stackrel{\→}{b})}{\min }\underset{k=1}{\overset{n}{\mathrm{\Σ}}}\frac{1}{{\mathrm{\σ}}_k^2}{\left|\right|{|(I_{3x3}+D)\×B_k-b|}^2-{\left|H\right|}^2\left|\right|}^2,$ 并且|H|²=常数   式26

然而，由于式26为D和b的高度非线性函数，所以不存在直接的线性解析解。

通过使用以下定义

$\mathrm{pD}=I_{3x3}+D=\left[\begin{array}{ccc}{\mathrm{pD}}_{11}& {\mathrm{pD}}_{12}& {\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{12}& {\mathrm{pD}}_{22}& {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{13}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{33}\end{array}\right]$               式27

$E=\mathrm{pD}\×\mathrm{pD}=\left[\begin{array}{ccc}{\mathrm{pD}}_{11}& {\mathrm{pD}}_{12}& {\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{12}& {\mathrm{pD}}_{22}& {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{13}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{33}\end{array}\right]\×\begin{array}{c}\end{array}\left[\begin{array}{ccc}{\mathrm{pD}}_{11}& {\mathrm{pD}}_{12}& {\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{12}& {\mathrm{pD}}_{22}& {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{13}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{33}\end{array}\right]$        式28

忽略式25中的噪声，以及

|pD×B_k-b|²=|H|²           式29

将式29展开，获得以下关系式

${\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]\·B_x^2+{\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]\·B_y^2+{\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]\·B_z^2+2{\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]\·B_x\·B_y$

$+2\·{\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]\·B_x\·B_z+2\·{\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]\·B_y\·B_z-2\·{\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\begin{array}{}\end{array}\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]\·B_x$

$-2\·{\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]}^T\×\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]\·B_y-2\·{\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]}^T\×\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]\·B_z+{\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]}^T\×\begin{array}{}\end{array}\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]-{\left|H\right|}^2=0$

                   式30

为了简化式30，Q元素被定义为

$Q\left(1\right)={\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right],Q\left(2\right)={\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right],Q\left(3\right)={\left[\begin{array}{c}{\text{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]$

$Q\left(4\right)={\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right],Q\left(5\right)={\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right],Q\left(6\right)={\left[\begin{array}{c}{\text{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]}^T\×\left[\begin{array}{c}{\mathrm{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]$

$Q\left(7\right)={\left[\begin{array}{c}{\mathrm{pD}}_{11}\\ {\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{13}\end{array}\right]}^T\×\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right],Q\left(8\right)={\left[\begin{array}{c}{\mathrm{pD}}_{12}\\ {\mathrm{pD}}_{22}\\ {\mathrm{pD}}_{23}\end{array}\right]}^T\×\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right],Q\left(9\right)={\left[\begin{array}{c}{\text{pD}}_{13}\\ {\mathrm{pD}}_{23}\\ {\mathrm{pD}}_{33}\end{array}\right]}^T\×\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]$

$Q\left(10\right)={\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]}^T\×\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]-{\left|H\right|}^2$

                        式31

接下来，基于式28，E为

               式32

矩阵pD可使用奇异值分解（SVD）方法来确定

u×s×v′=svd(E)              式33

其中，s为3×3的对角矩阵。然后，对S的每个元素取平方根，获得另一3×3对角矩阵w，这样，pD为

w=sqrt(s)         式34

pD=u×w×v′              式35

偏移b被计算为

$b={\left(\mathrm{pD}\right)}^{-1}\×\left[\begin{array}{c}Q\left(7\right)\\ Q\left(8\right)\\ Q\left(9\right)\end{array}\right]$             式36

为了确定Q，将Q(1)、Q(2)和Q(3)的三个平均值定义为

$\mathrm{co}=\frac{Q\left(1\right)+Q\left(2\right)+Q\left(3\right)}{3}$                    式37

使用新的参数向量K

$K={\left[\begin{array}{ccccccccc}\frac{\frac{Q\left(1\right)-Q\left(3\right)}{3}}{\mathrm{co}}& \frac{\frac{Q\left(1\right)-Q\left(2\right)}{3}}{\mathrm{co}}& \frac{Q\left(4\right)}{\mathrm{co}}& \frac{Q\left(5\right)}{\mathrm{co}}& \frac{Q\left(6\right)}{\mathrm{co}}& \frac{Q\left(7\right)}{\mathrm{co}}& \frac{Q\left(8\right)}{\mathrm{co}}& \frac{Q\left(9\right)}{\mathrm{co}}& \frac{Q\left(10\right)}{\mathrm{co}}\end{array}\right]}^T$

                          式38

式29变为

$\left[\begin{array}{ccccccccc}{B}_x^2+B_y^2-2B_z^2& B_x^2-2B_y^2+B_z^2& 2B_x\·B_y& 2B_x\·B_z& 2\·B_y\·B_z& -2B_x& -2B_y& -2B_z& 1\end{array}\right]\×K=$

$-(B_x^2+B_y^2+B_z^2)$

                       式39

定义N×9矩阵T和N×1矩阵U

$\text{T=}\left[\begin{array}{c}{\left[\begin{array}{ccccccccc}{B}_x^2+B_y^2-2B_z^2& B_x^2-2B_y^2+B_z^2& 2B_x\·B_y& 2B_x\·B_z& 2\·B_y\·B_z& -2B_x& -2B_y& -2B_z& 1\end{array}\right]}_1\\ ...\\ {\left[\begin{array}{ccccccccc}{B}_x^2+B_y^2-2B_z^2& B_x^2-2B_y^2+B_z^{\text{2}}& \text{2}{B}_x\·B_y& 2B_x\·B_z& & 2\·B_y\·B_z& -2B_x& -2B_y& -2B_z\end{array},1\right]}_N\end{array}\right]$

                      式40

$U=\left[\begin{array}{c}-{(B_x^2+B_y^2+B_z^2)}_1\\ ...\\ -{(B_x^2+B_y^2+B_z^2)}_N\end{array}\right]$                 式41

通过此符号，对于N个样本测量数据，式39为

T×K=U          式42

并且可求解为

K=(T^T×T)^-1×T^T×U

                 式43

这样，通过式38和式32，E可写为

$E=\mathrm{co}\·\left[\begin{array}{ccc}1+K\left(1\right)+K\left(2\right)& K\left(3\right)& K\left(4\right)\\ K\left(3\right)& 1+K\left(1\right)-2K\left(2\right)& K\left(5\right)\\ K\left(4\right)& K\left(5\right)& 1-2K\left(1\right)+K\left(2\right)\end{array}\right]$       式44

定义

$F=\left[\begin{array}{ccc}1+K\left(1\right)+K\left(2\right)& K\left(3\right)& K\left(4\right)\\ K\left(3\right)& 1+K\left(1\right)-2K\left(2\right)& K\left(5\right)\\ K\left(4\right)& K\left(5\right)& 1-2K\left(1\right)+K\left(2\right)\end{array}\right]=G\×G$    式45

然后，使用式33-35以与确定pD相同的方式来确定G

pD=sqrt(co)·G         式46

通过将式36、38和46组合计算出b

b=sqrt(co)·G^-1×[K(6) K(7) K(8)]^T        式47

将K(9)的定义代入式38并将式47代入式31，co被计算如下

$\mathrm{co}=\frac{{\left|H\right|}^2}{\left[\begin{array}{ccc}K\left(6\right)& K\left(7\right)& K\left(8\right)\end{array}\right]\×F^{-1}\×{\left[\begin{array}{ccc}K\left(6\right)& K\left(7\right)& K\left(8\right)\end{array}\right]}^T-K\left(9\right)}$          式48

最后，将式48代入式46和式47，然后代入式27，完全确定D和b。

|H|²可被称为局部地磁场强度的平方。即使该强度具有未知的值，其也可被预设为任意常数，解的唯一差异是在全部三个轴的所有计算出的9个元素（3个标度、3个斜交和3个偏移）上恒定的标度差异。

基于以上描述的形式，在实时的示例性实施方案中，对于每个时步，数据采集引擎520存储两个变量矩阵：被称为covPlnvAccum_的一个9×9矩阵被用于积累T^T×T被称为zAccum_的另一可变9×1矩阵被用于积累T^T×U。在时步n+1处，根据下式来更新矩阵

${\mathrm{covP}\ln \mathrm{vAccum}}_{\_n+1}={\mathrm{covP}\ln \mathrm{vAccum}}_{\_n}+(T_{n+1}^T\×T_{n+1})$     式49

${\mathrm{zAccum}}_{\_n+1}={\mathrm{zAccum}}_{\_n}+(T_{n+1}^T\×U_{n+1})$   式50

T_n+1为T的n+1行的元素，U_n+1为U的n+1行的元素，T_n+1和U_n+1仅为当前时步处磁力计样本测量数据的函数。然后，基于式43来确定K，然后使用式33-35确定G。临时变量计算为

$\tilde{b}=G^{-1}\×{\left[\begin{array}{ccc}K\left(6\right)& K\left(7\right)& K\left(8\right)\end{array}\right]}^T$                式51

通过替代式45将该

插入式48，获得co。

此外，式51被代入式47，并且计算出的co被应用到式46-47，然后使用式27，获得了D和b（即，完整的校正参数设置）。

可应用以下算法来确定D和b的精度。K的估计的误差协方差矩阵为

$P_{\mathrm{KK}}={\mathrm{\σ}}_z^2\·{(\mathrm{covP}\ln \mathrm{vAccum}\_)}^{-1}$           式52

其中， ${\mathrm{\σ}}_z^2=12\·{\left|H\right|}^2\·{\mathrm{\σ}}^2+6\·{\mathrm{\σ}}^4$             式53

K相对于所确定的参数的雅克比矩阵

J=[b_x b_y b_z pD₁₁ pD₂₂ pD₃₃ pD₁₂ pD₁₃ pD₂₃]^T       式54

如下

$\frac{\∂K}{\∂J}=\frac{1}{\mathrm{co}}\·(M_1-M_2)$           式55

$M_1=\left[\begin{array}{ccccccccc}0& 0& 0& \frac{2}{3}{\mathrm{pD}}_{11}& 0& -\frac{2}{3}{\mathrm{pD}}_{33}& \frac{2}{3}{\mathrm{pD}}_{12}& 0& -\frac{2}{3}{\mathrm{pD}}_{23}\\ 0& 0& 0& \frac{2}{3}{\mathrm{pD}}_{11}& -\frac{2}{3}{\mathrm{pD}}_{22}& 0& 0& \frac{2}{3}{\mathrm{pD}}_{13}& -\frac{2}{3}{\mathrm{pD}}_{23}\\ 0& 0& 0& {\mathrm{pD}}_{12}& {\mathrm{pD}}_{12}& 0& {\mathrm{pD}}_{11}+{\mathrm{pD}}_{22}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{13}\\ 0& 0& 0& {\mathrm{pd}}_{13}& 0& {\mathrm{pD}}_{13}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{11}+{\mathrm{pD}}_{33}& {\mathrm{pD}}_{12}\\ 0& 0& 0& 0& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{13}& {\mathrm{pD}}_{12}& {\mathrm{pD}}_{22}+{\mathrm{pD}}_{33}\\ {\mathrm{pD}}_{11}& {\mathrm{pD}}_{12}& {\mathrm{pD}}_{13}& b_x& 0& 0& b_y& b_z& 0\\ {\mathrm{pD}}_{12}& {\mathrm{pD}}_{22}& {\mathrm{pD}}_{23}& 0& b_y& 0& b_x& 0& b_z\\ {\mathrm{pD}}_{13}& {\mathrm{pD}}_{23}& {\mathrm{pD}}_{33}& 0& 0& b_z& 0& b_x& b_y\\ 2b_x& 2b_y& 2b_z& 0& 0& 0& 0& 0& 0\end{array}\right]$

              式56

$M_2=K\×\left[\begin{array}{ccccccccc}0& 0& 0& \frac{4}{3}{\mathrm{pD}}_{11}& \frac{4}{3}{\mathrm{pD}}_{22}& \frac{4}{3}{\mathrm{pD}}_{33}& 2{\mathrm{pD}}_{12}& 2{\mathrm{pD}}_{13}& 2{\mathrm{pD}}_{23}\end{array}\right]$         式57

因此，J的估计的误差协方差矩阵为

$P_{\mathrm{JJ}}={\left(\frac{\∂K}{\∂J}\right)}^{-1}\×P_{\mathrm{KK}}\×{\left(\frac{\∂K}{\∂J}\right)}^{-1}$              式58

估计J的误差为

ε_J=sqrt(diag(P_JJ))         式59

根据以上述形式的用于校正与姿态无关的参数的方法可应用于校正在地固参照系中测量恒定物理质量向量的任何传感器，诸如测量地球重力的加速计。这些方法可应用于计算完整的参数集以将任何椭圆体拟合为球体，其中椭圆体可从原点偏移和/或可以是斜交的。可将方法用于动态时变|H|²,只要|H|²对于每个样本测量数据是已知的即可。

定义co的方式可以与式37不同，例如Q(1)、Q(2)和Q(3)的其它线性组合导致类似的或者甚至更好的结果。这种线性组合的一般形式为

co=a₁·Q(1）+a₂·Q(2)+a₃·Q(3)           式60

其中，这些系数的和为1，即

a₁+a₂+a₃=1             式61

式40和41可延伸到考虑不同的样本中的测量噪声，使用噪声方差的倒数作为权重的展开式

$T=\left[\begin{array}{c}{\frac{1}{{\mathrm{\σ}}_1^2}\·\left[\begin{array}{ccccccccc}{B}_x^2+B_y^2-2B_z^2& B_x^2-2B_y^2+B_z^2& 2B_x{B}_y& 2B_x\·B_z& 2\·B_y\·B_z& -2B_x& -2B_y& -2B_z& 1\end{array}\right]}_1\\ ...\\ \frac{1}{{\mathrm{\σ}}_N^2}\·{\left[\begin{array}{ccccccccc}{B}_x^2+B_y^2-2B_z^2& B_x^2-2B_y^2+B_z^2& 2B_x\·B_y& 2\·B_x\·B_z& 2\·B_y\·B_z& -2B_x& -2B_y& -2B_z& 1\end{array}\right]}_N\end{array}\right]$

               式62

$U=\left[\begin{array}{c}\frac{-1}{{\mathrm{\σ}}_1^2}\·{(B_x^2+B_y^2+B_z^2)}_1\\ ...\\ \frac{-1}{{\mathrm{\σ}}_N^2}\·{(B_x^2+B_y^2+B_z^2)}_N\end{array}\right]$          式63

其他测量误差函数也可以同样的方式用作T和U的权重。

传统非线性最小二乘拟合方法的缺陷在于，解可能会发散或者会收敛于局部最小值而非全局最小值，因而传统非线性最小二乘拟合方法需要迭代。传统的校正方法均不会以完全解析的封闭形式确定D和b。例如，一个传统方法仅确定标度，而不考虑斜交（即，基于斜交为零的假设仅确定9个元素中的6个元素）。

用于校正姿态相关磁力计对准参数的方法

提供了用于在没有与磁场尤其是倾角（即，偏离垂直于局部地球磁场的重力的平面）相关的先验知识的情况下将3D磁力计对准至地固重力参照系并且允许未知恒定的初始偏航角相对于地固重力参照系以角度位置的并发测量数据的顺序偏移的方法。还以同样的方式解决了因软铁效应导致的等同偏差效应问题。改进了用于对准精度的验证方法以控制对准算法动力学。将校正与验证结合使得算法收敛更快，但同时保持足够的稳定性。其还使实时实施能够可靠、稳健且直接。

图8是根据示例性实施方式的用于将3D磁力计对准至地固重力参照（即，校正姿态相关参数）的方法600的框图。方法600具有使用磁力计测量数据并且使用校正后的与姿态无关的参数计算出的磁场610和经受未知初始偏航偏移的角度位置620的输入。使用这些输入，用于传感器对准630的算法输出3D磁力计相对于装置物体参照系的对准矩阵640，使用对准矩阵640能够计算测量的磁场的完整校正值650。

图9是根据示例性实施方式的用于在九轴系统中对准3D磁力计的方法700的另一框图。图9的框图着重于数据流。九轴系统710包括3D磁力计、3D加速计和3D旋转传感器，它们的感测信号被发送至传感器解释块720。传感器提供与磁场、线性加速度和装置的角速度对应的噪声和失真感测信号。传感器解释块720使用预先计算的参数（例如，与姿态无关的参数）以将感测信号转换为标准化的单位，以（1）将标度、斜交和偏移从磁力计测量数据移除但不修正对准，（2）移除加速计的标度、斜交、偏移和非线性性，（3）移除旋转传感器的标度、斜交、偏移和线性加速效应，以及（4）将加速计和旋转传感器与物体参照系对准。接下来角度位置估计算法730使用这些被解释的加速计和旋转传感器的信号（例如，使用自由专利中描述的方法或其他方法），来生成未知初始偏航角偏移之外的装置的姿态估计（即，相对于地固重力参照系的角度位置）。以时间顺序估计出的姿态以及磁场的与姿态无关的校正值被输入算法740以用于磁力计对准估计。接下来，估计出的初始偏航角偏移和倾斜角以及磁力计样本被输入对准验证算法750以评估精度。对准验证算法750提供关于对准估计算法740是否进行得足够好的可靠指示。

下面的表3为符号列表，其用于阐述涉及校正姿态相关参数的方法的算法。

表3

对准误差的主要来源在于磁力计相对于装置的不完善的安装（即，相对于装置的物体参照系的偏差）以及软铁效应的影响。在时步t_n处与姿态无关的校正磁力计测量

$B_n^M=R_n^E{}_M\×H_E$            式64

其中，

可分解为

$R_n^E{}_M=R_D^M\×R_n^E{}_D$            式65

是磁力计的测量与装置物体参照系之间的偏差矩阵，

是时步t_n处相对于地固坐标系的真实角度位置。使用三轴加速计和三轴旋转传感器得到的

的最优估计被指示为

除了初始偏航角偏移之外，该估计在较短时间段具有较高精度。

        式66

^EH可表示为

^EH=[cosθ 0 sinθ]^T·|^EH|         式67

在不受限制的情况下，磁北被用作地固重力参照系的正X轴。将式65-67代入式64，得到

      式68

        式69

然后的问题是在考虑

和

的矩阵的情况下估计

和为了简化，将

记为A，并将C定义为

       式70

那么，扩展卡尔曼滤波（EKF）结构的6个元素为

          式71

其中，[q₀ q₁ q₂ q₃]为表征向量旋转的四元数的标度和向量元素，θ为局部磁场的倾斜角，

为参照系的角度位置中的初始偏航角偏移。

X和P₀的初始值为

X₀=[1 0 0 0 0 0]          式72

$P_0=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& \text{0}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$             式73

用于此状态的过程模型是静态的，即，X_n+1|n=X_n|n。测量模型为

      式74

预定的测量为

        式75

状态X的四元数与对准矩阵

之间的关系式为

                式76

A关于[q₀ q₁ q₂ q₃]的偏微分为

$\frac{\∂A}{{\∂q}_0}=2\·\left[\begin{array}{ccc}{q}_0& -q_3& q_2\\ q_3& q_0& -q_1\\ -q_2& q_1& q_0\end{array}\right]$          式77

$\frac{\∂A}{{\∂q}_1}=2\·\left[\begin{array}{ccc}{q}_1& q_2& q_3\\ q_2& -q_1& -q_0\\ q_3& q_0& -q_1\end{array}\right]$        式78

$\frac{\∂A}{{\∂q}_2}=2\·\left[\begin{array}{ccc}-q_2& q_1& q_0\\ q_1& q_2& q_3\\ -q_0& q_3& -q_2\end{array}\right]$           式79

$\frac{\∂A}{{\∂q}_3}=2\·\left[\begin{array}{ccc}-q_3& -q_0& q_1\\ q_0& -q_3& q_2\\ q_1& q_2& q_3\end{array}\right]$             式80

C关于θ和

的偏微分为

          式81

         式82

G被定义为

           式83

元素为h关于X的偏微分的雅克比矩阵为

            式84

标准的EKF计算过程被用于状态并且其误差协方差矩阵更新如下：

（1）误差协方差矩阵

P_n+1|n=P_n|n+Q_n            式85

（2）更新计算

             式86

将式75代入式86，获得

       式87

（3）卡尔曼增益计算

$S_{n+1}={\tilde{H}}_{n+1}\×P_{n+1|n}\×{\left({\tilde{H}}_{n+1}\right)}^T+R_{n+1}$            式88

其中，R为由下式给出的磁力计测量噪声协方差

$R_n={\left[\begin{array}{ccc}{\mathrm{\σ}}_x^2& 0& 0\\ 0& {\mathrm{\σ}}_y^2& 0\\ 0& 0& {\mathrm{\σ}}_z^2\end{array}\right]}_n$            式89

$K_{n+1}=P_{n+1|n}\×{\left({\tilde{H}}_{n+1}\right)}^T\×{\left(S_{n+1}\right)}^{-1}$            式90

（4）状态修正

X_n+1|n+1=X_n+1|n+K_n+1×r_n+1           式91

（5）误差协方差修正

$P_{n+1|n+1}=(I_{6x6}-K_{n+1}\×{\tilde{H}}_{n+1})\×P_{n+1|n}$            式92

在EKF的标准过程之外，该方法运行两个以上的步骤以将状态保持在边界内，以使递归滤波稳定并阻止其发散。

（6）四元数标准化，表征旋转矩阵的有效四元数的振幅为1

$X_{n+1}(1:4)=\frac{X_{n+1|n+1}(1:4)}{\left|X_{n+1|n+1}(1:4)\right|}$

（7）在倾斜角和初始偏航角偏移上进行相位包容，有效倾斜角被界定在

与

之间，有效偏航角被界定在-π与π之间。首先，倾斜角估计被限定在(-π,π]，例如，通过使用

X_n+1(5)=phaseLimiter(X_n+1|n+1(5))         式93

其中，y=phaseLimiter(x)函数执行以下代码：

代码1

其次，倾斜角估计还被限定在

内，由于该操作改变余弦和正弦的符号，所以需要伴随初始偏航角偏移估计的适当改变，示例性代码如下

代码2

最后，初始偏航角偏移估计被限定在(-π,π]内，

X_n+1(6)=phaseLimiter(X_n+1|n+1(6))         式94

步骤6和7是必要且关键的，虽然它们不足以保持滤波稳定并且没有使滤波收敛地更快。

被添加到该方法的另一控制因数为动态Q调整。在常规方法中，由于估计的状态是随时间恒定的，所以Q=0。然而，这会使得当数据顺序不是很好时收敛速度太慢。例如，如果最初采集的全部数据点长时间内都来自角度位置附近的非常小的区域，最终导致P极小，这是因为每个时步使得P更小一点。接下来，从更广泛的角度位置但在非常短的时间系统中采集更多的数据点时，滤波器因极小的P而不能将其状态快速地更新成真实状态。

该方法允许非零值的Q，这使得滤波器能够以合理的步调更新系统。通常，增加P的风险为使P变得非常大并且滤波器不稳定，但是该方法允许动态地调整Q并因而确定起具有快速收敛并且还足够稳定的优势。为此，恒定基线Q₀被设定为滤波器相对于全部动态范围能够进行的最大改变，并且可以为每个时步取此变量

$Q_0=\left[\begin{array}{cccccc}{\mathrm{Const}}^2& 0& 0& 0& 0& 0\\ 0& {\mathrm{Const}}^2& 0& 0& 0& 0\\ 0& 0& {\mathrm{Const}}^2& 0& 0& 0\\ 0& 0& 0& {\mathrm{Const}}^2& 0& 0\\ 0& 0& 0& 0& \frac{{\mathrm{\π}}^2}{4}\·{\mathrm{Const}}^2& 0\\ 0& 0& 0& 0& 0& {\mathrm{\π}}^2\·{\mathrm{Const}}^2\end{array}\right]$          式95

在该方法中使用了两个动态改变乘法因数以调整每个时步处的最后的Q：

Q_n=k₁·k₂·Q₀         式96

k₁被设计为当前的系统状态与通过精度验证算法所获得的系统状态之间的估计偏差角度差的函数。当差异足够大时，k₁=1使滤波器能够以其最大收敛速度运行。当差异与期望精度相比足够小时，k₁<<1以确保滤波器减慢并进行微调整。在示例性实施方式中，这样的关系在每个时步处实现如下：

代码3

其中，α为非负常数并且远小于1。

k₂为衰减因数。当角度位置处于固定的角度位置附近时，k₂以指数的方式衰减。当角度位置改变成大于预定阈值ANGLE_TOL时，k₂跳回1。通过这样做，避免了当装置处于非常窄的角度位置空间内时滤波器具有非常大的P。因而确保了稳定性。两个角度位置之差通过以下代码给出

dcmDiff=A＊Aold′;

[v,phi]=qdecomp(dcm2q(dcmDiff));

代码4

其中，A和Aold分别为两个四元数的方向余弦矩阵表示，q=dcm2q(dcm)为将方向余弦矩阵转换为四元数表示的函数，[v,phi]=qdecomp(q)为分开四元数的旋转分量的单位向量和角度的函数。

k₂计算的示例性实施方案通过以下代码给出

代码5

DECAY_FACTOR例如可以被设定为0.95。

当使用最新测量更新状态时，估计的倾斜角和初始偏航角偏移被用于构造

        式97

的最优序列。

考虑到

和

的序列对，求解A_n成为公知的Wahba问题。为解决该问题已经研究了许多替换算法。此处使用的LandisMarkley的SVD（Singular Value Decomposition，奇异值分解）算法被描述为以下步骤1-4：

（1）构成3×3矩阵L

         式98

（2）使用奇异值分解（SVD）分解L

[u s v]=SVD(L)          式99

（3）计算符号并构造w

$w=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& \det (u\&times;v^T)\end{array}\right]$         式100

（4）计算A

A=u×w×v^T            式101

当计算出A时，该方法将此A与以上EKF的最后状态所获得的一个值进行比较，然后使用代码4计算角度差。该角度差为估计出的对准矩阵的精度的估计。如上所述，角度差还被反馈以确定所设计的EKF的动态Q调整中k₁的乘法因数。

为了更容易地实时实现，使用如下9个1×3持久向量变量递归地存储历史数据：

      式102

因此，可使用下式计算式98，

$L_{n+1}=\left[\begin{array}{ccc}{\mathrm{ele}1}_{n+1}\&times;C_{n+1}& {\mathrm{ele}4}_{n+1}\&times;C_{n+1}& {\mathrm{ele}7}_{n+1}\&times;C_{n+1}\\ {\mathrm{ele}2}_{n+1}\&times;C_{n+1}& {\mathrm{ele}5}_{n+1}\&times;C_{n+1}& {\mathrm{ele}8}_{n+1}\&times;C_{n+1}\\ {\mathrm{ele}3}_{n+1}\&times;C_{n+1}& {\mathrm{ele}6}_{n+1}\&times;C_{n+1}& {\mathrm{ele}9}_{n+1}\&times;C_{n+1}\end{array}\right]$        式103

角度位置的参照序列可来自任何其它运动传感器的组合，甚至来自其他磁力计。该方法可用于具有3D加速计和3D旋转传感器的九轴型传感器单元的其它传感器单元。可使用各种传感器融合算法获得角度位置的参照序列。

地固重力参照系可被定义为具有如x轴和z轴的其它方向而非具有重力和磁北方向，只要重力参照系的轴可使用重力方向和磁北方向定位即可。

如果参考的角度位置没有未知的初始偏航偏移，则

可以作为局部磁场相对于参考的地固坐标系的偏航角度，式（67）被重写为

       式104

在获得这种对准矩阵后，由于在EKF状态下同时解出

和θ，所以还在地固坐标系中自动地求解出局部磁场向量。

对准算法可用于任何传感器3D与任何参照的装置主体的对准，并且不限于磁力计或惯性主体传感器。

对准算法可以一次取一批数据以在一个步骤中对其求解。

该方法可采用其他算法来替代上述用于精度验证算法的一个算法以解决Wahba问题。

此外，可使用稳定性计数器来确保角度差小于若干迭代的预定容差以避免重合（即，在解决方案没有改进时进行循环）。

可使用其它初始化的EKF实现类似结果。对准估计算法对初始化不敏感。

在以上示例性实施方式中使用的常数可被调整以实现特定的目的。k₁和k₂的值以及它们的适应性改变行为可以根据环境、传感器以及应用等在示例性实施方式中不相同。

总而言之，在本章节描述的方法提供简单、快速且稳定的方式，只要所有其他参数（标度、斜交和偏移）已被预先校正或以其它方式已知具有足够的精度，就在相对于任何未知环境中所参照的装置物固参照系、未知倾斜角和参照的姿态中未知的初始偏航角偏移（总共5个独立变量）实时地估计磁力计的偏差。这些方法不需要关于地固重力参照系中局部磁场的先验知识。用于对准精度的验证方法与对准算法关联，以使实时操作能够可靠、稳健并良好地进行。

用于追踪并补偿近场的方法

提供了使用磁力计相对于地固重力参照系的3D角度位置估计通过磁力计测量来动态地追踪并补偿动态磁近场的方法。3D角度位置不是非常精确的，其可包括滚动角、俯仰角的误差以及至少偏航角漂移。为动态近场所补偿的磁场测量对于指南针或3D角度位置是确定有用的。还未发现能够获得类似结构的传统方法。

根据示例性实施方式，图10是根据示例性实施方式的用于追踪并补偿动态磁近场的方法800的框图。在完全校正磁力计810和通过主体传感器的并发测量推断参照角度位置之后计算出的测量磁场值被输入用于追踪并补偿动态磁近场的算法830。应用算法830的结果为静态局部3D磁场值840（即，校正的并被近场补偿的磁力计测量）和与静态局部3D磁场值840关联的误差估计850。

图11是根据另一示例性实施方式的用于追踪并补偿磁近场的方法900的框图。图11的框图着重于数据流。包括3D磁力计的传感器块910将感测信号提供至传感器解释块920。传感器解释块920使用预先计算的参数以改进并将失真的传感器信号转换为标准单位，移除标度、斜交、偏移和偏差。磁场值被输出至动态磁近场追踪和补偿算法930。装置相对于地固重力参照系的角度位置940也被输入至算法930。角度位置经过随机滚动和俯仰角误差，尤其是随机偏航角误差漂移。算法930追踪因动态磁近场而产生的改变，并补偿装置物体参照系中输入的磁场值。算法930还使用补偿后的磁测量以修正被输入的角度位置中的误差，尤其是偏航角漂移。

下面的表4为用于阐述涉及追踪和补偿近场的方法的算法的符号列表。

表4

当地固重力参照系中的磁场不变时，装置的物体参照系中由磁力计测量的磁场可用于确定装置的物体参照系相对于地固重力参照系的3D方位（角度位置）。然而，当地固重力参照系中的磁场随时间改变时，磁力计测量明显地改变。这种时间相关性可因诸如耳机、扬声器、移动电话的任何近场扰动、添加或去除硬铁效应或软铁效应的来源等而改变。

如果在磁力计用于方位估计或指南针时近场扰动的存在是未知的，那么估计出的方位或北方向是不准确的。因此，要在实际情况中使用磁力计测量来确定3D方位和指南针，则需要磁近场追踪和补偿。此外，通过包括3D加速计和3D旋转传感器的组合而获得的角度位置会受偏航角漂移问题的影响，这是因为不能直接观察到装置的物体参照系相对于地固重力参照系的绝对偏航角。用于近场补偿的磁场值修正了引起偏航角漂移问题的缺陷。

校正后的磁力计（包括软铁和硬铁效应校正）测量：

^DB_n+1=(^DB₀+^DB_NF)_n+1       式105

其中， $B_0^D=Q_E^D\&times;H_0^E$       式106

以及 $B_{\mathrm{NF}}^D=Q_E^D\&times;H_{\mathrm{NF}}^E$         式107

该方法动态地追踪^EH_NF并使用其估计^DB_NF，然后将其从^DB_n补偿以获得

所估计的准备用于3D方位测量和指南针。方法可包括以下步骤。

步骤1：在两个持久3×1向量中，存储动态的^EH_NF的估计和最后稳定的^EH_NF的估计，动态的^EH_NF的估计和最后稳定的^EH_NF的估计分别被指示为

和

步骤2：在地固重力参照系中构造虚拟恒定的3×1向量

^EA=[0 0 |^EH₀|]^T       式108

步骤3：在地固重力参照系中构造观察向量

^EV=[^EH₀ ^EA]     式109

对于每个时步执行以下步骤。

步骤4：使用参考的方位（角度位置）计算装置的物体参照系中^EA的表示

          式110

通过以式108中指示的方式构造^EA，^DA_n+1不受

中偏航角误差的影响。^EA的z轴值可被设定为|^EH₀|的任何函数以表示向量^EA相对于^EH₀的相对权重。

步骤5：计算^DB_n+1与^DA_n+1之间的角∠^DB_n+1 ^DA_n+1;

步骤6：预测地固重力参照系中的磁场（包括近场）：

          式111

步骤7：计算当前磁场估计与

之差

       式112

步骤8：例如使用单指数平滑滤波器更新当前磁场估计

        式113

步骤9：计算的总大小，并将其减去^DB_n+1的大小

       式114

步骤10：计算

与^EA之间的角

步骤11：计算

与∠^DB_n+1 ^DA_n+1之间的角度差

            式115

步骤12：例如使用以下示例性实施方式估算磁近场是否是稳定的：

代码6

其中，sampleCount_的持久变量用于记录磁近场没有改变的时间。示例性地，k₁可设定为3，k₂可设定为4。σ为

$\mathrm{\&sigma;}=\sqrt{{\mathrm{\&sigma;}}_x^2+{\mathrm{\&sigma;}}_y^2+{\mathrm{\&sigma;}}_z^2}$       式116

步骤13：当sampleCount_大于预定阈值（例如，阈值可设定为等于1秒）时，将

更新为

然后将sampleCount_重设为0。步骤13的示例性实施方式为以下代码

代码7

步骤14：例如通过进行以下子步骤来评估当前样本是否与最新估计的稳定磁场一致。

子步骤14.1：计算

与∠^DB_n+1 ^DA_n+1之间的角度差

      式117

子步骤14.2：计算

的总和大小，并取其与^DB_n+1的大小之差

      式118

子步骤14.3：使用以下代码将在14.1和14.2计算的差与预定阈值进行比较

if

是，当前样本处于估计的稳定磁近场，前进至步骤15和16else

否，跳过步骤15和16，当前样本没有被近场补偿，

需要考虑方位估计或指南针，

等待下一样本

end

代码8

其中，k₁和k₂可设定为适当大小以允许包含更多的样本。应该注意，代码8中“else”步骤的选项是更新当前模型以使其更好地反映当前磁场。

步骤15：如果步骤14中的结果为当前样本与最新估计的稳定磁场一致，则进行以下子步骤。

子步骤15.1：使用在地固重力参照系中构造向量观察

     式119

子步骤15.2：在装置的物体参照系中构造向量观察

^DV_n+1=[^DB_n+1 ^DA_n+1]      式120

子步骤15.3：在装置的物体参照系和地固重力参照系中用向量观察形成3×3矩阵：

       式121

子步骤15.4：解出修正的

该子步骤可使用各种不同的算法来实现。以下描述使用奇异值分解（SVD）方法的示例性实施方式。

（1）使用SVD分解G

[u s v]=SVD(G)        式122

（2）计算符号并构建w

$w=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& \det (u\&times;v^T)\end{array}\right]$       式123

（3）计算

     式124

步骤16：计算

其中磁近场被补偿

      式125

步骤17：使用

估计与偏航角确定关联的误差

${\mathrm{\&epsiv;}}_{\mathrm{yaw}}=\sqrt{\frac{\mathrm{\&Delta;}{{\tilde{L}}_{n+1}}^2}{H_0^E{\left(1\right)}^2+H_0^E{\left(2\right)}^2}+\mathrm{\&Delta;}{{\widetilde{\mathrm{\&beta;}}}_{n+1}}^2+\frac{{\mathrm{\&sigma;}}^2}{3\&CenterDot;(H_0^E{\left(1\right)}^2+H_0^E{\left(2\right)}^2)}}$      式126

参数k₁和k₂可设定为磁力计校正的精度的动态函数。

用于融合不同偏航角估计以获得最优偏航角估计的方法

提供了用于融合（即，组合）偏航角的噪声估计的方法。在九轴型装置中，可使用校正的磁力计来获得一个偏航角估计，可通过诸如3D旋转传感器（例如，陀螺仪）的运动传感器获得另一短期稳定但长期漂移的偏航角估计。当偏航角误差较小时该方法允许平滑的小调整，而当偏航角较大时，允许快速的大调整。以下描述的方法实现了偏航角的高精度，从而在误差较小时得到平滑的稳定值，而在误差较大时得到快速响应的调整。应该注意，该相同的方式同样可应用于其他方位和位置参数，尤其可应用于俯仰角和滚动角。

根据示例性实施方式，图12为用于融合偏航角估计以获得最优偏航角估计的方法1000的框图。来自3D校正的磁力计的偏航角估计1010和来自主体传感器的偏航角测量1020被输入融合算法1030。算法1030输出最优偏航角估计1040和与最优偏航角估计1040关联的误差1050。

在以下对用于融合不同偏航角估计以获得最优偏航角估计的方法的算法的描述中，指数n指示时步n处的值。

本方法的一些实施方式使用在偏航角领域中运行的一维适应性滤波器。可选地，可使用Boolean变量（例如，被称为“noYawCorrectFromMag_”）指示是否执行用于融合的方法（即，保持来自磁力计的偏航角估计）。Boolean变量的值可根据是否满足预定条件而在默认值与其他值之间进行切换。方法可包括以下步骤。

步骤1：根据装置是否是平稳的（使用各种方法之一）确定是否使用融合（例如，将noYawCorrectFromMag_设置为假）。

步骤2：使用主体传感器获得预测的偏航角例如，可将3D加速计和3D陀螺仪用作主体传感器估计全角度位置。

步骤3：使用校正的以及近场补偿的磁场估计（根据使用磁北和重力定义地固重力参照系的方式）连同磁北与参照偏航零方向之间的相对初始偏航角偏移计算偏航角估计

步骤4：考虑以下a、b和c中的一个或多个计算总估计误差

a.校正精度

b.因传感器噪声、滚动和俯仰估计误差导致的偏航角计算误差

c.近场补偿误差

步骤5：应用适应性滤波器的修正方案，使用来自步骤2和3的偏航角估计和

作为对适应性滤波器的输入。适应性滤波器的输出为偏航角的最优估计

可使用以下过程中的任意一个过程或者这些过程的任意组合的结果来计算适应性滤波器的参数totalK。

过程1：K₁通常为更新

与步骤4计算出的totError（总误差）的比率的函数。更新为来自磁力计的当前偏航角

与来自最新状态的适应性滤波器的偏航角的预测最优估计之差。

    式127

在示例性实施方式中，K₁是更新

与“totError”

的比率的三阶多项式函数

     式128

K₁=0.033＊ratio_K1^3-0.083＊ratio_K1^2+0.054＊ratio_K1

         式129

其中，K₁被限定在0与1之间。

过程2：K₂为通过主体传感器（例如，陀螺仪）的预测偏航方差

与totError的平方

的比率

      式130

过程3：如果“totError”

不大于阈值

则K₃为1，否则为更新与针对主体传感器（例如，陀螺仪）的预测偏航误差的比率的函数。例如：

    式131

K₃计算的示例性实施方式为

代码9

过程4：如果更新

的绝对值大于阈值则K₄为1，否则为较小的常数值，例如0.001.

步骤6：计算totalK（k_n）。例如，

k_n=K₁·K₂·K₃·K₄      式132

如果满足某些条件，将totalK设定为0。这些条件为

1）更新

的绝对值小于校正的精度；

2）总估计误差“totError”

大于阈值

3）成员变量noYawCorrectFromMag_为真；

4）来自估计磁场的测量偏航角的IIR低通滤波版本与瞬时版本之差大于预定阈值（例如，0.04弧度）。

最优偏航估计计算为

      式133

或者计算为

    式134

其中，f(k_n)为k_n的函数。在示例性实施方式中，使用经过点[0,0.002]和[4,1]的非线性曲线并且在1处饱和。在另一示例性实施方式中，f(k_n)=k_n。假设来自磁力计的偏航角估计的误差被较好地界定，则通常提供具有较好界定的精度的偏航角，并因而可帮助修正从惯性传感器（例如，3D陀螺仪）估计的随意性大的漂移的偏航角。由于滤波器是适应性的，所以每个步骤的修正量是动态的，并可帮助更快地减少偏航误差而在装置处于平稳时仍保持稳定。

步骤7：可选地，如果应用使用角度位置，则将具有修正的偏航角的欧拉角转换成四元数（全角度位置）。

步骤8：可选地，如果（1）修正后的偏航角与使用估计的磁场测量的偏航角之差不大于预定阈值（例如，0.02弧度）以及（2）装置被检测为处于平稳，则将noYawCorrectFromMag_设定为真（当装置被手持时并且仅检测到震颤时其可被视为真）。

上述方法可单独地或组合地使用。图13示出了根据示例性实施方式的、使用运动传感器和附接至装置的磁力计来估计装置的物体参照系相对于重力参照系的偏航角的方法1100的流程图。术语“运动传感器”是指可提供滚动和俯仰的测量以及至少提供相对偏航（即，偏航的粗略估计）的测量的任何感测元件。

方法1100包括S1110，在S1110中从运动传感器和磁力计接收测量。所接收的测量可以为并发测量。术语“并发”是指以相同的时间间隔或相同的时步执行。

方法1100还包括S1120，在S1120中基于接收到的测量在物体参照系中确定装置的测量出的3D磁场、滚动角、俯仰角和偏航角的粗略估计。此处术语“测量出的3D磁场”是指基于从磁力计接收的测量（信号）确定的向量值。为常数的或者在磁力计的校正过程中所确定的各种参数可用于确定测量的3D磁场。类似地，通过从运动传感器接收的测量并使用为常数或者在运动传感器的校正过程中确定的参数来确定当前的滚动、俯仰和粗略估计偏航。

方法1100还包括S1130，在S1130中从测量的3D磁场提取局部3D磁场。可针对软铁效应、硬铁效应和磁力计相对于物体参照系的相对对准中的一个或多个来修正局部3D磁场。针对动态的近场补偿局部3D磁场。

方法1100还包括S1140，在S1140中使用至少两种不同的方法，基于提取的局部3D磁场、滚动角、俯仰角和偏航角的粗略估计计算重力参照系中装置的物体参照系的倾斜的补偿偏航角，其中，对于至少两种不同的方法，滚动角估计的误差、俯仰角估计的误差和提取的局部3D磁场的误差分别不同地影响倾斜的补偿偏航角的误差。根据上述示例性实施方式，该操作可使用用于通过使用滚动和俯仰补偿的倾斜来计算偏航角的方法或者用于融合不同偏航角估计以获得最优偏航角估计的方法中的任何一种方法来进行。

在图14中示出了根据示例性实施方式的使用运动传感器和附接至装置的磁力计的并发测量校正磁力计的方法1200的流程图。方法1200包括S1210，在S1210中从运动传感器和从磁力计接收并发测量集。

方法1200还包括S1220，在S1220中基于从磁力计接收的并发测量集中的测量确定用于计算所测量的磁场的参数，确定步骤使用从运动传感器接收的并发测量集中的测量获得当前滚动、俯仰和相对偏航，至少一些参数以解析的方式确定。根据上述示例性实施方式，该操作可使用确定（校正）与姿态无关的参数的方法和确定（校正）姿态相关参数（即，用于对准磁力计）的方法来进行。

所公开的示例性实施方式提供了可用于在磁力计与其他传感器结合来确定装置的方位时使用的工具包的一部分的方法，还提供了能够使用该工具包的系统。这些方法可在计算机程序产品中实现。应该理解，该描述不试图限制该发明。相反地，示例性实施方式旨在覆盖被包含在本发明的精神和范围内的替代、修改和等同实施，本发明的精神和范围由所附的权利要求限定。此外，在示例性实施方式的详细描述中，若干特定细节被提及以提供对所保护的发明的全面理解。然而，本领域技术人员应该理解，各种实施方式可被实践而无需这些特定细节。

示例性实施方式可表现为完全硬件的实施方式或结合硬件和软件方面的实施方式的形式。而且，示例性实施方式可表现为存储在计算机可读存储介质上的计算机程序产品，该计算机可读存储介质具有嵌入该介质中的计算机可读指令。可采用的任何适合的计算机可读介质包括硬盘、CD-ROM、数字化通用磁盘（DVD）、光存储装置或诸如软盘或磁带的磁存储装置。计算机可读介质的其它非限制性示例包括快闪型存储器或其它已知存储器。

虽然在特殊组合的实施方式中描述了本申请的示例性实施方式的特征和元件，但是每个特征或元件可以无需实施方式的其它特征和元件单独地使用，或者以具有或不具有本文所公开的其它特征和元件的各种组合中使用。本申请中提供的方法或流程图可以通过特殊编程的计算机或处理器执行而具体地体现为计算机可读存储介质中的计算机程序、软件或固件。

Claims (34)

1.使用运动传感器和附接至装置的磁力计来估计所述装置的物体参照系相对于重力参照系的偏航角的方法（1100），所述方法包括：

（S1110）从所述运动传感器和所述磁力计接收测量数据；

（S1120）基于所接收的测量数据，确定所述物体参照系中所测量的3D磁场、所述装置的滚动角、俯仰角和偏航角的概估；

（S1130）从所测量的3D磁场提取局部3D磁场；以及

（S1140）基于所提取的局部3D磁场、所述滚动角、所述俯仰角和所述偏航角的概估，使用至少两种不同的方法计算所述重力参照系中所述装置的所述物体参照系的倾斜的补偿偏航角，其中，对于所述至少两种不同的方法，所述滚动角的估计误差、所述俯仰角的估计误差、以及提取的局部3D磁场的误差对所述倾斜的补偿偏航角的误差具有不同影响。

2.如权利要求1所述的方法，其中，对于软铁效应、硬铁效应以及所述磁力计相对于所述物体参照系的相对对准中的一个或多个来修正所述局部3D磁场。

3.如权利要求1所述的方法，其中，所述局部3D磁场用于补偿动态近场。

4.如权利要求1所述的方法，其中，所述重力参照系为相对于重力和地球的磁场方向限定的地固正交参照系。

5.如权利要求1所述的方法，其中，所接收的测量数据为并发测量数据。

6.如权利要求3所述的方法，其中，所述局部3D磁场基于所测量的3D磁场的追踪评估来补偿动态近场。

7.如权利要求1所述的方法，其中，使用与传感器的本质特性相关的参数计算所测量的3D磁场。

8.如权利要求7所述的方法，其中，所述与传感器的本质特性相关的参数包括偏移、标度和斜交/交叉耦合矩阵中的一个或多个。

9.如权利要求1所述的方法，其中，

所述运动传感器包括加速计，所述加速计使用测量数据来确定所述装置的所述物体参照系相对于重力的斜度。

10.如权利要求1所述的方法，其中，所述计算的步骤包括估计所述倾斜的补偿偏航角的误差。

11.如权利要求1所述的方法，其中，所述计算的步骤包括：

获得与所述装置相关的另一参照系中的滚动和俯仰，所述另一参照系具有沿重力方向的z轴，以及

在所述重力参照系中估计静态磁场。

12.如权利要求11所述的方法，其中，所述获得的步骤包括对所述静态局部磁场和与重力相反的方向之间的角度进行估计。

13.如权利要求1所述的方法，其中，对使用所述至少两种不同的方法中的每一种所计算的所述倾斜的补偿偏航角的误差进行估计，输出与估计出的误差的最小值对应的倾斜的补偿偏航角的值。

14.如权利要求1所述的方法，其中，所述至少两种方法中的一种方法将所述偏航角计算为

其中，

和

为倾斜的修正滚动角和倾斜的修正俯仰角，

其中，

和

为使用所述偏航角的概估计算出的、

在所述重力参照系中的分量，

为提取的局部3D磁场和与重力相反的方向之间的角度，

为所述物体参照系中所述局部3D磁场的估计，

为所述物体参照系中与所述局部3D磁场的重力平行的分量的估计，以及

为所述物体参照系中与所述局部3D磁场的重力垂直的分量的估计。

15.如权利要求1所述的方法，其中，所述至少两种方法中的一种方法将所述偏航角计算为

其中，

和

为倾斜的修正滚动角和倾斜的修正俯仰角，

其中，

和

为使用所述偏航角的概估计算出的、

在所述重力参照系中的分量，

为提取的局部3D磁场和与重力相反的方向之间的角度，

为所述物体参照系中所述局部3D磁场的估计，

为所述物体参照系中与所述局部3D磁场的重力平行的分量的估计，以及

为所述物体参照系中与所述局部3D磁场的重力垂直的分量的估计。

16.如权利要求1所述的方法，其中，所述至少两种方法中的一种方法将所述偏航角计算为

其中，

和

为倾斜的修正滚动角和倾斜的修正俯仰角，

其中，

和

为使用所述偏航角的概估计算出的、

在所述重力参照系中的分量，

为提取的局部3D磁场和与重力相反的方向之间的角度，

为所述物体参照系中所述局部3D磁场的估计，

为所述物体参照系中与所述局部3D磁场的重力平行的分量述物体参照系中与所述局部3D磁场的重力垂直的分量的估计。

17.如权利要求6所述的方法，其中，使用与不同的时步对应的所述测量的3D磁场的第一值和使用磁场模型预测的所述磁场的第二值追踪动态近场，其中，对所述第一值和所述第二值进行比较以确定所述测量的3D磁场是否不同于所述磁场模型的预测。

18.如权利要求17所述的方法，其中，如果比较的结果为所测量的3D磁场与所述磁场模型的预测相同，则估计偏航角的误差。

19.如权利要求17所述的方法，其中，如果比较的结果为所测量的3D磁场与所述磁场模型的预测相同，则估计滚动角的误差。

20.如权利要求17所述的方法，其中，如果比较的结果为所测量的3D磁场与所述磁场模型的预测相同，则估计俯仰角的误差。

21.如权利要求17所述的方法，其中，如果比较的结果为所测量的3D磁场与所述磁场模型的预测不同，则更新所述磁场模型。

22.如权利要求1所述的方法，其中，

所述运动传感器包括惯性传感器，所述惯性传感器的测量数据产生惯性传感器偏航角，以及

所述计算的步骤包括基于所述倾斜的补偿偏航角和所述惯性传感器偏航角确定最优偏航角估计，

其中，确定所述最优偏航角估计包括计算与所述倾斜的补偿偏航角和所述惯性传感器偏航角关联的误差。

23.如权利要求22所述的方法，其中，所述确定的步骤包括使用适应性滤波器来组合所述倾斜的补偿偏航角和所述惯性传感器偏航角。

24.如权利要求23所述的方法，其中，所述确定的步骤包括基于校正精度、因传感器噪声导致的偏航角计算误差、滚动和俯仰估计误差、以及近场补偿误差中的一个或多个，使用计算出的总估计误差计算适应性滤波器的增益系数。

25.如权利要求24所述的方法，其中，所述适应性滤波器的系数为更新变量的绝对值与总估计误差之比，所述更新变量为从磁力计测量数据推断出的当前偏航角与从所述适应性滤波器的先前输入得到的偏航角的预测最优估计之差。

26.如权利要求24所述的方法，其中，所述适应性滤波器的系数为在使用所述惯性传感器时预测的偏航误差的第一平方值与所述总估计误差的第二平方值之比。

27.如权利要求24所述的方法，其中，如果所述总估计误差小于预定阈值，则所述适应性滤波器的系数为1，否则所述适应性滤波器的系数为所述更新变量的绝对值与在使用所述惯性传感器时预测的偏航角误差之比的函数，所述更新变量为从磁力计测量数据推断出的当前偏航角与从所述适应性滤波器的先前输入得到的偏航角的预测最优估计之差。

28.如权利要求24所述的方法，其中，如果更新变量小于预定阈值，所述适应性滤波器的系数为1，否则所述适应性滤波器的系数为预定的较小值。

29.如权利要求24所述的方法，其中，所述适应性滤波器的系数为以下量中的两个或更多个的乘积

（1）更新变量的绝对值与所述总估计误差之比，

（2）在使用所述惯性传感器时预测的偏航误差的第一平方值与所述总估计误差的第二平方值之比，

（3）如果所述总估计误差小于第一预定阈值则为1，否则为更新变量的绝对值与在使用所述惯性传感器时预测的偏航角误差之比的函数，

（4）如果所述更新变量小于第二预定阈值则为1，否则为预定的较小值，

所述更新变量为从磁力计测量数据推断出的当前偏航角与从所述适应性滤波器的先前输入得到的偏航角的预测最优估计之差。

30.如权利要求24所述的方法，其中，所述最优偏航角估计为（A）和（B）之和，其中（A）为基于前一步骤的最优偏航角估计通过所述惯性传感器得到的预测偏航角，（B）为更新变量与所述适应性滤波器的系数的函数之积，所述更新变量为从磁力计测量数据推断出的当前偏航角与从所述适应性滤波器的先前输入得到的偏航角的预测最优估计之差。

31.一种设备（100），包括：

具有刚性主体（101）的装置；

3D磁力计（120），安装在所述装置上并且被配置为生成与局部磁场对应的测量数据；

运动传感器（110），安装在所述装置上并且被配置为生成与所述刚性主体的方位对应的测量数据；以及

至少一个处理单元（130），所述至少一个处理单元被配置为

（1）从所述运动传感器和从所述磁力计接收测量数据；

（2）基于所接收的测量数据，确定物体参照系中所测量的3D磁场、所述装置的滚动角、俯仰角和偏航角的概估；

（3）从所测量的3D磁场提取局部3D磁场；以及

（4）基于所提取的局部3D磁场、所述滚动角、所述俯仰角和所述偏航角的概估，使用至少两种不同的方法计算所述重力参照系中所述装置的所述物体参照系的倾斜的补偿偏航角，其中，对于所述至少两种不同的方法，所述滚动角的估计误差、所述俯仰角的估计误差、以及提取的局部3D磁场的误差对所述倾斜的补偿偏航角的误差具有不同影响。

32.如权利要求31所述的设备，其中，所述至少一个处理单元包括设置在所述装置中的并且被配置为执行（1）-（4）中至少一个步骤的处理单元。

33.如权利要求31所述的设备，其中，所述至少一个处理单元包括远程地定位并且被配置为执行（1）-（4）中至少一个步骤的处理单元，所述设备还包括发送器，所述发送器安装在所述装置上并且被配置为向远程定位的处理单元发送数据。

34.一种被配置为存储可执行代码的计算机可读存储介质（135），当所述可执行代码在计算机上执行时使所述计算机执行使用运动传感器和附接至装置的磁力计来估计所述装置的物体参照系相对于重力参照系的偏航角的方法，所述方法包括：

（S1110）从所述运动传感器和所述磁力计接收测量数据；

（S1120）基于所接收的测量数据，确定所述物体参照系中所测量的3D磁场、所述装置的滚动角、俯仰角和偏航角的概估；

（S1130）从所测量的3D磁场提取局部3D磁场；以及

（S1140）基于所提取的局部3D磁场、所述滚动角、所述俯仰角和所述偏航角的概估，使用至少两种不同的方法计算所述重力参照系中所述装置的所述物体参照系的倾斜的补偿偏航角，其中，对于所述至少两种不同的方法，所述滚动角的估计误差、所述俯仰角的估计误差、以及提取的局部3D磁场的误差对所述倾斜的补偿偏航角的误差具有不同影响。

Images