JP2018063142A - 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム - Google Patents
運動パラメータ推定装置、運動パラメータ推定方法及びプログラム Download PDFInfo
- Publication number
- JP2018063142A JP2018063142A JP2016200719A JP2016200719A JP2018063142A JP 2018063142 A JP2018063142 A JP 2018063142A JP 2016200719 A JP2016200719 A JP 2016200719A JP 2016200719 A JP2016200719 A JP 2016200719A JP 2018063142 A JP2018063142 A JP 2018063142A
- Authority
- JP
- Japan
- Prior art keywords
- measurement
- motion
- motion parameter
- equation
- value
- 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.)
- Granted
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
【課題】アクティブセンサとパッシブセンサとの組み合わせにより、対象物の運動の推定精度を高められるようにする。【解決手段】測定部は、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いてアクティブセンサでの測定周期よりも短い周期で対象物の方向を測定する。駆動雑音サンプル設定部は、対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する。粒子算出部は、駆動雑音のサンプルの各々を状態方程式に代入して、粒子フィルタにおける粒子を複数算出する。第一推定部は、粒子算出部が算出した粒子の各々を観測方程式に代入して、測定部の測定値の推定値を算出する。選択部は、測定部の測定値と第一推定部の推定値との誤差に基づいて、複数の粒子のうち一部の粒子を選択する。第二推定部は、選択部が選択した粒子に基づいて、対象物の運動を示す運動パラメータの推定値を算出する。【選択図】図１
Description
本発明は、運動パラメータ推定装置、運動パラメータ推定方法及びプログラムに関する。
ソナー等のセンサを用いて対象物の位置を測定するための幾つかの技術が提案されている。例えば、特許文献１に記載の技術では、パッシブ方式のソナー使用時に、別の周波数でアクティブ方式のソナーを使用する。
アクティブセンサとパッシブセンサとの組み合わせにより、それぞれのセンサを単独で用いる場合よりも対象物の運動の推定精度を高められることが好ましい。これに対し、特許文献１では、アクティブ方式での測定結果とパッシブ方式での測定結果とを組み合わせて対象物の運動の推定精度を高める具体的方法は示されていない。
本発明は、アクティブセンサとパッシブセンサとの組み合わせにより、対象物の運動の推定精度を高めることができる運動パラメータ推定装置、運動パラメータ推定方法及びプログラムを提供する。
本発明の第１の態様によれば、運動パラメータ推定装置は、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定部と、前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定部と、前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出部と、前記粒子算出部が算出した粒子の各々を観測方程式に代入して、前記測定部の測定値の推定値を算出する第一推定部と、前記測定部の測定値と前記第一推定部の推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択部と、前記選択部が選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定部と、を備える。
前記状態方程式は前記対象物の運動に加えて前記運動パラメータ推定装置の運動を示し、前記観測方程式は、前記対象物の運動の測定値の方程式に加えて前記運動パラメータ推定装置の運動の測定値の方程式を含み、前記第二推定部は、前記対象物の運動および前記運動パラメータ推定装置の運動を示す前記運動パラメータの推定値を算出するようにしてもよい。
前記観測方程式は、前記運動パラメータ推定装置の速度の測定値の方程式を含むようにしてもよい。
前記観測方程式は、前記アクティブセンサにおけるドップラー効果に基づく前記対象物の速度の測定値の方程式を含むようにしてもよい。
前記第一推定部は、前記アクティブセンサ、前記パッシブセンサそれぞれの観測方程式を用いて前記測定部の測定値の推定値を算出するようにしてもよい。
本発明の第二の態様によれば、運動パラメータ推定方法は、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、を含む。
本発明の第三の態様によれば、プログラムは、コンピュータに、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、を実行させるためのプログラムである。
上記した運動パラメータ推定装置、運動パラメータ推定方法及びプログラムによれば、できる。アクティブセンサとパッシブセンサとの組み合わせにより、対象物の運動の推定精度を高めることができる。
以下、本発明の実施形態を説明するが、以下の実施形態は特許請求の範囲にかかる発明を限定するものではない。また、実施形態の中で説明されている特徴の組み合わせの全てが発明の解決手段に必須であるとは限らない。
＜第１実施形態＞
図１は、本発明の第１実施形態に係る運動パラメータ推定装置の機能構成を示す概略ブロック図である。同図に示すように、運動パラメータ推定装置１００は、測定部１１０と、記憶部１８０と、制御部１９０とを備える。測定部１１０は、パッシブセンサ１１１と、アクティブセンサ１１２とを備える。制御部１９０は、駆動雑音サンプル設定部１９１と、粒子算出部１９２と、第一推定部１９３と、選択部１９４と、補正部１９５と、第二推定部１９６とを備える。また、運動パラメータ推定装置１００は、移動体１０に搭載されている。
図１は、本発明の第１実施形態に係る運動パラメータ推定装置の機能構成を示す概略ブロック図である。同図に示すように、運動パラメータ推定装置１００は、測定部１１０と、記憶部１８０と、制御部１９０とを備える。測定部１１０は、パッシブセンサ１１１と、アクティブセンサ１１２とを備える。制御部１９０は、駆動雑音サンプル設定部１９１と、粒子算出部１９２と、第一推定部１９３と、選択部１９４と、補正部１９５と、第二推定部１９６とを備える。また、運動パラメータ推定装置１００は、移動体１０に搭載されている。
運動パラメータ推定装置１００は、対象物の運動を示す運動パラメータの値を推定する。運動パラメータ推定装置１００は、例えばセンサを備えたコンピュータを用いて構成される。
図２は、運動パラメータ推定装置１００が測定を行う測定系の構成例を示す図である。図２に示すように、測定系１は、運動パラメータ推定装置１００を搭載した移動体１０と、運動パラメータ推定装置１００が運動を測定する対象である対象物２０とを含む。
線Ｌ１１は、移動体１０の軌跡の例を示す。線Ｌ１２は、対象物２０の軌跡の例を示す。矢印Ｂ１１は、移動体１０の速度ベクトルの例を示す。矢印Ｂ１２は、対象物２０の速度ベクトルの例を示す。
図２は、運動パラメータ推定装置１００が測定を行う測定系の構成例を示す図である。図２に示すように、測定系１は、運動パラメータ推定装置１００を搭載した移動体１０と、運動パラメータ推定装置１００が運動を測定する対象である対象物２０とを含む。
線Ｌ１１は、移動体１０の軌跡の例を示す。線Ｌ１２は、対象物２０の軌跡の例を示す。矢印Ｂ１１は、移動体１０の速度ベクトルの例を示す。矢印Ｂ１２は、対象物２０の速度ベクトルの例を示す。
運動パラメータ推定装置１００は、移動するいろいろな物を測定対象（運動パラメータ推定対象）とすることができる。例えば、運動パラメータ推定装置１００の測定対象領域は海中等の水中であってもよく、対象物２０は、水中航走体であってもよい。あるいは、運動パラメータ推定装置１００の測定対象領域は海上などの水面上であってもよく、対象物２０は船舶など水面上を移動する物であってもよい。あるいは、運動パラメータ推定装置１００の測定対象領域は陸上（地面）であってもよく、対象物２０は、車両など陸上を走行する物であってもよい。あるいは、運動パラメータ推定装置１００の測定対象領域は空中であってもよく、対象物２０は、飛行機など空中を移動する物であってもよい。あるいは、運動パラメータ推定装置１００の測定対象は宇宙であってもよく、対象物２０は宇宙船など宇宙を移動する物であってもよい。
対象物２０と同様、移動体１０も、いろいろな物とすることができる。例えば、移動体１０は、水中航走体であってもよい。あるいは、移動体１０は、船舶など水面上を移動する物であってもよい。あるいは、移動体１０は、車両など陸上を走行する物であってもよい。あるいは、移動体１０は、飛行機など空中を移動する物であってもよい。あるいは、移動体１０は、宇宙船など宇宙を移動する物であってもよい。
なお、以下では、運動パラメータ推定装置１００が移動体１０に搭載されて移動する場合を例に説明するが、運動パラメータ推定装置１００が固定的に設置されていてもよい。例えば、運動パラメータ推定装置１００が移動体１０に代えて建物内など移動しない構造物に設けられていてもよい。
なお、以下では、運動パラメータ推定装置１００が移動体１０に搭載されて移動する場合を例に説明するが、運動パラメータ推定装置１００が固定的に設置されていてもよい。例えば、運動パラメータ推定装置１００が移動体１０に代えて建物内など移動しない構造物に設けられていてもよい。
また、以下では、運動パラメータ推定装置１００の測定対象領域が２次元の領域であり、運動パラメータ推定装置１００がｘｙ座標（２次元直交座標）を用いる場合を例に説明するが、運動パラメータ推定装置１００の測定対象領域は３次元であってもよい。運動パラメータ推定装置１００の測定対象領域が３次元の領域である場合、運動パラメータ推定装置１００がｘｙｚ座標（３次元直交座標）を用いるようにしてもよい。
運動パラメータ推定装置１００の測定対象領域が３次元の領域である場合、運動パラメータ推定装置１００は、以下に説明するｘ座標及びｙ座標に関する処理をｚ座標についても行う。また、運動パラメータ推定装置１００は、以下に説明する２次元の角又は方向に関する処理と同様に、３次元の角又は方向について処理を行う。
運動パラメータ推定装置１００の測定対象領域が３次元の領域である場合、運動パラメータ推定装置１００は、以下に説明するｘ座標及びｙ座標に関する処理をｚ座標についても行う。また、運動パラメータ推定装置１００は、以下に説明する２次元の角又は方向に関する処理と同様に、３次元の角又は方向について処理を行う。
測定部１１０は、運動パラメータを測定する。
パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した物理的エネルギーを測定することにより、測定部１１０の位置（移動体１０の位置）から見た対象物２０の方向を測定する。対象物２０の方向は、運動パラメータの１つである。
パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した物理的エネルギーを測定することにより、測定部１１０の位置（移動体１０の位置）から見た対象物２０の方向を測定する。対象物２０の方向は、運動パラメータの１つである。
図３は、パッシブセンサ１１１による対象物２０の方向の測定例を示す図である。図３の例で、対象物２０からパッシブセンサ１１１に到達した物理的エネルギーを矢印Ｂ２１で示している。
例えば、パッシブセンサ１１１がパッシブソナーである場合、パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した音を測定する。この音は、対象物２０が発した音であってもよいし、対象物２０以外の物が発した音を対象物２０が反射した音であってもよい。
あるいは、パッシブセンサ１１１がパッシブなレーダーである場合、パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した電磁波を測定する。この電磁波は、対象物２０が発した電磁波であってもよいし、対象物２０以外の物が発した電磁波を対象物２０が反射した電磁波であってもよい。
例えば、パッシブセンサ１１１がパッシブソナーである場合、パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した音を測定する。この音は、対象物２０が発した音であってもよいし、対象物２０以外の物が発した音を対象物２０が反射した音であってもよい。
あるいは、パッシブセンサ１１１がパッシブなレーダーである場合、パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した電磁波を測定する。この電磁波は、対象物２０が発した電磁波であってもよいし、対象物２０以外の物が発した電磁波を対象物２０が反射した電磁波であってもよい。
アクティブセンサ１１２は、対象物２０に向けて物理的エネルギーを出力し、対象物２０で反射してパッシブセンサ１１１に到達した物理的エネルギーを測定することにより、対象物２０の位置を測定する。対象物２０の位置は、運動パラメータの１つである。
図４は、アクティブセンサ１１２による対象物２０の位置の測定例を示す図である。図４の例で、アクティブセンサ１１２が対象物２０に向けて出力した物理的エネルギーを矢印Ｂ３１で示している。また、対象物２０で反射してパッシブセンサ１１１に到達した物理的エネルギーを矢印Ｂ３２で示している。
例えば、アクティブセンサ１１２がアクティブソナーである場合、アクティブセンサ１１２は、対象物２０に向けて音（音波）を出力し、対象物２０で反射してアクティブセンサ１１２に到達した音を測定する。
あるいは、アクティブセンサ１１２がアクティブなレーダーである場合、アクティブセンサ１１２は、対象物２０に向けて電磁波を出力し、対象物２０で反射してアクティブセンサ１１２に到達した電磁波を測定する。
アクティブセンサ１１２は、アクティブセンサ１１２の位置（移動体１０の位置）に対する対象物２０の相対位置を測定する。アクティブセンサ１１２が、この相対位置とアクティブセンサ１１２自らの位置とに基づいて、アクティブセンサ１１２の位置以外の位置を基準とした対象物２０の位置を求めるようにしてもよい。
例えば、アクティブセンサ１１２がアクティブソナーである場合、アクティブセンサ１１２は、対象物２０に向けて音（音波）を出力し、対象物２０で反射してアクティブセンサ１１２に到達した音を測定する。
あるいは、アクティブセンサ１１２がアクティブなレーダーである場合、アクティブセンサ１１２は、対象物２０に向けて電磁波を出力し、対象物２０で反射してアクティブセンサ１１２に到達した電磁波を測定する。
アクティブセンサ１１２は、アクティブセンサ１１２の位置（移動体１０の位置）に対する対象物２０の相対位置を測定する。アクティブセンサ１１２が、この相対位置とアクティブセンサ１１２自らの位置とに基づいて、アクティブセンサ１１２の位置以外の位置を基準とした対象物２０の位置を求めるようにしてもよい。
記憶部１８０は、運動パラメータ推定装置１００が備える記憶デバイスを用いて構成され、各種情報を記憶する。
制御部１９０は、運動パラメータ推定装置１００の各部を制御して各種処理を実行する。制御部１９０は、例えば運動パラメータ推定装置１００が備えるＣＰＵ（Central Processing Unit、中央処理装置）が、記憶部１８０からプログラムを読み出して実行することで構成される。
制御部１９０は、運動パラメータ推定装置１００の各部を制御して各種処理を実行する。制御部１９０は、例えば運動パラメータ推定装置１００が備えるＣＰＵ（Central Processing Unit、中央処理装置）が、記憶部１８０からプログラムを読み出して実行することで構成される。
制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いて対象物２０の運動パラメータを推定する。制御部１９０は、例えば対象物２０の位置を推定する。
ここで、パッシブセンサ１１１は、測定部１１０の位置から見た対象物２０の方向を測定することはできるが、測定部１１０と対象物２０との相対距離を測定することはできない。このため、パッシブセンサ１１１の測定結果のみを用いたのでは、対象物２０の位置を高精度に推定することは困難である。
これに対し、制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いることで、パッシブセンサ１１１の測定結果のみを用いる場合よりも高精度に対象物２０の位置を測定することができる。
ここで、パッシブセンサ１１１は、測定部１１０の位置から見た対象物２０の方向を測定することはできるが、測定部１１０と対象物２０との相対距離を測定することはできない。このため、パッシブセンサ１１１の測定結果のみを用いたのでは、対象物２０の位置を高精度に推定することは困難である。
これに対し、制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いることで、パッシブセンサ１１１の測定結果のみを用いる場合よりも高精度に対象物２０の位置を測定することができる。
また、アクティブセンサ１１２は、対象物２０に対して物理的エネルギーを出力した後、対象物２０で反射した物理的エネルギーの到達を待ち受ける。このため、アクティブセンサ１１２の測定結果のみを用いたのでは、サンプリング周期をある程度大きくする必要がある。サンプリング周期がある程度大きくなることで、例えば運動パラメータの初期値が未知である場合など、運動パラメータの推定値が真値に近い値に収束するまでに時間を要する可能性がある。
これに対し、制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いることで、アクティブセンサ１１２の測定結果のみを用いる場合よりも速やかに、運動パラメータの推定値を真値に近い値に収束させることができる。
これに対し、制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いることで、アクティブセンサ１１２の測定結果のみを用いる場合よりも速やかに、運動パラメータの推定値を真値に近い値に収束させることができる。
駆動雑音サンプル設定部１９１は、駆動雑音のサンプルを設定する。ここでいう駆動雑音は、対象物２０の運動モデルを用いて推定される対象物２０の運動を、対象物２０の実際の運動に合わせるためのパラメータである。制御部１９０は、対象物２０の運動モデルとして対象物２０の状態方程式を用いており、状態方程式に駆動雑音が含まれている。
制御部１９０が用いる対象物２０の状態方程式は、例えば式（１）のように定義される。
制御部１９０が用いる対象物２０の状態方程式は、例えば式（１）のように定義される。
ここで、ｋはパッシブセンサ１１１のサンプリングタイミングのインデックスを示す。具体的には、時刻０を基準時刻として、ｋは、パッシブセンサ１１１が基準時刻よりも後に行ったサンプリングの回数を示す。パッシブセンサ１１１のサンプリング周期をＴＳで表すと、ｋ回目のサンプリング時刻はｋＴＳと表される。
以下では、パッシブセンサによる基準時刻よりも後のｋ回目のサンプリングのタイミング（すなわち、インデックスｋで示されるサンプリングタイミング）を、タイミングｋと称する。
以下では、パッシブセンサによる基準時刻よりも後のｋ回目のサンプリングのタイミング（すなわち、インデックスｋで示されるサンプリングタイミング）を、タイミングｋと称する。
運動パラメータベクトルｑ（ｋ）は、タイミングｋにおける運動パラメータを示す。特に、運動パラメータベクトルｑ（ｋ）は、タイミングｋにおける対象の位置、速度及び加速度を示す。運動パラメータベクトルｑ（ｋ）は、式（２）のように示される。
ここで、ｘ（ｋ）、ｙ（ｋ）は、それぞれタイミングｋにおける対象の位置のｘ座標値、ｙ座標値を示す。ｘ’（ｋ）、ｙ’（ｋ）は、それぞれタイミングｋにおける対象の速度のｘ座標値、ｙ座標値を示す。１階微分を’で示している。ｘ’’（ｋ）、ｙ’’（ｋ）は、それぞれタイミングｋにおける対象の加速度のｘ座標値、ｙ座標値を示す。２階微分を’’で示している。
行列又はベクトルの右に上付きで示されたＴは、行列の転置又はベクトルの転置を示す。
行列又はベクトルの右に上付きで示されたＴは、行列の転置又はベクトルの転置を示す。
駆動雑音ベクトルｕ（ｋ）は、タイミングｋにおける駆動雑音を示す。駆動雑音ベクトルｕ（ｋ）は、２つのスカラ変数ｕ１（ｋ）及びｕ２（ｋ）を用いて式（３）のように示される。
式（１）の行列Ａは、係数を示す行列である。行列Ａは、式（４）のように示される。
上記のようにＴＳは、パッシブセンサ１１１のサンプリング周期を示す。
行列Ｂは、係数を示す行列である。行列Ｂは、式（５）のように示される。
行列Ｂは、係数を示す行列である。行列Ｂは、式（５）のように示される。
式（１）に示される状態方程式は、タイミングｋにおける運動パラメータの値（対象の位置、速度、加速度）及び駆動雑音の値と、タイミングｋ＋１における運動パラメータの値（対象の位置、速度及び加速度）との関係を示している。
駆動雑音サンプル設定部１９１は、駆動雑音のサンプルを所定の確率分布に基づいて複数設定する。ここで、制御部１９０は、粒子フィルタを用いて運動パラメータの推定値を算出する。駆動雑音サンプル設定部１９１は、粒子フィルタにおける粒子数だけ駆動雑音を設定する。
駆動雑音サンプル設定部１９１が駆動雑音のサンプルの設定に用いる確率分布は特定の確率分布に限定されない。例えば、駆動雑音サンプル設定部１９１がガウス分布に基づいて駆動雑音の設定を行うようにしてもよい。あるいは、駆動雑音サンプル設定部１９１がガウス分布以外の分布に基づいて駆動雑音の設定を行うようにしてもよい。
駆動雑音サンプル設定部１９１が駆動雑音のサンプルの設定に用いる確率分布は特定の確率分布に限定されない。例えば、駆動雑音サンプル設定部１９１がガウス分布に基づいて駆動雑音の設定を行うようにしてもよい。あるいは、駆動雑音サンプル設定部１９１がガウス分布以外の分布に基づいて駆動雑音の設定を行うようにしてもよい。
粒子算出部１９２は、駆動雑音サンプル設定部１９１が設定した駆動雑音のサンプルの各々を状態方程式に代入して粒子フィルタにおける粒子を複数算出する。粒子算出部１９２は、タイミングｋ−１における運動パラメータの値及び駆動雑音の値を状態方程式に代入して、タイミングｋにおける運動パラメータの値を、粒子フィルタにおける粒子として算出する。
第一推定部１９３は、粒子算出部１９２が算出した粒子の各々を観測方程式に代入して運動パラメータの測定値の推定値を算出する。ここでいう観測方程式は運動パラメータから測定部１１０の測定値を推定するための方程式であり、運動パラメータの値に観測誤差（測定雑音）を付加する。
アクティブセンサ１１２における観測方程式は、例えば式（６）のように定義される。
アクティブセンサ１１２における観測方程式は、例えば式（６）のように定義される。
ここで、ベクトルｍ（ｋ）は、タイミングｋにおける運動パラメータの測定値を示す。アクティブセンサ１１２の場合、ベクトルｍ（ｋ）は、式（７）のように示される。
ここで、ｘｍ（ｋ）は、タイミングｋにおける対象の位置のｘ座標値の測定値を示す。ｙｍ（ｋ）は、タイミングｋにおける対象の位置のｙ座標値の測定値を示す。
式（６）の行列Ｃは、係数を示す行列である。行列Ｃは、式（８）のように示される。
また、式（６）のｗＡ（ｋ）は、タイミングｋにおけるアクティブセンサ１１２の観測誤差を示す。
一方、パッシブセンサ１１１における観測方程式は、例えば式（９）のように定義される。
一方、パッシブセンサ１１１における観測方程式は、例えば式（９）のように定義される。
ここで、ａｔａｎは、アークタンジェントを示す。ｘＯ（ｋ）は、タイミングｋにおける測定部１１０の位置のｘ座標値を示す。ｙＯ（ｋ）は、タイミングｋにおける測定部１１０の位置のｙ座標値を示す。
ｗＰ（ｋ）は、タイミングｋにおけるパッシブセンサ１１１の観測誤差を示す。
また、パッシブセンサ１１１の場合、ｍ（ｋ）は式（１０）のように示される。
ｗＰ（ｋ）は、タイミングｋにおけるパッシブセンサ１１１の観測誤差を示す。
また、パッシブセンサ１１１の場合、ｍ（ｋ）は式（１０）のように示される。
ここで、θｍ（ｋ）は、タイミングｋにおける測定部１１０の位置に対する対象の方位の測定値（測角計測値）を示す。
選択部１９４は、測定部１１０による運動パラメータの測定値と、第一推定部１９３が算出した運動パラメータの測定値の推定値との誤差に基づいて、複数の粒子のうち一部の粒子を選択する。具体的には、選択部１９４は、誤差の逆数から粒子の尤度を算出し、尤度が高い順に所定個の粒子を選択する。
補正部１９５は、選択部１９４が選択した粒子の各々に拡張カルマンフィルタを適用して、粒子の各々を補正する。補正部１９５は、各粒子の尤度を高めるように補正を行う。
第二推定部１９６は、補正部１９５による補正後の粒子に基づいて、運動パラメータの推定値を求める。さらに、第二推定部１９６は、運動パラメータに示される対象物２０の位置及び速度に基づいて、測定部１１０と対象物２０との間の距離、測定部１１０の位置から見た対象物２０の方向、及び、対象物２０の速度の大きさを算出する。
第二推定部１９６は、補正部１９５による補正後の粒子に基づいて、運動パラメータの推定値を求める。さらに、第二推定部１９６は、運動パラメータに示される対象物２０の位置及び速度に基づいて、測定部１１０と対象物２０との間の距離、測定部１１０の位置から見た対象物２０の方向、及び、対象物２０の速度の大きさを算出する。
次に、図５を参照して運動パラメータ推定装置１００の動作について説明する。図５は、運動パラメータ推定装置１００が行う処理の手順の例を示すフローチャートである。運動パラメータ推定装置１００は、パッシブセンサ１１１のサンプリングタイミング毎に、図５のステップＳ１０１〜Ｓ１１０の処理を繰り返す。
ここで、アクティブセンサ１１２では、対象物２０に向けて出力した物理的エネルギーが反射してアクティブセンサ１１２に戻ってくるまで待つ必要がある。これに対し、パッシブセンサ１１１では、アクティブセンサ１１２の場合のような待ち時間の制約はない。このため、パッシブセンサ１１１のサンプリング周期はアクティブセンサ１１２のサンプリング周期よりも短く設定できる。
そこで、運動パラメータ推定装置１００は、アクティブセンサ１１２のサンプリング周期よりもパッシブセンサ１１１のサンプリング周期を短く設定して運動パラメータの測定を行う。具体的には、パッシブセンサ１１１が運動パラメータをｎ回（ｎは正整数）測定する毎に、アクティブセンサ１１２が運動パラメータを１回測定する。これにより、パッシブセンサ１１１とアクティブセンサ１１２とが運動パラメータを測定するサンプリングタイミングと、パッシブセンサ１１１のみが運動パラメータを測定するサンプリングタイミングとがある。
ここで、アクティブセンサ１１２では、対象物２０に向けて出力した物理的エネルギーが反射してアクティブセンサ１１２に戻ってくるまで待つ必要がある。これに対し、パッシブセンサ１１１では、アクティブセンサ１１２の場合のような待ち時間の制約はない。このため、パッシブセンサ１１１のサンプリング周期はアクティブセンサ１１２のサンプリング周期よりも短く設定できる。
そこで、運動パラメータ推定装置１００は、アクティブセンサ１１２のサンプリング周期よりもパッシブセンサ１１１のサンプリング周期を短く設定して運動パラメータの測定を行う。具体的には、パッシブセンサ１１１が運動パラメータをｎ回（ｎは正整数）測定する毎に、アクティブセンサ１１２が運動パラメータを１回測定する。これにより、パッシブセンサ１１１とアクティブセンサ１１２とが運動パラメータを測定するサンプリングタイミングと、パッシブセンサ１１１のみが運動パラメータを測定するサンプリングタイミングとがある。
（ステップＳ１０１）
駆動雑音サンプル設定部１９１は、駆動雑音（駆動雑音ベクトルｕ（ｋ））のサンプルを粒子数Ｐだけ生成する。以下、タイミングｋにおける駆動雑音のサンプルをｕｐ（ｋ）と表記する。
ステップＳ１０１の後、ステップＳ１０２へ遷移する。
駆動雑音サンプル設定部１９１は、駆動雑音（駆動雑音ベクトルｕ（ｋ））のサンプルを粒子数Ｐだけ生成する。以下、タイミングｋにおける駆動雑音のサンプルをｕｐ（ｋ）と表記する。
ステップＳ１０１の後、ステップＳ１０２へ遷移する。
（ステップＳ１０２）
粒子算出部１９２は、ステップＳ１０１で生成したＰ個のサンプル（ベクトルｕｐ（ｋ））の各々を状態方程式（式（１））に代入し、Ｐ個の粒子を算出する。以下では、タイミングｋにおける粒子をｑｐ（ｋ）と表記する。
ステップＳ１０２の後、ステップＳ１０３へ遷移する。
粒子算出部１９２は、ステップＳ１０１で生成したＰ個のサンプル（ベクトルｕｐ（ｋ））の各々を状態方程式（式（１））に代入し、Ｐ個の粒子を算出する。以下では、タイミングｋにおける粒子をｑｐ（ｋ）と表記する。
ステップＳ１０２の後、ステップＳ１０３へ遷移する。
（ステップＳ１０３）
第一推定部１９３は、ステップＳ１０２で得られた粒子の各々を観測方程式に代入し、測定部１１０による測定値の推定値を算出する。上記のように、アクティブセンサ１１２の場合の観測方程式（式（６））と、パッシブセンサ１１１の場合の観測方程式（式（９））とがある。ここでは、第一推定部１９３は、パッシブセンサ１１１の場合の観測方程式（式（９））を用いて、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値を求める。以下では、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値をθｐ（ｋ）と表記する。
なお、観測誤差ＷＡ（ｋ）については、例えば、センサ（パッシブセンサ１１１又はアクティブセンサ１１２）の特性、及び、実環境（測定を行う環境）に基づいて、観測誤差が従う確率分布及びそのパラメータを予め設定しておく。そして、設定した確率分布及びパラメータに基づいて乱数を生成することで、各粒子に対応する観測誤差を生成することができる。
ステップＳ１０３の後、ステップＳ１０４へ遷移する。
第一推定部１９３は、ステップＳ１０２で得られた粒子の各々を観測方程式に代入し、測定部１１０による測定値の推定値を算出する。上記のように、アクティブセンサ１１２の場合の観測方程式（式（６））と、パッシブセンサ１１１の場合の観測方程式（式（９））とがある。ここでは、第一推定部１９３は、パッシブセンサ１１１の場合の観測方程式（式（９））を用いて、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値を求める。以下では、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値をθｐ（ｋ）と表記する。
なお、観測誤差ＷＡ（ｋ）については、例えば、センサ（パッシブセンサ１１１又はアクティブセンサ１１２）の特性、及び、実環境（測定を行う環境）に基づいて、観測誤差が従う確率分布及びそのパラメータを予め設定しておく。そして、設定した確率分布及びパラメータに基づいて乱数を生成することで、各粒子に対応する観測誤差を生成することができる。
ステップＳ１０３の後、ステップＳ１０４へ遷移する。
（ステップＳ１０４）
選択部１９４は、ステップＳ１０３で得られた測定値の推定値と、測定値との誤差を算出する。
ここでは、選択部１９４は、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値θｐ（ｋ）から、タイミングｋにおけるセンサに対する対象の方位の測定値θｍ（ｋ）を減算した差を誤差として算出する。
以下では、ステップＳ１０３で得られた測定値の推定値と、測定部１１０による測定値との誤差をｅｐ（ｋ）と表記する。
ステップＳ１０４の後、ステップＳ１０５へ遷移する。
選択部１９４は、ステップＳ１０３で得られた測定値の推定値と、測定値との誤差を算出する。
ここでは、選択部１９４は、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値θｐ（ｋ）から、タイミングｋにおけるセンサに対する対象の方位の測定値θｍ（ｋ）を減算した差を誤差として算出する。
以下では、ステップＳ１０３で得られた測定値の推定値と、測定部１１０による測定値との誤差をｅｐ（ｋ）と表記する。
ステップＳ１０４の後、ステップＳ１０５へ遷移する。
（ステップＳ１０５）
選択部１９４は、ステップＳ１０４で算出した誤差ｅｐ（ｋ）の逆数ｅｐ（ｋ）−１から、タイミングｋにおける尤度πｐ（ｋ）を算出する。選択部１９４は、誤差の逆数ｅｐ（ｋ）−１に対して総和が１になるように正規化を行う。タイミングｋにおける尤度πｐ（ｋ）は、式（１１）のように示される。
選択部１９４は、ステップＳ１０４で算出した誤差ｅｐ（ｋ）の逆数ｅｐ（ｋ）−１から、タイミングｋにおける尤度πｐ（ｋ）を算出する。選択部１９４は、誤差の逆数ｅｐ（ｋ）−１に対して総和が１になるように正規化を行う。タイミングｋにおける尤度πｐ（ｋ）は、式（１１）のように示される。
ステップＳ１０５の後、ステップＳ１０６へ遷移する。
（ステップＳ１０６）
選択部１９４は、尤度の高い粒子（尤度πｐ（ｋ）の値が大きい粒子）から順にＰ個（Ｐは正整数）の粒子を選択（リサンプリング）する。ここで、Ｐは予め定められている定数である。
選択部１９４は、選択したＰ個の粒子以外の粒子は棄却する。
ステップＳ１０６の後、ステップＳ１０７へ遷移する。
選択部１９４は、尤度の高い粒子（尤度πｐ（ｋ）の値が大きい粒子）から順にＰ個（Ｐは正整数）の粒子を選択（リサンプリング）する。ここで、Ｐは予め定められている定数である。
選択部１９４は、選択したＰ個の粒子以外の粒子は棄却する。
ステップＳ１０６の後、ステップＳ１０７へ遷移する。
（ステップＳ１０７）
補正部１９５は、ステップＳ１０６で選択したＰ個の粒子の各々に拡張カルマンフィルタを適用して各粒子の値を補正する。補正部１９５は、各粒子の尤度を高めるように補正を行う。そのために、補正部１９５は、選択部１９４が選択した粒子の各々を用いてソナーによる測定値の推定値を再度算出する。ここでは、ステップＳ１０３の場合と異なり、補正部１９５は、アクティブセンシング時は、アクティブセンサ１１２による観測方程式（式（６））を用いる。一方、補正部１９５は、パッシブセンシング時は、パッシブセンサ１１１による観測方程式（式（９））を用いる。これにより、アクティブセンサ１１２による測距の情報を粒子の尤度補正に反映させることができる。
補正部１９５は、ステップＳ１０６で選択したＰ個の粒子の各々に拡張カルマンフィルタを適用して各粒子の値を補正する。補正部１９５は、各粒子の尤度を高めるように補正を行う。そのために、補正部１９５は、選択部１９４が選択した粒子の各々を用いてソナーによる測定値の推定値を再度算出する。ここでは、ステップＳ１０３の場合と異なり、補正部１９５は、アクティブセンシング時は、アクティブセンサ１１２による観測方程式（式（６））を用いる。一方、補正部１９５は、パッシブセンシング時は、パッシブセンサ１１１による観測方程式（式（９））を用いる。これにより、アクティブセンサ１１２による測距の情報を粒子の尤度補正に反映させることができる。
ここで、パッシブセンシング時については、パッシブソナーによる観測方程式はアークタンジェント（ａｔａｎ）を含む非線形な式である。そこで、拡張カルマンフィルタの枠組みに基づき、観測方程式をテイラー展開して１次の項で打ち切った近似値を採用する。具体的には、式（９）を式（１２）のように変形して用いる。
ここで、ベクトルＣ（ｋ）は係数を示すベクトルであり、式（１３）のように示される。
偏微分∂ｈｋ／∂ｘ（ｋ）は、式（１４）のように示される。
α（ｋ）は、センサに対する対象の相対位置のｙ座標値をｘ座標値で除算した比を示す。α（ｋ）は、式（１５）のように示される。
偏微分∂ｈｋ／∂ｙ（ｋ）は、式（１６）のように示される。
式（１２）に示される観測方程式を用いることにより、補正部１９５は、パッシブセンシング時もアクティブセンシング時と同様にカルマンゲインを算出することができる。補正部１９５は、各粒子に対してカルマンゲインを用いた補正を行う。
ここでのカルマンゲインＬ（ｋ）は、式（１７）のように示される。
ここでのカルマンゲインＬ（ｋ）は、式（１７）のように示される。
Ｐ（ｋ）は、タイミングｋにおける推定誤差共分散行列を示す。Ｗ（ｋ）は、タイミングｋにおける観測誤差の分散行列を示す。
ここでのカルマンゲインＬ（ｋ）を用いた粒子の補正は、式（１８）のように示される。
ここでのカルマンゲインＬ（ｋ）を用いた粒子の補正は、式（１８）のように示される。
式（１８）の左辺のｑｐ（ｋ）は、タイミングｋにおける補正後の粒子を示す。右辺のｑｐ（ｋ）は、タイミングｋにおける補正前の粒子を示す。また、ｑｅ ｐ（ｋ）は、タイミングｋにおける運動パラメータの推定値を示す。
ステップＳ１０７の後、ステップＳ１０８へ遷移する。
ステップＳ１０７の後、ステップＳ１０８へ遷移する。
（ステップＳ１０８）
第二推定部１９６は、カルマンゲインを用いた補正後の粒子の算術平均を算出する。第二推定部１９６は、得られた平均値を、タイミングｋにおける対象の運動パラメータ推定値とする。以下では、タイミングｋにおける対象の運動パラメータ推定値をｑｅ（ｋ）と表記する。
ステップＳ１０８の後、ステップＳ１０９へ遷移する。
第二推定部１９６は、カルマンゲインを用いた補正後の粒子の算術平均を算出する。第二推定部１９６は、得られた平均値を、タイミングｋにおける対象の運動パラメータ推定値とする。以下では、タイミングｋにおける対象の運動パラメータ推定値をｑｅ（ｋ）と表記する。
ステップＳ１０８の後、ステップＳ１０９へ遷移する。
（ステップＳ１０９）
第二推定部１９６は、タイミングｋにおける対象の運動パラメータ推定値ｑｅ（ｋ）と、センサの位置とに基づいて、センサと対象との距離、センサに対する対象の方位及び相対速度を算出する。タイミングｋにおけるセンサと対象との距離Ｒ（ｋ）は、式（１９）のように示される。
第二推定部１９６は、タイミングｋにおける対象の運動パラメータ推定値ｑｅ（ｋ）と、センサの位置とに基づいて、センサと対象との距離、センサに対する対象の方位及び相対速度を算出する。タイミングｋにおけるセンサと対象との距離Ｒ（ｋ）は、式（１９）のように示される。
ここでの対象の位置のｘ座標値ｘ（ｋ）、ｙ座標値ｙ（ｋ）として、いずれもｑｅ（ｋ）における値を用いる。
タイミングｋにおけるセンサに対する対象の方位θ（ｋ）は、式（２０）のように示される。
タイミングｋにおけるセンサに対する対象の方位θ（ｋ）は、式（２０）のように示される。
ここでの対象の位置のｘ座標値ｘ（ｋ）、ｙ座標値ｙ（ｋ）として、いずれもｑｅ（ｋ）における値を用いる。
タイミングｋにおけるセンサに対する対象の相対速度ｖ（ｋ）は、式（２１）のように示される。
タイミングｋにおけるセンサに対する対象の相対速度ｖ（ｋ）は、式（２１）のように示される。
ここでの対象の速度のｘ座標値ｘ’（ｋ）、ｙ座標値ｙ’（ｋ）として、いずれもｑｅ（ｋ）における値を用いる。
ステップＳ１０９の後、ステップＳ１１０へ遷移する。
ステップＳ１０９の後、ステップＳ１１０へ遷移する。
（ステップＳ１１０）
制御部１９０は、ｋ：＝ｋ＋１とする。すなわち、サンプリングタイミングを示すインデックスの値を１進める（大きくする）。
ステップＳ１１０の後、次のサンプリングタイミングでステップＳ１０１からの処理を繰り返す。
制御部１９０は、ｋ：＝ｋ＋１とする。すなわち、サンプリングタイミングを示すインデックスの値を１進める（大きくする）。
ステップＳ１１０の後、次のサンプリングタイミングでステップＳ１０１からの処理を繰り返す。
図６は、運動パラメータ推定装置１００による運動パラメータの推定値の真値付近への収束例を示す図である。図６に示すグラフの横軸は時刻を示し、縦軸は運動パラメータの値を示す。線Ｌ２１は、運動パラメータの真値を示す。線Ｌ２２は、運動パラメータ推定装置１００による運動パラメータの推定値を示す。線Ｌ２３は、アクティブセンサのみによる場合の運動パラメータの推定値を示す。時間ｔ１１は、アクティブセンサのみによる場合のサンプリング周期を示す。
アクティブセンサのみによる場合、上記のようにアクティブセンサでは物理的エネルギーを対象物に向けて出力してから対象物で反射した物理的エネルギーを受信するまでの待ち時間が生じるため、時間ｔ１１で示されるようにある程度のサンプリング間隔が生じる。これに対し、運動パラメータ推定装置１００では、アクティブセンサ１１２のサンプリング間隔より短いサンプリング間隔にてパッシブセンサ１１１が対象物の運動を測定することができる。この点で、運動パラメータ推定装置１００によれば、アクティブセンサのみによる場合よりも短い周期で対象物の運動を測定することができ、より高精度に対象物の運動を推定することが期待され、また、より速やかに真値付近に収束することが期待される。
アクティブセンサのみによる場合、上記のようにアクティブセンサでは物理的エネルギーを対象物に向けて出力してから対象物で反射した物理的エネルギーを受信するまでの待ち時間が生じるため、時間ｔ１１で示されるようにある程度のサンプリング間隔が生じる。これに対し、運動パラメータ推定装置１００では、アクティブセンサ１１２のサンプリング間隔より短いサンプリング間隔にてパッシブセンサ１１１が対象物の運動を測定することができる。この点で、運動パラメータ推定装置１００によれば、アクティブセンサのみによる場合よりも短い周期で対象物の運動を測定することができ、より高精度に対象物の運動を推定することが期待され、また、より速やかに真値付近に収束することが期待される。
以上のように、測定部１１０は、アクティブセンサ１１２を用いて対象物の位置を測定し、パッシブセンサ１１１を用いてアクティブセンサ１１２での測定周期よりも短い周期で対象物２０の方向を測定する。駆動雑音サンプル設定部１９１は、状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する。粒子算出部１９２は、駆動雑音のサンプルの各々を状態方程式に代入して、粒子フィルタにおける粒子を複数算出する。第一推定部１９３は、粒子算出部１９２が算出した粒子の各々を観測方程式に代入して、測定部１１０の測定値の推定値を算出する。選択部１９４は、測定部１１０の測定値と第一推定部１９３の推定値との誤差に基づいて、複数の粒子のうち一部の粒子を選択する。第二推定部１９６は、選択部１９４が選択した粒子に基づいて、運動パラメータの推定値を算出する。
このように、運動パラメータ推定装置１００では、状態方程式及び観測方程式を含むモデルに粒子フィルタを適用することで、アクティブセンサ１１２の測定結果とパッシブセンサ１１１の測定結果を同一のモデルに適用することができる。これにより、運動パラメータ推定装置１００では、アクティブセンサ１１２による対象物２０の位置の測定結果を用いることができ、かつ、アクティブセンサ１１２のみによる場合よりも短いサンプリング周期で対象物２０の運動を測定することができる。この点で、運動パラメータ推定装置１００によれば、アクティブセンサ１１２とパッシブセンサ１１１との組み合わせにより、対象物２０の運動の推定精度を高めることができる。
このように、運動パラメータ推定装置１００では、状態方程式及び観測方程式を含むモデルに粒子フィルタを適用することで、アクティブセンサ１１２の測定結果とパッシブセンサ１１１の測定結果を同一のモデルに適用することができる。これにより、運動パラメータ推定装置１００では、アクティブセンサ１１２による対象物２０の位置の測定結果を用いることができ、かつ、アクティブセンサ１１２のみによる場合よりも短いサンプリング周期で対象物２０の運動を測定することができる。この点で、運動パラメータ推定装置１００によれば、アクティブセンサ１１２とパッシブセンサ１１１との組み合わせにより、対象物２０の運動の推定精度を高めることができる。
＜第２実施形態＞
第１実施形態では、センサの位置（［ｘＯ（ｋ） ｙＯ（ｋ）］）の誤差については考慮していない。これに対し、ＧＮＳＳ（Global Navigation Satellite System、全球測位衛星システム）を用いた測位、デッドレコニング（Dead Reckoning）など、いずれの測位でもセンサの位置の測位結果に誤差が生じ得る。
そこで、第２実施形態では、対象の運動パラメータに加えてセンサ自らの運動パラメータも推定する。
第１実施形態では、センサの位置（［ｘＯ（ｋ） ｙＯ（ｋ）］）の誤差については考慮していない。これに対し、ＧＮＳＳ（Global Navigation Satellite System、全球測位衛星システム）を用いた測位、デッドレコニング（Dead Reckoning）など、いずれの測位でもセンサの位置の測位結果に誤差が生じ得る。
そこで、第２実施形態では、対象の運動パラメータに加えてセンサ自らの運動パラメータも推定する。
第２実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第２実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第２実施形態では、数式モデルを以下のように拡張する。
運動パラメータベクトルｑ（ｋ）には、対象の運動パラメータに加えてセンサの運動パラメータも含める。第２実施形態でのタイミングｋにおける運動パラメータ態ベクトルｑ（ｋ）は、式（２２）のように示される。
運動パラメータベクトルｑ（ｋ）には、対象の運動パラメータに加えてセンサの運動パラメータも含める。第２実施形態でのタイミングｋにおける運動パラメータ態ベクトルｑ（ｋ）は、式（２２）のように示される。
第２実施形態では、駆動雑音も拡張する。第２実施形態でのタイミングｋにおける駆動雑音ベクトルｕ（ｋ）は、式（２３）のように示される。
ｕ１（ｋ）及びｕ２（ｋ）と同様、ｕ３（ｋ）、ｕ４（ｋ）のいずれもスカラ変数である。
第２実施形態では、式（１）に示される状態方程式の行列Ａ及びＢも拡張する。第２実施形態では、式（１）で行列Ａに代えて行列Ａ１を用いる。行列Ａ１は、式（２４）のように示される。
第２実施形態では、式（１）に示される状態方程式の行列Ａ及びＢも拡張する。第２実施形態では、式（１）で行列Ａに代えて行列Ａ１を用いる。行列Ａ１は、式（２４）のように示される。
ここで、Ｒ１２×１２は、１２行１２列の実数行列を示す。
第２実施形態では、式（１）で行列Ｂに代えて行列Ｂ１を用いる。行列Ｂ１は、式（２５）のように示される。
第２実施形態では、式（１）で行列Ｂに代えて行列Ｂ１を用いる。行列Ｂ１は、式（２５）のように示される。
ここで、０６×２は、６行２列のゼロ行列を示す。Ｒ１２×４は、１２行４列の実数行列を示す。
第２実施形態では、式（６）に示される観測方程式の行列Ｃも拡張する。第２実施形態では、式（６）で行列Ｃに代えて行列Ｃ１を用いる。行列Ｃ１は、式（２６）のように示される。
第２実施形態では、式（６）に示される観測方程式の行列Ｃも拡張する。第２実施形態では、式（６）で行列Ｃに代えて行列Ｃ１を用いる。行列Ｃ１は、式（２６）のように示される。
ここで、０２×６は、２行６列のゼロ行列を示す。
第２実施形態では、観測値ベクトルに自己位置の測定値も追加する拡張を行う。第２実施形態では、アクティブソナーの場合の観測値ベクトルとして式（７）に代えて式（２７）を用いる。
第２実施形態では、観測値ベクトルに自己位置の測定値も追加する拡張を行う。第２実施形態では、アクティブソナーの場合の観測値ベクトルとして式（７）に代えて式（２７）を用いる。
ここで、ｘＯｍは、センサ自らの位置の測定値（測位結果）のｘ座標値を示す。ｙＯｍは、センサ自らの位置の測定値のｙ座標値を示す。
センサの位置は、アクティブソナー、パッシブソナーとは別に測定できるため、パッシブソナーの場合の観測値ベクトルとして式（２８）を用いる。
センサの位置は、アクティブソナー、パッシブソナーとは別に測定できるため、パッシブソナーの場合の観測値ベクトルとして式（２８）を用いる。
θｍ（ｋ）は、式（９）及び式（１２）を参照して説明したとおりである。
図５のステップＳ１０７におけるパッシブ時の非線形関数の近似として、式（１３）に代えて式（２９）を用いる。
図５のステップＳ１０７におけるパッシブ時の非線形関数の近似として、式（１３）に代えて式（２９）を用いる。
また、パッシブセンサの場合、式（６）に示される観測方程式の行列Ｃに代えて、式（３０）に示される行列Ｃ１を用いる。
Ｃ２は、式（３１）のように示される。
上記のように、０２×６は、２行６列のゼロ行列を示す。また、Ｒ２×１２は、２行１２列の実数行列を示す。
以上のように、第２実施形態では、状態方程式は対象物２０の運動に加えて運動パラメータ推定装置１００の運動を示す。また、観測方程式は、対象物２０の運動の測定値の方程式に加えて運動パラメータ推定装置１００の運動の測定値の方程式を含む。第二推定部１９６は、対象物２０の運動および運動パラメータ推定装置１００の運動を示す運動パラメータの推定値を算出する。
これにより、第２実施形態の運動パラメータ推定装置１００では、運動パラメータ推定装置１００自らの運動の測定誤差に対しても補正を行うことができる。この点で、対象物２０の運動の推定精度をより高めることができる。
これにより、第２実施形態の運動パラメータ推定装置１００では、運動パラメータ推定装置１００自らの運動の測定誤差に対しても補正を行うことができる。この点で、対象物２０の運動の推定精度をより高めることができる。
＜第３実施形態＞
センサ自らの位置に加えてセンサの速度の測定値を得られる場合、センサの測定の測定値を用いることで状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。そこで、第３実施形態では、第２実施形態における拡張に加えて、さらにセンサ自らの速度を観測値ベクトルに含める拡張を行う。
第３実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第３実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第３実施形態では、数式モデルを第２実施形態の場合からさらに拡張する。
第３実施形態でのアクティブセンシング時の観測値ベクトルは式（３２）のように示される。
センサ自らの位置に加えてセンサの速度の測定値を得られる場合、センサの測定の測定値を用いることで状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。そこで、第３実施形態では、第２実施形態における拡張に加えて、さらにセンサ自らの速度を観測値ベクトルに含める拡張を行う。
第３実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第３実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第３実施形態では、数式モデルを第２実施形態の場合からさらに拡張する。
第３実施形態でのアクティブセンシング時の観測値ベクトルは式（３２）のように示される。
ｘＯｍ（ｋ）’は、タイミングｋにおけるセンサの速度のｘ座標値の測定値を示す。ｙＯｍ（ｋ）’は、タイミングｋにおけるセンサの速度のｙ座標値の測定値を示す。
また、第３実施形態では、式（６）で行列Ｃに代えて行列Ｃ２を用いる。行列Ｃ２は、式（３３）のように示される。
また、第３実施形態では、式（６）で行列Ｃに代えて行列Ｃ２を用いる。行列Ｃ２は、式（３３）のように示される。
ここで、０２×６は、２行６列のゼロ行列を示す。０４×６は、４行６列のゼロ行列を示す。また、行列Ｃ３は、式（３４）のように示される。
一方、パッシブセンシング時の測定値ベクトルは、式（３５）のように示される。
この場合、式（６）で行列Ｃに代えてＣ４（ｋ）を用いる。Ｃ４（ｋ）は、式（３６）のように示される。
ここで、Ｒ５×１２は、５行１２列の実数行列を示す。
行列Ｃ５は、式（３７）のように示される。
行列Ｃ５は、式（３７）のように示される。
上記のように、０４×６は、４行６列のゼロ行列を示す。Ｒ４×１２は、４行１２列の実数行列を示す。
以上のように、第３実施形態では、観測方程式は、運動パラメータ推定装置１００の速度の測定値の方程式を含む。これにより、第３実施形態の運動パラメータ推定装置１００では、運動パラメータ推定装置１００自らの運動の推定精度をより高めることができ、もって、対象物２０の運動の推定精度をより高めることができる。
＜第４実施形態＞
アクティブセンサ１１２によるドップラー効果を測定することで、対象の速度のうちセンサ方向の速度成分を測定することができる。これを測定値の１つに加える拡張を行うことで、状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。そこで、第４実施形態では、対象の速度のうちセンサ方向の速度成分の測定値を観測値ベクトルに含める拡張を行う。第１実施形態〜第３実施形態のいずれにも第４実施形態を適用可能である。
第４実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第４実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第４実施形態では、数式モデルを以下のように拡張する。
アクティブセンサ１１２によるドップラー効果を測定することで、対象の速度のうちセンサ方向の速度成分を測定することができる。これを測定値の１つに加える拡張を行うことで、状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。そこで、第４実施形態では、対象の速度のうちセンサ方向の速度成分の測定値を観測値ベクトルに含める拡張を行う。第１実施形態〜第３実施形態のいずれにも第４実施形態を適用可能である。
第４実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第４実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第４実施形態では、数式モデルを以下のように拡張する。
アクティブセンサ１１２によるドップラー効果は、対象物２０の速度のうち測定部１１０の方向の速度成分に現れる。タイミングｋにおける対象物２０の速度のうち測定部１１０の方向の速度成分ｖｄ（ｋ）は、式（３８）のように示される。
ここで、ｖ（ｋ）は、タイミングｋにおける対象の速度を示す。φ（ｋ）は、対象の進行方向をｘ軸に対する角度で示す。
推定すべき状態（運動パラメータベクトルｑ（ｋ）の成分）を用いて、式（３８）は式（３９）のように示される。
推定すべき状態（運動パラメータベクトルｑ（ｋ）の成分）を用いて、式（３８）は式（３９）のように示される。
式（３９）を、アクティブセンシング時の観測方程式の１つとして追加する。
以上のように、第４実施形態では、観測方程式は、アクティブセンサ１１２におけるドップラー効果に基づく対象物２０の速度の測定値の方程式を含む。
対象物２０の運動の測定値が増える点で、第４実施形態における運動パラメータ推定装置１００によれば、対象物２０の運動の推定精度をより高めることができる。
対象物２０の運動の測定値が増える点で、第４実施形態における運動パラメータ推定装置１００によれば、対象物２０の運動の推定精度をより高めることができる。
＜第５実施形態＞
図５に示される処理では、ステップＳ１０３で常にパッシブセンシングによる測定値（センサから見た対象の方位）を用いている。このため、ステップＳ１０４では、常にパッシブセンシングによる測定値を用いて誤差を算出している。これに対し、アクティブセンシング時には対象の方位のみならず距離の測定値も用いることで、状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。
そこで、第５実施形態では、アクティブセンシング時とパッシブセンシング時とで誤差計測の方法を切り替える。アクティブセンシング時には、対象の方位のみならず距離の測定値も用いる。具体的には、図５のステップＳ１０３で、アクティブセンシング時には、式（６）に示される観測方程式を用いる。
一方、パッシブセンシング時は、式（９）に示される観測方程式を用いる。
第５実施形態は、第１実施形態〜第４実施形態のいずれにも第５実施形態を適用可能である。
図５に示される処理では、ステップＳ１０３で常にパッシブセンシングによる測定値（センサから見た対象の方位）を用いている。このため、ステップＳ１０４では、常にパッシブセンシングによる測定値を用いて誤差を算出している。これに対し、アクティブセンシング時には対象の方位のみならず距離の測定値も用いることで、状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。
そこで、第５実施形態では、アクティブセンシング時とパッシブセンシング時とで誤差計測の方法を切り替える。アクティブセンシング時には、対象の方位のみならず距離の測定値も用いる。具体的には、図５のステップＳ１０３で、アクティブセンシング時には、式（６）に示される観測方程式を用いる。
一方、パッシブセンシング時は、式（９）に示される観測方程式を用いる。
第５実施形態は、第１実施形態〜第４実施形態のいずれにも第５実施形態を適用可能である。
以上のように、第一推定部１９３は、アクティブセンサ１１２、パッシブセンサ１１１それぞれの観測方程式を用いて測定部１１０の測定値の推定値を算出する。
第一推定部１９３が対象物２０の方向のみならず対象物２０の位置に基づいて測定部１１０の測定値の推定値を算出することで、第一推定部１９３が算出する推定値の精度が向上し、対象物２０の運動の推定精度をより高められることが期待される。
第一推定部１９３が対象物２０の方向のみならず対象物２０の位置に基づいて測定部１１０の測定値の推定値を算出することで、第一推定部１９３が算出する推定値の精度が向上し、対象物２０の運動の推定精度をより高められることが期待される。
なお、制御部１９０の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間の間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。
また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間の間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計変更等も含まれる。
１ 測定系
１０ 移動体
２０ 対象物
１００ 運動パラメータ推定装置
１１０ 測定部
１１１ パッシブセンサ
１１２ アクティブセンサ
１８０ 記憶部
１９０ 制御部
１９１ 駆動雑音サンプル設定部
１９２ 粒子算出部
１９３ 第一推定部
１９４ 選択部
１９５ 補正部
１９６ 第二推定部
１０ 移動体
２０ 対象物
１００ 運動パラメータ推定装置
１１０ 測定部
１１１ パッシブセンサ
１１２ アクティブセンサ
１８０ 記憶部
１９０ 制御部
１９１ 駆動雑音サンプル設定部
１９２ 粒子算出部
１９３ 第一推定部
１９４ 選択部
１９５ 補正部
１９６ 第二推定部
Claims (7)
- アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定部と、
前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定部と、
前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出部と、
前記粒子算出部が算出した粒子の各々を観測方程式に代入して、前記測定部の測定値の推定値を算出する第一推定部と、
前記測定部の測定値と前記第一推定部の推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択部と、
前記選択部が選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定部と、
を備える運動パラメータ推定装置。 - 前記状態方程式は前記対象物の運動に加えて前記運動パラメータ推定装置の運動を示し、
前記観測方程式は、前記対象物の運動の測定値の方程式に加えて前記運動パラメータ推定装置の運動の測定値の方程式を含み、
前記第二推定部は、前記対象物の運動および前記運動パラメータ推定装置の運動を示す前記運動パラメータの推定値を算出する、
請求項１に記載の運動パラメータ推定装置。 - 前記観測方程式は、前記運動パラメータ推定装置の速度の測定値の方程式を含む、
請求項２に記載の運動パラメータ推定装置。 - 前記観測方程式は、前記アクティブセンサにおけるドップラー効果に基づく前記対象物の速度の測定値の方程式を含む、
請求項１から３のいずれか一項に記載の運動パラメータ推定装置。 - 前記第一推定部は、前記アクティブセンサ、前記パッシブセンサそれぞれの観測方程式を用いて前記測定部の測定値の推定値を算出する、
請求項１から４のいずれか一項に記載の運動パラメータ推定装置。 - アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、
前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、
前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、
前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、
前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、
前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、
を含む運動パラメータ推定方法。 - コンピュータに、
アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、
前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、
前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、
前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、
前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、
前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、
を実行させるためのプログラム。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2016200719A JP6757227B2 (ja) | 2016-10-12 | 2016-10-12 | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2016200719A JP6757227B2 (ja) | 2016-10-12 | 2016-10-12 | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2018063142A true JP2018063142A (ja) | 2018-04-19 |
| JP6757227B2 JP6757227B2 (ja) | 2020-09-16 |
Family
ID=61967725
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2016200719A Active JP6757227B2 (ja) | 2016-10-12 | 2016-10-12 | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6757227B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112068089A (zh) * | 2020-08-13 | 2020-12-11 | 中国人民解放军海军工程大学 | 一种基于粒子滤波的序列检索方法 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08211146A (ja) * | 1995-02-02 | 1996-08-20 | Mitsubishi Heavy Ind Ltd | レーダ装置 |
| JP2002341024A (ja) * | 2001-05-17 | 2002-11-27 | Mitsubishi Electric Corp | 多目標追尾装置 |
| JP2005274300A (ja) * | 2004-03-24 | 2005-10-06 | Mitsubishi Electric Corp | 目標追尾装置 |
| JP2010054284A (ja) * | 2008-08-27 | 2010-03-11 | Toshiba Corp | 複合センサ装置 |
| JP2011520097A (ja) * | 2008-04-14 | 2011-07-14 | モジクス， インコーポレイティッド | 無線自動識別タグの位置を推定及び追跡するシステム並びに方法 |
| JP2012177602A (ja) * | 2011-02-25 | 2012-09-13 | Toshiba Corp | 目標追跡装置 |
| JP2013231636A (ja) * | 2012-04-27 | 2013-11-14 | Mitsubishi Electric Corp | クロック番号・時刻対応付回路、指定クロック時刻生成回路、イベント実施指示・時間差分生成回路、イベント実施装置、レーダ装置及び通信装置 |
| JP2014169865A (ja) * | 2013-03-01 | 2014-09-18 | Hitachi Ltd | 目標トラッキング装置、目標トラッキングプログラム及び目標トラッキング方法 |
| US20140372073A1 (en) * | 2011-05-04 | 2014-12-18 | Jacques Georgy | Two-stage filtering based method for multiple target tracking |
-
2016
- 2016-10-12 JP JP2016200719A patent/JP6757227B2/ja active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08211146A (ja) * | 1995-02-02 | 1996-08-20 | Mitsubishi Heavy Ind Ltd | レーダ装置 |
| JP2002341024A (ja) * | 2001-05-17 | 2002-11-27 | Mitsubishi Electric Corp | 多目標追尾装置 |
| JP2005274300A (ja) * | 2004-03-24 | 2005-10-06 | Mitsubishi Electric Corp | 目標追尾装置 |
| JP2011520097A (ja) * | 2008-04-14 | 2011-07-14 | モジクス， インコーポレイティッド | 無線自動識別タグの位置を推定及び追跡するシステム並びに方法 |
| JP2017122735A (ja) * | 2008-04-14 | 2017-07-13 | モジクス， インコーポレイティッド | 無線自動識別タグの位置を推定及び追跡するシステム並びに方法 |
| JP2010054284A (ja) * | 2008-08-27 | 2010-03-11 | Toshiba Corp | 複合センサ装置 |
| JP2012177602A (ja) * | 2011-02-25 | 2012-09-13 | Toshiba Corp | 目標追跡装置 |
| US20140372073A1 (en) * | 2011-05-04 | 2014-12-18 | Jacques Georgy | Two-stage filtering based method for multiple target tracking |
| JP2013231636A (ja) * | 2012-04-27 | 2013-11-14 | Mitsubishi Electric Corp | クロック番号・時刻対応付回路、指定クロック時刻生成回路、イベント実施指示・時間差分生成回路、イベント実施装置、レーダ装置及び通信装置 |
| JP2014169865A (ja) * | 2013-03-01 | 2014-09-18 | Hitachi Ltd | 目標トラッキング装置、目標トラッキングプログラム及び目標トラッキング方法 |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112068089A (zh) * | 2020-08-13 | 2020-12-11 | 中国人民解放军海军工程大学 | 一种基于粒子滤波的序列检索方法 |
| CN112068089B (zh) * | 2020-08-13 | 2023-07-21 | 中国人民解放军海军工程大学 | 一种基于粒子滤波的序列检索方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP6757227B2 (ja) | 2020-09-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101914550B1 (ko) | 레이더의 표적 위치 추적 방법 | |
| KR20170042260A (ko) | 목표의 실시간 위치 측정과 지도 구축 방법 및 장치 | |
| Chen et al. | Towards autonomous localization and mapping of AUVs: a survey | |
| US8816896B2 (en) | On-board INS quadratic correction method using maximum likelihood motion estimation of ground scatterers from radar data | |
| CN110889808A (zh) | 一种定位的方法、装置、设备及存储介质 | |
| Henson et al. | Attitude-trajectory estimation for forward-looking multibeam sonar based on acoustic image registration | |
| CN104182609B (zh) | 基于去相关的无偏转换量测的三维目标跟踪方法 | |
| US20140358434A1 (en) | Peer-Assisted Dead Reckoning | |
| CN109643116A (zh) | 用于定位移动物体的系统和方法 | |
| CN110132281B (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
| Odom et al. | Passive towed array shape estimation using heading and acoustic data | |
| CN110345936B (zh) | 运动装置的轨迹数据处理方法及其处理系统 | |
| RU2611564C1 (ru) | Способ навигации летательных аппаратов | |
| Masmitja et al. | Range-only single-beacon tracking of underwater targets from an autonomous vehicle: From theory to practice | |
| JP2020063958A (ja) | 位置推定装置及び方法 | |
| KR101833007B1 (ko) | 도플러 비컨을 이용한 수중 이동체의 위치 및 속도 추정 방법 및 시스템 | |
| Zhu et al. | Kalman-based underwater tracking with unknown effective sound velocity | |
| Hu et al. | 1d-lrf aided visual-inertial odometry for high-altitude mav flight | |
| JP6757227B2 (ja) | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム | |
| Bikmaev et al. | Improving the accuracy of supporting mobile objects with the use of the algorithm of complex processing of signals with a monocular camera and LiDAR | |
| CN112666519A (zh) | 一种基于广义二阶时延差的水下目标高精度定位方法 | |
| CN116929343A (zh) | 位姿估计方法、相关设备及存储介质 | |
| KR101480834B1 (ko) | 다중 경로 음파 전달 모델 및 표적 식별을 이용한 표적 기동분석 방법 | |
| Kavitha et al. | Shifted Rayleigh filter: A novel estimation filtering algorithm for pervasive underwater passive target tracking for computation in 3D by bearing and elevation measurements | |
| Garapati Vaishnavi et al. | Underwater bearings-only tracking using particle filter |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A821 Effective date: 20161013 |
|
| RD03 | Notification of appointment of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7423 Effective date: 20181109 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20190712 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20200630 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20200818 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20200828 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6757227 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |