状態推定装置
【課題】非線形カルマンフィルタを用いて高速且つ正確な状態推定を行う。
【解決手段】携帯機器１は、３次元地磁気センサ７０、３次元加速度センサ８０、ＣＰＵ１０、状態推定プログラム１００を備え、状態推定プログラム１００は、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面の中心点ｃＳの座標を算出する中心点算出モジュール３００と、複数の加速度データａ１〜ａＭに基づいて、携帯機器の動きが安定しているか否かを判定する安定性判定モジュール４００と、安定性判定モジュール４００が行う判定結果が肯定である場合に初期ベクトルＩＮＩを算出する初期ベクトル生成モジュール５００と、初期ベクトルＩＮＩを初期値とする状態ベクトルｘを観測値ベクトルｙを用いて更新するカルマンフィルタモジュール６００とを備える。
【解決手段】携帯機器１は、３次元地磁気センサ７０、３次元加速度センサ８０、ＣＰＵ１０、状態推定プログラム１００を備え、状態推定プログラム１００は、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面の中心点ｃＳの座標を算出する中心点算出モジュール３００と、複数の加速度データａ１〜ａＭに基づいて、携帯機器の動きが安定しているか否かを判定する安定性判定モジュール４００と、安定性判定モジュール４００が行う判定結果が肯定である場合に初期ベクトルＩＮＩを算出する初期ベクトル生成モジュール５００と、初期ベクトルＩＮＩを初期値とする状態ベクトルｘを観測値ベクトルｙを用いて更新するカルマンフィルタモジュール６００とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置に関する。
【背景技術】
【０００２】
地磁気の向きや、物体の姿勢等を算出する場合、地磁気センサからの出力結果のみに基づいて算出するよりも、地磁気センサの他に加速度センサや角速度センサ等の異種の物理量を測定する複数のセンサの出力結果を統合して算出した方が、短時間で正確な値を求めることができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、非線形カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合し、姿勢を推定する方法が開示されている。
【０００４】
一般的に、非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、非線形カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量を推定する値（状態変数）を要素とする状態ベクトルを逐次更新することにより、状態ベクトルの各要素の示す値を、実際の物理量（真値）に近づける演算を行う。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００６】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかし、非線形カルマンフィルタは、状態ベクトルと、状態ベクトルに基づいて算出される観測残差とを用いて、状態ベクトルを逐次更新する演算であるため、状態ベクトルの初期値の要素が真値から乖離した値に設定された場合、状態ベクトルの各要素が真値に近い値に収束するまでに長時間を要することがあり、更には、状態ベクトルの各要素が真値とは異なる不正確な値に収束することも起こり得る。
【０００８】
本発明は、上述した点に鑑みてなされたものであり、真値に近い値を要素とする状態ベクトルの初期値を算出して非線形カルマンフィルタの演算を行うことで、地磁気の向きや物体の姿勢を高速且つ正確に推定することを解決課題とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決するため、本発明に係る状態推定装置は、磁界を発生させる部品を備えた機器に組み込まれる状態推定装置であって、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサと、加速度を直交する３軸で検出して、３軸の座標系において表現される３次元のベクトルデータとして加速度データを順次出力する３次元加速度センサと、前記３次元磁気センサから順次出力される磁気データを蓄積する蓄積手段と、前記蓄積手段に蓄積された複数の前記磁気データに基づいて、前記部品の発する磁界を表す前記３次元磁気センサのオフセットの近似値を示す中心点の３軸の座標を算出する中心点算出手段と、複数の前記加速度データに基づいて、前記機器の動きが安定しているか否かを判定する安定性判定手段と、前記安定性判定手段の判定結果が肯定である場合、前記中心点の３軸の座標、前記磁気データ、及び、前記加速度データに基づいて、前記オフセットの推定値及び前記機器の姿勢を推定するベクトルを要素として含む初期ベクトルを生成する初期ベクトル生成手段と、前記初期ベクトルを初期値とする状態ベクトルを、前記３次元磁気センサ及び前記３次元加速度センサを含む前記機器に組み込まれた複数のセンサの出力を要素とする観測値ベクトルを用いて更新するカルマンフィルタ演算手段と、を備えることを特徴とする。
【００１０】
カルマンフィルタの演算において、状態ベクトルの初期値（初期ベクトル）の各要素が真値から乖離した値である場合、状態ベクトルの各要素が真値に近い値に収束するまでに長い時間を必要とする。
この発明によれば、中心点算出手段が算出する中心点の座標に基づいて定められる３次元磁気センサのオフセット推定値と、機器の動きが安定していると判定されたときの磁気データ及び加速度データとに基づいて算出される姿勢の推定値とを、初期ベクトルの要素とする。
中心点算出手段が算出する中心点は、３次元磁気センサのオフセットの近似値を示す座標なので、オフセット推定値を真値に近い値に設定することができる。そして、３次元磁気センサのオフセット推定値及び磁気データを用いて、正確な地磁気の向きを得ることができる。
また、機器の動きが安定しているときの加速度データより重力加速度の向きを得ることができるため、重力加速度の向き及び地磁気の向きに基づいて、真値に近い姿勢の推定値を算出することができる。
すなわち、この発明によれば、初期ベクトルの各要素を真値に近い値に設定することができ、カルマンフィルタの演算において状態ベクトルの各要素を高速に真値に近い値に収束させることが可能となる。
【００１１】
また、上述した状態推定装置において、前記中心点算出手段が前記中心点の算出に用いる複数の前記磁気データの取得を開始する前に、利用者に前記機器の姿勢を変化させるように促す第１の報知手段を、更に備えることが好ましい。
【００１２】
３次元磁気センサは、地磁気と、部品の発する磁界とを検出する。３次元磁気センサの姿勢を変化させる場合、地磁気は、大きさは一定であるが向きが変化する磁界として検出される。一方、部品の発する磁界は、３次元磁気センサの姿勢をどのように変化させても、一定の向き及び大ききを有する磁界として検出される。従って、３次元磁気センサの姿勢を３次元的に変化させつつ検出された複数の磁気データが、３次元磁気センサに固定された座標系において示す座標は、部品の発する磁界を表すベクトルの示す座標を中心点とし地磁気の大きさを半径とする球面近傍に分布する。そして、当該球面の中心点の座標が、３次元磁気センサのオフセットの近似値を表す。
よって、複数の磁気データに基づいてオフセットの近似値を算出するためには、複数の磁気データの示す座標を近傍に有する球面を一意に特定することが必要であり、そのためには、複数の磁気データの示す座標が、２次元的に分布せず、３次元的な広がりを有して分布していることが必要である。
この発明によれば、利用者に機器の姿勢を変化させるように促す第１の報知手段を備えるため、複数の磁気データを取得する際に利用者が機器の姿勢を３次元的に変化させることが期待される。機器の姿勢を３次元的に変化させつつ取得した複数の磁気データの示す座標は、３次元的な広がりを有して分布する。従って、複数の磁気データの示す座標を近傍に有する球面を一意に特定することが可能となり、３次元磁気センサのオフセットの近似値を示す中心点の座標を算出することが可能となる。
【００１３】
また、上述した状態推定装置において、前記中心点算出手段が前記中心点を算出した後に、利用者に前記機器の姿勢を変化させないように促す第２の報知手段を、更に備えることが好ましい。
【００１４】
この発明によれば、利用者に機器の姿勢を変化させないように促す第２の報知手段を備えるため、利用者が機器を動かさず安定的な状態に置くことが期待される。
機器の動きが安定している場合、加速度データより重力加速度の向きを得ることができるため、重力加速度の向き及び地磁気の向きに基づいて、真値に近い姿勢の推定値を得ることが可能となる。
【００１５】
また、上述した状態推定装置において、前記安定性判定手段は、複数の前記加速度データに基づいて基準加速度データを算出するローパスフィルタを備え、前記加速度データの示す座標と前記基準加速度データの示す座標との誤差を表す安定性指標を、所定個の前記加速度データの各々について算出し、前記安定性指標の全てが第１閾値以下である場合、前記機器の動きが安定していると判定することを特徴とすることが好ましい。
【００１６】
この発明によれば、加速度データをローパスフィルタに通過させることで得られる基準加速度データと、所定個の加速度データとの誤差を表す所定個の安定性指標を算出することで、所定個の加速度データがほぼ一定の値を示すものであるか否かを判定する。
複数の加速度データの示す値の変動が小さくほぼ一定の値を示すとき、例えば機器が等加速度運動をする等の特殊な場合を除き、加速度データは重力加速度を示すものと看做すことができる。従って、本発明によれば、加速度データから重力加速度の向きを算出することができるか否かを判定し、判定結果が肯定である場合には、加速度データから算出された重力加速度の向きと地磁気の向きとに基づいて、真値に近い姿勢の推定値を得ることが可能となる。
【００１７】
また、上述した状態推定装置において、前記安定性判定手段は、所定個の前記加速度データの示す３軸の座標の分散を、安定性指標として算出し、前記安定性指標が、第２閾値以下である場合、前記機器の動きが安定していると判定することを特徴とすることが好ましい。
【００１８】
この発明によれば、所定個の前記加速度データの示す座標の分散を表す安定性指標を算出し、所定個の加速度データがほぼ一定の値を示すものであるか否かを判定する。従って、本発明によれば、加速度データから重力加速度の向きを算出することができるか否かを判定し、判定結果が肯定である場合には、加速度データから算出された重力加速度の向きと地磁気の向きとに基づいて、真値に近い姿勢の推定値を得ることが可能となる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係る３次元磁気センサが検出する磁界の概要を説明する概念図である。
【図４】本発明の実施形態に係る状態推定プログラムの処理の流れを示すフローチャートである。
【図５】本発明の実施形態に係る安定性判定モジュールを実行することで実現される機能を表す機能ブロック図である。
【図６】本発明の実施形態に係るカルマンフィルタモジュールを実行することで実現される機能を表す機能ブロック図である。
【発明を実施するための形態】
【００２０】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００２１】
［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１は、画面の姿勢に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢と、携帯機器１から見た地磁気Ｂｇの向きとを推定することによって実現される。
【００２２】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（蓄積部）２０、状態推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。
また、携帯機器１は、地磁気Ｂｇ等の磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００２３】
表示部５０は、ＣＰＵ１０が状態推定プログラム１００を実行することにより推定したシステムの状態に基づいて算出された地磁気Ｂｇの向きや携帯機器１の姿勢μ等を、矢印等の画像により表示する。
ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。
【００２４】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｑを出力する。この磁気データｑは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。
【００２５】
ところで、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力するベクトルデータ（磁気データｑ）は、地磁気Ｂｇを表すベクトルの他に、携帯機器１に搭載された部品が発する磁界（内部磁界Ｂｉ）を表すベクトルも含む値となる。従って、地磁気Ｂｇの向きを正確に知るためには、３次元磁気センサが出力するベクトルデータ（磁気データｑ）から、携帯機器１の部品が発する内部磁界Ｂｉを表すベクトルを取り除く補正処理が必要となる。
【００２６】
補正処理を説明するための前提として、まず、地磁気Ｂｇ及び内部磁界Ｂｉの性質について説明する。
図３（Ａ）に示すように、地磁気Ｂｇは、磁極北に向かう水平成分と伏角方向の鉛直成分を有する磁界であり、地上に固定された座標系である地上座標系ΣＧにおいて、一定の向き及び大きさを有するベクトルＧＢｇとして表される。一方、内部磁界Ｂｉは、携帯機器１の部品が発する磁界であり、携帯機器１に対して一定の方向を向いた、一定の大きさの磁界である。つまり、内部磁界Ｂｉは、地上座標系ΣＧにおいて、携帯機器１の姿勢μの変化に連動して向きを変える、一定の大きさのベクトルＧＢｉ（μ）として表される。
【００２７】
ここで、携帯機器１（厳密には３次元磁気センサ７０）に固定された座標系であるセンサ座標系ΣＳを導入し、磁気データｑを、センサ座標系ΣＳ上の座標として表現する。より具体的には、磁気データｑが示すセンサ座標系ΣＳ上の座標のうち、ｘ軸成分は、Ｘ軸磁気センサ７１からの出力値であり、ｙ軸成分は、Ｙ軸磁気センサ７２からの出力値であり、ｚ軸成分は、Ｚ軸磁気センサ７３からの出力値である。
このようなセンサ座標系ΣＳから見た場合、地磁気Ｂｇは、携帯機器１の姿勢μに連動して向きを変える、一定の大きさのベクトルＳＢｇ（μ）として表され、内部磁界Ｂｉは、一定の向き及び大きさを有するベクトルＳＢｉとして表される。
従って、地上座標系ΣＧから見た携帯機器１の姿勢μをμ１〜μＮと変化させつつ取得した複数の磁気データｑ１〜ｑＮは、センサ座標系ΣＳにおいて、図３（Ｂ）に示すように、内部磁界Ｂｉを表すベクトルＳＢｉの示す座標を中心とし、地磁気Ｂｇの大きさを半径とする、球面Ｓ上に分布する。そして、磁気データｑの示す座標から、内部磁界Ｂｉを表すベクトルＳＢｉの示す座標を減算することにより、センサ座標系ΣＳにおける地磁気Ｂｇの向きが特定される。
このように、検出対象である地磁気Ｂｇの正確な向きを特定するために、補正処理において、３次元磁気センサ７０から出力される磁気データｑから取り除かれる内部磁界Ｂｉを表すベクトルを磁気センサのオフセットｃＯＦＦと呼ぶ。
なお、複数の磁気データｑ１〜ｑＮの示す座標は、３次元磁気センサ７０の測定誤差を考慮すると、球面Ｓ上には分布せず、球面Ｓの近傍に分布する。従って、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面Ｓの中心点ｃＳの座標は、内部磁界Ｂｉを表すベクトルＳＢｉの示す座標の近似値（すなわち、オフセットｃＯＦＦの近似値）となる。
【００２８】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００２９】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示すベクトルデータである。
【００３０】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、携帯機器１の姿勢や磁気センサのオフセット等のシステムの状態を表す複数の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することにより、状態推定装置として機能する。
【００３１】
状態推定プログラム１００は、バッファ管理モジュール２００、中心点算出モジュール３００、安定性判定モジュール４００、初期ベクトル生成モジュール５００、及びカルマンフィルタモジュール６００等のモジュール群で構成される。
バッファ管理モジュール２００は、３次元磁気センサ７０から順次出力される磁気データｑをバッファに蓄積する。これら複数の磁気データの蓄積手段としては、ＲＡＭ２０を用いる。
中心点算出モジュール３００は、ＲＡＭ２０に蓄積された複数の磁気データｑ１〜ｑＮに基づいて、センサ座標系ΣＳにおいて複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面Ｓの中心点ｃＳの座標を算出する。
安定性判定モジュール４００は、３次元加速度センサ８０から出力される加速度データａに基づいて、携帯機器１の動きが安定しているか否かを判定する。
初期ベクトル生成モジュール５００は、安定性判定モジュール４００による判定結果が肯定である場合に、初期ベクトルＩＮＩを生成する。
カルマンフィルタモジュール６００は、初期ベクトルＩＮＩを初期値とする状態ベクトルｘを、観測値ベクトルｙに基づいて更新する演算（非線形カルマンフィルタの演算）を実行することで、システムの状態を推定する。
【００３２】
ここで、状態ベクトルｘとは、複数の状態変数を要素とするベクトルである。状態ベクトルｘの要素である複数の状態変数の各々は、非線形カルマンフィルタが推定するシステムの状態を示す複数の物理量を表す。すなわち、状態ベクトルｘは、非線形カルマンフィルタが推定するシステムの状態を表すベクトルである。
本実施形態は、状態ベクトルｘの要素（状態変数）として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサ９１〜９３のオフセット推定値ｂＯ、及び磁気センサ７１〜７３のオフセット推定値ｃＯを採用し、これらを推定の対象とする。
時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。
【数１】
【００３３】
ここで、地磁気の強さｒはスカラーであり、地磁気の伏角φはスカラーであり、角速度ωは３次元ベクトルであり、角速度センサのオフセット推定値ｂＯは３次元ベクトルであり、磁気センサのオフセット推定値ｃＯは３次元ベクトルである。
また、姿勢μは、クォータニオンを用いて表現される。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。例えば、センサ座標系ΣＳの各軸と、地上座標系ΣＧの各軸とが一致する姿勢μを基準姿勢と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢μは、クォータニオンを用いて、式（２）で与えられる。
【数２】
【００３４】
観測値ベクトルｙとは、３次元磁気センサ７０から出力される磁気データｑ、３次元加速度センサ８０から出力される加速度データａ、及び、３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルである。
時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数３】
【００３５】
ところで、非線形カルマンフィルタの演算は、状態ベクトルと観測値ベクトルとに基づいて算出される観測残差を用いて、状態ベクトルを周期的に更新する演算であるため、状態ベクトルの初期値の各要素（各状態変数）の示す値が、真値から乖離した値である場合、状態ベクトルの各要素を真値に近い値に収束させるのに長い時間を要することがある。
また、非線形カルマンフィルタの演算は、非線形演算であるため、状態ベクトルの初期値の各要素が真値から大きく乖離した値である場合、状態ベクトルの更新を繰り返しても、状態ベクトルの各要素が真値（または、真値に近い値）に収束せずに、真値とは異なる値に収束することがある。
従って、非線形カルマンフィルタの演算において、システムの状態を正確且つ高速に推定するためには、状態ベクトルｘの初期値（つまり初期ベクトルＩＮＩ）の各要素を、真値に近い値に設定する必要がある。
ここで、「真値」とは、状態ベクトルｘｋの各要素に対応する実際の物理量を表す値である。また、「状態ベクトルの真値」とは、状態ベクトルｘｋの各要素を真値としたときのベクトルである。
【００３６】
［２． 状態推定装置の処理フロー］
図４は、ＣＰＵ１０が実行する状態推定プログラム１００の処理の流れを表すフローチャートである。以下では、ＣＰＵ１０が状態推定プログラム１００を実行したときの処理の流れを、図４に加えて、当該フローチャートの各ステップが実行された時に実現される機能の詳細を説明する図５乃至図６を参照しつつ、説明する。
【００３７】
まず、ステップＳ１０１において、ＣＰＵ１０は、初期化処理を実行する。具体的には、ＣＰＵ１０は、バッファ管理モジュール２００を呼び出すことで、ＲＡＭ２０に記憶した磁気データｑの全部を廃棄する。なお、本実施形態では、ＣＰＵ１０は、磁気データｑの全部を廃棄するが、ＲＡＭ２０に蓄積されたデータのうち古い方から一定割合のデータのみを廃棄しても良い。
【００３８】
ステップＳ１０２において、ＣＰＵ１０は、第１の報知を行う。具体的には、ＣＰＵ１０は、表示部５０に画像や動画等を表示させることで、携帯機器１の利用者に対して、携帯機器１の姿勢を３次元的に変化させるように指示する。すなわち、ＣＰＵ１０は、ステップＳ１０２の第１の報知を行うことにより、第１の報知手段として機能する。
なお、本実施形態では、ＣＰＵ１０は、第１の報知を、表示部５０を用いて画像や動画等を表示することにより行うことを想定しているが、音声等を用いて行ってもよい。
【００３９】
ステップＳ１０３において、ＣＰＵ１０は、磁気データ蓄積処理を実行する。具体的には、ＣＰＵ１０は、バッファ管理モジュール２００を呼び出すことにより、３次元磁気センサ７０から順次出力されるＮ個の磁気データｑ１〜ｑＮを、ＲＡＭ２０に格納する（ここで、Ｎは、後述する中心点算出処理において、精度よく中心点ｃＳを導出するために必要な磁気データの規定測定回数を表す５以上の自然数）。すなわち、ＣＰＵ１０は、ステップＳ１０３の磁気データ蓄積処理を実行することにより、蓄積手段として機能する。
【００４０】
次に、ステップＳ１０４において、ＣＰＵ１０は、中心点算出処理を実行する。具体的には、ＣＰＵ１０は、中心点算出モジュール３００を呼び出すことにより、ＲＡＭ２０に蓄積された複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面Ｓの中心点ｃＳの座標を算出する。すなわち、ＣＰＵ１０は、ステップＳ１０４の中心点算出処理を実行することにより、中心点算出手段として機能する。
なお、前述の通り、中心点ｃＳの座標を示すベクトルは、オフセットｃＯＦＦの近似値である。中心点ｃＳの座標の算出は、公知の方法を適宜適用して行えばよい。例えば、本件出願人が、特開２００７−２４０２７０号公報で開示している方法等を適用してもよい。
【００４１】
次に、ステップＳ１０５において、ＣＰＵ１０は、第２の報知を行う。具体的には、ＣＰＵ１０は、表示部５０に画像や動画等を表示させることで、携帯機器１の利用者に対して、携帯機器１を動かさないように指示する。すなわち、ＣＰＵ１０は、ステップＳ１０５の第２の報知を行うことにより、第２の報知手段として機能する。
なお、本実施形態では、ＣＰＵ１０は、第２の報知を、表示部５０を用いて画像や動画等を表示することにより行うことを想定しているが、音声等を用いて行ってもよい。
【００４２】
その後、ステップＳ１０６において、ＣＰＵ１０は、安定性判定処理を実行する。具体的には、ＣＰＵ１０は、安定性判定モジュール４００を呼び出すことにより、３次元加速度センサ８０から出力される加速度データａに基づいて、携帯機器１の動きが安定しているか否かを判定する。すなわち、ＣＰＵ１０は、ステップＳ１０６の安定性判定処理を実行することにより、安定性判定手段として機能する。
【００４３】
図５は、ＣＰＵ１０が、ステップＳ１０６の安定性判定処理を実行することで実現される安定性判定手段の機能を表す機能ブロック図である。
図５に示すように、安定性判定手段には、平滑化手段４１０、安定性指標算出手段４２０、及び条件判定手段４３０が含まれる。
【００４４】
平滑化手段４１０は、３次元加速度センサ８０から順次出力される加速度データａの各成分を、ローパスフィルタを通して平滑化することで、基準加速度データａＣを算出する。
ここで、ローパスフィルタには、公知のフィルタを適宜適用すればよい。例えば、ＦＩＲ（Finite impulse response）フィルタ、または、ＩＩＲ（Infinite impulse response）フィルタを用いてもよい。
なお、本実施形態では、平滑化手段４１０への加速度データａの入力の開始は、ＣＰＵ１０がステップＳ１０６の安定性判定処理を開始するタイミングであるが、それ以外のタイミングで入力を開始してもよい。例えば、ＣＰＵ１０がステップＳ１０１の初期化処理を開始するタイミングで、平滑化手段４１０への加速度データａの入力を開始してもよい。この場合、ＣＰＵ１０は、ステップＳ１０１の初期化処理を開始するタイミングで、ステップＳ１０６の安定性判定処理を開始して、平滑化手段４１０を機能させる。
【００４５】
次に、安定性指標算出手段４２０は、３次元加速度センサ８０から出力される加速度データａと、平滑化手段４１０から出力される基準加速度データａＣとに基づいて、安定性指標αを算出する。
時刻ｔにおいて算出される安定性指標αｔは、以下の式（４ａ）で表される。なお、式（４ｂ）に示される加速度データａtは、時刻ｔにおける加速度データであり、式（４ｃ）に示される基準加速度データａＣtは、時刻ｔにおける基準加速度データである。
【数４】
【００４６】
安定性指標算出手段４２０は、時刻ｔ＝１から時刻ｔ＝Mまでの期間に、Ｍ個の安定性指標αｔ（ｔ＝１、…、Ｍ）を算出する。
ここで、Ｍは、１以上の任意の自然数である。なお、携帯機器１の動きが安定しているか否かを、より厳密に判定することが必要な場合には、Ｍを、できるだけ大きな自然数に設定すればよい。
【００４７】
条件判定手段４３０は、以下の式（５）に示すように、Ｍ個の安定性指標αｔ（ｔ＝１、…、M）の全てが、第１閾値αＴＨ以下であるか否かを判定する。
【数５】
【００４８】
このように、ＣＰＵ１０は、ステップＳ１０６の安定性判定処理を実行することにより、まず、Ｍ個の安定性指標αｔを算出し、次に、Ｍ個の安定性指標αｔの全てが第１閾値αＴＨ以下であるか否かを判定する。そして、ＣＰＵ１０は、安定性判定処理における判定結果が肯定である場合、処理をステップＳ１０７に進める。一方、ＣＰＵ１０は、安定性判定処理における判定結果が否定である場合、処理をステップＳ１０５に戻す。
【００４９】
次に、ステップＳ１０７において、ＣＰＵ１０は、初期ベクトル生成処理を実行する。具体的には、ＣＰＵ１０は、初期ベクトル生成モジュール５００を呼び出すことにより、時刻ｔ＝Ｍ（つまり、条件判定手段４３０の判定結果が肯定となったタイミング）に３次元磁気センサ７０から出力される磁気データｑ、時刻ｔ＝Ｍに３次元加速度センサ８０から出力される加速度データａ、及び、ステップＳ１０４において算出される中心点ｃＳの座標に基づいて、初期ベクトルＩＮＩを生成する。すなわち、ＣＰＵ１０は、ステップＳ１０７の初期ベクトル生成処理を実行することにより、初期ベクトル生成手段として機能する。
前述の通り、初期ベクトルＩＮＩは、状態ベクトルｘｋの初期値、つまり時刻Ｔ＝０における状態ベクトルｘ０であり、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｂＯの初期値ｂＯ,０、及び磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０を要素として含む、以下の式（６）により示されるベクトルである。なお、以下では、ＣＰＵ１０は、初期ベクトル生成処理を実行することにより初期ベクトルＩＮＩを生成するタイミングを、時刻Ｔ＝０と表現する。時刻Ｔ＝０は、時刻ｔ＝Ｍと等しい時刻である。
【数６】
【００５０】
以下、初期ベクトルＩＮＩの生成方法を、具体的に説明する。
まず、時刻Ｔ＝０に３次元磁気センサ７０から出力される磁気データｑ０は、地上座標系ΣＧから地磁気Ｂｇを表したベクトルＧＢｇと、３次元磁気センサ７０のオフセット推定値ｃＯの初期値ｃＯ,０とを用いて、以下の式（７）で表される。
ここで、行列Ｂ（μ）は、携帯機器１が姿勢μである場合に、地上座標系ΣＧにおいて表現されたベクトルを、センサ座標系ΣＳで表現するための座標変換行列であり、以下の式（８）で表される。また、時刻Ｔ＝０における地磁気Ｂｇを地上座標系ΣＧから表したベクトルＧＢｇは、地磁気の強さｒの初期値ｒ０と、地磁気の伏角φの初期値φ０とを用いて、以下の式（９）で表される。なお、本実施形態では、地上座標系ΣＧを、ｙ軸が磁極北を向き、ｚ軸が鉛直上を向くように定める。
また、３次元磁気センサ７０のオフセット推定値ｃＯの初期値ｃＯ,０には、以下の式（１０）に示すように、中心点ｃＳの座標が設定される。
【数７】
【００５１】
一方、時刻Ｔ＝０に３次元加速度センサ８０から出力される加速度データａ０は、携帯機器１の動きが安定していれば（例えば、携帯機器１が静止状態または等速直線運動状態にあれば）、以下の式（１１）で表される。なお、ベクトルＧＧＲＶは、地上座標系ΣＧにおいて重力加速度を表したベクトルを、重力加速度の大きさで正規化した３次元のベクトルであり、以下の式（１２）で表される。
【数８】
【００５２】
センサ座標系ΣＳにおいて地磁気Ｂｇの向き及び大きさを表すベクトルＳＢｇは、磁気データｑと３次元磁気センサ７０のオフセット推定値ｃＯとを用いて、以下の式（１３ａ）によって表すことができる。
携帯機器１の姿勢は、以下の式（１３ａ）に示すベクトルＳＢｇと、加速度データａとを用いて、以下の式（１３ｂ）に示す３行３列の行列Ａとして表現することができる。そして、行列Ａにより表された携帯機器１の姿勢を、クォータニオンを用いて表現し直すことにより、携帯機器１の姿勢μを算出することができる。つまり、姿勢μの初期値μ０は、以下の式（１３ａ）及び式（１３ｂ）に対して、時刻Ｔ＝０に３次元加速度センサ８０から出力される加速度データａ０と、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０とを適用することにより算出される行列Ａに基づいて、求めることができる。
なお、３行３列の行列Ａからクォータニオンへの変換は、公知の方法を適宜用いて行えば良い。行列Ａは、式（８）に示した行列Ｂ（μ）の逆行列であるので、行列Ａの各成分と行列Ｂ（μ）の逆行列の各成分とを比較することで、クォータニオンを用いて表される姿勢μの各要素を計算すればよい。例えば、以下の公知文献に記載された方法を用いて、行列Ａから姿勢μを算出することもできる。
Malcolm D. Shuster and Gregory A. Natanson, “Quaternion Computation from a Geometric Point of View", The Journal of the Astronautical Sciences, Vol. 41, No. 4, October-December 1993, pp. 545-556.
【数９】
【００５３】
地磁気の強さｒの初期値ｒ０は、以下の式（１４ａ）に基づいて算出することができる。具体的には、地磁気の強さｒの初期値ｒ０は、まず、式（１３ａ）に対して磁気データｑと磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０とを代入してベクトルＳＢｇを求め、次に、ベクトルＳＢｇを以下の式（１４ａ）に代入することで算出される。
また、地磁気の伏角φの初期値φ０は、ベクトルＳＢｇの各要素を以下の式（１４ｂ）で表した場合、以下の式（１４ｃ）及び式（１４ｄ）に基づいて算出することができる。
【数１０】
【００５４】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。また、角速度センサのオフセット推定値ｂＯの初期値ｂＯ,０は、通常「０」近辺に調整されているため、「０」に設定する。
【００５５】
このように、ＣＰＵ１０は、ステップＳ１０７の初期ベクトル生成処理を実行することにより、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｂＯの初期値ｂＯ,０、及び磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０を要素とするベクトルである初期値ＩＮＩを生成し、これを出力する。
【００５６】
次に、ステップＳ１０８において、ＣＰＵ１０は、カルマンフィルタ演算処理を実行する。具体的には、ＣＰＵ１０は、カルマンフィルタモジュール６００を呼び出すことにより、非線形カルマンフィルタの演算を行い、システムの状態を推定する。すなわち、ＣＰＵ１０は、ステップＳ１０８のカルマンフィルタ演算処理を実行することにより、カルマンフィルタ演算手段として機能する。
以下、非線形カルマンフィルタの演算の詳細な内容について説明する。
【００５７】
［３． 非線形カルマンフィルタによる演算］
一般的にカルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘ＋ｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定する。この推定値を、推定状態ベクトルｘ−ｋと称する。そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、推定状態ベクトルｘ−ｋから観測値ベクトルｙｋを推定する。この推定値を、推定観測値ベクトルｙ−ｋと称する。
次に、カルマンフィルタは、推定観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｚｋとして算出し、観測残差ｚｋに基づいてカルマンゲインＫｋを算出する。さらに、カルマンフィルタは、観測残差ｚｋ、カルマンゲインＫｋ、及び、推定状態ベクトルｘ−ｋを用いて、状態ベクトルｘ＋ｋ−１を更新することにより、更新後の状態ベクトルｘ＋ｋを算出する。
以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタの演算により、状態ベクトルｘｋの各要素を実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける。
【００５８】
なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。シグマポイントカルマンフィルタは、まず、状態ベクトルｘ＋ｋ−１の周囲に複数のシグマポイントχ＋ｋ−１を設定し、これらの複数のシグマポイントχ＋ｋ−１を状態遷移モデルに適用することで、単位時間経過後の複数のシグマポイントを推定する。この推定された複数のシグマポイントを、推定シグマポイントχ−ｋと称する。次に、シグマポイントカルマンフィルタは、複数の推定シグマポイントχ−ｋの平均を算出することにより、推定状態ベクトルｘ−ｋを求める。従って、推定観測値ベクトルｙ−ｋは、厳密には、推定状態ベクトルｘ−ｋの周辺に存在する複数の推定シグマポイントχ−ｋに基づいて算出される。
【００５９】
本実施形態における、非線形カルマンフィルタの状態遷移モデルは以下に示す式（１５）で与えられ、観測モデルは以下に示す式（１６）で与えられる。なお、本実施形態では、式（１５）に現れる関数ｆ及び式（１６）に現れる関数ｈは、非線形の関数である。
【数１１】
【００６０】
ここで、状態ベクトルｘｋはｄｉｍ（ｘ）次元のベクトルであり、観測値ベクトルｙｋはｄｉｍ（ｙ）次元のベクトルである。なお、本実施形態では、ｄｉｍ（ｘ）＝１５であり、ｄｉｍ（ｙ）＝９である。また、式（１５）に現れるプロセスノイズｗｋ及び式（１６）に現れる観測ノイズｖｋは０を中心とするガウスノイズである。
式（１５）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗｋ−１とを加算することにより推定されることを示している。
また、式（１６）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗｋの共分散をＱｋ、観測ノイズｖｋの共分散をＲｋと表す。
【００６１】
時刻Ｔ＝ｋにおける観測残差ｚｋは、実際の観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（１７）で表される。カルマンフィルタは、以下の式（１９）に示すように、観測残差ｚｋ、推定状態ベクトルｘ−ｋ、及び、式（１８）に示すカルマンゲインＫｋを用いて、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタは、以下の式（２０）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
ここで、Ｐ−ｋは、推定状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。また、Ｐｙｙｋは、観測残差ｚｋの共分散であり、Ｐｘｙｋは、推定状態ベクトルｘ−ｋと、推定観測値ベクトルｙ−ｋとの相互共分散行列である。
【数１２】
【００６２】
なお、推定観測値ベクトルｙ−ｋは、式（１６）に示すように、状態遷移モデルを用いて算出された推定状態ベクトルｘ−ｋ（厳密には、推定シグマポイントχ−ｋ）を、観測モデルに適用することで算出される値である。よって、推定観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｚｋは、推定状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
式（１９）に示すように、観測残差ｚｋを用いて、推定状態ベクトルｘ−ｋを、更新後の状態ベクトルｘ＋ｋへと更新することにより、状態ベクトルｘｋの各要素を真値に近い値へと近付けることができる。
【００６３】
本実施形態に係る状態推定装置は、アンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて、カルマンフィルタ演算処理を実行する。
図６は、ＣＰＵ１０がカルマンフィルタモジュール６００を呼び出すことで実現されるカルマンフィルタ演算手段の機能を表す機能ブロック図である。
【００６４】
図６に示すように、遅延部６１０は、加算器６９０から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部６２０に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部６１０は、初期ベクトル生成モジュール５００が出力する初期値ＩＮＩを、状態ベクトルｘ＋ｋ−１に適用する。
【００６５】
シグマポイント生成部６２０は、「ｄｉｍ（ｘ）」行「ｄｉｍ（ｘ）」列の共分散行列Ｐ＋ｋ−１及びＱｋ−１を用いて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントを生成する。
具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマポイントの広がりを表すスケーリングパラメータλを用いて、式（２１）及び式（２２）に示すベクトルσｋを定義する。
【数１３】
【００６６】
このとき、シグマポイント生成部６２０は、ベクトルσｋ−１と状態ベクトルｘ＋ｋ−１とに基づいて、式（２３）及び式（２４）で示される「２ｄｉｍ（ｘ）＋１」個のシグマポイントχｋ−１を生成する。
【数１４】
【００６７】
状態遷移モデル部６３０は、式（２５）に示すように、時刻Ｔ＝ｋ−１における「２ｄｉｍ（ｘ）＋１」個のシグマポイントχｋ−１（ｊ）の各々を、状態遷移モデルに適用することにより、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（ｊ）を算出する。
【数１５】
【００６８】
次に、平均算出部６４０は、式（２６）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋの平均値を計算することで、推定状態ベクトルｘ−ｋを算出する。
【数１６】
【００６９】
また、平均算出部６４０は、式（２７）に示す、推定状態ベクトルｘ−ｋの誤差の共分散Ｐ−ｋを算出する。
【数１７】
【００７０】
一方、観測モデル部６５０は、式（２８）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（ｊ）の各々を、観測モデルに適用することにより、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（ｊ）を算出する。
【数１８】
【００７１】
そして、平均処理部６６０は、式（２９）に示すように、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（ｊ）の平均を演算することにより、推定観測値ベクトルｙ−ｋを算出する。
【数１９】
【００７２】
次に、平均処理部６６０は、式（３０）に示す観測残差の共分散Ｐｙｙｋを算出する。
【数２０】
【００７３】
減算器６７０は、式（１７）に示したように、観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとの差分として、観測残差ｚｋを算出する。
カルマンゲイン付与部６８０は、式（３１）に示す相互共分散行列Ｐｘｙｋを算出する。そして、カルマンゲイン付与部６８０は、式（１８）に示したように、残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出し、式（１９）の右辺第２項（Ｋｋｚｋ）の演算を実行する。また、カルマンゲイン付与部６８０は、式（２０）に示したように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを、Ｐ−ｋからＰ＋ｋに更新する。
【数２１】
【００７４】
加算器６９０は、式（１９）に示したように、推定状態ベクトルｘ−ｋと、カルマンゲイン付与部６８０から出力される式（１９）の右辺第２項（Ｋｋｚｋ）とを加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
【００７５】
次に、状態遷移モデル部６３０における演算で用いる状態遷移モデルについて説明する。
【００７６】
本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘｋを構成する状態変数のうち、姿勢μについての状態遷移は、式（３２）により定義される。式（３２）は、時刻Ｔ＝ｋ−１における姿勢μ＋ｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μ−ｋを推定する演算を表す。ここで、μ−ｋは、時刻ｋにおける推定状態ベクトルｘ−ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（３２）の右辺の演算子Ωは、式（３３）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、演算子［ｌ×］は、式（３４）で定義される。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ＋ｋのうち角速度を表す状態変数に対応する要素をω＋ｋで表したとき、演算子Ωを構成する成分Ψ＋ｋは、式（３５）で定義される。
【数２２】
【００７７】
姿勢μは、クォータニオンで表現され、式（２）に示すように、正規化条件||μ||＝１が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘｋを更新する場合、更新後の状態ベクトルｘ＋ｋは、式（２６）に示すように、推定シグマポイントχ−ｋ（ｊ）の平均として算出されるため、（更新前の）状態ベクトルｘ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘ＋ｋ−１の要素である姿勢μ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋの要素である姿勢μ＋ｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢μについての正規化条件が満たされなくなる可能性が存在する。そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルｘｋを構成する要素のうち姿勢μｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
【００７８】
地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角φｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角φｋ−１とは各々等しい値であると仮定する。
同様に、角速度センサのオフセット推定値ｂＯは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度センサのオフセット推定値ｂＯ,Ｋと、時刻Ｔ＝ｋ−１における角速度センサのオフセット推定値ｂＯ,Ｋ−１とは等しい値であると仮定する。
【００７９】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００８０】
前述の通り、磁気センサのオフセットｃＯＦＦは、携帯機器１の部品が発する内部磁界Ｂｉの方向及び大きさをセンサ座標系ΣＳにおいて表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、磁気センサのオフセットｃＯＦＦも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、磁気センサのオフセットｃＯＦＦも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、磁気センサのオフセットｃＯＦＦが変化するタイミングや磁気センサのオフセットｃＯＦＦの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｃＯ,Ｋ−１を用いて、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｃＯ,Ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｃＯ,Ｋと、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｃＯ,Ｋ−１とは等しいと仮定する。
【００８１】
このように、本実施形態に係る状態遷移モデルでは、以下に示す式（３６）のように、状態ベクトルｘｋを構成する複数の状態変数のうち、姿勢μを表す状態変数以外は、前の時刻から変化しないという前提を置く。
【数２３】
【００８２】
なお、カルマンフィルタの演算においては、状態ベクトルｘｋは、観測残差ｚｋに基づいて更新される。従って、式（３６）に示された姿勢μ以外の値が、全く変化しないわけではない。具体的には、状態ベクトルｘｋの各要素は、加算器６９０において、観測残差ｚｋに基づいて真値に近づくように更新される。
【００８３】
次に、観測モデル部６５０における演算で用いられる観測モデルについて説明する。
【００８４】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｂＯとを用いて、式（３７）で与えられる。
【数２４】
【００８５】
また、地上座標系ΣＧにおいて地磁気Ｂｇを表すベクトルＧＢｇは式（３８）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｑの推定値γｍａｇは、磁気センサのオフセット推定値ｃＯと、式（８）で示した行列Ｂ（μ）とを用いて、式（３９）で与えられる。
【数２５】
【００８６】
また、観測モデルにおいて、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（８）で示した行列Ｂ（μ）と、式（１２）で示したベクトルＧＧＲＶとを用いて、式（４０）で与えられる。
【数２６】
【００８７】
式（３）を式（１７）の右辺第１項に代入し、式（３７）、式（３９）、及び式（４０）を用いて式（１７）の右辺第２項を表すことにより、式（１７）を、以下の式（４１）に変形することができる。このとき、観測残差ｚｋは、式（４１）により算出される。
【数２７】
【００８８】
［４． 結論］
以上に示したように、ＣＰＵ１０は、カルマンフィルタモジュール６００を呼び出すことで、ステップＳ１０８のカルマンフィルタ演算処理を実行することにより、状態ベクトルｘｋを更新する。具体的には、時刻Ｔ＝ｋにおける状態ベクトルｘｋは、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を、状態ベクトルｘｋ−１を用いて算出される観測残差ｚｋに基づいて更新することで算出される。同様に、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１は、時刻Ｔ＝ｋ−２における状態ベクトルｘｋ−２を、状態ベクトルｘｋ−２を用いて算出される観測残差ｚｋ−１に基づいて更新することで算出される。すなわち、カルマンフィルタ演算処理において、状態ベクトルｘｋは、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１の値を考慮して、更新される。
従って、初期ベクトルＩＮＩの各要素が真値から大きく乖離した値に設定された場合、カルマンフィルタ演算処理を行っても、状態ベクトルｘｋの各要素は真値に近づかなくなることがあり、また、仮に状態ベクトルｘｋの各要素が真値に近づくとしても、真値に近い値に収束するまでに長い時間を要することになる。
【００８９】
前述の通り、磁気センサのオフセットｃＯＦＦは、携帯機器１に搭載された部品が発する内部磁界Ｂｉを表すベクトルである。従って、携帯機器１に搭載された部品が大きな内部磁界Ｂｉを発する場合、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０に対して、事前に定められた固定値（例えば、原点［０，０，０］Ｔ等）を適用すると、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０が、磁気センサのオフセットｃＯＦＦから大きく乖離した値となることがある。また、姿勢μについても、予め定めた固定値を姿勢μの初期値μ０に設定する場合、真の姿勢から最大で１８０度ずれることになる。このように、初期ベクトルＩＮＩを構成する要素のうち、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０と、姿勢μの初期値μ０とは、真値から大きく乖離した値となることがある。
従って、初期ベクトルＩＮＩの各要素が不適切な（真値から乖離した）値に設定されることを防止するためには、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０と、姿勢μの初期値μ０とを、真値に近い正確な値に設定することが、特に重要となる。
【００９０】
本実施形態では、まず、中心点算出処理において、オフセットｃＯＦＦの近似値である中心点ｃＳの座標を算出し、次に、初期ベクトル生成処理において、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０に、中心点ｃＳの座標を適用することで、初期ベクトルＩＮＩを算出した。これにより、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０が、真値から大きく乖離した値に設定されることを防止した。
また、本実施形態では、安定性判定処理において、携帯機器１の動きが安定していることを判定したうえで、姿勢μの初期値μ０を算出することで、姿勢μの初期値μ０が、真値から大きく乖離した値に設定されることを防止した。
このように、本実施形態は、初期ベクトルＩＮＩの各要素を、真値に近い正確な値に設定するため、カルマンフィルタ演算処理において、正確且つ高速な状態推定が可能となった。
【００９１】
＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【００９２】
（１）変形例１
上述した実施形態では、安定性判定処理において、ローパスフィルタを用いて算出された基準加速度データａＣと、加速度データａとに基づいて安定性指標αを算出したが、本発明はこのような形態に限定されるものでは無く、ローパスフィルタを用いずに安定性指標を算出してもよい。
例えば、以下の式（４３）で示される安定性指標βが、以下の式（４２）で示される条件を満たすか否かにより、携帯機器１の動きが安定しているか否かを判定してもよい。
ここで、安定性指標βは、以下の式（４３）に示すように、時刻ｔ＝１から時刻ｔ＝Ｍまでの期間に３次元加速度センサ８０が出力する複数の加速度データａ１〜ａＭの分散を表す値である。また、式（４２）に示すように、安定性指標βが第２閾値βＴＨ以下となり十分に小さな値となる場合、加速度データａ１〜ａＭの示す値の変動が小さいため、携帯機器１の動きが安定していると看做すことができる（Ｍは、加速度データａの分散を算出するために必要な加速度データの規定測定回数を表す自然数）。
なお、式（４３）に現れる値ａｘ,ＡＶＥ、ａｙ,ＡＶＥ、及びａｚ,ＡＶＥは、複数の加速度データａ１〜ａＭの各要素の平均値であり、それぞれ、式（４４）、式（４５）、及び式（４６）により表される。
【数２８】
【００９３】
（２）変形例２
上述した実施形態及び変形例に係る状態推定装置は、ＣＰＵ１０が、ステップＳ１０１〜Ｓ１０８の各処理を実行するものであったが、本発明はこのような形態に限定されるものでは無く、ＣＰＵ１０が、ステップＳ１０１〜Ｓ１０８のうち一部の処理のみを実行するものであってもよい。
例えば、状態推定装置は、第２の報知手段を備えず、ＣＰＵ１０が、ステップＳ１０５の第２の報知を実行しないものであってもよい。
携帯機器１が、長期間継続的に動かされることは想定し難く、携帯機器１は、通常安定した状態に置かれる。そして、ステップＳ１０６の安定性判定処理で、携帯機器１が安定していることを確認したうえで、初期ベクトルＩＮＩが算出されるため、状態推定プログラム１００が第２の報知手段を備えなくても、姿勢μの初期値μ０が、真値から大きく乖離した値に設定されることを防止することができる。
また、例えば、状態推定装置は、第１の報知手段を備えず、ＣＰＵ１０が、ステップＳ１０２の第１の報知を実行しないものであってもよい。この場合、ＣＰＵ１０は、ステップＳ１０３の磁気データ蓄積処理において、蓄積された複数の磁気データｑ１〜ｑＮが、球面Ｓの中心点ｃＳを算出するために適切であるか否かを判定し、適切であると判定した場合に、処理をステップＳ１０４へと進めてもよい。具体的には、ＣＰＵ１０は、ステップＳ１０３の磁気データ蓄積処理において、複数の磁気データｑ１〜ｑＮの各々がセンサ座標系ΣＳにおいて示す座標の広がりが３次元的であるか否かを判定し、座標の広がりが３次元的であると判定した場合に、処理をステップＳ１０４へと進めてもよい。
また、例えば、状態推定装置は、第１の報知手段及び第２の報知手段を備えず、ＣＰＵ１０が、ステップＳ１０２の第１の報知、及び、ステップＳ１０５の第２の報知を実行しないものであってもよい。
【００９４】
（３）変形例３
上述した実施形態及び変形例に係る状態推定装置において、ＣＰＵ１０は、カルマンフィルタ演算処理を、シグマポイントカルマンフィルタを用いて実行したが、本発明はこのような形態に限定されるものでは無く、公知の非線形カルマンフィルタを適宜適用して演算を行ってもよい。例えば、拡張カルマンフィルタを用いて、カルマンフィルタ演算処理を実行してもよい。
【符号の説明】
【００９５】
１…携帯機器、１０…ＣＰＵ、５０…表示部、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定プログラム、２００…バッファ管理モジュール、３００…中心点算出モジュール、４００…安定性判定モジュール、５００…初期ベクトル生成モジュール、６００…カルマンフィルタモジュール、ｑ…磁気データ、ａ…加速度データ、ａＣ…基準加速度データ、α…安定性指標、αＴＨ…第１閾値、β…安定性指標、βＴＨ…第２閾値、ｘ…状態ベクトル、ｙ…観測値ベクトル、ｚ…観測残差、ｃＳ…中心点、ｃＯＦＦ…磁気センサのオフセット、ｃＯ…磁気センサのオフセット推定値、Ｂｇ…地磁気、Ｂｉ…内部磁界。
【技術分野】
【０００１】
本発明は、状態推定装置に関する。
【背景技術】
【０００２】
地磁気の向きや、物体の姿勢等を算出する場合、地磁気センサからの出力結果のみに基づいて算出するよりも、地磁気センサの他に加速度センサや角速度センサ等の異種の物理量を測定する複数のセンサの出力結果を統合して算出した方が、短時間で正確な値を求めることができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、非線形カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合し、姿勢を推定する方法が開示されている。
【０００４】
一般的に、非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、非線形カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量を推定する値（状態変数）を要素とする状態ベクトルを逐次更新することにより、状態ベクトルの各要素の示す値を、実際の物理量（真値）に近づける演算を行う。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００６】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかし、非線形カルマンフィルタは、状態ベクトルと、状態ベクトルに基づいて算出される観測残差とを用いて、状態ベクトルを逐次更新する演算であるため、状態ベクトルの初期値の要素が真値から乖離した値に設定された場合、状態ベクトルの各要素が真値に近い値に収束するまでに長時間を要することがあり、更には、状態ベクトルの各要素が真値とは異なる不正確な値に収束することも起こり得る。
【０００８】
本発明は、上述した点に鑑みてなされたものであり、真値に近い値を要素とする状態ベクトルの初期値を算出して非線形カルマンフィルタの演算を行うことで、地磁気の向きや物体の姿勢を高速且つ正確に推定することを解決課題とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決するため、本発明に係る状態推定装置は、磁界を発生させる部品を備えた機器に組み込まれる状態推定装置であって、互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサと、加速度を直交する３軸で検出して、３軸の座標系において表現される３次元のベクトルデータとして加速度データを順次出力する３次元加速度センサと、前記３次元磁気センサから順次出力される磁気データを蓄積する蓄積手段と、前記蓄積手段に蓄積された複数の前記磁気データに基づいて、前記部品の発する磁界を表す前記３次元磁気センサのオフセットの近似値を示す中心点の３軸の座標を算出する中心点算出手段と、複数の前記加速度データに基づいて、前記機器の動きが安定しているか否かを判定する安定性判定手段と、前記安定性判定手段の判定結果が肯定である場合、前記中心点の３軸の座標、前記磁気データ、及び、前記加速度データに基づいて、前記オフセットの推定値及び前記機器の姿勢を推定するベクトルを要素として含む初期ベクトルを生成する初期ベクトル生成手段と、前記初期ベクトルを初期値とする状態ベクトルを、前記３次元磁気センサ及び前記３次元加速度センサを含む前記機器に組み込まれた複数のセンサの出力を要素とする観測値ベクトルを用いて更新するカルマンフィルタ演算手段と、を備えることを特徴とする。
【００１０】
カルマンフィルタの演算において、状態ベクトルの初期値（初期ベクトル）の各要素が真値から乖離した値である場合、状態ベクトルの各要素が真値に近い値に収束するまでに長い時間を必要とする。
この発明によれば、中心点算出手段が算出する中心点の座標に基づいて定められる３次元磁気センサのオフセット推定値と、機器の動きが安定していると判定されたときの磁気データ及び加速度データとに基づいて算出される姿勢の推定値とを、初期ベクトルの要素とする。
中心点算出手段が算出する中心点は、３次元磁気センサのオフセットの近似値を示す座標なので、オフセット推定値を真値に近い値に設定することができる。そして、３次元磁気センサのオフセット推定値及び磁気データを用いて、正確な地磁気の向きを得ることができる。
また、機器の動きが安定しているときの加速度データより重力加速度の向きを得ることができるため、重力加速度の向き及び地磁気の向きに基づいて、真値に近い姿勢の推定値を算出することができる。
すなわち、この発明によれば、初期ベクトルの各要素を真値に近い値に設定することができ、カルマンフィルタの演算において状態ベクトルの各要素を高速に真値に近い値に収束させることが可能となる。
【００１１】
また、上述した状態推定装置において、前記中心点算出手段が前記中心点の算出に用いる複数の前記磁気データの取得を開始する前に、利用者に前記機器の姿勢を変化させるように促す第１の報知手段を、更に備えることが好ましい。
【００１２】
３次元磁気センサは、地磁気と、部品の発する磁界とを検出する。３次元磁気センサの姿勢を変化させる場合、地磁気は、大きさは一定であるが向きが変化する磁界として検出される。一方、部品の発する磁界は、３次元磁気センサの姿勢をどのように変化させても、一定の向き及び大ききを有する磁界として検出される。従って、３次元磁気センサの姿勢を３次元的に変化させつつ検出された複数の磁気データが、３次元磁気センサに固定された座標系において示す座標は、部品の発する磁界を表すベクトルの示す座標を中心点とし地磁気の大きさを半径とする球面近傍に分布する。そして、当該球面の中心点の座標が、３次元磁気センサのオフセットの近似値を表す。
よって、複数の磁気データに基づいてオフセットの近似値を算出するためには、複数の磁気データの示す座標を近傍に有する球面を一意に特定することが必要であり、そのためには、複数の磁気データの示す座標が、２次元的に分布せず、３次元的な広がりを有して分布していることが必要である。
この発明によれば、利用者に機器の姿勢を変化させるように促す第１の報知手段を備えるため、複数の磁気データを取得する際に利用者が機器の姿勢を３次元的に変化させることが期待される。機器の姿勢を３次元的に変化させつつ取得した複数の磁気データの示す座標は、３次元的な広がりを有して分布する。従って、複数の磁気データの示す座標を近傍に有する球面を一意に特定することが可能となり、３次元磁気センサのオフセットの近似値を示す中心点の座標を算出することが可能となる。
【００１３】
また、上述した状態推定装置において、前記中心点算出手段が前記中心点を算出した後に、利用者に前記機器の姿勢を変化させないように促す第２の報知手段を、更に備えることが好ましい。
【００１４】
この発明によれば、利用者に機器の姿勢を変化させないように促す第２の報知手段を備えるため、利用者が機器を動かさず安定的な状態に置くことが期待される。
機器の動きが安定している場合、加速度データより重力加速度の向きを得ることができるため、重力加速度の向き及び地磁気の向きに基づいて、真値に近い姿勢の推定値を得ることが可能となる。
【００１５】
また、上述した状態推定装置において、前記安定性判定手段は、複数の前記加速度データに基づいて基準加速度データを算出するローパスフィルタを備え、前記加速度データの示す座標と前記基準加速度データの示す座標との誤差を表す安定性指標を、所定個の前記加速度データの各々について算出し、前記安定性指標の全てが第１閾値以下である場合、前記機器の動きが安定していると判定することを特徴とすることが好ましい。
【００１６】
この発明によれば、加速度データをローパスフィルタに通過させることで得られる基準加速度データと、所定個の加速度データとの誤差を表す所定個の安定性指標を算出することで、所定個の加速度データがほぼ一定の値を示すものであるか否かを判定する。
複数の加速度データの示す値の変動が小さくほぼ一定の値を示すとき、例えば機器が等加速度運動をする等の特殊な場合を除き、加速度データは重力加速度を示すものと看做すことができる。従って、本発明によれば、加速度データから重力加速度の向きを算出することができるか否かを判定し、判定結果が肯定である場合には、加速度データから算出された重力加速度の向きと地磁気の向きとに基づいて、真値に近い姿勢の推定値を得ることが可能となる。
【００１７】
また、上述した状態推定装置において、前記安定性判定手段は、所定個の前記加速度データの示す３軸の座標の分散を、安定性指標として算出し、前記安定性指標が、第２閾値以下である場合、前記機器の動きが安定していると判定することを特徴とすることが好ましい。
【００１８】
この発明によれば、所定個の前記加速度データの示す座標の分散を表す安定性指標を算出し、所定個の加速度データがほぼ一定の値を示すものであるか否かを判定する。従って、本発明によれば、加速度データから重力加速度の向きを算出することができるか否かを判定し、判定結果が肯定である場合には、加速度データから算出された重力加速度の向きと地磁気の向きとに基づいて、真値に近い姿勢の推定値を得ることが可能となる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係る３次元磁気センサが検出する磁界の概要を説明する概念図である。
【図４】本発明の実施形態に係る状態推定プログラムの処理の流れを示すフローチャートである。
【図５】本発明の実施形態に係る安定性判定モジュールを実行することで実現される機能を表す機能ブロック図である。
【図６】本発明の実施形態に係るカルマンフィルタモジュールを実行することで実現される機能を表す機能ブロック図である。
【発明を実施するための形態】
【００２０】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００２１】
［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１は、画面の姿勢に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢と、携帯機器１から見た地磁気Ｂｇの向きとを推定することによって実現される。
【００２２】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（蓄積部）２０、状態推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。
また、携帯機器１は、地磁気Ｂｇ等の磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００２３】
表示部５０は、ＣＰＵ１０が状態推定プログラム１００を実行することにより推定したシステムの状態に基づいて算出された地磁気Ｂｇの向きや携帯機器１の姿勢μ等を、矢印等の画像により表示する。
ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。
【００２４】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｑを出力する。この磁気データｑは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。
【００２５】
ところで、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力するベクトルデータ（磁気データｑ）は、地磁気Ｂｇを表すベクトルの他に、携帯機器１に搭載された部品が発する磁界（内部磁界Ｂｉ）を表すベクトルも含む値となる。従って、地磁気Ｂｇの向きを正確に知るためには、３次元磁気センサが出力するベクトルデータ（磁気データｑ）から、携帯機器１の部品が発する内部磁界Ｂｉを表すベクトルを取り除く補正処理が必要となる。
【００２６】
補正処理を説明するための前提として、まず、地磁気Ｂｇ及び内部磁界Ｂｉの性質について説明する。
図３（Ａ）に示すように、地磁気Ｂｇは、磁極北に向かう水平成分と伏角方向の鉛直成分を有する磁界であり、地上に固定された座標系である地上座標系ΣＧにおいて、一定の向き及び大きさを有するベクトルＧＢｇとして表される。一方、内部磁界Ｂｉは、携帯機器１の部品が発する磁界であり、携帯機器１に対して一定の方向を向いた、一定の大きさの磁界である。つまり、内部磁界Ｂｉは、地上座標系ΣＧにおいて、携帯機器１の姿勢μの変化に連動して向きを変える、一定の大きさのベクトルＧＢｉ（μ）として表される。
【００２７】
ここで、携帯機器１（厳密には３次元磁気センサ７０）に固定された座標系であるセンサ座標系ΣＳを導入し、磁気データｑを、センサ座標系ΣＳ上の座標として表現する。より具体的には、磁気データｑが示すセンサ座標系ΣＳ上の座標のうち、ｘ軸成分は、Ｘ軸磁気センサ７１からの出力値であり、ｙ軸成分は、Ｙ軸磁気センサ７２からの出力値であり、ｚ軸成分は、Ｚ軸磁気センサ７３からの出力値である。
このようなセンサ座標系ΣＳから見た場合、地磁気Ｂｇは、携帯機器１の姿勢μに連動して向きを変える、一定の大きさのベクトルＳＢｇ（μ）として表され、内部磁界Ｂｉは、一定の向き及び大きさを有するベクトルＳＢｉとして表される。
従って、地上座標系ΣＧから見た携帯機器１の姿勢μをμ１〜μＮと変化させつつ取得した複数の磁気データｑ１〜ｑＮは、センサ座標系ΣＳにおいて、図３（Ｂ）に示すように、内部磁界Ｂｉを表すベクトルＳＢｉの示す座標を中心とし、地磁気Ｂｇの大きさを半径とする、球面Ｓ上に分布する。そして、磁気データｑの示す座標から、内部磁界Ｂｉを表すベクトルＳＢｉの示す座標を減算することにより、センサ座標系ΣＳにおける地磁気Ｂｇの向きが特定される。
このように、検出対象である地磁気Ｂｇの正確な向きを特定するために、補正処理において、３次元磁気センサ７０から出力される磁気データｑから取り除かれる内部磁界Ｂｉを表すベクトルを磁気センサのオフセットｃＯＦＦと呼ぶ。
なお、複数の磁気データｑ１〜ｑＮの示す座標は、３次元磁気センサ７０の測定誤差を考慮すると、球面Ｓ上には分布せず、球面Ｓの近傍に分布する。従って、複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面Ｓの中心点ｃＳの座標は、内部磁界Ｂｉを表すベクトルＳＢｉの示す座標の近似値（すなわち、オフセットｃＯＦＦの近似値）となる。
【００２８】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００２９】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示すベクトルデータである。
【００３０】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、携帯機器１の姿勢や磁気センサのオフセット等のシステムの状態を表す複数の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することにより、状態推定装置として機能する。
【００３１】
状態推定プログラム１００は、バッファ管理モジュール２００、中心点算出モジュール３００、安定性判定モジュール４００、初期ベクトル生成モジュール５００、及びカルマンフィルタモジュール６００等のモジュール群で構成される。
バッファ管理モジュール２００は、３次元磁気センサ７０から順次出力される磁気データｑをバッファに蓄積する。これら複数の磁気データの蓄積手段としては、ＲＡＭ２０を用いる。
中心点算出モジュール３００は、ＲＡＭ２０に蓄積された複数の磁気データｑ１〜ｑＮに基づいて、センサ座標系ΣＳにおいて複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面Ｓの中心点ｃＳの座標を算出する。
安定性判定モジュール４００は、３次元加速度センサ８０から出力される加速度データａに基づいて、携帯機器１の動きが安定しているか否かを判定する。
初期ベクトル生成モジュール５００は、安定性判定モジュール４００による判定結果が肯定である場合に、初期ベクトルＩＮＩを生成する。
カルマンフィルタモジュール６００は、初期ベクトルＩＮＩを初期値とする状態ベクトルｘを、観測値ベクトルｙに基づいて更新する演算（非線形カルマンフィルタの演算）を実行することで、システムの状態を推定する。
【００３２】
ここで、状態ベクトルｘとは、複数の状態変数を要素とするベクトルである。状態ベクトルｘの要素である複数の状態変数の各々は、非線形カルマンフィルタが推定するシステムの状態を示す複数の物理量を表す。すなわち、状態ベクトルｘは、非線形カルマンフィルタが推定するシステムの状態を表すベクトルである。
本実施形態は、状態ベクトルｘの要素（状態変数）として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、角速度センサ９１〜９３のオフセット推定値ｂＯ、及び磁気センサ７１〜７３のオフセット推定値ｃＯを採用し、これらを推定の対象とする。
時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。
【数１】
【００３３】
ここで、地磁気の強さｒはスカラーであり、地磁気の伏角φはスカラーであり、角速度ωは３次元ベクトルであり、角速度センサのオフセット推定値ｂＯは３次元ベクトルであり、磁気センサのオフセット推定値ｃＯは３次元ベクトルである。
また、姿勢μは、クォータニオンを用いて表現される。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。例えば、センサ座標系ΣＳの各軸と、地上座標系ΣＧの各軸とが一致する姿勢μを基準姿勢と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢μは、クォータニオンを用いて、式（２）で与えられる。
【数２】
【００３４】
観測値ベクトルｙとは、３次元磁気センサ７０から出力される磁気データｑ、３次元加速度センサ８０から出力される加速度データａ、及び、３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルである。
時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数３】
【００３５】
ところで、非線形カルマンフィルタの演算は、状態ベクトルと観測値ベクトルとに基づいて算出される観測残差を用いて、状態ベクトルを周期的に更新する演算であるため、状態ベクトルの初期値の各要素（各状態変数）の示す値が、真値から乖離した値である場合、状態ベクトルの各要素を真値に近い値に収束させるのに長い時間を要することがある。
また、非線形カルマンフィルタの演算は、非線形演算であるため、状態ベクトルの初期値の各要素が真値から大きく乖離した値である場合、状態ベクトルの更新を繰り返しても、状態ベクトルの各要素が真値（または、真値に近い値）に収束せずに、真値とは異なる値に収束することがある。
従って、非線形カルマンフィルタの演算において、システムの状態を正確且つ高速に推定するためには、状態ベクトルｘの初期値（つまり初期ベクトルＩＮＩ）の各要素を、真値に近い値に設定する必要がある。
ここで、「真値」とは、状態ベクトルｘｋの各要素に対応する実際の物理量を表す値である。また、「状態ベクトルの真値」とは、状態ベクトルｘｋの各要素を真値としたときのベクトルである。
【００３６】
［２． 状態推定装置の処理フロー］
図４は、ＣＰＵ１０が実行する状態推定プログラム１００の処理の流れを表すフローチャートである。以下では、ＣＰＵ１０が状態推定プログラム１００を実行したときの処理の流れを、図４に加えて、当該フローチャートの各ステップが実行された時に実現される機能の詳細を説明する図５乃至図６を参照しつつ、説明する。
【００３７】
まず、ステップＳ１０１において、ＣＰＵ１０は、初期化処理を実行する。具体的には、ＣＰＵ１０は、バッファ管理モジュール２００を呼び出すことで、ＲＡＭ２０に記憶した磁気データｑの全部を廃棄する。なお、本実施形態では、ＣＰＵ１０は、磁気データｑの全部を廃棄するが、ＲＡＭ２０に蓄積されたデータのうち古い方から一定割合のデータのみを廃棄しても良い。
【００３８】
ステップＳ１０２において、ＣＰＵ１０は、第１の報知を行う。具体的には、ＣＰＵ１０は、表示部５０に画像や動画等を表示させることで、携帯機器１の利用者に対して、携帯機器１の姿勢を３次元的に変化させるように指示する。すなわち、ＣＰＵ１０は、ステップＳ１０２の第１の報知を行うことにより、第１の報知手段として機能する。
なお、本実施形態では、ＣＰＵ１０は、第１の報知を、表示部５０を用いて画像や動画等を表示することにより行うことを想定しているが、音声等を用いて行ってもよい。
【００３９】
ステップＳ１０３において、ＣＰＵ１０は、磁気データ蓄積処理を実行する。具体的には、ＣＰＵ１０は、バッファ管理モジュール２００を呼び出すことにより、３次元磁気センサ７０から順次出力されるＮ個の磁気データｑ１〜ｑＮを、ＲＡＭ２０に格納する（ここで、Ｎは、後述する中心点算出処理において、精度よく中心点ｃＳを導出するために必要な磁気データの規定測定回数を表す５以上の自然数）。すなわち、ＣＰＵ１０は、ステップＳ１０３の磁気データ蓄積処理を実行することにより、蓄積手段として機能する。
【００４０】
次に、ステップＳ１０４において、ＣＰＵ１０は、中心点算出処理を実行する。具体的には、ＣＰＵ１０は、中心点算出モジュール３００を呼び出すことにより、ＲＡＭ２０に蓄積された複数の磁気データｑ１〜ｑＮの示す座標を近傍に有する球面Ｓの中心点ｃＳの座標を算出する。すなわち、ＣＰＵ１０は、ステップＳ１０４の中心点算出処理を実行することにより、中心点算出手段として機能する。
なお、前述の通り、中心点ｃＳの座標を示すベクトルは、オフセットｃＯＦＦの近似値である。中心点ｃＳの座標の算出は、公知の方法を適宜適用して行えばよい。例えば、本件出願人が、特開２００７−２４０２７０号公報で開示している方法等を適用してもよい。
【００４１】
次に、ステップＳ１０５において、ＣＰＵ１０は、第２の報知を行う。具体的には、ＣＰＵ１０は、表示部５０に画像や動画等を表示させることで、携帯機器１の利用者に対して、携帯機器１を動かさないように指示する。すなわち、ＣＰＵ１０は、ステップＳ１０５の第２の報知を行うことにより、第２の報知手段として機能する。
なお、本実施形態では、ＣＰＵ１０は、第２の報知を、表示部５０を用いて画像や動画等を表示することにより行うことを想定しているが、音声等を用いて行ってもよい。
【００４２】
その後、ステップＳ１０６において、ＣＰＵ１０は、安定性判定処理を実行する。具体的には、ＣＰＵ１０は、安定性判定モジュール４００を呼び出すことにより、３次元加速度センサ８０から出力される加速度データａに基づいて、携帯機器１の動きが安定しているか否かを判定する。すなわち、ＣＰＵ１０は、ステップＳ１０６の安定性判定処理を実行することにより、安定性判定手段として機能する。
【００４３】
図５は、ＣＰＵ１０が、ステップＳ１０６の安定性判定処理を実行することで実現される安定性判定手段の機能を表す機能ブロック図である。
図５に示すように、安定性判定手段には、平滑化手段４１０、安定性指標算出手段４２０、及び条件判定手段４３０が含まれる。
【００４４】
平滑化手段４１０は、３次元加速度センサ８０から順次出力される加速度データａの各成分を、ローパスフィルタを通して平滑化することで、基準加速度データａＣを算出する。
ここで、ローパスフィルタには、公知のフィルタを適宜適用すればよい。例えば、ＦＩＲ（Finite impulse response）フィルタ、または、ＩＩＲ（Infinite impulse response）フィルタを用いてもよい。
なお、本実施形態では、平滑化手段４１０への加速度データａの入力の開始は、ＣＰＵ１０がステップＳ１０６の安定性判定処理を開始するタイミングであるが、それ以外のタイミングで入力を開始してもよい。例えば、ＣＰＵ１０がステップＳ１０１の初期化処理を開始するタイミングで、平滑化手段４１０への加速度データａの入力を開始してもよい。この場合、ＣＰＵ１０は、ステップＳ１０１の初期化処理を開始するタイミングで、ステップＳ１０６の安定性判定処理を開始して、平滑化手段４１０を機能させる。
【００４５】
次に、安定性指標算出手段４２０は、３次元加速度センサ８０から出力される加速度データａと、平滑化手段４１０から出力される基準加速度データａＣとに基づいて、安定性指標αを算出する。
時刻ｔにおいて算出される安定性指標αｔは、以下の式（４ａ）で表される。なお、式（４ｂ）に示される加速度データａtは、時刻ｔにおける加速度データであり、式（４ｃ）に示される基準加速度データａＣtは、時刻ｔにおける基準加速度データである。
【数４】
【００４６】
安定性指標算出手段４２０は、時刻ｔ＝１から時刻ｔ＝Mまでの期間に、Ｍ個の安定性指標αｔ（ｔ＝１、…、Ｍ）を算出する。
ここで、Ｍは、１以上の任意の自然数である。なお、携帯機器１の動きが安定しているか否かを、より厳密に判定することが必要な場合には、Ｍを、できるだけ大きな自然数に設定すればよい。
【００４７】
条件判定手段４３０は、以下の式（５）に示すように、Ｍ個の安定性指標αｔ（ｔ＝１、…、M）の全てが、第１閾値αＴＨ以下であるか否かを判定する。
【数５】
【００４８】
このように、ＣＰＵ１０は、ステップＳ１０６の安定性判定処理を実行することにより、まず、Ｍ個の安定性指標αｔを算出し、次に、Ｍ個の安定性指標αｔの全てが第１閾値αＴＨ以下であるか否かを判定する。そして、ＣＰＵ１０は、安定性判定処理における判定結果が肯定である場合、処理をステップＳ１０７に進める。一方、ＣＰＵ１０は、安定性判定処理における判定結果が否定である場合、処理をステップＳ１０５に戻す。
【００４９】
次に、ステップＳ１０７において、ＣＰＵ１０は、初期ベクトル生成処理を実行する。具体的には、ＣＰＵ１０は、初期ベクトル生成モジュール５００を呼び出すことにより、時刻ｔ＝Ｍ（つまり、条件判定手段４３０の判定結果が肯定となったタイミング）に３次元磁気センサ７０から出力される磁気データｑ、時刻ｔ＝Ｍに３次元加速度センサ８０から出力される加速度データａ、及び、ステップＳ１０４において算出される中心点ｃＳの座標に基づいて、初期ベクトルＩＮＩを生成する。すなわち、ＣＰＵ１０は、ステップＳ１０７の初期ベクトル生成処理を実行することにより、初期ベクトル生成手段として機能する。
前述の通り、初期ベクトルＩＮＩは、状態ベクトルｘｋの初期値、つまり時刻Ｔ＝０における状態ベクトルｘ０であり、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｂＯの初期値ｂＯ,０、及び磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０を要素として含む、以下の式（６）により示されるベクトルである。なお、以下では、ＣＰＵ１０は、初期ベクトル生成処理を実行することにより初期ベクトルＩＮＩを生成するタイミングを、時刻Ｔ＝０と表現する。時刻Ｔ＝０は、時刻ｔ＝Ｍと等しい時刻である。
【数６】
【００５０】
以下、初期ベクトルＩＮＩの生成方法を、具体的に説明する。
まず、時刻Ｔ＝０に３次元磁気センサ７０から出力される磁気データｑ０は、地上座標系ΣＧから地磁気Ｂｇを表したベクトルＧＢｇと、３次元磁気センサ７０のオフセット推定値ｃＯの初期値ｃＯ,０とを用いて、以下の式（７）で表される。
ここで、行列Ｂ（μ）は、携帯機器１が姿勢μである場合に、地上座標系ΣＧにおいて表現されたベクトルを、センサ座標系ΣＳで表現するための座標変換行列であり、以下の式（８）で表される。また、時刻Ｔ＝０における地磁気Ｂｇを地上座標系ΣＧから表したベクトルＧＢｇは、地磁気の強さｒの初期値ｒ０と、地磁気の伏角φの初期値φ０とを用いて、以下の式（９）で表される。なお、本実施形態では、地上座標系ΣＧを、ｙ軸が磁極北を向き、ｚ軸が鉛直上を向くように定める。
また、３次元磁気センサ７０のオフセット推定値ｃＯの初期値ｃＯ,０には、以下の式（１０）に示すように、中心点ｃＳの座標が設定される。
【数７】
【００５１】
一方、時刻Ｔ＝０に３次元加速度センサ８０から出力される加速度データａ０は、携帯機器１の動きが安定していれば（例えば、携帯機器１が静止状態または等速直線運動状態にあれば）、以下の式（１１）で表される。なお、ベクトルＧＧＲＶは、地上座標系ΣＧにおいて重力加速度を表したベクトルを、重力加速度の大きさで正規化した３次元のベクトルであり、以下の式（１２）で表される。
【数８】
【００５２】
センサ座標系ΣＳにおいて地磁気Ｂｇの向き及び大きさを表すベクトルＳＢｇは、磁気データｑと３次元磁気センサ７０のオフセット推定値ｃＯとを用いて、以下の式（１３ａ）によって表すことができる。
携帯機器１の姿勢は、以下の式（１３ａ）に示すベクトルＳＢｇと、加速度データａとを用いて、以下の式（１３ｂ）に示す３行３列の行列Ａとして表現することができる。そして、行列Ａにより表された携帯機器１の姿勢を、クォータニオンを用いて表現し直すことにより、携帯機器１の姿勢μを算出することができる。つまり、姿勢μの初期値μ０は、以下の式（１３ａ）及び式（１３ｂ）に対して、時刻Ｔ＝０に３次元加速度センサ８０から出力される加速度データａ０と、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０とを適用することにより算出される行列Ａに基づいて、求めることができる。
なお、３行３列の行列Ａからクォータニオンへの変換は、公知の方法を適宜用いて行えば良い。行列Ａは、式（８）に示した行列Ｂ（μ）の逆行列であるので、行列Ａの各成分と行列Ｂ（μ）の逆行列の各成分とを比較することで、クォータニオンを用いて表される姿勢μの各要素を計算すればよい。例えば、以下の公知文献に記載された方法を用いて、行列Ａから姿勢μを算出することもできる。
Malcolm D. Shuster and Gregory A. Natanson, “Quaternion Computation from a Geometric Point of View", The Journal of the Astronautical Sciences, Vol. 41, No. 4, October-December 1993, pp. 545-556.
【数９】
【００５３】
地磁気の強さｒの初期値ｒ０は、以下の式（１４ａ）に基づいて算出することができる。具体的には、地磁気の強さｒの初期値ｒ０は、まず、式（１３ａ）に対して磁気データｑと磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０とを代入してベクトルＳＢｇを求め、次に、ベクトルＳＢｇを以下の式（１４ａ）に代入することで算出される。
また、地磁気の伏角φの初期値φ０は、ベクトルＳＢｇの各要素を以下の式（１４ｂ）で表した場合、以下の式（１４ｃ）及び式（１４ｄ）に基づいて算出することができる。
【数１０】
【００５４】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。また、角速度センサのオフセット推定値ｂＯの初期値ｂＯ,０は、通常「０」近辺に調整されているため、「０」に設定する。
【００５５】
このように、ＣＰＵ１０は、ステップＳ１０７の初期ベクトル生成処理を実行することにより、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、角速度センサのオフセット推定値ｂＯの初期値ｂＯ,０、及び磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０を要素とするベクトルである初期値ＩＮＩを生成し、これを出力する。
【００５６】
次に、ステップＳ１０８において、ＣＰＵ１０は、カルマンフィルタ演算処理を実行する。具体的には、ＣＰＵ１０は、カルマンフィルタモジュール６００を呼び出すことにより、非線形カルマンフィルタの演算を行い、システムの状態を推定する。すなわち、ＣＰＵ１０は、ステップＳ１０８のカルマンフィルタ演算処理を実行することにより、カルマンフィルタ演算手段として機能する。
以下、非線形カルマンフィルタの演算の詳細な内容について説明する。
【００５７】
［３． 非線形カルマンフィルタによる演算］
一般的にカルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘ＋ｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定する。この推定値を、推定状態ベクトルｘ−ｋと称する。そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、推定状態ベクトルｘ−ｋから観測値ベクトルｙｋを推定する。この推定値を、推定観測値ベクトルｙ−ｋと称する。
次に、カルマンフィルタは、推定観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｚｋとして算出し、観測残差ｚｋに基づいてカルマンゲインＫｋを算出する。さらに、カルマンフィルタは、観測残差ｚｋ、カルマンゲインＫｋ、及び、推定状態ベクトルｘ−ｋを用いて、状態ベクトルｘ＋ｋ−１を更新することにより、更新後の状態ベクトルｘ＋ｋを算出する。
以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタの演算により、状態ベクトルｘｋの各要素を実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける。
【００５８】
なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。シグマポイントカルマンフィルタは、まず、状態ベクトルｘ＋ｋ−１の周囲に複数のシグマポイントχ＋ｋ−１を設定し、これらの複数のシグマポイントχ＋ｋ−１を状態遷移モデルに適用することで、単位時間経過後の複数のシグマポイントを推定する。この推定された複数のシグマポイントを、推定シグマポイントχ−ｋと称する。次に、シグマポイントカルマンフィルタは、複数の推定シグマポイントχ−ｋの平均を算出することにより、推定状態ベクトルｘ−ｋを求める。従って、推定観測値ベクトルｙ−ｋは、厳密には、推定状態ベクトルｘ−ｋの周辺に存在する複数の推定シグマポイントχ−ｋに基づいて算出される。
【００５９】
本実施形態における、非線形カルマンフィルタの状態遷移モデルは以下に示す式（１５）で与えられ、観測モデルは以下に示す式（１６）で与えられる。なお、本実施形態では、式（１５）に現れる関数ｆ及び式（１６）に現れる関数ｈは、非線形の関数である。
【数１１】
【００６０】
ここで、状態ベクトルｘｋはｄｉｍ（ｘ）次元のベクトルであり、観測値ベクトルｙｋはｄｉｍ（ｙ）次元のベクトルである。なお、本実施形態では、ｄｉｍ（ｘ）＝１５であり、ｄｉｍ（ｙ）＝９である。また、式（１５）に現れるプロセスノイズｗｋ及び式（１６）に現れる観測ノイズｖｋは０を中心とするガウスノイズである。
式（１５）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗｋ−１とを加算することにより推定されることを示している。
また、式（１６）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗｋの共分散をＱｋ、観測ノイズｖｋの共分散をＲｋと表す。
【００６１】
時刻Ｔ＝ｋにおける観測残差ｚｋは、実際の観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（１７）で表される。カルマンフィルタは、以下の式（１９）に示すように、観測残差ｚｋ、推定状態ベクトルｘ−ｋ、及び、式（１８）に示すカルマンゲインＫｋを用いて、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタは、以下の式（２０）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
ここで、Ｐ−ｋは、推定状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。また、Ｐｙｙｋは、観測残差ｚｋの共分散であり、Ｐｘｙｋは、推定状態ベクトルｘ−ｋと、推定観測値ベクトルｙ−ｋとの相互共分散行列である。
【数１２】
【００６２】
なお、推定観測値ベクトルｙ−ｋは、式（１６）に示すように、状態遷移モデルを用いて算出された推定状態ベクトルｘ−ｋ（厳密には、推定シグマポイントχ−ｋ）を、観測モデルに適用することで算出される値である。よって、推定観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｚｋは、推定状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
式（１９）に示すように、観測残差ｚｋを用いて、推定状態ベクトルｘ−ｋを、更新後の状態ベクトルｘ＋ｋへと更新することにより、状態ベクトルｘｋの各要素を真値に近い値へと近付けることができる。
【００６３】
本実施形態に係る状態推定装置は、アンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて、カルマンフィルタ演算処理を実行する。
図６は、ＣＰＵ１０がカルマンフィルタモジュール６００を呼び出すことで実現されるカルマンフィルタ演算手段の機能を表す機能ブロック図である。
【００６４】
図６に示すように、遅延部６１０は、加算器６９０から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部６２０に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部６１０は、初期ベクトル生成モジュール５００が出力する初期値ＩＮＩを、状態ベクトルｘ＋ｋ−１に適用する。
【００６５】
シグマポイント生成部６２０は、「ｄｉｍ（ｘ）」行「ｄｉｍ（ｘ）」列の共分散行列Ｐ＋ｋ−１及びＱｋ−１を用いて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントを生成する。
具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマポイントの広がりを表すスケーリングパラメータλを用いて、式（２１）及び式（２２）に示すベクトルσｋを定義する。
【数１３】
【００６６】
このとき、シグマポイント生成部６２０は、ベクトルσｋ−１と状態ベクトルｘ＋ｋ−１とに基づいて、式（２３）及び式（２４）で示される「２ｄｉｍ（ｘ）＋１」個のシグマポイントχｋ−１を生成する。
【数１４】
【００６７】
状態遷移モデル部６３０は、式（２５）に示すように、時刻Ｔ＝ｋ−１における「２ｄｉｍ（ｘ）＋１」個のシグマポイントχｋ−１（ｊ）の各々を、状態遷移モデルに適用することにより、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（ｊ）を算出する。
【数１５】
【００６８】
次に、平均算出部６４０は、式（２６）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋの平均値を計算することで、推定状態ベクトルｘ−ｋを算出する。
【数１６】
【００６９】
また、平均算出部６４０は、式（２７）に示す、推定状態ベクトルｘ−ｋの誤差の共分散Ｐ−ｋを算出する。
【数１７】
【００７０】
一方、観測モデル部６５０は、式（２８）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（ｊ）の各々を、観測モデルに適用することにより、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（ｊ）を算出する。
【数１８】
【００７１】
そして、平均処理部６６０は、式（２９）に示すように、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（ｊ）の平均を演算することにより、推定観測値ベクトルｙ−ｋを算出する。
【数１９】
【００７２】
次に、平均処理部６６０は、式（３０）に示す観測残差の共分散Ｐｙｙｋを算出する。
【数２０】
【００７３】
減算器６７０は、式（１７）に示したように、観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとの差分として、観測残差ｚｋを算出する。
カルマンゲイン付与部６８０は、式（３１）に示す相互共分散行列Ｐｘｙｋを算出する。そして、カルマンゲイン付与部６８０は、式（１８）に示したように、残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出し、式（１９）の右辺第２項（Ｋｋｚｋ）の演算を実行する。また、カルマンゲイン付与部６８０は、式（２０）に示したように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを、Ｐ−ｋからＰ＋ｋに更新する。
【数２１】
【００７４】
加算器６９０は、式（１９）に示したように、推定状態ベクトルｘ−ｋと、カルマンゲイン付与部６８０から出力される式（１９）の右辺第２項（Ｋｋｚｋ）とを加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
【００７５】
次に、状態遷移モデル部６３０における演算で用いる状態遷移モデルについて説明する。
【００７６】
本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘｋを構成する状態変数のうち、姿勢μについての状態遷移は、式（３２）により定義される。式（３２）は、時刻Ｔ＝ｋ−１における姿勢μ＋ｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μ−ｋを推定する演算を表す。ここで、μ−ｋは、時刻ｋにおける推定状態ベクトルｘ−ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（３２）の右辺の演算子Ωは、式（３３）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、演算子［ｌ×］は、式（３４）で定義される。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ＋ｋのうち角速度を表す状態変数に対応する要素をω＋ｋで表したとき、演算子Ωを構成する成分Ψ＋ｋは、式（３５）で定義される。
【数２２】
【００７７】
姿勢μは、クォータニオンで表現され、式（２）に示すように、正規化条件||μ||＝１が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘｋを更新する場合、更新後の状態ベクトルｘ＋ｋは、式（２６）に示すように、推定シグマポイントχ−ｋ（ｊ）の平均として算出されるため、（更新前の）状態ベクトルｘ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘ＋ｋ−１の要素である姿勢μ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋの要素である姿勢μ＋ｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢μについての正規化条件が満たされなくなる可能性が存在する。そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルｘｋを構成する要素のうち姿勢μｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
【００７８】
地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角φｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角φｋ−１とは各々等しい値であると仮定する。
同様に、角速度センサのオフセット推定値ｂＯは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度センサのオフセット推定値ｂＯ,Ｋと、時刻Ｔ＝ｋ−１における角速度センサのオフセット推定値ｂＯ,Ｋ−１とは等しい値であると仮定する。
【００７９】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００８０】
前述の通り、磁気センサのオフセットｃＯＦＦは、携帯機器１の部品が発する内部磁界Ｂｉの方向及び大きさをセンサ座標系ΣＳにおいて表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、磁気センサのオフセットｃＯＦＦも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、磁気センサのオフセットｃＯＦＦも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、磁気センサのオフセットｃＯＦＦが変化するタイミングや磁気センサのオフセットｃＯＦＦの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｃＯ,Ｋ−１を用いて、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｃＯ,Ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける磁気センサのオフセット推定値ｃＯ,Ｋと、時刻Ｔ＝ｋ−１における磁気センサのオフセット推定値ｃＯ,Ｋ−１とは等しいと仮定する。
【００８１】
このように、本実施形態に係る状態遷移モデルでは、以下に示す式（３６）のように、状態ベクトルｘｋを構成する複数の状態変数のうち、姿勢μを表す状態変数以外は、前の時刻から変化しないという前提を置く。
【数２３】
【００８２】
なお、カルマンフィルタの演算においては、状態ベクトルｘｋは、観測残差ｚｋに基づいて更新される。従って、式（３６）に示された姿勢μ以外の値が、全く変化しないわけではない。具体的には、状態ベクトルｘｋの各要素は、加算器６９０において、観測残差ｚｋに基づいて真値に近づくように更新される。
【００８３】
次に、観測モデル部６５０における演算で用いられる観測モデルについて説明する。
【００８４】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｂＯとを用いて、式（３７）で与えられる。
【数２４】
【００８５】
また、地上座標系ΣＧにおいて地磁気Ｂｇを表すベクトルＧＢｇは式（３８）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｑの推定値γｍａｇは、磁気センサのオフセット推定値ｃＯと、式（８）で示した行列Ｂ（μ）とを用いて、式（３９）で与えられる。
【数２５】
【００８６】
また、観測モデルにおいて、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（８）で示した行列Ｂ（μ）と、式（１２）で示したベクトルＧＧＲＶとを用いて、式（４０）で与えられる。
【数２６】
【００８７】
式（３）を式（１７）の右辺第１項に代入し、式（３７）、式（３９）、及び式（４０）を用いて式（１７）の右辺第２項を表すことにより、式（１７）を、以下の式（４１）に変形することができる。このとき、観測残差ｚｋは、式（４１）により算出される。
【数２７】
【００８８】
［４． 結論］
以上に示したように、ＣＰＵ１０は、カルマンフィルタモジュール６００を呼び出すことで、ステップＳ１０８のカルマンフィルタ演算処理を実行することにより、状態ベクトルｘｋを更新する。具体的には、時刻Ｔ＝ｋにおける状態ベクトルｘｋは、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を、状態ベクトルｘｋ−１を用いて算出される観測残差ｚｋに基づいて更新することで算出される。同様に、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１は、時刻Ｔ＝ｋ−２における状態ベクトルｘｋ−２を、状態ベクトルｘｋ−２を用いて算出される観測残差ｚｋ−１に基づいて更新することで算出される。すなわち、カルマンフィルタ演算処理において、状態ベクトルｘｋは、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１の値を考慮して、更新される。
従って、初期ベクトルＩＮＩの各要素が真値から大きく乖離した値に設定された場合、カルマンフィルタ演算処理を行っても、状態ベクトルｘｋの各要素は真値に近づかなくなることがあり、また、仮に状態ベクトルｘｋの各要素が真値に近づくとしても、真値に近い値に収束するまでに長い時間を要することになる。
【００８９】
前述の通り、磁気センサのオフセットｃＯＦＦは、携帯機器１に搭載された部品が発する内部磁界Ｂｉを表すベクトルである。従って、携帯機器１に搭載された部品が大きな内部磁界Ｂｉを発する場合、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０に対して、事前に定められた固定値（例えば、原点［０，０，０］Ｔ等）を適用すると、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０が、磁気センサのオフセットｃＯＦＦから大きく乖離した値となることがある。また、姿勢μについても、予め定めた固定値を姿勢μの初期値μ０に設定する場合、真の姿勢から最大で１８０度ずれることになる。このように、初期ベクトルＩＮＩを構成する要素のうち、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０と、姿勢μの初期値μ０とは、真値から大きく乖離した値となることがある。
従って、初期ベクトルＩＮＩの各要素が不適切な（真値から乖離した）値に設定されることを防止するためには、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０と、姿勢μの初期値μ０とを、真値に近い正確な値に設定することが、特に重要となる。
【００９０】
本実施形態では、まず、中心点算出処理において、オフセットｃＯＦＦの近似値である中心点ｃＳの座標を算出し、次に、初期ベクトル生成処理において、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０に、中心点ｃＳの座標を適用することで、初期ベクトルＩＮＩを算出した。これにより、磁気センサのオフセット推定値ｃＯの初期値ｃＯ,０が、真値から大きく乖離した値に設定されることを防止した。
また、本実施形態では、安定性判定処理において、携帯機器１の動きが安定していることを判定したうえで、姿勢μの初期値μ０を算出することで、姿勢μの初期値μ０が、真値から大きく乖離した値に設定されることを防止した。
このように、本実施形態は、初期ベクトルＩＮＩの各要素を、真値に近い正確な値に設定するため、カルマンフィルタ演算処理において、正確且つ高速な状態推定が可能となった。
【００９１】
＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【００９２】
（１）変形例１
上述した実施形態では、安定性判定処理において、ローパスフィルタを用いて算出された基準加速度データａＣと、加速度データａとに基づいて安定性指標αを算出したが、本発明はこのような形態に限定されるものでは無く、ローパスフィルタを用いずに安定性指標を算出してもよい。
例えば、以下の式（４３）で示される安定性指標βが、以下の式（４２）で示される条件を満たすか否かにより、携帯機器１の動きが安定しているか否かを判定してもよい。
ここで、安定性指標βは、以下の式（４３）に示すように、時刻ｔ＝１から時刻ｔ＝Ｍまでの期間に３次元加速度センサ８０が出力する複数の加速度データａ１〜ａＭの分散を表す値である。また、式（４２）に示すように、安定性指標βが第２閾値βＴＨ以下となり十分に小さな値となる場合、加速度データａ１〜ａＭの示す値の変動が小さいため、携帯機器１の動きが安定していると看做すことができる（Ｍは、加速度データａの分散を算出するために必要な加速度データの規定測定回数を表す自然数）。
なお、式（４３）に現れる値ａｘ,ＡＶＥ、ａｙ,ＡＶＥ、及びａｚ,ＡＶＥは、複数の加速度データａ１〜ａＭの各要素の平均値であり、それぞれ、式（４４）、式（４５）、及び式（４６）により表される。
【数２８】
【００９３】
（２）変形例２
上述した実施形態及び変形例に係る状態推定装置は、ＣＰＵ１０が、ステップＳ１０１〜Ｓ１０８の各処理を実行するものであったが、本発明はこのような形態に限定されるものでは無く、ＣＰＵ１０が、ステップＳ１０１〜Ｓ１０８のうち一部の処理のみを実行するものであってもよい。
例えば、状態推定装置は、第２の報知手段を備えず、ＣＰＵ１０が、ステップＳ１０５の第２の報知を実行しないものであってもよい。
携帯機器１が、長期間継続的に動かされることは想定し難く、携帯機器１は、通常安定した状態に置かれる。そして、ステップＳ１０６の安定性判定処理で、携帯機器１が安定していることを確認したうえで、初期ベクトルＩＮＩが算出されるため、状態推定プログラム１００が第２の報知手段を備えなくても、姿勢μの初期値μ０が、真値から大きく乖離した値に設定されることを防止することができる。
また、例えば、状態推定装置は、第１の報知手段を備えず、ＣＰＵ１０が、ステップＳ１０２の第１の報知を実行しないものであってもよい。この場合、ＣＰＵ１０は、ステップＳ１０３の磁気データ蓄積処理において、蓄積された複数の磁気データｑ１〜ｑＮが、球面Ｓの中心点ｃＳを算出するために適切であるか否かを判定し、適切であると判定した場合に、処理をステップＳ１０４へと進めてもよい。具体的には、ＣＰＵ１０は、ステップＳ１０３の磁気データ蓄積処理において、複数の磁気データｑ１〜ｑＮの各々がセンサ座標系ΣＳにおいて示す座標の広がりが３次元的であるか否かを判定し、座標の広がりが３次元的であると判定した場合に、処理をステップＳ１０４へと進めてもよい。
また、例えば、状態推定装置は、第１の報知手段及び第２の報知手段を備えず、ＣＰＵ１０が、ステップＳ１０２の第１の報知、及び、ステップＳ１０５の第２の報知を実行しないものであってもよい。
【００９４】
（３）変形例３
上述した実施形態及び変形例に係る状態推定装置において、ＣＰＵ１０は、カルマンフィルタ演算処理を、シグマポイントカルマンフィルタを用いて実行したが、本発明はこのような形態に限定されるものでは無く、公知の非線形カルマンフィルタを適宜適用して演算を行ってもよい。例えば、拡張カルマンフィルタを用いて、カルマンフィルタ演算処理を実行してもよい。
【符号の説明】
【００９５】
１…携帯機器、１０…ＣＰＵ、５０…表示部、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定プログラム、２００…バッファ管理モジュール、３００…中心点算出モジュール、４００…安定性判定モジュール、５００…初期ベクトル生成モジュール、６００…カルマンフィルタモジュール、ｑ…磁気データ、ａ…加速度データ、ａＣ…基準加速度データ、α…安定性指標、αＴＨ…第１閾値、β…安定性指標、βＴＨ…第２閾値、ｘ…状態ベクトル、ｙ…観測値ベクトル、ｚ…観測残差、ｃＳ…中心点、ｃＯＦＦ…磁気センサのオフセット、ｃＯ…磁気センサのオフセット推定値、Ｂｇ…地磁気、Ｂｉ…内部磁界。
【特許請求の範囲】
【請求項１】
磁界を発生させる部品を備えた機器に組み込まれる状態推定装置であって、
互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサと、
加速度を直交する３軸で検出して、３軸の座標系において表現される３次元のベクトルデータとして加速度データを順次出力する３次元加速度センサと、
前記３次元磁気センサから順次出力される磁気データを蓄積する蓄積手段と、
前記蓄積手段に蓄積された複数の前記磁気データに基づいて、前記部品の発する磁界を表す前記３次元磁気センサのオフセットの近似値を示す中心点の３軸の座標を算出する中心点算出手段と、
複数の前記加速度データに基づいて、前記機器の動きが安定しているか否かを判定する安定性判定手段と、
前記安定性判定手段の判定結果が肯定である場合、前記中心点の３軸の座標、前記磁気データ、及び、前記加速度データに基づいて、前記オフセットの推定値及び前記機器の姿勢を推定するベクトルを要素として含む初期ベクトルを生成する初期ベクトル生成手段と、
前記初期ベクトルを初期値とする状態ベクトルを、前記３次元磁気センサ及び前記３次元加速度センサを含む前記機器に組み込まれた複数のセンサの出力を要素とする観測値ベクトルを用いて更新するカルマンフィルタ演算手段と、
を備えることを特徴とする、状態推定装置。
【請求項２】
前記中心点算出手段が前記中心点の算出に用いる複数の前記磁気データの取得を開始する前に、利用者に前記機器の姿勢を変化させるように促す第１の報知手段を、更に備えることを特徴とする、
請求項１に記載の状態推定装置。
【請求項３】
前記中心点算出手段が前記中心点を算出した後に、利用者に前記機器の姿勢を変化させないように促す第２の報知手段を、更に備えることを特徴とする、
請求項２に記載の状態推定装置。
【請求項４】
前記安定性判定手段は、
複数の前記加速度データに基づいて基準加速度データを算出するローパスフィルタを備え、
前記加速度データの示す座標と前記基準加速度データの示す座標との誤差を表す安定性指標を、所定個の前記加速度データの各々について算出し、
前記安定性指標の全てが第１閾値以下である場合、前記機器の動きが安定していると判定することを特徴とする、
請求項１乃至３のうちいずれか１項に記載の状態推定装置。
【請求項５】
前記安定性判定手段は、
所定個の前記加速度データの示す３軸の座標の分散を、安定性指標として算出し、
前記安定性指標が、第２閾値以下である場合、前記機器の動きが安定していると判定することを特徴とする、
請求項１乃至３のうちいずれか１項に記載の状態推定装置。
【請求項１】
磁界を発生させる部品を備えた機器に組み込まれる状態推定装置であって、
互いに直交する３方向の磁気成分をそれぞれ検出して、３軸の座標系において表現される３次元のベクトルデータとして磁気データを順次出力する３次元磁気センサと、
加速度を直交する３軸で検出して、３軸の座標系において表現される３次元のベクトルデータとして加速度データを順次出力する３次元加速度センサと、
前記３次元磁気センサから順次出力される磁気データを蓄積する蓄積手段と、
前記蓄積手段に蓄積された複数の前記磁気データに基づいて、前記部品の発する磁界を表す前記３次元磁気センサのオフセットの近似値を示す中心点の３軸の座標を算出する中心点算出手段と、
複数の前記加速度データに基づいて、前記機器の動きが安定しているか否かを判定する安定性判定手段と、
前記安定性判定手段の判定結果が肯定である場合、前記中心点の３軸の座標、前記磁気データ、及び、前記加速度データに基づいて、前記オフセットの推定値及び前記機器の姿勢を推定するベクトルを要素として含む初期ベクトルを生成する初期ベクトル生成手段と、
前記初期ベクトルを初期値とする状態ベクトルを、前記３次元磁気センサ及び前記３次元加速度センサを含む前記機器に組み込まれた複数のセンサの出力を要素とする観測値ベクトルを用いて更新するカルマンフィルタ演算手段と、
を備えることを特徴とする、状態推定装置。
【請求項２】
前記中心点算出手段が前記中心点の算出に用いる複数の前記磁気データの取得を開始する前に、利用者に前記機器の姿勢を変化させるように促す第１の報知手段を、更に備えることを特徴とする、
請求項１に記載の状態推定装置。
【請求項３】
前記中心点算出手段が前記中心点を算出した後に、利用者に前記機器の姿勢を変化させないように促す第２の報知手段を、更に備えることを特徴とする、
請求項２に記載の状態推定装置。
【請求項４】
前記安定性判定手段は、
複数の前記加速度データに基づいて基準加速度データを算出するローパスフィルタを備え、
前記加速度データの示す座標と前記基準加速度データの示す座標との誤差を表す安定性指標を、所定個の前記加速度データの各々について算出し、
前記安定性指標の全てが第１閾値以下である場合、前記機器の動きが安定していると判定することを特徴とする、
請求項１乃至３のうちいずれか１項に記載の状態推定装置。
【請求項５】
前記安定性判定手段は、
所定個の前記加速度データの示す３軸の座標の分散を、安定性指標として算出し、
前記安定性指標が、第２閾値以下である場合、前記機器の動きが安定していると判定することを特徴とする、
請求項１乃至３のうちいずれか１項に記載の状態推定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１３−９６７２４（Ｐ２０１３−９６７２４Ａ）
【公開日】平成２５年５月２０日（２０１３．５．２０）
【国際特許分類】
【出願番号】特願２０１１−２３７０４５（Ｐ２０１１−２３７０４５）
【出願日】平成２３年１０月２８日（２０１１．１０．２８）
【出願人】（０００００４０７５）ヤマハ株式会社 (5,930)
【Ｆターム（参考）】
【公開日】平成２５年５月２０日（２０１３．５．２０）
【国際特許分類】
【出願日】平成２３年１０月２８日（２０１１．１０．２８）
【出願人】（０００００４０７５）ヤマハ株式会社 (5,930)
【Ｆターム（参考）】
[ Back to top ]