(57)【要約】 【課題】非線形カルマンフィルタを用いて正確な状態推定を行う。【解決手段】システムを観測して観測値を各々出力する複数のセンサと、位置情報Ｐｓを出力するＧＰＳ部６０と、物体Ｏｂｊを撮影し画像情報Ｉｍｇを出力するカメラ１１０と、を備える携帯機器１に組み込まれ、システムの状態を推定する状態推定装置であって、携帯機器１の姿勢μを推定する状態変数を要素として含む状態ベクトルｘと、観測値の各々を要素とする観測値ベクトルｙとを用いて、状態ベクトルｘを更新するカルマンフィルタＫＦを１以上備えるカルマンフィルタ演算部３００と、画像情報Ｉｍｇ及び位置情報Ｐｓに基づいて携帯機器１の姿勢μの初期値μ０を算出したうえで、当該初期値μ０を要素とする初期ベクトルＩＮＩを生成し、これを、状態ベクトルｘの初期値としてカルマンフィルタＫＦに供給する初期値生成部２００とを備える。【選択図】図３
2013185898.tif 000002
【特許請求の範囲】
【請求項１】
システムを観測して観測値を各々出力する複数のセンサと、位置情報を出力する位置情報出力部と、物体を撮影し画像情報を出力する画像取得部と、を備える機器に組み込まれ、前記システムの状態を推定する状態推定装置であって、
前記システムの状態を推定する値である複数の状態変数を要素とする状態ベクトルと、前記観測値の各々を要素とする観測値ベクトルとを用いて、前記状態ベクトルを更新するカルマンフィルタを１以上備えるカルマンフィルタ演算部と、
前記状態ベクトルの初期値である初期ベクトルを生成する初期値生成部と、
を備え、
前記複数の状態変数は、
前記機器の姿勢を推定する状態変数を含み、
前記初期値生成部は、
１以上の物体を前記画像取得部が撮影した際に前記画像取得部が出力する前記画像情報と、前記位置情報出力部が出力する前記位置情報と、に基づいて、前記機器の姿勢を推定する状態変数の初期値である初期姿勢推定値を算出し、前記初期姿勢推定値を要素とする前記初期ベクトルを生成する、
ことを特徴とする状態推定装置。
【請求項２】
前記カルマンフィルタ演算部は、
並列に動作する所定数の前記カルマンフィルタを備え、
前記初期値生成部は、
前記画像取得部が、１の物体を撮影して前記画像情報を出力した場合に、前記画像情報に基づいて、前記機器に固定された３軸の座標系である機器座標系において、前記機器から見た前記１の物体の方向を表した第１方向ベクトルを算出する画像処理部と、
前記位置情報に基づいて、地球上に固定された３軸の座標系である地上座標系において、前記機器から見た前記１の物体の方向を表した第２方向ベクトルを算出し、前記第１方向ベクトル及び前記第２方向ベクトルに基づいて、初期姿勢候補値を算出し、前記初期姿勢候補値を、前記第２方向ベクトルを回転軸として所定角度ずつ回転させることで、前記所定数の前記初期姿勢推定値を算出する初期姿勢算出部と、
前記所定数の前記初期姿勢推定値の各々に対応する、前記所定数の前記初期ベクトルを生成し、これらを前記所定数の前記カルマンフィルタにそれぞれ供給する初期ベクトル生成部と、を備える、
ことを特徴とする請求項１に記載の状態推定装置。
【請求項３】
前記カルマンフィルタ演算部は、
１つの前記カルマンフィルタを備え、
前記初期値生成部は、
前記画像取得部が、２つの物体を撮影して前記画像情報を出力した場合に、前記画像情報に基づいて、前記機器に固定された３軸の座標系である機器座標系において、前記機器から見た前記２つの物体の各々の方向を表した２つの第１方向ベクトルを算出する画像処理部と、
前記位置情報に基づいて、地球上に固定された３軸の座標系である地上座標系において、前記機器から見た前記２つの物体の各々の方向を表した２つの第２方向ベクトルを算出し、前記２つの第１方向ベクトル及び前記２つの第２方向ベクトルに基づいて、前記初期姿勢推定値を算出する、初期姿勢算出部と、
前記初期姿勢推定値を要素とする前記初期ベクトルを生成し、これを前記カルマンフィルタに供給する初期ベクトル生成部と、を備える、
ことを特徴とする請求項１に記載の状態推定装置。
【請求項４】
前記状態推定装置は、
時刻情報を出力する時刻出力部を備え、
前記初期姿勢算出部は、
前記位置情報と、前記時刻情報と、に基づいて、前記第２方向ベクトルを算出する、
ことを特徴とする請求項２または３に記載の状態推定装置。
【請求項５】
前記物体は、
前記地上座標系における座標が既知であり、
前記初期姿勢算出部は、前記位置情報と、前記物体の前記地上座標系における座標とに基づいて、前記第２方向ベクトルを算出する、
ことを特徴とする請求項２または３に記載の状態推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は状態推定装置に関する。
【背景技術】
【０００２】
地磁気の向きや、物体の姿勢等を算出する場合、地磁気センサからの出力結果のみに基づいて算出するよりも、地磁気センサの他に加速度センサや角速度センサ等の異種の物理量を測定する複数のセンサの出力結果を統合して算出した方が、短時間で正確な値を求めることができる。
【０００３】
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、非線形カルマンフィルタを用いる方法が知られている。例えば、特許文献１には、３軸の角速度センサ及び３軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献１には、３軸角速度センサ、３軸加速度センサ、及び３軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合し、姿勢を推定する方法が開示されている。
【０００４】
一般的に、非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値（観測値）を推定する観測モデルとを有する。そして、非線形カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分（観測残差）を用いて、動的システムの状態を表す複数の物理量の各々の推定値（状態変数）を要素とする状態ベクトルを推定し逐次更新することにより、状態ベクトルの各要素の示す値を、実際の物理量（真値）に近づける演算を行う。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平９−５１０４号公報
【非特許文献】
【０００６】
【非特許文献１】Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008
【発明の概要】
【発明が解決しようとする課題】
【０００７】
ところで、非線形カルマンフィルタは、状態ベクトルと、状態ベクトルに基づいて算出される観測残差とを用いて、状態ベクトルを逐次更新する演算であるため、状態ベクトルの初期値の各要素が真値から乖離した値に設定された場合、状態ベクトルの各要素が真値に近い値に収束するまでに長時間を要することがあり、更には、状態ベクトルの各要素が真値とは異なる不正確な値に収束することも起こり得る。この場合、非線形カルマンフィルタは、システムの状態を正確に推定することはできない。
【０００８】
本発明は、上述した点に鑑みてなされたものであり、非線形カルマンフィルタを用いて正確な状態推定を行うことを解決課題とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決するため、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、位置情報を出力する位置情報出力部と、物体を撮影し画像情報を出力する画像取得部と、を備える機器に組み込まれ、前記システムの状態を推定する状態推定装置であって、前記システムの状態を推定する値である複数の状態変数を要素とする状態ベクトルと、前記観測値の各々を要素とする観測値ベクトルとを用いて、前記状態ベクトルを更新するカルマンフィルタを１以上備えるカルマンフィルタ演算部と、前記状態ベクトルの初期値である初期ベクトルを生成する初期値生成部と、を備え、前記複数の状態変数は、前記機器の姿勢を推定する状態変数を含み、前記初期値生成部は、１以上の物体を前記画像取得部が撮影した際に前記画像取得部が出力する前記画像情報と、前記位置情報出力部が出力する前記位置情報と、に基づいて、前記機器の姿勢を推定する状態変数の初期値である初期姿勢推定値を算出し、前記初期姿勢推定値を要素とする前記初期ベクトルを生成する、ことを特徴とする。
【００１０】
機器の姿勢は、当該機器の利用者の動作に依存するため、初期姿勢推定値を事前に定められた何らかの固定値とした場合には、初期姿勢推定値と、機器の真の姿勢とは大きく乖離する場合がある。
この発明によれば、画像情報と位置情報とに基づいて初期姿勢推定値を算出する。画像情報に基づいて、機器から見た物体の方向を算出することができる。従って、機器の位置を表す位置情報に基づいて、機器と物体との位置関係を算出できる場合、機器から見た物体の方向と、機器と物体との位置関係とに基づいて、機器の姿勢を算出することができる。これにより、機器の姿勢を推定する状態変数と、機器の真の姿勢とが大きく乖離することを防止することができ、状態推定装置がシステムの状態を正確に推定することが可能となる。
【００１１】
また、上述した状態推定装置において、前記カルマンフィルタ演算部は、並列に動作する所定数の前記カルマンフィルタを備え、前記初期値生成部は、前記画像取得部が、１の物体を撮影して前記画像情報を出力した場合に、前記画像情報に基づいて、前記機器に固定された３軸の座標系である機器座標系において、前記機器から見た前記１の物体の方向を表した第１方向ベクトルを算出する画像処理部と、前記位置情報に基づいて、地球上に固定された３軸の座標系である地上座標系において、前記機器から見た前記１の物体の方向を表した第２方向ベクトルを算出し、前記第１方向ベクトル及び前記第２方向ベクトルに基づいて、初期姿勢候補値を算出し、前記初期姿勢候補値を、前記第２方向ベクトルを回転軸として所定角度ずつ回転させることで、前記所定数の前記初期姿勢推定値を算出する初期姿勢算出部と、前記所定数の前記初期姿勢推定値の各々に対応する、前記所定数の前記初期ベクトルを生成し、これらを前記所定数の前記カルマンフィルタにそれぞれ供給する初期ベクトル生成部と、を備える、ことが好ましい。
【００１２】
この発明によれば、初期値生成部は、所定数の初期姿勢推定値を算出し、所定数の初期姿勢推定値の各々に対応して、所定数の初期ベクトルを生成する。そして、初期値生成部は、これらの所定数の初期ベクトルを、所定数のカルマンフィルタにそれぞれ供給する。
所定数の初期ベクトルは、互いに異なる値を示す。また、所定数の初期ベクトルの各々は、画像情報に基づいて定められる第１方向ベクトルと、位置情報に基づいて定められる第２方向ベクトルとを用いて算出される。従って、所定数の初期ベクトルの中には、各要素が、実際の物理量から大きく乖離しない値を有する初期ベクトルが含まれる。当該初期ベクトルが供給されるカルマンフィルタは、システムの状態を正確に推定することが可能である。これにより、本発明に係る状態推定装置は、システムの状態を正確に推定することが可能となる。
【００１３】
また、上述した状態推定装置において、前記カルマンフィルタ演算部は、１つの前記カルマンフィルタを備え、前記初期値生成部は、前記画像取得部が、２つの物体を撮影して前記画像情報を出力した場合に、前記画像情報に基づいて、前記機器に固定された３軸の座標系である機器座標系において、前記機器から見た前記２つの物体の各々の方向を表した２つの第１方向ベクトルを算出する画像処理部と、前記位置情報に基づいて、地球上に固定された３軸の座標系である地上座標系において、前記機器から見た前記２つの物体の各々の方向を表した２つの第２方向ベクトルを算出し、前記２つの第１方向ベクトル及び前記２つの第２方向ベクトルに基づいて、前記初期姿勢推定値を算出する、初期姿勢算出部と、前記初期姿勢推定値を要素とする前記初期ベクトルを生成し、これを前記カルマンフィルタに供給する初期ベクトル生成部と、を備える、ことが好ましい。
【００１４】
この発明によれば、２つの第１方向ベクトル及び２つの第２方向ベクトルに基づいて、初期姿勢推定値を算出するため、初期姿勢推定値は、機器の実際の姿勢を正確に表す値となる。従って、本発明に係る状態推定装置は、正確な状態推定を行うことが可能となる。
【００１５】
また、上述した状態推定装置は、時刻情報を出力する時刻出力部を備え、前記初期姿勢算出部は、前記位置情報と、前記時刻情報と、に基づいて、前記第２方向ベクトルを算出する、ことが好ましい。
【００１６】
この発明に係る物体は、例えば、太陽、月等の天体が該当する。天体は、位置情報と時刻情報とにより、地上座標系において機器から見た物体の方向を特定することができる。すなわち、位置情報と時刻情報とに基づいて算出される第２方向ベクトルは、機器から見た物体の方向を正確に表す。
状態推定装置は、このような第２方向ベクトルに基づいて初期姿勢推定値を算出するため、正確な状態推定を行うことが可能となる。
【００１７】
また、上述した状態推定装置において、前記物体は、前記地上座標系における座標が既知であり、前記初期姿勢算出部は、前記位置情報と、前記物体の前記地上座標系における座標とに基づいて、前記第２方向ベクトルを算出する、ことを特徴とすることが好ましい。
【００１８】
この発明に係る物体は、例えば、著名な建造物等のランドマークのうち、地上座標系における座標が既知のランドマークが該当する。地上座標系における座標が既知の物体は、当該座標と、機器の位置情報とに基づいて、機器から見た物体の方向を特定することができる。すなわち、位置情報と、物体の地上座標系における座標とに基づいて算出される第２方向ベクトルは、機器から見た物体の方向を正確に表す。
状態推定装置は、このような第２方向ベクトルに基づいて初期姿勢推定値を算出するため、正確な状態推定を行うことが可能となる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施形態に係る携帯機器の構成を示すブロック図である。
【図２】携帯機器の外観を示す斜視図である。
【図３】状態推定装置の機能を表す機能ブロック図である。
【図４】携帯機器の姿勢と物体との関係を表す説明図である。
【図５】カルマンフィルタ演算部の機能を表す機能ブロック図である。
【図６】変形例１に係る初期値生成部の機能を表す機能ブロック図である。
【図７】変形例５に係る状態推定装置の機能を表す機能ブロック図である。
【図８】変形例６に係るカルマンフィルタ演算部の機能を表す機能ブロック図である。
【発明を実施するための形態】
【００２０】
＜Ａ．実施形態＞
本発明の実施の形態について図面を参照して説明する。
【００２１】
［１．  機器構成及びソフトウェア構成について］  
図１は、本発明の実施形態に係る携帯機器のブロック図であり、図２は携帯機器の外観を示す斜視図である。携帯機器１（機器）は、携帯機器１の姿勢変化に応じて、携帯機器１が備える画面に表示される地図などの画像を回転させることで、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯機器１の姿勢や、携帯機器１から見た地磁気の向き等を推定することによって実現される。
【００２２】
携帯機器１は、各種の構成要素とバスを介して接続され装置全体を制御するＣＰＵ１０、ＣＰＵ１０の作業領域として機能するＲＡＭ（蓄積部）２０、状態推定プログラム１００等の各種プログラム及び各種設定情報を記憶したＲＯＭ３０、通信を実行する通信部４０、画像を表示する表示部５０、ＧＰＳ衛星からの情報（具体的には、携帯機器１の緯度、経度についての情報）を取得してこれを出力するＧＰＳ部６０、携帯機器１の外部に存在する物体Ｏｂｊを撮影して画像情報Ｉｍｇを出力するカメラ１１０（画像取得部）、携帯機器１の利用者の各種入力に用いられる操作部１２０、及び、時刻情報Ｔｉｍｅを出力する時計１３０（時刻出力部）を備える。
また、携帯機器１は、地磁気等の磁気を検出して磁気データｑを出力する３次元磁気センサ７０、加速度を検出して加速度データａを出力する３次元加速度センサ８０、及び角速度を検出して角速度データｇを出力する３次元角速度センサ９０を備える。
なお、本実施形態は、各種プログラム及び各種設定情報をＲＯＭ３０に格納するが、各種プログラム及び各種設定情報のうちの一部または全部をＲＡＭ２０に格納してもよい。
【００２３】
表示部５０は、ＣＰＵ１０が状態推定プログラム１００を実行することにより推定したシステムの状態に基づいて算出された地磁気の向きや携帯機器１の姿勢等を、矢印等の画像により表示する。なお、表示部５０は、カメラ１１０が物体Ｏｂｊを撮影して物体Ｏｂｊの描かれた画像情報Ｉｍｇを出力する場合に、当該画像情報Ｉｍｇに基づいた画像を表示するものであってもよい。
ＧＰＳ部６０は、ＧＰＳ衛星からの信号を受信して携帯機器１の位置情報Ｐｓ（緯度、経度）を出力する。すなわち、ＧＰＳ部６０は、携帯機器１の位置情報Ｐｓを取得し、これを出力する「位置情報出力部」として機能する。
操作部１２０は、携帯機器１の利用者が、カメラ１１０を用いた物体Ｏｂｊを撮影するための指示の入力に用いられる。
【００２４】
３次元磁気センサ７０は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及びＺ軸磁気センサ７３を備える。各センサは、ＭＩ素子（磁気インピーダンス素子）、ＭＲ素子（磁気抵抗効果素子）などを用いて構成することができる。磁気センサＩ／Ｆ７４は、Ｘ軸磁気センサ７１、Ｙ軸磁気センサ７２、及び、Ｚ軸磁気センサ７３からの出力信号をＡＤ変換して磁気データｑを出力する。この磁気データｑは、ｘ軸、ｙ軸およびｚ軸の３成分によって表されるベクトルデータである。より具体的には、磁気データｑは、携帯機器１に固定（厳密には、３次元磁気センサ７０に固定）された３軸の直交座標系において、Ｘ軸磁気センサ７１からの出力値がｘ軸成分として表され、Ｙ軸磁気センサ７２からの出力値がｙ軸成分として表され、Ｚ軸磁気センサ７３からの出力値がｚ軸成分として表されるベクトルデータである。
以下において、携帯機器１に固定された３軸の直交座標系を、「機器座標系ΣＳ」と称する。なお、３次元磁気センサ７０に固定された座標系、カメラ１１０に固定された座標系、３次元加速度センサ８０に固定された座標系、及び、３次元角速度センサ９０に固定された座標系は、厳密には、それぞれ異なる位置に原点を有するが、以下では、説明の便宜上、機器座標系ΣＳと総称する場合がある。
【００２５】
ところで、３次元磁気センサ７０が検出する磁気データｑには、地磁気Ｂｇが含まれる。
地磁気Ｂｇは、磁極北に向かう水平成分と伏角方向の鉛直成分を有する磁界であり、地上のある一点に固定された３軸の直交座標系（以下、「地上座標系ΣＧ」と称する）において、一定の向き及び大きさを有するベクトルＧＢｇとして表される。
ここで、ベクトルの符号の左上に示す添え字「Ｇ」は、当該ベクトルが地上座標系ΣＧで表されたベクトルであることを意味する。なお、地上座標系ΣＧの原点と、地上座標系ΣＧが有する互いに直交する３軸の向きとは、どのように定めてもよい。本実施形態では、地上座標系ΣＧを、ｙ軸が磁極北を向き、ｚ軸が鉛直上を向くように定める。
よって、機器座標系ΣＳにおいて表した場合、地磁気Ｂｇは、携帯機器１の姿勢μに連動して向きを変える、一定の大きさのベクトルＳＢｇ（μ）として表される（なお、ベクトルの符号の左上に示す添え字「Ｓ」は、当該ベクトルが機器座標系ΣＳで表されたベクトルであることを意味する）。従って、機器座標系ΣＳにおいて地磁気Ｂｇを表すベクトルＳＢｇ（μ）に基づいて、携帯機器１の姿勢μを求めることができる。
一方、３次元磁気センサ７０が搭載される携帯機器１は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、３次元磁気センサ７０が出力するベクトルデータ（磁気データｑ）は、地磁気Ｂｇを表すベクトルと、携帯機器１に搭載された部品が発する磁界（内部磁界Ｂｉ）を表すベクトルとを加算したベクトルとなる。従って、機器座標系ΣＳにおける地磁気Ｂｇを表すベクトルＳＢｇ（μ）を算出し、携帯機器１から見た地磁気Ｂｇの向きを正確に求めるためには、機器座標系ΣＳにおいて、３次元磁気センサが出力する磁気データｑの示す座標から、携帯機器１の部品が発する内部磁界Ｂｉを表すベクトルＳＢｉを減算する補正処理が必要となる。
このように、検出対象である地磁気Ｂｇの正確な向きを特定するために、補正処理において、３次元磁気センサ７０から出力される磁気データｑから取り除かれる内部磁界Ｂｉを表すベクトルＳＢｉを、３次元磁気センサ７０のオフセットと呼ぶ。
【００２６】
３次元加速度センサ８０は、Ｘ軸加速度センサ８１、Ｙ軸加速度センサ８２、及びＺ軸加速度センサ８３を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などのような検知方式であってもよい。加速度センサＩ／Ｆ８４は、各センサからの出力信号をＡＤ変換して加速度データａを出力する。この加速度データａは、機器座標系ΣＳ（厳密には、３次元加速度センサ８０に固定された座標系）における慣性力と重力との合力を、ｘ軸、ｙ軸及びｚ軸の３成分を有するベクトルとして示すデータである。従って、携帯機器１が静止状態または等速直線運動状態にあれば、加速度データａは、機器座標系ΣＳにおいて重力加速度の大きさと方向とを示すベクトルデータとなる。
【００２７】
３次元角速度センサ９０は、Ｘ軸角速度センサ９１、Ｙ軸角速度センサ９２、及びＺ軸角速度センサ９３を備える。角速度センサＩ／Ｆ９４は、各センサからの出力信号をＡＤ変換して角速度データｇを出力する。角速度データｇは、機器座標系ΣＳ（厳密には、３次元角速度センサ９０に固定された座標系）の各軸の回りの角速度を示すベクトルデータである。
【００２８】
ＣＰＵ１０は、ＲＯＭ３０に格納されている状態推定プログラム１００を実行することによって、携帯機器１の姿勢や３次元磁気センサ７０のオフセット等のシステムの状態を表す複数の物理量を推定する。すなわち、携帯機器１は、ＣＰＵ１０が状態推定プログラム１００を実行することにより、状態推定装置として機能する。
【００２９】
図３は、状態推定装置のうち、ＣＰＵ１０が状態推定プログラム１００を実行することにより実現される機能を表す、機能ブロック図である。
図３に示すように、状態推定装置は、互いに異なる複数の初期ベクトルＩＮＩを出力する初期値生成部２００、並列に動作する複数のカルマンフィルタを用いて並列的に非線形カルマンフィルタの演算を実行し複数の状態ベクトルｘを出力するカルマンフィルタ演算部３００、カルマンフィルタ演算部３００が出力する複数の状態ベクトルｘから１つの状態ベクトルｘを選択してこれを出力する出力情報制御部４００、及び、出力情報制御部４００が出力する１つの状態ベクトルｘに基づいて出力情報を生成する出力情報生成部５００を備える。
【００３０】
本実施形態において、カルマンフィルタ部３００は、４個のカルマンフィルタＫＦを備える。各々のカルマンフィルタＫＦは、非線形カルマンフィルタの演算を行う。
詳細は後述するが、非線形カルマンフィルタの演算とは、状態推定装置が推定するシステムの状態を表すベクトルである状態ベクトルｘと、状態推定装置が備えるセンサからの出力値を要素とするベクトルである観測値ベクトルｙと、を用いて算出される観測残差ｚに基づいて、状態ベクトルｘを周期的に更新することにより、システムの状態を推定する演算である。なお、状態ベクトルｘは、初期ベクトルＩＮＩを初期値とする。
以下では、説明の便宜上、４個のカルマンフィルタＫＦを区別する場合、４個のカルマンフィルタのそれぞれを、ＫＦ［１］、ＫＦ［２］、ＫＦ［３］、ＫＦ［４］と表記する。また、４個のカルマンフィルタの中で、第ｍ番目（ｍは、１≦ｍ≦４を満たす自然数）のカルマンフィルタを、ＫＦ［ｍ］と表記する場合がある。
４個のカルマンフィルタＫＦの各々は、非線カルマンフィルタの演算を実行することで算出される状態ベクトルｘと、観測残差ｚとを出力する。
【００３１】
ここで、状態ベクトルｘとは、複数の状態変数を要素とするベクトルである。状態ベクトルｘの要素である複数の状態変数の各々は、それぞれ、非線形カルマンフィルタが推定するシステムの状態を示す物理量を表す。
本実施形態は、状態ベクトルｘの要素（状態変数）として、携帯機器１の姿勢μ、地磁気の強さｒ、地磁気の伏角φ、携帯機器１の角速度ω、３次元角速度センサ９０のオフセット推定値ｇＯＦＦ、及び、３次元磁気センサ７０のオフセット推定値ｑＯＦＦを採用し、これらを非線形カルマンフィルタの演算における推定の対象とする。
時刻Ｔ＝ｋにおける状態ベクトルｘｋは、以下の式（１）で表される。各状態変数の右下に付された添え字「ｋ」は、当該状態変数が時刻Ｔ＝ｋにおける値であることを表す。なお、以下では、各種変数、ベクトル、行列の右下に添え字「ｋ」を付すことで、当該変数、ベクトル、行列等が時刻Ｔ＝ｋにおける値であることを示す場合がある。
【数１】
2013185898.tif 000003
【００３２】
ここで、地磁気の強さｒはスカラーであり、地磁気の伏角φはスカラーであり、角速度ωは３次元ベクトルであり、３次元角速度センサ９０のオフセット推定値ｇＯＦＦは３次元ベクトルであり、３次元磁気センサ７０のオフセット推定値ｑＯＦＦは３次元ベクトルである。
また、姿勢μは、クォータニオンを用いて表現される。クォータニオンとは、物体の姿勢（回転状態）を表す４次元数である。例えば、機器座標系ΣＳの各軸と、地上座標系ΣＧの各軸とが一致する姿勢μを「基準姿勢」と定義し、基準姿勢をμ＝（０、０、０、１）と表現する。このとき、携帯機器１の任意の姿勢μは、単位ベクトルρを回転軸とし、単位ベクトルρの示す方向から見て、基準姿勢を角度ψだけ反時計回りに回転した姿勢として表現できる。当該姿勢μは、クォータニオンを用いて、式（２）で表される。
【数２】
2013185898.tif 000004
【００３３】
また、観測値ベクトルｙとは、３次元磁気センサ７０から出力される磁気データｑ、３次元加速度センサ８０から出力される加速度データａ、及び、３次元角速度センサ９０から出力される角速度データｇを要素とするベクトルである。時刻Ｔ＝ｋにおける観測値ベクトルｙｋは式（３）で与えられる。
【数３】
2013185898.tif 000005
【００３４】
各々のカルマンフィルタＫＦは、非線形カルマンフィルタの演算を実行し、当該カルマンフィルタＫＦが有する状態ベクトルｘの各要素が真値に近づくように状態ベクトルｘを周期的に更新することで、システムの状態を表す複数の物理量を推定する。
ここで、「真値」とは、状態ベクトルｘｋの各要素に対応する実際の物理量を表す値である。また、以下では、状態ベクトルｘｋの各要素を真値としたベクトルを、「状態ベクトルの真値」と称する。なお、非線形カルマンフィルタの演算の詳細については、後述する。
説明の便宜上、以下では、第ｍ番目のカルマンフィルタＫＦ［ｍ］が有する状態ベクトルｘｋに、符号［ｍ］を付す場合がある。また、当該第ｍ番目のカルマンフィルタＫＦ［ｍ］が非線形カルマンフィルタの演算で用いる各種行列、ベクトル等についても、符号［ｍ］を付して表現する場合がある。
なお、詳細は後述するが、本実施形態では、各々のカルマンフィルタＫＦが更新する状態ベクトルｘｋとは、厳密には、更新後の状態ベクトルｘ＋ｋである。
【００３５】
初期値生成部２００は、ＲＯＭ３０に格納された設定情報（具体的には、後述する情報Ｐｏｂｊ、位置情報と地磁気の強さ及び伏角とを対応づけた情報、等）、ＧＰＳ部６０が出力する位置情報Ｐｓ、カメラ１１０が出力する画像情報Ｉｍｇ等に基づいて、互いに異なる４個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］を生成し、これらを出力する。
４個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］は、それぞれ、４個のカルマンフィルタＫＦ［１］〜ＫＦ［４］に供給され、各カルマンフィルタＫＦが非線形カルマンフィルタの演算を実行する際に使用する状態ベクトルｘの初期値として採用される。すなわち、初期ベクトルＩＮＩ［ｍ］は、状態ベクトルｘｋの初期値、つまり時刻Ｔ＝０における状態ベクトルｘ０である。
【００３６】
第ｍ番目のカルマンフィルタＫＦ［ｍ］に供給される初期ベクトルＩＮＩ［ｍ］は、携帯機器１の姿勢μ［ｍ］の初期値μ０［ｍ］（初期姿勢推定値）、地磁気の強さｒ［ｍ］の初期値ｒ０［ｍ］、地磁気の伏角φ［ｍ］の初期値φ０［ｍ］、携帯機器１の角速度ω［ｍ］の初期値ω０［ｍ］、３次元角速度センサ９０のオフセット推定値ｇＯＦＦ［ｍ］の初期値ｇＯＦＦ,０［ｍ］、及び３次元磁気センサ７０のオフセット推定値ｑＯＦＦ［ｍ］の初期値ｑＯＦＦ,０［ｍ］を要素として含む。この初期ベクトルＩＮＩ［ｍ］は、以下の式（４）により示される。
なお、初期ベクトルＩＮＩ［ｍ］の各要素の生成方法については、後述する。
【数４】
2013185898.tif 000006
【００３７】
初期値生成部２００は、画像処理部２１０、初期姿勢算出部２２０、及び、初期ベクトル生成部２３０を備える。
画像処理部２１０は、カメラ１１０の出力する画像情報Ｉｍｇに基づいて、携帯機器１から見た物体Ｏｂｊの方向を機器座標系ΣＳにおいて表した第１方向ベクトルαを算出する。初期姿勢算出部２２０は、ＧＰＳ部６０が出力する位置情報Ｐｓ、ＲＯＭ３０に格納された情報Ｐｏｂｊ、及び、画像処理部２１０の出力する第１方向ベクトルαに基づいて、姿勢μの初期値μ０［１］〜μ０［４］（初期姿勢推定値）を算出する。初期ベクトル生成部２３０は、初期姿勢算出部２２０が算出する姿勢μの初期値μ０［１］〜μ０［４］と、ＲＯＭ３０に格納された設定情報（具体的には、位置情報と地磁気の強さ及び伏角とを対応づけた情報）と、に基づいて、初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］を算出する。
【００３８】
出力情報制御部４００は、４個のカルマンフィルタＫＦ［１］〜ＫＦ［４］からの出力値に基づいて、４個のカルマンフィルタＫＦ［１］〜ＫＦ［４］の中で、推定精度が最も高いカルマンフィルタＫＦ［Ｓｅｌ］を選択する。そして出力情報制御部４００は、選択したカルマンフィルタＫＦ［Ｓｅｌ］が出力する状態ベクトルｘｋ［Ｓｅｌ］を、出力情報生成部５００に対して供給する。
【００３９】
なお、カルマンフィルタＫＦの推定精度とは、カルマンフィルタＫＦがシステムの状態をどの程度正確に推定しているかについて表す指標であり、具体的には、状態ベクトルｘｋの示す値と状態ベクトルの真値との誤差の大きさに基づいた値（誤差評価値）である。そして、カルマンフィルタＫＦの推定精度が高い状態とは、状態ベクトルｘｋの示す値と状態ベクトルの真値との誤差が小さい（つまり、誤差評価値が小さい）状態のことを意味する。
状態ベクトルｘｋの示す値と状態ベクトルの真値との誤差が小さい場合（つまり、状態ベクトルｘｋの各要素が真値に近い正確な値を示す場合）、当該状態ベクトルｘｋを有するカルマンフィルタＫＦは、システムを正確に推定していると看做すことができる。
【００４０】
出力情報制御部４００は、４個のピークモニタＰＭ［１］〜ＰＭ［４］、比較部４１０、及び、選択部４２０を備える。
第ｍ番目のピークモニタＰＭ［ｍ］は、以下の式（５）に示すように、第ｍ番目のカルマンフィルタＫＦ［ｍ］が出力する観測残差ｚｋ［ｍ］を、時刻Ｔ＝ｋ−τ０から時刻Ｔ＝ｋまでの所定期間モニターし、当該期間における観測残差ｚｋ［ｍ］のノルムの最大値を、誤差評価値ＭＡＸｚｋ［ｍ］として出力する。
【数５】
2013185898.tif 000007
【００４１】
なお、本実施形態では、一定期間における観測残差ｚｋ［ｍ］のノルムの最大値を誤差評価値ＭＡＸｚｋ［ｍ］として採用するが、本発明はこのような形態に限定されるものではなく、観測残差ｚｋ［ｍ］を構成する複数の要素のうち、一部の要素から構成されるベクトルのノルムの所定期間における最大値を誤差評価値として採用してもよい。
詳細は後述するが、本実施形態に係る観測残差ｚｋ［ｍ］は、磁気データｑに対応する要素、加速度データａに対応する要素、及び、角速度データｇに対応する要素により構成されるベクトルである（式（５２）参照）。この場合、ピークモニタＰＭ［ｍ］は、例えば、観測残差ｚｋ［ｍ］を構成する要素のうち、磁気データｑに対応する要素について所定期間モニターし、当該要素のノルムの最大値を誤差評価値ＭＡＸｚｋ［ｍ］として採用してもよい。
【００４２】
比較部４１０は、４個のピークモニタＰＭ［１］〜ＰＭ［４］がそれぞれ出力する誤差評価値ＭＡＸｚｋ［１］〜ＭＡＸｚｋ［４］のうち、最小となる誤差評価値ＭＡＸｚｋ［Ｓｅｌ］に対応するカルマンフィルタＫＦ［Ｓｅｌ］を示す選択値Ｓｅｌを出力する（但し、Ｓｅｌは、１≦Ｓｅｌ≦４を満たす自然数）。
前述のとおり、観測残差ｚは、状態ベクトルｘと観測値ベクトルｙとを用いて算出される値であり、観測残差ｚに基づいて、状態ベクトルｘの示す値と状態ベクトルの真値との誤差の大きさを表すことができる。しかし、観測値ベクトルｙの各要素が示す値は、ノイズ（例えば、各種センサの測定誤差）が重畳した値であり、時刻の経過と共に変動する値である。従って、観測残差ｚの各要素が示す値も、時刻の経過と共に変動する、ノイズが重畳した値である。
そこで、本実施形態に係る比較部４１０は、観測残差ｚｋ［ｍ］を所定期間モニターし、所定期間における観測残差ｚｋ［ｍ］のノルムの最大値を誤差評価値ＭＡＸｚｋ［ｍ］として採用し、誤差評価値ＭＡＸｚｋ［ｍ］を用いて、状態ベクトルｘｋの示す値と状態ベクトルの真値との誤差の大きさを評価する。これにより、本実施形態に係る比較部４１０は、ノイズの影響を低減させて、状態ベクトルｘｋの示す値と状態ベクトルの真値との誤差の大きさを正確に評価することができる。
【００４３】
選択部４２０は、比較部４１０が出力する選択値Ｓｅｌに基づいて、カルマンフィルタＫＦ［１］〜ＫＦ［４］が出力する状態ベクトルｘｋ［１］〜ｘｋ［４］から、状態ベクトルｘｋ［Ｓｅｌ］を選択し、これを出力情報生成部５００に対して供給する。
【００４４】
出力情報生成部５００は、出力情報制御部４００が出力する状態ベクトルｘｋ［Ｓｅｌ］に基づいて、地磁気Ｂｇの向きや、携帯機器１の姿勢等の出力情報を算出する。
【００４５】
［２．  初期ベクトルの生成］  
上述のとおり、各カルマンフィルタＫＦが実行する非線形カルマンフィルタの演算とは、状態ベクトルｘを、観測残差ｚに基づいて更新する演算である。より具体的には、非線形カルマンフィルタの演算とは、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１と、時刻Ｔ＝ｋにおける観測値ベクトルｙｋとに基づいて算出される観測残差ｚｋを用いて、時刻Ｔ＝ｋにおける状態ベクトルｘｋを推定する演算である。すなわち、時刻Ｔ＝ｋにおける状態ベクトルｘｋは、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１に基づいて算出される。換言すれば、状態ベクトルｘｋは、初期状態（時刻Ｔ＝０）から時刻Ｔ＝ｋ−１に至る全ての状態ベクトルｘ０〜ｘｋ−１に基づいて算出される。
非線形カルマンフィルタの演算において、状態ベクトルｘｋの各要素が、真値から乖離した値に更新された場合、その後、状態ベクトルｘｋの各要素が真値に近い値に収束するまでに長時間を要することがあり、更には、状態ベクトルｘｋの各要素が真値とは異なる不正確な値に収束することがある。この場合、状態推定装置はシステムの状態を正確に推定することはできない。従って、状態推定装置がシステムの状態を正確且つ迅速に推定するためには、状態ベクトルｘｋの初期値（状態ベクトルｘ０）である初期ベクトルＩＮＩの各要素を、真値に近い正確な値に設定することが望ましい。
そこで、本実施形態に係る初期値生成部２００は、初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］のうち少なくとも１つの初期ベクトルＩＮＩの各要素が、真値に近い正確な値となるように、初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］を生成する。これにより、４個のカルマンフィルタＫＦ［１］〜ＫＦ［４］のうち少なくとも１つのカルマンフィルタＫＦにより、システムの状態を正確に推定することが可能となる。
以下では、初期値生成部２００における初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］の生成について詳述する。
【００４６】
［２．１．  姿勢の初期値について］  
まず、初期ベクトルＩＮＩ［ｍ］のうち、携帯機器１の姿勢μ［ｍ］の初期値μ０［ｍ］について検討する。
携帯機器１の姿勢μは、当該携帯機器１の利用者による携帯機器１の所持の仕方に依存して決定される。従って、仮に、姿勢μ［ｍ］の初期値μ０［ｍ］を、事前に定められた１つの固定値（例えば、μ０［ｍ］＝［０，０，０，１］（基準姿勢））に定める場合、初期値μ０［ｍ］は真の姿勢（真値）から最大で１８０度ずれることになる。初期値μ０［ｍ］が真の姿勢（真値）から大きく乖離し、初期ベクトルＩＮＩ［ｍ］がシステムの状態を正確に表さない場合、状態推定装置はシステムの状態を正確に推定することはできない。
そこで、本実施形態では、画像情報Ｉｍｇと位置情報Ｐｓとを用いることで、初期値μ０［１］〜μ０［４］のうち少なくとも１つの初期値μ０が真値に近い正確な値となるように、初期値μ０［１］〜μ０［４］（初期姿勢推定値）を生成する。
以下において、初期値μ０［１］〜μ０［４］の生成方法を具体的に説明する。
【００４７】
まず、画像処理部２１０は、カメラ１１０が物体Ｏｂｊを撮影した際に出力する画像情報Ｉｍｇに基づいて、第１方向ベクトルαを算出する。
ここで、第１方向ベクトルαとは、機器座標系ΣＳにおいて、携帯機器１（厳密には、カメラ１１０）から見た物体Ｏｂｊの方向を表す３次元の単位ベクトルである。以下では、第１方向ベクトルαの算出方法の詳細について説明する。
【００４８】
カメラ１１０は、例えば、１以上のレンズと、当該１以上のレンズの光軸に垂直な平面である受光平面上にマトリックス状（またはハニカム状）に配列された複数のイメージセンサとを備える。カメラ１１０は、物体Ｏｂｊが発した光線（または、物体Ｏｂｊが反射した光線）を、レンズを通過させることで、受光平面上の一部の領域に集光させる。次に、カメラ１１０は、受光平面上の各々の位置の明るさを、複数のイメージセンサを用いて検出する。そして、カメラ１１０は、複数のイメージセンサの出力値に基づいて画像を構成するとともに、当該画像における物体Ｏｂｊの位置（２次元座標）である画像情報Ｉｍｇを生成する。
本実施形態において、画像情報Ｉｍｇの生成は、カメラ１１０にて行われるが、図３に示す画像処理部２１０にて行われてもよい。なお、表示部５０は、物体Ｏｂｊの映された画像を表示することができる（図２参照）。従って、携帯機器１の利用者が操作部１２０介して、画像に映された物体Ｏｂｊを指定することで、画像情報Ｉｍｇを生成してもよい。
【００４９】
ここで、簡単のために凸レンズを例に挙げて、レンズを通過する光の動きについて説明する。
レンズの一般的な性質として、レンズの中心を通過する光は、レンズの通過後もその方向を変えずに直進する。また、レンズの焦点を通過する光は、レンズを通過後に、レンズの光軸と平行な光となる。他方、レンズの光軸に平行な光は、レンズを通過後に、レンズの焦点を通過する。このようなレンズの光学的性質を利用することで、点光源の発する光をレンズにより１点（像点）に集光させた場合の結像位置に基づいて、レンズから見た当該光の入射方向を算出することができる。
より具体的には、理想的な凸レンズにおいて、レンズの中心を通る光の経路を表す直線と受光平面との交点が像点となるため、レンズの中心を像点から表したベクトルと、当該光の入射方向を表すベクトルとは、平行となる。また、画像情報Ｉｍｇは、受光平面上の像点の位置であるので、画像情報Ｉｍｇに基づいて、レンズの中心を像点から表したベクトルを求めることもできる。よって、画像情報Ｉｍｇに基づいて、当該光の入射方向を求めることが可能となる。
また、レンズの収差を考慮する場合、或いは、カメラ１１０が複数のレンズを備える場合であっても、受光平面上の像点の位置と、レンズに入射する光の入射方向とは、１対１に対応する。従って、仮に、レンズの中心を像点から表したベクトルと、当該光の入射方向とを表すベクトルとが、平行とならない場合であっても、ＲＯＭ３０或いはＲＡＭ２０上に、受光平面上の像点の位置（画像情報Ｉｍｇ）と、レンズに入射する光の入射方向とを、対応付けて記憶したルックアップテーブルを設けることにより、画像情報Ｉｍｇに基づいて、レンズの中心から見た当該光の入射方向とを表すベクトルを求めることが可能となる。
なお、画像情報Ｉｍｇは、受光平面上の像点の位置の他に、レンズの中心と受光平面との間の距離を含むものであってもよい。受光平面上の像点の位置の他に、受光平面とレンズとの距離を画像情報Ｉｍｇに含ませることで、受光平面をレンズから遠ざける場合或いは近づける場合であっても、画像情報Ｉｍｇに基づいて、レンズから見た光の入射方向を算出することが可能となる。
このように、物体Ｏｂｊをカメラ１１０で撮影した際に出力される画像情報Ｉｍｇに基づいて、カメラ１１０から見た物体Ｏｂｊの方向を算出することができる。なお、レンズの光軸は、機器座標系ΣＳにおいて一定の方向を向く。従って、画像処理部２１０は、画像情報Ｉｍｇに基づいて、携帯機器１から見た物体Ｏｂｊの方向、すなわち、機器座標系ΣＳにおいて表した第１方向ベクトルαを算出することができる。
【００５０】
図３に示すように、初期姿勢算出部２２０は、撮影対象情報取得部２２１と、姿勢計算部２２２とを備える。撮影対象情報取得部２２１は、ＧＰＳ部６０が出力する位置情報Ｐｓと、地上座標系ΣＧにおける物体Ｏｂｊの座標と、に基づいて第２方向ベクトルβを算出する。姿勢計算部２２２は、第１方向ベクトルα及び第２方向ベクトルβに基づいて、携帯機器１の姿勢μの初期値μ０［１］〜μ０［４］を算出する。
ここで、第２方向ベクトルβとは、地上座標系ΣＧにおいて、携帯機器１から見た物体Ｏｂｊの方向を表す３次元の単位ベクトルである。
【００５１】
なお、本実施形態では、物体Ｏｂｊは、地上座標系ΣＧにおける座標が既知のランドマークである。ここで、ランドマークとは、日本各地（或いは世界各地）に存在する建造物、日本各地（或いは世界各地）に存在する地形等である。なお、ランドマークは、建造物等に備え付けられた光源であってもよい。
ＲＯＭ３０には、物体Ｏｂｊの候補となり得る複数のランドマークの各々についての情報Ｐｏｂｊを記憶したルックアップテーブルＬＵＴ１が格納される。ここで、情報Ｐｏｂｊの具体的な内容は、各ランドマークの位置を特定するための、経度、緯度、高度等の情報である。なお、情報Ｐｏｂｊは、各ランドマークの形状に関する情報を含んでもよい。また、本実施形態は、情報Ｐｏｂｊを、ＲＯＭ３０に格納されたルックアップテーブルＬＵＴ１に記憶するが、情報Ｐｏｂｊを、携帯機器１の外部から通信部４０を介して都度取得し、これをＲＡＭ２０に格納してもよい。
撮影対象情報取得部２２１は、ルックアップテーブルＬＵＴ１を参照して、カメラ１１０が撮影した物体Ｏｂｊについての情報Ｐｏｂｊを取得する。そして、撮影対象情報取得部２２１は、情報Ｐｏｂｊに基づいて、地上座標系ΣＧにおける当該物体Ｏｂｊの座標を算出するとともに、携帯機器１の位置情報Ｐｓに基づいて、地上座標系ΣＧにおける携帯機器１の座標を算出する。その後、撮影対象情報取得部２２１は、地上座標系ΣＧにおける当該物体Ｏｂｊの座標から、地上座標系ΣＧにおける携帯機器１の座標を減算することにより、第２方向ベクトルβを算出する。
なお、本実施形態において、位置情報Ｐｓには、携帯機器１の緯度及び経度が含まれるが、位置情報Ｐｓが、携帯機器１の高度を含むものであってもよい。この場合、第２方向ベクトルβをより正確に算出することが可能となる。
【００５２】
本実施形態では、携帯機器１の利用者が、複数のランドマークから、実際にカメラ１１０が撮影する物体Ｏｂｊを選択する。
但し、本発明はこのような態様に限定されるものではなく、撮影対象情報取得部２２１が、位置情報Ｐｓと情報Ｐｏｂｊとに基づいて、実際にカメラ１１０が撮影した物体Ｏｂｊを特定してもよい。
また、ＣＰＵ１０が状態推定プログラム１００を実行する際、ＣＰＵ１０は、位置情報Ｐｓと情報Ｐｏｂｊとに基づいて、複数のランドマークの中から携帯機器１の利用者が撮影可能な物体Ｏｂｊ（例えば、携帯機器１の近傍に存在するランドマーク）を選択し、当該物体Ｏｂｊを撮影するように促すメッセージを、表示部５０に表示してもよい。この場合、ＣＰＵ１０は、位置情報Ｐｓと情報Ｐｏｂｊとに基づいて、利用者から見た物体Ｏｂｊの方角を算出し、これを表示部５０に表示するものであってもよい。
【００５３】
次に、姿勢計算部２２２は、第１方向ベクトルα及び第２方向ベクトルβに基づいて、携帯機器１の姿勢μの初期値μ０［１］〜μ０［４］を算出する。
以下では、第１方向ベクトルα及び第２方向ベクトルβに基づいて、初期値μ０［１］〜μ０［４］を生成する方法について、具体的に説明する。
【００５４】
段落［００３２］において記したとおり、物体の任意の姿勢は、単位ベクトルρを回転軸として、基準姿勢を角度ψだけ回転した姿勢として表現できる。また、単位ベクトルρを回転軸とし、単位ベクトルρの示す方向から見て角度ψだけ反時計回りに回転させる３行３列の回転行列Ｒ（ρ，ψ）は、以下の式（６）で表される（ロドリゲスの公式）。従って、物体の任意の姿勢は、クォータニオンの代わりに、回転行列Ｒ（ρ，ψ）を用いて表現することもできる。ここで、式（６）に現れる［ρ×］は３行３列の行列であり、単位ベクトルρを式（７）で表した場合、［ρ×］は式（８）により表される。
なお、携帯機器１の姿勢が基準姿勢であるとき、機器座標系ΣＳと地上座標系ΣＧとが一致する一方、携帯機器１の姿勢が回転行列Ｒ（ρ，ψ）で表される場合には、機器座標系ΣＳと地上座標系ΣＧとの関係も、回転行列Ｒ（ρ，ψ）により表現される。具体的には、携帯機器１の姿勢を回転行列Ｒ（ρ，ψ）で表した場合、回転行列Ｒ（ρ，ψ）は、地上座標系ΣＧのｘ軸、ｙ軸、及びｚ軸を機器座標系ΣＳから表した３つの単位ベクトルを、それぞれ、第１列、第２列、第３列に並べた行列となる。
【数６】
2013185898.tif 000008
【００５５】
以下では、携帯機器１の姿勢を回転行列により表現する場合、「姿勢Ａ」等のように符号「Ａ」を用いる。
ここで、式（９）に示すような、単位ベクトルρＣを回転軸として基準姿勢を単位ベクトルρＣの示す方向から見て反時計回りに角度ψＣだけ回転する回転行列Ｒ（ρＣ，ψＣ）によって表現される姿勢を、姿勢ＡＣ（初期姿勢候補値）とする。そして、携帯機器１の姿勢がＡＣのときに、第１方向ベクトルα及び第２方向ベクトルβが算出されたと仮定する。
この場合、単位ベクトルρＣは、第１方向ベクトルα及び第２方向ベクトルβを用いて、式（１０）で表される。また、角度ψＣは、第１方向ベクトルα及び第２方向ベクトルβを用いて、式（１１）で表される。
【数７】
2013185898.tif 000009
【００５６】
上述のとおり、回転行列Ｒ（ρ，ψ）は、機器座標系ΣＳにおいて、地上座標系ΣＧの３軸を表す３つの単位ベクトルを各列に並べた行列である。従って、回転行列Ｒ（ρ，ψ）は、地上座標系ΣＧにおいて表現された任意の３次元ベクトルを、機器座標系ΣＳにおいて表現するための座標変換行列という性質も有する。
よって、以下の式（１２）に示すように、地上座標系ΣＧにおいて表した第２方向ベクトルβを、回転行列Ｒ（ρ，ψ）によって変換することにより、携帯機器１から見た物体Ｏｂｊの方向、すなわち、機器座標系ΣＳにおいて表した第１方向ベクトルαを算出することができる。
【数８】
2013185898.tif 000010
【００５７】
ところで、第１方向ベクトルα及び第２方向ベクトルβが算出された場合であっても、携帯機器１の姿勢を一意に特定することはできない。すなわち、式（９）〜式（１１）により、第１方向ベクトルα及び第２方向ベクトルβに基づいて姿勢ＡＣを算出しても、当該姿勢ＡＣは、携帯機器１の姿勢を正確に表さない可能性が高い。
図４（Ａ）は、携帯機器１と物体Ｏｂｊとの位置関係を表す概念図である。なお、図４（Ａ）では、便宜上、地上座標系ΣＧの原点と、機器座標系ΣＳ（機器座標系ΣＳ１、機器座標系ΣＳ２）の原点とが一致するように、地上座標系ΣＧ及び機器座標系ΣＳが設けられる場合を想定する。
図４（Ａ）に示すように、携帯機器１の姿勢がＡＣ１である場合に、携帯機器１と物体Ｏｂｊとを結ぶ直線ＡＸを軸として、当該機器を任意の角度θだけ回転させた姿勢をＡＣ２とする。このとき、姿勢ＡＣ１の携帯機器１に固定された機器座標系ΣＳ１から見た物体Ｏｂｊの方向と、姿勢ＡＣ２の携帯機器１に固定された機器座標系ΣＳ２から見た物体Ｏｂｊの方向とは、一致する。すなわち、携帯機器１の姿勢がＡＣ１である場合に算出される第１方向ベクトルαと、携帯機器１の姿勢がＡＣ２である場合に算出される第１方向ベクトルαとは、等しい。
また、第２方向ベクトルβは、地上座標系ΣＧにおいて表されるベクトルである。よって、携帯機器１の姿勢が変化しても、携帯機器１の位置（位置情報Ｐｓ）が変化しない限りは、第２方向ベクトルβは一定である。すなわち、携帯機器１の姿勢がＡＣ１である場合に算出される第２方向ベクトルβと、携帯機器１の姿勢がＡＣ２である場合に算出される第２方向ベクトルβとは、等しい。
この場合、仮に、第１方向ベクトルα及び第２方向ベクトルβに基づいて姿勢ＡＣ１が算出された場合であっても、携帯機器１の真の姿勢は姿勢ＡＣ２かもしれない。すなわち、携帯機器１の姿勢がＡＣ１のときに式（１２）が成立すれば、携帯機器１の姿勢がＡＣ２のときにも式（１２）が成立する。
【００５８】
ここで、図４（Ｂ）に示すように、第２方向ベクトルβ（直線ＡＸ）を軸として任意の角度θだけ回転させる回転行列Ｒ（β，θ）を用いて姿勢ＡＣの携帯機器１を回転させることを検討する。
上述のとおり、携帯機器１の姿勢がＡＣのときに式（１２）が成立する場合、姿勢ＡＣの携帯機器１を回転行列Ｒ（β，θ）により回転させた場合にも式（１２）が成立する。すなわち、式（１２）が成立する場合には、以下の式（１３）も同時に成立する。
ここで、式（１４）に示すように、姿勢ＡＣの携帯機器１を回転行列Ｒ（β，θ）により回転させた姿勢を「Ａ」とする。この場合、式（１５）に示すように、姿勢Ａは、第１方向ベクトルα及び第２方向ベクトルβが算出された場合の、携帯機器１の姿勢の一般解を表す。
【数９】
2013185898.tif 000011
【００５９】
本実施形態に係る姿勢計算部２２２は、図４（Ｂ）に示すように、直線ＡＸ（第２方向ベクトルβ）を軸として、姿勢ＡＣを（π／２）ずつ回転させることにより、姿勢Ａ［１］〜Ａ［４］を生成する。具体的には、式（１６）に示すように、角度θを「０」に設定したときの姿勢をＡ［１］とする。姿勢Ａ［１］と、姿勢ＡＣとは等しい。次に、式（１７）〜式（１９）に示すように、角度θを、「π／２」、「π」、「（３π）／２」にそれぞれ設定して、３つの姿勢Ａ［２］、Ａ［３］、Ａ［４］を生成する。
そして、姿勢計算部２２２は、行列で表された姿勢Ａ［１］〜Ａ［４］を、クォータニオンに変換することで、初期値μ０［１］〜μ０［４］を生成する。
【数１０】
2013185898.tif 000012
【００６０】
なお、３行３列の行列で表される姿勢Ａから、クォータニオンで表される姿勢μへの変換は、公知の方法を適宜用いて行えば良い。
ここで、携帯機器１が姿勢μである場合に、地上座標系ΣＧにおいて表現されたベクトルを、機器座標系ΣＳで表現するための座標変換を行う行列Ｂ（μ）は、以下の式（２０）で表すことができる。行列Ａは、行列Ｂ（μ）の逆行列であるので、行列Ａの各成分と行列Ｂ（μ）の逆行列の各成分とを比較することで、クォータニオンを用いて表される姿勢μの各要素を計算してもよい。また、例えば、以下の公知文献に記載された方法を用いて、行列Ａから姿勢μを算出することもできる。
Malcolm D. Shuster and Gregory A. Natanson, “Quaternion Computation from a Geometric Point of View", The Journal of the Astronautical Sciences, Vol. 41, No. 4, October-December 1993, pp. 545-556.
【数１１】
2013185898.tif 000013
【００６１】
以上のように、本実施形態に係る初期姿勢算出部２２０は、画像情報Ｉｍｇと位置情報Ｐｓとを用いて、少なくとも１つが真値に近い値を示す初期値μ０［１］〜μ０［４］を生成することができる。
【００６２】
［２．２．  姿勢の初期値以外の値について］  
地磁気の強さｒ［ｍ］の初期値ｒ０［ｍ］、及び地磁気の伏角φ［ｍ］の初期値φ０［ｍ］は、例えば、ＧＰＳ部６０から供給される位置情報Ｐｓに基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ＲＯＭ３０には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルＬＵＴ２が格納される。初期値生成部２００は、ルックアップテーブルＬＵＴ２を参照して、位置情報Ｐｓに対応する地磁気の強さ及び伏角を、地磁気の強さｒ［ｍ］の初期値ｒ０［ｍ］及び地磁気の伏角φ［ｍ］の初期値φ０［ｍ］として設定する。
【００６３】
角速度ω［ｍ］の初期値ω０［ｍ］は、例えば、携帯機器１が静止していることを仮定して、「０」に設定する。また、３次元角速度センサ９０のオフセット推定値ｇＯＦＦ［ｍ］の初期値ｇＯＦＦ,０［ｍ］は、通常「０」近辺に調整されているため、「０」に設定する。
【００６４】
３次元磁気センサ７０のオフセット推定値ｑＯＦＦ［ｍ］の初期値ｑＯＦＦ,０［ｍ］は、時刻Ｔ＝０に３次元磁気センサ７０から出力される磁気データｑ０、地上座標系ΣＧにおいて地磁気Ｂｇを表したベクトルＧＢｇ［ｍ］、姿勢μ［ｍ］の初期値μ０［ｍ］、及び、式（２０）に示した行列Ｂ（μ）を用いて、以下に示す式（２１）により算出される。なお、時刻Ｔ＝０における地磁気Ｂｇを地上座標系ΣＧから表したベクトルＧＢｇ［ｍ］は、地磁気の強さｒ［ｍ］の初期値ｒ０［ｍ］と、地磁気の伏角φ［ｍ］の初期値φ０［ｍ］とを用いて、以下の式（２２）で表される。
【数１２】
2013185898.tif 000014
【００６５】
このように、初期値生成部２００は、真値に近い値を示すものが少なくとも１個は含まれるように、互いに異なる４個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］を生成する。そして、カルマンフィルタ演算部３００は、４個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］がそれぞれ供給される４個のカルマンフィルタＫＦ［１］〜ＫＦ［４］を並列動作させる。
この場合、時刻Ｔ＝０における状態ベクトルの真値に近い値を示す初期ベクトルＩＮＩが供給されて動作するカルマンフィルタＫＦは、システムの状態を正確に推定することが可能となる。
【００６６】
［３．  非線形カルマンフィルタによる演算について］    
次に、カルマンフィルタＫＦが行う非線形カルマンフィルタの演算について説明する。
【００６７】
一般的にカルマンフィルタは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻（時刻Ｔ＝ｋ−１）のシステムの状態を示す状態ベクトルｘ＋ｋ−１から単位時間が経過した後（時刻Ｔ＝ｋ）の状態ベクトルｘｋを推定する。この推定値を、推定状態ベクトルｘ−ｋと称する。
そして、カルマンフィルタは、各種センサからの出力を要素とする観測値ベクトルｙｋと状態ベクトルｘｋとの関係を表す観測モデルを用いて、推定状態ベクトルｘ−ｋから観測値ベクトルｙｋを推定する。この推定値を、推定観測値ベクトルｙ−ｋと称する。
次に、カルマンフィルタは、推定観測値ベクトルｙ−ｋと、実際の観測値を要素とする観測値ベクトルｙｋとの差分を観測残差ｚｋとして算出し、観測残差ｚｋに基づいてカルマンゲインＫｋを算出する。さらに、カルマンフィルタは、観測残差ｚｋ、カルマンゲインＫｋ、及び、推定状態ベクトルｘ−ｋを用いて、状態ベクトルｘ＋ｋ−１を更新することにより、更新後の状態ベクトルｘ＋ｋを算出する。
以上のような状態ベクトルｘｋの更新を繰り返すカルマンフィルタの演算により、状態ベクトルｘｋの各要素を実際の物理量を正確に表した値（真値）に近い正確な値へと近付ける。
【００６８】
なお、詳細は後述するが、本実施形態ではカルマンフィルタとして、非線形カルマンフィルタであるシグマポイントカルマンフィルタを用いる。
シグマポイントカルマンフィルタは、まず、状態ベクトルｘ＋ｋ−１の周囲に複数のシグマポイントχ＋ｋ−１を設定し、これらの複数のシグマポイントχ＋ｋ−１を状態遷移モデルに適用することで、単位時間経過後の複数のシグマポイントを推定する。この推定された複数のシグマポイントを、推定シグマポイントχ−ｋと称する。次に、シグマポイントカルマンフィルタは、複数の推定シグマポイントχ−ｋの平均を算出することにより、推定状態ベクトルｘ−ｋを求める。
従って、シグマポイントカルマンフィルタを用いてカルマンフィルタの演算を実行する場合、推定観測値ベクトルｙ−ｋは、厳密には、推定状態ベクトルｘ−ｋの周辺に存在する複数の推定シグマポイントχ−ｋに基づいて算出される。
【００６９】
本実施形態における、非線形カルマンフィルタの状態遷移モデルは以下に示す式（２３）で与えられ、観測モデルは以下に示す式（２４）で与えられる。なお、本実施形態では、式（２３）に現れる状態方程式ｆ、および、式（２４）に現れる観測方程式ｈは、非線形の方程式である。
【数１３】
2013185898.tif 000015
【００７０】
以下では、状態ベクトルｘｋの次元を「ｄｉｍ（ｘ）」と表し、観測値ベクトルｙｋの次元を「ｄｉｍ（ｙ）」と表す。本実施形態では、ｄｉｍ（ｘ）＝１５であり、ｄｉｍ（ｙ）＝９である。また、式（２３）に現れるプロセスノイズｗｋ及び式（２４）に現れる観測ノイズｕｋは「０」を中心とするガウスノイズである。
式（２３）は、時刻Ｔ＝ｋにおける状態ベクトルｘｋが、時刻Ｔ＝ｋ−１における状態ベクトルｘｋ−１を状態方程式ｆに代入して得られた値と、時刻Ｔ＝ｋ−１におけるプロセスノイズｗｋ−１とを加算することにより推定されることを示している。
また、式（２４）は、時刻Ｔ＝ｋにおける観測値ベクトルｙｋが、時刻Ｔ＝ｋにおける状態ベクトルｘｋを観測方程式ｈに代入して得られる値と、時刻Ｔ＝ｋにおける観測ノイズｕｋとを加算することにより推定されることを示している。
なお、プロセスノイズｗｋの共分散をＱｋ、観測ノイズｕｋの共分散をＵｋと表す。
【００７１】
時刻Ｔ＝ｋにおける観測残差ｚｋは、実際の観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとに基づいて定められるベクトルであり、以下に示す式（２５）で表される。カルマンフィルタは、以下の式（２７）に示すように、観測残差ｚｋ、推定状態ベクトルｘ−ｋ、及び、式（２６）に示すカルマンゲインＫｋを用いて、更新後の状態ベクトルｘ＋ｋを算出する。また、カルマンフィルタは、以下の式（２８）に示すように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを更新する。
ここで、Ｐ−ｋは、推定状態ベクトルｘ−ｋの推定誤差の共分散であり、Ｐ＋ｋは、更新後の状態ベクトルｘ＋ｋの推定誤差の共分散である。また、Ｐｙｙｋは、観測残差ｚｋの共分散であり、Ｐｘｙｋは、推定状態ベクトルｘ−ｋと、推定観測値ベクトルｙ−ｋとの相互共分散行列である。
【数１４】
2013185898.tif 000016
【００７２】
なお、推定観測値ベクトルｙ−ｋは、推定状態ベクトルｘ−ｋ（厳密には、推定シグマポイントχ−ｋ）を、式（２４）に示す観測モデルに適用することで算出される値である。よって、推定観測値ベクトルｙ−ｋと実際のセンサ出力に基づく観測値ベクトルｙｋとの差分である観測残差ｚｋは、推定状態ベクトルｘ−ｋと実際の物理量を正確に表した値（真値）との近似度を示す値である。
従って、式（２７）に示すように、観測残差ｚｋを用いて、推定状態ベクトルｘ−ｋを、更新後の状態ベクトルｘ＋ｋへと更新することにより、状態ベクトルｘｋの各要素を真値に近い値へと近付けることができる。
【００７３】
本実施形態に係るカルマンフィルタＫＦは、アンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて、非線形カルマンフィルタの演算を実行する。
図５は、カルマンフィルタＫＦ［ｍ］の動作を説明するための機能ブロック図である。図５に示すように、遅延部３１０は、加算器３９０から出力される更新後の状態ベクトルｘ＋ｋを、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋに相当する時間）だけ遅延させることで、状態ベクトルｘ＋ｋ−１を生成し、これを、シグマポイント生成部３２０に対して出力する。なお、初回の演算（時刻Ｔ＝０における演算）では、遅延部３１０は、初期値生成部２００が出力する初期ベクトルＩＮＩ［ｍ］を、状態ベクトルｘ＋ｋ−１に適用する。
【００７４】
次に、シグマポイント生成部３２０は、「ｄｉｍ（ｘ）」行「ｄｉｍ（ｘ）」列の共分散行列Ｐ＋ｋ−１及びＱｋ−１を用いて、「２ｄｉｍ（ｘ）＋１」個のシグマポイントを生成する。
具体的には、まず、複数のシグマポイントの平均に対して、平均のまわりのシグマポイントの広がりを表すスケーリングパラメータλを用いて、式（２９）及び式（３０）に示すベクトルσｋ（１）〜σｋ（２ｄｉｍ（ｘ））を定義する。
【数１５】
2013185898.tif 000017
【００７５】
このとき、シグマポイント生成部３２０は、ベクトルσｋ−１と状態ベクトルｘ＋ｋ−１とに基づいて、式（３１）及び式（３２）で示される「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ＋ｋ−１（０）〜χ＋ｋ−１（２ｄｉｍ（ｘ））を生成する。
【数１６】
2013185898.tif 000018
【００７６】
状態遷移演算部３３０は、式（３３）に示すように、時刻Ｔ＝ｋ−１における「２ｄｉｍ（ｘ）＋１」個のシグマポイントχ＋ｋ−１（０）〜χ＋ｋ−１（２ｄｉｍ（ｘ））の各々を、状態方程式ｆに適用することにより、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））を算出する。
【数１７】
2013185898.tif 000019
【００７７】
次に、推定状態ベクトル算出部３４０は、式（３４）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））の平均値を計算することで、推定状態ベクトルｘ−ｋを算出する。
【数１８】
2013185898.tif 000020
【００７８】
また、推定状態ベクトル算出部３４０は、式（３５）に示すように、推定状態ベクトルｘ−ｋの推定誤差の共分散Ｐ−ｋを算出する。ここで、ベクトルξｊは、以下の式（３６）に示す、推定シグマポイントχ−ｋ（ｊ）と推定状態ベクトルｘ−ｋとの差分を表すベクトルである。
【数１９】
2013185898.tif 000021
【００７９】
このように、シグマポイント生成部３２０、状態遷移演算部３３０、及び、推定状態ベクトル算出部３４０は、状態ベクトルｘ＋ｋ−１を状態遷移モデルに適用することで推定状態ベクトルｘ−ｋを算出する、状態遷移モデル部７１０として機能する。
【００８０】
一方、推定観測値算出部３５０は、式（３７）に示すように、時刻Ｔ＝ｋにおける「２ｄｉｍ（ｘ）＋１」個の推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））の各々を、観測方程式ｈに適用することにより、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（０）〜γｋ（２ｄｉｍ（ｘ））を算出する。
【数２０】
2013185898.tif 000022
【００８１】
そして、推定観測値ベクトル算出部３６０は、式（３８）に示すように、「２ｄｉｍ（ｘ）＋１」個の推定観測値γｋ（０）〜γｋ（２ｄｉｍ（ｘ））の平均を演算することにより、推定観測値ベクトルｙ−ｋを算出する。
【数２１】
2013185898.tif 000023
【００８２】
また、推定観測値ベクトル算出部３６０は、式（３９）に示すように、観測残差の共分散Ｐｙｙｋを算出する。ここで、ベクトルζｊは、以下の式（４０）に示すように、推定観測値γｋ（ｊ）と推定観測値ベクトルｙ−ｋとの差分を表すベクトルである。
【数２２】
2013185898.tif 000024
【００８３】
このように、推定観測値算出部３５０、及び、推定観測値ベクトル算出部３６０は、推定シグマポイントχ−ｋ（０）〜χ−ｋ（２ｄｉｍ（ｘ））の各々を観測モデルに適用することで推定観測値ベクトルｙ−ｋを算出する観測モデル部７２０として機能する。
【００８４】
減算器３７０は、式（２５）に示したように、観測値ベクトルｙｋと、推定観測値ベクトルｙ−ｋとの差分として、観測残差ｚｋを算出する。
カルマンゲイン付与部３８０は、式（４１）に示すように、相互共分散行列Ｐｘｙｋを算出する。そして、カルマンゲイン付与部３８０は、式（２６）に示したように、観測残差の共分散Ｐｙｙｋと、相互共分散行列Ｐｘｙｋとに基づいて、カルマンゲインＫｋを算出し、式（２７）の右辺第２項（Ｋｋｚｋ）の演算を実行する。
【数２３】
2013185898.tif 000025
【００８５】
加算器３９０は、式（２７）に示したように、推定状態ベクトルｘ−ｋと、カルマンゲイン付与部３８０から出力される式（２７）の右辺第２項（Ｋｋｚｋ）とを加算することにより、更新後の状態ベクトルｘ＋ｋを算出する。また、加算器３９０は、式（２８）に示したように、状態ベクトルｘｋの推定誤差の共分散Ｐｋを、Ｐ−ｋからＰ＋ｋに更新する。
【００８６】
このように、減算器３７０、カルマンゲイン付与部３８０、及び、加算器３９０は、観測残差ｚｋ、カルマンゲインＫｋ、及び、推定状態ベクトルｘ−ｋを用いて、状態ベクトルｘ＋ｋ−１を更新した更新後の状態ベクトルｘ＋ｋを算出する更新部７３０として機能する。
【００８７】
次に、状態遷移モデル部７１０において用いられる状態遷移モデルのうち、状態遷移演算部３３０における演算で用いられる状態方程式ｆについて説明する。
【００８８】
まず、状態方程式ｆのうち、時刻Ｔ＝ｋ−１における姿勢μ＋ｋ−１から、単位時間経過後の時刻Ｔ＝ｋにおける姿勢μ−ｋを推定する演算は、以下に示す式（４２）として表される。ここで、μ−ｋは、時刻ｋにおける推定状態ベクトルｘ−ｋのうち、姿勢μを表す状態変数に対応する要素である。
なお、式（４２）の右辺の演算子Ωは、式（４３）により定義される。ここで、Ｉ３×３は３行３列の単位行列を表す。３次元ベクトルｌ＝（ｌ１，ｌ２，ｌ３）に対して、演算子［ｌ×］は、式（４４）で定義される。また、単位時間（時刻Ｔ＝ｋ−１から時刻Ｔ＝ｋまでの測定時間間隔）をΔｔで表し、時刻Ｔ＝ｋにおける更新後の状態ベクトルｘ＋ｋのうち角速度を表す状態変数に対応する要素をω＋ｋで表したとき、演算子Ωを構成する成分Ψ＋ｋは、式（４５）で定義される。
【数２４】
2013185898.tif 000026
【００８９】
姿勢μは、クォータニオンで表現され、式（２）に示すように、正規化条件||μ||＝１を満たす必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルｘｋを更新する場合、更新後の状態ベクトルｘ＋ｋは、式（３４）に示すように、推定シグマポイントχ−ｋ（ｊ）の平均として算出される推定状態ベクトルｘ−ｋに基づいて定められるため、（更新前の）状態ベクトルｘ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、（更新前の）状態ベクトルｘ＋ｋ−１の要素である姿勢μ＋ｋ−１のノルムと、更新後の状態ベクトルｘ＋ｋの要素である姿勢μ＋ｋのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢μについての正規化条件が満たされなくなる可能性が存在する。
そこで、姿勢μに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。
なお、より厳密に正規化条件を保つためには、状態ベクトルｘｋを構成する要素のうち姿勢μｋについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
【００９０】
地磁気の強さｒ及び地磁気の伏角φは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける地磁気の強さｒｋ及び伏角φｋと、時刻Ｔ＝ｋ−１における地磁気の強さｒｋ−１及び伏角φｋ−１とは各々等しい値であると仮定する。
同様に、３次元角速度センサ９０のオフセット推定値ｇＯＦＦは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける３次元角速度センサ９０のオフセット推定値ｇＯＦＦ,Ｋと、時刻Ｔ＝ｋ−１における３次元角速度センサ９０のオフセット推定値ｇＯＦＦ,Ｋ−１とは等しい値であると仮定する。
【００９１】
携帯機器１の角速度ωは、携帯機器１の利用者による携帯機器１の動かし方に依存して変化するため、時刻Ｔ＝ｋ−１の角速度ωｋ−１を用いて、時刻Ｔ＝ｋにおける角速度ωｋを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける角速度ωｋと、時刻Ｔ＝ｋ−１における角速度ωｋ−１とは等しいと仮定する。
【００９２】
前述のとおり、３次元磁気センサ７０のオフセットは、携帯機器１の部品が発する内部磁界Ｂｉの方向及び大きさを機器座標系ΣＳにおいて表現したベクトルである。従って、携帯機器１の内部状態が一定である間は、３次元磁気センサ７０のオフセットも変化しない。一方、携帯機器１の内部状態が変化した場合、例えば、携帯機器１に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器１に搭載された部品の着磁状況が変化した場合には、３次元磁気センサ７０のオフセットも変化する。すなわち、携帯機器１の内部状態は、携帯機器１の利用者による携帯機器１の操作や、携帯機器１の外部の環境等に依存して変化する。従って、３次元磁気センサ７０のオフセットが変化するタイミングや３次元磁気センサ７０のオフセットの変化量を予測することは困難であり、時刻Ｔ＝ｋ−１における３次元磁気センサ７０のオフセット推定値ｑＯＦＦ,Ｋ−１を用いて、時刻Ｔ＝ｋにおける３次元磁気センサ７０のオフセット推定値ｑＯＦＦ,Ｋを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻Ｔ＝ｋにおける３次元磁気センサ７０のオフセット推定値ｑＯＦＦ,Ｋと、時刻Ｔ＝ｋ−１における３次元磁気センサ７０のオフセット推定値ｑＯＦＦ,Ｋ−１とは等しいと仮定する。
【００９３】
このように、本実施形態に係る状態遷移モデルにおける状態方程式ｆは、以下に示す式（４６）のように、状態ベクトルｘｋを構成する複数の状態変数のうち、姿勢μｋを表す状態変数以外は、前の時刻から変化しないことを表す方程式として、定式化される。
【数２５】
2013185898.tif 000027
【００９４】
このように、状態ベクトルｘｋを、状態方程式ｆに適用した場合、状態ベクトルｘｋのうち、姿勢μｋ以外の各要素の示す値は変化しない。
しかし、状態ベクトルｘｋのうち姿勢μｋ以外の各要素は、加算器３９０において、観測残差ｚｋに基づいて真値に近づくように更新される。つまり、式（２７）に示すように、状態ベクトルｘｋの各要素は、状態遷移モデルにより算出される推定状態ベクトルｘ−ｋと、複数のセンサからの出力値（観測値ベクトルｙｋ）を用いて算出される観測残差ｚｋとの双方に基づいて、真値に近づくように更新される。
【００９５】
次に、観測モデル部７２０において用いられる観測モデルのうち、推定観測値算出部３５０における演算で用いられる観測方程式ｈについて説明する。
【００９６】
３次元角速度センサ９０から出力される角速度データｇの推定値γｇｙｒｏは、角速度ωと、角速度センサのオフセット推定値ｇＯＦＦとを用いて、式（４７）で与えられる。
【数２６】
2013185898.tif 000028
【００９７】
また、地上座標系ΣＧにおいて地磁気Ｂｇを表すベクトルＧＢｇは式（４８）で与えられる。従って、３次元磁気センサ７０から出力される磁気データｑの推定値γｍａｇは、磁気センサのオフセット推定値ｑＯＦＦと、式（２０）で示した行列Ｂ（μ）とを用いて、式（４９）で与えられる。
【数２７】
2013185898.tif 000029
【００９８】
また、観測モデルにおいて、３次元加速度センサ８０から出力される加速度データａの推定値γａｃｃは、式（２０）で示した行列Ｂ（μ）と、ベクトルＧＧＲＶとを用いて、式（５０）で与えられる。なお、ベクトルＧＧＲＶは、以下の式（５１）に示すように、地上座標系ΣＧにおいて重力加速度を表したベクトルを、重力加速度の大きさで正規化した３次元のベクトルである。
【数２８】
2013185898.tif 000030
【００９９】
このように、本実施形態に係る観測モデルにおける観測方程式ｈは、式（４７）、式（４９）、及び式（５０）により定式化される。
そして、式（３）を式（２５）の右辺第１項に代入し、式（４７）、式（４９）、及び式（５０）を用いて式（２５）の右辺第２項を表すことにより、式（２５）を、以下の式（５２）に変形することができる。このとき、観測残差ｚｋは、式（５２）により算出される。
【数２９】
2013185898.tif 000031
【０１００】
［４．  結論］
初期ベクトルＩＮＩの各要素を、事前に定めた所定の値とした場合、姿勢μの初期値μ０は、姿勢μの初期値μ０以外の要素（例えば、地磁気の強さｒの初期値ｒ０や、地磁気の伏角φの初期値φ０等）に比べて、真値から大きく乖離した値となる可能性が高い。例えば、上述のとおり、初期値μ０を、事前に定められた１つの固定値に定める場合、初期値μ０は真の姿勢（真値）から最大で１８０度ずれることになる。この場合、状態推定装置は、システムの状態を正しく推定することができない。
これに対して、本実施形態に係る状態推定装置は、画像情報Ｉｍｇと位置情報Ｐｓとを用いて、姿勢μの初期値μ０［１］〜μ０［４］を生成するため、初期値μ０［１］〜μ０［４］のうち少なくとも１つの初期値μ０［Ｓｅｌ］を真値に近い値とすることが可能となる。
真値に近い初期値μ０［Ｓｅｌ］が供給されるカルマンフィルタＫＦ［Ｓｅｌ］は、システムの状態を正確に推定することができる。すなわち、カルマンフィルタＫＦ［Ｓｅｌ］が出力する状態ベクトルｘｋ［Ｓｅｌ］の各要素は、真値に近い正確な値となる。そして、出力情報制御部４００は、カルマンフィルタＫＦ［１］〜ＫＦ［４］が出力する状態ベクトルｘｋ［１］〜ｘｋ［４］から、状態ベクトルｘｋ［Ｓｅｌ］を選択し、これを出力情報生成部５００に供給する。この場合、出力情報生成部５００は、地磁気Ｂｇ、携帯機器１の姿勢等を正確に表す出力情報を生成することができる。
このように、本実施形態に係る状態推定装置は、システムの状態を正確に推定することができる。
【０１０１】
＜Ｂ．変形例＞                
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの２以上の変形例は、相互に矛盾しない範囲内で適宜に組み合わせることもできる。
【０１０２】
（１）変形例１  
上述した実施形態において、物体Ｏｂｊは、地上座標系ΣＧにおける座標が既知のランドマークであったが、本発明はこのような態様に限定されるものではなく、物体Ｏｂｊは、太陽、月、星等の天体であってもよい。
変形例１に係る状態推定装置は、初期値生成部２００の代わりに初期値生成部２００ａを備える点を除き、上述した実施形態に係る状態推定装置と同様に構成される。図６は、初期値生成部２００ａの機能ブロック図である。初期値生成部２００ａは、初期姿勢算出部２２０の代わりに、初期姿勢算出部２２０ａを備える点を除き、初期値生成部２００と同様に構成される。初期姿勢算出部２２０ａは、撮影対象情報取得部２２１の代わりに、撮影対象情報取得部２２１ａを備える点を除き、初期姿勢算出部２２０と同様に構成される。
変形例１に係る携帯機器が備えるＲＯＭには、物体Ｏｂｊの候補となり得る複数の天体の各々についての情報Ｐｏｂｊ２を記憶したルックアップテーブルが格納される。ここで、情報Ｐｏｂｊ２の具体的な内容は、各天体の軌道に関する情報（例えば、地球上の位置及び時刻と、当該位置及び時刻において各天体が見える方向とに関する情報）である。撮影対象情報取得部２２１ａは、携帯機器の位置情報Ｐｓ、物体Ｏｂｊの情報Ｐｏｂｊ２、及び、時刻情報Ｔｉｍｅに基づいて、第２方向ベクトルβを生成する。初期姿勢算出部２２０ａは、初期姿勢算出部２２０と同様に、少なくとも１つが真値に近い値を示す姿勢μの４つの初期値μ０［１］〜μ０［４］を生成することができる。従って、変形例１に係る状態推定装置は、システムの状態を正確に推定することができる。
【０１０３】
（２）変形例２  
上述した実施形態において、携帯機器１は時計１３０を備えるが、本発明に係る携帯機器はこのような構成に限定されるものではなく、携帯機器１は時計１３０を備えないものであってもよい。
【０１０４】
（３）変形例３  
上述した実施形態及び変形例に係る状態推定装置は、４個の初期ベクトルＩＮＩ［１］〜ＩＮＩ［４］に基づいて動作する４個のカルマンフィルタＫＦ［１］〜ＫＦ［４］を備えるものであったが、本発明はこのような形態に限定されるものではなく、所定数Ｍの初期ベクトルＩＮＩ［１］〜ＩＮＩ［Ｍ］に基づいて並列に動作する所定数ＭのカルマンフィルタＫＦ［１］〜ＫＦ［Ｍ］を備えるものであってもよい（Ｍは、１≦Ｍを満たす自然数）。
この場合、変形例３に係る初期値生成部２００は、所定数Ｍの初期ベクトルＩＮＩ［１］〜ＩＮＩ［Ｍ］を生成するものであればよい。具体的には、変形例３に係る初期値生成部２００は、第１方向ベクトルα及び第２方向ベクトルβに基づいて姿勢ＡＣ（姿勢Ａ［１］）を算出し、直線ＡＸ（第２方向ベクトルβ）を軸として当該姿勢ＡＣを所定角度（２π／Ｍ）ずつ回転させることで、姿勢Ａ［２］〜Ａ［Ｍ］を算出し、所定数Ｍの姿勢Ａ［１］〜Ａ［Ｍ］から、姿勢μの初期値μ０［１］〜μ０［Ｍ］を生成する初期姿勢算出部２２０と、所定数Ｍの初期値μ０［１］〜μ０［Ｍ］に基づいて、所定数Ｍの初期ベクトルＩＮＩ［１］〜ＩＮＩ［Ｍ］を生成する初期ベクトル生成部２３０と、を備えるものであればよい。
また、この場合、出力情報制御部４００は、所定数ＭのカルマンフィルタＫＦ［１］〜ＫＦ［Ｍ］の中から、推定精度が最も高いカルマンフィルタを選択するものであればよい。
所定数Ｍが、「４」よりも大きい場合、変形例３に係る状態推定装置は、上述した実施形態に係る状態推定装置に比べて、より正確且つ迅速に、システムの状態を推定することが可能となる。また、所定数Ｍが、「４」よりも小さい場合、変形例３に係る状態推定装置は、上述した実施形態に係る状態推定装置に比べて、非線形カルマンフィルタの演算における処理負荷を小さく抑えることが可能となるため、状態推定装置が組み込まれる携帯機器の低消費電力化が可能となる。
【０１０５】
（４）変形例４  
上述した実施形態及び変形例に係る状態推定装置は、カルマンフィルタＫＦの推定精度を、誤差評価値ＭＡＸｚｋ［ｍ］を用いて評価するものであったが、本発明はこのような態様に限定されるものではなく、状態ベクトルｘｋの示す値と状態ベクトルの真値との誤差の大きさを評価することのできる値であれば、いかなる値を用いてカルマンフィルタＫＦの推定精度を評価してもよい。例えば、状態ベクトルｘｋの推定誤差の共分散Ｐｋを用いて、カルマンフィルタＫＦの推定精度を評価するものであってもよい。
【０１０６】
（５）変形例５  
上述した実施形態及び変形例に係る状態推定装置は、カメラ１１０が１つの物体Ｏｂｊを撮影した際に出力される画像情報Ｉｍｇに基づいて複数の初期ベクトルＩＮＩ［１］〜ＩＮＩ［Ｍ］を生成し、これらが各々供給される複数のカルマンフィルタＫＦ［１］〜ＫＦ［Ｍ］を並列に動作させるものであったが、本発明はこのような態様に限定されるものではなく、カメラ１１０が２つの物体Ｏｂｊ（物体Ｏｂｊ１、物体Ｏｂｊ２）を撮影した際に出力される画像情報Ｉｍｇに基づいて１つの初期ベクトルＩＮＩ［１］を生成し、当該初期ベクトルＩＮＩ［１］が供給されるカルマンフィルタＫＦ［１］を単独で動作させるものであってもよい。
２つの物体Ｏｂｊを撮影して画像情報Ｉｍｇが生成される場合、当該画像情報Ｉｍｇに基づいて、２つの物体Ｏｂｊのそれぞれに対応する２つの第１方向ベクトルαと、２つの物体Ｏｂｊのそれぞれに対応する２つの第２方向ベクトルβと、が生成される。この場合、２つの第１方向ベクトルαと、２つの第２方向ベクトルβとに基づいて、携帯機器１の姿勢μの初期値μ０（姿勢Ａ）を一意に特定することができるため、真値に近い値を有する正確な初期ベクトルＩＮＩ［１］を、カルマンフィルタＫＦ［１］に供給することができる。
【０１０７】
図７は、変形例５に係る状態推定装置のうち、ＣＰＵ１０が変形例５に係る状態推定プログラムを実行することにより実現される機能を表した、機能ブロック図である。
図７に示すように、変形例５に係る状態推定装置は、初期値生成部２００の代わりに初期値生成部２００ｂを備える点、カルマンフィルタ演算部３００の代わりにカルマンフィルタ演算部３００ｂを備える点、及び、出力情報制御部４００を備えない点を除き、実施形態に係る状態推定装置と同様に構成される。
初期値生成部２００ｂは、画像処理部２１０ｂ、初期姿勢算出部２２０ｂ、及び、初期ベクトル生成部２３０ｂを備える。画像処理部２１０ｂは、カメラ１１０が２つの物体Ｏｂｊ（物体Ｏｂｊ１、物体Ｏｂｊ２）を撮影した際に出力される画像情報Ｉｍｇに基づいて、機器座標系ΣＳにおいて、携帯機器１から見た物体Ｏｂｊ１の方向を表す第１方向ベクトルα１と、携帯機器１から見た物体Ｏｂｊ２の方向を表す第１方向ベクトルα２とを算出する。第１方向ベクトルα１及び第１方向ベクトルα２は、長さが「１」に正規化されたベクトルである。
初期姿勢算出部２２０ｂは、撮影対象情報取得部２２１ｂと、姿勢計算部２２２ｂとを備える。撮影対象情報取得部２２１ｂは、カメラ１１０が撮影した２つの物体Ｏｂｊに係る情報Ｐｏｂｊ（または情報Ｐｏｂｊ２）と位置情報Ｐｓとに基づいて、地上座標系ΣＧにおいて、携帯機器１から見た物体Ｏｂｊ１の方向を表す第２方向ベクトルβ１と、携帯機器１から見た物体Ｏｂｊ２の方向を表す第２方向ベクトルβ２とを算出する。第２方向ベクトルβ１及び第２方向ベクトルβ２は、長さが「１」に正規化されたベクトルである。
カメラ１１０が２つの物体Ｏｂｊを撮影した際の、携帯機器１の姿勢をＡとしたとき、式（５３）及び式（５４）が成立する。式（５３）及び式（５４）は、式（５５）に変形できる。そして、式（５３）〜式（５５）は、式（５６）に変形できる。なお、式（５６）において、行列Ｄαは、式（５７）に示す３行３列の行列であり、行列Ｄβは、式（５８）に示す３行３列の行列である。従って、姿勢Ａは、式（５９）により、一意に算出することができる。姿勢計算部２２２ｂは、２つの第１方向ベクトルα１、α２と、２つの第２方向ベクトルβ１、β２とを用いて、式（５９）に基づいて、姿勢Ａ（携帯機器１の姿勢μの初期値μ０［１］）を算出する。初期ベクトル生成部２３０ｂは、初期値μ０［１］に基づいて、初期ベクトルＩＮＩ［１］を算出する。
カルマンフィルタ演算部３００ｂは、１個のカルマンフィルタＫＦ［１］を備え、カルマンフィルタＫＦ［１］には、初期ベクトルＩＮＩ［１］が供給される。カルマンフィルタＫＦ［１］は、周期的に状態ベクトルｘｋ［１］（厳密には、更新後の状態ベクトルｘ＋ｋ［１］）を出力する。そして、出力情報生成部５００は、状態ベクトルｘｋ［１］に基づいて、出力情報を生成する。
上述のとおり、初期ベクトルＩＮＩ［１］は、状態ベクトルｘ０［１］の真値に近い正確な値であるため、カルマンフィルタＫＦ［１］は、システムの状態を正確且つ迅速に推定することができる。
【数３０】
2013185898.tif 000032
【０１０８】
なお、ＣＰＵ１０は、カメラ１１０が１つの物体Ｏｂｊを撮影した場合、実施形態（または変形例１〜４）に係る状態推定プログラム１００を実行し、カメラ１１０が２つの物体Ｏｂｊ（物体Ｏｂｊ１、物体Ｏｂｊ２）を撮影した場合、変形例５に係る状態推定プログラムを実行してもよい。
【０１０９】
（６）変形例６  
上述した実施形態及び変形例に係る状態推定装置は、シグマポイントカルマンフィルタを用いて非線形カルマンフィルタの演算を実行するものであったが、本発明はこのような形態に限定されるものでは無く、拡張カルマンフィルタ等、公知の非線形カルマンフィルタを適宜適用して演算を行うものでもよい。
図８は、変形例６に係る状態推定プログラムを実行したときに実現される機能を表した機能ブロック図である。図８に示すように、変形例６に係る状態推定装置は、カルマンフィルタＫＦ［ｍ］の代わりにカルマンフィルタＫＦａ［ｍ］を備える点を除き、上述した実施形態及び変形例に係る状態推定装置と同様に構成される。カルマンフィルタＫＦａ［ｍ］は、状態遷移モデル部７１０を備える代わりに、状態遷移モデル部７１０ａを備える点と、観測モデル部７２０を備える代わりに、観測モデル部７２０ａを備える点とを除いて、カルマンフィルタＫＦ［ｍ］と同様に構成される。
状態遷移モデル部７１０ａは、状態ベクトルｘ＋ｋ−１を、式（２３）に示す状態遷移モデルに適用することで、推定状態ベクトルｘ−ｋを算出する。観測モデル部７２０ａは、推定状態ベクトルｘ−ｋを、式（２４）に示す観測モデルに適用することにより、推定観測値ベクトルｙ−ｋを算出する。
このように、変形例６に係る非線形カルマンフィルタの演算では、複数のシグマポイントを生成することなく、状態ベクトルｘｋを更新する。
【符号の説明】
【０１１０】
１…携帯機器、１０…ＣＰＵ、６０…ＧＰＳ部、７０…３次元磁気センサ、８０…３次元加速度センサ、９０…３次元角速度センサ、１００…状態推定プログラム、１１０…カメラ、２００…初期値生成部、２１０…画像処理部、２２０…初期姿勢算出部、２２１…撮影対象情報取得部、２２２…姿勢計算部、２３０…初期ベクトル生成部、３００…カルマンフィルタ演算部、４００…出力情報制御部、５００…出力情報生成部、ｑ…磁気データ、Ｉｍｇ…画像情報、Ｐｓ…位置情報、Ｐｏｂｊ…情報、μ…姿勢、μ０…姿勢μの初期値、ｘ…状態ベクトル、ｙ…観測値ベクトル、ｚ…観測残差、α…第１方向ベクトル、β…第２方向ベクトル、Ｏｂｊ…物体、ΣＳ…機器座標系、ΣＧ…地上座標系。
【図１】
2013185898.tif 000033
【図２】
2013185898.tif 000034
【図３】
2013185898.tif 000035
【図４】
2013185898.tif 000036
【図５】
2013185898.tif 000037
【図６】
2013185898.tif 000038
【図７】
2013185898.tif 000039
【図８】
2013185898.tif 000040