JP2014089113A

JP2014089113A - 姿勢推定装置及びプログラム

Info

Publication number: JP2014089113A
Application number: JP2012239131A
Authority: JP
Inventors: Ibuki Handa; 伊吹半田
Original assignee: Yamaha Corp
Current assignee: Yamaha Corp
Priority date: 2012-10-30
Filing date: 2012-10-30
Publication date: 2014-05-15
Also published as: US20140122015A1; CN103791905A; CN103791905B

Abstract

【課題】真の姿勢から乖離した姿勢の推定を防止する。
【解決手段】３次元磁気センサ７０と３次元加速度センサ８０を備える携帯機器１の姿勢ｑを推定する姿勢推定装置は、携帯機器１の動きが安定しているか否かを判定する判定部２２０と、３次元磁気センサ７０及び３次元加速度センサ８０からの出力値に基づいて観測値ベクトルｙを生成する観測値ベクトル生成部２４０と、単位時間経過後の携帯機器１の姿勢ｑの推定値である推定姿勢ｑ^＋ _ｋを算出するカルマンフィルタ部２６０とを備え、カルマンフィルタ部２６０は、単位時間経過後の姿勢を予測する状態遷移モデル演算部２６４と、前記予測結果に基づいて観測値ベクトルｙの値を推定する観測モデル部２８１と、前記予測結果と真の姿勢との差分を推定する誤差推定部２８２と、判定部２２０の判定結果と誤差推定部２８２の推定結果とに基づいて推定姿勢ｑ^＋ _ｋを算出する姿勢更新部２７５とを具備する。
【選択図】図４

Description

本発明は姿勢推定装置及びプログラムに関する。

物体の姿勢等、動的システムの状態を推定する演算を実行する場合、１種類のセンサの出力結果に基づいて算出するよりも、地磁気センサ、加速度センサ、角速度センサ等の異種の物理量を測定する複数のセンサの出力結果を統合して算出した方が、短時間に正確な推定をすることができる。

異種の物理量を測定する複数のセンサの出力を統合し動的システムの状態を推定する方法として、カルマンフィルタを用いる方法が知られている。例えば、非特許文献１には、非線形カルマンフィルタの１種であるシグマポイントカルマンフィルタを用いて、３軸の地磁気センサ、３軸の角速度センサ、及び、３軸の加速度センサからの出力を統合することで、姿勢を推定する姿勢推定装置が開示されている。

J. L. Crassidis, and F. L. Markley, "Unscented Filtering for Spacecraft Attitude Estimation," Journal of Guidance, Control and Dynamics, 26 (2003), pp.536-542

地磁気センサや加速度センサ等のセンサは、検出対象となる成分以外の成分（ノイズ）を検出する場合がある。例えば、加速度センサは、重力加速度を検出対象とする場合であっても、当該加速度センサが振動していれば重力加速度以外の振動に係る加速度成分を検出する。また、地磁気を検出対象とする地磁気センサは、当該地磁気センサの近傍に磁界を発生させる物体が存在すれば地磁気以外の磁気成分を検出する。
姿勢推定装置が利用するセンサが、検出対象となる成分以外のノイズを検出する場合、姿勢推定装置は、当該ノイズの影響を受け、真の姿勢とは大きくかけ離れた姿勢を推定することがある。

本発明は、上述した点に鑑みてなされたものであり、姿勢推定装置が利用するセンサが検出対象となる成分以外のノイズ成分を検出する場合であっても、真の姿勢から乖離した姿勢の推定を防止することのできる姿勢推定装置の提供を、解決課題とする。

上述した課題を解決するため、本発明に係る姿勢推定装置は、３方向の加速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元加速度センサを含む複数のセンサを備える機器に設けられ、当該機器の姿勢を推定する姿勢推定装置であって、前記３次元加速度センサからの出力値に基づいて、前記機器の動きが安定しているか否かを判定する判定部と、前記判定部の判定結果と、前記複数のセンサのうち少なくとも一部のセンサの出力値の各々を要素とする観測値ベクトルとに基づいて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢の推定値を算出するカルマンフィルタ部と、を備え、前記カルマンフィルタ部は、前記機器の姿勢の経時的な変化を表す状態遷移モデルを用いて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢を予測する状態遷移モデル部と、前記状態遷移モデル部の予測結果と前記観測値ベクトルとに基づいて、前記状態遷移モデル部の予測結果と前記ある時刻よりも単位時間経過後の時刻における真の姿勢との差分を推定した値である推定姿勢誤差を生成する予測値補正部と、前記判定部の判定結果が肯定である場合には、前記推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出し、前記判定部の判定結果が否定である場合には、前記推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出する姿勢更新部と、を具備する、ことを特徴とする。

状態推定装置が設けられる機器の動きが安定していない場合、３次元加速度センサからの出力値には機器の振動に起因するノイズが重畳し、３次元加速度センサからの出力値の示すベクトルと重力加速度を表すベクトルとが大きく乖離する場合がある。この場合、３次元加速度センサからの出力値に基づいて機器の姿勢を推定すると、推定された姿勢のうち重力加速度方向からの傾きを表す成分は、真の姿勢の傾きから大きく乖離する可能性が高い。
この発明によれば、姿勢推定装置が設けられる機器の動きが安定していないと判定される場合に、姿勢更新部は、観測値ベクトルに基づいて生成された推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差により、状態遷移モデル部の予測結果を補正することで、単位時間経過後の姿勢の推定値を算出する。すなわち、姿勢更新部は、状態推定装置が設けられる機器の動きが安定しない場合には、推定姿勢誤差から、重力加速度方向に対する傾きを表す成分を取り除く。よって、観測値ベクトルが３次元加速度センサからの出力値を要素とする場合であって、且つ、３次元加速度センサからの出力値の示すベクトルが重力加速度とは異なる方向を示す場合であっても、３次元加速度センサからの出力値の示すベクトルと重力加速度方向とが一致していると看做して機器の姿勢の重力加速度方向からの傾きを推定することを防止することができる。
これにより、本発明に係る姿勢推定装置は、機器の姿勢の推定値のうち重力加速度方向からの傾きを表す成分を、真の傾きから大きく乖離した傾きを表すように更新することなく、正確な姿勢を推定することが可能となる。

また、本発明に係る姿勢推定装置は、３方向の磁気成分をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元磁気センサと、３方向の加速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元加速度センサと、３方向にそれぞれ延在する３軸の周りの角速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元角速度センサと、を備える機器に設けられ、当該機器の姿勢を推定する姿勢推定装置であって、前記３次元加速度センサからの出力値に基づいて、前記機器の動きが安定しているか否かを判定する判定部と、前記判定部の判定結果が肯定である場合には、前記３次元磁気センサからの出力値と前記３次元加速度センサからの出力値とを要素とする観測値ベクトルを生成し、前記判定部の判定結果が否定である場合には、前記３次元磁気センサからの出力値を要素とする観測値ベクトルを生成する観測値ベクトル生成部と、前記判定部の判定結果と、前記観測値ベクトルとに基づいて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢の推定値を算出するカルマンフィルタ部と、を備え、前記カルマンフィルタ部は、前記機器の姿勢の経時的な変化を表す状態遷移モデルに対して、ある時刻の姿勢の推定値と前記３次元角速度センサからの出力値とを適用することで、当該ある時刻よりも単位時間経過後の時刻における姿勢を予測して、これを姿勢予測値として出力する状態遷移モデル部と、前記観測値ベクトルの各要素の値と前記機器の姿勢との関係を表す観測モデルに対して、前記姿勢予測値を適用することで、当該ある時刻よりも単位時間経過後の時刻における観測値ベクトルの各要素の値を推定して、これを推定観測値ベクトルとして出力する観測モデル部と、前記推定観測値ベクトルと、前記観測値ベクトルとの差分である観測残差を算出するとともに、算出した観測残差と、前記姿勢予測値とに基づいて、前記ある時刻よりも単位時間経過後の時刻における真の姿勢と前記姿勢予測値との差分を推定した値である推定姿勢誤差を生成する誤差推定部と、前記判定部の判定結果が肯定である場合には、前記推定姿勢誤差と、前記姿勢予測値とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出し、前記判定部の判定結果が否定である場合には、前記推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差と、前記姿勢予測値とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出する姿勢更新部と、を具備する、ことを特徴とする。

