(57)【要約】【課題】アクティブセンサとパッシブセンサとの組み合わせにより、対象物の運動の推定精度を高められるようにする。【解決手段】測定部は、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いてアクティブセンサでの測定周期よりも短い周期で対象物の方向を測定する。駆動雑音サンプル設定部は、対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する。粒子算出部は、駆動雑音のサンプルの各々を状態方程式に代入して、粒子フィルタにおける粒子を複数算出する。第一推定部は、粒子算出部が算出した粒子の各々を観測方程式に代入して、測定部の測定値の推定値を算出する。選択部は、測定部の測定値と第一推定部の推定値との誤差に基づいて、複数の粒子のうち一部の粒子を選択する。第二推定部は、選択部が選択した粒子に基づいて、対象物の運動を示す運動パラメータの推定値を算出する。【選択図】図１
2018063142.tif 000002
【特許請求の範囲】
【請求項１】
アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定部と、
前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定部と、
前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出部と、
前記粒子算出部が算出した粒子の各々を観測方程式に代入して、前記測定部の測定値の推定値を算出する第一推定部と、
前記測定部の測定値と前記第一推定部の推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択部と、
前記選択部が選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定部と、
を備える運動パラメータ推定装置。
【請求項２】
前記状態方程式は前記対象物の運動に加えて前記運動パラメータ推定装置の運動を示し、
前記観測方程式は、前記対象物の運動の測定値の方程式に加えて前記運動パラメータ推定装置の運動の測定値の方程式を含み、
前記第二推定部は、前記対象物の運動および前記運動パラメータ推定装置の運動を示す前記運動パラメータの推定値を算出する、
請求項１に記載の運動パラメータ推定装置。
【請求項３】
前記観測方程式は、前記運動パラメータ推定装置の速度の測定値の方程式を含む、
請求項２に記載の運動パラメータ推定装置。
【請求項４】
前記観測方程式は、前記アクティブセンサにおけるドップラー効果に基づく前記対象物の速度の測定値の方程式を含む、
請求項１から３のいずれか一項に記載の運動パラメータ推定装置。
【請求項５】
前記第一推定部は、前記アクティブセンサ、前記パッシブセンサそれぞれの観測方程式を用いて前記測定部の測定値の推定値を算出する、
請求項１から４のいずれか一項に記載の運動パラメータ推定装置。
【請求項６】
アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、
前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、
前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、
前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、
前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、
前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、
を含む運動パラメータ推定方法。
【請求項７】
コンピュータに、
アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、
前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、
前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、
前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、
前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、
前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、
を実行させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、運動パラメータ推定装置、運動パラメータ推定方法及びプログラムに関する。
【背景技術】
【０００２】
ソナー等のセンサを用いて対象物の位置を測定するための幾つかの技術が提案されている。例えば、特許文献１に記載の技術では、パッシブ方式のソナー使用時に、別の周波数でアクティブ方式のソナーを使用する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特公平０３−０６０３９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
アクティブセンサとパッシブセンサとの組み合わせにより、それぞれのセンサを単独で用いる場合よりも対象物の運動の推定精度を高められることが好ましい。これに対し、特許文献１では、アクティブ方式での測定結果とパッシブ方式での測定結果とを組み合わせて対象物の運動の推定精度を高める具体的方法は示されていない。
【０００５】
本発明は、アクティブセンサとパッシブセンサとの組み合わせにより、対象物の運動の推定精度を高めることができる運動パラメータ推定装置、運動パラメータ推定方法及びプログラムを提供する。
【課題を解決するための手段】
【０００６】
本発明の第１の態様によれば、運動パラメータ推定装置は、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定部と、前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定部と、前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出部と、前記粒子算出部が算出した粒子の各々を観測方程式に代入して、前記測定部の測定値の推定値を算出する第一推定部と、前記測定部の測定値と前記第一推定部の推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択部と、前記選択部が選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定部と、を備える。
【０００７】
前記状態方程式は前記対象物の運動に加えて前記運動パラメータ推定装置の運動を示し、前記観測方程式は、前記対象物の運動の測定値の方程式に加えて前記運動パラメータ推定装置の運動の測定値の方程式を含み、前記第二推定部は、前記対象物の運動および前記運動パラメータ推定装置の運動を示す前記運動パラメータの推定値を算出するようにしてもよい。
【０００８】
前記観測方程式は、前記運動パラメータ推定装置の速度の測定値の方程式を含むようにしてもよい。
【０００９】
前記観測方程式は、前記アクティブセンサにおけるドップラー効果に基づく前記対象物の速度の測定値の方程式を含むようにしてもよい。
【００１０】
前記第一推定部は、前記アクティブセンサ、前記パッシブセンサそれぞれの観測方程式を用いて前記測定部の測定値の推定値を算出するようにしてもよい。
【００１１】
本発明の第二の態様によれば、運動パラメータ推定方法は、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、を含む。
【００１２】
本発明の第三の態様によれば、プログラムは、コンピュータに、アクティブセンサを用いて対象物の位置を測定し、パッシブセンサを用いて前記アクティブセンサでの測定周期よりも短い周期で前記対象物の方向を測定する測定ステップと、前記対象物の運動を示す状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する駆動雑音サンプル設定ステップと、前記駆動雑音のサンプルの各々を前記状態方程式に代入して、粒子フィルタにおける粒子を複数算出する粒子算出ステップと、前記粒子算出ステップで算出した粒子の各々を観測方程式に代入して、前記測定ステップでの測定値の推定値を算出する第一推定ステップと、前記測定ステップでの測定値と前記第一推定ステップでの推定値との誤差に基づいて、複数の前記粒子のうち一部の粒子を選択する選択ステップと、前記選択ステップで選択した粒子に基づいて、前記対象物の運動を示す運動パラメータの推定値を算出する第二推定ステップと、を実行させるためのプログラムである。
【発明の効果】
【００１３】
上記した運動パラメータ推定装置、運動パラメータ推定方法及びプログラムによれば、できる。アクティブセンサとパッシブセンサとの組み合わせにより、対象物の運動の推定精度を高めることができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の第１実施形態に係る運動パラメータ推定装置の機能構成を示す概略ブロック図である。
【図２】本発明の第１実施形態に係る運動パラメータ推定装置が測定を行う測定系の構成例を示す図である。
【図３】本発明の第１実施形態に係るパッシブセンサによる対象物の方向の測定例を示す図である。
【図４】本発明の第１実施形態に係るアクティブセンサによる対象物の位置の測定例を示す図である。
【図５】本発明の第１実施形態に係る運動パラメータ推定装置が行う処理の手順の例を示すフローチャートである。
【図６】本発明の第１実施形態に係る運動パラメータ推定装置による運動パラメータの推定値の真値付近への収束例を示す図である。
【発明を実施するための形態】
【００１５】
以下、本発明の実施形態を説明するが、以下の実施形態は特許請求の範囲にかかる発明を限定するものではない。また、実施形態の中で説明されている特徴の組み合わせの全てが発明の解決手段に必須であるとは限らない。
【００１６】
＜第１実施形態＞
図１は、本発明の第１実施形態に係る運動パラメータ推定装置の機能構成を示す概略ブロック図である。同図に示すように、運動パラメータ推定装置１００は、測定部１１０と、記憶部１８０と、制御部１９０とを備える。測定部１１０は、パッシブセンサ１１１と、アクティブセンサ１１２とを備える。制御部１９０は、駆動雑音サンプル設定部１９１と、粒子算出部１９２と、第一推定部１９３と、選択部１９４と、補正部１９５と、第二推定部１９６とを備える。また、運動パラメータ推定装置１００は、移動体１０に搭載されている。
【００１７】
運動パラメータ推定装置１００は、対象物の運動を示す運動パラメータの値を推定する。運動パラメータ推定装置１００は、例えばセンサを備えたコンピュータを用いて構成される。
図２は、運動パラメータ推定装置１００が測定を行う測定系の構成例を示す図である。図２に示すように、測定系１は、運動パラメータ推定装置１００を搭載した移動体１０と、運動パラメータ推定装置１００が運動を測定する対象である対象物２０とを含む。
線Ｌ１１は、移動体１０の軌跡の例を示す。線Ｌ１２は、対象物２０の軌跡の例を示す。矢印Ｂ１１は、移動体１０の速度ベクトルの例を示す。矢印Ｂ１２は、対象物２０の速度ベクトルの例を示す。
【００１８】
運動パラメータ推定装置１００は、移動するいろいろな物を測定対象（運動パラメータ推定対象）とすることができる。例えば、運動パラメータ推定装置１００の測定対象領域は海中等の水中であってもよく、対象物２０は、水中航走体であってもよい。あるいは、運動パラメータ推定装置１００の測定対象領域は海上などの水面上であってもよく、対象物２０は船舶など水面上を移動する物であってもよい。あるいは、運動パラメータ推定装置１００の測定対象領域は陸上（地面）であってもよく、対象物２０は、車両など陸上を走行する物であってもよい。あるいは、運動パラメータ推定装置１００の測定対象領域は空中であってもよく、対象物２０は、飛行機など空中を移動する物であってもよい。あるいは、運動パラメータ推定装置１００の測定対象は宇宙であってもよく、対象物２０は宇宙船など宇宙を移動する物であってもよい。
【００１９】
対象物２０と同様、移動体１０も、いろいろな物とすることができる。例えば、移動体１０は、水中航走体であってもよい。あるいは、移動体１０は、船舶など水面上を移動する物であってもよい。あるいは、移動体１０は、車両など陸上を走行する物であってもよい。あるいは、移動体１０は、飛行機など空中を移動する物であってもよい。あるいは、移動体１０は、宇宙船など宇宙を移動する物であってもよい。
なお、以下では、運動パラメータ推定装置１００が移動体１０に搭載されて移動する場合を例に説明するが、運動パラメータ推定装置１００が固定的に設置されていてもよい。例えば、運動パラメータ推定装置１００が移動体１０に代えて建物内など移動しない構造物に設けられていてもよい。
【００２０】
また、以下では、運動パラメータ推定装置１００の測定対象領域が２次元の領域であり、運動パラメータ推定装置１００がｘｙ座標（２次元直交座標）を用いる場合を例に説明するが、運動パラメータ推定装置１００の測定対象領域は３次元であってもよい。運動パラメータ推定装置１００の測定対象領域が３次元の領域である場合、運動パラメータ推定装置１００がｘｙｚ座標（３次元直交座標）を用いるようにしてもよい。
運動パラメータ推定装置１００の測定対象領域が３次元の領域である場合、運動パラメータ推定装置１００は、以下に説明するｘ座標及びｙ座標に関する処理をｚ座標についても行う。また、運動パラメータ推定装置１００は、以下に説明する２次元の角又は方向に関する処理と同様に、３次元の角又は方向について処理を行う。
【００２１】
測定部１１０は、運動パラメータを測定する。
パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した物理的エネルギーを測定することにより、測定部１１０の位置（移動体１０の位置）から見た対象物２０の方向を測定する。対象物２０の方向は、運動パラメータの１つである。
【００２２】
図３は、パッシブセンサ１１１による対象物２０の方向の測定例を示す図である。図３の例で、対象物２０からパッシブセンサ１１１に到達した物理的エネルギーを矢印Ｂ２１で示している。
例えば、パッシブセンサ１１１がパッシブソナーである場合、パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した音を測定する。この音は、対象物２０が発した音であってもよいし、対象物２０以外の物が発した音を対象物２０が反射した音であってもよい。
あるいは、パッシブセンサ１１１がパッシブなレーダーである場合、パッシブセンサ１１１は、対象物２０からパッシブセンサ１１１に到達した電磁波を測定する。この電磁波は、対象物２０が発した電磁波であってもよいし、対象物２０以外の物が発した電磁波を対象物２０が反射した電磁波であってもよい。
【００２３】
アクティブセンサ１１２は、対象物２０に向けて物理的エネルギーを出力し、対象物２０で反射してパッシブセンサ１１１に到達した物理的エネルギーを測定することにより、対象物２０の位置を測定する。対象物２０の位置は、運動パラメータの１つである。
【００２４】
図４は、アクティブセンサ１１２による対象物２０の位置の測定例を示す図である。図４の例で、アクティブセンサ１１２が対象物２０に向けて出力した物理的エネルギーを矢印Ｂ３１で示している。また、対象物２０で反射してパッシブセンサ１１１に到達した物理的エネルギーを矢印Ｂ３２で示している。
例えば、アクティブセンサ１１２がアクティブソナーである場合、アクティブセンサ１１２は、対象物２０に向けて音（音波）を出力し、対象物２０で反射してアクティブセンサ１１２に到達した音を測定する。
あるいは、アクティブセンサ１１２がアクティブなレーダーである場合、アクティブセンサ１１２は、対象物２０に向けて電磁波を出力し、対象物２０で反射してアクティブセンサ１１２に到達した電磁波を測定する。
アクティブセンサ１１２は、アクティブセンサ１１２の位置（移動体１０の位置）に対する対象物２０の相対位置を測定する。アクティブセンサ１１２が、この相対位置とアクティブセンサ１１２自らの位置とに基づいて、アクティブセンサ１１２の位置以外の位置を基準とした対象物２０の位置を求めるようにしてもよい。
【００２５】
記憶部１８０は、運動パラメータ推定装置１００が備える記憶デバイスを用いて構成され、各種情報を記憶する。
制御部１９０は、運動パラメータ推定装置１００の各部を制御して各種処理を実行する。制御部１９０は、例えば運動パラメータ推定装置１００が備えるＣＰＵ（Central Processing Unit、中央処理装置）が、記憶部１８０からプログラムを読み出して実行することで構成される。
【００２６】
制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いて対象物２０の運動パラメータを推定する。制御部１９０は、例えば対象物２０の位置を推定する。
ここで、パッシブセンサ１１１は、測定部１１０の位置から見た対象物２０の方向を測定することはできるが、測定部１１０と対象物２０との相対距離を測定することはできない。このため、パッシブセンサ１１１の測定結果のみを用いたのでは、対象物２０の位置を高精度に推定することは困難である。
これに対し、制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いることで、パッシブセンサ１１１の測定結果のみを用いる場合よりも高精度に対象物２０の位置を測定することができる。
【００２７】
また、アクティブセンサ１１２は、対象物２０に対して物理的エネルギーを出力した後、対象物２０で反射した物理的エネルギーの到達を待ち受ける。このため、アクティブセンサ１１２の測定結果のみを用いたのでは、サンプリング周期をある程度大きくする必要がある。サンプリング周期がある程度大きくなることで、例えば運動パラメータの初期値が未知である場合など、運動パラメータの推定値が真値に近い値に収束するまでに時間を要する可能性がある。
これに対し、制御部１９０は、パッシブセンサ１１１による測定結果とアクティブセンサ１１２による測定結果とを用いることで、アクティブセンサ１１２の測定結果のみを用いる場合よりも速やかに、運動パラメータの推定値を真値に近い値に収束させることができる。
【００２８】
駆動雑音サンプル設定部１９１は、駆動雑音のサンプルを設定する。ここでいう駆動雑音は、対象物２０の運動モデルを用いて推定される対象物２０の運動を、対象物２０の実際の運動に合わせるためのパラメータである。制御部１９０は、対象物２０の運動モデルとして対象物２０の状態方程式を用いており、状態方程式に駆動雑音が含まれている。
制御部１９０が用いる対象物２０の状態方程式は、例えば式（１）のように定義される。
【００２９】
【数１】
2018063142.tif 000003
【００３０】
ここで、ｋはパッシブセンサ１１１のサンプリングタイミングのインデックスを示す。具体的には、時刻０を基準時刻として、ｋは、パッシブセンサ１１１が基準時刻よりも後に行ったサンプリングの回数を示す。パッシブセンサ１１１のサンプリング周期をＴＳで表すと、ｋ回目のサンプリング時刻はｋＴＳと表される。
以下では、パッシブセンサによる基準時刻よりも後のｋ回目のサンプリングのタイミング（すなわち、インデックスｋで示されるサンプリングタイミング）を、タイミングｋと称する。
【００３１】
運動パラメータベクトルｑ（ｋ）は、タイミングｋにおける運動パラメータを示す。特に、運動パラメータベクトルｑ（ｋ）は、タイミングｋにおける対象の位置、速度及び加速度を示す。運動パラメータベクトルｑ（ｋ）は、式（２）のように示される。
【００３２】
【数２】
2018063142.tif 000004
【００３３】
ここで、ｘ（ｋ）、ｙ（ｋ）は、それぞれタイミングｋにおける対象の位置のｘ座標値、ｙ座標値を示す。ｘ’（ｋ）、ｙ’（ｋ）は、それぞれタイミングｋにおける対象の速度のｘ座標値、ｙ座標値を示す。１階微分を’で示している。ｘ’’（ｋ）、ｙ’’（ｋ）は、それぞれタイミングｋにおける対象の加速度のｘ座標値、ｙ座標値を示す。２階微分を’’で示している。
行列又はベクトルの右に上付きで示されたＴは、行列の転置又はベクトルの転置を示す。
【００３４】
駆動雑音ベクトルｕ（ｋ）は、タイミングｋにおける駆動雑音を示す。駆動雑音ベクトルｕ（ｋ）は、２つのスカラ変数ｕ１（ｋ）及びｕ２（ｋ）を用いて式（３）のように示される。
【００３５】
【数３】
2018063142.tif 000005
【００３６】
式（１）の行列Ａは、係数を示す行列である。行列Ａは、式（４）のように示される。
【００３７】
【数４】
2018063142.tif 000006
【００３８】
上記のようにＴＳは、パッシブセンサ１１１のサンプリング周期を示す。
行列Ｂは、係数を示す行列である。行列Ｂは、式（５）のように示される。
【００３９】
【数５】
2018063142.tif 000007
【００４０】
式（１）に示される状態方程式は、タイミングｋにおける運動パラメータの値（対象の位置、速度、加速度）及び駆動雑音の値と、タイミングｋ＋１における運動パラメータの値（対象の位置、速度及び加速度）との関係を示している。
【００４１】
駆動雑音サンプル設定部１９１は、駆動雑音のサンプルを所定の確率分布に基づいて複数設定する。ここで、制御部１９０は、粒子フィルタを用いて運動パラメータの推定値を算出する。駆動雑音サンプル設定部１９１は、粒子フィルタにおける粒子数だけ駆動雑音を設定する。
駆動雑音サンプル設定部１９１が駆動雑音のサンプルの設定に用いる確率分布は特定の確率分布に限定されない。例えば、駆動雑音サンプル設定部１９１がガウス分布に基づいて駆動雑音の設定を行うようにしてもよい。あるいは、駆動雑音サンプル設定部１９１がガウス分布以外の分布に基づいて駆動雑音の設定を行うようにしてもよい。
【００４２】
粒子算出部１９２は、駆動雑音サンプル設定部１９１が設定した駆動雑音のサンプルの各々を状態方程式に代入して粒子フィルタにおける粒子を複数算出する。粒子算出部１９２は、タイミングｋ−１における運動パラメータの値及び駆動雑音の値を状態方程式に代入して、タイミングｋにおける運動パラメータの値を、粒子フィルタにおける粒子として算出する。
【００４３】
第一推定部１９３は、粒子算出部１９２が算出した粒子の各々を観測方程式に代入して運動パラメータの測定値の推定値を算出する。ここでいう観測方程式は運動パラメータから測定部１１０の測定値を推定するための方程式であり、運動パラメータの値に観測誤差（測定雑音）を付加する。
アクティブセンサ１１２における観測方程式は、例えば式（６）のように定義される。
【００４４】
【数６】
2018063142.tif 000008
【００４５】
ここで、ベクトルｍ（ｋ）は、タイミングｋにおける運動パラメータの測定値を示す。アクティブセンサ１１２の場合、ベクトルｍ（ｋ）は、式（７）のように示される。
【００４６】
【数７】
2018063142.tif 000009
【００４７】
ここで、ｘｍ（ｋ）は、タイミングｋにおける対象の位置のｘ座標値の測定値を示す。ｙｍ（ｋ）は、タイミングｋにおける対象の位置のｙ座標値の測定値を示す。
【００４８】
式（６）の行列Ｃは、係数を示す行列である。行列Ｃは、式（８）のように示される。
【００４９】
【数８】
2018063142.tif 000010
【００５０】
また、式（６）のｗＡ（ｋ）は、タイミングｋにおけるアクティブセンサ１１２の観測誤差を示す。
一方、パッシブセンサ１１１における観測方程式は、例えば式（９）のように定義される。
【００５１】
【数９】
2018063142.tif 000011
【００５２】
ここで、ａｔａｎは、アークタンジェントを示す。ｘＯ（ｋ）は、タイミングｋにおける測定部１１０の位置のｘ座標値を示す。ｙＯ（ｋ）は、タイミングｋにおける測定部１１０の位置のｙ座標値を示す。
ｗＰ（ｋ）は、タイミングｋにおけるパッシブセンサ１１１の観測誤差を示す。
また、パッシブセンサ１１１の場合、ｍ（ｋ）は式（１０）のように示される。
【００５３】
【数１０】
2018063142.tif 000012
【００５４】
ここで、θｍ（ｋ）は、タイミングｋにおける測定部１１０の位置に対する対象の方位の測定値（測角計測値）を示す。
【００５５】
選択部１９４は、測定部１１０による運動パラメータの測定値と、第一推定部１９３が算出した運動パラメータの測定値の推定値との誤差に基づいて、複数の粒子のうち一部の粒子を選択する。具体的には、選択部１９４は、誤差の逆数から粒子の尤度を算出し、尤度が高い順に所定個の粒子を選択する。
【００５６】
補正部１９５は、選択部１９４が選択した粒子の各々に拡張カルマンフィルタを適用して、粒子の各々を補正する。補正部１９５は、各粒子の尤度を高めるように補正を行う。
第二推定部１９６は、補正部１９５による補正後の粒子に基づいて、運動パラメータの推定値を求める。さらに、第二推定部１９６は、運動パラメータに示される対象物２０の位置及び速度に基づいて、測定部１１０と対象物２０との間の距離、測定部１１０の位置から見た対象物２０の方向、及び、対象物２０の速度の大きさを算出する。
【００５７】
次に、図５を参照して運動パラメータ推定装置１００の動作について説明する。図５は、運動パラメータ推定装置１００が行う処理の手順の例を示すフローチャートである。運動パラメータ推定装置１００は、パッシブセンサ１１１のサンプリングタイミング毎に、図５のステップＳ１０１〜Ｓ１１０の処理を繰り返す。
ここで、アクティブセンサ１１２では、対象物２０に向けて出力した物理的エネルギーが反射してアクティブセンサ１１２に戻ってくるまで待つ必要がある。これに対し、パッシブセンサ１１１では、アクティブセンサ１１２の場合のような待ち時間の制約はない。このため、パッシブセンサ１１１のサンプリング周期はアクティブセンサ１１２のサンプリング周期よりも短く設定できる。
そこで、運動パラメータ推定装置１００は、アクティブセンサ１１２のサンプリング周期よりもパッシブセンサ１１１のサンプリング周期を短く設定して運動パラメータの測定を行う。具体的には、パッシブセンサ１１１が運動パラメータをｎ回（ｎは正整数）測定する毎に、アクティブセンサ１１２が運動パラメータを１回測定する。これにより、パッシブセンサ１１１とアクティブセンサ１１２とが運動パラメータを測定するサンプリングタイミングと、パッシブセンサ１１１のみが運動パラメータを測定するサンプリングタイミングとがある。
【００５８】
（ステップＳ１０１）
駆動雑音サンプル設定部１９１は、駆動雑音（駆動雑音ベクトルｕ（ｋ））のサンプルを粒子数Ｐだけ生成する。以下、タイミングｋにおける駆動雑音のサンプルをｕｐ（ｋ）と表記する。
ステップＳ１０１の後、ステップＳ１０２へ遷移する。
【００５９】
（ステップＳ１０２）
粒子算出部１９２は、ステップＳ１０１で生成したＰ個のサンプル（ベクトルｕｐ（ｋ））の各々を状態方程式（式（１））に代入し、Ｐ個の粒子を算出する。以下では、タイミングｋにおける粒子をｑｐ（ｋ）と表記する。
ステップＳ１０２の後、ステップＳ１０３へ遷移する。
【００６０】
（ステップＳ１０３）
第一推定部１９３は、ステップＳ１０２で得られた粒子の各々を観測方程式に代入し、測定部１１０による測定値の推定値を算出する。上記のように、アクティブセンサ１１２の場合の観測方程式（式（６））と、パッシブセンサ１１１の場合の観測方程式（式（９））とがある。ここでは、第一推定部１９３は、パッシブセンサ１１１の場合の観測方程式（式（９））を用いて、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値を求める。以下では、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値をθｐ（ｋ）と表記する。
なお、観測誤差ＷＡ（ｋ）については、例えば、センサ（パッシブセンサ１１１又はアクティブセンサ１１２）の特性、及び、実環境（測定を行う環境）に基づいて、観測誤差が従う確率分布及びそのパラメータを予め設定しておく。そして、設定した確率分布及びパラメータに基づいて乱数を生成することで、各粒子に対応する観測誤差を生成することができる。
ステップＳ１０３の後、ステップＳ１０４へ遷移する。
【００６１】
（ステップＳ１０４）
選択部１９４は、ステップＳ１０３で得られた測定値の推定値と、測定値との誤差を算出する。
ここでは、選択部１９４は、タイミングｋにおけるセンサに対する対象の方位の測定値の推定値θｐ（ｋ）から、タイミングｋにおけるセンサに対する対象の方位の測定値θｍ（ｋ）を減算した差を誤差として算出する。
以下では、ステップＳ１０３で得られた測定値の推定値と、測定部１１０による測定値との誤差をｅｐ（ｋ）と表記する。
ステップＳ１０４の後、ステップＳ１０５へ遷移する。
【００６２】
（ステップＳ１０５）
選択部１９４は、ステップＳ１０４で算出した誤差ｅｐ（ｋ）の逆数ｅｐ（ｋ）−１から、タイミングｋにおける尤度πｐ（ｋ）を算出する。選択部１９４は、誤差の逆数ｅｐ（ｋ）−１に対して総和が１になるように正規化を行う。タイミングｋにおける尤度πｐ（ｋ）は、式（１１）のように示される。
【００６３】
【数１１】
2018063142.tif 000013
【００６４】
ステップＳ１０５の後、ステップＳ１０６へ遷移する。
【００６５】
（ステップＳ１０６）
選択部１９４は、尤度の高い粒子（尤度πｐ（ｋ）の値が大きい粒子）から順にＰ個（Ｐは正整数）の粒子を選択（リサンプリング）する。ここで、Ｐは予め定められている定数である。
選択部１９４は、選択したＰ個の粒子以外の粒子は棄却する。
ステップＳ１０６の後、ステップＳ１０７へ遷移する。
【００６６】
（ステップＳ１０７）
補正部１９５は、ステップＳ１０６で選択したＰ個の粒子の各々に拡張カルマンフィルタを適用して各粒子の値を補正する。補正部１９５は、各粒子の尤度を高めるように補正を行う。そのために、補正部１９５は、選択部１９４が選択した粒子の各々を用いてソナーによる測定値の推定値を再度算出する。ここでは、ステップＳ１０３の場合と異なり、補正部１９５は、アクティブセンシング時は、アクティブセンサ１１２による観測方程式（式（６））を用いる。一方、補正部１９５は、パッシブセンシング時は、パッシブセンサ１１１による観測方程式（式（９））を用いる。これにより、アクティブセンサ１１２による測距の情報を粒子の尤度補正に反映させることができる。
【００６７】
ここで、パッシブセンシング時については、パッシブソナーによる観測方程式はアークタンジェント（ａｔａｎ）を含む非線形な式である。そこで、拡張カルマンフィルタの枠組みに基づき、観測方程式をテイラー展開して１次の項で打ち切った近似値を採用する。具体的には、式（９）を式（１２）のように変形して用いる。
【００６８】
【数１２】
2018063142.tif 000014
【００６９】
ここで、ベクトルＣ（ｋ）は係数を示すベクトルであり、式（１３）のように示される。
【００７０】
【数１３】
2018063142.tif 000015
【００７１】
偏微分∂ｈｋ／∂ｘ（ｋ）は、式（１４）のように示される。
【００７２】
【数１４】
2018063142.tif 000016
【００７３】
α（ｋ）は、センサに対する対象の相対位置のｙ座標値をｘ座標値で除算した比を示す。α（ｋ）は、式（１５）のように示される。
【００７４】
【数１５】
2018063142.tif 000017
【００７５】
偏微分∂ｈｋ／∂ｙ（ｋ）は、式（１６）のように示される。
【００７６】
【数１６】
2018063142.tif 000018
【００７７】
式（１２）に示される観測方程式を用いることにより、補正部１９５は、パッシブセンシング時もアクティブセンシング時と同様にカルマンゲインを算出することができる。補正部１９５は、各粒子に対してカルマンゲインを用いた補正を行う。
ここでのカルマンゲインＬ（ｋ）は、式（１７）のように示される。
【００７８】
【数１７】
2018063142.tif 000019
【００７９】
Ｐ（ｋ）は、タイミングｋにおける推定誤差共分散行列を示す。Ｗ（ｋ）は、タイミングｋにおける観測誤差の分散行列を示す。
ここでのカルマンゲインＬ（ｋ）を用いた粒子の補正は、式（１８）のように示される。
【００８０】
【数１８】
2018063142.tif 000020
【００８１】
式（１８）の左辺のｑｐ（ｋ）は、タイミングｋにおける補正後の粒子を示す。右辺のｑｐ（ｋ）は、タイミングｋにおける補正前の粒子を示す。また、ｑｅｐ（ｋ）は、タイミングｋにおける運動パラメータの推定値を示す。
ステップＳ１０７の後、ステップＳ１０８へ遷移する。
【００８２】
（ステップＳ１０８）
第二推定部１９６は、カルマンゲインを用いた補正後の粒子の算術平均を算出する。第二推定部１９６は、得られた平均値を、タイミングｋにおける対象の運動パラメータ推定値とする。以下では、タイミングｋにおける対象の運動パラメータ推定値をｑｅ（ｋ）と表記する。
ステップＳ１０８の後、ステップＳ１０９へ遷移する。
【００８３】
（ステップＳ１０９）
第二推定部１９６は、タイミングｋにおける対象の運動パラメータ推定値ｑｅ（ｋ）と、センサの位置とに基づいて、センサと対象との距離、センサに対する対象の方位及び相対速度を算出する。タイミングｋにおけるセンサと対象との距離Ｒ（ｋ）は、式（１９）のように示される。
【００８４】
【数１９】
2018063142.tif 000021
【００８５】
ここでの対象の位置のｘ座標値ｘ（ｋ）、ｙ座標値ｙ（ｋ）として、いずれもｑｅ（ｋ）における値を用いる。
タイミングｋにおけるセンサに対する対象の方位θ（ｋ）は、式（２０）のように示される。
【００８６】
【数２０】
2018063142.tif 000022
【００８７】
ここでの対象の位置のｘ座標値ｘ（ｋ）、ｙ座標値ｙ（ｋ）として、いずれもｑｅ（ｋ）における値を用いる。
タイミングｋにおけるセンサに対する対象の相対速度ｖ（ｋ）は、式（２１）のように示される。
【００８８】
【数２１】
2018063142.tif 000023
【００８９】
ここでの対象の速度のｘ座標値ｘ’（ｋ）、ｙ座標値ｙ’（ｋ）として、いずれもｑｅ（ｋ）における値を用いる。
ステップＳ１０９の後、ステップＳ１１０へ遷移する。
【００９０】
（ステップＳ１１０）
制御部１９０は、ｋ：＝ｋ＋１とする。すなわち、サンプリングタイミングを示すインデックスの値を１進める（大きくする）。
ステップＳ１１０の後、次のサンプリングタイミングでステップＳ１０１からの処理を繰り返す。
【００９１】
図６は、運動パラメータ推定装置１００による運動パラメータの推定値の真値付近への収束例を示す図である。図６に示すグラフの横軸は時刻を示し、縦軸は運動パラメータの値を示す。線Ｌ２１は、運動パラメータの真値を示す。線Ｌ２２は、運動パラメータ推定装置１００による運動パラメータの推定値を示す。線Ｌ２３は、アクティブセンサのみによる場合の運動パラメータの推定値を示す。時間ｔ１１は、アクティブセンサのみによる場合のサンプリング周期を示す。
アクティブセンサのみによる場合、上記のようにアクティブセンサでは物理的エネルギーを対象物に向けて出力してから対象物で反射した物理的エネルギーを受信するまでの待ち時間が生じるため、時間ｔ１１で示されるようにある程度のサンプリング間隔が生じる。これに対し、運動パラメータ推定装置１００では、アクティブセンサ１１２のサンプリング間隔より短いサンプリング間隔にてパッシブセンサ１１１が対象物の運動を測定することができる。この点で、運動パラメータ推定装置１００によれば、アクティブセンサのみによる場合よりも短い周期で対象物の運動を測定することができ、より高精度に対象物の運動を推定することが期待され、また、より速やかに真値付近に収束することが期待される。
【００９２】
以上のように、測定部１１０は、アクティブセンサ１１２を用いて対象物の位置を測定し、パッシブセンサ１１１を用いてアクティブセンサ１１２での測定周期よりも短い周期で対象物２０の方向を測定する。駆動雑音サンプル設定部１９１は、状態方程式における駆動雑音のサンプルを所定の確率分布に基づいて複数設定する。粒子算出部１９２は、駆動雑音のサンプルの各々を状態方程式に代入して、粒子フィルタにおける粒子を複数算出する。第一推定部１９３は、粒子算出部１９２が算出した粒子の各々を観測方程式に代入して、測定部１１０の測定値の推定値を算出する。選択部１９４は、測定部１１０の測定値と第一推定部１９３の推定値との誤差に基づいて、複数の粒子のうち一部の粒子を選択する。第二推定部１９６は、選択部１９４が選択した粒子に基づいて、運動パラメータの推定値を算出する。
このように、運動パラメータ推定装置１００では、状態方程式及び観測方程式を含むモデルに粒子フィルタを適用することで、アクティブセンサ１１２の測定結果とパッシブセンサ１１１の測定結果を同一のモデルに適用することができる。これにより、運動パラメータ推定装置１００では、アクティブセンサ１１２による対象物２０の位置の測定結果を用いることができ、かつ、アクティブセンサ１１２のみによる場合よりも短いサンプリング周期で対象物２０の運動を測定することができる。この点で、運動パラメータ推定装置１００によれば、アクティブセンサ１１２とパッシブセンサ１１１との組み合わせにより、対象物２０の運動の推定精度を高めることができる。
【００９３】
＜第２実施形態＞
第１実施形態では、センサの位置（［ｘＯ（ｋ）  ｙＯ（ｋ）］）の誤差については考慮していない。これに対し、ＧＮＳＳ（Global Navigation Satellite System、全球測位衛星システム）を用いた測位、デッドレコニング（Dead Reckoning）など、いずれの測位でもセンサの位置の測位結果に誤差が生じ得る。
そこで、第２実施形態では、対象の運動パラメータに加えてセンサ自らの運動パラメータも推定する。
【００９４】
第２実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第２実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第２実施形態では、数式モデルを以下のように拡張する。
運動パラメータベクトルｑ（ｋ）には、対象の運動パラメータに加えてセンサの運動パラメータも含める。第２実施形態でのタイミングｋにおける運動パラメータ態ベクトルｑ（ｋ）は、式（２２）のように示される。
【００９５】
【数２２】
2018063142.tif 000024
【００９６】
第２実施形態では、駆動雑音も拡張する。第２実施形態でのタイミングｋにおける駆動雑音ベクトルｕ（ｋ）は、式（２３）のように示される。
【００９７】
【数２３】
2018063142.tif 000025
【００９８】
ｕ１（ｋ）及びｕ２（ｋ）と同様、ｕ３（ｋ）、ｕ４（ｋ）のいずれもスカラ変数である。
第２実施形態では、式（１）に示される状態方程式の行列Ａ及びＢも拡張する。第２実施形態では、式（１）で行列Ａに代えて行列Ａ１を用いる。行列Ａ１は、式（２４）のように示される。
【００９９】
【数２４】
2018063142.tif 000026
【０１００】
ここで、Ｒ１２×１２は、１２行１２列の実数行列を示す。
第２実施形態では、式（１）で行列Ｂに代えて行列Ｂ１を用いる。行列Ｂ１は、式（２５）のように示される。
【０１０１】
【数２５】
2018063142.tif 000027
【０１０２】
ここで、０６×２は、６行２列のゼロ行列を示す。Ｒ１２×４は、１２行４列の実数行列を示す。
第２実施形態では、式（６）に示される観測方程式の行列Ｃも拡張する。第２実施形態では、式（６）で行列Ｃに代えて行列Ｃ１を用いる。行列Ｃ１は、式（２６）のように示される。
【０１０３】
【数２６】
2018063142.tif 000028
【０１０４】
ここで、０２×６は、２行６列のゼロ行列を示す。
第２実施形態では、観測値ベクトルに自己位置の測定値も追加する拡張を行う。第２実施形態では、アクティブソナーの場合の観測値ベクトルとして式（７）に代えて式（２７）を用いる。
【０１０５】
【数２７】
2018063142.tif 000029
【０１０６】
ここで、ｘＯｍは、センサ自らの位置の測定値（測位結果）のｘ座標値を示す。ｙＯｍは、センサ自らの位置の測定値のｙ座標値を示す。
センサの位置は、アクティブソナー、パッシブソナーとは別に測定できるため、パッシブソナーの場合の観測値ベクトルとして式（２８）を用いる。
【０１０７】
【数２８】
2018063142.tif 000030
【０１０８】
θｍ（ｋ）は、式（９）及び式（１２）を参照して説明したとおりである。
図５のステップＳ１０７におけるパッシブ時の非線形関数の近似として、式（１３）に代えて式（２９）を用いる。
【０１０９】
【数２９】
2018063142.tif 000031
【０１１０】
また、パッシブセンサの場合、式（６）に示される観測方程式の行列Ｃに代えて、式（３０）に示される行列Ｃ１を用いる。
【０１１１】
【数３０】
2018063142.tif 000032
【０１１２】
Ｃ２は、式（３１）のように示される。
【０１１３】
【数３１】
2018063142.tif 000033
【０１１４】
上記のように、０２×６は、２行６列のゼロ行列を示す。また、Ｒ２×１２は、２行１２列の実数行列を示す。
【０１１５】
以上のように、第２実施形態では、状態方程式は対象物２０の運動に加えて運動パラメータ推定装置１００の運動を示す。また、観測方程式は、対象物２０の運動の測定値の方程式に加えて運動パラメータ推定装置１００の運動の測定値の方程式を含む。第二推定部１９６は、対象物２０の運動および運動パラメータ推定装置１００の運動を示す運動パラメータの推定値を算出する。
これにより、第２実施形態の運動パラメータ推定装置１００では、運動パラメータ推定装置１００自らの運動の測定誤差に対しても補正を行うことができる。この点で、対象物２０の運動の推定精度をより高めることができる。
【０１１６】
＜第３実施形態＞
センサ自らの位置に加えてセンサの速度の測定値を得られる場合、センサの測定の測定値を用いることで状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。そこで、第３実施形態では、第２実施形態における拡張に加えて、さらにセンサ自らの速度を観測値ベクトルに含める拡張を行う。
第３実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第３実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第３実施形態では、数式モデルを第２実施形態の場合からさらに拡張する。
第３実施形態でのアクティブセンシング時の観測値ベクトルは式（３２）のように示される。
【０１１７】
【数３２】
2018063142.tif 000034
【０１１８】
ｘＯｍ（ｋ）’は、タイミングｋにおけるセンサの速度のｘ座標値の測定値を示す。ｙＯｍ（ｋ）’は、タイミングｋにおけるセンサの速度のｙ座標値の測定値を示す。
また、第３実施形態では、式（６）で行列Ｃに代えて行列Ｃ２を用いる。行列Ｃ２は、式（３３）のように示される。
【０１１９】
【数３３】
2018063142.tif 000035
【０１２０】
ここで、０２×６は、２行６列のゼロ行列を示す。０４×６は、４行６列のゼロ行列を示す。また、行列Ｃ３は、式（３４）のように示される。
【０１２１】
【数３４】
2018063142.tif 000036
【０１２２】
一方、パッシブセンシング時の測定値ベクトルは、式（３５）のように示される。
【０１２３】
【数３５】
2018063142.tif 000037
【０１２４】
この場合、式（６）で行列Ｃに代えてＣ４（ｋ）を用いる。Ｃ４（ｋ）は、式（３６）のように示される。
【０１２５】
【数３６】
2018063142.tif 000038
【０１２６】
ここで、Ｒ５×１２は、５行１２列の実数行列を示す。
行列Ｃ５は、式（３７）のように示される。
【０１２７】
【数３７】
2018063142.tif 000039
【０１２８】
上記のように、０４×６は、４行６列のゼロ行列を示す。Ｒ４×１２は、４行１２列の実数行列を示す。
【０１２９】
以上のように、第３実施形態では、観測方程式は、運動パラメータ推定装置１００の速度の測定値の方程式を含む。これにより、第３実施形態の運動パラメータ推定装置１００では、運動パラメータ推定装置１００自らの運動の推定精度をより高めることができ、もって、対象物２０の運動の推定精度をより高めることができる。
【０１３０】
＜第４実施形態＞
アクティブセンサ１１２によるドップラー効果を測定することで、対象の速度のうちセンサ方向の速度成分を測定することができる。これを測定値の１つに加える拡張を行うことで、状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。そこで、第４実施形態では、対象の速度のうちセンサ方向の速度成分の測定値を観測値ベクトルに含める拡張を行う。第１実施形態〜第３実施形態のいずれにも第４実施形態を適用可能である。
第４実施形態における運動パラメータ推定装置１００の構成は、図１を参照して説明した第１実施形態の場合と同様である。また、第４実施形態における処理手順は、図５を参照して説明した第１実施形態の場合の処理手順と同様である。但し、第４実施形態では、数式モデルを以下のように拡張する。
【０１３１】
アクティブセンサ１１２によるドップラー効果は、対象物２０の速度のうち測定部１１０の方向の速度成分に現れる。タイミングｋにおける対象物２０の速度のうち測定部１１０の方向の速度成分ｖｄ（ｋ）は、式（３８）のように示される。
【０１３２】
【数３８】
2018063142.tif 000040
【０１３３】
ここで、ｖ（ｋ）は、タイミングｋにおける対象の速度を示す。φ（ｋ）は、対象の進行方向をｘ軸に対する角度で示す。
推定すべき状態（運動パラメータベクトルｑ（ｋ）の成分）を用いて、式（３８）は式（３９）のように示される。
【０１３４】
【数３９】
2018063142.tif 000041
【０１３５】
式（３９）を、アクティブセンシング時の観測方程式の１つとして追加する。
【０１３６】
以上のように、第４実施形態では、観測方程式は、アクティブセンサ１１２におけるドップラー効果に基づく対象物２０の速度の測定値の方程式を含む。
対象物２０の運動の測定値が増える点で、第４実施形態における運動パラメータ推定装置１００によれば、対象物２０の運動の推定精度をより高めることができる。
【０１３７】
＜第５実施形態＞
図５に示される処理では、ステップＳ１０３で常にパッシブセンシングによる測定値（センサから見た対象の方位）を用いている。このため、ステップＳ１０４では、常にパッシブセンシングによる測定値を用いて誤差を算出している。これに対し、アクティブセンシング時には対象の方位のみならず距離の測定値も用いることで、状態の推定速度、又は、真値付近への集束速度、あるいはこれら両方がさらに向上する可能性がある。
そこで、第５実施形態では、アクティブセンシング時とパッシブセンシング時とで誤差計測の方法を切り替える。アクティブセンシング時には、対象の方位のみならず距離の測定値も用いる。具体的には、図５のステップＳ１０３で、アクティブセンシング時には、式（６）に示される観測方程式を用いる。
一方、パッシブセンシング時は、式（９）に示される観測方程式を用いる。
第５実施形態は、第１実施形態〜第４実施形態のいずれにも第５実施形態を適用可能である。
【０１３８】
以上のように、第一推定部１９３は、アクティブセンサ１１２、パッシブセンサ１１１それぞれの観測方程式を用いて測定部１１０の測定値の推定値を算出する。
第一推定部１９３が対象物２０の方向のみならず対象物２０の位置に基づいて測定部１１０の測定値の推定値を算出することで、第一推定部１９３が算出する推定値の精度が向上し、対象物２０の運動の推定精度をより高められることが期待される。
【０１３９】
なお、制御部１９０の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ、ＣＤ−ＲＯＭ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムを送信する場合の通信線のように、短時間の間、動的にプログラムを保持するもの、その場合のサーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。
【０１４０】
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計変更等も含まれる。
【符号の説明】
【０１４１】
１  測定系
１０  移動体
２０  対象物
１００  運動パラメータ推定装置
１１０  測定部
１１１  パッシブセンサ
１１２  アクティブセンサ
１８０  記憶部
１９０  制御部
１９１  駆動雑音サンプル設定部
１９２  粒子算出部
１９３  第一推定部
１９４  選択部
１９５  補正部
１９６  第二推定部
【図１】
2018063142.tif 000042
【図２】
2018063142.tif 000043
【図３】
2018063142.tif 000044
【図４】
2018063142.tif 000045
【図５】
2018063142.tif 000046
【図６】
2018063142.tif 000047