(57)【要約】【課題】            異なる運動モデルに基づく複数のカルマンフィルタを用いて目標の観測値からその目標の未来位置を予測する。【解決手段】        等速直線運動モデルに基づくカルマンフィルタを用いて観測値から第１の平滑値と第１の予測値とを算出するカルマンフィルタ４と、蛇行運動モデルに基づくカルマンフィルタを用いて前記観測値から第２の平滑値と第２の予測値とを算出するカルマンフィルタ５と、第１及び第２の予測値に基づいて前記目標の運動状態を判別する運動判別処理器３と、第２の平滑値の収束状況を判定する収束判定器７と、前記目標の運動状態と前記収束状況とに基づいて、第１の平滑値と第２の平滑値とのいずれかを前記適切な平滑値として選択するフィルタ出力選択器８と、を備えた。【選択図】          図１
2005274300.tif 000002
【特許請求の範囲】
【請求項１】
異なる運動モデルに基づく複数のカルマンフィルタを用いて目標の観測値から複数の平滑値を算出するとともに、これらの平滑値から適切な平滑値を選択して前記目標の未来位置を予測する目標追尾装置において、
等速直線運動モデルに基づくカルマンフィルタを用いて前記観測値から第１の平滑値と第１の予測値とを算出する等速直線運動カルマンフィルタ処理手段と、
蛇行運動モデルに基づくカルマンフィルタを用いて前記観測値から第２の平滑値と第２の予測値とを算出する蛇行運動カルマンフィルタ処理手段と、
第１及び第２の予測値に基づいて前記目標の運動状態を判別する運動判別処理手段と、
第２の平滑値の収束状況を判定する収束判定手段と、
前記運動判別処理手段が判別した前記目標の運動状態と前記収束判定手段が判定した収束状況とに基づいて、第１の平滑値と第２の平滑値とのいずれかを前記適切な平滑値として選択するフィルタ出力選択手段と、
を備えたことを特徴とする目標追尾装置。
【請求項２】
前記運動判別処理手段は、前記第１及び第２の予測値から残差２次形式をそれぞれ算出するとともに、これらの各残差２次形式から前記各運動モデルの判定評価関数値を求め、これらの運動モデルの判定評価関数と所定のしきい値とを比較して、前記目標の運動状態を判別する、
ことを特徴とする請求項１に記載の目標追尾装置。
【請求項３】
前記蛇行運動カルマンフィルタ処理手段は、正弦関数運動成分と等速直線運動成分とを含む蛇行運動モデルに基づくカルマンフィルタを用いて前記平滑値と予測値とを算出し、
前記収束判定手段は、前記蛇行運動モデルを構成する等速直線運動成分の平滑速度ベクトルの方向の所定サンプル間における時間変位に基づいて前記第２の平滑値の収束状況を判定する、
ことを特徴とする請求項１に記載の目標追尾装置。
【請求項４】
前記収束判定手段は、前記第２の平滑値が収束したと判定した場合にあって、さらに第１の平滑値を取得して前記等速直線運動モデルの方向と前記蛇行運動モデルを構成する等速直線運動成分の平滑速度ベクトルの方向との比較を行い、前記第２の平滑値の収束状況を判定する、
ことを特徴とする請求項３に記載の目標追尾装置。
【請求項５】
前記収束判定手段は、前記第２の平滑値から異なるサンプル時点における第１の未来位置と第２の未来位置とをそれぞれ予測し、これら第１の未来位置と第２の未来位置との距離を算出して、この距離に基づいて前記第２の平滑値の収束状況を判定する、
ことを特徴とする請求項１に記載の目標追尾装置。
【請求項６】
複数の定数加速度を有する運動を仮定する多重運動モデルに基づくカルマンフィルタを用いて前記観測値から平滑値と予測値とを算出する多重運動カルマンフィルタ処理手段、
を備えたことを特徴とする請求項１に記載の目標追尾装置。
【請求項７】
前記収束判定手段は、前記第２の平滑値が収束したと判定した後に、前記蛇行運動モデルを構成する等速直線運動成分の平滑速度ベクトルの方向が所定値を超えて変化した場合に前記第２の平滑値が未収束であると判定し、
前記フィルタ出力選択手段は、前記第２の平滑値が収束したと判定された後、前記平滑速度ベクトルの変化の結果として前記第２の平滑値が未収束であると判定された場合に、前記多重運動カルマンフィルタの平滑値を前記最適な平滑値として選択する、
ことを特徴とする請求項６に記載の目標追尾装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は目標の未来位置を予測する目標追尾装置に係るものであり、特に複数の運動モデルの中から適切な運動モデルを選択する技術に関するものである。
【背景技術】
【０００２】
目標追尾フィルタとして、等速直線運動モデルに基づくカルマンフィルタ（例えば非特許文献１）が知られている。このような目標追尾フィルタでは、等速直線運動モデルに基づくカルマンフィルタの平滑位置ベクトル及び平滑速度ベクトルより、Ｎサンプル後の未来位置を予測するのが一般的である。
【０００３】
一方、蛇行目標に対する追尾フィルタとしては、追尾目標が蛇行して運動する場合に、加速度ｘ"(t)を正弦関数ｘ"(t)＝ｎｔｓｉｎ（ωｔ＋φ）で表した運動モデルに基づくカルマンフィルタを用いて、追尾目標の運動諸元を予測していた（例えば、特許文献１や非特許文献２）。ただしここで、ｘは追尾目標の位置であり、ｘ"は時間ｔによる二階微分を表す（ｘ"＝ｄ2ｘ／ｄｔ2）。また、以下の説明において、ｘ'は時間ｔによる一階微分を表すものとする（ｘ'＝ｄｘ／ｄｔ）。またｎｔは蛇行運動の最大振幅、ωは蛇行周波数、φは位相差である。
【０００４】
【特許文献１】特開２００２−１８９５１９「追跡システム、及び、追跡方法」公報  第４図、段落００３８
【非特許文献１】A. Gelb, ed.、 Applied Optimal Estimation, The M.I.T Press、 Cambridge, 1974．
【非特許文献２】"旋回（蛇行）目標追尾を行う多重運動モデルに基づくカルマンフィルタ"、電子情報通信学会総合大会、Ｂ−２−２１、２００３年３月
【発明の開示】
【発明が解決しようとする課題】
【０００５】
従来の目標追尾フィルタを用いて未来位置予測を行う場合、追尾フィルタの平滑位置ベクトル及び平滑速度ベクトルに基づき、目標の運動が所定の運動モデルに従うものと仮定して、外装法を用いて未来位置を算出する。したがって、良好な未来位置予測結果を得るには、目標の運動モデルと実際の運動が一致していることが求められる。そうでない場合、すなわち例えば、目標の運動と一致しない追尾フィルタを使用した場合や目標運動が途中で変化してしまうような場合など、目標の運動モデルと実際の運動とが食い違う場合は、Ｎサンプル後の未来位置予測精度は劣化してしまう。
この発明は、複数の運動モデルの中から最適な運動モデルを選択することにより、より精度の高い未来位置予測を行うことを目的とするものである。
【課題を解決するための手段】
【０００６】
この発明に係る目標追尾装置は、異なる運動モデルに基づく複数のカルマンフィルタを用いて目標の観測値から複数の平滑値を算出するとともに、これらの平滑値から適切な平滑値を選択して前記目標の未来位置を予測する目標追尾装置において、
等速直線運動モデルに基づくカルマンフィルタを用いて前記観測値から第１の平滑値と第１の予測値とを算出する等速直線運動カルマンフィルタ処理手段と、
蛇行運動モデルに基づくカルマンフィルタを用いて前記観測値から第２の平滑値と第２の予測値とを算出する蛇行運動カルマンフィルタ処理手段と、
第１及び第２の予測値に基づいて前記目標の運動状態を判別する運動判別処理手段と、
第２の平滑値の収束状況を判定する収束判定手段と、
前記運動判別処理手段が判別した前記目標の運動状態と前記収束判定手段が判定した収束状況とに基づいて、第１の平滑値と第２の平滑値とのいずれかを前記適切な平滑値として選択するフィルタ出力選択手段と、
を備えたものである。
【発明の効果】
【０００７】
このように、この発明による目標追尾装置によれば、複数の運動モデルに基づくカルマンフィルタによって得られた平滑速度ベクトルを組み合わせて目標の運動判別を行い、未来位置予測を行う上で最も有利な追尾フィルタの出力を選択することとしたので、未来位置を予測精度を向上させることが可能となるのである。
【発明を実施するための最良の形態】
【０００８】
以下、この発明の実施の形態について図を用いて説明する。
実施の形態１．
図１は、この発明の実施の形態１における目標追尾装置の構成を示すブロック図である。図において、観測装置１はこのシステムが未来位置予測を行う対象となる目標が放射又は反射する信号を、アンテナなどを例とするセンサにより検出し、そのセンサによって検出された信号を信号処理して探知データとして出力する装置である。このような装置の例としては電波を処理するレーダ装置があるが、音波信号を処理するソナー装置やその他の装置であってもよい。
【０００９】
追尾維持装置２は、観測装置１により得られた探知データの中から目標からの観測値として適したデータをゲート判定処理により抽出し、追尾フィルタ、例えばカルマンフィルタによる運動諸元の推定及び予測を行って、運動諸元予測値として出力する部位である。なおこの説明及び以下の説明において、部位という語はそのような処理を行う専用の回路又は素子によって構成されるものとするが、汎用のコンピュータとそのコンピュータに所定の処理を実行させるコンピュータプログラムとを組み合わせて構成するようにしても構わない。
【００１０】
追尾維持装置２から出力された運動諸元予測値は、運動判別処理器３、等速直線運動モデルに基づくカルマンフィルタ４（以下、カルマンフィルタ４）、蛇行運動モデルに基づくカルマンフィルタ５（以下、カルマンフィルタ５）、多重運動モデルに基づくカルマンフィルタ６（以下、カルマンフィルタ６）にそれぞれ出力される。
【００１１】
運動判別処理器３は、等速直線運動モデルに基づくカルマンフィルタ４と、蛇行運動モデルに基づくカルマンフィルタ５が出力する処理結果に基づいて、目標の運動状態を分類する部位である。なお運動判別処理器３は運動判別処理手段の例をなすものである。図２は、運動判別処理器３の詳細な構成を示すブロック図である。図において、残差２次形式算出器３１−１はカルマンフィルタ４の処理結果である予測値（実際には複数の値の組み合わせから構成されるベクトル値となる場合がある）、予測誤差共分散行列、観測誤差共分散行列から残差２次形式を算出する部位である。同様に、残差２次形式算出器３１−２はカルマンフィルタ５の処理結果から残差２次形式を算出する部位である。判定評価関数算出器３２−１及び３２−２は、算出された残差２次形式に基づいて、一定サンプリング間における平均値となる判定評価関数を算出する部位である。運動判定器３３は、判定評価関数算出器３２−１及び３２−２より得られた判定評価関数に基づいて、目標の運動状態を判別する部位である。より具体的には、判定評価関数算出器３２−１及び３２−２より得られた判定評価関数を所定のしきい値と比較し、そのしきい値判定結果の組み合わせにより、目標の状態を、「等速直線運動」、「蛇行運動」、「蛇行以外のマニューバ運動」に判別するようになっている。
【００１２】
引き続き、図１に示した構成要素を説明する。カルマンフィルタ４は等速直線運動カルマンフィルタ処理手段の例をなすものであって、等速直線運動モデルに基づいて、追尾維持装置２が出力する運動諸元予測値から各回サンプリングについて予測値、予測誤差共分散行列、観測誤差共分散行列を算出する部位である。図３は、カルマンフィルタ４の詳細な構成を示すブロック図である。図が示すように、カルマンフィルタ４は、遅延器４１、予測器４２、平滑器４３を備えている。遅延器４１は、前回サンプリングの処理結果を１サンプリングの間保持して、今回サンプリングに出力する部位である。このような処理結果には、平滑値及び平滑誤差共分散行列が含まれる。予測器４２は、遅延器４１より得られた１サンプリング前の平滑値及び平滑誤差共分散行列を用いて、等速直線運動モデルに基づく予測値及び予測誤差共分散行列を算出する部位である。ここで算出された予測値及び予測誤差共分散行列は、運動判別処理器３とともに後述する平滑器４３に出力されるようになっている。平滑器４３は、追尾維持装置２より運動諸元予測値を取得して、フィルタゲイン、平滑値及び平滑誤差共分散行列を算出する部位である。これらフィルタゲイン、平滑値及び平滑誤差共分散行列の算出にあたっては、運動諸元予測値に加えて、予測器４２の処理結果が供され、さらに平滑器４３の処理結果は、遅延器４１に戻されるようになっている。
【００１３】
続いてカルマンフィルタ５の構成について説明する。カルマンフィルタ５は蛇行運動カルマンフィルタ処理手段の例をなすものであって、蛇行運動モデルに基づくカルマンフィルタを処理する部位である。ここで蛇行運動とは、一定の直線（基準直線という）の近傍を移動する運動であって、基準直線を所定の周期で横切るような運動をいうものとする。したがって蛇行運動は基準直線を横切る周波数（蛇行周波数）と基準直線の傾き及び切片とで表現される。特にカルマンフィルタ５が基づいている蛇行運動モデルは、等速直線運動成分と正弦関数運動成分とを含んでいるものを仮定する。
【００１４】
目標の追尾に用いる蛇行運動モデルとしては複数の蛇行周波数を予め準備しておくことが現実的である。そこでカルマンフィルタ５は蛇行周波数ω１〜ωＮ（Ｎは１以上の自然数）で蛇行する蛇行運動モデル（以降、それぞれの運動モデルをモデル１〜モデルＮと呼ぶ）に基づいて、追尾維持装置２が出力する運動諸元予測値から各回サンプリングについて予測値、予測誤差共分散行列、観測誤差共分散行列を算出するようになっている。図４は、カルマンフィルタ４の詳細な構成を示すブロック図である。図が示すように、カルマンフィルタ５は所定の蛇行周波数による運動モデル毎にカルマンフィルタを処理する系統（遅延器、混合器、予測器、平滑器）を有している。例えば、蛇行周波数ωＮによる運動モデルに対する処理系統５−Ｎは遅延器５１−Ｎ、混合器５２−Ｎ、予測器５３−Ｎ、平滑器５４−Ｎ（Ｎは１以上の自然数とする）を備えている。
【００１５】
そのうち、遅延器５１−Ｎはカルマンフィルタ４における遅延器４１と同様に、前回サンプリングの処理結果を保持して、今回サンプリングに出力する部位である。混合器５２−Ｎは、目標の蛇行運動の蛇行周波数が１次マルコフ過程に基づいて１サンプリングの間に他のモデルの蛇行周波数に遷移するとの仮定に従って、モデル１〜モデルＮまでの平滑値及び平滑誤差共分散行列、信頼度計算器５７より得られた事後信頼度から、混合平滑値及び混合平滑誤差共分散行列を算出する部位である。予測器５３−Ｎは、等速直線運動と正弦関数運動の合成により表される運動モデルに基づき、混合器５２−Ｎが算出した混合平滑値及び混合平滑誤差共分散行列を用いて、予測値及び予測誤差共分散行列を算出する部位である。平滑器５４−Ｎは、カルマンフィルタ４の平滑器４３と同様に、追尾維持装置２より運動諸元予測値を取得して、フィルタゲイン、平滑値及び平滑誤差共分散行列を算出する部位である。
【００１６】
なお、図４に示したカルマンフィルタ５は、遅延器５１−Ｎ、混合器５２−Ｎ、予測器５３−Ｎ、平滑器５４−Ｎは、個別の蛇行周波数ごとに一つずつ別体に配置する構成としているが、これらの一部あるいは全部の部位を蛇行周波数ω１〜ωＮによる運動モデル間で共有するような構成としても構わない。蛇行周波数ω１〜ωＮによる運動モデル間でこれらの部位を共有する場合は、例えば、セレクタやスイッチを設けて高速にそれぞれの運動モデル間で部位の入出力を切り替えるようにすればよいのである。
【００１７】
尤度計算器５６は、予測器５３−１〜Ｎから出力される予測値及び予測誤差共分散行列と、追尾維持装置２が出力する運動諸元予測値からモデル１〜モデルＮまでの尤度を算出する部位である。信頼度計算器５７は、モデル１〜モデルＮまでの尤度と１サンプリング前の事後信頼度とに基づき、モデル１〜モデルＮまでの事後信頼度を算出する部位である。統合平滑器５９は、平滑器５４−１〜Ｎが出力した運動モデル毎の平滑値及び平滑誤差共分散行列、信頼度計算器より得られた事後信頼度に基づき、統合平滑値及び統合平滑誤差共分散行列を算出する部位である。統合予測器５８は、予測器５３−１〜Ｎが出力した運動モデル毎の予測値及び予測誤差共分散行列、信頼度計算器より得られた事後信頼度と所定のモデル推移確率より、事前信頼度、統合予測値及び統合予測誤差共分散行列を算出する部位である。なお、蛇行運動モデルに基づくカルマンフィルタはある１軸（例えばｘ座標軸）上の蛇行運動を処理するので、２次元空間あるいは３次元空間用には、このような追尾フィルタを２つ又は３つ使用することになる。
【００１８】
続いてカルマンフィルタ６の構成について説明する。カルマンフィルタ６は多重運動カルマンフィルタ処理手段の例をなすものであって、多重運動モデルに基づくカルマンフィルタを処理する部位である。すなわち、カルマンフィルタ６は、１つ以上の加速度α１〜αＮ（Ｎは１以上の自然数）による定数加速度モデルを用いてそれぞれの加速度ごとの予測値を算出し、モデル信頼度により旋回加速度を推定するようになっている。このような多重運動モデルに基づくカルマンフィルタを採用することで、旋回等のマニューバ運動に対する追従性能を向上することができるのである。
【００１９】
図５は、カルマンフィルタ６の詳細な構成を示したブロック図である。図において、遅延器６は前回サンプリングの処理結果を保持して、今回サンプリングに出力する部位である。カルマンフィルタ６は加速度α１〜αＮのそれぞれによる運動モデル（モデル１〜モデルＮという）を用いて予測処理と平滑処理を行うが、カルマンフィルタ６の遅延器６１は複数の運動モデルで共有されるようになっている。遅延器６１が出力する平滑値や平滑誤差共分散行列などの前回サンプリング処理結果は、加速度α１〜αＮのそれぞれの運動モデル毎の処理系統６２−１〜Ｎに出力される。各処理系統６２−１〜Ｎは予測器６３−１〜Ｎをそれぞれ備えており、それぞれの加速度により表される運動モデルに基づいて、予測値及び予測誤差共分散行列を算出する部位である。
【００２０】
尤度計算器６４は、予測器６３−１〜６３−Ｎが出力する予測値及び予測誤差共分散行列、追尾維持装置２から出力される運動諸元予測値からモデル１〜モデルNの尤度を算出する部位である。信頼度計算器６５は、上記運動モデル毎の尤度と１サンプリング前の事後信頼度に基づき、運動モデル毎の事後信頼度を算出する部位である。統合予測器６６は、遅延器６１を介して得られた１サンプリング前の統合平滑値、統合平滑誤差共分散行列、信頼度計算器６５が算出した事前信頼度より、統合予測器及び統合予測誤差共分散行列を算出する部位である。
【００２１】
統合平滑器６７は、統合予測器６６が算出した統合予測値、統合予測誤差共分散行列、信頼度計算器６５が算出した事後信頼度より、フィルタゲイン、統合平滑ベクトル、統合平滑誤差共分散行列を算出する部位である。算出された統合平滑ベクトル、統合平滑誤差共分散行列は遅延器６１に今回サンプリングの算出結果として出力されるとともに、カルマンフィルタ６の出力結果として外部に出力されるようになっている。
【００２２】
収束判定器７は収束判定手段の例をなすものであって、蛇行運動モデルに基づくカルマンフィルタ５が算出した平滑値の安定状況（例えば、平滑速度ベクトルが安定したかどうか）を判定する部位である。等速直線運動モデルに基づくカルマンフィルタ４の平滑速度ベクトルと蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動モデルの平滑速度ベクトルとを比較すると、追尾開始直後において蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動モデルの平滑速度ベクトルが安定するのに時間を要する傾向にある。このため、未収束の段階で未来位置予測に等速直線運動モデルの平滑速度ベクトルを用いると、未来位置予測精度が劣化することとなる。このような問題を避けるために収束判定器７を設けて、カルマンフィルタ５が算出した平滑速度ベクトルの収束判定を行って、未収束の段階にある場合はカルマンフィルタ５が算出した平滑速度ベクトルを使用しないようにしている。
【００２３】
図６は、収束判定器７の詳細な構成を示すブロック図である。図において、速度ベクトル角度算出器７１は、隣接する２個のサンプリング時刻ｋと時刻ｋ−１における等速直線運動モデルの平滑速度ベクトルのなす角度を算出する部位である。角度評価関数算出器７２は、一定のサンプリング数における角度の平均値である角度評価関数を算出する。しきい値判定器７３は、しきい値を角度評価関数が下回った場合、等速直線運動モデルの平滑速度ベクトルは収束したと判定する部位である。判定結果メモリ７４は、角度評価関数及び収束判定結果を記憶する記憶素子又は回路である。
【００２４】
フィルタ出力選択器８はフィルタ出力選択手段の例をなすものであって、運動判別処理器３が出力した運動判別結果に基づき、適切な平滑値を選択する部位である。具体的に言えば、運動判別処理器３が「等速直線運動」と判定した場合は等速直線運動モデルに基づくカルマンフィルタ４の平滑値を、「蛇行運動」と判定した場合は蛇行運動モデルに基づくカルマンフィルタ５の平滑値を、「蛇行運動以外のマニューバ運動」と判定した場合は、多重運動モデルに基づくカルマンフィルタ６の平滑値を選択する部位である。なお、フィルタ出力選択器８は、収束判定器７の判定結果をも採り入れて動作するようになっており、追尾開始後、カルマンフィルタ５の平滑値が収束したと収束判定器７によって判断されるまでは、カルマンフィルタ５の算出結果を採用しないようにしている。未来位置予測器９は、フィルタ出力選択器８より得られた平滑位置及び平滑速度ベクトルより、Ｎサンプリング後の未来位置を外挿法により算出する部位である。
【００２５】
次に、この発明の実施の形態１による目標追尾装置の動作について説明する。これまでの説明からも理解されるように、この目標追尾装置は、等速直線運動モデルに基づくカルマンフィルタ４、蛇行運動モデルに基づくカルマンフィルタ５及び多重運動モデルに基づくカルマンフィルタ６による追尾処理を行い、残差を用いた運動判別処理及び蛇行運動モデルに基づくカルマンフィルタの収束判定結果に基づき、フィルタ出力を選択し、未来位置予測を行う。以下に、各フィルタ及び収束判定器７、フィルタ出力選択器８の動作について説明する。
【００２６】
（等速直線運動モデルに基づくカルマンフィルタ）
まずカルマンフィルタ４における処理について説明する。等速直線運動モデルに基づくカルマンフィルタ４は、各サンプリング時刻において、平滑及び予測ベクトル、平滑及び予測誤差共分散行列を算出する。北基準直交座標系における等速直線運動モデルに基づくカルマンフィルタ４の運動モデルは、式（１）〜（４）によって表される。
【数１】
2005274300.tif 000003
【００２７】
ここで、ｘkはサンプリング時刻ｔkの状態ベクトル、Φiはサンプリング時刻ｔkからｔk+1への状態遷移行列、ｗkはサンプリング時刻ｔkの駆動雑音ベクトル、Γｋは駆動雑音変換行列、τkはサンプリング間隔、Ｉ3×3は３×３の単位行列である。なお、駆動雑音ベクトルｗkは、平均０、共分散行列Ｑiのｎ変量正規分布に従う白色雑音と仮定する。
【００２８】
次に、北基準直交座標系におけるレーダの観測モデルは、式（５）及び式（６）によって表される。
【数２】
2005274300.tif 000004
【００２９】
ここで、ｚkはサンプリング時刻ｔkの観測ベクトル、Ｈは観測行列、Λｋはレーダの観測座標系からフィルタの座標系への変換行列、νkはサンプリング時刻ｔkの観測雑音ベクトルである。
【００３０】
式（１）〜（６）で示した運動モデル及び観測モデルより、以下に示す予測処理及び平滑処理のリカッチ方程式が導出される。サンプリング時刻ｔk-1までの追尾処理により、平滑ベクトルｘ^k-1(+)及び平滑誤差共分散行列Ｐk(+)が得られているとした場合、予測器４２は、式（７）及び（８）に基づいて、予測ベクトルｘ^k(-)は、予測誤差共分散行列Ｐk(-)を算出する。
【数３】
2005274300.tif 000005
なお、文章による説明で用いるｘ^という表記の^という記号は、数式上はｘの上に表示されるものとする。
【００３１】
続いて、平滑器４３は、サンプリング時刻ｔkに得られた観測値ｚkと予測ベクトル及び予測誤差共分散行列より、式（９）及び（１０）に基づいて平滑ベクトルｘ^k(-)、平滑誤差共分散行列Ｐk(+)は、カルマンゲインＫkを算出する。
【数４】
2005274300.tif 000006
なお、Ｒkは観測誤差共分散行列である。以上が等速直線運動モデルに基づくカルマンフィルタ４における処理である。
【００３２】
（蛇行運動モデルに基づくカルマンフィルタ）
蛇行運動モデルに基づくカルマンフィルタ５は、目標位置を、等速直線運動とその運動直線を軸とする正弦関数運動との和として求められる蛇行運動により表現する。図７はｘ−ｔ平面における正弦関数運動モデルと等速直線運動モデル、さらには蛇行運動モデルとの関係を示す図である。符号１０１が示すのは正弦関数運動モデルによる目標位置の軌跡を示した曲線である。符号１０２は等速直線運動モデルによる目標位置の軌跡を示した直線である。そして符号１０３は正弦関数運動モデルと等速直線運動モデルとを合成して得た蛇行運動による軌跡を示した曲線である。このように正弦関数運動モデルと等速直線運動モデルとを合成して得た蛇行運動によって目標位置を表現することで、ｘ軸に対する平均速度が０でない目標物の位置を精度よく表現しつつ、同時に等速直線運動では表現し得ない振動運動までも精度よく表現できるようになるのである。
【００３３】
このような蛇行運動の連続時間における運動モデルは、式（１２）〜（１４）によって表現される。
【数５】
2005274300.tif 000007
ここで、ｘ1(t)は正弦関数運動モデルによる位置成分を、ｘ2(t)は等速直線運動モデルによる位置成分を、ｎtは正弦波の最大振幅を、ωは正弦波の蛇行周波数を表す。また、ａは軸の時間軸に対する傾きを、ｂは切片を表す。 
【００３４】
図８は、式（１２）〜（１４）で表現される蛇行運動モデルをブロック線図として表現した図である。このブロック線図の上段は正弦関数運動モデル、下段が等速直線運動モデルを表している。蛇行関数運動モデルにおいて、蛇行を開始する時刻はランダムであり、白色ガウス雑音により表される。
【００３５】
式（１２）〜（１４）による運動モデルを微分方程式として表現すると、式（１５）及び（１６）となる。
【数６】
2005274300.tif 000008
【００３６】
また、微分方程式（１５）、（１６）を状態空間において表現すると、式（１７）〜（２１）となる。
【数７】
2005274300.tif 000009
ここで、ｘ(t)は等速直線運動成分と正弦関数運動成分の位置及び速度についての状態変数ベクトルを表す。
【００３７】
サンプリング周期をＴとし、サンプリング時刻ｔ＝ｋＴ（ｋ＝０，１，２‥）におけるｘ(t)の値をｘkとする。ｔ0＝ｋＴ、ｔ＝（ｋ＋１）Ｔとすることにより、北基準直交座標系における離散時間運動モデルは式（２２）〜（２４）で表される
【数８】
2005274300.tif 000010
ここで、ｘkはサンプリング時刻ｔkの状態ベクトル、Φiはサンプリング時刻ｔkからｔk+1への状態遷移行列、ｗkはサンプリング時刻ｔkの駆動雑音ベクトル、Γkは駆動雑音変換行列である。なお、駆動雑音ベクトルｗkは、平均０、共分散行列Ｑiのｎ変量正規分布に従う白色雑音と仮定する。
【００３８】
次に、離散時間における目標の観測方程式は、式（２５）及び（２６）により表される。
2005274300.tif 000011
【００３９】
ここで、Ｈ1は観測行列を、Λkは観測雑音の座標変換行列、νkは観測雑音ベクトル、平均０、共分散Ｒkの白色ガウス雑音となる。サンプリング時刻ｔkにおける距離、仰角、方位のそれぞれの観測雑音をνR,k、νE,k、νBy,kとした場合、νkは式（２７）により与えられる。
【数９】
2005274300.tif 000012
【００４０】
式（１２）〜（１７）で与えられる運動モデル及び観測モデルから、カルマンフィルタ５のモデル１〜モデルＮにおける予測処理と平滑処理のリカッチ方程式が導出される。まず平滑器５４−１〜５４−Ｎは、サンプリングｋにおける運動モデル毎の平滑ベクトルｘ^k,a(+)及び平滑誤差共分散行列Ｐk,a(+)を式（２８）〜（３０）によって算出する。
【数１０】
2005274300.tif 000013
ここで、Ｋk,aはカルマンゲイン、Ｈ1は観測行列、Ｒkは観測誤差共分散行列、Λkはレーダの観測座標系からフィルタの座標系への座標変換行列である。平滑器５４−１〜５４−Ｎによって算出されたモデル１〜モデルＮの平滑ベクトルｘ^k,a(-)及び平滑誤差共分散行列Ｐk,a(-)は、それぞれ遅延器５１−１〜５１−Ｎを経由して、混合器５２−１〜５２−Ｎに出力される一方で、統合平滑器５５に出力する。
【００４１】
混合器５２−１〜５２−Ｎは、遅延器５１−１〜５１−Ｎを介して得られた平滑ベクトルｘ^k,a(+)及び平滑誤差共分散行列Ｐk,a(+)から、モデル間の遷移を考慮した混合平滑ベクトルｘ^mixk-1,a(+)及び混合平滑誤差共分散行列Ｐmixk,a(+)を式（３１）及び（３２）に基づいて算出する。
【数１１】
2005274300.tif 000014
ここで、βk,b|aは逆推移確率であり、式（３３）により与えられる。
【数１２】
2005274300.tif 000015
このようにして算出されたモデル１〜モデルＮの混合平滑ベクトル及び混合平滑誤差共分散行列は、モデル１〜モデルＮの予測器５３−１〜５３−Ｎに出力される。
【００４２】
予測器５３−１〜５３−Ｎは、サンプリング時刻ｔk-1までの追尾処理によって得られた混合平滑ベクトルｘ^mixk-1,a(+)及び混合平滑誤差共分散行列Ｐmixk,a(+)に基づいて、サンプリングｋにおけるモデル１〜モデルＮの予測ベクトルｘ^k,a(-)及び予測誤差共分散行列Ｐk,a(-)を式（３４）及び（３５）から算出する。
【数１３】
2005274300.tif 000016
ここで、Φk-1,aは運動モデル毎の状態遷移行列、Ｑk-1,aは運動モデル毎の駆動雑音の誤差共分散行列を表す。Φk-1,aは、式（２４）にモデル毎に設定した蛇行周波数ωを代入することにより得られる。
【００４３】
尤度計算器５６は、予測器５３−１〜５３−Ｎが算出した運動モデル毎の予測ベクトル、予測誤差共分散行列、追尾維持装置２から出力された観測値及び尤度計算器５６が算出した観測誤差共分散行列より、式（３６）に基づいてそれぞれのモデルの尤度を算出する。
【数１４】
2005274300.tif 000017
ここで、ｇ(ｚ；ａ，Ａ)は平均ａ、共分散行列Ａの３変量正規分布のｚにおける確率密度関数を表す。また、Ｒkは観測雑音ベクトルの共分散行列である。
【００４４】
さらに信頼度計算器５７は、運動モデルの事後信頼度βk,aを式（３７）より算出する。
【数１５】
2005274300.tif 000018
【００４５】
ここでνk,aはフィルタ毎の尤度、β^k,a(-)は式（３８）より得られる事前信頼度である。
【００４６】
【数１６】
2005274300.tif 000019
上記において、Ｐa|bは状態遷移確率、βk-1,bは1サンプリング前の事後信頼度である。このようにして算出された事後信頼度は統合平滑器５５及び混合器５２−１〜５２−Ｎに出力され、事前信頼度は統合予測器５８に出力される。
【００４７】
統合平滑器５５は、サンプリングｋにおいて平滑器５４−１〜５４−Ｎが算出した平滑値ｘ^k(+)及び平滑誤差共分散行列Ｐk(+)、信頼度計算器より得られる事後信頼度βk,aから、サンプリングｋにおける統合平滑ベクトルｘ^k(+)及び統合平滑誤差共分散行列Ｐk(+)を式（３９）及び（４０）に基づいて算出する。
【数１７】
2005274300.tif 000020
ここで、βk,aは事後信頼度を表す。
【００４８】
統合平滑ベクトルｘ^k(+)は、等速直線運動モデルと正弦関数運動モデルの統合平滑ベクトルより構成されるため、式（４１）に示すように、両ベクトルを加算したｘ^int,k(+)が統合平滑器５５によって算出される実際の目標の平滑位置及び平滑速度ベクトルとなる。
【数１８】
2005274300.tif 000021
また、推定蛇行周波数は式（４２）より算出される。
【数１９】
2005274300.tif 000022
ここで、ωaは各フィルタに事前に設定される蛇行周波数である。
【００４９】
統合予測器５８は、予測器５３−１〜５３−Ｎが算出した運動モデル毎の予測ベクトルｘ^k,a(-)及び予測誤差共分散行列Ｐk,a(-)、信頼度計算器５７が算出した事前信頼度β^k,a (-)より、統合予測ベクトルｘ^k(-)及び統合予測誤差共分散行列Ｐk(-)を式（４３）及び（４４）に基づいて算出する。
【数２０】
2005274300.tif 000023
ここで、ｘ^k(-)は統合予測ベクトルを、Ｐk(-)は統合予測誤差共分散行列を、β^k,a (-)は運動モデル毎の事前信頼度を表す。以上が蛇行運動モデルに基づくカルマンフィルタ５における処理である。
【００５０】
多重運動モデルに基づくカルマンフィルタ６は、複数の定数加速度モデルに基づく予測値を算出し、各運動モデルの信頼度より加速度推定を行うとともに、統合平滑値を算出する。加速度推定を行うことで、等速直線運動モデルに基づくカルマンフィルタと比べて、特に旋回等のマニューバ運動に対する追随性能が高いという特徴がある。カルマンフィルタ６における目標の運動モデルは、式（４５）に基づいて算出される。
【数２１】
2005274300.tif 000024
ここで、ｘkはサンプリング時刻ｔkの状態ベクトル、Φkはサンプリング時刻ｔkからｔk+1への状態遷移行列、ｗkはサンプリング時刻ｔkの駆動雑音ベクトル、ｕkはサンプリング時刻ｔkの定数加速度ベクトル、Γkは駆動雑音変換行列、Γ'kは加速度ベクトル変換行列である。なお、駆動雑音ベクトルｗkは、平均０、共分散行列Ｑkのｎ変量正規分布に従う白色雑音と仮定する。
【００５１】
また、状態ベクトル及び状態遷移行列は式（４６）及び（４７）に基づいて設定される。
【数２２】
2005274300.tif 000025
観測モデルについては、等速直線運動モデルに基づくカルマンフィルタ４と同様である。
【００５２】
これらの運動モデル及び観測モデルより、カルマンフィルタ６のモデル１〜モデルＮにおける予測処理と平滑処理のリカッチ方程式が導出される。まず予測器６３−１〜６３−Ｎは、サンプリング時刻ｔk-1までの追尾処理により算出される統合平滑ベクトルｘ^k-1(+)及び統合平滑誤差共分散行列Ｐk(+)から、式（４９）及び（５０）に基づいて運動モデル毎の予測ベクトルｘ^k,a(-)、運動モデル毎の予測誤差共分散行列Ｐk,a(-)を算出する。
【数２３】
2005274300.tif 000026
ここで、αaは運動モデル毎の定数加速度ベクトルである。なお、Ｐk,a(-)はモデルによらず、統合予測誤差共分散行列Ｐk(-)と同じである。このようにして算出された運動モデル毎の予測ベクトル及び予測誤差共分散行列は、尤度計算器６４に出力される。
【００５３】
次に尤度計算器６４は、予測器６３−１〜６３−Ｎが算出したた予測ベクトルと予測誤差共分散行列、追尾維持装置２が出力した観測値から、式（５１）によってそれぞれのモデルの尤度を算出する。
【数２４】
2005274300.tif 000027
ここで、ｇ(ｚ；ａ，Ａ)は平均ａ、共分散行列Ａの３変量正規分布のｚにおける確率密度関数を表す。また、Ｒkは観測雑音ベクトルの共分散行列である。
【００５４】
信頼度計算器６５は、尤度計算器６４が算出した各モデルの尤度に基づいて、運動モデルの事後信頼度を式（５２）に基づいて算出する。
【数２５】
2005274300.tif 000028
ここで、νk,aはモデル１〜モデルＮの尤度、β^k,a(-)は式（５３）より得られる事前信頼度である。
【数２６】
2005274300.tif 000029
ここで、Ｐa|bは状態遷移確率、βk-1,bは１サンプリング前の事後信頼度である。ここで算出された事後信頼度は統合平滑器６７に出力され、事前信頼度は統合予測器６６に出力される。
【００５５】
統合予測器６６は、遅延器６１を介して得られた１サンプル前の統合平滑値と統合平滑誤差共分散行列、信頼度計算器６５が算出した事前信頼度を用いて、サンプリングｋにおける統合予測ベクトルｘ^k(-)及び統合予測誤差共分散行列Ｐk(-)を式（５４）〜（５６）に基づいて算出する。
【数２７】
2005274300.tif 000030
ここで、Ｐabはモデル遷移確率、β^k,aは事前信頼度、ｕ^k-1(-)は予測加速度を表す。
【００５６】
統合平滑器６７は、統合予測器６６が算出した統合予測ベクトルｘ^k(-)及び統合予測誤差共分散行列Ｐk(-)、追尾維持装置２が算出した観測値ｚk、そして信頼度計算器６５が算出した事後信頼度βk,aから、式（５７）〜（６０）に基づいて平滑処理を行う。
【数２８】
2005274300.tif 000031
ここで、Ｋkはカルマンゲイン、Ｈは観測行列、Ｒkは観測誤差共分散行列、Λkはレーダの観測座標系からフィルタの座標系への変換行列、ｕ^k-1(+)は加速度推定値を表す。以上が多重運動モデルに基づくカルマンフィルタ６における処理である。
【００５７】
（運動判定処理）
次に運動判別処理器３の動作について説明する。等速直線運動モデルに基づくカルマンフィルタ４が算出した予測値は、運動判別処理器３の残差２次形式算出器３１−１に出力される。ここで、等速直線運動モデルに基づくカルマンフィルタ４が算出した予測値としては、予測ベクトル及び予測誤差共分散行列などがある。また観測誤差共分散行列についても残差２次形式算出器３１−１に出力される。
【００５８】
すると、残差２次形式算出器３１−１は式（６１）〜（６３）に基づいて観測誤差共分散行列より残差２次形式δk,cvを算出する。
【数２９】
2005274300.tif 000032
ここで、ｚkは観測ベクトル、Ｒkは観測誤差共分散行列、Λkはレーダの観測座標系からフィルタの座標系への変換行列を表す。また、Ｈは観測行列、ｘ^k,cv(-)は等速直線運動モデルに基づくカルマンフィルタの統合予測ベクトル、Ｐk,cv(-)は統合予測誤差共分散行列を表す。
【００５９】
同様に、蛇行運動モデルに基づくカルマンフィルタ５の予測値及び予測誤差共分散行列、及び観測誤差共分散行列は、運動判別器３の残差２次形式算出器３１−２に出力される。蛇行運動モデルに基づくカルマンフィルタの残差２次形式δk,wvは、式（６４）〜（６６）に基づいて算出される。
【数３０】
2005274300.tif 000033
ここで、ｚkは観測ベクトル、Ｒkは観測誤差共分散行列、Λkはレーダの観測座標系からフィルタの座標系への変換行列を表す。また、Ｈ1は観測行列、ｘ^k,wv(-)は蛇行運動モデルに基づくカルマンフィルタの統合予測ベクトル、Ｐk,wv(-)は統合予測誤差共分散行列を表す。
【００６０】
蛇行運動モデルに基づくカルマンフィルタはある座標軸で動作するため、２次元あるいは３次元追尾を行う場合、座標軸毎に残差ベクトルεk,wvを算出し、それぞれの残差共分散を対角要素とする残差共分散行列を用いて式（６６）より残差２次形式を算出する。
【００６１】
算出された等速直線運動モデルに基づくカルマンフィルタ及び蛇行運動モデルに基づくカルマンフィルタよりの残差２次形式は、それぞれの判定評価関数算出器３２−１及び３２−２に出力され、一定サンプル数Δにおける残差２次形式の平均値である判定評価関数μcvk及びμwvkが式（６７）及び（６８）によって算出される。
【数３１】
2005274300.tif 000034
ここで、αは忘却係数であって、Δとの関係として式（６９）を満たす。
【数３２】
2005274300.tif 000035
【００６２】
運動判定器３３は、式（６７）及び（６８）から算出された判定評価関数を取得して、しきい値判定を行う。具体的には図９に示すように、それぞれ大きさの異なる２つのしきい値を設定して、判定評価関数がしきい値を超えるか否かの判定を行う。ここで、しきい値ｈ1としてｈ2よりも大きい値を設定し、またしきい値ｈ3としてｈ4よりも大きい値を設定する。これは、式（６７）及び（６８）から算出された判定評価関数がしきい値付近で変動するために、頻繁に切り替えが繰り返されるのを防ぐためである。
【００６３】
また図１０に示すように、等速直線運動モデルに基づくカルマンフィルタ４の判定評価関数は、等速直線運動目標に対しては小さく、蛇行や旋回目標等のマニューバ運動に対しては大きい特性がある。一方、蛇行運動モデルに基づくカルマンフィルタ５は、等速直線運動及び蛇行運動に対しては小さく、それ以外の旋回等の運動に対しては増大する特性がある。
【００６４】
このように、「蛇行運動モデルを用いたカルマンフィルタ」の残差２次形式が、等速及び蛇行運動に対しては小さく、蛇行以外のマニューバ運動に対しては大きいという性質に着目し、「等速直線運動モデルを用いたカルマンフィルタ」の残差２次形式の結果と組み合わせることによって、「等速」「蛇行」「蛇行以外のマニューバ運動」の判定を可能としているのである。
【００６５】
図１１は、運動判別の状態遷移図である。このようにして、図１０の各フィルタの目標運動に対する判定評価関数の特性を利用し、目標の運動状態を、「等速直線運動」、「蛇行運動」、「蛇行運動以外のマニューバ運動」に判別する。判別結果は、フィルタ出力選択器８へ出力する。
【００６６】
このように運動判別処理器３は、各運動モデルの残差２次形式を算出するとともに、これらの各残差２次形式から運動モデルごとの判定評価関数値を求め、これらの運動モデルの判定評価関数と所定のしきい値とを比較して、前記目標の運動状態を判別することとしたので、各サンプルの値を蓄積して逐次処理により平均を算出する処理と等価となり、値が平均化されて、誤判定を抑制することができる。
【００６７】
（収束判定処理）
次に、収束判定器７の動作について説明する。収束判定器７は、蛇行運動モデルに基づくカルマンフィルタ４が出力する平滑速度ベクトルに対する収束判定を行う。カルマンフィルタ４は、等速直線運動モデルと正弦関数運動モデルから導出される蛇行運動に基づいて処理を行うが、追尾開始直後は観測雑音が大きい場合、両モデルを合わせた統合平滑値に係る平滑誤差は小さくても、モデル毎の平滑値は安定せず、目標運動を等速成分と正弦関数成分が分離されるのに時間を要する。このような場合、等速直線運動モデルの平滑速度ベクトルは安定しないため、このような平滑値に基づいて未来位置予測を行うと未来予測位置が大きく変動して誤差が大きくなってしまう。
【００６８】
そこで、収束判定器７では、等速直線運動モデルについて、サンプリング時刻ｋと時刻ｋ−１において算出される２つの平滑速度ベクトルがなす角度（平滑速度ベクトルの方向成分の時間変位）の評価関数を算出し、しきい値と比較することにより、等速直線運動モデルの平滑速度ベクトルについての収束判定を行う。
【００６９】
速度ベクトル角度算出器７１は、図１２に示すようにサンプリング時刻ｋにおける等速直線運動モデルの平滑速度ベクトルとサンプリング時刻ｋ−１における等速直線運動モデルの平滑速度ベクトルとのなす角を算出する。サンプリング時刻ｋにおける等速直線運動モデルの平滑速度ベクトルをν^kとし、サンプリング時刻ｋ−１における等速直線運動モデルの平滑速度ベクトルをν^k-1とすれば、平滑速度ベクトルν^kとν^k-1がなす角度角度θk［°］は式（７０）から算出される。
【数３３】
2005274300.tif 000036
【００７０】
続いて、角度評価関数算出器７２は、角度θkを平均化して式（７１）から角度評価関数μkθを算出し、しきい値判定器７３へ出力する。
【数３４】
2005274300.tif 000037
2005274300.tif 000038
【００７１】
図１３に、蛇行運動モデルに基づくカルマンフィルタにより蛇行目標を追尾した場合の、等速直線運動モデルの平滑速度ベクトル収束判定評価関数についてのシミュレーション結果の一例を示す。横軸は時間、縦軸は収束判定評価関数を表す。追尾開始直後における評価関数は１００°以上あり、平滑速度ベクトルのふらつきが著しい。しかし、時間経過とともに急速に減少し、未来位置予測に使用可能な程度に平滑速度ベクトルが安定する。
【００７２】
そこでしきい値判定器７３は、角度評価関数μθkがしきい値ｈ5を下回る場合、等速直線運動モデルの平滑速度ベクトルは収束していると判定する。その後にしきい値ｈ6を超える場合には、再び等速直線運動モデルの平滑速度ベクトルは収束していないと判定する。なお、しきい値ｈ5にはしきい値ｈ6よりも小さい値が設定される。これは、上述の評価値がしきい値付近で変動する場合に、頻繁に異なる判定が繰り返されるのを防ぐためである。
【００７３】
このように収束判定器７を設けて、蛇行運動モデルの安定度合いを判断することとしたので、カルマンフィルタ５の平滑値が不安定な状態でこの平滑値に基づく未来位置予測を行うことを回避し、結果として安定した未来位置予測を行うことができるのである。また蛇行運動モデルを構成する等速直線運動成分の平滑速度ベクトルの方向の時間変位に基づいて蛇行運動の安定度合いを判断することとしたので、カルマンフィルタ５の平滑値の収束状況を数値化することができ、客観的かつ正確な判断が可能となったのである。
【００７４】
なお、上述した処理の例では、蛇行運動モデルを構成する等速直線運動成分の平滑速度ベクトルの方向の、前回サンプルと今回サンプルとの間の変化（時間変位）に基づいて処理を行うこととした。しかしながら、サンプリングの間隔は前回サンプルと今回サンプルのように隣接するサンプルに限るものではないことはいうまでもない。
【００７５】
（フィルタ出力選択処理）
フィルタ出力選択器８は、運動判別処理器３が出力した運動判別結果と、収束判定器７が出力した収束状況に関する判定結果をもとに、カルマンフィルタ４〜６が出力した平滑値のうちいずれか一つを選択し、未来位置予測器９へ出力する。フィルタ出力選択器８がどのカルマンフィルタの値を選択するかについては図１４に示している。図が示すように、運動判別処理器３の判定結果が等速運動である場合には、等速直線運動モデルに基づくカルマンフィルタ４による平滑値を選択する。また運動判別処理器３の判定結果が蛇行運動である場合には、収束判定器７の判定結果を参照して、それが未収束である場合には、等速直線運動モデルに基づくカルマンフィルタ４による平滑値を選択する。また収束である場合には、蛇行運動モデルに基づくカルマンフィルタ５の平滑値を選択する。運動判別処理器３の判定結果が蛇行以外のマニューバである場合には、多重運動モデルに基づくカルマンフィルタ６による平滑値を選択する。
【００７６】
このように、追尾開始後一度も蛇行運動モデルに基づくカルマンフィルタ５の平滑速度ベクトルが収束判定されない場合、運動判別処理器３により蛇行目標と判定されても蛇行運動モデルに基づくカルマンフィルタ５の平滑値は選択されず、替わって等速直線運動モデルに基づくカルマンフィルタ４の平滑値が未来位置予測器９に出力されるのである。
【００７７】
（未来位置予測）
未来位置予測器９は、フィルタ出力選択器より得られた平滑位置及び平滑速度ベクトルより、Ｎサンプリング後の未来位置を式（７２）及び（７３）より算出する。
【数３５】
2005274300.tif 000039
ここで、ｘ^k+N(-)は未来予測値を表す。また、ｘ^intk-1(+)はフィルタ出力選択器８により選択された追尾フィルタの平滑値を表す。さらには、ＴNはサンプリング時刻ｋからｋ＋Ｎまでの時間を表す。蛇行運動モデルに基づくカルマンフィルタ５の場合、各座標軸において得られた等速直線運動モデルの統合平滑値がｘ^intk-1(+)となる。一方、等速直線運動モデルに基づくカルマンフィルタ４、及び多重運動モデルに基づくカルマンフィルタ６は、それぞれ統合平滑値がｘ^intk-1(+)となる。
【００７８】
以上から明らかなように、この発明の実施の形態１における目標追尾装置によれば、目標の運動判別及び蛇行運動モデルに基づくカルマンフィルタにおける等速直線運動モデルの平滑速度ベクトル収束判定により、未来位置予測を行う上で最も有利な追尾フィルタの出力を選択し、サンプリング時刻ｋにおける平滑値に基づきＮサンプル後の未来位置を予測することが可能となる。
【００７９】
なお、この発明の実施の形態１による目標追尾装置の特徴の一部である運動判別処理器３及び収束判定器７は多重運動モデルに基づくカルマンフィルタ６がない場合であっても、その効果を発揮することは明らかである。
【００８０】
実施の形態２．
実施の形態１による目標追尾装置において、収束判定器７は、蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動成分の平滑速度ベクトルの収束判定を行っている。等速直線運動モデルの平滑速度ベクトルは、追尾開始後において収束するのに時間を要するため、フィルタ選択器８は未収束の蛇行運動モデルに基づくカルマンフィルタの出力を選択せず、運動判別処理器３により「蛇行運動」と判定された場合であっても、等速直線運動モデルに基づくカルマンフィルタ４を選択している。一方、目標の旋回等のマニューバにより、蛇行運動モデルの等速直線運動成分の方向が大きく変化する場合がありうる。この場合において、角度評価関数は一旦収束していながら、再度増大することになる。このような状態においては、運動判別処理器３により「蛇行運動」と判定されていても、平滑速度ベクトルが収束していない。したがって、フィルタ出力選択器８が蛇行運動モデルに基づくカルマンフィルタ５の出力を選択したところで、良好な未来位置予測精度は得られないことになる。
【００８１】
そこで、このような問題を回避するために、図１５に示すように角度評価関数が収束後に急増した場合には、収束判定器７により、蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動成分の平滑速度ベクトルは再度未収束の状態になったと判定するようにしてもよい。この発明の実施の形態２による目標追尾装置はこのような特徴を有するものである。図１６はこの発明の実施の形態２による目標追尾装置における収束判定器７の構成を示すブロック図である。図において、しきい値設定器７５は新たなしきい値を設定する部位である。その他、図６と同一の符号を付した構成要素については実施の形態１と同様であるので説明を省略する。しきい値設定器７５の作用によって、しきい値判定器７３は、再度しきい値を下回るまでは、等速直線運動モデルにおける平滑速度ベクトルの収束判定を行わないようになる。
【００８２】
ここで、しきい値判定器７３は、角度評価関数による未収束判定を、例えば式（７４）に示す不等式が満たされた場合に行うようにする。
【数３６】
2005274300.tif 000040
ｈ７は現在のしきい値である。
【００８３】
しきい値判定器７３により未収束判定が行われた場合、しきい値設定器７５は新たなしきい値を式（７５）及び（７６）に示すようにしきい値判定器７３に対して再設定する。
【数３７】
2005274300.tif 000041
【００８４】
しきい値判定器７３は、角度評価関数μθkがしきい値ｈ'5を下回る場合、等速直線運動モデルの平滑速度ベクトルが収束していると判定する。その後にしきい値ｈ'6を超える場合には、再び等速直線運動モデルの平滑速度ベクトルは収束していないと判定する。また再度の未収束判定は旋回等のマニューバ運動により生じるものであるので、運動判別処理器３により「蛇行運動」と判定されている場合であっても、フィルタ出力選択器８は多重運動モデルに基づくカルマンフィルタ６を選択する。図１７は、フィルタ出力選択器８が運動判別処理器３及び収束判定器７の判定結果に基づいてどのカルマンフィルタの平滑値を選択するかを示した図である。この図の構成については図１４と同様である。
【００８５】
このように、運動判別処理器３により「蛇行運動」と判定され、かつ平滑速度ベクトルがまだ収束していない段階にあって、目標の旋回等のマニューバにより角度評価関数が収束した後に再度増大する場合であっても、収束判定器７におけるしきい値を適切に設定して収束判定を工夫することにより、目標の運動に適した運動モデルを選択することができるので、未来位置予測の精度を向上させることができるのである。
【００８６】
実施の形態３．
また収束判定器７における収束判定処理に、蛇行運動モデルに基づくカルマンフィルタ５が算出した平滑値に加えて、等速直線運動モデルに基づくカルマンフィルタ４が算出した平滑値を考慮するようにしてもよい。この発明の実施の形態３による目標追尾装置はこのような特徴を有するものである。図１８はこの発明の実施の形態３による目標追尾装置の構成を示すブロック図である。図１８が図１と異なる点は、図１８では等速直線運動モデルに基づくカルマンフィルタ４の処理結果が収束判定器７にも出力される点であり、その他については同様である。
【００８７】
図１９は、この発明の実施の形態３における目標追尾装置の収束判定器７の詳細な構成を示したブロック図である。図において速度ベクトル角度算出器７１−１及び７１−２は図６における速度ベクトル角度算出器７１に相当するものであって、それぞれ等速直線運動モデルに基づくカルマンフィルタの平滑値と蛇行運動モデルに基づくカルマンフィルタの平滑値とを取得して処理を行うようになっている。角度評価関数算出器７２−１及び７２−２は、図６における角度評価関数算出器７２に相当するものであって、それぞれ等速直線運動モデルに基づくカルマンフィルタと蛇行運動モデルに基づくカルマンフィルタに対して処理を行う。その他図６と同一の符号を付した構成要素については実施の形態１と同様であるので説明を省略する。
【００８８】
次にこの発明の実施の形態３における目標追尾装置の収束判定器７の動作について説明する。速度ベクトル角度算出器７１−１は等速直線運動モデルに基づくカルマンフィルタ４の平滑速度ベクトルを取得する。また速度ベクトル角度算出器７１−２は蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動成分の平滑速度ベクトルを取得する。そして蛇行運動モデルを構成する等速直線運動成分の平滑速度ベクトルの方向成分の時間変位を評価することに加えて、平滑速度ベクトルの方向成分と等速直線運動モデルの方向成分との比較を行う。
【００８９】
具体的にいえば、角度評価関数算出器７２−１及び７２−２は、実施の形態１と同様に式（７１）に示す平滑速度ベクトルについての角度評価関数μθk,cv及びμθk,wvをそれぞれ算出する。そして、しきい値判定器７３は、蛇行運動モデルによる平滑値が収束していると判定する場合において、平滑速度ベクトルの方向成分と等速直線運動モデルの方向成分とをさらに比較することにより、蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動成分の平滑速度ベクトルの収束判定を相対的に行う。
【００９０】
そしてフィルタ出力選択器８は、図１４に基づいて、運動判別処理器３により「蛇行運動」が判定されても追尾開始後一度も蛇行運動モデルに基づくカルマンフィルタの角度評価関数が、等速直線運動モデルに基づくカルマンフィルタの角度評価関数を下回らない場合、等速直線運動モデルに基づくカルマンフィルタ４の出力を選択するのである。
【００９１】
このよう、この発明の実施の形態３によれば、等速直線運動モデルに基づくカルマンフィルタの角度評価関数をも収束判定に用いるようにした。これによって、蛇行運動の基準直線（等速直線運動成分の傾きと切片によって表される直線）が変動してしまい平滑値が十分に安定していないにもかかわらず、平滑速度ベクトルの方向成分の時間変位が小さいために、蛇行運動モデルに基づくカルマンフィルタ５の平滑値単独では収束したと判定される場合であっても、平滑値が未収束であると判断できる。したがって、不安定な平滑値に基づいて未来位置予測をすることを回避し、より精度の高い未来位置予測が可能となるのである。
【００９２】
実施の形態４．
なお、収束判定器７の内部でＮサンプル先までの未来位置予測を行い、Ｎサンプル先の未来位置予測値とＮ−１サンプル先の未来位置予測値との距離を算出して、この距離に基づいて収束判定をするようにしてもよい。この発明の実施の形態４による目標追尾装置はこのような特徴を有するものである。
【００９３】
図２０は、この発明の実施の形態４による目標追尾装置の収束判定器７の詳細な構成を示すブロック図である。図に示すように、この発明の実施の形態４による目標追尾装置の収束判定器７は未来位置予測器７５、未来位置予測値間距離算出器７６、未来位置予測値メモリ７７、未来予測値間距離判定器７８から構成されている。
【００９４】
以下にこれらの各構成要素の作用について説明する。未来位置予測器７５は、蛇行運動モデルに基づくカルマンフィルタにおける等速直線運動成分の平滑値を取得し、Ｎサンプル後の未来予測値を事前に算出する。ここで算出された未来予測値は未来位置予測値間距離算出器７６に出力される。未来位置予測値間距離算出器７６は未来位置予測器７５から取得したＮサンプル先の未来予測値を未来位置予測値メモリ７７に蓄積するとともに、Ｎ−１サンプル先の未来位置予測値を未来位置予測値メモリ７７から取得して、Ｎサンプル先とＮ−１サンプル先における未来位置予測値間のユークリッド距離を算出する。
【００９５】
続いて未来位置予測値間距離判定器７８は、未来位置予測値間のユークリッド距離をしきい値と比較する。そしてこの距離がしきい値を超える場合には、蛇行運動モデルに基づくカルマンフィルタにおける等速直線運動モデルの平滑速度ベクトルは収束していないと判定する。この結果、フィルタ出力選択器８は、図１４に示したように各判定結果に基づいてどのカルマンフィルタを選択するか決定する。
【００９６】
このように、この発明の実施の形態４による目標追尾装置によれば、運動判別処理器３により蛇行運動と判定されても、追尾開始後、一度もユークリッド距離がしきい値を下回らない場合、蛇行運動モデルに基づくカルマンフィルタ５における等速直線運動モデルの平滑速度ベクトルは収束していないと判定し、フィルタ出力選択器８は、等速直線運動モデルに基づくカルマンフィルタ４の出力を選択するようになる。
【００９７】
なお、上述の処理において、距離を算出するのに用いる未来位置予測値として、Ｎサンプル先とＮ−１サンプル先、のように隣接するサンプルにおける未来位置を用いることとしたが、これらは原理的に異なるサンプル時点であればよいのであり、隣接する場合に限定されるものではない。
【産業上の利用可能性】
【００９８】
この発明は、特に多様な運動を行う目標に対して各種の運動モデルを準備して目標追尾を行う目標追尾システムの処理精度を改善する上で有用である。
【図面の簡単な説明】
【００９９】
【図１】この発明の実施の形態１における目標追尾装置の構成を示すブロック図である。
【図２】この発明の実施の形態１における目標追尾装置の詳細な構成を示すブロック図である。
【図３】この発明の実施の形態１における目標追尾装置の別の詳細な構成を示すブロック図である。
【図４】この発明の実施の形態１における目標追尾装置のさらに別の詳細な構成を示すブロック図である。
【図５】この発明の実施の形態１における目標追尾装置の別の詳細な構成を示すブロック図である。
【図６】この発明の実施の形態１における目標追尾装置の別の詳細な構成を示すブロック図である。
【図７】ｘ−ｔ平面における正弦関数運動モデルと等速直線運動モデル、蛇行運動モデルとの関係を示す図である。
【図８】蛇行運動モデルをブロック線図として表現した図である。
【図９】各カルマンフィルタと評価関数、しきい値との関係を表した表である。
【図１０】目標の各運動に対して判定評価関数が示す特性を示した表である。
【図１１】この発明の実施の形態１における目標追尾装置による運動判別の状態遷移図である。
【図１２】この発明の実施の形態１における目標追尾装置による各サンプリング間の等速直線運動モデルの平滑速度ベクトルがなす角を算出する方法を説明するための図である。
【図１３】蛇行運動モデルに基づくカルマンフィルタにより蛇行目標を追尾した場合の等速直線運動モデルの平滑速度ベクトル収束判定評価関数のシミュレーション結果の例を示す図である。
【図１４】この発明の実施の形態１における目標追尾装置における運動判定処理結果と選択されるカルマンフィルタとの関係を示す図である。
【図１５】蛇行運動モデルに基づくカルマンフィルタにより蛇行目標を追尾した場合の等速直線運動モデルの平滑速度ベクトル収束判定評価関数のシミュレーション結果の別の例を示す図である。
【図１６】この発明の実施の形態２における目標追尾装置の詳細な構成を示すブロック図である。
【図１７】この発明の実施の形態２における目標追尾装置における運動判定処理結果と選択されるカルマンフィルタとの関係を示す図である。
【図１８】この発明の実施の形態３における目標追尾装置の構成を示すブロック図である。
【図１９】この発明の実施の形態３における目標追尾装置の詳細な構成を示すブロック図である。
【図２０】この発明の実施の形態４における目標追尾装置の詳細な構成を示すブロック図である。
【符号の説明】
【０１００】
４  等速直線運動モデルに基づくカルマンフィルタ、
５  蛇行運動モデルに基づくカルマンフィルタ、
６  多重運動モデルに基づくカルマンフィルタ、
７  収束判定器、
８  フィルタ出力選択器。
【図１】
2005274300.tif 000042
【図２】
2005274300.tif 000043
【図３】
2005274300.tif 000044
【図４】
2005274300.tif 000045
【図５】
2005274300.tif 000046
【図６】
2005274300.tif 000047
【図７】
2005274300.tif 000048
【図８】
2005274300.tif 000049
【図９】
2005274300.tif 000050
【図１０】
2005274300.tif 000051
【図１１】
2005274300.tif 000052
【図１２】
2005274300.tif 000053
【図１３】
2005274300.tif 000054
【図１４】
2005274300.tif 000055
【図１５】
2005274300.tif 000056
【図１６】
2005274300.tif 000057
【図１７】
2005274300.tif 000058
【図１８】
2005274300.tif 000059
【図１９】
2005274300.tif 000060
【図２０】
2005274300.tif 000061