CN103791905B - 姿态估计方法和装置 - Google Patents

Abstract

本发明提供一种姿态估计方法和装置。从设备在第一时刻的姿态的第一估计值导出设备在第二时刻的姿态的第二估计值。为此，通过将第一估计值应用于表示设备姿态的变换的状态变换模型来产生第二时刻的姿态的预测值。随后，根据从设备中的传感器输出的数据来计算设备姿态的预测值与设备姿态的真实值之差，从而产生设备的预测姿态与真实姿态之间的估计姿态误差。而且，从估计姿态误差的多个姿态分量中提取特定姿态分量。最后，根据姿态的预测值和特定姿态分量来计算第二估计值。

Description

姿态估计方法和装置

技术领域

本发明涉及姿态估计方法和装置。

背景技术

在对诸如对象姿态之类的动态系统状态进行估计的情况下，通过对诸如地磁传感器、加速度传感器和角速度传感器之类的用于测量不同类型的物理量的多个传感器的输出结果进行综合和计算，相对于通过基于一类传感器的输出结果进行计算，可以在更短的时间段内估计出正确值。

一种利用卡尔曼（Kalman）滤波器的方法是公知的一种对用于测量不同类型的物理量的多个传感器的输出进行综合以估计动态系统的状态的方法。例如，非专利文献1公开了一种姿态估计装置，其利用sigma点卡尔曼滤波器（一种非线性卡尔曼滤波器）对来自三轴地磁传感器、三轴角速度传感器和三轴加速度传感器的输出进行综合，以对姿态进行估计。

[非专利文献1]J.L.Crassidis和F.L.Markley,"UnscentedFiltering for Spacecraft Attitude Estimation,"Journal of Guidance,Control and Dynamics，26(2003),pp.536-542

诸如地磁传感器或加速度传感器之类的传感器可能检测到噪声分量而不是待检测的信号分量。例如，即使在要检测重力加速度的情况下，如果加速度传感器振动，则该加速度传感器会检测到与振动有关的加速度分量而不是重力加速度。此外，如果地磁传感器的附近存在产生磁场的对象，则配置成检测地磁的地磁传感器将检测到磁分量而不是地磁。

在姿态估计装置中所采用的传感器检测到噪声分量而不是待检测的信号分量的情况下，姿态估计装置被噪声影响，由此姿态估计装置可能估计出与实际姿态偏离极大的姿态。

发明内容

鉴于上述问题而做出本发明，并且本发明的一个目的是提供一种姿态估计方法，即使在姿态估计装置中所使用的传感器检测到噪声分量而不是待检测的信号分量的情况下，所述姿态估计方法也能够防止姿态估计偏离实际姿态。

为了解决上述问题，本发明提供了一种对具有多个传感器的设备的姿态进行估计的方法，所述多个传感器包括加速度传感器，所述方法包括步骤：产生设备在第一时刻的姿态的第一估计值；根据从加速度传感器输出的加速度数据来确定设备的姿态是否稳定；通过将第一估计值应用于表示设备的姿态的变换的状态变换模型，来预测设备在从第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态的预测值；根据从多个传感器中所包含的至少一个传感器输出的数据来估计设备的姿态的预测值与姿态的真实值之差，从而产生设备的预测姿态与真实姿态之间的估计姿态误差；当设备的姿态被确定为稳定时，根据姿态的预测值和估计姿态误差来计算设备在第二时刻的姿态的第二估计值；当设备的姿态被确定为不稳定时，从估计姿态误差的多个姿态分量中提取特定姿态分量；以及当设备的姿态被确定为不稳定时，根据姿态的预测值和从估计姿态误差中提取出来的特定姿态分量，来计算设备在第二时刻的姿态的第二估计值。

在优选形式中，估计姿态误差的多个姿态分量包括表示相对于沿重力加速度的方向延伸的轴的倾斜的倾斜分量以及围绕该轴的旋转分量，并且特定姿态分量是通过从估计姿态误差中去除倾斜分量而得到的旋转分量。

在优选形式中，多个姿态分量是通过将设备的基准姿态改变所述估计姿态误差而得到的。

在优选形式中，设备的姿态由四元数表示。

在优选形式中，多个传感器还包括磁传感器和角速度传感器，并且所述方法还包括步骤:在设备的姿态被确定为稳定时，产生包括从磁传感器输出的磁数据以及从加速度传感器输出的加速度数据作为其元素的输入观测值;以及在设备的姿态被确定为不稳定时，产生包括从磁传感器输出的磁数据作为其元素而不包括从加速度传感器输出的加速度数据的输入观测值。然后，通过将第一时刻的第一估计值和从角速度传感器输出的角速度数据应用于状态变换模型来预测设备在第二时刻的姿态；并且通过以下步骤来产生估计姿态误差:将第二时刻的姿态的预测值应用于表示观测值的元素和姿态的值之间的关系的观测模型，以便对在第二时刻的观测值的元素进行估计，从而产生具有所估计的元素的估计观测值;计算估计观测值与输入观测值之差以产生表示所计算出的差的观测残差;并且根据观测残差以及设备的姿态的预测值来产生估计姿态误差。

在优选形式中，在从加速度传感器输出的加速度数据的幅值与重力加速度的幅值之差的绝对值不大于预定值的情况下，设备的姿态被确定为稳定。

本发明还提供了一种对具有传感器的设备的姿态进行估计的方法，所述方法包括步骤：产生设备在第一时刻的姿态的第一估计值；通过将第一估计值应用于表示设备的姿态的变换的状态变换模型，来预测所述设备在从第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态的预测值；根据从传感器输出的数据来估计设备的姿态的预测值与姿态的真实值之差，从而产生设备的预测姿态与真实姿态之间的估计姿态误差；从估计姿态误差的多个姿态分量中提取特定姿态分量；以及根据姿态的预测值和从估计姿态误差中提取出来的特定姿态分量，来计算设备在第二时刻的姿态的第二估计值。

在优选形式中，传感器包括用于输出表示地磁的磁数据的磁传感器。

在优选形式中，估计姿态误差的多个姿态分量包括表示相对于沿重力加速度的方向延伸的轴的倾斜的倾斜分量以及围绕该轴的旋转分量，并且特定姿态分量是通过从估计姿态误差中去除倾斜分量而得到的旋转分量。

在优选形式中，多个姿态分量是通过将设备的基准姿态改变所述估计姿态误差而得到的。

本发明还提供了一种用于对具有多个传感器的设备的姿态进行估计的装置，多个传感器包括加速度传感器，所述装置包括一个或多个处理器，所述一个或多个处理器被配置成用于：产生设备在第一时刻的姿态的第一估计值；根据从加速度传感器输出的加速度数据来确定设备的姿态是否稳定；通过将第一估计值应用于表示设备的姿态的变换的状态变换模型，来预测设备在从第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态的预测值；根据从多个传感器中包含的至少一个传感器输出的数据来估计设备的姿态的预测值与姿态的真实值之差，从而产生设备的预测姿态与真实姿态之间的估计姿态误差；当设备的姿态被确定为稳定时，根据姿态的预测值和估计姿态误差来计算设备在第二时刻的姿态的第二估计值；当设备的姿态被确定为不稳定时，从估计姿态误差的多个姿态分量中提取特定姿态分量；以及当设备的姿态被确定为不稳定时，根据姿态的预测值和从估计姿态误差中提取出来的特定姿态分量，来计算设备在第二时刻的姿态的第二估计值。

本发明还提供了一种用于对具有传感器的设备的姿态进行估计的装置，所述装置包括一个或多个处理器，所述一个或多个处理器被配置成用于：产生设备在第一时刻的姿态的第一估计值；通过将第一估计值应用于表示设备的姿态的变换的状态变换模型，来预测设备在从第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态的预测值；根据从传感器输出的数据来估计设备的姿态的预测值与姿态的真实值之差，从而产生设备的预测姿态与真实姿态之间的估计姿态误差；从估计姿态误差的多个姿态分量中提取特定姿态分量；以及根据姿态的预测值和从估计姿态误差中提取出来的特定姿态分量，来计算设备在第二时刻的姿态的第二估计值。

