JP2012251904A - 追尾装置 - Google Patents

Abstract

【課題】フィルタを簡易に切り替えて早期に目標に追従可能とし、計算量の削減を図る。
【解決手段】等速直線運動モデル、蛇行運動モデル、多重運動モデルに基づくカルマンフィルタを用い、観測値から予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する各カルマンフィルタ処理手段３〜５と、観測値、カルマンフィルタ処理手段３による各値に基づいて、目標が等速直線運動であるかを判定する等速直線運動判定手段７と、観測値、各カルマンフィルタ処理手段による各値に基づいて、各運動モデルに対する尤度を算出する尤度計算手段６と、各運動モデルに対する尤度および等速直線運動判定手段７による判定結果に基づいて、各運動モデルに対する信頼度を算出する信頼度計算手段８と、各運動モデルに対する信頼度に基づいて、各平滑値の加重平均値を算出する加重平均計算手段９とを備えた。
【選択図】図１

Description

この発明は、異なる運動モデルに基づく複数のカルマンフィルタを用いて目標の多様な運動に対処する追尾装置に関するものである。

追尾装置による目標追尾においては、等速直線運動、蛇行運動、蛇行以外（旋廻等）のマニューバ運動等、多様な運動に対処する必要がある。そこで、等速直線運動モデルに基づくカルマンフィルタ、蛇行運動モデルに基づくカルマンフィルタ、および多重運動モデルに基づくカルマンフィルタを用いて、フィルタバンクによる並列処理を行い、目標の運動状態を判別して、目標に対する適切な平滑値を選択（フィルタ選択）する方法がある（例えば特許文献１参照）。

この特許文献１に開示された追尾装置では、取得した目標に対する観測値に基づいて、等速直線運動カルマンフィルタ処理器で、等速直線運動モデルに基づくカルマンフィルタを用いて、平滑値（第１の平滑値）および予測値（第１の予測値）を算出し、蛇行運動カルマンフィルタ処理器で、蛇行運動モデルに基づくカルマンフィルタを用いて、平滑値（第２の平滑値）および予測値（第２の予測値）を算出している。なお、蛇行運動モデルは、等速直線運動モデルおよび正弦関数運動モデルから構成される。
そして、運動判別処理器で、第１，２の予測値に基づいて目標の運動状態を判別し、収束判定器で、第２の平滑値の収束状況を判定している。なお、運動判別処理器では、第１，２の予測値から残差２次形式をそれぞれ算出するとともに、各残差２次形式から各運動モデルの判定評価関数値を求め、これらの判定評価関数値と所定のしきい値とを比較することによって、目標の運動状態を判別している。

そして、フィルタ出力選択器で、運動判別処理器が判別した目標の運動状態、および、収束判定器が判定した収束状況に基づいて、第１の平滑値および第２の平滑値のうちの一方を適切な平滑値として選択する。また、第２の平滑値が収束したと判定した後に、蛇行運動モデルを構成する等速直線運動モデルの平滑速度ベクトルの方向が所定値を超えて変化した場合には、第２の平滑値が未収束であると判定し、多重運動モデルに基づくカルマンフィルタを用いて算出した平滑値を適切な平滑値として選択する。

特開２００５−２７４３００号公報

上述したように、特許文献１で開示された方法では、第１，２の予測値から残差２次形式をそれぞれ算出し、各残差２次形式から各運動モデルの判定評価関数値を求め、判定評価関数値と所定のしきい値とを比較することで、目標の運動状態を判別している。また、蛇行運動モデルに基づくカルマンフィルタを用いて算出した第２の平滑値に対する収束判定を行っている。

しかしながら、追尾開始直後に観測雑音が大きい場合、蛇行運動モデルでの平滑誤差が小さくても、この蛇行運動モデルを構成する等速直線運動モデルと正弦関数運動モデルでの平滑値は安定せず、目標運動を等速直線運動成分と正弦関数運動成分とに分離するのに時間を要する。このような場合、等速直線運動モデルの平滑速度ベクトルが安定しないため、この平滑値を用いて目標の未来位置予測を行った際に、未来予測位置が大きく変動して誤差が大きくなってしまう。そのため、特許文献１では、目標が蛇行運動をしている場合であっても、蛇行運動モデルを構成する等速直線運動モデルの平滑速度ベクトルが安定するまでは、等速直線運動モデルに基づくカルマンフィルタを選択している。そのため、誤差が大きくなってしまうという課題がある。

また、判定評価関数値と所定のしきい値とを比較することによる目標の運動状態の判別は処理が複雑であるという課題がある。さらに、等速直線運動モデルに基づくカルマンフィルタ、蛇行運動モデルに基づくカルマンフィルタおよび多重運動モデルに基づくカルマンフィルタを用いたフィルタバンク方式は計算負荷が大きいという課題がある。

この発明は、上記のような課題を解決するためになされたもので、フィルタを簡易に切り替えることで早期に目標に追従可能であり、また、計算量の削減を図ることができる追尾装置を提供することを目的としている。

この発明に係る追尾装置は、等速直線運動モデルに基づくカルマンフィルタを用いて、取得した目標に対する観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する等速直線運動カルマンフィルタ処理手段と、蛇行運動モデルに基づくカルマンフィルタを用いて、観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する蛇行運動カルマンフィルタ処理手段と、多重運動モデルに基づくカルマンフィルタを用いて、観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する多重運動カルマンフィルタ処理手段と、観測値、および、等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定する等速直線運動判定手段と、観測値、および、各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度を算出する尤度計算手段と、尤度計算手段により算出された各運動モデルに対する尤度および等速直線運動判定手段による判定結果に基づいて、各運動モデルに対する信頼度を算出する信頼度計算手段と、信頼度計算手段により算出された各運動モデルに対する信頼度に基づいて、各カルマンフィルタ処理手段によりそれぞれ算出された平滑値の加重平均値を算出する加重平均計算手段とを備えたものである。

この発明によれば、上記のように構成したので、フィルタを簡易に切り替えることで早期に目標に追従可能であり、また、計算量の削減を図ることができる。

