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