CN103017763B - 状态估计设备和偏移更新方法 - Google Patents

Abstract

状态估计设备具有包括用于检测磁分量的三维磁性传感器的多个传感器。卡尔曼滤波器用于基于观测残差更新状态向量。状态向量具有代表多个状态变量的分量，多个状态变量包括用于估计磁测数据的偏移量的状态变量。中心点计算单元假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面附近，并且计算第一球面的中心点。卡尔曼滤波器使用第一球面的中心点来对用于估计磁测数据的偏移量的状态变量进行更新。

Description

状态估计设备和偏移更新方法

技术领域

本发明涉及状态估计设备和偏移更新方法。

背景技术

近年来，随着便携式仪器的性能和用途的提升，人们正在积极研发具有估计地磁方向和便携式仪器姿态的功能的便携式仪器。在估计地磁的方向和便携式仪器的姿态的情况下，使用测量各种不同类型物理量的多个传感器的输出的综合来实现估计，速度比仅仅使用单独一个传感器（比如地磁传感器）的输出实现的估计要快得多。

使用卡尔曼（Kalman）滤波器的方法是一种众所周知的对测量各种不同类型物理量的多个传感器的输出进行综合以估计动态***状态的方法。例如，专利文献1公开了一种姿态角度测量设备，具有安装在其中的三轴角速度传感器、三轴加速度传感器和一个非线性卡尔曼滤波器。而且，非专利文献1公开了一种使用扩展卡尔曼滤波器或西格玛点（sigma point）卡尔曼滤波器（使用无损变换（unscentedtransformation））对三轴角速度传感器、三轴加速度传感器和三轴地磁传感器输出的信号进行综合的方法。

[专利文献1]日本专利申请公开第H9-5104号

[非专利文献1]Wolfgang Gunthner所著的《Enhancing CognitiveAssistance Systems with Inertial Measurement Units》，Springer，2008

一般来说，卡尔曼滤波器具有用于估计代表动态***状态的多个物理量的时基变化的状态转移模型和用于根据动态***的估计状态来估计由动态***的多个传感器测得的观测值的观测模型。而且，卡尔曼滤波器使用估计的观测值与传感器实际测量的观测值之间的差值（观测残差），将以代表动态***状态的物理量为元素的状态向量更新为更加接近于实际值的值。

不过，观测残差是利用更新前的状态向量计算出来的值。在动态***的状态突然和明显变化的情况下，状态向量会被更新为不同于实际值的假值。

发明内容

本发明是鉴于上述问题而做出的，并且本发明的一个目的是，即使在动态***的状态突然发生大幅变化的情况下，也能正确地估计动态***的状态。

在下文中，将会介绍本发明。同时，为了便于理解，在括号中给出了实施方式、变型例和附图中的附图标记，不过，本发明并不局限于这些实施方式。

为了解决上述问题，按照本发明的状态估计设备包括：

多个传感器，其中包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的各磁分量的磁测数据（q_i）；卡尔曼滤波器，其构成为用来基于观测残差（z_k）更新状态向量（x_k），该状态向量具有代表多个状态变量的分量，所述多个状态变量包括用于估计磁测数据的偏移量（q_OFF）的状态变量（q_O），所述观测残差是使用状态向量和具有代表多个传感器的输出的分量的观测向量（y_k）计算出来的；累积单元，其构成为用来累积从三维磁性传感器中相继输出的磁测数据；和中心点计算单元，其构成为用来假设由预定数量（N）的累积磁测数据表示的坐标有可能分布在第一球面（S）附近，并且构成为用来计算表示该第一球面的中心点（c_O）的坐标，其中，卡尔曼滤波器使用表示第一球面的中心点（c_O）的坐标来对用于估计磁测数据的偏移量（q_OFF）的状态变量（q_O）进行更新或重写。

便利地，三维磁性传感器被安装在包括产生磁场（B_i）的部件的仪器中。在这种情况下，所述偏移量（q_OFF）是代表由该仪器的所述部件产生的磁场（B_i）的分量的三维向量。

按照本发明，包括中心点计算单元和卡尔曼滤波器的状态估计设备使用由中心点计算单元计算出来的中心点的坐标来对用于估计三维磁性传感器的偏移量的状态变量（该状态变量是卡尔曼滤波操作中使用的状态向量的分量之一）进行更新或重写，并且进行卡尔曼滤波操作。

卡尔曼滤波器基于使用观测向量和状态向量计算出来的观测残差来执行更新状态向量的操作，从而使得状态向量接近于动态***的实际值，所述状态向量的分量物理量（状态变量）代表了某一时刻下的动态***状态，所述观测向量的分量是来自测量各种不同种类物理量的多个传感器的输出值。

具体地说，卡尔曼滤波器使用代表动态***状态的时基变化的状态转移模型，根据某一时刻的状态向量来估计经过单位时间之后的状态向量。随后，卡尔曼滤波器使用根据动态***状态的估计值用于估计传感器的输出值的观测模型，根据估计的状态向量来估计观测向量。此外，卡尔曼滤波器计算估计的观测向量与以来自各种不同种类的传感器的实际输出（实际观测值）为其分量的观测向量之间的差值，以计算观测残差。而且，卡尔曼滤波器使用观测残差更新状态向量。

同时，安装了三维磁性传感器的仪器常常包括产生磁场的机械或电子部件，比如可以被磁化的各种类型的金属和电路。在这种情况下，三维磁性传感器输出的向量数据除了代表目标地磁的向量之外，还包括代表由仪器中安装的部件产生的磁场的向量。因此，为了正确检测地磁的值，必须进行修正处理，以从三维磁性传感器输出的向量数据中除去代表仪器的部件所产生的内部磁场的向量。在修正处理中为了获得所要检测的地磁的正确值而从三维磁性传感器输出的数据中除掉了所述内部磁场，代表该内部磁场的向量被称为三维磁性传感器的偏移量。

在仪器的内部状态没有变化的情况下，内部磁场具有恒定不变的方向和幅度。另一方面，在仪器的内部状态变化的情况下，内部磁场的方向和幅度都发生变化。取决于仪器的用户操作、仪器的外部环境等，仪器的内部状态可能会突然发生很大变化。结果，仪器部件产生的内部磁场（即，三维磁性传感器的偏移量）也会随着仪器的内部状态的变化而突然发生很大变化。

如前面所介绍的，卡尔曼滤波操作包括使用代表动态***状态的时基变化的状态转移模型根据某一时刻的状态向量的值来估计经过单位时间之后的状态向量的值的操作。因此，由卡尔曼滤波器估计的状态变量最好是能够在状态转移模型中定式化（formulized）的物理量。不过，由于三维磁性传感器的偏移量会因用户对仪器的操作而突然急剧变化，因此很难确定偏移量值的变化。

而且，用于更新状态向量的观测残差是基于更新之前状态向量的值来计算的。即，通过卡尔曼滤波操作计算出来的更新之后的状态向量的值受到更新之前的在先状态向量的值的影响。此外，更新之前的在先状态向量的值受到再之前的更新之前的过去状态向量的值的影响。因此，在卡尔曼滤波操作重复进行的情况下，由卡尔曼滤波器估计的状态向量的值是考虑了从卡尔曼滤波操作开始时到估计出状态向量的值时这样的范围内的所有步骤中的状态向量的值的前提下计算出来的。因此，在三维磁性传感器的偏移量突然发生很大变化的情况下，用于估计三维磁性传感器的偏移量的更新之后的状态变量的值可能会大大背离于变化之后的三维磁性传感器的偏移量。

在状态向量的值收敛于大大偏离正确表示实际物理量的值（实际值）的情况下，状态向量的值可能不会接近实际值，尽管随后会重复进行卡尔曼滤波操作。而且，即使状态向量的值接近实际值，也要花费很长时间，状态向量的值才会收敛于实际值。在这种情况下，状态估计设备不能正确估计***状态，并且因此，不能基于由状态估计设备计算出来的状态变量来计算地磁的方向和仪器的姿态。

地磁是具有指向地球的北磁极的水平分量和垂直于磁倾角方向的分量的一种磁场。地磁是相对于地面具有恒定不变的方向和恒定不变的幅度的均匀磁场。因此，在仪器的姿态相对于地面发生变化的情况下，从仪器看去的地磁方向也会变化。即，当从仪器中安装的三维磁性传感器角度看去时，地磁被表示为方向随着仪器姿态的改变而改变而幅度恒定不变的向量。

另一方面，在仪器的内部状态不变的情况下，内部磁场具有相对于仪器恒定不变的方向和恒定不变的幅度。因此，在仪器的内部状态不变的情况下，即使在从仪器中安装的三维磁性传感器看去时仪器的姿态发生了改变的情况下，内部磁场也被表示为方向恒定不变且幅度恒定不变的向量。

结果，在改变三维磁性传感器的姿态的同时获取的多个磁测数据分布在中心由代表内部磁场的方向和幅度的向量的前端确定并且半径与地磁的幅度相对应的球面的附近。

假设多个磁测数据所表示的坐标有可能分布在某一球面（第一球面）的附近，中心点计算单元计算第一球面的中心点的坐标，以计算内部磁场，即，三维磁性传感器的偏移量。即使在三维磁性传感器的偏移量突然大幅变化的情况下，中心点计算单元也可以使用多个后来获得的磁测数据来计算正确的偏移量。

在按照本发明的状态估计设备中，使用中心点计算单元输出的中心点来对用于估计三维磁性传感器的偏移量的状态变量进行重写，该状态变量是构成状态向量的分量之一。

因此，即使在三维磁性传感器的偏移量突然发生很大变化的情况下，中心点计算单元也可以计算出代表变化之后的偏移量的中心点的坐标，并且可以使用中心点的坐标来对用于估计三维磁性传感器的偏移量的状态变量进行重写。结果，可以防止用于估计三维磁性传感器的偏移量的状态变量长时间背离实际值（三维磁性传感器的偏移量）。即，按照本发明的状态估计设备正确估计***状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。

在实际状态下，卡尔曼滤波器被构成为用来使用状态转移模型（f）根据某一时刻的状态向量（x_k-1）来估计经过单位时间之后的状态向量(x^- _k)，构成为用来使用观测模型(h)根据经过单位时间之后的估计的状态向量(x^- _k)来计算估计的观测向量(y^- _k)，构成为用来基于估计的观测向量(y^- _k)和具有代表多个传感器的输出的分量的观测向量(y_k)来计算观测残差(z_k)，并且构成为用来基于观测残差(z_k)和经过单位时间之后的估计的状态向量(x^- _k)来更新状态向量(x_k)。

状态估计设备此外还包括：分布判定单元，其构成为用来计算表示预定数量(N)的累积磁测数据的坐标分布的三维分散度的分散评估值，从而使得中心点计算单元构成为用来在该分散评估值等于或大于可容许的分散值(λ_O)的情况下计算表示第一球面(S)的中心点(c_O)的坐标；变形判定单元，其构成为用来假设多个累积磁测数据所表示的坐标有可能分布在通过将第二球面(S₂)和曲面(SX)组合起来而获得的三维图形(SD)的表面附近，构成为用来基于所述多个累积磁测数据对表示三维图形的形状与第二球面的形状之间的差异程度的变形评估值(g_D(E))进行计算，并且构成为用来判断该变形评估值是否等于或小于可容许的变形值(δ_O)；和中心点输出单元，其构成为用来在变形判定单元的判断结果是肯定结果的情况下输出表示第一球面的中心点的坐标。

在这种情况下，卡尔曼滤波器被构成为用来在中心点输出单元输出表示第一球面的中心点的坐标的情况下，使用表示第一球面的中心点的坐标来对用于估计偏移量的状态变量进行更新。

如前面所介绍的，中心点计算单元在假设由磁测数据表示的坐标分布在第一球面附近的前提下，计算第一球面的中心点的坐标。因此，在磁测数据具有平面分布而不具有三维扩展的情况下，不能正确确定第一球面的中心点的坐标。

按照本发明，首先，分布判定单元计算表明磁测数据的三维扩展程度的分散评估值，并且在分散评估值等于或大于可允许的分散值的情况下，中心点计算单元计算中心点的坐标。因此，在磁测数据是二维分布的情况下，可以防止中心点计算单元计算不正确的中心点坐标。

结果，可以防止修正处理使用不正确的偏移量而计算出不正确的地磁方向和幅度。而且，由于可以防止进行计算不正确中心点的坐标的操作处理，因此可以实现状态估计设备的低功耗。

而且，按照本发明，变形判定单元基于变形评估值判断磁测数据的坐标在其表面附近的三维图形的形状与第二球面的形状之间的不同达到什么程度。

在变形评估值大于阈值的情况下，三维图形具有不同于第二球面的形状的变形的形状。结果，在三维图形表面附近的由磁测数据表示的坐标并不能被认为分布在第一球面附近。由于中心点计算单元以磁测数据处于第一球面的附近为前提计算作为三维磁性传感器的偏移量的候选值的中心点坐标，因此表示在其附近没有磁测数据的第一球面的中心点的坐标的向量并不代表三维磁性传感器的偏移量。由于按照本发明的状态估计设备包括变形判定单元，可以防止使用表示不能被当作三维磁性传感器的偏移量的不正确中心点坐标的向量来对用于评估三维磁性传感器偏移量的状态变量进行重写。

如上所述，按照本发明的状态估计设备包括分布判定单元和变形判定单元。因此，仅仅在获得了接近于实际值的可以被当作三维磁性传感器的偏移量的中心点坐标的情况下，状态估计设备才使用表示接近于实际值的中心点坐标的向量来对用于估计三维磁性传感器的偏移量的状态变量进行重写。结果，按照本发明的状态估计设备可以正确估计***状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。

而且，作为本发明的具体实施方式，状态估计设备可以包括这样的单元：在变形判定单元的判断结果为否定结果的情况下，督促用户以不改变仪器位置的方式改变仪器的姿态。

按照本发明，在变形判定单元判定变形评估值大于可允许的变形值的情况下，即，在三维磁性传感器输出的磁测数据表示的坐标分布在三维图形表面附近的时候，三维图形的形状与球面间具有很大变形的情况下，状态估计设备包括用于督促用户以不改变仪器位置的方式改变仪器姿态的单元。

除了要测量的地磁之外，三维磁性传感器还可以检测仪器的机械或电子部件产生的内部磁场和仪器外部的物体产生的不均匀外部磁场。在非均匀外部磁场的影响很大的情况下，磁测数据表示的坐标分布在具有大不同于球面的变形形状的三维图形表面附近，从而不能采用表示由中心点计算单元计算出来的中心点的坐标的向量作为三维磁性传感器的偏移量。

不过，在生成外部磁场的仪器外部物体和三维磁性传感器之间的相对位置关系不变的情况下非均匀外部磁场仅仅是具有恒定不变幅度的磁场。即，在三维磁性传感器的姿态在三维磁性传感器的位置固定的状态下发生改变的情况下，外部磁场被三维磁性传感器检测为均匀磁场，其幅度是恒定不变的并且只有方向发生变化。在这种情况下，由三维磁性传感器输出的磁测数据表示的坐标分布在某一球面的附近。而且，这个球面的中心点的坐标与表示仪器部件产生的磁场（内部磁场）分量的向量所表示的坐标几乎相同。

如前面所介绍的，第一球面是在假设由磁测数据表示的坐标分布在第一球面附近的前提下被设置的。因此，在磁测数据表示的坐标分布得具有球面的形状的情况下，由磁测数据表示的坐标可以被视为分布在以中心点计算单元计算出来的中心点为中心的第一球面附近。结果，第一球面的中心点的坐标与表示内部磁场分量的向量所表示的坐标几乎相同，并且因此，可以采用表示第一球面的中心点的坐标的向量作为三维磁性传感器的偏移量。

如上所述，状态估计设备包括用于督促用户以改变仪器位置的方式改变仪器姿态的单元，并且因此，即使在存在非均匀外部磁场的位置上，也可以督促用户进行计算适当偏移量的操作。