この発明の実施の形態１に係る追尾装置の構成を示すブロック図である。 この発明の実施の形態１における等速直線運動判定器の構成を示すブロック図である。 この発明の実施の形態１に係る追尾装置による動作を示すフローチャートである。 この発明の実施の形態２に係る追尾装置の構成を示すブロック図である。 この発明の実施の形態２における蛇行運動カルマンフィルタ処理器の構成を示すブロック図である。 この発明の実施の形態２に係る追尾装置による動作を示すフローチャートである。 この発明の実施の形態３に係る追尾装置の構成を示すブロック図である。 この発明の実施の形態３における等速直線運動判定器の構成を示すブロック図である。 この発明の実施の形態３における等速直線運動判定を示す図である。 この発明の実施の形態４に係る追尾装置の構成を示すブロック図である。 この発明の実施の形態４における蛇行運動カルマンフィルタ処理器の構成を示すブロック図である。 この発明の実施の形態５に係る追尾装置の構成を示すブロック図である。 この発明の実施の形態５における重み係数設定器の構成を示すブロック図である。 この発明の実施の形態５に係る追尾装置による動作を示すフローチャートである。 この発明の実施の形態５に係る追尾装置において、式（１７）の第１項目が示すサンプリング時刻ごとのトラックスコアの積みあがりを示す図である。

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る追尾装置の構成を示すブロック図である。
追尾装置は、図１に示すように、観測装置１、追尾維持装置２、等速直線運動カルマンフィルタ処理器（等速直線運動カルマンフィルタ処理手段）３、蛇行運動カルマンフィルタ処理器（蛇行運動カルマンフィルタ処理手段）４、多重運動カルマンフィルタ処理器（多重運動カルマンフィルタ処理手段）５、尤度計算器（尤度計算手段）６、等速直線運動判定器（等速直線運動判定手段）７、信頼度計算器（信頼度計算手段）８、加重平均計算器（加重平均計算手段）９および未来位置予測器１０から構成されている。

観測装置１は、アンテナ、赤外線センサや光学カメラ等を用いて、対象となる目標が放射または反射する信号を検出し、この信号に対して信号処理を行って探知データとして出力するものである。

追尾維持装置２は、観測装置１により出力された探知データの中から目標に対する観測値として適したデータをゲート判定処理により抽出し、追尾フィルタ（例えばカルマンフィルタ）による運動諸元の推定および予測を行って、運動諸元予測値（以下、観測値と称す）として出力するものである。

等速直線運動カルマンフィルタ処理器３（以下、カルマンフィルタ処理器３と称す）は、等速直線運動モデルに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻（観測時刻）での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。

蛇行運動カルマンフィルタ処理器４（以下、カルマンフィルタ処理器４と称す）は、蛇行運動モデルに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。ここで蛇行運動とは、所定の基準直線（主軸成分）の近傍を移動する運動であって、基準直線を所定の周期で横切るような運動をいう。したがって蛇行運動は基準直線を横切る周波数（蛇行周波数）と基準直線の傾きおよび切片とで表現される。特にカルマンフィルタが基づいている蛇行運動モデルは、等速直線運動モデルと正弦関数運動モデルとから構成されているものを仮定する。

多重運動カルマンフィルタ処理器５（以下、カルマンフィルタ処理器５と称す）は、蛇行以外（旋廻等）のマニューバ運動に対処する多重運動モデルに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。

尤度計算器６は、追尾維持装置２により出力された観測値、および、各カルマンフィルタ処理器３〜５によりそれぞれ算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデル（等速直線運動モデル、蛇行運動モデルおよび多重運動モデル）に対する尤度をそれぞれ算出するものである。

等速直線運動判定器７は、追尾装置２により出力された観測値、および、カルマンフィルタ処理器３により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定し、その判定結果を信頼度計算器８に出力するものである。この等速直線運動判定器７の構成については後述する。

信頼度計算器８は、尤度計算器６により算出された各運動モデルに対する各尤度および等速直線運動判定器７による判定結果に基づいて、各運動モデルに対する信頼度を算出するものである。ここで、信頼度計算器８は、等速直線運動判定器７により目標の運動状態が等速直線運動であると判定された場合には、等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にする。

加重平均計算器９は、信頼度計算器８により算出された各運動モデルに対する信頼度に基づいて、各カルマンフィルタ処理器３〜５によりそれぞれ算出された平滑値の加重平均値（統合平滑値）を算出するものである。
未来位置予測器１０は、加重平均計算器９により算出された加重平均値に基づいて、外挿法により、Ｎサンプリング時刻後の目標の未来位置を予測するものである。

次に、等速直線運動判定器７の構成について説明する。図２はこの発明の実施の形態１における等速直線運動判定器７の構成を示すブロック図である。
等速直線運動判定器７は、図２に示すように、残差２次形式算出器７１、時間平均計算器（時間平均計算手段）７２、判定器（判定手段）７３およびしきい値設定器７４から構成されている。

残差２次形式算出器７１は、追尾維持装置２により出力された観測値、および、カルマンフィルタ処理器３により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差２次形式を算出するものである。
時間平均計算器７２は、残差２次形式算出器７１により算出された残差２次形式の所定期間分の平均値を算出するものである。

判定器７３は、時間平均計算器７２により算出された残差２次形式の平均値としきい値設定器７４により設定されたしきい値とを比較し、目標の運動状態が等速直線運動であるかを判定するものである。ここで、判定器７３は、残差２次形式の平均値がしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器８に判定結果を出力する。
しきい値設定器７４は、判定器７３で残差２次形式の平均値と比較する際に用いるしきい値を設定するものである。なお、しきい値は任意に設定可能である。

次に、上記のように構成された追尾装置の一連の動作について説明する。図３はこの発明の実施の形態１に係る追尾装置の動作を示すフローチャートである。なお、観測装置１、追尾維持装置２、各カルマンフィルタ処理器３〜５および未来位置予測器１０の動作は従来技術（例えば特許文献１）における動作と同様であるため、その説明を簡略化する。
追尾装置の動作では、図３に示すように、まず、観測装置１は、アンテナ、赤外線センサや光学カメラ等を用いて、対象となる目標が放射又は反射する信号を検出し、この信号に対して信号処理を行って探知データとして出力する（ステップＳＴ３１）。

次いで、追尾維持装置２は、観測装置１により出力された探知データの中から目標に対する観測値として適したデータをゲート判定処理により抽出し、追尾フィルタによる運動諸元の推定および予測を行って観測値として出力する（ステップＳＴ３２）。

次いで、カルマンフィルタ処理器３は、等速直線運動モデルに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する（ステップＳＴ３３）。

また、カルマンフィルタ処理器４は、蛇行運動モデルに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する（ステップＳＴ３４）。

また、カルマンフィルタ処理器５は、多重運動モデルに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する（ステップＳＴ３５）。

次いで、尤度計算器６は、追尾維持装置２により出力された観測値、および、各カルマンフィルタ処理器３〜５によりそれぞれ算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度をそれぞれ算出する（ステップＳＴ３６）。

