カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム
【課題】カルマンフィルタの計算量を低減する
【解決手段】カルマンフィルタＫＦは、姿勢ｑ＋ｋ−１及びベクトルβ＋ｋ−１を要素とする状態ベクトルｘ＋ｋ−１を、状態遷移モデルに適用して、姿勢ｑ−ｋ及びベクトルβ−ｋを要素とする状態ベクトルｘ−ｋを算出する推定状態ベクトル算出部１４０と、状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋを算出する共分散算出部１２５とを備える。推定状態ベクトル算出部は、ベクトルβ−ｋをベクトルβ＋ｋ−１と等しい値に設定し、共分散算出部１２５は、共分散Ｐ−ｋのうち、ベクトルβ−ｋの推定誤差の共分散を表す成分Ｐ−ββ，ｋを、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１のうち、ベクトルβ＋ｋ−１の推定誤差の共分散を表す成分Ｐ＋ββ，ｋ−１、及び、状態遷移モデルのプロセスノイズの共分散（Ｑｋ）のうち、ベクトルβ＋ｋ−１のプロセスノイズの共分散を表す成分Ｑββの和として算出する。
【解決手段】カルマンフィルタＫＦは、姿勢ｑ＋ｋ−１及びベクトルβ＋ｋ−１を要素とする状態ベクトルｘ＋ｋ−１を、状態遷移モデルに適用して、姿勢ｑ−ｋ及びベクトルβ−ｋを要素とする状態ベクトルｘ−ｋを算出する推定状態ベクトル算出部１４０と、状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋを算出する共分散算出部１２５とを備える。推定状態ベクトル算出部は、ベクトルβ−ｋをベクトルβ＋ｋ−１と等しい値に設定し、共分散算出部１２５は、共分散Ｐ−ｋのうち、ベクトルβ−ｋの推定誤差の共分散を表す成分Ｐ−ββ，ｋを、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１のうち、ベクトルβ＋ｋ−１の推定誤差の共分散を表す成分Ｐ＋ββ，ｋ−１、及び、状態遷移モデルのプロセスノイズの共分散（Ｑｋ）のうち、ベクトルβ＋ｋ−１のプロセスノイズの共分散を表す成分Ｑββの和として算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラムに関する。
【背景技術】
【０００２】
近年、携帯機器の高性能化及び多用途化に伴い、地磁気の方向や携帯機器の姿勢等の推定機能を備える携帯機器の研究開発が盛んに行われている。地磁気の方向や携帯機器の姿勢等を推定する場合、地磁気センサ等の単体のセンサからの出力のみを利用して推定するよりも、異種の物理量を測定する複数のセンサからの出力を統合して推定する方が、より高速に正確な値を推定することができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、拡張カルマンフィルタ（ＥＫＦ）やシグマポイントカルマンフィルタ（ＳＰＫＦ）等の非線形カルマンフィルタを用いる方法が知られている。
例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、アンセンテッド変換を用いたシグマポイントカルマンフィルタによって統合し、姿勢を推定する方法が開示されている。シグマポイントカルマンフィルタは、他のカルマンフィルタに比べて、非線形システムのモデル化に適した演算方法であり、シグマポイントカルマンフィルタを用いることで、非線形な動的システムの状態を正確に推定することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００５】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、シグマポイントカルマンフィルタによる演算は計算量が多いため、シグマポイントカルマンフィルタが実装される機器には、大容量のメモリと高性能のＣＰＵとを備えることが必要となる。そして、シグマポイントカルマンフィルタによる演算を行う場合、多くの電力が消費される。特に、シグマポイントカルマンフィルタを実装した機器が、バッテリーで給電される携帯機器の場合、機器の長時間使用が困難になるという課題が存在した。
本発明は、上述した点に鑑みてなされたものであり、シグマポイントカルマンフィルタの演算を簡素化し、高速且つ低消費電力な状態推定装置の実現を解決課題とする。
【課題を解決するための手段】
【０００７】
以下、本発明について説明する。なお、本発明の理解を容易にするために本実施形態、変形例、及び添付図面の参照符号を括弧書きにて付記するが、それにより本発明が本実施形態に限定されるものではない。
【０００８】
上記課題を解決するため、本発明に係るカルマンフィルタは、第１ベクトル（β＋ｋ−１）及び第２ベクトル（ｑ＋ｋ−１）を要素とする状態ベクトル（ｘ＋ｋ−１）と、前記状態ベクトルの推定誤差の共分散（Ｐ＋ｋ−１）と、に基づいて、状態遷移モデル（ｆ）を用いて、推定第１ベクトル（β−ｋ）及び推定第２ベクトル（ｑ−ｋ）を要素とする推定状態ベクトル（ｘ−ｋ）を算出する推定状態ベクトル算出部と、前記推定状態ベクトルの推定誤差の共分散（Ｐ−ｋ）を算出する共分散算出部と、前記状態遷移モデル及び観測モデル（ｈ）を用いて、前記状態ベクトルから、推定観測値ベクトル（ｙ−ｋ）を算出する推定観測値ベクトル算出部と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトル（ｙｋ）とに基づいて、観測残差（ｅｋ）を算出する観測残差生成部と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、前記推定状態ベクトル算出部は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分（Ｐ＋ββ，ｋ−１）、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分（Ｑββ）を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分（Ｐ−ββ，ｋ）を算出する、ことを特徴とする。
【０００９】
この発明によれば、共分散算出部は、推定状態ベクトルの推定誤差の共分散のうち推定第１ベクトルの推定誤差の共分散を表す成分を、２個の行列を加算することで算出する。
一般的に、あるベクトルの推定誤差の共分散は、まず、当該あるベクトルの推定誤差を表すベクトルを算出し、その後、推定誤差を表すベクトルと、推定誤差を表すベクトルを転置したベクトルとの積を算出することにより求められる。よって、あるベクトルの推定誤差の共分散を求めるためには、推定誤差を表すベクトルを算出する演算の他に、当該あるベクトルの次元を表す数の二乗に相当する回数の演算が必要になる。
シグマポイントカルマンフィルタは、複数のシグマポイントに基づいて、推定状態ベクトルの推定誤差を表すベクトルを複数生成し、生成した複数の推定誤差を表すベクトルを用いて、推定状態ベクトルの推定誤差の共分散を算出する。従って、推定状態ベクトルの推定誤差の共分散を求めるためには、推定状態ベクトルの次元を表す数の二乗に相当する回数の演算を、更に、シグマポイントの個数に相当する回数だけ繰り返すことが必要となる。このように、推定状態ベクトルの推定誤差の共分散を求める演算は、計算量が多く、シグマポイントカルマンフィルタの処理負荷を増大させる要因となっていた。
これに対して、この発明の共分散算出部は、推定状態ベクトルの推定誤差の共分散のうちの一部の成分を、２つの行列の加算という単純な演算により算出するため、従来の方法に従って計算した場合に比べて、推定状態ベクトルの推定誤差の共分散を求める演算に係る計算量の大幅な低減を可能とした。これにより、シグマポイントカルマンフィルタの演算の高速化と、シグマポイントカルマンフィルタの演算に必要な電力の低減が可能となった。
なお、この発明の推定状態ベクトル算出部は、推定第１ベクトルを第１ベクトルと等しい値に設定する。しかし、第１ベクトルは、カルマンフィルタの演算において、値を全く変化させない訳ではない。すなわち、推定第１ベクトルを要素として含む推定状態ベクトルは、更新部において観測残差による更新がなされ、カルマンフィルタの推定対象であるシステムの状態を正確に表す値（真値）へと更新される。これにより、この発明に係るカルマンフィルタは、システムの状態を正確に推定することができる。
【００１０】
また、上述したカルマンフィルタにおいて、前記推定状態ベクトル算出部は、前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイント（χ＋ｋ−１）を生成するシグマポイント生成部と、前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイント（χ−ｋ）を算出する状態遷移モデル部と、前記推定状態ベクトル（ｘ−ｋ）を算出する平均算出部と、を備え、前記状態遷移モデル部は、前記推定シグマポイントのうち、前記推定第２ベクトル（ｑ−ｋ）を算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第１ベクトル（β＋ｋ−１）に基づいて生成された要素と等しい値に設定し、前記平均算出部は、前記推定第１ベクトル（β−ｋ）を、前記第１ベクトル（β＋ｋ−１）と等しい値に設定し、前記推定第２ベクトル（ｑ−ｋ）を、前記複数の推定シグマポイント（χ−ｋ）に基づいて算出することが好ましい。
【００１１】
この発明によれば、状態ベクトルの推定値を、複数のシグマポイントを用いて算出する。これにより、状態遷移モデルを用いた状態ベクトルの推定において、状態ベクトルが有する誤差による影響と、カルマンフィルタが推定対象とするシステムの非線形性による影響とを低減させることができ、状態遷移モデルにおける推定精度の低下を防止することができる。すなわち、この発明によれば、カルマンフィルタが、システムの状態を正確に推定することが可能となる。
また、この発明によれば、推定第１ベクトルを、第１ベクトルと等しい値に設定し、推定第２ベクトルのみを、複数の推定シグマポイントに基づいて算出するため、推定状態ベクトルの算出に係る計算負荷を低減させることが可能となる。
【００１２】
次に、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、上記のうちいずれかのカルマンフィルタとを備えることを特徴とする。
この発明によれば、高速なカルマンフィルタの演算が可能で、且つ低消費電力な状態推定装置が実現される。
【００１３】
次に、本発明に係るカルマンフィルタの制御方法は、システムの状態を推定するカルマンフィルタの制御方法であって、第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する工程と、前記推定状態ベクトルの推定誤差の共分散を算出する工程と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、前記推定状態ベクトルを算出する工程は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。
【００１４】
この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。
【００１５】
次に、本発明に係るカルマンフィルタの制御プログラムは、システムの状態を推定するカルマンフィルタの制御プログラムであって、第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する処理と、前記推定状態ベクトルの推定誤差の共分散を算出する処理と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、前記推定状態ベクトルを算出する処理は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。
【００１６】
この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係るカルマンフィルタの機能ブロック図である。
【発明を実施するための形態】
【００１８】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００１９】
［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１は、携帯機器１の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表される方位を、現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢等を推定することによって実現される。
【００２０】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ２０、各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成するものである。また、携帯機器１は、地磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００２１】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｍを出力する。この磁気データｍは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。
【００２２】
ところで、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力する磁気データｍは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する地磁気を表すベクトルの他に、機器に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、３次元磁気センサが出力するベクトルデータ（磁気データｍ）から、携帯機器１の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。
このように、検出対象である地磁気の正確な値を得るために、補正処理において、３次元磁気センサ７０から出力されるデータから取り除かれるべき内部磁界の値を３次元磁気センサ７０のオフセットｍＯＦＦと呼ぶ。
なお、地磁気検出部７０は、携帯機器１の外部にある外部物が発する磁界についても検出する。しかし、本実施形態では、外部物が発する磁界は無視できる程小さいものとする。
【００２３】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。したがって、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００２４】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示す３次元のベクトルデータである。なお、角速度データｇには、３次元角速度センサ９０のオフセットｇＯＦＦが重畳する。
【００２５】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、動的システムの状態を表す複数の物理量、例えば、携帯機器１の姿勢、３次元磁気センサ７０のオフセット、地磁気の強さや伏角等の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することにより、状態推定装置として機能する。
【００２６】
状態推定プログラム１００は、初期値生成モジュール１１０と、カルマンフィルタモジュール１２０とを備える。カルマンフィルタモジュール１２０は、カルマンフィルタＫＦの演算を実行する。
【００２７】
［２． カルマンフィルタの観測対象及び推定対象］
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムを観測する複数のセンサが計測する値（観測値）を推定する観測モデルと、を有する。そして、カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量（状態変数）を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定する。次に、カルマンフィルタは、推定された状態ベクトルに基づいて、動的システムの状態を測定する複数のセンサの出力値を要素とする観測値ベクトルの値を推定する。さらに、推定された観測値ベクトルと、実際のセンサの出力値を要素とする観測値ベクトルとの差分として算出される観測残差に基づいて、推定された状態ベクトルを、実際の値（真値）に近い値へと更新し、更新後の状態ベクトルを出力する。
カルマンフィルタは、以上のような演算を繰り返すことにより、状態ベクトルを構成する複数の状態変数の各々を、実際の物理量を正確に表した値（真値）へと近付ける。
【００２８】
本実施形態において、カルマンフィルタＫＦは、携帯機器１の姿勢ｑ、地磁気の強さｒ、地磁気の伏角θ、携帯機器１の角速度ω、３次元角速度センサ９０のオフセットｇＯＦＦの推定値ｇＯ、及び３次元磁気センサ７０のオフセットｍＯＦＦの推定値ｍＯを状態変数として採用し、これらを推定の対象とする。
これらの状態変数を要素とする時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。
【数１】
【００２９】
本実施形態では、姿勢ｑを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。具体的には、地上に固定された座標系（例えば、地上の任意の一点を原点とし、互いに直交する３つの方向、例えば、水平東、水平北、及び鉛直上向きを、それぞれｘ軸、ｙ軸、及びｚ軸とする座標系）を定め、当該地上に固定された座標系の各軸と、携帯機器１に固定された座標系の各軸とが一致する姿勢ｑを基準姿勢と定義し、基準姿勢をｑ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢ｑは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢ｑは、式（２）で与えられる。
【数２】
【００３０】
本実施形態におけるカルマンフィルタの観測対象は、３次元磁気センサ７０から出力される磁気データｍ、３次元加速度センサ８０から出力される加速度データａ、及び３次元角速度センサ９０から出力される角速度データｇである。観測値ベクトルは、これら磁気データｍ、加速度データａ、及び角速度データｇを要素とする。時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数３】
【００３１】
初期値生成モジュール１１０は、時刻Ｔ＝０における状態変数を要素とする初期値ＩＮＩを算出する。初期値ＩＮＩは、姿勢ｑの初期値ｑ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角θの初期値θ０、角速度ωの初期値ω０、３次元角速度センサ９０のオフセット推定値ｇＯの初期値ｇＯ,０、及び３次元磁気センサ７０のオフセット推定値ｍＯの初期値ｍＯ,０を要素として含む。
初期値ＩＮＩは、カルマンフィルタの演算によって状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下のような値に設定してもよい。
【００３２】
地磁気の強さｒの初期値ｒ０、及び地磁気の伏角θの初期値θ０は、例えば、ＧＰＳ部６０から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ１が格納される。初期値生成モジュール１１０は、ルックアップテーブルＬＵＴ１を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さｒの初期値ｒ０及び地磁気の伏角θの初期値θ０として設定する。
なお、携帯機器１が衛星からの電波の届かない場所（例えば、地下街）にある場合には、ＧＰＳ部６０から位置情報が得られない。そのような場合には、携帯機器１が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルＬＵＴ１に格納されている。初期値生成モジュール１１０は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルＬＵＴ１から読み出す。例えば、日本で販売された携帯機器１については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さｒの初期値ｒ０、及び地磁気の伏角θの初期値θ０を算出する。
【００３３】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。３次元角速度センサ９０のオフセット推定値ｇＯの初期値ｇＯ,０は、通常「０」近辺に調整されているため、「０」に設定する。姿勢ｑの初期値ｑ０に関しては、例えば、携帯機器１が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。
【００３４】
３次元磁気センサ７０のオフセット推定値ｍＯの初期値ｍＯ,０は、例えば、時刻Ｔ＝０の３次元磁気センサ７０の観測値ｍ０、姿勢ｑの初期値ｑ０、携帯機器１を使用する地域の地磁気ベクトルｍtypical、及び、式（５）に示す行列Ｂ（ｑ）を用いて、以下に示す式（４）で与えられる値に設定する。ここで、行列Ｂ（ｑ）は、携帯機器１が姿勢ｑである場合に、地上に固定された座標系において表現されたベクトルを、携帯機器１に固定された座標系において表現するための座標変換行列である。なお、ＲＯＭ３０には、位置情報と地磁気ベクトルｍtypicalとを対応づけて記憶したルックアップテーブルＬＵＴ２が記憶されている。初期値生成モジュール１１０は、ＧＰＳ部６０で生成される位置情報に基づいてルックアップテーブルＬＵＴ２を参照して地磁気ベクトルｍtypicalを取得し、式（４）を演算することによって、オフセット推定値ｍＯの初期値ｍＯ,０を得る。
【数４】
【００３５】
初期値生成モジュール１１０は、以上のようにして生成された姿勢ｑの初期値ｑ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角θの初期値θ０、角速度ωの初期値ω０、３次元角速度センサ９０のオフセット推定値ｇＯの初期値ｇＯ,０、及び３次元磁気センサ７０のオフセット推定値ｍＯの初期値ｍＯ,０を要素とするベクトルである初期値ＩＮＩを生成し、これを出力する。
【００３６】
［３． カルマンフィルタによる演算］
次に、本実施形態に係るカルマンフィルタＫＦによる演算の概要について説明する。
カルマンフィルタＫＦは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定し、この推定値を、推定された状態ベクトル（推定状態ベクトル）ｘ−ｋとして出力する。そして、カルマンフィルタＫＦは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、状態遷移モデルにより推定された状態ベクトルｘ−ｋに基づいて観測値ベクトルｙｋを推定し、この推定値を、推定された観測値ベクトル（推定観測値ベクトル）ｙ−ｋとして出力する。なお、本実施形態のカルマンフィルタＫＦは、非線形カルマンフィルタであるシグマポイントカルマンフィルタにより構成される。シグマポイントカルマンフィルタとは、状態ベクトルｘｋ−１の周囲に複数のシグマポイントχｋ−１を設定し、これらの複数のシグマポイントχｋ−１を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ−ｋを推定し、シグマポイントの推定値（推定シグマポイント）χ−ｋの平均を算出することにより、推定された状態ベクトルｘ−ｋを求める演算である。従って、推定された観測値ベクトルｙ−ｋは、厳密には、推定された状態ベクトルｘ−ｋの周辺に存在する複数のシグマポイントの推定値χ−ｋに基づいて算出される。
その後、カルマンフィルタＫＦは、推定された観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｅｋとして算出する。また、カルマンフィルタＫＦは、推定された状態ベクトルｘ−ｋと、推定された観測値ベクトルｙ−ｋと、に基づいてカルマンゲインＫｋを算出する。そして、カルマンフィルタＫＦは、観測残差ｅｋとカルマンゲインＫｋとを用いて推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトル（状態ベクトルを更新したベクトル）ｘ＋ｋを算出する。
以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタＫＦの演算により、状態ベクトルｘｋを実際の物理量を正確に表した値（真値）に近い正確な値へと近付けることができる。
【００３７】
［３．１． カルマンフィルタ演算部の動作の概要］
本実施形態におけるカルマンフィルタＫＦの状態遷移モデルは以下に示す式（６）で与えられ、観測モデルは以下に示す式（７）で与えられる。本実施形態では、式（６）に現れる関数ｆ及び式（７）に現れる関数ｈは、非線形の関数である。
【数５】
【００３８】
ここで、状態ベクトルｘｋはｎ次元のベクトルであり、観測値ベクトルｙｋはｍ次元のベクトルである。なお、本実施形態では、ｎ＝１５であり、ｍ＝９である。また、式（６）に現れるプロセスノイズｕｋ及び式（７）に現れる観測ノイズｖｋは０を中心とするガウスノイズである。
式（６）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｕｋ−１とを加算することにより推定されるものであることを示している。
また、式（７）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｕｋの共分散をＱ、観測ノイズｖｋの共分散をＲと表す。
【００３９】
時刻Ｔ＝ｋにおける観測残差ｅｋは、実際の観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（８）で表される。カルマンフィルタＫＦは、観測残差ｅｋと、式（９）に示すカルマンゲインＫｋとを用いて、以下の式（１０）に示すように、推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタＫＦは、以下の式（１１）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
なお、Ｐ−ｋは、推定された状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。
また、Ｐｙｙｋは、観測残差ｅｋの共分散であり、Ｐｘｙｋは、推定された状態ベクトルｘ−ｋと、推定された観測値ベクトルｙ−ｋとの相互共分散行列である。
【数６】
【００４０】
観測モデルを用いて推定された観測値ベクトルｙ−ｋは、式（７）に示すように、観測モデルを用いて推定された状態ベクトルｘ−ｋに基づいて算出される理論値である。よって、観測モデルを用いて推定された観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｅｋは、推定された状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
カルマンフィルタＫＦは、式（１０）に示すように、観測残差ｅｋを用いて、推定された状態ベクトルｘ−ｋを、真値に近い更新後の状態ベクトルｘ＋ｋへと更新する。
【００４１】
［３．２． シグマポイントカルマンフィルタによる演算］
本実施形態において、カルマンフィルタＫＦは、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、図３に示すカルマンフィルタＫＦの機能ブロック図を参照しつつ、カルマンフィルタＫＦによる演算の詳細を説明する。
【００４２】
遅延部１２１は、加算器１３２から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部１２２に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部１２１は、初期値生成モジュール１１０が出力する初期値ＩＮＩを用いて、状態ベクトルｘ＋ｋ−１を生成する。
また、遅延部１２１は、減算器１３３から出力される更新後の状態ベクトルｘ＋ｋの推定誤差の共分散Ｐ＋ｋを単位時間遅延させることで、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１を生成し、これを、シグマポイント生成部１２２及び共分散算出部１２５に対して出力する。
【００４３】
シグマポイント生成部１２２は、状態ベクトルｘ＋ｋ−１に基づいて、ａ個のシグマポイントχ＋ｋ−１（ｉ）（ｉ＝1，2，…ａ）を生成する（ａは、２以上の自然数）。シグマポイントχ＋ｋ−１（ｉ）の生成は、以下に示す公知の方法を適用する。
シグマポイント生成部１２２は、まず、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１から、式（１２）に示す共分散Ｐ＋ｋ−１の平方根を表すｎ行ｎ列の行列を生成する。次に、シグマポイント生成部１２２は、共分散Ｐ＋ｋ−１の平方根を表す行列と、各種係数とに基づいて、ａ個のシグマポイント算出用ベクトルを定める。シグマポイント算出用ベクトルは、ｎ次元のベクトルである。その後、シグマポイント生成部１２２は、状態ベクトルｘ＋ｋ−１と、ａ個のシグマポイント算出用ベクトルとに基づいて、ａ個のシグマポイントχ＋ｋ−１（ｉ）を生成し、これを状態遷移モデル部１２３に対して出力する。
なお、ａ個のシグマポイントχ＋ｋ−１（ｉ）の生成において用いられる各種係数は、後述する式（１４）における重みｗｉの生成にも用いられる値であり、公知の方法により定められる。また、ａ個のシグマポイント算出用ベクトルには０ベクトルが含まれていても構わない。
【数７】
【００４４】
状態遷移モデル部１２３は、式（１３）に示すように、時刻Ｔ＝ｋ−１におけるａ個のシグマポイントχ＋ｋ−１（ｉ）の各々を、状態遷移モデルに適用することにより、時刻Ｔ＝ｋにおけるａ個のシグマポイントの推定値χ−ｋ（ｉ）を算出する。
【数８】
【００４５】
次に、平均算出部１２４は、式（１４）に示すように、時刻Ｔ＝ｋにおけるａ個のシグマポイントの推定値χ−ｋ（ｉ）の重み付き平均を計算することで、推定された状態ベクトルｘ−ｋを算出し、これを出力する。なお、式（１４）に現れる重みｗｉは、シグマポイントχ＋ｋ−１（ｉ）の生成において用いられた各種係数に基づいて定められる。
このように、シグマポイント生成部１２２、状態遷移モデル部１２３、及び平均算出部１２４は、状態ベクトルｘ＋ｋ−１から、推定された状態ベクトルｘ−ｋを算出する推定状態ベクトル算出部１４０として機能する。
【数９】
【００４６】
共分散算出部１２５は、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋを算出する。推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋは、式（１５）に示すように、ａ個のシグマポイントの推定値χ−ｋ（ｉ）、推定された状態ベクトルｘ−ｋ、重みｗｉ、及びプロセスノイズｕｋ−１の共分散Ｑに基づいて算出することができる。
但し、本実施形態では、処理負荷を削減する観点から、共分散Ｐ−ｋのうち一部の成分は、式（１５）を用いずに、後述する式（３０）に基づいて算出する。この点についての詳細は後述する。
【数１０】
【００４７】
一方、観測モデル部１２６は、式（１６）に示すように、時刻Ｔ＝ｋにおけるａ個のシグマポイントの推定値χ−ｋ（ｉ）の各々を、観測モデルに適用することにより、ａ個の推定観測値γｋ（ｉ）を算出する。
【数１１】
【００４８】
そして、平均処理部１２７は、式（１７）に示すように、ａ個の推定観測値γｋ（ｉ）の平均を演算することにより、推定された観測値ベクトルｙ−ｋを算出する。
このように、シグマポイント生成部１２２、状態遷移モデル部１２３、観測モデル部１２６、及び平均処理部１２７は、状態ベクトルｘ＋ｋ−１から、推定された観測値ベクトルｙ−ｋを算出する推定観測値ベクトル算出部１５０として機能する。
【数１２】
【００４９】
次に、平均処理部１２７は、式（１８）に示す観測残差の共分散Ｐｙｙｋを算出する。
【数１３】
【００５０】
減算器（観測残差生成部）１３１は、式（８）に示したように、観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとの差分として、観測残差ｅｋを算出する。
カルマンゲイン付与部１２８は、式（１９）に示す相互共分散行列Ｐｘｙｋを算出する。次に、カルマンゲイン付与部１２８は、式（９）に示したように、観測残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出する。その後、カルマンゲイン付与部１２８は、式（１０）の右辺第２項（Ｋｋｅｋ）を演算し、その結果を加算器１３２に対して出力する。
また、カルマンゲイン付与部１２８は、観測残差の共分散ＰｙｙｋとカルマンゲインＫｋとに基づいて、式（１１）の右辺第２項（ＫｋＰｙｙｋＫＴｋ）を演算し、その結果を減算器１３３に対して出力する。
【数１４】
【００５１】
加算器（更新部）１３２は、式（１０）に示したように、推定された状態ベクトルｘ−ｋと、カルマンゲイン付与部１２８から出力される式（１０）の右辺第２項（Ｋｋｅｋ）と、を加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
減算器１３３は、式（１１）に示したように、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋと、カルマンゲイン付与部１２８から出力される式（１１）の右辺第２項（ＫｋＰｙｙｋＫＴｋ）との差分として、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散Ｐ＋ｋを算出する。
【００５２】
［３．３． 状態遷移モデル］
次に、状態遷移モデル部１２３の演算で用いる状態遷移モデルについて説明する。
【００５３】
本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘｋを構成する状態変数のうち、姿勢ｑについての状態遷移は、式（２１）に示す関数ｆｑ（ｑ，ω）を用いて、式（２０）により定義される。式（２０）は、時刻Ｔ＝ｋにおける推定された状態ベクトルｘ−ｋを構成する状態変数である姿勢ｑ−ｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘ＋ｋ−１を構成する状態変数である姿勢ｑ＋ｋ−１及び角速度ω＋ｋ−１に基づいて推定されることを示している。
式（２１）に示す関数ｆｑ（ｑ，ω）は、式（６）に示した関数ｆのうち、姿勢ｑを算出する部分のみを表した関数である。式（２１）の右辺の演算子Ωは、式（２２）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表し、３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、［ｌ×］という記号を式（２３）で定義する。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表したとき、演算子Ωを構成する成分Ψを、式（２４）で定義する。
【数１５】
【００５４】
姿勢ｑは、クォータニオンで表現されるため、正規化条件||ｑ||＝１が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘｋを更新する場合、更新後の状態ベクトルｘ＋ｋは、式（１４）に示すように、シグマポイントの推定値χ−ｋ（ｉ）に重みｗｉを付与した重み付き平均として算出されるため、（更新前の）状態ベクトルｘｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘｋ−１の要素である姿勢ｑｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋの要素である姿勢ｑｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢ｑについての正規化条件が満たされなくなる可能性が存在する。
そこで、姿勢ｑに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルｘｋを構成する要素のうち姿勢ｑｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
【００５５】
地磁気の強さｒ及び地磁気の伏角θは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角θｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角θｋ−１とは等しい値であるという前提を置く。
同様に、３次元角速度センサ９０のオフセット推定値ｇＯも、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおけるオフセット推定値ｇＯ,Ｋと、時刻Ｔ＝ｋ−１におけるオフセット推定値ｇＯ,Ｋ−１とは等しい値であるという前提を置く。
【００５６】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００５７】
前述のとおり、３次元磁気センサ７０のオフセットｍＯＦＦは、携帯機器１の部品が発する内部磁界である。従って、携帯機器１の内部状態が一定である間は、オフセットｍＯＦＦも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、オフセットｍＯＦＦも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、オフセットｍＯＦＦが変化するタイミングやオフセットｍＯＦＦの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１におけるオフセット推定値ｍＯ,Ｋ−１を用いて、時刻Ｔ＝ｋにおけるオフセット推定値ｍＯ,ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおけるオフセット推定値ｍＯ,ｋと、時刻Ｔ＝ｋ−１におけるオフセット推定値ｍＯ,ｋ−１とは等しいと仮定する。
【００５８】
このように、本実施形態に係る状態遷移モデルでは、以下に示す式（２５）のように、状態ベクトルｘｋを構成する状態変数のうち、姿勢ｑ以外は、前の時刻から変化しないという前提を置く。
【数１６】
【００５９】
ここで、式（１）に示した状態ベクトルｘｋを、式（２６）のように、姿勢ｑｋと、姿勢ｑｋ以外の要素を表すベクトルβｋとに分離して表現した場合、ベクトルβｋについて、式（２７）が成立する。すなわち、状態ベクトルｘ＋ｋ−１のうち、姿勢（第２ベクトル）ｑ＋ｋ−１以外の要素を表すベクトル（第１ベクトル）β＋ｋ−１と、推定された状態ベクトルｘ−ｋのうち、姿勢（推定第２ベクトル）ｑ−ｋ以外の要素を表すベクトル（推定第１ベクトル）β−ｋとは等しい。
【数１７】
【００６０】
式（２７）が成立する場合、式（１３）から明らかなように、シグマポイントχ＋ｋ−１（ｉ）のうちベクトルβ＋ｋ−１に相当する要素と、シグマポイントの推定値χ−ｋ（ｉ）のうちベクトルβ−ｋに相当する要素とは、等しくなる。
従って、式（２７）が成立する場合、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１のうち、ベクトルβ＋ｋ−１の推定誤差の共分散を表す成分と、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋのうち、ベクトルβ−ｋの推定誤差の共分散を表す成分とは、プロセスノイズｕｋ−１の共分散Ｑを考慮しなければ、等しいものと看做すことができる（段落［００４６］参照）。
【００６１】
ここで、共分散Ｐｋを、式（２８）に示すように、行列Ｐｑｑ，ｋ、行列Ｐｑβ，ｋ、行列Ｐｑβ，ｋＴ、及び行列Ｐββ，ｋの、４つの行列に分割する。行列Ｐｑｑ，ｋは、姿勢ｑｋに基づいて定められる、４行４列の行列である。行列Ｐｑβ，ｋは、姿勢ｑｋとベクトルβｋとに基づいて定められる、４行１１列の行列である。行列Ｐββ，ｋは、ベクトルβｋに基づいて定められる、１１行１１列の行列である。具体的には、行列Ｐββ，ｋは、ベクトルβｋの推定誤差の共分散を表す行列である。
【数１８】
【００６２】
また、プロセスノイズｕｋの共分散Ｑを、式（２９）のように、４行４列の行列Ｑｑｑ、４行１１列の行列Ｑｑβ、及び１１行１１列の行列Ｑββを用いて表す。
式（６）に示したとおり、プロセスノイズｕｋは、状態ベクトルｘｋを状態遷移モデルに適用する際に生じるプロセスノイズであり、式（２９）に示す共分散Ｑは、当該プロセスノイズｕｋの共分散を表す。従って、式（２９）に現れる、行列Ｑββは、状態ベクトルｘｋを状態遷移モデルに適用する際に生じるプロセスノイズのうち、状態ベクトルｘｋの中で姿勢ｑｋ以外の要素を表すベクトルβｋに生じるプロセスノイズの共分散を表す。
このとき、行列Ｐ−ββ，ｋは、式（３０）に示すように、行列Ｐ＋ββ，ｋ−１と、行列Ｑββとの和として定められる。
【数１９】
【００６３】
共分散Ｐ−ｋの全ての成分を式（１５）に基づいて求める場合、ｎ行１列のベクトルと１行ｎ列のベクトルとを乗算してｎ行ｎ列の行列を求める演算を、ａ回繰り返すことが必要になる。
一方、式（３０）を用いる場合、行列Ｐ−ββ，ｋは、１ステップ前に（時刻Ｔ＝ｋ−１において）既に算出されている行列Ｐ＋ββ，ｋ−１と、行列Ｑββとを単純に加算することにより算出される。そして、共分散Ｐ−ｋは、行列Ｐ−ββ，ｋ以外の成分、すなわち、４行４列の行列Ｐ−ｑｑ，ｋ及び４行１１列の行列Ｐ−ｑβ，ｋのみを、式（１５）による演算により求めればよい。この結果、１５行１５列の共分散Ｐ−ｋの全ての成分を、式（１５）を用いて算出する場合に比べて、共分散算出部１２５における計算量を大幅に低減することが可能となる。
【００６４】
なお、状態遷移モデルにおいて、式（２５）及び式（２７）に示すように、姿勢ｑ以外は、前の時刻から変化しないという前提を置いた場合であっても、状態ベクトルｘｋは、カルマンフィルタの演算において観測残差ｅｋに基づいて更新される。従って、姿勢ｑｋ以外の要素を表すベクトルβｋも、カルマンフィルタの演算を繰り返す中で、観測残差ｅｋに基づいて真値に近づくように更新される。
【００６５】
［３．４． 観測モデル］
次に、観測モデル部１２６で行われる演算において用いられる観測モデルについて説明する。
【００６６】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、３次元角速度センサ９０のオフセット推定値ｇＯとを用いて、式（３１）で与えられる。
【数２０】
【００６７】
また、地上に固定された座標系において、地磁気を表すベクトルは、式（３２）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｍの推定値γｍａｇは、３次元磁気センサ７０のオフセット推定値ｍＯと、式（５）で示した行列Ｂ（ｑ）とを用いて、式（３３）で与えられる。
【数２１】
【００６８】
また、地上に固定された座標系において、重力を表すベクトルを重力加速度で正規化したベクトルは、式（３４）で与えられる。従って、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（５）で示した行列Ｂ（ｑ）を用いて、式（３５）で与えられる。
【数２２】
【００６９】
従って、式（３）を式（８）の右辺第１項に代入し、式（３１）、式（３３）、及び式（３５）を用いて式（８）の右辺第２項を表すことにより、式（８）を、以下の式（３６）に変形することができる。このとき、観測残差ｅｋは、式（３６）により算出される。
【数２３】
【００７０】
［４． 結論］
以上に示したように、本実施形態に係る状態推定装置は、シグマポイントカルマンフィルタの演算において用いられる状態ベクトルｘｋを、姿勢ｑｋと、ベクトルβｋとに分離して表現し、状態ベクトルｘ＋ｋ−１を構成する要素の一部であるベクトルβ＋ｋ−１と、推定された状態ベクトルｘ−ｋを構成する要素の一部であるベクトルβ−ｋとが等しいという前提を置いた。
そして、本実施形態に係る状態推定装置は、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋのうちベクトルβ−ｋの推定誤差の共分散を表す成分を、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１のうちベクトルβ＋ｋ−１の推定誤差の共分散を表す成分と、プロセスノイズｕｋ−１の共分散Ｑのうちベクトルβ＋ｋ−１に生じるプロセスノイズの共分散を表す成分とを単純に加算することで算出した。
これにより、共分散Ｐ−ｋの全ての成分を式（１５）に基づいて求める場合と比較して、シグマポイントカルマンフィルタの計算負荷を大幅に低減することが可能となり、状態推定装置の処理速度の向上、及び携帯機器１の低消費電力化が可能となった。
【００７１】
＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【００７２】
（１）変形例１
上述した実施形態では、状態ベクトルｘを構成する要素して、携帯機器１の姿勢ｑ、地磁気の強さｒ、地磁気の伏角θ、携帯機器１の角速度ω、３次元角速度センサ９０のオフセット推定値ｇＯ、及び３次元磁気センサ７０のオフセット推定値ｍＯを採用し、これら６個の状態変数をカルマンフィルタの演算における推定の対象としたが、本発明はこれに限定されるものでは無い。例えば、状態ベクトルｘを、これら６個の状態変数のうちの一部の状態変数により構成し、当該一部の状態変数をカルマンフィルタの演算における推定の対象とするものであってもよい。
【００７３】
（２）変形例２
上述した実施形態では、観測値ベクトルｙを、３次元磁気センサ７０から出力される磁気データｍ、３次元加速度センサ８０から出力される加速度データａ、及び３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、磁気データｍ、加速度データａ、及び角速度データｇのうちの一部のみを利用して観測値ベクトルｙを生成してもよい。
【００７４】
（３）変形例３
上述した実施形態では、平均算出部１２４は、式（１４）に示すように、推定された状態ベクトルｘ−ｋを、ａ個のシグマポイントの推定値χ−ｋ（ｉ）の重み付き平均として算出したが、本発明はこれに限定されるものではない。
例えば、平均算出部１２４は、推定された状態ベクトルｘ−ｋを構成する要素の一部であるベクトルβ−ｋを、式（１４）を用いず、単純に、状態ベクトルｘ＋ｋ−１を構成する要素の一部であるベクトルβ＋ｋ−１と等しい値に設定してもよい。この場合、平均算出部１２４は、推定された状態ベクトルｘ−ｋのうち、姿勢ｑ−ｋについてのみ、式（１４）の重み付き平均を算出する演算により算出する。これにより、１５次元のベクトルである推定された状態ベクトルｘ−ｋのうち、１１次元のベクトルβ−ｋについては、単純な値の代入により算出することが可能となるため、推定された状態ベクトルｘ−ｋの算出に係る処理負荷を、大幅に低減することができる。
【符号の説明】
【００７５】
１…携帯機器、ＫＦ…カルマンフィルタ、１２１…遅延部、１２２…シグマポイント生成部、１２３…状態遷移モデル部、１２４…平均算出部、１２５…共分散算出部、１２６…観測モデル部、１２７…平均処理部、１２７…推定観測値ベクトル算出部、１２８…カルマンゲイン付与部、１３１…減算器、１３２…加算器、１３３…減算器、１４０…推定状態ベクトル算出部、１５０…推定観測値ベクトル算出部、Ｋ…カルマンゲイン、ｘｋ…状態ベクトル、ｘ−ｋ…推定された状態ベクトル、ｑｋ…姿勢、βｋ…ベクトル、Ｐｋ…状態ベクトルの推定誤差の共分散、Ｑ…プロセスノイズの共分散、ｅｋ…観測残差、ｙｋ…観測値ベクトル、ｙ−ｋ…推定された観測値ベクトル。
【技術分野】
【０００１】
本発明は、カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラムに関する。
【背景技術】
【０００２】
近年、携帯機器の高性能化及び多用途化に伴い、地磁気の方向や携帯機器の姿勢等の推定機能を備える携帯機器の研究開発が盛んに行われている。地磁気の方向や携帯機器の姿勢等を推定する場合、地磁気センサ等の単体のセンサからの出力のみを利用して推定するよりも、異種の物理量を測定する複数のセンサからの出力を統合して推定する方が、より高速に正確な値を推定することができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、拡張カルマンフィルタ（ＥＫＦ）やシグマポイントカルマンフィルタ（ＳＰＫＦ）等の非線形カルマンフィルタを用いる方法が知られている。
例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、アンセンテッド変換を用いたシグマポイントカルマンフィルタによって統合し、姿勢を推定する方法が開示されている。シグマポイントカルマンフィルタは、他のカルマンフィルタに比べて、非線形システムのモデル化に適した演算方法であり、シグマポイントカルマンフィルタを用いることで、非線形な動的システムの状態を正確に推定することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００５】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、シグマポイントカルマンフィルタによる演算は計算量が多いため、シグマポイントカルマンフィルタが実装される機器には、大容量のメモリと高性能のＣＰＵとを備えることが必要となる。そして、シグマポイントカルマンフィルタによる演算を行う場合、多くの電力が消費される。特に、シグマポイントカルマンフィルタを実装した機器が、バッテリーで給電される携帯機器の場合、機器の長時間使用が困難になるという課題が存在した。
本発明は、上述した点に鑑みてなされたものであり、シグマポイントカルマンフィルタの演算を簡素化し、高速且つ低消費電力な状態推定装置の実現を解決課題とする。
【課題を解決するための手段】
【０００７】
以下、本発明について説明する。なお、本発明の理解を容易にするために本実施形態、変形例、及び添付図面の参照符号を括弧書きにて付記するが、それにより本発明が本実施形態に限定されるものではない。
【０００８】
上記課題を解決するため、本発明に係るカルマンフィルタは、第１ベクトル（β＋ｋ−１）及び第２ベクトル（ｑ＋ｋ−１）を要素とする状態ベクトル（ｘ＋ｋ−１）と、前記状態ベクトルの推定誤差の共分散（Ｐ＋ｋ−１）と、に基づいて、状態遷移モデル（ｆ）を用いて、推定第１ベクトル（β−ｋ）及び推定第２ベクトル（ｑ−ｋ）を要素とする推定状態ベクトル（ｘ−ｋ）を算出する推定状態ベクトル算出部と、前記推定状態ベクトルの推定誤差の共分散（Ｐ−ｋ）を算出する共分散算出部と、前記状態遷移モデル及び観測モデル（ｈ）を用いて、前記状態ベクトルから、推定観測値ベクトル（ｙ−ｋ）を算出する推定観測値ベクトル算出部と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトル（ｙｋ）とに基づいて、観測残差（ｅｋ）を算出する観測残差生成部と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、前記推定状態ベクトル算出部は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分（Ｐ＋ββ，ｋ−１）、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分（Ｑββ）を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分（Ｐ−ββ，ｋ）を算出する、ことを特徴とする。
【０００９】
この発明によれば、共分散算出部は、推定状態ベクトルの推定誤差の共分散のうち推定第１ベクトルの推定誤差の共分散を表す成分を、２個の行列を加算することで算出する。
一般的に、あるベクトルの推定誤差の共分散は、まず、当該あるベクトルの推定誤差を表すベクトルを算出し、その後、推定誤差を表すベクトルと、推定誤差を表すベクトルを転置したベクトルとの積を算出することにより求められる。よって、あるベクトルの推定誤差の共分散を求めるためには、推定誤差を表すベクトルを算出する演算の他に、当該あるベクトルの次元を表す数の二乗に相当する回数の演算が必要になる。
シグマポイントカルマンフィルタは、複数のシグマポイントに基づいて、推定状態ベクトルの推定誤差を表すベクトルを複数生成し、生成した複数の推定誤差を表すベクトルを用いて、推定状態ベクトルの推定誤差の共分散を算出する。従って、推定状態ベクトルの推定誤差の共分散を求めるためには、推定状態ベクトルの次元を表す数の二乗に相当する回数の演算を、更に、シグマポイントの個数に相当する回数だけ繰り返すことが必要となる。このように、推定状態ベクトルの推定誤差の共分散を求める演算は、計算量が多く、シグマポイントカルマンフィルタの処理負荷を増大させる要因となっていた。
これに対して、この発明の共分散算出部は、推定状態ベクトルの推定誤差の共分散のうちの一部の成分を、２つの行列の加算という単純な演算により算出するため、従来の方法に従って計算した場合に比べて、推定状態ベクトルの推定誤差の共分散を求める演算に係る計算量の大幅な低減を可能とした。これにより、シグマポイントカルマンフィルタの演算の高速化と、シグマポイントカルマンフィルタの演算に必要な電力の低減が可能となった。
なお、この発明の推定状態ベクトル算出部は、推定第１ベクトルを第１ベクトルと等しい値に設定する。しかし、第１ベクトルは、カルマンフィルタの演算において、値を全く変化させない訳ではない。すなわち、推定第１ベクトルを要素として含む推定状態ベクトルは、更新部において観測残差による更新がなされ、カルマンフィルタの推定対象であるシステムの状態を正確に表す値（真値）へと更新される。これにより、この発明に係るカルマンフィルタは、システムの状態を正確に推定することができる。
【００１０】
また、上述したカルマンフィルタにおいて、前記推定状態ベクトル算出部は、前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイント（χ＋ｋ−１）を生成するシグマポイント生成部と、前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイント（χ−ｋ）を算出する状態遷移モデル部と、前記推定状態ベクトル（ｘ−ｋ）を算出する平均算出部と、を備え、前記状態遷移モデル部は、前記推定シグマポイントのうち、前記推定第２ベクトル（ｑ−ｋ）を算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第１ベクトル（β＋ｋ−１）に基づいて生成された要素と等しい値に設定し、前記平均算出部は、前記推定第１ベクトル（β−ｋ）を、前記第１ベクトル（β＋ｋ−１）と等しい値に設定し、前記推定第２ベクトル（ｑ−ｋ）を、前記複数の推定シグマポイント（χ−ｋ）に基づいて算出することが好ましい。
【００１１】
この発明によれば、状態ベクトルの推定値を、複数のシグマポイントを用いて算出する。これにより、状態遷移モデルを用いた状態ベクトルの推定において、状態ベクトルが有する誤差による影響と、カルマンフィルタが推定対象とするシステムの非線形性による影響とを低減させることができ、状態遷移モデルにおける推定精度の低下を防止することができる。すなわち、この発明によれば、カルマンフィルタが、システムの状態を正確に推定することが可能となる。
また、この発明によれば、推定第１ベクトルを、第１ベクトルと等しい値に設定し、推定第２ベクトルのみを、複数の推定シグマポイントに基づいて算出するため、推定状態ベクトルの算出に係る計算負荷を低減させることが可能となる。
【００１２】
次に、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、上記のうちいずれかのカルマンフィルタとを備えることを特徴とする。
この発明によれば、高速なカルマンフィルタの演算が可能で、且つ低消費電力な状態推定装置が実現される。
【００１３】
次に、本発明に係るカルマンフィルタの制御方法は、システムの状態を推定するカルマンフィルタの制御方法であって、第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する工程と、前記推定状態ベクトルの推定誤差の共分散を算出する工程と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、前記推定状態ベクトルを算出する工程は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。
【００１４】
この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。
【００１５】
次に、本発明に係るカルマンフィルタの制御プログラムは、システムの状態を推定するカルマンフィルタの制御プログラムであって、第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する処理と、前記推定状態ベクトルの推定誤差の共分散を算出する処理と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、前記推定状態ベクトルを算出する処理は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。
【００１６】
この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。
【図面の簡単な説明】
【００１７】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】本発明の実施形態に係るカルマンフィルタの機能ブロック図である。
【発明を実施するための形態】
【００１８】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００１９】
［１． 機器構成及びソフトウェア構成］
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１は、携帯機器１の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表される方位を、現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢等を推定することによって実現される。
【００２０】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ２０、各種のプログラムやデータを記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、及びＧＰＳ部６０を備える。ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報（緯度、経度）を生成するものである。また、携帯機器１は、地磁気を検出して磁気データを出力する３次元磁気センサ７０、加速度を検出して加速度データを出力する３次元加速度センサ８０、及び角速度を検出して角速度データを出力する３次元角速度センサ９０を備える。
【００２１】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、各センサからの出力信号をＡＤ変換して磁気データｍを出力する。この磁気データｍは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。
【００２２】
ところで、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力する磁気データｍは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する地磁気を表すベクトルの他に、機器に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、３次元磁気センサが出力するベクトルデータ（磁気データｍ）から、携帯機器１の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。
このように、検出対象である地磁気の正確な値を得るために、補正処理において、３次元磁気センサ７０から出力されるデータから取り除かれるべき内部磁界の値を３次元磁気センサ７０のオフセットｍＯＦＦと呼ぶ。
なお、地磁気検出部７０は、携帯機器１の外部にある外部物が発する磁界についても検出する。しかし、本実施形態では、外部物が発する磁界は無視できる程小さいものとする。
【００２３】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、携帯機器１に固定された座標系における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。したがって、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは携帯機器１に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
【００２４】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、各軸の回りの角速度を示す３次元のベクトルデータである。なお、角速度データｇには、３次元角速度センサ９０のオフセットｇＯＦＦが重畳する。
【００２５】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、動的システムの状態を表す複数の物理量、例えば、携帯機器１の姿勢、３次元磁気センサ７０のオフセット、地磁気の強さや伏角等の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することにより、状態推定装置として機能する。
【００２６】
状態推定プログラム１００は、初期値生成モジュール１１０と、カルマンフィルタモジュール１２０とを備える。カルマンフィルタモジュール１２０は、カルマンフィルタＫＦの演算を実行する。
【００２７】
［２． カルマンフィルタの観測対象及び推定対象］
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムを観測する複数のセンサが計測する値（観測値）を推定する観測モデルと、を有する。そして、カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量（状態変数）を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定する。次に、カルマンフィルタは、推定された状態ベクトルに基づいて、動的システムの状態を測定する複数のセンサの出力値を要素とする観測値ベクトルの値を推定する。さらに、推定された観測値ベクトルと、実際のセンサの出力値を要素とする観測値ベクトルとの差分として算出される観測残差に基づいて、推定された状態ベクトルを、実際の値（真値）に近い値へと更新し、更新後の状態ベクトルを出力する。
カルマンフィルタは、以上のような演算を繰り返すことにより、状態ベクトルを構成する複数の状態変数の各々を、実際の物理量を正確に表した値（真値）へと近付ける。
【００２８】
本実施形態において、カルマンフィルタＫＦは、携帯機器１の姿勢ｑ、地磁気の強さｒ、地磁気の伏角θ、携帯機器１の角速度ω、３次元角速度センサ９０のオフセットｇＯＦＦの推定値ｇＯ、及び３次元磁気センサ７０のオフセットｍＯＦＦの推定値ｍＯを状態変数として採用し、これらを推定の対象とする。
これらの状態変数を要素とする時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。なお、各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。
【数１】
【００２９】
本実施形態では、姿勢ｑを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。具体的には、地上に固定された座標系（例えば、地上の任意の一点を原点とし、互いに直交する３つの方向、例えば、水平東、水平北、及び鉛直上向きを、それぞれｘ軸、ｙ軸、及びｚ軸とする座標系）を定め、当該地上に固定された座標系の各軸と、携帯機器１に固定された座標系の各軸とが一致する姿勢ｑを基準姿勢と定義し、基準姿勢をｑ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢ｑは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢ｑは、式（２）で与えられる。
【数２】
【００３０】
本実施形態におけるカルマンフィルタの観測対象は、３次元磁気センサ７０から出力される磁気データｍ、３次元加速度センサ８０から出力される加速度データａ、及び３次元角速度センサ９０から出力される角速度データｇである。観測値ベクトルは、これら磁気データｍ、加速度データａ、及び角速度データｇを要素とする。時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数３】
【００３１】
初期値生成モジュール１１０は、時刻Ｔ＝０における状態変数を要素とする初期値ＩＮＩを算出する。初期値ＩＮＩは、姿勢ｑの初期値ｑ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角θの初期値θ０、角速度ωの初期値ω０、３次元角速度センサ９０のオフセット推定値ｇＯの初期値ｇＯ,０、及び３次元磁気センサ７０のオフセット推定値ｍＯの初期値ｍＯ,０を要素として含む。
初期値ＩＮＩは、カルマンフィルタの演算によって状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下のような値に設定してもよい。
【００３２】
地磁気の強さｒの初期値ｒ０、及び地磁気の伏角θの初期値θ０は、例えば、ＧＰＳ部６０から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ１が格納される。初期値生成モジュール１１０は、ルックアップテーブルＬＵＴ１を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さｒの初期値ｒ０及び地磁気の伏角θの初期値θ０として設定する。
なお、携帯機器１が衛星からの電波の届かない場所（例えば、地下街）にある場合には、ＧＰＳ部６０から位置情報が得られない。そのような場合には、携帯機器１が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルＬＵＴ１に格納されている。初期値生成モジュール１１０は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルＬＵＴ１から読み出す。例えば、日本で販売された携帯機器１については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さｒの初期値ｒ０、及び地磁気の伏角θの初期値θ０を算出する。
【００３３】
角速度ωの初期値ω０は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。３次元角速度センサ９０のオフセット推定値ｇＯの初期値ｇＯ,０は、通常「０」近辺に調整されているため、「０」に設定する。姿勢ｑの初期値ｑ０に関しては、例えば、携帯機器１が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。
【００３４】
３次元磁気センサ７０のオフセット推定値ｍＯの初期値ｍＯ,０は、例えば、時刻Ｔ＝０の３次元磁気センサ７０の観測値ｍ０、姿勢ｑの初期値ｑ０、携帯機器１を使用する地域の地磁気ベクトルｍtypical、及び、式（５）に示す行列Ｂ（ｑ）を用いて、以下に示す式（４）で与えられる値に設定する。ここで、行列Ｂ（ｑ）は、携帯機器１が姿勢ｑである場合に、地上に固定された座標系において表現されたベクトルを、携帯機器１に固定された座標系において表現するための座標変換行列である。なお、ＲＯＭ３０には、位置情報と地磁気ベクトルｍtypicalとを対応づけて記憶したルックアップテーブルＬＵＴ２が記憶されている。初期値生成モジュール１１０は、ＧＰＳ部６０で生成される位置情報に基づいてルックアップテーブルＬＵＴ２を参照して地磁気ベクトルｍtypicalを取得し、式（４）を演算することによって、オフセット推定値ｍＯの初期値ｍＯ,０を得る。
【数４】
【００３５】
初期値生成モジュール１１０は、以上のようにして生成された姿勢ｑの初期値ｑ０、地磁気の強さｒの初期値ｒ０、地磁気の伏角θの初期値θ０、角速度ωの初期値ω０、３次元角速度センサ９０のオフセット推定値ｇＯの初期値ｇＯ,０、及び３次元磁気センサ７０のオフセット推定値ｍＯの初期値ｍＯ,０を要素とするベクトルである初期値ＩＮＩを生成し、これを出力する。
【００３６】
［３． カルマンフィルタによる演算］
次に、本実施形態に係るカルマンフィルタＫＦによる演算の概要について説明する。
カルマンフィルタＫＦは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定し、この推定値を、推定された状態ベクトル（推定状態ベクトル）ｘ−ｋとして出力する。そして、カルマンフィルタＫＦは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、状態遷移モデルにより推定された状態ベクトルｘ−ｋに基づいて観測値ベクトルｙｋを推定し、この推定値を、推定された観測値ベクトル（推定観測値ベクトル）ｙ−ｋとして出力する。なお、本実施形態のカルマンフィルタＫＦは、非線形カルマンフィルタであるシグマポイントカルマンフィルタにより構成される。シグマポイントカルマンフィルタとは、状態ベクトルｘｋ−１の周囲に複数のシグマポイントχｋ−１を設定し、これらの複数のシグマポイントχｋ−１を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ−ｋを推定し、シグマポイントの推定値（推定シグマポイント）χ−ｋの平均を算出することにより、推定された状態ベクトルｘ−ｋを求める演算である。従って、推定された観測値ベクトルｙ−ｋは、厳密には、推定された状態ベクトルｘ−ｋの周辺に存在する複数のシグマポイントの推定値χ−ｋに基づいて算出される。
その後、カルマンフィルタＫＦは、推定された観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｅｋとして算出する。また、カルマンフィルタＫＦは、推定された状態ベクトルｘ−ｋと、推定された観測値ベクトルｙ−ｋと、に基づいてカルマンゲインＫｋを算出する。そして、カルマンフィルタＫＦは、観測残差ｅｋとカルマンゲインＫｋとを用いて推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトル（状態ベクトルを更新したベクトル）ｘ＋ｋを算出する。
以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタＫＦの演算により、状態ベクトルｘｋを実際の物理量を正確に表した値（真値）に近い正確な値へと近付けることができる。
【００３７】
［３．１． カルマンフィルタ演算部の動作の概要］
本実施形態におけるカルマンフィルタＫＦの状態遷移モデルは以下に示す式（６）で与えられ、観測モデルは以下に示す式（７）で与えられる。本実施形態では、式（６）に現れる関数ｆ及び式（７）に現れる関数ｈは、非線形の関数である。
【数５】
【００３８】
ここで、状態ベクトルｘｋはｎ次元のベクトルであり、観測値ベクトルｙｋはｍ次元のベクトルである。なお、本実施形態では、ｎ＝１５であり、ｍ＝９である。また、式（６）に現れるプロセスノイズｕｋ及び式（７）に現れる観測ノイズｖｋは０を中心とするガウスノイズである。
式（６）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を関数ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｕｋ−１とを加算することにより推定されるものであることを示している。
また、式（７）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを関数ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｖｋとを加算することにより推定されることを示している。
なお、プロセスノイズｕｋの共分散をＱ、観測ノイズｖｋの共分散をＲと表す。
【００３９】
時刻Ｔ＝ｋにおける観測残差ｅｋは、実際の観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（８）で表される。カルマンフィルタＫＦは、観測残差ｅｋと、式（９）に示すカルマンゲインＫｋとを用いて、以下の式（１０）に示すように、推定された状態ベクトルｘ−ｋを更新し、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタＫＦは、以下の式（１１）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
なお、Ｐ−ｋは、推定された状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。
また、Ｐｙｙｋは、観測残差ｅｋの共分散であり、Ｐｘｙｋは、推定された状態ベクトルｘ−ｋと、推定された観測値ベクトルｙ−ｋとの相互共分散行列である。
【数６】
【００４０】
観測モデルを用いて推定された観測値ベクトルｙ−ｋは、式（７）に示すように、観測モデルを用いて推定された状態ベクトルｘ−ｋに基づいて算出される理論値である。よって、観測モデルを用いて推定された観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｅｋは、推定された状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
カルマンフィルタＫＦは、式（１０）に示すように、観測残差ｅｋを用いて、推定された状態ベクトルｘ−ｋを、真値に近い更新後の状態ベクトルｘ＋ｋへと更新する。
【００４１】
［３．２． シグマポイントカルマンフィルタによる演算］
本実施形態において、カルマンフィルタＫＦは、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、図３に示すカルマンフィルタＫＦの機能ブロック図を参照しつつ、カルマンフィルタＫＦによる演算の詳細を説明する。
【００４２】
遅延部１２１は、加算器１３２から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部１２２に対して出力する。なお、初回の演算（時刻Ｔ＝０）では、遅延部１２１は、初期値生成モジュール１１０が出力する初期値ＩＮＩを用いて、状態ベクトルｘ＋ｋ−１を生成する。
また、遅延部１２１は、減算器１３３から出力される更新後の状態ベクトルｘ＋ｋの推定誤差の共分散Ｐ＋ｋを単位時間遅延させることで、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１を生成し、これを、シグマポイント生成部１２２及び共分散算出部１２５に対して出力する。
【００４３】
シグマポイント生成部１２２は、状態ベクトルｘ＋ｋ−１に基づいて、ａ個のシグマポイントχ＋ｋ−１（ｉ）（ｉ＝1，2，…ａ）を生成する（ａは、２以上の自然数）。シグマポイントχ＋ｋ−１（ｉ）の生成は、以下に示す公知の方法を適用する。
シグマポイント生成部１２２は、まず、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１から、式（１２）に示す共分散Ｐ＋ｋ−１の平方根を表すｎ行ｎ列の行列を生成する。次に、シグマポイント生成部１２２は、共分散Ｐ＋ｋ−１の平方根を表す行列と、各種係数とに基づいて、ａ個のシグマポイント算出用ベクトルを定める。シグマポイント算出用ベクトルは、ｎ次元のベクトルである。その後、シグマポイント生成部１２２は、状態ベクトルｘ＋ｋ−１と、ａ個のシグマポイント算出用ベクトルとに基づいて、ａ個のシグマポイントχ＋ｋ−１（ｉ）を生成し、これを状態遷移モデル部１２３に対して出力する。
なお、ａ個のシグマポイントχ＋ｋ−１（ｉ）の生成において用いられる各種係数は、後述する式（１４）における重みｗｉの生成にも用いられる値であり、公知の方法により定められる。また、ａ個のシグマポイント算出用ベクトルには０ベクトルが含まれていても構わない。
【数７】
【００４４】
状態遷移モデル部１２３は、式（１３）に示すように、時刻Ｔ＝ｋ−１におけるａ個のシグマポイントχ＋ｋ−１（ｉ）の各々を、状態遷移モデルに適用することにより、時刻Ｔ＝ｋにおけるａ個のシグマポイントの推定値χ−ｋ（ｉ）を算出する。
【数８】
【００４５】
次に、平均算出部１２４は、式（１４）に示すように、時刻Ｔ＝ｋにおけるａ個のシグマポイントの推定値χ−ｋ（ｉ）の重み付き平均を計算することで、推定された状態ベクトルｘ−ｋを算出し、これを出力する。なお、式（１４）に現れる重みｗｉは、シグマポイントχ＋ｋ−１（ｉ）の生成において用いられた各種係数に基づいて定められる。
このように、シグマポイント生成部１２２、状態遷移モデル部１２３、及び平均算出部１２４は、状態ベクトルｘ＋ｋ−１から、推定された状態ベクトルｘ−ｋを算出する推定状態ベクトル算出部１４０として機能する。
【数９】
【００４６】
共分散算出部１２５は、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋを算出する。推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋは、式（１５）に示すように、ａ個のシグマポイントの推定値χ−ｋ（ｉ）、推定された状態ベクトルｘ−ｋ、重みｗｉ、及びプロセスノイズｕｋ−１の共分散Ｑに基づいて算出することができる。
但し、本実施形態では、処理負荷を削減する観点から、共分散Ｐ−ｋのうち一部の成分は、式（１５）を用いずに、後述する式（３０）に基づいて算出する。この点についての詳細は後述する。
【数１０】
【００４７】
一方、観測モデル部１２６は、式（１６）に示すように、時刻Ｔ＝ｋにおけるａ個のシグマポイントの推定値χ−ｋ（ｉ）の各々を、観測モデルに適用することにより、ａ個の推定観測値γｋ（ｉ）を算出する。
【数１１】
【００４８】
そして、平均処理部１２７は、式（１７）に示すように、ａ個の推定観測値γｋ（ｉ）の平均を演算することにより、推定された観測値ベクトルｙ−ｋを算出する。
このように、シグマポイント生成部１２２、状態遷移モデル部１２３、観測モデル部１２６、及び平均処理部１２７は、状態ベクトルｘ＋ｋ−１から、推定された観測値ベクトルｙ−ｋを算出する推定観測値ベクトル算出部１５０として機能する。
【数１２】
【００４９】
次に、平均処理部１２７は、式（１８）に示す観測残差の共分散Ｐｙｙｋを算出する。
【数１３】
【００５０】
減算器（観測残差生成部）１３１は、式（８）に示したように、観測値ベクトルｙｋと、推定された観測値ベクトルｙ−ｋとの差分として、観測残差ｅｋを算出する。
カルマンゲイン付与部１２８は、式（１９）に示す相互共分散行列Ｐｘｙｋを算出する。次に、カルマンゲイン付与部１２８は、式（９）に示したように、観測残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出する。その後、カルマンゲイン付与部１２８は、式（１０）の右辺第２項（Ｋｋｅｋ）を演算し、その結果を加算器１３２に対して出力する。
また、カルマンゲイン付与部１２８は、観測残差の共分散ＰｙｙｋとカルマンゲインＫｋとに基づいて、式（１１）の右辺第２項（ＫｋＰｙｙｋＫＴｋ）を演算し、その結果を減算器１３３に対して出力する。
【数１４】
【００５１】
加算器（更新部）１３２は、式（１０）に示したように、推定された状態ベクトルｘ−ｋと、カルマンゲイン付与部１２８から出力される式（１０）の右辺第２項（Ｋｋｅｋ）と、を加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。
減算器１３３は、式（１１）に示したように、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋと、カルマンゲイン付与部１２８から出力される式（１１）の右辺第２項（ＫｋＰｙｙｋＫＴｋ）との差分として、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散Ｐ＋ｋを算出する。
【００５２】
［３．３． 状態遷移モデル］
次に、状態遷移モデル部１２３の演算で用いる状態遷移モデルについて説明する。
【００５３】
本実施形態に係る状態遷移モデルにおいて、状態ベクトルｘｋを構成する状態変数のうち、姿勢ｑについての状態遷移は、式（２１）に示す関数ｆｑ（ｑ，ω）を用いて、式（２０）により定義される。式（２０）は、時刻Ｔ＝ｋにおける推定された状態ベクトルｘ−ｋを構成する状態変数である姿勢ｑ−ｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘ＋ｋ−１を構成する状態変数である姿勢ｑ＋ｋ−１及び角速度ω＋ｋ−１に基づいて推定されることを示している。
式（２１）に示す関数ｆｑ（ｑ，ω）は、式（６）に示した関数ｆのうち、姿勢ｑを算出する部分のみを表した関数である。式（２１）の右辺の演算子Ωは、式（２２）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表し、３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、［ｌ×］という記号を式（２３）で定義する。また、測定時間間隔（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの間隔）をΔｔで表したとき、演算子Ωを構成する成分Ψを、式（２４）で定義する。
【数１５】
【００５４】
姿勢ｑは、クォータニオンで表現されるため、正規化条件||ｑ||＝１が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘｋを更新する場合、更新後の状態ベクトルｘ＋ｋは、式（１４）に示すように、シグマポイントの推定値χ−ｋ（ｉ）に重みｗｉを付与した重み付き平均として算出されるため、（更新前の）状態ベクトルｘｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘｋ−１の要素である姿勢ｑｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋの要素である姿勢ｑｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢ｑについての正規化条件が満たされなくなる可能性が存在する。
そこで、姿勢ｑに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルｘｋを構成する要素のうち姿勢ｑｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
【００５５】
地磁気の強さｒ及び地磁気の伏角θは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角θｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角θｋ−１とは等しい値であるという前提を置く。
同様に、３次元角速度センサ９０のオフセット推定値ｇＯも、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおけるオフセット推定値ｇＯ,Ｋと、時刻Ｔ＝ｋ−１におけるオフセット推定値ｇＯ,Ｋ−１とは等しい値であるという前提を置く。
【００５６】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００５７】
前述のとおり、３次元磁気センサ７０のオフセットｍＯＦＦは、携帯機器１の部品が発する内部磁界である。従って、携帯機器１の内部状態が一定である間は、オフセットｍＯＦＦも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、オフセットｍＯＦＦも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、オフセットｍＯＦＦが変化するタイミングやオフセットｍＯＦＦの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１におけるオフセット推定値ｍＯ,Ｋ−１を用いて、時刻Ｔ＝ｋにおけるオフセット推定値ｍＯ,ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおけるオフセット推定値ｍＯ,ｋと、時刻Ｔ＝ｋ−１におけるオフセット推定値ｍＯ,ｋ−１とは等しいと仮定する。
【００５８】
このように、本実施形態に係る状態遷移モデルでは、以下に示す式（２５）のように、状態ベクトルｘｋを構成する状態変数のうち、姿勢ｑ以外は、前の時刻から変化しないという前提を置く。
【数１６】
【００５９】
ここで、式（１）に示した状態ベクトルｘｋを、式（２６）のように、姿勢ｑｋと、姿勢ｑｋ以外の要素を表すベクトルβｋとに分離して表現した場合、ベクトルβｋについて、式（２７）が成立する。すなわち、状態ベクトルｘ＋ｋ−１のうち、姿勢（第２ベクトル）ｑ＋ｋ−１以外の要素を表すベクトル（第１ベクトル）β＋ｋ−１と、推定された状態ベクトルｘ−ｋのうち、姿勢（推定第２ベクトル）ｑ−ｋ以外の要素を表すベクトル（推定第１ベクトル）β−ｋとは等しい。
【数１７】
【００６０】
式（２７）が成立する場合、式（１３）から明らかなように、シグマポイントχ＋ｋ−１（ｉ）のうちベクトルβ＋ｋ−１に相当する要素と、シグマポイントの推定値χ−ｋ（ｉ）のうちベクトルβ−ｋに相当する要素とは、等しくなる。
従って、式（２７）が成立する場合、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１のうち、ベクトルβ＋ｋ−１の推定誤差の共分散を表す成分と、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋのうち、ベクトルβ−ｋの推定誤差の共分散を表す成分とは、プロセスノイズｕｋ−１の共分散Ｑを考慮しなければ、等しいものと看做すことができる（段落［００４６］参照）。
【００６１】
ここで、共分散Ｐｋを、式（２８）に示すように、行列Ｐｑｑ，ｋ、行列Ｐｑβ，ｋ、行列Ｐｑβ，ｋＴ、及び行列Ｐββ，ｋの、４つの行列に分割する。行列Ｐｑｑ，ｋは、姿勢ｑｋに基づいて定められる、４行４列の行列である。行列Ｐｑβ，ｋは、姿勢ｑｋとベクトルβｋとに基づいて定められる、４行１１列の行列である。行列Ｐββ，ｋは、ベクトルβｋに基づいて定められる、１１行１１列の行列である。具体的には、行列Ｐββ，ｋは、ベクトルβｋの推定誤差の共分散を表す行列である。
【数１８】
【００６２】
また、プロセスノイズｕｋの共分散Ｑを、式（２９）のように、４行４列の行列Ｑｑｑ、４行１１列の行列Ｑｑβ、及び１１行１１列の行列Ｑββを用いて表す。
式（６）に示したとおり、プロセスノイズｕｋは、状態ベクトルｘｋを状態遷移モデルに適用する際に生じるプロセスノイズであり、式（２９）に示す共分散Ｑは、当該プロセスノイズｕｋの共分散を表す。従って、式（２９）に現れる、行列Ｑββは、状態ベクトルｘｋを状態遷移モデルに適用する際に生じるプロセスノイズのうち、状態ベクトルｘｋの中で姿勢ｑｋ以外の要素を表すベクトルβｋに生じるプロセスノイズの共分散を表す。
このとき、行列Ｐ−ββ，ｋは、式（３０）に示すように、行列Ｐ＋ββ，ｋ−１と、行列Ｑββとの和として定められる。
【数１９】
【００６３】
共分散Ｐ−ｋの全ての成分を式（１５）に基づいて求める場合、ｎ行１列のベクトルと１行ｎ列のベクトルとを乗算してｎ行ｎ列の行列を求める演算を、ａ回繰り返すことが必要になる。
一方、式（３０）を用いる場合、行列Ｐ−ββ，ｋは、１ステップ前に（時刻Ｔ＝ｋ−１において）既に算出されている行列Ｐ＋ββ，ｋ−１と、行列Ｑββとを単純に加算することにより算出される。そして、共分散Ｐ−ｋは、行列Ｐ−ββ，ｋ以外の成分、すなわち、４行４列の行列Ｐ−ｑｑ，ｋ及び４行１１列の行列Ｐ−ｑβ，ｋのみを、式（１５）による演算により求めればよい。この結果、１５行１５列の共分散Ｐ−ｋの全ての成分を、式（１５）を用いて算出する場合に比べて、共分散算出部１２５における計算量を大幅に低減することが可能となる。
【００６４】
なお、状態遷移モデルにおいて、式（２５）及び式（２７）に示すように、姿勢ｑ以外は、前の時刻から変化しないという前提を置いた場合であっても、状態ベクトルｘｋは、カルマンフィルタの演算において観測残差ｅｋに基づいて更新される。従って、姿勢ｑｋ以外の要素を表すベクトルβｋも、カルマンフィルタの演算を繰り返す中で、観測残差ｅｋに基づいて真値に近づくように更新される。
【００６５】
［３．４． 観測モデル］
次に、観測モデル部１２６で行われる演算において用いられる観測モデルについて説明する。
【００６６】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、３次元角速度センサ９０のオフセット推定値ｇＯとを用いて、式（３１）で与えられる。
【数２０】
【００６７】
また、地上に固定された座標系において、地磁気を表すベクトルは、式（３２）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｍの推定値γｍａｇは、３次元磁気センサ７０のオフセット推定値ｍＯと、式（５）で示した行列Ｂ（ｑ）とを用いて、式（３３）で与えられる。
【数２１】
【００６８】
また、地上に固定された座標系において、重力を表すベクトルを重力加速度で正規化したベクトルは、式（３４）で与えられる。従って、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（５）で示した行列Ｂ（ｑ）を用いて、式（３５）で与えられる。
【数２２】
【００６９】
従って、式（３）を式（８）の右辺第１項に代入し、式（３１）、式（３３）、及び式（３５）を用いて式（８）の右辺第２項を表すことにより、式（８）を、以下の式（３６）に変形することができる。このとき、観測残差ｅｋは、式（３６）により算出される。
【数２３】
【００７０】
［４． 結論］
以上に示したように、本実施形態に係る状態推定装置は、シグマポイントカルマンフィルタの演算において用いられる状態ベクトルｘｋを、姿勢ｑｋと、ベクトルβｋとに分離して表現し、状態ベクトルｘ＋ｋ−１を構成する要素の一部であるベクトルβ＋ｋ−１と、推定された状態ベクトルｘ−ｋを構成する要素の一部であるベクトルβ−ｋとが等しいという前提を置いた。
そして、本実施形態に係る状態推定装置は、推定された状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋのうちベクトルβ−ｋの推定誤差の共分散を表す成分を、状態ベクトルｘ＋ｋ−１の推定誤差の共分散Ｐ＋ｋ−１のうちベクトルβ＋ｋ−１の推定誤差の共分散を表す成分と、プロセスノイズｕｋ−１の共分散Ｑのうちベクトルβ＋ｋ−１に生じるプロセスノイズの共分散を表す成分とを単純に加算することで算出した。
これにより、共分散Ｐ−ｋの全ての成分を式（１５）に基づいて求める場合と比較して、シグマポイントカルマンフィルタの計算負荷を大幅に低減することが可能となり、状態推定装置の処理速度の向上、及び携帯機器１の低消費電力化が可能となった。
【００７１】
＜Ｂ．変形例＞
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例を組み合わせることもできる。
【００７２】
（１）変形例１
上述した実施形態では、状態ベクトルｘを構成する要素して、携帯機器１の姿勢ｑ、地磁気の強さｒ、地磁気の伏角θ、携帯機器１の角速度ω、３次元角速度センサ９０のオフセット推定値ｇＯ、及び３次元磁気センサ７０のオフセット推定値ｍＯを採用し、これら６個の状態変数をカルマンフィルタの演算における推定の対象としたが、本発明はこれに限定されるものでは無い。例えば、状態ベクトルｘを、これら６個の状態変数のうちの一部の状態変数により構成し、当該一部の状態変数をカルマンフィルタの演算における推定の対象とするものであってもよい。
【００７３】
（２）変形例２
上述した実施形態では、観測値ベクトルｙを、３次元磁気センサ７０から出力される磁気データｍ、３次元加速度センサ８０から出力される加速度データａ、及び３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、磁気データｍ、加速度データａ、及び角速度データｇのうちの一部のみを利用して観測値ベクトルｙを生成してもよい。
【００７４】
（３）変形例３
上述した実施形態では、平均算出部１２４は、式（１４）に示すように、推定された状態ベクトルｘ−ｋを、ａ個のシグマポイントの推定値χ−ｋ（ｉ）の重み付き平均として算出したが、本発明はこれに限定されるものではない。
例えば、平均算出部１２４は、推定された状態ベクトルｘ−ｋを構成する要素の一部であるベクトルβ−ｋを、式（１４）を用いず、単純に、状態ベクトルｘ＋ｋ−１を構成する要素の一部であるベクトルβ＋ｋ−１と等しい値に設定してもよい。この場合、平均算出部１２４は、推定された状態ベクトルｘ−ｋのうち、姿勢ｑ−ｋについてのみ、式（１４）の重み付き平均を算出する演算により算出する。これにより、１５次元のベクトルである推定された状態ベクトルｘ−ｋのうち、１１次元のベクトルβ−ｋについては、単純な値の代入により算出することが可能となるため、推定された状態ベクトルｘ−ｋの算出に係る処理負荷を、大幅に低減することができる。
【符号の説明】
【００７５】
１…携帯機器、ＫＦ…カルマンフィルタ、１２１…遅延部、１２２…シグマポイント生成部、１２３…状態遷移モデル部、１２４…平均算出部、１２５…共分散算出部、１２６…観測モデル部、１２７…平均処理部、１２７…推定観測値ベクトル算出部、１２８…カルマンゲイン付与部、１３１…減算器、１３２…加算器、１３３…減算器、１４０…推定状態ベクトル算出部、１５０…推定観測値ベクトル算出部、Ｋ…カルマンゲイン、ｘｋ…状態ベクトル、ｘ−ｋ…推定された状態ベクトル、ｑｋ…姿勢、βｋ…ベクトル、Ｐｋ…状態ベクトルの推定誤差の共分散、Ｑ…プロセスノイズの共分散、ｅｋ…観測残差、ｙｋ…観測値ベクトル、ｙ−ｋ…推定された観測値ベクトル。
【特許請求の範囲】
【請求項１】
第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する推定状態ベクトル算出部と、
前記推定状態ベクトルの推定誤差の共分散を算出する共分散算出部と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する推定観測値ベクトル算出部と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する観測残差生成部と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、
前記推定状態ベクトル算出部は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、
前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタ。
【請求項２】
前記推定状態ベクトル算出部は、
前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイントを生成するシグマポイント生成部と、
前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイントを算出する状態遷移モデル部と、
前記推定状態ベクトルを算出する平均算出部と、を備え、
前記状態遷移モデル部は、
前記推定シグマポイントのうち、前記推定第２ベクトルを算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第１ベクトルに基づいて生成された要素と等しい値に設定し、
前記平均算出部は、
前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記推定第２ベクトルを、前記複数の推定シグマポイントに基づいて算出することを特徴とする、
請求項１に記載のカルマンフィルタ。
【請求項３】
システムを観測して観測値を各々出力する複数のセンサと、請求項１または２に記載のカルマンフィルタとを備えることを特徴とする、状態推定装置。
【請求項４】
システムの状態を推定するカルマンフィルタの制御方法であって、
第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する工程と、
前記推定状態ベクトルの推定誤差の共分散を算出する工程と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、
前記推定状態ベクトルを算出する工程は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、
前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御方法。
【請求項５】
システムの状態を推定するカルマンフィルタの制御プログラムであって、
第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する処理と、
前記推定状態ベクトルの推定誤差の共分散を算出する処理と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、
前記推定状態ベクトルを算出する処理は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、
前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御プログラム。
【請求項１】
第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する推定状態ベクトル算出部と、
前記推定状態ベクトルの推定誤差の共分散を算出する共分散算出部と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する推定観測値ベクトル算出部と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する観測残差生成部と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、
前記推定状態ベクトル算出部は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、
前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタ。
【請求項２】
前記推定状態ベクトル算出部は、
前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイントを生成するシグマポイント生成部と、
前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイントを算出する状態遷移モデル部と、
前記推定状態ベクトルを算出する平均算出部と、を備え、
前記状態遷移モデル部は、
前記推定シグマポイントのうち、前記推定第２ベクトルを算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第１ベクトルに基づいて生成された要素と等しい値に設定し、
前記平均算出部は、
前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、前記推定第２ベクトルを、前記複数の推定シグマポイントに基づいて算出することを特徴とする、
請求項１に記載のカルマンフィルタ。
【請求項３】
システムを観測して観測値を各々出力する複数のセンサと、請求項１または２に記載のカルマンフィルタとを備えることを特徴とする、状態推定装置。
【請求項４】
システムの状態を推定するカルマンフィルタの制御方法であって、
第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する工程と、
前記推定状態ベクトルの推定誤差の共分散を算出する工程と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、
前記推定状態ベクトルを算出する工程は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、
前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御方法。
【請求項５】
システムの状態を推定するカルマンフィルタの制御プログラムであって、
第１ベクトル及び第２ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第１ベクトル及び推定第２ベクトルを要素とする推定状態ベクトルを算出する処理と、
前記推定状態ベクトルの推定誤差の共分散を算出する処理と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、
前記推定状態ベクトルを算出する処理は、前記推定第１ベクトルを、前記第１ベクトルと等しい値に設定し、
前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第１ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第１ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第１ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御プログラム。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２０１３−６１３０９（Ｐ２０１３−６１３０９Ａ）
【公開日】平成２５年４月４日（２０１３．４．４）
【国際特許分類】
【出願番号】特願２０１１−２０１５４４（Ｐ２０１１−２０１５４４）
【出願日】平成２３年９月１５日（２０１１．９．１５）
【出願人】（０００００４０７５）ヤマハ株式会社 (5,930)
【Ｆターム（参考）】
【公開日】平成２５年４月４日（２０１３．４．４）
【国際特許分類】
【出願日】平成２３年９月１５日（２０１１．９．１５）
【出願人】（０００００４０７５）ヤマハ株式会社 (5,930)
【Ｆターム（参考）】
[ Back to top ]