而且，作为本发明的具体实施方式，在状态估计设备中，可以将分散评估值设置为代表磁测数据表示的三轴坐标的分散度的方差-协方差矩阵（A）的最小特征值（λ₃）。

按照本发明，代表磁测数据的分散度的方差-协方差矩阵的最小特征值被用作分散评估值。

由于磁测数据表示三轴坐标，因此代表磁测数据分散度的方差-协方差矩阵是3×3的矩阵。从该方差-协方差矩阵获得三个特征向量和三个特征值。三个特征向量之一代表磁测数据最大分布的方向。与这一特征向量相对应的特征值变为最大的特征值。另一方面，与表示磁测数据最小分布的方向的特征向量相对应的特征值变为最小特征值。在磁测数据是理想的二维分布的情况下，最小特征值具有无限接近于零的值。在磁测数据是三维分布的情况下，按照磁测数据分布的三维分散度，最小特征值具有大值。结果，该方差-协方差矩阵的最小特征值被用作分散评估值，并且因此，可以正确掌握磁测数据分布的三维分散度。

而且，按照本发明的具体实施方式，在状态估计设备中，当针对各个磁测数据计算代表以第一球面的中心点为原点的坐标系中代表磁测数据的三轴向量(qi-c_O)与通过使用变形评估矩阵（E）（其是对称矩阵）转换三轴向量而获得的向量（E(qi-c_O)）的内积时，以计算结果为其分量的向量被设置为变形误差向量（k(E)），代表由磁测数据所表示的三轴坐标确定的位置与第二球面（S₂）之间的误差的向量被设置为第二球面误差向量（δ_S2），变形误差向量和第二球面误差向量的和被设置为固定误差向量（δ_SD）。当在变形评估矩阵（E）的分量（e₁₁到e₃₃）和表示第二球面的中心点（c_O2）的三轴坐标被设置为变量的同时，代表固定误差向量的幅度的函数被设置为变形评估函数（f_SD(E,c)）的时候，在变形评估函数最小时，变形评估值可以是变形评估矩阵的范数（norm）。

按照本发明，固定误差向量的幅度被最小化，以计算变形评估矩阵和第二球面中心点的坐标。而且，变形评估值被计算为变形评估矩阵的范数。由于变形评估矩阵是用于转换三轴向量的坐标的对称矩阵，因此变形评估矩阵是3×3矩阵。结果，变形评估值具有三个特征值，并且具有最大绝对值的特征值（三个特征值之一）的绝对值成为变形评估值。

固定误差向量被确定为第二球面误差向量和变形误差向量之和。第二球面误差向量仅仅是代表由磁测数据表示的坐标与第二球面之间的误差的向量。在第二球面被设置得使磁测数据表示的坐标与第二球面之间的误差最小的情况下，球面误差向量所表示的所有误差成为白噪声，白噪声是对称的并且不取决于方向。

另一方面，变形误差向量是：针对各个磁测数据，安排从第一球面的中心点看去的表示磁测数据所表示的坐标的向量与使用变形评估矩阵转换表示由磁测数据表示的坐标的向量而获得的向量的内积，而获得的向量。换句话说，变形误差向量的各个分量被配置成三变量二次型的形式，以变形评估矩阵（对称矩阵）为系数矩阵并且以代表从中心点看去的磁测数据的坐标的向量的三个分量中的每一个作为变量。即，变形误差向量是：在由磁测数据表示的坐标与第二球面之间的误差存在于基于二次型中给出的相同函数的曲面上这一限制条件下，表示由磁测数据表示的坐标与第二球面之间的各个误差的向量。可以通过使用变形误差向量表示磁测数据和第二球面之间的误差，表示除了白噪声之外的基于二次函数的曲面所代表的噪声（即，来自球面的变形）。

固定误差向量是通过将球面误差向量与表示多个磁测数据位于其表面附近的三维图形的变形误差向量相加而获得的。即，三维图形是通过将球面和曲面叠加来表示的，并且不同于球面。而且，固定误差向量中变形误差向量的幅度可以通过确定三维图形的形状不同于（变形于）第二球面的形状的程度（即，由于不同于白噪声的表面发生的变形造成的误差的幅度）来评估。结果，在评估出三维图形与第二球面彼此近似达到了三维图形与第二球面具有基本相同形状的程度的情况下，由磁测数据表示的坐标可以被认为也分布在第二球面的附近。如前面所介绍的，在磁测数据所表示的坐标分布得具有球面的形状的情况下，可以将磁测数据所表示的坐标看作也分布在第一球面的附近。因此，在评估出三维图形与第二球面彼此近似达到了三维图形与第二球面具有基本相同形状的程度的情况下，可以采用表示第一球面的中心点的坐标的向量作为三维磁性传感器的偏移量。另一方面，在评估出出三维图形具有大大不同于第二球面的形状的变形形状的情况下，磁测数据所表示的坐标不能被看作分布在第二球面的附近，并且因此，需要避免采用表示第一球面的中心点的坐标的向量作为三维磁性传感器的偏移量。

即，状态估计设备可以在中心点计算单元计算出来的中心点的坐标当中，仅仅采用不受非均匀外部磁场影响的多个磁测数据计算出来的、表示适当中心点坐标的向量，作为三维磁性传感器的偏移量。结果，按照本发明的状态估计设备正确估计***状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。

而且，在前面介绍的状态估计设备中，中心点计算单元被构成为用来假设由磁测数据表示的坐标所指定的位置有可能分布在第一球面的附近，构成为用来设置代表由磁测数据表示的坐标所指定的位置与第一球面之间的误差的第一球面误差向量（δ_S），构成为用来设置包含三维向量（c）作为变量并且表示第一球面误差向量（δ_S）的幅度的中心点计算函数（f_S(c)），并且构成为用来计算表示第一球面的中心点的坐标，作为当中心点计算函数的值最小时由三维向量表示的坐标。

按照本发明，可以通过简单的计算过程计算出变为三维磁性传感器的偏移量的候选值的坐标。

而且，按照本发明的具体实施方式，在状态估计设备中，变形评估函数可以是由下列的f_SD(E,c)表示。

f_SD(E，c)＝‖δ_SD‖₂

其中，N是等于或大于5的自然数，并且

当由相应磁测数据表示的三轴坐标由q_i(i=1,...,N)表示时，表示第一球面的中心点的三轴坐标由c_O表示，这是一个三维向量，将变形评估矩阵设置为由下列的E表示的3×3矩阵，由下列k(E)表示的N维向量表示变形误差向量，

c被设置为以代表第二球面的中心点的三个变量为其分量的三维向量，X被设置为下列的N×3矩阵，q_C被设置为下列的三维向量，R被设置为下列的值，j被设置为下列的N维向量，并且第二球面误差向量由下列δ_S2所表示的N维向量表示，

固定误差向量是由下列δ_SD表示的N维向量。

δ_SD＝δ_S2+k(E)

$k\left(E\right)=\left[\begin{array}{c}{(q_1-c_O)}^{T}E(q_1-c_O)\\ .\\ .\\ .\\ {(q_N-c_O)}^{T}E(q_N-c_O)\end{array}\right]$ $E=\left[\begin{array}{ccc}{e}_{11}& e_{12}& e_{13}\\ e_{12}& e_{22}& e_{23}\\ e_{13}& e_{23}& e_{33}\end{array}\right]$

δ_S2＝X(c-qC)-j $X=\left[\begin{array}{c}{(q_1-q_C)}^T\\ .\\ .\\ .\\ {(q_N-q_C)}^T\end{array}\right]$

$j=\frac{1}{2}\left[\begin{array}{c}{(q_1-q_C)}^T(q_1-q_C)-R\\ .\\ .\\ .\\ {(q_N-q_C)}^T(q_N-q_C)-R\end{array}\right]$ $q_C=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{q}_i$

$R=\frac{1}{N}\stackrel{N}{\mathrm{\Σ}}{(q_i-q_C)}^T(q_i-q_C)$

按照本发明，可以通过简单的计算过程来计算变形评估值，该变形评估值是变形评估函数最小时变形评估矩阵的范数。结果，前面介绍的状态估计设备具有可以提高处理速度和实现低功耗的优点。

而且，按照本发明的具体实施方式，在状态估计设备中，可以由下列的f_s(c)表示中心点计算函数。

f_S(c)＝‖δ_S‖₂

其中，N是等于或大于5的自然数，并且

当由相应磁测数据表示的三轴坐标由q_i(i=1,..,N)表示时，c被设置为以用于代表表示第一球面的中心点的三轴坐标的三个变量作为其分量的三维向量，X被设置为下列的N×3矩阵，q_C被设置为下列的三维向量，R被设置为下列的值，并且j被设置为下列的N维向量，第一球面误差向量是由下列的δ_S表示的N维向量，

δ_S＝X(C-q_C)-j

$X=\left[\begin{array}{c}{(q_1-q_C)}^T\\ .\\ .\\ .\\ {(q_N-q_C)}^T\end{array}\right]$ $q_C=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{q}_i$

$j=\frac{1}{2}\left[\begin{array}{c}{(q_1-q_C)}^T(q_1-q_C)-R\\ .\\ .\\ .\\ {(q_N-q_C)}^T(q_N-q_C)-R\end{array}\right]$ $R=\frac{1}{N}\stackrel{N}{\mathrm{\Σ}}{(q_i-q_C)}^T(q_i-q_C)$

按照本发明，可以通过简单的计算过程计算出中心点。结果，前面介绍的状态估计设备具有可以提高处理速度和实现低功耗的优点。

而且，本发明提供了一种在状态估计设备中进行的偏移量更新方法，该状态估计设备包括多个传感器，所述多个传感器包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的磁分量的磁测数据，该偏移量更新方法包括：基于观测残差更新状态向量，该状态向量具有代表多个状态变量的多个分量，所述多个状态变量包括用于估计磁测数据的偏移量的状态变量，所述观测残差是使用状态向量和具有代表多个传感器的输出的多个分量的观测向量计算出来的；累积从三维磁性传感器相继输出的磁测数据；假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面的附近；计算表示第一球面的中心点的坐标；使用表示第一球面的中心点的坐标来对用于估计磁测数据的偏移量的状态变量进行更新。

按照本发明，使用基于磁测数据计算出来的中心点的坐标，重写用于估计三维磁性传感器的偏移量的状态变量（其是构成状态向量的分量之一）。即使在三维磁性传感器的偏移量突然发生很大变化的情况下，也可以防止用于估计三维磁性传感器的偏移量的状态变量背离于实际值（三维磁性传感器的偏移量）。即，状态估计设备正确估计***状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。

而且，本发明提供了一种安装在状态估计设备中的偏移量更新程序，该状态估计设备包括多个传感器，所述多个传感器包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的磁分量的磁测数据。该偏移量更新程序由状态估计设备中的计算机运行，以执行下列处理：基于观测残差更新状态向量，该状态向量具有代表多个状态变量的多个分量，所述多个状态变量包括用于估计磁测数据的偏移量的状态变量，所述观测残差是使用状态向量和具有代表多个传感器的输出的多个分量的观测向量计算出来的；累积从三维磁性传感器相继输出的磁测数据；假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面的附近；计算表示第一球面的中心点的坐标；使用表示第一球面的中心点的坐标来对用于估计磁测数据的偏移量的状态变量进行更新。

按照本发明，使用基于磁测数据计算出来的中心点的坐标，重写（更新，重置或初始化）用于估计三维磁性传感器的偏移量的状态变量（其是构成状态向量的分量之一）。因此，即使在三维磁性传感器的偏移量突然地发生急剧变化的情况下，也可以防止用于估计三维磁性传感器的偏移量的状态变量背离于实际值（三维磁性传感器的偏移量）。即，状态估计设备正确估计***状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。

附图说明

图1是表示按照本发明的实施方式的便携式仪器的结构的框图。

图2是表示便携式仪器的外观的立体图。

图3是图解说明按照本发明的实施方式的三维磁性传感器检测到的磁场的轮廓的概念图。

图4是表示通过运行按照本发明实施方式的状态估计程序而实现的功能的功能框图。

图5是表示按照本发明的实施方式的状态估计单元的操作的流程图。

图6是按照本发明实施方式的卡尔曼滤波操作单元的功能框图。

图7是表示按照本发明的实施方式的中心点推导单元的操作的流程图。

图8是由按照本发明的实施方式的三维磁性传感器检测到的地磁和内部磁场的概念图。

图9是图解说明按照本发明实施方式的中心点计算处理的概念图。

图10是图解说明由按照本发明实施方式的三维磁性传感器检测到的磁测数据为二维分布的情况的概念图。

图11是图解说明按照本发明实施方式的磁测数据分布系数计算处理的概念图。

图12是由按照本发明的实施方式的三维磁性传感器检测到的地磁、内部磁场和外部磁场的概念图。

图13（A）和13（B）是图解说明按照本发明实施方式的变形判定处理的概念图。

图14是图解说明按照本发明的实施方式的变形判定处理中处理的三维图形的概念图。

图15是图解说明按照本发明的实施方式的变形判定处理的概念图。

图16是表示通过运行按照本发明的变型例的状态估计程序而实现的功能的功能框图。

具体实施方式

<A.实施方式>

在下文中，将参照附图介绍本发明的示范性实施方式。

[1.仪器的结构和软件的结构]

图1是按照本发明实施方式的便携式仪器1的框图，图2是表示便携式仪器1的外观的立体图。便携式仪器1具有这样的功能：响应于屏幕的姿态旋转诸如地图之类的画面，从而该画面所显示的方位会随着实际空间的方位变化。这一功能是这样实现的：基于各种传感器的输出进行卡尔曼滤波操作，以估计便携式仪器1的姿态和从便携式仪器1看去的地磁方向。

便携式仪器1包括用于控制整个设备的经由总线连接到各种类型的结构元件的中央处理单元（CPU）10、用作CPU 10的工作区的随机存取存储器（RAM）20、用于存储各种程序和数据的只读存储器（ROM）30、用于进行通信的通信单元40、用于显示画面的显示单元50和全球定位***（GPS）单元60。在ROM 30由闪存存储器构成的情况下，可以重写ROM 30中存储的各种程序和数据，并且可以额外写入新的数据和程序。GPS单元60接收来自GPS卫星的信号，以生成便携式仪器1的位置信息（经度和纬度）。而且，便携式仪器1包括用于检测地磁以输出磁测数据的三维磁性传感器70、用于检测加速度以输出加速度数据的三维加速度传感器80和用于检测角速度以输出角速度数据的三维角速度传感器90。

三维磁性传感器70包括X轴磁性传感器71、Y轴磁性传感器72和Z轴磁性传感器73。可以使用磁阻抗装置（MI装置）或者磁阻效应装置（MR装置）构成各个传感器。磁性传感器接口（I/F）74将来自各个传感器的模拟输出信号转换为数字信号，以输出磁测数据q。磁测数据q是由x轴、y轴和z轴分量表达的向量数据。

同时，安装了三维磁性传感器70的便携式仪器1常常包括产生磁场的机械或电子部件，比如可以被磁化的各种类型的金属和电路。出于这一原因，除了代表地磁的向量（其具有指向北磁极的水平分量和垂直于磁倾角方向的分量），三维磁性传感器70输出的磁测数据q还包括代表由便携式仪器1中安装的部件产生的磁场的向量。因此，为了正确检测地磁的值，必须进行修正处理，以从三维磁性传感器输出的向量数据（磁测数据q）中除去代表便携式仪器1的部件产生的内部磁场的向量。

代表为了要在修正处理中获得所要检测的地磁的正确值而从三维磁性传感器70输出的数据中除掉的内部磁场的向量，被称为磁性传感器的偏移量q_off。

而且，如图3所示，除了地磁B_g和内部磁场B_i之外，三维磁性传感器70还可以检测到由处于便携式仪器1外部的物体2产生的外部磁场B_x。外部磁场B_x是非均匀磁场，后面将对此进行详细介绍。结果，很难计算出外部磁场B_x分量并且难以从三维磁性传感器70输出的数据中除去计算出来的外部磁场B_x分量，因此难以计算出地磁B_g的正确值。另一方面，在不存在外部磁场B_x的情况下或者在外部磁场B_x的影响非常小的情况下，可以进行将内部磁场B_i作为偏移量从三维磁性传感器70的输出中除去的处理，从而计算出正确的地磁B_g。