具体的には、サンプリング時刻をｋ、各運動モデルをａ（＝１〜３）、観測値（観測ベクトル）をｚ_ｋ、予測値（予測ベクトル）をｘ（ハット）_ｋ，ａ（−）、予測誤差共分散行列をＰ_ｋ，ａ（−）、観測誤差共分散行列をＲ_ｋとした場合、尤度計算器６は式（１）を用いて尤度ν_ｋ，ａを算出する。

ここで、ｇ（ｚ_ｋ；ｍ，Ａ）は、平均ｍ、共分散行列Ａの３変量正規分布の観測値ｚ_ｋにおける確率密度関数を示している。また、Ｈ_ａは各カルマンフィルタ処理器３〜５の観測行列、Λ_ｋはレーダの観測座標系からフィルタの座標系への変換行列を示している。

次いで、等速直線運動判定器７は、追尾装置２により出力された観測値、および、カルマンフィルタ処理器３により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定する（ステップＳＴ３７）。

具体的には、カルマンフィルタ処理器３により算出された予測値（予測ベクトル）をｘ（ハット）_ｋ，ｃｖ（−）、予測誤差共分散行列をＰ_ｋ，ｃｖ（−）とした場合、まず、等速直線運動判定器７内の残差２次形式算出器７１は式（２）〜（４）を用いて残差２次形式δ_ｋ，ｃｖを算出する。

ここで、Ｈ_ｃｖはカルマンフィルタ処理器３の観測行列を示している。

そして、時間平均計算器７２は、式（５）を用いて、サンプリング時刻ｋからＴ時刻前までの残差２次形式の平均値δ（バー）_ｋ，ｃｖを算出することで、過去の残差２次形式の時間変化を観測する。

ここで、目標が等速直線運動を行っている場合、等速直線運動モデルに基づく残差２次形式の平均値δ（バー）_ｋ，ｃｖは小さくなる。
そこで、判定器７３は、時間平均計算器７２により算出された残差２次形式の平均値δ（バー）_ｋ，ｃｖと、しきい値設定器７４により設定された所定のしきい値とを比較する。そして、平均値δ（バー）_ｋ，ｃｖがしきい値以下であれば目標の運動状態が等速直線運動であると判定し、平均値δ（バー）_ｋ，ｃｖがしきい値よりも大きければ目標の運動状態が等速直線運動ではないと判定し、その判定結果を信頼度計算器８に出力する。

次いで、信頼度計算器８は、尤度計算器６により算出された各運動モデルに対する尤度および等速直線運動判定器７による判定結果に基づいて、各運動モデルに対する信頼度を算出する（ステップＳＴ３８）。
具体的には、信頼度計算器８は式（６）を用いて信頼度μ_ｋ，ａを算出する。

ここで、μ_ｋ，１は等速直線運動モデルに対する信頼度、μ_ｋ，２は蛇行運動モデルに対する信頼度、μ_ｋ，３は多重運動モデルに対する信頼度を示している。

なお、信頼度計算器８は、等速直線運動判定器７による判定結果が、目標の運動状態が等速直線運動であることを示している場合には、式（６）を用いて算出した信頼度ではなく式（７）に示す信頼度を用いる。

すなわち、等速直線運動モデルに対する信頼度μ_ｋ，１を最大値（＝１）にし、蛇行運動モデルおよび多重運動モデルに対する信頼度μ_ｋ，２，μ_ｋ，３を最小値（＝０）にする。

次いで、加重平均計算器９は、信頼度計算器８により計算された信頼度に基づいて、各カルマンフィルタ処理器３〜５によりそれぞれ算出された平滑値の加重平均値を算出する（ステップＳＴ３９）。
具体的には、加重平均計算器９は式（８）を用いて加重平均値ｘ（ハット）_ｋ（＋）を算出する。

次いで、未来位置予測器１０は、加重平均計算器９により算出された加重平均値に基づいて、外挿法により、Ｎサンプリング時刻後の目標の未来位置を算出する（ステップＳＴ４０）。

ここで、実施の形態１に係る追尾装置では、従来の追尾装置のような運動状態の判別によるフィルタ選択ではなく、各運動モデルに対する信頼度に基づいて、各運動モデルによる平滑値の加重平均値を算出している。そのため、簡易なフィルタ切り替えが可能となり、早期に目標の運動に対し追従できる。
すなわち、例えば等速直線運動を行っている目標が急に旋廻した場合、等速直線運動モデルに基づくカルマンフィルタから多重運動モデルに基づくカルマンフィルタにフィルタを切り替える必要がある。しかしながら、従来の追尾装置では、運動状態が確定してからフィルタが切り替わるため、旋回直後では等速直線運動に基づくカルマンフィルタを選択してしまい、追尾誤差が大きくなってしまう。それに対して、実施の形態１に係る追尾装置では、各運動モデルに対する信頼度に基づいた各平滑値の加重平均値を算出することで、ある程度追尾できているフィルタの出力結果を使用することができ、追尾誤差の増大を抑えることができる。

一方、各運動モデルに対する信頼度を用いる場合、目標が等速直線運動を行っているときには各運動モデル（等速直線運動モデル、蛇行運動モデルおよび多重運動モデル）に対する信頼度に差がなくなり、カルマンフィルタ処理器４およびカルマンフィルタ処理器５により算出された平滑値も加重平均されて、等速直線運動を行っている目標に対して追尾誤差が大きくなってしまう。そこで、実施の形態１に係る追尾装置では、等速直線運動判定器７を設けて、目標の運動状態が等速直線運動であると判定した場合には等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にしている。これにより、等速直線運動を行っている目標に対しても精度よく追尾可能となる。

以上のように、この実施の形態１によれば、運動状態の判別によってフィルタ選択を行うのではなく、各運動モデルに対する信頼度に基づいて、各運動モデルによる平滑値の加重平均値を算出するように構成したので、簡易にフィルタを切り替えることができ、早期に目標に追従可能となる。また、目標の運動状態が等速直線運動の場合には、等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にすることで早期に目標に追従可能となるため、多様な運動に対して早期に追従することができる。

実施の形態２．
図４はこの発明の実施の形態２に係る追尾装置の構成を示すブロック図であり、図５は蛇行運動カルマンフィルタ処理器４ｂ（以下、カルマンフィルタ処理器４ｂと称す）の構成を示すブロック図である。
図４に示す実施の形態２に係る追尾装置は、図１に示す実施の形態１に係る追尾装置からカルマンフィルタ処理器３を削除し、カルマンフィルタ処理器４をカルマンフィルタ処理器４ｂに変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。

