"システムを観測して観測値を各々出力する複数のセンサと、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行するカルマンフィルタと、を備え、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする地磁気測定装置。前記第1変数をρとし、前記第2変数をηとしたとき、前記第1関数は、expρで表され、前記第2関数は、tan-1ηで表されることを特徴とする、請求項1に記載の地磁気測定装置。システムを観測して観測値を各々出力する複数のセンサを備える地磁気測定装置に用いられる地磁気測定方法であって、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新し、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする地磁気測定方法。システムを観測して観測値を各々出力する複数のセンサを備える地磁気測定装置に用いられる地磁気測定プログラムであって、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定する処理と、観測値モデルを用いて前記状態ベクトルから推定観測値を算出する処理と、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出する処理と、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する処理とを、コンピュータに実行させ、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする地磁気測定プログラム。一般的に、非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値観測値を推定する観測モデルと、を有する。そして、非線形カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分観測残差を用いて、動的システムの状態を表す複数の物理量を要素とする状態ベクトルを、より真値に近い値へと更新する。しかし、非線形カルマンフィルタを用いた演算を繰り返す場合、観測値等に重畳しているノイズの影響や、状態遷移モデルと観測モデルとが有する非線形性の影響により、動的システムの状態が実際の値から大きく外れた値として推定される場合が存在するという問題点がある。本発明は、上述した点に鑑みてなされたものであり、非線形カルマンフィルタを用いて動的システムの状態を推定する場合に、推定される値が実際の値から大きく外れた不適切な値になることを防止し、正確な地磁気の値を算出することを解決課題とする。上記課題を解決するため、本発明に係る地磁気測定装置は、システムを観測して観測値を各々出力する複数のセンサと、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行するカルマンフィルタと、を備え、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする。この発明によれば、カルマンフィルタを演算することにより、地磁気の強さを表す第1関数の変数第1変数と地磁気の伏角を表す第2関数の変数第2変数とを要素として含む状態ベクトルを、観測残差を用いて更新し、真の値に近づける。カルマンフィルタを用いた演算を行う場合、観測値に重畳しているノイズの影響により、算出される状態ベクトルを構成する状態変数が、真の値から大きく外れた値として算出される場合がある。状態変数が、一度、真の値から大きく外れた値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、当該状態変数は真の値に近づかない可能性が高い。地磁気の強さは、地球上のどの位置で観測しても、ある値以上の値を有している。よって、地磁気の強さが当該ある値以下の値となる場合には、真の値から大きく外れた値であると言える。従って、カルマンフィルタの演算により磁気の強さが当該ある値以下の値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の強さが、当該ある値以上の正しい値に復元される可能性は低い。この発明によれば、地磁気の強さを第1の値よりも大きな値となる第1関数で表す。従って、非線形カルマンフィルタを用いた演算の結果として第1変数がどのような値に変化しても、地磁気の強さが、真の値から大きく外れた第1の値以下の値として算出されることは無い。これにより、地磁気の強さが、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。また、地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地磁気の伏角は、地球上のどの位置で観測しても、ある範囲の値となる。よって、カルマンフィルタの演算により、地磁気の伏角が当該ある範囲を超えた値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の伏角が当該ある範囲内の正しい値に復元される可能性は低い。この発明によれば、地磁気の伏角を、第2の値よりも大きく且つ第3の値よりも小さな値をとる第2関数で表す。従って、非線形カルマンフィルタを用いた演算の結果として第2変数がどのような値に変化しても、地磁気の伏角が、真の値から大きく外れた第2の値よりも小さな値、または、第3の値よりも大きな値として算出されることは無い。これにより、地磁気の伏角が、真の値から大きく外れた値として算出することを防ぎ、その結果として不正確な地磁気の値を算出することを防止することができる。また、上述した地磁気測定装置において、前記第1変数をρとし、前記第2変数をηとしたとき、前記第1関数は、expρで表され、前記第2関数は、tan-1ηで表されることが好ましい。この発明によれば、地磁気の強さを、第1変数ρを用いた第1関数expρとして表すため、変数ρがいかなる値に変化しても、expρ>0が満たされる。地磁気の強さは、正の値である必要がある。よって、地磁気の強さが負の値となる場合には、真の値から大きく外れた値であると言える。従って、カルマンフィルタの演算により磁気の強さが負の値として算出される場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の強さが正の値に復元される可能性は低い。この発明によれば、地磁気の強さを、0よりも大きな値の第1関数expρで表すため、カルマンフィルタを用いた演算の結果として第1変数ρがどのような値に変化しても、地磁気の強さが、真の値から大きく外れた0以下の値として算出されることは無い。これにより、地磁気の強さが、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。同様に、この発明によれば、地磁気の伏角を、第2変数ηを用いた第2関数tan-1ηとして表すため、変数ηがいかなる値に変化しても、-π/2<tan-1η<π/2が満たされる。地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地磁気の伏角は、北の磁極に近づくに従って、90度に近づき、南の磁極に近づくに従って、-90度に近づく。つまり、地磁気の伏角は、-π/2よりも大きく、且つ、π/2よりも小さな値となる必要がある。よって、カルマンフィルタの演算により、地磁気の伏角が、-π/2以下の値、または、π/2以上の値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の伏角が-π/2よりも大きく、且つ、π/2よりも小さな正しい値に復元される可能性は低い。この発明によれば、地磁気の伏角を、-π/2よりも大きく且つπ/2よりも小さな値をとる第2関数tan-1ηで表すため、カルマンフィルタを用いた演算の結果として第2変数ηがどのような値に変化しても、地磁気の伏角が、真の値から大きく外れた-π/2以下の値、または、π/2以上の値として算出されることは無い。これにより、地磁気の伏角が、真の値から大きく外れた値として算出することを防ぎ、その結果として不正確な地磁気の値を算出することを防止することができる。次に、本発明に係る地磁気測定方法は、システムを観測して観測値を各々出力する複数のセンサを備える状態推定装置に用いられる状態推定方法であって、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新し、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする。この発明によれば、地磁気の強さ及び伏角が、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。次に、本発明にかかる地磁気測定装置に用いられる地磁気測定プログラムは、システムを観測して観測値を各々出力する複数のセンサを備える地磁気測定装置に用いられる地磁気測定プログラムであって、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定する処理と、観測値モデルを用いて前記状態ベクトルから推定観測値を算出する処理と、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出する処理と、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する処理とを、コンピュータに実行させ、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする。この発明によれば、地磁気の強さ及び伏角が、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。<A.実施形態>本発明の実施の形態について図面を参照して説明する。[1.機器構成及びソフトウェア構成]図1は、本発明の実施形態に係る携帯電話機のブロック図であり、図2は携帯電話機の外観を示す斜視図である。携帯電話機1は、携帯電話機1の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯電話機1の姿勢を推定することによって実現される。携帯電話機1は、各種の構成要素とバスを介して接続され装置全体を制御するCPU10、CPU10の作業領域として機能するRAM20、各種のプログラムやデータを記憶したROM30、通信を実行する通信部40、画像を表示する表示部50、及びGPS部60を備える。GPS部60は、GPS衛星からの信号を受信して携帯電話機1の位置情報緯度、経度を生成するものである。地磁気を検出して地磁気データを出力する地磁気検出部70、加速度を検出して加速度データを出力する加速度検出部80、角速度を検出して角速度データを出力する角速度検出部90を備える。地磁気検出部70は、X軸地磁気センサ71、Y軸地磁気センサ72、及びZ軸地磁気センサ73を備える。各センサは、MI素子磁気インピーダンス素子、MR素子磁気抵抗効果素子などを用いて構成することができる。地磁気センサI/F74は、各センサからの出力信号をAD変換して地磁気データを出力する。この地磁気データは、x軸、y軸およびz軸の3成分によって、磁極北に向かう水平成分と伏角方向の鉛直成分とを有するベクトルを示すデータである。なお、地磁気検出部70は、携帯電話機1に設けられるので、携帯電話機1の内部の部品スピーカなどが出す磁気と、携帯電話機1の外部にある外部物が出す磁気と、実際の地磁気とが合成された磁界を検出する。しかしながら、後述する地磁気測定プログラム100で想定している状態遷移モデルでは、外部物が出す磁気は無視できる程小さいものとする。加速度検出部80は、X軸加速度センサ81、Y軸加速度センサ82、及びZ軸加速度センサ83を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサI/F84は、各センサからの出力信号をAD変換して加速度データを出力する。この加速度データは、加速度検出部80と一体となって運動する物体固定の系における慣性力と重力との合力を、x軸、y軸及びz軸の3成分を有するベクトルとして示すデータである。したがって、携帯電話機1が静止状態または等速直線運動状態にあれば、加速度データは画面に対して固定されたxyz座標空間において重力加速度の大きさと方向とを示すベクトルデータである。角速度検出部90は、X軸角速度センサ91、Y軸角速度センサ92、及びZ軸角速度センサ93を備える。角速度センサI/F94は、各センサからの出力信号をAD変換して角速度データを出力する。角速度データは、各軸の回りの角速度を示す。CPU10は、ROM30に格納されている地磁気測定プログラム100を実行することによって、動的システムの状態を表す複数の物理量、例えば、携帯電話機1の姿勢や、地磁気の強さや伏角等を推定する。すなわち、携帯電話機1は、動的システムの状態を表す複数の物理量の推定値に基づいて地磁気の正確な値を算出する、地磁気測定装置として機能する。地磁気測定プログラム100は、初期値生成モジュール110と、非線形カルマンフィルタモジュール120とを備える。非線形カルマンフィルタモジュール120は、非線形カルマンフィルタKFの演算を実行する。非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムの有する複数のセンサが計測する値観測値を推定する観測モデルと、を有する。そして、非線形カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定し、推定された状態ベクトルに基づいて複数の観測値を要素とする観測値ベクトルを推定する。ここで、観測値ベクトルとは、地磁気検出部70から出力される地磁気データの値、加速度検出部80から出力される加速度データの値、及び角速度検出部90から出力される角速度データの値を要素とするベクトルである。次に、非線形カルマンフィルタは、推定された観測値ベクトルと、地磁気検出部70、加速度検出部80、及び角速度検出部90からの実際の出力を要素とする観測値ベクトルとの差分として算出される観測残差に基づいて、状態ベクトルをより真値に近い値へと更新する。非線形カルマンフィルタは、以上のような演算を繰り返すことにより、動的システムの状態を表す複数の物理量の推定値を、より実際の値真値に近い正確な値へと更新する。本実施形態における非線形カルマンフィルタは、動的システムの状態を表す物理量として、携帯電話機1の姿勢を表すクォータニオンq、地磁気の強さr、地磁気の伏角θ、携帯電話機1の角速度ω、角速度センサ91~93のオフセット推定値go、及び地磁気センサ71~73のオフセット推定値moを採用し、これらを推定の対象とする。ところで、非線形カルマンフィルタを用いた演算を繰り返す場合、観測値に重畳しているノイズの影響や、状態遷移モデル及び観測モデルが有する非線形性の影響により、推定される物理量が実際の値から大きく外れた値として算出される場合がある。この場合には、その後、非線形カルマンフィルタによる演算を繰り返しても、真の値に近い物理量を推定することは難しい。そこで、本実施形態では、動的システムの状態を表す複数の物理量のうち、値の範囲を定めることのできる物理量は、当該範囲外の値に変化しないような状態変数を用いて表す。そして、複数の物理量の各々に対応する状態変数を要素とするような状態ベクトルxkを定め、カルマンフィルタの演算を実行する。以下において、本実施形態に係る状態変数について述べる。まず、動的システムの状態を表す複数の物理量のうち、地磁気の強さrと、地磁気の伏角θについて検討する。前述の通り、地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界である。具体的には、地磁気の伏角は、北の磁極に近づくに従って、90度第3の値に近づき、南の磁極に近づくに従って、-90度第2の値に近づく。つまり、地磁気の伏角θは、-π/2<θ<π/2を満たす。また、地磁気の強さrは、0第1の値よりも大きな値となる。そこで、本実施形態では、地磁気の強さrを、変数ρを用いて式1に示す関数第1関数expρとして表し、この変数ρを状態変数とする。また、地磁気の伏角θを、変数ηを用いて式2に示す関数第2関数tan-1ηとして表し、この変数ηを状態変数とする。この場合、変数ρがいかなる値に変化しても、expρ>0が満たされる。すなわち、非線形カルマンフィルタの演算を繰り返した場合であっても、地磁気の強さrは、r>0という条件を満たすことになる。また、変数ηがいかなる値に変化しても、-π/2<tan-1η<π/2が満たされる。すなわち、非線形カルマンフィルタの演算を繰り返した場合であっても、地磁気の伏角θは、-π/2<θ<π/2という条件を満たすことになる。携帯電話機1の姿勢を表すクォータニオンqは、物体の姿勢回転状態を表す数学的表現であって、4次元数で与えられる。ここで、地面に固定された座標系での地磁気ベクトルと重力ベクトルがそれぞれme、geであるとする。物体が静止していると仮定したとき、センサ固定の座標系で磁気センサと加速度センサの出力がそれぞれme、geとなる姿勢を基準姿勢とし、その基準姿勢を表すクォータニオンをq=0、0、0、1と定める。任意の姿勢は基準姿勢に対して単位ベクトルαの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢を表すクォータニオンqは以下に示す式3で与えられる。携帯電話機1は、任意の姿勢を取り得るので、姿勢を表すクォータニオンqの値の範囲を制限することができない。従って、姿勢を表すクォータニオンqは、値に制限を加えることなく、そのまま状態変数として採用する。また、角速度ω、角速度センサのオフセット推定値go、及び地磁気センサのオフセット推定値moについても、任意の値を取り得るため、値に制限を加えることなく、そのまま状態変数として採用する。なお、状態変数に地磁気センサのオフセット推定値mo及び角速度センサのオフセット推定値goを含ませたのは、地磁気データにはセンサ71~73のオフセットが重畳し、角速度データにはセンサ91~93のオフセットが重畳するからである。例えば、携帯電話機1には磁石を備えたスピーカが内蔵されているため、地磁気センサは磁石の磁力を常時検出してしまう。あるいは、温度変化によってもオフセットが変動する。動的システムの状態を表す複数の物理量を正確に推定するためには、オフセット推定値moにより補正された地磁気データ及びオフセット推定値goにより補正された角速度データを用いる必要がある。以上のように定めた状態変数を要素とする時刻kにおける状態ベクトルxkは、以下の式4で表される。次に、カルマンフィルタにおいて観測の対象となるのは、3軸の地磁気センサ71~73の出力である観測値m、3軸の加速度センサ81~83の出力である観測値a、及び3軸の角速度センサ91~93の出力である観測値gである。観測値m、a、gは各々X軸、Y軸及びZ軸の3つの要素からなる観測値ベクトルyである。時刻kにおける観測値ベクトルykは式5で与えられる。非線形カルマンフィルタKFは、このような、状態ベクトルxk及び観測値ベクトルykを用いて、動的システムの状態を表す複数の物理量を推定する。具体的には、非線形カルマンフィルタKFは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、現在時刻時刻k-1のシステムの状態を示す状態ベクトルxk-1から単位時間が経過した後時刻kの状態ベクトルxkを推定する。次に、動的システムの状態から各種センサの観測値を推定する観測モデルを用いて、推定された状態ベクトルxkから、観測値ベクトルykを推定する。そして、推定された観測値ベクトル推定観測値と、各種センサの出力実際の観測値を表す観測値ベクトルykとの差分を観測残差ekとして算出し、観測残差ekに基づいてカルマンゲインKkを算出する。このようにして算出された観測残差ekと、カルマンゲインKkとに基づいて、非線形カルマンフィルタKFは、状態ベクトルxkをより正確な値に更新する。初期値生成モジュール110は、時刻k=0における状態変数を要素とする初期値INI状態ベクトルを算出し、非線形カルマンフィルタモジュール120に対して出力する。初期値INIは、姿勢を表すクォータニオンqの初期値q0、変数ρの初期値ρ0、変数ηの初期値η0、角速度ωの初期値ω0、角速度センサのオフセット推定値goの初期値go,0、及び地磁気センサのオフセット推定値moの初期値mo,0を含む。初期値INIは、状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下で説明するような値に設定しても良い。地磁気の強さrを算出するための変数ρの初期値ρ0、及び地磁気の伏角θを算出するための変数ηの初期値η0は、例えば、GPS部60から供給される位置情報に基づいて生成する。これは、地球上の位置が特定できれば、その位置における地磁気の強さr及び伏角θを知ることができるからである。具体的には、初期値生成モジュール110は、ROM30に位置情報と地磁気の強さr及び伏角θとを対応づけて記憶したルックアップテーブルLUT1を格納しておき、これを参照して位置情報に対応する地磁気の強さr及び伏角θを読み出す。そして、初期値生成モジュール110は、位置情報に対応する地磁気の強さrに基づいて初期値ρ0を算出すると共に、位置情報に対応する伏角θに基づいて初期値η0を算出する。なお、携帯電話機1が衛星からの電波の届かない場所例えば、地下街にある場合には、GPS部60から位置情報が得られない。そのような場合には、携帯電話機1が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルLUT1に格納されている。初期値生成モジュール110は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルLUT1から読み出す。例えば、日本で販売された携帯電話機1については、明石市の地磁気の強さr及び伏角θに基づいて、初期値ρ0及び初期値η0を算出すればよい。角速度ωの初期値の初期値ω0は、例えば、携帯電話機1が静止していることを仮定して、0に設定する。角速度センサのオフセット推定値g。の初期値go,0についても、例えば、携帯電話機1が静止していることを仮定して、0に設定する。姿勢を表すクォータニオンqの初期値q0に関しては、例えば、携帯電話機1が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を、初期値INIに設定する。地磁気センサのオフセット推定値moの初期値mo,0は、例えば、時刻k=0の地磁気センサの観測値m0、携帯電話機1を使用する地域の地磁気ベクトルmtypical、及び、式7に示す行列Bqを用いて、以下に示す式6で与えられる値に設定する。ここで、行列Bqは、地面に固定された座標系を、姿勢qを用いて携帯電話機1の地磁気センサに固定された座標系に変換するために用いられる行列であり、以下に示す式7で与えられる。なお、ROM30には、位置情報経度、緯度と地磁気ベクトルmtypicalとを対応づけて記憶したルックアップテーブルLUT2が記憶されている。初期値生成モジュール110は、GPS部60で生成される位置情報に基づいてルックアップテーブルLUT2を参照して地磁気ベクトルmtypicalを取得し、式6を演算することによって、地磁気センサのオフセット推定値moの初期値mo,0を得る。以上ように、初期値生成モジュール110は、初期値INIを、非線形カルマンフィルタKFに対して出力する。[2.非線形カルマンフィルタ]次に、非線形カルマンフィルタKFの内容について説明する。[2.1.非線形カルマンフィルタの概要]図3は、非線形カルマンフィルタKFの機能ブロック図である。非線形カルマンフィルタKFのシステムの状態遷移モデルは以下に示す式8で与えられ、観測モデルは以下に示す式9で与えられる。ここで、式8に現れる状態ベクトルxkはn次元のベクトルであり、式9に現れる観測値ベクトルykはm次元のベクトルである本実施形態では、n=15、m=9である。式4で示したように状態ベクトルxkは、姿勢を表すクォータニオンqkや地磁気センサのオフセット推定値mok等の状態変数を要素とする。プロセスノイズwkと観測ノイズυkは0を中心とするガウスノイズである。式8は、非線形カルマンフィルタKFの状態遷移モデルを表す。式8は、時刻kにおける状態ベクトルxkが、時刻k-1における状態ベクトルxk-1を関数fに代入し、その結果と時刻k-1におけるプロセスノイズwk-1とを加算したものであることを示している。また、式9は、非線形カルマンフィルタKFの観測モデルを表す。式9は、時刻kにおける観測値ベクトルykが、時刻kにおける状態ベクトルxkを関数hに代入し、その結果と時刻kにおける観測ノイズυkとを加算したものであることを示している。なお、以下の説明では、プロセスノイズwkの共分散をQk、観測ノイズυkの共分散をRkとする。観測残差ekは、時刻kの観測値ベクトルykと、観測モデルを用いて推定された観測値ベクトルとの差分であり、以下に示す式10で表すことができる。また、非線形カルマンフィルタは、観測残差ekと、式11に示すカルマンゲインKkとを用いて、以下の式12に示すように、状態ベクトルの推定値を更新し、以下の式13に示すように、状態ベクトルの推定誤差の共分散を更新する。式10に示すように、観測モデルを用いて推定された観測値ベクトルは、状態ベクトルxkの推定値を、観測モデルに適用して算出される。従って、観測値ベクトルykと、観測モデルを用いて推定された観測値ベクトルとの差分である観測残差ekにより、状態ベクトルxkが実際の値に近い適切な値に収束したか否かを判断することができる。式10の減算は、図3に示す減算器260で実行される。また、式12の右辺第2項の演算は、カルマンゲイン付与部270において実行され、式12の加算は加算器280において実行される。遅延部290は、加算器280の出力を次の時刻まで遅延させる。[2.2.シグマポイントカルマンフィルタによる演算]なお、本実施形態では、非線形カルマンフィルタKFは、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、シグマポイントカルマンフィルタを用いる場合の、状態ベクトルの更新、状態ベクトルの推定誤差の算出、観測値ベクトルの推定値の算出、及び相互相関行列の算出について、具体的に説明する。シグマポイントカルマンフィルタでは、n行n列の共分散行列を用いて、最初に2n1個のシグマポイントを生成する。まず、複数のシグマポイントの平均に対して、平均のまわりのシグマボイントの広がりを表すスケーリングパラメータλを用いて、式14及び式15を定義する。このとき、シグマポイントは、式16及び式17で決定される。式14乃至式17の演算は、シグマポイント生成部210によって実行される。なお、シグマポイント生成部210には、初期値INIが供給される。シグマポイントカルマンフィルタの初回の演算では、初期値INIを式16の右辺に用いてシグマポイント生成部210はシグマポイントを生成する。状態遷移モデル部220は、ある時刻k-1のシグマポイントから、単位時間が経過した後の時刻kのシグマポイントを推定する。この演算は式18で与えられる。また、推定される状態ベクトルの平均は、以下に示す式19で与えられる。この演算は平均算出部230において実行される。また、推定される状態ベクトルの誤差の共分散は、以下に示す式20で与えられる。次に、推定される観測値は、以下に示す式21で与えられる。γkiの演算は観測モデル部240において実行され、式21の演算は平均処理部250において実行される。ここで、残差の共分散は、以下に示す式22で与えられる。また、相互相関行列は式23で与えられる。[2.3.状態遷移モデル]次に、状態遷移モデル部220の演算で用いる状態遷移モデルについて説明する。本実施形態における姿勢推定モデルでは、以下に示す式24のように、状態ベクトルxkを構成する状態変数のうち、姿勢を表すクォータニオンq以外は、前の時刻から変化しないと仮定する。但し、実際にはカルマンゲインKkと観測残差ekとによって、次の状態ベクトルxkが補正されるので、値が全く変化しないわけではなく、真値に近づいてゆく。ここで、姿勢の回転を表す演算子Ωを、以下の式25で定義する。但し、I3×3を3行3列の単位行列とし、また、3次元ベクトルu=u1,u2,u3に対して、[u×]という記号を式26で定義する。また、ΔTを、測定時間間隔として、演算子Ωを構成する成分を、式27で定義する。このとき、ある時刻k-1の姿勢を表すクォータニオンqk-1から、単位時間が経過した後の時刻kの姿勢を表すクォータニオンqkを推定する演算は、式28により表される。姿勢を表すクォータニオンqは、正規化条件q=1が満たされていないといけないが、シグマポイントの平均を求めるとその条件が満たされなくなってしまう。クォータニオンqに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化するようにすればそのような問題は解決される。より厳密に正規化条件を保つためには、状態ベクトルのうち姿勢についてはMRPsを用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。これにより、システム全体として姿勢を推定する装置として機能する。[2.4.観測モデル]次に、観測モデル部240で実行する観測モデルの演算について説明する。角速度センサ91~93の出力である観測値gの推定値γgyroは、角速度ωと、角速度センサのオフセット推定値goとを用いて、式29で与えられる。地面に固定された水平東、水平北、鉛直上の順の座標系での地磁気ベクトルは式30で与えられると推定している。従って地磁気センサ71~73の出力である観測値mの推定値γmagは、式7の行列を用いて、式31で与えられる。また、地面に固定された水平東、水平北、鉛直上の順の座標系での重力ベクトルが重力加速度で正規化されているとすると、式32が成立する。従って、加速度センサ81~83の出力である観測値aの推定値γaccは、式7の行列を用いて、式33で与えられる。従って、式5、式29、式31、及び式33を、式10に適用することにより、観測残差ekが式34で表される。[3.状態変数の収束]以上に述べた本実施形態に係る非線形カルマンフィルタKFは、動的システムの状態を表す複数の物理量をそのまま状態ベクトルxkとして採用するのではなく、値の範囲を限定することのできる物理量については、当該物理量が当該範囲外の値に変化しないような状態変数により表現して、状態ベクトルxkの要素とした。これにより、本実施形態に係る非線形カルマンフィルタKFは、状態ベクトルxkが不適切な値に収束することを防止し、動的システムの状態を表す複数の物理量について、真値に近い値を推定することが可能である。以下では、本実施形態に係る非線形カルマンフィルタKFの有効性を示すために、本実施形態と、動的システムの状態を表す複数の物理量をそのまま状態変数として採用した場合以下、対比例と称するとを、比較して説明する。対比例は、以下の式35に示す状態ベクトルCxkを用いる。状態ベクトルCxkは、その要素として、変数ρの代わりに地磁気の強さrを含み、変数ηの代わりに地磁気の伏角θを含む点で、式4に示した本実施形態に係る状態ベクトルxkと相違する。非線形カルマンフィルタKFに状態ベクトルCxkを用いる場合、地磁気の強さr及び地磁気の伏角θが現実とは大きく異なる値に収束することがある。図4に示すように、地磁気の強さrの実際の値真値が0.5ガウスで、地磁気の伏角θの実際の値真値がπ/3である場合を想定し、携帯電話機1が基準姿勢である場合を仮定した事例を用いて、本実施形態に係る状態ベクトルxkが収束する様子と、状態ベクトルCxkが真値から大きく離れた値に収束する様子とについて、説明する。本実施形態のように、地磁気の強さrが変数ρを用いて表される場合には、地磁気の強さrが、rの取り得る範囲r>0から逸脱することは無い。同様に、地磁気の伏角θが変数ηを用いて表される場合には、地磁気の伏角θが、θの取り得る範囲-π/2<θ<π/2から逸脱することは無い。この場合には、地磁気の強さr及び伏角θが真値に収束する可能性が高くなる。図4の事例では、例えば、状態ベクトルxkのうち、変数ρにより表される地磁気の強さrは、r=0.5ガウスに収束し、変数ηにより表される地磁気の伏角θは、θ=π/3に収束する可能性が高い。一方、対比例では、ある瞬間k=kaにおいて、地磁気の強さr及び伏角θが真値から大きく離れた値となり、その後再び真値に近づくことなく偽値に収束する場合がある。図4の事例では、例えば、状態ベクトルCxkのうち、地磁気の強さrがr=-0.5ガウスに収束し、地磁気の伏角θがθ=-2π/3に収束する場合がある。この場合、状態ベクトルxkが真値に収束した場合の観測残差と、状態ベクトルCxkが偽値に収束した場合の観測残差とは、等しい値となる。具体的には、式36に示す本実施形態に係る推定値γmagと、式37に示す対比例に係る推定値Cγmagとは、等しい値となるため、式34より、両者の観測残差は等しくなる。従って、本実施形態に係る推定値γmagが、真値に近い値である場合には、対比例に係る推定値Cγmagも真値に近い値と看做されるため、対比例に係る状態ベクトルCxkが、観測残差によって、真値に近づくように補正されることは無い。すなわち、対比例にかかる非線形カルマンフィルタKFで推定された地磁気の強さr及び伏角θは、真値とは大きく異なる偽値であるが、観測残差による更新によっても補正されない局所最適解であるため、その後、正しい値に収束復元することは無い。このように、本実施形態では、非線形カルマンフィルタKFの推定対象となる動的システムの状態を表す複数の物理量のうち、値の範囲を定めることのできる物理量については、定められた範囲以外の値とならないように、状態変数を定めた。具体的には、非線形カルマンフィルタKFの推定対象となる地磁気の強さr及び伏角θを、変数ρを用いた関数expρ及び変数ηを用いた関数tan-1ηとして表すことで、地磁気の強さrがr>0という条件から逸脱せず、また、地磁気の伏角θが-π/2<θ<π/2という条件から逸脱しないようにすることを可能とした。これにより、状態ベクトルxkが局所最適解に陥ることを防止し、状態ベクトルxkが真値とはかけ離れた値で安定することを防止することが可能となった。<B.変形例>本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの2以上の変形例を組み合わせることもできる。1変形例1上述した実施形態では、非線形カルマンフィルタの一種であるシグマポイントカルマンフィルタを一例として説明したが、本発明はこれに限定されるのではなく、拡張カルマンフィルタなど、どのような非線形カルマンフィルタに適用してもよい。動的システムの状態を表す物理量のうち、値の範囲を定めることのできる物理量について、当該物理量が当該範囲から外れた値に変化しないような状態変数を定めることで、非線形カルマンフィルタを用いた演算において、状態ベクトルが局所最適解に陥ることを防止することが可能となる。2変形例2上述した実施形態では、動的システムの状態を表す複数の物理量のうち、地磁気の強さr及び伏角θの値の取り得る範囲を制限するように状態変数xkを定めたが、本発明はこれに限定されるものでは無く、動的システムの状態を表す物理量のうち、値の取り得る範囲を定めることのできる物理量については、当該範囲から外れた値とならないような状態変数を定めても良い。これにより、拡張カルマンフィルタに適用した場合であっても、状態ベクトルが局所最適解に陥ることを防止することが可能となる。3変形例3本発明では、非線形カルマンフィルタKFを用いて推定する動的システムの状態を表す物理量を、姿勢を表すクォータニオンq、地磁気の強さr、地磁気の伏角θ、角速度ω、角速度センサのオフセット推定値go、及び地磁気センサのオフセット推定値moとしたが、本発明はこれに限定されるものでは無く、これらのうちの一部について推定するものであっても良い。4変形例4本発明では、観測値ベクトルykを、地磁気センサの出力である観測値m、加速度センサの出力である観測値a、及び角速度センサの出力である観測値gを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、このうちの一部のみを利用して観測値ベクトルを生成しても良い。"