この発明に係る姿勢推定装置は、姿勢推定装置が設けられる機器の動きが安定していないと判定される場合に、３次元加速度センサからの出力値を含まない観測値ベクトルを生成する。これにより、姿勢推定装置は、３次元加速度センサからの出力値にノイズが重畳している場合に、３次元加速度センサからの出力値に基づいて機器の姿勢を推定することを防止することができる。

なお、通常、地磁気の方向は地磁気の伏角をパラメータとして表現される。そのため、３次元磁気センサからの出力値の示すベクトルと地磁気を表すベクトルとが一致していると看做すことができる場合、３次元磁気センサからの出力値は地磁気の伏角を正確に反映した値となる。また、この場合、３次元磁気センサからの出力値から求められる地磁気の重力加速度成分の方向と、真の重力加速度方向とは、一致していると看做すことができる。よって、この場合、３次元磁気センサからの出力値に基づいて正確に求められる重力加速度方向を参照することで、機器の姿勢の重力加速度方向からの傾きを正確に推定することができる。
しかし、機器の近傍に磁界を発生させる物体が存在する場合、当該物体の発する磁界に起因するノイズが３次元磁気センサからの出力値に重畳する。この場合、３次元磁気センサからの出力値は地磁気の伏角を正確に反映した値とはならず、３次元磁気センサからの出力値から求められる地磁気の重力加速度成分の方向と、真の重力加速度方向とは、異なることになる。よって、この場合、３次元磁気センサからの出力値に基づいて、機器の姿勢の重力加速度方向からの傾きを正確に推定することができない。
この発明によれば、姿勢推定装置が設けられる機器の動きが安定していないと判定され、３次元磁気センサからの出力値のみに基づいて観測値ベクトルが生成される場合に、姿勢更新部は、当該観測値ベクトル（３次元磁気センサからの出力値）に基づいて生成された推定姿勢誤差から重力加速度方向に対する傾きを表す成分を取り除いた抽出推定姿勢誤差により、状態遷移モデル部の予測結果を補正し、単位時間経過後の姿勢の推定値を算出する。よって、３次元磁気センサからの出力値から求められる地磁気の重力加速度成分の方向と、真の重力加速度方向とが、異なる方向を示す場合であっても、これらを一致していると看做して機器の姿勢の重力加速度方向からの傾きを推定することを防止することができる。これにより、本発明に係る姿勢推定装置は、機器の姿勢の推定値のうち重力加速度方向からの傾きを表す成分を、真の傾きから大きく乖離した傾きを表すように更新することなく、正確な姿勢を推定することが可能となる。

また、上述した態様において、前記判定部は、前記３次元加速度センサからの出力値を表すベクトルの大きさと、重力加速度の大きさとの差分値の絶対値が、所定の値以下である場合に、前記機器の動きが安定していると判定する、ことを特徴とすることが好ましい。
機器の動きが安定していない場合、機器に加わる加速度がノイズとして３次元加速度センサの出力値に重畳する。この態様によれば、判定部が、３次元加速度センサの出力値の大きさと重力加速度の大きさとを比較するため、機器の動きが安定しているか否かを把握することが可能となる。

また、本発明に係るプログラムは、３方向の加速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元加速度センサを含む複数のセンサを備える機器に設けられ、当該機器の姿勢を推定する姿勢推定装置を制御するコンピュータのプログラムであって、前記コンピュータを、前記３次元加速度センサからの出力値に基づいて、前記機器の動きが安定しているか否かを判定する判定部と、前記判定部の判定結果と、前記複数のセンサのうち少なくとも一部のセンサの出力値の各々を要素とする観測値ベクトルとに基づいて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢の推定値を算出するカルマンフィルタ部と、して機能させ、更に、前記カルマンフィルタ部を、前記機器の姿勢の経時的な変化を表す状態遷移モデルを用いて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢を予測する状態遷移モデル部と、前記状態遷移モデル部の予測結果と前記観測値ベクトルとに基づいて、前記状態遷移モデル部の予測結果と前記ある時刻よりも単位時間経過後の時刻における真の姿勢との差分を推定した値である推定姿勢誤差を生成する予測値補正部と、前記判定部の判定結果が肯定である場合には、前記推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出し、前記判定部の判定結果が否定である場合には、前記推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出する姿勢更新部と、して機能させる、ことを特徴とする

本発明の実施形態に係る携帯機器１の構成を示すブロック図である。本発明の実施形態に係る携帯機器１の外観を示す斜視図である。本発明の実施形態に係る姿勢推定部２００の機能を表すブロック図である。本発明の実施形態に係るカルマンフィルタ部２６０の機能を表すブロック図である。本発明の実施形態に係る姿勢更新部２７５の機能を説明するための説明図である。

＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。

＜１．機器構成及びソフトウェア構成について＞
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１は、携帯機器１の姿勢に応じて携帯機器１が備える画面（後述する表示部５０）に表示されている地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢を推定することによって実現される。

携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（蓄積部）２０、姿勢推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。
また、携帯機器１は、地磁気等の磁気を検出して磁気データｍを出力する３次元磁気センサ７０、加速度を検出して加速度データａを出力する３次元加速度センサ８０、及び角速度を検出して角速度データωを出力する３次元角速度センサ９０を備える。

表示部５０は、ＣＰＵ１０が姿勢推定プログラム１００を実行することにより推定した携帯機器１の姿勢ｑの推定値に基づいて、方位などを表す矢印などの画像を表示する。
ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。

３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及び、Ｚ軸磁気センサ７３からの出力信号をＡＤ変換して磁気データｍを出力する。この磁気データｍは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。より具体的には、磁気データｍは、携帯機器１に固定された座標系において、Ｘ軸磁気センサ７１からの出力値をｘ軸成分として表し、Ｙ軸磁気センサ７２からの出力値をｙ軸成分として表し、Ｚ軸磁気センサ７３からの出力値をｚ軸成分として表すベクトルデータである。

ところで、３次元磁気センサ７０が検出する磁気データｍには、地磁気Ｂｇが含まれる。
地磁気Ｂｇは、一般的に、磁極北に向かう水平成分と伏角により定められる鉛直成分を有する磁界であり、地上に固定された座標系において、一定の向き及び大きさを有するベクトル^ＧＢｇとして表される（なお、ベクトルの符号の左上に示す添え字「Ｇ」は、当該ベクトルが地上に固定された座標系で表されたベクトルであることを意味する）。つまり、携帯機器１に固定された座標系において、地磁気Ｂｇは、携帯機器１の姿勢ｑに連動して向きを変える、一定の大きさのベクトル^ＳＢｇ（ｑ）として表される（なお、ベクトルの符号の左上に示す添え字「Ｓ」は、当該ベクトルが携帯機器１に固定された座標系で表されたベクトルであることを意味する）。従って、携帯機器１に固定された座標系において地磁気Ｂｇを表すベクトル^ＳＢｇ（ｑ）を算出することで、携帯機器１の姿勢ｑを求めることができる。
しかし、携帯機器１の近傍に、スピーカー等の磁界を発生させる物品や、各種金属等の着磁性を有する物品が存在する場合、これらの物品からの磁界（ノイズ）の影響を受けることがある。この場合、磁気データｍの示す値は、地磁気Ｂｇと、携帯機器１の近傍に存在する物品が発する磁界とが重畳した値を示す。すなわち、携帯機器１の近傍に磁界を発生させる物体が存在するときには、磁気データｍに基づいて地磁気Ｂｇの示す方向（つまり、ベクトル^ＳＢｇ（ｑ）の方向）を正確に求めることは困難である場合がある。この場合、磁気データｍに基づいて携帯機器１の姿勢ｑを正確に求めることはできない。