为了介绍方便，如图3中所示，引入了地面坐标系Σ_G和传感器坐标系Σ_S。地面坐标系Σ_G是相对于地面固定的坐标系。具体地说，地面坐标系Σ_G是具有作为x轴、y轴和z轴的彼此垂直的三个方向（例如水平向东、水平向北和垂直它们向上的方向）、以地面上的任意点作为原点的坐标系。传感器坐标系Σ_S是相对于便携式仪器1固定的坐标系。三维磁性传感器70输出的磁测数据被绘制在传感器坐标系Σ_S的x轴、y轴和z轴上，并且被表示为传感器坐标系Σ_S的向量数据。而且，从地面坐标系Σ_G看去时的传感器坐标系Σ_S原点的位置和姿态（即，从地面坐标系Σ_G看去时便携式仪器1的位置和姿态）分别被表示为位置P_S和姿态μ。

同时，图3中所记的各个向量的左上部所附的上标G的意思是，各个向量是地面坐标系Σ_G中表述的向量。

三维加速度传感器80包括一个X轴加速度传感器81、Y轴加速度传感器82和Z轴加速度传感器83。各个传感器可以是压敏电阻型传感器、电容型传感器或者热检测型传感器。加速度传感器接口（I/F）84将来自各个传感器的模拟输出信号转换为数字信号，以输出加速度数据a。加速度数据a是代表相对于安装有三维加速度传感器80的便携式仪器1固定的坐标系内惯性力和重力的合力的数据，是具有x轴、y轴和z轴分量的向量。因此，当便携式仪器1处于静止状态或者作直线匀速运动时，加速度数据a就变成了表示相对于便携式仪器1固定的坐标系内重力加速度的幅度和方向的向量数据。

三维角速度传感器90包括X轴角速度传感器91、Y轴角速度传感器92和Z轴角速度传感器93。角速度传感器接口（I/F）94将来自各个传感器的模拟输出信号转换为数字信号，以输出角速度数据g。角速度数据g是表示各个轴的旋转角速度的向量数据。

CPU 10运行ROM 30中存储的状态估计程序，以估计物理量（状态变量），比如便携式仪器1的姿态和磁性传感器的偏移量。即，CPU 10执行状态估计程序，因此，便携式仪器1起到状态估计设备的作用。

图4是表示由作为运行状态估计程序的状态估计设备的CPU 10实现的功能的功能框图。如图4中所示，状态估计设备包括状态估计单元100和中心点推导单元200。状态估计单元100包括卡尔曼滤波操作单元120和初始值生成单元140。卡尔曼滤波操作单元120使用以磁测数据q、加速度数据a和角速度数据g为分量的观测向量y执行卡尔曼滤波操作，从而周期性地更新状态向量x和输出更新后的状态向量x⁺。初始值生成单元140基于各种类型的设置信息生成一个初始值INI，并且输出所生成的初始值INI。

中心点推导单元200累积三维磁性传感器70提供的磁测数据q，基于多个累积的磁测数据q₁到q_N计算中心点（第一球面的中心点）c_O的坐标，并且输出所计算出来的中心点c_O的坐标（N是大于等于5的自然数，代表高精度得出中心点c_O坐标所需的测量磁测数据的预定次数）。中心点c_O是表示传感器坐标系Σ_S上的坐标的点，而表示中心点c_O的坐标的向量成为了磁性传感器的偏移量q_OFF的候选值，后面将对此进行详细介绍。

[2.卡尔曼滤波器]

图5是表示状态估计单元100的操作的流程图。该流程图中所示的状态估计处理是由运行状态估计程序的CPU 10发起的。

在步骤S101，CPU 10执行初始值生成处理。具体地说，CPU 10参照ROM 30中记录的各种类型的设置信息，生成初始值INI。即，CPU 10执行步骤S101的初始值生成处理，因此，CPU 10起到初始值生成单元140的作用。

在步骤S102，CPU 10执行中心点推导判决处理。具体地说，CPU 10判定是否已在中心点推导处理中生成了中心点c_O的坐标（见图7），后面将对此进行介绍。（更加具体地讲，CPU 10判定是否已经从步骤S206的中心点输出处理导出了中心点c_O的坐标，后面将对此进行介绍）。随后，在中心点推导处理中已经生成了中心点c_O的坐标的情况下，CPU 10将处理过程前进到步骤S103。另一方面，在中心点推导处理中没有生成中心点c_O的坐标的情况下，CPU 10将处理过程前进到步骤S104。

在步骤S103，CPU 10执行状态变量重写处理。要在状态向量更新处理（步骤S104）中进行的卡尔曼滤波操作所针对的状态向量x，是以多个状态变量作为分量的向量，并且这些向量分量包括磁性传感器的偏移量估计值q_O，它是一个三维向量，后面将对此详细介绍。在状态变量重写处理中，CPU 10用中心点推导处理获得的代表中心点c_O的三轴坐标分量，重写构成磁性传感器的偏移量估计值q_O（q_O是三维向量）的各个分量。

在步骤S104，CPU 10执行状态向量更新处理。具体地说，CPU10使用观测向量y进行卡尔曼滤波操作，以周期性地更新状态向量x并输出更新后的状态向量x⁺。

在步骤S105，CPU 10执行状态向量输出处理。具体地说，CPU10输出状态向量更新处理中计算出来的更新后的状态向量x⁺。

CPU 10执行如前所述的步骤S102到S105的处理，并且因此，CPU 10起到卡尔曼滤波操作单元120的作用。结果，对于便携式仪器1而言，可以使用执行步骤S102到S105的处理的CPU 10生成的更新后的状态向量x⁺ _k（即，多个状态变量），连续计算地磁B_g的当前方向和便携式仪器1的当前姿态。

随后，CPU 10判定是否满足结束状态估计程序的结束条件（步骤S106）。可以基于便携式仪器1的具体情况适当设定结束条件。例如，可以将关闭便携式仪器1设定为结束条件。在满足结束条件的情况下，CPU 10结束流程图中所示的状态估计处理。另一方面，在不满足结束条件的情况下，CPU 10使处理前进到步骤S102。

一般来说，卡尔曼滤波器具有用于估计代表动态***状态的多个物理量的时基变化的状态转移模型和用于估计动态***中所包含的多个传感器测得的值（观测值）的观测模型。而且，卡尔曼滤波器使用状态转移模型，根据以代表某一时刻动态***的状态的多个物理量（状态变量）为元素的状态向量，来估计经过单位时间之后的状态向量。随后，卡尔曼滤波器基于所估计的状态向量，估计以测量动态***状态的多个传感器的输出值为元素的观测向量的值。此外，卡尔曼滤波器基于通过计算所估计的观测向量与以传感器的实际输出值为元素的观测向量之间的差值而得到的观测残差，将所估计的状态向量更新为接近于实际值（真实值）的值，并且输出更新后的状态向量。

卡尔曼滤波器重复进行上述操作，以使得构成状态向量的各个状态变量都接近于正确表示尽可能接近真实物理量的物理量的值（真实值）。

在这一实施方式中，卡尔曼滤波器采用便携式仪器1的姿态μ、地磁的强度r、地磁的磁倾角便携式仪器1的角速度ω、角速度传感器91到93的偏移量估计值g_O和磁性传感器71到73的偏移量估计值q_O作为进行估计所针对的状态变量。同时，将角速度传感器的偏移量估计值g_O包含在状态变量中的原因在于，角速度传感器91到93的偏移量与角速度数据g相重叠。

在时刻T=k，以这些状态变量为元素的状态向量x_k由下列公式（1）表示。同时，加在各个状态变量右下部的下标k表示各个状态变量是T=k时刻的值。