此外，本发明还提供了一种用于对姿态估计装置进行控制的计算机程序，姿态估计装置被布置在具有多个传感器的设备中，多个传感器包括三维加速度传感器，三维加速度传感器用于检测三个方向上的加速度并依次输出检测结果作为三轴坐标系中的矢量数据，计算机程序用于估计设备的姿态，其中所述程序使得计算机起到下述功能:判定单元，用于根据从三维加速度传感器输出的值来判断设备的运动是否稳定;以及卡尔曼滤波器单元，用于根据判定单元的判断结果以及以来自所述多个传感器中的至少一部分传感器的输出值作为元素的观测值矢量，从特定时刻的姿态的估计值中计算出从该特定时刻开始过去单位时间之后的时刻的姿态的估计值。所述程序使得卡尔曼滤波器单元起到下述功能:状态变换模型单元，用于利用表示设备的姿态基于时间的变化的状态变换模型，从特定时刻的姿态的估计值中估计出从该特定时刻开始过去单位时间之后的时刻的姿态;估计值校正单元，用于根据状态变换模型单元的预测结果和观测值矢量来产生估计姿态误差，估计姿态误差是通过对状态变换模型单元的预测结果与从该特定时刻开始过去单位时间之后的时刻的实际姿态之差进行估计而得到的值;以及姿态更新单元，用于在判定单元的判断结果为肯定的情况下，根据估计姿态误差和状态变换模型单元的预测结果来计算从该特定时刻开始过去单位时间之后的姿态的估计值，以及用于在判定单元的判断结果为否定的情况下，根据所提取的估计姿态误差和状态变换模型单元的预测结果来计算从该特定时刻开始过去单位时间之后的姿态的估计值，所提取的估计姿态误差是通过从估计姿态误差中提取以沿重力加速度的方向延伸的轴作为旋转轴的旋转分量而得到的。

附图说明

图1是示出了根据本发明实施例的便携设备的配置的框图。

图2是示出了根据本发明实施例的便携设备的外观的透视图。

图3是示出了根据本发明实施例的姿态估计单元的功能的框图。

图4是示出了根据本发明实施例的卡尔曼滤波器单元的功能的框图。

图5是示出了根据本发明实施例的姿态更新单元的功能的视图。

具体实施方式

<A.实施例>

将参考附图来描述本发明的实施例。

<1.设备的配置以及软件的配置>

图1是根据本发明实施例的便携设备的框图，图2是示出了该便携设备的外观的透视图。便携设备1具有用于响应于便携设备1的姿态而对显示在屏幕（显示单元50，将在下文予以描述）上的诸如地图之类的图像进行旋转以使得该图像所显示的方位角追随实际空间的方向角的功能。通过基于各种传感器的输出执行卡尔曼滤波计算以估计出便携设备1的姿态来实现该功能。

便携设备1包括：经由总线连接至各种类型的配置组件的用于控制整个姿态估计装置的CPU10，作为CPU10的工作区的RAM（累积单元）20，用于存储各种类型的程序（例如姿态估计程序100）和数据的ROM30，用于执行通信的通信单元40，用于显示图像的显示单元50，以及GPS单元60。

此外，便携设备1包括用于检测诸如地磁之类的磁以输出磁数据m的三维磁传感器70、用于检测加速度以输出加速度数据a的三维加速度传感器80、以及用于检测角速度以输出角速度数据ω的三维角速度传感器90。

显示单元50基于执行姿态估计程序100的CPU10所估计出来的便携设备1的姿态q的估计值来显示诸如指示定向的箭头之类的图像。

GPS单元60接收来自GPS卫星的信号以产生便携设备1的位置信息（经度和纬度）。

三维磁传感器70包括X轴磁传感器71、Y轴磁传感器72和Z轴磁传感器73。每个传感器可由磁阻抗装置（MI装置）、磁阻效应装置（MR装置）等配置而成。磁传感器I/F74将来自X轴磁传感器71、Y轴磁传感器72和Z轴磁传感器73的模拟输出信号转换成数字信号以输出磁数据m。更具体地说，在固定至便携设备1的坐标系中，磁数据m是表示作为x轴分量的来自X轴磁传感器71的输出值、作为y轴分量的来自Y轴磁传感器72的输出值、和作为z轴分量的来自Z轴磁传感器73的输出值的矢量数据。

同时，地磁B_g包括在三维磁传感器70检测到的磁数据m中。

一般地，地磁B_g是具有指向北磁极的水平分量和由磁倾角决定的垂直分量的磁场。在固定至地的坐标系中，将地磁B_g表示为具有统一方向和幅值的矢量^GB_g（同时，矢量符号左上部分附加的上标G表示该矢量是在固定至地的坐标系中表示的）。即，在固定至便携设备1的坐标系中，将地磁B_g表示为具有根据便携设备1的姿态q而变化的方向和统一幅值的矢量^SB_g（q）（同时，矢量符号左上部分附加的上标S表示该矢量是在固定至便携设备1的坐标系中表示的）。由此，可以通过计算表示固定至便携设备1的坐标系中的地磁B_g的矢量^SB_g（q）来得到便携设备1的姿态q。

但是，在诸如扬声器之类的产生磁场的对象以及诸如各种金属之类的呈现磁性的对象存在于便携设备1附近的情况下，便携设备1可能受到来自这些对象的磁场（噪声）的影响。在这种情况下，表示磁数据m的值是通过将存在于便携设备1附近的对象所产生的磁场叠加至地磁B_g而得到的值。即，当产生磁场的对象出现在便携设备1附近时，可能很难根据磁数据m正确地得到表示地磁B_g的方向（即，矢量^SB_g（q）的方向）。在这种情况下，不可能根据磁数据m正确地得到便携设备1的姿态q。

三维加速度传感器80包括X轴加速度传感器81、Y轴加速度传感器82和Z轴加速度传感器83。每个传感器可以是一个压阻式传感器、电容式传感器或检测型传感器，例如热检测型传感器。加速度传感器I/F84将来自各个传感器的模拟输出信号转换成数字信号以输出加速度数据a。加速度数据a是表示固定至便携设备1的坐标系中的惯性力和重力的合力的数据，三维加速度传感器80与便携设备1集成以使得三维加速度传感器80与便携设备1同步操作，加速度数据是这样的矢量，该矢量是包含具有x轴分量、y轴分量和z轴分量的矢量。

因此，当便携设备1处于静止状态或者沿直线匀速运动时，加速度数据a变成表示固定至便携设备1的坐标系中的重力加速度g的幅值和方向的矢量数据。

三维角速度传感器90包括X轴角速度传感器91、Y轴角速度传感器92和Z轴角速度传感器93。角速度传感器I/F94将来自各个传感器的模拟输出信号转换成数字信号以输出角速度数据ω。角速度数据ω是表示围绕沿着固定至便携设备1的坐标系中的三个方向延伸的各个轴的角速度的矢量数据。

CPU10执行存储在ROM30中的姿态估计程序100以估计便携设备1的姿态q。即，由于CPU10执行姿态估计程序100，所以便携设备1起到了姿态估计装置的作用。

图3是示出了姿态估计装置的功能（即，由执行姿态估计程序100的CPU10所实现的功能）的功能框图。如图3所示，姿态估计装置包括用于根据加速度数据a、磁数据m和角速度数据ω来估计便携设备1的姿态q的姿态估计单元200。姿态估计单元200包括判定单元220、观测值矢量产生单元240、以及卡尔曼滤波器单元260。

判定单元220根据加速度数据a判断便携设备1的运动是否稳定，即便携设备1中是否产生了大振动。