３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。
従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度ｇの大きさと方向とを示すベクトルデータとなる。

３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データωを出力する。角速度データωは、携帯機器１に固定された座標系において、３方向に延在する各軸の周りの角速度を示すベクトルデータである。

ＣＰＵ１０は、ＲＯＭ３０に格納されている姿勢推定プログラム１００を実行することによって、携帯機器１の姿勢ｑを推定する。すなわち、携帯機器１は、ＣＰＵ１０が姿勢推定プログラム１００を実行することにより、姿勢推定装置として機能する。

図３は、姿勢推定装置の機能（すなわち、ＣＰＵ１０が姿勢推定プログラム１００を実行することにより実現される機能）を表した機能ブロック図である。図３に示すように、姿勢推定装置は、加速度データａ、磁気データｍ、及び、角速度データωに基づいて、携帯機器１の姿勢ｑを推定する姿勢推定部２００を備える。姿勢推定部２００は、判定部２２０、観測値ベクトル生成部２４０、及び、カルマンフィルタ部２６０を備える。

判定部２２０は、加速度データａに基づいて、携帯機器１の動きが安定しているか否か、すなわち、携帯機器１に大きな振動が生じていないか否かを判定する。
具体的には、判定部２２０は、以下の式（１）に示す条件、すなわち、「加速度データａが示すベクトルの大きさと重力加速度の大きさｇとの差分値の絶対値が、所定の閾値ε_Ｇよりも小さい」という条件を充足するか否かを判定する。そして、判定部２２０は、当該条件を充足する場合には、携帯機器１の動きが安定していると判定し、充足しない場合には、携帯機器１の動きが不安定である（携帯機器１が振動している）と判定する。
なお、各種変数、ベクトル、行列等の右下に付された添え字「ｋ」は、当該各種変数、ベクトル、行列等の時刻Ｔ＝ｋにおける値であることを意味する。以下では、時刻Ｔ＝ｋにおける加速度データａの示す３次元ベクトルを加速度ベクトルａ_ｋと称し、時刻Ｔ＝ｋにおける磁気データｍの示す３次元ベクトルを磁気ベクトルｍ_ｋと称し、時刻Ｔ＝ｋにおける角速度データωの示す３次元ベクトルを角速度ベクトルω_ｋと称する。

観測値ベクトル生成部２４０は、判定部２２０の判定結果に基づいて、加速度データａの各成分及び磁気データｍの各成分のうちの少なくとも一部を要素とする観測値ベクトルｙを生成し、これをカルマンフィルタ部２６０に対して出力する。
具体的には、観測値ベクトル生成部２４０は、判定部２２０の判定結果が肯定である場合（つまり、携帯機器１の動きが安定していると判定された場合）には、以下の式（２）に示す、加速度ベクトルａ_ｋと磁気ベクトルｍ_ｋとを要素とする６次元のベクトルである観測値ベクトルｙ_ｋを生成する。
一方、観測値ベクトル生成部２４０は、判定部２２０の判定結果が否定である場合（つまり、携帯機器１が振動していると判定された場合）には、以下の式（３）に示す、磁気ベクトルｍ_ｋを要素とする３次元のベクトルである観測値ベクトルｙ_ｋを生成する。

本実施形態において、観測値ベクトル生成部２４０は、３次元加速度センサ８０とカルマンフィルタ部２６０との間に設けられたスイッチＳＷを含んで構成される。
スイッチＳＷは、判定部２２０の判定結果が肯定である場合にオンする。これにより、観測値ベクトル生成部２４０は、３次元加速度センサ８０が出力する加速度データａ、及び、３次元磁気センサ７０が出力する磁気データｍの双方を、カルマンフィルタ部２６０に対して伝達させる。
一方、スイッチＳＷは、判定部２２０の判定結果が否定である場合にオフする。これにより、観測値ベクトル生成部２４０は、３次元磁気センサ７０が出力する磁気データｍのみを、カルマンフィルタ部２６０に対して伝達させる。

カルマンフィルタ部２６０は、３次元角速度センサ９０が出力する角速度データωと、観測値ベクトルｙとに基づいて、カルマンフィルタの演算を実行し、携帯機器１の姿勢ｑを推定する。
上述のとおり、観測値ベクトルｙは、判定部２２０の判定結果が肯定である場合には６次元のベクトルであり、判定部２２０の判定結果が否定である場合には３次元のベクトルである。そのため、本実施形態に係るカルマンフィルタ部２６０は、判定部２２０の判定結果に基づいて、観測値ベクトルｙの次元を考慮しつつ姿勢ｑを推定する演算を実行する。
以下、カルマンフィルタ部２６０の動作について詳述する。

＜２．カルマンフィルタの演算について＞
一般的にカルマンフィルタは、動的システムの状態の経時的な変化を表す論理上のモデルである状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態から、当該ある時刻よりも単位時間経過した後（時刻Ｔ＝ｋ）のシステムの状態を予測する。
また、カルマンフィルタは、状態遷移モデルを用いて予測したシステムの状態の予測値と観測値ベクトルｙ_ｋとの関係を表す論理上のモデルである観測モデルを用いて、システムの状態の予測値から、観測値ベクトルｙ_ｋを推定する。以下では、観測値ベクトルｙ_ｋの推定値を推定観測値ベクトルｙ⁻ _ｋと称する。
次に、カルマンフィルタは、推定観測値ベクトルｙ⁻ _ｋと、実際の観測値である観測値ベクトルｙ_ｋとの差分である観測残差ｅ_ｋを算出する。
そして、カルマンフィルタは、状態遷移モデルを用いて予測したシステムの状態の予測値を、観測残差ｅ_ｋに基づいて補正（更新）することで、ある時刻から単位時間が経過した後のシステムの状態の推定値を算出する。

本実施形態におけるカルマンフィルタ（カルマンフィルタ部２６０）が推定するシステムの状態には、携帯機器１の姿勢ｑが含まれる。すなわち、カルマンフィルタ部２６０は、時刻Ｔ＝ｋ−１における携帯機器１の姿勢ｑの推定値である推定姿勢ｑ^＋ _ｋ−１に基づいて、時刻Ｔ＝ｋにおける姿勢ｑの推定値である推定姿勢ｑ^＋ _ｋを算出（推定）する。
本実施形態では、姿勢ｑを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。例えば、携帯機器１に固定された座標系の各軸と、地上に固定された座標系の各軸とが一致する姿勢を基準姿勢と定義し、基準姿勢をｑ＝［０、０、０、１］^Ｔと表現する。このとき、携帯機器１の任意の姿勢ｑは、基準姿勢に対して３次元の単位ベクトルρの方向に延在する軸を回転軸として角度ψだけ回転した姿勢として表現できる。すなわち、姿勢ｑは、クォータニオンを用いて、以下の式（４）で表される。

