(57)【特許請求の範囲】
【請求項１】
目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定される定数加速度周波数を決定する運動モデル制御手段と、
前記運動モデルを持つ複数のカルマンフィルタを用いて、前記各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に前記目標の運動諸元を推定する運動諸元推定手段と
を備えたことを特徴とする目標追尾装置。
【請求項２】
前記運動諸元推定手段は、
前記各運動モデルにそれぞれ異なる前記定数加速度周波数を設定した前記複数のカルマンフィルタを並列的に並べて使用し、前記各運動モデルにより平滑値を算出する推定手段と、
前記各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、前記平滑値群を統合した統合平滑値を出力する平滑諸元統合手段と
を備えたことを特徴とする請求項１に記載の目標追尾装置。
【請求項３】
前記運動諸元推定手段は、
前記各運動モデルにそれぞれ異なる前記定数加速度周波数を設定した前記複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、前記各運動モデルの平滑値を算出する推定手段と、
前記各運動モデルの平滑値群の中から選択された平滑値、あるいは、前記平滑値群を統合した前記統合平滑値を出力する平滑諸元統合手段と
を備えたことを特徴とする請求項１に記載の目標追尾装置。
【請求項４】
前記運動諸元推定手段は、
前記各運動モデルにそれぞれ異なる前記定数加速度周波数を設定した前記複数のカルマンフィルタを並列的に並べて使用し、前記各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、前記各運動モデルの平滑値を算出する推定手段と、
前記各運動モデルの平滑値群の中から選択された平滑値、あるいは、前記平滑値群を統合した統合平滑値を出力する平滑諸元統合手段と
を備えたことを特徴とする請求項１に記載の目標追尾装置。
【請求項５】
前記運動モデル制御手段は、
前記運動諸元推定手段より取得した前記目標の状態変数および前記事後確率に基づいて、前記運動モデルの数と前記定数加速度周波数とを決定することを特徴とする請求項１から請求項４までのいずれか１項に記載の目標追尾装置。
【請求項６】
前記平滑諸元統合手段は、
前記運動モデルの数が所定値よりも多い場合、前記各運動モデルの事後確率に基づいて前記運動モデルを選択し、選択された前記運動モデルの前記平滑値を出力することを特徴とする請求項２から請求項４までのいずれか１項に記載の目標追尾装置。
【請求項７】
前記平滑諸元統合手段は、
前記運動モデルの数が所定値以下の場合、前記各運動モデルの事後確率に基づいて、前記平滑値群を統合し、統合された統合平滑値を出力することを特徴とする請求項２から請求項４までのいずれか１項に記載の目標追尾装置。
【請求項８】
目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定する定数加速度周波数を決定する運動モデル制御ステップと、
前記運動モデルを持つ複数のカルマンフィルタを用いて、前記各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に前記目標の運動諸元を推定する運動諸元推定ステップと
を備えたことを特徴とする目標追尾方法。
【請求項９】
前記運動諸元推定ステップは、
前記各運動モデルにそれぞれ異なる前記定数加速度周波数を設定した前記複数のカルマンフィルタを並列的に並べて使用し、前記各運動モデルにより平滑値を算出する推定ステップと、
前記各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、前記平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップと
を備えたことを特徴とする請求項８に記載の目標追尾方法。
【請求項１０】
前記運動諸元推定ステップは、
前記各運動モデルにそれぞれ異なる前記定数加速度周波数を設定した前記複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、前記各運動モデルの平滑値を算出する推定ステップと、
前記各運動モデルの平滑値群の中から選択された平滑値、あるいは、前記平滑値群を統合した前記統合平滑値を出力する平滑諸元統合ステップと
を備えたことを特徴とする請求項８に記載の目標追尾方法。
【請求項１１】
前記運動諸元推定ステップは、
前記各運動モデルにそれぞれ異なる前記定数加速度周波数を設定した前記複数のカルマンフィルタを並列的に並べて使用し、前記各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、前記各運動モデルの平滑値を算出する推定ステップと、
前記各運動モデルの平滑値群の中から選択された平滑値、あるいは、前記平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップと
を備えたことを特徴とする請求項８に記載の目標追尾方法。
【請求項１２】
前記運動モデル制御ステップは、
前記運動諸元推定ステップより取得した前記目標の状態変数および前記事後確率に基づいて、前記運動モデルの数と前記定数加速度周波数とを決定することを特徴とする請求項８から請求項１１までのいずれか１項に記載の目標追尾方法。
【請求項１３】
前記平滑諸元統合ステップは、
前記運動モデルの数が所定値よりも多い場合、前記各運動モデルの事後確率に基づいて前記運動モデルを選択し、選択された前記運動モデルの前記平滑値を出力することを特徴とする請求項９から請求項１１までのいずれか１項に記載の目標追尾方法。
【請求項１４】
前記平滑諸元統合ステップは、
前記運動モデルの数が所定値以下の場合、前記各運動モデルの事後確率に基づいて、前記平滑値群を統合し、統合された統合平滑値を出力することを特徴とする請求項９から請求項１１までのいずれか１項に記載の目標追尾方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、センサ等からの目標の位置情報に関する観測情報に基づいて目標の位置や速度などの運動諸元を推定する目標追尾装置およびその方法に関するものである。
【０００２】
【従来の技術】
図６は、「Ｐａｕｌ  Ｚａｒｃｈａｎ  ａｎｄ  Ｄｉｍｉｔｒｉｏｓ  ”Ｆｉｌｔｅｒｉｎｇ  Ｓｔｒａｔｅｇｉｅｓ  ｆｏｒ  Ｓｐｉｒａｌｉｎｇ  Ｔａｒｇｅｔｓ”，ＡＩＡＡ／ＢＭＤＯ  Ｔｅｃｈｎｏｌｏｇｙ  Ｃｏｎｆｅｒｅｎｃｅ  ａｎｄ  Ｅｘｈｉｂｉｔ．９ｔｈ．Ｈｅｌｄ  ｉｎ  Ｓａｎ  Ｄｉｅｇｏ，ＣＡ，Ｊｕｌｙ  ２０００．」に記載された従来の目標追尾装置を示すブロック図である。
【０００３】
図６において、１はセンサ等を備えた目標観測装置、２は目標観測装置１からの観測値に基づいて、所定のサンプリングタイミング毎に目標の位置や速度などの運動諸元を推定する目標追尾装置、３は目標追尾装置２での推定結果を表示する表示装置を示す。
【０００４】
次に、目標追尾装置２の構成について説明する。目標追尾装置２は、目標観測装置１からの観測値が追尾に有効なデータであるかを判定するゲート判定器４と、観測値、ゲイン行列、および、１サンプリングタイミング前の平滑値および平滑誤差共分散行列に基づいて、平滑値および平滑誤差共分散を算出する平滑器５とを備えている。
【０００５】
また、平滑値および平滑誤差共分散行列の初期値を格納する初期値メモリ６と、１サンプリングタイミング前の平滑値および平滑誤差共分散行列を格納する平滑諸元用メモリ７と、平滑値および平滑誤差共分散行列に基づいて予測値および予測誤差共分散行列を算出する予測器８と、ゲイン行列を算出するゲイン行列算出器９とを備えている。
【０００６】
さらに、拡張カルマンフィルタの線形近似により状態遷移行列を算出する状態遷移行列算出器１０と、予測器８より出力された予測値および予測誤差共分散行列を単位時間だけ遅延させる第１の遅延回路と、第１の遅延回路と同様の機能を持つ第２の遅延回路とを備えている。
【０００７】
次に、図６とともに、図７を参照しながら、従来の目標追尾装置２の動作について説明する。図７は、従来の目標追尾装置２の動作を示すフローチャートである。
【０００８】
従来の目標追尾装置２は、拡張カルマンフィルタを用いて、センサと目標との相対位置、相対速度、目標の加速度、加速度の１階微分値であるジャーク項および目標加速度の周波数の運動諸元を推定する。
【０００９】
目標の加速度が正弦波であり、その開始時刻が飛行時間において一様に分布していると仮定した場合、拡張カルマンフィルタの状態方程式は次式のように与えられる。
【００１０】
【数１】
0004144730.tif 000002
【００１１】
式（１）において、ｙはセンサと目標との相対位置を示し、ｙｔは目標の位置を示し、ωは加速度を正弦波とした場合の正弦波の周波数成分を示す。加速度周波数の微分値は白色ガウス雑音であると仮定される。非線形状態方程式の非線形関数ｆが滑らかであるという条件の下に、推定値のまわりに線形化すると、連続時間における状態遷移行列は次式のように与えられる。
【００１２】
【数２】
0004144730.tif 000003
【００１３】
連続時間における状態遷移行列の要素が１サンプリング間Ｔｓにおいて一定であるという近似を用いて、離散時間における状態遷移行列を次式のように得る。
【００１４】
【数３】
0004144730.tif 000004
【００１５】
式（３）において、
【数４】
0004144730.tif 000005
は加速度周波数の推定値を示し、
【数５】
0004144730.tif 000006
は目標の加速度推定値を示す。
【００１６】
さらに、フィルタが得ることができる観測情報は、目標の相対位置のみであるため、観測方程式は以下の式より表される。
【００１７】
【数６】
0004144730.tif 000007
【００１８】
式（４）、式（５）において、Ｈは観測行列を示し、
【数７】
0004144730.tif 000008
は離散時間の観測雑音で平均０、共分散Ｒｋの白色ガウス雑音と仮定される。
【００１９】
以上のように示した状態方程式および運動方程式により、目標の相対位置、相対速度、目標加速度、目標ジャークおよび加速度周波数の推定を行う。
【００２０】
図７において、初期値メモリ６に各運動モデルの平滑値の初期値と平滑誤差共分散行列の初期値とを設定し、追尾処理を開始する（ステップＳ７０１)。以降、新たな観測値が得られる度に、サンプリング時刻を１つ進めて追尾処理が実行される（ステップＳ７０８）。
【００２１】
次に、状態遷移行列算出器１０は、平滑器５から出力される平滑値と平滑誤差共分散行列とを読み込み、式（３）に示す拡張カルマンフィルタの離散時間における状態遷移行列を算出する（ステップＳ７０２）。
【００２２】
次に、予測器８は、状態遷移行列算出器１０を介して、平滑値、平滑誤差共分散行列、および状態遷移行列を読み込み、予測値および予測誤差共分散行列を次式より算出する（ステップＳ７０３）。
【００２３】
【数８】
0004144730.tif 000009
【００２４】
次に、目標観測装置１からの観測値が読み込まれる（ステップＳ７０４）。この観測値は、目標からの信号検出およびゲート判定による追尾目標の選択結果で、位置情報で構成されており、目標位置観測情報と呼ばれる。
【００２５】
次に、センサからの観測値（観測情報）が入力されると、ゲート判定器４は、観測値が有効なデータであるかどうかの判定を行い、極端に観測精度の悪いデータを除外する（ステップＳ７０５）。
【００２６】
このゲート判定は、予測値を使用して、以下の式（８）および式（９）により行われる。
【００２７】
【数９】
0004144730.tif 000010
【００２８】
次に、平滑器５は、予測器８より予測値と予測誤差共分散行列とを読み込み、平滑値、平滑誤差共分散行列およびカルマンゲインを次式より算出する（ステップＳ７０６）。
【００２９】
【数１０】
0004144730.tif 000011
【００３０】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ステップＳ７０７）。ステップＳ７０７において、処理の終了が要求されていないと判別される場合には（すなわち、ＮＯ）、ステップＳ７０８を経由して、ステップＳ７０２に戻り、処理が繰り返される。また、処理の終了が要求されていると判別される場合には（すなわち、ＹＥＳ）、図７の処理ルーチンを終了する。
【００３１】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように、揺動目標の加速度を正弦波とし、加速度周波数の１階微分値は白色ガウス雑音であるとの運動モデルに基づく拡張カルマンフィルタを用いて、目標の相対位置、相対速度、加速度、ジャークおよび加速度周波数を推定している。しかし拡張カルマンフィルタは、非線形問題をテイラー近似により線形化問題として扱うために、特に観測雑音が大きい場合や、状態変数の変化が大きい場合には追尾精度が劣化するという問題点があった。
【００３２】
この発明は上記のような問題点を解決するためになされたもので、高い追尾性能を得ることのできる目標追尾装置およびその方法を得ることを目的とする。
【００３３】
【課題を解決するための手段】
この発明に係る目標追尾装置は、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定される定数加速度周波数を決定する運動モデル制御手段と、運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定手段とを備えたものである。
【００３４】
また、この発明に係る目標追尾装置の運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定手段と、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたものである。
【００３５】
また、この発明に係る目標追尾装置の運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたものである。
【００３６】
また、この発明に係る目標追尾装置の運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたものである。
【００３７】
また、この発明に係る目標追尾装置の運動モデル制御手段は、運動諸元推定手段より取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するものである。
【００３８】
また、この発明に係る目標追尾装置の平滑諸元統合手段は、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するものである。
【００３９】
また、この発明に係る目標追尾装置の平滑諸元統合手段は、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するものである。
【００４０】
また、この発明に係る目標追尾方法は、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定する定数加速度周波数を決定する運動モデル制御ステップと、運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定ステップとを備えたものである。
【００４１】
また、この発明に係る目標追尾方法の運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定ステップと、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたものである。
【００４２】
また、この発明に係る目標追尾方法の運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたものである。
【００４３】
また、この発明に係る目標追尾方法の運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたものである。
【００４４】
また、この発明に係る目標追尾方法の運動モデル制御ステップは、運動諸元推定ステップより取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するものである。
【００４５】
また、この発明に係る目標追尾方法の平滑諸元統合ステップは、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するものである。
【００４６】
さらに、この発明に係る目標追尾方法の平滑諸元統合ステップは、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するものである。
【００４７】
【発明の実施の形態】
実施の形態１．
以下、図面を参照しながら、この発明の実施の形態１について詳細に説明する。図１は、この発明の実施の形態１を示すブロック構成図である。
【００４８】
図１において、１はセンサなどを備えた目標観測装置、２は目標観測装置１からの観測値に基づいて、目標の位置や速度などを推定する目標追尾装置、３は目標追尾装置２での推定結果を表示する表示装置を示す。
【００４９】
次に、目標追尾装置２の構成について説明する。目標追尾装置２は、統合平滑値、統合平滑誤差共分散行列および第１〜第Ｎの運動モデルの事後確率を格納する統合平滑値用メモリ４と、第１〜第Ｎの各運動モデルによる平滑値と平滑誤差共分散行列とを格納する平滑諸元用メモリ５１〜５３と、第１〜第Ｎの各運動モデルによる事後確率を格納する第１〜第Ｎの運動モデル確率用メモリ６１〜６３とを備えている。
【００５０】
また、運動モデル数と各運動モデルに設定する加速度周波数を決定する運動モデル制御器７と、第１〜第Ｎの運動モデルの状態遷移行列、加速度周波数を設定する第１〜第Ｎの運動モデル設定器８１〜８３と、第１〜第Ｎの運動モデルによる予測処理を行う第１〜第Ｎの運動モデル用予測器９１〜９３と、第１〜第Ｎの運動モデルの事前確率（事前信頼度）を算出する第１〜第Ｎの運動モデルの事前確率算出器１０１〜１０３と、第１〜第Ｎの運動モデルによる予測ベクトルを統合した統合予測ベクトルとその誤差共分散行列である統合予測誤差共分散行列とを算出する予測諸元統合器１１とを備えている。
【００５１】
また、観測値が追尾に有効なデータであるかを判定するゲート判定器１２と、観測値に基づいて、それぞれの第１〜第Ｎの運動モデルの尤度を算出する第１〜第Ｎの運動モデルの尤度算出器１３１〜１３３と、尤度の総和を算出するモデル尤度算出器１４とを備えている。
【００５２】
さらに、第１〜第Ｎの運動モデルの事後確率（事後信頼度）を算出する第１〜第Ｎの運動モデルの事後確率算出器１５１〜１５３と、第１〜第Ｎの運動モデルによる平滑処理を行う第１〜第Ｎの運動モデル用平滑器１６１〜１６３と、統合平滑値と統合平滑誤差共分散行列とを算出して、表示装置３に出力する平滑諸元統合器１７とを備えている。
【００５３】
このように、目標追尾装置２は、運動モデル制御器７からなる運動モデル制御手段と、それ以外からなる運動諸元推定手段とから構成される。また、運動諸元推定手段は、平滑諸元統合器１７からなる平滑諸元統合手段と、それ以外からなる推定手段とから構成される。
【００５４】
次に、この発明の実施の形態１による動作について説明する。
【００５５】
目標の加速度が正弦波であり、その開始時刻が飛行時間において一様に分布していると仮定した場合、連続時間における目標の運動モデルは、次式のように表される。
【００５６】
【数１１】
0004144730.tif 000012
【００５７】
ただし、
【００５８】
【数１２】
0004144730.tif 000013
【００５９】
ここで、ｘ（ｔ）は相対位置、相対速度、目標加速度、目標ジャーク（Ｊｅｒｋ）のｙ成分の真値を表す連続時間状態変数ベクトルを示す。また、駆動雑音ｗ（ｔ）は連続時間の白色雑音過程である。
【００６０】
離散時間における第１〜第Ｎの運動モデル毎の運動モデルは、式（１）の微分方程式の解より、以下の式（１７）のように与えられる。
【００６１】
【数１３】
0004144730.tif 000014
【００６２】
ただし、
【００６３】
【数１４】
0004144730.tif 000015
【００６４】
ここで、
【数１５】
0004144730.tif 000016
は離散時間の状態変数を示し、
【数１６】
0004144730.tif 000017
は第１〜第Ｎの運動モデル毎の状態遷移行列を示す。
【数１７】
0004144730.tif 000018
は目標の駆動雑音ベクトルで、平均０、共分散行列Ｑの離散時間白色雑音行列であり、共分散行列Ｑは以下の式より算出される。
【００６５】
【数１８】
0004144730.tif 000019
【００６６】
ここで、
【数１９】
0004144730.tif 000020
は駆動雑音のパワースペクトル密度を示す。以上に示した
【数２０】
0004144730.tif 000021
および
【数２１】
0004144730.tif 000022
は、加速度周波数
【数２２】
0004144730.tif 000023
が真である仮説によるため、運動モデルの数と同数となる。
【００６７】
ここで、らせん周波数
【数２３】
0004144730.tif 000024
が真である仮説は、以下の式（２３）のように表される。
【００６８】
【数２４】
0004144730.tif 000025
【００６９】
さらに、フィルタが得ることができる観測情報は、目標の相対位置のみであるため、観測方程式は従来の目標追尾装置と同様に、式（４）および式（５）により表される。
【００７０】
図１とともに、図２を参照しながら、この発明の実施の形態１による動作について説明する。図２は、この発明の実施の形態１による動作を示すフローチャートである。
【００７１】
図２において、統合平滑値用メモリ４に各運動モデルの平滑値の初期値と平滑誤差共分散行列の初期値とを設定し、さらに第１〜第Ｎのモデル確率用メモリ６１〜６３に各運動モデルの事後確率の初期値を設定して、追尾処理を開始する（ステップＳ２０１）。以降、新たな観測値が得られる度に、サンプリング時刻ｋを１つ進めて追尾処理が実行される（ステップＳ２１５）。
【００７２】
次に、運動モデル制御器７は、平滑諸元統合器１７より統合平滑値用メモリ４を介して取得した目標の状態変数および事後確率（モデル確率）に基づいて、安定した追尾精度を得るのに必要な運動モデル数と、それぞれの運動モデル設定器８１〜８３に設定する加速度周波数（定数加速度周波数）とを決定し、第１〜第Ｎの運動モデル設定器８１〜８３に出力する（ステップＳ２０２）。
【００７３】
ステップＳ２０２において決定した運動モデル数および加速度周波数（パラメータ）に基づいて、第１〜第Ｎの運動モデル設定器８１〜８３は、状態遷移行列、駆動雑音の誤差共分散行列および定数加速度周波数を設定し、それぞれ対応する第１〜第Ｎの運動モデル予測器９１〜９３へ出力する（ステップＳ２０３）。
【００７４】
第１〜第Ｎの運動モデル予測器９１〜９３は、それぞれに対応する第１〜第Ｎの平滑諸元用メモリ５１〜５３より各運動モデルの１サンプリングタイミング前の平滑値と平滑誤差共分散行列とを読み出し、次式にしたがって、各運動モデル毎の予測ベクトル（予測値）
【数２５】
0004144730.tif 000026
および予測誤差共分散行列Ｐｋ，ａ（−）を算出する（ステップＳ２０４）。
【数２６】
0004144730.tif 000027
【００７５】
次に、第１〜第Ｎの運動モデルの事前確率算出器１０１〜１０３は、それぞれに対応する第１〜第Ｎの確率用メモリ６１〜６３より、各運動モデルの１サンプリングタイミング前の事後確率を読み出し、次式により、各運動モデル毎の事前確率を算出する（ステップＳ２０５）。
【００７６】
【数２７】
0004144730.tif 000028
【００７７】
ここで、μｋ，ａ（−）は事前信頼度、μｋ，ａ（＋）は事後信頼度、Ｐａｂはモデルｂからモデルａへの推移確率を示す。
【００７８】
次に、予測諸元統合器１１は、各運動モデルの予測値、予測誤差共分散行列、運動モデルの事前確率を読み込み、１サンプリングタイミング前までの観測値Ｚｋ−１に基づいて、各運動モデルの仮説毎の予測ベクトルおよび予測誤差共分散行列を統合した統合予測ベクトルおよび統合予測誤差共分散行列を、次式にしたがって算出する（ステップＳ２０６）。
【００７９】
【数２８】
0004144730.tif 000029
【００８０】
次に、目標観測装置（センサ）１より観測値が入力されると（ステップＳ２０７）、ゲート判定器１２は、式（８）および式（９）に示す判定式に基づいて、観測値が有効なデータであるかどうかの判定を行い、極端に観測精度の悪いデータを除外する（ステップＳ２０８）。
【００８１】
ゲート判定器１２により観測値が有効なデータであると判定された場合、第１〜第Ｎの運動モデルの尤度算出器１３１〜１３３は、観測値と、それぞれに対応する運動モデルの事前確率、予測値および予測誤差共分散行列とを読み込み、次式により、各運動モデル毎の尤度を算出する（ステップＳ２０９）。
【００８２】
【数２９】
0004144730.tif 000030
【００８３】
ここで、ｇ（ｚ；ａ，Ａ）は平均ａ、共分散行列Ａの３変量正規分布Ｚにおける確率密度関数を示す。また、Ｒｋは観測雑音ベクトルの共分散行列を示す。
【００８４】
次に、モデル尤度集計器１４は、第１〜第Ｎの各運動モデル毎の尤度を集計して、合計値（尤度の総和）を算出し（ステップＳ２１０）、第１〜第Ｎの各運動モデル毎の事後確率算出器１５１〜１５３は、尤度の合計値に基づいて、第１〜第Ｎの各運動モデル毎の事後確率を算出する（ステップＳ２１１）。
【数３０】
0004144730.tif 000031
【００８５】
次に、第１〜第Ｎの各モデル用平滑器１６１〜１６３は、それぞれに対応する第１〜第Ｎの各モデル用予測器９１〜９３より、各運動モデルの予測値と予測誤差共分散行列とを読み込み、次式にしたがって、各運動モデル毎の平滑値、平滑誤差共分散行列およびカルマンゲインを算出する（ステップＳ２１２）。
【００８６】
【数３１】
0004144730.tif 000032
【００８７】
さらに、平滑諸元統合器１７は、各運動モデルの平滑値、平滑誤差共分散行列、および事後確率を取得し、次式にしたがって、各運動モデルの平滑値および平滑誤差共分散行列を統合した統合平滑値および統合平滑誤差共分散行列を算出する（ステップＳ２１３）。
【００８８】
【数３２】
0004144730.tif 000033
【００８９】
さらに、各運動モデルの事後確率および各運動モデルの定数加速度周波数を用いて、以下の式（３６）より加速度周波数推定値を算出する。
【００９０】
【数３３】
0004144730.tif 000034
【００９１】
ここで、平滑諸元統合器１７は、運動モデル数が所定値よりも多い場合には、各運動モデルの事後確率（適合度）より最適な運動モデルを選択し、その最適モデルの平滑値を統合平滑値として出力する。
【００９２】
各サンプリング時刻における処理が追尾終了か否かを判定する（ステップＳ２１４）。ステップＳ２１４において、終了でないと判定された場合（すなわち、ＮＯ）、ステップＳ２１５を経由して、ステップＳ２０２に戻る。また、終了と判定された場合、図２の処理ルーチンを終了する。
【００９３】
このように、それぞれ異なる加速度周波数を持つ複数の運動モデルを同時に使用するとともに、観測情報より各運動モデルに対する事後確率を算出して重み付け統合する方式を取り入れ、相対位置、相対速度、目標加速度、目標ジャーク、目標加速度周波数の目標諸元の推定値を算出する。
【００９４】
以上のように、目標の加速度周波数が時間により変動する場合においても、追尾精度を劣化させることなく、精度良く振動目標の相対位置、相対速度、加速度、目標ジャーク、加速度周波数などを精度良く推定することができる。
【００９５】
また、加速度周波数を状態変数に組み入れずに外部入力項とすることにより、揺動目標の運動を線形問題として扱うことができる。
【００９６】
また、目標の状態変数および事後確率に基づいて、適切な運動モデル数と加速度周波数とを決定するので、高い追尾性能を得ることができる。
【００９７】
実施の形態２．
なお、上記実施の形態１では、各運動モデルの１サンプリングタイミング前の平滑値および平滑誤差分散行列に基づいて目標諸元の推定を行ったが、１サンプリングタイミング前の統合平滑値および統合平滑誤差共分散行列を各運動モデルのフィルタへフィードバックさせてもよい。
【００９８】
図３は、この発明の実施の形態２を示すブロック構成図である。図３において、前述（図１参照）と同様のものについては、同一符号を付して詳述を省略する。
【００９９】
この発明の実施の形態２による動作は、図２と同様であって詳述を省略し、一部動作の異なるステップＳ２０４だけ説明する。
【０１００】
図２において、第１〜第Ｎの運動モデル用予測器９１〜９３は、それぞれに対応する第１〜第Ｎの平滑諸元用メモリ５１〜５３から１サンプリングタイミング前の統合平滑値と統合平滑誤差共分散行列とを読み出し、次式にしたがって、各運動モデル毎の予測ベクトル
【数３４】
0004144730.tif 000035
および予測誤差共分散行列Ｐｋ，ａ（−）を算出する（ステップＳ２０４）。
【０１０１】
【数３５】
0004144730.tif 000036
【０１０２】
なお、算出された統合平滑値および統合平滑誤差共分散行列は、次回のサンプリングタイミング時の算出のために、第１〜第Ｎの平滑諸元用メモリ５１〜５３に記録される。
【０１０３】
このように、異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリング間における現在の運動モデルから他の運動モデルへの推移を考慮して、各運動モデル用予測器９１〜９３に１サンプリングタイミング前の統合平滑値および統合平滑誤差共分散行列を入力させて目標諸元の推定を行うことにより、目標の加速度周波数の変化に対する反応が早まり、精度の高い追尾を行うことができる。
【０１０４】
実施の形態３．
なお、上記実施の形態２では、１サンプリングタイミング前の統合平滑値および統合平滑誤差共分散行列に基づいて目標諸元の推定を行ったが、１サンプリングタイミング前の平滑値および平滑誤差共分散行列をミキシングした値に基づいて目標諸元の推定を行ってもよい。
【０１０５】
図４は、この発明の実施の形態３を示すブロック構成図である。図４において、前述（図１、図３参照）と同様のものについては同一符号を付して詳述を省略する。
【０１０６】
図４において、目標追尾装置２は、各運動モデルの平滑値と平滑誤差共分散行列とをミキシングと呼ばれる処理を行う混合平滑器１８を備え、推定手段に含まれる。
【０１０７】
次に、この発明の実施の形態３による動作について説明する。図５は、この発明の実施の形態３による動作を示すフローチャートである。
【０１０８】
図５において、ステップＳ５０１〜Ｓ５０３、ステップＳ５０５〜ステップＳ５１６は、図２のステップＳ２０１〜ステップＳ２０３、ステップＳ２０４〜ステップＳ２１５に対応しており、詳述を省略する。
【０１０９】
各運動モデルが設定されると、混合平滑器１８は、全運動モデルの平滑値および平滑誤差共分散行列を入力して、ミキシングを行い、混合平滑値および混合平滑誤差共分散行列を算出する（ステップＳ５０４）。
【０１１０】
算出された混合平滑値および混合平滑誤差共分散行列は、各運動モデルの平滑諸元用メモリ５１〜５３に記録され、第１〜第Ｎの運動モデル用予測器９１〜９３は、各運動モデルの平滑諸元用メモリ５１〜５３に記録された混合平滑値および混合平滑誤差共分散行列に基づいて、以下の式により各運動モデルの予測値および予測誤差共分散行列を算出する（ステップＳ５０５）。
【０１１１】
【数３６】
0004144730.tif 000037
【０１１２】
ここで、混合確率μｋ−１，ｂ｜ａ、混合平滑値および混合平滑誤差共分散行列は、以下の式より算出される。
【０１１３】
【数３７】
0004144730.tif 000038
【０１１４】
以上のように、１サンプリング間における運動モデルの推移すべてについて考慮した混合平滑値および混合平滑誤差共分散行列に基づいて、目標諸元の推定を行うので、実施の形態１、２よりも計算処理ステップが増加するが、目標の加速度周波数の急な変化に対応することができ、より精度の高い追尾を行うことができる。
【０１１５】
【発明の効果】
以上のように、この発明によれば、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定される定数加速度周波数を決定する運動モデル制御手段と、運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定手段とを備えたので、目標の加速度周波数が時間により変動する場合においても、追尾精度を劣化させることなく、精度良く振動目標の相対位置、相対速度、加速度、目標ジャーク、加速度周波数などを精度良く推定することのできる目標追尾装置が得られる効果がある。
【０１１６】
また、この発明によれば、運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定手段と、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたので、目標の加速度周波数が変動しても、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１１７】
また、この発明によれば、運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたので、目標の加速度周波数の変化に対する反応が早まり、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１１８】
また、この発明によれば、運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたので、目標の加速度周波数の急な変化に対応することができ、より精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１１９】
また、この発明によれば、運動モデル制御手段は、運動諸元推定手段より取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するので、目標の状態変数および事後確率に基づいて、適切な運動モデル数と加速度周波数とを決定し、高い追尾性能を得ることのできる目標追尾装置が得られる効果がある。
【０１２０】
また、この発明によれば、平滑諸元統合手段は、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１２１】
また、この発明によれば、平滑諸元統合手段は、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１２２】
また、この発明によれば、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定する定数加速度周波数を決定する運動モデル制御ステップと、それぞれ運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定ステップとを備えたので、目標の加速度周波数が時間により変動する場合においても、追尾精度を劣化させることなく、精度良く振動目標の相対位置、相対速度、加速度、目標ジャーク、加速度周波数などを精度良く推定することのできる目標追尾方法が得られる効果がある。
【０１２３】
また、この発明によれば、運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定ステップと、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたので、目標の加速度周波数が変動しても、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２４】
また、この発明によれば、運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたので、目標の加速度周波数の変化に対する反応が早まり、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２５】
また、この発明によれば、運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたので、目標の加速度周波数の急な変化に対応することができ、より精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２６】
また、この発明によれば、運動モデル制御ステップは、運動諸元推定ステップより取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するので、目標の状態変数および事後確率に基づいて、適切な運動モデル数と加速度周波数とを決定し、高い追尾性能を得ることのできる目標追尾方法が得られる効果がある。
【０１２７】
また、この発明によれば、平滑諸元統合ステップは、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２８】
さらに、この発明によれば、平滑諸元統合ステップは、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【図面の簡単な説明】
【図１】  この発明の実施の形態１を示すブロック構成図である。
【図２】  この発明の実施の形態１による動作を示すフローチャートである。
【図３】  この発明の実施の形態２を示すブロック構成図である。
【図４】  この発明の実施の形態３を示すブロック構成図である。
【図５】  この発明の実施の形態３による動作を示すフローチャートである。
【図６】  従来の目標追尾装置を示すブロック構成図である。
【図７】  従来の目標追尾装置の動作を示すフローチャートである。
【符号の説明】
１  目標観測装置、２  目標追尾装置、３  表示装置、４  統合平滑用メモリ、統合予測値用メモリ、７  運動モデル制御器、１１  予測諸元統合器、１２  ゲート判定器、１４  モデル尤度集計器、１７  平滑諸元統合器、１８  混合平滑器、５１  第１の平滑諸元用メモリ、５２  第２の平滑諸元用メモリ、５３  第Ｎの平滑諸元用メモリ、６１  第１の運動モデル確率用メモリ、６２  第２の運動モデル確率用メモリ、６３  第Ｎの運動モデル確率用メモリ、８１  第１の運動モデル設定器、８２  第２の運動モデル設定器、８３  第Ｎの運動モデル設定器、９１  第１の運動モデル用予測器、９２  第２の運動モデル用予測器、９３  第Ｎの運動モデル用予測器、１０１  第１の運動モデルの事前確率算出器、１０２  第２の運動モデルの事前確率算出器、１０３  第Ｎの運動モデルの事前確率算出器、１３１  第１の運動モデル尤度算出器、１３２  第２の運動モデル尤度算出器、１３３  第Ｎの運動モデル尤度算出器、１５１  第１の運動モデル事後確率算出器、１５２  第２の運動モデル事後確率算出器、１５３  第Ｎの運動モデル事後確率算出器、１６１  第１の運動モデル用平滑器、１６２  第２の運動モデル用平滑器、１６３  第Ｎの運動モデル用平滑器。
【図１】
0004144730.tif 000039
【図２】
0004144730.tif 000040
【図３】
0004144730.tif 000041
【図４】
0004144730.tif 000042
【図５】
0004144730.tif 000043
【図６】
0004144730.tif 000044
【図７】
0004144730.tif 000045