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

专利名称：状态估计设备和偏移更新方法
技术领域：
本发明涉及状态估计设备和偏移更新方法。
背景技术：
近年来，随着便携式仪器的性能和用途的提升，人们正在积极研发具有估计地磁方向和便携式仪器姿态的功能的便携式仪器。在估计地磁的方向和便携式仪器的姿态的情况下，使用测量各种不同类型物理量的多个传感器的输出的综合来实现估计，速度比仅仅使用单独一个传感器(比如地磁传感器)的输出实现的估计要快得多。使用卡尔曼(Kalman)滤波器的方法是一种众所周知的对测量各种不同类型物理量的多个传感器的输出进行综合以估计动态系统状态的方法。例如，专利文献I公开了一种姿态角度测量设备，具有安装在其中的三轴角速度传感器、三轴加速度传感器和一个非 线性卡尔曼滤波器。而且，非专利文献I公开了一种使用扩展卡尔曼滤波器或西格玛点(sigma point)卡尔曼滤波器(使用无损变换(unscented transformation))对三轴角速度传感器、三轴加速度传感器和三轴地磁传感器输出的信号进行综合的方法。[专利文献I]日本专利申请公开第H9-5104号[非专利文献 l]Wolfgang Gunthner 所著的《Enhancing Cognitive AssistanceSystems with Inertial Measurement Units》，Springer,2008一般来说，卡尔曼滤波器具有用于估计代表动态系统状态的多个物理量的时基变化的状态转移模型和用于根据动态系统的估计状态来估计由动态系统的多个传感器测得的观测值的观测模型。而且，卡尔曼滤波器使用估计的观测值与传感器实际测量的观测值之间的差值(观测残差)，将以代表动态系统状态的物理量为元素的状态向量更新为更加接近于实际值的值。不过，观测残差是利用更新前的状态向量计算出来的值。在动态系统的状态突然和明显变化的情况下，状态向量会被更新为不同于实际值的假值。