また、本実施形態におけるカルマンフィルタ（カルマンフィルタ部２６０）が推定するシステムの状態には、状態遷移モデルを用いて求めた単位時間経過後（時刻Ｔ＝ｋ）の姿勢ｑの予測値である姿勢予測値ｑ⁻ _ｋと、単位時間経過後の真の姿勢ｑとの差分（以下、単に「真の姿勢との差分」と称する場合がある）を推定した値である推定姿勢誤差δｑ^＋ _ｋが含まれる。本実施形態では、推定姿勢誤差δｑ^＋ _ｋは、クォータニオンを用いて表現される。

本実施形態における、カルマンフィルタの状態遷移モデルは、非線形関数ｆを用いて、以下に示す式（５）で与えられる。式（５）に示される状態ベクトルｘ_ｋは、姿勢の変化量である推定姿勢誤差δｑ^＋ _ｋをMRPs (modified Rodrigues parameters)を用いて表現するための３次元のベクトルであり、後述する更新後の状態ベクトルｘ^＋ _ｋは、推定姿勢誤差δｑ^＋ _ｋと一致する（厳密には、表現形式のみが相違する）。また、式（５）に示されるプロセスノイズｗ_ｋは、３次元のベクトルであり、０を中心とするガウスノイズである。なお、プロセスノイズｗ_ｋの共分散行列をＱ_ｋと表す。共分散行列Ｑ_ｋは、３行３列の正方行列である。
式（５）に示すように、状態遷移モデルは、時刻Ｔ＝ｋ−１における角速度ベクトルω_ｋ−１に基づいて、単位時間経過後の時刻Ｔ＝ｋにおける状態ベクトルｘ_ｋを予測するものである。以下では、状態遷移モデルを用いて予測した状態ベクトルｘ_ｋを、予測状態ベクトルｘ⁻ _ｋと称する。

状態遷移モデルは、式（５）のように、姿勢の変化量である状態ベクトルｘ_ｋを予測するものとして表現されるが、以下の式（６）のように、姿勢ｑの経時的な変化を予測するものとしてより具体的に表現することもできる。
式（６）の右辺の演算子Ωは、式（７）で定義される。また、式（７）のＩ_３×３は、３行３列の単位行列を表す。式（７）の演算子Γは、３次元ベクトルｌ＝（ｌ_１，ｌ_２，ｌ_３）を用いて、式（８）で定義される。Δｔは、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの時間）である。式（７）の成分Ψ_ｋは、式（９）で定義される。
式（６）に示すように、状態遷移モデルは、時刻Ｔ＝ｋ−１における姿勢ｑ_ｋ−１と、時刻Ｔ＝ｋ−１における角速度ベクトルω_ｋ−１に基づいて、単位時間経過後の時刻Ｔ＝ｋにおける姿勢ｑ_ｋを予測するものである。上述のとおり、状態遷移モデルを用いて予測した姿勢ｑ_ｋが、姿勢予測値ｑ⁻ _ｋである。

本実施形態における、カルマンフィルタの観測モデルは、非線形関数ｈを用いて、以下に示す式（１０）で与えられる。式（１０）に示される観測ノイズｖ_ｋは、０を中心とするガウスノイズである。なお、観測ノイズｖ_ｋの共分散行列をＲ_ｋと表す。
上述のとおり、観測値ベクトルｙ_ｋは、判定部２２０の判定結果が肯定である場合、加速度ベクトルａ_ｋと磁気ベクトルｍ_ｋとを要素とする６次元のベクトルであり、判定部２２０の判定結果が否定の場合、磁気ベクトルｍ_ｋを要素とする３次元のベクトルである。従って、判定部２２０の判定結果が肯定である場合、観測ノイズｖ_ｋは、３次元磁気センサ７０のノイズと３次元加速度センサ８０のノイズとを表す６次元のベクトルであり、共分散行列Ｒ_ｋは６行６列の正方行列である。一方、判定部２２０の判定結果が否定である場合、観測ノイズｖ_ｋは、３次元磁気センサ７０のノイズを表す３次元のベクトルであり、共分散行列Ｒ_ｋは３行３列の正方行列である。
式（１０）に示すように、観測モデルは、時刻Ｔ＝ｋにおける姿勢ｑ_ｋを用いて、時刻Ｔ＝ｋにおける観測値ベクトルｙ_ｋを推定するものである。上述のとおり、観測モデルを用いて推定した観測値ベクトルを、推定観測値ベクトルｙ⁻ _ｋと称する。
なお、非線形関数ｈの詳細については、後述する。

時刻Ｔ＝ｋにおける観測残差ｅ_ｋは、観測値ベクトルｙ_ｋと推定観測値ベクトルｙ⁻ _ｋとの差分を表すベクトルであり、以下に示す式（１１）で表される。式（１０）及び式（１１）から明らかなように、観測残差ｅ_ｋは、判定部２２０の判定結果が肯定である場合は６次元のベクトルであり、判定部２２０の判定結果が否定の場合は３次元のベクトルである。
カルマンフィルタは、以下の式（１２）に示すように、観測残差ｅ_ｋ、及び、式（１３）に示すカルマンゲインＫ_ｋを用いて、状態ベクトルｘ_ｋを、予測状態ベクトルｘ⁻ _ｋから状態ベクトルｘ^＋ _ｋへと更新する。また、カルマンフィルタは、以下の式（１４）に示すように、状態ベクトルｘ_ｋの推定誤差の共分散行列Ｐ_ｋを更新する。ここで、Ｐ⁻ _ｋは、予測状態ベクトルｘ⁻ _ｋの推定誤差の共分散行列であり、Ｐ^＋ _ｋは、更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散行列である。また、Ｐ^ｙｙ _ｋは、観測残差ｅ_ｋの共分散行列であり、Ｐ^ｘｙ _ｋは、予測状態ベクトルｘ⁻ _ｋと推定観測値ベクトルｙ⁻ _ｋとの相互共分散行列である。

図４に、カルマンフィルタ部２６０の機能ブロック図を示す。カルマンフィルタ部２６０は、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより、式（６）〜式（１４）に示した非線形カルマンフィルタの演算を実行する。

図４に示すように、初期化部２６１は、加算器２７２が出力する更新後の状態ベクトルｘ^＋ _ｋを単位時間だけ遅延させた状態ベクトルｘ^＋ _ｋ−１の各要素を「０」に設定して、これを出力する。すなわち、初期化部２６１は、実質的には、ゼロベクトルＯ_３＝［０，０，０］^Ｔを生成してこれを出力する。
また、初期化部２６１は、更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散行列Ｐ^＋ _ｋを単位時間遅延させることで、状態ベクトルｘ^＋ _ｋ−１の推定誤差の共分散行列Ｐ^＋ _ｋ−１を生成し、これらを出力する。

シグマポイント生成部２６２は、共分散行列Ｐ^＋ _ｋ−１、及び、ゼロベクトルＯ_３（つまり、各要素が「０」に設定された状態ベクトルｘ^＋ _ｋ−１）に基づいて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ−１（ｉ）（但し、ｉ＝１，２，３，…，２ｄｉｍ（ｘ））を生成する。ここで、シグマポイントχ_ｋ−１（ｉ）の各々は３次元ベクトルであり、シグマポイントχ_ｋ−１（０）は各要素が「０」に設定された状態ベクトルｘ^＋ _ｋ−１（すなわち、ゼロベクトルＯ_３）である。また、「ｄｉｍ（ｘ）」は、状態ベクトルｘ_ｋの次元を表す整数、すなわち「３」である。つまり、シグマポイント生成部２６２は、７個のシグマポイントを生成する。このようなシグマポイントの生成は、公知の方法、例えば非特許文献１に記載された方法等を適宜用いて行えばよい。
また、シグマポイント生成部２６２は、３次元のベクトルである「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ−１（ｉ）の各々の表現形式を、クォータニオンに変換することで、「２ｄｉｍ（ｘ）＋１」個のシグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）に変換する。シグマポイントχ_ｋ−１（ｉ）からシグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）へと変換する演算、すなわち、姿勢の表現形式をMRPsからクォータニオンに変換する演算は、公知の方法、例えば非特許文献１に記載された方法等を適宜用いて行えばよい。
シグマポイントχ_ｋ−１（０）から変換されたシグマポイントδｑ^＋ _{χ，ｋ−１}（０）は、基準姿勢［０、０、０、１］^Ｔを表す。また、シグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）は、基準姿勢からの姿勢変化量を表す。