$x_k=\left[\begin{array}{c}{\mathrm{\&mu;}}_k\\ r_k\\ {\mathrm{\&phi;}}_k\\ {\mathrm{\&omega;}}_k\\ g_{O,k}\\ q_{O,k}\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(1\right)$

在这一实施方式中，姿态μ是使用四元数表示的。四元数是代表物体姿态（旋转状态）的四维数。具体地说，相对于便携式仪器1固定的传感器坐标系Σ_S的各个轴与地面坐标系Σ_G的各轴一致时的姿态μ被定义为基准姿态，并且基准姿态被表示为μ=（0，0，0，1）。此时，可以将便携式仪器1的任意姿态μ表示为，以单位向量α的方向为旋转轴，将便携式仪器1相对于基准姿态旋转角度ψ时的姿态。旋转之后的姿态μ由公式（2）表示。

$\mathrm{\&mu;}=\left[\begin{array}{c}{\mathrm{\&mu;}}_1\\ {\mathrm{\&mu;}}_2\\ {\mathrm{\&mu;}}_3\\ {\mathrm{\&mu;}}_4\end{array}\right]=\left[\begin{array}{c}\mathrm{\&alpha;}\sin \left(\frac{\mathrm{\&psi;}}{2}\right)\\ \cos \left(\frac{\mathrm{\&psi;}}{2}\right)\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(2\right)$

在这一实施方式中，卡尔曼滤波器要观测的目标是从三个轴磁性传感器71到73输出的磁测数据q、从三个轴加速度传感器81到83输出的加速度数据a和从三个轴角速度传感器91到93输出的角速度数据g。此时，T=k时刻的观测向量y_k由公式（3）表示。

$y_k=\left[\begin{array}{c}{q}_k\\ a_k\\ g_k\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(3\right)$

如前面所介绍的，初始值生成单元140生成以T=0时刻的状态变量为元素的初始值INI（初始值生成处理）。初始值INI包括姿态μ的初始值μ₀、地磁强度r的初始值r₀、地磁的磁倾角的初始值角速度ω的初始值ω₀、角速度传感器的偏移量估计值g_O的初始值g_O,0和磁性传感器的偏移量估计值q_O的初始值q_O,0作为元素。

可以将初始值INI适当地设定为这样一个值：按照这个值，通过卡尔曼滤波操作，状态变量尽可能快地收敛为正确的值。例如，可以将初始值INI设定为下列值。

可以基于由GPS单元60提供的位置信息生成地磁强度r的初始值r₀和地磁磁倾角的初始值这是因为，如果可以指定地球上的位置，就可以检测该位置上地磁的强度和磁倾角。具体地说，以对应的方式包含该位置信息和地磁强度与磁倾角的查找表LUT1被存储在ROM 30中。初始值生成单元140参照查找表LUT1将与该位置信息相对应的地磁强度和磁倾角设置为地磁强度r的初始值r0和地磁磁倾角的初始值

同时，在将便携式仪器1放置在卫星发出的无线电波无法到达的地方（例如，地下通道）的情况下，无法从GPS单元60获得位置信息。在此情况下，可以采用便携式仪器1的可用地域的典型值。这个值也保存在查找表LUT1中。在无法获得位置信息的情况下，初始值生成单元140从查找表LUT1中读取该典型值。例如，对于在日本出售的便携式仪器1，地磁强度r的初始值r₀和地磁磁倾角的初始值是基于明石（Akashi）的地磁强度和磁倾角计算出来的。

例如，假设便携式仪器1处于静止状态下，角速度ω的初始值ω₀被设置为0。角速度传感器的偏移量估计值g_O的初始值g_O,0通常会被调节为接近于0，因此，角速度传感器的偏移量估计值g_O的初始值g_O，0被设置为0。例如，假设便携式仪器1处于静止状态，同时便携式仪器1朝向恒定不变的方向的情况下，姿态μ的初始值μ₀被设置为一个与实际初始姿态之间的偏差较小的值。

磁性传感器的偏移量估计值q_O的初始值q_O，0被设置为由下列公式（4）例如使用T=0时刻时磁性传感器的观测值q_O、姿态μ的初始值μ₀、使用便携式仪器1的地域的地磁向量q_typical和公式（5）描述的矩阵B(μ)表述的值。这里，在便携式仪器1采用姿态μ的情况下，矩阵B(μ)是用来转换地面坐标系Σ_G中表达的向量的坐标的坐标变换矩阵，从而可以在传感器坐标系Σ_S中表达该向量。同时，以对应的方式包含位置信息和地磁向量q_typical的查找表LUT2被存储在ROM30中。初始值生成单元140基于由GPS单元60产生的位置信息参考查找表LUT2获取地磁向量q_typical，并且运算公式（4）来获得磁性传感器的偏移量估计值q_O的初始值q_O,0。

q_O,0=q_O－B(μ₀)q_typical.......(4)

其中

$B\left(\mathrm{\&mu;}\right)=\left[\begin{array}{ccc}{{\mathrm{\&mu;}}_4}^2+{{\mathrm{\&mu;}}_1}^2-{{\mathrm{\&mu;}}_2}^2-{{\mathrm{\&mu;}}_3}^2& 2({\mathrm{\&mu;}}_1{\mathrm{\&mu;}}_2+{\mathrm{\&mu;}}_4{\mathrm{\&mu;}}_3)& 2({\mathrm{\&mu;}}_1{\mathrm{\&mu;}}_3-{\mathrm{\&mu;}}_4{\mathrm{\&mu;}}_2)\\ 2({\mathrm{\&mu;}}_2{\mathrm{\&mu;}}_1-{\mathrm{\&mu;}}_4{\mathrm{\&mu;}}_3)& {{\mathrm{\&mu;}}_4}^2-{{\mathrm{\&mu;}}_1}^2+{{\mathrm{\&mu;}}_2}^2-{{\mathrm{\&mu;}}_3}^2& 2({\mathrm{\&mu;}}_2{\mathrm{\&mu;}}_3+{\mathrm{\&mu;}}_4{\mathrm{\&mu;}}_1)\\ 2({\mathrm{\&mu;}}_3{\mathrm{\&mu;}}_1+{\mathrm{\&mu;}}_4{\mathrm{\&mu;}}_2)& 2({\mathrm{\&mu;}}_3{\mathrm{\&mu;}}_2-{\mathrm{\&mu;}}_4{\mathrm{\&mu;}}_1)& {{\mathrm{\&mu;}}_4}^2-{{\mathrm{\&mu;}}_1}^2-{{\mathrm{\&mu;}}_2}^2+{{\mathrm{\&mu;}}_3}^2\end{array}\right]$

……(5)

初始值生成单元140生成和输出初始值INI，初始值INI是以如前所述生成的姿态μ的始值μ₀、地磁强度r的初始值r₀、地磁磁倾角的初始值角速度ω的初始值ω₀、角速度传感器的偏移量估计值g_O的初始值g_O,0和磁性传感器的偏移量估计值q_O的初始值q_O,0为元素的向量。

[3.卡尔曼滤波操作]

接下来，将会介绍步骤S104中进行的状态向量更新处理的内容，即，由按照本实施方式的卡尔曼滤波器进行的操作的内容。

卡尔曼滤波器使用代表动态***状态时基变化的状态转移模型，根据表示某一时刻（时刻T=k-1）动态***状态的状态向量x_k-1，估计经过了单位时间之后（时刻T=k）的状态向量x_k，并且输出这一估计值作为估计的状态向量x^- _k。而且，卡尔曼滤波器使用代表以各种传感器的输出为元素的观测向量y_k与状态向量x_k之间关系的观测模型，基于由状态转移模型估计出来的状态向量x^- _k，估计观测向量y_k，并且输出这一估计值作为估计的观测向量y^- _k。

同时，在这一实施方式中，使用作为非线性卡尔曼滤波器的西格玛点卡尔曼滤波器作为卡尔曼滤波器，后面将对此进行详细介绍。西格玛点卡尔曼滤波是这样一种操作：在状态向量x_k-1附近设置多个西格玛点χ_k-1并且将这些西格玛点χ_k-1应用到状态转移模型中，以估计经过单位时间之后的西格玛点χ^- _k，并且计算估计出来的西格玛点χ^- _k的平均值，从而找出估计的状态向量x^- _k。因此，严格来说，估计的观测向量y^- _k是基于存在于估计的状态向量x^- _k附近的估计的西格玛点χ^- _k而计算出来的。

随后，卡尔曼滤波器计算估计的观测向量y^- _k和以实际观测值为元素的观测向量y_k之间的差值作为观测残差z_k，并且基于观测残差z_k计算卡尔曼增益K_k。此外，卡尔曼滤波器使用观测残差z_k和卡尔曼增益K_k更新估计的状态向量x^- _k，以计算更新后的状态向量x⁺ _k。

状态向量更新处理是通过如前所述的卡尔曼滤波运算重复更新状态向量x_k，使得状态向量x_k接近于近似等于正确表示实际物理量的值（真实值）的处理。

[3.1.卡尔曼滤波操作的概述]

在这一实施方式中，由下列公式（6）描述卡尔曼滤波器的状态转移模型，并且由下列公式（7）描述观测模型。同时，在这一实施方式中，公式（6）中的函数f和公式（7）中的函数h是非线性函数。

x_k＝f(x_k-1，k-1)+w_k-1……(6)

y_k＝h(x_k，k)+v_k      ……(7)

这里，状态向量x_k是a维向量，观测向量y_k是b维向量。同时，在这一实施方式中，a=15，b=9。而且，公式（6）中存在的处理噪声w_k和公式（7）中存在的观测噪声v_k是以0为中心的高斯噪声。

公式（6）表示，T=k时刻的状态向量x_k是通过将时刻T=k-1的处理噪声w_k-1加上一个值来估计的，这个值是通过将时刻T=k-1的状态向量x_k-1代入到函数f中获得的。

而且，公式（7）表示，时刻T=k的观测向量y_k是通过将时刻T=k的观测噪声v_k加上一个值来估计的，这个值是通过将时刻T=k的状态向量x_k代入到函数h中获得的。

同时，处理噪声w_k的协方差用Q_k表示，观测噪声v_k的协方差用R_k表示。

时刻T=k的观测残差z_k是基于实际观测向量y_k和估计观测向量y^- _k的向量集，并且由下列公式（8）表示。卡尔曼滤波器使用观测残差z_k和由公式（9）表示的卡尔曼增益K_k，如下列公式（10）所表示的那样更新估计的状态向量x^- _k，以计算更新之后的状态向量x⁺ _k。而且，卡尔曼滤波器如下列公式（11）所表示的那样，更新状态向量x_k的估计误差的协方差P_k。

$z_k=y_k-h(x_k^{-},k)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(8\right)$

$K_k=P_k^{\mathrm{xy}}{\left(P_k^{\mathrm{yy}}\right)}^{-1}\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(9\right)$

$x_k^{+}=x_k^{-}+K_k{z}_k\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(10\right)$

$P_k^{+}=P_k^{-}-K_k{P}_k^{\mathrm{yy}}{K}_k^T\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(11\right)$

其中，P^- _k是x^- _k的估计误差的协方差，P⁺ _k是x⁺ _k的估计误差的协方差，P_k ^yy是观测残差的协方差，p_k ^xy是x^- _k和y^- _k的互方差-协方差矩阵。

使用观测模型估计的观测向量y^- _k是如公式（7）所表示的那样，通过将用状态转移模型估计的状态向量x^- _k施加到观测模型而计算出来的逻辑值。结果，使用观测模型估计的观测向量y^- _k与基于实际传感器输出的观测向量y_k之间的差，即观测残差z_k，是表示估计的状态向量x^- _k和正确表达实际物理量的值（真实值）之间的逼近度的值。

在公式（10）中，使用观测残差z_k将估计的状态向量x^- _k更新为接近于真实值的更新后的状态向量x⁺ _k。

[3.2.由西格玛点卡尔曼滤波器进行的操作]

其中，在这一实施方式中，卡尔曼滤波器（卡尔曼滤波操作单元120）是由使用无损变换的西格玛点卡尔曼滤波器构成的。在下文中，将会参照图6中所示的卡尔曼滤波操作单元120的功能框图详细介绍由西格玛点卡尔曼滤波器进行的状态向量更新。

延迟单元121将加法器129输出的更新后的状态向量x⁺ _k延迟单位时间（从时刻T=k-1到时刻T=k的时间），以产生状态向量x⁺ _k-1并且将所产生的状态向量输出到西格玛点产生单元122。同时，在第一操作（时刻T=0）中，延迟单元121使用初始值生成单元140输出的初始值INI生成状态向量x⁺ _k-1。

而且，在中心点推导单元200输出中心点c_O的情况下，延迟单元121使用从中心点推导单元200输出的中心点c_O，重写与磁性传感器的偏移量估计值q_O相对应的状态向量x⁺ _k-1的元素，以生成状态向量x⁺ _k-1（步骤S103的状态变量重写处理）。

西格玛点产生单元122使用a×a的方差-协方差矩阵P⁺ _k-1和Q_k-1生成2a+1个西格玛点。具体地说，首先，针对多个西格玛点的平均，使用代表与平均值有关的信号点扩大的缩放参数λ，定义了由公式（12）和公式（13）表示的向量σ_k。

${\mathrm{\&sigma;}}_k\left(j\right)=\sqrt{(a+\mathrm{\&lambda;})(P_k^{+}+Q_k)}(j=1,\&CenterDot;\&CenterDot;\&CenterDot;,a)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(12\right)$

${\mathrm{\&sigma;}}_k(a+j)=-\sqrt{(a+\mathrm{\&lambda;})(P_k^{+}+Q_k)}(j=1,\&CenterDot;\&CenterDot;\&CenterDot;,a)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(13\right)$

其中，满足ZZ^T=Z的P由表示。

此时，西格玛点产生单元122基于向量σ_k-1和状态向量x⁺ _k-1，生成2a+1个由公式（14）和公式（15）表示的西格玛点χ_k-1。

${\mathrm{\&chi;}}_{k-1}\left(0\right)=x_{k-1}^{+}\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(14\right)$

${\mathrm{\&chi;}}_{k-1}\left(j\right)={\mathrm{\&sigma;}}_{k-1}\left(j\right)+x_{k-1}^{+}(j=1,\&CenterDot;\&CenterDot;\&CenterDot;,2a)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(15\right)$

状态转移模型单元123将T=k-1时刻的2a+1个西格玛点χ_k-1(j)施加到状态转移模型，以计算由公式（16）表示的T=k时刻的2a+1个西格玛点的估计值χ^- _k(j)。

${\mathrm{\&chi;}}_k^{-}\left(j\right)=f({\mathrm{\&chi;}}_{k-1}\left(j\right),k-1)(j=0,\&CenterDot;\&CenterDot;\&CenterDot;,2a)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(16\right)$

随后，平均值计算单元124计算T=k时刻的2a+1个西格玛点的估计值χ^- _k的平均值，以计算由公式（17）表示的估计的状态向量x^- _k。

$x_k^{-}=\frac{1}{a+\mathrm{\&lambda;}}(\mathrm{\&lambda;}{\mathrm{\&chi;}}_k^{-}\left(0\right)+\frac{1}{2}\underset{j=1}{\overset{2a}{\mathrm{\&Sigma;}}}{\mathrm{\&chi;}}_k^{-}\left(j\right))\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(17\right)$

而且，平均值计算单元124计算由公式（18）表示的估计的状态向量x^- _k的误差的协方差P^- _k。

$P_k^{-}=\frac{1}{a+\mathrm{\&lambda;}}(\mathrm{\&lambda;}{\mathrm{\&xi;}}_0{\mathrm{\&xi;}}_0^T+\frac{1}{2}\underset{j=1}{\overset{2a}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_j{\mathrm{\&xi;}}_j^T)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(18\right)$

其中， ${\mathrm{\&xi;}}_j={\mathrm{\&chi;}}_k^{-}\left(j\right)-x_k^{-}$

另一方面，观测模型单元125将T=k时刻的2a+1个西格玛点χ_k(j)施加到观测模型，以计算由公式（19）表示的2a+1个估计的观测值γ_k(j)。

${\mathrm{\&gamma;}}_k\left(j\right)=h({\mathrm{\&chi;}}_k^{-}\left(j\right),k-1)(j=0,\&CenterDot;\&CenterDot;\&CenterDot;,2a)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(19\right)$

平均值处理单元126计算2a+1个估计的观测值γ_k(j)的平均值，以计算公式（20）所表示的估计的观测向量y^- _k。

$y_k^{-}=\frac{1}{a+\mathrm{\&lambda;}}(\mathrm{\&lambda;}{\mathrm{\&gamma;}}_k\left(0\right)+\frac{1}{2}\underset{j=1}{\overset{2a}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}_k\left(j\right))\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(20\right)$

随后，平均值处理单元126计算公式（21）所表示的观测残差的协方差P^yy _k。

$P_k^{\mathrm{yy}}=\frac{1}{a+\mathrm{\&lambda;}}(\mathrm{\&lambda;}{\mathrm{\&zeta;}}_0{\mathrm{\&zeta;}}_0^T+\frac{1}{2}\underset{j=1}{\overset{2a}{\mathrm{\&Sigma;}}}{\mathrm{\&zeta;}}_j{\mathrm{\&zeta;}}_j^T)+R_k\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(21\right)$

其中， ${\mathrm{\&zeta;}}_j={\mathrm{\&gamma;}}_k\left(j\right)-y_k^{-}$

减法器127计算观测残差z_k，如公式（8）所示，该观测残差z_k是观测向量y_k和估计的观测向量y^- _k之间的差值。

卡尔曼增益赋予单元128计算由公式（22）表示的互方差-协方差矩阵P^xy _k。此外，卡尔曼增益赋予单元128如公式（9）所示的那样，基于残差的协方差P^yy _k和互方差-协方差矩阵P^xy _k计算卡尔曼增益K_k，并且执行公式（10）右侧第二项（K_kz_k）的运算。而且，卡尔曼增益赋予单元128如公式（11）所表示的那样，将状态向量x_k的估计误差的协方差P_k从P^- _k更新为P⁺ _k。

$P_k^{\mathrm{xy}}=\frac{1}{a+\mathrm{\&lambda;}}(\mathrm{\&lambda;}{\mathrm{\&xi;}}_0{\mathrm{\&zeta;}}_0^T+\frac{1}{2}\underset{j=1}{\overset{2a}{\mathrm{\&Sigma;}}}{\mathrm{\&xi;}}_j{\mathrm{\&zeta;}}_j^T)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(22\right)$

加法器129将估计的状态向量x^- _k与从卡尔曼增益赋予单元128输出的公式（10）右侧的第二项（K_kz_k）相加，以如公式（10）所示的那样，计算更新后的状态向量x⁺ _k。

[3.3.状态转移模型]

在下文中，将会介绍状态转移模型单元123操作中使用的状态转移模型。

在按照本实施方式的状态转移模型中，构成状态向量x_k的状态变量的姿态μ的状态转移是由公式（23）定义的。公式（23）代表根据时刻T=k-1的姿态μ_k-1估计经过单位时间之后的时刻T=k的姿态μ_k的操作。这里，μ^- _k是在时刻k估计的状态向量x^- _k的与代表姿态μ的状态变量相应的元素。

同时，公式（23）右侧的算子Ω是由公式（24）定义的。这里，I_3×3代表3×3的单位矩阵。针对三维向量l=（l₁，l₂，l₃），由公式（25）定义了算子[lX]。而且，当以Δt表示测量时间间隔（从时刻T=k-1到时刻T=k的间隔）并且以ω⁺ _k表示与代表时刻T=k的更新后的状态向量x⁺ _k的角速度的状态变量相对应的元素时，构成算子Ω的分量ψ⁺ _k由公式（26）定义。

${\mathrm{\&mu;}}_k^{-}=\mathrm{\&Omega;}\left({\mathrm{\&omega;}}_{k-1}^{+}\right){\mathrm{\&mu;}}_{k-1}^{+}\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(23\right)$

其中

$\mathrm{\&Omega;}\left({\mathrm{\&omega;}}_k^{+}\right)=\left[\begin{array}{cc}\cos \left(\frac{1}{2}\right|\left|{\mathrm{\&omega;}}_k^{+}\right|\left|\mathrm{\&Delta;t}\right)I_{3\&times;3}-[{\mathrm{\&Psi;}}_k^{+}\&times;]& {\mathrm{\&Psi;}}_k^{+}\\ -{{\mathrm{\&Psi;}}_k^{+}}^T& \cos \left(\frac{1}{2}\right|\left|{\mathrm{\&omega;}}_k^{+}\right|\left|\mathrm{\&Delta;t}\right)\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\left(24\right)$

$[l\&times;]=\left[\begin{array}{ccc}0& -l_3& l_2\\ l_3& 0& -l_1\\ -l_2& l_1& 0\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\left(25\right)$

${\mathrm{\&Psi;}}_k^{+}=\sin \left(\frac{1}{2}\right|\left|{\mathrm{\&omega;}}_k^{+}\right|\left|\mathrm{\&Delta;t}\right)\frac{{\mathrm{\&omega;}}_k^{+}}{\left|\right|{\mathrm{\&omega;}}_k^{+}\left|\right|}\&CenterDot;\&CenterDot;\&CenterDot;\left(26\right)$

由于姿态μ是使用四元数表示的，因此必须满足‖μ‖=1的归一化条件。不过，如果找到西格玛点的平均值，那么归一化条件就得不到满足。出于这一原因，当进行任何针对姿态μ的运算时，都要将运算后的结果归一化为向量的大小。同时，为了更加严格地维持归一化条件，可以使用经过修改的Rodrigues参数（MPR），将状态向量x_k的姿态μ_k仅限于与当前时刻和前一时刻之间差值有关的信息，并且可以基于从卡尔曼滤波器获得的差值信息更新卡尔曼滤波器外部的姿态信息。

很难预测地磁的强度r和磁倾角φ的变化。出于这一原因，为了简便起见，在按照本实施方式的状态转移模型中，假设地磁在时刻T=k时的强度r_k和磁倾角φ_k等于地磁在时刻T=k-1时的强度r_k-1和磁倾角φ_k-1。

同样地，很难预测角速度传感器的偏移量估计值g_O的变化。出于这一原因，为了简便起见，在按照本实施方式的状态转移模型中，假设角速度传感器在时刻T=k时的偏移量估计值g_O,K等于角速度传感器在时刻T=k-1时的偏移量估计值g_O,K-1。

由于便携式仪器1的角速度ω随着由便携式仪器1的用户造成的便携式仪器1的运动而改变，因此很难使用时刻T=k-1的角速度ω_k-1确定时刻T=k的角速度ω_k。出于这一原因，为了简便起见，在按照本实施方式的状态转移模型中，假设时刻T=k的角速度ω_k等于时刻T=k-1的角速度ω_k-1。

如前面所介绍的，磁性传感器的偏移量q_OFF是表示由便携式仪器1的部件生成的内部磁场B_i在传感器坐标系Σ_S内的方向和幅度。因此，在便携式仪器1的内部状态均匀的情况下，磁性传感器的偏移量q_OFF也不会变化。另一方面，在便携式仪器1的内部状态改变的情况下，例如，在便携式仪器1中安装的部件中的电流幅度改变的情况下或者在便携式仪器1中安装的部件的磁化状态改变的情况下，磁性传感器的偏移量q_OFF也会改变。就是说，便携式仪器1的内部状态随着便携式仪器1的用户操作和便携式仪器1的外部环境而改变。结果，很难预测磁性传感器的偏移量q_OFF发生变化的时刻和磁性传感器的偏移量q_OFF的变化量，并且很难使用磁性传感器在时刻T=k-1的偏移量估计值q_O,K-1确定磁性传感器在时刻T=k的偏移量估计值q_O,K。

出于这一原因，为了简便起见，在按照本实施方式的状态转移模型中，假设磁性传感器在时刻T=k的偏移量估计值q_O,K等于磁性传感器在时刻T=k-1的偏移量估计值q_O,K-1。

这样，在按照本实施方式的状态转移模型中，以如下列公式（27）所表示的构成状态向量x_k的状态变量的除了代表姿态μ的状态变量之外的剩余部分从前一时刻起不会发生变化为前提。

$\left[\begin{array}{c}{r}_k\\ {\mathrm{\&phi;}}_k\\ {\mathrm{\&omega;}}_k\\ g_{O,k}\\ q_{O,k}\end{array}\right]=\left[\begin{array}{c}{r}_{k-1}\\ {\mathrm{\&phi;}}_{k-1}\\ {\mathrm{\&omega;}}_{k-1}\\ g_{O,k-1}\\ q_{O,k-1}\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(27\right)$

同时，在卡尔曼滤波操作中，状态向量x_k是基于观测残差z_k加以更新的。结果，公式（27）中所示的所有的值，除了姿态μ之外，都改变了。由加法器129基于观测残差z_k对状态向量x_k进行了更新，从而接近于实际值。

具体地说，时刻T=k的状态向量x_k是如公式（6）、公式（8）和公式（10）所示的那样，基于使用时刻T=k-1的状态向量x_k-1计算出来的观测残差z_k来更新的。同样地，时刻T=k-1的状态向量x_k-1是基于使用时刻T=k-2的状态向量x_k-2计算出来的观测残差z_k-1来更新的。即，在卡尔曼滤波操作中，状态向量x_k是考虑了从初始状态（时刻T=0）到时刻T=k-1的所有状态向量x₀到x_k-1的值来更新的。因此，在状态向量x_k的值收敛于大大偏离实际值的值的情况下，状态向量x_k的值可能不会接近实际值，尽管随后会重复进行卡尔曼滤波操作。而且，即使状态向量x_k的值接近实际值，也要花费很长时间，状态向量x_k的值才会收敛于实际值。

如前面所介绍的，磁性传感器的偏移量q_OFF可能会随着便携式仪器1内部状态的改变而突然大幅变化。在磁性传感器的偏移量q_OFF突然大幅变化的情况下，磁性传感器的偏移量估计值q_O,K和磁性传感器的偏移量q_OFF（实际值）会彼此大不相同。在磁性传感器的偏移量估计值q_O,K大大偏离实际值的情况下，磁性传感器的偏移量估计值q_O,K可能不会接近于实际值，尽管卡尔曼滤波器会反复更新状态向量x_k。而且，即使磁性传感器的偏移量估计值q_O,K能够接近于实际值，但也要花费很长时间，磁性传感器的偏移量估计值q_O,K才会收敛于实际值。在磁性传感器的偏移量估计值q_O,K大大不同于实际值的情况下，对于便携式仪器1而言，可以计算地磁B_g的方向和便携式仪器1的姿态μ。

因此，在这一实施方式中，在中心点推导单元200输出中心点c_O的坐标的情况下，使用中心点c_O的坐标重写磁性传感器的偏移量估计值q_O（步骤S103的状态变量重写处理）。

中心点c_O的坐标是基于N个磁测数据q₁到q_N而计算出来的。结果，对于中心点推导单元200而言，可以使用磁性传感器的偏移量q_OFF变化之后获得的N个磁测数据q₁到q_N，计算正确表达磁性传感器的偏移量q_OFF的中心点c_O的坐标，尽管磁性传感器的偏移量q_OFF是突然发生很大变化。即，计算出来的中心点c_O的坐标是这样一个值：该值比考虑了从初始状态（时刻T=0）到时刻T=k-1的所有状态向量x₀到x_k-1的值计算出来的磁性传感器的偏移量估计值q_O,K更接近于与磁性传感器的偏移量q_OFF的突然变化相对应的实际值。

因此，即便在磁性传感器的偏移量q_OFF突然大幅变化的情况下，对于按照本实施方式的状态估计设备而言，也可以使用中心点c_O的坐标重写磁性传感器的偏移量估计值q_O，从而防止状态向量x_k背离实际值。

[3.4.观测模型]

在下文中，将会介绍观测模型单元125进行的操作中使用的观测模型。

公式（28）使用角速度传感器的角速度ω和偏移量估计值g_O表示三维角速度传感器90输出的角速度数据g的估计值γ_gyro。

Y_gyro＝ω+g_o  ……(28)

而且，在观测模型中，地面坐标系Σ_G中的地磁向量是由公式（29）表示的。结果，三维磁性传感器70输出的磁测数据q的估计值γ_mag是由公式（30）使用角速度传感器的偏移量估计值g_O和公式（5）表示的矩阵B(μ)来表示的。

$\left[\begin{array}{c}0\\ r\cos \mathrm{\&phi;}\\ -r\sin \mathrm{\&phi;}\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(29\right)$

${\mathrm{\&gamma;}}_{\mathrm{mag}}=B\left(\mathrm{\&mu;}\right)\left[\begin{array}{c}0\\ r\cos \mathrm{\&phi;}\\ -r\sin \mathrm{\&phi;}\end{array}\right]+q_O\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(30\right)$

而且，在观测模型中，地面坐标系Σ_G中的重力向量被归一化为重力加速度，并且由公式（31）表示。结果，三维加速度传感器80输出的加速度数据a的估计值γ_acc是由公式（32）使用公式（5）表示的矩阵B(μ)来表示的。

$\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(31\right)$

${\mathrm{\&gamma;}}_{\mathrm{acc}}=B\left(\mathrm{\&mu;}\right)\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(32\right)$

结果，公式（3）、公式（28）、公式（30）和公式（32）被应用到公式（8）中，从而由公式（33）表示观测残差z_k。

$z_k=\left[\begin{array}{c}{q}_k\\ a_k\\ g_k\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&gamma;}}_{\mathrm{mag}}\\ {\mathrm{\&gamma;}}_{\mathrm{acc}}\\ {\mathrm{\&gamma;}}_{\mathrm{gyro}}\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(33\right)$

[4.中心点推导单元200的操作]

图7是表示中心点推导单元200的操作的流程图。该流程图中所示的中心点推导处理是由运行状态估计程序的CPU 10发起的。

在步骤S201，CPU 10执行初始化处理。具体地说，CPU 10清除RAM 20中存储的磁测数据。

同时，在按照本实施方式的初始化处理中，虽然CPU 10删除了所有的磁测数据，但是本发明并不局限于此。例如，CPU 10可以仅仅删除RAM 20中累积的数据的预定较早部分。

在步骤S202，CPU 10执行磁测数据累积处理。具体地说，CPU10将从三维磁性传感器70相继输出的磁测数据q_i存储在RAM 20中。在N个或更多个磁测数据q_i被累积在RAM 20中的情况下，CPU 10使处理前进到步骤S203。

在步骤S203，CPU 10执行磁测数据分布判定处理。具体地说，CPU 10判断通过步骤S202的磁测数据累积处理累积在RAM 20中的磁测数据中的N个较新的磁测数据q₁到q_N（N是大于等于5的自然数，表示测量高精度得出中心点c_O坐标所需的磁测数据的规定次数）是否适合作为步骤S204进行的中心点计算处理中使用的数据，后面将详细介绍步骤S204。在判断结果为肯定结果的情况下，CPU 10使处理前进到步骤S204。另一方面，在判断结果为否定结果的情况下，CPU 10删除RAM 20中累积的所有磁测数据，并且然后将处理过程返回到步骤S202。

同时，在这一实施方式中，虽然在磁测数据分布判定处理的判断结果为否定结果的情况下，CPU 10会删除RAM 20中累积的所有磁测数据，但是本发明并不局限于此。例如，在判断结果为否定结果的情况下，CPU 10可以等待预定的时间段，在此时间段内不删除RAM 20中累积的磁测数据，可以将所述预定时间段内在RAM 20中累积的多个磁测数据添加到作出判决时RAM 20中已累积的磁测数据中，并且可以再次进行判决处理。

在步骤S204，CPU 10执行中心点计算处理。具体地说，假设通过步骤S202的磁测数据累积处理在RAM 20中累积的N个磁测数据q₁到q_N所表示的坐标很可能分布在某一球面（第一球面）S附近，则CPU 10计算球面S中心点c_O的坐标。

在步骤S205，CPU 10执行变形判定处理。具体地说，假设通过步骤S202的磁测数据累积处理在RAM 20中累积的N个磁测数据q₁到q_N所表示的坐标分布在某一三维图形SD的表面附近，则CPU 10判断三维图形SD的形状与所述球面的形状是否彼此近似到可以认为三维图形SD的形状与球面的形状相同的程度。在判断结果为肯定结果的情况下，即，在判定由N个磁测数据q₁到q_N表示的坐标分布在形状近似于球面形状的三维图形SD的表面附近的情况下，CPU 10使处理过程前进到步骤S206。另一方面，在判断结果为否定结果的情况下，即，在判定三维图形SD具有不同于球面形状的变形程度很大的形状的情况下，CPU 10将处理过程返回到步骤S201。

同时，虽然，在按照本实施方式的变形判定处理中，在判定三维图形SD和球面具有不同形状的情况下，CPU 10使处理过程返回到步骤S201，但是本发明并不局限于此。例如，在其中作为判定处理的结果而判定该三维图形SD和该球面具有不同形状的情况下，CPU 10可以输出任何信息至该显示单元50，并且可以暂时停止处理，直到接收到来自便携式仪器1的用户的重新开始步骤S201的处理的指令。

如果便携式仪器1的用户在以不摇晃便携式仪器1的方式握住便携式仪器1来使便携式仪器1的位置P_S（严格来说，是三维磁性传感器70的位置）固定不变的状态下，仅仅改变便携式仪器1的姿态μ，从而使得便携式仪器1的位置P_S在获得了N个磁测数据q₁到q_N的时候发生变化，那么就可以使得外部磁场B_x的影响最小（见图13（B））。在步骤S205中判定三维图形SD具有不同于球面形状的变形程度很大的形状的情况下，可以指示便携式仪器1的用户在便携式仪器1的位置固定不变的状态下旋转便携式仪器1。向便携式仪器1的用户进行的指示可以是通过在便携式仪器1的显示单元50上显示画面或动画或者通过输出语音来进行的。

在步骤S206，CPU 10执行中心点输出处理。具体地说，CPU 10将通过步骤S204的中心点计算处理计算出来的中心点c_O的坐标传递到前面介绍的状态估计处理（见图5）。

CPU 10执行如前所述的步骤S201到S206的处理，并且因此，CPU 10起到中心点推导单元200的作用。

随后，CPU 10判断是否满足结束状态估计程序的结束条件（步骤S207）。在满足结束条件的情况下，CPU 10结束流程图中所示的中心点推导处理。另一方面，在不满足结束条件的情况下，CPU 10使处理前进到步骤S201。

其中，虽然，在这一实施方式中，在步骤S207中结束条件得不到满足的情况下，CPU 10使处理过程前进到步骤S201，但是本发明并不局限于此。例如，在步骤S207的结束条件得不到满足的情况下，CPU 10可以将处理暂时停止预定的一段时间，并且可以在经过了预定的时间段之后，使处理过程前进到步骤S201。而且，在步骤S207的结束条件得不到满意的情况下，CPU 10可以判断是否需要基于通过状态向量更新处理（步骤S104）生成的观测残差z_k生成中心点c_O的坐标（例如，观测残差z_k的模是否等于或大于阈值），并且在确定需要生成中心点c_O的坐标时，可以使处理过程前进到步骤S201。

下文中，将会详细介绍磁测数据分布判定处理（步骤S203）、中心点计算处理（步骤S204）和变形判定处理（步骤S205）。其中，为了介绍方便，将首先介绍步骤S204的中心点计算处理，然后将会介绍步骤S203的磁测数据分布判定处理。

[5.中心点计算处理]

中心点计算处理是这样的处理：使用内部磁场B_i和地磁B_g之间属性上的差异，将磁场B_i分量从N个磁测数据q₁到q_N所表示的坐标中分离出来，以计算中心点c_O的坐标，这些坐标是成为磁性传感器的偏移量q_OFF候选值的坐标。

下文中，将会在5.1节中介绍内部磁场B_i和地磁B_g之间属性上的差异，这是中心点计算处理的前提，在5.2节中，将会介绍中心点计算处理。

[5.1.内部磁场和地磁的属性]

图8是表示，在三维磁性传感器70在姿态μ变为μ₁到μ_N的同时测量内部磁场B_i和地磁B_g的情况下，在传感器坐标系Σ_S中标绘三维磁性传感器70输出的N个磁测数据q₁到q_N的图形。

此时，图8中所记的各个向量的左上部所附的上标S的意思是，该向量是以传感器坐标系Σ_S表述的。

内部磁场Bi是由便携式仪器1的部件产生的磁场。内部磁场Bi具有相对于便携式仪器1恒定不变的方向和恒定不变的幅度，除非便携式仪器1的内部状态发生变化。即，内部磁场B_i被表示为传感器坐标系Σ_S中方向和幅度恒定不变的向量^SB_i，其方向和幅度不取决于位置P_S和三维磁性传感器70的姿态μ。具体地说，如图8所示，代表内部磁场的向量^SB_i是以传感器坐标系Σ_S的原点为起点并且以中心点c_OG为终点的向量。

另一方面，地磁B_g是具有指向北磁极的水平分量和垂直于磁倾角方向的分量的磁场。地磁B_g是在地面坐标系Σ_G中具有恒定不变的方向和恒定不变的幅度的磁场。因此，在便携式仪器1的姿态μ发生改变的情况下，地磁B_g的幅度不会发生变化，但是从便携式仪器1看去的地磁B_g的方向会发生变化。就是说，地磁B_g被表示为方向随着便携式仪器1的姿态μ的变化而变化并且幅度恒定不变的向量^SB_g(μ)。

输出磁测数据q₁到q_N，作为由这样一个向量代表的坐标：该向量代表向量^SB_i和向量^SB_g(μ)的和，向量^SB_i代表内部磁场、向量^SB_g（μ）在传感器坐标系Σ_S中代表地磁。结果，传感器坐标系Σ_S中由磁测数据q₁到q_N表示的坐标被分布在以中心点c_OG为中心并且以地磁B_g的幅度为半径的球面S_G上。同时，三维磁性传感器70的测量值具有测量误差。因此，严格地说，由磁测数据q₁到q_N表示的坐标有可能分布在球面S_G附近。

由表示内部磁场的向量^SB_i表示的球面S_G的中心点c_OG的坐标与磁性传感器的偏移量q_OFF所表示的坐标是一样的。结果，磁性传感器的偏移量q_OFF是通过找出基于磁测数据q₁到q_N的球面S_G的中心点c_OG的坐标而计算出来的。就是说，可以通过将磁测数据q_i表示的坐标减去磁性传感器的偏移量q_OFF所表示的坐标，来计算地磁B_g的正确方向和幅度。

[5.2.中心点计算处理的详情]

在下文中，将会参照图9介绍步骤S204的中心点计算处理。中心点计算处理是这样一种处理：假设三维磁性传感器70输出的N个磁测数据q₁到q_N所表示的坐标分布在球面S的附近，计算半径为r_S的球面S的中心点c_O的坐标。

其中，球面S是为了在传感器坐标系Σ_S中计算方便而引入的球面，为的是找出被设置为磁测数据q₁到q_N所表示的坐标在其附近的球面的中心点，并且因此，球面S不同于代表地磁B_g的球面S_G。

不过，在三维磁性传感器70仅仅检测内部磁场B_i和地磁B_g的情况下，由磁测数据q₁到q_N表示的坐标分布在球面S_G的附近。因此，在这种情况下，被设置为磁测数据q₁到q_N所表示的坐标处于其附近的球面S与球面S_G几乎是相同的，从而球面S的中心点c_O和球面S_G的中心点c_OG几乎是相同的。因此，在三维磁性传感器70仅仅检测内部磁场B_i和地磁B_g的情况下，可以通过计算被设置为磁测数据q₁到q_N所表示的坐标处于其附近的球面S的中心点c_O的坐标来计算球面S_G的中心点c_OG的坐标，即，磁性传感器的偏移量q_OFF。

在下文中，将会详细介绍球面S的中心点c_O的计算方法。其中，下面介绍的向量和坐标都是以传感器坐标系Σ_S表达的，除非另有说明。

假设磁测数据q₁到q_N所表示的坐标存在于半径为r_S的球面S上，磁测数据q_i所表示的坐标与球面S的中心点c_O之间的距离是r_s，因此，实现了下面的公式（34）。其中，由磁测数据q_i表示的坐标的各个元素是由公式（35）表达的，并且中心点c_O的坐标的各个元素由公式（36）表达。

‖q_i-c_o‖₂ ²＝r_s ²(i＝1，…，N)……(34)

其中，q_i＝[q_ix q_iy q_iz]^T……(35)

c_O＝[c_Ox c_Oy c_Oz]^T  ……(36)

这里，由磁测数据q₁到q_N表示的坐标的重心q_C是由公式（37）定义的。其中，重心q_C的各个元素是由公式（38）表达的。通过将磁测数据q_i表示的坐标和中心点c_O的坐标表示为以重心q_C为起点的向量，将公式（34）修改为公式（39）。

$q_C=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{q}_i\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(37\right)$

其中，q_C＝[q_cx q_cy q_cz]^T  ……(38)

‖(q_i-q_C)-(c_O-q_C)‖₂ ²＝r_s ²(i＝1，…，N)……(39)

通过将由磁测数据q₁到q_N所表示的坐标代入到公式（39）的磁测数据q_i表示的坐标中所获得的结果被减去磁测数据的数量N，获得下列公式（40）。而且，求公式（39）与公式（40）之间的差，获得下列公式（41）。

$\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{(q_i-q_C)}^T(q_i-q_C)+{(c_O-q_C)}^T(c_O-q_C)={r_s}^2\&CenterDot;\&CenterDot;\&CenterDot;\left(40\right)$

${(q_i-q_C)}^T(c_O-q_C)$

$=\frac{1}{2}[{(q_i-q_C)}^T(q_i-q_C)-\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{(q_i-q_C)}^T(q_i-q_C)]$

(i＝1，…，N)……(41)

这样，表示由作为磁测数据的变量q_i表示的坐标存在于球面S上的公式34被改为公式（41）。使用矩阵X将通过把N个磁测数据q₁到q_N表示的坐标代入到公式（41）的变量q_i中获得的N个公式表示为公式（42）。其中，矩阵X是由公式（43）表达的3×3矩阵。而且，向量j是由公式（44）表示的N维向量，并且值R是由公式（45）表示的值。

X(c_O-q_C)＝j  ……(42)

其中， $X=\left[\begin{array}{c}{(q_1-q_C)}^T\\ .\\ .\\ .\\ {(q_N-q_C)}^T\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(43\right)$

$j=\frac{1}{2}\left[\begin{array}{c}{(q_1-q_C)}^T(q_1-q_C)-R\\ .\\ .\\ .\\ {(q_N-q_C)}^T(q_N-q_C)-R\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(44\right)$

$R=\frac{1}{N}\stackrel{N}{\mathrm{\&Sigma;}}{(q_i-q_C)}^T(q_i-q_C)\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(45\right)$

在由磁测数据q₁到q_N表示的所有坐标完全与以中心点c_O为中心的球面S一致的情况下，公式（42）有解。不过，在考虑了三维磁性传感器70的测量误差时，所有的磁测数据q₁到q_N并不完全与球面S一致，因此，公式（42）无解。因此，为了使用统计法获得合理近似解，引入了第一球面误差向量δ_S，该向量是吸收由公式（46）表示的误差的向量。这里，由公式（47）表示的三维向量c是用来表示中心点c_O坐标的可变向量。

δ_S＝X(c-q_C)-j    ……(46)

其中，c＝[c_x  c_y  c_z]^T  ……(47)

可以将由使得第一球面误差向量δ_S的模最小的向量c（即，使(δ_S)^T(δ_S)最小的向量c）表示的坐标取为球面S的中心点c_O的坐标。这里，当定义了由下列公式（48）表示的目标函数（中心点计算函数）f_S(c)时，由使得目标函数f_S(c)最小的向量c表示的坐标具有这样的值：该值被取作球面S的中心点c_O的坐标。在公式（50）表示的方差-协方差矩阵A为正则的情况下，中心点c_O的坐标由公式（49）表示。

f_S(c)＝‖δ_S‖₂

＝‖X(c-q_C)-j‖₂……(48)

c_O＝A^-1X^Tj+q_C   ……(49)

其中，A＝X^TX    ……(50)

如前面所介绍的，在三维磁性传感器70仅仅检测内部磁场B_i和地磁B_g的情况下，球面S和表示地磁B_g的球面S_G变为几乎相同的球面，从而球面S的中心点c_O和球面S_G的中心点c_OG变为几乎相同的坐标。因此，在三维磁性传感器70仅仅检测内部磁场B_i和地磁B_g的情况下，可以采用表示由公式（49）代表的中心点c_O的坐标的向量作为磁性传感器的偏移量q_OFF。

[6.磁测数据分布判定处理]

下文中，将会介绍步骤S203中进行的磁测数据分布判定处理。

在前面介绍的中心点计算处理中，需要分布由磁测数据q₁到q_N表示的坐标，使得由磁测数据q₁到q_N表示的坐标具有在传感器坐标系Σ_S中的三维散布，以便计算球面S的中心点c_O。不过，由于便携式仪器1（三维磁性传感器70）的姿态μ在便携式仪器1的用户在手持便携式仪器1的同时移动该便携式仪器1的时候发生变化，因此如果便携式仪器1的运动不充分，仪器1的姿态可能不是三维变化的，而可能是二维变化。在这种情况下，传感器坐标系Σ_S中磁测数据q₁到q_N表示的坐标是二维分布的，不是三维散布的。

例如，在如图10中所示磁测数据q₁到q_N表示的坐标二维分布在传感器坐标系Σ_S的平面π上的圆π_C附近的情况下，球面S被唯一确定为以圆π_C为横截面的球面。以圆π_C为横截面的球面可以是以穿过圆π_C的中心点π_CO的垂直于平面π的直线π_L上的中心点c_π1为中心的球面S_π1或者以直线π_L上的中心点c_π2为中心的球面S_π2。即，可以确定球面S的中心点c_O位于直线π_L上；不过，不能具体确定球面S的中心点c_S位于直线π_L上的位置。因此，在由磁测数据q₁到q_N表示的坐标是二维分布的情况下，不能基于磁测数据q₁到q_N计算中心点c_O的正确坐标。

为了基于磁测数据q₁到q_N计算球面S的中心点c_O，需要磁测数据q₁到q_N表示的坐标如图11中所示那样在传感器坐标系Σ_S中以三维散布方式分布。在磁测数据分布判定处理中，CPU 10（中心点推导单元200）使用由公式（50）代表的方差-协方差矩阵A判定磁测数据q₁到q_N所表示的坐标是否是三维分布的。在下文中，将会介绍方差-协方差矩阵A的属性。

方差-协方差矩阵A的特征值按照大小的顺序被设为最大特征值λ1、中间特征值λ2和最小特征值λ3，并且与这些特征值对应的归一化为大小l的特征值被设为u₁、u₂和u₃。而且，以前面提到的重心q_C为原点的重心坐标系Σ_C中表示由磁测数据q_i代表的坐标的向量由^Cq_i表示。此时，特征值λ_j(j=1,2,和3)等于特征向量u_j方向上的方差ρ² _j。

如图11中所示，各个特征向量u₁，u₂和u₃被配置成使得各个特征向量u₁，u₂和u₃以重心坐标系Σ_C的原点q_C为起点。此时，例如，检验j＝1的情况。特征值λ₁等于这样获得的值：针对N个磁测数据^Cq_i（i=1,2，...，和N），通过将这些向量^Cqi投影到特征向量u₁上获得长度L_i1，对这些长度L_i1的平方(L_i1)²求平均而获得所述值。即，特征值λ_j代表N个磁测数据q_i所表示的坐标在特征向量u_j的方向上远离重心q_C的程度，即，由磁测数据q₁到q_N表示的坐标在特征向量u_j的方向上的分布扩散到了什么程度。

与最小特征值λ₃相对应的特征向量u₃的方向是由磁测数据q₁到q_N表示的坐标的分布具有最小扩展度的方向，并且最小特征值λ₃是用于表示在由磁测数据q₁到q_N表示的坐标的分布具有最小扩展度的方向上的扩展度的系数。因此,为了使磁测数据q₁到q_N表示的坐标能够三维分布，最小特征值λ₃可以具有大于等于预定阈值（允许的方差值）λ_O的值。

在磁测数据分布判定处理中，如果方差-协方差矩阵A的最小特征值λ₃等于或大于阈值λ_O，那么中心点推导单元200确定由磁测数据q₁到q_N表示的坐标是充分三维分布的，并且使得处理过程前进到前面提到的步骤S204的中心点计算处理。另一方面，在最小特征值λ₃小于阈值λ_O的情况下，中心点推导单元200确定由磁测数据q₁到q_N表示的坐标是二维分布的，而不是三维散布的，并且将处理过程返回到步骤S202的磁测数据累积处理。

[7.变形判定处理]

其中，如图3所示，除了地磁B_g和内部磁场B_i之外，三维磁性传感器70可以检测由处于便携式仪器1外部的物体2产生的外部磁场B_x。外部磁场B_x是非均匀磁场，其方向和幅度依照物体2与便携式仪器1之间的相对位置关系而变化。

第5节中介绍的中心点计算处理，在三维磁性传感器70仅仅检测地磁B_g和内部磁场B_i的前提下，计算球面S的中心点c_O的坐标，这些坐标是磁性传感器的偏移量q_OFF代表的坐标的候选值。因此，在三维磁性传感器70除了地磁B_g和内部磁场B_i之外还检测非均匀外部磁场B_x的情况下，通过中心点计算处理计算的中心点c_O的坐标由于外部磁场B_x的影响而有误差，中心点c_O的坐标不同于表示内部磁场B_i的坐标（即，磁性传感器的偏移量q_OFF）的可能性很大。在采用代表前面所述的中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF的情况下，不能进行适当的修正处理，因此，便携式仪器1不能计算地磁B_g的正确方向和幅度。

在步骤S205的变形判定处理中，CPU 10（中心点推导单元200）判定是否存在非均匀外部磁场B_x并且评估外部磁场B_x影响的幅度，以确定采用通过中心点计算处理计算出来的中心点c_O的坐标表示的向量作为磁性传感器的偏移量q_OFF是否正确。

在下文中，在7.1节中将会介绍要在变形判定处理中判定的外部磁场B_x的属性，然后在7.2节中将会详细介绍变形判定处理。

[7.1.外部磁场的属性]

图12是表示，当三维磁性传感器70的位置P_S变为P_S1到P_SN，并且此外，三维磁性传感器70的姿态μ变为μ₁到μ_N来测量磁场的时候，在传感器坐标系Σ_S中标绘出三维磁性传感器70输出的由磁测数据q₁到q_N表示的坐标。其中，在图12中，假设内部磁场B_i、地磁B_g和外部磁场B_x都存在。

外部磁场B_x是非均匀磁场，外部磁场B_x的方向和幅度依照产生外部磁场B_x的物体2与便携式仪器1之间的相对位置关系而变化。因此，在三维磁性传感器70的位置P_S发生变化的情况下，由三维磁性传感器70检测到的外部磁场B_x的方向和幅度也会变化。同样地，因此在三维磁性传感器70的姿态μ发生变化的情况下，由三维磁性传感器70检测到的外部磁场B_x的方向也会变化。即，外部磁场B_x被表示为传感器坐标系Σ_S中的向量^SB_x(μ,P_S)，其方向和幅度依照三维磁性传感器70在传感器坐标系Σ_S中的姿态μ和位置P_S而改变。

由磁测数据q₁到q_N表示的坐标由代表向量^SB_i、向量^SB_g(μ)和向量^SB_x(μ,P_S)的总和的向量表示，向量^SB_i代表内部磁场，向量^SB_g（μ）代表地磁，向量^SB_x(μ,P_S)代表外部磁场。结果，由磁测数据q₁到q_N表示的坐标被分布在通过重叠球面S_G和曲面SX获得的三维图形SD的表面附近，球面S_G代表向量^SB_g(μ)的终点，向量^SB_g(μ)代表以中心点c_OG为起点的地磁，曲面SX代表向量^SB_x(μ,P_S)的终点，向量^SB_x(μ,P_S)代表以中心点c_OG为起点的外部磁场。

在代表外部磁场B_x的曲面SX具有不同于球面的变形形状的情况下，三维图形SD也具有不同于球面的变形形状。在由磁测数据q₁到q_N表示的坐标被分布在具有不同于球面的变形形状的三维图形SD表面的附近的情况下，由磁测数据q₁到q_N表示的坐标与球面S之间的距离大大增加，尽管球面S被设置得使由磁测数据q₁到q_N表示的坐标尽可能处于该球面附近。在这种情况下，分布在三维图形SD表面附近的由磁测数据q₁到q_N表示的坐标并未分布在球面S附近。同样地，由磁测数据q₁到q_N表示的坐标也未分布在代表地磁B_g的球面SG的附近。

因此，在由磁测数据q₁到q_N表示的坐标分布在具有不同于球面的变形形状的三维图形SD的表面附近的情况下，球面S的中心点c_O和球面S_G的中心点c_OG具有不同坐标的可能性很大，从而不能采用表示中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF（见图15）。

不过，在非均匀外部磁场B_x的影响很小并且三维图形SD的形状几乎可以视为球面的情况下，被设为使由磁测数据q₁到q_N表示的坐标处于其附近的球面S和三维图形SD可以被视为几乎相同的图形，并且球面S的中心点c_O和代表地磁B_g的球面S_G的中心点c_OG具有几乎相同的坐标。因此，在三维图形SD的形状几乎被视为球面的情况下，可以采用表示球面S中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF。在下文中，在将会参照图13详细介绍外部磁场B_x的影响很小的情况。

图13（A）表示外部磁场B_x很弱的情况。

三维图形SD是通过将代表地磁B_g的球面S_G和代表外部磁场B_x的曲面SX重叠起来获得的。因此，在外部磁场B_x很弱的情况下，代表外部磁场B_x的曲面SX很小，因此，三维图形SD和球面S_G变得几乎相同。此时，分布在三维图形SD表面附近的由磁测数据q₁到q_N表示的坐标可以被认为也分布在球面S_G附近。

而且，由于附近有由磁测数据q₁到q_N表示的坐标的三维图形SD的形状几乎是球面，因此设为使得由磁测数据q₁到q_N表示的坐标在其附近的球面S和三维图形SD变为几乎相同的图形。即，三维图形SD、球面S_G和球面S变成几乎相同的图形，从而球面S的中心点c_O和球面S_G的中心点c_OG具有几乎相同的坐标。

因此，在如图13（A）所示那样外部磁场B_x很弱的情况下，可以采用表示通过中心点计算处理计算出来的中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF。

同时，即使在如图13(B)中所示那样非均匀外部磁场B_x很强的情况下，三维图形SD的形状也可以被视为几乎为球面。

例如，即使在存在非均匀外部磁场B_x的情况下，但是在便携式仪器1的用户在便携式仪器1的位置P_S（严格来说，是三维磁性传感器70的位置）固定的状态下手持便携式仪器1而不摇晃便携式仪器1地仅仅改变便携式仪器1的姿态μ，从而在获取N个磁测数据q₁到q_N的时候便携式仪器1的位置P_S发生变化的情况下，外部磁场B_x被表示为向量^SB_x(μ)，只有其方向随着三维磁性传感器70的姿态μ发生变化，而其幅度在传感器坐标系Σ_S中是恒定不变的。在这种情况下，代表外部磁场B_x的曲面SX的形状变为以中心点c_OG为中心的球面，因此，通过将球面S_G和曲面SX重叠起来获得的三维图形SD的形状也会变为几乎是球面。而且，三维图形SD是通过将以中心点c_OG为中心的球面S_G与以中心点c_OG为中心而形状为球面的曲面SX重叠起来获得的图形，因此，由三维图形SD表示的球面的中心点几乎与球面S_G的中心点c_OG一致。在三维图形SD的形状如前所述几乎为球面的情况下，设为使得由磁测数据q₁到q_N表示的坐标处于附近的球面S和三维图形SD变为几乎相同的图形。结果，球面S的中心点c_O和球面S_G的中心点c_OG具有几乎相同的坐标。

因此，在外部磁场B_x被检测为如图13（B）所示那样的具有恒定不变幅度的向量的情况下，可以采用表示通过中心点计算处理计算出来的中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF。

在如前所述非均匀外部磁场B_x的影响很大的情况下，即，在三维图形SD具有不同于球面的变形形状的情况下，不能采用表示通过中心点计算处理计算出来的中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF。

另一方面，在三维图形SD的形状几乎被视为球面的情况下，例如，在非均匀外部磁场B_x很弱的情况下或者在三维磁性传感器70将非均匀外部磁场B_x检测为幅度恒定不变的向量的情况下，可以采用表示球面S的中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF。

[7.2.变形判定处理的详情]

在下文中，将会参照图13到15详细介绍步骤S205的变形判定处理。

在前面介绍的中心点计算处理中，假设由磁测数据q₁到q_N表示的坐标分布在由公式（34）表示的球面S附近。另一方面，在本节介绍的变形判定处理中，假设由磁测数据q₁到q_N表示的坐标分布在具有不同于球面S的变形形状的三维图形SD的表面附近。

三维图形SD由公式（51）表示。如图14中所示，由公式（51）表示的三维图形SD是通过将代表球面的分量X(c-q_C)-j加到由公式（52）表示的N维向量的变形误差向量k(E)上而获得的。同时，公式（51）右侧的0_N右下角所附的下标N代表零向量的维数。而且，由公式（53）表示的变形评估矩阵E是3×3的对称矩阵。

同时，由公式（51）中的分量X(c-q_C)-j表示的球面和由中心点计算单元计算出来的球面S不必代表传感器坐标系Σ_S中相同的图形。因此，在下文中，将会把表示为公式（51）中的分量X(c-q_C)-j的球面称为球面（第二球面）S₂，并且将会把球面S₂的中心点称为中心点（第二球面的中心点）c_O2。而且，尽管公式（51）中使用的向量c是由公式（47）表示的三维向量，但在本节中，向量c被用作计算由中心点c_O2表示的坐标的可变向量。

变形判定处理评估代表公式（51）的变形的分量k(E)的幅度，以评估三维图形SD的形状与球面S₂的形状彼此不同的程度。

X(c-q_C)+k(E)-j＝0_N    ……(51)

其中， $k\left(E\right)=\left[\begin{array}{c}{(q_1-c_0)}^{T}E(q_1-c_O)\\ .\\ .\\ .\\ {(q_N-c_0)}^{T}E(q_N-c_O)\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(52\right)$

$E=\left[\begin{array}{ccc}{e}_{11}& e_{12}& e_{13}\\ e_{12}& e_{22}& e_{23}\\ e_{13}& e_{23}& e_{33}\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(53\right)$

构成变形误差向量k(E)的元素当中的第i行元素ke(q_i-c_O)是通过将代表磁测数据q_i的、以中心点c_O为起点的向量(q_i-c_O)代入到由下列公式（54）表示的函数ke(p)中而给出的。函数ke(p)是以公式（53）表示的变形评估矩阵E为系数矩阵并且以公式（55）表示的向量p的三个元素为变量的二次型函数。即，函数ke(p)表示向量p与使用变形评估矩阵E对向量p进行转换而获得的向量Ep的内积。

结果，变形误差向量k(E)的第i行元素ke(q_i-c_O)是代表磁测数据q_i坐标、以球面S的中心点c_O为起点的向量(q_i-c_O)与通过使用变形评估矩阵E对向量(q_i-c_O)进行转换而获得的向量E(q_i-c_O)的内积。

ke(p)＝p^TEp  ……(54)

其中，p＝[p_x  p_y  p_z]^T  ……(55)

同时，当考虑三维磁性传感器70的测量误差时，所有由磁测数据q₁到q_N表示的坐标都不在与三维图形SD完全一致的位置上，从而公式（51）无解。因此，为了使用统计法获得合理近似解，引入了固定误差向量δ_SD，该向量是吸收由公式（56）表示的误差的向量。固定误差向量δ_SD是通过将第二球面误差向量δ_S2与变形误差向量k(E)相加而获得的。同时，第二球面误差向量δ_S2是与代表公式（51）的球面的分量X(c-q_C)-j相对应的分量，后面将对此进行详细介绍。

固定误差向量δ_SD是表示由磁测数据q₁到q_N表示的坐标与三维图形SD表面之间的误差的N维向量。由磁测数据q₁到q_N表示的坐标处于其表面附近的三维图形SD是基于这样的向量c来表示的：使得固定误差向量δ_SD和变形评估矩阵E的范数最小的向量c，即，使得由公式（57）表示的目标函数（变形评估函数）f_SD(E,c)和变形评估矩阵E最小的向量c。

变形判定处理是基于由公式（58）和公式（59）表示的变形评估值g_D(E)，评估变形误差向量k(E)对三维图形SD形状的影响程度的处理，后面将对公式（58）和公式（59）加以介绍。可以通过评估变形误差向量k(E)对三维图形SD形状的影响程度，知道三维图形SD的形状与球面S的形状彼此不同的程度。

δ_SD＝δ_S2+k(E)

＝X(c-q_C)+k(E)-j    ……(56)

f_SD(E，c)＝‖δ_SD‖₂

＝‖X(c-q_C)+k(E)-j‖₂……(57)

下文中，将会介绍由公式（56）表示的固定误差向量δ_SD的属性，同时将其与公式（46）表示的第一球面误差向量δ_S的属性进行比较。

首先，第一球面误差向量δ_S是用来吸收由磁测数据q₁到q_N表示的坐标与球面S之间的误差的向量。构成第一球面误差向量δ_S的第一行元素和第N行元素是独立变量。因此，在由磁测数据q₁到q_N表示的坐标和球面S之间的误差被第一球面误差向量δ_S吸收的情况下，由磁测数据q₁到q_N表示的坐标和球面S之间的N个误差变为无限制地独立设定的值。即，由第一球面误差向量δ_S表示的N个误差是可以独立设置的。所有N个误差都是对称且不依赖方向的白噪声。

即，中心点计算处理是这样的处理：使用第一球面误差向量δ_S表示由磁测数据q₁到q_N表示的坐标和球面S之间的误差（其为白噪声），并且找出球面S的中心点c_O的坐标以使第一球面误差向量δ_S最小。

另一方面，固定误差向量δ_SD是由第二球面误差向量δ_S2和用于吸收由磁测数据q₁到q_N表示的坐标与三维图形SD之间的误差的变形误差向量k(E)之和表示的向量。

按照与第一球面误差向量δ_S相同的方式，第二球面误差向量δ_S2是存在于公式（46）右侧的，用来将由磁测数据q₁到q_N表示的坐标与球面S₂之间的误差表示为白噪声的向量。

另一方面，变形误差向量k(E)是以由公式（54）表示的函数ke(p)为各个元素的向量，函数ke(p)被配置为具有三个变量的二次型。具有三个变量的二次型是变量构成二次项的函数。可以表示三维空间中的各种曲面，比如直线、平面、柱面、球面、椭圆面、锥面、单叶双曲面、双叶双曲面和各种抛物面。结果，变形误差向量k(E)并不将由磁测数据q₁到q_N表示的坐标与球面S₂之间的N个误差表示为独立值，而是将所有的N个误差表示为具有这样限制的值：N个误差存在于三维空间中由同一函数ke(p)表示的曲面上。

这样，固定误差向量δ_SD将由磁测数据q₁到q_N表示的坐标与球面S₂之间的N个误差分别表示为第二球面误差向量δ_S2（其为白噪声）和具有由于平滑曲面造成的从球面S₂发生变形的属性的变形误差向量k(E)。

在下文中，将会参照图13和15介绍变形误差向量k(E)对三维图形SD形状的影响，和采用表示通过中心点计算处理计算出来的球面S的中心点c_O的坐标的向量作为磁性传感器的偏移量q_OFF是否正确的确定过程。

图13（A）和13（B）表示变形误差向量k(E)对三维图形SD形状的影响很小的情况。在变形误差向量k(E)的影响很小的情况下，变形误差向量k(E)的幅度很小，因此，变形误差向量k(E)的影响很小的情况意味着这样一种情况：如果实现了表示三维图形SD的公式（51），则认为表示球面S的公式（42）也得到了实现。即，在变形误差向量k(E)的影响很小的情况下，通过使公式（57）设定的目标函数f_SD(E,c)最小而获得的三维图形SD和通过使由公式（48）设定的目标函数f_S(c)最小而获得的球面S变为几乎是一样的。在如前所述三维图形SD的形状被认为是球面的情况下，由磁测数据q₁到q_N表示的坐标分布在三维图形SD的表面附近，并且同时，可以被视为也分布在球面S的附近。而且，由三维图形SD表示的球面的中心点几乎与球面S_G的中心点c_OG一致，并且因此，球面S的中心点c_O和球面S_G的中心点c_OG具有几乎相同的坐标。因此，在变形误差向量k(E)的影响很小的情况下，可以采用表示球面S中心点c_O坐标的向量作为磁性传感器的偏移量q_OFF。同时，在变形评估矩阵E变为零矩阵的情况下，变形误差向量k(E)也会变成零向量。结果，公式（48）和公式（57）变为彼此相等，因此，三维图形SD和球面S变成完全一样的图形。

图15表示变形误差向量k(E)的影响很大的情况。

变形误差向量k(E)的影响很大的情况指的是这样一种情况：虽然球面S₂被设置为使得由磁测数据q₁到q_N表示的坐标尽可能处于其附近，但是不能使用第二球面误差向量δ_S2吸收由磁测数据q₁到q_N表示的坐标和球面S₂之间的误差，因此，需要使用表示从球面S₂发生平滑变形的变形误差向量k(E)吸收这些误差。即，在这种情况下，通过使目标函数f_SD(E,c)最小而获得的三维图形SD具有变形的形状，该形状大大不同于通过使目标函数f_S(c)最小而获得的球面S的形状。在如前所述三维图形SD的形状是不同于球面形状的变形形状的情况下，分布在三维图形SD表面附近的由磁测数据q₁到q_N表示的坐标不能被认为分布在球面S的附近。中心点计算处理是这样的处理：以由磁测数据q₁到q_N表示的坐标存在于球面S附近为前提，计算中心点c_O的坐标，作为磁性传感器的偏移量q_OFF的候选值。结果，在其附近没有由磁测数据q₁到q_N表示的坐标的球面S的中心点c_O并不表达磁性传感器的偏移量q_OFF。因此，在这种情况下，不能采用表示中心点c_O的坐标的向量作为磁性传感器的偏移量q_OFF。

在如前所述变形误差向量k(E)对三维图形SD的形状的影响很小的情况下，分布在三维图形SD上的由磁测数据q₁到q_N表示的坐标可以被视为也分布在球面S上，因此，可以采用表示由中心点计算单元计算出来的球面S的中心点c_O的坐标的向量作为磁性传感器的偏移量q_OFF。另一方面，在变形误差向量k(E)对三维图形SD的形状的影响很大的情况下，中心点c_O的坐标具有有误差的不正确值，因此，需要防止表示中心点c_O坐标的向量被采用为磁性传感器的偏移量q_OFF。

在下文中，将会介绍评估变形误差向量k(E)对三维图形SD形状的影响幅度的方法。

这里，由公式（58）和（59）表示的变形评估值g_D(E)被定义为评估变形误差向量k(E)对三维图形SD形状的影响幅度的评估值。变形评估值g_D(E)是具有最大绝对值（变形评估矩阵E的范数）的最大特征值λ_E1的绝对值，该特征值是变形评估矩阵E的三个特征值之一。

如果变形评估值g_D(E)是等于或小于预定阈值（可允许的变形值）δ_O的较小值，则可以认为三维图形SD和球面S₂是相同的图形，因此，也可以认为分布在三维图形SD表面附近的由磁测数据q₁到q_N表示的坐标也分布在球面S附近。而且，可以采用表示通过中心点计算处理获得的球面S的中心点c_O的坐标的向量作为磁性传感器的偏移量q_OFF。

g_D(E)＝|λ_E1|＝‖E‖₂……(58)

其中，f_SD(E,c)→最小......(59)

在下文中，将会介绍得出变形评估值g_D(E)的方法。

首先，公式（54）表示的函数ke(p)是以二次型表示的函数，并且因此，可以将函数ke(p)表示为由下列公式（60）表示的向量内积。而且，变形误差向量k(E)的第i行元素ke(q_i-c_O)是通过将向量(q_i-c_O)代入到函数ke(p)中而获得的值，并且因此，第i行元素ke(q_i-c_O)是由公式（61）使用六维向量ke₂(i)和六维向量e_E表示的，六维向量ke₂(i)由公式（62）表示，六维向量e_E中的变形评估矩阵E的分量被安排成由公式（63）表示。

$\mathrm{ke}\left(p\right)=e_{11}{p_x}^2+e_{22}{p_y}^2+e_{33}{p_z}^2+2e_{12}{p}_x{p}_y+2e_{13}{p}_x{p}_z+2e_{23}{p}_y{p}_z$

$={\left[\begin{array}{c}{p_x}^2\\ {p_y}^2\\ 2p_x{p}_y\\ {p_z}^2\\ 2p_y{p}_z\\ 2p_x{p}_z\end{array}\right]}^T\left[\begin{array}{c}{e}_{11}\\ e_{22}\\ e_{12}\\ e_{33}\\ e_{23}\\ e_{13}\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(60\right)$

ke(q_i-c_o)＝ke₂(i)^Te_E    (i＝1，…，N)……(61)

其中，

${\mathrm{ke}}_2\left(i\right)=\left[\begin{array}{c}{(q_{\mathrm{ix}}-c_{\mathrm{Ox}})}^2\\ {(q_{\mathrm{iy}}-c_{\mathrm{Oy}})}^2\\ 2(q_{\mathrm{ix}}-c_{\mathrm{Ox}})(q_{\mathrm{iy}}-c_{\mathrm{Oy}})\\ {(q_{\mathrm{iz}}-c_{\mathrm{Oz}})}^2\\ 2(q_{\mathrm{iy}}-c_{\mathrm{Oy}})(q_{\mathrm{iz}}-c_{\mathrm{Oz}})\\ 2(q_{\mathrm{ix}}-c_{\mathrm{Ox}})(q_{\mathrm{iz}}-c_{\mathrm{Oz}})\end{array}\right](i=1,\&CenterDot;\&CenterDot;\&CenterDot;,N)---\left(62\right)$

e_E＝[e₁₁ e₂₂ e₁₂ e₃₃ e₂₃ e₁₃]^T    ……(63)

这里，引入了由公式（64）表示的矩阵X2。矩阵X2是通过排布对公式（62）表示的六维向量ke₂(i)进行转置获得的向量和对三维向量（q_i-q_C）在每一行进行转置获得的向量而产生的N×9矩阵。

$X_2=\left[\begin{array}{cc}{\mathrm{ke}}_2{\left(1\right)}^T& {(q_1-q_C)}^T\\ {\mathrm{ke}}_2{\left(2\right)}^T& {(q_2-q_C)}^T\\ .& .\\ .& .\\ .& .\\ {\mathrm{ke}}_2{\left(N\right)}^T& {(q_N-q_C)}^T\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(64\right)$

可以使用公式（64）表示的矩阵X2，将由公式（57）表示的目标函数f_SD(E,c)修改为由公式（65）表示的目标函数g_SD(e)。同时，向量e是如下列公式（66）所表示的，其中排列着由公式（63）表示的六维向量e_E和由公式（67）表示的三维向量e_X的九维向量。而且，公式（67）表示的向量e_X是通过将公式（47）表示的可变向量减去公式（37）表示的重心q_C而获得的向量。

g_SD(e)＝‖X₂e-j‖₂……(65)

其中， $e=\left[\begin{array}{c}{e}_E\\ e_X\end{array}\right]\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\&CenterDot;\left(66\right)$

此处，e_X＝c-q_C……(67)

通过对下列公式（68）表示的联立方程运用高斯消去法或者Cholesky因子分解法，得出使公式（65）表示的目标函数g_SD(e)最小的解e=e_O。其中，公式（68）是通过对公式（65）运用最小二乘法而计算出来的联立方程。

(X₂ ^TX₂)e₀＝X₂ ^Tj  ……(68)

基于如前所述那样获得的解e_O，恢复公式（53）的变形评估矩阵E。而且，得出公式（58）表示的变形评估值g_D(E)，即，变形评估矩阵E的范数，并且判断变形评估值g_D(E)是否等于或小于阈值δ_O。同时，变形评估矩阵E的范数等于具有最大绝对值的特征值λ_E1的绝对值，该特征值λ_E1是变形评估矩阵E的三个特征值之一，并且因此，可以使用雅克比法（Jacobi method）或幂法得出变形评估矩阵E的范数。

而且，在变形评估值g_D(E)等于或小于阈值δ_O的情况下，中心点推导单元200使处理过程前进到步骤S206的中心点输出处理。另一方面，在变形评估值g_D(E)大于阈值δ_O的情况下，中心点推导单元200使处理过程返回到步骤S201的初始化处理。

[8.中心点输出处理]

步骤S206的中心点输出处理是在这种情况下进行的：在变形判定处理中判定变形评估值g_D(E)等于或小于阈值δ_O。在中心点输出处理中，CPU 10（中心点推导单元200）输出通过中心点计算处理计算出来的球面S的中心点c_O的坐标并且将输出坐标提供给状态估计单元100。

虽然，在这一实施方式中，中心点推导单元200输出的是球面S中心点c_O的坐标，但是中心点推导单元200也可以输出球面S₂中心点c_O2的坐标。这是因为，在变形评估值g_D(E)等于或小于阈值δ_O的情况下，通过中心点计算处理计算出来的球面S的中心点c_O和球面S₂的中心点c_O2具有几乎相同的坐标，并且因此，可以采用表示中心点c_O的向量和表示中心点c_O2的向量作为磁性传感器的偏移量qoff。

同时，球面S₂的中心点c_O2的坐标是通过将解e_O的使公式（66）的e_X对应的目标函数g_SD(e)最小的部分代入到公式（67）中获得的。

[9.结论]

如上所述，按照本实施方式的包括中心点推导单元200和状态估计单元100（包括卡尔曼滤波操作单元120）的状态估计设备，使用由中心点推导单元200输出的中心点c_O的坐标，重写磁性传感器的偏移量估计值q_O，以进行卡尔曼滤波操作，该偏移量估计值q_O是用于卡尔曼滤波操作单元120进行卡尔曼滤波操作的状态向量x_k的元素之一。如前面所述的，不能定式化卡尔曼滤波器的状态转移模型中的磁性传感器的偏移量估计值q_O的时基变化，并且卡尔曼滤波器的状态向量x_k受到更新之前的状态向量x_k的影响。出于这一原因，磁性传感器的偏移量估计值q_O可能会严重背离实际值（磁性传感器的偏移量q_OFF）。在这种情况下，状态估计设备不能正确估计***状态，并且因此，不能基于由状态估计设备计算出来的状态变量来计算地磁B_g的方向和便携式仪器1的姿态μ。

另一方面，按照本实施方式的卡尔曼滤波操作单元120使用由中心点推导单元200输出的中心点c_O的坐标，重写磁性传感器的偏移量估计值q_O，该偏移量估计值q_O是状态向量x_k的元素之一（步骤S103中进行的状态变量重写处理）。如前面所介绍的，中心点c_O的坐标是基于在相对较短时段内从三维磁性传感器70输出的磁测数据q₁到q_N来计算的。即，即使在磁性传感器的偏移量q_OFF突然发生很大变化的情况下，表示中心点c_O坐标的向量也可以正确表示变化之后的磁性传感器的偏移量q_OFF。

如前所述，磁性传感器的偏移量估计值q_O是使用中心点c_O坐标来重写的，从而防止了状态向量x_k具有很大不同于实际值的值。此外，便携式仪器1可以基于状态向量x_k计算地磁B_g的正确方向和幅度。

而且，按照本实施方式的中心点推导单元200在进行中心点计算处理之前进行磁测数据分布判定处理，以判断中心点计算处理中使用的磁测数据q₁到q_N是否适合于进行中心点计算处理。结果，中心点推导单元200掌握了由磁测数据q₁到q_N表示的坐标分布的三维散布程度，并且在由磁测数据q₁到q_N表示的坐标具有平面分布的情况下，不进行中心点计算处理。结果，可以防止基于具有平面分布的磁测数据q₁到q_N计算不正确中心点c_O（不同于实际值的中心点c_O）的坐标，从而由于防止了不必要的操作处理，实现了低功耗。

而且，按照本实施方式的中心点推导单元200在进行了中心点计算处理之后，进行变形判定处理，以判断通过中心点计算处理获得的球面S的中心点c_O的坐标是否是受到外部磁场B_x影响的不适当的值（不同于实际值的值）。结果，可以防止输出受到外部磁场B_x影响的不适当中心点c_O（不能被当作磁性传感器的偏移量q_OFF的中心点c_O）的坐标。

这样，按照本实施方式的中心点推导单元200通过磁测数据分布判定处理和变形判定处理，仅仅输出可以被当作磁性传感器的偏移量q_OFF的接近于实际值的中心点c_O的坐标。而且，卡尔曼滤波操作单元120使用接近于实际值的中心点c_O的坐标重写磁性传感器的偏移量估计值q_O，并且因此，可以将状态向量x_k更新为更加接近于实际值的正确值。结果，按照本实施方式的便携式仪器1可以基于由状态估计设备计算出来的状态向量x_k稳定地计算出地磁B_g的正确值。

<B.变型例>

发明并不局限于上述实施方式，而是可以进行如下改变。而且，可以将两个或多个下述变型例适当组合起来。

（1）第一变型例

虽然，在前面介绍的实施方式中，卡尔曼滤波操作单元120使用从中心点推导单元200输出的中心点c_O的坐标重写代表磁性传感器的偏移量估计值q_O的分量（其是状态向量x_k的分量之一），以进行卡尔曼滤波操作，但是本发明并不局限于此。进行卡尔曼滤波操作，也可以不使用中心点c_O的坐标重写状态向量x_k。

图16是表示运行按照第一变型例的状态估计程序的按照第一变型例的状态估计设备的CPU 10实现的功能的功能框图。

如图16中所示，除了按照第一变型例的状态估计设备包括输出合成单元300和代替状态估计单元100的状态估计单元100a以外，按照第一变型例的状态估计设备在结构上与按照本发明实施方式的状态估计设备相同。除了状态估计单元100a包括代替卡尔曼滤波操作单元120的卡尔曼滤波操作单元120a以外，状态估计单元100a在结构上与状态估计单元100一样。除了卡尔曼滤波操作单元120a并不使用中心点c_O的坐标重写状态向量x_k以外，卡尔曼滤波操作单元120a以与卡尔曼滤波操作单元120相同的方式进行操作。

输出合成单元300基于卡尔曼滤波操作单元120a输出的更新后的状态向量x⁺ _k和从中心点推导单元200输出的中心点c_O的坐标，输出状态向量xc_k。

按照与状态向量x_k相同的方式，状态向量xc_k是以姿态μ、地磁强度r、地磁磁倾角φ、角速度ω、角速度传感器的偏移量估计值g_O和磁性传感器的偏移量估计值q_O为元素的向量。在状态向量xc_k中，从初始状态（时刻k=0）到中心点推导单元200输出中心点c_O的坐标为止的时间段被设置为与状态向量x_k相同的值。另一方面，在中心点推导单元200输出中心点c_O的坐标之后，磁性传感器的偏移量估计值q_O（其是构成状态向量xc_k的分量之一）被设置为与中心点c_O的坐标相同的值。

卡尔曼滤波操作单元120a综合三个传感器（比如三维磁性传感器70、三维加速度传感器80和三维角速度传感器90）的输出，以评估各种状态变量，并且因此，可以高速计算磁性传感器的偏移量估计值q_O。另一方面，中心点推导单元200基于N个磁测数据q₁到q_N计算中心点c_O的坐标，从而在计算出中心点c_O的坐标之前会花费预定的时间；不过，通过变形判定处理判断中心点c_O的坐标是否合适，并且因此，表示中心点c_O的坐标的向量具有达到这种程度的正确值：中心点c_O的坐标可以被作为磁性传感器的偏移量q_OFF。

结果，按照第一变型例的便携式仪器1可以使用卡尔曼滤波操作单元120a计算出来的磁性传感器的偏移量估计值q_O获得粗略的地磁B_g方向和幅度，直到中心点推导单元200计算出中心点c_O的坐标。结果，便携式仪器1无需便携式仪器1的用户等待很长时间就可以显示出地磁B_g的方向和幅度，从而改善了使用的便利性。

（2）第二变型例

虽然，在前面介绍的实施方式中，中心点推导单元200执行初始化处理、磁测数据累积处理、磁测数据分布判定处理、中心点计算处理、变形判定处理和中心点输出处理，但是本发明并不局限于此。中心点推导单元可以执行上述处理中的一些。例如，在执行中心点输出处理的时候，中心点推导单元200可以输出通过中心点计算处理计算出来的中心点c_O的坐标，而不进行变形判定处理。而且，中心点推导单元200可以进行中心点计算处理，而不执行磁测数据分布判定处理。

在这种情况下，即使在磁性传感器的偏移量q_OFF突然发生很大变化的情况下，中心点推导单元200也可以容易地计算出变化之后的磁性传感器的偏移量q_OFF的值。

（3）第三变型例

虽然，在前面介绍的实施方式中，卡尔曼滤波操作单元120通过延迟单元121使用从中心点推导单元200输出的中心点c_O的坐标重写代表磁性传感器的偏移量估计值q_O的分量（其是状态向量x的分量之一），但是本发明并不局限于此。卡尔曼滤波操作单元可以通过除了延迟单元121之外的单元来使用中心点c_O的坐标以重写状态向量x。

例如，在生成2a+1个西格玛点时，西格玛点产生单元122可以使用中心点c_O的坐标重写代表磁性传感器的偏移量估计值q_O的分量，该分量是存在于公式（14）和公式（15）右侧的更新后的状态向量x⁺ _k的分量之一。而且，状态转移模型单元123的操作中使用的状态转移模型可以使用中心点c_O的坐标重写公式（27）右侧的代表磁性传感器的偏移量估计值q_O,K-1的分量。此外，可以使用中心点c_O的坐标重写加法器129计算出来的代表磁性传感器的偏移量估计值q_O的分量，该分量是更新后的状态向量x⁺ _k的分量之一。

（4）第四变型例

虽然，在前面介绍的实施方式中，卡尔曼滤波操作单元120使用作为非线性卡尔曼滤波器的西格玛点卡尔曼滤波器进行操作，但是本发明并不局限于此。除了西格玛点卡尔曼滤波器之外，卡尔曼滤波操作单元120可以使用另一种非线性卡尔曼滤波器进行操作，比如扩展的卡尔曼滤波器。

（5）第五变型例

虽然，在前面介绍的实施方式中，采用了便携式仪器1的姿态μ、地磁强度r、地磁的磁倾角φ、便携式仪器1的角速度ω、角速度传感器的偏移量估计值g_O和磁性传感器的偏移量估计值q_O作为状态向量x的分量，并且通过卡尔曼滤波操作估计这六个状态变量，但是本发明并不局限于此。例如，状态向量x可以由这六个状态变量中的一部分来构成，并且这六个状态变量中的一部分可以是估计出来的。

（6）第六变型例

虽然，在前面介绍的实施方式中，观测向量y_k是以磁性传感器输出的观测值q、加速度传感器输出的观测值a和角速度传感器输出的观测值g作为分量的向量，但是本发明并不局限于此。观测向量可以是仅使用其中部分分量生成的。

Claims (8)

1.一种状态估计设备，包括：

多个传感器，其中包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的各磁分量的磁测数据；

卡尔曼滤波器，其构成为用来基于观测残差更新状态向量，该状态向量具有代表多个状态变量的多个分量，所述多个状态变量包括用于估计磁测数据的偏移量的状态变量，所述观测残差是使用所述状态向量和具有代表所述多个传感器的输出的多个分量的观测向量计算出来的；

累积单元，其构成为用来累积从三维磁性传感器中相继输出的磁测数据；和

中心点计算单元，其构成为用来假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面附近，并且构成为用来计算表示该第一球面的中心点的坐标，其中

卡尔曼滤波器使用表示第一球面的中心点的坐标来对用于估计磁测数据的偏移量的状态变量进行更新。

2.按照权利要求1所述的状态估计设备，其中，卡尔曼滤波器构成为使用状态转移模型根据某一时刻的状态向量来估计经过单位时间之后的状态向量，卡尔曼滤波器构成为使用观测模型根据估计出的经过单位时间之后的状态向量来计算估计的观测向量，卡尔曼滤波器构成为基于估计出的观测向量和具有代表所述多个传感器的输出的多个分量的观测向量来计算观测残差，并且卡尔曼滤波器构成为用来基于观测残差和估计出的经过单位时间之后的状态向量来更新状态向量。

3.按照权利要求1所述的状态估计设备，还包括：

分布判定单元，其构成为用来计算表示预定数量的累积磁测数据的坐标分布的三维分散度的分散评估值，从而使得中心点计算单元构成为用来在该分散评估值等于或大于可容许的分散值的情况下计算表示第一球面的中心点的坐标；

变形判定单元，该变形判定单元构成为用来假设多个累积磁测数据表示的坐标有可能分布在通过将第二球面和曲面组合起来而获得的三维图形的表面附近，该变形判定单元构成为用来基于所述多个累积磁测数据对表示所述三维图形的形状与第二球面的形状之间的差异程度的变形评估值进行计算，并且该变形判定单元构成为用来判断该变形评估值是否等于或小于可容许的变形值；和

中心点输出单元，其构成为用来在变形判定单元的判定结果表明变形评估值等于或小于可容许的变形值的情况下输出表示第一球面的中心点的坐标。

4.按照权利要求3所述的状态估计设备，其中卡尔曼滤波器构成为用来在中心点输出单元输出表示第一球面的中心点的坐标的情况下，使用表示第一球面的中心点的坐标来对用于估计偏移量的状态变量进行更新。

5.按照权利要求1、2、3或4所述的状态估计设备，其中中心点计算单元构成为用来假设由磁测数据表示的坐标所指定的位置有可能分布在第一球面的附近，中心点计算单元构成为用来设置表示了由磁测数据表示的坐标所指定的位置与第一球面之间的误差的第一球面误差向量，中心点计算单元构成为用来设置包含三维向量作为变量并且表示第一球面误差向量的幅度的中心点计算函数，并且中心点计算单元构成为用来计算表示第一球面的中心点的坐标，该坐标作为当中心点计算函数的值最小时由三维向量表示的坐标。

6.按照权利要求1、2、3或4所述的状态估计设备，其中在包括产生磁场的部件的仪器中安装所述三维磁性传感器。

7.按照权利要求6所述的状态估计设备，其中所述偏移量是代表由该仪器的部件所产生的磁场的分量的三维向量。

8.一种在状态估计设备中进行的偏移量更新方法，该状态估计设备包括多个传感器，所述多个传感器包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的各磁分量的磁测数据，该方法包括：

基于观测残差更新状态向量，该状态向量具有代表多个状态变量的多个分量，所述多个状态变量包括用于估计磁测数据的偏移量的状态变量，所述观测残差是使用所述状态向量和具有代表所述多个传感器的输出的多个分量的观测向量计算出来的；

累积从三维磁性传感器相继输出的磁测数据；

假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面的附近；

计算表示第一球面的中心点的坐标；和

使用表示第一球面的中心点的坐标来对用于估计磁测数据的偏移量的状态变量进行更新。