カルマンフィルタ処理器４ｂは、実施の形態１におけるカルマンフィルタ処理器４による蛇行運動モデル（等速直線運動モデルおよび正弦関数運動モデル）に基づく目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値の算出・出力処理に加えて、これらの各値の等速直線運動成分（主軸成分）も出力する。
このカルマンフィルタ処理器４ｂは、図５に示すように、複数の周波数別カルマンフィルタ処理器（周波数別カルマンフィルタ処理手段）４１−１〜４１−Ｎ、信頼度計算器４２、統合予測器４３および統合平滑器４４から構成されている。

各周波数別カルマンフィルタ処理器４１−１〜４１−Ｎは、それぞれ異なる所定の蛇行周波数ω１〜ωＮで蛇行する蛇行運動モデル１〜Ｎに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。この各周波数別カルマンフィルタ処理器４１−１〜４１−Ｎはそれぞれ、遅延器４５−１〜４５−Ｎ、混合器４６−１〜４６−Ｎ、予測器４７−１〜４７−Ｎ、平滑器４８−１〜４８−Ｎおよび尤度計算器４９−１〜４９−Ｎから構成されている。
以下では、周波数別カルマンフィルタ処理器４１−１内の各機能部についてのみ説明を行うが、それ以外の周波数別カルマンフィルタ処理器４１−２〜４１−Ｎ内の各機能部についても同様である。

遅延器４５−１は、平滑器４８−１により算出された前回サンプリング時刻でのフィルタゲイン、平滑値および平滑誤差共分散行列を１サンプリング期間保持した後、次回サンプリング時刻にて混合器４６−１に出力するものである。

混合器４６−１は、目標の蛇行運動の蛇行周波数が１次マルコフ過程に基づいて１サンプリング時刻の間に他の蛇行周波数に遷移するとの仮定に従って、各蛇行運動モデル１〜Ｎに対応した遅延器４５−１〜４５−Ｎにより出力された平滑値および平滑誤差共分散行列、および、信頼度計算器４２により算出された各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、混合平滑値および混合平滑誤差共分散行列を算出するものである。

予測器４７−１は、等速直線運動モデルおよび正弦関数運動モデルの統合により表される蛇行運動モデルに従って、混合器４６−１により算出された混合平滑値および混合平滑誤差共分散行列に基づいて、予測値および予測誤差共分散行列を算出するものである。

平滑器４８−１は、追尾維持装置２により出力された観測値、および、予測器４７−１により算出された予測値および予測誤差共分散行列に基づいて、フィルタゲイン、平滑値および平滑誤差共分散行列を算出するものである。

尤度計算器４９−１は、追尾維持装置２により出力された観測値、および、予測器４７−１により算出された予測値および予測誤差共分散行列に基づいて、蛇行運動モデル１に対する尤度を算出するものである。

また、信頼度計算器４２は、各蛇行運動モデル１〜Ｎに対応した尤度計算器４９−１〜４９−Ｎにより算出された尤度および前回サンプリング時刻での事後信頼度に基づいて、各蛇行運動モデル１〜Ｎに対する事後信頼度を算出するものである。

統合予測器４３は、各蛇行運動モデル１〜Ｎに対応した予測器４７−１〜４７−Ｎにより算出された予測値および予測誤差共分散行列、および、信頼度計算器４２により算出された各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出するものである。この演算結果は、カルマンフィルタ４ｂにより算出された予測値および予測誤差共分散行列として尤度計算器６に出力され、また、この演算結果の等速直線運動成分（主軸成分）は、尤度計算器６および等速直線運動判定器７に出力される。

統合平滑器４４は、各蛇行運動モデル１〜Ｎに対応した平滑器４８−１〜４８−Ｎにより算出された平滑値および平滑誤差共分散行列、および、信頼度計算器４２により算出された各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出するものである。この演算結果は、カルマンフィルタ４ｂにより算出された平滑値および平滑誤差共分散行列として加重平均計算器９に出力され、また、この演算結果の等速直線運動成分（主軸成分）は、加重平均計算器９に出力される。

次に、上記のように構成された追尾装置の一連の動作について説明する。なお、図６に示す実施の形態２に係る追尾装置の動作は、図３に示す実施の形態１に係る追尾装観測装置１のステップＳＴ３３，３４をステップＳＴ６１，６２に変更したものであり、それ以外の動作は同様であるため、このステップＳＴ６１，６２の動作についてのみ説明を行う。また以下では、蛇行運動モデルや観測方程式は従来技術（例えば特許文献１）と同様であり、関係する式のみを用いて説明を行う。
なお、追尾維持装置２により出力されたサンプリング時刻ｋでの観測値の状態ベクトルｘ_ｋは式（９）のように定義される。

ここで、ｘ_ｋはサンプリング時刻ｋでの状態ベクトル、ｘ_１，ｋは正弦関数運動成分の位置、ｘ_１，ｋは正弦関数運動成分の速度、ｘ_２，ｋは等速直線運動成分の位置、ｘ_２，ｋは等速直線運動成分の速度を示している。

ステップＳＴ６１において、カルマンフィルタ処理器４ｂは、蛇行運動モデルに基づくカルマンフィルタ処理を行う。具体的には、まず、各周波数別カルマンフィルタ処理器４１−１〜４１−Ｎは、それぞれ異なる所定の蛇行周波数ω１〜ωＮで蛇行する蛇行運動モデル１〜Ｎに基づくカルマンフィルタを用いて、追尾維持装置２により出力された観測値に基づいて、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する。そして、信頼度計算器４２は、各蛇行運動モデル１〜Ｎに対応した各尤度計算器４９−１〜４９−Ｎにより算出された尤度および前回サンプリング時刻での各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、各蛇行運動モデル１〜Ｎに対する事後信頼度を算出する。

そして、統合予測器４３は、各蛇行運動モデル１〜Ｎに対応した予測器４７−１〜４７−Ｎにより算出された予測値および予測誤差共分散行列、および、信頼度計算器４２により算出された各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出する。
具体的には、予測値をｘ（ハット）_ｋ，ａ（−）、予測誤差共分散行列をＰ（ハット）_ｋ，ａ（−）、事前信頼度をβ（ハット）_ｋ，ａ（−）とした場合、統合予測器４３は式（１０）および（１１）を用いて統合予測ベクトルｘ（ハット）_ｋ（−）および統合予測誤差共分散行列Ｐ_ｋ（−）を算出する。

