JP4144730B2

JP4144730B2 - Target tracking apparatus and method

Info

Publication number: JP4144730B2
Application number: JP2002071998A
Authority: JP
Inventors: 徹也川瀬; 正義系; 義夫小菅
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2002-03-15
Filing date: 2002-03-15
Publication date: 2008-09-03
Anticipated expiration: 2022-03-15
Also published as: JP2003270338A

Description

【０００１】
【発明の属する技術分野】
この発明は、センサ等からの目標の位置情報に関する観測情報に基づいて目標の位置や速度などの運動諸元を推定する目標追尾装置およびその方法に関するものである。
【０００２】
【従来の技術】
図６は、「ＰａｕｌＺａｒｃｈａｎａｎｄＤｉｍｉｔｒｉｏｓ ”ＦｉｌｔｅｒｉｎｇＳｔｒａｔｅｇｉｅｓｆｏｒＳｐｉｒａｌｉｎｇＴａｒｇｅｔｓ”，ＡＩＡＡ／ＢＭＤＯＴｅｃｈｎｏｌｏｇｙＣｏｎｆｅｒｅｎｃｅａｎｄＥｘｈｉｂｉｔ．９ｔｈ．ＨｅｌｄｉｎＳａｎＤｉｅｇｏ，ＣＡ，Ｊｕｌｙ２０００．」に記載された従来の目標追尾装置を示すブロック図である。
【０００３】
図６において、１はセンサ等を備えた目標観測装置、２は目標観測装置１からの観測値に基づいて、所定のサンプリングタイミング毎に目標の位置や速度などの運動諸元を推定する目標追尾装置、３は目標追尾装置２での推定結果を表示する表示装置を示す。
【０００４】
次に、目標追尾装置２の構成について説明する。目標追尾装置２は、目標観測装置１からの観測値が追尾に有効なデータであるかを判定するゲート判定器４と、観測値、ゲイン行列、および、１サンプリングタイミング前の平滑値および平滑誤差共分散行列に基づいて、平滑値および平滑誤差共分散を算出する平滑器５とを備えている。
【０００５】
また、平滑値および平滑誤差共分散行列の初期値を格納する初期値メモリ６と、１サンプリングタイミング前の平滑値および平滑誤差共分散行列を格納する平滑諸元用メモリ７と、平滑値および平滑誤差共分散行列に基づいて予測値および予測誤差共分散行列を算出する予測器８と、ゲイン行列を算出するゲイン行列算出器９とを備えている。
【０００６】
さらに、拡張カルマンフィルタの線形近似により状態遷移行列を算出する状態遷移行列算出器１０と、予測器８より出力された予測値および予測誤差共分散行列を単位時間だけ遅延させる第１の遅延回路と、第１の遅延回路と同様の機能を持つ第２の遅延回路とを備えている。
【０００７】
次に、図６とともに、図７を参照しながら、従来の目標追尾装置２の動作について説明する。図７は、従来の目標追尾装置２の動作を示すフローチャートである。
【０００８】
従来の目標追尾装置２は、拡張カルマンフィルタを用いて、センサと目標との相対位置、相対速度、目標の加速度、加速度の１階微分値であるジャーク項および目標加速度の周波数の運動諸元を推定する。
【０００９】
目標の加速度が正弦波であり、その開始時刻が飛行時間において一様に分布していると仮定した場合、拡張カルマンフィルタの状態方程式は次式のように与えられる。
【００１０】
【数１】

【００１１】
式（１）において、ｙはセンサと目標との相対位置を示し、ｙ_ｔは目標の位置を示し、ωは加速度を正弦波とした場合の正弦波の周波数成分を示す。加速度周波数の微分値は白色ガウス雑音であると仮定される。非線形状態方程式の非線形関数ｆが滑らかであるという条件の下に、推定値のまわりに線形化すると、連続時間における状態遷移行列は次式のように与えられる。
【００１２】
【数２】

【００１３】
連続時間における状態遷移行列の要素が１サンプリング間Ｔ_ｓにおいて一定であるという近似を用いて、離散時間における状態遷移行列を次式のように得る。
【００１４】
【数３】

【００１５】
式（３）において、
【数４】

は加速度周波数の推定値を示し、
【数５】

は目標の加速度推定値を示す。
【００１６】
さらに、フィルタが得ることができる観測情報は、目標の相対位置のみであるため、観測方程式は以下の式より表される。
【００１７】
【数６】

【００１８】
式（４）、式（５）において、Ｈは観測行列を示し、
【数７】

は離散時間の観測雑音で平均０、共分散Ｒｋの白色ガウス雑音と仮定される。
【００１９】
以上のように示した状態方程式および運動方程式により、目標の相対位置、相対速度、目標加速度、目標ジャークおよび加速度周波数の推定を行う。
【００２０】
図７において、初期値メモリ６に各運動モデルの平滑値の初期値と平滑誤差共分散行列の初期値とを設定し、追尾処理を開始する（ステップＳ７０１)。以降、新たな観測値が得られる度に、サンプリング時刻を１つ進めて追尾処理が実行される（ステップＳ７０８）。
【００２１】
次に、状態遷移行列算出器１０は、平滑器５から出力される平滑値と平滑誤差共分散行列とを読み込み、式（３）に示す拡張カルマンフィルタの離散時間における状態遷移行列を算出する（ステップＳ７０２）。
【００２２】
次に、予測器８は、状態遷移行列算出器１０を介して、平滑値、平滑誤差共分散行列、および状態遷移行列を読み込み、予測値および予測誤差共分散行列を次式より算出する（ステップＳ７０３）。
【００２３】
【数８】

【００２４】
次に、目標観測装置１からの観測値が読み込まれる（ステップＳ７０４）。この観測値は、目標からの信号検出およびゲート判定による追尾目標の選択結果で、位置情報で構成されており、目標位置観測情報と呼ばれる。
【００２５】
次に、センサからの観測値（観測情報）が入力されると、ゲート判定器４は、観測値が有効なデータであるかどうかの判定を行い、極端に観測精度の悪いデータを除外する（ステップＳ７０５）。
【００２６】
このゲート判定は、予測値を使用して、以下の式（８）および式（９）により行われる。
【００２７】
【数９】

【００２８】
次に、平滑器５は、予測器８より予測値と予測誤差共分散行列とを読み込み、平滑値、平滑誤差共分散行列およびカルマンゲインを次式より算出する（ステップＳ７０６）。
【００２９】
【数１０】