发明内容
本发明是鉴于上述问题而做出的，并且本发明的一个目的是，即使在动态系统的状态突然发生大幅变化的情况下，也能正确地估计动态系统的状态。在下文中，将会介绍本发明。同时，为了便于理解，在括号中给出了实施方式、变型例和附图
中的附图标记，不过，本发明并不局限于这些实施方式。为了解决上述问题，按照本发明的状态估计设备包括多个传感器，其中包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的各磁分量的磁测数据(Qi);卡尔曼滤波器，其构成为用来基于观测残差(Zk)更新状态向量(xk)，该状态向量具有代表多个状态变量的分量，所述多个状态变量包括用于估计磁测数据的偏移量(q<w)的状态变量( )，所述观测残差是使用状态向量和具有代表多个传感器的输出的分量的观测向量(yk)计算出来的；累积单元，其构成为用来累积从三维磁性传感器中相继输出的磁测数据；和中心点计算单元，其构成为用来假设由预定数量(N)的累积磁测数据表示的坐标有可能分布在第一球面(S)附近，并且构成为用来计算表示该第一球面的中心点(C())的坐标，其中，卡尔曼滤波器使用表示第一球面的中心点( )的坐标来对用于估计磁测数据的偏移量(q0FF)的状态变量( )进行更新或重写。便利地，三维磁性传感器被安装在包括产生磁场(Bi)的部件的仪器中。在这种情况下，所述偏移量(q<w)是代表由该仪器的所述部件产生的磁场(Bi)的分量的三维向量。按照本发明，包括中心点计算单元和卡尔曼滤波器的状态估计设备使用由中心点计算单元计算出来的中心点的坐标来对用于估计三维磁性传感器的偏移量的状态变量(该状态变量是卡尔曼滤波操作中使用的状态向量的分量之一)进行更新或重写，并且进行卡尔曼滤波操作。卡尔曼滤波器基于使用观测向量和状态向量计算出来的观测残差来执行更新状态向量的操作，从而使得状态向量接近于动态系统的实际值，所述状态向量的分量物理量 (状态变量)代表了某一时刻下的动态系统状态，所述观测向量的分量是来自测量各种不同种类物理量的多个传感器的输出值。具体地说，卡尔曼滤波器使用代表动态系统状态的时基变化的状态转移模型，根据某一时刻的状态向量来估计经过单位时间之后的状态向量。随后，卡尔曼滤波器使用根据动态系统状态的估计值用于估计传感器的输出值的观测模型，根据估计的状态向量来估计观测向量。此外，卡尔曼滤波器计算估计的观测向量与以来自各种不同种类的传感器的实际输出(实际观测值)为其分量的观测向量之间的差值，以计算观测残差。而且，卡尔曼滤波器使用观测残差更新状态向量。同时，安装了三维磁性传感器的仪器常常包括产生磁场的机械或电子部件，比如可以被磁化的各种类型的金属和电路。在这种情况下，三维磁性传感器输出的向量数据除了代表目标地磁的向量之外，还包括代表由仪器中安装的部件产生的磁场的向量。因此，为了正确检测地磁的值，必须进行修正处理，以从三维磁性传感器输出的向量数据中除去代表仪器的部件所产生的内部磁场的向量。在修正处理中为了获得所要检测的地磁的正确值而从三维磁性传感器输出的数据中除掉了所述内部磁场，代表该内部磁场的向量被称为三维磁性传感器的偏移量。在仪器的内部状态没有变化的情况下，内部磁场具有恒定不变的方向和幅度。另一方面，在仪器的内部状态变化的情况下，内部磁场的方向和幅度都发生变化。取决于仪器的用户操作、仪器的外部环境等，仪器的内部状态可能会突然发生很大变化。结果，仪器部件产生的内部磁场(即，三维磁性传感器的偏移量)也会随着仪器的内部状态的变化而突然发生很大变化。如前面所介绍的，卡尔曼滤波操作包括使用代表动态系统状态的时基变化的状态转移模型根据某一时刻的状态向量的值来估计经过单位时间之后的状态向量的值的操作。因此，由卡尔曼滤波器估计的状态变量最好是能够在状态转移模型中定式化(formulized)的物理量。不过，由于三维磁性传感器的偏移量会因用户对仪器的操作而突然急剧变化，因此很难确定偏移量值的变化。而且，用于更新状态向量的观测残差是基于更新之前状态向量的值来计算的。gp，通过卡尔曼滤波操作计算出来的更新之后的状态向量的值受到更新之前的在先状态向量的值的影响。此外，更新之前的在先状态向量的值受到再之前的更新之前的过去状态向量的值的影响。因此，在卡尔曼滤波操作重复进行的情况下，由卡尔曼滤波器估计的状态向量的值是考虑了从卡尔曼滤波操作开始时到估计出状态向量的值时这样的范围内的所有步骤中的状态向量的值的前提下计算出来的。因此，在三维磁性传感器的偏移量突然发生很大变化的情况下，用于估计三维磁性传感器的偏移量的更新之后的状态变量的值可能会大大背离于变化之后的三维磁性传感器的偏移量。在状态向量的值收敛于大大偏离正确表示实际物理量的值(实际值)的情况下，状态向量的值可能不会接近实际值，尽管随后会重复进行卡尔曼滤波操作。而且，即使状态向量的值接近实际值，也要花费很长时间，状态向量的值才会收敛于实际值。在这种情况下，状态估计设备不能正确估计系统状态，并且因此，不能基于由状态估计设备计算出来的状态变量来计算地磁的方向和仪器的姿态。地磁是具有指向地球的北磁极的水平分量和垂直于磁倾角方向的分量的一种磁场。地磁是相对于地面具有恒定不变的方向和恒定不变的幅度的均匀磁场。因此，在仪器的姿态相对于地面发生变化的情况下，从仪器看去的地磁方向也会变化。即，当从仪器中安装的三维磁性传感器角度看去时，地磁被表示为方向随着仪器姿态的改变而改变而幅度恒定不变的向量。另一方面，在仪器的内部状态不变的情况下，内部磁场具有相对于仪器恒定不变的方向和恒定不变的幅度。因此，在仪器的内部状态不变的情况下，即使在从仪器中安装的三维磁性传感器看去时仪器的姿态发生了改变的情况下，内部磁场也被表示为方向恒定不变且幅度恒定不变的向量。结果，在改变三维磁性传感器的姿态的同时获取的多个磁测数据分布在中心由代表内部磁场的方向和幅度的向量的前端确定并且半径与地磁的幅度相对应的球面的附近。假设多个磁测数据所表示的坐标有可能分布在某一球面(第一球面)的附近，中心点计算单元计算第一球面的中心点的坐标，以计算内部磁场，即，三维磁性传感器的偏移量。即使在三维磁性传感器的偏移量突然大幅变化的情况下，中心点计算单元也可以使用多个后来获得的磁测数据来计算正确的偏移量。在按照本发明的状态估计设备中，使用中心点计算单元输出的中心点来对用于估计三维磁性传感器的偏移量的状态变量进行重写，该状态变量是构成状态向量的分量之
O因此，即使在三维磁性传感器的偏移量突然发生很大变化的情况下，中心点计算单元也可以计算出代表变化之后的偏移量的中心点的坐标，并且可以使用中心点的坐标来对用于估计三维磁性传感器的偏移量的状态变量进行重写。结果，可以防止用于估计三维磁性传感器的偏移量的状态变量长时间背离实际值(三维磁性传感器的偏移量)。即，按照本发明的状态估计设备正确估计系统状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。在实际状态下，卡尔曼滤波器被构成为用来使用状态转移模型(f)根据某一时刻的状态向量(Xlri)来估计经过单位时间之后的状态向量(x_k)，构成为用来使用观测模型(h)根据经过单位时间之后的估计的状态向量(x-k)来计算估计的观测向量(y-k)，构成为用来基于估计的观测向量(y_k)和具有代表多个传感器的输出的分量的观测向量(yk)来计算观测残差(Zk)，并且构成为用来基于观测残差(Zk)和经过单位时间之后的估计的状态向量(x_k)来更新状态向量(Xk)。状态估计设备此外还包括分布判定单元，其构成为用来计算表示预定数量(N)的累积磁测数据的坐标分布的三维分散度的分散评估值，从而使得中心点计算单元构成为用来在该分散评估值等于或大于可容许的分散值(Xtj)的情况下计算表示第一球面(S)的中心点(Cd的坐标；变形判定单元，其构成为用来假设多个累积磁测数据所表示的坐标有可能分布在通过将第二球面(S2)和曲面(SX)组合起来而获得的三维图形(SD)的表面附近，构成为用来基于所述多个累积磁测数据对表示三维图形的形状与第二球面的形状之间的差异程度的变形评估值(gD(E))进行计算，并且构成为用来判断该变形评估值是否等于或小于可容许的变形值(Stj);和中心点输出单元，其构成为用来在变形判定单元的判断结果是肯定结果的情况下输出表示第一球面的中心点的坐标。在这种情况下，卡尔曼滤波器被构成为用来在中心点输出单元输出表示第一球面的中心点的坐标的情况下，使用表示第一球面的中心点的坐标来对用于估计偏移量的状态变量进行更新。如前面所介绍的，中心点计算单元在假设由磁测数据表示的坐标分布在第一球面附近的前提下，计算第一球面的中心点的坐标。因此，在磁测数据具有平面分布而不具有三维扩展的情况下，不能正确确定第一球面的中心点的坐标。按照本发明，首先，分布判定单元计算表明磁测数据的三维扩展程度的分散评估值，并且在分散评估值等于或大于可允许的分散值的情况下，中心点计算单元计算中心点的坐标。因此，在磁测数据是二维分布的情况下，可以防止中心点计算单元计算不正确的中心点坐标。结果，可以防止修正处理使用不正确的偏移量而计算出不正确的地磁方向和幅度。而且，由于可以防止进行计算不正确中心点的坐标的操作处理，因此可以实现状态估计设备的低功耗。而且，按照本发明，变形判定单元基于变形评估值判断磁测数据的坐标在其表面附近的三维图形的形状与第二球面的形状之间的不同达到什么程度。在变形评估值大于阈值的情况下，三维图形具有不同于第二球面的形状的变形的形状。结果，在三维图形表面附近的由磁测数据表示的坐标并不能被认为分布在第一球面附近。由于中心点计算单元以磁测数据处于第一球面的附近为前提计算作为三维磁性传感器的偏移量的候选值的中心点坐标，因此表示在其附近没有磁测数据的第一球面的中心点的坐标的向量并不代表三维磁性传感器的偏移量。由于按照本发明的状态估计设备包括变形判定单元，可以防止使用表示不能被当作三维磁性传感器的偏移量的不正确中心点坐标的向量来对用于评估三维磁性传感器偏移量的状态变量进行重写。如上所述，按照本发明的状态估计设备包括分布判定单元和变形判定单元。因此，仅仅在获得了接近于实际值的可以被当作三维磁性传感器的偏移量的中心点坐标的情况下，状态估计设备才使用表示接近于实际值的中心点坐标的向量来对用于估计三维磁性传感器的偏移量的状态变量进行重写。结果，按照本发明的状态估计设备可以正确估计系统状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。而且，作为本发明的具体实施方式
，状态估计设备可以包括这样的单元在变形判定单元的判断结果为否定结果的情况下，督促用户以不改变仪器位置的方式改变仪器的姿态。按照本发明，在变形判定单元判定变形评估值大于可允许的变形值的情况下，SP，在三维磁性传感器输出的磁测数据表示的坐标分布在三维图形表面附近的时候，三维图形的形状与球面间具有很大变形的情况下，状态估计设备包括用于督促用户以不改变仪器位置的方式改变仪器姿态的单元。除了要测量的地磁之外，三维磁性传感器还可以检测仪器的机械或电子部件产生的内部磁场和仪器外部的物体产生的不均匀外部磁场。在非均匀外部磁场的影响很大的情况下，磁测数据表示的坐标分布在具有大不同于球面的变形形状的三维图形表面附近，从而不能采用表示由中心点计算单元计算出来的中心点的坐标的向量作为三维磁性传感器的偏移量。 不过，在生成外部磁场的仪器外部物体和三维磁性传感器之间的相对位置关系不变的情况下非均匀外部磁场仅仅是具有恒定不变幅度的磁场。即，在三维磁性传感器的姿态在三维磁性传感器的位置固定的状态下发生改变的情况下，外部磁场被三维磁性传感器检测为均匀磁场，其幅度是恒定不变的并且只有方向发生变化。在这种情况下，由三维磁性传感器输出的磁测数据表示的坐标分布在某一球面的附近。而且，这个球面的中心点的坐标与表不仪器部件产生的磁场(内部磁场)分量的向量所表不的坐标几乎相同。如前面所介绍的，第一球面是在假设由磁测数据表示的坐标分布在第一球面附近的前提下被设置的。因此，在磁测数据表示的坐标分布得具有球面的形状的情况下，由磁测数据表示的坐标可以被视为分布在以中心点计算单元计算出来的中心点为中心的第一球面附近。结果，第一球面的中心点的坐标与表示内部磁场分量的向量所表示的坐标几乎相同，并且因此，可以采用表示第一球面的中心点的坐标的向量作为三维磁性传感器的偏移量。如上所述，状态估计设备包括用于督促用户以改变仪器位置的方式改变仪器姿态的单元，并且因此，即使在存在非均匀外部磁场的位置上，也可以督促用户进行计算适当偏移量的操作。而且，作为本发明的具体实施方式
，在状态估计设备中，可以将分散评估值设置为代表磁测数据表示的三轴坐标的分散度的方差-协方差矩阵(A)的最小特征值(λ 3)。按照本发明，代表磁测数据的分散度的方差-协方差矩阵的最小特征值被用作分散评估值。由于磁测数据表示三轴坐标，因此代表磁测数据分散度的方差-协方差矩阵是3X3的矩阵。从该方差-协方差矩阵获得三个特征向量和三个特征值。三个特征向量之一代表磁测数据最大分布的方向。与这一特征向量相对应的特征值变为最大的特征值。另一方面，与表示磁测数据最小分布的方向的特征向量相对应的特征值变为最小特征值。在磁测数据是理想的二维分布的情况下，最小特征值具有无限接近于零的值。在磁测数据是三维分布的情况下，按照磁测数据分布的三维分散度，最小特征值具有大值。结果，该方差-协方差矩阵的最小特征值被用作分散评估值，并且因此，可以正确掌握磁测数据分布的三维分散度。而且，按照本发明的具体实施方式
，在状态估计设备中，当针对各个磁测数据计算代表以第一球面的中心点为原点的坐标系中代表磁测数据的三轴向量(q1-cj与通过使用变形评估矩阵(E)(其是对称矩阵)转换三轴向量而获得的向量(E(qi_C()))的内积时，以计算结果为其分量的向量被设置为变形误差向量(k(E))，代表由磁测数据所表示的三轴坐标确定的位置与第二球面(S2)之间的误差的向量被设置为第二球面误差向量(sS2)，变形误差向量和第二球面误差向量的和被设置为固定误差向量(S SD)。当在变形评估矩阵(E)的分量(eil到e33)和表示第二球面的中心点(Cffi)的三轴坐标被设置为变量的同时，代表固定误差向量的幅度的函数被设置为变形评估函数(fSD(E，c))的时候，在变形评估函数最小时，变形评估值可以是变形评估矩阵的范数(norm)。按照本发明，固定误差向量的幅度被最小化，以计算变形评估矩阵和第二球面中心点的坐标。而且，变形评估值被计算为变形评估矩阵的范数。由于变形评估矩阵是用于转换三轴向量的坐标的对称矩阵，因此变形评估矩阵是3X3矩阵。结果，变形评估值具有三个特征值，并且具有最大绝对值的特征值(三个特征值之一)的绝对值成为变形评估值。固定误差向量被确定为第二球面误差向量和变形误差向量之和。第二球面误差向量仅仅是代表由磁测数据表示的坐标与第二球面之间的误差的向量。在第二球面被设置得使磁测数据表示的坐标与第二球面之间的误差最小的情况下，球面误差向量所表示的所有误差成为白噪声，白噪声是对称的并且不取决于方向。另一方面，变形误差向量是针对各个磁测数据，安排从第一球面的中心点看去的表示磁测数据所表示的坐标的向量与使用变形评估矩阵转换表示由磁测数据表示的坐标的向量而获得的向量的内积，而获得的向量。换句话说，变形误差向量的各个分量被配置成三变量二次型的形式，以变形评估矩阵(对称矩阵)为系数矩阵并且以代表从中心点看去的磁测数据的坐标的向量的三个分量中的每一个作为变量。即，变形误差向量是在由磁测数据表示的坐标与第二球面之间的误差存在于基于二次型中给出的相同函数的曲面上这一限制条件下，表示由磁测数据表示的坐标与第二球面之间的各个误差的向量。可以通过使用变形误差向量表示磁测数据和第二球面之间的误差，表示除了白噪声之外的基于二次函数的曲面所代表的噪声(即，来自球面的变形)。固定误差向量是通过将球面误差向量与表示多个磁测数据位于其表面附近的三维图形的变形误差向量相加而获得的。即，三维图形是通过将球面和曲面叠加来表示的，并且不同于球面。而且，固定误差向量中变形误差向量的幅度可以通过确定三维图形的形状不同于(变形于)第二球面的形状的程度(即，由于不同于白噪声的表面发生的变形造成的误差的幅度)来评估。结果，在评估出三维图形与第二球面彼此近似达到了三维图形与第二球面具有基本相同形状的程度的情况下，由磁测数据表示的坐标可以被认为也分布在第二球面的附近。如前面所介绍的，在磁测数据所表示的坐标分布得具有球面的形状的情况下，可以将磁测数据所表示的坐标看作也分布在第一球面的附近。因此，在评估出三维图形与第二球面彼此近似达到了三维图形与第二球面具有基本相同形状的程度的情况下，可以采用表不第一球面的中心点的坐标的向量作为三维磁性传感器的偏移量。另一方面，在评估出出三维图形具有大大不同于第二球面的形状的变形形状的情况下，磁测数据所表示的坐标不能被看作分布在第二球面的附近，并且因此，需要避免采用表示第一球面的中心点的坐标的向量作为三维磁性传感器的偏移量。即，状态估计设备可以在中心点计算单元计算出来的中心点的坐标当中，仅仅采用不受非均匀外部磁场影响的多个磁测数据计算出来的、表示适当中心点坐标的向量，作为三维磁性传感器的偏移量。结果，按照本发明的状态估计设备正确估计系统状态，并且因此，可以基于由状态估计设备计算出来的状态变量来计算地磁的正确方向和仪器的正确姿态。而且，在前面介绍的状态估计设备中，中心点计算单元被构成为用来假设由磁测数据表示的坐标所指定的位置有可能分布在第一球面的附近，构成为用来设置代表由磁测数据表示的坐标所指定的位置与第一球面之间的误差的第一球面误差向量(S s)，构成为用来设置包含三维向量(C)作为变量并且表示第一球面误差向量(Ss)的幅度的中心点计算函数(fs (c))，并且构成为用来计算表示第一球面的中心点的坐标，作为当中心点计算函数的值最小时由三维向量表示的坐标。·按照本发明，可以通过简单的计算过程计算出变为三维磁性传感器的偏移量的候选值的坐标。而且，按照本发明的具体实施方式
，在状态估计设备中，变形评估函数可以是由下列的fSD(E, c)表示。fSD (E, c) = Il δ SD K 2其中，N是等于或大于5的自然数，并且当由相应磁测数据表示的三轴坐标由qi(i=l，. . . , N)表示时，表示第一球面的中心点的三轴坐标由C0表示，这是一个三维向量，将变形评估矩阵设置为由下列的E表示的3X3矩阵，由下列k(E)表示的N维向量表示变形误差向量，c被设置为以代表第二球面的中心点的三个变量为其分量的三维向量，X被设置为下列的NX 3矩阵，q。被设置为下列的三维向量，R被设置为下列的值，j被设置为下列的N维向量，并且第二球面误差向量由下列δ S2所表示的N维向量表示，固定误差向量是由下列Ssd表示的N维向量。δ SD = δ S2+k (E)
e12 e13
e22 e23e23 e33 k(E]
￡m mmmm f * Mt I #> jf9 IMN cO )cO J按照本发明，可以通过简单的计算过程来计算变形评估值，该变形评估值是变形评估函数最小时变形评估矩阵的范数。结果，前面介绍的状态估计设备具有可以提高处理速度和实现低功耗的优点。而且，按照本发明的具体实施方式
，在状态估计设备中，可以由下列的fs (C)表示中心点计算函数。fs(c) = Il δ s (I 2其中，N是等于或大于5的自然数，并且当由相应磁测数据表示的三轴坐标由qi(i=l，..，N)表示时，c被设置为以用于代表表示第一球面的中心点的三轴坐标的三个变量作为其分量的三维向量，X被设置为下列的NX 3矩阵，q。被设置为下列的三维向量，R被设置为下列的值，并且j被设置为下列的N维向量，第一球面误差向量是由下列的\表示的N维向量，
权利要求
1.一种状态估计设备，包括 多个传感器，其中包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的各磁分量的磁测数据； 卡尔曼滤波器，其构成为用来基于观测残差更新状态向量，该状态向量具有代表多个状态变量的多个分量，所述多个状态变量包括用于估计磁测数据的偏移量的状态变量，所述观测残差是使用所述状态向量和具有代表所述多个传感器的输出的多个分量的观测向量计算出来的； 累积单元，其构成为用来累积从三维磁性传感器中相继输出的磁测数据；和 中心点计算单元，其构成为用来假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面附近，并且构成为用来计算表示该第一球面的中心点的坐标，其中 卡尔曼滤波器使用表示第一球面的中心点的坐标来对用于估计磁测数据的偏移量的状态变量进行更新。
2.按照权利要求1所述的状态估计设备，其中，卡尔曼滤波器构成为使用状态转移模型根据某一时刻的状态向量来估计经过单位时间之后的状态向量，卡尔曼滤波器构成为使用观测模型根据估计出的经过单位时间之后的状态向量来计算估计的观测向量，卡尔曼滤波器构成为基于估计出的观测向量和具有代表所述多个传感器的输出的多个分量的观测向量来计算观测残差，并且卡尔曼滤波器构成为用来基于观测残差和估计出的经过单位时间之后的状态向量来更新状态向量。
3.按照权利要求1所述的状态估计设备，还包括 分布判定单元，其构成为用来计算表示预定数量的累积磁测数据的坐标分布的三维分散度的分散评估值，从而使得中心点计算单元构成为用来在该分散评估值等于或大于可容许的分散值的情况下计算表示第一球面的中心点的坐标； 变形判定单元，该变形判定单元构成为用来假设多个累积磁测数据表示的坐标有可能分布在通过将第二球面和曲面组合起来而获得的三维图形的表面附近，该变形判定单元构成为用来基于所述多个累积磁测数据对表示所述三维图形的形状与第二球面的形状之间的差异程度的变形评估值进行计算，并且该变形判定单元构成为用来判断该变形评估值是否等于或小于可容许的变形值；和 中心点输出单元，其构成为用来在变形判定单元的判定结果是肯定结果的情况下输出表示第一球面的中心点的坐标。
4.按照权利要求3所述的状态估计设备，其中卡尔曼滤波器构成为用来在中心点输出单元输出表示第一球面的中心点的坐标的情况下，使用表示第一球面的中心点的坐标来对用于估计偏移量的状态变量进行更新。
5.按照权利要求1、2、3或4所述的状态估计设备，其中中心点计算单元构成为用来假设由磁测数据表示的坐标所指定的位置有可能分布在第一球面的附近，中心点计算单元构成为用来设置表示了由磁测数据表示的坐标所指定的位置与第一球面之间的误差的第一球面误差向量，中心点计算单元构成为用来设置包含三维向量作为变量并且表示第一球面误差向量的幅度的中心点计算函数，并且中心点计算单元构成为用来计算表示第一球面的中心点的坐标,该坐标作为当中心点计算函数的值最小时由三维向量表示的坐标。
6.按照权利要求1、2、3、4或5所述的状态估计设备，其中在包括产生磁场的部件的仪器中安装所述三维磁性传感器。
7.按照权利要求6所述的状态估计设备，其中所述偏移量是代表由该仪器的部件所产生的磁场的分量的三维向量。
8.—种在状态估计设备中进行的偏移量更新方法，该状态估计设备包括多个传感器，所述多个传感器包括三维磁性传感器，该三维磁性传感器构成为用来检测三个方向上的磁分量并且构成为用来相继输出代表所检测到的各磁分量的磁测数据，该方法包括 基于观测残差更新状态向量，该状态向量具有代表多个状态变量的多个分量，所述多个状态变量包括用于估计磁测数据的偏移量的状态变量，所述观测残差是使用所述状态向量和具有代表所述多个传感器的输出的多个分量的观测向量计算出来的； 累积从三维磁性传感器相继输出的磁测数据； 假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面的附近； 计算表示第一球面的中心点的坐标；和 使用表示第一球面的中心点的坐标来对用于估计磁测数据的偏移量的状态变量进行更新。
全文摘要
状态估计设备具有包括用于检测磁分量的三维磁性传感器的多个传感器。卡尔曼滤波器用于基于观测残差更新状态向量。状态向量具有代表多个状态变量的分量，多个状态变量包括用于估计磁测数据的偏移量的状态变量。中心点计算单元假设由预定数量的累积磁测数据表示的坐标有可能分布在第一球面附近，并且计算第一球面的中心点。卡尔曼滤波器使用第一球面的中心点来对用于估计磁测数据的偏移量的状态变量进行更新。
文档编号G01C21/12GK103017763SQ20121035361
公开日2013年4月3日 申请日期2012年9月20日 优先权日2011年9月20日
发明者半田伊吹 申请人:雅马哈株式会社