この統合予測器４３により算出された統合予測値および統合予測誤差共分散行列は、カルマンフィルタ４ｂによる予測値および予測誤差共分散行列として尤度計算器６に出力される。

そして、統合平滑器４４は、各蛇行運動モデル１〜Ｎに対応した平滑器４８−１〜４８−Ｎにより算出された平滑値および平滑誤差共分散行列、および、信頼度計算器４２により算出された各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出する。
具体的には、平滑値をｘ（ハット）_ｋ，ａ（＋）、平滑誤差共分散行列をＰ（ハット）_ｋ，ａ（＋）とした場合、統合平滑器４４は式（１２）および（１３）を用いて統合平滑ベクトルｘ（ハット）_ｋ（＋）および統合平滑誤差共分散行列Ｐ_ｋ（＋）を算出する。

なお、カルマンフィルタ処理器４ｂの平滑値としては、正弦関数運動モデルおよび等速直線運動モデルの統合平滑値として構成されるため、式（１４）を用いて両ベクトルを加算したｘ（ハット）_{ｉｎｔ，ａ}（＋）が実際の目標に対する平滑位置および平滑速度ベクトルとなる。

この統合平滑器４４により算出された統合平滑値および統合平滑誤差共分散行列は、カルマンフィルタ４ｂによる平滑値および平滑誤差共分散行列として加重平均計算器９に出力される。

次いで、ステップＳＴ６２において、カルマンフィルタ処理器４ｂ（統合予測器４３および統合平滑器４４）は、蛇行運動モデルを構成する等速直線運動モデルに基づくカルマンフィルタ処理の結果を出力する。
ここで、主軸成分である等速直線運動成分の予測ベクトルｘ（ハット）_{２＿ｃｖ，ｋ}（−）および平滑ベクトルｘ（ハット）_{２＿ｃｖ，ｋ}（＋）は式（１５）および式（１６）のように示される。

以上のように、この実施の形態２によれば、カルマンフィルタ処理器４ｂにて、蛇行運動モデルを構成する等速直線運動モデルに基づくカルマンフィルタ処理の結果をカルマンフィルタ処理器３の代わりに用いるように構成したので、カルマンフィルタ処理器３は不要となり、このカルマンフィルタ処理器３による計算負荷を削減できる。

実施の形態３．
図７はこの発明の実施の形態３に係る追尾装置の構成を示すブロック図であり、図８は等速直線運動判定器７ｂの構成を示すブロック図であり、図９は等速直線運動判定を示す図である。
図７に示す実施の形態３に係る追尾装置は、図４に示す実施の形態２における等速直線運動判定器７を等速直線運動判定器７ｂに変更し、カルマンフィルタ処理器４ｂからの蛇行運動モデルの出力と、その等速直線運動成分（主軸成分）の出力とが等速直線運動判定器７ｂへ入力するように変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。

ここで、図９（ａ）に示すように、目標の運動状態が蛇行運動の場合では、蛇行運動モデルの平滑値（等速直線運動成分（主軸成分）と正弦関数運動成分とを統合した統合平滑値）と、蛇行運動モデルの等速直線運動成分の平滑値との差は大きい。一方、図９（ｂ）に示すように、目標の運動状態が等速直線運動の場合では、その差は小さい。
そこで、等速直線運動判定器７ｂによる等速直線運動判定では、上記特性を利用して、上記差分値が所定のしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器８に判定結果を出力する。

図８に示す等速直線運動判定器７ｂは、図２に示す実施の形態２における等速直線運動判定器７の残差２次形式算出器７１を差分計算器（差分計算手段）７１ｂに変更し、時間平均計算器７２を削除したものである。

差分計算器７１ｂは、カルマンフィルタ処理器４ｂにより算出された目標に対する平滑値と、この平滑値の等速直線運動成分（主軸成分）との差分値を算出するものである。

また、判定器７３は、差分計算器７１ｂにより算出された差分値としきい値設定器７４により設定されたしきい値とを比較し、目標の運動状態が等速直線運動であるかを判定する。ここで、判定器７３は、差分値がしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器８に判定結果を出力する。
また、しきい値設定器７４は、判定器７３で差分値と比較する際に用いるしきい値を設定する。なお、しきい値は任意に設定可能である。

以上のように、この実施の形態３によれば、等速直線運動判定器７ｂにて、カルマンフィルタ処理器４ｂからの蛇行運動モデルの平滑値とその等速直線成分（主軸成分）との差分値がしきい値以下の場合に、目標の運動状態が等速直線運動であると見なすように構成したので、運動状態が等速直線運動である目標に対して、より精度のよい追尾が可能となる。

実施の形態４．
図１０はこの発明の実施の形態４に係る追尾装置の構成を示すブロック図であり、図１１は蛇行運動カルマンフィルタ処理器４ｃ（以下、カルマンフィルタ処理器４ｃと称す）の構成を示すブロック図である。
図１０に示す実施の形態４に係る追尾装置は、図４に示す実施の形態２に係る追尾装置のカルマンフィルタ処理器４ｂをカルマンフィルタ処理器４ｃに変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。

実施の形態２におけるカルマンフィルタ処理器４ｂでは、等速直線運動成分（主軸成分）を統合予測器４３および統合平滑器４４から出力している。この場合、等速直線運動モデルおよび正弦関数運動モデルから構成される蛇行運動モデルに基づいて処理を行うことになる。ここで、追尾開始直後に観測雑音が大きい場合、両モデルを合わせた統合平滑値に係る平滑誤差は小さくても、モデルごとの平滑値は安定しない。そのため、追尾初期において、等速直線運動成分を取り出した際に追尾誤差が大きくなってしまう。

そこで、図１０，１１に示すように、実施の形態４におけるカルマンフィルタ処理器４ｃでは、蛇行運動モデルとして用意されている複数の蛇行周波数ω１〜ωＮの中で最も高い周波数（図１１では、周波数別カルマンフィルタ処理器４１−Ｎの蛇行周波数）の等速直線運動成分（主軸成分）のみを取り出す。すなわち、高周波の蛇行周波数の振幅は低周波の蛇行周波数の振幅よりも小さいため、観測雑音に埋もれると、統合平滑としてはほとんど主軸成分しか残らないことを利用する。