【００３０】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ステップＳ７０７）。ステップＳ７０７において、処理の終了が要求されていないと判別される場合には（すなわち、ＮＯ）、ステップＳ７０８を経由して、ステップＳ７０２に戻り、処理が繰り返される。また、処理の終了が要求されていると判別される場合には（すなわち、ＹＥＳ）、図７の処理ルーチンを終了する。
【００３１】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように、揺動目標の加速度を正弦波とし、加速度周波数の１階微分値は白色ガウス雑音であるとの運動モデルに基づく拡張カルマンフィルタを用いて、目標の相対位置、相対速度、加速度、ジャークおよび加速度周波数を推定している。しかし拡張カルマンフィルタは、非線形問題をテイラー近似により線形化問題として扱うために、特に観測雑音が大きい場合や、状態変数の変化が大きい場合には追尾精度が劣化するという問題点があった。
【００３２】
この発明は上記のような問題点を解決するためになされたもので、高い追尾性能を得ることのできる目標追尾装置およびその方法を得ることを目的とする。
【００３３】
【課題を解決するための手段】
この発明に係る目標追尾装置は、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定される定数加速度周波数を決定する運動モデル制御手段と、運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定手段とを備えたものである。
【００３４】
また、この発明に係る目標追尾装置の運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定手段と、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたものである。
【００３５】
また、この発明に係る目標追尾装置の運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたものである。
【００３６】
また、この発明に係る目標追尾装置の運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたものである。
【００３７】
また、この発明に係る目標追尾装置の運動モデル制御手段は、運動諸元推定手段より取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するものである。
【００３８】
また、この発明に係る目標追尾装置の平滑諸元統合手段は、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するものである。
【００３９】
また、この発明に係る目標追尾装置の平滑諸元統合手段は、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するものである。
【００４０】
また、この発明に係る目標追尾方法は、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定する定数加速度周波数を決定する運動モデル制御ステップと、運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定ステップとを備えたものである。
【００４１】
また、この発明に係る目標追尾方法の運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定ステップと、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたものである。
【００４２】
また、この発明に係る目標追尾方法の運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたものである。
【００４３】
また、この発明に係る目標追尾方法の運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたものである。
【００４４】
また、この発明に係る目標追尾方法の運動モデル制御ステップは、運動諸元推定ステップより取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するものである。
【００４５】
また、この発明に係る目標追尾方法の平滑諸元統合ステップは、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するものである。
【００４６】
さらに、この発明に係る目標追尾方法の平滑諸元統合ステップは、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するものである。
【００４７】
【発明の実施の形態】
実施の形態１．
以下、図面を参照しながら、この発明の実施の形態１について詳細に説明する。図１は、この発明の実施の形態１を示すブロック構成図である。
【００４８】
図１において、１はセンサなどを備えた目標観測装置、２は目標観測装置１からの観測値に基づいて、目標の位置や速度などを推定する目標追尾装置、３は目標追尾装置２での推定結果を表示する表示装置を示す。
【００４９】
次に、目標追尾装置２の構成について説明する。目標追尾装置２は、統合平滑値、統合平滑誤差共分散行列および第１〜第Ｎの運動モデルの事後確率を格納する統合平滑値用メモリ４と、第１〜第Ｎの各運動モデルによる平滑値と平滑誤差共分散行列とを格納する平滑諸元用メモリ５１〜５３と、第１〜第Ｎの各運動モデルによる事後確率を格納する第１〜第Ｎの運動モデル確率用メモリ６１〜６３とを備えている。
【００５０】
また、運動モデル数と各運動モデルに設定する加速度周波数を決定する運動モデル制御器７と、第１〜第Ｎの運動モデルの状態遷移行列、加速度周波数を設定する第１〜第Ｎの運動モデル設定器８１〜８３と、第１〜第Ｎの運動モデルによる予測処理を行う第１〜第Ｎの運動モデル用予測器９１〜９３と、第１〜第Ｎの運動モデルの事前確率（事前信頼度）を算出する第１〜第Ｎの運動モデルの事前確率算出器１０１〜１０３と、第１〜第Ｎの運動モデルによる予測ベクトルを統合した統合予測ベクトルとその誤差共分散行列である統合予測誤差共分散行列とを算出する予測諸元統合器１１とを備えている。
【００５１】
また、観測値が追尾に有効なデータであるかを判定するゲート判定器１２と、観測値に基づいて、それぞれの第１〜第Ｎの運動モデルの尤度を算出する第１〜第Ｎの運動モデルの尤度算出器１３１〜１３３と、尤度の総和を算出するモデル尤度算出器１４とを備えている。
【００５２】
さらに、第１〜第Ｎの運動モデルの事後確率（事後信頼度）を算出する第１〜第Ｎの運動モデルの事後確率算出器１５１〜１５３と、第１〜第Ｎの運動モデルによる平滑処理を行う第１〜第Ｎの運動モデル用平滑器１６１〜１６３と、統合平滑値と統合平滑誤差共分散行列とを算出して、表示装置３に出力する平滑諸元統合器１７とを備えている。
【００５３】
このように、目標追尾装置２は、運動モデル制御器７からなる運動モデル制御手段と、それ以外からなる運動諸元推定手段とから構成される。また、運動諸元推定手段は、平滑諸元統合器１７からなる平滑諸元統合手段と、それ以外からなる推定手段とから構成される。
【００５４】
次に、この発明の実施の形態１による動作について説明する。
【００５５】
目標の加速度が正弦波であり、その開始時刻が飛行時間において一様に分布していると仮定した場合、連続時間における目標の運動モデルは、次式のように表される。
【００５６】
【数１１】

【００５７】
ただし、
【００５８】
【数１２】

【００５９】
ここで、ｘ（ｔ）は相対位置、相対速度、目標加速度、目標ジャーク（Ｊｅｒｋ）のｙ成分の真値を表す連続時間状態変数ベクトルを示す。また、駆動雑音ｗ（ｔ）は連続時間の白色雑音過程である。
【００６０】
離散時間における第１〜第Ｎの運動モデル毎の運動モデルは、式（１）の微分方程式の解より、以下の式（１７）のように与えられる。
【００６１】
【数１３】

【００６２】
ただし、
【００６３】
【数１４】

【００６４】
ここで、
【数１５】

は離散時間の状態変数を示し、
【数１６】

は第１〜第Ｎの運動モデル毎の状態遷移行列を示す。
【数１７】

は目標の駆動雑音ベクトルで、平均０、共分散行列Ｑの離散時間白色雑音行列であり、共分散行列Ｑは以下の式より算出される。
【００６５】
【数１８】

【００６６】
ここで、
【数１９】

は駆動雑音のパワースペクトル密度を示す。以上に示した
【数２０】

および
【数２１】

は、加速度周波数
【数２２】

が真である仮説によるため、運動モデルの数と同数となる。
【００６７】
ここで、らせん周波数
【数２３】

が真である仮説は、以下の式（２３）のように表される。
【００６８】
【数２４】

【００６９】
さらに、フィルタが得ることができる観測情報は、目標の相対位置のみであるため、観測方程式は従来の目標追尾装置と同様に、式（４）および式（５）により表される。
【００７０】
図１とともに、図２を参照しながら、この発明の実施の形態１による動作について説明する。図２は、この発明の実施の形態１による動作を示すフローチャートである。
【００７１】
図２において、統合平滑値用メモリ４に各運動モデルの平滑値の初期値と平滑誤差共分散行列の初期値とを設定し、さらに第１〜第Ｎのモデル確率用メモリ６１〜６３に各運動モデルの事後確率の初期値を設定して、追尾処理を開始する（ステップＳ２０１）。以降、新たな観測値が得られる度に、サンプリング時刻ｋを１つ進めて追尾処理が実行される（ステップＳ２１５）。
【００７２】
次に、運動モデル制御器７は、平滑諸元統合器１７より統合平滑値用メモリ４を介して取得した目標の状態変数および事後確率（モデル確率）に基づいて、安定した追尾精度を得るのに必要な運動モデル数と、それぞれの運動モデル設定器８１〜８３に設定する加速度周波数（定数加速度周波数）とを決定し、第１〜第Ｎの運動モデル設定器８１〜８３に出力する（ステップＳ２０２）。
【００７３】
ステップＳ２０２において決定した運動モデル数および加速度周波数（パラメータ）に基づいて、第１〜第Ｎの運動モデル設定器８１〜８３は、状態遷移行列、駆動雑音の誤差共分散行列および定数加速度周波数を設定し、それぞれ対応する第１〜第Ｎの運動モデル予測器９１〜９３へ出力する（ステップＳ２０３）。
【００７４】
第１〜第Ｎの運動モデル予測器９１〜９３は、それぞれに対応する第１〜第Ｎの平滑諸元用メモリ５１〜５３より各運動モデルの１サンプリングタイミング前の平滑値と平滑誤差共分散行列とを読み出し、次式にしたがって、各運動モデル毎の予測ベクトル（予測値）
【数２５】

および予測誤差共分散行列Ｐ _{ｋ，ａ（−）}を算出する（ステップＳ２０４）。
【数２６】

【００７５】
次に、第１〜第Ｎの運動モデルの事前確率算出器１０１〜１０３は、それぞれに対応する第１〜第Ｎの確率用メモリ６１〜６３より、各運動モデルの１サンプリングタイミング前の事後確率を読み出し、次式により、各運動モデル毎の事前確率を算出する（ステップＳ２０５）。
【００７６】
【数２７】

【００７７】
ここで、μ_ｋ，ａ（−）は事前信頼度、μ_ｋ，ａ（＋）は事後信頼度、Ｐａｂはモデルｂからモデルａへの推移確率を示す。
【００７８】
次に、予測諸元統合器１１は、各運動モデルの予測値、予測誤差共分散行列、運動モデルの事前確率を読み込み、１サンプリングタイミング前までの観測値Ｚ^ｋ−１に基づいて、各運動モデルの仮説毎の予測ベクトルおよび予測誤差共分散行列を統合した統合予測ベクトルおよび統合予測誤差共分散行列を、次式にしたがって算出する（ステップＳ２０６）。
【００７９】
【数２８】

【００８０】
次に、目標観測装置（センサ）１より観測値が入力されると（ステップＳ２０７）、ゲート判定器１２は、式（８）および式（９）に示す判定式に基づいて、観測値が有効なデータであるかどうかの判定を行い、極端に観測精度の悪いデータを除外する（ステップＳ２０８）。
【００８１】
ゲート判定器１２により観測値が有効なデータであると判定された場合、第１〜第Ｎの運動モデルの尤度算出器１３１〜１３３は、観測値と、それぞれに対応する運動モデルの事前確率、予測値および予測誤差共分散行列とを読み込み、次式により、各運動モデル毎の尤度を算出する（ステップＳ２０９）。
【００８２】
【数２９】

【００８３】
ここで、ｇ（ｚ；ａ，Ａ）は平均ａ、共分散行列Ａの３変量正規分布Ｚにおける確率密度関数を示す。また、Ｒｋは観測雑音ベクトルの共分散行列を示す。
【００８４】
次に、モデル尤度集計器１４は、第１〜第Ｎの各運動モデル毎の尤度を集計して、合計値（尤度の総和）を算出し（ステップＳ２１０）、第１〜第Ｎの各運動モデル毎の事後確率算出器１５１〜１５３は、尤度の合計値に基づいて、第１〜第Ｎの各運動モデル毎の事後確率を算出する（ステップＳ２１１）。
【数３０】

【００８５】
次に、第１〜第Ｎの各モデル用平滑器１６１〜１６３は、それぞれに対応する第１〜第Ｎの各モデル用予測器９１〜９３より、各運動モデルの予測値と予測誤差共分散行列とを読み込み、次式にしたがって、各運動モデル毎の平滑値、平滑誤差共分散行列およびカルマンゲインを算出する（ステップＳ２１２）。
【００８６】
【数３１】

【００８７】
さらに、平滑諸元統合器１７は、各運動モデルの平滑値、平滑誤差共分散行列、および事後確率を取得し、次式にしたがって、各運動モデルの平滑値および平滑誤差共分散行列を統合した統合平滑値および統合平滑誤差共分散行列を算出する（ステップＳ２１３）。
【００８８】
【数３２】

【００８９】
さらに、各運動モデルの事後確率および各運動モデルの定数加速度周波数を用いて、以下の式（３６）より加速度周波数推定値を算出する。
【００９０】
【数３３】

【００９１】
ここで、平滑諸元統合器１７は、運動モデル数が所定値よりも多い場合には、各運動モデルの事後確率（適合度）より最適な運動モデルを選択し、その最適モデルの平滑値を統合平滑値として出力する。
【００９２】
各サンプリング時刻における処理が追尾終了か否かを判定する（ステップＳ２１４）。ステップＳ２１４において、終了でないと判定された場合（すなわち、ＮＯ）、ステップＳ２１５を経由して、ステップＳ２０２に戻る。また、終了と判定された場合、図２の処理ルーチンを終了する。
【００９３】
このように、それぞれ異なる加速度周波数を持つ複数の運動モデルを同時に使用するとともに、観測情報より各運動モデルに対する事後確率を算出して重み付け統合する方式を取り入れ、相対位置、相対速度、目標加速度、目標ジャーク、目標加速度周波数の目標諸元の推定値を算出する。
【００９４】
以上のように、目標の加速度周波数が時間により変動する場合においても、追尾精度を劣化させることなく、精度良く振動目標の相対位置、相対速度、加速度、目標ジャーク、加速度周波数などを精度良く推定することができる。
【００９５】
また、加速度周波数を状態変数に組み入れずに外部入力項とすることにより、揺動目標の運動を線形問題として扱うことができる。
【００９６】
また、目標の状態変数および事後確率に基づいて、適切な運動モデル数と加速度周波数とを決定するので、高い追尾性能を得ることができる。
【００９７】
実施の形態２．
なお、上記実施の形態１では、各運動モデルの１サンプリングタイミング前の平滑値および平滑誤差分散行列に基づいて目標諸元の推定を行ったが、１サンプリングタイミング前の統合平滑値および統合平滑誤差共分散行列を各運動モデルのフィルタへフィードバックさせてもよい。
【００９８】
図３は、この発明の実施の形態２を示すブロック構成図である。図３において、前述（図１参照）と同様のものについては、同一符号を付して詳述を省略する。
【００９９】
この発明の実施の形態２による動作は、図２と同様であって詳述を省略し、一部動作の異なるステップＳ２０４だけ説明する。
【０１００】
図２において、第１〜第Ｎの運動モデル用予測器９１〜９３は、それぞれに対応する第１〜第Ｎの平滑諸元用メモリ５１〜５３から１サンプリングタイミング前の統合平滑値と統合平滑誤差共分散行列とを読み出し、次式にしたがって、各運動モデル毎の予測ベクトル
【数３４】

および予測誤差共分散行列Ｐ _{ｋ，ａ（−）}を算出する（ステップＳ２０４）。
【０１０１】
【数３５】

【０１０２】
なお、算出された統合平滑値および統合平滑誤差共分散行列は、次回のサンプリングタイミング時の算出のために、第１〜第Ｎの平滑諸元用メモリ５１〜５３に記録される。
【０１０３】
このように、異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリング間における現在の運動モデルから他の運動モデルへの推移を考慮して、各運動モデル用予測器９１〜９３に１サンプリングタイミング前の統合平滑値および統合平滑誤差共分散行列を入力させて目標諸元の推定を行うことにより、目標の加速度周波数の変化に対する反応が早まり、精度の高い追尾を行うことができる。
【０１０４】
実施の形態３．
なお、上記実施の形態２では、１サンプリングタイミング前の統合平滑値および統合平滑誤差共分散行列に基づいて目標諸元の推定を行ったが、１サンプリングタイミング前の平滑値および平滑誤差共分散行列をミキシングした値に基づいて目標諸元の推定を行ってもよい。
【０１０５】
図４は、この発明の実施の形態３を示すブロック構成図である。図４において、前述（図１、図３参照）と同様のものについては同一符号を付して詳述を省略する。
【０１０６】
図４において、目標追尾装置２は、各運動モデルの平滑値と平滑誤差共分散行列とをミキシングと呼ばれる処理を行う混合平滑器１８を備え、推定手段に含まれる。
【０１０７】
次に、この発明の実施の形態３による動作について説明する。図５は、この発明の実施の形態３による動作を示すフローチャートである。
【０１０８】
図５において、ステップＳ５０１〜Ｓ５０３、ステップＳ５０５〜ステップＳ５１６は、図２のステップＳ２０１〜ステップＳ２０３、ステップＳ２０４〜ステップＳ２１５に対応しており、詳述を省略する。
【０１０９】
各運動モデルが設定されると、混合平滑器１８は、全運動モデルの平滑値および平滑誤差共分散行列を入力して、ミキシングを行い、混合平滑値および混合平滑誤差共分散行列を算出する（ステップＳ５０４）。
【０１１０】
算出された混合平滑値および混合平滑誤差共分散行列は、各運動モデルの平滑諸元用メモリ５１〜５３に記録され、第１〜第Ｎの運動モデル用予測器９１〜９３は、各運動モデルの平滑諸元用メモリ５１〜５３に記録された混合平滑値および混合平滑誤差共分散行列に基づいて、以下の式により各運動モデルの予測値および予測誤差共分散行列を算出する（ステップＳ５０５）。
【０１１１】
【数３６】

【０１１２】
ここで、混合確率μ_{ｋ−１，ｂ｜ａ}、混合平滑値および混合平滑誤差共分散行列は、以下の式より算出される。
【０１１３】
【数３７】

【０１１４】
以上のように、１サンプリング間における運動モデルの推移すべてについて考慮した混合平滑値および混合平滑誤差共分散行列に基づいて、目標諸元の推定を行うので、実施の形態１、２よりも計算処理ステップが増加するが、目標の加速度周波数の急な変化に対応することができ、より精度の高い追尾を行うことができる。
【０１１５】
【発明の効果】
以上のように、この発明によれば、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定される定数加速度周波数を決定する運動モデル制御手段と、運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定手段とを備えたので、目標の加速度周波数が時間により変動する場合においても、追尾精度を劣化させることなく、精度良く振動目標の相対位置、相対速度、加速度、目標ジャーク、加速度周波数などを精度良く推定することのできる目標追尾装置が得られる効果がある。
【０１１６】
また、この発明によれば、運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定手段と、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたので、目標の加速度周波数が変動しても、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１１７】
また、この発明によれば、運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたので、目標の加速度周波数の変化に対する反応が早まり、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１１８】
また、この発明によれば、運動諸元推定手段は、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定手段と、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合手段とを備えたので、目標の加速度周波数の急な変化に対応することができ、より精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１１９】
また、この発明によれば、運動モデル制御手段は、運動諸元推定手段より取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するので、目標の状態変数および事後確率に基づいて、適切な運動モデル数と加速度周波数とを決定し、高い追尾性能を得ることのできる目標追尾装置が得られる効果がある。
【０１２０】
また、この発明によれば、平滑諸元統合手段は、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１２１】
また、この発明によれば、平滑諸元統合手段は、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾装置が得られる効果がある。
【０１２２】
また、この発明によれば、目標の加速度を正弦波とする運動モデルの数および各運動モデルに設定する定数加速度周波数を決定する運動モデル制御ステップと、それぞれ運動モデルを持つ複数のカルマンフィルタを用いて、各運動モデルの事後確率を算出して、所定のサンプリングタイミング毎に目標の運動諸元を推定する運動諸元推定ステップとを備えたので、目標の加速度周波数が時間により変動する場合においても、追尾精度を劣化させることなく、精度良く振動目標の相対位置、相対速度、加速度、目標ジャーク、加速度周波数などを精度良く推定することのできる目標追尾方法が得られる効果がある。
【０１２３】
また、この発明によれば、運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルにより平滑値を算出する推定ステップと、各運動モデルにより算出された平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたので、目標の加速度周波数が変動しても、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２４】
また、この発明によれば、運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、１サンプリングタイミング前の統合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたので、目標の加速度周波数の変化に対する反応が早まり、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２５】
また、この発明によれば、運動諸元推定ステップは、各運動モデルにそれぞれ異なる定数加速度周波数を設定した複数のカルマンフィルタを並列的に並べて使用し、各運動モデルの１サンプリングタイミング前の平滑値群をミキシング処理された混合平滑値に基づいて、各運動モデルの平滑値を算出する推定ステップと、各運動モデルの平滑値群の中から選択された平滑値、あるいは、平滑値群を統合した統合平滑値を出力する平滑諸元統合ステップとを備えたので、目標の加速度周波数の急な変化に対応することができ、より精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２６】
また、この発明によれば、運動モデル制御ステップは、運動諸元推定ステップより取得した目標の状態変数および事後確率に基づいて、運動モデルの数と定数加速度周波数とを決定するので、目標の状態変数および事後確率に基づいて、適切な運動モデル数と加速度周波数とを決定し、高い追尾性能を得ることのできる目標追尾方法が得られる効果がある。
【０１２７】
また、この発明によれば、平滑諸元統合ステップは、運動モデルの数が所定値よりも多い場合、各運動モデルの事後確率に基づいて運動モデルを選択し、選択された運動モデルの平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【０１２８】
さらに、この発明によれば、平滑諸元統合ステップは、運動モデルの数が所定値以下の場合、各運動モデルの事後確率に基づいて、平滑値群を統合し、統合された統合平滑値を出力するので、精度の高い追尾を行うことのできる目標追尾方法が得られる効果がある。
【図面の簡単な説明】
【図１】この発明の実施の形態１を示すブロック構成図である。
【図２】この発明の実施の形態１による動作を示すフローチャートである。
【図３】この発明の実施の形態２を示すブロック構成図である。
【図４】この発明の実施の形態３を示すブロック構成図である。
【図５】この発明の実施の形態３による動作を示すフローチャートである。
【図６】従来の目標追尾装置を示すブロック構成図である。
【図７】従来の目標追尾装置の動作を示すフローチャートである。
【符号の説明】
１目標観測装置、２目標追尾装置、３表示装置、４統合平滑用メモリ、統合予測値用メモリ、７運動モデル制御器、１１予測諸元統合器、１２ゲート判定器、１４モデル尤度集計器、１７平滑諸元統合器、１８混合平滑器、５１第１の平滑諸元用メモリ、５２第２の平滑諸元用メモリ、５３第Ｎの平滑諸元用メモリ、６１第１の運動モデル確率用メモリ、６２第２の運動モデル確率用メモリ、６３第Ｎの運動モデル確率用メモリ、８１第１の運動モデル設定器、８２第２の運動モデル設定器、８３第Ｎの運動モデル設定器、９１第１の運動モデル用予測器、９２第２の運動モデル用予測器、９３第Ｎの運動モデル用予測器、１０１第１の運動モデルの事前確率算出器、１０２第２の運動モデルの事前確率算出器、１０３第Ｎの運動モデルの事前確率算出器、１３１第１の運動モデル尤度算出器、１３２第２の運動モデル尤度算出器、１３３第Ｎの運動モデル尤度算出器、１５１第１の運動モデル事後確率算出器、１５２第２の運動モデル事後確率算出器、１５３第Ｎの運動モデル事後確率算出器、１６１第１の運動モデル用平滑器、１６２第２の運動モデル用平滑器、１６３第Ｎの運動モデル用平滑器。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a target tracking apparatus and method for estimating motion specifications such as a target position and speed based on observation information about target position information from a sensor or the like.
[0002]
[Prior art]
6 shows “Paul Zarchan and Dimitrios” Filtering Strategies for Spiral Targets ”, AIAA / BMDO Technology Conferencing and Exhibit. FIG.
[0003]
In FIG. 6, reference numeral 1 denotes a target observation apparatus provided with a sensor or the like, and 2 denotes a target tracking for estimating motion specifications such as a target position and speed at predetermined sampling timings based on observation values from the target observation apparatus 1. A device 3 indicates a display device that displays an estimation result of the target tracking device 2.
[0004]
Next, the configuration of the target tracking device 2 will be described. The target tracking device 2 includes a gate determination unit 4 that determines whether the observation value from the target observation device 1 is valid data for tracking, the observation value, the gain matrix, and the smoothed value and smoothing error before one sampling timing. And a smoother 5 that calculates a smooth value and a smooth error covariance based on the covariance matrix.
[0005]
Also, an initial value memory 6 for storing initial values of smooth values and smoothing error covariance matrices, a smoothing specification memory 7 for storing smoothing values and smoothing error covariance matrices before one sampling timing, smoothing values and smoothing A predictor 8 that calculates a prediction value and a prediction error covariance matrix based on an error covariance matrix, and a gain matrix calculator 9 that calculates a gain matrix are provided.
[0006]
Furthermore, a state transition matrix calculator 10 that calculates a state transition matrix by linear approximation of an extended Kalman filter, a first delay circuit that delays the prediction value and the prediction error covariance matrix output from the predictor 8 by unit time, And a second delay circuit having a function similar to that of the first delay circuit.
[0007]
Next, the operation of the conventional target tracking device 2 will be described with reference to FIG. 7 together with FIG. FIG. 7 is a flowchart showing the operation of the conventional target tracking device 2.
[0008]
The conventional target tracking device 2 uses the extended Kalman filter to estimate the relative position between the sensor and the target, the relative speed, the target acceleration, the jerk term that is the first-order differential value of the acceleration, and the motion parameters of the target acceleration frequency. To do.
[0009]
Assuming that the target acceleration is a sine wave and the start time is uniformly distributed in the flight time, the state equation of the extended Kalman filter is given by the following equation.
[0010]
[Expression 1]

[0011]
In equation (1), y represents the relative position between the sensor and the target, y_tIndicates the target position, and ω indicates the frequency component of the sine wave when the acceleration is a sine wave. The differential value of the acceleration frequency is assumed to be white Gaussian noise. When linearizing around the estimated value under the condition that the nonlinear function f of the nonlinear state equation is smooth, the state transition matrix in continuous time is given by the following equation.
[0012]
[Expression 2]

[0013]
The element of the state transition matrix in continuous time is T for one sampling._sIs used to obtain a state transition matrix in discrete time as follows:
[0014]
[Equation 3]

[0015]
In equation (3),
[Expression 4]

Indicates the estimated acceleration frequency,
[Equation 5]

Indicates the target acceleration estimate.
[0016]
Furthermore, since the observation information that can be obtained by the filter is only the relative position of the target, the observation equation is expressed by the following equation.
[0017]
[Formula 6]

[0018]
In equations (4) and (5), H represents an observation matrix,
[Expression 7]

Is assumed to be a white-time Gaussian noise with an average of 0 and a covariance Rk of discrete-time observation noise.
[0019]
The target relative position, relative speed, target acceleration, target jerk and acceleration frequency are estimated by the state equation and the motion equation shown above.
[0020]
In FIG. 7, the initial value of the smooth value of each motion model and the initial value of the smoothing error covariance matrix are set in the initial value memory 6, and the tracking process is started (step S701). Thereafter, every time a new observation value is obtained, the tracking process is executed with the sampling time advanced by one (step S708).
[0021]
Next, the state transition matrix calculator 10 reads the smoothed value and the smoothing error covariance matrix output from the smoother 5, and calculates the state transition matrix at the discrete time of the extended Kalman filter shown in Expression (3) (step). S702).
[0022]
Next, the predictor 8 reads the smooth value, the smooth error covariance matrix, and the state transition matrix via the state transition matrix calculator 10 and calculates the predicted value and the prediction error covariance matrix from the following equations (step) S703).
[0023]
[Equation 8]

[0024]
Next, the observation value from the target observation device 1 is read (step S704). This observation value is a result of selection of a tracking target by signal detection from the target and gate determination, and is composed of position information, and is called target position observation information.
[0025]
Next, when an observation value (observation information) is input from the sensor, the gate determination unit 4 determines whether the observation value is valid data, and excludes data with extremely poor observation accuracy ( Step S705).
[0026]
This gate determination is performed by the following formula (8) and formula (9) using the predicted value.
[0027]
[Equation 9]

[0028]
Next, the smoother 5 reads the prediction value and the prediction error covariance matrix from the predictor 8, and calculates a smooth value, a smooth error covariance matrix, and a Kalman gain from the following equations (step S706).
[0029]
[Expression 10]

[0030]
When the series of processes described above is completed, it is determined whether or not the end of the tracking process is requested (step S707). In step S707, when it is determined that the end of the process is not requested (that is, NO), the process returns to step S702 via step S708, and the process is repeated. If it is determined that the end of the process is requested (that is, YES), the process routine of FIG. 7 is ended.
[0031]
[Problems to be solved by the invention]
As described above, the conventional target tracking device uses the extended Kalman filter based on the motion model that the acceleration of the swing target is a sine wave and the first-order differential value of the acceleration frequency is white Gaussian noise. Estimate, relative speed, acceleration, jerk and acceleration frequency. However, since the extended Kalman filter treats the nonlinear problem as a linearization problem by Taylor approximation, there is a problem that the tracking accuracy is deteriorated particularly when the observation noise is large or the change of the state variable is large.
[0032]
The present invention has been made to solve the above-described problems, and an object of the present invention is to provide a target tracking apparatus and method capable of obtaining high tracking performance.
[0033]
[Means for Solving the Problems]
The target tracking device according to the present invention uses a motion model control means for determining the number of motion models having a target acceleration as a sine wave and a constant acceleration frequency set in each motion model, and a plurality of Kalman filters having the motion models. Thus, the apparatus includes a motion parameter estimation unit that calculates a posteriori probability of each motion model and estimates a target motion parameter at each predetermined sampling timing.
[0034]
Further, the motion specification estimating means of the target tracking device according to the present invention uses a plurality of Kalman filters in which different constant acceleration frequencies are set for each motion model in parallel, and uses each motion model to calculate a smooth value. And a smooth specification integrating means for outputting a smooth value selected from the smooth value group calculated by each motion model or an integrated smooth value obtained by integrating the smooth value group.
[0035]
Further, the motion specification estimating means of the target tracking device according to the present invention uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and based on the integrated smooth value before one sampling timing. The smoothing unit integrating means for calculating the smooth value of each motion model and the smooth value selected from the smooth value group of each motion model or the integrated smooth value integrating the smooth value group It is equipped with.
[0036]
Further, the motion specification estimating means of the target tracking device according to the present invention uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and smoothes the motion model before one sampling timing. Based on the mixed smooth value obtained by mixing the value group, the estimation means for calculating the smooth value of each motion model and the smooth value selected from the smooth value group of each motion model or the smooth value group are integrated. Smoothing unit integration means for outputting the integrated smoothed value.
[0037]
Further, the motion model control means of the target tracking device according to the present invention determines the number of motion models and the constant acceleration frequency based on the target state variable and the posterior probability acquired from the motion specification estimation means. .
[0038]
Further, the smoothing unit integrating means of the target tracking device according to the present invention selects a motion model based on the posterior probability of each motion model when the number of motion models is greater than a predetermined value, A smooth value is output.
[0039]
In addition, the smooth specification integrating means of the target tracking device according to the present invention integrates smooth value groups based on the posterior probabilities of each motion model when the number of motion models is equal to or less than a predetermined value. A value is output.
[0040]
Further, the target tracking method according to the present invention includes a motion model control step for determining the number of motion models having a target acceleration as a sine wave and a constant acceleration frequency set for each motion model, and a plurality of Kalman filters having the motion models. And a motion parameter estimation step for calculating a posteriori probability of each motion model and estimating a target motion parameter at each predetermined sampling timing.
[0041]
Further, the motion specification estimating step of the target tracking method according to the present invention uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and uses the motion models to calculate a smooth value. And a smooth specification integration step for outputting a smooth value selected from the smooth value group calculated by each motion model or an integrated smooth value obtained by integrating the smooth value group.
[0042]
The motion specification estimation step of the target tracking method according to the present invention uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and is based on an integrated smooth value before one sampling timing. An estimation step for calculating a smooth value of each motion model, and a smooth specification integration step for outputting a smooth value selected from the smooth value group of each motion model or an integrated smooth value obtained by integrating the smooth value group It is equipped with.
[0043]
Further, the motion specification estimation step of the target tracking method according to the present invention uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and smoothes one motion sample before one sampling timing. Based on the mixed smooth value obtained by mixing the value group, the estimation step for calculating the smooth value of each motion model and the smooth value selected from the smooth value group of each motion model or the smooth value group are integrated. And a smooth specification integration step for outputting the integrated smooth value.
[0044]
The motion model control step of the target tracking method according to the present invention determines the number of motion models and the constant acceleration frequency based on the target state variables and posterior probabilities obtained from the motion specification estimation step. .
[0045]
In addition, in the smooth specification integration step of the target tracking method according to the present invention, when the number of motion models is greater than a predetermined value, the motion model is selected based on the posterior probability of each motion model. A smooth value is output.
[0046]
Further, in the smoothing specification integration step of the target tracking method according to the present invention, when the number of motion models is equal to or less than a predetermined value, the smooth value group is integrated based on the posterior probability of each motion model, and the integrated integrated smoothing is performed. A value is output.
[0047]
DETAILED DESCRIPTION OF THE INVENTION
Embodiment 1 FIG.
Hereinafter, the first embodiment of the present invention will be described in detail with reference to the drawings. FIG. 1 is a block diagram showing a first embodiment of the present invention.
[0048]
In FIG. 1, reference numeral 1 denotes a target observation device provided with a sensor, 2 denotes a target tracking device that estimates a target position and speed based on observation values from the

target observation device