具体地说，判定单元220判断是否满足下述公式（1）所表达的条件，即，条件“加速度数据a所表示的矢量的幅值与重力加速度的幅值g之间的差值的绝对值小于预定阈值ε_G”。一旦判定满足该条件，则判定单元220确定便携设备1的运动稳定。一旦判定不满足该条件，则判定单元220确定便携设备1的运动不稳定（便携设备1在振动）。

同时，变量、矢量和矩阵的右下部分附加的下标k表示变量、矢量和矩阵在时刻T=k时的值。下文中，表示在时刻T=k时的加速度数据a的三维矢量被称为加速度矢量a_k，表示在时刻T=k时的磁数据m的三维矢量被称为磁矢量m_k，并且表示在时刻T=k时的角速度数据ω的三维矢量被称为角速度矢量ω_k。

|||a_k||-g|<ε_G......公式（1）

观测值矢量产生单元240根据判定单元220的判断结果产生以加速度数据a的各个分量和磁数据m的各个分量中的至少一个分量作为元素的观测值矢量y，并将所产生的观测值矢量输出至卡尔曼滤波器单元260。在一些情况下，观测值矢量可简单地被称为“观测值”。

具体地说，在判定单元220的判断结果是肯定的情况下（即，在判定便携设备1的运动稳定的情况下），观测值矢量产生单元240产生观测值矢量y_k，该观测值矢量y_k是由下述公式（2）表示的以加速度矢量a_k和磁矢量m_k作为元素的六维矢量。

另一方面，在判定单元220的判断结果是否定的情况下（即，在判定便携设备1在振动的情况下），观测值矢量产生单元240产生观测值矢量y_k，该观测值矢量y_k是由下述公式（3）表示的以磁矢量m_k作为元素的三维矢量。

$y_k=\left[\begin{array}{c}{m}_k\\ a_k\end{array}\right]$ ......公式（2）

y_k=[m_k]......公式（3）

在本实施例中，观测值矢量产生单元240包括布置在三维加速度传感器80和卡尔曼滤波器单元260之间的开关SW。

开关SW在判定单元220的判断结果是肯定的情况下接通。在这种情况下，观测值矢量产生单元240将三维加速度传感器80所输出的加速度矢量a和三维磁传感器70所输出的磁矢量m都传输给卡尔曼滤波器单元260。

另一方面，开关SW在判定单元220的判断结果是否定的情况下断开。在这种情况下，观测值矢量产生单元240仅仅将三维磁传感器70所输出的磁矢量m传输给卡尔曼滤波器单元260。

卡尔曼滤波器单元260根据三维角速度传感器90所输出的角速度数据ω和观测值矢量y来执行卡尔曼滤波计算以估计便携设备1的姿态q。

如上所述，观测值矢量y在判定单元220的判断结果是肯定的情况下是六维矢量，在判定单元220的判断结果是否定的情况下是三维矢量。为此，根据本实施例的卡尔曼滤波器单元260根据判定单元220的判断结果在考虑观测值矢量y的维数的同时估计便携设备1的姿态q。

下文中将详细描述卡尔曼滤波器单元260的操作。

<2.卡尔曼滤波计算>

一般地，卡尔曼滤波器利用表示系统状态基于时间的变化或变换的作为逻辑模型的状态变换模型来预测动态系统在预定时间段（即，系统在特定时刻（在时刻T=k-1时）的状态开始的单位时间）过去之后（在时刻T=k时）的状态。

此外，卡尔曼滤波器利用如下的观测模型（其为逻辑模型）从系统状态的预测值估计出观测值矢量y_k，该观测模型表示利用状态变换模型预测出来的系统状态的预测值与观测值矢量y_k之间的关系。下文中，观测值矢量y_k的估计值将被称为估计观测值矢量y^- _k。

接下来，卡尔曼滤波器计算观测残差e_k，观测残差e_k是估计观测值矢量y^- _k与作为实际观测值的观测值矢量y_k之差。

此外，卡尔曼滤波器根据观测残差e_k来对利用状态变换模型预测出的系统状态的预测值进行校正（更新），以计算系统在从特定时刻开始过去单位时间之后的状态的估计值。

在本实施例中，便携设备1的姿态q包含在卡尔曼滤波器（卡尔曼滤波器单元260）估计出来的系统状态中。即，卡尔曼滤波器单元260根据作为便携设备1在时刻T=k-1（第一时刻）时的姿态q的估计值的估计姿态q⁺ _k-1，来计算（估计）估计姿态q⁺ _k，估计姿态q⁺ _k是便携设备1在时刻T=k（第二时刻）时的姿态q的估计值。

在本实施例中，姿态q由四元数表示。该四元数是表示对象的姿态（旋转状态）的四维数。例如，固定至便携设备1的坐标系的各个轴与固定至地的坐标系的各个轴一致时的姿态被定义为基准姿态，并且基准姿态被表示为q=[0，0，0，1]^T。这时，便携设备1的任意姿态q可表示为在利用沿三维单位矢量ρ的方向延伸的轴作为相对于基准姿态的旋转轴将便携设备1旋转角度ψ时的姿态。即，利用四元数由下述公式（4）表示姿态q。

$q=\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{c}\mathrm{\&rho;}\sin \left(\frac{\mathrm{\&psi;}}{2}\right)\\ \cos \left(\frac{\mathrm{\&psi;}}{2}\right)\end{array}\right]$ ......公式（4）

此外，在本实施例中，估计姿态误差δq⁺ _k包含在卡尔曼滤波器（卡尔曼滤波器单元260）估计出的系统状态中，估计姿态误差δq⁺ _k是对姿态预测值q^- _k（利用状态变换模型得到的在单位时间过去之后（在时刻T=k时）姿态q的预测值）与单位时间过去之后的实际姿态q之差（下文中也简称为与实际姿态之差）进行估计的值。在本实施例中，利用四元数来表示估计姿态误差δq⁺ _k。

在本实施例中，利用非线性函数f通过下述公式（5）来表示卡尔曼滤波器的状态变换模型。公式（5）中出现的状态矢量x_k是利用修正的Rodrigues参数（MRP）表示作为姿态变化量的估计姿态误差δq⁺ _k的三维矢量。将在下文中描述的更新之后的状态矢量x⁺ _k与估计姿态误差δq⁺ _k一致（严格地说，仅仅表达类型不同）。此外，公式（5）中出现的过程噪声w_k是作为以0为中心的高斯噪声的三维矢量。同时，过程噪声w_k的协方差矩阵被表示为Q_k。协方差矩阵Q_k是3×3的方矩阵。

如公式(5)所表示，状态变换模型根据时刻T=k-1时的角速度矢量ω_k-1来预测单位时间过去之后的时刻T=k时的状态矢量x_k。一般地，时刻T=k-1被称为“第一时刻”，而时刻T=k被称为“第二时刻”。下文中，利用状态变换模型预测出的状态矢量x_k将被称为预测状态矢量x^- _k。

x_k=f(ω_k-1)+w_k-1......公式(5)

虽然状态变换模型被表达为用于预测状态矢量x_k的方式，状态矢量x_k是如公式(5)所示的姿态的变化量，然而，状态变换模型可更具体地表示为如下面的公式(6)所示的用于预测姿态q的基于时间的变化的方式。

公式(6)右侧出现的运算符Ω由公式(7)定义。此外，公式(7)的I_3×3表示3×3的单位矩阵。公式(7)的运算符Γ由公式(8)利用三维矢量l=(l₁，l₂，l₃)来定义。Δt是单位时间(从时刻T=k-1至时刻T=k的时间)。公式(7)的量Ψ_k由公式(9)定义。

如公式(6)所表示，状态变换模型根据时刻T=k-1时的姿态q_k-1以及时刻T=k-1时的角速度矢量ω_k-1来预测单位时间过去之后的时刻T=k时的姿态q_k。如上所述，利用状态变换模型预测的姿态q_k是姿态预测值q^- _k。

q_k=Ω(ω_k-1)q_k-1......公式(6)