遅延部２７６は、姿勢更新部２７５が出力した時刻Ｔ＝ｋにおける推定姿勢ｑ^＋ _ｋを単位時間だけ遅延させた推定姿勢ｑ^＋ _ｋ−１を出力する。

姿勢演算部２６３は、以下の式（１５）に示すように、推定姿勢ｑ^＋ _ｋ−１とシグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）とに基づいて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）の各々対応する、「２ｄｉｍ（ｘ）＋１」個の姿勢ｑ^＋ _{χ，ｋ−１}（ｉ）を生成する。
ここで、式（１５）に表される演算子［×］は、クォータニオン積を意味する。クォータニオンは、式（４）からも明らかなように、姿勢そのものを表す以外に、姿勢の変化量（任意の軸を中心とする回転の大きさ）を表現するものでもある。そして、クォータニオン積は、一方のクォータニオンを姿勢と看做し、他方のクォータニオンを姿勢変化量と看做すことで、一方のクォータニオンで表された姿勢を、他方のクォータニオンで表された姿勢変化量だけ姿勢変化させる、公知の演算である。
上述のとおり、シグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）は基準姿勢からの姿勢変化量を表す。よって、式（１５）の、姿勢ｑ^＋ _{χ，ｋ−１}（ｉ）は、推定姿勢ｑ^＋ _ｋ−１を、シグマポイントδｑ^＋ _{χ，ｋ−１}（ｉ）で表される姿勢変化量だけ姿勢変化させた姿勢である。
基準姿勢を表すシグマポイントδｑ^＋ _{χ，ｋ−１}（０）は、姿勢変化が無いこと（姿勢変化量がゼロであること）を表す。よって、式（１６）に示すように、推定姿勢ｑ^＋ _ｋ−１をシグマポイントδｑ^＋ _{χ，ｋ−１}（０）により姿勢変化させた姿勢ｑ^＋ _{χ，ｋ−１}（０）は推定姿勢ｑ^＋ _ｋ−１に等しくなる。

状態遷移モデル演算部２６４（以下、単に「状態遷移モデル部」と称する場合がある）は、姿勢演算部２６３が生成した「２ｄｉｍ（ｘ）＋１」個の姿勢ｑ^＋ _{χ，ｋ−１}（ｉ）のそれぞれと、角速度ベクトルω_ｋ−１とを、式（６）に示す状態遷移モデルに適用して、「２ｄｉｍ（ｘ）＋１」個の姿勢予測値ｑ⁻ _χ，ｋ（ｉ）を生成する。
なお、姿勢ｑ^＋ _{χ，ｋ−１}（０）を式（６）に適用して得られる姿勢予測値ｑ⁻ _χ，ｋ（０）は、遅延部２７６が出力した推定姿勢ｑ^＋ _ｋ−１を式（６）に適用して得られる姿勢予測値ｑ⁻ _ｋと等しい。

差分姿勢演算部２６５は、状態遷移モデル演算部２６４が生成した姿勢予測値ｑ⁻ _χ，ｋ（ｉ）と、姿勢予測値ｑ⁻ _χ，ｋ（０）と、に基づいて、姿勢予測値ｑ⁻ _χ，ｋ（０）の示す姿勢から姿勢予測値ｑ⁻ _χ，ｋ（ｉ）の示す姿勢に姿勢変化させるときの姿勢変化量を演算する。当該姿勢変化量を求める演算は、公知の方法、例えば非特許文献１に記載された方法等を適宜用いて行えばよい。これにより、差分姿勢演算部２６５は、「２ｄｉｍ（ｘ）＋１」個の姿勢予測値ｑ⁻ _χ，ｋ（ｉ）のそれぞれを、姿勢予測値ｑ⁻ _χ，ｋ（０）からの姿勢の変化量である、「２ｄｉｍ（ｘ）＋１」個のシグマポイントδｑ⁻ _χ，ｋ（ｉ）に変換する。
そして、差分姿勢演算部２６５は、クォータニオンで表された「２ｄｉｍ（ｘ）＋１」個のシグマポイントδｑ⁻ _χ，ｋ（ｉ）のそれぞれの表現形式をMRPsに変換することで、「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ（ｉ）を生成する。姿勢の表現形式をクォータニオンからMRPsに変換する演算は、公知の方法、例えば非特許文献１に記載された方法等を適宜用いて行えばよい。

予測状態ベクトル生成部２６６は、差分姿勢演算部２６５が生成した「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ（ｉ）の加重平均である予測状態ベクトルｘ⁻ _ｋを生成する。なお、当該加重平均の演算は、公知の方法を適宜用いて行えばよい。

共分散生成部２６７は、予測状態ベクトルｘ⁻ _ｋの推定誤差の共分散行列Ｐ⁻ _ｋを、予測状態ベクトルｘ⁻ _ｋ、及び、シグマポイントχ_ｋ（ｉ）に基づいて生成する。この演算は、公知の方法を適宜用いて行えばよい。

観測モデル演算部２６８は、以下の式（１７）に示すように、状態遷移モデル演算部２６４が生成した「２ｄｉｍ（ｘ）＋１」個の姿勢予測値ｑ⁻ _χ，ｋ（ｉ）のそれぞれを、観測モデルに適用することで、「２ｄｉｍ（ｘ）＋１」個の推定観測値γ_ｋ（ｉ）を生成する。

ここで、観測モデルで用いられる非線形関数ｈの詳細について、説明する。
非線形関数ｈは、判定部２２０の判定結果が肯定の場合には、以下の式（１８）で表され、判定部２２０の判定結果が否定の場合には、以下の式（１９）で表される。
ここで、γ_ｍは、磁気ベクトルｍ_ｋの推定値であり、以下の式（２０）で表される。式（２０）に現れるベクトル^ＧＢｇは、地上に固定された座標系において地磁気Ｂｇの向き及び大きさを表すベクトルであり、以下の式（２１）で表される。式（２１）に現れる、値ｒは、地磁気Ｂｇの強さを表す値であり、値φは、地磁気Ｂｇの伏角を表す値である。また、式（２０）に現れる行列Ｂ（ｑ）は、携帯機器１が姿勢ｑである場合に、地上に固定された座標系において表現されたベクトルを、携帯機器１に固定された座標系において表現するための座標変換を行う行列であり、式（２２）により表される。
γ_ａは、加速度ベクトルａ_ｋの推定値であり、以下の式（２３）で表される。式（２３）に現れるベクトル^ＧＧ_ＲＶは、地上に固定された座標系において重力加速度の向き及び大きさを表したベクトルを、重力加速度の大きさｇで正規化した３次元のベクトルである。

推定観測値ベクトル生成部２６９は、観測モデル演算部２６８が生成した「２ｄｉｍ（ｘ）＋１」個の推定観測値γ_ｋ（ｉ）の加重平均である推定観測値ベクトルｙ⁻ _ｋを生成する。なお、当該加重平均の演算は、公知の方法を適宜用いて行えばよい。

観測モデル演算部２６８、及び、推定観測値ベクトル生成部２６９は、観測モデルに対して姿勢予測値ｑ⁻ _χ，ｋ（ｉ）を適用することで、推定観測値ベクトルｙ⁻ _ｋを生成する観測モデル部２８１として機能する。