このカルマンフィルタ処理器４ｃでは、蛇行周波数ω１〜ωＮで蛇行する蛇行運動モデル１〜Ｎに基づいて、蛇行運動モデル１〜Ｎごとの予測値および予測誤差共分散行列を算出し、各蛇行運動モデル１〜Ｎに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出する。この統合予測値および統合予測誤差共分散行列は、カルマンフィルタ４ｃによる予測値および予測誤差共分散行列として尤度計算器６に出力される。同様に、蛇行運動モデル１〜Ｎごとの平滑値、平滑誤差共分散行列および事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出する。この統合平滑値および統合平滑誤差共分散行列は、カルマンフィルタ４ｃによる平滑値および平滑誤差共分散行列として加重平均計算器９に出力される。
さらに、カルマンフィルタ処理器４ｃは、蛇行運動モデル１〜Ｎが蛇行周波数ごとのモデルであるとした場合、最も高い蛇行周波数である蛇行運動モデルの等速直線運動成分（主軸成分）を取り出す。この取り出された予測値および予測誤差共分散行列の等速直線運動成分は、尤度計算器６および等速直線運動判定器７に出力され、平滑値および統合平滑誤差共分散行列の等速直線運動成分は、加重平均計算器９に出力される。

以上のように、この実施の形態４によれば、カルマンフィルタ処理器４ｃにて、最も高い蛇行周波数である蛇行運動モデルの等速直線運動成分をカルマンフィルタ処理器３の代わりに用いるように構成したので、追尾初期において等速直線運動成分が十分に安定しない場合であっても、高周波である蛇行運動モデルの等速直線運動成分を取り出すことで、運動状態が等速直線運動である目標に対して、より精度のよい追尾が可能となる。

実施の形態５．
実施の形態１〜４では、各運動モデルに対する信頼度に基づいて各平滑値の加重平均を算出するものについて示したが、実施の形態５では、非特許文献１で用いられているトラックスコア（ｔｒａｃｋ ｓｃｏｒｅ）なる指標値を用いて、各サンプリング時刻での各運動状態（等速直線運動状態、蛇行運動状態または多重運動状態）に対する評価（重み付け）を行い、これに基づいて各平滑値の加重平均を算出するものについて示す。
［非特許文献１］
Ｊ．Ａｒｎｏｌｄ、Ｓ．Ｓｈａｗ，Ｈ． Ｐａｓｔｅｒｎａｃｋ “Ｅｆｆｉｃｉｅｎｔ ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ ｕｓｉｎｇ ｄｙｎａｍｉｃ ｐｒｏｇｒａｍｍｉｎｇ”，ＩＥＥＥ Ｔｒａｎｓ．Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍ，Ｖｏｌ．２９，Ｎｏ．１，ｐｐ．４５６６（１９９３）

図１２はこの発明の実施の形態５に係る追尾装置の構成を示すブロック図であり、図１３は重み係数設定器１１の構成を示すブロック図である。
図１２に示す実施の形態５に係る追尾装置は、図４に示す実施の形態２に係る追尾装置から等速直線運動判定器７を削除し、尤度計算器６および信頼度計算器８を重み係数設定器（重み係数設定手段）１１に変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。なお、実施の形態５に係る追尾装置は、図１に示す実施の形態１の追尾装置に対しても同様に適応可能である。

重み係数設定器１１は、追尾維持装置２により出力された観測値、カルマンフィルタ処理器４ｂ，５により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器４ｂにより算出された各値の等速直線運動成分（図１に示す追尾装置に実施の形態５の追尾装置の構成を適用した場合にはカルマンフィルタ処理器３により算出された各値）に基づいて、各運動モデルに対する重み係数を算出するものである。この重み係数設定器１１は、図１３に示すように、尤度計算器１１１、推移確率設定器（推移確率設定手段）１１２、トラックスコア算出器（トラックスコア算出手段）１１３、重み係数算出器（重み係数算出手段）１１４、残差２次形式算出器（残差２次形式算出手段）１１５、推移確率補正器（推移確率補正手段）１１６および遅延器１１７から構成されている。

尤度計算器１１１は、追尾維持装置２により出力された観測値、カルマンフィルタ処理器４ｂ，５により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器４ｂにより算出された各値の等速直線運動成分に基づいて、今回サンプリング時刻での各運動モデルに対する尤度をそれぞれ計算するものである。

推移確率設定器１１２は、目標が前回サンプリング時刻において所定の運動状態（各推移元運動状態）であった場合に、この推移元運動状態から今回サンプリング時刻において各運動状態へ推移する推移確率を設定するものである。なおこの際、推移確率設定器１１２は、遅延器１１７により出力された補正値に基づいて、推移確率を補正する。

トラックスコア算出器１１３は、尤度計算器１１１により算出された各運動モデルに対する尤度および推移確率設定器１１２により設定された推移確率に基づいて、今回サンプリング時刻での各運動状態に対する評価値（トラックスコア）を算出するものである。このトラックスコア算出器１１３では、まず、今回サンプリング時刻での各運動状態に対して、前回サンプリング時刻での運動状態に対するトラックスコアと、推移確率設定器１１２により設定された前回サンプリング時刻での運動状態から今回サンプリング時刻での運動状態への推移確率との加算値が最大となる前回サンプリング時刻での運動状態（推移元運動状態）を選択する。そして、この推移元運動状態に対するトラックスコア、推移元運動状態からの推移確率および尤度計算器１１１により算出された各運動モデルに対する尤度に基づいて、今回サンプリング時刻での各運動状態に対するトラックスコアを算出する。

重み係数算出器１１４は、トラックスコア算出器１１３により算出された各運動状態に対するトラックスコアに基づいて、各運動モデルに対する重み係数を算出するものである。この重み係数算出器１１４により算出された重み係数は信頼度として加重平均計算器９に出力される。

残差２次形式算出器１１５は、追尾維持装置２により出力された観測値、カルマンフィルタ処理器４ｂ，５により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器４ｂにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する残差２次形式をそれぞれ算出するものである。

推移確率補正器１１６は、残差２次形式算出器１１５により算出された各運動モデルに対する残差２次形式に基づいて、推移確率に対する補正値を算出するものである。
遅延器１１７は、推移確率補正器１１６により算出された前回サンプリング時刻での補正値を１サンプリング期間保持して、今回サンプリング時刻にて推移確率設定器１１２に出力するものである。

次に、上記のように構成された追尾装置の一連の動作について説明する。なお、図１４に示す実施の形態５に係る追尾装置の動作は、図６に示す実施の形態２に係る追尾装観測装置１のステップＳＴ３６〜３８をステップＳＴ１１１に変更したものであり、それ以外の動作は同様であるため、このステップＳＴ１１１についてのみ説明を行う。