$\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{\&Gamma;}\left[{\mathrm{\&Psi;}}_k\right]& {\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]$ ......公式(7)

$\mathrm{\&Gamma;}\left[l\right]=\left[\begin{array}{ccc}0& -l_3& l_2\\ l_3& 0& -l_1\\ -l_2& l_1& 0\end{array}\right]$ ......公式(8)

${\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|}$ ......公式(9)

在本实施例中，卡尔曼滤波器的观测模型由下述公式(10)利用非线性函数h来表示。公式(10)中出现的观测噪声v_k是以0为中心的高斯噪声。同时，观测噪声v_k的协方差矩阵被表示为R_k。

如上所述，观测值矢量y_k在判定单元220的判断结果是肯定的情况下是以加速度矢量a_k和磁矢量m_k作为元素的六维矢量，在判定单元220的判断结果是否定的情况下是以磁矢量m_k作为元素的三维矢量。因此，在判定单元220的判断结果是肯定的情况下，观测噪声v_k是表示三维磁传感器70的噪声和三维加速度传感器80的噪声的六维矢量，并且协方差矩阵R_k是6×6的方矩阵。另一方面，在判定单元220的判断结果是否定的情况下，观测噪声v_k是表示三维磁传感器70的噪声的三维矢量，并且协方差矩阵R_k是3×3的方矩阵。

如公式(10)所表示，观测模型利用时刻T=k时的姿态q_k来估计时刻T=k时的观测值矢量y_k。如上所述，利用观测模型估计出的观测值矢量被称为估计观测值矢量y^- _k或被简单地称为“估计观测值”。

同时，非线性函数h的详情将在下文中详细描述。

y_k=h(q_k)+v_k......公式(10)

时刻T=k时的观测残差e_k是表示输入的实际观测值矢量y_k与估计观测值矢量y^- _k之差的矢量，并且由下述公式(11)表示。如从公式(10)和公式(11)可以看出的那样，观测残差e_k在判定单元220的判断结果是肯定的情况下是六维矢量，而在判定单元220的判断结果是否定的情况下是三维矢量。

如下述公式(12)所示，卡尔曼滤波器利用观测残差e_k和公式(13)所表示的卡尔曼增益K_k将状态矢量x_k从预测状态矢量x^- _k更新为状态矢量x⁺ _k。此外，卡尔曼滤波器按照下述公式(14)所示来更新状态矢量x_k的估计误差的协方差矩阵P_k。此处，P^- _k是预测状态矢量x^- _k的估计误差的协方差矩阵，P⁺ _k是更新后的状态矢量x⁺ _k的估计误差的协方差矩阵。此外，P^yy _k是观测残差e_k的方差矩阵，P^xy _k是预测状态矢量x^- _k和估计观测值矢量y^- _k的互协方差矩阵。

$e_k=y_k-y_k^{-}$ ......公式(11)

$x_k^{+}=x_k^{-}+K_k{e}_k$ ......公式(12)

$K_k=P_k^{\mathrm{xy}}{\left(P_k^{\mathrm{yy}}\right)}^{-1}$ ......公式(13)

$P_k^{+}=P_k^{-}-K_k{P}_k^{\mathrm{yy}}{K}_k^T$ ......公式(14)

图4示出了卡尔曼滤波器单元260的功能框图。卡尔曼滤波器单元260通过利用无迹变换（unscented transformation）的sigma点卡尔曼滤波器来执行公式(6)至公式(14)所表示的非线性卡尔曼滤波计算。

如图4所示，初始化单元261将状态矢量x⁺ _k-1的每个元素设置为0，并随后将其输出，状态矢量x⁺ _k-1是通过将加法器272所输出的更新后的状态矢量x⁺ _k延迟单位时间而得到的。即，初始化单元261实质上产生并输出零矢量O₃=[0，0，0]^T。

此外，初始化单元261将更新后的状态矢量x⁺ _k的估计误差的协方差矩阵P⁺ _k延迟单位时间，以产生并输出状态矢量x⁺ _k-1的估计误差的协方差矩阵P⁺ _k-1。

Sigma点产生单元262根据协方差矩阵P⁺ _k-1和零矢量O₃（即状态矢量x⁺ _k-1，其每个元素被设置为0）产生2dim(x)+1个sigma点χ_k-1(i)（其中i=1，2，3，…，2dim(x)）。此处，每个sigma点χ_k-1(i)都是三维矢量，sigma点χ_k-1(0)是状态矢量x⁺ _k-1，其每个元素被设置0（即，零矢量O₃）。此外，dim(x)是表示状态矢量x_k的维数的整数，即3。即，sigma点产生单元262产生七(7)个sigma点。可利用已知方法适当地产生sigma点，例如非专利文献1公开的方法。

此外，sigma点产生单元262将2dim(x)+1个sigma点χ_k-1(i)（其为三维矢量）中的每一个的表达类型转换为四元数，从而将2dim(x)+1个sigma点χ_k-1(i)转换为2dim(x)+1个sigma点δq⁺ _χ,k-1(i)。可利用已知方法（例如非专利文献1中公开的方法）适当地执行将sigma点χ_k-1(i)转换为sigma点δq⁺ _χ,k-1(i)的操作，即将姿态的表达类型从MRP转换为四元数的操作。

从sigma点χ_k-1(0)转换的sigma点δq⁺ _χ,k-1(0)表示基准姿态[0，0，0，1]^T。此外，每个sigma点δq⁺ _χ,k-1(i)都表示了相对于基准姿态的姿态变化量。

延迟单元276输出通过将姿态更新单元275所输出的在时刻T=k时的估计姿态q⁺ _k延迟单位时间而得到的估计姿态q⁺ _k-1。延迟单元276所延迟的在时刻T=k-1时的估计姿态q⁺ _k-1的值被称为“第一估计值”。

姿态计算单元263按照下述公式(15)所示根据估计姿态q⁺ _k-1和sigma点δq⁺ _χ,k-1(i)来产生与2dim(x)+1个sigma点δq⁺ _χ,k-1(i)相对应的2dim(x)+1个姿态q⁺ _χ,k-1(i)。

在此，公式(15)中出现的运算符[X]指的是四元数乘积。从公式(4)还可以看出，四元数指示了除了该姿态之外的姿态的变化量(围绕任意轴的旋转幅度)。此外，四元数乘积是把一个四元数看作姿态而把另一个四元数看作姿态的变化量以通过该另一个四元数所表示的姿态的变化量来改变由该一个四元数表示的姿态的已知运算。

如上所述，sigma点δq⁺ _χ,k-1(i)表示相对于基准姿态的姿态变化量。由此，通过使估计姿态q⁺ _k-1改变sigma点δq⁺ _χ,k-1(i)所表示的姿态变化量来得到公式(15)的姿态q⁺ _χ,k-1(i)。

表示基准姿态的sigma点δq⁺ _χ,k-1(0)表示姿态没有改变(姿态的变化量为零)。因此，如公式(16)所示，通过借助sigma点δq⁺ _χ,k-1(0)改变估计姿态q⁺ _k-1而得到的姿态q⁺ _χ,k-1(0)等于估计姿态q⁺ _k-1。

$q_{\mathrm{\&chi;},k-1}^{+}\left(i\right)=\mathrm{\&delta;}{q}_{\mathrm{\&chi;},k-1}^{+}\left(i\right)[\&times;]q_{k-1}^{+},(i=0,...,2\dim \left(x\right))$ ......公式(15)

$q_{\mathrm{\&chi;},k-1}^{+}\left(0\right)=q_{k-1}^{+}$ ......公式(16)

状态变换模型计算单元264(下文中，也简称为“状态变换模型单元”)将姿态计算单元263所产生的2dim(x)+1个姿态q⁺ _χ,k-1(i)和角速度矢量ω_k-1应用于公式(6)所表示的状态变换模型，以产生2dim(x)+1个姿态预测值q^- _χ,k(i)。