1, and 3 denotes a target tracking device 2. The display apparatus which displays an estimation result is shown.
[0049]
Next, the configuration of the target tracking device 2 will be described. The target tracking device 2 includes an integrated smooth value, an integrated smooth error covariance matrix, an integrated smooth value memory 4 for storing the posterior probabilities of the first to Nth motion models, and smoothing by each of the first to Nth motion models. Smoothing specification memories 51 to 53 for storing values and smoothing error covariance matrices, and first to Nth motion model probability memories 61 to 63 for storing posterior probabilities of the first to Nth motion models. And.
[0050]
In addition, the motion model controller 7 that determines the number of motion models and the acceleration frequency set for each motion model, the state transition matrix of the first to Nth motion models, and the first to Nth motion models that set the acceleration frequency Setters 81 to 83, first to Nth motion model predictors 91 to 93 that perform prediction processing based on the first to Nth motion models, and prior probabilities (preliminary trust) of the first to Nth motion models Degree prediction), the first to Nth motion model prior probability calculators 101 to 103, the integrated prediction vector obtained by integrating the prediction vectors based on the first to Nth motion models, and the integrated prediction that is an error covariance matrix. And a prediction specification integrator 11 for calculating an error covariance matrix.
[0051]
In addition, a gate determination unit 12 that determines whether the observed value is effective data for tracking, and first to Nth motion models for calculating the likelihood of each of the first to Nth motion models based on the observed value. There are provided exercise model likelihood calculators 131 to 133 and a model likelihood calculator 14 for calculating the sum of the likelihoods.
[0052]
Further, the posterior probability calculators 151 to 153 for the first to Nth motion models for calculating the posterior probabilities (posterior reliability) of the first to Nth motion models, and smoothing processing by the first to Nth motion models. 1 to Nth motion model smoothers 161 to 163, and a smooth specification integrator 17 that calculates an integrated smooth value and an integrated smoothing error covariance matrix and outputs them to the display device 3. Yes.
[0053]
As described above, the target tracking device 2 includes the motion model control means including the motion model controller 7 and the motion specification estimation means including the other motion model control means. Further, the motion specification estimation means is constituted by smooth specification integration means comprising the smooth specification integrator 17 and estimation means comprising the other.
[0054]
Next, the operation according to the first embodiment of the present invention will be described.
[0055]
Assuming that the target acceleration is a sine wave and the start time is uniformly distributed in the flight time, the target motion model in continuous time is expressed as the following equation.
[0056]
[Expression 11]

[0057]
However,
[0058]
[Expression 12]

[0059]
Here, x (t) represents a continuous time state variable vector representing the true value of the y component of the relative position, relative speed, target acceleration, and target jerk (Jerk). The driving noise w (t) is a continuous time white noise process.
[0060]
The motion model for each of the first to Nth motion models in discrete time is given by the following equation (17) from the solution of the differential equation of equation (1).
[0061]
[Formula 13]

[0062]
However,
[0063]
[Expression 14]

[0064]
here,
[Expression 15]

Indicates discrete-time state variables,
[Expression 16]

Indicates a state transition matrix for each of the first to Nth motion models.
[Expression 17]

Is a target driving noise vector, which is a discrete-time white noise matrix having an average of 0 and a covariance matrix Q. The covariance matrix Q is calculated by the following equation.
[0065]
[Expression 18]

[0066]
here,
[Equation 19]

Indicates the power spectral density of the driving noise. As shown above
[Expression 20]

and
[Expression 21]

Is the acceleration frequency
[Expression 22]

Because of the hypothesis that is true, the number of motion models is the same.
[0067]
Where the spiral frequency
[Expression 23]

The hypothesis that is true is expressed as the following equation (23).
[0068]
[Expression 24]

[0069]
Furthermore, since the observation information that can be obtained by the filter is only the relative position of the target, the observation equation is expressed by Expression (4) and Expression (5) as in the conventional target tracking device.
[0070]
The operation according to the first embodiment of the present invention will be described with reference to FIG. 2 together with FIG. FIG. 2 is a flowchart showing the operation according to the first embodiment of the present invention.
[0071]
In FIG. 2, the initial value of the smooth value of each motion model and the initial value of the smoothing error covariance matrix are set in the integrated smooth value memory 4, and each of the first to Nth model probability memories 61 to 63 is set. The initial value of the posterior probability of the motion model is set, and the tracking process is started (step S201). Thereafter, every time a new observed value is obtained, the tracking process is executed with the sampling time k advanced by one (step S215).
[0072]
Next, the motion model controller 7 obtains stable tracking accuracy based on the target state variable and the posterior probability (model probability) acquired from the smoothing specification integrator 17 via the integrated smoothing value memory 4. The number of motion models necessary for the motion and the acceleration frequencies (constant acceleration frequencies) set in the respective motion model setting devices 81 to 83 are determined and output to the first to Nth motion model setting devices 81 to 83 (steps). S202).
[0073]
Based on the number of motion models and the acceleration frequency (parameter) determined in step S202, the first to Nth motion model setting devices 81 to 83 set a state transition matrix, an error covariance matrix of driving noise, and a constant acceleration frequency. And it outputs to the respectively corresponding 1st-Nth motion model predictors 91-93 (Step S203).
[0074]
The first to Nth motion model predictors 91 to 93 respectively store the smoothed values and smoothing error covariances of the respective motion models one sampling time from the corresponding first to Nth smoothing specification memories 51 to 53. Read out matrix and predict vector (predicted value) for each motion model according to the following formula
[Expression 25]

And prediction error covariance matrixP _{k, a (-)}Is calculated (step S204).
[Equation 26]

[0075]
Next, the prior probability calculators 101 to 103 of the first to Nth motion models respectively use the posterior probabilities of one motion model before one sampling timing from the corresponding first to Nth probability memories 61 to 63. And a prior probability for each motion model is calculated by the following equation (step S205).
[0076]
[Expression 27]

[0077]
Where μ_{k, a}(-) Is prior reliability, μ_{k, a}(+) Indicates the posterior reliability, and Pab indicates the transition probability from the model b to the model a.
[0078]
Next, the prediction specification integrator 11 reads the predicted value of each motion model, the prediction error covariance matrix, and the prior probability of the motion model, and the observed value Z before one sampling timing.^k-1Based on the above, an integrated prediction vector and an integrated prediction error covariance matrix obtained by integrating the prediction vector and prediction error covariance matrix for each hypothesis of each motion model are calculated according to the following equations (step S206).
[0079]
[Expression 28]

[0080]
Next, when an observation value is input from the target observation device (sensor) 1 (step S207), the gate determination unit 12 determines that the observation value is valid based on the determination expressions shown in Expression (8) and Expression (9). It is determined whether or not the data is correct, and data with extremely poor observation accuracy is excluded (step S208).
[0081]
When the gate determination unit 12 determines that the observation value is valid data, the likelihood calculators 131 to 133 of the first to N-th motion models have the observation values and the prior probabilities of the corresponding motion models. Then, the prediction value and the prediction error covariance matrix are read, and the likelihood for each motion model is calculated by the following equation (step S209).
[0082]
[Expression 29]

[0083]
Here, g (z; a, A) represents a probability density function in the trivariate normal distribution Z of the mean a and the covariance matrix A. Rk represents a covariance matrix of observation noise vectors.
[0084]
Next, the model likelihood totalizer 14 totals the likelihood for each of the first to Nth motion models and calculates a total value (total likelihood) (step S210). The posterior probability calculators 151 to 153 for the respective motion models calculate the posterior probabilities for the first to Nth motion models based on the total likelihood values (step S211).
[30]

[0085]
Next, the first to N-th model smoothers 161 to 163 respectively provide predicted values and prediction error covariances of the motion models from the corresponding first to N-th model predictors 91 to 93, respectively. The matrix is read, and the smooth value, smooth error covariance matrix and Kalman gain for each motion model are calculated according to the following equation (step S212).
[0086]
[31]

[0087]
Further, the smoothing specification integrator 17 acquires the smooth value, smoothing error covariance matrix, and posterior probability of each motion model, and integrates the smoothing value and smoothing error covariance matrix of each motion model according to the following equation. An integrated smooth value and an integrated smooth error covariance matrix are calculated (step S213).
[0088]
[Expression 32]

[0089]
Further, using the posterior probability of each motion model and the constant acceleration frequency of each motion model, an acceleration frequency estimated value is calculated from the following equation (36).
[0090]
[Expression 33]

