(57)【要約】 【課題】処理負荷を抑えつつ、姿勢を推定する。【解決手段】３方向の磁気成分を周期的に検出して検出結果を磁気データｍとして順次出力する磁気センサ７０と、３方向の角速度を周期的に検出して検出結果を角速度データωとして順次出力する角速度センサ９０と、を備えた携帯機器１に設けられ、携帯機器１の姿勢ｑを推定する姿勢推定装置であって、角速度データωに基づいて、現時刻よりも過去の時刻における携帯機器１の姿勢ｑから、現時刻における携帯機器１の姿勢ｑを予測する姿勢予測部２１と、磁気データｍに基づいて、姿勢予測部２１の予測結果を補正する姿勢補正部２２と、姿勢予測部２１の予測結果または姿勢補正部２２の補正結果のうち何れか一方を、携帯機器１の姿勢ｑの推定値として出力する推定姿勢出力部２３と、を備える、ことを特徴とする姿勢推定装置。【選択図】図４
2016109609.tif 000002
【特許請求の範囲】
【請求項１】
３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、
３方向の磁気成分を周期的に検出して検出結果を磁気データとして順次出力する磁気センサと、
を備えた機器に設けられた姿勢推定装置であって、
前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、
前記磁気データに基づいて、前記予測部の予測結果を補正する補正部と、
を備える、
ことを特徴とする姿勢推定装置。
【請求項２】
前記磁気データが地磁気を示すか否かを判定する判定部を備え、
前記補正部は、
前記判定部の判定結果が肯定の場合に、前記予測部の予測結果を補正する、
ことを特徴とする、請求項１に記載の姿勢推定装置。
【請求項３】
前記補正部は、
現時刻に前記磁気センサが出力する磁気データの示す値と、前記機器が前記予測部の予測結果の示す姿勢である場合に前記磁気センサが出力する磁気データの示す値との、加重平均値に基づいて、前記予測部の予測結果を補正する、
ことを特徴とする、請求項１または２に記載の姿勢推定装置。
【請求項４】
３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、
３方向の磁気成分を周期的に検出して検出結果を磁気データとして順次出力する磁気センサと、
コンピュータと、
を備えた機器に設けられた姿勢推定装置の制御プログラムであって、
前記コンピュータを、
前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、
前記磁気データに基づいて、前記予測部の予測結果を補正する補正部と、
して機能させる、
ことを特徴とする姿勢推定装置の制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は姿勢推定装置及び姿勢推定装置の制御プログラムに関する。
【背景技術】
【０００２】
物体の姿勢等、動的システムの状態を推定する演算を実行する場合、１種類のセンサの出力結果に基づいて算出するよりも、複数のセンサの出力結果を統合して算出した方が、短時間に正確な推定をすることができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し動的システムの状態を推定する方法として、カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、非線形カルマンフィルタの１種であるシグマポイントカルマンフィルタを用いて、３軸の角速度センサ及び３軸の地磁気センサ等の複数のセンサからの出力を統合することで、姿勢を含む動的システムの状態を推定する推定装置が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１２−１３２７１３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、カルマンフィルタの演算では、姿勢を含むシステムの状態を周期的に推定する演算を実行するのに加えて、当該状態の推定値の推定誤差の共分散を周期的に演算することが必要となる。このため、カルマンフィルタを用いて姿勢を推定する場合には、処理負荷が大きくなるという問題が存在した。また、カルマンフィルタの演算では、状態モデルを用いて姿勢を推定するため、当該状態モデルが正確でない場合には、正確な姿勢の推定も困難になるという問題が存在した。
【０００６】
本発明は、上述した点に鑑みてなされたものであり、処理負荷を抑えつつ、姿勢を推定する技術を提供することを、解決課題の一つとする。
【課題を解決するための手段】
【０００７】
上述した課題を解決するため、本発明に係る姿勢推定装置は、３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、３方向の磁気成分を周期的に検出して検出結果を磁気データとして順次出力する磁気センサと、を備えた機器に設けられた姿勢推定装置であって、前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、前記磁気データに基づいて、前記予測部の予測結果を補正する補正部と、を備える、ことを特徴とする。
【０００８】
この発明に係る姿勢推定装置によれば、角速度データに基づいて機器の姿勢の予測値を算出し、磁気データに基づいて機器の姿勢の予測値を補正することで、機器の姿勢を推定する。すなわち、本発明にかかる姿勢推定装置では、機器の姿勢の推定値の推定誤差の共分散を算出することなく、機器の姿勢を推定する。よって、本発明にかかる姿勢推定装置では、カルマンフィルタ等のように、推定値の推定誤差の共分散を算出しつつ姿勢を推定する場合と比較して、計算負荷を軽減することができる。
また、この発明によれば、磁気データ及び角速度データの双方に基づいて機器の姿勢を推定することができるため、磁気データまたは角速度データのいずれか一方に基づいて機器の姿勢を推定する場合と比較して、機器の真の姿勢に近い推定値を算出することができる。
【０００９】
上述した姿勢推定装置において、前記磁気データが地磁気を示すか否かを判定する判定部を備え、前記補正部は、前記判定部の判定結果が肯定の場合に、前記予測部の予測結果を補正する、ことを特徴とすることが好ましい。
この態様によれば、磁気データに地磁気以外のノイズが重畳している場合に、磁気データを用いた補正の演算を実行しないことができる。このため、磁気データに重畳するノイズが、機器の姿勢の推定に対して及ぼす影響を小さくすることができる。すなわち、この態様によれば、判定部の判定結果に関わらず補正の演算を実行する場合と比較して、精度の高い姿勢の推定が可能となる。
【００１０】
上述した姿勢推定装置において、前記補正部は、現時刻に前記磁気センサが出力する磁気データの示す値と、前記機器が前記予測部の予測結果の示す姿勢である場合に前記磁気センサが出力する磁気データの示す値との、加重平均値に基づいて、前記予測部の予測結果を補正する、ことを特徴とすることが好ましい。
この態様によれば、磁気データ及び角速度データの双方に基づいて機器の姿勢を推定するため、磁気データまたは角速度データのいずれか一方に基づいて機器の姿勢を推定する場合と比較して、精度の高い姿勢の推定が可能となる。
【００１１】
また、本発明に係る姿勢推定装置の制御プログラムは、３方向の角速度を周期的に検出して検出結果を角速度データとして順次出力する角速度センサと、３方向の磁気成分を周期的に検出して検出結果を磁気データとして順次出力する磁気センサと、コンピュータと、を備えた機器に設けられた姿勢推定装置の制御プログラムであって、前記コンピュータを、前記角速度データに基づいて、現時刻よりも過去の時刻における前記機器の姿勢から、現時刻における前記機器の姿勢を予測する予測部と、前記磁気データに基づいて、前記予測部の予測結果を補正する補正部と、して機能させる、ことを特徴とする。
【００１２】
この発明によれば、カルマンフィルタ等のように推定値の推定誤差の共分散を算出しつつ姿勢を推定する場合と比較して、計算負荷を抑えつつ、機器の姿勢を推定することができる。また、この発明によれば、磁気データまたは角速度データのいずれか一方に基づいて機器の姿勢を推定する場合と比較して、機器の姿勢を精度良く推定することができる。
【図面の簡単な説明】
【００１３】
【図１】本発明の実施形態に係る携帯機器１の構成を示すブロック図である。
【図２】携帯機器１の外観を示す斜視図である。
【図３】姿勢推定部２を表すブロック図である。
【図４】演算部２０を表すブロック図である。
【図５】補正磁気ベクトルｍＣを説明するための説明図である。
【発明を実施するための形態】
【００１４】
以下、本発明を実施するための形態について図面を参照して説明する。ただし、各図において、各部の寸法及び縮尺は、実際のものと適宜に異ならせてある。また、以下に述べる実施の形態は、本発明の好適な具体例であるから、技術的に好ましい種々の限定が付されているが、本発明の範囲は、以下の説明において特に本発明を限定する旨の記載がない限り、これらの形態に限られるものではない。
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００１５】
＜１．  機器構成及びソフトウェア構成について＞
図１は、本発明の実施形態に係る携帯機器１（「機器」の一例）のブロック図であり、図２は携帯機器１の外観を示す斜視図である。携帯機器１は、携帯機器１の姿勢に応じて携帯機器１が備える画面（後述する表示部６２）に表示されている地図等の画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいて、携帯機器１の姿勢を推定することによって実現される。
【００１６】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（Random Access Memory）５１、姿勢推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ（Read Only Memory）５２、通信を実行する通信部６１、画像を表示する表示部６２、及び、ＧＰＳ部６３を備える。また、携帯機器１は、地磁気を含む磁気を検出して磁気データｍを出力する磁気センサ７０と、角速度を検出して角速度データωを出力する角速度センサ９０を備える。
【００１７】
表示部６２は、ＣＰＵ１０が姿勢推定プログラム１００を実行することにより推定した携帯機器１の姿勢ｑの推定値に基づいて、方位等を表す矢印等の画像を表示する。ＧＰＳ部６３は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。
【００１８】
磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及び、Ｚ軸磁気センサ７３を備える。磁気センサには、例えば、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などの検知方式のセンサがある。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｍを単位時間毎に周期的に出力する。磁気データｍは、磁気センサ７０と一体となって運動する携帯機器１に固定された座標系において、Ｘ軸磁気センサ７１からの出力値をｘ軸成分として表し、Ｙ軸磁気センサ７２からの出力値をｙ軸成分として表し、Ｚ軸磁気センサ７３からの出力値をｚ軸成分として表す３次元のベクトルデータである。
磁気センサ７０が検出する磁気データｍには、地磁気Ｂを示す成分が含まれる。地磁気Ｂは、一般的に、磁北に向かう水平成分と伏角により定められる鉛直成分とを有する磁気であり、地上に固定された座標系において、一定の向き及び大きさを有する地磁気ベクトルＢＧとして表される。なお、本実施形態において、地上に固定された座標系は、Ｚ軸が鉛直方向を向き、Ｙ軸が磁北に向かう方向を向くように定められることを想定する。
【００１９】
角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及び、Ｚ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データωを単位時間毎に周期的に出力する。角速度データωは、携帯機器１に固定された座標系において、３方向に延在する３軸（Ｘ軸、Ｙ軸、Ｚ軸）の周りのそれぞれの角速度を示す３次元のベクトルデータである。
【００２０】
ＣＰＵ１０は、ＲＯＭ５２に格納されている姿勢推定プログラム１００を実行することによって、携帯機器１の姿勢ｑを単位時間毎に周期的に推定する。すなわち、携帯機器１に設けられたＣＰＵ１０は、姿勢推定プログラム１００を実行することにより、姿勢推定部２（「姿勢推定装置」の一例）として機能する。
【００２１】
＜２．  姿勢推定部について＞
図３は、姿勢推定部２の機能を表した機能ブロック図である。姿勢推定部２は、単位時間毎に出力される磁気データｍ及び角速度データωに基づいて、単位時間毎に携帯機器１の姿勢ｑを推定する。以下、姿勢推定部２が推定した携帯機器１の姿勢を、推定姿勢ｑＯＵＴと称する。
【００２２】
本実施形態では、携帯機器１の姿勢ｑを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢を表す４次元数である。例えば、携帯機器１に固定された座標系の各軸と、地上に固定された座標系の各軸とが一致する姿勢を基準姿勢と定義し、基準姿勢をｑ＝［０、０、０、１］Ｔと表現する。このとき、携帯機器１の任意の姿勢ｑは、地上に固定された座標系における３次元の単位ベクトルρの方向に延在する軸を回転軸として、基準姿勢である携帯機器１を角度ψだけ回転した姿勢として表現できる。すなわち、携帯機器１の姿勢ｑは、クォータニオンを用いて、以下の式（１）で表される。
【数１】
2016109609.tif 000003
【００２３】
図３に示すように、姿勢推定部２は、推定姿勢ｑＯＵＴを推定する演算を実行する演算部２０と、磁気データｍが地磁気Ｂを表すデータであると看做せるか否かを判定し、判定結果を示す判定情報ＪDを出力する判定部３０と、判定部３０の判定情報ＪDに基づいて、磁気データｍを演算部２０に対して供給するか否かを制御するスイッチＳＷと、を備える。
【００２４】
判定部３０は、上述のとおり、磁気データｍが地磁気Ｂを表すデータであると看做せるか否かを判定する。具体的には、判定部３０は、以下の式（２）に例示する条件、すなわち、「磁気データｍの示すベクトルの大きさと地磁気ベクトルＢＧの典型的な大きさＢＧ-Typicalとの差分値の絶対値が、所定の閾値εＭよりも小さい」という条件を充足するか否かを判定する。
なお、各種変数、ベクトル、行列等を表す符号に付された添え字ｋは、０≦ｋを満たす自然数であり、当該変数、ベクトル、行列等が時刻Ｔｋにおける値であることを意味する。ここで、時刻Ｔｋ＋１は時刻Ｔｋよりも単位時間だけ後の時刻を示し、時刻Ｔｋ−１は時刻Ｔｋよりも単位時間だけ過去の時刻を示すこととする。また、以下では、時刻Ｔｋにおける磁気データｍの示す３次元ベクトルを磁気ベクトルｍｋと称し、時刻Ｔｋにおける角速度データωの示す３次元ベクトルを角速度ベクトルωｋと称する。また、磁気ベクトルｍｋを、磁気センサ７０により観測される観測値と称し、角速度ベクトルωｋを、角速度センサ９０により観測される観測値と称する場合がある。
【数２】
2016109609.tif 000004
【００２５】
磁気センサ７０が地磁気Ｂのみを検出する場合には、磁気ベクトルｍｋは、携帯機器１の姿勢ｑに連動して向きを変える一定の大きさ（地磁気ベクトルＢＧと同じ大きさ）のベクトルとなる。
しかし、携帯機器１の近傍に、スピーカー等の磁界を発生させる物品や、各種金属等の着磁性を有する物品が存在する場合、これらの物品が発する磁界が、ノイズとして磁気データｍに重畳することがある。この場合、磁気ベクトルｍｋは、地磁気ベクトルＢＧを携帯機器１に固定された座標系で表したベクトルと、携帯機器１の近傍に存在する物品が発する磁界を携帯機器１に固定された座標系で表したベクトルとが重畳したベクトルとなる。すなわち、携帯機器１の近傍に磁界を発生させる物体が存在するときには、磁気ベクトルｍｋの大きさと地磁気ベクトルＢＧの大きさとは乖離することとなる。
携帯機器１の近傍の物品が発する磁界は、当該物品と携帯機器１との位置関係により様々な方向を向く。よって、磁気ベクトルｍｋに対して、携帯機器１の近傍の物品が発する磁界がノイズとして重畳する場合であって、当該ノイズの影響が大きい場合には、当該磁気ベクトルｍｋに基づいて姿勢ｑを推定することは適切ではない。
このため、本実施形態では、式（２）に示すように、磁気ベクトルｍｋの大きさと地磁気ベクトルＢＧの典型的な大きさＢＧ-Typicalとの差分の絶対値が閾値εＭよりも小さいか否かを判定することで、磁気ベクトルｍｋと、地磁気ベクトルＢＧを携帯機器１に固定された座標系で表したベクトルとを略同じと看做せるか否か、すなわち、携帯機器１の近傍の物品が発するノイズの影響が小さく、磁気ベクトルｍｋに基づいて姿勢ｑを推定することが適切であるか否かを判定する。
【００２６】
なお、地磁気ベクトルＢＧの大きさは携帯機器１の位置（緯度、経度）に応じて変化する。よって、本実施形態において、判定部３０は、式（２）に示すように、磁気ベクトルｍｋの大きさと、地磁気ベクトルＢＧの典型的な大きさＢＧ-Typicalとを比較する。
【００２７】
判定部３０は、式（２）に示す条件が充足される場合には、磁気データｍが地磁気Ｂを表すデータであると看做せると判定し、判定情報ＪDに対して、磁気データｍが地磁気Ｂを表すデータであると看做せることを示す値、例えば「１」を設定する。また、判定部３０は、式（２）に示す条件が充足されない場合には、磁気データｍに地磁気Ｂ以外のノイズが重畳していると判定し、判定情報ＪDに対して、磁気データｍに地磁気Ｂ以外のノイズが重畳していることを示す値、例えば「０」を設定する。
【００２８】
スイッチＳＷは、磁気センサ７０と演算部２０との間に設けられ、判定情報ＪDに基づいて、磁気データｍを演算部２０に対して供給するか否かを制御する。
具体的には、スイッチＳＷは、判定部３０における判定結果が肯定である場合、すなわち、判定部３０が出力する判定情報ＪDが「１」を示す場合、オンする。この場合、演算部２０は、磁気センサ７０が出力する磁気データｍと、角速度センサ９０が出力する角速度データωと、を用いて推定姿勢ｑＯＵＴを算出する。
一方、スイッチＳＷは、判定部３０における判定結果が否定である場合、すなわち、判定部３０が出力する判定情報ＪDが「０」を示す場合、オフする。この場合、演算部２０は、磁気センサ７０が出力する磁気データｍを用いずに、角速度センサ９０が出力する角速度データωを用いて推定姿勢ｑＯＵＴを算出する。
以下、演算部２０の動作について詳述する。
【００２９】
＜３．  演算部について＞
図４は、演算部２０の機能ブロック図である。この図に示すように、演算部２０は、姿勢予測部２１（「予測部」の一例）と、姿勢補正部２２（「補正部」の一例）と、推定姿勢出力部２３（「出力部」の一例）と、を備える。
【００３０】
姿勢予測部２１は、時刻Ｔｋ−１において出力される角速度データωの示す角速度ベクトルωｋ−１に基づいて、姿勢推定部２により推定された時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴから、時刻Ｔｋにおける携帯機器１の姿勢ｑの予測値（以下、「予測姿勢ｑＧ」と称する）を算出する。
姿勢補正部２２は、時刻Ｔｋにおいて出力される磁気データｍの示す磁気ベクトルｍｋに基づいて予測姿勢ｑＧを補正することで、時刻Ｔｋにおける携帯機器１の姿勢ｑの予測値の補正値（以下、「補正姿勢ｑＭ」と称する）を算出する。
推定姿勢出力部２３は、姿勢予測部２１が算出した予測姿勢ｑＧ、または、姿勢補正部２２が算出した補正姿勢ｑＭを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。
以下、これら姿勢予測部２１、姿勢補正部２２、及び、推定姿勢出力部２３について詳細に説明する。
【００３１】
上述のとおり、姿勢予測部２１は、角速度ベクトルωｋ−１に基づいて、時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴから、予測姿勢ｑＧを予測する演算を実行する。
なお、以下では、図４に示すように、時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴが、姿勢予測部２１による時刻Ｔｋにおける予測姿勢ｑＧの算出に用いられる場合には、説明の便宜上、当該推定姿勢ｑＯＵＴを予測前姿勢ｑＧ−ＩＮと称する場合がある。
【００３２】
姿勢予測部２１は、例えば、以下に示す回転運動の方程式（３）に従って、予測前姿勢ｑＧ−ＩＮから予測姿勢ｑＧを算出する。
ここで、式（３）の演算子Ωは、式（４）で定義される。また、式（４）の行列Ｉ３×３は、３行３列の単位行列である。また、式（４）の演算子Γは、３次元ベクトルＬ＝（Ｌ１，Ｌ２，Ｌ３）を用いて、式（５）で定義される。また、符号Δｔは、時刻Ｔｋ−１から時刻Ｔｋまでの単位時間を示す。また、式（４）の成分Ψｋは、式（６）で定義される。
【数３】
2016109609.tif 000005
【００３３】
上述のとおり、姿勢補正部２２は、磁気ベクトルｍｋに基づいて、予測姿勢ｑＧを補正し、補正姿勢ｑＭを算出する。
この姿勢補正部２２は、図４に例示するように、予測観測値演算部２２１と、補正観測値演算部２２２と、補正量演算部２２３と、補正姿勢演算部２２４と、を備える。以下、これら姿勢補正部２２の各構成要素について説明する。
【００３４】
予測観測値演算部２２１は、時刻Ｔｋにおける携帯機器１の姿勢が予測姿勢ｑＧである場合に、時刻Ｔｋにおいて磁気センサ７０により観測される観測値である磁気ベクトルｍｋを予測する演算を実行する。そして、予測観測値演算部２２１は、当該予測の結果を表す予測磁気ベクトルｍＯを出力する。
具体的には、予測観測値演算部２２１は、以下に示す式（７）に従って、予測磁気ベクトルｍＯを算出する。ここで、式（７）の行列Ｒ（ｑ）は、地上に固定された座標系から、姿勢ｑである携帯機器１に固定された座標系へと座標変換するための３行３列の行列であり、以下の式（８）で表される。また、式（７）の単位地磁気ベクトルＢＧＵは、地磁気ベクトルＢＧのうち水平成分を正規化したベクトルであり、式（９）に示すように、磁北を向く単位ベクトルとして表される。
【数４】
2016109609.tif 000006
【００３５】
上述のとおり、判定部３０における判定結果が肯定である場合、すなわち、磁気データｍが地磁気Ｂを表していると看做すことができる場合に、演算部２０に対して磁気ベクトルｍｋが供給される。また、上述のとおり、地磁気ベクトルＢＧの水平成分は磁北を向くため、地磁気ベクトルＢＧの水平成分を正規化した単位地磁気ベクトルＢＧＵも磁北を向く。このため、予測観測値演算部２２１は、式（７）に示すように、時刻Ｔｋにおける携帯機器１の姿勢ｑを予測姿勢ｑＧとしたうえで、単位地磁気ベクトルＢＧＵを携帯機器１に固定された座標系に座標変換し、当該座標変換により得られたベクトルを、磁気ベクトルｍｋの予測値である予測磁気ベクトルｍＯとする。
なお、上述のとおり、地磁気ベクトルＢＧは、磁北に向かう水平成分の他に、伏角により定められる鉛直成分を有する。しかし、伏角により定められる鉛直成分は、携帯機器１の位置（緯度、経度）に応じて変化する。このため、本実施形態では、演算部２０における演算を簡素化するために、地磁気ベクトルＢＧの鉛直成分を使用せず、地磁気ベクトルＢＧの水平成分のみを使用して、携帯機器１の姿勢ｑを推定する。
同様に、地磁気ベクトルＢＧの大きさは、上述のとおり、携帯機器１の位置（緯度、経度）に応じて変化する。このため、本実施形態では、演算部２０における演算を簡素化するために、地磁気ベクトルＢＧの大きさを考慮せず、地磁気ベクトルＢＧの水平成分を正規化した単位地磁気ベクトルＢＧＵを使用して、携帯機器１の姿勢ｑを推定する。
【００３６】
補正観測値演算部２２２は、予測観測値演算部２２１が算出した予測磁気ベクトルｍＯと、磁気センサ７０が供給する磁気データｍの示す磁気ベクトルｍｋと、に基づいて、補正磁気ベクトルｍＣを生成する。以下、補正観測値演算部２２２による補正磁気ベクトルｍＣの算出について説明する。
【００３７】
補正観測値演算部２２２は、磁気センサ７０から磁気データｍが供給されると、式（１０）に示すように、磁気データｍの示す磁気ベクトルｍｋを、行列Ｒの転置行列「ＲＴ」を用いて、地上に固定された座標系に変換することで、磁気ベクトルｍＧを算出する。
次に、補正観測値演算部２２２は、式（１１）に示すように、磁気ベクトルｍＧから鉛直成分を取り除くことで、水平磁気ベクトルｍＧＨを生成する。
次に、補正観測値演算部２２２は、式（１２）に示すように、水平磁気ベクトルｍＧＨを、携帯機器１に固定された座標系に変換することで、水平磁気ベクトルｍＨを算出する。
そして、補正観測値演算部２２２は、式（１３）に示すように、水平磁気ベクトルｍＨを正規化することで、単位水平磁気ベクトルｍＵを算出する。
【数５】
2016109609.tif 000007
【００３８】
補正観測値演算部２２２は、式（１４）に示すように、磁気ベクトルｍｋに基づいて生成した単位水平磁気ベクトルｍＵと、予測観測値演算部２２１が生成した予測磁気ベクトルｍＯと、を加重平均することで、補正磁気ベクトルｍＣを生成する。なお、式（１４）において、係数αＭは、０＜αＭ＜１を満たす実数である。
【数６】
2016109609.tif 000008
【００３９】
図５は、補正磁気ベクトルｍＣ、予測磁気ベクトルｍＯ、及び、単位水平磁気ベクトルｍＵの関係について説明するための説明図である。この図に示すように、補正磁気ベクトルｍＣは、単位水平磁気ベクトルｍＵ及び予測磁気ベクトルｍＯに垂直な単位ベクトルｄＭに垂直なベクトルであって、単位水平磁気ベクトルｍＵ及び予測磁気ベクトルｍＯの間の方向を向いたベクトルとなる。以下、この図に示すように、予測磁気ベクトルｍＯ及び補正磁気ベクトルｍＣのなす角度を、角度θＭと称する。
【００４０】
補正量演算部２２３は、補正磁気ベクトルｍＣと予測磁気ベクトルｍＯとに基づいて、予測姿勢ｑＧを補正するためのクォータニオンである姿勢変化量ＰＭを算出する。具体的には、補正量演算部２２３は、以下に示す式（１５）に基づいて、姿勢変化量ＰＭを算出する。なお、単位ベクトルｄＭは、式（１６）により求めることができる。式（１６）において、「ｍＯ×ｍＣ」は、予測磁気ベクトルｍＯと補正磁気ベクトルｍＣとのクロス積を意味する。
【数７】
2016109609.tif 000009
【００４１】
式（１）からも明らかなように、クォータニオンは、姿勢そのものを表すことができる他に、姿勢の変化量を表すことができる。例えば、姿勢の変化が、ある軸を中心とした回転である場合に、当該姿勢の変化量（例えば、姿勢変化量ＰＭ）を、軸の延在する方向（例えば、単位ベクトルｄＭ）と、当該軸を中心とする回転の大きさ（例えば、角度θＭ）と、を示すクォータニオンを用いて表すことができる。
すなわち、クォータニオンである姿勢変化量ＰＭは、予測磁気ベクトルｍＯを補正磁気ベクトルｍＣへと姿勢変化させる場合の、姿勢の変化量を表す。換言すれば、姿勢変化量ＰＭは、携帯機器１の姿勢ｑを、磁気センサ７０が観測する単位地磁気ベクトルＢＧＵが予測磁気ベクトルｍＯとなる予測姿勢ｑＧから、磁気センサ７０が観測する単位地磁気ベクトルＢＧＵが補正磁気ベクトルｍＣとなる補正姿勢ｑＭへと、変化させる場合の、携帯機器１の姿勢の変化量を示すクォータニオンである。
【００４２】
補正姿勢演算部２２４は、予測姿勢ｑＧを、姿勢変化量ＰＭだけ変化させることで、補正姿勢ｑＭを算出する。
なお、以下では、図４に示すように、予測姿勢ｑＧが、補正姿勢演算部２２４による補正姿勢ｑＭの算出に用いられる場合には、説明の便宜上、当該予測姿勢ｑＧを補正前姿勢ｑＭ−ＩＮと称する場合がある。
【００４３】
補正姿勢演算部２２４は、以下の式（１７）に従って、補正前姿勢ｑＭ−ＩＮを姿勢変化量ＰＭだけ変化させる補正を実行することで、補正姿勢ｑＭを算出する。
ここで、演算子［×］は、クォータニオン積である。クォータニオン積とは、２つのクォータニオンのうち、一方のクォータニオンを姿勢と看做し、他方のクォータニオンを姿勢変化量と看做すことで、一方のクォータニオンとして表された姿勢を、他方のクォータニオンとして表された姿勢変化量だけ姿勢変化させる演算である。
【数８】
2016109609.tif 000010
【００４４】
以上において説明したように、姿勢補正部２２は、角速度センサ９０からの出力値である角速度ベクトルωｋ−１に応じて定められる予測磁気ベクトルｍＯと、磁気センサ７０からの出力値である磁気ベクトルｍｋに応じて定められる単位水平磁気ベクトルｍＵとの、加重平均として、補正磁気ベクトルｍＣを算出する（式（１４）参照）。そして、予測磁気ベクトルｍＯを、予測磁気ベクトルｍＯ及び単位水平磁気ベクトルｍＵの間の方向を向く補正磁気ベクトルｍＣへと姿勢変化させる場合の姿勢変化量ＰＭを求め、当該姿勢変化量ＰＭにより予測姿勢ｑＧを補正することで、補正姿勢ｑＭを算出する。すなわち、本実施形態において、補正姿勢ｑＭは、角速度センサ９０が出力する角速度データωと、磁気センサ７０が出力する磁気データｍとの双方に基づいて定められる。
ところで、仮に、式（１４）において、係数αＭを「１」とする場合、補正磁気ベクトルｍＣは、単位水平磁気ベクトルｍＵとなる。この場合、補正姿勢ｑＭは、角速度データωの影響を受けることなく、磁気データｍのみに基づいて定められる。そして、この場合、補正姿勢ｑＭは、磁気データｍに重畳するノイズや、磁気センサ７０における測定誤差の影響により、真の姿勢から乖離した姿勢となることがある。
また、仮に、式（１４）において、係数αＭを「０」とする場合、補正磁気ベクトルｍＣは、予測磁気ベクトルｍＯとなる。この場合、補正姿勢ｑＭは、磁気ベクトルｍｋの影響を受けることなく、角速度データωに基づいて定められる。そして、この場合、補正姿勢ｑＭは、角速度データωに重畳するノイズや、角速度センサ９０における測定誤差の影響により、真の姿勢から乖離した姿勢となることがある。
これに対して本実施形態では、式（１４）に示すように、補正磁気ベクトルｍＣを、予測磁気ベクトルｍＯと単位水平磁気ベクトルｍＵとの加重平均として算出し、補正姿勢ｑＭを、磁気データｍ及び角速度データωの双方に基づいて定める。このため、本実施形態では、補正姿勢ｑＭを、磁気データｍまたは角速度データωの何れか一方に基づいて定める場合と比較して、磁気データｍ及び角速度データωに重畳するノイズの影響や、磁気センサ７０及び角速度センサ９０における測定誤差の影響を小さく抑えることができる。これにより、本実施形態では、真の姿勢に近い正確な補正姿勢ｑＭの算出が可能となる。
【００４５】
推定姿勢出力部２３は、上述のとおり、予測姿勢ｑＧまたは補正姿勢ｑＭを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。
より具体的には、推定姿勢出力部２３は、判定情報ＪDが「１」を示す場合、すなわち、判定部３０における判定結果が肯定である場合には、姿勢補正部２２が予測姿勢ｑＧを補正して補正姿勢ｑＭを算出するため、当該補正姿勢ｑＭを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。一方、推定姿勢出力部２３は、判定情報ＪDが「０」を示す場合、すなわち、判定部３０における判定結果が否定である場合には、姿勢補正部２２による予測姿勢ｑＧに対する補正が実行されないため、姿勢予測部２１により算出された予測姿勢ｑＧを、時刻Ｔｋにおける携帯機器１の推定姿勢ｑＯＵＴとして出力する。
【００４６】
このように、姿勢推定部２は、単位時間毎に角速度センサ９０が出力する角速度データωに基づいて、携帯機器１の姿勢ｑの予測値である予測姿勢ｑＧを生成する。また、姿勢推定部２は、単位時間毎に磁気センサ７０が出力する磁気データｍが地磁気Ｂを表すデータであると看做すことができる場合には、当該磁気データｍに基づいて、予測姿勢ｑＧを補正することで補正姿勢ｑＭを算出する。そして、姿勢推定部２は、予測姿勢ｑＧが補正姿勢ｑＭへと補正された場合には、補正姿勢ｑＭを携帯機器１の推定姿勢ｑＯＵＴとして出力し、また、予測姿勢ｑＧが補正されない場合には、予測姿勢ｑＧを携帯機器１の推定姿勢ｑＯＵＴとして出力する。
このように、本実施形態に係る姿勢推定部２は、角速度センサ９０の出力する角速度データωと、磁気センサ７０の出力する磁気データｍとの双方に基づいて、時刻Ｔｋ−１における推定姿勢ｑＯＵＴから、時刻Ｔｋにおける推定姿勢ｑＯＵＴを算出することで、携帯機器１の姿勢ｑを単位時間毎に推定する。
【００４７】
＜４．  実施形態の結論＞
以上において説明したように、本実施形態に係る姿勢推定部２は、角速度センサ９０が出力する角速度データωに基づいて算出された予測姿勢ｑＧを、磁気センサ７０が出力する磁気データｍに基づいて補正することで、補正姿勢ｑＭを算出する。すなわち、本実施形態では、角速度データωと、磁気データｍとの双方に基づいて、携帯機器１の姿勢ｑを推定する。
【００４８】
ところで、上述のとおり、磁気データｍには、携帯機器１の姿勢ｑの推定のために磁気センサ７０が検出すべき地磁気Ｂの他に、携帯機器１の近傍の物品が発する磁界がノイズとして重畳する可能性が高い。他方、角速度データωは、一般的に、携帯機器１の姿勢ｑの推定のために角速度センサ９０が検出すべき携帯機器１の姿勢変化のみを検出するため、磁気データｍと比較してノイズが重畳する可能性が低い。これらのことから、携帯機器１の姿勢ｑの推定において、信頼性の高いデータのみを使用する態様、すなわち、角速度データωに基づいて算出された予測姿勢ｑＧを推定姿勢ｑＯＵＴとして出力し、予測姿勢ｑＧに対する補正を実行しない態様（以下、「対比例」と称する）も想定されうる。
【００４９】
しかし、時刻Ｔｋにおける携帯機器１の姿勢ｑの予測結果である予測姿勢ｑＧは、時刻Ｔｋ−１における携帯機器１の推定姿勢ｑＯＵＴを、時刻Ｔｋ−１から時刻Ｔｋまでの単位時間における携帯機器１の姿勢変化量だけ変化させた姿勢である（式（３）参照）。つまり、姿勢予測部２１が実行する携帯機器１の姿勢ｑを予測する演算（以下、「予測演算」と称する）は、単位時間における携帯機器１の姿勢ｑの相対的な変化量を求める演算に過ぎない。
よって、対比例においては、例えば、初期状態（時刻Ｔ０）における携帯機器１の推定姿勢ｑＯＵＴと、時刻Ｔ０における携帯機器１の真の姿勢ｑとの間に誤差が存在する場合、姿勢予測部２１による予測演算を繰り返し実行しても、当該誤差は解消されない。
また、対比例において、時刻Ｔｋにおける携帯機器１の予測姿勢ｑＧは、初期状態（時刻Ｔ０）から時刻Ｔｋまでの間に姿勢予測部２１が実行したｋ回の予測演算において得られるｋ個の姿勢変化量Ω（ω０）、Ω（ω１）、…、Ω（ωｋ−１）の合計値に基づいて算出される。このため、仮に、角速度センサ９０の信頼性が高く、角速度データωに重畳するノイズが小さい場合であっても、時刻Ｔ０と時刻Ｔｋとの時間長が長くなり、予測演算の実行回数が増加するに従って、角速度データωに重畳するノイズの予測姿勢ｑＧに対する影響も蓄積する。つまり、時刻Ｔ０と現時刻Ｔｋとの時間長が長くなるに従って、携帯機器１の予測姿勢ｑＧと携帯機器１の真の姿勢ｑとの誤差も蓄積し増大する。
【００５０】
これに対して、本実施形態に係る姿勢推定部２は、判定部３０における判定結果が肯定である場合、すなわち、磁気データｍが地磁気Ｂを表していると看做すことができる場合に、磁気ベクトルｍｋにより予測姿勢ｑＧを補正する。
携帯機器１の姿勢ｑが変化している場合、時間の経過と共に、携帯機器１に固定された座標系の向き（Ｘ軸、Ｙ軸、Ｚ軸の向き）も変化する。この場合、時刻Ｔｋ−１における携帯機器１に固定された座標系は、時刻Ｔｋにおける携帯機器１に固定された座標系とは異なる座標系となる。よって、携帯機器１の姿勢ｑが変化している場合、ある時刻における携帯機器１に固定された座標系から見た、当該ある時刻における携帯機器１の姿勢と、当該ある時刻における携帯機器１に固定された座標系から見た、当該ある時刻とは異なる時刻における携帯機器１の姿勢とは、異なる姿勢となる場合がある。このような、ある時刻における携帯機器１に固定された座標系から見た、携帯機器１の姿勢は、角速度ベクトルωｋに基づいて表される。具体的には、角速度ベクトルωｋ−１は、時刻Ｔｋ−１における携帯機器１に固定された座標系から見た時刻Ｔｋにおける携帯機器１の謂わば相対的な姿勢を示す（式（３）参照）。
これに対して、磁気ベクトルｍｋは、地上に固定された座標系から見た携帯機器１の謂わば絶対的な姿勢を示す。
このため、本実施形態のように、磁気ベクトルｍｋを用いて予測姿勢ｑＧに対する補正を実行することで、初期状態において生じていた推定姿勢ｑＯＵＴの誤差を小さくすることができ、また、予測演算を繰り返すことで蓄積される誤差を小さくすることができる。これにより、本実施形態にかかる姿勢推定部２は、携帯機器１の姿勢ｑを正確に推定することができる。
【００５１】
ところで、上述のとおり、演算部２０が備える姿勢補正部２２は、磁気ベクトルｍｋと、地磁気ベクトルＢＧを携帯機器１に固定された座標系において表したベクトルとが、略同じであることを前提として予測姿勢ｑＧを補正する。しかし、携帯機器１の近傍の物品が発する磁界がノイズとして磁気データｍに重畳する場合がある。この場合、磁気ベクトルｍｋと、地磁気ベクトルＢＧを携帯機器１に固定された座標系において表したベクトルとを、略同じと看做すことはできない。よって、磁気データｍに地磁気Ｂ以外のノイズが重畳している場合に、磁気ベクトルｍｋを用いて予測姿勢ｑＧを補正して得られる補正姿勢ｑＭは、携帯機器１の真の姿勢から大きく乖離している可能性が高い。つまり、この場合、姿勢推定部２が、補正姿勢ｑＭを推定姿勢ｑＯＵＴとして出力すると、姿勢推定部２は、携帯機器１の真の姿勢から大きく乖離した姿勢ｑを推定することとなる。
これに対して、本実施形態に係る姿勢推定部２は、判定部３０における判定結果が肯定である場合、すなわち、磁気データｍが地磁気Ｂを表していると看做せる場合にのみ、磁気データｍを演算部２０に供給し、磁気ベクトルｍｋによる予測姿勢ｑＧに対する補正を実行する。このため、本実施形態に係る姿勢推定部２では、携帯機器１の真の姿勢から大きく乖離した推定姿勢ｑＯＵＴが算出されることを防止することができる。
【００５２】
このように、本実施形態に係る姿勢推定部２は、角速度センサ９０の出力する角速度データωと、磁気センサ７０の出力する磁気データｍとの双方に基づいて推定姿勢ｑＯＵＴを算出するため、角速度データωまたは磁気データｍの一方のみに基づいて推定姿勢ｑＯＵＴを算出する場合と比較して、携帯機器１の真の姿勢により近い推定姿勢ｑＯＵＴを算出することができる。
更に、本実施形態にかかる姿勢推定部２は、カルマンフィルタによる演算のように、姿勢ｑの推定値の推定誤差の共分散を単位時間毎に演算する必要が無いため、カルマンフィルタを用いて携帯機器１の姿勢ｑを推定する場合と比較して、処理負荷を軽減することができる。
【００５３】
＜Ｂ．変形例＞
以上の実施形態は多様に変形され得る。具体的な変形の態様を以下に例示する。以下の例示から任意に選択された２以上の態様は、相互に矛盾しない範囲内で適宜に併合され得る。
なお、以下に例示する変形例において作用や機能が実施形態と同等である要素については、以上の説明で参照した符号を流用して各々の詳細な説明を適宜に省略する。
【００５４】
＜変形例１＞
上述した実施形態に係る判定部３０は、上述した式（２）に例示したように、磁気ベクトルｍｋの大きさと、地磁気ベクトルＢＧの典型的な大きさＢＧ-Typicalとを比較することで、磁気データｍが地磁気Ｂを表していると看做せるか否かを判定するが、本発明はこのような形態に限定されるものでは無く、判定部３０は、磁気ベクトルｍｋの大きさと、地磁気ベクトルＢＧの大きさとを、直接比較してもよい。
この場合、判定部３０は、例えば、ＧＰＳ部６３が生成する携帯機器１の位置情報に基づいて、地磁気ベクトルＢＧの大きさを求めることができればよい。また、この場合、地磁気ベクトルＢＧの大きさに係る情報は、携帯機器１の位置情報と対応付けてＲＯＭ５２に記憶されていてもよいし、通信部６１を介して携帯機器１の外部から取得しうる情報であってもよい。
【００５５】
また、上述した実施形態では、地磁気ベクトルＢＧの大きさを考慮せず、地磁気ベクトルＢＧの水平成分を正規化した単位地磁気ベクトルＢＧＵを使用して、携帯機器１の姿勢ｑを推定するが、本発明はこのような形態に限定されるものでは無く、ＧＰＳ部６３が生成する位置情報に基づいて、地磁気ベクトルＢＧの鉛直成分及び大きさを求めた上で、地磁気ベクトルＢＧに基づいて、姿勢ｑを推定してもよい。
この場合、予測観測値演算部２２１は、式（７）において、単位地磁気ベクトルＢＧＵに基づいて予測磁気ベクトルｍＯを算出する代わりに、地磁気ベクトルＢＧに基づいて予測磁気ベクトルｍＯを算出すればよい。
また、この場合、補正観測値演算部２２２は、式（１０）乃至式（１４）において、磁気ベクトルｍｋを単位水平磁気ベクトルｍＵに変換して単位水平磁気ベクトルｍＵと予測磁気ベクトルｍＯとの加重平均として補正磁気ベクトルｍＣを算出する代わりに、磁気ベクトルｍｋの大きさを「１」に正規化した単位磁気ベクトルｍｋＵと、大きさが「１」の予測磁気ベクトルｍＯ（式（７）〜式（９）参照）と、の加重平均として補正磁気ベクトルｍＣを算出すればよい。
【００５６】
＜変形例２＞
上述した実施形態及び変形例において、姿勢推定部２はスイッチＳＷを備えるが、本発明はこのような形態に限定されるものでは無く、姿勢推定部２はスイッチＳＷを備えない構成であってもよい。
【００５７】
姿勢推定部２がスイッチＳＷを備えない場合、判定部３０の判定結果に関わらず、磁気ベクトルｍｋを演算部２０に供給してもよい。この場合、姿勢補正部２２は、判定部３０の判定結果に関わらず、予測姿勢ｑＧを磁気ベクトルｍｋにより補正して補正姿勢ｑＭを算出してもよいし、また、推定姿勢出力部２３は、判定部３０の判定結果に関わらず、補正姿勢ｑＭを推定姿勢ｑＯＵＴとして出力してもよい。
【００５８】
なお、姿勢推定部２がスイッチＳＷを供えない場合、上述した式（１４）に示す、補正磁気ベクトルｍＣを算出する加重平均の演算において、単位水平磁気ベクトルｍＵ（または、変形例１のように、地磁気ベクトルＢＧの鉛直成分及び大きさを求める場合には、単位磁気ベクトルｍｋＵ）に付される重みである係数αＭを、判定部３０の判定結果に基づいて変化させてもよい。例えば、判定部３０の判定結果が肯定であり磁気データｍが地磁気Ｂを表していると看做せる場合には、式（１４）に示すとおり係数αＭを用いて補正磁気ベクトルｍＣを算出し、判定部３０の判定結果が否定であり磁気データｍに地磁気Ｂ以外のノイズが重畳している場合には、式（１４）において、係数αＭの代わりに、０≦αＭ２＜αＭ＜１を満たす実数である係数αＭ２を用いて、補正磁気ベクトルｍＣを算出してもよい。
磁気データｍに地磁気Ｂ以外のノイズが重畳している場合、磁気ベクトルｍｋに基づいて算出される補正姿勢ｑＭも、磁気ベクトルｍｋに含まれるノイズの影響を受ける。この場合、補正姿勢ｑＭが、携帯機器１の真の姿勢から大きく乖離したものとして算出される。
本変形例では、判定部３０における判定結果が否定である場合、式（１４）に示す補正磁気ベクトルｍＣを算出するための加重平均の演算において、係数αＭの代わりに係数αＭ２を使用するため、係数αＭを使用する場合と比較して、予測磁気ベクトルｍＯに対応する重みである係数（１−αＭ２）を大きくすることができる。このため、携帯機器１の動作が不安定であっても、磁気ベクトルｍｋの有するノイズの補正姿勢ｑＭに対する影響を小さくできる。
【００５９】
また、上述した実施形態及び変形例において、姿勢推定部２は判定部３０を備えるが、本発明はこのような形態に限定されるものでは無く、姿勢推定部２は判定部３０を備えない構成であってもよい。この場合、姿勢推定部２はスイッチＳＷを備えない構成であってもよい。
姿勢推定部２が判定部３０を備えない場合、磁気センサ７０の出力する磁気データｍが、常に演算部２０に供給されることとしてもよい。この場合、演算部２０は、磁気ベクトルｍｋに基づいて予測姿勢ｑＧを補正して得られる補正姿勢ｑＭを、推定姿勢ｑＯＵＴとして出力すればよい。
なお、姿勢推定部２が判定部３０を供えない場合、式（１４）における係数αＭの代わりに０＜αＭ３＜αＭ＜１を満たす実数である係数αＭ３を用いて、補正磁気ベクトルｍＣを算出してもよい。この場合、予測磁気ベクトルｍＯに対応する重みである係数（１−αＭ３）を大きくすることができる。このため、携帯機器１の動作が不安定であっても、携帯機器１の真の姿勢に近い補正姿勢ｑＭを算出することができる。
【００６０】
＜変形例３＞
上述した実施形態及び変形例において、携帯機器１は、磁気センサ７０及び角速度センサ９０を備えるが、本発明はこのような形態に限定されるものでは無く、携帯機器１は、これらに加えて、３方向の加速度を周期的に検出して検出結果を出力する加速度センサを備えるものであってもよい。
この場合、姿勢推定部２は、上述した実施形態及び変形例のように、磁気センサ７０及び角速度センサ９０からの出力結果を用いて、磁極北に向かう方向からの携帯機器１の傾きを推定するのに加え、加速度センサ及び角速度センサ９０からの出力結果を用いて、鉛直方向（重力加速度の示す方向）からの携帯機器１の傾きを推定するものであってもよい。
【００６１】
＜変形例４＞
上述した実施形態及び変形例において、磁気センサ７０及び角速度センサ９０等のセンサは、単位時間毎に周期的に検出した観測値を出力するが、本発明はこのような形態に限定されるものでは無く、任意のタイミングで観測値を出力するものであればよい。この場合、姿勢予測部２１は、角速度データωが出力されるタイミングで、予測姿勢ｑＧを生成するための演算を実行すればよく、姿勢補正部２２は、磁気データｍが出力されるタイミングで、補正姿勢ｑＭを生成するための演算を実行すればよい。すなわち、演算部２０は、角速度データωに基づいて予測姿勢ｑＧを生成する姿勢予測部２１の処理と、磁気データｍに基づいて補正姿勢ｑＭを生成する姿勢補正部２２の処理とを、独立して、非同期で実行してもよい。
更に、姿勢予測部２１と姿勢補正部２２とが非同期で動作する場合、姿勢補正部２２は、予測姿勢ｑＧの代わりに推定姿勢ｑＯＵＴに基づいて、補正姿勢ｑＭを生成してもよい。すなわち、演算部２０は、推定姿勢ｑＯＵＴを予測前姿勢ｑＧ−ＩＮとして当該予測前姿勢ｑＧ−ＩＮに基づいて予測姿勢ｑＧを生成する姿勢予測部２１における処理と、推定姿勢ｑＯＵＴを補正前姿勢ｑＭ−ＩＮとして当該補正前姿勢ｑＭ−ＩＮに基づいて補正姿勢ｑＭを生成する姿勢補正部２２における処理とを、互いに排他的なタイミングで且つ互いに独立して実行するものであってもよい。
【符号の説明】
【００６２】
１…携帯機器、２…姿勢推定部、１０…ＣＰＵ、２０…演算部、２１…姿勢予測部、２２…姿勢補正部、２３…推定姿勢出力部、２２１…予測観測値演算部、２２２…補正観測値演算部、２２３…補正量演算部、２２４…補正姿勢演算部、３０…判定部、７０…磁気センサ、９０…角速度センサ。
【図１】
2016109609.tif 000011
【図２】
2016109609.tif 000012
【図３】
2016109609.tif 000013
【図４】
2016109609.tif 000014
【図５】
2016109609.tif 000015