"第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する推定状態ベクトル算出部と、前記推定状態ベクトルの推定誤差の共分散を算出する共分散算出部と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する推定観測値ベクトル算出部と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する観測残差生成部と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、前記推定状態ベクトル算出部は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタ。前記推定状態ベクトル算出部は、前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイントを生成するシグマポイント生成部と、前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイントを算出する状態遷移モデル部と、前記推定状態ベクトルを算出する平均算出部と、を備え、前記状態遷移モデル部は、前記推定シグマポイントのうち、前記推定第2ベクトルを算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第1ベクトルに基づいて生成された要素と等しい値に設定し、前記平均算出部は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定第2ベクトルを、前記複数の推定シグマポイントに基づいて算出することを特徴とする、請求項1に記載のカルマンフィルタ。システムを観測して観測値を各々出力する複数のセンサと、請求項1または2に記載のカルマンフィルタとを備えることを特徴とする、状態推定装置。システムの状態を推定するカルマンフィルタの制御方法であって、第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する工程と、前記推定状態ベクトルの推定誤差の共分散を算出する工程と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、前記推定状態ベクトルを算出する工程は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御方法。システムの状態を推定するカルマンフィルタの制御プログラムであって、第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する処理と、前記推定状態ベクトルの推定誤差の共分散を算出する処理と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、前記推定状態ベクトルを算出する処理は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御プログラム。しかし、シグマポイントカルマンフィルタによる演算は計算量が多いため、シグマポイントカルマンフィルタが実装される機器には、大容量のメモリと高性能のCPUとを備えることが必要となる。そして、シグマポイントカルマンフィルタによる演算を行う場合、多くの電力が消費される。特に、シグマポイントカルマンフィルタを実装した機器が、バッテリーで給電される携帯機器の場合、機器の長時間使用が困難になるという課題が存在した。本発明は、上述した点に鑑みてなされたものであり、シグマポイントカルマンフィルタの演算を簡素化し、高速且つ低消費電力な状態推定装置の実現を解決課題とする。以下、本発明について説明する。なお、本発明の理解を容易にするために本実施形態、変形例、及び添付図面の参照符号を括弧書きにて付記するが、それにより本発明が本実施形態に限定されるものではない。上記課題を解決するため、本発明に係るカルマンフィルタは、第1ベクトルβk-1及び第2ベクトルqk-1を要素とする状態ベクトルxk-1と、前記状態ベクトルの推定誤差の共分散Pk-1と、に基づいて、状態遷移モデルfを用いて、推定第1ベクトルβ-k及び推定第2ベクトルq-kを要素とする推定状態ベクトルx-kを算出する推定状態ベクトル算出部と、前記推定状態ベクトルの推定誤差の共分散P-kを算出する共分散算出部と、前記状態遷移モデル及び観測モデルhを用いて、前記状態ベクトルから、推定観測値ベクトルy-kを算出する推定観測値ベクトル算出部と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルykとに基づいて、観測残差ekを算出する観測残差生成部と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、前記推定状態ベクトル算出部は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分Pββ,k-1、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分Qββを加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分P-ββ,kを算出する、ことを特徴とする。この発明によれば、共分散算出部は、推定状態ベクトルの推定誤差の共分散のうち推定第1ベクトルの推定誤差の共分散を表す成分を、2個の行列を加算することで算出する。一般的に、あるベクトルの推定誤差の共分散は、まず、当該あるベクトルの推定誤差を表すベクトルを算出し、その後、推定誤差を表すベクトルと、推定誤差を表すベクトルを転置したベクトルとの積を算出することにより求められる。よって、あるベクトルの推定誤差の共分散を求めるためには、推定誤差を表すベクトルを算出する演算の他に、当該あるベクトルの次元を表す数の二乗に相当する回数の演算が必要になる。シグマポイントカルマンフィルタは、複数のシグマポイントに基づいて、推定状態ベクトルの推定誤差を表すベクトルを複数生成し、生成した複数の推定誤差を表すベクトルを用いて、推定状態ベクトルの推定誤差の共分散を算出する。従って、推定状態ベクトルの推定誤差の共分散を求めるためには、推定状態ベクトルの次元を表す数の二乗に相当する回数の演算を、更に、シグマポイントの個数に相当する回数だけ繰り返すことが必要となる。このように、推定状態ベクトルの推定誤差の共分散を求める演算は、計算量が多く、シグマポイントカルマンフィルタの処理負荷を増大させる要因となっていた。これに対して、この発明の共分散算出部は、推定状態ベクトルの推定誤差の共分散のうちの一部の成分を、2つの行列の加算という単純な演算により算出するため、従来の方法に従って計算した場合に比べて、推定状態ベクトルの推定誤差の共分散を求める演算に係る計算量の大幅な低減を可能とした。これにより、シグマポイントカルマンフィルタの演算の高速化と、シグマポイントカルマンフィルタの演算に必要な電力の低減が可能となった。なお、この発明の推定状態ベクトル算出部は、推定第1ベクトルを第1ベクトルと等しい値に設定する。しかし、第1ベクトルは、カルマンフィルタの演算において、値を全く変化させない訳ではない。すなわち、推定第1ベクトルを要素として含む推定状態ベクトルは、更新部において観測残差による更新がなされ、カルマンフィルタの推定対象であるシステムの状態を正確に表す値真値へと更新される。これにより、この発明に係るカルマンフィルタは、システムの状態を正確に推定することができる。また、上述したカルマンフィルタにおいて、前記推定状態ベクトル算出部は、前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイントχk-1を生成するシグマポイント生成部と、前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイントχ-kを算出する状態遷移モデル部と、前記推定状態ベクトルx-kを算出する平均算出部と、を備え、前記状態遷移モデル部は、前記推定シグマポイントのうち、前記推定第2ベクトルq-kを算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第1ベクトルβk-1に基づいて生成された要素と等しい値に設定し、前記平均算出部は、前記推定第1ベクトルβ-kを、前記第1ベクトルβk-1と等しい値に設定し、前記推定第2ベクトルq-kを、前記複数の推定シグマポイントχ-kに基づいて算出することが好ましい。この発明によれば、状態ベクトルの推定値を、複数のシグマポイントを用いて算出する。これにより、状態遷移モデルを用いた状態ベクトルの推定において、状態ベクトルが有する誤差による影響と、カルマンフィルタが推定対象とするシステムの非線形性による影響とを低減させることができ、状態遷移モデルにおける推定精度の低下を防止することができる。すなわち、この発明によれば、カルマンフィルタが、システムの状態を正確に推定することが可能となる。また、この発明によれば、推定第1ベクトルを、第1ベクトルと等しい値に設定し、推定第2ベクトルのみを、複数の推定シグマポイントに基づいて算出するため、推定状態ベクトルの算出に係る計算負荷を低減させることが可能となる。次に、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、上記のうちいずれかのカルマンフィルタとを備えることを特徴とする。この発明によれば、高速なカルマンフィルタの演算が可能で、且つ低消費電力な状態推定装置が実現される。次に、本発明に係るカルマンフィルタの制御方法は、システムの状態を推定するカルマンフィルタの制御方法であって、第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する工程と、前記推定状態ベクトルの推定誤差の共分散を算出する工程と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、前記推定状態ベクトルを算出する工程は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。次に、本発明に係るカルマンフィルタの制御プログラムは、システムの状態を推定するカルマンフィルタの制御プログラムであって、第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する処理と、前記推定状態ベクトルの推定誤差の共分散を算出する処理と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、前記推定状態ベクトルを算出する処理は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。<A.実施形態>本発明の実施の形態について図面を参照して説明する。[1.機器構成及びソフトウェア構成]図1は、本発明の実施形態に係る携帯機器のブロック図であり、図2は携帯機器の外観を示す斜視図である。携帯機器1は、携帯機器1の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表される方位を、現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器1の姿勢等を推定することによって実現される。携帯機器1は、各種の構成要素とバスを介して接続され装置全体を制御するCPU10、CPU10の作業領域として機能するRAM20、各種のプログラムやデータを記憶したROM30、通信を実行する通信部40、画像を表示する表示部50、及びGPS部60を備える。GPS部60は、GPS衛星からの信号を受信して携帯機器1の位置情報緯度、経度を生成するものである。また、携帯機器1は、地磁気を検出して磁気データを出力する3次元磁気センサ70、加速度を検出して加速度データを出力する3次元加速度センサ80、及び角速度を検出して角速度データを出力する3次元角速度センサ90を備える。3次元磁気センサ70は、X軸磁気センサ71、Y軸磁気センサ72、及びZ軸磁気センサ73を備える。各センサは、MI素子磁気インピーダンス素子、MR素子磁気抵抗効果素子などを用いて構成することができる。磁気センサI/F74は、各センサからの出力信号をAD変換して磁気データmを出力する。この磁気データmは、x軸、y軸およびz軸の3成分によって表されるベクトルデータである。ところで、3次元磁気センサ70が搭載される携帯機器1は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、3次元磁気センサ70が出力する磁気データmは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する地磁気を表すベクトルの他に、機器に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、3次元磁気センサが出力するベクトルデータ磁気データmから、携帯機器1の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。このように、検出対象である地磁気の正確な値を得るために、補正処理において、3次元磁気センサ70から出力されるデータから取り除かれるべき内部磁界の値を3次元磁気センサ70のオフセットmOFFと呼ぶ。なお、地磁気検出部70は、携帯機器1の外部にある外部物が発する磁界についても検出する。しかし、本実施形態では、外部物が発する磁界は無視できる程小さいものとする。3次元加速度センサ80は、X軸加速度センサ81、Y軸加速度センサ82、及びZ軸加速度センサ83を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサI/F84は、各センサからの出力信号をAD変換して加速度データaを出力する。この加速度データaは、携帯機器1に固定された座標系における慣性力と重力との合力を、x軸、y軸及びz軸の3成分を有するベクトルとして示すデータである。したがって、携帯機器1が静止状態または等速直線運動状態にあれば、加速度データaは携帯機器1に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。3次元角速度センサ90は、X軸角速度センサ91、Y軸角速度センサ92、及びZ軸角速度センサ93を備える。角速度センサI/F94は、各センサからの出力信号をAD変換して角速度データgを出力する。角速度データgは、各軸の回りの角速度を示す3次元のベクトルデータである。なお、角速度データgには、3次元角速度センサ90のオフセットgOFFが重畳する。CPU10は、ROM30に格納されている状態推定プログラム100を実行することによって、動的システムの状態を表す複数の物理量、例えば、携帯機器1の姿勢、3次元磁気センサ70のオフセット、地磁気の強さや伏角等の物理量を推定する。すなわち、携帯機器1は、CPU10が状態推定プログラム100を実行することにより、状態推定装置として機能する。状態推定プログラム100は、初期値生成モジュール110と、カルマンフィルタモジュール120とを備える。カルマンフィルタモジュール120は、カルマンフィルタKFの演算を実行する。[2.カルマンフィルタの観測対象及び推定対象]一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムを観測する複数のセンサが計測する値観測値を推定する観測モデルと、を有する。そして、カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量状態変数を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定する。次に、カルマンフィルタは、推定された状態ベクトルに基づいて、動的システムの状態を測定する複数のセンサの出力値を要素とする観測値ベクトルの値を推定する。さらに、推定された観測値ベクトルと、実際のセンサの出力値を要素とする観測値ベクトルとの差分として算出される観測残差に基づいて、推定された状態ベクトルを、実際の値真値に近い値へと更新し、更新後の状態ベクトルを出力する。カルマンフィルタは、以上のような演算を繰り返すことにより、状態ベクトルを構成する複数の状態変数の各々を、実際の物理量を正確に表した値真値へと近付ける。本実施形態において、カルマンフィルタKFは、携帯機器1の姿勢q、地磁気の強さr、地磁気の伏角θ、携帯機器1の角速度ω、3次元角速度センサ90のオフセットgOFFの推定値gO、及び3次元磁気センサ70のオフセットmOFFの推定値mOを状態変数として採用し、これらを推定の対象とする。これらの状態変数を要素とする時刻T=kにおける状態ベクトルxkは、以下の式1で表される。なお、各状態変数の右下に付された添え字kは、当該状態変数が時刻T=kにおける値であることを表す。本実施形態では、姿勢qを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢回転状態を表す4次元数である。具体的には、地上に固定された座標系例えば、地上の任意の一点を原点とし、互いに直交する3つの方向、例えば、水平東、水平北、及び鉛直上向きを、それぞれx軸、y軸、及びz軸とする座標系を定め、当該地上に固定された座標系の各軸と、携帯機器1に固定された座標系の各軸とが一致する姿勢qを基準姿勢と定義し、基準姿勢をq=0、0、0、1と表現する。このとき、携帯機器1の任意の姿勢qは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢qは、式2で与えられる。本実施形態におけるカルマンフィルタの観測対象は、3次元磁気センサ70から出力される磁気データm、3次元加速度センサ80から出力される加速度データa、及び3次元角速度センサ90から出力される角速度データgである。観測値ベクトルは、これら磁気データm、加速度データa、及び角速度データgを要素とする。時刻T=kにおける観測値ベクトルykは式3で与えられる。初期値生成モジュール110は、時刻T=0における状態変数を要素とする初期値INIを算出する。初期値INIは、姿勢qの初期値q0、地磁気の強さrの初期値r0、地磁気の伏角θの初期値θ0、角速度ωの初期値ω0、3次元角速度センサ90のオフセット推定値gOの初期値gO0、及び3次元磁気センサ70のオフセット推定値mOの初期値mO0を要素として含む。初期値INIは、カルマンフィルタの演算によって状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下のような値に設定してもよい。地磁気の強さrの初期値r0、及び地磁気の伏角θの初期値θ0は、例えば、GPS部60から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ROM30には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルLUT1が格納される。初期値生成モジュール110は、ルックアップテーブルLUT1を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さrの初期値r0及び地磁気の伏角θの初期値θ0として設定する。なお、携帯機器1が衛星からの電波の届かない場所例えば、地下街にある場合には、GPS部60から位置情報が得られない。そのような場合には、携帯機器1が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルLUT1に格納されている。初期値生成モジュール110は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルLUT1から読み出す。例えば、日本で販売された携帯機器1については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さrの初期値r0、及び地磁気の伏角θの初期値θ0を算出する。角速度ωの初期値ω0は、例えば、携帯機器1が静止していることを仮定して、0に設定する。3次元角速度センサ90のオフセット推定値gOの初期値gO0は、通常0近辺に調整されているため、0に設定する。姿勢qの初期値q0に関しては、例えば、携帯機器1が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。3次元磁気センサ70のオフセット推定値mOの初期値mO0は、例えば、時刻T=0の3次元磁気センサ70の観測値m0、姿勢qの初期値q0、携帯機器1を使用する地域の地磁気ベクトルmtypical、及び、式5に示す行列Bqを用いて、以下に示す式4で与えられる値に設定する。ここで、行列Bqは、携帯機器1が姿勢qである場合に、地上に固定された座標系において表現されたベクトルを、携帯機器1に固定された座標系において表現するための座標変換行列である。なお、ROM30には、位置情報と地磁気ベクトルmtypicalとを対応づけて記憶したルックアップテーブルLUT2が記憶されている。初期値生成モジュール110は、GPS部60で生成される位置情報に基づいてルックアップテーブルLUT2を参照して地磁気ベクトルmtypicalを取得し、式4を演算することによって、オフセット推定値mOの初期値mO0を得る。初期値生成モジュール110は、以上のようにして生成された姿勢qの初期値q0、地磁気の強さrの初期値r0、地磁気の伏角θの初期値θ0、角速度ωの初期値ω0、3次元角速度センサ90のオフセット推定値gOの初期値gO0、及び3次元磁気センサ70のオフセット推定値mOの初期値mO0を要素とするベクトルである初期値INIを生成し、これを出力する。[3.カルマンフィルタによる演算]次に、本実施形態に係るカルマンフィルタKFによる演算の概要について説明する。カルマンフィルタKFは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻時刻T=k-1のシステムの状態を示す状態ベクトルxk-1から単位時間が経過した後時刻T=kの状態ベクトルxkを推定し、この推定値を、推定された状態ベクトル推定状態ベクトルx-kとして出力する。そして、カルマンフィルタKFは、各種センサからの出力を要素とする観測値ベクトルykと状態ベクトルxkとの関係を表す観測モデルを用いて、状態遷移モデルにより推定された状態ベクトルx-kに基づいて観測値ベクトルykを推定し、この推定値を、推定された観測値ベクトル推定観測値ベクトルy-kとして出力する。なお、本実施形態のカルマンフィルタKFは、非線形カルマンフィルタであるシグマポイントカルマンフィルタにより構成される。シグマポイントカルマンフィルタとは、状態ベクトルxk-1の周囲に複数のシグマポイントχk-1を設定し、これらの複数のシグマポイントχk-1を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ-kを推定し、シグマポイントの推定値推定シグマポイントχ-kの平均を算出することにより、推定された状態ベクトルx-kを求める演算である。従って、推定された観測値ベクトルy-kは、厳密には、推定された状態ベクトルx-kの周辺に存在する複数のシグマポイントの推定値χ-kに基づいて算出される。その後、カルマンフィルタKFは、推定された観測値ベクトルy-kと、実際の観測値を要素とする観測値ベクトルykとの差分を観測残差ekとして算出する。また、カルマンフィルタKFは、推定された状態ベクトルx-kと、推定された観測値ベクトルy-kと、に基づいてカルマンゲインKkを算出する。そして、カルマンフィルタKFは、観測残差ekとカルマンゲインKkとを用いて推定された状態ベクトルx-kを更新し、更新後の状態ベクトル状態ベクトルを更新したベクトルxkを算出する。以上のような状態ベクトルxkの更新を繰り返すカルマンフィルタKFの演算により、状態ベクトルxkを実際の物理量を正確に表した値真値に近い正確な値へと近付けることができる。[3.1.カルマンフィルタ演算部の動作の概要]本実施形態におけるカルマンフィルタKFの状態遷移モデルは以下に示す式6で与えられ、観測モデルは以下に示す式7で与えられる。本実施形態では、式6に現れる関数f及び式7に現れる関数hは、非線形の関数である。ここで、状態ベクトルxkはn次元のベクトルであり、観測値ベクトルykはm次元のベクトルである。なお、本実施形態では、n=15であり、m=9である。また、式6に現れるプロセスノイズuk及び式7に現れる観測ノイズvkは0を中心とするガウスノイズである。式6は、時刻T=kにおける状態ベクトルxkが、時刻T=k-1における状態ベクトルxk-1を関数fに代入して得られた値と、時刻T=k-1におけるプロセスノイズuk-1とを加算することにより推定されるものであることを示している。また、式7は、時刻T=kにおける観測値ベクトルykが、時刻T=kにおける状態ベクトルxkを関数hに代入して得られる値と、時刻T=kにおける観測ノイズvkとを加算することにより推定されることを示している。なお、プロセスノイズukの共分散をQ、観測ノイズvkの共分散をRと表す。時刻T=kにおける観測残差ekは、実際の観測値ベクトルykと、推定された観測値ベクトルy-kとに基づいて定められるベクトルであり、以下に示す式8で表される。カルマンフィルタKFは、観測残差ekと、式9に示すカルマンゲインKkとを用いて、以下の式10に示すように、推定された状態ベクトルx-kを更新し、更新後の状態ベクトルxkを算出する。また、カルマンフィルタKFは、以下の式11に示すように、状態ベクトルxkの推定誤差の共分散Pkを更新する。なお、P-kは、推定された状態ベクトルx-kの推定誤差の共分散であり、Pkは、更新後の状態ベクトルxkの推定誤差の共分散である。また、Pyykは、観測残差ekの共分散であり、Pxykは、推定された状態ベクトルx-kと、推定された観測値ベクトルy-kとの相互共分散行列である。観測モデルを用いて推定された観測値ベクトルy-kは、式7に示すように、観測モデルを用いて推定された状態ベクトルx-kに基づいて算出される理論値である。よって、観測モデルを用いて推定された観測値ベクトルy-kと実際のセンサ出力に基づく観測値ベクトルykとの差分である観測残差ekは、推定された状態ベクトルx-kと実際の物理量を正確に表した値真値との近似度を示す値である。カルマンフィルタKFは、式10に示すように、観測残差ekを用いて、推定された状態ベクトルx-kを、真値に近い更新後の状態ベクトルxkへと更新する。[3.2.シグマポイントカルマンフィルタによる演算]本実施形態において、カルマンフィルタKFは、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、図3に示すカルマンフィルタKFの機能ブロック図を参照しつつ、カルマンフィルタKFによる演算の詳細を説明する。遅延部121は、加算器132から出力される更新後の状態ベクトルxkを、単位時間時刻T=k-1から時刻T=kに相当する時間だけ遅延させることで、状態ベクトルxk-1を生成し、これを、シグマポイント生成部122に対して出力する。なお、初回の演算時刻T=0では、遅延部121は、初期値生成モジュール110が出力する初期値INIを用いて、状態ベクトルxk-1を生成する。また、遅延部121は、減算器133から出力される更新後の状態ベクトルxkの推定誤差の共分散Pkを単位時間遅延させることで、状態ベクトルxk-1の推定誤差の共分散Pk-1を生成し、これを、シグマポイント生成部122及び共分散算出部125に対して出力する。シグマポイント生成部122は、状態ベクトルxk-1に基づいて、a個のシグマポイントχk-1ii=1,2,...aを生成するaは、2以上の自然数。シグマポイントχk-1iの生成は、以下に示す公知の方法を適用する。シグマポイント生成部122は、まず、状態ベクトルxk-1の推定誤差の共分散Pk-1から、式12に示す共分散Pk-1の平方根を表すn行n列の行列を生成する。次に、シグマポイント生成部122は、共分散Pk-1の平方根を表す行列と、各種係数とに基づいて、a個のシグマポイント算出用ベクトルを定める。シグマポイント算出用ベクトルは、n次元のベクトルである。その後、シグマポイント生成部122は、状態ベクトルxk-1と、a個のシグマポイント算出用ベクトルとに基づいて、a個のシグマポイントχk-1iを生成し、これを状態遷移モデル部123に対して出力する。なお、a個のシグマポイントχk-1iの生成において用いられる各種係数は、後述する式14における重みwiの生成にも用いられる値であり、公知の方法により定められる。また、a個のシグマポイント算出用ベクトルには0ベクトルが含まれていても構わない。状態遷移モデル部123は、式13に示すように、時刻T=k-1におけるa個のシグマポイントχk-1iの各々を、状態遷移モデルに適用することにより、時刻T=kにおけるa個のシグマポイントの推定値χ-kiを算出する。次に、平均算出部124は、式14に示すように、時刻T=kにおけるa個のシグマポイントの推定値χ-kiの重み付き平均を計算することで、推定された状態ベクトルx-kを算出し、これを出力する。なお、式14に現れる重みwiは、シグマポイントχk-1iの生成において用いられた各種係数に基づいて定められる。このように、シグマポイント生成部122、状態遷移モデル部123、及び平均算出部124は、状態ベクトルxk-1から、推定された状態ベクトルx-kを算出する推定状態ベクトル算出部140として機能する。共分散算出部125は、推定された状態ベクトルx-kの推定誤差の共分散P-kを算出する。推定された状態ベクトルx-kの推定誤差の共分散P-kは、式15に示すように、a個のシグマポイントの推定値χ-ki、推定された状態ベクトルx-k、重みwi、及びプロセスノイズuk-1の共分散Qに基づいて算出することができる。但し、本実施形態では、処理負荷を削減する観点から、共分散P-kのうち一部の成分は、式15を用いずに、後述する式30に基づいて算出する。この点についての詳細は後述する。一方、観測モデル部126は、式16に示すように、時刻T=kにおけるa個のシグマポイントの推定値χ-kiの各々を、観測モデルに適用することにより、a個の推定観測値γkiを算出する。そして、平均処理部127は、式17に示すように、a個の推定観測値γkiの平均を演算することにより、推定された観測値ベクトルy-kを算出する。このように、シグマポイント生成部122、状態遷移モデル部123、観測モデル部126、及び平均処理部127は、状態ベクトルxk-1から、推定された観測値ベクトルy-kを算出する推定観測値ベクトル算出部150として機能する。次に、平均処理部127は、式18に示す観測残差の共分散Pyykを算出する。減算器観測残差生成部131は、式8に示したように、観測値ベクトルykと、推定された観測値ベクトルy-kとの差分として、観測残差ekを算出する。カルマンゲイン付与部128は、式19に示す相互共分散行列Pxykを算出する。次に、カルマンゲイン付与部128は、式9に示したように、観測残差の共分散Pyykと、相互共分散行列Pxykとに基づいて、カルマンゲインKkを算出する。その後、カルマンゲイン付与部128は、式10の右辺第2項Kkekを演算し、その結果を加算器132に対して出力する。また、カルマンゲイン付与部128は、観測残差の共分散PyykとカルマンゲインKkとに基づいて、式11の右辺第2項KkPyykKTkを演算し、その結果を減算器133に対して出力する。加算器更新部132は、式10に示したように、推定された状態ベクトルx-kと、カルマンゲイン付与部128から出力される式10の右辺第2項Kkekと、を加算することにより、更新後の状態ベクトルxkを算出する。減算器133は、式11に示したように、推定された状態ベクトルx-kの推定誤差の共分散P-kと、カルマンゲイン付与部128から出力される式11の右辺第2項KkPyykKTkとの差分として、更新後の状態ベクトルxkの推定誤差の共分散Pkを算出する。[3.3.状態遷移モデル]次に、状態遷移モデル部123の演算で用いる状態遷移モデルについて説明する。本実施形態に係る状態遷移モデルにおいて、状態ベクトルxkを構成する状態変数のうち、姿勢qについての状態遷移は、式21に示す関数fqq,ωを用いて、式20により定義される。式20は、時刻T=kにおける推定された状態ベクトルx-kを構成する状態変数である姿勢q-kが、時刻T=k-1における状態ベクトルxk-1を構成する状態変数である姿勢qk-1及び角速度ωk-1に基づいて推定されることを示している。式21に示す関数fqq,ωは、式6に示した関数fのうち、姿勢qを算出する部分のみを表した関数である。式21の右辺の演算子Ωは、式22により定義される。ここで、I3×3は3行3列の単位行列を表し、3次元ベクトルl=l1,l2,l3に対して、[l×]という記号を式23で定義する。また、測定時間間隔時刻T=k-1から時刻T=kまでの間隔をΔtで表したとき、演算子Ωを構成する成分Ψを、式24で定義する。姿勢qは、クォータニオンで表現されるため、正規化条件q=1が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルxkを更新する場合、更新後の状態ベクトルxkは、式14に示すように、シグマポイントの推定値χ-kiに重みwiを付与した重み付き平均として算出されるため、更新前の状態ベクトルxk-1のノルムと、更新後の状態ベクトルxkのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、更新前の状態ベクトルxk-1の要素である姿勢qk-1のノルムと、更新後の状態ベクトルxkの要素である姿勢qkのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢qについての正規化条件が満たされなくなる可能性が存在する。そこで、姿勢qに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルxkを構成する要素のうち姿勢qkについてはMRPsを用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。地磁気の強さr及び地磁気の伏角θは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおける地磁気の強さrk及び伏角θkと、時刻T=k-1における地磁気の強さrk-1及び伏角θk-1とは等しい値であるという前提を置く。同様に、3次元角速度センサ90のオフセット推定値gOも、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおけるオフセット推定値gOKと、時刻T=k-1におけるオフセット推定値gOK-1とは等しい値であるという前提を置く。携帯機器1の角速度ωは、携帯機器1の利用者による携帯機器1の動かし方に依存して変化するため、時刻T=k-1の角速度ωk-1を用いて、時刻T=kにおける角速度ωkを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおける角速度ωkと、時刻T=k-1における角速度ωk-1とは等しいと仮定する。前述のとおり、3次元磁気センサ70のオフセットmOFFは、携帯機器1の部品が発する内部磁界である。従って、携帯機器1の内部状態が一定である間は、オフセットmOFFも変化しない。一方、携帯機器1の内部状態が変化した場合、例えば、携帯機器1に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器1に搭載された部品の着磁状況が変化した場合には、オフセットmOFFも変化する。すなわち、携帯機器1の内部状態は、携帯機器1の利用者による携帯機器1の操作や、携帯機器1の外部の環境等に依存して変化する。従って、オフセットmOFFが変化するタイミングやオフセットmOFFの変化量を予測することは困難であり、時刻T=k-1におけるオフセット推定値mOK-1を用いて、時刻T=kにおけるオフセット推定値mOkを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおけるオフセット推定値mOkと、時刻T=k-1におけるオフセット推定値mOk-1とは等しいと仮定する。このように、本実施形態に係る状態遷移モデルでは、以下に示す式25のように、状態ベクトルxkを構成する状態変数のうち、姿勢q以外は、前の時刻から変化しないという前提を置く。ここで、式1に示した状態ベクトルxkを、式26のように、姿勢qkと、姿勢qk以外の要素を表すベクトルβkとに分離して表現した場合、ベクトルβkについて、式27が成立する。すなわち、状態ベクトルxk-1のうち、姿勢第2ベクトルqk-1以外の要素を表すベクトル第1ベクトルβk-1と、推定された状態ベクトルx-kのうち、姿勢推定第2ベクトルq-k以外の要素を表すベクトル推定第1ベクトルβ-kとは等しい。式27が成立する場合、式13から明らかなように、シグマポイントχk-1iのうちベクトルβk-1に相当する要素と、シグマポイントの推定値χ-kiのうちベクトルβ-kに相当する要素とは、等しくなる。従って、式27が成立する場合、状態ベクトルxk-1の推定誤差の共分散Pk-1のうち、ベクトルβk-1の推定誤差の共分散を表す成分と、推定された状態ベクトルx-kの推定誤差の共分散P-kのうち、ベクトルβ-kの推定誤差の共分散を表す成分とは、プロセスノイズuk-1の共分散Qを考慮しなければ、等しいものと看做すことができる段落[0046]参照。ここで、共分散Pkを、式28に示すように、行列Pqq,k、行列Pqβ,k、行列Pqβ,kT、及び行列Pββ,kの、4つの行列に分割する。行列Pqq,kは、姿勢qkに基づいて定められる、4行4列の行列である。行列Pqβ,kは、姿勢qkとベクトルβkとに基づいて定められる、4行11列の行列である。行列Pββ,kは、ベクトルβkに基づいて定められる、11行11列の行列である。具体的には、行列Pββ,kは、ベクトルβkの推定誤差の共分散を表す行列である。また、プロセスノイズukの共分散Qを、式29のように、4行4列の行列Qqq、4行11列の行列Qqβ、及び11行11列の行列Qββを用いて表す。式6に示したとおり、プロセスノイズukは、状態ベクトルxkを状態遷移モデルに適用する際に生じるプロセスノイズであり、式29に示す共分散Qは、当該プロセスノイズukの共分散を表す。従って、式29に現れる、行列Qββは、状態ベクトルxkを状態遷移モデルに適用する際に生じるプロセスノイズのうち、状態ベクトルxkの中で姿勢qk以外の要素を表すベクトルβkに生じるプロセスノイズの共分散を表す。このとき、行列P-ββ,kは、式30に示すように、行列Pββ,k-1と、行列Qββとの和として定められる。共分散P-kの全ての成分を式15に基づいて求める場合、n行1列のベクトルと1行n列のベクトルとを乗算してn行n列の行列を求める演算を、a回繰り返すことが必要になる。一方、式30を用いる場合、行列P-ββ,kは、1ステップ前に時刻T=k-1において既に算出されている行列Pββ,k-1と、行列Qββとを単純に加算することにより算出される。そして、共分散P-kは、行列P-ββ,k以外の成分、すなわち、4行4列の行列P-qq,k及び4行11列の行列P-qβ,kのみを、式15による演算により求めればよい。この結果、15行15列の共分散P-kの全ての成分を、式15を用いて算出する場合に比べて、共分散算出部125における計算量を大幅に低減することが可能となる。なお、状態遷移モデルにおいて、式25及び式27に示すように、姿勢q以外は、前の時刻から変化しないという前提を置いた場合であっても、状態ベクトルxkは、カルマンフィルタの演算において観測残差ekに基づいて更新される。従って、姿勢qk以外の要素を表すベクトルβkも、カルマンフィルタの演算を繰り返す中で、観測残差ekに基づいて真値に近づくように更新される。[3.4.観測モデル]次に、観測モデル部126で行われる演算において用いられる観測モデルについて説明する。3次元角速度センサ90から出力される角速度データgの推定値γgyroは、角速度ωと、3次元角速度センサ90のオフセット推定値gOとを用いて、式31で与えられる。また、地上に固定された座標系において、地磁気を表すベクトルは、式32で与えられる。従って、3次元磁気センサ70から出力される磁気データmの推定値γmagは、3次元磁気センサ70のオフセット推定値mOと、式5で示した行列Bqとを用いて、式33で与えられる。また、地上に固定された座標系において、重力を表すベクトルを重力加速度で正規化したベクトルは、式34で与えられる。従って、3次元加速度センサ80から出力される加速度データaの推定値γaccは、式5で示した行列Bqを用いて、式35で与えられる。従って、式3を式8の右辺第1項に代入し、式31、式33、及び式35を用いて式8の右辺第2項を表すことにより、式8を、以下の式36に変形することができる。このとき、観測残差ekは、式36により算出される。[4.結論]以上に示したように、本実施形態に係る状態推定装置は、シグマポイントカルマンフィルタの演算において用いられる状態ベクトルxkを、姿勢qkと、ベクトルβkとに分離して表現し、状態ベクトルxk-1を構成する要素の一部であるベクトルβk-1と、推定された状態ベクトルx-kを構成する要素の一部であるベクトルβ-kとが等しいという前提を置いた。そして、本実施形態に係る状態推定装置は、推定された状態ベクトルx-kの推定誤差の共分散P-kのうちベクトルβ-kの推定誤差の共分散を表す成分を、状態ベクトルxk-1の推定誤差の共分散Pk-1のうちベクトルβk-1の推定誤差の共分散を表す成分と、プロセスノイズuk-1の共分散Qのうちベクトルβk-1に生じるプロセスノイズの共分散を表す成分とを単純に加算することで算出した。これにより、共分散P-kの全ての成分を式15に基づいて求める場合と比較して、シグマポイントカルマンフィルタの計算負荷を大幅に低減することが可能となり、状態推定装置の処理速度の向上、及び携帯機器1の低消費電力化が可能となった。<B.変形例>本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの2以上の変形例を組み合わせることもできる。1変形例1上述した実施形態では、状態ベクトルxを構成する要素して、携帯機器1の姿勢q、地磁気の強さr、地磁気の伏角θ、携帯機器1の角速度ω、3次元角速度センサ90のオフセット推定値gO、及び3次元磁気センサ70のオフセット推定値mOを採用し、これら6個の状態変数をカルマンフィルタの演算における推定の対象としたが、本発明はこれに限定されるものでは無い。例えば、状態ベクトルxを、これら6個の状態変数のうちの一部の状態変数により構成し、当該一部の状態変数をカルマンフィルタの演算における推定の対象とするものであってもよい。2変形例2上述した実施形態では、観測値ベクトルyを、3次元磁気センサ70から出力される磁気データm、3次元加速度センサ80から出力される加速度データa、及び3次元角速度センサ90から出力される角速度データgを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、磁気データm、加速度データa、及び角速度データgのうちの一部のみを利用して観測値ベクトルyを生成してもよい。3変形例3上述した実施形態では、平均算出部124は、式14に示すように、推定された状態ベクトルx-kを、a個のシグマポイントの推定値χ-kiの重み付き平均として算出したが、本発明はこれに限定されるものではない。例えば、平均算出部124は、推定された状態ベクトルx-kを構成する要素の一部であるベクトルβ-kを、式14を用いず、単純に、状態ベクトルxk-1を構成する要素の一部であるベクトルβk-1と等しい値に設定してもよい。この場合、平均算出部124は、推定された状態ベクトルx-kのうち、姿勢q-kについてのみ、式14の重み付き平均を算出する演算により算出する。これにより、15次元のベクトルである推定された状態ベクトルx-kのうち、11次元のベクトルβ-kについては、単純な値の代入により算出することが可能となるため、推定された状態ベクトルx-kの算出に係る処理負荷を、大幅に低減することができる。"