JP2012132713A - 状態推定装置、状態推定方法および状態推定プログラム - Google Patents
状態推定装置、状態推定方法および状態推定プログラム Download PDFInfo
- Publication number
- JP2012132713A JP2012132713A JP2010283243A JP2010283243A JP2012132713A JP 2012132713 A JP2012132713 A JP 2012132713A JP 2010283243 A JP2010283243 A JP 2010283243A JP 2010283243 A JP2010283243 A JP 2010283243A JP 2012132713 A JP2012132713 A JP 2012132713A
- Authority
- JP
- Japan
- Prior art keywords
- state
- observation
- state vector
- kalman filter
- estimated
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Navigation (AREA)
Abstract
【課題】高速に状態ベクトルを収束させる。
【解決手段】状態推定装置は、非線形カルマンフィルタKF1〜KF10と、各フィルタの観測残差ekのノルムの最大値を検出する観測残差ピークモニタPM1〜PM10と、互いに異なる初期姿勢を示す初期値INI1〜INI10を生成する初期値生成部130Aを備える。初期値INI1〜INI10が与えられると、非線形カルマンフィルタKF1〜KF10は並列に動作する。そして、比較部110は、観測残差ekのノルムの最大値を比較し、最も小さいノルムの最大値に対応する非線形カルマンフィルタの出力を選択するように選択部120を制御する。
【選択図】図3
【解決手段】状態推定装置は、非線形カルマンフィルタKF1〜KF10と、各フィルタの観測残差ekのノルムの最大値を検出する観測残差ピークモニタPM1〜PM10と、互いに異なる初期姿勢を示す初期値INI1〜INI10を生成する初期値生成部130Aを備える。初期値INI1〜INI10が与えられると、非線形カルマンフィルタKF1〜KF10は並列に動作する。そして、比較部110は、観測残差ekのノルムの最大値を比較し、最も小さいノルムの最大値に対応する非線形カルマンフィルタの出力を選択するように選択部120を制御する。
【選択図】図3
Description
本発明は、状態推定装置、状態推定方法および状態推定プログラムに関する。
物体の姿勢を正確に知るために異種の物理量を測定するセンサの出力を統合する方法として、拡張カルマンフィルタ(EKF)などの非線形カルマンフィルタを用いることが知られている。例えば,特許文献1には、3軸の角速度センサ及び3軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。
また、3軸角速度センサと3軸加速度センサに加え、3軸地磁気センサの出力信号を拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタ(SPKF)を用いて統合して姿勢を推定する方法が非特許文献1に開示されている。
一般に、カルマンフィルタは、誤差を含む観測値を用いて、ある動的システムの状態を推定するために用いられる。例えば、3軸角速度センサ、3軸加速度センサ、及び3軸地磁気センサの出力値が誤差を含む観測値であり、推定すべき状態が姿勢である。
さらに、特許文献2には、複数のカルマンフィルタに異なる初期値を与えて並列に動作させ、発散状態を検出し、発散していないカルマンフィルタのうち、観測残差が最も少ないものを用いて状態の推定を実行する技術が開示されている。
また、3軸角速度センサと3軸加速度センサに加え、3軸地磁気センサの出力信号を拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタ(SPKF)を用いて統合して姿勢を推定する方法が非特許文献1に開示されている。
一般に、カルマンフィルタは、誤差を含む観測値を用いて、ある動的システムの状態を推定するために用いられる。例えば、3軸角速度センサ、3軸加速度センサ、及び3軸地磁気センサの出力値が誤差を含む観測値であり、推定すべき状態が姿勢である。
さらに、特許文献2には、複数のカルマンフィルタに異なる初期値を与えて並列に動作させ、発散状態を検出し、発散していないカルマンフィルタのうち、観測残差が最も少ないものを用いて状態の推定を実行する技術が開示されている。
Wolfgang Gunthner, "Enhancing Cognitive Assistance Systems with Inertial Measurement Units", Springer, 2008
ところで、観測残差にはノイズが重畳しており、また、状態ベクトルが初期値から収束するまでの過程は様々な態様を取り得るので、ある時刻で選択したカルマンフィルタが、次の時刻において最も観測残差が小さくなるとは限らない。従って、従来の技術のように、観測残差が最小になるカルマンフィルタを常に選択すると、不適切なものを選択してしまう可能性があるといった問題がある。
さらに、従来の技術では、単に異なる複数の初期値を設定するだけであり、どのような初期値を採用すれば状態ベクトルを短時間で収束させることができるかについては検討されていない。
くわえて、初期値の数と並列に動作させるカルマンフィルタの数とを増加させれば、状態ベクトルを収束させる時間を短縮することができるが、処理負荷が増加してしまう。すなわち、状態ベクトルが収束までの時間と処理負荷とはトレードオフの関係にあるといった問題があった。
さらに、従来の技術では、単に異なる複数の初期値を設定するだけであり、どのような初期値を採用すれば状態ベクトルを短時間で収束させることができるかについては検討されていない。
くわえて、初期値の数と並列に動作させるカルマンフィルタの数とを増加させれば、状態ベクトルを収束させる時間を短縮することができるが、処理負荷が増加してしまう。すなわち、状態ベクトルが収束までの時間と処理負荷とはトレードオフの関係にあるといった問題があった。
本発明は、上述した点に鑑みてなされたものであり、複数のカルマンフィルタを並列に動作させる場合に、適切にカルマンフィルタを選択すること、状態ベクトルを収束させる時間を短縮することなどを解決課題とする。
上記課題を解決するため、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行する複数のカルマンフィルタと、前記状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、前記複数のカルマンフィルタの各々に供給する初期値生成部と、前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差に基づいて、前記状態ベクトルが最も早く収束するカルマンフィルタを特定し、特定したカルマンフィルタの状態ベクトルを出力する出力部とを備え、前記複数のカルマンフィルタは、並列に動作して前記観測残差を同時に算出し、前記出力部は、現在時刻から所定時間だけ過去の時刻までの所定期間における前記少なくとも1つのセンサに係る観測残差の最大値を前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値のうち最も小さい前記最大値に対応するカルマンフィルタを、前記状態ベクトルが最も早く収束するカルマンフィルタとして特定する、ことを特徴とする。
この発明によれば、状態ベクトルの初期値を複数設定し、互いに異なる複数の初期値を並列に動作する複数のカルマンフィルタに供給する。初期値が真値から乖離していると、状態ベクトルが収束するまでに長時間を必要とする。この発明では、異なる初期値を各カルマンフィルタに供給して動作させるので、その中には短時間で状態ベクトルが収束するものがある。出力部は観測残差に基づいて状態ベクトルが最も早く収束するカルマンフィルタを特定するので、状態ベクトルを高速に収束させることが可能となる。
また、観測残差が小さいことは、状態ベクトルの推定誤差が小さいことを意味する。しかし、瞬時的な観測残差に着目すると、ノイズなどの影響によって不適切なものを選択する可能性がある。この発明は、観測残差の瞬時値ではなく、所定期間における観測残差の最大値が小さくなるようにカルマンフィルタを採用するので、安定して状態ベクトルを高速に収束させることが可能なカルマンフィルタを選択することが可能となる。
上述した状態推定装置において、前記初期値生成部は、前記複数の初期値を記憶する記憶部を備え、前記記憶部から前記複数の初期値を読み出して、前記複数のカルマンフィルタの各々に供給してもよい。状態べクトルを高速に収束させるためには、真値に近い初期値を設定する必要がある。しかしながら、初期値がどのような値を取るかが全く予測できない場合もあり得る。そのような場合には、代表的な初期値を予め用意しておくことが好ましい。この発明によれば、初期値を設定する手掛かりが何もない場合にも、状態ベクトルを高速に収束させることができる。
より具体的には、前記システムの状態ベクトルは、物体の姿勢を示す状態変数を含み、前記複数の初期値は、前記物体の基準姿勢と、互いに等しい角度で交わる3以上の軸の各々を中心として、等しい角度だけ前記基準姿勢を回転させた複数の初期姿勢であることが好ましい。例えば、3以上の軸は、X軸、Y軸、及びZ軸である。これらの軸は互いに直交している。そして、各軸の周りに基準姿勢を90度、180度、270度、回転させて軸ごとに3つの初期姿勢を定めてもよい。このように基準姿勢と複数の初期姿勢とを定めると、初期値によって定まる姿勢を空間上で均等に分散させて配置することができる。この結果、初期状態において物体がどのような姿勢であっても、状態ベクトルを高速で収束させることが可能となる。
また、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行する複数のカルマンフィルタと、前記状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、前記複数のカルマンフィルタの各々に供給する初期値生成部と、前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差に基づいて、前記状態ベクトルが最も早く収束するカルマンフィルタを特定し、特定したカルマンフィルタの状態ベクトルを出力する出力部とを備え、前記複数のカルマンフィルタは、並列に動作して前記観測残差を同時に算出し、前記初期値生成部は、前記複数のセンサの一部又は全部から出力される前記観測値に基づいて、前記複数の初期値を生成することを特徴とする。
この発明によれば、システムを観測することによって得られた観測値を用いて複数の初期値を生成するので、複数の初期値を真値に近づけることができる。すなわち、観測値によって、初期値として取り得る範囲を限定し、限定された範囲の中で複数の初期値を配置すればよい。このため、より高速に状態ベクトルを収束させることが可能となる。あるいは、初期値とカルマンフィルタの数を減らすことができ、構成を簡素化することができる。
ここで、前記出力部は、現在時刻から所定時間だけ過去の時刻までの所定期間における前記少なくとも1つのセンサに係る観測残差の最大値を前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値のうち最も小さい前記最大値に対応するカルマンフィルタを、前記状態ベクトルが最も早く収束するカルマンフィルタとして特定することが好ましい。この発明によれば、安定して状態ベクトルを高速に収束させることが可能なカルマンフィルタを選択することが可能となる。
上述した状態推定装置において、前記システムの状態ベクトルは、物体の姿勢を示す状態変数を含み、前記センサは、前記物体の加速度を直交する3軸で検出する加速度センサを備え、前記初期値生成部は、前記加速度センサの前記観測値に基づいて、前記物体の水平面に対する傾きと一致し、水平面に対して垂直な軸を中心として等しい角度だけ前記物体を回転した複数の姿勢を各々示すように前記複数の初期値を生成することが好ましい。物体を固定しても重力加速度が作用するので、3軸の加速センサの出力から重力加速度が物体に対してどの向きに働いているかを特定できる。その向きから、物体の水平面に対する傾きが特定できる。但し、物体が東西南北のどの向きを向いているかは特定することができない。そこで、水平面に対する傾きを保ち、水平面に対して垂直な軸を中心として等しい角度だけ回転した複数の姿勢を初期姿勢とする。これによって、状態ベクトルを高速に収束させることが可能となる。
また、上述した状態推定装置の具体的な態様として、前記複数のセンサは、前記システムを直交する3軸で観測する所定のセンサを含み、前記観測残差は、前記所定のセンサの観測値に対応する特定要素を含む複数の要素からなり、前記出力部は、前記所定期間における前記観測残差の前記特定要素の最大値を前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値のうち最も小さい前記最大値に対応するカルマンフィルタを、前記状態ベクトルが最も早く収束するカルマンフィルタとして特定することが好ましい。
この発明によれば、観測残差の一部に着目してカルマンフィルタを選択することができるので、処理を簡素化することができる。
この発明によれば、観測残差の一部に着目してカルマンフィルタを選択することができるので、処理を簡素化することができる。
また、本発明は、方法の発明として捉えることができる。この方法は、システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定方法であって、前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行し、現在時刻から所定時間だけ過去の時刻までの所定期間における前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出し、複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定し、特定したカルマンフィルタ処理の状態ベクトルを出力する、ことを特徴とする。
さらに、本発明は、プログラムの発明として捉えることもできる。このプログラムは、システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定装置に用いられる状態推定プログラムであって、前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成する処理と、状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行する処理と、現在時刻から所定時間だけ過去の時刻までの所定期間における前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出する処理と、複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定する処理と、特定したカルマンフィルタ処理の状態ベクトルを出力する処理とを、コンピュータに実行させる。
また、本発明に係る状態推定方法は、システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する方法であって、前記複数のセンサの一部又は全部から出力される前記観測値に基づいて、前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行し、前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出し、複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定し、特定したカルマンフィルタ処理の状態ベクトルを出力する、ことを特徴とする。
また、本発明に係る状態推定プログラムは、システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定装置に用いられるものであって、前記複数のセンサの一部又は全部から出力される前記観測値に基づいて、前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成する処理と、状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行する処理と、前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出する処理と、複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定する処理と、特定したカルマンフィルタ処理の状態ベクトルを出力する処理とを、コンピュータに実行させる。
なお、上述したプログラムは、装置のメモリに予め記憶されてもよいし、あるいは、インターネットなどの通信網を介してダウンロードすることによって更新されてもよい。
本発明の実施の形態について図面を参照して説明する。
<1.第1実施形態>
図1は、本発明の第1実施形態に係る携帯電話機のブロック図であり、図2は携帯電話機の外観を示す斜視図である。携帯電話機1は、携帯電話機1の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいて携帯電話機1の姿勢をカルマンフィルタを用いた演算により算出することによって実現される。
<1.第1実施形態>
図1は、本発明の第1実施形態に係る携帯電話機のブロック図であり、図2は携帯電話機の外観を示す斜視図である。携帯電話機1は、携帯電話機1の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいて携帯電話機1の姿勢をカルマンフィルタを用いた演算により算出することによって実現される。
携帯電話機1は、各種の構成要素とバスを介して接続され装置全体を制御するCPU10、CPU10の作業領域として機能するRAM20、各種のプログラムやデータを記憶したROM30、通信を実行する通信部40、画像を表示する表示部50、及びGPS部60を備える。GPS部60は、GPS衛星からの信号を受信して携帯電話機1の位置情報(緯度、経度)を生成するものである。
地磁気を検出して地磁気データを出力する地磁気検出部70、加速度を検出して加速度データを出力する加速度検出部80、角速度を検出して角速度データを出力する角速度検出部90を備える。
地磁気を検出して地磁気データを出力する地磁気検出部70、加速度を検出して加速度データを出力する加速度検出部80、角速度を検出して角速度データを出力する角速度検出部90を備える。
地磁気検出部70は、X軸地磁気センサ71、Y軸地磁気センサ72、及びZ軸地磁気センサ73を備える。各センサは、MI素子(磁気インピーダンス素子)、MR素子(磁気抵抗効果素子)などを用いて構成することができる。地磁気センサI/F74は、各センサからの出力信号をAD変換して地磁気データを出力する。この地磁気データは、x軸、y軸およびz軸の3成分によって磁極北に向かう方向を持つベクトルを示す。なお、地磁気検出部70は、携帯電話機1に設けられるので、携帯電話機1の内部の部品(スピーカなど)が出す磁気と、携帯電話機1の外部物が出す磁気と、実際の地磁気とが合成された磁界を検出する。しかしながら、後述する姿勢推定プログラムで想定している状態遷移モデルでは、外部物が出す磁気は無視できる程小さいものとする。
加速度検出部80は、X軸加速度センサ81、Y軸加速度センサ82、及びZ軸加速度センサ83を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサI/F84は、各センサからの出力信号をAD変換して加速度データを出力する。この加速度データは、加速度検出部80と一体となって運動する物体固定の系における慣性力と重力との合力をx軸、y軸及びz軸の3成分を有するベクトルを示す。したがって、携帯電話機1が静止状態または等速直線運動状態にあれば、加速度データは画面に対して固定されたxyz座標空間において重力加速度の大きさと方向とを示すベクトルデータである。
角速度検出部90は、X軸角速度センサ91、Y軸角速度センサ92、及びZ軸角速度センサ93を備える。角速度センサI/F94は、各センサからの出力信号をAD変換して角速度データを出力する。角速度データは、各軸の回りの角速度を示す。
CPU10は、ROM30に格納されている姿勢推定プログラム(状態推定プログラム)を実行することによって、携帯電話機1の姿勢を特定する。この姿勢推定プログラムでは並列化された非線形カルマンフィルタの演算を実行している。本実施形態における非線形カルマンフィルタでは、姿勢を表すクォータニオンq、地磁気の強さr、地磁気の伏角θ、角速度ω、角速度センサのオフセット推定値go、地磁気センサのオフセット推定値moを状態変数とし、これらを推定の対象とする。時刻kにおいて状態変数を要素とする状態ベクトルxkは、以下に示す式(1)で与えられる。
クォータニオンqは、物体の姿勢(回転状態)を表す数学的表現であって、4次元数で与えられる。また、状態ベクトルxkの要素として、地磁気センサのオフセット推定値mo及び角速度センサのオフセット推定値goを含ませたのは、地磁気データにはセンサ71〜73のオフセットが重畳し、角速度データにはセンサ91〜93のオフセットが重畳するからである。例えば、携帯電話機1には磁石を備えたスピーカが内蔵されているため、地磁気センサは磁石の磁力を常時検出してしまう。あるいは、温度変化によってもオフセットが変動する。正確に姿勢を検知するためには、オフセット推定値moにより補正された地磁気データ及びオフセット推定値goにより補正された角速度データを用いて、状態ベクトルxkを算出する必要がある。なお、オフセット推定値mo及びオフセット推定値goによる補正はCPU10が実行する。
次に、カルマンフィルタにおいて観測の対象となるのは、3軸の地磁気センサ71〜73の出力である観測値m、3軸の加速度センサ81〜83の出力である観測値a、及び3軸の角速度センサ91〜93の出力である観測値gである。観測値m、a、gは各々X軸、Y軸及びZ軸の3つの要素からなるベクトルである。時刻kにおけるカルマンフィルタの出力ベクトルykは式(2)で与えられる。
この携帯電話機1は、CPU10が姿勢推定プログラムを実行することによって姿勢推定装置として機能する。図3に、プログラムの実行によって得られる姿勢推定装置の機能ブロック図を示す。
姿勢推定装置は、10個の非線形カルマンフィルタKF1〜KF10及び観測残差ピークモニタPM1〜PM10、比較部110、選択部120、並びに初期値生成部130Aを備える。このうち、観測残差ピークモニタPM1〜PM10、比較部110、及び選択部120は、非線形カルマンフィルタKF1〜KF10の各々の観測残差に基づいて、状態ベクトルが最も早く収束する非線形カルマンフィルタを特定し、特定した非線形カルマンフィルタの状態ベクトルを出力する出力部として機能する。
姿勢推定装置は、10個の非線形カルマンフィルタKF1〜KF10及び観測残差ピークモニタPM1〜PM10、比較部110、選択部120、並びに初期値生成部130Aを備える。このうち、観測残差ピークモニタPM1〜PM10、比較部110、及び選択部120は、非線形カルマンフィルタKF1〜KF10の各々の観測残差に基づいて、状態ベクトルが最も早く収束する非線形カルマンフィルタを特定し、特定した非線形カルマンフィルタの状態ベクトルを出力する出力部として機能する。
非線形カルマンフィルタKF1〜KF10の各々は、携帯電話機1の姿勢に関するシステムの状態遷移モデルを用いて、現在時刻のシステムの状態を示す状態変数から単位時間が経過した後の状態変数を推定し、その状態変数の推定値に基づいて観測値を推定し、推定した観測値と各種センサの観測値との差分を観測残差ek+1として算出する。非線形カルマンフィルタKF1〜KF10の構成は同じであり、与えられる初期値INI1〜INI10のみが相違する。
観測残差ピークモニタPM1〜PM10は、観測残差ekを監視し、現在時刻から所定時間だけ過去の所定期間における観測残差ekのノルムの最大値を出力する。
比較部110は、観測残差ピークモニタPM1〜PM10の出力信号を比較し、所定期間の観測残差ekのノルムの最大値のうち最小のものを特定し、観測残差ekのノルムが最小となる非線形カルマンフィルタKFを特定する制御信号CTLを生成する。
比較部110は、観測残差ピークモニタPM1〜PM10の出力信号を比較し、所定期間の観測残差ekのノルムの最大値のうち最小のものを特定し、観測残差ekのノルムが最小となる非線形カルマンフィルタKFを特定する制御信号CTLを生成する。
より具体的には、時刻kにおけるi番目のフィルタの観測残差ekのうちの地磁気センサに対応する3成分をzi,kとする。観測残差ekの最も小さい非線形カルマンフィルタが適切な姿勢を推定していると考えられるが、ある瞬間の観測残差ekだけに着目すると不適切なものが採用されてしまうことがある。そこで、時刻(k-τ0)から時刻kまでの間の観測残差ekのノルムの最大値を地磁気について着目し、式(3)に従って算出する。この演算は、観測残差ピークモニタPM1〜PM10で実行される。比較部110は、この指標が最小となるi番目のフィルタを選択するように制御信号CTLを生成する。
選択部120は、制御信号CTLに従って、非線形カルマンフィルタKF1〜KF10から出力される状態ベクトルの一つを選択して出力する。この状態ベクトルは、上述した式(1)で示したように姿勢を表すクォータニオンq、地磁気の強さr、地磁気の伏角θ、角速度ω、角速度センサのオフセット推定値go、地磁気センサのオフセット推定値moを状態変数とする。
初期値を適切に選ぶことが非線形カルマンフィルタを用いた姿勢推定装置では重要である。初期値生成部130Aは、地磁気の強さr及び伏角θの初期値INIを、GPS部60から供給される位置情報に基づいて生成する。これは、地球上の位置が特定できれば、その位置における地磁気の強さr及び伏角θを知ることができるからである。具体的には、ROM30に位置情報と地磁気の強さr及び伏角θとを対応づけて記憶したルックアップテーブルLUT1を格納しておき、これを参照して位置情報に対応する地磁気の強さr及び伏角θを読み出している。なお、携帯電話機1が衛星からの電波の届かない場所(例えば、地下街)にある場合には、GPS部60から位置情報が得られない。そのような場合には、携帯電話機1が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルLUT1に格納されている。初期値生成部130Aは、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルLUT1から読み出す。例えば、日本で販売された携帯電話機1については、明石市の地磁気の強さr及び伏角θを初期INIとして用いればよい。
角速度ωの初期値については静止を仮定して0とすればよい。角速度センサのオフセット推定値g。は、通常、ゼロ近辺に調整されているので0とすればよいが。あるいは、最初の測定値が静止状態で得られたものであると仮定し、その値を初期値INIとして与えてもよい。
重要となるのは地磁気センサのオフセット推定値m0と姿勢qである。地磁気センサは地磁気ベクトルの向きと大きさを知るために用いられる。しかし、携帯電話機1のスピーカなどの周辺部品が大きな磁界を発生させている。この場合には、地磁気の大きさよりはるかに大きなオフセットとなるのが通常である。従って、いかなる場合にも同じ値にすることは避け、なるべく真値に近い値を与えることが望ましい。
姿勢qに関しても、ある一つの姿勢を仮定すると最悪の場合には180度ずれることになる。そのように初期値と実際の初期姿勢とのずれが大きい場合には、状態ベクトルが正しく収束する場合であってもその速度は極めて遅く、更には状態変数が正しい値に収束しないこともある。
そこで、本実施形態においては、10個の非線形カルマンフィルタKF1〜KF10に異なる初期値INI1〜INI10を与え並列して動作させる。
姿勢qに関しても、ある一つの姿勢を仮定すると最悪の場合には180度ずれることになる。そのように初期値と実際の初期姿勢とのずれが大きい場合には、状態ベクトルが正しく収束する場合であってもその速度は極めて遅く、更には状態変数が正しい値に収束しないこともある。
そこで、本実施形態においては、10個の非線形カルマンフィルタKF1〜KF10に異なる初期値INI1〜INI10を与え並列して動作させる。
地面に固定された座標系での地磁気ベクトルと重力ベクトルがそれぞれme、geであるとする。物体が静止していると仮定したとき,センサ固定の座標系で磁気センサと加速度センサの出力がそれぞれme、geとなる姿勢を基準姿勢とし、その基準姿勢を表すクォータニオンをq=(0,0, 0,1)と定めることとする。任意の姿勢は基準姿勢に対して単位ベクトルαの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢を表すクォータニオンqは以下に示す式(4)で与えられる。
初期姿勢を表すクォータニオンq0として以下の10通りを用意する。図4乃至図6に、理解を容易にするため、ティーポットの図形で初期姿勢を例示する。なお、携帯電話機1でも同様である。
図4に示す1)q0=(0,0, 0,1)は基準姿勢(初期姿勢の一部)であり、ティーポットの注ぎ口がX軸方向に伸びている。また、2)〜4)はZ軸でティーポットを回転させたものである。2)q0=(0,0, 1/21/2, 1/21/2)はZ軸上で基準姿勢を90度回転させた姿勢である。2)は式(4)に、α=(0,0, 1)及び、ψ=90を代入すれば得られる。他の姿勢3)〜10)についても同様である。3)q0=(0,0, 1,0)は基準姿勢をZ軸上で180度回転させた姿勢である。4)q0=(0,0,− 1/21/2, 1/21/2)はZ軸上で基準姿勢を−90度回転させた姿勢である。
また、図5に示す5)q0=(0,1/21/2, 0, 1/21/2)はY軸上で基準姿勢を90度回転させた姿勢である。6)q0=(0,1,0,0)は基準姿をY軸上で180度回転させた姿勢である。7)q0=(0,− 1/21/2,0, 1/21/2)はY軸上で基準姿勢を−90度回転させた姿勢である。
また、図6に示す8)q0=(1/21/2,0,0, 1/21/2)はX軸上で基準姿勢を90度回転させた姿勢である。9)q0=(1,0,0,0)は基準姿勢をX軸上で180度回転させた姿勢である。10)q0=(− 1/21/2,0,0, 1/21/2)はX軸上で基準姿勢を−90度回転させた姿勢である。
図4に示す1)q0=(0,0, 0,1)は基準姿勢(初期姿勢の一部)であり、ティーポットの注ぎ口がX軸方向に伸びている。また、2)〜4)はZ軸でティーポットを回転させたものである。2)q0=(0,0, 1/21/2, 1/21/2)はZ軸上で基準姿勢を90度回転させた姿勢である。2)は式(4)に、α=(0,0, 1)及び、ψ=90を代入すれば得られる。他の姿勢3)〜10)についても同様である。3)q0=(0,0, 1,0)は基準姿勢をZ軸上で180度回転させた姿勢である。4)q0=(0,0,− 1/21/2, 1/21/2)はZ軸上で基準姿勢を−90度回転させた姿勢である。
また、図5に示す5)q0=(0,1/21/2, 0, 1/21/2)はY軸上で基準姿勢を90度回転させた姿勢である。6)q0=(0,1,0,0)は基準姿をY軸上で180度回転させた姿勢である。7)q0=(0,− 1/21/2,0, 1/21/2)はY軸上で基準姿勢を−90度回転させた姿勢である。
また、図6に示す8)q0=(1/21/2,0,0, 1/21/2)はX軸上で基準姿勢を90度回転させた姿勢である。9)q0=(1,0,0,0)は基準姿勢をX軸上で180度回転させた姿勢である。10)q0=(− 1/21/2,0,0, 1/21/2)はX軸上で基準姿勢を−90度回転させた姿勢である。
姿勢がqの場合、地磁気センサの出力は、B(q)me+mbとなる。上述したようにmeは、地面に固定された座標系での地磁気ベクトルである。一方、携帯電話機1に設けられた地磁気センサは、携帯電話機1の姿勢が変われば、XYZ軸の各々で検出される地磁気の強さも変化する。B(q)は地面に固定された座標系を姿勢qを用いて携帯電話機1の地磁気センサの座標系に変換するために用いられる行列であり、以下に示す式(5)で与えられる。
姿勢を変更してもスピーカと地磁気センサとの相対的な位置に変化はないので、スピーカの磁石が発生する磁界は、姿勢と無関係に地磁気センサで検出される。これが、推定して求めるべき地磁気センサの真のオフセットmbに相当する。
地磁気センサのオフセットmbの推定値の初期値mo,kは,時刻k=0の地磁気センサの観測値m0と、携帯電話機1を使用する地域の地磁気ベクトルmtypicalを用いて、以下に示す式(6)で与えられる。
地磁気センサのオフセットmbの推定値の初期値mo,kは,時刻k=0の地磁気センサの観測値m0と、携帯電話機1を使用する地域の地磁気ベクトルmtypicalを用いて、以下に示す式(6)で与えられる。
式(6)において、B(q0)mtypicalの項は、地磁気ベクトルmtypicalを地磁気センサの座標軸に変換した地磁気成分である。地磁気センサの観測値m0は地磁気成分とオフセットが加算されたものになっている。このため、観測値m0から地磁気成分B(q0)mtypicalを差し引いてオフセットmbの推定値の初期値mo,0を算出している。なお、ROM30には、位置情報と地磁気ベクトルmtypicalとを対応づけて記憶したルックアップテーブルLUT2が記憶されている。初期値生成部130Aは、GPS部60で生成される位置情報に基づいてルックアップテーブルLUT2を参照して地磁気ベクトルmtypicalを取得し、式(6)を演算することによって、オフセットmbの推定値の初期値mo,0を得る。
このように、初期値生成部130Aは、10通りの初期姿勢を生成し、各々の初期姿勢に応じた初期値INI1〜INI10を生成し、非線形カルマンフィルタKF1〜KF10に供給する。実際の姿勢と近い初期姿勢に基づいて生成された初期値INIが供給された非線形カルマンフィルタKFは、状態ベクトルが短い時間で収束する。状態ベクトルが収束したか否かは、観測残差ek+1により知ることができる。
本実施形態では、観測残差ek+1の最大値を比較部110で比較することによって、非線形カルマンフィルタKF1〜KF10のうち、最も収束している非線形カルマンフィルタKFを選択する。この結果、短時間で正確な姿勢を算出することが可能となる。
本実施形態では、観測残差ek+1の最大値を比較部110で比較することによって、非線形カルマンフィルタKF1〜KF10のうち、最も収束している非線形カルマンフィルタKFを選択する。この結果、短時間で正確な姿勢を算出することが可能となる。
次に、非線形カルマンフィルタKF1〜KF10の内容について説明する。図7は、非線形カルマンフィルタKF1の機能ブロック図である。なお、他の非線形カルマンフィルタKF2〜KF10も同様の構成である。
この非線形カルマンフィルタKF1は、シグマポイントカルマンフィルタによって構成されており、そのシステムのモデルは、以下に示す式(7)及び式(8)で与えられる。
この非線形カルマンフィルタKF1は、シグマポイントカルマンフィルタによって構成されており、そのシステムのモデルは、以下に示す式(7)及び式(8)で与えられる。
ここでxkはn次元の状態ベクトルであり、ykはm次元の観測値ベクトルである。式(1)で示したように状態ベクトルxkは、姿勢qkや地磁気センサのオフセット推定値mo,kを状態変数として含む。プロセスノイズuと観測ノイズυは0を中心とするガウスノイズである。式(7)は、時刻k+1における状態ベクトルxk+1は、時刻kにおける状態ベクトルxkを関数fに代入し、その結果と時刻kにおけるプロセスノイズukとを加算したものであることを示している。また、式(8)は、時刻kにおける観測値ベクトルykは時刻kにおける状態ベクトルxkを関数hに代入し、その結果と時刻kにおける観測ノイズυkとを加算したものであることを示している。なお、以下の説明では、プロセスノイズukの共分散をQk、観測ノイズの共分散をRkであるとする。
観測残差ekは、時刻kの観測値ベクトルykと観測モデルを用いて推定されたベクトルとの差分となるため、以下に示す式(9)で表すことができる。また、非線形カルマンフィルタの更新は観測残差ekを用いて、以下に示す式(10)及び式(11)で与えられる。
式(9)の減算は図7に示す減算器260で実行される。また、式(10)の右辺第2項の演算は、カルマンゲイン付与部270において実行され、式(10)の加算は加算器280において実行される。遅延部290は、加算器280の出力を次の時刻まで遅延させる。
また、カルマンゲインは以下に示す式(12)で与えられる。
また、カルマンゲインは以下に示す式(12)で与えられる。
シグマポイントカルマンフィルタでは、n行n列の共分散行列を用いて最初に(2n+1)個のシグマポイントをシグマポイント生成部210で生成する。平均のまわりのシグマボイントの広がりを表すスケーリングパラメータλを用いて表し、式 (13)及び式(14)を定義する。
さらにシグマポイントは、式(15)及び式(16)で決定される。式(13)乃至式(16)の演算は、シグマポイント生成部210によって実行される。なお、シグマポイント生成部210には、初期値INI1が供給される。シグマポイントカルマンフィルタの初回の演算では、初期値INI1を式(15)の右辺に用いてシグマポイント生成部210はシグマポイントを生成する。
次に、状態遷移モデル部220の演算で用いる状態遷移モデルについて説明する。本実施形態における姿勢推定モデルでは、状態べクトルを構成する状態変数のうち、姿勢q以外は前の時刻から変化しないと仮定した。このため、以下に示す式(23)が成立する。但し、実際にはカルマンゲインと観測残差によって、次の状態ベクトルが補正されるので、値が全く変化しないわけではなく、真値に近づいてゆく。
状態ベクトルのうち姿勢を表すクォータニオンqは、正規化条件??q??=1が満たされていないといけないが、シグマポイントの平均を求めるとその条件が満たされなくなってしまう。クォータニオンqに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化するようにすればそのような問題は解決される。
より厳密に正規化条件を保つためには、状態ベクトルのうち姿勢についてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。これにより、システム全体として姿勢を推定する装置として機能する。
より厳密に正規化条件を保つためには、状態ベクトルのうち姿勢についてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。これにより、システム全体として姿勢を推定する装置として機能する。
このように本実施形態によれば、状態ベクトルxkの状態変数のうち、初期状態を予測不能な状態変数(この例では、姿勢のクォータニオンq)については、互いに異なる値を予め設定したので、状態ベクトルが収束するまでの時間を大幅に短縮することができる。
特に、姿勢のクォータニオンqの設定においては、基準姿勢を定め、この基準姿勢を直交する3軸の各々において等しい角度だけ回転させて(例えば、90度)、初期姿勢を定めた。すなわち、空間で均等に分散させた姿勢を初期姿勢として採用したので、携帯電話機1がどのような姿勢であっても、状態ベクトルを高速に収束させることができる。
特に、姿勢のクォータニオンqの設定においては、基準姿勢を定め、この基準姿勢を直交する3軸の各々において等しい角度だけ回転させて(例えば、90度)、初期姿勢を定めた。すなわち、空間で均等に分散させた姿勢を初期姿勢として採用したので、携帯電話機1がどのような姿勢であっても、状態ベクトルを高速に収束させることができる。
<2.第2実施形態>
上述した第1実施形態では、初期状態の姿勢が予測不能であることを前提とした。これに対して、第2実施形態に係る携帯電話機1では、姿勢の初期状態を観測値の一部からある程度予測して、初期姿勢を設定する点で相違する。
第2実施形態の携帯電話機1は、CPU10が実行する姿勢推定プログラムの内容が第1実施形態と相違する。図8にプログラムの実行によって得られる姿勢推定装置の機能ブロック図を示す。図8の機能ブロックが、図3に示す第1実施形態の機能ブロックと相違するのは、非線形カルマンフィルタKF1〜KF10の替わりに非線形カルマンフィルタKF1〜KF4を用いる点及び初期値生成部130Aの替わりに初期値生成部130Bを用いる点である。
上述した第1実施形態では、初期状態の姿勢が予測不能であることを前提とした。これに対して、第2実施形態に係る携帯電話機1では、姿勢の初期状態を観測値の一部からある程度予測して、初期姿勢を設定する点で相違する。
第2実施形態の携帯電話機1は、CPU10が実行する姿勢推定プログラムの内容が第1実施形態と相違する。図8にプログラムの実行によって得られる姿勢推定装置の機能ブロック図を示す。図8の機能ブロックが、図3に示す第1実施形態の機能ブロックと相違するのは、非線形カルマンフィルタKF1〜KF10の替わりに非線形カルマンフィルタKF1〜KF4を用いる点及び初期値生成部130Aの替わりに初期値生成部130Bを用いる点である。
初期状態(時刻k=0)において、携帯電話機1が振動や加速していないと仮定すると、加速度センサは重力加速度のみを検出することになる。初期値生成部130Bは、その値を用いて状態ベクトルの初期値の候補を決定する。加速度センサの観測値a0=(a0,1, a0,2, a0,3)を用いて以下のように定める。図9に示すように式(34)の角度φは重力加速度ベクトルのXY平面との角度を表しており、重力加速度ベクトルがz軸下向きのときφ=0である。式(35)のβは重力加速度ベクトルのXY平面上での大きさを示している。
このようにして決定したpaは姿勢を表すクォータニオンとみなしたとき、傾きだけは正確である。どちらに傾いているかは不確実なため、同じ傾きであるが傾いている向きが異なる複数の初期値を設定する。具体的には鉛直軸を中心に対称な4つの初期姿勢を設けることが好ましい。
4つの初期姿勢を特定するためには、まず、式(38)〜式(41)で定まる4つのクォータニオンp0、p1、p2、p3を想定する。
p0=(0, 0, 0,1) ……式(38)
p1=(0, 0,1/21/2 , 1/21/2) ……式(39)
p2=(0, 0, 1,0) ……式(40)
p3=(0, 0, −1/21/2 , 1/21/2)……式(41)
4つの初期姿勢を特定するためには、まず、式(38)〜式(41)で定まる4つのクォータニオンp0、p1、p2、p3を想定する。
p0=(0, 0, 0,1) ……式(38)
p1=(0, 0,1/21/2 , 1/21/2) ……式(39)
p2=(0, 0, 1,0) ……式(40)
p3=(0, 0, −1/21/2 , 1/21/2)……式(41)
次に、(×)をクォータニオン積としたとき、pa(×) p0、pa(×) p1、pa(×) p2、pa(×) p3を算出し、これらを初期姿勢とする。この演算は、初期値生成部130Bで実行し、初期値INI1〜INI4を生成する。すなわち、初期値生成部130Bは、加速度センサの観測値に基づいて、携帯電話機1の水平面に対する傾きと一致し、水平面に対して垂直な軸を中心として等しい角度だけ回転した複数の姿勢を各々示すように複数の初期値INI1〜INI4を生成する。
このように、第2実施形態では、状態ベクトルの初期値を設定する場合に、観測値を得るためのセンサの出力を用いて初期値の範囲を限定し、限定した範囲の中で複数の初期値を設定した。このため、並列で動作させる非線形カルマンフィルタの個数を減らすことができ、CPU10に処理負荷を低減することができる。また、限定された範囲で、第1実施形態と同様に10個の初期値を用いる場合には、より高速に状態ベクトルを収束させることが可能となる。
<3.変形例>
本発明は上述した実施形態に限定されるものではなく、例えば、以下に述べる各種の変形が可能である。
(1)上述した第1及び第2実施形態では、非線形カルマンフィルタの一種であるシグマポイントカルマンフィルタを一例として説明したが、本発明はこれに限定されるのではなく、拡張カルマンフィルタなど、どのようなカルマンフィルタに適用してもよい。拡張カルマンフィルタに適用した場合であっても、状態ベクトルを短時間で収束させることが可能である。
本発明は上述した実施形態に限定されるものではなく、例えば、以下に述べる各種の変形が可能である。
(1)上述した第1及び第2実施形態では、非線形カルマンフィルタの一種であるシグマポイントカルマンフィルタを一例として説明したが、本発明はこれに限定されるのではなく、拡張カルマンフィルタなど、どのようなカルマンフィルタに適用してもよい。拡張カルマンフィルタに適用した場合であっても、状態ベクトルを短時間で収束させることが可能である。
(2)上述した第1及び第2実施形態では、姿勢推定にカルマンフィルタを適用したものであったが、本発明において推定の対象は姿勢に限定されるのではなく、どのようなものを推定の対象としてもよい。要は、カルマンフィルタを用いて状態ベクトルを推定する全ての装置に適用可能である。
(3)上述した第1及び第2実施形態では、初期値として設定する状態ベクトルは複数の状態変数を含み、その一部について(姿勢)互いに異なる値を設定することによって、互いに異なる状態べクトルの初期値を定めたが、本発明はこれに限定されるものではない。すなわち、初期値として設定する状態ベクトルの状態変数の全てについて、互いに異なる値を設定してもよい。
(4)上述した第1実施形態では、物体のある姿勢を基準姿勢とし、直交するXYZ軸の各々を中心として90度ずつ基準姿勢を回転させた姿勢を初期姿勢としたが本発明はこれに限定されるものではなく、互いに等しい角度で交わる3以上の軸の各々を中心として、等しい角度だけ基準姿勢を回転させた姿勢を初期姿勢とすればよい。要は、複数の初期値に対応する複数の初期姿勢は、空間上で均等に分散して配置されていることが望ましい。
(5)上述した第1及び第2実施形態では、観測残差ekの一部である地磁気センサの3軸の成分に基づいて、どの非線形カルマンフィルタKFを選択するかを決定したが、本発明はこれに限定されるものではない。複数種類のセンサを備え、複数の観測対象について観測値を生成する場合、観測残差ekは複数の観測対象の要素から構成されるが、その一部に基づいてどの非線形カルマンフィルタKFを選択するかを決定してもよいし、あるいは、その全部に基づいてどの非線形カルマンフィルタKFを選択するかを決定してもよい。
(6)上述した第2実施形態では、観測値の一部である加速度センサの出力に基づいて、複数の初期値を生成したが、本発明はこれに限定されるものではなく、観測値の全部を用いて複数の初期値を生成してもよい。
KF1〜KF10…非線形カルマンフィルタ、110…比較部、120…選択部、130A,130B…初期値生成部、PM1〜PM10…観測残差モニタ、40A,40B…選択信号生成回路、INI1〜INI10…初期値、210…シグマポイント生成部、220…状態遷移モデル部、230…平均算出部、240…観測モデル部、250…平均処理部、270…カルマンゲイン付与部。
Claims (11)
- システムを観測して観測値を各々出力する複数のセンサと、
状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行する複数のカルマンフィルタと、
前記状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、前記複数のカルマンフィルタの各々に供給する初期値生成部と、
前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差に基づいて、前記状態ベクトルが最も早く収束するカルマンフィルタを特定し、特定したカルマンフィルタの状態ベクトルを出力する出力部とを備え、
前記複数のカルマンフィルタは、並列に動作して前記観測残差を同時に算出し、
前記出力部は、現在時刻から所定時間だけ過去の時刻までの所定期間における前記少なくとも1つのセンサに係る観測残差の最大値を前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値のうち最も小さい前記最大値に対応するカルマンフィルタを、前記状態ベクトルが最も早く収束するカルマンフィルタとして特定する、
ことを特徴とする状態推定装置。 - 前記初期値生成部は、前記複数の初期値を記憶する記憶部を備え、前記記憶部から前記複数の初期値を読み出して、前記複数のカルマンフィルタの各々に供給することを特徴とする請求項1に記載の状態推定装置。
- 前記システムの状態ベクトルは、物体の姿勢を示す状態変数を含み、
前記複数の初期値は、前記物体の基準姿勢と、互いに等しい角度で交わる3以上の軸の各々を中心として、等しい角度だけ前記基準姿勢を回転させた複数の初期姿勢である、
ことを特徴とする請求項2に記載の状態推定装置。 - システムを観測して観測値を各々出力する複数のセンサと、
状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行する複数のカルマンフィルタと、
前記状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、前記複数のカルマンフィルタの各々に供給する初期値生成部と、
前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差に基づいて、前記状態ベクトルが最も早く収束するカルマンフィルタを特定し、特定したカルマンフィルタの状態ベクトルを出力する出力部とを備え、
前記複数のカルマンフィルタは、並列に動作して前記観測残差を同時に算出し、
前記初期値生成部は、前記複数のセンサの一部又は全部から出力される前記観測値に基づいて、前記複数の初期値を生成する、
ことを特徴とする状態推定装置。 - 前記出力部は、現在時刻から所定時間だけ過去の時刻までの所定期間における前記少なくとも1つのセンサに係る観測残差の最大値を前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値のうち最も小さい前記最大値に対応するカルマンフィルタを、前記状態ベクトルが最も早く収束するカルマンフィルタとして特定する、
ことを特徴とする請求項4に記載の状態推定装置。 - 前記システムの状態ベクトルは、物体の姿勢を示す状態変数を含み、
前記複数のセンサは、前記物体の加速度を直交する3軸で検出する加速度センサを備え、
前記初期値生成部は、前記加速度センサの前記観測値に基づいて、前記物体の水平面に対する傾きと一致し、水平面に対して垂直な軸を中心として等しい角度だけ回転した複数の姿勢を各々示すように前記複数の初期値を生成する、
ことを特徴とする請求項4又は5に記載の状態推定装置。 - 前記複数のセンサは、前記システムを直交する3軸で観測する所定のセンサを含み、
前記観測残差は、前記所定のセンサの観測値に対応する特定要素を含む複数の要素からなり、
前記出力部は、前記所定期間における前記観測残差の前記特定要素の最大値を前記複数のカルマンフィルタごとに算出し、算出した複数の前記最大値のうち最も小さい前記最大値に対応するカルマンフィルタを、前記状態ベクトルが最も早く収束するカルマンフィルタとして特定する、
ことを特徴とする請求項1乃至6のうちいずれか1項に記載の状態推定装置。 - システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定方法であって、
前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、
状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行し、
現在時刻から所定時間だけ過去の時刻までの所定期間における前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出し、
複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定し、
特定したカルマンフィルタ処理の状態ベクトルを出力する、
ことを特徴とする状態推定方法。 - システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定装置に用いられる状態推定プログラムであって、
前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成する処理と、
状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行する処理と、
現在時刻から所定時間だけ過去の時刻までの所定期間における前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出する処理と、
複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定する処理と、
特定したカルマンフィルタ処理の状態ベクトルを出力する処理とを、
コンピュータに実行させる状態推定プログラム。 - システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定方法であって、
前記複数のセンサの一部又は全部から出力される前記観測値に基づいて、前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成し、
状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行し、
前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出し、
複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定し、
特定したカルマンフィルタ処理の状態ベクトルを出力する、
ことを特徴とする状態推定方法。 - システムを観測する複数のセンサから出力される観測値に基づいて、前記システムの状態を推定する状態推定装置に用いられる状態推定プログラムであって、
前記複数のセンサの一部又は全部から出力される前記観測値に基づいて、前記システムの状態を示す複数の状態変数を要素とする状態ベクトルの初期状態として互いに異なる複数の初期値を生成する処理と、
状態遷移モデルを用いて前記状態ベクトルを推定し、観測値モデルを用いて推定した前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と推定した前記状態ベクトルとに基づいて前記状態ベクトルを更新するカルマンフィルタ処理を、前記複数の初期値の各々について並列に実行する処理と、
前記観測残差に含まれる、前記複数のセンサのうち少なくとも1つのセンサに係る観測残差についての最大値を複数の前記カルマンフィルタ処理ごとに算出する処理と、
複数の前記カルマンフィルタ処理ごとに算出した最大値のうち最も小さい前記最大値に対応するカルマンフィルタ処理を、前記状態ベクトルが最も早く収束するカルマンフィルタ処理として特定する処理と、
特定したカルマンフィルタ処理の状態ベクトルを出力する処理とを、
コンピュータに実行させる状態推定プログラム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010283243A JP2012132713A (ja) | 2010-12-20 | 2010-12-20 | 状態推定装置、状態推定方法および状態推定プログラム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2010283243A JP2012132713A (ja) | 2010-12-20 | 2010-12-20 | 状態推定装置、状態推定方法および状態推定プログラム |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2012132713A true JP2012132713A (ja) | 2012-07-12 |
Family
ID=46648488
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2010283243A Pending JP2012132713A (ja) | 2010-12-20 | 2010-12-20 | 状態推定装置、状態推定方法および状態推定プログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2012132713A (ja) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2019057198A (ja) * | 2017-09-22 | 2019-04-11 | 株式会社神戸製鋼所 | 油圧システムのパラメータ推定方法 |
JP7478474B2 (ja) | 2018-02-08 | 2024-05-07 | ビタ インクリナータ アイピー ホールディングス エルエルシー | 吊荷安定システム及び方法 |
-
2010
- 2010-12-20 JP JP2010283243A patent/JP2012132713A/ja active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2019057198A (ja) * | 2017-09-22 | 2019-04-11 | 株式会社神戸製鋼所 | 油圧システムのパラメータ推定方法 |
JP7478474B2 (ja) | 2018-02-08 | 2024-05-07 | ビタ インクリナータ アイピー ホールディングス エルエルシー | 吊荷安定システム及び方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109238262B (zh) | 一种航向姿态解算及罗盘校准抗干扰方法 | |
JP5934296B2 (ja) | モバイル機器でのセンサ測定値の較正 | |
US20140122015A1 (en) | Attitude estimation method and apparatus | |
JP6215337B2 (ja) | 適応型スケールおよび/または重力推定 | |
JP2013064695A (ja) | 状態推定装置、オフセット更新方法およびオフセット更新プログラム | |
JP7025215B2 (ja) | 測位システム及び測位方法 | |
WO2014119799A1 (en) | Inertial device, method, and program | |
KR20140025319A (ko) | 근 자기장의 동적 추적 및 보상을 위한 장치 및 방법 | |
JP2012173190A (ja) | 測位システム、測位方法 | |
JP2017166895A (ja) | 電子機器及びセンサ較正方法、センサ較正プログラム | |
JP2013096724A (ja) | 状態推定装置 | |
JP2013029512A (ja) | 携帯型電子デバイスのための、磁場センサおよび加速度計を使用して姿勢および角速度を検知するシステムおよび方法 | |
Vertzberger et al. | Attitude adaptive estimation with smartphone classification for pedestrian navigation | |
JP2013122384A (ja) | カルマンフィルタ、及び、状態推定装置 | |
JP2013061309A (ja) | カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム | |
JP2013185898A (ja) | 状態推定装置 | |
Allotta et al. | Underwater vehicles attitude estimation in presence of magnetic disturbances | |
JP2014219340A (ja) | オフセット補正方法及びオフセット補正装置 | |
JP2012132713A (ja) | 状態推定装置、状態推定方法および状態推定プログラム | |
JP6384194B2 (ja) | 情報処理装置、情報処理方法及び情報処理プログラム | |
JP2006038650A (ja) | 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム | |
US10648812B2 (en) | Method for filtering the signals arising from a sensor assembly comprising at least one sensor for measuring a vector physical field which is substantially constant over time and in space in a reference frame | |
KR101140379B1 (ko) | 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치 | |
JP2013088162A (ja) | 状態推定装置 | |
JP2020528560A (ja) | 磁場の測定から判定された方位を特徴付ける方法および装置 |