減算器２７０は、観測値ベクトルｙ_ｋから、推定観測値ベクトルｙ⁻ _ｋを減算することで、観測残差ｅ_ｋを生成する。

カルマンゲイン生成部２７１は、推定観測値ベクトルｙ⁻ _ｋ、「２ｄｉｍ（ｘ）＋１」個の推定観測値γ_ｋ（ｉ）、及び、観測ノイズｖ_ｋの共分散行列Ｒ_ｋに基づいて、観測残差ｅ_ｋの共分散行列Ｐ^ｙｙ _ｋを生成する。また、カルマンゲイン生成部２７１は、予測状態ベクトルｘ⁻ _ｋ、推定観測値ベクトルｙ⁻ _ｋ、「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ_ｋ（ｉ）、及び、「２ｄｉｍ（ｘ）＋１」個の推定観測値γ_ｋ（ｉ）に基づいて、予測状態ベクトルｘ⁻ _ｋと推定観測値ベクトルｙ⁻ _ｋとの相互共分散行列Ｐ^ｘｙ _ｋを生成する。なお、カルマンゲイン生成部２７１が実行する、共分散行列Ｐ^ｙｙ _ｋ、及び、相互共分散行列Ｐ^ｘｙ _ｋを生成する具体的な演算は、公知の方法、例えば非特許文献１に記載された方法等を適宜用いて行えばよい。
次に、カルマンゲイン生成部２７１は、式（１３）に示すように、共分散行列Ｐ^ｙｙ _ｋと相互共分散行列Ｐ^ｘｙ _ｋとに基づいて、カルマンゲインＫ_ｋを生成する。カルマンゲインＫ_ｋは、判定部２２０の判定結果が肯定の場合には、３行６列の行列であり、判定部２２０の判定結果が否定の場合には、３行３列の行列である。
そして、カルマンゲイン生成部２７１は、カルマンゲインＫ_ｋ及び観測残差ｅ_ｋに基づいて、式（１２）の右辺第２項に示される３次元ベクトル「Ｋ_ｋｅ_ｋ」を生成するとともに、
カルマンゲインＫ_ｋ及び共分散行列Ｐ^ｙｙ _ｋに基づいて、式（１４）の右辺第２項に示される３行３列の行列「Ｋ_ｋＰ^ｙｙ _ｋＫ_ｋ ^Ｔ」を生成する。

加算器２７２は、式（１２）に示すように、予測状態ベクトルｘ⁻ _ｋに、カルマンゲイン生成部２７１が算出したベクトル「Ｋ_ｋｅ_ｋ」を加算することで、更新後の状態ベクトルｘ^＋ _ｋを生成する。

推定姿勢誤差演算部２７３は、MRPsを用いて表現された更新後の状態ベクトルｘ^＋ _ｋの表現形式をクォータニオンに変換することで、推定姿勢誤差δｑ^＋ _ｋを生成する。

このように、差分姿勢演算部２６５、予測状態ベクトル生成部２６６、減算器２７０、カルマンゲイン生成部２７１、加算器２７２、及び、推定姿勢誤差演算部２７３は、観測値ベクトルｙ_ｋに基づいて観測残差ｅ_ｋを生成するとともに、観測残差ｅ_ｋと姿勢予測値ｑ⁻ _χ，ｋ（ｉ）とに基づいて、推定姿勢誤差δｑ^＋ _ｋを生成する誤差推定部２８２として機能する。
なお、以下では、観測モデル部２８１、及び、誤差推定部２８２を、「予測値補正部」と総称する場合がある。

減算器２７４は、式（１４）に示すように、予測状態ベクトルｘ⁻ _ｋの推定誤差の共分散行列Ｐ⁻ _ｋから、カルマンゲイン生成部２７１が算出した行列「Ｋ_ｋＰ^ｙｙ _ｋＫ_ｋ ^Ｔ」を減算することで、更新後の状態ベクトルｘ^＋ _ｋの推定誤差の共分散行列Ｐ^＋ _ｋを生成する。

姿勢更新部２７５は、状態遷移モデル演算部２６４で生成された姿勢予測値ｑ⁻ _χ，ｋ（０）と、推定姿勢誤差δｑ^＋ _ｋとに基づいて、推定姿勢ｑ^＋ _ｋを生成する。姿勢更新部２７５が生成する推定姿勢ｑ^＋ _ｋは、携帯機器１の姿勢ｑの推定値として出力される。
推定姿勢ｑ^＋ _ｋを生成するための演算は、判定部２２０の判定結果により異なる手順となる。そこで、以下では、判定部２２０の判定結果が肯定である場合、及び、否定である場合のそれぞれについて、推定姿勢ｑ^＋ _ｋを生成する手順を個別に説明する。

まず、判定部２２０の判定結果が肯定である場合、姿勢更新部２７５は、以下の式（２５）に示すように、推定姿勢誤差δｑ^＋ _ｋと姿勢予測値ｑ⁻ _χ，ｋ（０）とのクォータニオン積として、推定姿勢ｑ^＋ _ｋを算出する。

上述のとおり、推定姿勢誤差δｑ^＋ _ｋは、姿勢予測値ｑ⁻ _χ，ｋ（０）と真の姿勢ｑとの差分を、３次元磁気センサ７０からの出力値と３次元加速度センサ８０からの出力値とを用いて推定した値である。また、姿勢予測値ｑ⁻ _χ，ｋ（０）は、状態遷移モデルを用いて、３次元角速度センサ９０からの出力値に基づいて求めた姿勢ｑの予測値である。すなわち、カルマンフィルタ部２６０は、３次元磁気センサ７０、３次元加速度センサ８０、及び、３次元角速度センサ９０の全てのセンサからの出力値を用いて、推定姿勢ｑ^＋ _ｋを推定する。
このように、カルマンフィルタ部２６０は、判定部２２０の判定結果が肯定である場合、３つのセンサからの出力値を統合して姿勢を推定する演算を行うため、姿勢を正確に推定することができる。

