(57)【要約】 【課題】センサに温度変化が生じる場合に、カルマンフィルタを用いた正確な状態推定を行う。【解決手段】第１温度検出部７５が設けられた３次元地磁気センサ７０を含む複数のセンサを備えた携帯機器１に組み込まれ、システムの状態を推定する状態推定装置は、カルマンフィルタ演算部３００を備える。カルマンフィルタ演算部３００は、状態ベクトルｘ＋ｋ−１を状態遷移モデルに適用して推定状態ベクトルｘ−ｋを算出する状態遷移モデル部４１０と、複数のセンサの出力値を要素とする観測値ベクトルｙｋに基づいて算出される観測残差ｚｋ及び推定状態ベクトルｘ−ｋに基づいて、更新後の状態ベクトルｘ＋ｋを算出する更新部４３０とを備え、状態遷移モデル部４１０において規定されるプロセスノイズｗｋの共分散Ｑｋは、第１温度検出部７５が出力する温度データＴｍ，ｋの単位時間当たりの変化量である温度変化量ΔＴｍ，ｋに基づいて定められる。【選択図】図３
2013122384.tif 000002
【特許請求の範囲】
【請求項１】
互いに直交する３方向の磁気成分を検出し、３軸の座標系においてベクトルデータとして表現される磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、
前記３次元磁気センサの温度を検出して温度データを順次出力する温度検出部と、
を備えた機器に組み込まれ、
システムの状態を推定する状態推定装置に用いられるカルマンフィルタであって、
前記３次元磁気センサのオフセットを推定した値を示すオフセット推定ベクトルを含む複数の状態変数を要素とする状態ベクトルを、
システムの経時的な変化を表す状態遷移モデルに対して適用することで、
単位時間経過後の前記状態ベクトルを推定し、これを推定状態ベクトルとして算出する、
状態遷移モデル部と、
前記複数のセンサの出力値の各々を要素とする観測値ベクトルに基づいて観測残差を算出し、
前記推定状態ベクトルと、前記観測残差とに基づいて、前記状態ベクトルを更新し、
これを更新後の状態ベクトルとして算出する、
更新部と、を備え、
前記単位時間における、前記温度データの変化量を、温度変化量としたとき、
前記状態遷移モデルで規定されるプロセスノイズの共分散のうち、
前記オフセット推定ベクトルに生じるプロセスノイズの共分散の各成分を、
前記温度変化量に基づいて定める、
ことを特徴とする、カルマンフィルタ。
【請求項２】
複数のセンサと、
前記複数のセンサのうち、１以上のセンサの各々の温度を検出し、前記１以上のセンサの各々に対応する温度データを出力する温度検出部と、
を備えた機器に組み込まれ、
システムの状態を推定する状態推定装置に用いられる
カルマンフィルタであって、
前記複数のセンサのうち前記１以上のセンサの各々のオフセットを推定した値を要素とするベクトルをオフセット推定ベクトルとしたとき、
前記オフセット推定ベクトルを含む複数の状態変数を要素とする状態ベクトルを、
システムの経時的な変化を表す状態遷移モデルに対して適用することで、
単位時間経過後の前記状態ベクトルを推定し、これを推定状態ベクトルとして算出する、
状態遷移モデル部と、
前記複数のセンサの出力値の各々を要素とする観測値ベクトルに基づいて観測残差を算出し、
前記推定状態ベクトルと、前記観測残差とに基づいて、前記状態ベクトルを更新し、
これを更新後の状態ベクトルとして算出する、
更新部と、を備え、
前記状態遷移モデルで規定されるプロセスノイズの共分散のうち、
前記オフセット推定ベクトルに生じるプロセスノイズの共分散の各成分を、
前記単位時間における、１以上の前記温度データの各々の温度変化量に基づいて定める、
ことを特徴とする、カルマンフィルタ。
【請求項３】
前記複数のセンサには、
互いに直交する３方向の磁気成分を検出し、３軸の座標系においてベクトルデータとして表現される磁気データを順次出力する３次元磁気センサと、
互いに直交する３軸の周りの角速度を検出し、３軸の座標系においてベクトルデータとして表現される角速度データを順次出力する３次元角速度センサとが含まれ、
前記１以上のセンサには、
前記３次元磁気センサ、または、前記３次元角速度センサのうち、少なくとも一方が含まれる、
ことを特徴とする、
請求項２に記載のカルマンフィルタ。
【請求項４】
請求項１乃至３のうちいずれか１項に記載のカルマンフィルタと、
各々の出力が前記観測値ベクトルの要素となる前記複数のセンサと、
前記複数のセンサのうち１以上のセンサの温度を各々検出し、前記１以上のセンサに対応する１以上の前記温度データを出力する温度検出部と
を備える
ことを特徴とする状態推定装置。
【請求項５】
前記温度検出部は、
前記複数のセンサのうち前記１以上のセンサの内部に配置される
ことを特徴とする、
請求項４に記載の状態推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、カルマンフィルタ、及び、状態推定装置に関する。
【背景技術】
【０００２】
地磁気の向きや、物体の姿勢等を算出する場合、地磁気センサからの出力結果のみに基づいて算出するよりも、地磁気センサの他に加速度センサや角速度センサ等の異種の物理量を測定する複数のセンサの出力結果を統合して算出した方が、短時間で正確な値を求めることができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合し、姿勢を推定する方法が開示されている。
【０００４】
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量の各々を推定する値（状態変数）を要素とする状態ベクトルを推定し逐次更新することにより、状態ベクトルの各要素の示す値を、実際の物理量（真値）に近づける演算を行う。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００６】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところで、地磁気センサや角速度センサ等のセンサは、検出対象となる成分以外の成分を検出する場合がある。この場合、センサの検出対象である成分を正確に求めるためには、センサの出力値から、検出対象となる成分以外の成分を取り除く補正処理が必要となる。この補正処理において取り除かれる成分を、センサのオフセットと呼ぶ。
センサのオフセットは、センサの温度変化に伴い変化する場合がある。センサに急激な温度変化が生じ、センサのオフセットが急激に変化する場合、状態ベクトルの各要素の示す値と真値とが乖離し、カルマンフィルタは、動的システムの状態を正確に推定することができなくなる場合がある。
【０００８】
本発明は、上述した点に鑑みてなされたものであり、センサに温度変化が生じる場合に、正確な状態推定を行うことができるカルマンフィルタの提供を解決課題とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決するため、本発明に係るカルマンフィルタは、互いに直交する３方向の磁気成分を検出し、３軸の座標系においてベクトルデータとして表現される磁気データを順次出力する３次元磁気センサを含む、複数のセンサと、前記３次元磁気センサの温度を検出して温度データを順次出力する温度検出部と、を備えた機器に組み込まれ、システムの状態を推定する状態推定装置に用いられるカルマンフィルタであって、前記３次元磁気センサのオフセットを推定した値を示すオフセット推定ベクトルを含む複数の状態変数を要素とする状態ベクトルを、システムの経時的な変化を表す状態遷移モデルに対して適用することで、単位時間経過後の前記状態ベクトルを推定し、これを推定状態ベクトルとして算出する、状態遷移モデル部と、前記複数のセンサの出力値の各々を要素とする観測値ベクトルに基づいて観測残差を算出し、前記推定状態ベクトルと、前記観測残差とに基づいて、前記状態ベクトルを更新し、これを更新後の状態ベクトルとして算出する、更新部と、を備え、前記単位時間における、前記温度データの変化量を、温度変化量としたとき、前記状態遷移モデルで規定されるプロセスノイズの共分散のうち、前記オフセット推定ベクトルに生じるプロセスノイズの共分散の各成分を、前記温度変化量に基づいて定める、ことを特徴とする。
【００１０】
この発明によれば、３次元磁気センサのオフセット推定値を状態ベクトルの要素（状態変数）とするカルマンフィルタの演算において、状態遷移モデルで規定されるプロセスノイズの共分散を、３次元磁気センサの温度変化量に基づいて定める。
３次元磁気センサのオフセットは、３次元磁気センサの温度変化に伴い、変化する。従って、３次元磁気センサに温度変化が生じ、３次元磁気センサのオフセットが変化する場合、カルマンフィルタの演算で用いる状態ベクトルのうち、３次元磁気センサのオフセットを推定する状態変数の示す値が、真値から乖離する。
これに対して、本発明に係るカルマンフィルタは、状態遷移モデルにおいて規定されるプロセスノイズの共分散を、３次元磁気センサの温度変化量に基づいて定め、３次元磁気センサの温度変化に伴い生じる３次元磁気センサのオフセットの変化を考慮して状態ベクトルの更新を行うため、正確な状態推定が可能となる。
【００１１】
また、本発明に係るカルマンフィルタは、複数のセンサと、前記複数のセンサのうち、１以上のセンサの各々の温度を検出し、前記１以上のセンサの各々に対応する温度データを出力する温度検出部と、を備えた機器に組み込まれ、システムの状態を推定する状態推定装置に用いられるカルマンフィルタであって、前記複数のセンサのうち前記１以上のセンサの各々のオフセットを推定した値を要素とするベクトルをオフセット推定ベクトルとしたとき、前記オフセット推定ベクトルを含む複数の状態変数を要素とする状態ベクトルを、システムの経時的な変化を表す状態遷移モデルに対して適用することで、単位時間経過後の前記状態ベクトルを推定し、これを推定状態ベクトルとして算出する、状態遷移モデル部と、前記複数のセンサの出力値の各々を要素とする観測値ベクトルに基づいて観測残差を算出し、前記推定状態ベクトルと、前記観測残差とに基づいて、前記状態ベクトルを更新し、これを更新後の状態ベクトルとして算出する、更新部と、を備え、前記状態遷移モデルで規定されるプロセスノイズの共分散のうち、前記オフセット推定ベクトルに生じるプロセスノイズの共分散の各成分を、前記単位時間における、１以上の前記温度データの各々の温度変化量に基づいて定める、ことを特徴としてもよい。
【００１２】
この発明によれば、１以上のセンサの各々のオフセットを推定した値を要素に含む状態ベクトルを更新するカルマンフィルタの演算において、状態遷移モデルで規定されるプロセスノイズの共分散が、１以上のセンサの各々の温度変化量に基づいて定められる。
この場合、１以上のセンサの各々に生じる温度変化に伴う、１以上のセンサの各々のオフセットの変化を考慮して、状態ベクトルの更新を行うことが可能となるため、正確な状態推定が可能となる。
【００１３】
また、上述したカルマンフィルタにおいて、前記複数のセンサには、互いに直交する３方向の磁気成分を検出し、３軸の座標系においてベクトルデータとして表現される磁気データを順次出力する３次元磁気センサと、互いに直交する３軸の周りの角速度を検出し、３軸の座標系においてベクトルデータとして表現される角速度データを順次出力する３次元角速度センサとが含まれ、前記１以上のセンサには、前記３次元磁気センサ、または、前記３次元角速度センサのうち、少なくとも一方が含まれる、ことを特徴とすることが好ましい。
【００１４】
この発明によれば、３次元磁気センサと３次元角速度センサとを備える機器に組み込まれた状態推定装置が行うカルマンフィルタの演算において、３次元磁気センサまたは３次元角速度センサに温度変化が生じても、正確な状態推定を行うことが可能となる。
【００１５】
また、本発明に係る状態推定装置は、上述したカルマンフィルタのうちいずれかのカルマンフィルタと、各々の出力が前記観測値ベクトルの要素となる前記複数のセンサと、前記複数のセンサのうち１以上のセンサの温度を各々検出し、前記１以上のセンサに対応する１以上の前記温度データを出力する温度検出部とを備えることを特徴とする。
【００１６】
この発明によれば、１以上のセンサの各々のオフセットを推定した値を要素に含む状態ベクトルを更新するカルマンフィルタの演算において、状態遷移モデルで規定されるプロセスノイズの共分散が、１以上のセンサの各々の温度変化量に基づいて定められる。
この場合、状態推定装置は、１以上のセンサの各々に生じる温度変化に伴い生じる、１以上のセンサの各々のオフセットの変化を考慮して、状態ベクトルの更新を行うことが可能となるため、正確な状態推定が可能となる。
【００１７】
また、上述した状態推定装置において、前記温度検出部は、前記複数のセンサのうち前記１以上のセンサの内部に配置されることを特徴とすることが好ましい。
【００１８】
本発明において、１以上のセンサの各々の温度を検出する温度検出部は、１以上のセンサの各々の内部にそれぞれ配置されるものであってもよいし、１以上のセンサの外部に配置されるものであってもよい。すなわち、温度検出部は、１以上のセンサの各々の温度を検出し、単位時間毎に１以上のセンサの各々に対応する温度データを出力するものであればよい。
温度検出部が１以上のセンサの内部に配置される場合、または、外部に配置される場合のいずれの場合であっても、状態推定装置は、１以上のセンサの各々に生じる温度変化に伴い生じる、１以上のセンサの各々のオフセットの変化を考慮して、状態ベクトルの更新を行うことが可能となるため、正確な状態推定が可能となる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】本発明の実施形態に係る携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係るカルマンフィルタ演算部の機能を表す機能ブロック図である。
【図４】本発明の変形例１に係るカルマンフィルタ演算部の機能を表す機能ブロック図である。
【図５】本発明の変形例３に係る携帯機器の構成を示すブロック図である。
【図６】本発明の変形例４に係る携帯機器の構成を示すブロック図である。
【発明を実施するための形態】
【００２０】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００２１】
［１．  機器構成及びソフトウェア構成について］  
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１は、画面の姿勢に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢と、携帯機器１から見た地磁気Ｂｇの向きとを推定することによって実現される。
【００２２】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（蓄積部）２０、状態推定プログラム１００等の各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。
また、携帯機器１は、地磁気Ｂｇ等の磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００２３】
表示部５０は、ＣＰＵ１０が状態推定プログラム１００を実行することにより推定したシステムの状態に基づいて算出された地磁気Ｂｇの向きや携帯機器１の姿勢μ等を、矢印等の画像により表示する。
ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成する。
【００２４】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及び、Ｚ軸磁気センサ７３からの出力信号をＡＤ変換して磁気データｍを出力する。この磁気データｍは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。より具体的には、磁気データｍは、３次元磁気センサ７０に固定された座標系（以下、「センサ座標系ΣＳ」と称する）において、Ｘ軸磁気センサ７１からの出力値をｘ軸成分として表し、Ｙ軸磁気センサ７２からの出力値をｙ軸成分として表し、Ｚ軸磁気センサ７３からの出力値をｚ軸成分として表すベクトルデータである。
また、３次元磁気センサ７０は、３次元磁気センサ７０の温度を検出する第１温度検出部７５を備える。磁気センサＩ／Ｆ７４は、第１温度検出部７５からの出力信号をＡＤ変換して温度データＴｍを出力する。すなわち、第１温度検出部７５は、携帯機器１の備えるセンサの温度を検出する温度検出部として機能する。
【００２５】
ところで、３次元磁気センサ７０が検出する磁気データｍには、地磁気Ｂｇが含まれる。
地磁気Ｂｇは、磁極北に向かう水平成分と伏角方向の鉛直成分を有する磁界であり、地上のある一点に固定された座標系（以下、「地上座標系ΣＧ」と称する）において、一定の向き及び大きさを有するベクトルＧＢｇとして表される（なお、ベクトルの符号の左上に示す添え字「Ｇ」は、当該ベクトルが地上座標系ΣＧで表されたベクトルであることを意味する）。つまり、センサ座標系ΣＳにおいて、地磁気Ｂｇは、携帯機器１の姿勢μに連動して向きを変える、一定の大きさのベクトルＳＢｇ（μ）として表される（なお、ベクトルの符号の左上に示す添え字「Ｓ」は、当該ベクトルがセンサ座標系ΣＳで表されたベクトルであることを意味する）。従って、センサ座標系ΣＳにおいて地磁気Ｂｇを表すベクトルＳＢｇ（μ）を算出することで、携帯機器１の姿勢μを求めることができる。
一方、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力するベクトルデータ（磁気データｍ）は、地磁気Ｂｇを表すベクトルと、携帯機器１に搭載された部品が発する磁界（内部磁界Ｂｉ）を表すベクトルとを加算したベクトルとなる。従って、センサ座標系ΣＳにおける地磁気Ｂｇを表すベクトルＳＢｇ（μ）を算出し、携帯機器１から見た地磁気Ｂｇの向きを正確に求めるためには、センサ座標系ΣＳにおいて、３次元磁気センサが出力する磁気データｍの示す座標から、携帯機器１の部品が発する内部磁界Ｂｉを表すベクトルＳＢｉを減算する補正処理が必要となる。
このように、検出対象である地磁気Ｂｇの正確な向きを特定するために、補正処理において、３次元磁気センサ７０から出力される磁気データｍから取り除かれる内部磁界Ｂｉを表すベクトルＳＢｉを、３次元磁気センサ７０のオフセットと呼ぶ。
【００２６】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、３次元加速度センサ８０と一体となって運動する携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００２７】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示すベクトルデータである。
【００２８】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、携帯機器１の姿勢や３次元磁気センサ７０のオフセット等のシステムの状態を表す複数の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することにより、状態推定装置として機能する。
【００２９】
図３は、状態推定装置のうち、ＣＰＵ１０が状態推定プログラム１００を実行することにより実現される機能を表した機能ブロック図である。図３に示すように、状態推定装置は、初期値生成部２００、カルマンフィルタ演算部３００、及び出力情報生成部５００を備える。
初期値生成部２００は、各種設定情報に基づき、初期ベクトルＩＮＩを生成し、これを出力する。
カルマンフィルタ演算部３００は、３次元磁気センサ７０から出力される磁気データｍ、３次元加速度センサ８０から出力される加速度データａ、及び３次元角速度センサ９０から出力される角速度データｇを要素とする観測値ベクトルｙを用いて、初期ベクトルＩＮＩを初期値とする状態ベクトルｘを逐次更新する非線形カルマンフィルタの演算を実行することで、システムの状態を推定する。
出力情報生成部５００は、カルマンフィルタ演算部３００から周期的に出力される状態ベクトルｘに基づいて、地磁気Ｂｇの向き等の出力情報を算出する。
【００３０】
ここで、状態ベクトルｘとは、複数の状態変数を要素とするベクトルである。状態ベクトルｘの要素である複数の状態変数の各々は、非線形カルマンフィルタが推定するシステムの状態を示す複数の物理量を表す。すなわち、状態ベクトルｘは、非線形カルマンフィルタが推定するシステムの状態を表すベクトルである。
本実施形態は、状態ベクトルｘの要素（状態変数）として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、３次元角速度センサ９０のオフセット推定値ｇＯＦＦ、及び３次元磁気センサ７０のオフセット推定値ｍＯＦＦを採用し、これらを推定の対象とする。
時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各種変数、ベクトル、行列等の右下に付された添え字「ｋ」は、当該各種変数、ベクトル、行列等の時刻Ｔ＝ｋにおける値であることを意味する。
【数１】
2013122384.tif 000003
【００３１】
ここで、地磁気の強さｒはスカラーであり、地磁気の伏角φはスカラーであり、角速度ωは３次元ベクトルであり、３次元角速度センサ９０のオフセット推定値ｇＯＦＦは３次元ベクトルであり、３次元磁気センサ７０のオフセット推定値ｍＯＦＦは３次元ベクトルである。
また、姿勢μは、クォータニオンを用いて表現される。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。例えば、センサ座標系ΣＳの各軸と、地上座標系ΣＧの各軸とが一致する姿勢μを基準姿勢と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢μは、クォータニオンを用いて、式（２）で与えられる。
【数２】
2013122384.tif 000004
【００３２】
観測値ベクトルｙとは、３次元磁気センサ７０から出力される磁気データｍ、３次元加速度センサ８０から出力される加速度データａ、及び、３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルである。時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数３】
2013122384.tif 000005
【００３３】
カルマンフィルタ演算部３００は、上述した状態ベクトルｘｋと観測値ベクトルｙｋとを用いた非線形カルマンフィルタの演算を実行して、状態ベクトルｘｋの各要素が真値に近づくように状態ベクトルｘｋを周期的に更新することで、システムの状態を表す複数の物理量を推定する。
ここで、「真値」とは、状態ベクトルｘｋの各要素に対応する実際の物理量を表す値である。また、以下では、状態ベクトルｘｋの各要素を真値としたベクトルを、「状態ベクトルの真値」と称する。なお、非線形カルマンフィルタの演算の詳細については、後述する。
【００３４】
初期値生成部２００は、初期ベクトルＩＮＩを生成し、これを出力する。
初期ベクトルＩＮＩは、以下の式（４）に示すように、状態ベクトルｘｋの初期値、つまり時刻Ｔ＝０における状態ベクトルｘ０であり、姿勢μの初期値μ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角φの初期値φ０、角速度ωの初期値ω０、３次元角速度センサ９０のオフセット推定値ｇＯＦＦの初期値ｇＯＦＦ,０、及び３次元磁気センサ７０のオフセット推定値ｍＯＦＦの初期値ｍＯＦＦ,０を要素として含む。
【数４】
2013122384.tif 000006
【００３５】
なお、初期ベクトルＩＮＩの各要素は、真値から大きく乖離しないように適宜定めればよい。以下では、初期ベクトルＩＮＩの各要素の設定例を示す。
【００３６】
地磁気の強さｒの初期値ｒ０、及び地磁気の伏角φの初期値φ０は、例えば、ＧＰＳ部６０から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ１が格納される。初期値生成部２００は、ルックアップテーブルＬＵＴ１を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さｒの初期値ｒ０及び地磁気の伏角φの初期値φ０として設定する。
なお、携帯機器１が衛星からの電波の届かない場所（例えば、地下街）にある場合には、ＧＰＳ部６０から位置情報が得られない。そのような場合には、携帯機器１が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルＬＵＴ１に格納されている。初期値生成部２００は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルＬＵＴ１から読み出す。例えば、日本で販売された携帯機器１については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さｒの初期値ｒ０、及び地磁気の伏角φの初期値φ０を算出する。
【００３７】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。
また、３次元角速度センサ９０のオフセット推定値ｇＯＦＦの初期値ｇＯＦＦ,０は、通常「０」近辺に調整されているため、「０」に設定する。
姿勢μの初期値μ０は、例えば、携帯機器１が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。
【００３８】
３次元磁気センサ７０のオフセット推定値ｍＯＦＦの初期値ｍＯＦＦ,０は、時刻Ｔ＝０に３次元磁気センサ７０から出力される磁気データｍ０、地上座標系ΣＧにおいて地磁気Ｂｇを表したベクトルＧＢｇ、及び、行列Ｂ（μ）を用いて、以下に示す式（５）により算出される。
ここで、行列Ｂ（μ）は、携帯機器１が姿勢μである場合に、地上座標系ΣＧにおいて表現されたベクトルを、センサ座標系ΣＳで表現するための座標変換行列であり、以下の式（６）で表される。また、時刻Ｔ＝０における地磁気Ｂｇを地上座標系ΣＧから表したベクトルＧＢｇは、地磁気の強さｒの初期値ｒ０と、地磁気の伏角φの初期値φ０とを用いて、以下の式（７）で表される。なお、本実施形態では、地上座標系ΣＧを、ｙ軸が磁極北を向き、ｚ軸が鉛直上を向くように定める。
【数５】
2013122384.tif 000007
【００３９】
このように、初期値生成部２００は、初期ベクトルＩＮＩを生成しこれを出力する。そして、カルマンフィルタ演算部３００は、初期ベクトルＩＮＩを初期値とする状態ベクトルｘｋを更新する非線形カルマンフィルタの演算を実行することで、システムの状態を推定する。以下では、カルマンフィルタ演算部３００が行う非線形カルマンフィルタの演算の詳細について説明する。
【００４０】
［２．  非線形カルマンフィルタによる演算について］    
一般的にカルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘ＋ｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定する。この推定値を、推定状態ベクトルｘ−ｋと称する。
そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、推定状態ベクトルｘ−ｋから観測値ベクトルｙｋを推定する。この推定値を、推定観測値ベクトルｙ−ｋと称する。
次に、カルマンフィルタは、推定観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｚｋとして算出し、観測残差ｚｋに基づいてカルマンゲインＫｋを算出する。さらに、カルマンフィルタは、観測残差ｚｋ、カルマンゲインＫｋ、及び、推定状態ベクトルｘ−ｋを用いて、状態ベクトルｘ＋ｋ−１を更新することにより、更新後の状態ベクトルｘ＋ｋを算出する。
以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタの演算により、状態ベクトルｘｋの各要素を実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける。
【００４１】
なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。
シグマポイントカルマンフィルタは、まず、状態ベクトルｘ＋ｋ−１の周囲に複数のシグマポイントχ＋ｋ−１を設定し、これらの複数のシグマポイントχ＋ｋ−１を状態遷移モデルに適用することで、単位時間経過後の複数のシグマポイントを推定する。この推定された複数のシグマポイントを、推定シグマポイントχ−ｋと称する。次に、シグマポイントカルマンフィルタは、複数の推定シグマポイントχ−ｋの平均を算出することにより、推定状態ベクトルｘ−ｋを求める。
従って、シグマポイントカルマンフィルタを用いてカルマンフィルタの演算を実行する場合、推定観測値ベクトルｙ−ｋは、厳密には、推定状態ベクトルｘ−ｋの周辺に存在する複数の推定シグマポイントχ−ｋに基づいて算出される。
【００４２】
本実施形態における、非線形カルマンフィルタの状態遷移モデルは以下に示す式（８）で与えられ、観測モデルは以下に示す式（９）で与えられる。なお、本実施形態では、式（８）に現れる状態方程式ｆ、および、式（９）に現れる観測方程式ｈは、非線形の関数である。
【数６】
2013122384.tif 000008
【００４３】
ここで、状態ベクトルｘｋはｄｉｍ（ｘ）次元のベクトルであり、観測値ベクトルｙｋはｄｉｍ（ｙ）次元のベクトルである。本実施形態では、ｄｉｍ（ｘ）＝１５であり、ｄｉｍ（ｙ）＝９である。
ここで、Ｐ−ｋは、推定状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。また、Ｐｙｙｋは、観測残差ｚｋの共分散であり、Ｐｘｙｋは、推定状態ベクトルｘ−ｋと、推定観測値ベクトルｙ−ｋとの相互共分散行列である。
また、式（８）に現れるプロセスノイズｗｋ及び式（９）に現れる観測ノイズｖｋは「０」を中心とするガウスノイズである。
式（８）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を状態方程式ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗｋ−１とを加算することにより推定されることを示している。
また、式（９）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを観測方程式ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗｋの共分散をＱｋ、観測ノイズｖｋの共分散をＲｋと表す。
【００４４】
プロセスノイズｗｋの共分散Ｑｋは、以下の式（１０）に示すように、行列Ｑｃｓｔ，１、行列Ｑｃｓｔ，２、及び、行列Ｑｍ，ｋにより構成される１５行１５列の対称行列である。
ここで、行列Ｑｃｓｔ，１は、１２行１２列の対称行列であり、行列Ｑｃｓｔ，１の各成分は定数である。この行列Ｑｃｓｔ，１は、状態遷移モデルにおいて状態ベクトルｘｋに生じるプロセスノイズｗｋのうち、３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋ以外の要素に生じるプロセスノイズの共分散を表す行列である。行列Ｑｃｓｔ，２は、１２行３列の零行列である。
行列Ｑｍ，ｋは、３行３列の対称行列であり、状態遷移モデルにおいて状態ベクトルｘｋに生じるプロセスノイズｗｋのうち、３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋに生じるプロセスノイズの共分散を表す行列である。行列Ｑｍ，ｋは、以下の式（１１）に示すように、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて定められる。なお、式（１１）に現れる係数αｍは、正の実数である。また、行列Ｑｃｓｔ，ｍは、３次元磁気センサ７０に温度変化が生じても３次元磁気センサ７０のオフセットが変化しないと仮定した状態遷移モデルにおいて、状態ベクトルｘｋに生じるプロセスノイズｗｋのうち、３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋに生じるプロセスノイズの共分散を表す行列である。本実施形態では、行列Ｑｃｓｔ，ｍの各成分は定数とする。
また、式（１２）に示すように、温度変化量ΔＴｍ，ｋは、時刻Ｔ＝ｋにおける温度データＴｍ，ｋと、時刻Ｔ＝ｋ−１における温度データＴｍ，ｋ−１との差分の絶対値を表す。なお、温度変化量ΔＴｍに対して添え字「ｋ」を付す場合には、時刻Ｔ＝ｋと時刻Ｔ＝ｋ−１との間に生じる、３次元磁気センサ７０の温度変化量の絶対値であることを表す。
【数７】
2013122384.tif 000009
【００４５】
３次元磁気センサ７０のオフセットは、３次元磁気センサ７０の温度変化に伴い、変化する。より具体的には、３次元磁気センサ７０のオフセットの変化の大きさは、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋの大きさに基づいて定められる。
詳細は後述するが、３次元磁気センサ７０のオフセットの経時的な変化を定式化することは難しい。つまり、３次元磁気センサ７０のオフセットの経時的な変化を、３次元磁気センサ７０の温度変化まで考慮したうえで、状態方程式ｆとして定式化することは難しい。
状態方程式ｆが３次元磁気センサ７０の温度変化を考慮していない場合、状態遷移モデルは、３次元磁気センサ７０に温度変化が生じたときに、状態推定装置が推定するシステムの経時的な変化を正確に表現することができなくなる。そして、状態遷移モデルが、システムの経時的な変化を正確に表現していない場合、状態遷移モデルを信頼して状態ベクトルｘｋを更新すると、状態ベクトルｘｋの各要素（特に、３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋ）は、真値から乖離した値となる可能性が高い。
このように、状態ベクトルｘｋの各要素が真値から乖離した値となる場合、状態推定装置は、システムの状態を正確に推定できない。また、状態ベクトルｘｋの各要素が真値から乖離した値となった場合、その後、非線形カルマンフィルタの演算を繰り返しても、状態ベクトルｘｋの各要素が真値に近い値に収束するまでに長い時間を要することになる。
【００４６】
これに対して、本実施形態では、式（１１）に示した通り、プロセスノイズｗｋの共分散Ｑｋのうち、３次元磁気センサ７０のオフセット推定値ｍＯＦＦに対応する成分である行列Ｑｍ，ｋを、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて定める。
この場合、状態遷移モデルは、状態方程式ｆにおいて定式化することができない３次元磁気センサ７０の温度変化に伴う３次元磁気センサ７０のオフセットの変化を、プロセスノイズｗｋとして考慮する。これにより、本実施形態に係る状態推定装置は、状態遷移モデルと、状態推定装置が推定するシステムの経時的な変化との間に存在する誤差を考慮しつつ、状態ベクトルｘｋの更新を行うことができるため、更新後の状態ベクトルｘ＋ｋの各要素が真値から乖離する可能性を低減させることが可能となる。
【００４７】
なお、センサのオフセットを推定する値を表す状態変数のうち、当該センサの温度変化量に基づいて、当該状態変数に生じるプロセスノイズの共分散が定められる状態変数を、「オフセット推定ベクトル」と称する場合がある。
本実施形態では、センサのオフセットを推定する値を表す状態変数が、３次元角速度センサ９０のオフセット推定値ｇＯＦＦ、及び３次元磁気センサ７０のオフセット推定値ｍＯＦＦであるのに対して、センサの温度変化量に基づいて状態変数に生じるプロセスノイズの共分散が定められる状態変数は、３次元磁気センサ７０のオフセット推定値ｍＯＦＦである。つまり、本実施形態では、３次元磁気センサ７０のオフセット推定値ｍＯＦＦが、オフセット推定ベクトルに該当する。
【００４８】
時刻Ｔ＝ｋにおける観測残差ｚｋは、実際の観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（１３）で表される。カルマンフィルタは、以下の式（１５）に示すように、観測残差ｚｋ、推定状態ベクトルｘ−ｋ、及び、式（１４）に示すカルマンゲインＫｋを用いて、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタは、以下の式（１６）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
ここで、Ｐ−ｋは、推定状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。また、Ｐｙｙｋは、観測残差ｚｋの共分散であり、Ｐｘｙｋは、推定状態ベクトルｘ−ｋと、推定観測値ベクトルｙ−ｋとの相互共分散行列である。
【数８】
2013122384.tif 000010
【００４９】
なお、推定観測値ベクトルｙ−ｋは、推定状態ベクトルｘ−ｋ（厳密には、推定シグマポイントχ−ｋ）を、式（９）に示す観測モデルに適用することで算出される値である。よって、推定観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｚｋは、推定状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
従って、式（１５）に示すように、観測残差ｚｋを用いて、推定状態ベクトルｘ−ｋを、更新後の状態ベクトルｘ＋ｋへと更新することにより、状態ベクトルｘｋの各要素を真値に近い値へと近付けることができる。
【００５０】
カルマンフィルタ演算部３００は、アンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて、非線形カルマンフィルタの演算を実行する。
図３に示すように、遅延部３１０は、加算器３９０から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部３２０に対して出力する。なお、初回の演算（時刻Ｔ＝０における演算）では、遅延部３１０は、初期値生成部２００が出力する初期ベクトルＩＮＩを、状態ベクトルｘ＋ｋ−１に適用する。
【００５１】
シグマポイント生成部３２０は、３次元磁気センサ７０が順次出力する温度データＴｍ，ｋを式（１１）に適用し、行列Ｑｍ，ｋ−１を算出することで、式（１０）に示すプロセスノイズｗｋの共分散Ｑｋ−１を算出する。
【００５２】
また、シグマポイント生成部３２０は、「ｄｉｍ（ｘ）」行「ｄｉｍ（ｘ）」列の共分散行列Ｐ＋ｋ−１及びＱｋ−１を用いて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントを生成する。
具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマポイントの広がりを表すスケーリングパラメータλを用いて、式（１７）及び式（１８）に示すベクトルσｋ（１）〜σｋ（２ｄｉｍ（ｘ））を定義する。
【数９】
2013122384.tif 000011
【００５３】
このとき、シグマポイント生成部３２０は、ベクトルσｋ−１と状態ベクトルｘ＋ｋ−１とに基づいて、式（１９）及び式（２０）で示される「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ＋ｋ−１（０）〜χ＋ｋ−１（２ｄｉｍ（ｘ））を生成する。
【数１０】
2013122384.tif 000012
【００５４】
状態遷移演算部３３０は、式（２１）に示すように、時刻Ｔ＝ｋ−１における「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ＋ｋ−１（０）〜χ＋ｋ−１（２ｄｉｍ（ｘ））の各々を、状態方程式ｆに適用することにより、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））を算出する。
【数１１】
2013122384.tif 000013
【００５５】
次に、推定状態ベクトル算出部３４０は、式（２２）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））の平均値を計算することで、推定状態ベクトルｘ−ｋを算出する。
【数１２】
2013122384.tif 000014
【００５６】
また、推定状態ベクトル算出部３４０は、式（２３）に示すように、推定状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋを算出する。ここで、ベクトルξｊは、以下の式（２４）に示す、推定シグマポイントχ−ｋ（ｊ）と推定状態ベクトルｘ−ｋとの差分を表すベクトルである。
【数１３】
2013122384.tif 000015
【００５７】
このように、シグマポイント生成部３２０、状態遷移演算部３３０、及び、推定状態ベクトル算出部３４０は、状態遷移モデルにおいて状態ベクトルｘ＋ｋ−１に生じるプロセスノイズｗｋの共分散Ｑｋ−１を算出するとともに、状態ベクトルｘ＋ｋ−１を状態遷移モデルに適用することで推定状態ベクトルｘ−ｋを算出する、状態遷移モデル部４１０として機能する。
【００５８】
一方、推定観測値算出部３５０は、式（２５）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））の各々を、観測方程式ｈに適用することにより、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（０）〜γｋ（２ｄｉｍ（ｘ））を算出する。
【数１４】
2013122384.tif 000016
【００５９】
そして、推定観測値ベクトル算出部３６０は、式（２６）に示すように、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（０）〜γｋ（２ｄｉｍ（ｘ））の平均を演算することにより、推定観測値ベクトルｙ−ｋを算出する。
【数１５】
2013122384.tif 000017
【００６０】
また、推定観測値ベクトル算出部３６０は、式（２７）に示すように、観測残差の共分散Ｐｙｙｋを算出する。ここで、ベクトルζｊは、以下の式（２８）に示すように、推定観測値γｋ（ｊ）と推定観測値ベクトルｙ−ｋとの差分を表すベクトルである。
【数１６】
2013122384.tif 000018
【００６１】
このように、推定観測値算出部３５０、及び、推定観測値ベクトル算出部３６０は、推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））の各々を観測モデルに適用することで推定観測値ベクトルｙ−ｋを算出する観測モデル部４２０として機能する。
【００６２】
減算器３７０は、式（１３）に示したように、観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとの差分として、観測残差ｚｋを算出する。
カルマンゲイン付与部３８０は、式（２９）に示すように、相互共分散行列Ｐｘｙｋを算出する。そして、カルマンゲイン付与部３８０は、式（１４）に示したように、観測残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出し、式（１５）の右辺第２項（Ｋｋｚｋ）の演算を実行する。また、カルマンゲイン付与部３８０は、式（１６）に示したように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを、Ｐ−ｋからＰ＋ｋに更新する。
【数１７】
2013122384.tif 000019
【００６３】
加算器３９０は、式（１５）に示したように、推定状態ベクトルｘ−ｋと、カルマンゲイン付与部３８０から出力される式（１５）の右辺第２項（Ｋｋｚｋ）とを加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
【００６４】
このように、減算器３７０、カルマンゲイン付与部３８０、及び、加算器３９０は、観測残差ｚｋ、カルマンゲインＫｋ、及び、推定状態ベクトルｘ−ｋを用いて、状態ベクトルｘ＋ｋ−１を更新した更新後の状態ベクトルｘ＋ｋを算出する更新部４３０として機能する。
【００６５】
次に、状態遷移モデル部４１０において用いられる状態遷移モデルのうち、状態遷移演算部３３０における演算で用いられる状態方程式ｆについて説明する。
【００６６】
まず、状態方程式ｆのうち、時刻Ｔ＝ｋ−１における姿勢μ＋ｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μ−ｋを推定する演算は、以下に示す式（３０）として表される。ここで、μ−ｋは、時刻ｋにおける推定状態ベクトルｘ−ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（３０）の右辺の演算子Ωは、式（３１）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、演算子［ｌ×］は、式（３２）で定義される。また、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの測定時間間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ＋ｋのうち角速度を表す状態変数に対応する要素をω＋ｋで表したとき、演算子Ωを構成する成分Ψ＋ｋは、式（３３）で定義される。
【数１８】
2013122384.tif 000020
【００６７】
姿勢μは、クォータニオンで表現され、式（２）に示すように、正規化条件||μ||＝１を満たす必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘｋを更新する場合、更新後の状態ベクトルｘ＋ｋは、式（２２）に示すように、推定シグマポイントχ−ｋ（ｊ）の平均として算出される推定状態ベクトルｘ−ｋに基づいて定められるため、（更新前の）状態ベクトルｘ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘ＋ｋ−１の要素である姿勢μ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋの要素である姿勢μ＋ｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢μについての正規化条件が満たされなくなる可能性が存在する。
そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。
なお、より厳密に正規化条件を保つためには、状態ベクトルｘｋを構成する要素のうち姿勢μｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
【００６８】
地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角φｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角φｋ−１とは各々等しい値であると仮定する。
同様に、３次元角速度センサ９０のオフセット推定値ｇＯＦＦは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける３次元角速度センサ９０のオフセット推定値ｇＯＦＦ,Ｋと、時刻Ｔ＝ｋ−１における３次元角速度センサ９０のオフセット推定値ｇＯＦＦ,Ｋ−１とは等しい値であると仮定する。
【００６９】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００７０】
前述の通り、３次元磁気センサ７０のオフセットは、携帯機器１の部品が発する内部磁界Ｂｉの方向及び大きさをセンサ座標系ΣＳにおいて表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、３次元磁気センサ７０のオフセットも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、３次元磁気センサ７０のオフセットも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、３次元磁気センサ７０のオフセットが変化するタイミングや３次元磁気センサ７０のオフセットの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における３次元磁気センサ７０のオフセット推定値ｍＯＦＦ,Ｋ−１を用いて、時刻Ｔ＝ｋにおける３次元磁気センサ７０のオフセット推定値ｍＯＦＦ,Ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける３次元磁気センサ７０のオフセット推定値ｍＯＦＦ,Ｋと、時刻Ｔ＝ｋ−１における３次元磁気センサ７０のオフセット推定値ｍＯＦＦ,Ｋ−１とは等しいと仮定する。
【００７１】
このように、本実施形態に係る状態遷移モデルにおける状態方程式ｆは、以下に示す式（３４）のように、状態ベクトルｘｋを構成する複数の状態変数のうち、姿勢μｋを表す状態変数以外は、前の時刻から変化しないことを表す方程式として、定式化される。
【数１９】
2013122384.tif 000021
【００７２】
このように、状態ベクトルｘｋを、状態方程式ｆに適用した場合、状態ベクトルｘｋのうち、姿勢μｋ以外の各要素の示す値は変化しない。
しかし、状態ベクトルｘｋのうち姿勢μｋ以外の各要素は、加算器３９０において、観測残差ｚｋに基づいて真値に近づくように更新される。つまり、式（１５）に示すように、状態ベクトルｘｋの各要素は、状態遷移モデルにより算出される推定状態ベクトルｘ−ｋと、複数のセンサからの出力値（観測値ベクトルｙｋ）を用いて算出される観測残差ｚｋとの双方に基づいて、真値に近づくように更新される。
【００７３】
式（１５）に示すように、推定状態ベクトルｘ−ｋ及び観測残差ｚｋの、更新後の状態ベクトルｘ＋ｋに対する寄与の割合は、カルマンゲインＫｋによって定められる。
すなわち、カルマンゲインＫｋの各成分を適切に定めることができれば、更新後の状態ベクトルｘ＋ｋに占める推定状態ベクトルｘ−ｋの割合と、更新後の状態ベクトルｘ＋ｋに占める観測残差ｚｋの割合とが、適切に調整され、更新後の状態ベクトルｘ＋ｋの各要素が真値から乖離することを防止することが可能となる。
【００７４】
ところで、状態遷移モデルにおいて用いられる状態方程式が、状態推定装置が推定するシステムの経時的な変化を正確に表すことができない場合、状態方程式を用いて算出された推定状態ベクトルｘ−ｋの各要素は、真値から乖離した値となっている可能性が高い。
この場合、例えば、カルマンゲインＫｋの各成分を大きな値とし、式（１５）における第２項（Ｋｋｚｋ）の各要素を大きな値とすることで、更新後の状態ベクトルｘ＋ｋに占める推定状態ベクトルｘ−ｋの割合を相対的に小さくする。これにより、更新後の状態ベクトルｘ＋ｋの各要素が真値から乖離することを防止することが可能となる。
【００７５】
本実施形態に係る状態方程式ｆは、式（３４）に示すように、３次元磁気センサ７０のオフセット推定値ｍＯＦＦが一定の値であること（経時的な変化をしないこと）を表した方程式である。しかし、実際には、３次元磁気センサ７０に温度変化が生じた場合、３次元磁気センサ７０のオフセット（真値）は変化する。そして、３次元磁気センサ７０のオフセットの変化の大きさは、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋの大きさに基づいて定められる。すなわち、温度変化量ΔＴｍ，ｋが大きな値となる場合、状態方程式ｆを用いて算出される３次元磁気センサ７０のオフセット推定値ｍＯＦＦは、３次元磁気センサ７０のオフセットの実際の値（真値）とは乖離した値として算出される。つまり、温度変化量ΔＴｍ，ｋが大きな値となる場合、状態方程式ｆは、状態推定装置が推定するシステムを表す物理量の経時的な変化を正確に表現することができない。
【００７６】
そこで、本実施形態では、式（１０）、式（１１）、及び式（１２）に示すように、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて、プロセスノイズｗｋの共分散Ｑｋを定める。
プロセスノイズｗｋは、状態方程式ｆと、状態推定装置が推定するシステムを表す物理量の経時的な変化との間に存在する誤差を表現する。よって、３次元磁気センサ７０の温度変化に起因する、状態方程式ｆと、システムの経時的な変化との間に存在する誤差は、プロセスノイズｗｋ（及び、その共分散Ｑｋ）により表現することが可能となる。
詳細は後述するが、カルマンゲインＫｋは、プロセスノイズｗｋ−１の共分散Ｑｋ−１に基づいて定められる。よって、カルマンゲインＫｋは、温度変化量ΔＴｍ，ｋ−１に基づいて定められる。その結果、式（１５）に示すカルマンゲインＫｋを用いた状態ベクトルｘｋの更新において、更新後の状態ベクトルｘ＋ｋは、３次元磁気センサ７０の温度変化に起因して状態方程式ｆとシステムの経時的な変化との間に生じる誤差を考慮して算出されることになる。
すなわち、本実施形態によれば、３次元磁気センサ７０の温度変化に起因して、状態方程式ｆとシステムの経時的な変化との間に誤差が生じた場合であっても、当該誤差を考慮して更新後の状態ベクトルｘ＋ｋが定められるため、更新後の状態ベクトルｘ＋ｋの各要素が真値から乖離する可能性を低減させることが可能となる。
【００７７】
以下、カルマンゲインＫｋが、プロセスノイズｗｋ−１の共分散Ｑｋ−１に基づいて定められることについて説明する。
まず、式（１７）及び式（１８）に示すように、複数のシグマポイントχ＋ｋ−１（１）〜χ＋ｋ−１（２ｄｉｍ（ｘ））の平均に対する広がりの大きさは、プロセスノイズｗｋ−１の共分散Ｑｋ−１に基づいて定められる。そして、式（２１）、式（２２）、及び式（２４）に示すように、複数のシグマポイントχ＋ｋ−１（１）〜χ＋ｋ−１（２ｄｉｍ（ｘ））の広がりの大きさに基づいて、ベクトルξ１〜ξ２ｄｉｍ（ｘ）の各々が定められる。また、式（２３）に示すように、推定状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋは、ベクトルξ１〜ξ２ｄｉｍ（ｘ）に基づいて定められる。このように、推定状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋは、プロセスノイズｗｋ−１の共分散Ｑｋ−１に基づいて定められる。
同様に、式（２５）、式（２６）、及び式（２８）に示すように、複数のシグマポイントχ＋ｋ−１（１）〜χ＋ｋ−１（２ｄｉｍ（ｘ））の広がりの大きさに基づいて、ベクトルζ１〜ζ２ｄｉｍ（ｘ）の各々が定められる。そして、式（２９）に示すように、相互共分散行列Ｐｘｙｋは、ベクトルξ１〜ξ２ｄｉｍ（ｘ）と、ベクトルζ１〜ζ２ｄｉｍ（ｘ）とに基づいて定められる。また、式（２７）に示すように、観測残差の共分散Ｐｙｙｋは、ベクトルζ１〜ζ２ｄｉｍ（ｘ）に基づいて定められる。このように、相互共分散行列Ｐｘｙｋと、観測残差の共分散Ｐｙｙｋとは、プロセスノイズｗｋ−１の共分散Ｑｋ−１に基づいて定められる。
カルマンゲインＫｋは、式（１４）に示すように、相互共分散行列Ｐｘｙｋと、観測残差の共分散Ｐｙｙｋとに基づいて定められる。従って、カルマンゲインＫｋは、プロセスノイズｗｋ−１の共分散Ｑｋ−１に基づいて定められることになる。
【００７８】
仮に、プロセスノイズｗｋ−１の共分散Ｑｋ−１が、温度変化量ΔＴｍ，ｋ−１を考慮せずに定められる場合、カルマンゲインＫｋの各成分も、温度変化量ΔＴｍ，ｋ−１を考慮せずに定められる。この場合、更新後の状態ベクトルｘ＋ｋも、温度変化量ΔＴｍ，ｋ−１を考慮せずに定められるため、更新後の状態ベクトルｘ＋ｋの各要素が、真値から乖離した値となる可能性が高い。
これに対して、本実施形態に係る状態推定装置は、上述したように、温度変化量ΔＴｍ，ｋ−１に基づいて、プロセスノイズｗｋ−１の共分散Ｑｋ−１を定める。
具体的には、状態遷移モデルにおいて状態ベクトルｘｋに生じるプロセスノイズｗｋ−１の共分散Ｑｋ−１のうち、３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋに生じるプロセスノイズの共分散を表す行列Ｑｍ，ｋ−１は、温度変化量ΔＴｍ，ｋ−１に基づいて定められる。従って、カルマンゲインＫｋのうち、３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋに対応する成分の各々の値は、温度変化量ΔＴｍ，ｋ−１に基づいて決定される。
その結果、更新後の状態ベクトルｘ＋ｋのうち３次元磁気センサ７０のオフセット推定値ｍＯＦＦ＋ｋに対応する要素が、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋ−１を考慮して算出されるため、更新後の状態ベクトルｘ＋ｋのうち３次元磁気センサ７０のオフセット推定値ｍＯＦＦ＋ｋに対応する要素が、３次元磁気センサ７０のオフセットの実際の値（真値）とは乖離した値として算出されることを防止することができる。
【００７９】
次に、観測モデル部４２０において用いられる観測モデルのうち、推定観測値算出部３５０における演算で用いられる観測方程式ｈについて説明する。
【００８０】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｇＯＦＦとを用いて、式（３５）で与えられる。
【数２０】
2013122384.tif 000022
【００８１】
また、地上座標系ΣＧにおいて地磁気Ｂｇを表すベクトルＧＢｇは式（３６）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｍの推定値γｍａｇは、磁気センサのオフセット推定値ｍＯＦＦと、式（６）で示した行列Ｂ（μ）とを用いて、式（３７）で与えられる。
【数２１】
2013122384.tif 000023
【００８２】
また、観測モデルにおいて、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（６）で示した行列Ｂ（μ）と、ベクトルＧＧＲＶとを用いて、式（３８）で与えられる。なお、ベクトルＧＧＲＶは、以下の式（３９）に示すように、地上座標系ΣＧにおいて重力加速度を表したベクトルを、重力加速度の大きさで正規化した３次元のベクトルである。
【数２２】
2013122384.tif 000024
【００８３】
このように、本実施形態に係る観測モデルにおける観測方程式ｈは、式（３５）、式（３７）、及び式（３８）により定式化される。
そして、式（３）を式（１３）の右辺第１項に代入し、式（３５）、式（３７）、及び式（３８）を用いて式（１３）の右辺第２項を表すことにより、式（１３）を、以下の式（４０）に変形することができる。このとき、観測残差ｚｋは、式（４０）により算出される。
【数２３】
2013122384.tif 000025
【００８４】
［３．  出力情報について］
出力情報生成部５００は、カルマンフィルタ演算部３００が周期的に出力する状態ベクトルｘｋに基づいて、携帯機器１の姿勢μを算出し、これを出力情報として出力する。
なお、本実施形態では、出力情報生成部５００は姿勢μを出力情報として出力するが、本発明はこのような形態に限られるものではなく、携帯機器１が備える各種センサの出力値と、状態ベクトルｘｋとに基づいて算出される情報を出力情報としてもよい。
例えば、出力情報生成部５００は、携帯機器１から見た地磁気Ｂｇの向きを出力情報として算出するものであってもよい。具体的には、出力情報生成部５００は、３次元磁気センサ７０から出力される磁気データｍｋと、状態ベクトルｘｋの要素である３次元磁気センサ７０のオフセット推定値ｍＯＦＦ，ｋとに基づいて、センサ座標系ΣＳにおける地磁気Ｂｇを表すベクトルＳＢｇを算出し、これを出力情報として出力するものであってもよい。
【００８５】
［４．  結論］
以上に示したように、本実施形態に係る状態推定装置は、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて、行列Ｑｍ，ｋの各成分の値を設定する。
３次元磁気センサ７０の温度変化量ΔＴｍ，ｋが大きな値となる場合、状態遷移モデルにおける３次元磁気センサ７０のオフセット推定値ｍＯＦＦの経時的な変化と、３次元磁気センサ７０のオフセット（真値）の経時的な変化とが、異なる場合がある。これに対して本実施形態では、行列Ｑｍ，ｋを、温度変化量ΔＴｍ，ｋに基づいて定めるため、３次元磁気センサ７０の温度が変化し、状態遷移モデルがシステムの経時的な変化を正確に表現できなくなったときに生じる、状態遷移モデルが表現するシステムの経時的な変化と、実際のシステムの経時的な変化との誤差を考慮して状態ベクトルｘｋの更新を行うことが可能となる。これにより、状態ベクトルｘｋの各要素が、真値から乖離する可能性を低減させることが可能となった。すなわち、本実施形態に係る状態推定装置は、非線形カルマンフィルタの演算において、状態ベクトルｘｋの各要素が真値から乖離する可能性を低減させることで、正確且つ高速な状態推定が可能となった。
【００８６】
＜Ｂ．変形例＞                
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【００８７】
（１）変形例１    
上述した実施形態に係る状態推定装置は、シグマポイントカルマンフィルタを用いて非線形カルマンフィルタの演算を実行するものであったが、本発明はこのような形態に限定されるものでは無く、拡張カルマンフィルタ等、公知の非線形カルマンフィルタを適宜適用して演算を行うものでもよい。
図４は、変形例１に係る状態推定プログラムを実行したときに実現される機能を表した機能ブロック図である。図４に示すように、変形例１に係る状態推定装置は、カルマンフィルタ演算部３００の代わりにカルマンフィルタ演算部３００ａを備える点を除き、実施形態に係る状態推定装置と同様に構成される。そして、変形例１に係るカルマンフィルタ演算部３００ａは、状態遷移モデル部４１０を備える代わりに、状態遷移モデル部４１０ａを備える点と、観測モデル部４２０を備える代わりに、観測モデル部４２０ａを備える点とを除いて、カルマンフィルタ演算部３００と同様に構成される。
【００８８】
状態遷移モデル部４１０ａは、状態ベクトルｘ＋ｋ−１を、式（８）に示す状態遷移モデルに適用することで、推定状態ベクトルｘ−ｋを算出する。また、状態遷移モデル部４１０ａは、式（１０）〜式（１２）に示すように、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて、プロセスノイズｗｋの共分散Ｑｋを算出する。
観測モデル部４２０ａは、推定状態ベクトルｘ−ｋを、式（９）に示す観測モデルに適用することにより、推定観測値ベクトルｙ−ｋを算出する。
このように、変形例１に係る非線形カルマンフィルタの演算では、複数のシグマポイントを生成することなく、状態ベクトルｘ＋ｋを更新する。
【００８９】
一般的に非線形カルマンフィルタは、カルマンゲインＫを、プロセスノイズｗの共分散Ｑに基づいて定める。変形例１に係る状態推定装置は、実施形態に係る状態推定装置と同様、プロセスノイズｗｋの共分散Ｑｋを、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて定める。従って、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋが大きな値となる場合であっても、更新後の状態ベクトルｘ＋ｋ＋１の各要素が真値から乖離する可能性を低減させることが可能となる。
すなわち、本発明は、状態推定装置が備えるセンサの温度変化に起因する、状態推定精度の劣化を防止することを目的としたものであり、状態推定装置が使用する非線形カルマンフィルタの種類を問わず適用することが可能である。
【００９０】
（２）変形例２  
上述した実施形態及び変形例に係る状態推定装置は、非線形カルマンフィルタを用いた演算を行うことでシステムの状態を推定するものであったが、本発明はこれに限定されるものではなく、線形カルマンフィルタを用いた演算によりシステムの状態を推定するものであってもよい。すなわち、状態方程式ｆ及び観測方程式ｈは線形関数であってもよい。
一般的に、線形カルマンフィルタは、カルマンゲインＫを、プロセスノイズｗの共分散Ｑに基づいて定める。従って、線形カルマンフィルタを用いた演算によりシステムの状態を推定する場合であっても、プロセスノイズｗｋの共分散Ｑｋを、３次元磁気センサ７０の温度変化量ΔＴｍ，ｋに基づいて定めることにより、３次元磁気センサ７０に温度変化が生じても、状態ベクトルｘｋが真値から乖離した値に更新されることを防止することが可能となる。
【００９１】
（３）変形例３  
上述した実施形態及び変形例に係る状態推定装置は、温度検出部がセンサの内部に配置されるものであったが、本発明はこのような構成に限定されるものではなく、温度検出部がセンサの外部に配置されるものであってもよい。
図５は、変形例３に係る携帯機器１ａを表すブロック図である。携帯機器１ａは、第１温度検出部７５ａを備える点と、３次元磁気センサ７０の代わりに３次元磁気センサ７０ａを備える点とを除き、実施形態に係る携帯機器１と同様に構成される。
３次元磁気センサ７０ａは、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及び、Ｚ軸磁気センサ７３と、これら磁気センサ７１〜７３からの出力信号をＡＤ変換して、磁気データｍを出力する磁気センサＩ／Ｆ７４ａとを備える。第１温度検出部７５ａは、３次元磁気センサ７０ａの温度を検出し、温度データＴｍを出力する。すなわち、第１温度検出部７５ａは、携帯機器１ａの備えるセンサの温度を検出する温度検出部として機能する。
変形例３に係る状態推定装置は、３次元磁気センサ７０ａの外部に配置される第１温度検出部７５ａが、３次元磁気センサ７０ａの温度を検出するため、プロセスノイズｗｋの共分散Ｑｋを、３次元磁気センサ７０ａの温度変化量に基づいて定めることができる。これにより、状態ベクトルｘｋが真値から乖離した値に更新されることを防止することができる。
【００９２】
（４）変形例４  
上述した実施形態及び変形例は、３次元磁気センサ７０の温度を表す温度データＴｍに基づいて、プロセスノイズｗｋの共分散Ｑｋを定めるものであったが、本発明はこのような形態に限定されるものでは無く、３次元磁気センサ７０の温度を表す温度データＴｍに加え、３次元角速度センサ９０の温度を表す温度データＴｇを考慮して、プロセスノイズｗｋの共分散Ｑｋを定めてもよい。
図６は、変形例４に係る携帯機器１ｂを表すブロック図である。携帯機器１ｂは、３次元角速度センサ９０の代わりに、３次元角速度センサ９０ａを備える点を除いて、携帯機器１と同様に構成される。
３次元角速度センサ９０ａは、３次元角速度センサ９０ａの温度を検出する第２温度検出部９５を備える点と、角速度センサＩ／Ｆ９４の代わりに角速度センサＩ／Ｆ９４ａを備える点とを除いて、３次元角速度センサ９０と同様に構成される。角速度センサＩ／Ｆ９４ａは、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及び、Ｚ軸角速度センサ９３からの出力信号をＡＤ変換して角速度データｇを出力するとともに、第２温度検出部９５からの出力信号をＡＤ変換して、温度データＴｇを出力する。
すなわち、変形例４において、第１温度検出部７５及び第２温度検出部９５は、携帯機器１ｂの備えるセンサの温度を検出する温度検出部６００として機能する。
【００９３】
変形例４に係る状態推定装置において、プロセスノイズｗｋの共分散Ｑｋは、以下の式（４１）に示す値に設定される。すなわち、プロセスノイズｗｋの共分散Ｑｋは、以下の式（４１）に示すように、行列Ｑｃｓｔ，１１、行列Ｑｃｓｔ，１２、行列Ｑｃｓｔ，１３、行列Ｑｃｓｔ，２３、行列Ｑｇ，ｋ、及び、行列Ｑｍ，ｋにより構成される。
ここで、行列Ｑｃｓｔ，１１は９行９列の対称行列であり、行列Ｑｃｓｔ，１１の各成分は定数である。また、行列Ｑｃｓｔ，１２は９行３列の零行列であり、行列Ｑｃｓｔ，１３は９行３列の零行列であり、行列Ｑｃｓｔ，２３は３行３列の零行列である。行列Ｑｍ，ｋは、実施形態と同様に、式（１１）及び式（１２）により定められる３行３列の対称行列である。
行列Ｑｇ，ｋは、以下の式（４２）及び式（４３）に基づいて定められる。ここで、温度変化量ΔＴｇ，ｋは、以下の式（４３）に示すように、時刻Ｔ＝ｋにおける温度データＴｇ，ｋと、時刻Ｔ＝ｋ−１における温度データＴｇ，ｋ−１との差分の絶対値を表す値である。また、係数αｇは、正の実数である。行列Ｑｃｓｔ，ｇは、３次元角速度センサ９０ａに温度変化が生じても３次元角速度センサ９０ａのオフセットが変化しないと仮定した状態遷移モデルにおいて、状態ベクトルｘｋに生じるプロセスノイズｗｋのうち、３次元角速度センサ９０ａのオフセット推定値ｇＯＦＦ，ｋに生じるプロセスノイズの共分散を表す行列である。本実施形態では、行列Ｑｃｓｔ，ｇの各成分は定数とする。
【数２４】
2013122384.tif 000026
【００９４】
このように、変形例４に係る状態推定装置は、温度検出部６００を備えるため、３次元磁気センサ７０の温度変化と、３次元角速度センサ９０ａの温度変化との双方を考慮して、状態ベクトルｘｋを更新することが可能となる。
これにより、３次元磁気センサ７０及び３次元角速度センサ９０ａの双方の温度が変化した場合であっても、状態ベクトルｘｋの各要素が真値から乖離した値となる可能性を低減することが可能となり、正確且つ高速な状態推定が可能となる。
【００９５】
なお、変形例４では、３次元角速度センサ９０ａのオフセット推定値ｇＯＦＦと、３次元磁気センサ７０のオフセット推定値ｍＯＦＦとを要素とする６次元のベクトルが、オフセット推定ベクトルに該当する。
【００９６】
変形例４において、温度検出部６００は、３次元磁気センサ７０及び３次元角速度センサ９０ａの内部に配置されているが、温度検出部６００は、３次元磁気センサ７０及び３次元角速度センサ９０ａの外部に配置されるものであってもよい。
具体的には、第１温度検出部７５は３次元磁気センサ７０の外部に配置されるものであってもよく、第２温度検出部９５は３次元角速度センサ９０ａの外部に配置されるものであってもよい。
【００９７】
（５）変形例５  
上述した実施形態及び変形例において、状態推定装置は、３次元磁気センサ７０、３次元加速度センサ８０、及び３次元角速度センサ９０を備えたが、本発明はこのような形態に限定されるものではなく、これら３種類のセンサのうち一部のセンサのみを備えるものであってもよい。また、これら３種類のセンサ以外のセンサを備えるものであってもよい。
つまり、本発明に係る状態推定装置は、異種の物理量を測定する複数のセンサを備え、これら複数のセンサの出力を統合しシステムの状態を推定するカルマンフィルタの演算を行うものであればよい。
【００９８】
例えば、状態推定装置が、異種の物理量を測定する５つのセンサを備え、当該５つのセンサのうち、３つのセンサ（以下、「第１センサ群」と称する）の各々のオフセットを推定の対象とする場合を想定する。
この場合、状態推定装置は、第１センサ群を構成する３つのセンサのうち、１以上のセンサ（以下、「第２センサ群」と称する）の各々の温度を検出する温度検出部を備えるものであってもよい。この例において、温度検出部は、第２センサ群を構成する１以上のセンサの各々に対応する１以上の温度データを出力する。そして、状態推定装置は、温度検出部が出力する１以上の温度データの各々の変化量に基づいて、状態遷移モデルにおいて生じるプロセスノイズの共分散を定める。
すなわち、この例において、第２センサ群を構成する１以上のセンサの各々のオフセットを推定した値を要素とするベクトルが、オフセット推定ベクトルに該当する。そして、状態推定装置は、状態遷移モデルにおいて規定されるプロセスノイズの共分散のうち、オフセット推定ベクトルに生じるプロセスノイズの共分散を、温度検出部が出力する１以上の温度データの各々の変化量に基づいて定める。
これにより、第２センサ群を構成する１以上のセンサのうちいずれかの温度が変化した場合であっても、状態ベクトルｘｋの各要素が真値から乖離した値となる可能性を低減することが可能となり、正確且つ高速な状態推定が可能となる。
【００９９】
なお、変形例５に係る状態推定装置は、５つのセンサを備え、第１センサ群は３つのセンサより構成されているが、これはあくまでも例示である。すなわち、本発明に係る状態推定装置は、２以上のセンサを備えるものであればよく、第１センサ群は、状態推定装置の備える２以上のセンサのうち１以上のセンサから構成されるものであればよい。また、この場合、第２センサ群は、第１センサ群を構成する１以上のセンサの全てを含んでもよいし、第１センサ群を構成する１以上のセンサの一部のセンサから構成されるものであってもよい。
状態推定装置が備えるセンサの種類や、センサの個数に関わらず、センサの温度変化量に基づいてプロセスノイズの共分散を定めることにより、状態ベクトルｘｋが真値から乖離した値に更新されることを防止することが可能となる。
【符号の説明】
【０１００】
１…携帯機器、１０…ＣＰＵ、５０…表示部、７０…３次元磁気センサ、７５…第１温度検出部、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定プログラム、２００…初期値生成部、３００…カルマンフィルタ演算部、５００…出力情報生成部５００、ｍ…磁気データ、Ｔｍ…温度データ、ΔＴｍ，ｋ…温度変化量、ｘ…状態ベクトル、ｗｋ…プロセスノイズ、Ｑｋ…プロセスノイズの共分散、ｙ…観測値ベクトル、ｚ…観測残差。
【図１】
2013122384.tif 000027
【図２】
2013122384.tif 000028
【図３】
2013122384.tif 000029
【図４】
2013122384.tif 000030
【図５】
2013122384.tif 000031
【図６】
2013122384.tif 000032