[0091]
Here, when the number of motion models is larger than a predetermined value, the smoothing specification integrator 17 selects an optimal motion model from the posterior probability (fitness) of each motion model, and calculates the smooth value of the optimal model. Output as an integrated smooth value.
[0092]
It is determined whether or not the processing at each sampling time is the end of tracking (step S214). If it is determined in step S214 that the process has not ended (ie, NO), the process returns to step S202 via step S215. If it is determined that the processing is to be terminated, the processing routine of FIG. 2 is terminated.
[0093]
In this way, multiple motion models with different acceleration frequencies are used at the same time, and a posterior probability for each motion model is calculated from observation information and weighted integration is adopted, and the relative position, relative speed, target acceleration, target Estimate target specifications for jerk and target acceleration frequency.
[0094]
As described above, even when the target acceleration frequency varies with time, the relative position, relative speed, acceleration, target jerk, acceleration frequency, etc. of the vibration target can be accurately estimated without degrading the tracking accuracy. be able to.
[0095]
In addition, by using the acceleration frequency as an external input term without incorporating it into the state variable, the motion of the swing target can be treated as a linear problem.
[0096]
Further, since the appropriate number of motion models and acceleration frequency are determined based on the target state variable and the posterior probability, high tracking performance can be obtained.
[0097]
Embodiment 2. FIG.
In the first embodiment, the target specification is estimated based on the smoothed value and the smoothing error variance matrix of each motion model before one sampling timing. However, the integrated smoothing value and the integrated smoothing error before one sampling timing are used. The covariance matrix may be fed back to the filter of each motion model.
[0098]
FIG. 3 is a block diagram showing a second embodiment of the present invention. In FIG. 3, the same components as those described above (see FIG. 1) are denoted by the same reference numerals, and detailed description thereof is omitted.
[0099]
The operation according to the second embodiment of the present invention is the same as that shown in FIG. 2 and will not be described in detail. Only step S204, which is partially different, will be described.
[0100]
In FIG. 2, first to Nth motion model predictors 91 to 93 are integrated smoothing values and integrated smoothings one sampling time before from the corresponding first to Nth smoothing specification memories 51 to 53, respectively. Read error covariance matrix and predict vector for each motion model according to the following formula
[Expression 34]

And prediction error covariance matrixP _{k, a (-)}Is calculated (step S204).
[0101]
[Expression 35]

[0102]
The calculated integrated smoothing value and integrated smoothing error covariance matrix are recorded in the first to Nth smoothing specification memories 51 to 53 for calculation at the next sampling timing.
[0103]
In this way, a plurality of Kalman filters set with different constant acceleration frequencies are used in parallel, and the transition from the current motion model to another motion model during one sampling is taken into consideration, and each motion model predictor 91 -93 input the integrated smoothing value and integrated smoothing error covariance matrix before one sampling timing to estimate the target specifications, thereby speeding up the response to the change in the target acceleration frequency and performing highly accurate tracking. Can do.
[0104]
Embodiment 3 FIG.
In the second embodiment, the target specification is estimated based on the integrated smooth value and integrated smooth error covariance matrix before one sampling timing. However, the smooth value and smooth error covariance matrix before one sampling timing is used. The target specification may be estimated based on a value obtained by mixing the values.
[0105]
FIG. 4 is a block diagram showing a third embodiment of the present invention. 4, the same components as those described above (see FIGS. 1 and 3) are denoted by the same reference numerals, and detailed description thereof is omitted.
[0106]
In FIG. 4, the target tracking device 2 includes a mixed smoother 18 that performs a process called mixing of the smoothed value and smoothing error covariance matrix of each motion model, and is included in the estimation means.
[0107]
Next, an operation according to Embodiment 3 of the present invention will be described. FIG. 5 is a flowchart showing the operation according to the third embodiment of the present invention.
[0108]
5, Steps S501 to S503 and Steps S505 to S516 correspond to Steps S201 to S203 and Steps S204 to S215 in FIG. 2 and will not be described in detail.
[0109]
When each motion model is set, the mixing smoother 18 receives the smooth value and the smoothing error covariance matrix of the entire motion model, performs mixing, and calculates a mixed smoothing value and a mixed smoothing error covariance matrix ( Step S504).
[0110]
The calculated mixed smoothing value and mixed smoothing error covariance matrix are recorded in the smooth specification memory 51-53 of each motion model, and the first to Nth motion model predictors 91-93 are stored in each motion model. Based on the mixed smooth value and the mixed smooth error covariance matrix recorded in the smooth specification memories 51 to 53, the predicted value and the prediction error covariance matrix of each motion model are calculated by the following equations (step S505). .
[0111]
[Expression 36]

[0112]
Where the mixing probability μ_{k-1, b | a}The mixed smoothing value and the mixed smoothing error covariance matrix are calculated from the following equations.
[0113]
[Expression 37]

[0114]
As described above, since the target specification is estimated based on the mixed smoothing value and the mixed smoothing error covariance matrix considering all transitions of the motion model during one sampling, the calculation process is more than that of the first and second embodiments. Although the number of steps increases, it is possible to cope with a sudden change in the target acceleration frequency, and to perform tracking with higher accuracy.
[0115]
【The invention's effect】
As described above, according to the present invention, the motion model control means for determining the number of motion models whose target acceleration is a sine wave and the constant acceleration frequency set for each motion model, and the plurality of Kalman filters having the motion models When the acceleration frequency of the target fluctuates with time, it is equipped with a motion parameter estimation means that calculates the posterior probability of each motion model using the, and estimates the target motion parameters at each predetermined sampling timing. However, there is an effect that a target tracking device capable of accurately estimating the relative position, relative speed, acceleration, target jerk, acceleration frequency, etc. of the vibration target with high accuracy without degrading the tracking accuracy can be obtained.
[0116]
Further, according to the present invention, the motion specification estimating means uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and uses the estimation means for calculating a smooth value by each motion model. Smoothing value integration means for outputting a smooth value selected from the smooth value group calculated by each motion model, or an integrated smooth value obtained by integrating the smooth value group, so that the target acceleration frequency is Even if it fluctuates, there is an effect that a target tracking device that can perform tracking with high accuracy can be obtained.
[0117]
Further, according to the present invention, the motion specification estimating means uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, based on the integrated smooth value before one sampling timing, Estimating means for calculating a smooth value of each motion model, and smooth specification integrating means for outputting a smooth value selected from the smooth value group of each motion model or an integrated smooth value obtained by integrating smooth value groups. Since it is provided, there is an effect that a response to a change in the target acceleration frequency is accelerated and a target tracking device capable of performing tracking with high accuracy can be obtained.
[0118]
Further, according to the present invention, the motion specification estimating means uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel, and uses a smooth value group one sampling time before each motion model. Based on the mixed smoothed value that has been mixed, the estimation means that calculates the smooth value of each motion model, and the smooth value selected from the smooth value group of each motion model, or integration that integrates the smooth value group Since there is provided a smooth specification integration means for outputting a smooth value, it is possible to cope with a sudden change in a target acceleration frequency and to obtain a target tracking device capable of performing tracking with higher accuracy. .
[0119]
Further, according to the present invention, the motion model control means determines the number of motion models and the constant acceleration frequency based on the target state variable and the posterior probability acquired from the motion specification estimation means. Based on the variable and the posterior probability, an appropriate number of motion models and an acceleration frequency are determined, and a target tracking device capable of obtaining high tracking performance is obtained.
[0120]
Further, according to the present invention, when the number of motion models is greater than a predetermined value, the smooth specification integration unit selects a motion model based on the posterior probability of each motion model, and the smooth value of the selected motion model Therefore, there is an effect that a target tracking device that can perform tracking with high accuracy can be obtained.
[0121]
Further, according to the present invention, the smooth specification integrating means integrates the smooth value group based on the posterior probability of each motion model when the number of motion models is equal to or less than a predetermined value, and obtains the integrated integrated smooth value. Since the output is performed, there is an effect that a target tracking device capable of performing tracking with high accuracy can be obtained.
[0122]
According to the present invention, the number of motion models having a target acceleration as a sine wave and the motion model control step for determining the constant acceleration frequency set in each motion model, and a plurality of Kalman filters each having a motion model are used. In addition, since it has a motion parameter estimation step for calculating the posterior probability of each motion model and estimating the target motion parameters at each predetermined sampling timing, even when the target acceleration frequency varies with time, There is an effect of obtaining a target tracking method capable of accurately estimating the relative position, relative speed, acceleration, target jerk, acceleration frequency, etc. of the vibration target with high accuracy without degrading the tracking accuracy.
[0123]
Further, according to the present invention, the motion specification estimation step uses a plurality of Kalman filters set with different constant acceleration frequencies for each motion model in parallel and uses an estimation step for calculating a smooth value by each motion model; And a smooth specification integration step for outputting a smooth value selected from the smooth value group calculated by each motion model or an integrated smooth value obtained by integrating the smooth value group. Even if it fluctuates, there is an effect that a target tracking method capable of performing tracking with high accuracy can be obtained.
[0124]
Further, according to the present invention, the motion specification estimation step uses a plurality of Kalman filters in which different constant acceleration frequencies are set for each motion model in parallel, based on the integrated smooth value before one sampling timing, An estimation step for calculating a smooth value of each motion model and a smooth specification integration step for outputting a smooth value selected from the smooth value group of each motion model or an integrated smooth value obtained by integrating the smooth value group. Since it is provided, there is an effect that a response to a change in the target acceleration frequency is accelerated and a target tracking method capable of performing tracking with high accuracy can be obtained.
[0125]
According to the present invention, the motion specification estimation step uses a plurality of Kalman filters in which different constant acceleration frequencies are set for each motion model in parallel, and uses a smooth value group one sampling timing before each motion model. Based on the mixed smoothed value that has been mixed, the estimation step that calculates the smooth value of each motion model, and the smooth value selected from the smooth value group of each motion model, or integration that integrates the smooth value group And a smooth specification integration step for outputting a smooth value, so that it is possible to cope with a sudden change in the target acceleration frequency and to obtain a target tracking method capable of performing tracking with higher accuracy. .
[0126]
According to the present invention, the motion model control step determines the number of motion models and the constant acceleration frequency based on the target state variable and the posterior probability acquired from the motion specification estimation step. Based on the variable and the posterior probability, an appropriate number of motion models and acceleration frequency are determined, and a target tracking method capable of obtaining high tracking performance is obtained.
[0127]
Further, according to the present invention, when the number of motion models is greater than a predetermined value, the smooth specification integration step selects a motion model based on the posterior probability of each motion model, and the smooth value of the selected motion model Is output, so that it is possible to obtain a target tracking method capable of performing tracking with high accuracy.
[0128]
Further, according to the present invention, the smoothing specification integration step integrates the smooth value group based on the posterior probability of each motion model when the number of motion models is equal to or less than a predetermined value, and calculates the integrated integrated smooth value. Since the data is output, there is an effect that a target tracking method capable of performing tracking with high accuracy can be obtained.
[Brief description of the drawings]
FIG. 1 is a block configuration diagram showing a first embodiment of the present invention.
FIG. 2 is a flowchart showing an operation according to the first embodiment of the present invention.
FIG. 3 is a block configuration diagram showing a second embodiment of the present invention.
FIG. 4 is a block diagram showing a third embodiment of the present invention.
FIG. 5 is a flowchart showing an operation according to Embodiment 3 of the present invention.
FIG. 6 is a block diagram showing a conventional target tracking device.
FIG. 7 is a flowchart showing the operation of a conventional target tracking device.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Target observation apparatus, 2 Target tracking apparatus, 3 Display apparatus, 4 Integrated smoothing memory, Integrated prediction value memory, 7 Motion model controller, 11 Prediction specification integration machine, 12 Gate judgment unit, 14 Model likelihood totalizer , 17 Smoothing element integrator, 18 Mixing smoother, 51 First smoothing specification memory, 52 Second smoothing specification memory, 53 Nth smoothing specification memory, 61 First motion model probability Memory, 62 second motion model probability memory, 63 Nth motion model probability memory, 81 first motion model setter, 82 second motion model setter, 83 Nth motion model setter, 91 Predictor for the first motion model, 92 Predictor for the second motion model, 93 Predictor for the Nth motion model, 101 A priori probability calculator for the first motion model, 102 A priori for the second motion model Probability calculation 103, an Nth motion model prior probability calculator, 131 a first motion model likelihood calculator, 132 a second motion model likelihood calculator, 133 an Nth motion model likelihood calculator, 151 first Motion model posterior probability calculator, 152 second motion model posterior probability calculator, 153 Nth motion model posterior probability calculator, 161 first motion model smoother, 162 second motion model smoother, 163 A smoother for the Nth motion model.