他方、判定部２２０の判定結果が否定である場合、姿勢更新部２７５は、以下の手順に従い推定姿勢ｑ^＋ _ｋを算出する。なお、上述のとおり、クォータニオンは、姿勢そのものを表すことも、姿勢の変化量を表すこともできる。以下の演算で登場するクォータニオンｑ_Ａ〜ｑ_Ｃについても、その両方の意味で使用される。
第１に、姿勢更新部２７５は、以下の式（２６）に従い、クォータニオンｑ_Ａを生成する。クォータニオンｑ_Ａは、姿勢予測値ｑ⁻ _χ，ｋ（０）を、推定姿勢誤差δｑ^＋ _ｋにより姿勢変化させた姿勢を表すクォータニオンである。
第２に、姿勢更新部２７５は、以下の式（２７）に従い、クォータニオンｑ_Ｂを生成する。式（２７）に現れる［ｑ⁻ _χ，ｋ（０）］^−１は、姿勢予測値ｑ⁻ _χ，ｋ（０）の逆クォータニオンである。
逆クォータニオンとは、クォータニオンが表す姿勢変化とは逆の姿勢変化を表す４次元数である。具体的には、例えば、クォータニオンｑ_０が、姿勢ｑ_１から姿勢ｑ_２への姿勢変化を表している場合、逆クォータニオン［ｑ_０］^−１は、姿勢ｑ_２から姿勢ｑ_１への姿勢変化を表す。
すなわち、クォータニオンｑ_Ｂは、クォータニオンｑ_Ａとして表される姿勢を、姿勢予測値ｑ⁻ _χ，ｋ（０）により表される姿勢変化とは逆向きに姿勢変化させた姿勢を表す。換言すれば、クォータニオンｑ_Ａは、姿勢予測値ｑ⁻ _χ，ｋ（０）で表される姿勢を、推定姿勢誤差δｑ^＋ _ｋにより姿勢変化させた姿勢であるのに対して、クォータニオンｑ_Ｂは、基準姿勢＝［０、０、０、１］^Ｔを、推定姿勢誤差δｑ^＋ _ｋにより姿勢変化させた姿勢（つまり、推定姿勢誤差δｑ^＋ _ｋそのもの）を表す。
第３に、姿勢更新部２７５は、クォータニオンｑ_Ｃ（以下、「抽出推定姿勢誤差」と称する場合がある）を生成する。クォータニオンｑ_Ｃは、図５に示すように、クォータニオンｑ_Ｂにより示される姿勢変化のうち、地上に固定された座標系Σ_ＧのＺ軸（つまり、重力加速度方向に延在する軸）からの傾きを表す成分を取り除き、当該Ｚ軸を回転軸とする回転成分を抽出したクォータニオンである。
より具体的には、クォータニオンｑ_Ｂの各成分を、以下の式（２８）で表したときに、クォータニオンｑ_Ｂの成分ｑ_Ｂ４が０以上の場合には、クォータニオンｑ_Ｃは、式（２９）に従って算出され、クォータニオンｑ_Ｂの成分ｑ_Ｂ４が０よりも小さい場合には、クォータニオンｑ_Ｃは、式（３０）に従って算出される。
第４に、姿勢更新部２７５は、式（３１）に従い、姿勢予測値ｑ⁻ _χ，ｋ（０）とクォータニオンｑ_Ｃとのクォータニオン積として、推定姿勢ｑ^＋ _ｋを算出する。

判定部２２０の判定結果が否定である場合、観測値ベクトルｙには、加速度ベクトルａ_ｋは含まれず、磁気ベクトルｍ_ｋのみが含まれる。すなわち、判定部２２０の判定結果が否定である場合、観測モデルにおいて、加速度ベクトルａ_ｋの推定値γ_ａは算出されず、磁気ベクトルｍ_ｋの推定値γ_ｍのみが推定される。

式（２３）に示す加速度ベクトルａ_ｋの推定値γ_ａは、地上に固定された座標系において重力加速度方向を表すベクトル^ＧＧ_ＲＶを携帯機器１に固定された座標系において表したベクトル^ＳＧ_ＲＶの推定値である。
加速度ベクトルａ_ｋとベクトル^ＳＧ_ＲＶとが一致していると看做すことができる場合、加速度ベクトルａ_ｋより、重力加速度方向を知ることができる。よって、この場合、観測残差ｅ_ｋ（より具体的には、加速度ベクトルａ_ｋの推定値γ_ａと加速度ベクトルａ_ｋの差分を表す成分）に基づいて、カルマンフィルタ部２６０が推定する携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分を、真の傾きに近づくように更新することができる。すなわち、判定部２２０の判定結果が肯定であり、加速度ベクトルａ_ｋとベクトル^ＳＧ_ＲＶとが一致していると看做すことができれば、加速度ベクトルａ_ｋは、携帯機器１の姿勢ｑのうち重力加速度方向からの傾きを表す成分の推定に大きく貢献する。

しかし、判定部２２０の判定結果が否定である場合、加速度ベクトルａ_ｋとベクトル^ＳＧ_ＲＶとは大きく異なることがある。この場合、加速度ベクトルａ_ｋに基づいて、重力加速度方向を知ることはできない。よって、この場合、カルマンフィルタ部２６０が推定する携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分は、真の傾きからは大きく異なる傾きを表すように更新されることになる。
これに対して本実施形態に係るカルマンフィルタ部２６０は、判定部２２０の判定結果が否定である場合、加速度ベクトルａ_ｋを算出せずに、携帯機器１の姿勢ｑを推定する。従って、加速度ベクトルａ_ｋとベクトル^ＳＧ_ＲＶとは大きく異なる場合であっても、携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分が、真の傾きからは大きく異なる傾きを表すように更新されることを防止することが可能となる。

一方、磁気ベクトルｍ_ｋの推定値γ_ｍは、地上に固定された座標系において地磁気Ｂｇの方向及び大きさを表すベクトル^ＧＢｇを、携帯機器１に固定された座標系において表したベクトル^ＳＢｇ（ｑ）の推定値である。そして、本実施形態では、式（２１）に示すように、地磁気Ｂｇの方向及び大きさを表すベクトル^ＧＢｇは、地磁気Ｂｇの伏角φをパラメータとして用いて表現される。そのため、磁気ベクトルｍ_ｋとベクトル^ＳＢｇ（ｑ）とが一致していると看做すことができる場合、磁気ベクトルｍ_ｋは、地磁気Ｂｇの伏角φを正確に反映した値となる。
また、上述のとおり、地磁気Ｂｇの鉛直成分（重力加速度方向の成分）は、伏角φにより定められる。よって、磁気ベクトルｍ_ｋが、地磁気Ｂｇの伏角φを正確に反映した値となる場合には、磁気ベクトルｍ_ｋから求められる地磁気Ｂｇの鉛直成分の方向と重力加速度方向とが一致する。すなわち、伏角φを正確に知ることができれば、地磁気Ｂｇの向きから、重力加速度方向を知ることができる。この場合、磁気ベクトルｍ_ｋと磁気ベクトルｍ_ｋの推定値γ_ｍとに基づいて、携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分を、真の傾きに近づくように更新することができる。
しかし、上述のとおり、携帯機器１の近傍に磁界を発生させる物体が存在し、磁気データｍの示す値（磁気ベクトルｍ_ｋ）に、当該物体が発する磁界がノイズとして重畳する場合、磁気ベクトルｍ_ｋとベクトル^ＳＢｇ（ｑ）とが大きく乖離することがある。この場合、磁気ベクトルｍ_ｋは、地磁気Ｂｇの伏角φを正確に反映した値とはならず、磁気ベクトルｍ_ｋから求められる地磁気Ｂｇの鉛直成分の方向と重力加速度方向とが大きく乖離する。よって、この場合、携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分が、真の傾きから異なる傾きを表すように更新されることになる。すなわち、磁気ベクトルｍ_ｋのみに基づいて携帯機器１の姿勢ｑを推定しても、姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分が正しく推定されない可能性がある。

これに対して本実施形態に係る姿勢更新部２７５は、判定部２２０の判定結果が否定である場合、磁気ベクトルｍ_ｋと磁気ベクトルｍ_ｋの推定値γ_ｍとに基づいて生成される推定姿勢誤差δｑ^＋ _ｋから、重力加速度方向に延在する軸からの傾きを表す成分を取り除くことで当該軸を回転軸とする回転成分を抽出したクォータニオンｑ_Ｃを用いて、推定姿勢ｑ^＋ _ｋを算出する。これにより、姿勢更新部２７５は、判定部２２０の判定結果が否定である場合に、磁気ベクトルｍ_ｋに基づいて、携帯機器１の姿勢ｑのうち重力加速度方向からの傾きを表す成分を推定することを防止する。
従って、磁気ベクトルｍ_ｋとベクトル^ＳＢｇ（ｑ）とは大きく異なり、磁気ベクトルｍ_ｋが地磁気Ｂｇの伏角φを正確に反映した値とならない場合であっても、携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分が、真の傾きからは大きく異なる傾きを表すように更新されることを防止することが可能となる。
よって、本実施形態に係る姿勢推定装置は、センサが検出対象となる成分以外のノイズ成分を検出する場合であっても、真の姿勢とは大きくかけ離れた姿勢の推定を防止することができる。

＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。

