状態推定装置、オフセット更新方法およびオフセット更新プログラム
【課題】カルマンフィルタを用いて地磁気センサの正確なオフセット値を推定する
【解決手段】携帯機器１は、３次元地磁気センサ７０、３次元加速度センサ８０、３次元角速度センサ９０、カルマンフィルタ演算部１２０及び初期値生成部１４０を備える状態推定部１００、中心点導出部２００、及び内部磁界Ｂｉを発生させる部品を備える。中心点導出部２００は、３次元地磁気センサ７０から順次出力される複数の磁気データｑｉで示される座標が内部磁界Ｂｉの成分を示す座標を中心点とする第１球面Ｓの近傍に確率的に分布すると仮定して、第１球面Ｓの中心点ｃＯを算出する。カルマンフィルタ演算部１２０は、３次元地磁気センサ７０のオフセットｑＯＦＦを推定する状態変数ｑＯを含む複数の状態変数を要素とする状態ベクトルｘｋを更新し、中心点導出部２００が中心点ｃＯを算出した場合には、状態変数ｑＯを中心点ｃＯによって上書きする。
【解決手段】携帯機器１は、３次元地磁気センサ７０、３次元加速度センサ８０、３次元角速度センサ９０、カルマンフィルタ演算部１２０及び初期値生成部１４０を備える状態推定部１００、中心点導出部２００、及び内部磁界Ｂｉを発生させる部品を備える。中心点導出部２００は、３次元地磁気センサ７０から順次出力される複数の磁気データｑｉで示される座標が内部磁界Ｂｉの成分を示す座標を中心点とする第１球面Ｓの近傍に確率的に分布すると仮定して、第１球面Ｓの中心点ｃＯを算出する。カルマンフィルタ演算部１２０は、３次元地磁気センサ７０のオフセットｑＯＦＦを推定する状態変数ｑＯを含む複数の状態変数を要素とする状態ベクトルｘｋを更新し、中心点導出部２００が中心点ｃＯを算出した場合には、状態変数ｑＯを中心点ｃＯによって上書きする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置、オフセット更新方法およびオフセット更新プログラムに関する。
【背景技術】
【０００２】
近年、携帯機器の高性能化及び多用途化に伴い、地磁気の方向や携帯機器の姿勢等の推定機能を備える携帯機器の研究開発が盛んに行われている。地磁気の方向や携帯機器の姿勢等を推定する場合、地磁気センサ等の単体のセンサからの出力のみを利用して推定するよりも、異種の物理量を測定する複数のセンサからの出力を統合して推定する方が、より高速な推定が可能となる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合する方法が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００５】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００６】
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量を要素とする状態ベクトルを、より真値に近い値へと更新する。
しかし、観測残差は、更新前の状態ベクトルを用いて算出される値であるため、動的システムの状態が急激且つ大きく変化した場合には、状態ベクトルが真値から離れた値に更新されてしまうという問題が存在した。
【０００７】
本発明は、上述した点に鑑みてなされたものであり、動的システムの状態が急激且つ大きく変化した場合にも、動的システムの状態を正確に推定することを解決課題とする。
【課題を解決するための手段】
【０００８】
以下、本発明について説明する。なお、本発明の理解を容易にするために本実施形態、変形例、及び添付図面の参照符号を括弧書きにて付記するが、それにより本発明が本実施形態に限定されるものではない。
【０００９】
上述した課題を解決するため、本発明に係る状態推定装置は、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データ（ｑｉ）を順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データのうち前記部品の発生する磁界（Ｂｉ）の成分を表す３次元のベクトルデータであるオフセット（ｑＯＦＦ）を推定する状態変数（ｑＯ）を含む複数の状態変数を要素とするベクトルを状態ベクトル（ｘｋ）とし、前記複数のセンサの出力を要素とする観測値ベクトル（ｙｋ）及び前記状態ベクトルを用いて算出される観測残差（ｚｋ）に基づいて、前記状態ベクトルを更新するカルマンフィルタと、前記磁気データを蓄積する蓄積手段と、前記磁気データが所定数（Ｎ）蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面（Ｓ）の近傍に確率的に分布すると仮定して、前記第１球面の中心点（ｃＯ）を示す３軸の座標を算出する、中心点算出手段と、を備え、前記カルマンフィルタは、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、ことを特徴とする。
【００１０】
この発明によれば、状態推定装置は、中心点算出手段と、カルマンフィルタとを備え、カルマンフィルタがその演算に用いる状態ベクトルの要素のうち３次元磁気センサのオフセットを推定する状態変数を、中心点算出手段が算出する中心点の座標によって更新したうえで、カルマンフィルタの演算を行う。
【００１１】
カルマンフィルタは、ある時刻における動的システムの状態を表す物理量（状態変数）を要素とする状態ベクトルを、異種の物理量を測定する複数のセンサからの出力値を要素とする観測値ベクトルと状態ベクトルとを用いて算出される観測残差に基づいて、動的システムの真の値へと近付けるように更新する演算である。
具体的には、カルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻における状態ベクトルから、当該ある時刻から単位時間経過後の状態ベクトルを推定する。次に、カルマンフィルタは、動的システムの状態の推定値から複数のセンサからの出力値を推定する観測モデルを用いて、推定された状態ベクトルから観測値ベクトルを推定する。さらに、カルマンフィルタは、推定された観測値ベクトルと、各種センサからの実際の出力（実際の観測値）を要素とする観測値ベクトルとの差分を演算することにより、観測残差を算出する。そして、カルマンフィルタは、観測残差を用いて、状態ベクトルを更新する。
【００１２】
ところで、３次元磁気センサが搭載される機器は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。この場合、３次元磁気センサが出力するベクトルデータは、地磁気を表すベクトルの他に、機器に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、３次元磁気センサが出力するベクトルデータから、機器の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。このように、検出対象である地磁気の正確な値を得るために、補正処理において、３次元磁気センサから出力されるデータから取り除かれる内部磁界を表すベクトルを、３次元磁気センサのオフセットと呼ぶ。
内部磁界は、機器の内部状態が変化しない場合には、一定の方向及び大きさを有するが、機器の内部状態が変化した場合には、方向及び大きさが変化する。機器の内部状態は、機器の利用者による機器の操作や、機器の外部の環境等に依存して、急激且つ大きな変化を生ずる場合がある。そして、機器の内部状態の変化に伴い、機器に備えられた部品が発する内部磁界（すなわち、３次元磁気センサのオフセット）も、急激且つ大きな変化をする。
【００１３】
前述の通り、カルマンフィルタが行う演算には、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻における状態変数の値から、当該ある時刻よりも単位時間経過後の状態変数の値を推定する演算が含まれる。従って、カルマンフィルタが推定する状態変数は、状態遷移モデルにおいて定式化することのできる物理量であることが好ましい。しかし、３次元磁気センサのオフセットは、機器の利用者による機器の操作等により急激且つ大きな変化をするため、この値の変化を定式化することは困難である。
また、状態ベクトルを更新するために用いられる観測残差は、更新前の状態ベクトルの値に基づいて算出される。つまり、カルマンフィルタの演算により算出される更新された状態ベクトルの値は、更新前の状態ベクトルの値の影響を受けた値となる。そして、更新前の状態ベクトルの値は、それ以前の状態ベクトルの値の影響を受けた値となる。従って、カルマンフィルタの演算を繰り返す場合、カルマンフィルタが推定する状態ベクトルの値は、カルマンフィルタの演算を開始した時点から、状態ベクトルの値が推定される時点に至る全てのステップにおける状態ベクトルの値を考慮して算出される値である。よって、３次元磁気センサのオフセットが、急激且つ大きな変化をした場合には、３次元磁気センサのオフセットを推定する状態変数の更新後の値は、変化後の３次元磁気センサのオフセットから大きく乖離した値となることがある。
状態ベクトルの値が、実際の物理量を正確に表した値（真値）から大きく乖離した値に収束している場合、その後、カルマンフィルタによる演算を繰り返しても、状態ベクトルの値は真値に近づかなくなることがあり、また、仮に状態ベクトルの値が真値に近づくことがあっても、真値に近い値に収束するまでに長い時間を要する。この場合、状態推定装置は、システムの状態を正確に推定できないため、状態推定装置が算出する状態変数に基づいて、地磁気の向きや機器の姿勢等を算出することはできない。
【００１４】
地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地面に対して一定の方向と一定の大きさとを有する一様な磁界である。よって、地面に対して機器の姿勢を変化させる場合には、機器から見た地磁気の方向も変化することになる。すなわち、機器に搭載された３次元磁気センサから見た場合、地磁気は、機器の姿勢の変化に伴い向きが変化する一定の大きさを持ったベクトルとして表される。
一方、内部磁界は、機器の内部状態が変化しない場合には、機器に対して一定の方向を向き、一定の大きさを有する磁界である。よって、機器の内部状態が変化しない場合、内部磁界は、機器に搭載された３次元磁気センサから見て、機器をどのような姿勢に変化させた場合であっても、一定の方向と一定の大きさを有するベクトルとして表される。
従って、３次元磁気センサの姿勢を変化させつつ取得した複数の磁気データは、内部磁界の方向及び大きさを表すベクトルの先端を中心とし、地磁気の大きさを半径とする球面の近傍に分布することになる。
中心点算出手段は、複数の磁気データの示す座標の各々が、ある球面（第１球面）の近傍に分布すると仮定し、当該第１球面の中心点の座標を算出することにより、内部磁界、すなわち、３次元磁気センサのオフセットを算出する。中心点算出手段は、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、その後に取得された複数の磁気データにより正確なオフセットを算出することが可能である。
【００１５】
本発明の状態推定装置によれば、状態ベクトルを構成する要素のうち３次元磁気センサのオフセットを推定する状態変数を、中心点算出手段により出力した中心点により上書きする。
従って、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、変化後のオフセットを表す中心点の座標を中心点算出手段が算出し、３次元磁気センサのオフセットを推定する状態変数を中心点の座標によって上書きするため、３次元磁気センサのオフセットを推定する状態変数が真値（３次元磁気センサのオフセット）から乖離した状態が長期間にわたって継続することを防止することができる。すなわち、本発明に係る状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００１６】
また、上述した状態推定装置において、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセット（ｑＯＦＦ）を推定する状態変数（ｑＯ）を含む複数の状態変数を要素とするベクトルを状態ベクトル（ｘｋ）とし、状態遷移モデル（ｆ）を用いて、ある時刻における前記状態ベクトル（ｘｋ−１）から当該ある時刻から単位時間後の前記状態ベクトル（ｘ−ｋ）を推定し、観測モデル（ｈ）を用いて、前記単位時間後の前記状態ベクトル（ｘ−ｋ）から、推定観測値ベクトル（ｙ−ｋ）を算出し、前記推定観測値ベクトル（ｙ−ｋ）と前記複数のセンサの出力を要素とする観測値ベクトル（ｙｋ）とに基づいて観測残差（ｚｋ）を算出し、前記観測残差と前記単位時間後の前記状態ベクトルとに基づいて前記状態ベクトル（ｘｋ）を更新するカルマンフィルタと、前記磁気データを蓄積する蓄積手段と、前記磁気データが所定数（Ｎ）蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標の分布の３次元的な広がりの程度を示す分散評価値を算出する、分布判定手段と、前記磁気データが所定数蓄積され、且つ、前記分散評価値が、分散許容値（λ０）以上である場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面（Ｓ）の近傍に確率的に分布すると仮定して、前記第１球面の中心点（ｃＯ）を示す３軸の座標を算出する、中心点算出手段と、複数の前記磁気データの各々が示す３軸の座標が、第２球面（Ｓ２）と曲面（ＳＸ）とを重ね合わせることで得られる立体（ＳＤ）の表面近傍に確率的に分布すると仮定して、複数の前記磁気データに基づいて、前記立体と、前記第２球面との形状の相違の程度を示す歪評価値（ｇＤ（Ｅ））を算出し、前記歪評価値が歪許容値（δ０）以下であるか否かを判定する、歪判定手段と、前記歪判定手段の判定結果が肯定である場合に、前記第１球面の中心点を示す３軸の座標を出力する、中心点出力手段と、を備え、前記カルマンフィルタは、前記中心点出力手段が、前記第１球面の中心点を示す３軸の座標を出力する場合に、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新することを特徴とすることが好ましい。
【００１７】
前述の通り、中心点算出手段は、複数の磁気データの示す座標が第１球面の近傍に分布すると仮定して、第１球面の中心点の座標を算出する。従って、複数の磁気データが３次元的な広がりを有さず平面的に分布する場合には、第１球面の中心点の座標を正確に特定することはできない。
この発明によれば、まず、分布判定手段により複数の磁気データの３次元的な広がりの程度を示す分散評価値を算出し、その後、分散評価値が分散許容値以上である場合に、中心点算出手段が中心点の座標を算出する。従って、複数の磁気データが２次元的に分布する場合に、中心点算出手段が不正確な中心点の座標を算出することを防止することが可能となる。
これにより、不正確なオフセットを用いた補正処理が施されて不正確な地磁気の方向及び大きさが算出されることを防止することが可能となる。また、不正確な中心点の座標を算出する演算処理を未然に防止することが可能となるため、状態推定装置の低消費電力化が可能となった。
【００１８】
また、この発明によれば、歪判定手段により、複数の磁気データの座標を表面近傍に有する立体と、第２球面との形状の相違の程度を、歪評価値に基づいて判定する。
歪評価値が閾値よりも大きな値を有する場合には、立体の形状が第２球面とは異なる歪んだ形状を有するため、立体の表面近傍に分布する複数の磁気データの示す座標は、第１球面の近傍に分布すると看做すことはできない。中心点算出手段は、第１球面の近傍に複数の磁気データが存在していることを前提として、３次元磁気センサのオフセットの候補である中心点の座標を算出するため、複数の磁気データを近傍に有さない第１球面の中心点の座標を示すベクトルは、３次元磁気センサのオフセットとしての意味を有さない。本発明に係る状態推定装置は、歪判定手段を備えるため、３次元磁気センサのオフセットと看做すことのできない不適切な中心点の座標を示すベクトルにより、３次元磁気センサのオフセットを推定する状態変数を上書きすることを防止することが可能となった。
【００１９】
このように、本発明に係る状態推定装置は、分布判定手段及び歪判定手段を備えるため、３次元磁気センサのオフセットと看做すことのできる真値に近い中心点の座標が得られた場合にのみ、当該真値に近い中心点の座標を示すベクトルにより、３次元磁気センサのオフセットを推定する状態変数を上書きする。これにより、本発明に係る状態推定装置は、システムの状態を正確に推定することができ、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００２０】
また、本発明の具体的な態様として、上述した状態推定装置は、前記歪判定手段の判定結果が否定である場合に、利用者に前記機器の位置を変化させずに前記機器の姿勢を変化させるように促す手段を備えてもよい。
【００２１】
この発明によれば、上述した状態推定装置は、歪判定手段において歪評価値が歪許容値よりも大きいと判定される場合、すなわち、３次元磁気センサから出力される複数の磁気データの示す座標が立体の表面近傍に分布するときに立体の形状が球面から大きな歪を有する場合に、利用者に対して機器の位置を変化させずに機器の姿勢を変化させるように促す手段を備える。
３次元磁気センサは、測定対象である地磁気の他に、機器に備えられた部品の発する内部磁界と、外部の物体の発する不均一な外部磁界とを検出する場合がある。そして、この不均一な外部磁界の影響が大きい場合には、複数の磁気データの示す座標は、球面とは大きく異なる歪んだ形状を有する立体の表面近傍に分布することになり、中心点算出手段により算出された中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することができなくなる。
しかし、このような不均一な外部磁界も、外部磁界を発生させる外部の物体と３次元磁気センサとの相互位置関係が変化しない場合には、一定の大きさの磁界にすぎない。つまり、３次元磁気センサの位置を変化させずに姿勢を変化させる場合、外部磁界は、３次元磁気センサによって、一定の大きさを有し方向のみを変化させる磁界として検出される。この場合、３次元磁気センサから出力される複数の磁気データで示される座標は、ある球面近傍に分布することになる。そして、この球面の中心点の座標は、機器に備えられた部品の発する磁界（内部磁界）の成分を表すベクトルが示す座標とほぼ同一の座標となる。
前述の通り、第１球面は、複数の磁気データの示す座標が第１球面の近傍に分布することを仮定して定められる。よって、複数の磁気データの示す座標が球面の形状となるように分布する場合、複数の磁気データの示す座標は、中心点算出手段によって算出された中心点を中心とする第１球面近傍にも分布すると看做すことができる。従って、第１球面の中心点は内部磁界の成分を表すベクトルが示す座標とほぼ同一の座標となるため、第１球面の中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することが可能となる。
このように、上述した状態推定装置は、利用者に機器の位置を変化させずに機器の姿勢を変化させるように促す手段を備えることにより、不均一な外部磁界が存在する場所であっても、適切なオフセットを算出することができるような動作を利用者に促すことが可能となる。
【００２２】
また、本発明の具体的な態様として、上述した状態推定装置において、前記分散評価値は、複数の前記磁気データの各々が示す３軸の座標の分散を表す共分散行列（Ａ）の最小固有値（λ３）であることとしてもよい。
【００２３】
この発明によれば、複数の磁気データの分散を表す共分散行列の最小固有値の値を、分散評価値として用いている。
磁気データは３軸の座標を示すので、複数の磁気データの分散を表す共分散行列は、３行３列の行列となる。この共分散行列から３つの固有ベクトルと３つの固有値が得られる。３つの固有ベクトルの一つは、複数の磁気データが最も大きく分布する方向を表している。この固有ベクトルに対応する固有値は最大固有値となり、逆に、複数の磁気データが最も小さく分布する方向の固有ベクトルに対応する固有値が最小固有値となる。複数の磁気データが理想的に２次元的に分布する場合、最小固有値の値は０に限りなく近い値を有し、複数の磁気データが３次元的に分布する場合、最小固有値の値は、複数の磁気データの分布の３次元的な広がりの程度に従って大きな値を有する。従って、共分散行列の最小固有値を、分散評価値として用いることにより、複数の磁気データの分布の３次元的な広がりの程度を的確に把握することが可能となる。
【００２４】
また、本発明の具体的な態様として、上述した状態推定装置において、前記磁気データを前記第１球面の中心点を原点とする座標系で表した３軸のベクトル（ｑｉ−ｃＯ）と、当該３軸のベクトルを対称行列である歪評価行列（Ｅ）により変換したベクトル（Ｅ(ｑｉ−ｃＯ)）との内積を、複数の前記磁気データの各々について算出し、複数の算出結果を要素とするベクトルを歪誤差ベクトル（ｋ(Ｅ)）とし、複数の前記磁気データで示される３軸の座標により特定されるそれぞれの位置と前記第２球面（Ｓ２）との誤差を表すベクトルを、第２球面誤差ベクトル（δＳ２）とし、前記歪誤差ベクトルと、前記第２球面誤差ベクトルとの和を立体誤差ベクトル（δＳＤ）とし、前記歪評価行列（Ｅ）の各々の成分（ｅ１１〜ｅ３３）と、前記第２球面の中心点（ｃＯ２）を示す３軸の座標とを変数とし、前記立体誤差ベクトルの大きさを表す関数を歪評価関数（ｆＳＤ(Ｅ,ｃ)）としたとき、前記歪評価値は、前記歪評価関数を最小化するときの、前記歪評価行列のノルムであることとしてもよい。
【００２５】
この発明によれば、立体誤差ベクトルの大きさを最小化することで、歪評価行列と、第２球面の中心点の座標とを算出する。そして、歪評価値は、歪評価行列のノルムとして算出される。歪評価行列は、３軸のベクトルを座標変換するための対称行列であるため、３行３列の行列である。従って、歪評価値は３つの固有値を有し、３つの固有値のうち、絶対値の大きさが最大となる固有値の絶対値が、歪評価値となる。
立体誤差ベクトルは、第２球面誤差ベクトルと、歪誤差ベクトルとの和で与えられる。
第２球面誤差ベクトルは、単に、複数の磁気データで示される座標と第２球面との誤差を表すためのベクトルである。従って、複数の磁気データで示される座標と第２球面との誤差を最小化するように第２球面を定めた場合、球面誤差ベクトルにより表される複数の誤差は、全体として対称性を有し、方向依存性の無い、ホワイトノイズとなる。
一方、歪誤差ベクトルは、第１球面の中心点から見た磁気データの示す座標を表すベクトルと、当該ベクトルを歪評価行列により変換したベクトルとの内積を、複数の磁気データの各々について並べたベクトルである。換言すれば、歪誤差ベクトルの各要素は、対称行列である歪評価行列を係数行列とし、中心点から見た磁気データの座標を表すベクトルの３つの要素の各々を変数とする、３変数二次形式となる。すなわち、歪誤差ベクトルは、複数の磁気データで示される座標と第２球面との誤差の各々を、二次形式で表される同一の関数に基づく曲面上に存在するという制約の下で表現するベクトルである。このような歪誤差ベクトルを用いて、複数の磁気データと第２球面との誤差を表現することにより、ホワイトノイズとは別に、二次形式の関数に基づく曲面で表される誤差（すなわち、球面からの歪）を表現することが可能となる。
球面誤差ベクトルと歪誤差ベクトルとを加算して得られる立体誤差ベクトルは、複数の磁気データを表面近傍に有し、球面と曲面とを重ね合わせた球面とは異なる立体を表現する。そして、立体誤差ベクトルのうち、歪誤差ベクトルの大きさ評価することで、立体の形状が、第２球面の形状からどの程度の差異（歪）を有するか（つまり、ホワイトノイズとは異なる球面からの歪による誤差の大きさ）を、評価することが可能となる。その結果、立体と第２球面とが実質的に同一の形状を有するほど近似していると評価される場合には、複数の磁気データの示す座標が第２球面近傍にも分布すると看做すことができる。前述の通り、複数の磁気データの示す座標が、球面の形状となるように分布する場合、複数の磁気データの示す座標は、第１球面近傍にも分布すると看做すことができる。従って、立体と第２球面とが実質的に同一の形状を有するほど近似していると評価される場合には、第１球面の中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することができる。一方、立体が第２球面とは大きく異なる歪んだ形状を有すると評価される場合には、複数の磁気データで示される座標が第２球面近傍に分布すると看做すことはできないため、第１球面の中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することを防止する必要がある。
すなわち、上述した状態推定装置は、中心点算出手段によって算出された中心点の座標のうち、不均一な外部磁界の影響を受けていない複数の磁気データより算出された、適切な中心点の座標を示すベクトルのみを、３次元磁気センサのオフセットとして採用することが可能である。この結果、上述した状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００２６】
また、上述した状態推定装置において、複数の前記磁気データの各々で示される３軸の座標として特定されるそれぞれの位置が、前記第１球面近傍に確率的に分布すると仮定した場合の、複数の前記磁気データで示される３軸の座標により特定されるそれぞれの位置と前記第１球面との誤差を表すベクトルを、第１球面誤差ベクトル（δＳ）とし、３次元のベクトル（ｃ）を変数とし、前記第１球面誤差ベクトル（δＳ）の大きさを表す関数を、中心点算出関数（ｆＳ（ｃ））としたとき、前記第１球面の中心点を示す３軸の座標は、前記中心点算出関数の値を最小化するときの、前記３次元のベクトルが示す３軸の座標であることを特徴とすることが好ましい。
【００２７】
この発明によれば、簡易な計算により、３次元磁気センサのオフセットの候補となる座標を算出することが可能となる。
【００２８】
また、本発明の具体的な態様として、上述した状態推定装置において、前記歪評価関数は、下記のｆＳＤ(Ｅ,ｃ)で表すこともできる。
【数１】
【００２９】
この発明によれば、歪評価関数を最小化するときの歪評価行列のノルムである歪評価値を簡単な計算により算出することができる。従って、上述した状態推定装置は、処理速度の向上、及び低消費電力化が可能になるという利点を有する。
【００３０】
また、本発明の具体的な態様として、上述した状態推定装置において、前記中心点算出関数は、下記のｆＳ（ｃ）で表すこともできる。
【数２】
【００３１】
この発明によれば、簡単な計算により中心点を算出することができる。従って、上述した状態推定装置は、処理速度の向上、及び低消費電力化が可能になるという利点を有する。
【００３２】
次に、本発明に係る状態推定装置に用いられるオフセット更新方法は、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データを蓄積する蓄積手段と、を備える状態推定装置に用いられるオフセット更新方法であって、前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新し、前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出し、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、ことを特徴とする。
【００３３】
この発明によれば、複数の磁気データに基づいて算出された中心点の座標により、状態ベクトルを構成する要素のうち３次元磁気センサのオフセットを推定する状態変数を上書きする。従って、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、３次元磁気センサのオフセットを推定する状態変数が、真値（３次元磁気センサのオフセット）から乖離することを防止することが可能となる。すなわち、状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００３４】
次に、本発明に係る状態推定装置に用いられるオフセット更新プログラムは、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データを蓄積する蓄積手段と、を備える状態推定装置に用いられるオフセット更新プログラムであって、前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新する処理と、前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する処理と、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する処理とを、コンピュータに実行させることを特徴とする。
【００３５】
この発明によれば、複数の磁気データに基づいて算出された中心点の座標により、状態ベクトルを構成する要素のうち３次元磁気センサのオフセットを推定する状態変数を上書きする。従って、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、３次元磁気センサのオフセットを推定する状態変数が、真値（３次元磁気センサのオフセット）から乖離することを防止することが可能となる。すなわち、状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【図面の簡単な説明】
【００３６】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係る３次元磁気センサが検出する磁界の概要を説明する概念図である。
【図４】本発明の実施形態に係る状態推定プログラムが実行されることにより実現される機能を表す機能ブロック図である。
【図５】本発明の実施形態に係る状態推定部の動作を表すフローチャートである。
【図６】本発明の実施形態に係るカルマンフィルタ演算部の機能ブロック図である。
【図７】本発明の実施形態に係る中心点導出部の動作を表すフローチャートである。
【図８】本発明の実施形態に係る３次元磁気センサが検出する地磁気と内部磁界とについて説明する概念図である。
【図９】本発明の実施形態に係る中心点算出処理を説明するための概念図である。
【図１０】本発明の実施形態に係る３次元磁気センサが検出する磁気データが２次元的に分布する場合について説明する概念図である。
【図１１】本発明の実施形態に係る磁気データ分布指標算出処理を説明するための概念図である。
【図１２】本発明の実施形態に係る３次元磁気センサが検出する地磁気、内部磁界、及び外部磁界について説明する概念図である。
【図１３】本発明の実施形態に係る歪み判定処理を説明するための概念図である。
【図１４】本発明の実施形態に係る歪み判定処理における立体を説明するための概念図である。
【図１５】本発明の実施形態に係る歪み判定処理を説明するための概念図である。
【図１６】本発明の変形例に係る状態推定プログラムが実行されることにより実現される機能を表す機能ブロック図である。
【発明を実施するための形態】
【００３７】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００３８】
［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器１のブロック図であり、図２は携帯機器１の外観を示す斜視図である。携帯機器１は、画面の姿勢に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢と、携帯機器１から見た地磁気の方向とを推定することによって実現される。
【００３９】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ２０、各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成するものである。また、携帯機器１は、地磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００４０】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｑを出力する。この磁気データｑは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。
【００４１】
ところで、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力する磁気データｑは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する地磁気を表すベクトルの他に、携帯機器１に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、３次元磁気センサが出力するベクトルデータ（磁気データｑ）から、携帯機器１の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。
このように、検出対象である地磁気の正確な値を得るために、補正処理において、３次元磁気センサ７０から出力されるデータから取り除かれる内部磁界を表すベクトルを磁気センサのオフセットｑＯＦＦと呼ぶ。
【００４２】
また、３次元磁気センサ７０は、図３に示すように、地磁気Ｂｇ及び内部磁界Ｂｉの他に、携帯機器１の外部に存在する物体２が出す外部磁界Ｂｘを検出する場合がある。詳細は後述するが、外部磁界Ｂｘは、不均一な磁界である。従って、外部磁界Ｂｘの成分を算出して、これを３次元磁気センサ７０の出力から取り除いて、正確な地磁気Ｂｇの値を算出することは困難である。一方、外部磁界Ｂｘが存在しない場合、または外部磁界Ｂｘの影響の小さな場合には、３次元磁気センサ７０の出力から、内部磁界Ｂｉをオフセットとして取り除く処理を施すことにより、正確な地磁気Ｂｇを算出することが可能である。
【００４３】
ここで、説明の便宜上、図３に示すような地上座標系ΣＧ及びセンサ座標系ΣＳを導入する。地上座標系ΣＧは、地上に固定された座標系であり、地上の任意の一点を原点とし、互いに直交する３つの方向、例えば、水平東、水平北、及び鉛直上向きを、それぞれｘ軸、ｙ軸、及びｚ軸とする座標系である。センサ座標系ΣＳは、携帯機器１に固定された座標系である。３次元磁気センサ７０が出力する磁気データは、センサ座標系ΣＳのｘ軸、ｙ軸、ｚ軸上にプロットされ、センサ座標系ΣＳのベクトルデータとして表現される。
また、地上座標系ΣＧから見たセンサ座標系ΣＳの原点の位置及び姿勢（つまり、地上座標系ΣＧから見た携帯機器１の位置及び姿勢）を、それぞれ、位置ＰＳ及び姿勢μと表現する。
なお、図３に記載された各ベクトルの左上に付された添字Ｇは、当該ベクトルが地上座標系ΣＧにおいて表現されたベクトルであることを意味する。
【００４４】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００４５】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示すベクトルデータである。
【００４６】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラムを実行することによって、携帯機器１の姿勢や磁気センサのオフセット等の物理量（状態変数）を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラムを実行することにより、状態推定装置として機能する。
【００４７】
図４は、状態推定装置のうち、ＣＰＵ１０が状態推定プログラムを実行することにより実現される機能を表す機能ブロック図である。図４に示すように、状態推定装置は、状態推定部１００と、中心点導出部２００とを備える。状態推定部１００は、カルマンフィルタ演算部１２０と、初期値生成部１４０とを備える。カルマンフィルタ演算部１２０は、磁気データｑ、加速度データａ、及び角速度データｇを要素とする観測値ベクトルｙを用いてカルマンフィルタの演算を実行することで、状態ベクトルｘを周期的に更新し、更新後の状態ベクトルｘ＋を出力する。初期値生成部１４０は、各種設定情報に基づいて初期値ＩＮＩを生成し、生成した初期値ＩＮＩを出力する。
【００４８】
中心点導出部２００は、３次元磁気センサ７０より供給される磁気データｑを複数個蓄積したうえで、複数個の磁気データｑ１〜ｑＮに基づいて中心点（第１球面の中心点）ｃＯの座標を算出し、算出した中心点ｃＯの座標を出力する（Ｎは精度よく中心点ｃＯの座標を導出するために必要な磁気データの規定測定回数を表す５以上の自然数）。詳細は後述するが、中心点ｃＯは、センサ座標系ΣＳ上の座標を示す点であり、中心点ｃＯの座標を示すベクトルは、磁気センサのオフセットｑＯＦＦの候補となる。
【００４９】
［２． カルマンフィルタについて］
図５は、状態推定部１００の動作を表すフローチャートである。当該フローチャートに示される状態推定処理は、ＣＰＵ１０が状態推定プログラムを実行することにより開始される。
ステップＳ１０１において、ＣＰＵ１０は、初期値生成処理を実行する。具体的には、ＣＰＵ１０は、ＲＯＭ３０に記録された各種設定情報を参照し、初期値ＩＮＩを生成する。すなわち、ＣＰＵ１０は、ステップＳ１０１の初期値生成処理を実行することにより、初期値生成部１４０として機能する。
【００５０】
ステップＳ１０２において、ＣＰＵ１０は、中心点導出有無判定処理を実行する。具体的には、ＣＰＵ１０は、後述する中心点導出処理（図７参照）において中心点ｃＯの座標が生成されたか否かを判定する（より具体的には、ＣＰＵ１０は、中心点ｃＯの座標が、後述するステップＳ２０６の中心点出力処理から引き渡されたか否かを判定する）。そして、中心点導出処理において中心点ｃＯの座標が生成された場合、ＣＰＵ１０は、処理をステップＳ１０３に進める。一方、中心点導出処理において中心点ｃＯの座標が生成されていない場合、ＣＰＵ１０は、処理をステップＳ１０４に進める。
ステップＳ１０３において、ＣＰＵ１０は、状態変数上書処理を実行する。詳細は後述するが、後述する状態ベクトル更新処理（ステップＳ１０４）において行われるカルマンフィルタの演算の対象である状態ベクトルｘは、複数の状態変数を要素とするベクトルであり、複数の要素には、３次元のベクトルである磁気センサのオフセット推定値ｑＯが含まれる。状態変数上書処理において、ＣＰＵ１０は、中心点導出処理によって得られた中心点ｃＯを表す３軸の座標の各成分によって、３次元のベクトルである磁気センサのオフセット推定値ｑＯを構成する各要素を上書きする。
ステップＳ１０４において、ＣＰＵ１０は、状態ベクトル更新処理を実行する。具体的には、ＣＰＵ１０は、観測値ベクトルｙを用いてカルマンフィルタの演算を実行することで、状態ベクトルｘを周期的に更新し、更新後の状態ベクトルｘ＋を算出する。
ステップＳ１０５において、ＣＰＵ１０は、状態ベクトル出力処理を実行する。具体的には、ＣＰＵ１０は、状態ベクトル更新処理で算出された更新後の状態ベクトルｘ＋を出力する。
ＣＰＵ１０は、以上に示したステップＳ１０２〜Ｓ１０５の処理を実行することにより、カルマンフィルタ演算部１２０として機能する。そして、携帯機器１は、ＣＰＵ１０がステップＳ１０２〜Ｓ１０５の処理を実行することにより生成される更新後の状態ベクトルｘ＋ｋ（つまり、複数の状態変数）を用いて、地磁気Ｂｇの向きや、携帯機器１の姿勢等を算出することができる。
次に、ＣＰＵ１０は、状態推定プログラムを終了する終了条件を充足するか否かを判定する（ステップＳ１０６）。終了条件は、携帯機器１の仕様等に基づいて適宜定めればよい。例えば、携帯機器１の電源がオフ状態になることを終了条件としてもよい。終了条件を充足する場合、ＣＰＵ１０は、当該フローチャートに示す状態推定処理を終了する。一方、終了条件を充足しない場合、ＣＰＵ１０は、処理をステップＳ１０２に進める。
【００５１】
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルと、を有する。そして、カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量（状態変数）を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定する。次に、カルマンフィルタは、推定された状態ベクトルに基づいて、動的システムの状態を測定する複数のセンサの出力値を要素とする観測値ベクトルの値を推定する。さらに、推定された観測値ベクトルと、実際のセンサの出力値を要素とする観測値ベクトルとの差分として算出される観測残差とに基づいて、推定された状態ベクトルを、実際の値（真値）に近い値へと更新し、更新後の状態ベクトルを出力する。
カルマンフィルタは、以上のような演算を繰り返すことにより、状態ベクトルを構成する複数の状態変数の各々を、実際の物理量に出来るだけ近く実際の物理量を正確に表した値（真値）へと近付ける。
【００５２】
本実施形態におけるカルマンフィルタは、状態変数として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサ９１〜９３のオフセット推定値ｇＯ、及び磁気センサ７１〜７３のオフセット推定値ｑＯを採用し、これらを推定の対象とする。なお、状態変数に、角速度センサのオフセット推定値ｇＯを含ませているのは、角速度データｇに、角速度センサセンサ９１〜９３のオフセットが重畳するからである。
これらの状態変数を要素とする時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。
【数３】
【００５３】
本実施形態では、姿勢μを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。具体的には、携帯機器１に固定されたセンサ座標系ΣＳの各軸と、地上座標系ΣＧの各軸とが一致する姿勢μを基準姿勢と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、基準姿勢に対して単位ベクトルαの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢μは、式（２）で与えられる。
【数４】
【００５４】
本実施形態におけるカルマンフィルタの観測対象は、３軸の磁気センサ７１〜７３から出力される磁気データｑ、３軸の加速度センサ８１〜８３から出力される加速度データａ、及び３軸の角速度センサ９１〜９３から出力される角速度データｇである。このとき、時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数５】
【００５５】
前述のとおり、初期値生成部１４０は、時刻Ｔ＝０における状態変数を要素とする初期値ＩＮＩを算出する（初期値生成処理）。初期値ＩＮＩは、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｇＯの初期値ｇＯ,０、及び磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０を要素として含む。
初期値ＩＮＩは、カルマンフィルタの演算によって状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下のような値に設定してもよい。
【００５６】
地磁気の強さｒの初期値ｒ０、及び地磁気の伏角φの初期値φ０は、例えば、ＧＰＳ部６０から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ１が格納される。初期値生成部１４０は、ルックアップテーブルＬＵＴ１を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さｒの初期値ｒ０及び地磁気の伏角φの初期値φ０として設定する。
なお、携帯機器１が衛星からの電波の届かない場所（例えば、地下街）にある場合には、ＧＰＳ部６０から位置情報が得られない。そのような場合には、携帯機器１が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルＬＵＴ１に格納されている。初期値生成部１４０は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルＬＵＴ１から読み出す。例えば、日本で販売された携帯機器１については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さｒの初期値ｒ０、及び地磁気の伏角φの初期値φ０を算出する。
【００５７】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。角速度センサのオフセット推定値ｇＯの初期値ｇＯ,０は、通常「０」近辺に調整されているため、「０」に設定する。姿勢μの初期値μ０に関しては、例えば、携帯機器１が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。
【００５８】
磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０は、例えば、時刻Ｔ＝０の磁気センサの観測値ｑ０、姿勢μの初期値μ０、携帯機器１を使用する地域の地磁気ベクトルｑtypical、及び、式（５）に示す行列Ｂ（μ）を用いて、以下に示す式（４）で与えられる値に設定する。ここで、行列Ｂ（μ）は、携帯機器１が姿勢μである場合に、地上座標系ΣＧにおいて表現されたベクトルを、センサ座標系ΣＳで表現するための座標変換行列である。なお、ＲＯＭ３０には、位置情報と地磁気ベクトルｑtypicalとを対応づけて記憶したルックアップテーブルＬＵＴ２が記憶されている。初期値生成部１４０は、ＧＰＳ部６０で生成される位置情報に基づいてルックアップテーブルＬＵＴ２を参照して地磁気ベクトルｑtypicalを取得し、式（４）を演算することによって、磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０を得る。
【数６】
【００５９】
初期値生成部１４０は、以上のようにして生成された姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｇＯの初期値ｇＯ,０、及び磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０を要素とするベクトルである初期値ＩＮＩを生成し、これを出力する。
【００６０】
［３． カルマンフィルタによる演算］
次に、ステップＳ１０４において行われる、状態ベクトル更新処理の内容、すなわち、本実施形態に係るカルマンフィルタによる演算の内容について説明する。
カルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定し、この推定値を、推定された状態ベクトルｘ−ｋとして出力する。そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、状態遷移モデルにより推定された状態ベクトルｘ−ｋに基づいて観測値ベクトルｙｋを推定し、この推定値を、推定された観測値ベクトル（推定観測値ベクトル）ｙ−ｋとして出力する。
なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。シグマポイントカルマンフィルタとは、状態ベクトルｘｋ−１の周囲に複数のシグマポイントχｋ−１を設定し、これらの複数のシグマポイントχｋ−１を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ−ｋを推定し、推定された複数のシグマポイントχ−ｋの平均を算出することにより、推定された状態ベクトルｘ−ｋを求める演算である。従って、推定された観測値ベクトルｙ−ｋは、厳密には、推定された状態ベクトルｘ−ｋの周辺に存在する複数のシグマポイントχ−ｋに基づいて算出される。
次に、カルマンフィルタは、推定された観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｚｋとして算出し、観測残差ｚｋに基づいてカルマンゲインＫｋを算出する。さらに、カルマンフィルタは、観測残差ｚｋとカルマンゲインＫｋとを用いて、推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトルｘ＋ｋを算出する。
状態ベクトル更新処理は、以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタの演算により、状態ベクトルｘｋを実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける処理である。
【００６１】
［３．１． カルマンフィルタによる演算の概要］
本実施形態における、カルマンフィルタの状態遷移モデルは以下に示す式（６）で与えられ、観測モデルは以下に示す式（７）で与えられる。なお、本実施形態では、式（６）に現れる関数ｆ及び式（７）に現れる関数ｈは、非線形の関数である。
【数７】
【００６２】
ここで、状態ベクトルｘｋはａ次元のベクトルであり、観測値ベクトルｙｋはｂ次元のベクトルである。なお、本実施形態では、ａ＝１５であり、ｂ＝９である。また、式（６）に現れるプロセスノイズｗｋ及び式（７）に現れる観測ノイズｖｋは０を中心とするガウスノイズである。
式（６）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗｋ−１とを加算することにより推定されることを示している。
また、式（７）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗｋの共分散をＱｋ、観測ノイズｖｋの共分散をＲｋと表す。
【００６３】
時刻Ｔ＝ｋにおける観測残差ｚｋは、実際の観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（８）で表される。カルマンフィルタは、観測残差ｚｋと、式（９）に示すカルマンゲインＫｋとを用いて、以下の式（１０）に示すように、推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタは、以下の式（１１）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
【数８】
【００６４】
観測モデルを用いて推定された観測値ベクトルｙ−ｋは、式（７）に示すように、状態遷移モデルを用いて推定された状態ベクトルｘ−ｋを、観測モデルに適用することで算出される理論値である。よって、観測モデルを用いて推定された観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｚｋは、推定された状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
式（１０）では、このような観測残差ｚｋを用いて、推定された状態ベクトルｘ−ｋを、真値に近い更新後の状態ベクトルｘ＋ｋへと更新する。
【００６５】
［３．２． シグマポイントカルマンフィルタによる演算］
なお、本実施形態では、カルマンフィルタ（カルマンフィルタ演算部１２０）は、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、図６に示す、カルマンフィルタ演算部１２０の機能ブロック図を参照しつつ、シグマポイントカルマンフィルタによる状態ベクトルの更新について、具体的に説明する。
【００６６】
遅延部１２１は、加算器１２９から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部１２２に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部１２１は、初期値生成部１４０が出力する初期値ＩＮＩを用いて、状態ベクトルｘ＋ｋ−１を生成する。
また、中心点導出部２００が中心点ｃＯを出力する場合、遅延部１２１は、状態ベクトルｘ＋ｋ−１のうち、磁気センサのオフセット推定値ｑＯに対応する要素を、中心点導出部２００より出力される中心点ｃＯにより上書きしたうえで、状態ベクトルｘ＋ｋ−１を生成する（ステップＳ１０３の状態変数上書処理）。
【００６７】
シグマポイント生成部１２２は、ａ行ａ列の共分散行列Ｐ＋ｋ−１及びＱｋ−１を用いて、２ａ+１個のシグマポイントを生成する。具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマボイントの広がりを表すスケーリングパラメータλを用いて、式（１２）及び式（１３）に示すベクトルσｋを定義する。
【数９】
【００６８】
このとき、シグマポイント生成部１２２は、ベクトルσｋ−１と状態ベクトルｘ＋ｋ−１とに基づいて、式（１４）及び式（１５）で示される２ａ+１個のシグマポイントχｋ−１を生成する。
【数１０】
【００６９】
状態遷移モデル部１２３は、式（１６）に示すように、時刻Ｔ＝ｋ−１における２ａ+１個のシグマポイントχｋ−１（ｊ）の各々を、状態遷移モデルに適用することにより、時刻Ｔ＝ｋにおける２ａ+１個のシグマポイントの推定値χ−ｋ（ｊ）を算出する。
【数１１】
【００７０】
次に、平均算出部１２４は、式（１７）に示すように、時刻Ｔ＝ｋにおける２ａ+１個のシグマポイントの推定値χ−ｋの平均値を計算することで、推定された状態ベクトルｘ−ｋを算出する。
【数１２】
【００７１】
また、平均算出部１２４は、式（１８）に示す、推定された状態ベクトルｘ−ｋの誤差の共分散Ｐ−ｋを算出する。
【数１３】
【００７２】
一方、観測モデル部１２５は、式（１９）に示すように、時刻Ｔ＝ｋにおける２ａ+１個のシグマポイントχｋ（ｊ）の各々を、観測モデルに適用することにより、２ａ+１個の推定観測値γｋ（ｊ）を算出する。
【数１４】
【００７３】
そして、平均処理部１２６は、式（２０）に示すように、２ａ+１個の推定観測値γｋ（ｊ）の平均を演算することにより、推定された観測値ベクトルｙ−ｋを算出する。
【数１５】
【００７４】
次に、平均処理部１２６は、式（２１）に示す観測残差の共分散Ｐｙｙｋを算出する。
【数１６】
【００７５】
減算器１２７は、式（８）に示したように、観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとの差分として、観測残差ｚｋを算出する。
カルマンゲイン付与部１２８は、式（２２）に示す相互共分散行列Ｐｘｙｋを算出する。そして、カルマンゲイン付与部１２８は、式（９）に示したように、残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出し、式（１０）の右辺第２項（Ｋｋｚｋ）の演算を実行する。また、カルマンゲイン付与部１２８は、式（１１）に示したように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを、Ｐ−ｋからＰ＋ｋに更新する。
【数１７】
【００７６】
加算器１２９は、式（１０）に示したように、推定された状態ベクトルｘ−ｋと、カルマンゲイン付与部１２８から出力される式（１０）の右辺第２項（Ｋｋｚｋ）とを加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
【００７７】
［３．３． 状態遷移モデル］
次に、状態遷移モデル部１２３の演算で用いる状態遷移モデルについて説明する。
【００７８】
本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘｋを構成する状態変数のうち、姿勢μについての状態遷移は、式（２３）により定義される。式（２３）は、時刻Ｔ＝ｋ−１における姿勢μｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μｋを推定する演算を表す。ここで、μーｋは、時刻ｋにおける推定された状態ベクトルｘ−ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（２３）の右辺の演算子Ωは、式（２４）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、演算子［ｌ×］は、式（２５）で定義される。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ＋ｋのうち角速度を表す状態変数に対応する要素をω＋ｋで表したとき、演算子Ωを構成する成分Ψ＋ｋは、式（２６）で定義される。
【数１８】
【００７９】
姿勢μは、クォータニオンで表現されるため、正規化条件||μ||＝１が満たされていないといけないが、シグマポイントの平均を求めると正規化条件が満たされなくなる。そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つため、状態ベクトルｘｋのうち姿勢μｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新してもよい。
【００８０】
地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角φｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角φｋ−１とは等しい値であると仮定する。
同様に、角速度センサのオフセット推定値ｇＯは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度センサのオフセット推定値ｇＯ,Ｋと、時刻Ｔ＝ｋ−１における角速度センサのオフセット推定値ｇＯ,Ｋ−１とは等しい値であると仮定する。
【００８１】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００８２】
前述の通り、磁気センサのオフセットｑＯＦＦは、携帯機器１の部品が発する内部磁界Ｂｉの方向及び大きさをセンサ座標系ΣＳにおいて表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、磁気センサのオフセットｑＯＦＦも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、磁気センサのオフセットｑＯＦＦも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、磁気センサのオフセットｑＯＦＦが変化するタイミングや磁気センサのオフセットｑＯＦＦの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｑＯ,Ｋ−１を用いて、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｑＯ,Ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｑＯ,Ｋと、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｑＯ,Ｋ−１とは等しいと仮定する。
【００８３】
このように、本実施形態に係る状態遷移モデルでは、以下に示す式（２７）のように、状態ベクトルｘｋを構成する複数の状態変数のうち、姿勢μを表す状態変数以外は、前の時刻から変化しないという前提を置く。
【数１９】
【００８４】
なお、カルマンフィルタの演算において、状態ベクトルｘｋは、観測残差ｚｋに基づいて更新される。従って、式（２７）に示された姿勢μ以外の値が、全く変化しないわけではない。状態ベクトルｘｋは、加算器１２９によって、観測残差ｚｋに基づいて真値に近づくように更新される。
具体的には、時刻Ｔ＝ｋにおける状態ベクトルｘｋは、式（６）、式（８）、及び式（１０）に示すように、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を用いて算出される観測残差ｚｋに基づいて更新される。同様に、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１は、時刻Ｔ＝ｋ−２における状態ベクトルｘｋ−２を用いて算出される観測残差ｚｋ−１に基づいて更新される。すなわち、カルマンフィルタの演算において、状態ベクトルｘｋは、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１の値を考慮して、更新される。従って、状態ベクトルｘｋの値が真値から大きく乖離した値に収束している場合、その後カルマンフィルタの演算を繰り返しても、状態ベクトルｘｋの値は真値に近づかなくなることがあり、また、仮に状態ベクトルｘｋの値が真値に近づくとしても、真値に近い値に収束するまでに長い時間を要することになる。
【００８５】
前述の通り、磁気センサのオフセットｑＯＦＦは、携帯機器１の内部状態の変化に伴い、急激に大きく変化をする場合がある。磁気センサのオフセットｑＯＦＦが急激に大きく変化する場合、磁気センサのオフセット推定値ｑＯ,Ｋと、磁気センサのオフセットｑＯＦＦ（真値）とは、大きく異なる値となる。磁気センサのオフセット推定値ｑＯ,Ｋが真値から大きく乖離した場合、カルマンフィルタによって状態ベクトルｘｋの更新を繰り返しても、磁気センサのオフセット推定値ｑＯ,Ｋが真値に近づかなくなることがあり、また、仮に真値に近づくことができても、真値に近い値に収束するまでに長い期間を要する。そして、磁気センサのオフセット推定値ｑＯ,Ｋが、真値とは大きく異なる値となる場合、携帯機器１は、地磁気Ｂｇの向きや、携帯機器１の姿勢μ等を算出することができない。
【００８６】
そこで、本実施形態では、中心点導出部２００から中心点ｃＯの座標が出力された場合、磁気センサのオフセット推定値ｑＯを、中心点ｃＯの座標によって上書きする（ステップＳ１０３の状態変数上書処理）。
中心点ｃＯの座標は、Ｎ個の磁気データｑ１〜ｑＮに基づいて算出される。従って、中心点導出部２００は、磁気センサのオフセットｑＯＦＦが急激に変化した場合であっても、オフセットｑＯＦＦの変化後に取得したＮ個の磁気データｑ１〜ｑＮを用いることで、磁気センサのオフセットｑＯＦＦを正確に表す中心点ｃＯの座標を算出することができる。つまり、中心点ｃＯの座標は、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１の値を考慮して算出される磁気センサのオフセット推定値ｑＯ,Ｋに比べて、磁気センサのオフセットｑＯＦＦの急激な変化に柔軟に対応した真値に近い値として算出される。
従って、本実施形態に係る状態推定装置は、磁気センサのオフセットｑＯＦＦが急激に変化した場合であっても、磁気センサのオフセット推定値ｑＯを、中心点ｃＯの座標によって上書きすることにより、状態ベクトルｘｋが真値から乖離することを防止することができる。
【００８７】
［３．４． 観測モデル］
次に、観測モデル部１２５において行われる演算で用いる観測モデルについて説明する。
【００８８】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｇＯとを用いて、式（２８）で与えられる。
【数２０】
【００８９】
また、観測モデルにおいて、地上座標系ΣＧでの地磁気ベクトルは式（２９）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｑの推定値γｍａｇは、磁気センサのオフセット推定値ｑＯと、式（５）で示した行列Ｂ（μ）とを用いて、式（３０）で与えられる。
【数２１】
【００９０】
また、観測モデルにおいて、地上座標系ΣＧでの重力ベクトルは、重力加速度で正規化して、式（３１）で与えられる。従って、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（５）で示した行列Ｂ（μ）を用いて、式（３２）で与えられる。
【数２２】
【００９１】
従って、式（３）、式（２８）、式（３０）、及び式（３２）を、式（８）に適用することにより、観測残差ｚｋは、式（３３）で与えられる。
【数２３】
【００９２】
［４． 中心点導出部の動作］
図７は、中心点導出部２００の動作を表すフローチャートである。当該フローチャートに示される中心点導出処理は、ＣＰＵ１０が状態推定プログラムを実行することにより開始される。
【００９３】
ステップＳ２０１において、ＣＰＵ１０は、初期化処理を実行する。具体的には、ＣＰＵ１０は、ＲＡＭ２０に記憶した磁気データを廃棄する。
なお、本実施形態に係る初期化処理では、ＣＰＵ１０は、磁気データの全部を廃棄するが、本発明はこのような形態に限定するものではない。例えば、ＣＰＵ１０は、ＲＡＭ２０に蓄積されたデータのうち古い方から一定割合のデータのみを廃棄してもよい。
【００９４】
ステップＳ２０２において、ＣＰＵ１０は、磁気データ蓄積処理を実行する。具体的には、ＣＰＵ１０は、３次元磁気センサ７０から順次出力される磁気データｑｉをＲＡＭ２０に格納する。そして、ＣＰＵ１０は、磁気データｑｉがＲＡＭ２０にＮ個以上蓄積された場合、処理をステップＳ２０３へと進める。
【００９５】
ステップＳ２０３において、ＣＰＵ１０は、磁気データ分布判定処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０２の磁気データ蓄積処理においてＲＡＭ２０等に蓄積された複数の磁気データのうち、新しい方からＮ個の磁気データｑ１〜ｑＮ（Ｎは精度よく中心点ｃＯの座標を導出するために必要な磁気データの規定測定回数を表す５以上の自然数）が、後続するステップ２０４で実施される中心点算出処理において使用するデータとして適切なものであるか否かを判定する。そして、判定結果が肯定である場合、ＣＰＵ１０は、処理をステップＳ２０４へと進める。一方、判定結果が否定である場合、ＣＰＵ１０は、ＲＡＭ２０に蓄積された複数の磁気データの全部を廃棄した後に、処理をステップＳ２０２へと戻す。
なお、本実施形態では、磁気データ分布判定処理における判定結果が否定である場合、ＣＰＵ１０は、ＲＡＭ２０に蓄積された複数の磁気データの全部を廃棄するが、本発明はこのような形態に限定されるものではない。例えば、ＣＰＵ１０は、判定結果が否定である場合に、ＲＡＭ２０に蓄積された複数の磁気データを廃棄せずに一定期間待機し、判定時にＲＡＭ２０に蓄積されていた複数の磁気データに対して、待機している一定期間に新たにＲＡＭ２０に蓄積された複数の磁気データを加えたうえで、再度判定処理を行ってもよい。
【００９６】
ステップＳ２０４において、ＣＰＵ１０は、中心点算出処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０２の磁気データ蓄積処理においてＲＡＭ２０等に蓄積されたＮ個の磁気データｑ１〜ｑＮの示す座標が、ある球面（第１球面）Ｓの近傍に確率的に分布すると仮定して、球面Ｓの中心点ｃＯの座標を算出する。
【００９７】
ステップＳ２０５において、ＣＰＵ１０は、歪判定処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０２の磁気データ蓄積処理においてＲＡＭ２０等に蓄積されたＮ個の磁気データｑ１〜ｑＮの示す座標がある立体ＳＤの表面近傍に分布すると仮定し、立体ＳＤの形状と球面の形状とが同一と看做すことができる程度に近似しているか否かを判定する。判定結果が肯定の場合、すなわち、Ｎ個の磁気データｑ１〜ｑＮの示す座標が球面に近い形状の立体ＳＤの表面近傍に分布すると判定した場合には、ＣＰＵ１０は、処理をステップＳ２０６に進める。一方、判定結果が否定の場合、すなわち、立体ＳＤが球面とは異なる大きく歪んだ形状を有すると判定した場合、ＣＰＵ１０は、処理をステップＳ２０１に戻す。
【００９８】
なお、本実施形態に係る歪判定処理では、立体ＳＤと球面とが異なる形状であると判定した場合、ＣＰＵ１０は、処理をステップＳ２０１に戻すが、本発明はこれに限定されるものではない。例えば、歪判定処理において、立体ＳＤと球面とが異なる形状であるとの判定結果が得られた場合に、ＣＰＵ１０は、何らかのメッセージを表示部５０に出力した上で処理を一旦停止させ、携帯機器１の利用者からの指示を待ってステップＳ２０１から処理を再開させてもよい。
Ｎ個の磁気データｑ１〜ｑＮを取得する際に、携帯機器１の利用者が携帯機器１を手で握り、携帯機器１の位置ＰＳが変化するように携帯機器１を振るのではなく、携帯機器１の位置（厳密には、３次元磁気センサ７０の位置）ＰＳを固定したままその姿勢μのみを変化させるようにすると、外部磁界Ｂｘの影響を低く抑えることができる（図１３（Ｂ）参照）。そのため、ステップＳ２０５において、立体ＳＤが球面とは異なる大きな歪を有する形状であると判定された場合には、携帯機器１の位置を固定したまま回転させることを、携帯機器１の利用者に対して指示してもよい。携帯機器１の利用者に対する指示は、携帯機器１の表示部５０に画像や動画等を表示したり、音声等を用いたりすることで、行ってもよい。
【００９９】
ステップＳ２０６において、ＣＰＵ１０は、中心点出力処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０４の中心点算出処理で算出した中心点ｃＯの座標を、前述した状態推定処理（図５参照）に引き渡す。
ＣＰＵ１０は、以上に示したステップＳ２０１〜Ｓ２０６を実行することにより、中心点導出部２００として機能する。
次にＣＰＵ１０は、状態推定プログラムを終了する終了条件を充足するか否かを判定する（ステップＳ２０７）。終了条件を充足する場合、ＣＰＵ１０は、当該フローチャートに示される中心点導出処理を終了する。一方、終了条件を充足しない場合、ＣＰＵ１０は、処理をステップＳ２０１に進める。
なお、本実施形態では、ＣＰＵ１０は、ステップＳ２０７において、終了条件を充足しない場合に、処理をステップＳ２０１に進めるが、本発明はこのような形態に限定されるものではない。例えば、ＣＰＵ１０は、ステップＳ２０７の終了条件を充足しない場合、処理を一定期間停止させ、一定期間経過後に処理をステップＳ２０１に進めてもよい。また、ＣＰＵ１０は、ステップＳ２０７の終了条件を充足しない場合、状態ベクトル更新処理（ステップＳ１０４）で生成される観測残差ｚｋ等に基づいて中心点ｃＯの座標の生成の要否を判定（例えば、観測残差ｚｋのノルムが閾値以上であるか否かを判定）し、中心点ｃＯの座標の生成が必要と判定された場合に、処理をステップＳ２０１へと進めてもよい。
【０１００】
以下において、磁気データ分布判定処理（ステップＳ２０３）、中心点算出処理（ステップＳ２０４）、及び歪判定処理（ステップＳ２０５）の詳細について説明する。なお、説明の便宜上、まずステップＳ２０４の中心点算出処理について説明した後に、ステップＳ２０３の磁気データ分布判定処理について説明する。
【０１０１】
［５． 中心点算出処理］
中心点算出処理は、内部磁界Ｂｉ及び地磁気Ｂｇの性質の違いを利用して、Ｎ個の磁気データｑ１〜ｑＮの示す座標から内部磁界Ｂｉの成分を分離することで、磁気センサのオフセットｑＯＦＦの候補となる座標である中心点ｃＯの座標を算出する処理である。
以下では、５．１節において、中心点算出処理の前提となる内部磁界Ｂｉ及び地磁気Ｂｇの性質の相違点について説明したうえで、５．２節において、中心点算出処理の詳細について述べる。
【０１０２】
［５．１． 内部磁界及び地磁気の性質］
図８は、３次元磁気センサ７０が、姿勢μをμ１〜μＮと変化させつつ内部磁界Ｂｉ及び地磁気Ｂｇを検出した場合に３次元磁気センサ７０から出力されるＮ個の磁気データｑ１〜ｑＮを、センサ座標系ΣＳにおいてをプロットした図である。
なお、図８に記載された各ベクトルの左上に付された添字Ｓは、当該ベクトルがセンサ座標系ΣＳにおいて表されたベクトルであることを意味する。
【０１０３】
内部磁界Ｂｉは、携帯機器１の部品が発する磁界である。内部磁界Ｂｉは、携帯機器１の内部状態が変化しなければ、携帯機器１に対して一定の方向を向き、一定の大きさを有する磁界である。つまり、内部磁界Ｂｉは、センサ座標系ΣＳにおいて、３次元磁気センサ７０の位置Ｐｓ及び姿勢μに依存しない、一定の方向及び大きさを持つベクトルＳＢｉとして表される。具体的には、図８に示すように、内部磁界を表すベクトルＳＢｉは、センサ座標系ΣＳの原点を起点とし、中心点ｃＯＧを終点とするベクトルである。
【０１０４】
一方、地磁気Ｂｇは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地上座標系ΣＧにおいて一様な向き及び大きさを有する磁界である。よって、携帯機器１の姿勢μを変化させる場合は、地磁気Ｂｇの大きさは変化しないが、携帯機器１から見た地磁気Ｂｇの向きは変化する。つまり、地磁気Ｂｇは、センサ座標系ΣＳにおいて、携帯機器１の姿勢μに伴い方向を変化させる一定の大きさのベクトルＳＢｇ（μ）として表現される。
【０１０５】
複数の磁気データｑ１〜ｑＮの各々は、センサ座標系ΣＳにおいて、内部磁界を表すベクトルＳＢｉと、地磁気を表すベクトルＳＢｇ（μ）との和を表すベクトルの示す座標として出力される。従って、複数の磁気データｑ１〜ｑＮの各々がセンサ座標系ΣＳにおいて示す座標は、中心点ｃＯＧを中心とし、地磁気Ｂｇの大きさを半径とする球面ＳＧ上に分布する。なお、３次元磁気センサ７０は、測定誤差を有するため、厳密には、複数の磁気データｑ１〜ｑＮの示す座標は球面ＳＧの近傍に確率的に分布する。
内部磁界を表すベクトルＳＢｉが示す球面ＳＧの中心点ｃＯＧの座標と、磁気センサのオフセットｑＯＦＦの示す座標とは、等しい。従って、磁気センサのオフセットｑＯＦＦは、複数の磁気データｑ１〜ｑＮに基づいて球面ＳＧの中心点ｃＯＧの座標を求めることで算出される。つまり、磁気データｑｉの示す座標から、磁気センサのオフセットｑＯＦＦの示す座標を減算することにより、地磁気Ｂｇの正確な方向及び大きさを算出することができる。
【０１０６】
［５．２． 中心点算出処理の詳細］
以下では、図９を参照しながら、ステップＳ２０４の中心点算出処理について説明する。中心点算出処理は、３次元磁気センサ７０が出力するＮ個の磁気データｑ１〜ｑＮの示す座標が、半径ｒＳの球面Ｓの近傍に分布すると仮定して、球面Ｓの中心点ｃＯの座標を算出する処理である。
なお、球面Ｓは、センサ座標系ΣＳ上で、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面の中心点を求めるために、計算の便宜上導入された球面であり、地磁気Ｂｇを表す球面ＳＧとは異なる球面である。
しかし、３次元磁気センサ７０が、内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合には、複数の磁気データｑ１〜ｑＮの示す座標は球面ＳＧの近傍に分布することになる。従って、この場合、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと球面ＳＧとは、ほぼ同一の球面となり、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとは、ほぼ同一の座標となる。よって、３次元磁気センサ７０が、内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓの中心点ｃＯの座標を算出することで、球面ＳＧの中心点ｃＯＧの座標、すなわち、磁気センサのオフセットｑＯＦＦを算出することができる。
以下、具体的に、球面Ｓの中心点ｃＯの座標の算出方法について述べる。なお、以下において記載されるベクトル及び座標は、特に断りが無い場合には、センサ座標系ΣＳにおいて表現されたものであるとする。
【０１０７】
複数の磁気データｑ１〜ｑＮの示す座標が、半径ｒＳの球面Ｓ上に存在すると仮定する場合、各磁気データｑｉの示す座標と、球面Ｓの中心点ｃＯとの距離はｒＳであるため、以下の式（３４）が成立する。なお、磁気データｑｉの示す座標の各要素は式（３５）で表され、中心点ｃＯの座標の各要素は式（３６）で表される。
【数２４】
【０１０８】
ここで、複数の磁気データｑ１〜ｑＮの示す座標の重心ｑＣを、式（３７）で定義する。なお、重心ｑＣの各要素は、式（３８）で示される。磁気データｑｉの示す座標及び中心点ｃＯの座標を、重心ｑＣを起点とするベクトルとして表現することで、式（３４）は、式（３９）に変形される。
【数２５】
【０１０９】
式（３９）の磁気データｑｉの示す座標に対して、複数の磁気データｑ１〜ｑＮの示す座標の各々を代入した結果を、磁気データの個数Ｎで割り算することにより、以下の式（４０）が得られる。そして、式（３９）と式（４０）との差分を取ることで、以下の式（４１）が得られる。
【数２６】
【０１１０】
このように、磁気データである変数ｑｉの示す座標が、球面Ｓ上に存在することを示す式（３４）は、式（４１）に変形された。この式（４１）の変数ｑｉに対して、Ｎ個の磁気データｑ１〜ｑＮの示す座標をそれぞれ代入することで得られるＮ個の方程式は、行列Ｘを用いて式（４２）として表現される。なお、行列Ｘは、式（４３）に示すＮ行３列の行列である。また、ベクトルｊは、式（４４）に示すＮ次元のベクトルであり、値Ｒは、式（４５）に示す値である。
【数２７】
【０１１１】
式（４２）は、複数の磁気データｑ１〜ｑＮの示す座標の全てが、中心点ｃＯを中心とする球面Ｓ上に完全に一致する場合には解を有する。しかし、３次元磁気センサ７０の測定誤差等を考慮すると、複数の磁気データｑ１〜ｑＮの示す座標の全てが、球面Ｓと完全に一致することは無いため、式（４２）は解を持たない。そこで、統計的な手法により尤もらしい解を得るために、式（４６）で表す誤差を吸収するベクトルである第１球面誤差ベクトルδＳを導入する。ここで、式（４７）により表される３次元のベクトルｃは、中心点ｃＯの座標を表すための変数ベクトルである。
【数２８】
【０１１２】
第１球面誤差ベクトルδＳのノルムを最小にするベクトルｃ、換言すれば、(δＳ)Ｔ(δＳ)を最小化するようなベクトルｃにより示される座標が、球面Ｓの中心点ｃＯの座標として尤もらしいものであるいえる。ここで、以下の式（４８）で示される目的関数（中心点算出関数）ｆＳ（ｃ）を定義すると、目的関数ｆＳ（ｃ）を最小化するベクトルｃにより示される座標が、球面Ｓの中心点ｃＯの座標として尤もらしい値となる。中心点ｃＯの座標は、式（５０）に示す共分散行列Ａが正則である場合には、式（４９）で表される。
【数２９】
【０１１３】
前述の通り、３次元磁気センサ７０が、内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合には、球面Ｓと、地磁気Ｂｇを表す球面ＳＧとは、ほぼ同一の球面となり、球面Ｓの中心点ｃＯと、球面ＳＧの中心点ｃＯＧとは、ほぼ同一の座標となる。従って、３次元磁気センサ７０が内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合、式（４９）に示される中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１１４】
［６． 磁気データ分布判定処理］
以下では、ステップＳ２０３で行われる、磁気データ分布判定処理について説明する。
【０１１５】
前述した、中心点算出処理において、球面Ｓの中心点ｃＯを算出するためには、複数の磁気データｑ１〜ｑＮの示す座標が、センサ座標系ΣＳにおいて３次元的な広がりを有して分布していることが必要となる。しかし、携帯機器１（３次元磁気センサ７０）の姿勢μは、携帯機器１の利用者が手で携帯機器１を動かすことにより変化するため、携帯機器１の動かし方が不十分な場合には、携帯機器１の姿勢変化は３次元的とはならず２次元的なものとなることがある。この場合、センサ座標系ΣＳにおける複数の磁気データｑ１〜ｑＮの示す座標は、３次元的な広がりを有さず２次元的に分布する。
例えば、図１０に示すように、複数の磁気データｑ１〜ｑＮの示す座標が、センサ座標系ΣＳの平面π上の円πＣ近傍に２次元的に分布する場合、球面Ｓは、円πＣを切断面に有するような球面であるということしか特定できない。円πＣを切断面に有するような球面は、円πＣの中心点πＣＯを通り平面πに直交する直線πＬ上の中心点ｃπ１を中心とする球面Ｓπ１であるかもしれないし、直線πＬ上の中心点ｃπ２を中心とする球面Ｓπ２であるかもしれない。つまり、球面Ｓの中心点ｃＯは、直線πＬ上に位置することまでは特定可能であるが、具体的に直線πＬ上のどの位置に存在するかについては特定不可能である。このように、複数の磁気データｑ１〜ｑＮの示す座標が２次元的に分布する場合、複数の磁気データｑ１〜ｑＮに基づいて、正確な中心点ｃＯの座標を算出することはできない。
【０１１６】
複数の磁気データｑ１〜ｑＮに基づいて球面Ｓの中心点ｃＯの座標を算出するためには、図１１に示すように、センサ座標系ΣＳにおいて複数の磁気データｑ１〜ｑＮの示す座標が３次元的に広がりを持って分布していることが必要となる。磁気データ分布判定処理において、ＣＰＵ１０（中心点導出部２００）は、複数の磁気データｑ１〜ｑＮの示す座標が３次元的に分布しているか否かを、式（５０）に示される共分散行列Ａを用いて判定する。以下において、共分散行列Ａの性質を説明する。
【０１１７】
共分散行列Ａの固有値を、大きい順番に、最大固有値λ１、中間固有値λ２、最小固有値λ３とし、それぞれの固有値に対応する大きさ１に正規化された固有ベクトルをｕ１、ｕ２、ｕ３とする。また、磁気データｑｉの示す座標を、前述した重心ｑＣを原点とする重心座標系ΣＣにおいて表したベクトルをＣｑｉと表す。このとき、固有値λｊ（ｊ＝１、２、３）は、固有ベクトルｕｊ方向の分散σ２ｊに等しい。
図１１に示すように、各固有ベクトルｕ１〜ｕ３を重心座標系ΣＣの原点ｑＣを起点となるように配置する。このとき、例えばｊ＝１の場合について検討する。固有値λ１は、ベクトルＣｑｉを、固有ベクトルｕ１へ射影した長さＬｉ１の二乗（Ｌｉ１）２を、Ｎ個の磁気データＣｑｉ（ｉ＝１、２、…Ｎ）について平均した値に等しい。すなわち、固有値λｊは、Ｎ個の磁気データｑｉの示す座標が、重心ｑＣから固有ベクトルｕｊ方向にどの程度離れているか、つまり、複数の磁気データｑ１〜ｑＮの示す座標の分布が固有ベクトルｕｊの方向にどの程度の広がりを有しているかを表す値である。
【０１１８】
最小固有値λ３に対応する固有ベクトルｕ３の方向が、複数の磁気データｑ１〜ｑＮの示す座標の分布の広がりが最も小さい方向であり、最小固有値λ３が、複数の磁気データｑ１〜ｑＮの示す座標の分布の広がりが最も小さい方向における広がりの程度を示す指標である。従って、複数の磁気データｑ１〜ｑＮの示す座標が３次元的に分布しているといえるためには、最小固有値λ３の値が、一定の閾値（分散許容値）λ０以上であればよい。
【０１１９】
磁気データ分布判定処理において、中心点導出部２００は、共分散行列Ａの最小固有値λ３が閾値λ０以上であれば、複数の磁気データｑ１〜ｑＮの示す座標の分布が十分に３次元的であると判断して、処理を前述したステップＳ２０４の中心点算出処理へと進める。一方、中心点導出部２００は、最小固有値λ３が閾値λ０未満である場合には、複数の磁気データｑ１〜ｑＮの示す座標が３次元的な広がりを有さず２次元的に分布すると判断し、処理をステップＳ２０２の磁気データ蓄積処理に戻す。
【０１２０】
［７． 歪判定処理］
ところで、３次元磁気センサ７０は、図３に示したように、地磁気Ｂｇ及び内部磁界Ｂｉの他に、携帯機器１の外部に存在する物体２が出す外部磁界Ｂｘを検出する場合がある。外部磁界Ｂｘは、物体２と携帯機器１との相対的位置関係により、方向及び大きさが変化する不均一な磁界である。
５節で説明した中心点算出処理は、３次元磁気センサ７０が、地磁気Ｂｇ及び内部磁界Ｂｉのみを検出することを前提として、磁気センサのオフセットｑＯＦＦの示す座標の候補である球面Ｓの中心点ｃＯの座標を算出するものである。従って、３次元磁気センサ７０が、地磁気Ｂｇ及び内部磁界Ｂｉの他に、不均一な外部磁界Ｂｘを検出する場合、中心点算出処理により算出される中心点ｃＯの座標は、外部磁界Ｂｘの影響による誤差を有し、内部磁界Ｂｉ（すなわち、磁気センサのオフセットｑＯＦＦ）を表す座標とは異なる座標となる可能性が高い。このような中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用した場合、正しい補正処理を行うことができず、携帯機器１は正確な地磁気Ｂｇの方向及び大きさを算出することができない。
ステップＳ２０５の歪判定処理において、ＣＰＵ１０（中心点導出部２００）は、不均一な外部磁界Ｂｘの有無、及び外部磁界Ｂｘの影響の大きさを評価することにより、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することの可否を判断する。
以下では、７．１節において、歪判定処理における判定の対象となる外部磁界Ｂｘの性質について説明したうえで、７．２節において、歪判定処理の詳細を説明する。
【０１２１】
［７．１． 外部磁界の性質］
図１２は、３次元磁気センサ７０の位置ＰｓをＰＳ１〜ＰＳＮと変化させると共に、姿勢μをμ１〜μＮと変化させて磁界を測定したときの、３次元磁気センサ７０が出力する複数の磁気データｑ１〜ｑＮの示す座標を、センサ座標系ΣＳにおいてプロットした図である。なお、図１２では、内部磁界Ｂｉ、地磁気Ｂｇ、及び外部磁界Ｂｘが存在する場合を想定している。
【０１２２】
外部磁界Ｂｘは不均一な磁界であり、外部磁界Ｂｘを発生させる物体２と、携帯機器１との相対的位置関係により、方向及び大きさが変化する。従って、３次元磁気センサ７０の位置Ｐｓが変化した場合、３次元磁気センサ７０が検出する外部磁界Ｂｘの向き及び大きさも変化する。同様に、３次元磁気センサ７０の姿勢μが変化した場合、３次元磁気センサ７０が検出する外部磁界Ｂｘの向きも変化する。すなわち、外部磁界Ｂｘは、センサ座標系ΣＳにおいて、３次元磁気センサ７０の姿勢μ及び位置Ｐｓに依存して方向及び大きさの双方を変化させるベクトルＳＢｘ（μ、Ｐｓ）として表現される。
複数の磁気データｑ１〜ｑＮの示す座標の各々は、内部磁界を表すベクトルＳＢｉ、地磁気を表すベクトルＳＢｇ（μ）、及び外部磁界を表すベクトルＳＢｘ（μ、Ｐｓ）の和を表すベクトルにより示される。従って、複数の磁気データｑ１〜ｑＮの示す座標は、中心点ｃＯＧを起点とする地磁気を表すベクトルＳＢｇ（μ）の終点を表す球面ＳＧと、中心点ｃＯＧを起点とする外部磁界を表すベクトルＳＢｘ（μ、Ｐｓ）の終点を表す曲面ＳＸとを、重ね合わせた立体ＳＤの表面近傍に分布する。
【０１２３】
外部磁界Ｂｘを表す曲面ＳＸが、球面とは異なる歪んだ形状を有する場合には、立体ＳＤも球面とは異なる歪んだ形状を有する。複数の磁気データｑ１〜ｑＮの示す座標が、球面とは異なる歪んだ形状の立体ＳＤの表面近傍に分布する場合、複数の磁気データｑ１〜ｑＮの示す座標をできるだけ近傍に有するように球面Ｓを定めても、複数の磁気データｑ１〜ｑＮの示す座標と球面Ｓとの間の距離は大きく離れる。すなわち、この場合は、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面Ｓの近傍には分布しない。同様に、磁気データｑ１〜ｑＮの示す座標は、地磁気Ｂｇを表す球面ＳＧの近傍にも分布しない。
従って、複数の磁気データｑ１〜ｑＮの示す座標が、球面とは異なる歪んだ形状の立体ＳＤの表面近傍に分布する場合、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとは異なる座標となる可能性が高く、中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することはできない（図１５参照）。
【０１２４】
但し、不均一な外部磁界Ｂｘの影響が小さく、立体ＳＤの形状がほぼ球面と看做せる場合、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと、立体ＳＤとをほぼ同一の図形と看做すことが可能となり、また、球面Ｓの中心点ｃＯと、地磁気を表す球面ＳＧの中心点ｃＯＧとはほぼ同一の座標となる。従って、立体ＳＤの形状がほぼ球面と看做せる場合は、球面Ｓの中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。以下、図１３を参照しながら、外部磁界Ｂｘの影響が小さい場合について具体的に説明する。
【０１２５】
図１３（Ａ）は、外部磁界Ｂｘが微弱である場合を示す。
立体ＳＤは、地磁気Ｂｇを表す球面ＳＧと、外部磁界Ｂｘを表す曲面ＳＸとの重ね合わせである。従って、外部磁界Ｂｘが微弱であり、外部磁界Ｂｘを表す曲面ＳＸが小さい場合には、立体ＳＤと球面ＳＧとは、ほぼ同一の図形となる。このとき、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面ＳＧの近傍にも分布するものと看做すことができる。
また、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する立体ＳＤの形状は、ほぼ球面であるため、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと、立体ＳＤとは、ほぼ同一の図形となる。すなわち、立体ＳＤ、球面ＳＧ、及び球面Ｓは、ほぼ同一の図形になり、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとは、ほぼ等しい座標になる。
従って、図１３（Ａ）に示すような、外部磁界Ｂｘが微弱である場合には、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１２６】
なお、図１３（Ｂ）に示すように、不均一な外部磁界Ｂｘが大きい場合であっても、立体ＳＤの形状がほぼ球面と看做せる場合がある。
例えば、不均一な外部磁界Ｂｘが存在する場合であっても、Ｎ個の磁気データｑ１〜ｑＮを取得する際に、携帯機器１の利用者が携帯機器１を手で握って携帯機器１の位置ＰＳが変化するように振るのではなく、携帯機器１の位置ＰＳ（厳密には、３次元磁気センサ７０の位置）を固定したままその姿勢μのみを変化させる場合には、外部磁界Ｂｘは、センサ座標系ΣＳにおいて、姿勢μに基づいてその方向のみを変化させる一定の大きさのベクトルＳＢｘ（μ）として表現される。この場合、外部磁界Ｂｘを表す曲面ＳＸの形状は、中心点ｃＯＧを中心とする球面となるため、球面ＳＧと曲面ＳＸとの重ね合わせである立体ＳＤの形状もほぼ球面となる。また、立体ＳＤは、中心点ｃＯＧを中心とする球面ＳＧと、中心点ｃＯＧを中心とする球面の形状を有する曲面ＳＸとを重ね合わせた図形であるため、立体ＳＤが表す球面の中心点は、球面ＳＧの中心点ｃＯＧと、ほぼ一致する。前述の通り、立体ＳＤの形状がほぼ球面である場合には、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと、立体ＳＤとは、ほぼ同一の図形となる。よって、球面Ｓの中心点ｃＯと、球面ＳＧの中心点ｃＯＧとは、ほぼ等しい座標になる。
従って、図１３（Ｂ）に示すような、外部磁界Ｂｘが一定の大きさのベクトルとして検出される場合には、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１２７】
このように、不均一な外部磁界Ｂｘの影響が大きい場合、すなわち、立体ＳＤが球面とは異なる歪んだ形状を有する場合には、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することはできない。
一方、不均一な外部磁界Ｂｘが微弱な場合、または、３次元磁気センサ７０が不均一な外部磁界Ｂｘを一定の大きさのベクトルとして検出する場合等、立体ＳＤの形状がほぼ球面と看做すことができる場合には、球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１２８】
［７．２． 歪判定処理の詳細］
以下では、図１３乃至図１５を参照しながら、ステップＳ２０５の歪判定処理の詳細について説明する。
【０１２９】
前述した中心点算出処理では、複数の磁気データｑ１〜ｑＮの示す座標が、式（３４）で示される球面Ｓの近傍に分布すると仮定した。これに対して本節で説明する歪判定処理は、複数の磁気データｑ１〜ｑＮの示す座標が、球面Ｓとは異なる歪んだ形状の立体ＳＤの表面近傍に分布すると仮定する。
【０１３０】
立体ＳＤは、式（５１）で表される。式（５１）に示される立体ＳＤは、図１４に示すように、球面を表す成分「Ｘ（ｃ−ｑＣ）−ｊ」と、式（５２）に示すＮ次元ベクトルの歪誤差ベクトルｋ（Ｅ）とを、加算したものである。なお、式（５１）の右辺に現れる０Ｎの右下の添え字「Ｎ」は、零ベクトルの次元を表している。また、式（５３）に示す歪評価行列Ｅは、３行３列の対称行列である。
なお、式（５１）に現れる成分「Ｘ（ｃ−ｑＣ）−ｊ」が表す球面と、中心点算出手段により算出される球面Ｓとは、必ずしもセンサ座標系ΣＳ上で同一の図形を表すものではない。そこで、以下では、式（５１）に現れる成分「Ｘ（ｃ−ｑＣ）−ｊ」として表される球面を球面（第２球面）Ｓ２と称し、球面Ｓ２の中心点を中心点（第２球面の中心点）ｃＯ２と称する。また、式（５１）において用いられるベクトルｃは、式（４７）に示した３次元のベクトルであるが、本節では、中心点ｃＯ２の示す座標を算出するための変数ベクトルとして用いる。
歪判定処理は、式（５１）のうち、歪を表す成分ｋ（Ｅ）の大きさを評価することにより、立体ＳＤの形状と、球面Ｓ２の形状とが、どの程度相違しているかについて評価するものである。
【数３０】
【０１３１】
歪誤差ベクトルｋ（Ｅ）を構成する要素のうち、第ｉ行目の要素ｋｅ(ｑｉ−ｃＯ)は、以下の式（５４）で表される関数ｋｅ（ｐ）に対して、中心点ｃＯを起点として磁気データｑｉを表すベクトル（ｑｉ−ｃＯ）を代入することにより与えられる。関数ｋｅ（ｐ）は、式（５３）に示す歪評価行列Ｅを係数行列とし、式（５５）に示すベクトルｐの３つの要素を変数とする、二次形式で表現される関数である。つまり、関数ｋｅ（ｐ）は、ベクトルｐと、ベクトルｐを歪評価行列Ｅにより変換したベクトルＥｐとの内積を示す。
従って、歪誤差ベクトルｋ（Ｅ）のうち、第ｉ行目の要素ｋｅ（ｑｉ−ｃＯ）は、球面Ｓの中心点ｃＯを起点として磁気データｑｉの座標を表したベクトル（ｑｉ−ｃＯ）と、ベクトル（ｑｉ−ｃＯ）を歪評価行列Ｅにより変換して得られるベクトルＥ（ｑｉ−ｃＯ）との内積である。
【数３１】
【０１３２】
なお、３次元磁気センサ７０の測定誤差等を考慮すると、複数の磁気データｑ１〜ｑＮの各々が示す座標の全てが、立体ＳＤと完全に一致する位置に存在することは無いため、式（５１）は解を持たない。そこで、統計的な手法により尤もらしい解を得るために、式（５６）で表される誤差を吸収するベクトルである立体誤差ベクトルδＳＤを導入する。立体誤差ベクトルδＳＤは、第２球面誤差ベクトルδＳ２と、歪誤差ベクトルｋ（Ｅ）とを、加算したものである。なお、詳細は後述するが、第２球面誤差ベクトルδＳ２は、式（５１）のうち、球面を表す成分「Ｘ（ｃ−ｑＣ）−ｊ」に対応する成分である。
立体誤差ベクトルδＳＤは、複数の磁気データｑ１〜ｑＮの座標と、立体ＳＤの表面との誤差を表す、Ｎ次元のベクトルである。立体誤差ベクトルδＳＤのノルムを最小にするようなベクトルｃ及び歪評価行列Ｅ、換言すれば、（５７）で示される目的関数（歪評価関数）ｆＳＤ（Ｅ、ｃ）を最小化するようなベクトルｃ及び歪評価行列Ｅによって、複数の磁気データｑ１〜ｑＮの示す座標を表面近傍に有する立体ＳＤが表現される。
歪判定処理は、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを、後述する式（５８）及び（５９）で示される歪評価値ｇＤ（Ｅ）に基づいて評価する処理である。立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを評価することで、立体ＳＤと球面Ｓとの形状の相違の程度について把握することが可能となる。
【数３２】
【０１３３】
以下で、式（５６）に示した立体誤差ベクトルδＳＤの性質を、式（４６）に示した第１球面誤差ベクトルδＳの性質と対比しつつ説明する。
まず、第１球面誤差ベクトルδＳは、複数の磁気データｑ１〜ｑＮが示す座標と、球面Ｓとの誤差を吸収するためのベクトルである。第１球面誤差ベクトルδＳを構成する１行目〜Ｎ行目の各要素は、それぞれ独立した変数である。従って、第１球面誤差ベクトルδＳによって、複数の磁気データｑ１〜ｑＮが示す座標と球面Ｓとの誤差を吸収する場合には、複数の磁気データｑ１〜ｑＮが示す座標と球面ＳとのＮ個の誤差の各々は、互いに制約の無い、Ｎ個が互いに独立して定められる値となる。つまり、第１球面誤差ベクトルδＳによって表されるＮ個の誤差は、それぞれが独立に確率的に定められるものであり、Ｎ個の誤差は全体として、対称性を有し、かつ方向依存性の無いホワイトノイズである。
すなわち、中心点算出処理は、複数の磁気データｑ１〜ｑＮが示す座標と、球面Ｓとの誤差をホワイトノイズである第１球面誤差ベクトルδＳにより表現し、第１球面誤差ベクトルδＳを最小化するような球面Ｓの中心点ｃＯの座標を求める処理である。
【０１３４】
一方、立体誤差ベクトルδＳＤは、第２球面誤差ベクトルδＳ２と歪誤差ベクトルｋ（Ｅ）との和によって表されるベクトルであり、磁気データｑ１〜ｑＮが示す座標と立体ＳＤとの誤差を吸収するベクトルである。
第２球面誤差ベクトルδＳ２は、第１球面誤差ベクトルδＳと同様に式（４６）の右辺で表され、磁気データｑ１〜ｑＮの座標と、球面Ｓ２との誤差を、ホワイトノイズとして表現するベクトルである。
一方、歪誤差ベクトルｋ（Ｅ）は、式（５４）で示した３変数二次形式の関数ｋｅ（ｐ）を各要素とするベクトルである。３変数の二次形式は、変数が２次の項から構成される関数であり、３次元空間上の様々な曲面、例えば、直線、平面、柱面、球面、楕円面、錐面、１葉双曲面、２葉双曲面、及び各種放物面等を描くことができる。従って、歪誤差ベクトルｋ（Ｅ）は、各磁気データｑ１〜ｑＮが示す座標と球面Ｓ２とのＮ個の誤差の各々を、互いに独立したものではなく、Ｎ個の誤差の全てが同一の関数ｋｅ（ｐ）により表される３次元空間上の曲面上に存在するという制約を持った値として表現する。
このように、立体誤差ベクトルδＳＤは、各磁気データｑ１〜ｑＮが示す座標と球面Ｓ２とのＮ個の誤差を、ホワイトノイズである第２球面誤差ベクトルδＳ２と、滑らかな曲面による球面Ｓ２からの歪という性質を有する歪誤差ベクトルｋ（Ｅ）とに分離して表現する。
【０１３５】
以下では、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさと、中心点算出処理で算出された球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することの可否についての判断との関係について、図１３及び図１５を参照しつつ説明する。
【０１３６】
図１３（Ａ）及び（Ｂ）は、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響が小さい場合を示している。歪誤差ベクトルｋ（Ｅ）の影響が小さい場合とは、歪誤差ベクトルｋ（Ｅ）の大きさが小さいため、立体ＳＤを表す式（５１）が成立すれば、同時に、球面Ｓを表す式（４２）についても成立するものと看做せる場合を意味する。すなわち、歪誤差ベクトルｋ（Ｅ）の影響が小さい場合には、式（５７）に定める目的関数ｆＳＤ（Ｅ、ｃ）を最小化して求められる立体ＳＤと、式（４８）に定める目的関数ｆＳ（ｃ）を最小化して求められる球面Ｓとが、ほぼ等しくなる。前述の通り、立体ＳＤの形状が球面と看做せる場合には、複数の磁気データｑ１〜ｑＮが示す座標は、立体ＳＤの表面近傍に分布すると同時に、球面Ｓの近傍にも分布すると看做すことができる。そして、立体ＳＤが表す球面の中心点は、球面ＳＧの中心点ｃＯＧと、ほぼ一致するため、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとがほぼ等しい座標となる。従って、歪誤差ベクトルｋ（Ｅ）の影響が小さい場合には、球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができる。なお、歪評価行列Ｅが零行列となる場合には、歪誤差ベクトルｋ（Ｅ）も零ベクトルとなるため、式（４８）と式（５７）とが等しい式となり、立体ＳＤと球面Ｓとは完全に同一の図形となる。
【０１３７】
図１５は、歪誤差ベクトルｋ（Ｅ）の影響が大きい場合を示している。
歪誤差ベクトルｋ（Ｅ）の影響が大きい場合とは、複数の磁気データｑ１〜ｑＮが示す座標をできるだけ近傍に有するように球面Ｓ２を定めても、複数の磁気データｑ１〜ｑＮが示す座標と球面Ｓ２との誤差をホワイトノイズである第２球面誤差ベクトルδＳ２によって吸収することができず、球面Ｓ２からの滑らかな歪を表す歪誤差ベクトルｋ（Ｅ）により吸収する必要がある場合を示している。すなわち、この場合、目的関数ｆＳＤ（Ｅ、ｃ）を最小化して求められる立体ＳＤは、目的関数ｆＳ（ｃ）を最小化して求める球面Ｓとは大きく異なる歪んだ形状になる。前述の通り、立体ＳＤの形状が球面とは異なる歪んだ形状を有する場合には、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮが示す座標は、球面Ｓの近傍に分布すると看做すことはできない。中心点算出処理は、球面Ｓの近傍に磁気データｑ１〜ｑＮの示す座標が存在していることを前提として、磁気センサのオフセットｑＯＦＦの候補である中心点ｃＯの座標を算出する処理であるため、磁気データｑ１〜ｑＮの示す座標を近傍に有さない球面Ｓの中心点ｃＯは、磁気センサのオフセットｑＯＦＦとしての意味を有さない。従って、この場合、中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することはできない。
【０１３８】
このように、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響が小さい場合、立体ＳＤ上に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面Ｓ上にも分布すると看做すことができ、中心点算出手段で算出した球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができるが、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響が大きければ、中心点ｃＯの座標は誤差を有する不正確な値となり、中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することを防止する必要がある。
以下では、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを評価する方法について述べる。
【０１３９】
ここで、以下の式（５８）及び（５９）に示す歪評価値ｇＤ（Ｅ）を、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを評価する評価値として定義する。歪評価値ｇＤ（Ｅ）は、歪評価行列Ｅの有する３つの固有値のうち、絶対値が最大となる最大固有値λＥ１の絶対値（歪評価行列Ｅのノルム）である。
歪評価値ｇＤ（Ｅ）の値が一定の閾値（歪許容値）δ０以下の小さな値であれば、立体ＳＤと球面Ｓ２とは等しい図形であると看做すことができるため、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面Ｓの近傍に分布すると看做すこともでき、中心点算出処理により求められた球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができる。
【数３３】
【０１４０】
以下に、歪評価値ｇＤ（Ｅ）を求める方法について説明する。
まず、式（５４）に示した関数ｋｅ（ｐ）は、二次形式で表される関数であるため、以下の式（６０）のように、ベクトルの内積として表現することもできる。また、歪誤差ベクトルｋ（Ｅ）の第ｉ行目の要素ｋｅ(ｑｉ−ｃＯ)は、関数ｋｅ（ｐ）に対して、ベクトル(ｑｉ−ｃＯ)を代入した値であるため、式（６２）によって示される６次元のベクトルｋｅ２（ｉ）、及び、式（６３）によって表される歪評価行列Ｅの各成分を並べた６次元のベクトルｅＥを用いて、式（６１）のように表せる。
【数３４】
【０１４１】
ここで、式（６４）で示す行列Ｘ２を導入する。行列Ｘ２は、各行に、式（６２）によって示される６次元のベクトルｋｅ２（ｉ）を転置したものと、３次元のベクトル(ｑｉ−ｑＣ)を転置したものを並べた、Ｎ行９列の行列である。
【数３５】
【０１４２】
式（６４）で表される行列Ｘ２を用いることで、式（５７）に示される目的関数ｆＳＤ（Ｅ、ｃ）を、式（６５）で表される目的関数ｇＳＤ（ｅ）に変形することができる。なお、ベクトルｅは、式（６６）に示すように、式（６３）に示した６次元のベクトルｅＥと、式（６７）に示す３次元のベクトルｅＸとを並べた、９次元のベクトルである。また、式（６７）に示すベクトルｅＸは、式（４７）に示す変数であるベクトルｃから、式（３７）に示した重心ｑＣを引いたベクトルである。
【数３６】
【０１４３】
式（６５）で表される目的関数ｇＳＤ（ｅ）を最小化する解ｅ＝ｅ０は、以下の式（６８）式で表現される連立方程式に対して、ガウス消去法や、コレスキー分解法などを適用することで、求められる。なお、式（６８）は、式（６５）に対して、最小二乗法を適用することによって算出される連立方程式である。
【数３７】
【０１４４】
このようにして得た解ｅ０に基づき、式（５３）の歪評価行列Ｅを復元する。そして、式（５８）に示した歪評価値ｇＤ（Ｅ）の値、即ち、歪評価行列Ｅのノルムを求め、歪評価値ｇＤ（Ｅ）の値が閾値δ０以下であるか否かを判定する。なお、歪評価行列Ｅのノルムは、歪評価行列Ｅの有する３つの固有値のうち絶対値が最大となる固有値λＥ１の絶対値に等しいため、ヤコビ法や、冪乗法により求めることができる。
そして、歪評価値ｇＤ（Ｅ）の値が閾値δ０以下である場合、中心点導出部２００は、処理をステップＳ２０６の中心点出力処理に進める。一方、歪評価値ｇＤ（Ｅ）の値が閾値δ０よりも大きな値である場合、中心点導出部２００は、処理をステップＳ２０１の初期化処理に戻す。
【０１４５】
［８． 中心点出力処理］
ステップＳ２０６の中心点出力処理は、歪判定処理において歪評価値ｇＤ（Ｅ）の値が閾値δ０以下であると判定された場合に実行される処理である。中心点出力処理において、ＣＰＵ１０（中心点導出部２００）は、中心点算出処理において算出された球面Ｓの中心点ｃＯの座標を出力し、状態推定部１００に対して供給する。
【０１４６】
本実施形態では、中心点導出部２００は、球面Ｓの中心点ｃＯの座標を出力しているが、球面Ｓ２の中心点ｃＯ２の座標を出力してもよい。歪評価値ｇＤ（Ｅ）の値が閾値δ０以下である場合には、中心点算出処理により求められた球面Ｓの中心点ｃＯと、球面Ｓ２の中心点ｃＯ２とは、ほぼ等しい座標となり、中心点ｃＯの座標を示すベクトルまたは中心点ｃＯ２の座標を示すベクトルのいずれも、磁気センサのオフセットｑＯＦＦとして採用することができるからである。
なお、球面Ｓ２の中心点ｃＯ２の座標は、目的関数ｇＳＤ（ｅ）を最小化する解ｅ０のうち式（６６）のｅＸに相当する部分を式（６７）に代入することにより求められる。
【０１４７】
［９． 結論］
以上に示したように、本実施形態に係る状態推定装置は、中心点導出部２００と、カルマンフィルタ演算部１２０を備える状態推定部１００とを備え、カルマンフィルタ演算部１２０がカルマンフィルタの演算に用いる状態ベクトルｘｋの要素のうち磁気センサのオフセット推定値ｑＯを、中心点導出部２００が出力する中心点ｃＯの座標によって上書きをしてカルマンフィルタの演算を行う。前述の通り、カルマンフィルタの状態遷移モデルにおいて磁気センサのオフセット推定値ｑＯの経時的な変化を定式化することはできず、また、カルマンフィルタによる状態ベクトルｘｋは更新前の状態ベクトルｘｋの影響を受けた値となるため、磁気センサのオフセット推定値ｑＯは、真値（磁気センサのオフセットｑＯＦＦ）から大きく乖離した値となる場合がある。この場合、状態推定装置は、システムの状態を正確に推定できないため、状態推定装置が算出する状態変数に基づいて、地磁気Ｂｇの向きや携帯機器１の姿勢μ等を算出することはできない。
これに対して、本実施形態におけるカルマンフィルタ演算部１２０は、中心点導出部２００が出力した中心点ｃＯの座標により、状態ベクトルｘｋの要素である磁気センサのオフセット推定値ｑＯを上書きする（ステップＳ１０３において行われる、状態変数上書処理）。前述の通り、中心点ｃＯの座標は、比較的短い期間に３次元磁気センサ７０から出力された磁気データｑ１〜ｑＮに基づいて算出される。すなわち、中心点ｃＯの座標を示すベクトルは、磁気センサのオフセットｑＯＦＦが急激且つ大きな変化をした場合であっても、変化後の磁気センサのオフセットｑＯＦＦを正確に表すことができる。
このような中心点ｃＯの座標によって、磁気センサのオフセット推定値ｑＯを上書きすることにより、状態ベクトルｘｋが真値から大きく異なる値となることを防止した。そして、携帯機器１が、状態ベクトルｘｋに基づいて、正確な地磁気Ｂｇの方向及び大きさ等を算出することを可能とした。
【０１４８】
また、本実施形態に係る、中心点導出部２００は、中心点算出処理を行う前に、磁気データ分布判定処理を行うことで、中心点算出処理で用いる複数の磁気データｑ１〜ｑＮが、中心点算出処理を行う上で適切なデータであるか否かの判定を行う。これにより、複数の磁気データｑ１〜ｑＮの示す座標の分布の３次元的な広がりの程度を把握し、複数の磁気データｑ１〜ｑＮの示す座標が平面的に分布している場合には、中心点算出処理を行わない。この結果、平面的に分布する複数の磁気データｑ１〜ｑＮに基づいた、不正確な（真値とは異なる）中心点ｃＯの座標の算出を未然に防止することが可能となり、不要な演算処理の防止による低消費電力化が可能となる。
また、本実施形態に係る、中心点導出部２００は、中心点算出処理を行った後に、歪判定処理を行い、中心点算出処理で算出された球面Ｓの中心点ｃＯの座標が、外部磁界Ｂｘによる影響を受けた不適切な（真値とは異なる）値であるか否かを判定する。これにより、外部磁界Ｂｘによる影響を受けた不適切な（磁気センサのオフセットｑＯＦＦと看做すことのできない）中心点ｃＯの座標の出力を防止することが可能となる。
このように、本実施形態に係る中心点導出部２００は、磁気データ分布判定処理及び歪判定処理により、磁気センサのオフセットｑＯＦＦと看做すことのできる真値に近い中心点ｃＯの座標のみを出力する。そして、カルマンフィルタ演算部１２０は、真値に近い中心点ｃＯの座標により磁気センサのオフセット推定値ｑＯを上書きすることで、状態ベクトルｘｋをより真値に近い正確な値へと更新することが可能となる。従って、本実施形態に係る携帯機器１は、状態推定装置が算出する状態ベクトルｘｋに基づいて、正確な地磁気Ｂｇの値を安定的に算出することが可能となった。
【０１４９】
＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【０１５０】
（１）変形例１
上述した実施形態では、カルマンフィルタ演算部１２０は、状態ベクトルｘのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点導出部２００から出力された中心点ｃＯの座標によって上書きをしてカルマンフィルタの演算を行うものであったが、本発明はこのような形態に限定されるものでは無く、状態ベクトルｘを中心点ｃＯの座標によって上書きせずにカルマンフィルタの演算を実行してもよい。
【０１５１】
図１６は、変形例１に係る状態推定装置のうち、ＣＰＵ１０が変形例１に係る状態推定プログラムを実行することにより実現される機能を表す機能ブロック図である。
図１６に示すように、変形例１に係る状態推定装置は、出力統合部３００を備える点と、状態推定部１００の代わりに状態推定部１００ａを備える点とを除いて、実施形態に係る状態推定装置と同様に構成される。状態推定部１００ａは、カルマンフィルタ演算部１２０の代わりにカルマンフィルタ演算部１２０ａを備える点を除き、状態推定部１００と同様に構成される。カルマンフィルタ演算部１２０ａは、状態ベクトルｘを中心点ｃＯの座標によって上書きしない点を除き、カルマンフィルタ演算部１２０と同様に動作する。
出力統合部３００は、カルマンフィルタ演算部１２０ａが出力する更新後の状態ベクトルｘ＋ｋと、中心点導出部２００が出力する中心点ｃＯの座標とに基づいて、状態ベクトルｘｃｋを出力する。
状態ベクトルｘｃｋは、状態ベクトルｘｋと同様に、姿勢μ、地磁気の強さｒ、地磁気の伏角φ、角速度ω、角速度センサのオフセット推定値ｇＯ、及び磁気センサのオフセット推定値ｑＯを要素とするベクトルである。状態ベクトルｘｃｋは、初期状態（時刻ｋ＝０）から、中心点導出部２００が中心点ｃＯの座標を出力するまでの期間は、状態ベクトルｘｋと等しい値に設定される。一方、中心点導出部２００が中心点ｃＯの座標を出力した後は、状態ベクトルｘｃｋを構成する要素のうち、磁気センサのオフセット推定値ｑＯは、中心点ｃＯの座標と等しい値に設定される。
【０１５２】
カルマンフィルタ演算部１２０ａは、３次元地磁気センサ７０、３次元加速度センサ８０、及び３次元角速度センサ９０の３つのセンサの出力を統合して、各種状態変数を推定するものであるため、高速に磁気センサのオフセット推定値ｑＯを算出することができる。一方、中心点導出部２００は、Ｎ個の磁気データｑ１〜ｑＮに基づいて中心点ｃＯの座標を算出するため、中心点ｃＯの座標が算出されるまでに一定の時間を要するが、歪判定処理により中心点ｃＯの座標の良否判定を行っているため、中心点ｃＯの座標を示すベクトルは磁気センサのオフセットｑＯＦＦと看做すことができる程度に正確な値である。
従って、変形例１に係る携帯機器１は、中心点導出部２００が中心点ｃＯの座標を算出するまでの間は、カルマンフィルタ演算部１２０ａが算出した磁気センサのオフセット推定値ｑＯを用いて地磁気Ｂｇのおおよその方向及び大きさを得ることができる。これにより、携帯機器１は、携帯機器１の利用者を長時間待たせること無く、地磁気Ｂｇの方向及び大きさを表示することが可能となり、利便性が向上する。
【０１５３】
（２）変形例２
上述した実施形態では、中心点導出部２００は、初期化処理、磁気データ蓄積処理、磁気データ分布判定処理、中心点算出処理、歪判定処理、及び中心点出力処理を行うが、本発明はこれに限定されるものでは無く、これらのうちの一部について行うものであってもよい。例えば、中心点導出部２００は、歪判定処理を行わず、中心点算出処理において算出された中心点ｃＯの座標を、中心点出力処理において出力してもよい。また、中心点導出部２００は、磁気データ分布判定処理を行わずに、中心点算出処理を実行してもよい。
この場合、中心点導出部２００は、磁気センサのオフセットｑＯＦＦが急激且つ大きな変化をした場合であっても、変化後の磁気センサのオフセットｑＯＦＦの値を容易に算出することができる。
【０１５４】
（３）変形例３
上述した実施形態では、カルマンフィルタ演算部１２０は、遅延部１２１において、状態ベクトルｘのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点導出部２００から出力された中心点ｃＯの座標によって上書きするものであるが、本発明はこのような形態に限定されるものでは無く、遅延部１２１以外において、状態ベクトルｘの中心点ｃＯの座標による上書きを行ってもよい。
例えば、シグマポイント生成部１２２において、２ａ+１個のシグマポイントχｋを生成する際に、式（１４）及び（１５）の右辺に現れる更新後の状態ベクトルｘ＋ｋのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点ｃＯの座標によって上書きしてもよい。また、状態遷移モデル部１２３の演算で用いる状態遷移モデルにおいて、式（２７）の右辺のうち磁気センサのオフセット推定値ｑＯ,Ｋ−１を表す要素を、中心点ｃＯの座標によって上書きしてもよい。さらには、加算器１２９において算出される、更新後の状態ベクトルｘ＋ｋのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点ｃＯの座標によって上書きしてもよい。
【０１５５】
（４）変形例４
上述した実施形態では、カルマンフィルタ演算部１２０は、非線形カルマンフィルタであるシグマポイントカルマンフィルタによる演算を実行しているが、本発明はこれに限定されるのではなく、カルマンフィルタ演算部１２０は、拡張カルマンフィルタなどの、シグマポイントカルマンフィルタ以外の非線形カルマンフィルタによる演算を実行するものであってもよい。
【０１５６】
（５）変形例５
上述した実施形態では、状態ベクトルｘを構成する要素して、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサのオフセット推定値ｇＯ、及び磁気センサのオフセット推定値ｑＯを採用し、これら６個の状態変数をカルマンフィルタの演算における推定の対象としたが、本発明はこれに限定されるものでは無い。例えば、状態ベクトルｘを、これら６個の状態変数のうちの一部により構成し、その一部について推定するものであってもよい。
【０１５７】
（６）変形例６
上述した実施形態では、観測値ベクトルｙｋを、磁気センサの出力である観測値ｑ、加速度センサの出力である観測値ａ、及び角速度センサの出力である観測値ｇを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、このうちの一部のみを利用して観測値ベクトルを生成してもよい。
【符号の説明】
【０１５８】
１…携帯機器、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定部、１２０…カルマンフィルタ演算部、１４０…初期値生成部、２００…中心点導出部、ｑ…磁気データ、ｘ…状態ベクトル、ｙ…観測値ベクトル、ｚ…観測残差、Ｓ…球面（第１球面）、ｃＯ…（第１球面の）中心点、Ｓ２…球面（第２球面）、ｃＯ２…（第２球面の）中心点、ｑＯＦＦ…磁気センサのオフセット、ｑＯ…磁気センサのオフセット推定値、Ｂｇ…地磁気、Ｂｉ…内部磁界、Ｂｘ…外部磁界。
【技術分野】
【０００１】
本発明は、状態推定装置、オフセット更新方法およびオフセット更新プログラムに関する。
【背景技術】
【０００２】
近年、携帯機器の高性能化及び多用途化に伴い、地磁気の方向や携帯機器の姿勢等の推定機能を備える携帯機器の研究開発が盛んに行われている。地磁気の方向や携帯機器の姿勢等を推定する場合、地磁気センサ等の単体のセンサからの出力のみを利用して推定するよりも、異種の物理量を測定する複数のセンサからの出力を統合して推定する方が、より高速な推定が可能となる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合する方法が開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００５】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００６】
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量を要素とする状態ベクトルを、より真値に近い値へと更新する。
しかし、観測残差は、更新前の状態ベクトルを用いて算出される値であるため、動的システムの状態が急激且つ大きく変化した場合には、状態ベクトルが真値から離れた値に更新されてしまうという問題が存在した。
【０００７】
本発明は、上述した点に鑑みてなされたものであり、動的システムの状態が急激且つ大きく変化した場合にも、動的システムの状態を正確に推定することを解決課題とする。
【課題を解決するための手段】
【０００８】
以下、本発明について説明する。なお、本発明の理解を容易にするために本実施形態、変形例、及び添付図面の参照符号を括弧書きにて付記するが、それにより本発明が本実施形態に限定されるものではない。
【０００９】
上述した課題を解決するため、本発明に係る状態推定装置は、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データ（ｑｉ）を順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データのうち前記部品の発生する磁界（Ｂｉ）の成分を表す３次元のベクトルデータであるオフセット（ｑＯＦＦ）を推定する状態変数（ｑＯ）を含む複数の状態変数を要素とするベクトルを状態ベクトル（ｘｋ）とし、前記複数のセンサの出力を要素とする観測値ベクトル（ｙｋ）及び前記状態ベクトルを用いて算出される観測残差（ｚｋ）に基づいて、前記状態ベクトルを更新するカルマンフィルタと、前記磁気データを蓄積する蓄積手段と、前記磁気データが所定数（Ｎ）蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面（Ｓ）の近傍に確率的に分布すると仮定して、前記第１球面の中心点（ｃＯ）を示す３軸の座標を算出する、中心点算出手段と、を備え、前記カルマンフィルタは、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、ことを特徴とする。
【００１０】
この発明によれば、状態推定装置は、中心点算出手段と、カルマンフィルタとを備え、カルマンフィルタがその演算に用いる状態ベクトルの要素のうち３次元磁気センサのオフセットを推定する状態変数を、中心点算出手段が算出する中心点の座標によって更新したうえで、カルマンフィルタの演算を行う。
【００１１】
カルマンフィルタは、ある時刻における動的システムの状態を表す物理量（状態変数）を要素とする状態ベクトルを、異種の物理量を測定する複数のセンサからの出力値を要素とする観測値ベクトルと状態ベクトルとを用いて算出される観測残差に基づいて、動的システムの真の値へと近付けるように更新する演算である。
具体的には、カルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻における状態ベクトルから、当該ある時刻から単位時間経過後の状態ベクトルを推定する。次に、カルマンフィルタは、動的システムの状態の推定値から複数のセンサからの出力値を推定する観測モデルを用いて、推定された状態ベクトルから観測値ベクトルを推定する。さらに、カルマンフィルタは、推定された観測値ベクトルと、各種センサからの実際の出力（実際の観測値）を要素とする観測値ベクトルとの差分を演算することにより、観測残差を算出する。そして、カルマンフィルタは、観測残差を用いて、状態ベクトルを更新する。
【００１２】
ところで、３次元磁気センサが搭載される機器は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。この場合、３次元磁気センサが出力するベクトルデータは、地磁気を表すベクトルの他に、機器に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、３次元磁気センサが出力するベクトルデータから、機器の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。このように、検出対象である地磁気の正確な値を得るために、補正処理において、３次元磁気センサから出力されるデータから取り除かれる内部磁界を表すベクトルを、３次元磁気センサのオフセットと呼ぶ。
内部磁界は、機器の内部状態が変化しない場合には、一定の方向及び大きさを有するが、機器の内部状態が変化した場合には、方向及び大きさが変化する。機器の内部状態は、機器の利用者による機器の操作や、機器の外部の環境等に依存して、急激且つ大きな変化を生ずる場合がある。そして、機器の内部状態の変化に伴い、機器に備えられた部品が発する内部磁界（すなわち、３次元磁気センサのオフセット）も、急激且つ大きな変化をする。
【００１３】
前述の通り、カルマンフィルタが行う演算には、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻における状態変数の値から、当該ある時刻よりも単位時間経過後の状態変数の値を推定する演算が含まれる。従って、カルマンフィルタが推定する状態変数は、状態遷移モデルにおいて定式化することのできる物理量であることが好ましい。しかし、３次元磁気センサのオフセットは、機器の利用者による機器の操作等により急激且つ大きな変化をするため、この値の変化を定式化することは困難である。
また、状態ベクトルを更新するために用いられる観測残差は、更新前の状態ベクトルの値に基づいて算出される。つまり、カルマンフィルタの演算により算出される更新された状態ベクトルの値は、更新前の状態ベクトルの値の影響を受けた値となる。そして、更新前の状態ベクトルの値は、それ以前の状態ベクトルの値の影響を受けた値となる。従って、カルマンフィルタの演算を繰り返す場合、カルマンフィルタが推定する状態ベクトルの値は、カルマンフィルタの演算を開始した時点から、状態ベクトルの値が推定される時点に至る全てのステップにおける状態ベクトルの値を考慮して算出される値である。よって、３次元磁気センサのオフセットが、急激且つ大きな変化をした場合には、３次元磁気センサのオフセットを推定する状態変数の更新後の値は、変化後の３次元磁気センサのオフセットから大きく乖離した値となることがある。
状態ベクトルの値が、実際の物理量を正確に表した値（真値）から大きく乖離した値に収束している場合、その後、カルマンフィルタによる演算を繰り返しても、状態ベクトルの値は真値に近づかなくなることがあり、また、仮に状態ベクトルの値が真値に近づくことがあっても、真値に近い値に収束するまでに長い時間を要する。この場合、状態推定装置は、システムの状態を正確に推定できないため、状態推定装置が算出する状態変数に基づいて、地磁気の向きや機器の姿勢等を算出することはできない。
【００１４】
地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地面に対して一定の方向と一定の大きさとを有する一様な磁界である。よって、地面に対して機器の姿勢を変化させる場合には、機器から見た地磁気の方向も変化することになる。すなわち、機器に搭載された３次元磁気センサから見た場合、地磁気は、機器の姿勢の変化に伴い向きが変化する一定の大きさを持ったベクトルとして表される。
一方、内部磁界は、機器の内部状態が変化しない場合には、機器に対して一定の方向を向き、一定の大きさを有する磁界である。よって、機器の内部状態が変化しない場合、内部磁界は、機器に搭載された３次元磁気センサから見て、機器をどのような姿勢に変化させた場合であっても、一定の方向と一定の大きさを有するベクトルとして表される。
従って、３次元磁気センサの姿勢を変化させつつ取得した複数の磁気データは、内部磁界の方向及び大きさを表すベクトルの先端を中心とし、地磁気の大きさを半径とする球面の近傍に分布することになる。
中心点算出手段は、複数の磁気データの示す座標の各々が、ある球面（第１球面）の近傍に分布すると仮定し、当該第１球面の中心点の座標を算出することにより、内部磁界、すなわち、３次元磁気センサのオフセットを算出する。中心点算出手段は、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、その後に取得された複数の磁気データにより正確なオフセットを算出することが可能である。
【００１５】
本発明の状態推定装置によれば、状態ベクトルを構成する要素のうち３次元磁気センサのオフセットを推定する状態変数を、中心点算出手段により出力した中心点により上書きする。
従って、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、変化後のオフセットを表す中心点の座標を中心点算出手段が算出し、３次元磁気センサのオフセットを推定する状態変数を中心点の座標によって上書きするため、３次元磁気センサのオフセットを推定する状態変数が真値（３次元磁気センサのオフセット）から乖離した状態が長期間にわたって継続することを防止することができる。すなわち、本発明に係る状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００１６】
また、上述した状態推定装置において、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセット（ｑＯＦＦ）を推定する状態変数（ｑＯ）を含む複数の状態変数を要素とするベクトルを状態ベクトル（ｘｋ）とし、状態遷移モデル（ｆ）を用いて、ある時刻における前記状態ベクトル（ｘｋ−１）から当該ある時刻から単位時間後の前記状態ベクトル（ｘ−ｋ）を推定し、観測モデル（ｈ）を用いて、前記単位時間後の前記状態ベクトル（ｘ−ｋ）から、推定観測値ベクトル（ｙ−ｋ）を算出し、前記推定観測値ベクトル（ｙ−ｋ）と前記複数のセンサの出力を要素とする観測値ベクトル（ｙｋ）とに基づいて観測残差（ｚｋ）を算出し、前記観測残差と前記単位時間後の前記状態ベクトルとに基づいて前記状態ベクトル（ｘｋ）を更新するカルマンフィルタと、前記磁気データを蓄積する蓄積手段と、前記磁気データが所定数（Ｎ）蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標の分布の３次元的な広がりの程度を示す分散評価値を算出する、分布判定手段と、前記磁気データが所定数蓄積され、且つ、前記分散評価値が、分散許容値（λ０）以上である場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面（Ｓ）の近傍に確率的に分布すると仮定して、前記第１球面の中心点（ｃＯ）を示す３軸の座標を算出する、中心点算出手段と、複数の前記磁気データの各々が示す３軸の座標が、第２球面（Ｓ２）と曲面（ＳＸ）とを重ね合わせることで得られる立体（ＳＤ）の表面近傍に確率的に分布すると仮定して、複数の前記磁気データに基づいて、前記立体と、前記第２球面との形状の相違の程度を示す歪評価値（ｇＤ（Ｅ））を算出し、前記歪評価値が歪許容値（δ０）以下であるか否かを判定する、歪判定手段と、前記歪判定手段の判定結果が肯定である場合に、前記第１球面の中心点を示す３軸の座標を出力する、中心点出力手段と、を備え、前記カルマンフィルタは、前記中心点出力手段が、前記第１球面の中心点を示す３軸の座標を出力する場合に、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新することを特徴とすることが好ましい。
【００１７】
前述の通り、中心点算出手段は、複数の磁気データの示す座標が第１球面の近傍に分布すると仮定して、第１球面の中心点の座標を算出する。従って、複数の磁気データが３次元的な広がりを有さず平面的に分布する場合には、第１球面の中心点の座標を正確に特定することはできない。
この発明によれば、まず、分布判定手段により複数の磁気データの３次元的な広がりの程度を示す分散評価値を算出し、その後、分散評価値が分散許容値以上である場合に、中心点算出手段が中心点の座標を算出する。従って、複数の磁気データが２次元的に分布する場合に、中心点算出手段が不正確な中心点の座標を算出することを防止することが可能となる。
これにより、不正確なオフセットを用いた補正処理が施されて不正確な地磁気の方向及び大きさが算出されることを防止することが可能となる。また、不正確な中心点の座標を算出する演算処理を未然に防止することが可能となるため、状態推定装置の低消費電力化が可能となった。
【００１８】
また、この発明によれば、歪判定手段により、複数の磁気データの座標を表面近傍に有する立体と、第２球面との形状の相違の程度を、歪評価値に基づいて判定する。
歪評価値が閾値よりも大きな値を有する場合には、立体の形状が第２球面とは異なる歪んだ形状を有するため、立体の表面近傍に分布する複数の磁気データの示す座標は、第１球面の近傍に分布すると看做すことはできない。中心点算出手段は、第１球面の近傍に複数の磁気データが存在していることを前提として、３次元磁気センサのオフセットの候補である中心点の座標を算出するため、複数の磁気データを近傍に有さない第１球面の中心点の座標を示すベクトルは、３次元磁気センサのオフセットとしての意味を有さない。本発明に係る状態推定装置は、歪判定手段を備えるため、３次元磁気センサのオフセットと看做すことのできない不適切な中心点の座標を示すベクトルにより、３次元磁気センサのオフセットを推定する状態変数を上書きすることを防止することが可能となった。
【００１９】
このように、本発明に係る状態推定装置は、分布判定手段及び歪判定手段を備えるため、３次元磁気センサのオフセットと看做すことのできる真値に近い中心点の座標が得られた場合にのみ、当該真値に近い中心点の座標を示すベクトルにより、３次元磁気センサのオフセットを推定する状態変数を上書きする。これにより、本発明に係る状態推定装置は、システムの状態を正確に推定することができ、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００２０】
また、本発明の具体的な態様として、上述した状態推定装置は、前記歪判定手段の判定結果が否定である場合に、利用者に前記機器の位置を変化させずに前記機器の姿勢を変化させるように促す手段を備えてもよい。
【００２１】
この発明によれば、上述した状態推定装置は、歪判定手段において歪評価値が歪許容値よりも大きいと判定される場合、すなわち、３次元磁気センサから出力される複数の磁気データの示す座標が立体の表面近傍に分布するときに立体の形状が球面から大きな歪を有する場合に、利用者に対して機器の位置を変化させずに機器の姿勢を変化させるように促す手段を備える。
３次元磁気センサは、測定対象である地磁気の他に、機器に備えられた部品の発する内部磁界と、外部の物体の発する不均一な外部磁界とを検出する場合がある。そして、この不均一な外部磁界の影響が大きい場合には、複数の磁気データの示す座標は、球面とは大きく異なる歪んだ形状を有する立体の表面近傍に分布することになり、中心点算出手段により算出された中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することができなくなる。
しかし、このような不均一な外部磁界も、外部磁界を発生させる外部の物体と３次元磁気センサとの相互位置関係が変化しない場合には、一定の大きさの磁界にすぎない。つまり、３次元磁気センサの位置を変化させずに姿勢を変化させる場合、外部磁界は、３次元磁気センサによって、一定の大きさを有し方向のみを変化させる磁界として検出される。この場合、３次元磁気センサから出力される複数の磁気データで示される座標は、ある球面近傍に分布することになる。そして、この球面の中心点の座標は、機器に備えられた部品の発する磁界（内部磁界）の成分を表すベクトルが示す座標とほぼ同一の座標となる。
前述の通り、第１球面は、複数の磁気データの示す座標が第１球面の近傍に分布することを仮定して定められる。よって、複数の磁気データの示す座標が球面の形状となるように分布する場合、複数の磁気データの示す座標は、中心点算出手段によって算出された中心点を中心とする第１球面近傍にも分布すると看做すことができる。従って、第１球面の中心点は内部磁界の成分を表すベクトルが示す座標とほぼ同一の座標となるため、第１球面の中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することが可能となる。
このように、上述した状態推定装置は、利用者に機器の位置を変化させずに機器の姿勢を変化させるように促す手段を備えることにより、不均一な外部磁界が存在する場所であっても、適切なオフセットを算出することができるような動作を利用者に促すことが可能となる。
【００２２】
また、本発明の具体的な態様として、上述した状態推定装置において、前記分散評価値は、複数の前記磁気データの各々が示す３軸の座標の分散を表す共分散行列（Ａ）の最小固有値（λ３）であることとしてもよい。
【００２３】
この発明によれば、複数の磁気データの分散を表す共分散行列の最小固有値の値を、分散評価値として用いている。
磁気データは３軸の座標を示すので、複数の磁気データの分散を表す共分散行列は、３行３列の行列となる。この共分散行列から３つの固有ベクトルと３つの固有値が得られる。３つの固有ベクトルの一つは、複数の磁気データが最も大きく分布する方向を表している。この固有ベクトルに対応する固有値は最大固有値となり、逆に、複数の磁気データが最も小さく分布する方向の固有ベクトルに対応する固有値が最小固有値となる。複数の磁気データが理想的に２次元的に分布する場合、最小固有値の値は０に限りなく近い値を有し、複数の磁気データが３次元的に分布する場合、最小固有値の値は、複数の磁気データの分布の３次元的な広がりの程度に従って大きな値を有する。従って、共分散行列の最小固有値を、分散評価値として用いることにより、複数の磁気データの分布の３次元的な広がりの程度を的確に把握することが可能となる。
【００２４】
また、本発明の具体的な態様として、上述した状態推定装置において、前記磁気データを前記第１球面の中心点を原点とする座標系で表した３軸のベクトル（ｑｉ−ｃＯ）と、当該３軸のベクトルを対称行列である歪評価行列（Ｅ）により変換したベクトル（Ｅ(ｑｉ−ｃＯ)）との内積を、複数の前記磁気データの各々について算出し、複数の算出結果を要素とするベクトルを歪誤差ベクトル（ｋ(Ｅ)）とし、複数の前記磁気データで示される３軸の座標により特定されるそれぞれの位置と前記第２球面（Ｓ２）との誤差を表すベクトルを、第２球面誤差ベクトル（δＳ２）とし、前記歪誤差ベクトルと、前記第２球面誤差ベクトルとの和を立体誤差ベクトル（δＳＤ）とし、前記歪評価行列（Ｅ）の各々の成分（ｅ１１〜ｅ３３）と、前記第２球面の中心点（ｃＯ２）を示す３軸の座標とを変数とし、前記立体誤差ベクトルの大きさを表す関数を歪評価関数（ｆＳＤ(Ｅ,ｃ)）としたとき、前記歪評価値は、前記歪評価関数を最小化するときの、前記歪評価行列のノルムであることとしてもよい。
【００２５】
この発明によれば、立体誤差ベクトルの大きさを最小化することで、歪評価行列と、第２球面の中心点の座標とを算出する。そして、歪評価値は、歪評価行列のノルムとして算出される。歪評価行列は、３軸のベクトルを座標変換するための対称行列であるため、３行３列の行列である。従って、歪評価値は３つの固有値を有し、３つの固有値のうち、絶対値の大きさが最大となる固有値の絶対値が、歪評価値となる。
立体誤差ベクトルは、第２球面誤差ベクトルと、歪誤差ベクトルとの和で与えられる。
第２球面誤差ベクトルは、単に、複数の磁気データで示される座標と第２球面との誤差を表すためのベクトルである。従って、複数の磁気データで示される座標と第２球面との誤差を最小化するように第２球面を定めた場合、球面誤差ベクトルにより表される複数の誤差は、全体として対称性を有し、方向依存性の無い、ホワイトノイズとなる。
一方、歪誤差ベクトルは、第１球面の中心点から見た磁気データの示す座標を表すベクトルと、当該ベクトルを歪評価行列により変換したベクトルとの内積を、複数の磁気データの各々について並べたベクトルである。換言すれば、歪誤差ベクトルの各要素は、対称行列である歪評価行列を係数行列とし、中心点から見た磁気データの座標を表すベクトルの３つの要素の各々を変数とする、３変数二次形式となる。すなわち、歪誤差ベクトルは、複数の磁気データで示される座標と第２球面との誤差の各々を、二次形式で表される同一の関数に基づく曲面上に存在するという制約の下で表現するベクトルである。このような歪誤差ベクトルを用いて、複数の磁気データと第２球面との誤差を表現することにより、ホワイトノイズとは別に、二次形式の関数に基づく曲面で表される誤差（すなわち、球面からの歪）を表現することが可能となる。
球面誤差ベクトルと歪誤差ベクトルとを加算して得られる立体誤差ベクトルは、複数の磁気データを表面近傍に有し、球面と曲面とを重ね合わせた球面とは異なる立体を表現する。そして、立体誤差ベクトルのうち、歪誤差ベクトルの大きさ評価することで、立体の形状が、第２球面の形状からどの程度の差異（歪）を有するか（つまり、ホワイトノイズとは異なる球面からの歪による誤差の大きさ）を、評価することが可能となる。その結果、立体と第２球面とが実質的に同一の形状を有するほど近似していると評価される場合には、複数の磁気データの示す座標が第２球面近傍にも分布すると看做すことができる。前述の通り、複数の磁気データの示す座標が、球面の形状となるように分布する場合、複数の磁気データの示す座標は、第１球面近傍にも分布すると看做すことができる。従って、立体と第２球面とが実質的に同一の形状を有するほど近似していると評価される場合には、第１球面の中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することができる。一方、立体が第２球面とは大きく異なる歪んだ形状を有すると評価される場合には、複数の磁気データで示される座標が第２球面近傍に分布すると看做すことはできないため、第１球面の中心点の座標を示すベクトルを３次元磁気センサのオフセットとして採用することを防止する必要がある。
すなわち、上述した状態推定装置は、中心点算出手段によって算出された中心点の座標のうち、不均一な外部磁界の影響を受けていない複数の磁気データより算出された、適切な中心点の座標を示すベクトルのみを、３次元磁気センサのオフセットとして採用することが可能である。この結果、上述した状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００２６】
また、上述した状態推定装置において、複数の前記磁気データの各々で示される３軸の座標として特定されるそれぞれの位置が、前記第１球面近傍に確率的に分布すると仮定した場合の、複数の前記磁気データで示される３軸の座標により特定されるそれぞれの位置と前記第１球面との誤差を表すベクトルを、第１球面誤差ベクトル（δＳ）とし、３次元のベクトル（ｃ）を変数とし、前記第１球面誤差ベクトル（δＳ）の大きさを表す関数を、中心点算出関数（ｆＳ（ｃ））としたとき、前記第１球面の中心点を示す３軸の座標は、前記中心点算出関数の値を最小化するときの、前記３次元のベクトルが示す３軸の座標であることを特徴とすることが好ましい。
【００２７】
この発明によれば、簡易な計算により、３次元磁気センサのオフセットの候補となる座標を算出することが可能となる。
【００２８】
また、本発明の具体的な態様として、上述した状態推定装置において、前記歪評価関数は、下記のｆＳＤ(Ｅ,ｃ)で表すこともできる。
【数１】
【００２９】
この発明によれば、歪評価関数を最小化するときの歪評価行列のノルムである歪評価値を簡単な計算により算出することができる。従って、上述した状態推定装置は、処理速度の向上、及び低消費電力化が可能になるという利点を有する。
【００３０】
また、本発明の具体的な態様として、上述した状態推定装置において、前記中心点算出関数は、下記のｆＳ（ｃ）で表すこともできる。
【数２】
【００３１】
この発明によれば、簡単な計算により中心点を算出することができる。従って、上述した状態推定装置は、処理速度の向上、及び低消費電力化が可能になるという利点を有する。
【００３２】
次に、本発明に係る状態推定装置に用いられるオフセット更新方法は、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データを蓄積する蓄積手段と、を備える状態推定装置に用いられるオフセット更新方法であって、前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新し、前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出し、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、ことを特徴とする。
【００３３】
この発明によれば、複数の磁気データに基づいて算出された中心点の座標により、状態ベクトルを構成する要素のうち３次元磁気センサのオフセットを推定する状態変数を上書きする。従って、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、３次元磁気センサのオフセットを推定する状態変数が、真値（３次元磁気センサのオフセット）から乖離することを防止することが可能となる。すなわち、状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【００３４】
次に、本発明に係る状態推定装置に用いられるオフセット更新プログラムは、磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記磁気データを蓄積する蓄積手段と、を備える状態推定装置に用いられるオフセット更新プログラムであって、前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新する処理と、前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する処理と、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する処理とを、コンピュータに実行させることを特徴とする。
【００３５】
この発明によれば、複数の磁気データに基づいて算出された中心点の座標により、状態ベクトルを構成する要素のうち３次元磁気センサのオフセットを推定する状態変数を上書きする。従って、３次元磁気センサのオフセットが急激且つ大きな変化をした場合であっても、３次元磁気センサのオフセットを推定する状態変数が、真値（３次元磁気センサのオフセット）から乖離することを防止することが可能となる。すなわち、状態推定装置は、システムの状態を正確に推定するため、状態推定装置が算出する状態変数に基づいて、正確な地磁気の向きや、正確な機器の姿勢等を算出することが可能となる。
【図面の簡単な説明】
【００３６】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係る３次元磁気センサが検出する磁界の概要を説明する概念図である。
【図４】本発明の実施形態に係る状態推定プログラムが実行されることにより実現される機能を表す機能ブロック図である。
【図５】本発明の実施形態に係る状態推定部の動作を表すフローチャートである。
【図６】本発明の実施形態に係るカルマンフィルタ演算部の機能ブロック図である。
【図７】本発明の実施形態に係る中心点導出部の動作を表すフローチャートである。
【図８】本発明の実施形態に係る３次元磁気センサが検出する地磁気と内部磁界とについて説明する概念図である。
【図９】本発明の実施形態に係る中心点算出処理を説明するための概念図である。
【図１０】本発明の実施形態に係る３次元磁気センサが検出する磁気データが２次元的に分布する場合について説明する概念図である。
【図１１】本発明の実施形態に係る磁気データ分布指標算出処理を説明するための概念図である。
【図１２】本発明の実施形態に係る３次元磁気センサが検出する地磁気、内部磁界、及び外部磁界について説明する概念図である。
【図１３】本発明の実施形態に係る歪み判定処理を説明するための概念図である。
【図１４】本発明の実施形態に係る歪み判定処理における立体を説明するための概念図である。
【図１５】本発明の実施形態に係る歪み判定処理を説明するための概念図である。
【図１６】本発明の変形例に係る状態推定プログラムが実行されることにより実現される機能を表す機能ブロック図である。
【発明を実施するための形態】
【００３７】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００３８】
［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器１のブロック図であり、図２は携帯機器１の外観を示す斜視図である。携帯機器１は、画面の姿勢に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢と、携帯機器１から見た地磁気の方向とを推定することによって実現される。
【００３９】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ２０、各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成するものである。また、携帯機器１は、地磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００４０】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｑを出力する。この磁気データｑは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。
【００４１】
ところで、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力する磁気データｑは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する地磁気を表すベクトルの他に、携帯機器１に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、３次元磁気センサが出力するベクトルデータ（磁気データｑ）から、携帯機器１の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。
このように、検出対象である地磁気の正確な値を得るために、補正処理において、３次元磁気センサ７０から出力されるデータから取り除かれる内部磁界を表すベクトルを磁気センサのオフセットｑＯＦＦと呼ぶ。
【００４２】
また、３次元磁気センサ７０は、図３に示すように、地磁気Ｂｇ及び内部磁界Ｂｉの他に、携帯機器１の外部に存在する物体２が出す外部磁界Ｂｘを検出する場合がある。詳細は後述するが、外部磁界Ｂｘは、不均一な磁界である。従って、外部磁界Ｂｘの成分を算出して、これを３次元磁気センサ７０の出力から取り除いて、正確な地磁気Ｂｇの値を算出することは困難である。一方、外部磁界Ｂｘが存在しない場合、または外部磁界Ｂｘの影響の小さな場合には、３次元磁気センサ７０の出力から、内部磁界Ｂｉをオフセットとして取り除く処理を施すことにより、正確な地磁気Ｂｇを算出することが可能である。
【００４３】
ここで、説明の便宜上、図３に示すような地上座標系ΣＧ及びセンサ座標系ΣＳを導入する。地上座標系ΣＧは、地上に固定された座標系であり、地上の任意の一点を原点とし、互いに直交する３つの方向、例えば、水平東、水平北、及び鉛直上向きを、それぞれｘ軸、ｙ軸、及びｚ軸とする座標系である。センサ座標系ΣＳは、携帯機器１に固定された座標系である。３次元磁気センサ７０が出力する磁気データは、センサ座標系ΣＳのｘ軸、ｙ軸、ｚ軸上にプロットされ、センサ座標系ΣＳのベクトルデータとして表現される。
また、地上座標系ΣＧから見たセンサ座標系ΣＳの原点の位置及び姿勢（つまり、地上座標系ΣＧから見た携帯機器１の位置及び姿勢）を、それぞれ、位置ＰＳ及び姿勢μと表現する。
なお、図３に記載された各ベクトルの左上に付された添字Ｇは、当該ベクトルが地上座標系ΣＧにおいて表現されたベクトルであることを意味する。
【００４４】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００４５】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示すベクトルデータである。
【００４６】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラムを実行することによって、携帯機器１の姿勢や磁気センサのオフセット等の物理量（状態変数）を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラムを実行することにより、状態推定装置として機能する。
【００４７】
図４は、状態推定装置のうち、ＣＰＵ１０が状態推定プログラムを実行することにより実現される機能を表す機能ブロック図である。図４に示すように、状態推定装置は、状態推定部１００と、中心点導出部２００とを備える。状態推定部１００は、カルマンフィルタ演算部１２０と、初期値生成部１４０とを備える。カルマンフィルタ演算部１２０は、磁気データｑ、加速度データａ、及び角速度データｇを要素とする観測値ベクトルｙを用いてカルマンフィルタの演算を実行することで、状態ベクトルｘを周期的に更新し、更新後の状態ベクトルｘ＋を出力する。初期値生成部１４０は、各種設定情報に基づいて初期値ＩＮＩを生成し、生成した初期値ＩＮＩを出力する。
【００４８】
中心点導出部２００は、３次元磁気センサ７０より供給される磁気データｑを複数個蓄積したうえで、複数個の磁気データｑ１〜ｑＮに基づいて中心点（第１球面の中心点）ｃＯの座標を算出し、算出した中心点ｃＯの座標を出力する（Ｎは精度よく中心点ｃＯの座標を導出するために必要な磁気データの規定測定回数を表す５以上の自然数）。詳細は後述するが、中心点ｃＯは、センサ座標系ΣＳ上の座標を示す点であり、中心点ｃＯの座標を示すベクトルは、磁気センサのオフセットｑＯＦＦの候補となる。
【００４９】
［２． カルマンフィルタについて］
図５は、状態推定部１００の動作を表すフローチャートである。当該フローチャートに示される状態推定処理は、ＣＰＵ１０が状態推定プログラムを実行することにより開始される。
ステップＳ１０１において、ＣＰＵ１０は、初期値生成処理を実行する。具体的には、ＣＰＵ１０は、ＲＯＭ３０に記録された各種設定情報を参照し、初期値ＩＮＩを生成する。すなわち、ＣＰＵ１０は、ステップＳ１０１の初期値生成処理を実行することにより、初期値生成部１４０として機能する。
【００５０】
ステップＳ１０２において、ＣＰＵ１０は、中心点導出有無判定処理を実行する。具体的には、ＣＰＵ１０は、後述する中心点導出処理（図７参照）において中心点ｃＯの座標が生成されたか否かを判定する（より具体的には、ＣＰＵ１０は、中心点ｃＯの座標が、後述するステップＳ２０６の中心点出力処理から引き渡されたか否かを判定する）。そして、中心点導出処理において中心点ｃＯの座標が生成された場合、ＣＰＵ１０は、処理をステップＳ１０３に進める。一方、中心点導出処理において中心点ｃＯの座標が生成されていない場合、ＣＰＵ１０は、処理をステップＳ１０４に進める。
ステップＳ１０３において、ＣＰＵ１０は、状態変数上書処理を実行する。詳細は後述するが、後述する状態ベクトル更新処理（ステップＳ１０４）において行われるカルマンフィルタの演算の対象である状態ベクトルｘは、複数の状態変数を要素とするベクトルであり、複数の要素には、３次元のベクトルである磁気センサのオフセット推定値ｑＯが含まれる。状態変数上書処理において、ＣＰＵ１０は、中心点導出処理によって得られた中心点ｃＯを表す３軸の座標の各成分によって、３次元のベクトルである磁気センサのオフセット推定値ｑＯを構成する各要素を上書きする。
ステップＳ１０４において、ＣＰＵ１０は、状態ベクトル更新処理を実行する。具体的には、ＣＰＵ１０は、観測値ベクトルｙを用いてカルマンフィルタの演算を実行することで、状態ベクトルｘを周期的に更新し、更新後の状態ベクトルｘ＋を算出する。
ステップＳ１０５において、ＣＰＵ１０は、状態ベクトル出力処理を実行する。具体的には、ＣＰＵ１０は、状態ベクトル更新処理で算出された更新後の状態ベクトルｘ＋を出力する。
ＣＰＵ１０は、以上に示したステップＳ１０２〜Ｓ１０５の処理を実行することにより、カルマンフィルタ演算部１２０として機能する。そして、携帯機器１は、ＣＰＵ１０がステップＳ１０２〜Ｓ１０５の処理を実行することにより生成される更新後の状態ベクトルｘ＋ｋ（つまり、複数の状態変数）を用いて、地磁気Ｂｇの向きや、携帯機器１の姿勢等を算出することができる。
次に、ＣＰＵ１０は、状態推定プログラムを終了する終了条件を充足するか否かを判定する（ステップＳ１０６）。終了条件は、携帯機器１の仕様等に基づいて適宜定めればよい。例えば、携帯機器１の電源がオフ状態になることを終了条件としてもよい。終了条件を充足する場合、ＣＰＵ１０は、当該フローチャートに示す状態推定処理を終了する。一方、終了条件を充足しない場合、ＣＰＵ１０は、処理をステップＳ１０２に進める。
【００５１】
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルと、を有する。そして、カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量（状態変数）を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定する。次に、カルマンフィルタは、推定された状態ベクトルに基づいて、動的システムの状態を測定する複数のセンサの出力値を要素とする観測値ベクトルの値を推定する。さらに、推定された観測値ベクトルと、実際のセンサの出力値を要素とする観測値ベクトルとの差分として算出される観測残差とに基づいて、推定された状態ベクトルを、実際の値（真値）に近い値へと更新し、更新後の状態ベクトルを出力する。
カルマンフィルタは、以上のような演算を繰り返すことにより、状態ベクトルを構成する複数の状態変数の各々を、実際の物理量に出来るだけ近く実際の物理量を正確に表した値（真値）へと近付ける。
【００５２】
本実施形態におけるカルマンフィルタは、状態変数として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサ９１〜９３のオフセット推定値ｇＯ、及び磁気センサ７１〜７３のオフセット推定値ｑＯを採用し、これらを推定の対象とする。なお、状態変数に、角速度センサのオフセット推定値ｇＯを含ませているのは、角速度データｇに、角速度センサセンサ９１〜９３のオフセットが重畳するからである。
これらの状態変数を要素とする時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。
【数３】
【００５３】
本実施形態では、姿勢μを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。具体的には、携帯機器１に固定されたセンサ座標系ΣＳの各軸と、地上座標系ΣＧの各軸とが一致する姿勢μを基準姿勢と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、基準姿勢に対して単位ベクトルαの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢μは、式（２）で与えられる。
【数４】
【００５４】
本実施形態におけるカルマンフィルタの観測対象は、３軸の磁気センサ７１〜７３から出力される磁気データｑ、３軸の加速度センサ８１〜８３から出力される加速度データａ、及び３軸の角速度センサ９１〜９３から出力される角速度データｇである。このとき、時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数５】
【００５５】
前述のとおり、初期値生成部１４０は、時刻Ｔ＝０における状態変数を要素とする初期値ＩＮＩを算出する（初期値生成処理）。初期値ＩＮＩは、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｇＯの初期値ｇＯ,０、及び磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０を要素として含む。
初期値ＩＮＩは、カルマンフィルタの演算によって状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下のような値に設定してもよい。
【００５６】
地磁気の強さｒの初期値ｒ０、及び地磁気の伏角φの初期値φ０は、例えば、ＧＰＳ部６０から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ１が格納される。初期値生成部１４０は、ルックアップテーブルＬＵＴ１を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さｒの初期値ｒ０及び地磁気の伏角φの初期値φ０として設定する。
なお、携帯機器１が衛星からの電波の届かない場所（例えば、地下街）にある場合には、ＧＰＳ部６０から位置情報が得られない。そのような場合には、携帯機器１が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルＬＵＴ１に格納されている。初期値生成部１４０は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルＬＵＴ１から読み出す。例えば、日本で販売された携帯機器１については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さｒの初期値ｒ０、及び地磁気の伏角φの初期値φ０を算出する。
【００５７】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。角速度センサのオフセット推定値ｇＯの初期値ｇＯ,０は、通常「０」近辺に調整されているため、「０」に設定する。姿勢μの初期値μ０に関しては、例えば、携帯機器１が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。
【００５８】
磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０は、例えば、時刻Ｔ＝０の磁気センサの観測値ｑ０、姿勢μの初期値μ０、携帯機器１を使用する地域の地磁気ベクトルｑtypical、及び、式（５）に示す行列Ｂ（μ）を用いて、以下に示す式（４）で与えられる値に設定する。ここで、行列Ｂ（μ）は、携帯機器１が姿勢μである場合に、地上座標系ΣＧにおいて表現されたベクトルを、センサ座標系ΣＳで表現するための座標変換行列である。なお、ＲＯＭ３０には、位置情報と地磁気ベクトルｑtypicalとを対応づけて記憶したルックアップテーブルＬＵＴ２が記憶されている。初期値生成部１４０は、ＧＰＳ部６０で生成される位置情報に基づいてルックアップテーブルＬＵＴ２を参照して地磁気ベクトルｑtypicalを取得し、式（４）を演算することによって、磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０を得る。
【数６】
【００５９】
初期値生成部１４０は、以上のようにして生成された姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｇＯの初期値ｇＯ,０、及び磁気センサのオフセット推定値ｑＯの初期値ｑＯ,０を要素とするベクトルである初期値ＩＮＩを生成し、これを出力する。
【００６０】
［３． カルマンフィルタによる演算］
次に、ステップＳ１０４において行われる、状態ベクトル更新処理の内容、すなわち、本実施形態に係るカルマンフィルタによる演算の内容について説明する。
カルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定し、この推定値を、推定された状態ベクトルｘ−ｋとして出力する。そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、状態遷移モデルにより推定された状態ベクトルｘ−ｋに基づいて観測値ベクトルｙｋを推定し、この推定値を、推定された観測値ベクトル（推定観測値ベクトル）ｙ−ｋとして出力する。
なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。シグマポイントカルマンフィルタとは、状態ベクトルｘｋ−１の周囲に複数のシグマポイントχｋ−１を設定し、これらの複数のシグマポイントχｋ−１を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ−ｋを推定し、推定された複数のシグマポイントχ−ｋの平均を算出することにより、推定された状態ベクトルｘ−ｋを求める演算である。従って、推定された観測値ベクトルｙ−ｋは、厳密には、推定された状態ベクトルｘ−ｋの周辺に存在する複数のシグマポイントχ−ｋに基づいて算出される。
次に、カルマンフィルタは、推定された観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｚｋとして算出し、観測残差ｚｋに基づいてカルマンゲインＫｋを算出する。さらに、カルマンフィルタは、観測残差ｚｋとカルマンゲインＫｋとを用いて、推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトルｘ＋ｋを算出する。
状態ベクトル更新処理は、以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタの演算により、状態ベクトルｘｋを実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける処理である。
【００６１】
［３．１． カルマンフィルタによる演算の概要］
本実施形態における、カルマンフィルタの状態遷移モデルは以下に示す式（６）で与えられ、観測モデルは以下に示す式（７）で与えられる。なお、本実施形態では、式（６）に現れる関数ｆ及び式（７）に現れる関数ｈは、非線形の関数である。
【数７】
【００６２】
ここで、状態ベクトルｘｋはａ次元のベクトルであり、観測値ベクトルｙｋはｂ次元のベクトルである。なお、本実施形態では、ａ＝１５であり、ｂ＝９である。また、式（６）に現れるプロセスノイズｗｋ及び式（７）に現れる観測ノイズｖｋは０を中心とするガウスノイズである。
式（６）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗｋ−１とを加算することにより推定されることを示している。
また、式（７）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗｋの共分散をＱｋ、観測ノイズｖｋの共分散をＲｋと表す。
【００６３】
時刻Ｔ＝ｋにおける観測残差ｚｋは、実際の観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（８）で表される。カルマンフィルタは、観測残差ｚｋと、式（９）に示すカルマンゲインＫｋとを用いて、以下の式（１０）に示すように、推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタは、以下の式（１１）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
【数８】
【００６４】
観測モデルを用いて推定された観測値ベクトルｙ−ｋは、式（７）に示すように、状態遷移モデルを用いて推定された状態ベクトルｘ−ｋを、観測モデルに適用することで算出される理論値である。よって、観測モデルを用いて推定された観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｚｋは、推定された状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
式（１０）では、このような観測残差ｚｋを用いて、推定された状態ベクトルｘ−ｋを、真値に近い更新後の状態ベクトルｘ＋ｋへと更新する。
【００６５】
［３．２． シグマポイントカルマンフィルタによる演算］
なお、本実施形態では、カルマンフィルタ（カルマンフィルタ演算部１２０）は、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、図６に示す、カルマンフィルタ演算部１２０の機能ブロック図を参照しつつ、シグマポイントカルマンフィルタによる状態ベクトルの更新について、具体的に説明する。
【００６６】
遅延部１２１は、加算器１２９から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部１２２に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部１２１は、初期値生成部１４０が出力する初期値ＩＮＩを用いて、状態ベクトルｘ＋ｋ−１を生成する。
また、中心点導出部２００が中心点ｃＯを出力する場合、遅延部１２１は、状態ベクトルｘ＋ｋ−１のうち、磁気センサのオフセット推定値ｑＯに対応する要素を、中心点導出部２００より出力される中心点ｃＯにより上書きしたうえで、状態ベクトルｘ＋ｋ−１を生成する（ステップＳ１０３の状態変数上書処理）。
【００６７】
シグマポイント生成部１２２は、ａ行ａ列の共分散行列Ｐ＋ｋ−１及びＱｋ−１を用いて、２ａ+１個のシグマポイントを生成する。具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマボイントの広がりを表すスケーリングパラメータλを用いて、式（１２）及び式（１３）に示すベクトルσｋを定義する。
【数９】
【００６８】
このとき、シグマポイント生成部１２２は、ベクトルσｋ−１と状態ベクトルｘ＋ｋ−１とに基づいて、式（１４）及び式（１５）で示される２ａ+１個のシグマポイントχｋ−１を生成する。
【数１０】
【００６９】
状態遷移モデル部１２３は、式（１６）に示すように、時刻Ｔ＝ｋ−１における２ａ+１個のシグマポイントχｋ−１（ｊ）の各々を、状態遷移モデルに適用することにより、時刻Ｔ＝ｋにおける２ａ+１個のシグマポイントの推定値χ−ｋ（ｊ）を算出する。
【数１１】
【００７０】
次に、平均算出部１２４は、式（１７）に示すように、時刻Ｔ＝ｋにおける２ａ+１個のシグマポイントの推定値χ−ｋの平均値を計算することで、推定された状態ベクトルｘ−ｋを算出する。
【数１２】
【００７１】
また、平均算出部１２４は、式（１８）に示す、推定された状態ベクトルｘ−ｋの誤差の共分散Ｐ−ｋを算出する。
【数１３】
【００７２】
一方、観測モデル部１２５は、式（１９）に示すように、時刻Ｔ＝ｋにおける２ａ+１個のシグマポイントχｋ（ｊ）の各々を、観測モデルに適用することにより、２ａ+１個の推定観測値γｋ（ｊ）を算出する。
【数１４】
【００７３】
そして、平均処理部１２６は、式（２０）に示すように、２ａ+１個の推定観測値γｋ（ｊ）の平均を演算することにより、推定された観測値ベクトルｙ−ｋを算出する。
【数１５】
【００７４】
次に、平均処理部１２６は、式（２１）に示す観測残差の共分散Ｐｙｙｋを算出する。
【数１６】
【００７５】
減算器１２７は、式（８）に示したように、観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとの差分として、観測残差ｚｋを算出する。
カルマンゲイン付与部１２８は、式（２２）に示す相互共分散行列Ｐｘｙｋを算出する。そして、カルマンゲイン付与部１２８は、式（９）に示したように、残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出し、式（１０）の右辺第２項（Ｋｋｚｋ）の演算を実行する。また、カルマンゲイン付与部１２８は、式（１１）に示したように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを、Ｐ−ｋからＰ＋ｋに更新する。
【数１７】
【００７６】
加算器１２９は、式（１０）に示したように、推定された状態ベクトルｘ−ｋと、カルマンゲイン付与部１２８から出力される式（１０）の右辺第２項（Ｋｋｚｋ）とを加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
【００７７】
［３．３． 状態遷移モデル］
次に、状態遷移モデル部１２３の演算で用いる状態遷移モデルについて説明する。
【００７８】
本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘｋを構成する状態変数のうち、姿勢μについての状態遷移は、式（２３）により定義される。式（２３）は、時刻Ｔ＝ｋ−１における姿勢μｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μｋを推定する演算を表す。ここで、μーｋは、時刻ｋにおける推定された状態ベクトルｘ−ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（２３）の右辺の演算子Ωは、式（２４）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、演算子［ｌ×］は、式（２５）で定義される。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ＋ｋのうち角速度を表す状態変数に対応する要素をω＋ｋで表したとき、演算子Ωを構成する成分Ψ＋ｋは、式（２６）で定義される。
【数１８】
【００７９】
姿勢μは、クォータニオンで表現されるため、正規化条件||μ||＝１が満たされていないといけないが、シグマポイントの平均を求めると正規化条件が満たされなくなる。そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つため、状態ベクトルｘｋのうち姿勢μｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新してもよい。
【００８０】
地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角φｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角φｋ−１とは等しい値であると仮定する。
同様に、角速度センサのオフセット推定値ｇＯは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度センサのオフセット推定値ｇＯ,Ｋと、時刻Ｔ＝ｋ−１における角速度センサのオフセット推定値ｇＯ,Ｋ−１とは等しい値であると仮定する。
【００８１】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００８２】
前述の通り、磁気センサのオフセットｑＯＦＦは、携帯機器１の部品が発する内部磁界Ｂｉの方向及び大きさをセンサ座標系ΣＳにおいて表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、磁気センサのオフセットｑＯＦＦも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、磁気センサのオフセットｑＯＦＦも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、磁気センサのオフセットｑＯＦＦが変化するタイミングや磁気センサのオフセットｑＯＦＦの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｑＯ,Ｋ−１を用いて、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｑＯ,Ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｑＯ,Ｋと、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｑＯ,Ｋ−１とは等しいと仮定する。
【００８３】
このように、本実施形態に係る状態遷移モデルでは、以下に示す式（２７）のように、状態ベクトルｘｋを構成する複数の状態変数のうち、姿勢μを表す状態変数以外は、前の時刻から変化しないという前提を置く。
【数１９】
【００８４】
なお、カルマンフィルタの演算において、状態ベクトルｘｋは、観測残差ｚｋに基づいて更新される。従って、式（２７）に示された姿勢μ以外の値が、全く変化しないわけではない。状態ベクトルｘｋは、加算器１２９によって、観測残差ｚｋに基づいて真値に近づくように更新される。
具体的には、時刻Ｔ＝ｋにおける状態ベクトルｘｋは、式（６）、式（８）、及び式（１０）に示すように、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を用いて算出される観測残差ｚｋに基づいて更新される。同様に、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１は、時刻Ｔ＝ｋ−２における状態ベクトルｘｋ−２を用いて算出される観測残差ｚｋ−１に基づいて更新される。すなわち、カルマンフィルタの演算において、状態ベクトルｘｋは、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１の値を考慮して、更新される。従って、状態ベクトルｘｋの値が真値から大きく乖離した値に収束している場合、その後カルマンフィルタの演算を繰り返しても、状態ベクトルｘｋの値は真値に近づかなくなることがあり、また、仮に状態ベクトルｘｋの値が真値に近づくとしても、真値に近い値に収束するまでに長い時間を要することになる。
【００８５】
前述の通り、磁気センサのオフセットｑＯＦＦは、携帯機器１の内部状態の変化に伴い、急激に大きく変化をする場合がある。磁気センサのオフセットｑＯＦＦが急激に大きく変化する場合、磁気センサのオフセット推定値ｑＯ,Ｋと、磁気センサのオフセットｑＯＦＦ（真値）とは、大きく異なる値となる。磁気センサのオフセット推定値ｑＯ,Ｋが真値から大きく乖離した場合、カルマンフィルタによって状態ベクトルｘｋの更新を繰り返しても、磁気センサのオフセット推定値ｑＯ,Ｋが真値に近づかなくなることがあり、また、仮に真値に近づくことができても、真値に近い値に収束するまでに長い期間を要する。そして、磁気センサのオフセット推定値ｑＯ,Ｋが、真値とは大きく異なる値となる場合、携帯機器１は、地磁気Ｂｇの向きや、携帯機器１の姿勢μ等を算出することができない。
【００８６】
そこで、本実施形態では、中心点導出部２００から中心点ｃＯの座標が出力された場合、磁気センサのオフセット推定値ｑＯを、中心点ｃＯの座標によって上書きする（ステップＳ１０３の状態変数上書処理）。
中心点ｃＯの座標は、Ｎ個の磁気データｑ１〜ｑＮに基づいて算出される。従って、中心点導出部２００は、磁気センサのオフセットｑＯＦＦが急激に変化した場合であっても、オフセットｑＯＦＦの変化後に取得したＮ個の磁気データｑ１〜ｑＮを用いることで、磁気センサのオフセットｑＯＦＦを正確に表す中心点ｃＯの座標を算出することができる。つまり、中心点ｃＯの座標は、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１の値を考慮して算出される磁気センサのオフセット推定値ｑＯ,Ｋに比べて、磁気センサのオフセットｑＯＦＦの急激な変化に柔軟に対応した真値に近い値として算出される。
従って、本実施形態に係る状態推定装置は、磁気センサのオフセットｑＯＦＦが急激に変化した場合であっても、磁気センサのオフセット推定値ｑＯを、中心点ｃＯの座標によって上書きすることにより、状態ベクトルｘｋが真値から乖離することを防止することができる。
【００８７】
［３．４． 観測モデル］
次に、観測モデル部１２５において行われる演算で用いる観測モデルについて説明する。
【００８８】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｇＯとを用いて、式（２８）で与えられる。
【数２０】
【００８９】
また、観測モデルにおいて、地上座標系ΣＧでの地磁気ベクトルは式（２９）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｑの推定値γｍａｇは、磁気センサのオフセット推定値ｑＯと、式（５）で示した行列Ｂ（μ）とを用いて、式（３０）で与えられる。
【数２１】
【００９０】
また、観測モデルにおいて、地上座標系ΣＧでの重力ベクトルは、重力加速度で正規化して、式（３１）で与えられる。従って、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（５）で示した行列Ｂ（μ）を用いて、式（３２）で与えられる。
【数２２】
【００９１】
従って、式（３）、式（２８）、式（３０）、及び式（３２）を、式（８）に適用することにより、観測残差ｚｋは、式（３３）で与えられる。
【数２３】
【００９２】
［４． 中心点導出部の動作］
図７は、中心点導出部２００の動作を表すフローチャートである。当該フローチャートに示される中心点導出処理は、ＣＰＵ１０が状態推定プログラムを実行することにより開始される。
【００９３】
ステップＳ２０１において、ＣＰＵ１０は、初期化処理を実行する。具体的には、ＣＰＵ１０は、ＲＡＭ２０に記憶した磁気データを廃棄する。
なお、本実施形態に係る初期化処理では、ＣＰＵ１０は、磁気データの全部を廃棄するが、本発明はこのような形態に限定するものではない。例えば、ＣＰＵ１０は、ＲＡＭ２０に蓄積されたデータのうち古い方から一定割合のデータのみを廃棄してもよい。
【００９４】
ステップＳ２０２において、ＣＰＵ１０は、磁気データ蓄積処理を実行する。具体的には、ＣＰＵ１０は、３次元磁気センサ７０から順次出力される磁気データｑｉをＲＡＭ２０に格納する。そして、ＣＰＵ１０は、磁気データｑｉがＲＡＭ２０にＮ個以上蓄積された場合、処理をステップＳ２０３へと進める。
【００９５】
ステップＳ２０３において、ＣＰＵ１０は、磁気データ分布判定処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０２の磁気データ蓄積処理においてＲＡＭ２０等に蓄積された複数の磁気データのうち、新しい方からＮ個の磁気データｑ１〜ｑＮ（Ｎは精度よく中心点ｃＯの座標を導出するために必要な磁気データの規定測定回数を表す５以上の自然数）が、後続するステップ２０４で実施される中心点算出処理において使用するデータとして適切なものであるか否かを判定する。そして、判定結果が肯定である場合、ＣＰＵ１０は、処理をステップＳ２０４へと進める。一方、判定結果が否定である場合、ＣＰＵ１０は、ＲＡＭ２０に蓄積された複数の磁気データの全部を廃棄した後に、処理をステップＳ２０２へと戻す。
なお、本実施形態では、磁気データ分布判定処理における判定結果が否定である場合、ＣＰＵ１０は、ＲＡＭ２０に蓄積された複数の磁気データの全部を廃棄するが、本発明はこのような形態に限定されるものではない。例えば、ＣＰＵ１０は、判定結果が否定である場合に、ＲＡＭ２０に蓄積された複数の磁気データを廃棄せずに一定期間待機し、判定時にＲＡＭ２０に蓄積されていた複数の磁気データに対して、待機している一定期間に新たにＲＡＭ２０に蓄積された複数の磁気データを加えたうえで、再度判定処理を行ってもよい。
【００９６】
ステップＳ２０４において、ＣＰＵ１０は、中心点算出処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０２の磁気データ蓄積処理においてＲＡＭ２０等に蓄積されたＮ個の磁気データｑ１〜ｑＮの示す座標が、ある球面（第１球面）Ｓの近傍に確率的に分布すると仮定して、球面Ｓの中心点ｃＯの座標を算出する。
【００９７】
ステップＳ２０５において、ＣＰＵ１０は、歪判定処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０２の磁気データ蓄積処理においてＲＡＭ２０等に蓄積されたＮ個の磁気データｑ１〜ｑＮの示す座標がある立体ＳＤの表面近傍に分布すると仮定し、立体ＳＤの形状と球面の形状とが同一と看做すことができる程度に近似しているか否かを判定する。判定結果が肯定の場合、すなわち、Ｎ個の磁気データｑ１〜ｑＮの示す座標が球面に近い形状の立体ＳＤの表面近傍に分布すると判定した場合には、ＣＰＵ１０は、処理をステップＳ２０６に進める。一方、判定結果が否定の場合、すなわち、立体ＳＤが球面とは異なる大きく歪んだ形状を有すると判定した場合、ＣＰＵ１０は、処理をステップＳ２０１に戻す。
【００９８】
なお、本実施形態に係る歪判定処理では、立体ＳＤと球面とが異なる形状であると判定した場合、ＣＰＵ１０は、処理をステップＳ２０１に戻すが、本発明はこれに限定されるものではない。例えば、歪判定処理において、立体ＳＤと球面とが異なる形状であるとの判定結果が得られた場合に、ＣＰＵ１０は、何らかのメッセージを表示部５０に出力した上で処理を一旦停止させ、携帯機器１の利用者からの指示を待ってステップＳ２０１から処理を再開させてもよい。
Ｎ個の磁気データｑ１〜ｑＮを取得する際に、携帯機器１の利用者が携帯機器１を手で握り、携帯機器１の位置ＰＳが変化するように携帯機器１を振るのではなく、携帯機器１の位置（厳密には、３次元磁気センサ７０の位置）ＰＳを固定したままその姿勢μのみを変化させるようにすると、外部磁界Ｂｘの影響を低く抑えることができる（図１３（Ｂ）参照）。そのため、ステップＳ２０５において、立体ＳＤが球面とは異なる大きな歪を有する形状であると判定された場合には、携帯機器１の位置を固定したまま回転させることを、携帯機器１の利用者に対して指示してもよい。携帯機器１の利用者に対する指示は、携帯機器１の表示部５０に画像や動画等を表示したり、音声等を用いたりすることで、行ってもよい。
【００９９】
ステップＳ２０６において、ＣＰＵ１０は、中心点出力処理を実行する。具体的には、ＣＰＵ１０は、ステップＳ２０４の中心点算出処理で算出した中心点ｃＯの座標を、前述した状態推定処理（図５参照）に引き渡す。
ＣＰＵ１０は、以上に示したステップＳ２０１〜Ｓ２０６を実行することにより、中心点導出部２００として機能する。
次にＣＰＵ１０は、状態推定プログラムを終了する終了条件を充足するか否かを判定する（ステップＳ２０７）。終了条件を充足する場合、ＣＰＵ１０は、当該フローチャートに示される中心点導出処理を終了する。一方、終了条件を充足しない場合、ＣＰＵ１０は、処理をステップＳ２０１に進める。
なお、本実施形態では、ＣＰＵ１０は、ステップＳ２０７において、終了条件を充足しない場合に、処理をステップＳ２０１に進めるが、本発明はこのような形態に限定されるものではない。例えば、ＣＰＵ１０は、ステップＳ２０７の終了条件を充足しない場合、処理を一定期間停止させ、一定期間経過後に処理をステップＳ２０１に進めてもよい。また、ＣＰＵ１０は、ステップＳ２０７の終了条件を充足しない場合、状態ベクトル更新処理（ステップＳ１０４）で生成される観測残差ｚｋ等に基づいて中心点ｃＯの座標の生成の要否を判定（例えば、観測残差ｚｋのノルムが閾値以上であるか否かを判定）し、中心点ｃＯの座標の生成が必要と判定された場合に、処理をステップＳ２０１へと進めてもよい。
【０１００】
以下において、磁気データ分布判定処理（ステップＳ２０３）、中心点算出処理（ステップＳ２０４）、及び歪判定処理（ステップＳ２０５）の詳細について説明する。なお、説明の便宜上、まずステップＳ２０４の中心点算出処理について説明した後に、ステップＳ２０３の磁気データ分布判定処理について説明する。
【０１０１】
［５． 中心点算出処理］
中心点算出処理は、内部磁界Ｂｉ及び地磁気Ｂｇの性質の違いを利用して、Ｎ個の磁気データｑ１〜ｑＮの示す座標から内部磁界Ｂｉの成分を分離することで、磁気センサのオフセットｑＯＦＦの候補となる座標である中心点ｃＯの座標を算出する処理である。
以下では、５．１節において、中心点算出処理の前提となる内部磁界Ｂｉ及び地磁気Ｂｇの性質の相違点について説明したうえで、５．２節において、中心点算出処理の詳細について述べる。
【０１０２】
［５．１． 内部磁界及び地磁気の性質］
図８は、３次元磁気センサ７０が、姿勢μをμ１〜μＮと変化させつつ内部磁界Ｂｉ及び地磁気Ｂｇを検出した場合に３次元磁気センサ７０から出力されるＮ個の磁気データｑ１〜ｑＮを、センサ座標系ΣＳにおいてをプロットした図である。
なお、図８に記載された各ベクトルの左上に付された添字Ｓは、当該ベクトルがセンサ座標系ΣＳにおいて表されたベクトルであることを意味する。
【０１０３】
内部磁界Ｂｉは、携帯機器１の部品が発する磁界である。内部磁界Ｂｉは、携帯機器１の内部状態が変化しなければ、携帯機器１に対して一定の方向を向き、一定の大きさを有する磁界である。つまり、内部磁界Ｂｉは、センサ座標系ΣＳにおいて、３次元磁気センサ７０の位置Ｐｓ及び姿勢μに依存しない、一定の方向及び大きさを持つベクトルＳＢｉとして表される。具体的には、図８に示すように、内部磁界を表すベクトルＳＢｉは、センサ座標系ΣＳの原点を起点とし、中心点ｃＯＧを終点とするベクトルである。
【０１０４】
一方、地磁気Ｂｇは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地上座標系ΣＧにおいて一様な向き及び大きさを有する磁界である。よって、携帯機器１の姿勢μを変化させる場合は、地磁気Ｂｇの大きさは変化しないが、携帯機器１から見た地磁気Ｂｇの向きは変化する。つまり、地磁気Ｂｇは、センサ座標系ΣＳにおいて、携帯機器１の姿勢μに伴い方向を変化させる一定の大きさのベクトルＳＢｇ（μ）として表現される。
【０１０５】
複数の磁気データｑ１〜ｑＮの各々は、センサ座標系ΣＳにおいて、内部磁界を表すベクトルＳＢｉと、地磁気を表すベクトルＳＢｇ（μ）との和を表すベクトルの示す座標として出力される。従って、複数の磁気データｑ１〜ｑＮの各々がセンサ座標系ΣＳにおいて示す座標は、中心点ｃＯＧを中心とし、地磁気Ｂｇの大きさを半径とする球面ＳＧ上に分布する。なお、３次元磁気センサ７０は、測定誤差を有するため、厳密には、複数の磁気データｑ１〜ｑＮの示す座標は球面ＳＧの近傍に確率的に分布する。
内部磁界を表すベクトルＳＢｉが示す球面ＳＧの中心点ｃＯＧの座標と、磁気センサのオフセットｑＯＦＦの示す座標とは、等しい。従って、磁気センサのオフセットｑＯＦＦは、複数の磁気データｑ１〜ｑＮに基づいて球面ＳＧの中心点ｃＯＧの座標を求めることで算出される。つまり、磁気データｑｉの示す座標から、磁気センサのオフセットｑＯＦＦの示す座標を減算することにより、地磁気Ｂｇの正確な方向及び大きさを算出することができる。
【０１０６】
［５．２． 中心点算出処理の詳細］
以下では、図９を参照しながら、ステップＳ２０４の中心点算出処理について説明する。中心点算出処理は、３次元磁気センサ７０が出力するＮ個の磁気データｑ１〜ｑＮの示す座標が、半径ｒＳの球面Ｓの近傍に分布すると仮定して、球面Ｓの中心点ｃＯの座標を算出する処理である。
なお、球面Ｓは、センサ座標系ΣＳ上で、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面の中心点を求めるために、計算の便宜上導入された球面であり、地磁気Ｂｇを表す球面ＳＧとは異なる球面である。
しかし、３次元磁気センサ７０が、内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合には、複数の磁気データｑ１〜ｑＮの示す座標は球面ＳＧの近傍に分布することになる。従って、この場合、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと球面ＳＧとは、ほぼ同一の球面となり、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとは、ほぼ同一の座標となる。よって、３次元磁気センサ７０が、内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓの中心点ｃＯの座標を算出することで、球面ＳＧの中心点ｃＯＧの座標、すなわち、磁気センサのオフセットｑＯＦＦを算出することができる。
以下、具体的に、球面Ｓの中心点ｃＯの座標の算出方法について述べる。なお、以下において記載されるベクトル及び座標は、特に断りが無い場合には、センサ座標系ΣＳにおいて表現されたものであるとする。
【０１０７】
複数の磁気データｑ１〜ｑＮの示す座標が、半径ｒＳの球面Ｓ上に存在すると仮定する場合、各磁気データｑｉの示す座標と、球面Ｓの中心点ｃＯとの距離はｒＳであるため、以下の式（３４）が成立する。なお、磁気データｑｉの示す座標の各要素は式（３５）で表され、中心点ｃＯの座標の各要素は式（３６）で表される。
【数２４】
【０１０８】
ここで、複数の磁気データｑ１〜ｑＮの示す座標の重心ｑＣを、式（３７）で定義する。なお、重心ｑＣの各要素は、式（３８）で示される。磁気データｑｉの示す座標及び中心点ｃＯの座標を、重心ｑＣを起点とするベクトルとして表現することで、式（３４）は、式（３９）に変形される。
【数２５】
【０１０９】
式（３９）の磁気データｑｉの示す座標に対して、複数の磁気データｑ１〜ｑＮの示す座標の各々を代入した結果を、磁気データの個数Ｎで割り算することにより、以下の式（４０）が得られる。そして、式（３９）と式（４０）との差分を取ることで、以下の式（４１）が得られる。
【数２６】
【０１１０】
このように、磁気データである変数ｑｉの示す座標が、球面Ｓ上に存在することを示す式（３４）は、式（４１）に変形された。この式（４１）の変数ｑｉに対して、Ｎ個の磁気データｑ１〜ｑＮの示す座標をそれぞれ代入することで得られるＮ個の方程式は、行列Ｘを用いて式（４２）として表現される。なお、行列Ｘは、式（４３）に示すＮ行３列の行列である。また、ベクトルｊは、式（４４）に示すＮ次元のベクトルであり、値Ｒは、式（４５）に示す値である。
【数２７】
【０１１１】
式（４２）は、複数の磁気データｑ１〜ｑＮの示す座標の全てが、中心点ｃＯを中心とする球面Ｓ上に完全に一致する場合には解を有する。しかし、３次元磁気センサ７０の測定誤差等を考慮すると、複数の磁気データｑ１〜ｑＮの示す座標の全てが、球面Ｓと完全に一致することは無いため、式（４２）は解を持たない。そこで、統計的な手法により尤もらしい解を得るために、式（４６）で表す誤差を吸収するベクトルである第１球面誤差ベクトルδＳを導入する。ここで、式（４７）により表される３次元のベクトルｃは、中心点ｃＯの座標を表すための変数ベクトルである。
【数２８】
【０１１２】
第１球面誤差ベクトルδＳのノルムを最小にするベクトルｃ、換言すれば、(δＳ)Ｔ(δＳ)を最小化するようなベクトルｃにより示される座標が、球面Ｓの中心点ｃＯの座標として尤もらしいものであるいえる。ここで、以下の式（４８）で示される目的関数（中心点算出関数）ｆＳ（ｃ）を定義すると、目的関数ｆＳ（ｃ）を最小化するベクトルｃにより示される座標が、球面Ｓの中心点ｃＯの座標として尤もらしい値となる。中心点ｃＯの座標は、式（５０）に示す共分散行列Ａが正則である場合には、式（４９）で表される。
【数２９】
【０１１３】
前述の通り、３次元磁気センサ７０が、内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合には、球面Ｓと、地磁気Ｂｇを表す球面ＳＧとは、ほぼ同一の球面となり、球面Ｓの中心点ｃＯと、球面ＳＧの中心点ｃＯＧとは、ほぼ同一の座標となる。従って、３次元磁気センサ７０が内部磁界Ｂｉ及び地磁気Ｂｇのみを検出する場合、式（４９）に示される中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１１４】
［６． 磁気データ分布判定処理］
以下では、ステップＳ２０３で行われる、磁気データ分布判定処理について説明する。
【０１１５】
前述した、中心点算出処理において、球面Ｓの中心点ｃＯを算出するためには、複数の磁気データｑ１〜ｑＮの示す座標が、センサ座標系ΣＳにおいて３次元的な広がりを有して分布していることが必要となる。しかし、携帯機器１（３次元磁気センサ７０）の姿勢μは、携帯機器１の利用者が手で携帯機器１を動かすことにより変化するため、携帯機器１の動かし方が不十分な場合には、携帯機器１の姿勢変化は３次元的とはならず２次元的なものとなることがある。この場合、センサ座標系ΣＳにおける複数の磁気データｑ１〜ｑＮの示す座標は、３次元的な広がりを有さず２次元的に分布する。
例えば、図１０に示すように、複数の磁気データｑ１〜ｑＮの示す座標が、センサ座標系ΣＳの平面π上の円πＣ近傍に２次元的に分布する場合、球面Ｓは、円πＣを切断面に有するような球面であるということしか特定できない。円πＣを切断面に有するような球面は、円πＣの中心点πＣＯを通り平面πに直交する直線πＬ上の中心点ｃπ１を中心とする球面Ｓπ１であるかもしれないし、直線πＬ上の中心点ｃπ２を中心とする球面Ｓπ２であるかもしれない。つまり、球面Ｓの中心点ｃＯは、直線πＬ上に位置することまでは特定可能であるが、具体的に直線πＬ上のどの位置に存在するかについては特定不可能である。このように、複数の磁気データｑ１〜ｑＮの示す座標が２次元的に分布する場合、複数の磁気データｑ１〜ｑＮに基づいて、正確な中心点ｃＯの座標を算出することはできない。
【０１１６】
複数の磁気データｑ１〜ｑＮに基づいて球面Ｓの中心点ｃＯの座標を算出するためには、図１１に示すように、センサ座標系ΣＳにおいて複数の磁気データｑ１〜ｑＮの示す座標が３次元的に広がりを持って分布していることが必要となる。磁気データ分布判定処理において、ＣＰＵ１０（中心点導出部２００）は、複数の磁気データｑ１〜ｑＮの示す座標が３次元的に分布しているか否かを、式（５０）に示される共分散行列Ａを用いて判定する。以下において、共分散行列Ａの性質を説明する。
【０１１７】
共分散行列Ａの固有値を、大きい順番に、最大固有値λ１、中間固有値λ２、最小固有値λ３とし、それぞれの固有値に対応する大きさ１に正規化された固有ベクトルをｕ１、ｕ２、ｕ３とする。また、磁気データｑｉの示す座標を、前述した重心ｑＣを原点とする重心座標系ΣＣにおいて表したベクトルをＣｑｉと表す。このとき、固有値λｊ（ｊ＝１、２、３）は、固有ベクトルｕｊ方向の分散σ２ｊに等しい。
図１１に示すように、各固有ベクトルｕ１〜ｕ３を重心座標系ΣＣの原点ｑＣを起点となるように配置する。このとき、例えばｊ＝１の場合について検討する。固有値λ１は、ベクトルＣｑｉを、固有ベクトルｕ１へ射影した長さＬｉ１の二乗（Ｌｉ１）２を、Ｎ個の磁気データＣｑｉ（ｉ＝１、２、…Ｎ）について平均した値に等しい。すなわち、固有値λｊは、Ｎ個の磁気データｑｉの示す座標が、重心ｑＣから固有ベクトルｕｊ方向にどの程度離れているか、つまり、複数の磁気データｑ１〜ｑＮの示す座標の分布が固有ベクトルｕｊの方向にどの程度の広がりを有しているかを表す値である。
【０１１８】
最小固有値λ３に対応する固有ベクトルｕ３の方向が、複数の磁気データｑ１〜ｑＮの示す座標の分布の広がりが最も小さい方向であり、最小固有値λ３が、複数の磁気データｑ１〜ｑＮの示す座標の分布の広がりが最も小さい方向における広がりの程度を示す指標である。従って、複数の磁気データｑ１〜ｑＮの示す座標が３次元的に分布しているといえるためには、最小固有値λ３の値が、一定の閾値（分散許容値）λ０以上であればよい。
【０１１９】
磁気データ分布判定処理において、中心点導出部２００は、共分散行列Ａの最小固有値λ３が閾値λ０以上であれば、複数の磁気データｑ１〜ｑＮの示す座標の分布が十分に３次元的であると判断して、処理を前述したステップＳ２０４の中心点算出処理へと進める。一方、中心点導出部２００は、最小固有値λ３が閾値λ０未満である場合には、複数の磁気データｑ１〜ｑＮの示す座標が３次元的な広がりを有さず２次元的に分布すると判断し、処理をステップＳ２０２の磁気データ蓄積処理に戻す。
【０１２０】
［７． 歪判定処理］
ところで、３次元磁気センサ７０は、図３に示したように、地磁気Ｂｇ及び内部磁界Ｂｉの他に、携帯機器１の外部に存在する物体２が出す外部磁界Ｂｘを検出する場合がある。外部磁界Ｂｘは、物体２と携帯機器１との相対的位置関係により、方向及び大きさが変化する不均一な磁界である。
５節で説明した中心点算出処理は、３次元磁気センサ７０が、地磁気Ｂｇ及び内部磁界Ｂｉのみを検出することを前提として、磁気センサのオフセットｑＯＦＦの示す座標の候補である球面Ｓの中心点ｃＯの座標を算出するものである。従って、３次元磁気センサ７０が、地磁気Ｂｇ及び内部磁界Ｂｉの他に、不均一な外部磁界Ｂｘを検出する場合、中心点算出処理により算出される中心点ｃＯの座標は、外部磁界Ｂｘの影響による誤差を有し、内部磁界Ｂｉ（すなわち、磁気センサのオフセットｑＯＦＦ）を表す座標とは異なる座標となる可能性が高い。このような中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用した場合、正しい補正処理を行うことができず、携帯機器１は正確な地磁気Ｂｇの方向及び大きさを算出することができない。
ステップＳ２０５の歪判定処理において、ＣＰＵ１０（中心点導出部２００）は、不均一な外部磁界Ｂｘの有無、及び外部磁界Ｂｘの影響の大きさを評価することにより、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することの可否を判断する。
以下では、７．１節において、歪判定処理における判定の対象となる外部磁界Ｂｘの性質について説明したうえで、７．２節において、歪判定処理の詳細を説明する。
【０１２１】
［７．１． 外部磁界の性質］
図１２は、３次元磁気センサ７０の位置ＰｓをＰＳ１〜ＰＳＮと変化させると共に、姿勢μをμ１〜μＮと変化させて磁界を測定したときの、３次元磁気センサ７０が出力する複数の磁気データｑ１〜ｑＮの示す座標を、センサ座標系ΣＳにおいてプロットした図である。なお、図１２では、内部磁界Ｂｉ、地磁気Ｂｇ、及び外部磁界Ｂｘが存在する場合を想定している。
【０１２２】
外部磁界Ｂｘは不均一な磁界であり、外部磁界Ｂｘを発生させる物体２と、携帯機器１との相対的位置関係により、方向及び大きさが変化する。従って、３次元磁気センサ７０の位置Ｐｓが変化した場合、３次元磁気センサ７０が検出する外部磁界Ｂｘの向き及び大きさも変化する。同様に、３次元磁気センサ７０の姿勢μが変化した場合、３次元磁気センサ７０が検出する外部磁界Ｂｘの向きも変化する。すなわち、外部磁界Ｂｘは、センサ座標系ΣＳにおいて、３次元磁気センサ７０の姿勢μ及び位置Ｐｓに依存して方向及び大きさの双方を変化させるベクトルＳＢｘ（μ、Ｐｓ）として表現される。
複数の磁気データｑ１〜ｑＮの示す座標の各々は、内部磁界を表すベクトルＳＢｉ、地磁気を表すベクトルＳＢｇ（μ）、及び外部磁界を表すベクトルＳＢｘ（μ、Ｐｓ）の和を表すベクトルにより示される。従って、複数の磁気データｑ１〜ｑＮの示す座標は、中心点ｃＯＧを起点とする地磁気を表すベクトルＳＢｇ（μ）の終点を表す球面ＳＧと、中心点ｃＯＧを起点とする外部磁界を表すベクトルＳＢｘ（μ、Ｐｓ）の終点を表す曲面ＳＸとを、重ね合わせた立体ＳＤの表面近傍に分布する。
【０１２３】
外部磁界Ｂｘを表す曲面ＳＸが、球面とは異なる歪んだ形状を有する場合には、立体ＳＤも球面とは異なる歪んだ形状を有する。複数の磁気データｑ１〜ｑＮの示す座標が、球面とは異なる歪んだ形状の立体ＳＤの表面近傍に分布する場合、複数の磁気データｑ１〜ｑＮの示す座標をできるだけ近傍に有するように球面Ｓを定めても、複数の磁気データｑ１〜ｑＮの示す座標と球面Ｓとの間の距離は大きく離れる。すなわち、この場合は、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面Ｓの近傍には分布しない。同様に、磁気データｑ１〜ｑＮの示す座標は、地磁気Ｂｇを表す球面ＳＧの近傍にも分布しない。
従って、複数の磁気データｑ１〜ｑＮの示す座標が、球面とは異なる歪んだ形状の立体ＳＤの表面近傍に分布する場合、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとは異なる座標となる可能性が高く、中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することはできない（図１５参照）。
【０１２４】
但し、不均一な外部磁界Ｂｘの影響が小さく、立体ＳＤの形状がほぼ球面と看做せる場合、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと、立体ＳＤとをほぼ同一の図形と看做すことが可能となり、また、球面Ｓの中心点ｃＯと、地磁気を表す球面ＳＧの中心点ｃＯＧとはほぼ同一の座標となる。従って、立体ＳＤの形状がほぼ球面と看做せる場合は、球面Ｓの中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。以下、図１３を参照しながら、外部磁界Ｂｘの影響が小さい場合について具体的に説明する。
【０１２５】
図１３（Ａ）は、外部磁界Ｂｘが微弱である場合を示す。
立体ＳＤは、地磁気Ｂｇを表す球面ＳＧと、外部磁界Ｂｘを表す曲面ＳＸとの重ね合わせである。従って、外部磁界Ｂｘが微弱であり、外部磁界Ｂｘを表す曲面ＳＸが小さい場合には、立体ＳＤと球面ＳＧとは、ほぼ同一の図形となる。このとき、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面ＳＧの近傍にも分布するものと看做すことができる。
また、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する立体ＳＤの形状は、ほぼ球面であるため、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと、立体ＳＤとは、ほぼ同一の図形となる。すなわち、立体ＳＤ、球面ＳＧ、及び球面Ｓは、ほぼ同一の図形になり、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとは、ほぼ等しい座標になる。
従って、図１３（Ａ）に示すような、外部磁界Ｂｘが微弱である場合には、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１２６】
なお、図１３（Ｂ）に示すように、不均一な外部磁界Ｂｘが大きい場合であっても、立体ＳＤの形状がほぼ球面と看做せる場合がある。
例えば、不均一な外部磁界Ｂｘが存在する場合であっても、Ｎ個の磁気データｑ１〜ｑＮを取得する際に、携帯機器１の利用者が携帯機器１を手で握って携帯機器１の位置ＰＳが変化するように振るのではなく、携帯機器１の位置ＰＳ（厳密には、３次元磁気センサ７０の位置）を固定したままその姿勢μのみを変化させる場合には、外部磁界Ｂｘは、センサ座標系ΣＳにおいて、姿勢μに基づいてその方向のみを変化させる一定の大きさのベクトルＳＢｘ（μ）として表現される。この場合、外部磁界Ｂｘを表す曲面ＳＸの形状は、中心点ｃＯＧを中心とする球面となるため、球面ＳＧと曲面ＳＸとの重ね合わせである立体ＳＤの形状もほぼ球面となる。また、立体ＳＤは、中心点ｃＯＧを中心とする球面ＳＧと、中心点ｃＯＧを中心とする球面の形状を有する曲面ＳＸとを重ね合わせた図形であるため、立体ＳＤが表す球面の中心点は、球面ＳＧの中心点ｃＯＧと、ほぼ一致する。前述の通り、立体ＳＤの形状がほぼ球面である場合には、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有するように定められる球面Ｓと、立体ＳＤとは、ほぼ同一の図形となる。よって、球面Ｓの中心点ｃＯと、球面ＳＧの中心点ｃＯＧとは、ほぼ等しい座標になる。
従って、図１３（Ｂ）に示すような、外部磁界Ｂｘが一定の大きさのベクトルとして検出される場合には、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１２７】
このように、不均一な外部磁界Ｂｘの影響が大きい場合、すなわち、立体ＳＤが球面とは異なる歪んだ形状を有する場合には、中心点算出処理により算出された中心点ｃＯの座標を示すベクトルを、磁気センサのオフセットｑＯＦＦとして採用することはできない。
一方、不均一な外部磁界Ｂｘが微弱な場合、または、３次元磁気センサ７０が不均一な外部磁界Ｂｘを一定の大きさのベクトルとして検出する場合等、立体ＳＤの形状がほぼ球面と看做すことができる場合には、球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができる。
【０１２８】
［７．２． 歪判定処理の詳細］
以下では、図１３乃至図１５を参照しながら、ステップＳ２０５の歪判定処理の詳細について説明する。
【０１２９】
前述した中心点算出処理では、複数の磁気データｑ１〜ｑＮの示す座標が、式（３４）で示される球面Ｓの近傍に分布すると仮定した。これに対して本節で説明する歪判定処理は、複数の磁気データｑ１〜ｑＮの示す座標が、球面Ｓとは異なる歪んだ形状の立体ＳＤの表面近傍に分布すると仮定する。
【０１３０】
立体ＳＤは、式（５１）で表される。式（５１）に示される立体ＳＤは、図１４に示すように、球面を表す成分「Ｘ（ｃ−ｑＣ）−ｊ」と、式（５２）に示すＮ次元ベクトルの歪誤差ベクトルｋ（Ｅ）とを、加算したものである。なお、式（５１）の右辺に現れる０Ｎの右下の添え字「Ｎ」は、零ベクトルの次元を表している。また、式（５３）に示す歪評価行列Ｅは、３行３列の対称行列である。
なお、式（５１）に現れる成分「Ｘ（ｃ−ｑＣ）−ｊ」が表す球面と、中心点算出手段により算出される球面Ｓとは、必ずしもセンサ座標系ΣＳ上で同一の図形を表すものではない。そこで、以下では、式（５１）に現れる成分「Ｘ（ｃ−ｑＣ）−ｊ」として表される球面を球面（第２球面）Ｓ２と称し、球面Ｓ２の中心点を中心点（第２球面の中心点）ｃＯ２と称する。また、式（５１）において用いられるベクトルｃは、式（４７）に示した３次元のベクトルであるが、本節では、中心点ｃＯ２の示す座標を算出するための変数ベクトルとして用いる。
歪判定処理は、式（５１）のうち、歪を表す成分ｋ（Ｅ）の大きさを評価することにより、立体ＳＤの形状と、球面Ｓ２の形状とが、どの程度相違しているかについて評価するものである。
【数３０】
【０１３１】
歪誤差ベクトルｋ（Ｅ）を構成する要素のうち、第ｉ行目の要素ｋｅ(ｑｉ−ｃＯ)は、以下の式（５４）で表される関数ｋｅ（ｐ）に対して、中心点ｃＯを起点として磁気データｑｉを表すベクトル（ｑｉ−ｃＯ）を代入することにより与えられる。関数ｋｅ（ｐ）は、式（５３）に示す歪評価行列Ｅを係数行列とし、式（５５）に示すベクトルｐの３つの要素を変数とする、二次形式で表現される関数である。つまり、関数ｋｅ（ｐ）は、ベクトルｐと、ベクトルｐを歪評価行列Ｅにより変換したベクトルＥｐとの内積を示す。
従って、歪誤差ベクトルｋ（Ｅ）のうち、第ｉ行目の要素ｋｅ（ｑｉ−ｃＯ）は、球面Ｓの中心点ｃＯを起点として磁気データｑｉの座標を表したベクトル（ｑｉ−ｃＯ）と、ベクトル（ｑｉ−ｃＯ）を歪評価行列Ｅにより変換して得られるベクトルＥ（ｑｉ−ｃＯ）との内積である。
【数３１】
【０１３２】
なお、３次元磁気センサ７０の測定誤差等を考慮すると、複数の磁気データｑ１〜ｑＮの各々が示す座標の全てが、立体ＳＤと完全に一致する位置に存在することは無いため、式（５１）は解を持たない。そこで、統計的な手法により尤もらしい解を得るために、式（５６）で表される誤差を吸収するベクトルである立体誤差ベクトルδＳＤを導入する。立体誤差ベクトルδＳＤは、第２球面誤差ベクトルδＳ２と、歪誤差ベクトルｋ（Ｅ）とを、加算したものである。なお、詳細は後述するが、第２球面誤差ベクトルδＳ２は、式（５１）のうち、球面を表す成分「Ｘ（ｃ−ｑＣ）−ｊ」に対応する成分である。
立体誤差ベクトルδＳＤは、複数の磁気データｑ１〜ｑＮの座標と、立体ＳＤの表面との誤差を表す、Ｎ次元のベクトルである。立体誤差ベクトルδＳＤのノルムを最小にするようなベクトルｃ及び歪評価行列Ｅ、換言すれば、（５７）で示される目的関数（歪評価関数）ｆＳＤ（Ｅ、ｃ）を最小化するようなベクトルｃ及び歪評価行列Ｅによって、複数の磁気データｑ１〜ｑＮの示す座標を表面近傍に有する立体ＳＤが表現される。
歪判定処理は、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを、後述する式（５８）及び（５９）で示される歪評価値ｇＤ（Ｅ）に基づいて評価する処理である。立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを評価することで、立体ＳＤと球面Ｓとの形状の相違の程度について把握することが可能となる。
【数３２】
【０１３３】
以下で、式（５６）に示した立体誤差ベクトルδＳＤの性質を、式（４６）に示した第１球面誤差ベクトルδＳの性質と対比しつつ説明する。
まず、第１球面誤差ベクトルδＳは、複数の磁気データｑ１〜ｑＮが示す座標と、球面Ｓとの誤差を吸収するためのベクトルである。第１球面誤差ベクトルδＳを構成する１行目〜Ｎ行目の各要素は、それぞれ独立した変数である。従って、第１球面誤差ベクトルδＳによって、複数の磁気データｑ１〜ｑＮが示す座標と球面Ｓとの誤差を吸収する場合には、複数の磁気データｑ１〜ｑＮが示す座標と球面ＳとのＮ個の誤差の各々は、互いに制約の無い、Ｎ個が互いに独立して定められる値となる。つまり、第１球面誤差ベクトルδＳによって表されるＮ個の誤差は、それぞれが独立に確率的に定められるものであり、Ｎ個の誤差は全体として、対称性を有し、かつ方向依存性の無いホワイトノイズである。
すなわち、中心点算出処理は、複数の磁気データｑ１〜ｑＮが示す座標と、球面Ｓとの誤差をホワイトノイズである第１球面誤差ベクトルδＳにより表現し、第１球面誤差ベクトルδＳを最小化するような球面Ｓの中心点ｃＯの座標を求める処理である。
【０１３４】
一方、立体誤差ベクトルδＳＤは、第２球面誤差ベクトルδＳ２と歪誤差ベクトルｋ（Ｅ）との和によって表されるベクトルであり、磁気データｑ１〜ｑＮが示す座標と立体ＳＤとの誤差を吸収するベクトルである。
第２球面誤差ベクトルδＳ２は、第１球面誤差ベクトルδＳと同様に式（４６）の右辺で表され、磁気データｑ１〜ｑＮの座標と、球面Ｓ２との誤差を、ホワイトノイズとして表現するベクトルである。
一方、歪誤差ベクトルｋ（Ｅ）は、式（５４）で示した３変数二次形式の関数ｋｅ（ｐ）を各要素とするベクトルである。３変数の二次形式は、変数が２次の項から構成される関数であり、３次元空間上の様々な曲面、例えば、直線、平面、柱面、球面、楕円面、錐面、１葉双曲面、２葉双曲面、及び各種放物面等を描くことができる。従って、歪誤差ベクトルｋ（Ｅ）は、各磁気データｑ１〜ｑＮが示す座標と球面Ｓ２とのＮ個の誤差の各々を、互いに独立したものではなく、Ｎ個の誤差の全てが同一の関数ｋｅ（ｐ）により表される３次元空間上の曲面上に存在するという制約を持った値として表現する。
このように、立体誤差ベクトルδＳＤは、各磁気データｑ１〜ｑＮが示す座標と球面Ｓ２とのＮ個の誤差を、ホワイトノイズである第２球面誤差ベクトルδＳ２と、滑らかな曲面による球面Ｓ２からの歪という性質を有する歪誤差ベクトルｋ（Ｅ）とに分離して表現する。
【０１３５】
以下では、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさと、中心点算出処理で算出された球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することの可否についての判断との関係について、図１３及び図１５を参照しつつ説明する。
【０１３６】
図１３（Ａ）及び（Ｂ）は、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響が小さい場合を示している。歪誤差ベクトルｋ（Ｅ）の影響が小さい場合とは、歪誤差ベクトルｋ（Ｅ）の大きさが小さいため、立体ＳＤを表す式（５１）が成立すれば、同時に、球面Ｓを表す式（４２）についても成立するものと看做せる場合を意味する。すなわち、歪誤差ベクトルｋ（Ｅ）の影響が小さい場合には、式（５７）に定める目的関数ｆＳＤ（Ｅ、ｃ）を最小化して求められる立体ＳＤと、式（４８）に定める目的関数ｆＳ（ｃ）を最小化して求められる球面Ｓとが、ほぼ等しくなる。前述の通り、立体ＳＤの形状が球面と看做せる場合には、複数の磁気データｑ１〜ｑＮが示す座標は、立体ＳＤの表面近傍に分布すると同時に、球面Ｓの近傍にも分布すると看做すことができる。そして、立体ＳＤが表す球面の中心点は、球面ＳＧの中心点ｃＯＧと、ほぼ一致するため、球面Ｓの中心点ｃＯと球面ＳＧの中心点ｃＯＧとがほぼ等しい座標となる。従って、歪誤差ベクトルｋ（Ｅ）の影響が小さい場合には、球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができる。なお、歪評価行列Ｅが零行列となる場合には、歪誤差ベクトルｋ（Ｅ）も零ベクトルとなるため、式（４８）と式（５７）とが等しい式となり、立体ＳＤと球面Ｓとは完全に同一の図形となる。
【０１３７】
図１５は、歪誤差ベクトルｋ（Ｅ）の影響が大きい場合を示している。
歪誤差ベクトルｋ（Ｅ）の影響が大きい場合とは、複数の磁気データｑ１〜ｑＮが示す座標をできるだけ近傍に有するように球面Ｓ２を定めても、複数の磁気データｑ１〜ｑＮが示す座標と球面Ｓ２との誤差をホワイトノイズである第２球面誤差ベクトルδＳ２によって吸収することができず、球面Ｓ２からの滑らかな歪を表す歪誤差ベクトルｋ（Ｅ）により吸収する必要がある場合を示している。すなわち、この場合、目的関数ｆＳＤ（Ｅ、ｃ）を最小化して求められる立体ＳＤは、目的関数ｆＳ（ｃ）を最小化して求める球面Ｓとは大きく異なる歪んだ形状になる。前述の通り、立体ＳＤの形状が球面とは異なる歪んだ形状を有する場合には、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮが示す座標は、球面Ｓの近傍に分布すると看做すことはできない。中心点算出処理は、球面Ｓの近傍に磁気データｑ１〜ｑＮの示す座標が存在していることを前提として、磁気センサのオフセットｑＯＦＦの候補である中心点ｃＯの座標を算出する処理であるため、磁気データｑ１〜ｑＮの示す座標を近傍に有さない球面Ｓの中心点ｃＯは、磁気センサのオフセットｑＯＦＦとしての意味を有さない。従って、この場合、中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することはできない。
【０１３８】
このように、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響が小さい場合、立体ＳＤ上に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面Ｓ上にも分布すると看做すことができ、中心点算出手段で算出した球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができるが、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響が大きければ、中心点ｃＯの座標は誤差を有する不正確な値となり、中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することを防止する必要がある。
以下では、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを評価する方法について述べる。
【０１３９】
ここで、以下の式（５８）及び（５９）に示す歪評価値ｇＤ（Ｅ）を、立体ＳＤの形状における歪誤差ベクトルｋ（Ｅ）の影響の大きさを評価する評価値として定義する。歪評価値ｇＤ（Ｅ）は、歪評価行列Ｅの有する３つの固有値のうち、絶対値が最大となる最大固有値λＥ１の絶対値（歪評価行列Ｅのノルム）である。
歪評価値ｇＤ（Ｅ）の値が一定の閾値（歪許容値）δ０以下の小さな値であれば、立体ＳＤと球面Ｓ２とは等しい図形であると看做すことができるため、立体ＳＤの表面近傍に分布する複数の磁気データｑ１〜ｑＮの示す座標は、球面Ｓの近傍に分布すると看做すこともでき、中心点算出処理により求められた球面Ｓの中心点ｃＯの座標を示すベクトルを磁気センサのオフセットｑＯＦＦとして採用することができる。
【数３３】
【０１４０】
以下に、歪評価値ｇＤ（Ｅ）を求める方法について説明する。
まず、式（５４）に示した関数ｋｅ（ｐ）は、二次形式で表される関数であるため、以下の式（６０）のように、ベクトルの内積として表現することもできる。また、歪誤差ベクトルｋ（Ｅ）の第ｉ行目の要素ｋｅ(ｑｉ−ｃＯ)は、関数ｋｅ（ｐ）に対して、ベクトル(ｑｉ−ｃＯ)を代入した値であるため、式（６２）によって示される６次元のベクトルｋｅ２（ｉ）、及び、式（６３）によって表される歪評価行列Ｅの各成分を並べた６次元のベクトルｅＥを用いて、式（６１）のように表せる。
【数３４】
【０１４１】
ここで、式（６４）で示す行列Ｘ２を導入する。行列Ｘ２は、各行に、式（６２）によって示される６次元のベクトルｋｅ２（ｉ）を転置したものと、３次元のベクトル(ｑｉ−ｑＣ)を転置したものを並べた、Ｎ行９列の行列である。
【数３５】
【０１４２】
式（６４）で表される行列Ｘ２を用いることで、式（５７）に示される目的関数ｆＳＤ（Ｅ、ｃ）を、式（６５）で表される目的関数ｇＳＤ（ｅ）に変形することができる。なお、ベクトルｅは、式（６６）に示すように、式（６３）に示した６次元のベクトルｅＥと、式（６７）に示す３次元のベクトルｅＸとを並べた、９次元のベクトルである。また、式（６７）に示すベクトルｅＸは、式（４７）に示す変数であるベクトルｃから、式（３７）に示した重心ｑＣを引いたベクトルである。
【数３６】
【０１４３】
式（６５）で表される目的関数ｇＳＤ（ｅ）を最小化する解ｅ＝ｅ０は、以下の式（６８）式で表現される連立方程式に対して、ガウス消去法や、コレスキー分解法などを適用することで、求められる。なお、式（６８）は、式（６５）に対して、最小二乗法を適用することによって算出される連立方程式である。
【数３７】
【０１４４】
このようにして得た解ｅ０に基づき、式（５３）の歪評価行列Ｅを復元する。そして、式（５８）に示した歪評価値ｇＤ（Ｅ）の値、即ち、歪評価行列Ｅのノルムを求め、歪評価値ｇＤ（Ｅ）の値が閾値δ０以下であるか否かを判定する。なお、歪評価行列Ｅのノルムは、歪評価行列Ｅの有する３つの固有値のうち絶対値が最大となる固有値λＥ１の絶対値に等しいため、ヤコビ法や、冪乗法により求めることができる。
そして、歪評価値ｇＤ（Ｅ）の値が閾値δ０以下である場合、中心点導出部２００は、処理をステップＳ２０６の中心点出力処理に進める。一方、歪評価値ｇＤ（Ｅ）の値が閾値δ０よりも大きな値である場合、中心点導出部２００は、処理をステップＳ２０１の初期化処理に戻す。
【０１４５】
［８． 中心点出力処理］
ステップＳ２０６の中心点出力処理は、歪判定処理において歪評価値ｇＤ（Ｅ）の値が閾値δ０以下であると判定された場合に実行される処理である。中心点出力処理において、ＣＰＵ１０（中心点導出部２００）は、中心点算出処理において算出された球面Ｓの中心点ｃＯの座標を出力し、状態推定部１００に対して供給する。
【０１４６】
本実施形態では、中心点導出部２００は、球面Ｓの中心点ｃＯの座標を出力しているが、球面Ｓ２の中心点ｃＯ２の座標を出力してもよい。歪評価値ｇＤ（Ｅ）の値が閾値δ０以下である場合には、中心点算出処理により求められた球面Ｓの中心点ｃＯと、球面Ｓ２の中心点ｃＯ２とは、ほぼ等しい座標となり、中心点ｃＯの座標を示すベクトルまたは中心点ｃＯ２の座標を示すベクトルのいずれも、磁気センサのオフセットｑＯＦＦとして採用することができるからである。
なお、球面Ｓ２の中心点ｃＯ２の座標は、目的関数ｇＳＤ（ｅ）を最小化する解ｅ０のうち式（６６）のｅＸに相当する部分を式（６７）に代入することにより求められる。
【０１４７】
［９． 結論］
以上に示したように、本実施形態に係る状態推定装置は、中心点導出部２００と、カルマンフィルタ演算部１２０を備える状態推定部１００とを備え、カルマンフィルタ演算部１２０がカルマンフィルタの演算に用いる状態ベクトルｘｋの要素のうち磁気センサのオフセット推定値ｑＯを、中心点導出部２００が出力する中心点ｃＯの座標によって上書きをしてカルマンフィルタの演算を行う。前述の通り、カルマンフィルタの状態遷移モデルにおいて磁気センサのオフセット推定値ｑＯの経時的な変化を定式化することはできず、また、カルマンフィルタによる状態ベクトルｘｋは更新前の状態ベクトルｘｋの影響を受けた値となるため、磁気センサのオフセット推定値ｑＯは、真値（磁気センサのオフセットｑＯＦＦ）から大きく乖離した値となる場合がある。この場合、状態推定装置は、システムの状態を正確に推定できないため、状態推定装置が算出する状態変数に基づいて、地磁気Ｂｇの向きや携帯機器１の姿勢μ等を算出することはできない。
これに対して、本実施形態におけるカルマンフィルタ演算部１２０は、中心点導出部２００が出力した中心点ｃＯの座標により、状態ベクトルｘｋの要素である磁気センサのオフセット推定値ｑＯを上書きする（ステップＳ１０３において行われる、状態変数上書処理）。前述の通り、中心点ｃＯの座標は、比較的短い期間に３次元磁気センサ７０から出力された磁気データｑ１〜ｑＮに基づいて算出される。すなわち、中心点ｃＯの座標を示すベクトルは、磁気センサのオフセットｑＯＦＦが急激且つ大きな変化をした場合であっても、変化後の磁気センサのオフセットｑＯＦＦを正確に表すことができる。
このような中心点ｃＯの座標によって、磁気センサのオフセット推定値ｑＯを上書きすることにより、状態ベクトルｘｋが真値から大きく異なる値となることを防止した。そして、携帯機器１が、状態ベクトルｘｋに基づいて、正確な地磁気Ｂｇの方向及び大きさ等を算出することを可能とした。
【０１４８】
また、本実施形態に係る、中心点導出部２００は、中心点算出処理を行う前に、磁気データ分布判定処理を行うことで、中心点算出処理で用いる複数の磁気データｑ１〜ｑＮが、中心点算出処理を行う上で適切なデータであるか否かの判定を行う。これにより、複数の磁気データｑ１〜ｑＮの示す座標の分布の３次元的な広がりの程度を把握し、複数の磁気データｑ１〜ｑＮの示す座標が平面的に分布している場合には、中心点算出処理を行わない。この結果、平面的に分布する複数の磁気データｑ１〜ｑＮに基づいた、不正確な（真値とは異なる）中心点ｃＯの座標の算出を未然に防止することが可能となり、不要な演算処理の防止による低消費電力化が可能となる。
また、本実施形態に係る、中心点導出部２００は、中心点算出処理を行った後に、歪判定処理を行い、中心点算出処理で算出された球面Ｓの中心点ｃＯの座標が、外部磁界Ｂｘによる影響を受けた不適切な（真値とは異なる）値であるか否かを判定する。これにより、外部磁界Ｂｘによる影響を受けた不適切な（磁気センサのオフセットｑＯＦＦと看做すことのできない）中心点ｃＯの座標の出力を防止することが可能となる。
このように、本実施形態に係る中心点導出部２００は、磁気データ分布判定処理及び歪判定処理により、磁気センサのオフセットｑＯＦＦと看做すことのできる真値に近い中心点ｃＯの座標のみを出力する。そして、カルマンフィルタ演算部１２０は、真値に近い中心点ｃＯの座標により磁気センサのオフセット推定値ｑＯを上書きすることで、状態ベクトルｘｋをより真値に近い正確な値へと更新することが可能となる。従って、本実施形態に係る携帯機器１は、状態推定装置が算出する状態ベクトルｘｋに基づいて、正確な地磁気Ｂｇの値を安定的に算出することが可能となった。
【０１４９】
＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【０１５０】
（１）変形例１
上述した実施形態では、カルマンフィルタ演算部１２０は、状態ベクトルｘのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点導出部２００から出力された中心点ｃＯの座標によって上書きをしてカルマンフィルタの演算を行うものであったが、本発明はこのような形態に限定されるものでは無く、状態ベクトルｘを中心点ｃＯの座標によって上書きせずにカルマンフィルタの演算を実行してもよい。
【０１５１】
図１６は、変形例１に係る状態推定装置のうち、ＣＰＵ１０が変形例１に係る状態推定プログラムを実行することにより実現される機能を表す機能ブロック図である。
図１６に示すように、変形例１に係る状態推定装置は、出力統合部３００を備える点と、状態推定部１００の代わりに状態推定部１００ａを備える点とを除いて、実施形態に係る状態推定装置と同様に構成される。状態推定部１００ａは、カルマンフィルタ演算部１２０の代わりにカルマンフィルタ演算部１２０ａを備える点を除き、状態推定部１００と同様に構成される。カルマンフィルタ演算部１２０ａは、状態ベクトルｘを中心点ｃＯの座標によって上書きしない点を除き、カルマンフィルタ演算部１２０と同様に動作する。
出力統合部３００は、カルマンフィルタ演算部１２０ａが出力する更新後の状態ベクトルｘ＋ｋと、中心点導出部２００が出力する中心点ｃＯの座標とに基づいて、状態ベクトルｘｃｋを出力する。
状態ベクトルｘｃｋは、状態ベクトルｘｋと同様に、姿勢μ、地磁気の強さｒ、地磁気の伏角φ、角速度ω、角速度センサのオフセット推定値ｇＯ、及び磁気センサのオフセット推定値ｑＯを要素とするベクトルである。状態ベクトルｘｃｋは、初期状態（時刻ｋ＝０）から、中心点導出部２００が中心点ｃＯの座標を出力するまでの期間は、状態ベクトルｘｋと等しい値に設定される。一方、中心点導出部２００が中心点ｃＯの座標を出力した後は、状態ベクトルｘｃｋを構成する要素のうち、磁気センサのオフセット推定値ｑＯは、中心点ｃＯの座標と等しい値に設定される。
【０１５２】
カルマンフィルタ演算部１２０ａは、３次元地磁気センサ７０、３次元加速度センサ８０、及び３次元角速度センサ９０の３つのセンサの出力を統合して、各種状態変数を推定するものであるため、高速に磁気センサのオフセット推定値ｑＯを算出することができる。一方、中心点導出部２００は、Ｎ個の磁気データｑ１〜ｑＮに基づいて中心点ｃＯの座標を算出するため、中心点ｃＯの座標が算出されるまでに一定の時間を要するが、歪判定処理により中心点ｃＯの座標の良否判定を行っているため、中心点ｃＯの座標を示すベクトルは磁気センサのオフセットｑＯＦＦと看做すことができる程度に正確な値である。
従って、変形例１に係る携帯機器１は、中心点導出部２００が中心点ｃＯの座標を算出するまでの間は、カルマンフィルタ演算部１２０ａが算出した磁気センサのオフセット推定値ｑＯを用いて地磁気Ｂｇのおおよその方向及び大きさを得ることができる。これにより、携帯機器１は、携帯機器１の利用者を長時間待たせること無く、地磁気Ｂｇの方向及び大きさを表示することが可能となり、利便性が向上する。
【０１５３】
（２）変形例２
上述した実施形態では、中心点導出部２００は、初期化処理、磁気データ蓄積処理、磁気データ分布判定処理、中心点算出処理、歪判定処理、及び中心点出力処理を行うが、本発明はこれに限定されるものでは無く、これらのうちの一部について行うものであってもよい。例えば、中心点導出部２００は、歪判定処理を行わず、中心点算出処理において算出された中心点ｃＯの座標を、中心点出力処理において出力してもよい。また、中心点導出部２００は、磁気データ分布判定処理を行わずに、中心点算出処理を実行してもよい。
この場合、中心点導出部２００は、磁気センサのオフセットｑＯＦＦが急激且つ大きな変化をした場合であっても、変化後の磁気センサのオフセットｑＯＦＦの値を容易に算出することができる。
【０１５４】
（３）変形例３
上述した実施形態では、カルマンフィルタ演算部１２０は、遅延部１２１において、状態ベクトルｘのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点導出部２００から出力された中心点ｃＯの座標によって上書きするものであるが、本発明はこのような形態に限定されるものでは無く、遅延部１２１以外において、状態ベクトルｘの中心点ｃＯの座標による上書きを行ってもよい。
例えば、シグマポイント生成部１２２において、２ａ+１個のシグマポイントχｋを生成する際に、式（１４）及び（１５）の右辺に現れる更新後の状態ベクトルｘ＋ｋのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点ｃＯの座標によって上書きしてもよい。また、状態遷移モデル部１２３の演算で用いる状態遷移モデルにおいて、式（２７）の右辺のうち磁気センサのオフセット推定値ｑＯ,Ｋ−１を表す要素を、中心点ｃＯの座標によって上書きしてもよい。さらには、加算器１２９において算出される、更新後の状態ベクトルｘ＋ｋのうち磁気センサのオフセット推定値ｑＯを表す要素を、中心点ｃＯの座標によって上書きしてもよい。
【０１５５】
（４）変形例４
上述した実施形態では、カルマンフィルタ演算部１２０は、非線形カルマンフィルタであるシグマポイントカルマンフィルタによる演算を実行しているが、本発明はこれに限定されるのではなく、カルマンフィルタ演算部１２０は、拡張カルマンフィルタなどの、シグマポイントカルマンフィルタ以外の非線形カルマンフィルタによる演算を実行するものであってもよい。
【０１５６】
（５）変形例５
上述した実施形態では、状態ベクトルｘを構成する要素して、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサのオフセット推定値ｇＯ、及び磁気センサのオフセット推定値ｑＯを採用し、これら６個の状態変数をカルマンフィルタの演算における推定の対象としたが、本発明はこれに限定されるものでは無い。例えば、状態ベクトルｘを、これら６個の状態変数のうちの一部により構成し、その一部について推定するものであってもよい。
【０１５７】
（６）変形例６
上述した実施形態では、観測値ベクトルｙｋを、磁気センサの出力である観測値ｑ、加速度センサの出力である観測値ａ、及び角速度センサの出力である観測値ｇを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、このうちの一部のみを利用して観測値ベクトルを生成してもよい。
【符号の説明】
【０１５８】
１…携帯機器、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定部、１２０…カルマンフィルタ演算部、１４０…初期値生成部、２００…中心点導出部、ｑ…磁気データ、ｘ…状態ベクトル、ｙ…観測値ベクトル、ｚ…観測残差、Ｓ…球面（第１球面）、ｃＯ…（第１球面の）中心点、Ｓ２…球面（第２球面）、ｃＯ２…（第２球面の）中心点、ｑＯＦＦ…磁気センサのオフセット、ｑＯ…磁気センサのオフセット推定値、Ｂｇ…地磁気、Ｂｉ…内部磁界、Ｂｘ…外部磁界。
【特許請求の範囲】
【請求項１】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新するカルマンフィルタと、
前記磁気データを蓄積する蓄積手段と、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する、中心点算出手段と、
を備え、
前記カルマンフィルタは、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、ことを特徴とする、状態推定装置。
【請求項２】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、状態遷移モデルを用いて、ある時刻における前記状態ベクトルから当該ある時刻から単位時間後の前記状態ベクトルを推定し、観測モデルを用いて、前記単位時間後の前記状態ベクトルから、推定観測値ベクトルを算出し、前記推定観測値ベクトルと前記複数のセンサの出力を要素とする観測値ベクトルとに基づいて観測残差を算出し、前記観測残差と前記単位時間後の前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタと、
前記磁気データを蓄積する蓄積手段と、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標の分布の３次元的な広がりの程度を示す分散評価値を算出する、分布判定手段と、
前記磁気データが所定数蓄積され、且つ、前記分散評価値が、分散許容値以上である場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する、中心点算出手段と、
複数の前記磁気データの各々が示す３軸の座標が、第２球面と曲面とを重ね合わせることで得られる立体の表面近傍に確率的に分布すると仮定して、複数の前記磁気データに基づいて、前記立体と、前記第２球面との形状の相違の程度を示す歪評価値を算出し、前記歪評価値が歪許容値以下であるか否かを判定する、歪判定手段と、
前記歪判定手段の判定結果が肯定である場合に、前記第１球面の中心点を示す３軸の座標を出力する、中心点出力手段と、
を備え、
前記カルマンフィルタは、前記中心点出力手段が、前記第１球面の中心点を示す３軸の座標を出力する場合に、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新することを特徴とする、状態推定装置。
【請求項３】
複数の前記磁気データの各々で示される３軸の座標として特定されるそれぞれの位置が、前記第１球面近傍に確率的に分布すると仮定した場合の、複数の前記磁気データで示される３軸の座標により特定されるそれぞれの位置と前記第１球面との誤差を表すベクトルを、第１球面誤差ベクトルとし、
３次元のベクトルを変数とし、前記第１球面誤差ベクトルの大きさを表す関数を、中心点算出関数としたとき、
前記第１球面の中心点を示す３軸の座標は、前記中心点算出関数の値を最小化するときの、前記３次元のベクトルが示す３軸の座標であることを特徴とする、
請求項１または２に記載の状態推定装置。
【請求項４】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データを蓄積する蓄積手段と、
を備える状態推定装置に用いられるオフセット更新方法であって、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新し、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出し、
前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、
ことを特徴とする、オフセット更新方法。
【請求項５】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データを蓄積する蓄積手段と、
を備える状態推定装置に用いられるオフセット更新プログラムであって、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新する処理と、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する処理と、
前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する処理とを、
コンピュータに実行させるオフセット更新プログラム。
【請求項１】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新するカルマンフィルタと、
前記磁気データを蓄積する蓄積手段と、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する、中心点算出手段と、
を備え、
前記カルマンフィルタは、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、ことを特徴とする、状態推定装置。
【請求項２】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、状態遷移モデルを用いて、ある時刻における前記状態ベクトルから当該ある時刻から単位時間後の前記状態ベクトルを推定し、観測モデルを用いて、前記単位時間後の前記状態ベクトルから、推定観測値ベクトルを算出し、前記推定観測値ベクトルと前記複数のセンサの出力を要素とする観測値ベクトルとに基づいて観測残差を算出し、前記観測残差と前記単位時間後の前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタと、
前記磁気データを蓄積する蓄積手段と、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標の分布の３次元的な広がりの程度を示す分散評価値を算出する、分布判定手段と、
前記磁気データが所定数蓄積され、且つ、前記分散評価値が、分散許容値以上である場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する、中心点算出手段と、
複数の前記磁気データの各々が示す３軸の座標が、第２球面と曲面とを重ね合わせることで得られる立体の表面近傍に確率的に分布すると仮定して、複数の前記磁気データに基づいて、前記立体と、前記第２球面との形状の相違の程度を示す歪評価値を算出し、前記歪評価値が歪許容値以下であるか否かを判定する、歪判定手段と、
前記歪判定手段の判定結果が肯定である場合に、前記第１球面の中心点を示す３軸の座標を出力する、中心点出力手段と、
を備え、
前記カルマンフィルタは、前記中心点出力手段が、前記第１球面の中心点を示す３軸の座標を出力する場合に、前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新することを特徴とする、状態推定装置。
【請求項３】
複数の前記磁気データの各々で示される３軸の座標として特定されるそれぞれの位置が、前記第１球面近傍に確率的に分布すると仮定した場合の、複数の前記磁気データで示される３軸の座標により特定されるそれぞれの位置と前記第１球面との誤差を表すベクトルを、第１球面誤差ベクトルとし、
３次元のベクトルを変数とし、前記第１球面誤差ベクトルの大きさを表す関数を、中心点算出関数としたとき、
前記第１球面の中心点を示す３軸の座標は、前記中心点算出関数の値を最小化するときの、前記３次元のベクトルが示す３軸の座標であることを特徴とする、
請求項１または２に記載の状態推定装置。
【請求項４】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データを蓄積する蓄積手段と、
を備える状態推定装置に用いられるオフセット更新方法であって、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新し、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出し、
前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する、
ことを特徴とする、オフセット更新方法。
【請求項５】
磁界を発生させる部品を備えた機器に組み込まれ、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記磁気データを蓄積する蓄積手段と、
を備える状態推定装置に用いられるオフセット更新プログラムであって、
前記磁気データのうち前記部品の発生する磁界の成分を表す３次元のベクトルデータであるオフセットを推定する状態変数を含む複数の状態変数を要素とするベクトルを状態ベクトルとし、前記複数のセンサの出力を要素とする観測値ベクトル及び前記状態ベクトルを用いて算出される観測残差に基づいて、前記状態ベクトルを更新する処理と、
前記磁気データが所定数蓄積された場合に、前記所定数の前記磁気データの各々が示す３軸の座標が、第１球面の近傍に確率的に分布すると仮定して、前記第１球面の中心点を示す３軸の座標を算出する処理と、
前記オフセットを推定する状態変数を、前記第１球面の中心点を示す３軸の座標によって更新する処理とを、
コンピュータに実行させるオフセット更新プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【公開番号】特開２０１３−６４６９５（Ｐ２０１３−６４６９５Ａ）
【公開日】平成２５年４月１１日（２０１３．４．１１）
【国際特許分類】
【出願番号】特願２０１１−２０４６８１（Ｐ２０１１−２０４６８１）
【出願日】平成２３年９月２０日（２０１１．９．２０）
【出願人】（０００００４０７５）ヤマハ株式会社 (5,930)
【Ｆターム（参考）】
【公開日】平成２５年４月１１日（２０１３．４．１１）
【国際特許分類】
【出願日】平成２３年９月２０日（２０１１．９．２０）
【出願人】（０００００４０７５）ヤマハ株式会社 (5,930)
【Ｆターム（参考）】
[ Back to top ]