Claims

Motion model control means for determining the number of motion models having a target acceleration as a sine wave and a constant acceleration frequency set in each motion model;
Using a plurality of Kalman filters having the motion model, calculating a posteriori probability of each motion model, and providing motion parameter estimation means for estimating the target motion parameter at each predetermined sampling timing. Feature target tracking device.

The motion item estimation means includes:
A plurality of Kalman filters set with different constant acceleration frequencies for each of the motion models, arranged in parallel, and estimating means for calculating a smooth value by each of the motion models;
5. A smooth specification integration unit that outputs a smooth value selected from the smooth value group calculated by each motion model or an integrated smooth value obtained by integrating the smooth value group. Item 10. The target tracking device according to Item 1.

The motion item estimation means includes:
Wherein using the plurality of Kalman filters that sets the constant acceleration a different frequency to each motion model arranged in parallel, based on the integration smoothed value before one sampling timing, it calculates a smoothed value of each motion model estimation Means,
Claim 1, characterized in that the selected smoothed value from the smoothed value group of each motion model, or, and a smoothing specifications integration means for outputting the integrated smoothed value obtained by integrating the smoothed value group The target tracking device described in 1.

The motion item estimation means includes:
A plurality of Kalman filters set with different constant acceleration frequencies for each motion model are used in parallel, and a smoothing value group before one sampling timing of each motion model is mixed based on a mixed smoothed value. Estimating means for calculating a smooth value of each motion model;
The smoothing specification integration means which outputs the smooth value selected from the smooth value group of each said motion model, or the integrated smooth value which integrated the said smooth value group is provided. The target tracking device described.

The motion model control means includes
On the basis of the state variables and the posterior probability of motion the target obtained from the specifications estimating means, claims 1 to 4, wherein the step of determining the number of the motion model and said constant acceleration frequency The target tracking device according to any one of the above.

The smooth item integrating means is:
The number of the motion models is larger than a predetermined value, the motion model is selected based on the posterior probability of each motion model, and the smooth value of the selected motion model is output. The target tracking device according to any one of claims 2 to 4.

The smooth item integrating means is:
3. The integrated smooth value is output by integrating the smooth value groups based on a posteriori probability of each of the motion models when the number of the motion models is a predetermined value or less. Item 5. The target tracking device according to any one of Items 4 to 4.

A motion model control step for determining the number of motion models having a target acceleration as a sine wave and a constant acceleration frequency set for each motion model;
Using a plurality of Kalman filters having the motion model, calculating a posteriori probability of each motion model and estimating a motion specification of the target at each predetermined sampling timing, Feature target tracking method.

The motion specification estimation step includes:
An estimation step of calculating a smooth value by each of the motion models, using the plurality of Kalman filters set with different constant acceleration frequencies for each of the motion models in parallel.
A smoothing value integration step of outputting a smooth value selected from the smooth value group calculated by each motion model or an integrated smooth value obtained by integrating the smooth value group. Item 9. The target tracking method according to Item 8.

The motion specification estimation step includes:
Wherein using the plurality of Kalman filters that sets the constant acceleration a different frequency to each motion model arranged in parallel, based on the integration smoothed value before one sampling timing, it calculates a smoothed value of each motion model estimation Steps,
Claim 8, characterized in that the selected smoothed value from the smoothed value group of each motion model, or, and a smoothing specifications integration step of outputting the integrated smoothed value obtained by integrating the smoothed value group The target tracking method described in 1.

The motion specification estimation step includes:
A plurality of Kalman filters set with different constant acceleration frequencies for each motion model are used in parallel, and a smoothing value group before one sampling timing of each motion model is mixed based on a mixed smoothed value. An estimation step for calculating a smooth value of each motion model;
9. A smooth specification integration step of outputting a smooth value selected from the smooth value group of each motion model or an integrated smooth value obtained by integrating the smooth value group. The target tracking method described.

The motion model control step includes
On the basis of the state variables and the posterior probability of the target obtained from the motion specification estimation step from claim 8 to claim 11, wherein the step of determining the number of the motion model and said constant acceleration frequency The target tracking method according to any one of the above.

The smooth specification integration step includes:
The number of the motion models is larger than a predetermined value, the motion model is selected based on the posterior probability of each motion model, and the smooth value of the selected motion model is output. target tracking method according to any one of up to nine et claim 11.

The smooth specification integration step includes:
10. When the number of the motion models is equal to or less than a predetermined value, the smooth value groups are integrated based on a posterior probability of each of the motion models, and an integrated integrated smooth value is output. Item 12. The target tracking method according to any one of Items 11 to 11.