ステップＳＴ１１１において、重み係数設定器１１は、追尾維持装置２により出力された観測値、カルマンフィルタ処理器４ｂ，５により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器４ｂにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する重み係数を算出する。

具体的には、まず、観測値をｚ_ｋ、予測値をｘ（ハット）_ｋ（−）、予測誤差共分散行列をＰ_ｋ（−）、観測誤差共分散行列をＲ_ｋとした場合、重み係数設定器１１内の尤度計算器１１１は、式（１）を用いて各運動モデルに対する尤度をそれぞれ算出する。

そして、トラックスコア算出器１１３は、各運動モデルに基づくカルマンフィルタごとに、式（１７）を用いて、今回サンプリング時刻ｋでの各運動状態に対するトラックスコアを算出する。

ここで、Ｘ_{ｋ−１，ｂ}は前回サンプリング時刻ｋ−１での運動状態に対するトラックスコアであり、Ｐ（θ_ｋ，ａ｜θ_ｋ，ｂ）は前回サンプリング時刻ｋ−１での運動状態から今回サンプリング時刻ｋでの運動状態への推移確率、ν_ｋ，ａは各運動モデルに対する尤度である。

式（１７）に示すトラックスコアの算出式は、従来技術（非特許文献１）と基本的な動作は同じであり、前回サンプリング時刻ｋ−１での推移元となる各運動状態の中から、今回サンプリング時刻ｋでの運動状態に対するトラックスコアを最大にする運動状態を選択し、これを推移元運動状態とする。そして、推移元運動状態に対するトラックスコアと、推移元運動状態から今回サンプリング時刻ｋでの運動状態への推移確率と、今回サンプリング時刻ｋでの運動モデルに対する尤度とから今回サンプリング時刻ｋでの運動状態に対するトラックスコアを求める。

図１５は式（１７）の第１項目が示すサンプリングごとのトラックスコアの積みあがりを示したものである。
図１５に示すように、サンプリング時刻０の場合には、各運動状態に対するトラックスコアはすべて０である。また、サンプリング時刻０からサンプリング時刻１となるとき、等速直線運動状態から等速直線運動状態への推移確率は３、等速直線運動状態から蛇行運動状態への推移確率は１、等速直線運動状態から多重運動状態への推移確率は０である。なおこの推移確率は推移確率設定器１１２により設定される。
そして、例えば、サンプリング時刻１での等速直線運動状態に対するトラックスコアを最大にするサンプリング時刻０での運動状態は、推移確率が最も高い等速直線運動状態となる。そして、サンプリング時刻１での等速直線運動状態に対するトラックスコアは、サンプリング時刻０での等速直線運動状態に対するトラックスコアである０と、サンプリング時刻０での等速直線運動状態からサンプリング時刻１での等速直線運動状態への推移確率である３との加算値である３となる。
同様にして、サンプリング時刻１での蛇行運動状態および多重運動状態に対するトラックスコアも算出される。

また、例えば、サンプリング時刻２での蛇行運動状態に対してでは、推移元運動状態としてサンプリング時刻１の等速直線運動状態を選択する。そして、サンプリング時刻２での蛇行運動状態に対するトラックスコアは、サンプリング時刻１での等速直線運動状態に対するトラックスコアである３と、サンプリング時刻１での等速直線運動状態からサンプリング時刻２での蛇行運動状態への推移確率である２との加算値である５となる。
同様にして、サンプリング時刻２での等速直線運動状態および多重運動状態に対するトラックスコアも算出される。
なお図１５は、式（１７）の第１項目の推移確率を考慮したトラックスコアの積みあがりを示したものであるので、実際には第１項目の値と第２項目の尤度との加算値が各運動状態に対するトラックスコアとなる。

そして、重み係数算出器１１４は、トラックスコア算出器１１３により算出された各運動状態に対するトラックスコアに基づいて、式（１８）を用いて各運動モデルに対する重み係数を算出する。

この重み係数算出器１１４により算出された重み係数は信頼度として加重平均計算器９に出力される。

一方、残差２次形式算出器１１５は、追尾維持装置２により出力された観測値、カルマンフィルタ処理器４ｂ，５により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器４ｂにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する残差２次形式をそれぞれ算出する。そして、推移確率補正器１１６は、残差２次形式算出器１１５により算出された各運動モデルに対する残差２次形式に基づいて、推移確率に対する補正値を算出し、遅延器１１７で１サンプリング期間保持された後、推移確率設定器１１２に出力される。

例えば、目標が等速直線運動を継続して行っている場合には、等速直線運動状態に対するトラックスコアはサンプリング時刻ごとに積みあがっていき、等速直線運動モデルに対する重み係数が大きくなっていく。その後、目標が急に旋回した場合、等速直線運動状態に対するトラックスコアは大きく積みあがっているため、多重運動モデルに対する重み係数と比べて等速直線運動モデルに対する重み係数が大きくなってしまう。
一方、多重運動状態に対するトラックスコアが大きくならない場合、等速直線運動モデルに基づくカルマンフィルタの残差２次形式は多重運動モデルに基づくカルマンフィルタの残差２次形式と比べると値が非常に大きくなる。そこで、それぞれのカルマンフィルタの残差２次形式の値の差を利用し、多重運動状態のトラックスコアが大きく積み上がるように推移確率に対する補正を行う。これによって、多重運動モデルに対する重み係数を大きくすることができ、目標が急旋回する場合であっても追従できることが可能となる。

以上のように、この実施の形態５によれば、運動状態の判別ではなく、トラックスコアを用いて、前回サンプリング時刻での推移元の運動状態の中から今回サンプリング時刻での運動状態に対するトラックスコアを最大にする運動状態を選択し、この推移元運動状態に対するトラックスコア、推移元運動状態からの推移確率および今回サンプリング時刻での運動モデルに対する尤度に基づいて、今回サンプリング時刻での運動状態に対するトラックスコアを算出し、各運動モデルに対する重み係数を算出するように構成したので、簡易にフィルタを切り替えることができ、早期に目標に追従可能となる。また、目標の運動状態が等速直線運動であり、各運動モデルに対する信頼度に差がなくなる場合であっても、等速直線運動状態に対するトラックスコアの積みあがりによって等速直線モデルに対する重み係数を大きくすることができる。そのため、早期に目標に追従することができる。さらに、目標が等速直線運動から急に旋廻した場合であっても、推移確率を補正することで多重運動状態に対するトラックスコアを積み上げることができ、多重運動モデルに対する重み係数を大きくすることができ、多様な運動に対しても早期に追従することができる。

なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

１ 観測装置、２ 追尾維持装置、３ 等速直線運動カルマンフィルタ処理器（等速直線運動カルマンフィルタ処理手段）、４，４ｂ，４ｃ 蛇行運動カルマンフィルタ処理器（蛇行運動カルマンフィルタ処理手段）、５ 多重運動カルマンフィルタ処理器（多重運動カルマンフィルタ処理手段）、６ 尤度計算器（尤度計算手段）、７，７ｂ 等速直線運動判定器（等速直線運動判定手段）、８ 信頼度計算器（信頼度計算手段）、９ 加重平均計算器（加重平均計算手段）、１０ 未来位置予測器、１１ 重み係数設定器、４１−１〜４１−Ｎ 周波数別カルマンフィルタ処理器（周波数別カルマンフィルタ処理手段）、４２ 信頼度計算器、４３ 統合予測器、４４ 統合平滑器、４５−１〜４５−Ｎ 遅延器、４６−１〜４６−Ｎ 混合器、４７−１〜４７−Ｎ 予測器、４８−１〜４８−Ｎ 平滑器、４９−１〜４９−Ｎ 尤度計算器、７１ 残差２次形式算出器（残差２次形式算出手段）、７１ｂ 差分計算器（差分計算手段）、７２ 時間平均計算器（時間平均計算手段）、７３ 判定器（判定手段）、７４ しきい値設定器、１１１ 尤度計算器、１１２ 推移確率設定器（推移確率設定手段）、１１３ トラックスコア算出器（トラックスコア算出手段）、１１４ 重み係数算出器（重み係数算出手段）、１１５ 残差２次形式算出器（残差２次形式算出手段）、１１６ 推移確率補正器（推移確率補正手段）、１１７ 遅延器。

Claims (7)

1. 等速直線運動モデルに基づくカルマンフィルタを用いて、取得した目標に対する観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する等速直線運動カルマンフィルタ処理手段と、
   蛇行運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する蛇行運動カルマンフィルタ処理手段と、
   多重運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する多重運動カルマンフィルタ処理手段と、
   前記観測値、および、前記等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、前記目標の運動状態が等速直線運動であるかを判定する等速直線運動判定手段と、
   前記観測値、および、前記各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度を算出する尤度計算手段と、
   前記尤度計算手段により算出された各運動モデルに対する尤度および前記等速直線運動判定手段による判定結果に基づいて、各運動モデルに対する信頼度を算出する信頼度計算手段と、
   前記信頼度計算手段により算出された各運動モデルに対する信頼度に基づいて、前記各カルマンフィルタ処理手段によりそれぞれ算出された平滑値の加重平均値を算出する加重平均計算手段と
   を備えた追尾装置。
2. 前記等速直線運動判定手段は、
   前記観測値、および、前記等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差２次形式を算出する残差２次形式算出手段と、
   前記残差２次形式算出手段により算出された残差２次形式の所定観測分の平均値を算出する時間平均計算手段と、
   前記時間平均計算手段により算出された平均値が所定のしきい値以下の場合に、前記目標の運動状態が等速直線運動であると判定する判定手段とを有し、
   前記信頼度計算手段は、前記等速直線運動判定手段により前記目標の運動状態が等速直線運動であると判定された場合に、前記等速直線運動モデルに対する信頼度を最大値にし、前記蛇行運動モデルおよび前記多重運動モデルに対する信頼度を最小値にする
   ことを特徴とする請求項１記載の追尾装置。
3. 前記等速直線運動カルマンフィルタ処理手段に代えて、
   前記蛇行運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値のそれぞれの等速直線運動成分を用いる
   ことを特徴とする請求項１または請求項２記載の追尾装置。
4. 前記等速直線運動判定手段は、
   前記蛇行運動カルマンフィルタ手段により算出された平滑値および当該平滑値の等速直線成分の差分値を算出する差分計算手段と、
   前記差分計算手段により算出された差分値が所定のしきい値以下の場合に、前記目標の運動状態が等速直線運動であると判定する判定手段とを有し、
   前記信頼度計算手段は、前記等速直線運動判定手段により前記目標の運動状態が等速直線運動であると判定された場合に、前記等速直線運動モデルに対する信頼度を最大値にし、前記蛇行運動モデルおよび前記多重運動モデルに対する信頼度を最小値にする
   ことを特徴とする請求項１または請求項３記載の追尾装置。
5. 前記蛇行運動カルマンフィルタ処理手段は、それぞれ異なる所定の蛇行周波数で蛇行する運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する周波数別カルマンフィルタ処理手段を複数備え、
   前記等速直線運動カルマンフィルタ処理手段に代えて、
   前記各周波数別カルマンフィルタ処理手段のうち、最も高い蛇行周波数に対応した周波数別カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値のそれぞれの等速直線運動成分を用いる
   ことを特徴とする請求項１から請求項４のうちのいずれか１項記載の追尾装置。
6. 等速直線運動判定手段および信頼度計算手段に代えて、
   前記目標が前回観測時刻において各運動状態であった場合に、当該各運動状態から今回観測時刻において各運動状態へ推移する推移確率を設定する推移確率設定手段と、
   今回観測時刻での各運動状態に対して、前回観測時刻での運動状態に対する評価値であるトラックスコアと、前記推移確率設定手段により設定された当該前回観測時刻での運動状態から今回観測時刻での運動状態への推移確率との加算値が最大となる前回観測時刻での運動状態を選択し、当該選択した運動状態に対するトラックスコア、当該選択した運動状態からの推移確率および前記各運動モデルに対する尤度に基づいて、当該今回観測時刻での各運動状態に対するトラックスコアを算出するトラックスコア算出手段と、
   前記トラックスコア算出手段により算出された各運動状態に対するトラックスコアに基づいて各運動モデルに対する重み係数を算出し、各運動モデルに対する信頼度として前記加重平均計算手段に出力する重み係数算出手段と
   を有する重み係数設定手段を備えた
   ことを特徴とする請求項１から請求項５のうちのいずれか１項記載の追尾装置。
7. 前記重み係数設定手段は、
   前記観測値、および、前記各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差２次形式を算出する残差２次形式算出手段と、
   前記残差２次形式算出手段により算出された残差２次形式に基づいて、推移確率に対する補正値を算出する推移確率補正手段とを有し、
   前記推移確率設定手段は、前記推移確率補正手段により算出された補正値に基づいて、前記推移確率を補正する
   ことを特徴とする請求項６記載の追尾装置。

Images