同时，将姿态q⁺ _χ,k-1(0)应用于公式(6)而得到的姿态预测值q^- _{χ， k}(0)等于将延迟单元276所输出的估计姿态q⁺ _k-1应用于公式(6)而得到的姿态预测值q^- _k。

差分姿态计算单元265在姿态从姿态预测值q^- _χ,k(0)所表示的姿态变成姿态预测值q^- _χ,k(i)所表示的姿态时，根据状态变换模型计算单元264所产生的姿态预测值q^- _χ,k(i)以及姿态预测值q^- _χ,k(0)来计算姿态的变化量。可利用诸如非专利文献1中公开的方法之类的已知方法来适当地计算姿态的变化量。由此，差分姿态计算单元265将2dim(x)+1个姿态预测值q^- _χ,k(i)转换成2dim(x)+1个sigma点δq^- _χ,k(i)，它们是姿态相对于姿态预测值q^- _χ,k(0)的变化量。

此外，差分姿态计算单元265将按照四元数表示的2dim(x)+1个sigma点δq^- _χ,k(i)中的每一个的表达类型转换成MRP，以产生2dim(x)+1个sigma点χ_k(i)。可利用诸如非专利文献1中公开的方法之类的已知方法来适当地执行姿态的表达类型从四元数至MRP的转换。

预测状态矢量产生单元266产生预测状态矢量x^- _k，其是差分姿态计算单元265所产生的2dim(x)+1个sigma点χ_k(i)的加权平均。可利用已知方法来适当地计算加权平均。

协方差产生单元267根据预测状态矢量x^- _k和sigma点χ_k(i)来产生预测状态矢量x^- _k的估计误差的协方差矩阵P^- _k。可利用已知方法来适当地执行该计算。

观测模型计算单元268将状态变换模型计算单元264所产生的2dim(x)+1个姿态预测值q^- _χ,k(i)应用于观测模型，以产生下述公式(17)所表示的2dim(x)+1个估计观测值γ_k(i)。

${\mathrm{\&gamma;}}_k\left(i\right)=h\left(q_{\mathrm{\&chi;},k}^{-}\left(i\right)\right),(i=0,...,2\dim \left(x\right))$ ......公式(17)

下文中，将详细描述观测模型中使用的非线性函数h。

在判定单元220的判断结果是肯定的情况下，非线性函数h由下述公式(18)表示。另一方面，在判定单元220的判断结果是否定的情况下，非线性函数h由下述公式(19)表示。

此处，γ_m是磁矢量m_k的估计值，γ_m由下述公式(20)表示。公式(20)中出现的矢量^GB_g是指示了地磁B_g在固定至地的坐标系中的方向和幅值的矢量，由下述公式(21)表示。公式(21)中出现的值r是指示了地磁B_g的强度的值，公式(21)中出现的值Φ是指示了地磁B_g的磁倾角的值。此外，公式(20)中出现的矩阵B(q)是在便携设备1处于姿态q的情况下执行从固定至地的坐标系中表示的矢量转换至固定至便携设备1的坐标系中表示的矢量的坐标转换的矩阵，B(q)由下述公式(22)表示。

γ_a是加速度矢量a_k的估计值，其由下述公式(23)表示。公式(23)中出现的矢量^GG_RV是通过将表示重力加速度的方向和幅值的矢量归一化成固定至地的坐标系中的重力加速度的幅值g而得到的三维矢量。

$h\left(q_k\right)=\left[\begin{array}{c}{\mathrm{\&gamma;}}_m\\ {\mathrm{\&gamma;}}_a\end{array}\right]$ ......公式(18)

h(q_k)=[γ_m]......公式(19)

γ_m=B(q_k)^GB_g......公式(20)

$B_g^G=\left[\begin{array}{c}0\\ r\cos \mathrm{\&Phi;}\\ -r\sin \mathrm{\&Phi;}\end{array}\right]$ ......公式(21)

$B\left(q\right)=\left[\begin{array}{ccc}{q_4}^2+{q_1}^2-{q_2}^2-{q_3}^2& 2(q_1{q}_2+q_4{q}_3)& 2(q_1{q}_3-q_4{q}_2)\\ 2(q_2{q}_1-q_4{q}_3)& {q_4}^2-{q_1}^2+{q_2}^2-{q_3}^2& 2(q_2{q}_3+q_4{q}_1)\\ 2(q_3{q}_1+q_4{q}_2)& 2(q_3{q}_2-q_4{q}_1)& {q_4}^2-{q_1}^2-{q_2}^2+{q_3}^2\end{array}\right]$

......公式(22)

γ_a=B(q_k)^GG_RV......公式(23)

^GG_RV=[0 0 -1]^T......公式(24)

估计观测值矢量产生单元269产生估计观测值矢量y^- _k，其是由观测模型计算单元268产生的2dim(x)+1个估计观测值γ_k(i)的加权平均。同时，可利用已知方法来适当地计算该加权平均。

观测模型计算单元268和估计观测值矢量产生单元269起到观测模型单元281的作用，用于将姿态预测值q^- _χ,k(i)应用于观测模型以产生估计观测值矢量y^- _k(估计观测值)。

减法器270从观测值矢量y_k(输入观测值)减去估计观测值矢量y^- _k以产生观测残差e_k。

卡尔曼增益产生单元271根据估计观测值矢量y^- _k、2dim(x)+1个估计观测值γ_k(i)和观测噪声v_k的协方差矩阵R_k来产生观测残差e_k的协方差矩阵P^yy _k。此外，卡尔曼增益产生单元271根据预测状态矢量x^- _k、估计观测值矢量y^- _k、2dim(x)+1个sigma点χ_k(i)和2dim(x)+1个估计观测值γ_k(i)来产生预测状态矢量x^- _k和估计观测值矢量y^- _k的互协方差矩阵P^xy _k。同时，卡尔曼增益产生单元271可利用诸如非专利文献1中公开的方法之类的已知方法来适当地产生协方差矩阵P^yy _k和互协方差矩阵P^xy _k。

接下来，卡尔曼增益产生单元271按照公式(13)所示根据协方差矩阵P^yy _k和互协方差矩阵P^xy _k来产生卡尔曼增益K_k。卡尔曼增益K_k在判定单元220的判断结果是肯定的情况下是3×6的矩阵，在判定单元220的判断结果是否定的情况下是3×3的矩阵。

此外，卡尔曼增益产生单元271根据卡尔曼增益K_k和观测残差e_k来产生公式(12)的右侧的第二项中出现的三维矢量K_ke_k，以根据卡尔曼增益K_k和协方差矩阵P^yy _k来产生公式(14)的右侧的第二项中出现的3×3矩阵K_kP^yy _kK_k ^T。

加法器272按照公式(12)所示将卡尔曼增益产生单元271计算出的矢量K_ke_k与预测状态矢量x^- _k相加以产生更新后的状态矢量x⁺ _k。

估计姿态误差计算单元273将利用MRP来表示的更新后的状态矢量x⁺ _k的表达类型转换成四元数，以产生估计姿态误差δq⁺ _k。

由此，差分姿态计算单元265、预测状态矢量产生单元266、减法器270、卡尔曼增益产生单元271、加法器272和估计姿态误差计算单元273起到误差估计单元282的作用，用于根据观测值矢量y_k来产生观测残差e_k以及根据观测残差e_k和姿态预测值q^- _{χ, k}(i)来产生估计姿态误差δq⁺ _k。

同时，在下面的描述中，观测模型单元281和误差估计单元282可被统称为“预测值校正单元”。

减法器274按照公式(14)所示，从预测状态矢量x^- _k的估计误差的协方差矩阵P^- _k减去由卡尔曼增益产生单元271计算出的矩阵K_kP^yy _kK_k ^T以产生更新后的状态矢量x⁺ _k的估计误差的协方差矩阵P⁺ _k。

