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