（１）変形例１
上述した実施形態に係る携帯機器１は、シグマポイントカルマンフィルタを用いて非線形カルマンフィルタの演算を実行するものであったが、本発明はこのような形態に限定されるものでは無く、拡張カルマンフィルタ等、公知の非線形カルマンフィルタを適宜適用して演算を行うものでもよい。

（２）変形例２
上述した実施形態及び変形例において、姿勢推定部２００は、観測値ベクトル生成部２４０を備えるが、これを備えないものであってもよい。すなわち、観測値ベクトルｙ_ｋは、常に、加速度ベクトルａ_ｋと磁気ベクトルｍ_ｋとを要素とするものであってもよい。
本発明に係るカルマンフィルタ部２６０は、判定部２２０の判定結果に基づいて、推定姿勢ｑ^＋ _ｋを算出する。すなわち、判定部２２０の判定結果が否定である場合には、推定姿勢誤差δｑ^＋ _ｋのうち、重力加速度方向に延在する軸を回転軸とする回転成分を抽出したクォータニオンｑ_Ｃを用いて、推定姿勢ｑ^＋ _ｋを算出する。従って、仮に、判定部２２０の判定結果が否定であり、加速度ベクトルａ_ｋとベクトル^ＳＧ_ＲＶとは大きく異なる場合であっても、携帯機器１の姿勢ｑの推定値のうち重力加速度方向からの傾きを表す成分が、真の傾きからは大きく異なる傾きを表すように更新されることを防止することができる。

（３）変形例３
上述した実施形態及び変形例において、携帯機器１は、３次元磁気センサ７０、３次元加速度センサ８０、及び３次元角速度センサ９０を備えるが、本発明はこのような形態に限定されるものではなく、これら３種類のセンサ以外のセンサを備えるものであってもよい。また、これら３種類のセンサのうち３次元加速度センサ８０を少なくとも含む複数のセンサを備えるものであってもよい。
つまり、本発明に係る姿勢推定装置は、異種の物理量を測定する複数のセンサを備える携帯機器１に設けられ、これら複数のセンサの出力を統合しシステムの状態を推定するカルマンフィルタの演算を行うものであればよい。

１…携帯機器、１０…ＣＰＵ、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…姿勢推定プログラム、２００…姿勢推定部、２２０…判定部、２４０…観測値ベクトル生成部、２６０…カルマンフィルタ部、２６４…状態遷移モデル演算部、２６８…観測モデル演算部、２６９…推定観測値ベクトル生成部、２７５…姿勢更新部、２８１…観測モデル部、２８２…誤差推定部。

Claims

３方向の加速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元加速度センサを含む複数のセンサを備える機器に設けられ、当該機器の姿勢を推定する姿勢推定装置であって、
前記３次元加速度センサからの出力値に基づいて、前記機器の動きが安定しているか否かを判定する判定部と、
前記判定部の判定結果と、前記複数のセンサのうち少なくとも一部のセンサの出力値の各々を要素とする観測値ベクトルとに基づいて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢の推定値を算出するカルマンフィルタ部と、
を備え、
前記カルマンフィルタ部は、
前記機器の姿勢の経時的な変化を表す状態遷移モデルを用いて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢を予測する状態遷移モデル部と、
前記状態遷移モデル部の予測結果と前記観測値ベクトルとに基づいて、前記状態遷移モデル部の予測結果と前記ある時刻よりも単位時間経過後の時刻における真の姿勢との差分を推定した値である推定姿勢誤差を生成する予測値補正部と、
前記判定部の判定結果が肯定である場合には、前記推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出し、前記判定部の判定結果が否定である場合には、前記推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出する姿勢更新部と、
を具備する、ことを特徴とする姿勢推定装置。
３方向の磁気成分をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元磁気センサと、
３方向の加速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元加速度センサと、
３方向にそれぞれ延在する３軸の周りの角速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元角速度センサと、
を備える機器に設けられ、
当該機器の姿勢を推定する姿勢推定装置であって、
前記３次元加速度センサからの出力値に基づいて、前記機器の動きが安定しているか否かを判定する判定部と、
前記判定部の判定結果が肯定である場合には、前記３次元磁気センサからの出力値と前記３次元加速度センサからの出力値とを要素とする観測値ベクトルを生成し、前記判定部の判定結果が否定である場合には、前記３次元磁気センサからの出力値を要素とする観測値ベクトルを生成する観測値ベクトル生成部と、
前記判定部の判定結果と、前記観測値ベクトルとに基づいて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢の推定値を算出するカルマンフィルタ部と、
を備え、
前記カルマンフィルタ部は、
前記機器の姿勢の経時的な変化を表す状態遷移モデルに対して、ある時刻の姿勢の推定値と前記３次元角速度センサからの出力値とを適用することで、当該ある時刻よりも単位時間経過後の時刻における姿勢を予測して、これを姿勢予測値として出力する状態遷移モデル部と、
前記観測値ベクトルの各要素の値と前記機器の姿勢との関係を表す観測モデルに対して、前記姿勢予測値を適用することで、当該ある時刻よりも単位時間経過後の時刻における観測値ベクトルの各要素の値を推定して、これを推定観測値ベクトルとして出力する観測モデル部と、
前記推定観測値ベクトルと、前記観測値ベクトルとの差分である観測残差を算出するとともに、算出した観測残差と、前記姿勢予測値とに基づいて、前記ある時刻よりも単位時間経過後の時刻における真の姿勢と前記姿勢予測値との差分を推定した値である推定姿勢誤差を生成する誤差推定部と、
前記判定部の判定結果が肯定である場合には、前記推定姿勢誤差と、前記姿勢予測値とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出し、前記判定部の判定結果が否定である場合には、前記推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差と、前記姿勢予測値とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出する姿勢更新部と、
を具備する、ことを特徴とする姿勢推定装置。
前記判定部は、
前記３次元加速度センサからの出力値を表すベクトルの大きさと、重力加速度の大きさとの差分値の絶対値が、所定の値以下である場合に、前記機器の動きが安定していると判定する、
ことを特徴とする、請求項１または２に記載の姿勢推定装置。
３方向の加速度をそれぞれ検出して、検出結果を３軸の座標系におけるベクトルデータとして順次出力する３次元加速度センサを含む複数のセンサを備える機器に設けられ、当該機器の姿勢を推定する姿勢推定装置を制御するコンピュータのプログラムであって、
前記コンピュータを、
前記３次元加速度センサからの出力値に基づいて、前記機器の動きが安定しているか否かを判定する判定部と、
前記判定部の判定結果と、前記複数のセンサのうち少なくとも一部のセンサの出力値の各々を要素とする観測値ベクトルとに基づいて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢の推定値を算出するカルマンフィルタ部と、
して機能させ、
更に、
前記カルマンフィルタ部を、
前記機器の姿勢の経時的な変化を表す状態遷移モデルを用いて、ある時刻の姿勢の推定値から、当該ある時刻よりも単位時間経過後の時刻における姿勢を予測する状態遷移モデル部と、
前記状態遷移モデル部の予測結果と前記観測値ベクトルとに基づいて、前記状態遷移モデル部の予測結果と前記ある時刻よりも単位時間経過後の時刻における真の姿勢との差分を推定した値である推定姿勢誤差を生成する予測値補正部と、
前記判定部の判定結果が肯定である場合には、前記推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出し、前記判定部の判定結果が否定である場合には、前記推定姿勢誤差から重力加速度方向に延在する軸を回転軸とする回転成分を抽出した抽出推定姿勢誤差と、前記状態遷移モデル部の予測結果とに基づいて、前記ある時刻から単位時間経過後の姿勢の推定値を算出する姿勢更新部と、
して機能させる、
ことを特徴とするプログラム。