姿态更新单元275根据状态变换模型计算单元264所产生的姿态预测值q^- _χ,k(0)以及估计姿态误差δq⁺ _k来产生估计姿态q⁺ _k。T=k时的估计姿态q⁺ _k的值被称为“第二估计值”。姿态更新单元275所产生的估计姿态q⁺ _k被输出作为便携设备1的姿态q的估计值。

产生估计姿态q⁺ _k的处理随着判定单元220的判断结果而变化。下文中，将分别描述在判定单元220的判断结果是肯定和否定的情况下产生估计姿态q⁺ _k的处理。

首先，在判定单元220的判断结果是肯定的情况下，姿态更新单元275按照下述公式(25)所示，通过估计姿态误差δq⁺ _k和姿态预测值q^- _χ,k(0)的四元数乘积来计算估计姿态q⁺ _k。

$q_k^{+}=\mathrm{\&delta;}{q}_k^{+}[\&times;]q_{\mathrm{\&chi;},k}^{-}\left(0\right)$ ......公式(25)

如上所述，估计姿态误差δq⁺ _k是通过利用来自三维磁传感器70的输出值和来自三维加速度传感器80的输出值对姿态预测值q^- _χ,k(0)与实际姿态q之间的差异进行估计而得到的值。此外，姿态预测值q^- _χ,k(0)是根据来自三维角速度传感器90的输出值利用状态变换模型得到的姿态q的预测值。即，卡尔曼滤波器单元260利用来自三维磁传感器70、三维加速度传感器80和三维角速度传感器90的输出值估计出估计姿态q⁺ _k。

由于卡尔曼滤波器单元260如上所述在判定单元220的判断结果是肯定的情况下对来自三个传感器的输出值进行综合以对姿态进行估计，所以可以正确地估计姿态。

另一方面，在判定单元220的判断结果是否定的情况下，姿态更新单元275如下地计算估计姿态q⁺ _k。同时，如上所述，四元数可以不仅指示姿态，而且指示姿态的变化量。下述计算中使用的四元数q_A至q_C涉及上述两个含义。

第一，姿态更新单元275根据下述公式(26)产生四元数q_A。四元数q_A是指示将姿态预测值q^- _χ,k(0)改变一个估计姿态误差δq⁺ _k而得到的姿态的四元数。

第二，姿态更新单元275根据下述公式(27)产生四元数q_B。公式(27)中出现的[q^- _χ,k(0)]^-1是姿态预测值q^- _χ,k(0)的逆四元数。

逆四元数是指示与四元数所指示的姿态改变相反的姿态改变的四维数。具体地说，例如，在四元数q_o指示从姿态q₁至姿态q₂的姿态改变的情况下，逆四元数[q_o]^-1指示从姿态q₂至姿态q₁的姿态改变。

即，四元数q_B指示通过沿着与姿态预测值q^- _χ,k(0)所指示的姿态改变相反的方向来改变如四元数q_A所指示的姿态而得到的姿态。换言之，四元数q_A指示通过使姿态预测值q^- _χ,k(0)所指示的姿态改变一个估计姿态误差δq⁺ _k而得到的姿态，而四元数q_B指示通过使基准姿态=[0，0，0，1]^T改变一个估计姿态误差δq⁺ _k(即仅仅估计姿态误差δq⁺ _k)而得到的姿态。

第三，姿态更新单元275产生四元数q_C(下文中，也称为“提取的估计姿态误差”)。如图5所示，四元数q_C是通过从四元数q_B所指示的姿态改变中去除指示相对于固定至地的坐标系Σ_G的Z轴(即沿重力加速度的方向延伸的轴)的歪斜或倾斜的分量、并提取出以Z轴作为旋转轴的旋转分量而得到的四元数。与估计姿态误差的倾斜分量相比，旋转分量是可靠的不受设备的不稳定状态影响的特定姿态分量。

更具体地说，当四元数q_B的每个分量由下述公式(28)表示时，在四元数q_B的分量q_B4等于或者大于0的情况下，根据公式(29)来计算四元数q_C。另一方面，在四元数q_B的分量q_B4小于0的情况下，根据公式(30)来计算四元数q_C。

第四，姿态更新单元275根据公式(31)通过姿态预测值q^- _χ,k(0)和四元数q_C的四元数乘积来计算估计姿态q⁺ _k。

$q_A=\mathrm{\&delta;}{q}_k^{+}[\&times;]q_{\mathrm{\&chi;},k}^{-}\left(0\right)$ ......公式(26)

$q_B={\left[q_{\mathrm{\&chi;},k}^{-}\left(0\right)\right]}^{-1}[\&times;]q_A$ ......公式(27)

$q_B=\left[\begin{array}{c}{q}_{B1}\\ q_{B2}\\ q_{B3}\\ q_{B4}\end{array}\right]$ ......公式(28)

$q_C=\left[\begin{array}{c}0\\ 0\\ q_{B3}\\ \sqrt{1-{q_{B3}}^2}\end{array}\right]$ (其中q_B4≥0)......公式(29)

$q_C=\left[\begin{array}{c}0\\ 0\\ q_{B3}\\ -\sqrt{1-{q_{B3}}^2}\end{array}\right]$ (其中q_B4<0)......公式(30)

$q_k^{+}=q_{\mathrm{\&chi;},k}^{-}\left(0\right)[\&times;]q_C$ ......公式(31)

在判定单元220的判断结果是否定的情况下，加速度矢量a_k不包含在观测值矢量y中，仅磁矢量m_k包含在观测值矢量y中。即，在判定单元220的判断结果是否定的情况下，在观测模型中，不计算加速度矢量a_k的估计值γ_a，而是仅仅估计磁矢量m_k的估计值γ_m。

公式(23)所表示的加速度矢量a_k的估计值γ_a是通过将指示固定至地的坐标系中重力加速度的方向的矢量^GG_RV表示为固定至便携设备1的坐标系中的矢量而得到的矢量^SG_RV的估计值。

在可以考虑加速度矢量a_k和矢量^SG_RV彼此一致的情况下，可以通过加速度矢量a_k知道重力加速度的方向。因此，在这种情况下，可以根据观测残差e_k(更具体地说，指示加速度矢量a_k的估计值γ_a与加速度矢量a_k之差的分量)来更新卡尔曼滤波器单元260所估计的便携设备1的姿态q的估计值中指示相对于重力加速度的方向的歪斜或倾斜的分量，以使得倾斜近似于实际倾斜。即，在判定单元220的判断结果是肯定、并且可以假设加速度矢量a_k与矢量^SG_RV彼此一致的情况下，加速度矢量a_k对指示便携设备1的姿态q中相对于重力加速度的方向的倾斜的分量的估计有极大的贡献。

然而，在判定单元220的判断结果是否定的情况下，加速度矢量a_k和矢量^SG_RV相互可能极其不同。在这种情况下，不可能根据加速度矢量a_k知道重力加速度的方向。因此，在这种情况下，卡尔曼滤波器单元260所估计的便携设备1的姿态q的估计值中的指示相对于重力加速度的方向的倾斜的分量可被更新成指示与实际倾斜极其不同的倾斜。

然而，在判定单元220的判断结果是否定的情况下，根据本实施例的卡尔曼滤波器单元260估计出便携设备1的姿态q，而不计算加速度矢量a_k。因此，即使在加速度矢量a_k和矢量^SG_RV彼此极其不同的情况下，也能够防止便携设备1的姿态q的估计值中的指示相对于重力加速度的方向的倾斜的分量被更新成指示与实际倾斜极其不同的倾斜。

另一方面，公式(20)所表示的磁矢量m_k的估计值γ_m是通过将指示固定至地的坐标系中的地磁B_g的方向和幅值的矢量^GB_g表示为固定至便携设备1的坐标系中的矢量而得到的矢量^SB_g(q)的估计值。此外，在本实施例中，可按照公式(21)所示，利用地磁B_g的磁倾角Φ作为参数来对指示了地磁B_g的方向和幅值的矢量^GB_g进行表示。因此，在可以假设磁矢量m_k与矢量^SB_g(q)彼此一致的情况下，磁矢量m_k变成其中正确地反映了地磁B_g的磁倾角Φ的值。

