JP3926602B2 - 目標追尾装置及び方法 - Google Patents
目標追尾装置及び方法 Download PDFInfo
- Publication number
- JP3926602B2 JP3926602B2 JP2001327781A JP2001327781A JP3926602B2 JP 3926602 B2 JP3926602 B2 JP 3926602B2 JP 2001327781 A JP2001327781 A JP 2001327781A JP 2001327781 A JP2001327781 A JP 2001327781A JP 3926602 B2 JP3926602 B2 JP 3926602B2
- Authority
- JP
- Japan
- Prior art keywords
- error
- target
- covariance matrix
- likelihood
- residual covariance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【０００１】
【発明の属する技術分野】
この発明は、センサ等からの目標の位置情報に関する観測データに基づいて目標の位置や速度などの目標運動諸元を推定する目標追尾装置及び方法に関し、特に複数の運動モデルの確率密度分布について所望の重なりを実現することにより、いかなる目標に対しても安定した追尾精度を維持する目標追尾装置および方法に関する。
【０００２】
【従来の技術】
図１０は、従来の目標追尾装置の構成を示すブロック図である。また、図１１は、図１０に示す目標追尾装置の動作を説明するためのフローチャートを示す。以下、図１０を参照して従来の目標追尾装置の構成を説明する。図１０において、１は、目標観測装置を示し、センサが観測する目標位置に関する観測情報（以下、「目標位置観測情報」と称する）を取得する。２は、ゲート判定器を示し、目標観測装置１からの目標位置観測情報と、予測値、予測誤差共分散行列とにより対象追尾目標の選択を行い、得られた観測値を後述する信頼度算出器、平滑器に出力する。
【０００３】
３は、信頼度算出器を示し、ゲート判定器２からの観測値と、運動モデルごとの予測値、運動モデルごとの予測誤差共分散行列、および１サンプリング前の各運動モデルの信頼度を基に、各運動モデルの信頼度を算出する。４は、信頼度メモリを示し、信頼度算出器３によって算出された１サンプリング前の信頼度を蓄積し、信頼度算出器３に出力する。５は、平滑器を示し、ゲート判定器２からの観測値と、各運動モデルの信頼度、定数加速度ベクトル、加速度平滑値、ゲイン行列、および１サンプリング前の平滑値及び平滑誤差共分散行列を基に、平滑値及び平滑誤差共分散を算出する。６は、平滑器メモリを示し、平滑器５によって出力された１サンプリング前の平滑値及び平滑誤差を蓄積し、平滑器５へ再び出力する。
【０００４】
７は、予測器を示し、平滑器６からの平滑値と、各運動モデル信頼度、定数加速度ベクトルを基に、予測値及び予測誤差共分散行列を算出する。８は、第１の遅延回路を示し、予測器７によって算出された予測値を単位時間だけ遅延させる。９は、各運動モデル毎の予測器を示し、平滑器５からの平滑値を基に、運動モデルごとの予測値及び予測誤差共分散行列を算出する。１０及び１１は、第２および第３の遅延回路を示し、それぞれ各モデル毎予測器９より算出された予測値および予測誤差共分散行列の位置成分と観測誤差共分散行列の和である残差共分散行列を単位時間だけ遅延させる。
【０００５】
１２は、ゲイン行列算出器を示し、あらかじめ設定された観測モデルより得られる観測雑音共分散行列および１サンプリング前に算出しておいた各モデル毎予測器９より得られる予測誤差共分散行列を後述する第４の遅延回路を介して入力し、平滑値算出に使用するゲイン行列を算出する。１３は、第４の遅延回路を示し、各モデル毎予測器９より算出された予測誤差共分散行列を単位時間だけ遅延させる。
【０００６】
１４は、モデル設定器を示し、定数加速度ベクトル及びモデル遷移行列を設定し、設定された定数加速度ベクトルを、平滑器５、予測器７、各モデル毎予測器９、後述する入力ベクトル平滑器へ、また、モデル遷移行列を、信頼度算出器３へ出力する。１５は、入力ベクトル平滑器を示し、算出された各モデルの信頼度と設定された定数ベクトルより、入力ベクトル平滑値を算出する。１６は、入力ベクトル予測器を示し、算出された各モデルの信頼度とモデル遷移行列より、入力ベクトル予測値を算出する。１７は、第５の遅延回路を示し、各モデルの事前信頼度を単位時間だけ遅延させる。
【０００７】
１８は、誤差楕円体重複度判定器を示し、観測値と各運動モデルの予測値間の距離を残差共分散行列で正規化した残差二次形式としきい値を比較、あるいは残差共分散共分散行列より算出した誤差楕円体の各軸半径を予測値間距離で除した誤差楕円体重複度係数をしきい値と比較することにより、誤差楕円体制御実行の可否を判定する。１９は、乗算係数算出器を示し、誤差楕円体拡大及び縮小のための乗算係数を算出し、信頼度計算に用いる残差共分散行列を定数倍する。２０は、乗算器を示し、第３の遅延回路１０と乗算係数算出器１９からの出力を乗算して信頼度算出器３に出力する。２１は、表示装置を示し、平滑器３からの平滑値を離散的に画面上に表示する。
【０００８】
なお、図１０に示す構成では、第３の遅延回路１１、誤差楕円体重複度判定器１８、乗算係数算出器１９及び乗算器２０により誤差楕円体制御装置が構成される。
【０００９】
次に、図１０に示す構成でなる目標追尾装置の動作について図１１に示すフローチャートを参照して説明する。はじめに、目標追尾装置では、目標観測装置１より得られる目標位置観測情報を基に、通常のカルマンフィルタ理論に基づき、目標位置・速度の平滑値、及び平滑誤差共分散行列の初期値が設定される（ステップＳ１０１）。次に、目標の運動モデルとして、Ｎ個の定数加速度ベクトル（零加速度ベクトルを含む）よりなるＮ個の運動モデルが設定される（ステップＳ１０２）。
【００１０】
そして、各モデル毎予測器９において、平滑器５から出力される平滑値およびＮ個の運動モデルを構成している定数加速度ベクトルより各運動モデルごとの予測値を算出し、平滑器５から出力される平滑誤差共分散行列及びあらかじめ設定されたの駆動雑音教分散行列を用いて、各運動モデルごとの予測誤差共分散行列を算出する（ステップＳ１０３）。
【００１１】
次に、予測器７において、平滑器５から出力される平滑値と平滑誤差共分散行列、信頼度算出器３から出力される各運動モデルの信頼度、および入力ベクトル予測器１６から出力される入力ベクトル予測値が読み込まれる。予測器７は、それらの入力データに基づき、予測値及び予測誤差共分散行列を算出する（ステップＳ１０４）。
【００１２】
次に、目標観測装置１において、目標位置観測情報が読み込まれる（ステップＳ１０５）。前記観測値は目標からの信号検出及びゲート判定による追尾目標選択の結果であり、位置情報で構成されている。そして、各運動モデルごとの予測値及び予測誤差共分散行列は、各モデル毎予測器９から誤差楕円体重複度判定器１８へ出力され、誤差楕円体重複度判定器１８は、観測値と各運動モデルの予測値間の距離について、予測誤差共分散行列の位置成分と予め設定されている観測雑音共分散行列の和である残差共分散行列で正規化した残差二次形式、あるいは、図１２に示すように、誤差楕円体の半径をＡ、予測値間の距離（楕円中心間距離）をＢとした場合の誤差楕円体重複度係数（Ｃ＝Ａ／Ｂ）を用いた誤差楕円体重複度判定を行い、生成した判定係数テーブルを乗算係数算出器１９へ出力する（ステップＳ１０６）。
【００１３】
次に、乗算係数算出器１９は、誤差楕円重複判定器１８を経由して、各モデル毎予測器９より運動モデルごとの予測値及び残差共分散行列を、また、誤差楕円体重複度判定器１８から判定係数テーブルを読み込み、誤差楕円体の拡大・縮小のための乗算係数を算出する（ステップＳ１０７）。
【００１４】
次に、ゲート判定で得られた観測値は、信頼度算出器３に出力される。信頼度算出器３は、更に、信頼度メモリ４に格納した現時刻より１サンプル前に算出した各運動モデルの信頼度を読み込み、各モデル毎予測器９が１サンプリング前に算出した運動モデルごとの予測値及び乗算係数により定数倍された残差共分散行列を第３の遅延回路１１及び乗算器２０を介して読み込む。信頼度算出器３は、これらに入力データと、モデル設定器１４から入力される運動モデルの推移確率に基づいて、運動モデルの信頼度を算出する（ステップＳ１０８）。
【００１５】
次に、ゲイン行列算出器１２において、現時刻より１サンプリング前に各モデル毎予測器９が算出した予測誤差共分散行列を第４の遅延回路１３を介して読み込む。ゲイン行列算出器１２は、その入力データと、予め設定された観測雑音共分散行列に基づいてゲイン行列を算出する（ステップＳ１０９）。
【００１６】
次に、平滑器５において、現時刻より１サンプリング前に算出しておいた平滑値及び平滑誤差共分散行列が平滑メモリ６より読み込まれる。平滑器３は、更に、信頼度算出器３が出力する各運動モデルの信頼度、ゲイン行列算出器１２が出力するゲイン行列、入力ベクトル平滑器１５が出力する入力ベクトル平滑値、モデル設定器１４が出力する定数加速度ベクトル、及び観測値を読み込む。平滑器５は、それらの入力データに基づいて平滑値及び平滑誤差共分散行列を算出する（ステップＳ１１０）。
【００１７】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ステップＳ１１１）。その結果、処理の終了が要求されていないと判別される場合、処理が繰り返される（ステップＳ１１２）。
【００１８】
【発明が解決しようとする課題】
上述したように、従来の目標追尾装置では、信頼度計算における各運動モデルの誤差楕円体同士の重複度を判定するために、各運動モデルの残差二次形式あるいは残差共分散行列より求めた誤差楕円体重複度係数をしきい値と比較することにより、誤差楕円体制御を行うかどうかを決定している。
【００１９】
しかしながら、残差二次形式により誤差楕円体の重複度を判定する際、観測雑音が大きい、あるいはサンプリング間隔が短い場合には、良好な判定結果が得られない可能性があり、また、誤差楕円体重複度係数を用いる際に、誤差楕円体重複度係数の算出に、残差共分散行列の固有値を求める必要があるため、残差を用いる方法よりも計算負荷が大きいという問題がある。
【００２０】
さらに、フィルタの残差共分散行列を乗算係数によって定数倍するため、図１３に示すように、誤差楕円体は、その形状を変えずに拡大、縮小が行われる。そのため、観測雑音の大きさが異なる距離及び角度方向で、所望の誤差楕円体重複度を各軸方向で独立して実現することが難しく、所望の重複度を得るには各方向について異なる定数加速度を設定して予測値間距離を調整する必要がある。
【００２１】
この発明は、前記の問題を解決するためになされたもので、誤差楕円体の重複度判定によりフィルタのモデルでは目標の運動を表現できないと判定された場合、信頼度計算における残差共分散行列を再計算することにより、観測雑音が異なる距離、角度方向それぞれについて所望の誤差楕円体重複度を実現し、追尾性能の向上を図ることができる目標追尾装置及び方法を得ることを目的とする。
また、小さい計算負荷で、より安定した誤差楕円体の重複度判定を行うことができる目標追尾装置及び方法を得ることを目的とする。
【００２２】
【課題を解決するための手段】
この発明に係る目標追尾装置は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾装置であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御手段を備え、前記誤差楕円体制御手段は、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、誤差楕円体重複度係数をデータベースより検索する誤差楕円体重複度検索手段と、前記誤差楕円体重複検索手段から得られた重複度係数と各運動モデルの予測値間距離より誤差楕円体の各軸半径を算出し、算出された半径よりＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出手段とを有することを特徴とする。
また、他の発明に係る目標追尾装置は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾装置であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御手段を備え、前記誤差楕円体制御手段は、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、各運動モデルの予測値間の中点における尤度をデータベースより検索する予測値間尤度検索手段と、前記予測値間尤度検索手段から得られた最適な尤度及び予測値と中点との距離を正規分布表に照らし合わせることにより、ＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出手段とを有することを特徴とする。
また、この発明に係る目標追尾方法は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾方法であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御ステップを備え、前記誤差楕円体制御ステップは、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定ステップと、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、誤差楕円体重複度係数をデータベースより検索する誤差楕円体重複度検索ステップと、前記誤差楕円体重複検索ステップから得られた重複度係数と各運動モデルの予測値間距離より誤差楕円体の各軸半径を算出し、算出された半径よりＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列 をフィルタの座標系へ変換する残差共分散行列算出ステップとを有することを特徴とする。
さらに、他の発明に係る目標追尾方法は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾方法であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御ステップを備え、前記誤差楕円体制御ステップは、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定ステップと、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、各運動モデルの予測値間の中点における尤度をデータベースより検索する予測値間尤度検索ステップと、前記予測値間尤度検索ステップから得られた最適な尤度及び予測値と中点との距離を正規分布表に照らし合わせることにより、ＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出ステップとを有することを特徴とする。
【００３６】
【発明の実施の形態】
実施の形態１．
以下に、この発明に係る目標追尾装置において追尾フィルタとして用いるＮ個の運動モデルを持つ多重運動モデルを用いたカルマンフィルタについて説明する。目標の運動モデルを式（１）のように定義する。
【数１】
【００３７】
ここで、Ｘkはサンプリング時刻ｔkにおける目標運動諸元の真値をあらわす状態ベクトルであり、例えば、ｘｙｚの直交座標系における目標位置ベクトルを式（２）、直交座標系における目標速度ベクトルを式（３）とした時、Ｘkは式（４）である。
【数２】
【００３８】
Φk-1はサンプリング時刻ｔk-1よりｔkへの状態ベクトルＸkの推移を表す推移行列で、目標が等速直線運動を行うと仮定した場合、式（５）である。ここでＩは式（６）の通り単位行列である。
【数３】
【００３９】
ｗkはサンプリング時刻ｔkにおける駆動雑音ベクトルで、Γkはサンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列である。目標の運動モデルを等速直線運動と仮定したことによる打切り誤差項をΓk-1ｗk-1とみれば、ｗkは加速度ベクトル相当であり、Γk-1は式（７）である。
【数４】
【００４０】
ｗkは平均０の３次元正規分布白色雑音で、式（８）、式（９）とする。ここで、０は零ベクトルを表し、Ｑkはサンプリング時刻ｔｋにおける駆動雑音共分散行列である。
【数５】
【００４１】
ｕkはサンプリング時刻ｔkにおいてＮ個の運動モデルを構成する定数加速度ベクトルであり式（１０）である。
【数６】
Γ’kはサンプリング時刻ｔkにおける定数加速度ベクトルの変換行列で、式（１１）である。
【数７】
【００４２】
次に、図１４は、定数加速度ベクトルを説明する図であり、図１４において、Ｏは目標観測装置を原点とした座標Ｏ−ｘｙｚの原点、Ｘは東方向をＸ軸の正とした座標Ｏ−ｘｙｚのＸ軸、Ｙは北方向をＹ軸の正とした座標Ｏ−ｘｙｚのＹ軸、Ｚは上方向をＺ軸の正とした座標Ｏ−ｘｙｚのＺ軸、α1はＹ軸正方向の定数加速度ベクトル、α2はＹ軸負方向の定数加速度ベクトル、α3はＸ軸正方向の定数ベクトル、α4はＸ軸正方向の定数加速度ベクトル、α5はＺ軸正方向の定数加速度ベクトル、α6はＺ軸負方向の定数加速度ベクトルである。この他に、定数加速度ベクトルα7＝０を考えた運動モデルの場合、モデル数Ｎは７となる。定数加速度ベクトルを定義する座標としては、図１５に示すＬＯＳ直交座標、運動座標、北基準直交座標などが考えられる。ここで、運動座標とは目標の推定速度ベクトルを１軸とした座標であり、ＬＯＳ直交座標とはレーダと目標を結んだベクトルを１軸とした座標のことである。
【００４３】
次に、サンプリング時刻ｔkにおいて式（１２）が真であるとの仮説を式（１３）と書く。
【数８】
運動モデルの推移にマルコフ性を仮定する。すなわち運動モデルΨk,aはサンプリング時刻ｔk-1の運動モデルより決まり、サンプリング時刻ｔk-2までの運動モデルには依存しないとする。
【００４４】
時刻ｔkにおける運動モデル間推移確率を式（１４）のように表す。例えば、最小サンプリング間隔をｔｓとし、サンプリング間隔ｔk−ｔk-1＝ｔｓの場合の運動モデル間推移確率を式（１４）と同様の意味でＰabとする。さらに、Ｎ×Ｎ行列でａ行ｂ列の要素をＰabとしたものを運動モデル間推移確率行列Πと表す。サンプリング間隔がｔk−ｔk-1＝ｍ・ｔs，（ｍ＝１，２，・・・）と表せる場合、サンプリング時刻ｔkでの運動モデル間推移確率行列Πkは式（１５）で表せる。このときのΠkのａ行ｂ列の要素が運動モデル間推移確率Ｐk,abである。
【数９】
【００４５】
サンプリング時刻ｔkまでの観測情報Ｚkに基づく、サンプリング時刻ｔkでの運動モデルの信頼度を条件付き確率密度関数により、式（１６）と定義すると、式（１７）で計算される。
【数１０】
ただし、νk,aは各運動モデルの観測ベクトルに対する尤度であり、観測ベクトルの正規分布近似Ｐ［Ｚk|Ψk,a，Ｚk-1］を多変量正規分布で近似することにより、式（１８）のように表される。
【数１１】
ここで、ｇ（ｚ；ａ，Ａ）は平均ａ、共分散Ａの３変量正規分布のｚにおける確率密度関数（尤度）を表す。
【００４６】
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくサンプリング時刻ｔkでの運動モデルの事前信頼度を条件付き確率密度関数により、式（１９）と定義すると、式（２０）と書ける。
【数１２】
【００４７】
サンプリング時刻ｔkまでの観測情報Ｚkに基づく、定数加速度ベクトルの推定値を式（２１）として定義すると、式（２２）と書ける。これを推定加速度ベクトルと呼ぶ。
【数１３】
【００４８】
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づく、定数加速度ベクトルの推定値を式（２３）として定義すると、式（２４）と書ける。これを予測加速度ベクトルと呼ぶ。
【数１４】
【００４９】
追尾対象目標よりの観測値はサンプリング時刻ｔkにおいて１つ得られるとし、その観測系モデルを式（２５）とする。ここで、Ｚkはサンプリング時刻ｔkにおける位置情報の観測値より構成される直交座標による位置観測ベクトル、Ｈはサンプリング時刻ｔkにおける観測行列で、式（２６）である。ｖkはサンプリング時刻ｔkでの観測ベクトルＺkに対応した観測雑音ベクトルであり、平均０の３次元正規分布白色雑音で、式（２７）、式（２８）である。なお、Ｒkはサンプリング時刻ｔkにおける観測雑音共分散行列で、運動モデルによらない値とする。また、駆動雑音ベクトルと観測雑音ベクトルは互いに独立であるとする。
【００５０】
【数１５】
また、サンプリング時刻ｔ1からｔkまでの観測値ベクトルの集積を式（２９）のように表す。
【数１６】
【００５１】
カルマンフィルタの理論によれば、前記モデルに従いサンプリング時刻ｔkで観測値が得られた場合の、状態ベクトルＸkの推定値Ｘk（＋）は式（３０）〜（３８）によって計算される。
【００５２】
【数１７】
【００５３】
ここで、Ｘk,a（−）、Ｘk,a（＋）、Ｘk（−）、Ｘk（＋）、Ｐk,a（−）、Ｐk,a（＋）、Ｐk（−）、Ｐk（＋）は、それぞれ式（３９）〜（４６）のように定義される。
【００５４】
【数１８】
【００５５】
Ｘk,a（−）はサンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値で、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aに基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００５６】
Ｘk,a（＋）はサンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値で、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００５７】
Ｘk（−）はサンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくＸkの条件付き平均値で、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００５８】
Ｘk（＋）はサンプリング時刻ｔkまでの観測情報Ｚkに基づくＸkの条件付き平均値で、サンプリング時刻ｔｋまでの観測情報Ｚkに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００５９】
Ｐk,a（−）,Ｐk,a（＋）、Ｐk（−）、Ｐk（＋）は、それぞれＸk,a（−）、Ｘk,a（＋）、Ｘk（−）、Ｘk（＋）の誤差共分散行列を表す運動モデルごとの予測誤差共分散行列、運動モデルごとの平滑誤差共分散行列、N個の運動モデルによる予測誤差共分散行列およびＮ個の運動モデルによる平滑誤差共分散行列である。
【００６０】
式（３４）はサンプリング時刻ｔkにおけるゲイン行列である。また、カルマンフィルタを通常適用する場合と同様にして、初期値Ｘ0（＋）、Ｐ0（＋）は別途定まっているものとする。
【００６１】
式（３１）より、Ｐk,a（−）は仮説Ψk,aによらない値なので、式（３４）よりＫk、式（３６）よりＰk,a（＋）も同様に仮説Ψk,aによらない値となる。
【００６２】
次に、図１は、本実施の形態１に係る目標追尾装置の構成を示すブロック図である。図１において、図１０に示す従来例と同一部分、つまり誤差楕円体制御装置以外の構成は同一のため同一符号を付しその説明は省略する。本実施の形態１に係る誤差楕円体制御装置としては、第３の遅延回路１１と、本実施の形態１に係る誤差楕円体重複度判定器２２と、誤差楕円体重複度検索器２３と、残差共分散行列算出器２４と、第６の遅延回路２５とを備えている。
【００６３】
ここで、誤差楕円体重複度判定器２２は、各モデル毎予測器９より運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は誤差楕円体重複度検索器２３へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出器３へ出力させるためのスイッチ切替を行う。
【００６４】
次に、誤差楕円体重複度検索器２３は、誤差楕円体重複度判定器２２により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測器９より各モデル毎の予測値、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した誤差楕円体重複度係数をデータベースより検索し、誤差楕円体重複度係数と各運動モデルの予測値を残差共分散行列算出器２４へ出力する。
【００６５】
また、残差共分散行列算出器２４は、誤差楕円体重複度検索器２３から入力された誤差楕円体重複度係数及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸半径を算出する。そして、算出した各軸半径より、ＬＯＳ直交座標における残差共分散行列を算出し、フィルタの座標系へ座標変換した後、信頼度算出器３へと出力される。
【００６６】
図２は、本実施の形態１に係る目標追尾装置の動作を説明するフローチャートである。誤差楕円体制御手段以外の動作は、図１０に示す従来例と同様であり、その説明は省略する。なお、フローチャートは、目標追尾方法の説明を兼ねる。誤差楕円体重複度判定ステップ（Ｓ２０１）は、各モデル予測ステップより運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は誤差楕円体重複度検索ステップ（Ｓ２０２）へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出ステップ（Ｓ１０８）へ出力させるための切替を行う。
【００６７】
次に、誤差楕円体重複度検索ステップ（Ｓ２０２）は、誤差楕円体重複度判定ステップ（Ｓ２０１）により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測ステップより各モデル毎の予測値、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。
【００６８】
そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した誤差楕円体重複度係数をデータベースより検索し、誤差楕円体重複度係数と各運動モデルの予測値を残差共分散行列算出ステップへ出力する。残差共分散行列算出ステップは、誤差楕円体重複度検索ステップ（Ｓ２０２）から入力された誤差楕円体重複度係数及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸半径を算出する（Ｓ２０３）。そして算出した各軸半径より、ＬＯＳ直交座標における残差共分散行列を算出し（Ｓ２０４）、フィルタの座標系へ座標変換した後、信頼度算出ステップ（Ｓ１０８）へと出力される。
【００６９】
以上のように、本実施の形態１に係る目標追尾装置及び方法によれば、誤差楕円体重複度係数あるいは残差二次形式より重複度を判定するよりも、各運動モデルの予測値間の中点における尤度を用いることで、観測雑音が大きい、あるいはサンプリング間隔が短い場合でも、小さい計算負荷で安定した誤差楕円体重複度判定を行うことが可能となる。
【００７０】
また、データベースより最適な誤差楕円体重複度係数を検索し、誤差楕円体重複度係数より誤差楕円体の各軸半径を介して残差共分散行列を算出することにより、図９に示すように、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００７１】
実施の形態２．
次に、実施の形態２に係る目標追尾装置における誤差楕円体重複度判定器２２の動作を説明するフローチャートを図３に示す。
はじめに、誤差楕円体重複度判定器２２は、各モデル毎予測器９より各運動モデルの予測値および残差共分散行列を読み込み、各モデルの予測値間の中点における位置座標を求める（Ｓ３０１）。次いで、式（４７）に予測値、予測値間の中点座標および残差共分散行列を代入することにより、各予測値間の中点における尤度を算出する（Ｓ３０２）。
【数１９】
ここで、νk,aは尤度、Ｘｍk,aは各モデルの予測値間の中点における位置座標、Ｓk,aは残差共分散行列となる。
【００７２】
次いで、図４に示すように、各運動モデル間の尤度と２つのしきい値を比較し、すべての尤度がしきい値２を越えた場合（Ｓ３０４）、各運動モデルの誤差楕円体の重複度は大きすぎると判定され、一方ですべての尤度がしきい値１を下回った場合（Ｓ３０３）、重複度は小さすぎると判定され、残差共分散行列の再計算が行われる（Ｓ３０５）。すべての尤度がしきい値１を下回らなく、かつしきい値２を下回った場合（Ｓ３０４）、重複度は適切と判定される（Ｓ３０６）。ステップＳ３０５，Ｓ３０６を経た後は、ステップＳ３０７に移行し、信頼度算出器３への出力切り替えが行われる。処理終了を判定し（Ｓ３０８）、終了されていない場合は、ステップＳ３０１にリターンする。
【００７３】
以上のように、本実施の形態２に係る目標追尾装置によれば、誤差楕円体重複度係数あるいは残差二次形式より重複度を判定するよりも、各運動モデルの予測値間の中点における尤度を用いることで、多目標あるいはクラッタ環境下においても小さい計算負荷で誤差楕円体重複度の判定を行うことが可能となる。
【００７４】
実施の形態３．
次に、図５は、本実施の形態３における誤差楕円体重複度検索器２３および残差共分散行列算出器２４の動作を説明するフローチャートである。はじめに、誤差楕円体重複度判定器２２により信頼度計算における残差共分散行列の再計算が必要とされた場合、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせ条件に最も適した誤差楕円体重複度係数をデータベースより検索する（Ｓ５０１）。ここで、誤差楕円体重複度係数γk,aは、以下の式より定義され、データベースへ事前に登録される。
【数２０】
なお、ｌk,iは誤差楕円体の各軸半径であり、ξk,iは各軸における運動モデルの予測値間距離となる。
【００７５】
次いで、データベース検索によって得られた最適な誤差楕円体重複度係数、および各モデル毎予測器９より得られた予測値間の距離を式（４８）に代入し、所望の誤差楕円体重複度を実現する誤差楕円体の各軸半径ｌk,iを求める（Ｓ５０２）。ここで、誤差楕円体はレーダと目標を結ぶＬＯＳベクトルに対して垂直であると仮定することにより、各軸半径よりＬＯＳ座標における残差共分散行列を求めることができる。
【００７６】
次いで、各軸半径ｌk,iを式（４９）に代入することにより、ＬＯＳ直交座標における残差共分散行列の対角成分である固有値λk,iが算出される。
【数２１】
なお、ｄは誤差楕円体の等角率ラインを決定する既知のパラメータであり、自由度３のχ2分布表により定める。
【００７７】
ここで、誤差楕円体がＬＯＳ軸に対して垂直であれば各軸の相関はなく、残差共分散行列の固有値λk,iは各軸方向の分散となる（Ｓ５０３）。そのため、残差共分散行列は、算出した固有値を用いて以下の式より算出される。
【数２２】
ここで、λk,el，λaz，λelはそれぞれＬＯＳ方向、ＬＯＳ軸に対して左右方向、上下方向の残差共分散行列の固有値となる。
【００７８】
算出されたＬＯＳ直交座標における残差共分散行列は、フィルタの状態ベクトルが北基準直交座標で定義されている場合、以下の行列Ｆにより座標変換される（Ｓ５０４）。
【数２３】
ここで、［ｘｙｚ］は、北基準直交座標系におけるレーダを原点とした目標の位置成分である。
次いで、座標変換された残差共分散行列は、信頼度計算器３へと出力される。
【００７９】
以上のように、本実施の形態３によれば、データベースより最適な誤差楕円体重複度係数を検索し、誤差楕円体重複度係数より誤差楕円体の各軸半径を介して残差共分散行列を算出することにより、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、サンプリング間隔が長く定数加速度が大きい場合や、逆にサンプリング間隔が短く定数加速度が小さい場合においても、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００８０】
実施の形態４．
次に、図６は、本実施の形態４に係る目標追尾装置の構成を示すブロック図である。図６において、図１０に示す従来例と同一部分、つまり誤差楕円体制御装置以外の構成は同一のため同一符号を付しその説明は省略する。本実施の形態４に係る誤差楕円体制御装置としては、第３の遅延回路１１と、誤差楕円体重複度判定器２２と、本実施の形態４に係る予測値間尤度検索器２６と、残差共分散行列算出器２４と、第６の遅延回路２５とを備えている。
【００８１】
ここで、誤差楕円体重複度判定器２２は、各モデル毎予測器９より運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は予測値間尤度検索器２６へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出器３へ出力させるためのスイッチ切替を行う。
【００８２】
次に、予測値間尤度検索器２６は、誤差楕円体重複度判定器２２により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測器９より各モデル毎の予測値、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した、各運動モデルの予測値間の中点における尤度をデータベースより検索し、尤度と各運動モデルの予測値を残差共分散行列算出器２４へ出力する。
【００８３】
残差共分散行列算出器２４は、予測値間尤度検索器２６から入力された尤度及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸方向の分散を算出する。そして算出した各軸方向の分散より、ＬＯＳ直交座標における残差共分散行列を算出し、フィルタの座標系へ座標変換した後、信頼度算出器３へ出力する。
【００８４】
図７は、本実施の形態４に係る目標追尾装置の動作を説明するフローチャートである。誤差楕円体制御装置以外の動作は、図１０に示す従来装置と同様であり、その説明は省略する。なお、フローチャートは目標追尾方法の説明を兼ねる。誤差楕円体重複度判定ステップ（Ｓ７０１）は、各モデル毎予測器９より運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は予測値間尤度検索ステップ（Ｓ７０２）へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出ステップ（Ｓ１０８）へ出力させるための切替を行う。
【００８５】
次に、予測値間尤度検索ステップ（Ｓ７０２）は、誤差楕円体重複度判定ステップ（Ｓ７０１）により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測ステップより各モデル毎の予測位置、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。
【００８６】
そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した、各運動モデルの予測値間の中点における尤度をデータベースより検索し、尤度と各運動モデルの予測値を残差共分散行列算出ステップへ出力する。残差共分散行列算出ステップは、予測値間尤度検索ステップ（Ｓ７０２）から入力された尤度及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸方向の分散を算出する（Ｓ７０３）。そして算出した各軸方向の分散より、ＬＯＳ直交座標における残差共分散行列を算出し（Ｓ７０４）、フィルタの座標系へ座標変換した後、信頼度算出ステップ（Ｓ１０８）へ出力する。
【００８７】
以上のように、本実施の形態４に係る目標追尾装置及び方法によれば、誤差楕円体重複度係数あるいは残差二次形式より重複度を判定するよりも、各運動モデルの予測値間の中点における尤度を用いることで、観測雑音が大きい、あるいはサンプリング間隔が短い場合でも、小さい計算負荷で安定した誤差楕円体重複度判定を行うことが可能となる。
【００８８】
また、データベースより最適な各運動モデルの予測値間の中点における尤度を検索し、尤度から新たに形成する誤差楕円体の分散を求めて残差共分散行列を算出することにより、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、サンプリング間隔が長く定数加速度が大きい場合や、逆にサンプリング間隔が短く定数加速度が小さい場合においても、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００８９】
実施の形態５．
次に、図８は、本実施の形態５における予測値間尤度検索器２６および残差共分散行列算出器２４の動作を説明するフローチャートである。以下に図を用いて説明する。はじめに、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせ条件に最も適した予測値間の中点における尤度をデータベースより検索する（Ｓ８０１）。
【００９０】
次いで、誤差楕円体がＬＯＳ軸に垂直であり、かつ目標の存在確率密度分布が３変量正規分布であるとの仮定のもとに、データベース検索によって得られた最適な尤度及び予測値と中点との距離を事前に用意した正規分布表と照らし合わせることにより、誤差楕円体の各軸方向の分散を求める（Ｓ８０２）。
【００９１】
ＬＯＳ直交座標における残差共分散行列Ｓ’k,aは、以下の式より表される（Ｓ８０３）。
【数２４】
【００９２】
ここで、σ2 r，σ2 az，σ2 elはそれぞれＬＯＳ方向、ＬＯＳ軸に対して左右方向、ＬＯＳ軸に対して上下方向における確率密度分布の分散となる。算出されたＬＯＳ直交座標における残差共分散行列は、フィルタの状態ベクトルが定義されている北基準直交座標へ座標変換される（Ｓ８０４）。
次いで、再計算された残差共分散行列は、信頼度計算器３へと出力される。
【００９３】
以上のように、本実施の形態５によれば、データベースより予測値間の中点における最適な尤度を検索し、ＬＯＳ直交座標の残差共分散行列の各軸における最適な分散を求めることで、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、サンプリング間隔が長く定数加速度が大きい場合や、逆にサンプリング間隔が短く定数加速度が小さい場合においても、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００９４】
【発明の効果】
以上のように、この発明によれば、誤差楕円体の重複度を判定することにより、小さい計算負荷で事前に精度良く追尾精度の劣化の可能性を知り、誤差楕円体を各軸方向について任意の大きさに制御して所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９５】
また、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、前記尤度と２つのしきい値を比較することにより、残差共分散行列より誤差楕円体重複度係数を求めて重複度を判定する方法よりも小さい計算負荷で、また、残差二次形式を求めて重複度を判定するよりも高い精度で誤差楕円体重複度判定を行うことができる。
【００９６】
また、ＬＯＳ直交座標における誤差楕円体はＬＯＳ軸に対して垂直であるとの仮定のもとに、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な確率密度分布の分散を算出して残差共分散行列を求め、前記残差共分散行列をＬＯＳ直交座標よりフィルタの座標系へと変換することにより、各軸方向それぞれについて任意の誤差楕円体の形状が得られるため、所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９７】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な誤差楕円体重複度係数から誤差楕円体の半径を算出し、さらに各軸方向の半径より誤差楕円体の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９８】
また、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な誤差楕円体重複度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９９】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な各運動モデルの予測値間の中点における尤度より、各軸における確率密度分布の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１００】
また、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な予測値間の中点における尤度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０１】
また、誤差楕円体の重複度を判定することにより、小さい計算負荷で事前に精度良く追尾精度の劣化の可能性を知り、誤差楕円体を各軸方向について任意の大きさに制御して所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０２】
また、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、前記尤度と２つのしきい値を比較することにより、残差共分散行列より誤差楕円体重複度係数を求めて重複度を判定する方法よりも小さい計算負荷で、また、残差二次形式を求めて重複度を判定するよりも高い精度で誤差楕円体重複度判定を行うことができる。
【０１０３】
また、ＬＯＳ直交座標における誤差楕円体はＬＯＳ軸に対して垂直であるとの仮定のもとに、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な確率密度分布の分散を算出して残差共分散行列を求め、前記残差共分散行列をＬＯＳ直交座標よりフィルタの座標系へと変換することにより、各軸方向それぞれについて任意の誤差楕円体の形状が得られるため、所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０４】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な誤差楕円体重複度係数から誤差楕円体の半径を算出し、さらに各軸方向の半径より誤差楕円体の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０５】
また、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な誤差楕円体重複度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０６】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な各運動モデルの予測値間の中点における尤度より、各軸における確率密度分布の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０７】
さらに、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な予測値間の中点における尤度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【図面の簡単な説明】
【図１】 この発明の実施の形態１に係る目標追尾装置の構成を示すブロック図である。
【図２】 この発明の実施の形態１に係る目標追尾方法を示すフローチャートである。
【図３】 この発明の実施の形態２に係る目標追尾装置の誤差楕円体重複度判定器２２の動作を示すフローチャートである。
【図４】 この発明の実施の形態２に係る誤差楕円体重複度判定器２２における尤度としきい値の比較の様子を示した図である。
【図５】 この発明の実施の形態３に係る目標追尾装置の誤差楕円体重複度係数検索器及び残差共分散行列算出器２４の動作を示すフローチャートである。
【図６】 この発明の実施の形態４に係る目標追尾装置の構成を示すブロック図である。
【図７】 この発明の実施の形態４に係る目標追尾方法を示すフローチャートである。
【図８】 この発明の実施の形態５に係る目標追尾装置の予測値間尤度検索器２６及び残差共分散行列算出器２４の動作を示すフローチャートである。
【図９】 残差共分散行列再計算による誤差楕円体制御の様子を示した図である。
【図１０】 従来の目標追尾装置の構成を示すブロック図である。
【図１１】 従来の目標追尾装置の動作を示すフローチャートである。
【図１２】 複数の運動モデルの誤差楕円体重複度係数と、予測値間の距離および各軸半径の関係を示した図である。
【図１３】 従来の残差を用いた誤差楕円体制御の様子を示した図である。
【図１４】 目標の運動モデルとして、設定されたＮ個の定数加速度ベクトル（零加速度ベクトルを含む）を示した図である。
【図１５】 北基準直座標とＬＯＳ直交座標、運動座標の関係を示した図である。
【符号の説明】
１ 目標観測装置、２ ゲート判定器、３ 信頼度算出器、４ 信頼度メモリ、５ 平滑器、６ 平滑器メモリ、７ 予測器、８ 第１の遅延回路、９ 各モデル毎予測器、１０ 第２の遅延回路、１１ 第３の遅延回路、１２ ゲイン行列算出器、１３ 第４の遅延回路、１４ モデル設定器、１５ 入力ベクトル平滑器、１６ 入力ベクトル予測器、１７ 第５の遅延回路、２２ 誤差楕円体重複度判定器、２３ 誤差楕円体重複度検索器、２４ 残差共分散行列算出器、２５ 第６の遅延回路、２６ 予測値間尤度検索器。
【発明の属する技術分野】
この発明は、センサ等からの目標の位置情報に関する観測データに基づいて目標の位置や速度などの目標運動諸元を推定する目標追尾装置及び方法に関し、特に複数の運動モデルの確率密度分布について所望の重なりを実現することにより、いかなる目標に対しても安定した追尾精度を維持する目標追尾装置および方法に関する。
【０００２】
【従来の技術】
図１０は、従来の目標追尾装置の構成を示すブロック図である。また、図１１は、図１０に示す目標追尾装置の動作を説明するためのフローチャートを示す。以下、図１０を参照して従来の目標追尾装置の構成を説明する。図１０において、１は、目標観測装置を示し、センサが観測する目標位置に関する観測情報（以下、「目標位置観測情報」と称する）を取得する。２は、ゲート判定器を示し、目標観測装置１からの目標位置観測情報と、予測値、予測誤差共分散行列とにより対象追尾目標の選択を行い、得られた観測値を後述する信頼度算出器、平滑器に出力する。
【０００３】
３は、信頼度算出器を示し、ゲート判定器２からの観測値と、運動モデルごとの予測値、運動モデルごとの予測誤差共分散行列、および１サンプリング前の各運動モデルの信頼度を基に、各運動モデルの信頼度を算出する。４は、信頼度メモリを示し、信頼度算出器３によって算出された１サンプリング前の信頼度を蓄積し、信頼度算出器３に出力する。５は、平滑器を示し、ゲート判定器２からの観測値と、各運動モデルの信頼度、定数加速度ベクトル、加速度平滑値、ゲイン行列、および１サンプリング前の平滑値及び平滑誤差共分散行列を基に、平滑値及び平滑誤差共分散を算出する。６は、平滑器メモリを示し、平滑器５によって出力された１サンプリング前の平滑値及び平滑誤差を蓄積し、平滑器５へ再び出力する。
【０００４】
７は、予測器を示し、平滑器６からの平滑値と、各運動モデル信頼度、定数加速度ベクトルを基に、予測値及び予測誤差共分散行列を算出する。８は、第１の遅延回路を示し、予測器７によって算出された予測値を単位時間だけ遅延させる。９は、各運動モデル毎の予測器を示し、平滑器５からの平滑値を基に、運動モデルごとの予測値及び予測誤差共分散行列を算出する。１０及び１１は、第２および第３の遅延回路を示し、それぞれ各モデル毎予測器９より算出された予測値および予測誤差共分散行列の位置成分と観測誤差共分散行列の和である残差共分散行列を単位時間だけ遅延させる。
【０００５】
１２は、ゲイン行列算出器を示し、あらかじめ設定された観測モデルより得られる観測雑音共分散行列および１サンプリング前に算出しておいた各モデル毎予測器９より得られる予測誤差共分散行列を後述する第４の遅延回路を介して入力し、平滑値算出に使用するゲイン行列を算出する。１３は、第４の遅延回路を示し、各モデル毎予測器９より算出された予測誤差共分散行列を単位時間だけ遅延させる。
【０００６】
１４は、モデル設定器を示し、定数加速度ベクトル及びモデル遷移行列を設定し、設定された定数加速度ベクトルを、平滑器５、予測器７、各モデル毎予測器９、後述する入力ベクトル平滑器へ、また、モデル遷移行列を、信頼度算出器３へ出力する。１５は、入力ベクトル平滑器を示し、算出された各モデルの信頼度と設定された定数ベクトルより、入力ベクトル平滑値を算出する。１６は、入力ベクトル予測器を示し、算出された各モデルの信頼度とモデル遷移行列より、入力ベクトル予測値を算出する。１７は、第５の遅延回路を示し、各モデルの事前信頼度を単位時間だけ遅延させる。
【０００７】
１８は、誤差楕円体重複度判定器を示し、観測値と各運動モデルの予測値間の距離を残差共分散行列で正規化した残差二次形式としきい値を比較、あるいは残差共分散共分散行列より算出した誤差楕円体の各軸半径を予測値間距離で除した誤差楕円体重複度係数をしきい値と比較することにより、誤差楕円体制御実行の可否を判定する。１９は、乗算係数算出器を示し、誤差楕円体拡大及び縮小のための乗算係数を算出し、信頼度計算に用いる残差共分散行列を定数倍する。２０は、乗算器を示し、第３の遅延回路１０と乗算係数算出器１９からの出力を乗算して信頼度算出器３に出力する。２１は、表示装置を示し、平滑器３からの平滑値を離散的に画面上に表示する。
【０００８】
なお、図１０に示す構成では、第３の遅延回路１１、誤差楕円体重複度判定器１８、乗算係数算出器１９及び乗算器２０により誤差楕円体制御装置が構成される。
【０００９】
次に、図１０に示す構成でなる目標追尾装置の動作について図１１に示すフローチャートを参照して説明する。はじめに、目標追尾装置では、目標観測装置１より得られる目標位置観測情報を基に、通常のカルマンフィルタ理論に基づき、目標位置・速度の平滑値、及び平滑誤差共分散行列の初期値が設定される（ステップＳ１０１）。次に、目標の運動モデルとして、Ｎ個の定数加速度ベクトル（零加速度ベクトルを含む）よりなるＮ個の運動モデルが設定される（ステップＳ１０２）。
【００１０】
そして、各モデル毎予測器９において、平滑器５から出力される平滑値およびＮ個の運動モデルを構成している定数加速度ベクトルより各運動モデルごとの予測値を算出し、平滑器５から出力される平滑誤差共分散行列及びあらかじめ設定されたの駆動雑音教分散行列を用いて、各運動モデルごとの予測誤差共分散行列を算出する（ステップＳ１０３）。
【００１１】
次に、予測器７において、平滑器５から出力される平滑値と平滑誤差共分散行列、信頼度算出器３から出力される各運動モデルの信頼度、および入力ベクトル予測器１６から出力される入力ベクトル予測値が読み込まれる。予測器７は、それらの入力データに基づき、予測値及び予測誤差共分散行列を算出する（ステップＳ１０４）。
【００１２】
次に、目標観測装置１において、目標位置観測情報が読み込まれる（ステップＳ１０５）。前記観測値は目標からの信号検出及びゲート判定による追尾目標選択の結果であり、位置情報で構成されている。そして、各運動モデルごとの予測値及び予測誤差共分散行列は、各モデル毎予測器９から誤差楕円体重複度判定器１８へ出力され、誤差楕円体重複度判定器１８は、観測値と各運動モデルの予測値間の距離について、予測誤差共分散行列の位置成分と予め設定されている観測雑音共分散行列の和である残差共分散行列で正規化した残差二次形式、あるいは、図１２に示すように、誤差楕円体の半径をＡ、予測値間の距離（楕円中心間距離）をＢとした場合の誤差楕円体重複度係数（Ｃ＝Ａ／Ｂ）を用いた誤差楕円体重複度判定を行い、生成した判定係数テーブルを乗算係数算出器１９へ出力する（ステップＳ１０６）。
【００１３】
次に、乗算係数算出器１９は、誤差楕円重複判定器１８を経由して、各モデル毎予測器９より運動モデルごとの予測値及び残差共分散行列を、また、誤差楕円体重複度判定器１８から判定係数テーブルを読み込み、誤差楕円体の拡大・縮小のための乗算係数を算出する（ステップＳ１０７）。
【００１４】
次に、ゲート判定で得られた観測値は、信頼度算出器３に出力される。信頼度算出器３は、更に、信頼度メモリ４に格納した現時刻より１サンプル前に算出した各運動モデルの信頼度を読み込み、各モデル毎予測器９が１サンプリング前に算出した運動モデルごとの予測値及び乗算係数により定数倍された残差共分散行列を第３の遅延回路１１及び乗算器２０を介して読み込む。信頼度算出器３は、これらに入力データと、モデル設定器１４から入力される運動モデルの推移確率に基づいて、運動モデルの信頼度を算出する（ステップＳ１０８）。
【００１５】
次に、ゲイン行列算出器１２において、現時刻より１サンプリング前に各モデル毎予測器９が算出した予測誤差共分散行列を第４の遅延回路１３を介して読み込む。ゲイン行列算出器１２は、その入力データと、予め設定された観測雑音共分散行列に基づいてゲイン行列を算出する（ステップＳ１０９）。
【００１６】
次に、平滑器５において、現時刻より１サンプリング前に算出しておいた平滑値及び平滑誤差共分散行列が平滑メモリ６より読み込まれる。平滑器３は、更に、信頼度算出器３が出力する各運動モデルの信頼度、ゲイン行列算出器１２が出力するゲイン行列、入力ベクトル平滑器１５が出力する入力ベクトル平滑値、モデル設定器１４が出力する定数加速度ベクトル、及び観測値を読み込む。平滑器５は、それらの入力データに基づいて平滑値及び平滑誤差共分散行列を算出する（ステップＳ１１０）。
【００１７】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ステップＳ１１１）。その結果、処理の終了が要求されていないと判別される場合、処理が繰り返される（ステップＳ１１２）。
【００１８】
【発明が解決しようとする課題】
上述したように、従来の目標追尾装置では、信頼度計算における各運動モデルの誤差楕円体同士の重複度を判定するために、各運動モデルの残差二次形式あるいは残差共分散行列より求めた誤差楕円体重複度係数をしきい値と比較することにより、誤差楕円体制御を行うかどうかを決定している。
【００１９】
しかしながら、残差二次形式により誤差楕円体の重複度を判定する際、観測雑音が大きい、あるいはサンプリング間隔が短い場合には、良好な判定結果が得られない可能性があり、また、誤差楕円体重複度係数を用いる際に、誤差楕円体重複度係数の算出に、残差共分散行列の固有値を求める必要があるため、残差を用いる方法よりも計算負荷が大きいという問題がある。
【００２０】
さらに、フィルタの残差共分散行列を乗算係数によって定数倍するため、図１３に示すように、誤差楕円体は、その形状を変えずに拡大、縮小が行われる。そのため、観測雑音の大きさが異なる距離及び角度方向で、所望の誤差楕円体重複度を各軸方向で独立して実現することが難しく、所望の重複度を得るには各方向について異なる定数加速度を設定して予測値間距離を調整する必要がある。
【００２１】
この発明は、前記の問題を解決するためになされたもので、誤差楕円体の重複度判定によりフィルタのモデルでは目標の運動を表現できないと判定された場合、信頼度計算における残差共分散行列を再計算することにより、観測雑音が異なる距離、角度方向それぞれについて所望の誤差楕円体重複度を実現し、追尾性能の向上を図ることができる目標追尾装置及び方法を得ることを目的とする。
また、小さい計算負荷で、より安定した誤差楕円体の重複度判定を行うことができる目標追尾装置及び方法を得ることを目的とする。
【００２２】
【課題を解決するための手段】
この発明に係る目標追尾装置は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾装置であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御手段を備え、前記誤差楕円体制御手段は、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、誤差楕円体重複度係数をデータベースより検索する誤差楕円体重複度検索手段と、前記誤差楕円体重複検索手段から得られた重複度係数と各運動モデルの予測値間距離より誤差楕円体の各軸半径を算出し、算出された半径よりＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出手段とを有することを特徴とする。
また、他の発明に係る目標追尾装置は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾装置であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御手段を備え、前記誤差楕円体制御手段は、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、各運動モデルの予測値間の中点における尤度をデータベースより検索する予測値間尤度検索手段と、前記予測値間尤度検索手段から得られた最適な尤度及び予測値と中点との距離を正規分布表に照らし合わせることにより、ＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出手段とを有することを特徴とする。
また、この発明に係る目標追尾方法は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾方法であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御ステップを備え、前記誤差楕円体制御ステップは、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定ステップと、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、誤差楕円体重複度係数をデータベースより検索する誤差楕円体重複度検索ステップと、前記誤差楕円体重複検索ステップから得られた重複度係数と各運動モデルの予測値間距離より誤差楕円体の各軸半径を算出し、算出された半径よりＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列 をフィルタの座標系へ変換する残差共分散行列算出ステップとを有することを特徴とする。
さらに、他の発明に係る目標追尾方法は、目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾方法であって、多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御ステップを備え、前記誤差楕円体制御ステップは、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定ステップと、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、各運動モデルの予測値間の中点における尤度をデータベースより検索する予測値間尤度検索ステップと、前記予測値間尤度検索ステップから得られた最適な尤度及び予測値と中点との距離を正規分布表に照らし合わせることにより、ＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出ステップとを有することを特徴とする。
【００３６】
【発明の実施の形態】
実施の形態１．
以下に、この発明に係る目標追尾装置において追尾フィルタとして用いるＮ個の運動モデルを持つ多重運動モデルを用いたカルマンフィルタについて説明する。目標の運動モデルを式（１）のように定義する。
【数１】
ここで、Ｘkはサンプリング時刻ｔkにおける目標運動諸元の真値をあらわす状態ベクトルであり、例えば、ｘｙｚの直交座標系における目標位置ベクトルを式（２）、直交座標系における目標速度ベクトルを式（３）とした時、Ｘkは式（４）である。
【数２】
Φk-1はサンプリング時刻ｔk-1よりｔkへの状態ベクトルＸkの推移を表す推移行列で、目標が等速直線運動を行うと仮定した場合、式（５）である。ここでＩは式（６）の通り単位行列である。
【数３】
ｗkはサンプリング時刻ｔkにおける駆動雑音ベクトルで、Γkはサンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列である。目標の運動モデルを等速直線運動と仮定したことによる打切り誤差項をΓk-1ｗk-1とみれば、ｗkは加速度ベクトル相当であり、Γk-1は式（７）である。
【数４】
ｗkは平均０の３次元正規分布白色雑音で、式（８）、式（９）とする。ここで、０は零ベクトルを表し、Ｑkはサンプリング時刻ｔｋにおける駆動雑音共分散行列である。
【数５】
ｕkはサンプリング時刻ｔkにおいてＮ個の運動モデルを構成する定数加速度ベクトルであり式（１０）である。
【数６】
【数７】
次に、図１４は、定数加速度ベクトルを説明する図であり、図１４において、Ｏは目標観測装置を原点とした座標Ｏ−ｘｙｚの原点、Ｘは東方向をＸ軸の正とした座標Ｏ−ｘｙｚのＸ軸、Ｙは北方向をＹ軸の正とした座標Ｏ−ｘｙｚのＹ軸、Ｚは上方向をＺ軸の正とした座標Ｏ−ｘｙｚのＺ軸、α1はＹ軸正方向の定数加速度ベクトル、α2はＹ軸負方向の定数加速度ベクトル、α3はＸ軸正方向の定数ベクトル、α4はＸ軸正方向の定数加速度ベクトル、α5はＺ軸正方向の定数加速度ベクトル、α6はＺ軸負方向の定数加速度ベクトルである。この他に、定数加速度ベクトルα7＝０を考えた運動モデルの場合、モデル数Ｎは７となる。定数加速度ベクトルを定義する座標としては、図１５に示すＬＯＳ直交座標、運動座標、北基準直交座標などが考えられる。ここで、運動座標とは目標の推定速度ベクトルを１軸とした座標であり、ＬＯＳ直交座標とはレーダと目標を結んだベクトルを１軸とした座標のことである。
【００４３】
次に、サンプリング時刻ｔkにおいて式（１２）が真であるとの仮説を式（１３）と書く。
【数８】
【００４４】
時刻ｔkにおける運動モデル間推移確率を式（１４）のように表す。例えば、最小サンプリング間隔をｔｓとし、サンプリング間隔ｔk−ｔk-1＝ｔｓの場合の運動モデル間推移確率を式（１４）と同様の意味でＰabとする。さらに、Ｎ×Ｎ行列でａ行ｂ列の要素をＰabとしたものを運動モデル間推移確率行列Πと表す。サンプリング間隔がｔk−ｔk-1＝ｍ・ｔs，（ｍ＝１，２，・・・）と表せる場合、サンプリング時刻ｔkでの運動モデル間推移確率行列Πkは式（１５）で表せる。このときのΠkのａ行ｂ列の要素が運動モデル間推移確率Ｐk,abである。
【数９】
サンプリング時刻ｔkまでの観測情報Ｚkに基づく、サンプリング時刻ｔkでの運動モデルの信頼度を条件付き確率密度関数により、式（１６）と定義すると、式（１７）で計算される。
【数１０】
【数１１】
【００４６】
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくサンプリング時刻ｔkでの運動モデルの事前信頼度を条件付き確率密度関数により、式（１９）と定義すると、式（２０）と書ける。
【数１２】
サンプリング時刻ｔkまでの観測情報Ｚkに基づく、定数加速度ベクトルの推定値を式（２１）として定義すると、式（２２）と書ける。これを推定加速度ベクトルと呼ぶ。
【数１３】
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づく、定数加速度ベクトルの推定値を式（２３）として定義すると、式（２４）と書ける。これを予測加速度ベクトルと呼ぶ。
【数１４】
追尾対象目標よりの観測値はサンプリング時刻ｔkにおいて１つ得られるとし、その観測系モデルを式（２５）とする。ここで、Ｚkはサンプリング時刻ｔkにおける位置情報の観測値より構成される直交座標による位置観測ベクトル、Ｈはサンプリング時刻ｔkにおける観測行列で、式（２６）である。ｖkはサンプリング時刻ｔkでの観測ベクトルＺkに対応した観測雑音ベクトルであり、平均０の３次元正規分布白色雑音で、式（２７）、式（２８）である。なお、Ｒkはサンプリング時刻ｔkにおける観測雑音共分散行列で、運動モデルによらない値とする。また、駆動雑音ベクトルと観測雑音ベクトルは互いに独立であるとする。
【００５０】
【数１５】
【数１６】
カルマンフィルタの理論によれば、前記モデルに従いサンプリング時刻ｔkで観測値が得られた場合の、状態ベクトルＸkの推定値Ｘk（＋）は式（３０）〜（３８）によって計算される。
【００５２】
【数１７】
ここで、Ｘk,a（−）、Ｘk,a（＋）、Ｘk（−）、Ｘk（＋）、Ｐk,a（−）、Ｐk,a（＋）、Ｐk（−）、Ｐk（＋）は、それぞれ式（３９）〜（４６）のように定義される。
【００５４】
【数１８】
Ｘk,a（−）はサンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値で、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aに基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００５６】
Ｘk,a（＋）はサンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値で、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００５７】
Ｘk（−）はサンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくＸkの条件付き平均値で、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００５８】
Ｘk（＋）はサンプリング時刻ｔkまでの観測情報Ｚkに基づくＸkの条件付き平均値で、サンプリング時刻ｔｋまでの観測情報Ｚkに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００５９】
Ｐk,a（−）,Ｐk,a（＋）、Ｐk（−）、Ｐk（＋）は、それぞれＸk,a（−）、Ｘk,a（＋）、Ｘk（−）、Ｘk（＋）の誤差共分散行列を表す運動モデルごとの予測誤差共分散行列、運動モデルごとの平滑誤差共分散行列、N個の運動モデルによる予測誤差共分散行列およびＮ個の運動モデルによる平滑誤差共分散行列である。
【００６０】
式（３４）はサンプリング時刻ｔkにおけるゲイン行列である。また、カルマンフィルタを通常適用する場合と同様にして、初期値Ｘ0（＋）、Ｐ0（＋）は別途定まっているものとする。
【００６１】
式（３１）より、Ｐk,a（−）は仮説Ψk,aによらない値なので、式（３４）よりＫk、式（３６）よりＰk,a（＋）も同様に仮説Ψk,aによらない値となる。
【００６２】
次に、図１は、本実施の形態１に係る目標追尾装置の構成を示すブロック図である。図１において、図１０に示す従来例と同一部分、つまり誤差楕円体制御装置以外の構成は同一のため同一符号を付しその説明は省略する。本実施の形態１に係る誤差楕円体制御装置としては、第３の遅延回路１１と、本実施の形態１に係る誤差楕円体重複度判定器２２と、誤差楕円体重複度検索器２３と、残差共分散行列算出器２４と、第６の遅延回路２５とを備えている。
【００６３】
ここで、誤差楕円体重複度判定器２２は、各モデル毎予測器９より運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は誤差楕円体重複度検索器２３へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出器３へ出力させるためのスイッチ切替を行う。
【００６４】
次に、誤差楕円体重複度検索器２３は、誤差楕円体重複度判定器２２により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測器９より各モデル毎の予測値、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した誤差楕円体重複度係数をデータベースより検索し、誤差楕円体重複度係数と各運動モデルの予測値を残差共分散行列算出器２４へ出力する。
【００６５】
また、残差共分散行列算出器２４は、誤差楕円体重複度検索器２３から入力された誤差楕円体重複度係数及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸半径を算出する。そして、算出した各軸半径より、ＬＯＳ直交座標における残差共分散行列を算出し、フィルタの座標系へ座標変換した後、信頼度算出器３へと出力される。
【００６６】
図２は、本実施の形態１に係る目標追尾装置の動作を説明するフローチャートである。誤差楕円体制御手段以外の動作は、図１０に示す従来例と同様であり、その説明は省略する。なお、フローチャートは、目標追尾方法の説明を兼ねる。誤差楕円体重複度判定ステップ（Ｓ２０１）は、各モデル予測ステップより運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は誤差楕円体重複度検索ステップ（Ｓ２０２）へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出ステップ（Ｓ１０８）へ出力させるための切替を行う。
【００６７】
次に、誤差楕円体重複度検索ステップ（Ｓ２０２）は、誤差楕円体重複度判定ステップ（Ｓ２０１）により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測ステップより各モデル毎の予測値、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。
【００６８】
そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した誤差楕円体重複度係数をデータベースより検索し、誤差楕円体重複度係数と各運動モデルの予測値を残差共分散行列算出ステップへ出力する。残差共分散行列算出ステップは、誤差楕円体重複度検索ステップ（Ｓ２０２）から入力された誤差楕円体重複度係数及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸半径を算出する（Ｓ２０３）。そして算出した各軸半径より、ＬＯＳ直交座標における残差共分散行列を算出し（Ｓ２０４）、フィルタの座標系へ座標変換した後、信頼度算出ステップ（Ｓ１０８）へと出力される。
【００６９】
以上のように、本実施の形態１に係る目標追尾装置及び方法によれば、誤差楕円体重複度係数あるいは残差二次形式より重複度を判定するよりも、各運動モデルの予測値間の中点における尤度を用いることで、観測雑音が大きい、あるいはサンプリング間隔が短い場合でも、小さい計算負荷で安定した誤差楕円体重複度判定を行うことが可能となる。
【００７０】
また、データベースより最適な誤差楕円体重複度係数を検索し、誤差楕円体重複度係数より誤差楕円体の各軸半径を介して残差共分散行列を算出することにより、図９に示すように、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００７１】
実施の形態２．
次に、実施の形態２に係る目標追尾装置における誤差楕円体重複度判定器２２の動作を説明するフローチャートを図３に示す。
はじめに、誤差楕円体重複度判定器２２は、各モデル毎予測器９より各運動モデルの予測値および残差共分散行列を読み込み、各モデルの予測値間の中点における位置座標を求める（Ｓ３０１）。次いで、式（４７）に予測値、予測値間の中点座標および残差共分散行列を代入することにより、各予測値間の中点における尤度を算出する（Ｓ３０２）。
【数１９】
【００７２】
次いで、図４に示すように、各運動モデル間の尤度と２つのしきい値を比較し、すべての尤度がしきい値２を越えた場合（Ｓ３０４）、各運動モデルの誤差楕円体の重複度は大きすぎると判定され、一方ですべての尤度がしきい値１を下回った場合（Ｓ３０３）、重複度は小さすぎると判定され、残差共分散行列の再計算が行われる（Ｓ３０５）。すべての尤度がしきい値１を下回らなく、かつしきい値２を下回った場合（Ｓ３０４）、重複度は適切と判定される（Ｓ３０６）。ステップＳ３０５，Ｓ３０６を経た後は、ステップＳ３０７に移行し、信頼度算出器３への出力切り替えが行われる。処理終了を判定し（Ｓ３０８）、終了されていない場合は、ステップＳ３０１にリターンする。
【００７３】
以上のように、本実施の形態２に係る目標追尾装置によれば、誤差楕円体重複度係数あるいは残差二次形式より重複度を判定するよりも、各運動モデルの予測値間の中点における尤度を用いることで、多目標あるいはクラッタ環境下においても小さい計算負荷で誤差楕円体重複度の判定を行うことが可能となる。
【００７４】
実施の形態３．
次に、図５は、本実施の形態３における誤差楕円体重複度検索器２３および残差共分散行列算出器２４の動作を説明するフローチャートである。はじめに、誤差楕円体重複度判定器２２により信頼度計算における残差共分散行列の再計算が必要とされた場合、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせ条件に最も適した誤差楕円体重複度係数をデータベースより検索する（Ｓ５０１）。ここで、誤差楕円体重複度係数γk,aは、以下の式より定義され、データベースへ事前に登録される。
【数２０】
【００７５】
次いで、データベース検索によって得られた最適な誤差楕円体重複度係数、および各モデル毎予測器９より得られた予測値間の距離を式（４８）に代入し、所望の誤差楕円体重複度を実現する誤差楕円体の各軸半径ｌk,iを求める（Ｓ５０２）。ここで、誤差楕円体はレーダと目標を結ぶＬＯＳベクトルに対して垂直であると仮定することにより、各軸半径よりＬＯＳ座標における残差共分散行列を求めることができる。
【００７６】
次いで、各軸半径ｌk,iを式（４９）に代入することにより、ＬＯＳ直交座標における残差共分散行列の対角成分である固有値λk,iが算出される。
【数２１】
【００７７】
ここで、誤差楕円体がＬＯＳ軸に対して垂直であれば各軸の相関はなく、残差共分散行列の固有値λk,iは各軸方向の分散となる（Ｓ５０３）。そのため、残差共分散行列は、算出した固有値を用いて以下の式より算出される。
【数２２】
【００７８】
算出されたＬＯＳ直交座標における残差共分散行列は、フィルタの状態ベクトルが北基準直交座標で定義されている場合、以下の行列Ｆにより座標変換される（Ｓ５０４）。
【数２３】
次いで、座標変換された残差共分散行列は、信頼度計算器３へと出力される。
【００７９】
以上のように、本実施の形態３によれば、データベースより最適な誤差楕円体重複度係数を検索し、誤差楕円体重複度係数より誤差楕円体の各軸半径を介して残差共分散行列を算出することにより、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、サンプリング間隔が長く定数加速度が大きい場合や、逆にサンプリング間隔が短く定数加速度が小さい場合においても、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００８０】
実施の形態４．
次に、図６は、本実施の形態４に係る目標追尾装置の構成を示すブロック図である。図６において、図１０に示す従来例と同一部分、つまり誤差楕円体制御装置以外の構成は同一のため同一符号を付しその説明は省略する。本実施の形態４に係る誤差楕円体制御装置としては、第３の遅延回路１１と、誤差楕円体重複度判定器２２と、本実施の形態４に係る予測値間尤度検索器２６と、残差共分散行列算出器２４と、第６の遅延回路２５とを備えている。
【００８１】
ここで、誤差楕円体重複度判定器２２は、各モデル毎予測器９より運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は予測値間尤度検索器２６へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出器３へ出力させるためのスイッチ切替を行う。
【００８２】
次に、予測値間尤度検索器２６は、誤差楕円体重複度判定器２２により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測器９より各モデル毎の予測値、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した、各運動モデルの予測値間の中点における尤度をデータベースより検索し、尤度と各運動モデルの予測値を残差共分散行列算出器２４へ出力する。
【００８３】
残差共分散行列算出器２４は、予測値間尤度検索器２６から入力された尤度及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸方向の分散を算出する。そして算出した各軸方向の分散より、ＬＯＳ直交座標における残差共分散行列を算出し、フィルタの座標系へ座標変換した後、信頼度算出器３へ出力する。
【００８４】
図７は、本実施の形態４に係る目標追尾装置の動作を説明するフローチャートである。誤差楕円体制御装置以外の動作は、図１０に示す従来装置と同様であり、その説明は省略する。なお、フローチャートは目標追尾方法の説明を兼ねる。誤差楕円体重複度判定ステップ（Ｓ７０１）は、各モデル毎予測器９より運動モデル毎の予測値、運動モデル毎の予測誤差共分散行列を読み込み、予測値間の中点における尤度としきい値を比較する。判定情報は予測値間尤度検索ステップ（Ｓ７０２）へ出力され、また、フィルタの残差共分散行列もしくは再計算により求められた残差共分散行列を信頼度算出ステップ（Ｓ１０８）へ出力させるための切替を行う。
【００８５】
次に、予測値間尤度検索ステップ（Ｓ７０２）は、誤差楕円体重複度判定ステップ（Ｓ７０１）により残差共分散行列の再計算が必要と判定された場合、各モデル毎予測ステップより各モデル毎の予測位置、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を取得する。
【００８６】
そして、レーダの観測条件および目標の運動条件の組み合わせに最も適した、各運動モデルの予測値間の中点における尤度をデータベースより検索し、尤度と各運動モデルの予測値を残差共分散行列算出ステップへ出力する。残差共分散行列算出ステップは、予測値間尤度検索ステップ（Ｓ７０２）から入力された尤度及び各運動モデルの予測値より、新たに形成する誤差楕円体の各軸方向の分散を算出する（Ｓ７０３）。そして算出した各軸方向の分散より、ＬＯＳ直交座標における残差共分散行列を算出し（Ｓ７０４）、フィルタの座標系へ座標変換した後、信頼度算出ステップ（Ｓ１０８）へ出力する。
【００８７】
以上のように、本実施の形態４に係る目標追尾装置及び方法によれば、誤差楕円体重複度係数あるいは残差二次形式より重複度を判定するよりも、各運動モデルの予測値間の中点における尤度を用いることで、観測雑音が大きい、あるいはサンプリング間隔が短い場合でも、小さい計算負荷で安定した誤差楕円体重複度判定を行うことが可能となる。
【００８８】
また、データベースより最適な各運動モデルの予測値間の中点における尤度を検索し、尤度から新たに形成する誤差楕円体の分散を求めて残差共分散行列を算出することにより、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、サンプリング間隔が長く定数加速度が大きい場合や、逆にサンプリング間隔が短く定数加速度が小さい場合においても、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００８９】
実施の形態５．
次に、図８は、本実施の形態５における予測値間尤度検索器２６および残差共分散行列算出器２４の動作を説明するフローチャートである。以下に図を用いて説明する。はじめに、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせ条件に最も適した予測値間の中点における尤度をデータベースより検索する（Ｓ８０１）。
【００９０】
次いで、誤差楕円体がＬＯＳ軸に垂直であり、かつ目標の存在確率密度分布が３変量正規分布であるとの仮定のもとに、データベース検索によって得られた最適な尤度及び予測値と中点との距離を事前に用意した正規分布表と照らし合わせることにより、誤差楕円体の各軸方向の分散を求める（Ｓ８０２）。
【００９１】
ＬＯＳ直交座標における残差共分散行列Ｓ’k,aは、以下の式より表される（Ｓ８０３）。
【数２４】
ここで、σ2 r，σ2 az，σ2 elはそれぞれＬＯＳ方向、ＬＯＳ軸に対して左右方向、ＬＯＳ軸に対して上下方向における確率密度分布の分散となる。算出されたＬＯＳ直交座標における残差共分散行列は、フィルタの状態ベクトルが定義されている北基準直交座標へ座標変換される（Ｓ８０４）。
次いで、再計算された残差共分散行列は、信頼度計算器３へと出力される。
【００９３】
以上のように、本実施の形態５によれば、データベースより予測値間の中点における最適な尤度を検索し、ＬＯＳ直交座標の残差共分散行列の各軸における最適な分散を求めることで、各軸方向において所望の誤差楕円体重複度を実現することが可能となり、サンプリング間隔が長く定数加速度が大きい場合や、逆にサンプリング間隔が短く定数加速度が小さい場合においても、等速直線運動目標および旋回目標に対してより良好な追尾精度を得ることができる。
【００９４】
【発明の効果】
以上のように、この発明によれば、誤差楕円体の重複度を判定することにより、小さい計算負荷で事前に精度良く追尾精度の劣化の可能性を知り、誤差楕円体を各軸方向について任意の大きさに制御して所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９５】
また、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、前記尤度と２つのしきい値を比較することにより、残差共分散行列より誤差楕円体重複度係数を求めて重複度を判定する方法よりも小さい計算負荷で、また、残差二次形式を求めて重複度を判定するよりも高い精度で誤差楕円体重複度判定を行うことができる。
【００９６】
また、ＬＯＳ直交座標における誤差楕円体はＬＯＳ軸に対して垂直であるとの仮定のもとに、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な確率密度分布の分散を算出して残差共分散行列を求め、前記残差共分散行列をＬＯＳ直交座標よりフィルタの座標系へと変換することにより、各軸方向それぞれについて任意の誤差楕円体の形状が得られるため、所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９７】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な誤差楕円体重複度係数から誤差楕円体の半径を算出し、さらに各軸方向の半径より誤差楕円体の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９８】
また、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な誤差楕円体重複度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【００９９】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な各運動モデルの予測値間の中点における尤度より、各軸における確率密度分布の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１００】
また、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な予測値間の中点における尤度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０１】
また、誤差楕円体の重複度を判定することにより、小さい計算負荷で事前に精度良く追尾精度の劣化の可能性を知り、誤差楕円体を各軸方向について任意の大きさに制御して所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０２】
また、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、前記尤度と２つのしきい値を比較することにより、残差共分散行列より誤差楕円体重複度係数を求めて重複度を判定する方法よりも小さい計算負荷で、また、残差二次形式を求めて重複度を判定するよりも高い精度で誤差楕円体重複度判定を行うことができる。
【０１０３】
また、ＬＯＳ直交座標における誤差楕円体はＬＯＳ軸に対して垂直であるとの仮定のもとに、レーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な確率密度分布の分散を算出して残差共分散行列を求め、前記残差共分散行列をＬＯＳ直交座標よりフィルタの座標系へと変換することにより、各軸方向それぞれについて任意の誤差楕円体の形状が得られるため、所望の誤差楕円体重複度を実現することにより、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０４】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な誤差楕円体重複度係数から誤差楕円体の半径を算出し、さらに各軸方向の半径より誤差楕円体の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０５】
また、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な誤差楕円体重複度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０６】
また、各運動モデルの予測値間距離とレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせに対する最適な各運動モデルの予測値間の中点における尤度より、各軸における確率密度分布の分散を算出して残差共分散行列を求めることにより、所望の誤差楕円体重複度を実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【０１０７】
さらに、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）を入力することにより、前記残差共分散行列算出に必要な、最適な予測値間の中点における尤度を得ることができ、所望の誤差楕円体の重なりを実現し、等速直線運動目標に対して観測雑音による誤差を低減しつつ、旋回目標に対しても良好な追尾精度を得ることができる。
【図面の簡単な説明】
【図１】 この発明の実施の形態１に係る目標追尾装置の構成を示すブロック図である。
【図２】 この発明の実施の形態１に係る目標追尾方法を示すフローチャートである。
【図３】 この発明の実施の形態２に係る目標追尾装置の誤差楕円体重複度判定器２２の動作を示すフローチャートである。
【図４】 この発明の実施の形態２に係る誤差楕円体重複度判定器２２における尤度としきい値の比較の様子を示した図である。
【図５】 この発明の実施の形態３に係る目標追尾装置の誤差楕円体重複度係数検索器及び残差共分散行列算出器２４の動作を示すフローチャートである。
【図６】 この発明の実施の形態４に係る目標追尾装置の構成を示すブロック図である。
【図７】 この発明の実施の形態４に係る目標追尾方法を示すフローチャートである。
【図８】 この発明の実施の形態５に係る目標追尾装置の予測値間尤度検索器２６及び残差共分散行列算出器２４の動作を示すフローチャートである。
【図９】 残差共分散行列再計算による誤差楕円体制御の様子を示した図である。
【図１０】 従来の目標追尾装置の構成を示すブロック図である。
【図１１】 従来の目標追尾装置の動作を示すフローチャートである。
【図１２】 複数の運動モデルの誤差楕円体重複度係数と、予測値間の距離および各軸半径の関係を示した図である。
【図１３】 従来の残差を用いた誤差楕円体制御の様子を示した図である。
【図１４】 目標の運動モデルとして、設定されたＮ個の定数加速度ベクトル（零加速度ベクトルを含む）を示した図である。
【図１５】 北基準直座標とＬＯＳ直交座標、運動座標の関係を示した図である。
【符号の説明】
１ 目標観測装置、２ ゲート判定器、３ 信頼度算出器、４ 信頼度メモリ、５ 平滑器、６ 平滑器メモリ、７ 予測器、８ 第１の遅延回路、９ 各モデル毎予測器、１０ 第２の遅延回路、１１ 第３の遅延回路、１２ ゲイン行列算出器、１３ 第４の遅延回路、１４ モデル設定器、１５ 入力ベクトル平滑器、１６ 入力ベクトル予測器、１７ 第５の遅延回路、２２ 誤差楕円体重複度判定器、２３ 誤差楕円体重複度検索器、２４ 残差共分散行列算出器、２５ 第６の遅延回路、２６ 予測値間尤度検索器。
Claims (8)
- 目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾装置であって、
多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御手段を備え、
前記誤差楕円体制御手段は、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、誤差楕円体重複度係数をデータベースより検索する誤差楕円体重複度検索手段と、前記誤差楕円体重複検索手段から得られた重複度係数と各運動モデルの予測値間距離より誤差楕円体の各軸半径を算出し、算出された半径よりＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出手段とを有する
ことを特徴とする目標追尾装置。 - 請求項１に記載の目標追尾装置において、
前記誤差楕円体重複度検索手段は、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせを入力し、これらの条件に適した、事前にデータベースに登録されている誤差楕円体重複度係数を出力する
ことを特徴とする目標追尾装置。 - 目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾装置であって、
多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御手段を備え、
前記誤差楕円体制御手段は、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、各運動モデルの予測値間の中点における尤度をデータベースより検索する予測値間尤度検索手段と、前記予測値間尤度検索手段から得られた最適な尤度及び予測値と中点との距離を正規分布表に照らし合わせることにより、ＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出手段とを有する
ことを特徴とする目標追尾装置。 - 請求項３に記載の目標追尾装置において、
前記予測値間尤度検索手段は、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせを入力し、条件と一致するこれらの条件に適した、事前にデータベースに登録されている尤度を出力する
ことを特徴とする目標追尾装置。 - 目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾方法であって、
多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御ステップを備え、
前記誤差楕円体制御ステップは、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定ステップと、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、誤差楕円体重複度係数をデータベースより検索する誤差楕円体重複度検索ステップと、前記誤差楕円体重複検索ステップから得られた重複度係数と各運動モデルの予測値間距離より誤差楕円体の各軸半径を算出し、算出された半径よりＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出ステップとを有する
ことを特徴とする目標追尾方法。 - 請求項５に記載の目標追尾方法において、
前記誤差楕円体重複度検索ステップは、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせを入力し、これらの条件に適した、事前にデータベースに登録されている誤差楕円体重複度係数を出力する
ことを特徴とする目標追尾方法。 - 目標観測装置で取得した目標位置に関する観測情報を基に目標の追尾を行う目標追尾方法であって、
多重運動モデルを用いたカルマンフィルタにおける各運動モデルの信頼度を計算する際、Ｎ個の運動モデルの予測値を中心とした目標の存在期待確率を示す等確率密度ラインである誤差楕円体同士の重複度を判定し、重複度が設定値を満たしていないと判定された場合に、信頼度算出に用いる残差共分散行列を再計算し、各軸方向で誤差楕円体を所望の大きさに制御する誤差楕円体制御ステップを備え、
前記誤差楕円体制御ステップは、誤差楕円体同士の重複度として、各運動モデルの予測値間の中点における尤度をフィルタの残差共分散行列を用いて算出し、算出された尤度と設定値としての２つの事前に設定したしきい値との比較に基づいて誤差楕円体の重複度を判定する誤差楕円体重複度判定ステップと、その判定結果により信頼度計算における残差共分散行列の再計算が必要と判断された場合に、各運動モデルの予測値間の中点における尤度をデータベースより検索する予測値間尤度検索ステップと、前記予測値間尤度検索ステップから得られた最適な尤度及び予測値と中点との距離を正規分布表に照らし合わせることにより、ＬＯＳ（ Line Of Sight ）直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列の対角成分である各軸方向の分散を算出し、算出したＬＯＳ直交座標における残差共分散行列をフィルタの座標系へ変換する残差共分散行列算出ステップとを有する
ことを特徴とする目標追尾方法。 - 請求項７に記載の目標追尾方法において、
前記予測値間尤度検索ステップは、データベースにレーダの観測条件（サンプリング間隔、観測雑音の標準偏差）および目標の運動条件（目標の平滑速度、ゲート内の観測値数）の組み合わせを入力し、条件と一致するこれらの条件に適した、事前にデータベースに登録されている尤度を出力する
ことを特徴とする目標追尾方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001327781A JP3926602B2 (ja) | 2001-10-25 | 2001-10-25 | 目標追尾装置及び方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001327781A JP3926602B2 (ja) | 2001-10-25 | 2001-10-25 | 目標追尾装置及び方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003130947A JP2003130947A (ja) | 2003-05-08 |
| JP3926602B2 true JP3926602B2 (ja) | 2007-06-06 |
Family
ID=19143956
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2001327781A Expired - Lifetime JP3926602B2 (ja) | 2001-10-25 | 2001-10-25 | 目標追尾装置及び方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3926602B2 (ja) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4176985B2 (ja) * | 2001-11-07 | 2008-11-05 | 三菱電機株式会社 | 追尾装置 |
| JP4977492B2 (ja) | 2007-02-26 | 2012-07-18 | 日本電気株式会社 | 目標追尾装置及び目標追尾方法 |
| JP5495581B2 (ja) * | 2009-02-20 | 2014-05-21 | 三菱電機株式会社 | センサ制御装置 |
| JP6944402B2 (ja) * | 2018-03-08 | 2021-10-06 | パナソニック インテレクチュアル プロパティ コーポレーション オブ アメリカＰａｎａｓｏｎｉｃ Ｉｎｔｅｌｌｅｃｔｕａｌ Ｐｒｏｐｅｒｔｙ Ｃｏｒｐｏｒａｔｉｏｎ ｏｆ Ａｍｅｒｉｃａ | 在不在判定方法、プログラム、センサ処理システム、及びセンサシステム |
| CN113155122A (zh) * | 2021-04-01 | 2021-07-23 | 广州大学 | 基于自适应滤波的机动目标跟踪方法 |
| CN117630907B (zh) * | 2023-11-29 | 2024-05-28 | 哈尔滨工业大学（威海） | 一种红外成像与毫米波雷达融合的海面目标跟踪方法 |
-
2001
- 2001-10-25 JP JP2001327781A patent/JP3926602B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003130947A (ja) | 2003-05-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Dellaert et al. | Monte carlo localization for mobile robots | |
| Chiu et al. | Robust vision-aided navigation using sliding-window factor graphs | |
| JP7131994B2 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、学習装置、学習方法及び学習プログラム | |
| JP4348535B2 (ja) | 目標追尾装置 | |
| US11525691B2 (en) | System and method for autonomous motion planning | |
| JP4176985B2 (ja) | 追尾装置 | |
| JP3926602B2 (ja) | 目標追尾装置及び方法 | |
| JP2004251660A (ja) | 目標追尾装置 | |
| CN114608585A (zh) | 一种移动机器人同步定位与建图方法及装置 | |
| JP5247066B2 (ja) | 目標追尾装置 | |
| JP2009014596A (ja) | 目標追尾装置及び目標追尾方法 | |
| US20240134033A1 (en) | Method for determining a movement state of a rigid body | |
| JP3659575B2 (ja) | 目標追尾装置 | |
| JP3739622B2 (ja) | 目標追尾装置および目標追尾方法 | |
| JP2003149328A (ja) | レーダの目標相関装置 | |
| JP3819237B2 (ja) | 追尾装置 | |
| JP3792927B2 (ja) | 目標追尾装置 | |
| JP2001153947A (ja) | 追尾処理装置及び方法 | |
| JP3323415B2 (ja) | 目標追尾装置及びその目標追尾方法 | |
| JP4115291B2 (ja) | 目標追尾装置 | |
| JP2001060265A (ja) | 画像処理装置および方法、並びに媒体 | |
| JP3440010B2 (ja) | 目標追尾装置 | |
| JP3302870B2 (ja) | センサのバイアス誤差推定装置 | |
| JP3848846B2 (ja) | 追尾装置 | |
| JP2843910B2 (ja) | 多目標追尾装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20040922 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20061128 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20061205 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070202 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20070227 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070228 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 3926602 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100309 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110309 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110309 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120309 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130309 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130309 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140309 Year of fee payment: 7 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| EXPY | Cancellation because of completion of term |