此外，如上所述，地磁B_g的垂直分量(沿重力加速度的方向的分量)由磁倾角Φ决定。因此，在磁矢量m_k是其中正确地反映了地磁B_g的磁倾角Φ的值的情况下，从磁矢量m_k得到的地磁B_g的垂直分量的方向与重力加速度的方向彼此一致。即，在可以正确地知道磁倾角Φ的情况下，可以从地磁B_g的方向知道重力加速度的方向。在这种情况下，可以根据磁矢量m_k和磁矢量m_k的估计值γ_m来更新便携设备1的姿态q的估计值中的指示相对于重力加速度的方向的倾斜的分量，以使得倾斜大致是实际倾斜。

然而，在便携设备1附近存在产生磁场的对象并且该对象所产生的磁场被叠加在指示磁数据m(磁矢量m_k)的值上作为噪声的情况下，磁矢量m_k和矢量^SB_g(q)相互之间可能极其不同。在这种情况下，磁矢量m_k不会变成其中正确地反映了地磁B_g的磁倾角Φ的值，并且从磁矢量m_k得到的地磁B_g的垂直分量的方向与重力加速度的方向相互之间极其不同。因此，在这种情况下，便携设备1的姿态q的估计值中的指示相对于重力加速度的方向的倾斜的分量被更新成指示与实际倾斜不同的倾斜。即，即使仅仅根据磁矢量m_k来估计出便携设备1的姿态q，也可能不能正确地估计出姿态q的估计值中的指示了相对于重力加速度的方向的倾斜的分量。

然而，根据本实施例的姿态更新单元275在判定单元220的判断结果是否定的情况下，利用如下的四元数q_C来计算估计姿态q⁺ _k，该四元数q_C是通过从根据磁矢量m_k和磁矢量m_k的估计值γ_m来产生的估计姿态误差δq⁺ _k中去除指示了相对于沿重力加速度的方向延伸的轴的倾斜的分量、并提取出以该轴作为旋转轴的旋转分量而得到的。因此，在判定单元220的判断结果是否定的情况下，防止姿态更新单元275根据磁矢量m_k来估计指示了便携设备1的姿态q中相对于重力加速度的方向的倾斜的分量。

因此，即使在磁矢量m_k和矢量^SB_g(q)彼此极其不同、并且磁矢量m_k不是其中正确地反映了地磁B_g的磁倾角Φ的值的情况下，也可以防止便携设备1的姿态q的估计值中指示了相对于重力加速度的方向的倾斜的分量被更新成指示极其不同于实际倾斜的倾斜。

由此，根据本实施例的姿态估计装置即使在检测出了噪声分量而不是传感器要检测的分量的情况下，也可防止姿态估计极大地偏离实际姿态。

<B.变型>

本发明并不限于上述实施例，而是可以进行下述修改。此外，两个或更多的下述变型可被组合。

(1)第一变型

虽然根据上述实施例的便携设备1利用sigma点卡尔曼滤波器来执行非线性卡尔曼滤波计算，但是本发明并不限于此。可利用已知的非线性卡尔曼滤波器（例如扩展卡尔曼滤波器）来适当地执行非线性卡尔曼滤波计算。

(2)第二变型

虽然在如上所述的实施例和变型中，姿态估计单元200包括观测值矢量产生单元240，但是可以不提供观测值矢量产生单元240。即，观测值矢量y_k可始终将加速度矢量a_k和磁矢量m_k作为元素。

根据本发明的卡尔曼滤波器单元260根据判定单元220的判断结果来计算估计姿态q⁺ _k。即，在判定单元220的判断结果是否定的情况下，卡尔曼滤波器单元260利用如下的四元数q_C来计算估计姿态q⁺ _k，该四元数q_C是通过提取出以沿估计姿态误差δq⁺ _k中的重力加速度的方向延伸的轴作为旋转轴的旋转分量而得到的。因此，即使在判定单元220的判断结果是否定、并且加速度矢量a_k和矢量^SG_RV可能彼此极其不同的情况下，也可以防止便携设备1的姿态q的估计值中指示了相对于重力加速度的方向的倾斜的分量被更新成指示了极其不同于实际倾斜的倾斜。

(3)第三变型

虽然在如上所述的实施例和变型中，便携设备1包括三维磁传感器70、三维加速度传感器80和三维角速度传感器90，但是本发明并不限于此。可提供这三类传感器之外的传感器。此外，可提供包括这三类传感器中的至少三维加速度传感器80的多个传感器。

即，根据本发明的姿态估计装置可布置在便携设备1中，便携设备1包括用于测量不同类型的物理量的多个传感器，以使得姿态估计装置执行卡尔曼滤波计算来将来自这些传感器的输出进行综合并对系统状态进行估计。

在配置有姿态估计装置的设备的运动不稳定的情况下，由设备振动引起的噪声可能叠加在来自三维加速度传感器的输出值上，结果，指示来自三维加速度传感器的输出值的矢量和指示重力加速度的矢量可能彼此极其不同。在这种情况下，如果根据来自三维加速度传感器的输出值来估计设备姿态，则指示相对于估计姿态中的重力加速度的方向的倾斜的分量可能极大地偏离实际姿态的倾斜。

根据本发明，在配置有姿态估计装置的设备的运动被确定为不稳定的情况下，姿态更新单元根据下文所述的提取的估计姿态误差来校正状态变换模型单元的预测结果，以计算单位时间过去之后的姿态的估计值，上述提取的估计姿态误差是从根据观测值矢量产生的估计姿态误差中提取出以沿重力加速度的方向延伸的轴作为旋转轴的旋转分量而得到的。即，在配置有姿态估计装置的设备的运动不稳定的情况下，姿态更新单元从估计姿态误差中去除指示相对于重力加速度的方向的歪斜或倾斜的分量。因此，即使在观测值矢量以来自三维加速度传感器的输出值作为元素并且指示来自三维加速度传感器的输出值的矢量具有不同于重力加速度的方向的方向的情况下，也可以在假设指示了来自三维加速度传感器的输出值的矢量的方向与重力加速度的方向彼此一致的同时防止设备姿态相对于重力加速度的方向的倾斜的估计。

由此，根据本发明的姿态估计装置可以估计设备的正确姿态，而不更新设备的姿态的估计值中指示了相对于重力加速度方向的倾斜的分量，从而防止指示极大地偏离真实倾斜的倾斜。

在配置有姿态估计装置的设备的运动被确定为不稳定的情况下，根据本发明的姿态估计装置产生不包括来自三维加速度传感器的输出值的观测值矢量。因此，在噪声叠加在来自三维加速度传感器的输出值上的情况下，姿态估计装置可以防止基于来自三维加速度传感器的输出值来对设备的姿态进行估计。

一般地，利用地磁的磁倾角作为参数来表示地磁的方向。因此，在可以假设指示来自三维磁传感器的输出值的矢量与指示地磁的矢量彼此一致的情况下，来自三维磁传感器的输出值变成其中正确地反映了地磁的磁倾角的值。此外，在这种情况下，可以假设从来自三维磁传感器的输出值获得的地磁的重力加速度分量的方向与重力加速度的实际方向彼此一致。因此，在这种情况下，在参考根据来自三维磁传感器的输出值而正确地得到的重力加速度方向的同时，可以正确地估计设备的姿态相对于重力加速度的方向的倾斜。

然而，在设备附近存在产生磁场的对象的情况下，对象产生的磁场所造成的噪声叠加在来自三维磁传感器的输出值上。在这种情况下，来自三维磁传感器的输出值不会变成其中正确地反映了地磁的磁倾角的值，而且从来自三维磁传感器的输出值得到的地磁的重力加速度分量的方向与重力加速度的实际方向彼此偏离。因此，在这种情况下，不可能根据来自三维磁传感器的输出值来正确地估计出设备的姿态相对于重力加速度的方向的倾斜。

根据本发明，在配置有姿态估计装置的设备的运动被确定为不稳定、并且仅仅根据来自三维磁传感器的输出值来产生观测值矢量的情况下，姿态更新单元利用下文所述的提取的估计姿态误差来校正状态变换模型单元的预测值，以计算单位时间过去之后的姿态的估计值，上述提取的估计姿态误差是通过从根据观测值矢量(来自三维磁传感器的输出值)产生的估计姿态误差中去除指示相对于重力加速度的方向的倾斜的分量而得到的。因此，即使在根据来自三维磁传感器的输出值而得到的地磁的重力加速度分量的方向与重力加速度的实际方向彼此不同的情况下，也可以在假设这些方向彼此一致的情况下，防止对设备的姿态相对于重力加速度的方向的倾斜进行估计。由此，根据本发明的姿态估计装置可以估计出设备的正确姿态，而不更新设备的姿态的估计值中指示相对于重力加速度方向的倾斜的分量，从而防止姿态估计装置指示出极大地偏离真实倾斜的倾斜。

此外，在上述实施例中，判定单元可在指示来自三维加速度传感器的输出值的矢量的幅值与重力加速度的幅值之差的绝对值等于或小于预定值的情况下判定设备的运动是稳定的。

在设备的运动不稳定的情况下，加在设备上的加速度被叠加至来自三维加速度传感器的输出值上，在该状态下，判定单元将来自三维加速度传感器的输出值的幅值与重力加速度的幅值进行比较。由此，判定单元可以确定设备的运动是否稳定。

Claims (10)

1.一种对具有多个传感器的设备的姿态进行估计的方法，所述多个传感器包括加速度传感器，所述方法包括步骤：

产生所述设备在第一时刻的姿态的第一估计值；

根据从所述加速度传感器输出的加速度数据来确定所述设备的姿态是否稳定；

通过将所述第一估计值应用于表示所述设备的姿态的变换的状态变换模型，来预测所述设备在从所述第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态预测值；

根据从所述多个传感器中所包含的至少一个传感器输出的数据以及所述姿态预测值来估计所述设备的所述姿态预测值与姿态的真实值之差，从而产生所述设备的预测姿态与真实姿态之间的估计姿态误差；

当所述设备的姿态被确定为稳定时，根据所述姿态预测值和所述估计姿态误差来计算所述设备在所述第二时刻的姿态的第二估计值；

当所述设备的姿态被确定为不稳定时，从所述估计姿态误差的多个姿态分量中提取特定姿态分量；以及

当所述设备的姿态被确定为不稳定时，根据所述姿态预测值和从所述估计姿态误差中提取出来的所述特定姿态分量，来计算所述设备在所述第二时刻的姿态的第二估计值。

2.根据权利要求1所述的方法，其中所述估计姿态误差的多个姿态分量包括表示相对于沿重力加速度的方向延伸的轴的倾斜的倾斜分量以及围绕该轴的旋转分量，并且所述特定姿态分量是通过从所述估计姿态误差中去除所述倾斜分量而得到的所述旋转分量。

3.根据权利要求1所述的方法，其中所述设备的姿态由四元数表示。

4.根据权利要求1所述的方法，

其中所述多个传感器还包括磁传感器和角速度传感器，

其中所述方法还包括步骤:在所述设备的姿态被确定为稳定时，产生这样的输入观测值，其包括从所述磁传感器输出的磁数据以及从所述加速度传感器输出的加速度数据，作为其元素；以及在所述设备的姿态被确定为不稳定时，产生这样的输入观测值，其包括从所述磁传感器输出的磁数据作为其元素，而不包括从所述加速度传感器输出的加速度数据，

其中通过将所述第一时刻的第一估计值和从所述角速度传感器输出的角速度数据应用于所述状态变换模型来预测所述设备在所述第二时刻的姿态，以及

其中通过以下步骤来产生所述估计姿态误差:将所述第二时刻的姿态预测值应用于表示观测值的元素与姿态的值之间的关系的观测模型，以便对所述第二时刻时的观测值的元素进行估计，从而产生具有所估计的元素的估计观测值；计算所述估计观测值与所述输入观测值之差以产生表示所计算出的差的观测残差；并且根据所述观测残差以及所述设备的姿态预测值来产生所述估计姿态误差。

5.根据权利要求1所述的方法，其中在从所述加速度传感器输出的加速度数据的幅值与重力加速度的幅值之差的绝对值不大于预定值的情况下，所述设备的姿态被确定为稳定。

6.一种对具有传感器的设备的姿态进行估计的方法，所述方法包括步骤：

产生所述设备在第一时刻的姿态的第一估计值；

通过将所述第一估计值应用于表示所述设备的姿态的变换的状态变换模型，来预测所述设备在从所述第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态预测值；

根据从所述传感器输出的数据以及所述姿态预测值来估计所述设备的所述姿态预测值与姿态的真实值之差，从而产生所述设备的预测姿态与真实姿态之间的估计姿态误差；

从所述估计姿态误差的多个姿态分量中提取特定姿态分量；以及

根据所述姿态预测值和从所述估计姿态误差中提取出来的所述特定姿态分量，来计算所述设备在所述第二时刻的姿态的第二估计值。

7.根据权利要求6所述的方法，其中所述传感器包括用于输出表示地磁的磁数据的磁传感器。

8.根据权利要求6所述的方法，其中所述估计姿态误差的多个姿态分量包括表示相对于沿重力加速度的方向延伸的轴的倾斜的倾斜分量以及围绕该轴的旋转分量，并且所述特定姿态分量是通过从所述估计姿态误差中去除所述倾斜分量而得到的所述旋转分量。

9.一种用于对具有多个传感器的设备的姿态进行估计的装置，所述多个传感器包括加速度传感器，所述装置包括一个或多个处理器，所述一个或多个处理器被配置成用于：

产生所述设备在第一时刻的姿态的第一估计值；

根据从所述加速度传感器输出的加速度数据来确定所述设备的姿态是否稳定；

通过将所述第一估计值应用于表示所述设备的姿态的变换的状态变换模型，来预测所述设备在从所述第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态预测值；

根据从所述多个传感器中所包含的至少一个传感器输出的数据以及所述姿态预测值来估计所述设备的所述姿态预测值与姿态的真实值之差，从而产生所述设备的预测姿态与真实姿态之间的估计姿态误差；

当所述设备的姿态被确定为稳定时，根据所述姿态预测值和所述估计姿态误差来计算所述设备在所述第二时刻的姿态的第二估计值；

当所述设备的姿态被确定为不稳定时，从所述估计姿态误差的多个姿态分量中提取特定姿态分量；以及

当所述设备的姿态被确定为不稳定时，根据所述姿态预测值和从所述估计姿态误差中提取出来的所述特定姿态分量，来计算所述设备在所述第二时刻的姿态的第二估计值。

10.一种用于对具有传感器的设备的姿态进行估计的装置，所述装置包括一个或多个处理器，所述一个或多个处理器被配置成用于：

产生所述设备在第一时刻的姿态的第一估计值；

通过将所述第一估计值应用于表示所述设备的姿态的变换的状态变换模型，来预测所述设备在从所述第一时刻开始过去预定时间段之后的第二时刻的姿态，从而产生姿态预测值；

根据从所述传感器输出的数据以及所述姿态预测值来估计所述设备的所述姿态预测值与姿态的真实值之差，从而产生所述设备的预测姿态与真实姿态之间的估计姿态误差；

从所述估计姿态误差的多个姿态分量中提取特定姿态分量；以及

根据所述姿态预测值和从所述估计姿态误差中提取出来的所述特定姿态分量，来计算所述设备在所述第二时刻的姿态的第二估计值。