JP4080722B2 - Target tracking device and target tracking method - Google Patents

Description

【０００５】
ここで、ΔＴは、直線状周波数変調方式で決まる定数である。上記文献では、この加算分ΔＴ(ドット)Ｒを考慮した数式モデルを用いた追尾方法を提案している。
【０００６】
以下、図面を参照しながら、従来の技術について詳細に説明する。図１７は従来のこの種の目標追尾装置のブロック構成図であり、図１８はその処理動作を示すフローチャート、図１９は従来の技術における動作を説明するための概念図である。
【０００７】
図１７において、目標観測装置２０は空間中の目標位置を観測するセンサからなる装置、目標追尾装置１０は目標観測装置２０からの観測データに基づいて目標追尾処理を実行する装置、航跡表示装置３０は目標追尾装置１０の処理結果である航跡を運用者に示す装置である。
【０００８】
目標追尾装置１０は、予測観測ベクトル算出部１０１、予測ベクトル算出部１０３、平滑ベクトル用メモリ１０５、ゲート判定部１０７、平滑ベクトル算出部１０９、残差共分散行列算出部１１１、ゲイン行列算出部１１３、観測雑音共分散行列設定部１１５、予測誤差共分散行列算出部１１７、駆動雑音共分散行列設定部１１９、平滑誤差共分散行列算出部１２１、平滑誤差共分散行列用メモリ１２３を備えている。
【０００９】
目標追尾装置１０内において、平滑ベクトル用メモリ１０５は、平滑ベクトル算出部１０９による平滑ベクトルの計算結果を格納する。また、平滑誤差共分散行列用メモリ１２３は、平滑誤差共分散行列算出部１２１による平滑誤差共分散行列の計算結果を格納する。予測ベクトル算出部１０３は、平滑ベクトル用メモリ１０５に格納された前時刻の平滑ベクトルから現時刻の予測ベクトルを算出する。駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列を設定する。予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３に格納された前時刻の平滑誤差共分散行列の計算結果と、駆動雑音共分散行列設定部１１９により設定された駆動雑音共分散行列の値とから、現時刻の予測誤差共分散行列を算出する。
【００１０】
観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトルの精度評価に関連したパラメータに基づいて、観測雑音共分散行列を設定する。残差共分散行列算出部１１１は、予測誤差共分散行列算出部１１７により算出された予測誤差共分散行列と、観測雑音共分散行列設定部１１５により設定された観測雑音共分散行列とから、残差共分散行列を算出する。予測観測ベクトル算出部１０１は、予測ベクトル算出部１０３により算出された予測ベクトルから予測観測ベクトルを算出する。ゲート判定部１０７は、予測観測ベクトル算出部１０１により算出された予測観測ベクトルと、目標観測装置２０からの観測ベクトルとに基づいて、残差ベクトルを算出するとともに、残差共分散行列算出部１１１により算出された残差共分散行列を用いて、観測ベクトルが追尾目標の探知データであるか否かのゲート判定を行う。
【００１１】
ゲイン行列算出部１１３は、予測誤差共分散行列算出部１１７により算出された予測誤差共分散行列と、残差共分散行列算出部１１１により算出された残差共分散行列とを用いて、フィルタのゲイン行列を算出する。平滑ベクトル算出部１０９は、ゲート判定部１０７により出力された観測ベクトルと、予測ベクトル算出部１０３より算出された予測ベクトルと、予測観測ベクトル算出部１０１より算出された予測観測ベクトルと、ゲイン行列算出部１１３により算出されたゲイン行列とを用いて、平滑ベクトルを算出する。平滑誤差共分散行列算出部１２１は、予測誤差共分散行列算出部１１７により算出された予測誤差共分散行列と、観測雑音共分散行列設定部１１５により算出された観測雑音共分散行列とを用いて、平滑誤差共分散行列を算出する。
【００１２】
図１８において、初期値設定ステップＳ１は、平滑ベクトルと平滑誤差共分散行列の初期値を設定する。観測ベクトルの入力ステップＳ２は、目標観測装置２０から現時刻の観測ベクトルを読み込む。予測ベクトルの算出ステップＳ３は、前時刻の平滑ベクトルから予測ベクトルを算出する。駆動雑音共分散行列の設定ステップＳ４は、駆動雑音共分散行列を設定する。
【００１３】
予測誤差共分散行列の算出ステップＳ５は、前時刻の平滑誤差共分散行列と駆動雑音共分散行列とから予測誤差共分散行列を算出する。予測観測ベクトルの算出ステップＳ６は、予測ベクトルから予測観測ベクトルを算出する。観測雑音共分散行列の設定ステップＳ７は、目標観測装置２０から入力された観測ベクトルの精度評価に関連したパラメータに基づいて観測雑音共分散行列を設定する。残差共分散行列の算出ステップＳ８は、予測誤差共分散行列と観測雑音共分散行列とを用いて残差共分散行列を算出する。
【００１４】
ゲート判定ステップＳ９は、予測観測ベクトルと目標観測装置２０からの観測ベクトルとを用いて残差ベクトルを算出するとともに、残差共分散行列を用いて、観測ベクトルが追尾目標の探知データであるか否かのゲート判定を行う。ゲイン行列の算出ステップＳ１０は、予測誤差共分散行列と残差共分散行列とを用いて、フィルタゲイン行列を算出する。平滑ベクトルの算出ステップＳ１１は、ゲート判定で追尾目標の探知データと判定された観測ベクトル、予測ベクトル、予測観測ベクトル、及びゲイン行列を用いて、平滑ベクトルを算出する。平滑誤差共分散行列の算出ステップＳ１２は、予測誤差共分散行列と観測雑音共分散行列とを用いて、平滑誤差共分散行列を算出する。追尾終了判定ステップＳ１３は、追尾処理終了か否かを判定して、追尾処理を続行する場合には、観測ベクトルの入力ステップＳ１に戻る。
【００１５】
ここで、従来の技術について、カルマンフィルタの理論で仮定される目標及びセンサの数式モデルについて簡単に説明する。カルマンフィルタの理論では、目標運動諸元の推定を行うために、目標の運動モデルとセンサの観測モデルを以下のように設定する。まず、目標の運動モデルを以下の数式(２)のように表す。
【００１６】
【数２】

【００１７】
数式(２)において、(アンダーバー)ｘ(ｋ)はサンプリング時刻(以下、単に「時刻」という)ｔ(ｋ)における目標運動諸元の真値を表すｎ次元状態ベクトル、Ф(ｋ−１)は時刻ｔ(ｋ−１)からｔ(ｋ)への状態ベクトルの推移を表すｎ行ｎ列の推移行列である。また、(アンダーバー)ｗ(ｋ)は時刻ｔ(ｋ)におけるｎ次元駆動雑音ベクトルであり、平均(アンダーバー)０、共分散行列Ｑ(ｋ)のｎ変量正規分布に従う白色雑音系列である。
【００１８】
時刻ｔ(ｋ)における目標運動諸元の真値(アンダーバー)ｘ(ｋ)を定義する際、座標系が必要となる。図２０は北基準直交座標と極座標を示し、図２０でレーダを原点に配置し、ｘ軸を東(east)、ｙ軸を北(north)、水平面鉛直上方をｚ軸にとった直交座標を北基準直交座標、また、レーダから目標までの距離をＲ、水平面より目標までの仰角をＥ、水平面内で北方向より目標までの方位角をＡzとした座標系を極座標系と呼ぶ。従来の技術では、目標の基本的な運動を、距離方向について等速運動すると仮定し、極座標系における数式(２)の運動モデルにおける状態ベクトルと推移行列を、以下の数式(３)及び数式(４)に示すように設定する。
【００１９】
【数３】

【００２０】
数式(３)及び数式(４)において、駆動雑音ベクトル(アンダーバー)ｗ(ｋ)は、現実の目標運動を距離方向についての等速運動に近似したことにともなう誤差項を表す。また、数式(３)において、文字上の「ドット」は微分を表し、「Ｔ」は行列の転置を表す。
【００２１】
次に、センサの観測モデルを以下の数式(５)のように表す。
【００２２】
【数４】

【００２３】
数式(５)において、(アンダーバー)ｚ(ｋ)は時刻ｔ(ｋ)におけるｍ次元観測ベクトル、Ｈ(ｋ)はｍ行ｎ列の観測行列である。また、(アンダーバー)ｖ(ｋ)は観測ベクトル(アンダーバー)ｚ(ｋ)に対応した時刻ｔ(ｋ)のｍ次元観測雑音ベクトルで、平均(アンダーバー)０、共分散行列Ｒ(ｋ)のｍ変数量正規分布にしたがう白色雑音系列である。但し、観測雑音ベクトル(アンダーバー)ｖ(ｋ)と駆動雑音ベクトル(アンダーバー)ｗ(ｋ)は互いに独立であるとする。
【００２４】
従来の技術では、直線状周波数変調方式によるパルス圧縮を用いたレーダなどのセンサにおいて、極座標系における目標距離方向の観測結果が得られるとし、観測モデルにおける観測行列と観測ベクトルを以下の通りに設定する。
【００２５】
【数５】

【００２６】
ここで、ΔＴは直線状周波数変調方式によって決まる定数である。また、Ｈ(ｋ)は定数行列である。数式(６)および数式(７)のように、観測値(アンダーバー)ｚ(ｋ)は、距離Ｒ(ｋ)と、距離変化率(ドット)Ｒ(ｋ)を定数ΔＴで乗算した値の和に、観測雑音(アンダーバー)ｖ(ｋ)が加えられた値となる。この観測モデルを用いることにより、ドップラシフト分ΔＴ(ドット)Ｒ(ｋ)を考慮にいれたカルマンフィルタを構成することが可能となる。
【００２７】
初期時刻ｔ(０)より時刻ｔ(ｋ)までの観測ベクトル(アンダーバー)ｚ(ｋ)の全体を、以下の数式(８)のように表す。
【００２８】
【数６】

【００２９】
周知のように、カルマンフィルタは、上記運動モデル及び観測モデルに基づき、観測ベクトルの時系列データを用いて、推定誤差の分散が最小となる意味で最適な状態ベクトル推定値を算出する逐次アルゴリズムである。
【００３０】
以下、図１８を参考にしながら、図１７に示した従来の目標追尾装置による動作について説明する。なお、ここでは、カルマンフィルタの理論に従い、時刻ｔ(ｋ−１)までの観測結果に基づき時刻ｔ(ｋ)の状態ベクトル(アンダーバー)ｘ(ｋ)を推定した予測ベクトルを(上下バー)ｘ(ｋ)と表し、時刻ｔ(ｋ)までの観測結果に基づき時刻ｔ(ｋ)の状態ベクトル(アンダーバー)ｘ(ｋ)を推定した平滑ベクトルを(アンダーバー・ハット)ｘ(ｋ)と表す。また、予測ベクトルの誤差共分散行列である予測誤差共分散行列を(バー)Ｐ(ｋ)、平滑ベクトルの誤差共分散行列である平滑誤差共分散行列をＰ(ｋ)と表す。
【００３１】
さらに、予測ベクトル(上下バー)ｘ(ｋ)より得られる時刻ｔ(ｋ)の観測ベクトル(アンダーバー)ｚ(ｋ)を推定した予測観測ベクトルを(上下バー)ｚ(ｋ)、センサから得られた観測ベクトル(アンダーバー)ｚ(ｋ)と上記予測観測ベクトル(上下バー)ｚ(ｋ)との誤差を表す残差ベクトルを(アンダーバー)ｒ(ｋ)、残差ベクトルの共分散行列である残差共分散行列をＳ(ｋ)と表す。ここで、予測誤差共分散行列及び平滑誤差共分散行列は、以下の数式(９)及び数式(１０)で定義される。
【００３２】
【数７】

【００３３】
数式(９)及び数式(１０)において、Ｅ[・|Ｚ(ｋ)]は、時刻ｔ(ｋ)までの観測ベクトルを得たもとでの条件付き期待値を表す記号である。
【００３４】
まず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベクトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の設定ステップＳ１)。
【００３５】
以降、ｋ＝1,2,・・・の各時刻毎の処理を実行する。時刻ｔ(ｋ)の処理において、目標観測装置２０から観測ベクトル(アンダーバー)ｚ(ｋ)が入力される(観測ベクトルの入力ステップＳ２)と、予測ベクトル算出部１０３は、平滑ベクトル用メモリ１０５から前時刻ｔ(ｋ−１)の平滑ベクトル(アンダーバー・ハット)ｘ(ｋ−１)を読み出し、以下の数式(１１)にしたがって予測ベクトルを算出する(予測ベクトルの算出ステップＳ３)。
【００３６】
【数８】

【００３７】
次に、駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列Ｑ(ｋ−１)を設定する(駆動雑音共分散行列の設定ステップＳ４)。通常、この駆動雑音共分散行列の値は、目標追尾装置１０の運用上で想定される最大の目標機動能力を考慮して、サンプリング間隔ｔ(ｋ)−ｔ(ｋ−１)の大きさに応じて決定される。次に、予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３から、前時刻ｔ(ｋ−１)の平滑誤差共分散行列Ｐ(ｋ−１)を読み出し、上記駆動雑音共分散行列Ｑ(ｋ−１)を用いて、以下の数式(１２)に従い予測誤差共分散行列を算出する(予測誤差共分散行列の算出ステップＳ５)。
【００３８】
【数９】

【００３９】
さらに、観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)の観測雑音共分散行列Ｒ(ｋ)を設定し、残差共分散行列算出部１１１及び平滑誤差共分散行列算出部１２１に送出する(観測雑音共分散行列の設定ステップＳ７)。なお、観測雑音共分散行列設定部１１５は、観測ベクトル(アンダーバー)ｚ(ｋ)の精度評価に関連したパラメータを用いて観測雑音共分散行列Ｒ(ｋ)を設定する。例えば、目標観測装置２０が距離により目標を探知するレーダの場合、レーダの測距離精度の情報に基づいて観測雑音共分散行列Ｒ(ｋ)を設定することができる。ここで、上記測距離精度は、レーダが時刻ｔ(ｋ)に検出した目標信号の強度(または信号対雑音比)に基づいて推定することができる。予測観測ベクトル算出部１０１は、予測ベクトル算出部１０３から入力された予測ベクトル(上下バー)ｘ(ｋ)を用いて、以下の数式(１３)により予測観測ベクトル(上下バー)ｚ(ｋ)を算出する(予測観測ベクトルの算出ステップＳ６)。
【００４０】
【数１０】

【００４１】
次に残差共分散行列算出部１１１は、観測雑音行列Ｒ(ｋ)と、予測誤差共分散行列(バー)Ｐ(ｋ)とを読み込み、以下の数式(１４)にしたがって残差共分散行列Ｓ(ｋ)を算出し、ゲート判定部１０７およびゲイン行列算出部１１３に伝送する(残差共分散行列の算出ステップＳ８)。
【００４２】
【数１１】

【００４３】
ゲート判定部１０７では、以下の数式(１５)により観測ベクトル(アンダーバー)ｚ(ｋ)に対する残差ベクトルを算出した後、残差共分散行列Ｓ(ｋ)を読み込み、以下の数式(１６)によりゲート判定処理を実行する(ゲート判定ステップＳ９)。
【００４４】
【数１２】

【００４５】
ただし、数式(１６)において、ｄはパラメータである。なお、ゲート判定部１０７により用いられるゲートは、予測観測ベクトル(上下バー)ｚ(ｋ)を中心として空間範囲であって、追尾目標からの観測ベクトルを所望の確率で捕捉可能な空間範囲である。上記のゲートの大きさや形状は、予測ベクトルの誤差の評価値である予測誤差共分散行列(バー)Ｐ(ｋ)と観測誤差の評価値である観測雑音共分散行列Ｒ(ｋ)から計算される上記数式(１４)の残差共分散行列Ｓ(ｋ)によって決定される。上記数式(１６)は、上記ゲート内に目標観測装置２０からの観測ベクトルが存在するか否かを判定する基準条件である。たとえば、ゲート判定部１０７は、残差ベクトル(アンダーバー)ｒ(ｋ)が数式(１６)を満たす場合、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)が追尾目標の探知結果であることを判定し、数式(１６)を満たす観測ベクトル(アンダーバー)ｚ(ｋ)を平滑ベクトル算出部１０９に送出する。一方、目標観測装置２０から数式(１６)を満たす観測ベクトルが得られなかった場合、ゲート判定部１０７は、追尾目標の観測ベクトルは探知されなかったものと見なし、平滑ベクトル算出部１０９への観測ベクトルの送出を実行しない。
【００４６】
次に、ゲイン行列算出部１１３は、予測誤差共分散行列(バー)Ｐ(ｋ)と残差共分散行列Ｓ(ｋ)とを読み込み、以下の数式(１７)にしたがって、フィルタゲインを算出する(ゲイン行列の算出ステップＳ１０)。
【００４７】
【数１３】

【００４８】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力された場合に、予測観測ベクトル(上下バー)ｚ(ｋ)、予測ベクトル(上下バー)ｘ(ｋ)、およびゲイン行列Ｋ(ｋ)とを読み込み、以下の数式(１８)にしたがって平滑ベクトルを算出する。
【００４９】
【数１４】

【００５０】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力されなかった場合は、以下の数式(１９)に示すように、予測ベクトル(上下バー)ｘ(ｋ)を平滑ベクトルとする(平滑ベクトルの算出ステップＳ１１)。
【００５１】
【数１５】

【００５２】
上記数式(１８)または数式(１９)により、平滑ベクトル算出部１０９で算出された平滑ベクトル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモリ１０５に格納されるとともに、航跡表示装置３０に伝送され、運用者に対して表示される。
【００５３】
一方、平滑誤差共分散行列算出部１２１は、ゲート判定部１０７で追尾目標の観測ベクトルが得られたと判定された場合には、予測誤差共分散行列(バー)Ｐ(ｋ)と観測雑音共分散行列Ｒ(ｋ)とを用いて、以下の数式(２０)により平滑誤差共分散行列を算出し、ゲート判定部１０７で追尾目標の観測ベクトルが得られなかったと判定された場合には、以下の数式(２１)に示すように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行列の算出ステップＳ１２)。
【００５４】
【数１６】

【００５５】
数式(２０)及び数式(２１)に従う平滑誤差共分散行列の算出結果は、平滑誤差共分散行列用メモリ１２３に格納される。以上で時刻ｔ(ｋ)における処理が終了する。以降、追尾終了が判定されるまで、上記一連の処理ステップを繰り返し実行する(追尾終了ステップ１３)。
【００５６】
このように、カルマンフィルタを用い、極座標系でドップラシフト項ΔＴ(ドット)Ｒ(ｋ)を観測モデルに記述した従来の目標追尾装置１０は、事前に仮定された等距離変化率運動モデルを用い、追尾航跡を算出する。図１９に、従来の技術を用いた平滑ベクトルの距離成分(平滑値)、予測観測ベクトル(ゲート中心)、および観測ベクトル(観測値)の時間推移を示す。図の実線は、状態ベクトルの距離成分の真の軌跡を表し、点線は、ドップラシフトの影響を受けた観測ベクトルの真値を示す。図のように、予測観測ベクトルは、ドップラシフトを考慮した観測行列を用いることにより、予測ベクトルをドップラシフト分だけシフトした値となり、ドップラシフトの影響を受けた観測値の近くに算出される。平滑ベクトルの距離成分は状態ベクトルの真の軌跡付近に算出される。
【００５７】
以上のように、従来の技術では、目標が距離方向のみに運動すると仮定し追尾を行う場合には有効である。しかしながら、目標の運動モデルが距離方向の１次元空間のみでなく、３次元空間に及ぶ場合、距離方向のみで仮定された運動モデル及び観測モデルでは、フィルタを構成することが困難である。特に、目標の運動が北基準直交座標系で定義される場合、従来の運動および観測モデルを用いることはできない。
【００５８】
さらに、上記従来の技術のドップラシフトを考慮したフィルタを追尾開始時から用いると、距離変化率の推定値が揺らぐため、距離変化率推定値の劣化が生じる。フィルタは、本質的に、距離変化率の推定値を用いて、観測値に含まれているドップラシフト分を補正し、距離推定値を算出する。よって、距離変化率推定精度の劣化が距離推定精度の劣化につながる。上記の理由から、距離推定精度の劣化を防ぐため、追尾初期は、ドップラシフトを考慮しないフィルタを用い、距離変化率推定精度が安定後、フィルタの切り替えを行うことが考えられる。図２１に、距離平滑値、予測観測値、および観測値の、切り替え前と切り替え後の推移に関する概念図を示す。ドップラシフトを考慮しないフィルタとは、観測行列を、数式(７)の代わりに、以下の数式(２２)を用いて構成されたカルマンフィルタである。
【００５９】
【数１７】

【００６０】
この場合、ドップラシフト分の補正を行わないため、ドップラシフト分の補正による、フィルタ開始時の距離変化率精度劣化の距離精度に対する影響がない。ここで、問題となるのは、矢印Ａで示すように切り替え時のゲート中心が観測値と離れた所に算出され、観測値がゲートに収まらない可能性があることである。この図に示すように、まず、ドップラシフトの影響を受けた観測値を入力としてドップラシフトを考慮しないフィルタを用いると、フィルタの平滑位置が目標の真の位置からずれて算出されてしまう。次に、フィルタの切り替え直後にその平滑値を用いて予測観測値を算出すると、ドップラシフト分だけずれた位置に予測観測値(ゲート中心)が算出されてしまい、結果的に予測観測値と観測値の距離が長くなってしまう。距離がながくなると、ゲート判定の際に、観測値をゲートに捕捉できない危険性が生じる。
【００６１】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように、目標の運動モデルが極座標系の距離方向で記述され、観測モデルが極座標系の距離方向で構成される場合、追尾装置を構成することが可能であるが、目標の運動が直交座標系で定義され、観測モデルもそうである場合、従来の運動及び観測モデルでは正確な目標追尾が困難であった。
【００６２】
さらに、ドップラシフトを考慮しないフィルタからドップラシフトを考慮したフィルタへの切り替え時に、予測観測値(ゲート中心)と観測値の距離がながくなり、観測値がゲート内に捕捉できない可能性があった。
【００６３】
本発明は、上記の問題点を解決するためになされたもので、運動モデル及び観測モデルを北基準直交座標系で構成することにより、直交座標系での目標追尾装置の構成を可能とし、さらに、ドップラシフトを考慮しない観測モデルを用いたフィルタからドップラシフト補正を考慮した観測モデルを用いたフィルタの切り替え時に、観測値とゲート中心の距離が増大する問題を解消した目標追尾装置および目標追尾方法を提供することを目的とする。
【００６４】
【課題を解決するための手段】
上記の目的に鑑み、この発明は、直線状周波数変調方式のパルス圧縮を用いた目標観測装置から入力される直交座標系における目標位置の座標成分を含む観測ベクトルに基づき、カルマンフィルタを使用して、直交座標系における追尾目標の位置、速度の座標成分を含む状態ベクトルを推定する目標追尾装置であって、前記状態ベクトルの予測値である予測ベクトルを極座標系に変換して得られる距離、仰角、方位角、距離変化率、仰角変化率、方位角変化率の値と、ドップラシフトの影響による距離観測誤差と目標の距離変化率との比を表すドップラシフト時定数の値とを用いて、ドップラシフトによる距離観測誤差を考慮した観測行列を算出する観測行列算出手段を備えたことを特徴とする目標追尾装置及びこれに係わる目標追尾方法等にある。
【００７４】
【発明の実施の形態】
以下この発明を各実施の形態に従って説明する。
実施の形態１．
図１はこの発明の実施の形態１による目標追尾装置を示すブロック構成図であり、図２はその処理動作を示すフローチャートである。図１において、目標追尾装置１０ａは、従来の技術を示す図２のブロック構成図と同様に、予測観測ベクトル算出部１０１、予測ベクトル算出部１０３、平滑ベクトル用メモリ１０５、ゲート判定部１０７、平滑ベクトル算出部１０９、残差共分散行列算出部１１１、ゲイン行列算出部１１３、観測雑音共分散行列設定部１１５、予測誤差共分散行列算出部１１７、駆動雑音共分散行列設定部１１９、平滑誤差共分散行列算出部１２１、平滑誤差共分散行列用メモリ１２３、を備えている。また、目標追尾装置１０ａは、上記構成要素に加えて、観測行列算出部１２５を備えている。観測行列算出部１２５は、予測ベクトル算出部１０３より算出された予測値を用いて、観測行列を算出する。
【００７５】
図２において、観測行列算出ステップＳ５ａは、予測ベクトルを用いて、観測行列を算出するステップである。但し、図５の各処理部、メモリ、及び図２の処理ステップについて、従来の技術で説明した距離と距離変化率を用いた二次元の状態ベクトルを用いず、北基準直交座標系で定義した６次元の状態ベクトルを用いるため、従来の技術とベクトル及び行列の次数が異なる。
【００７６】
実施の形態１において、カルマンフィルタの理論で仮定される目標及びセンサの数式モデルについて説明する。カルマンフィルタの理論では、目標運動諸元の推定を行うために、目標の運動モデルとセンサの観測モデルを以下のように設定する。まず、目標の運動モデルを以下の数式(２３)のように表す。
【００７７】
【数１８】

【００７８】
数式(２３)において、(アンダーバー)ｘ(ｋ)はサンプリング時刻(以下、単に「時刻」という)ｔ(ｋ)における目標運動諸元の真値を表すｎ次元状態ベクトル、Ф(ｋ−１)はｔ(ｋ−１)からｔ(ｋ)への状態ベクトルの推移を表すｎ行ｎ列の推移行列である。また、(アンダーバー)ｗ(ｋ)は時刻ｔ(ｋ)におけるｎ次元駆動雑音ベクトルであり、平均(アンダーバー)０、共分散行列Ｑ(ｋ)のｎ変量正規分布に従う白色雑音系列である。時刻ｔ(ｋ)における目標運動諸元の真値(アンダーバー)ｘ(ｋ)を、図２０における北基準直交座標において定義する。目標の基本的な運動を、等速運動すると仮定し、北基準直交座標における数式(２３)の運動モデルにおける状態ベクトルと推移行列を、以下の数式(２４)及び数式(２５)に示すように設定する。
【００７９】
【数１９】

【００８０】
数式(２３)、数式(２４)及び数式(２５)において、駆動雑音ベクトル(アンダーバー)ｗ(ｋ)は、現実の目標運動を等速直進に近似したことにともなう誤差項を表す。次に、センサの観測モデルを以下の数式(２６)のように表す。
【００８１】
【数２０】

【００８２】
数式(２６)において、(アンダーバー)ｚ(ｋ)は時刻ｔ(ｋ)におけるｍ次元観測ベクトル、Ｈ(ｋ)はｍ行ｎ列の観測行列である。また、(アンダーバー)ｖ(ｋ)は観測ベクトル(アンダーバー)ｚ(ｋ)に対応した時刻ｔ(ｋ)のｍ次元観測ベクトルで、平均(アンダーバー)０、共分散行列Ｒ(ｋ)のｍ変数量正規分布にしたがう白色雑音系列である。但し、観測雑音ベクトル(アンダーバー)ｖ(ｋ)と駆動雑音ベクトル(アンダーバー)ｗ(ｋ)は互いに独立であるとする。
【００８３】
センサにより、北基準直交標系における目標の位置の観測結果が得られる場合、数式(２６)の観測モデルにおける観測行列を以下の通りに設定する。
【００８４】
【数２１】

【００８５】
【数２２】

【００８６】
上記、数式(２７)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップラシフトの影響を反映した観測値を記述するための行列であり、観測値が距離方向にドップラシフトの影響分であるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表している。数式(２７)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で与えられた状態ベクトル(アンダーバー)ｘ(ｋ)を極座標系に変換した変数Ｒ(ｋ)、Ｅ(ｋ)、Ａｚ(ｋ)、(ドット)Ｒ(ｋ)、(ドット)Ｅ(ｋ)、(ドット)Ａｚ(ｋ)を用いて算出される。数式(３１)及び数式(３２)は、北基準直交座標系から極座標への変換の式を表している。数式(２８)におけるΔＴは、予め与えられる時定数である。但し、実際には、真の目標の運動諸元である状態ベクトル(アンダーバー)ｘ(ｋ)は未知であるため、その予測値である(上下バー)ｘ(ｋ)を用いて観測行列を算出する。
【００８７】
以下、図２を参照しながら、図１に示したこの発明の実施の形態１による処理動作について詳細に説明する。まず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベクトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の設定ステップＳ１)。ここで、状態ベクトル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義される、数式(３３)の６次元ベクトルである。
【００８８】
【数２３】

【００８９】
また、平滑誤差共分散行列は６行６列の行列である。以降、ｋ＝1,2,・・・の各時刻毎の処理を実行する。時刻ｔ(ｋ)の処理において、目標観測装置２０から観測ベクトル(アンダーバー)ｚ(ｋ)が入力される(観測ベクトルの入力ステップＳ２)と、予測ベクトル算出部１０３は、平滑ベクトル用メモリ１０５から前時刻ｔ(ｋ−１)の平滑ベクトル(アンダーバー・ハット)ｘ(ｋ−１)を読み出し、以下の数式(３４)にしたがって予測ベクトルを算出する(予測ベクトルの算出ステップＳ３)。
【００９０】
【数２４】

【００９１】
ここで、観測ベクトル(アンダーバー)ｚ(ｋ)は、ドップラシフトの影響を受けた、北基準直交座標系で得られる3次元ベクトルである。状態推移行列は、数式(３５)の行列で与えられる。
【００９２】
【数２５】

【００９３】
次に、駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列Ｑ(ｋ−１)を設定する(駆動雑音共分散行列の設定ステップＳ４)。通常、この駆動雑音共分散行列の値は、目標追尾装置１０ａの運用上で想定される最大の目標機動能力を考慮して、サンプリング間隔ｔ(ｋ)−ｔ(ｋ−１)の大きさに応じて決定される。次に、予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３から、前時刻ｔ(ｋ−１)の平滑誤差共分散行列Ｐ(ｋ−１)を読み出し、上記駆動雑音共分散行列Ｑ(ｋ−１)を用いて、以下の数式(３６)に従い予測誤差共分散行列を算出する(予測誤差共分散行列の算出ステップＳ５)。
【００９４】
【数２６】

【００９５】
次に、観測行列算出部１２５は、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ(ｋ)から、観測行列を算出する(観測行列の算出ステップＳ５ａ)。
【００９６】
【数２７】

【００９７】
【数２８】

【００９８】
上記、数式(３７)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップラシフトの影響を反映した観測値を記述するための行列であり、観測値が距離方向にドップラシフトの影響分であるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表している。数式(３７)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で与えられた予測ベクトル(上下バー)ｘ(ｋ)を極座標系に変換した変数(バー)Ｒ(ｋ)、(バー)Ｅ(ｋ)、(バー)Ａｚ(ｋ)、(バー・ドット)Ｒ(ｋ)、(バー・ドット)Ｅ(ｋ)、(バー・ドット)Ａｚ(ｋ)を用いて算出される。数式(４１)及び数式(４２)は、北基準直交座標系から極座標への変換の式を表している。数式(３８)におけるΔＴは、予め与えられる時定数である。
【００９９】
予測観測ベクトル算出部１０１は、予測ベクトル算出部１０３から入力された予測ベクトル(上下バー)ｘ(ｋ)と観測行列算出部１２５から入力された観測行列を用いて、以下の数式(４３)により予測観測ベクトル(上下バー)ｚ(ｋ)を算出する(予測観測ベクトルの算出ステップＳ６)。
【０１００】
【数２９】

【０１０１】
さらに、観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)の観測雑音共分散行列Ｒ(ｋ)を設定し、残差共分散行列算出部１１１及び平滑誤差共分散行列算出部１２１に送出する(観測雑音共分散行列の設定ステップＳ７)。なお、観測雑音共分散行列設定部１１５は、観測ベクトル(アンダーバー)ｚ(ｋ)の精度評価に関連したパラメータを用いて観測雑音共分散行列Ｒ(ｋ)を設定する。例えば、目標観測装置２０が北基準直交座標系におけるｘ，ｙ，ｚ軸方向について目標を探知するレーダの場合、レーダの角軸の観測精度の情報に基づいて観測雑音共分散行列Ｒ(ｋ)を設定することができる。ここで、上記観測精度は、レーダが時刻ｔ(ｋ)に検出した目標信号の強度(または信号対雑音比)に基づいて推定することができる。
【０１０２】
次に残差共分散行列算出部１１１は、観測雑音共分散行列Ｒ(ｋ)、予測誤差共分散行列(バー)Ｐ(ｋ)、観測行列Ｈ(ｋ)とを読み込み、以下の数式(４４)にしたがって残差共分散行列Ｓ(ｋ)を算出し、ゲート判定部１０７およびゲイン行列算出部１１３に伝送する(残差共分散行列の算出ステップＳ８)。
【０１０３】
【数３０】

【０１０４】
ゲート判定部１０７は、予測観測ベクトル算出部１０１から入力された予測観測ベクトル(上下バー)ｚ(ｋ)を用いて、以下の数式(４５)により観測ベクトル(アンダーバー)ｚ(ｋ)に対する残差ベクトルを算出した後、残差共分散行列Ｓ(ｋ)を読み込み、以下の数式(４６)によりゲート判定処理を実行する(ゲート判定ステップＳ９)。
【０１０５】
【数３１】

【０１０６】
ただし、数式(４６)において、ｄはパラメータである。なお、ゲート判定部１０７により用いられるゲートは、予測観測ベクトル(上下バー)ｚ(ｋ)を中心として空間範囲であって、追尾目標からの観測ベクトルを所望の確率で捕捉可能な空間範囲である。上記のゲートの大きさや形状は、予測ベクトル誤差の評価値である予測誤差共分散行列(バー)Ｐ(ｋ)、観測誤差の評価値である観測雑音共分散行列Ｒ(ｋ)、及び観測行列算出部１２５で算出される観測行列Ｈ(ｋ)から計算される上記数式(４４)の残差共分散行列Ｓ(ｋ)によって決定される。上記数式(４６)は、上記ゲート内に目標観測装置２０からの観測ベクトルが存在するか否かを判定する基準条件である。たとえば、ゲート判定部１０７は、残差ベクトル(アンダーバー)ｒ(ｋ)が数式(４６)を満たす場合、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)が追尾目標の探知結果であることを判定し、追尾目標からのであると判定された観測べクトル(アンダーバー)ｚ(ｋ)を平滑ベクトル算出部１０９に送出する。一方、目標観測装置２０から数式(４６)を満たす観測ベクトルが得られなかった場合、ゲート判定部１０７は、追尾目標の観測ベクトルは探知されなかったものと見なし、平滑ベクトル算出部１０９への観測ベクトルの送出を実行しない。
【０１０７】
次に、ゲイン行列算出部１１３は、予測誤差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、および観測行列Ｈ(ｋ)を読み込み、以下の数式(４７)にしたがって、フィルタゲインを算出する(ゲイン行列の算出ステップＳ１０)。
【０１０８】
【数３２】

【０１０９】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力された場合に、予測ベクトル(上下バー)ｘ(ｋ)、予測観測ベクトル(上下バー)ｚ(ｋ)、及びゲイン行列Ｋ(ｋ)とを読み込み、以下の数式(４８)にしたがって平滑ベクトルを算出する。
【０１１０】
【数３３】

【０１１１】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力されなかった場合は、以下の数式(４９)に示すように、予測ベクトル(上下バー)ｘ(ｋ)を平滑ベクトルとする(平滑ベクトルの算出ステップＳ１１)。
【０１１２】
【数３４】

【０１１３】
上記数式(４８)または数式(４９)により、平滑ベクトル算出部１０９で算出された平滑ベクトル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモリ１０５に格納されるとともに、航跡表示装置３０に伝送され、運用者に対して表示される。
【０１１４】
一方、平滑誤差共分散行列算出部１２１は、ゲート判定部１０７で追尾目標の観測ベクトルが得られたと判定された場合には、予測誤差共分散行列(バー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列Ｈ(ｋ)を用いて、以下の数式(５０)により平滑誤差共分散行列を算出し、ゲート判定部１０７で追尾目標の観測ベクトルが得られなかったと判定された場合には、以下の数式(５１)に示すように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行列の算出ステップＳ１２)。
【０１１５】
【数３５】

【０１１６】
数式(５０)及び数式(５１)に従う平滑誤差共分散行列の算出結果は、平滑誤差共分散行列用メモリ１２３に格納される。
【０１１７】
以上で時刻ｔ(ｋ)における処理が終了する。以降、追尾終了が判定されるまで、上記一連の処理ステップを繰り返し実行する(追尾終了ステップＳ１３)。
【０１１８】
このように、カルマンフィルタを用い、北基準直交座標系でドップラシフト項を観測モデルに記述した実施の形態１の目標追尾装置１０ａは、事前に仮定された等距離直線運動モデルを用い、追尾航跡を算出する。図３に実施の形態１を用いた平滑ベクトルの位置成分(平滑値)、予測観測ベクトル(ゲート中心)、および観測ベクトル(観測値)の時間推移を示す。図の実線は、状態ベクトルの位置成分の真の軌跡を表し、点線は、ドップラシフトの影響を受けた観測ベクトルの真値を示す。図のように、予測観測ベクトルは、ドップラシフトを考慮した観測行列を用いることにより、予測ベクトルをドップラシフト分だけシフトした値となり、ドップラシフトの影響を受けた観測値の近くに算出される。平滑ベクトルの距離成分は状態ベクトル真の軌跡付近に算出される。
【０１１９】
このように、ドップラシフト補正を考慮したフィルタを北基準直交座標系で構成するために、従来の技術で用いた極座標系の距離軸で表現した観測モデルではなく、北基準直交座標系における観測モデルを構成することにより、ドップラシフト補正を北基準直交座標系で行うことが可能となる。
【０１２０】
実施の形態２．
実施の形態１で示したように、カルマンフィルタを用い、北基準直交座標系でドップラシフト項を観測モデルに記述した目標追尾装置は、事前に仮定された等速直線運動モデルを用い、追尾航跡を算出する。しかし、上記実施の形態１のドップラシフトを考慮したフィルタを追尾開始時から用いると、速度(ドット)ｘ，(ドット)ｙ，(ドット)ｚの推定値が揺らぐため、これらの推定値を極座標表示することによって得られる距離変化率成分の劣化が生じる。フィルタは、本質的に、距離変化率の推定値を用いて、観測値に含まれているドップラシフト分を補正し、距離推定値を算出する。よって、距離変化率推定精度の劣化が距離推定精度の劣化につながる。上記の理由から、距離推定精度の劣化を防ぐため、追尾初期は、ドップラシフトを考慮しないフィルタを用い、速度(ドット)ｘ，(ドット)ｙ，(ドット)ｚ推定値より得られる距離変化率推定精度が安定後、フィルタの切り替えを行うことが考えられる。図２１に、位置平滑値、予測観測値、および観測値の、切り替え前と切り替え後の推移に関する概念図が示されている。ドップラシフトを考慮しないフィルタとは、観測行列を、数式(３７)の代わりに、以下の数式(５２)を用いて構成されたカルマンフィルタである。
【０１２１】
【数３６】

【０１２２】
この場合、ドップラシフト分の補正を行わないため、ドップラシフト分の補正による、フィルタ開始時の距離変化率精度劣化の距離精度に対する影響がない。ここで、問題となるのは、切り替え時のゲート中心が観測値と離れた所に算出され、観測値がゲートに収まらない可能性があることである。図２１に示すように、まず、ドップラシフトの影響を受けた観測値を入力としてドップラシフトを考慮しないフィルタを用いると、フィルタの平滑位置が目標の真の位置からずれて算出されてしまう。次に、フィルタの切り替え直後にその平滑値を用いて予測観測値を算出すると、ドップラシフト分だけずれた位置に予測観測値が算出されてしまい、結果的に予測観測値と観測値の距離が長くなってしまう。距離がながくなると、ゲート判定の際に、観測値をゲートに捕捉できない危険性が生じる。この問題を解決するため、以後、ドップラシフトを考慮しない観測モデルを用いたフィルタからドップラシフト補正を考慮した観測モデルを用いたフィルタの切り替え時に、観測値とゲート中心の距離が増大する問題を解決することを目的とする。
【０１２３】
図４はこの発明の実施の形態２による目標追尾装置を示すブロック構成図であり、また図５はその処理動作を示すフローチャートである。上記図１、図２と同様のものについては、詳述を省略する。また、図６はこの発明の実施の形態２における効果を示す概念図である。
【０１２４】
図４において目標追尾装置１０ａは、実施の形態１を示す図１と同様に、平滑ベクトル用メモリ１０５、予測ベクトル算出部１０３、ゲート判定部１０７、平滑ベクトル算出部１０９、残差共分散行列算出部１１１、ゲイン行列算出部１１３、観測雑音共分散行列設定部１１５、予測誤差共分散行列算出部１１７、駆動雑音共分散行列設定部１１９、平滑誤差共分散行列算出部１２１、平滑誤差共分散行列用メモリ１２３を備えている。
【０１２５】
また、目標追尾装置１０ａは、上記構成要素に加えて、切り替え制御部１２７、観測行列算出部１２５ａ(実施の形態１の図１におけるそれと動作が異なる)、平滑ベクトル算出用予測観測ベクトル算出部１３１、ゲート用予測観測ベクトル算出部１２９、切り替え直前の擬似平滑値算出部１３３を備えている。切り替え制御部１２７は、ドップラシフトを考慮しない観測モデルにより構成されたフィルタからドップラシフトを考慮した観測モデルにより構成されたフィルタへの切り替えタイミングを、観測行列算出部１２５ａおよび切り替え直前の擬似平滑値算出部１３３に出力する。
【０１２６】
観測行列算出部１２５ａは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻以後は、予測ベクトル算出部１０３より算出された予測値を用いて、ドップラシフトを考慮した観測行列を算出する。切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出する。
平滑ベクトル算出用予測観測ベクトル算出部１３１は、平滑ベクトル用メモリ１０５からの1時刻前における平滑ベクトル及び観測行列算出部１２５ａからの入力である観測行列より、平滑ベクトル算出部１０９で用いる予測観測ベクトルを算出する。
【０１２７】
ゲート用予測観測ベクトル算出部１２９は、予測ベクトル算出部１０３からの入力である予測ベクトル及び観測行列算出部１２５ａからの入力である観測行列を用いて、ゲート判定部１０７で用いる予測観測ベクトルを算出する。
【０１２８】
切り替え直前の擬似平滑値算出部１３３は、切り替え制御部１２７よりフィルタの切り替え時刻情報を得、現時刻が切り替えを行う時刻の場合、平滑ベクトル用メモリ１０５からの１時刻前における平滑ベクトルを用いて、擬似平滑値を算出する。切り替えを行う時刻でない場合、1時刻前の平滑値をそのまま出力する。
【０１２９】
図５において、実施の形態１の処理動作を示すフローチャートである図２と同様に、初期値の設定ステップＳ１、観測ベクトルの入力ステップＳ２、予測ベクトルの算出ステップＳ３、駆動雑音共分散行列の設定ステップＳ４、予測誤差共分散行列の算出ステップＳ５、観測雑音共分散行列の設定ステップＳ７、残差共分散行列の算出ステップＳ８、ゲート判定ステップＳ９、ゲイン行列の算出ステップＳ１０、平滑ベクトルの算出ステップＳ１１、平滑誤差共分散行列の算出ステップＳ１２を備えている。
【０１３０】
また、図５は上記処理ステップに加えて、切り替え直前の擬似平滑値の算出ステップＳ２ａ、ゲート用予測観測ベクトルの算出ステップ５ｂ、平滑ベクトル算出用予測観測ベクトルの算出ステップ９ａ、観測行列算出ステップ５ａａ(実施の形態１の処理動作を示す図２のそれと処理が異なる)、が挿入されている。
【０１３１】
図５において、切り替え直前の擬似平滑値の算出ステップＳ２ａは、切り替え制御部１２７よりフィルタの切り替え時刻情報を得、現時刻が切り替えを行う時刻の場合、平滑ベクトル用メモリ１０５からの１時刻前における平滑ベクトルを用いて、擬似平滑値を算出するステップである。現時刻が切り替えを行う時刻でない場合、平滑ベクトル用メモリ１０５からの1時刻前における平滑ベクトルをそのまま出力とする。
【０１３２】
ゲート用予測観測ベクトルの算出ステップＳ５ｂは、予測ベクトル算出部１０３で算出された予測ベクトルと、観測行列算出部１２５ａで算出された観測行列を用いて、ゲート判定に用いる予測観測ベクトルを算出するステップである。
【０１３３】
平滑ベクトル算出用予測観測ベクトルの算出ステップＳ９ａは、平滑ベクトル用メモリ１０５からの１時刻前における平滑ベクトルと、観測行列算出部１２５ａで算出された観測行列を用いて、平滑ベクトルの算出に用いる予測観測ベクトルを算出するステップである。
【０１３４】
観測行列算出ステップＳ５ａａは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻以後は、予測ベクトル算出部１０３より算出された予測値を用いて、観測行列を算出し、切り替わる以前は、ドップラシフトを考慮しない観測モデルを出力するステップである。
【０１３５】
以下、図４、図５を参照しながら、実施の形態２による処理動作について詳細に説明する。まず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベクトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の設定ステップＳ１)。ここで、状態ベクトル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義される、数式(５３)の６次元ベクトルである。
【０１３６】
【数３７】

【０１３７】
また、平滑誤差共分散行列は６行６列の行列である。以降、ｋ＝1,2,・・・の各時刻毎の処理を実行する。時刻ｔ(ｋ)の処理において、目標観測装置２０から観測ベクトル(アンダーバー)ｚ(ｋ)が入力される(観測ベクトルの入力ステップＳ２)。ここで、観測ベクトル(アンダーバー)ｚ(ｋ)は、ドップラシフトの影響を受けた、北基準直交座標系で得られる３次元ベクトルである。次に、切り替え直前の擬似平滑値の算出ステップ２ａでは、まず、平滑ベクトル用メモリ１０５から前時刻ｔ(ｋ−１)の平滑ベクトル(アンダーバー・ハット)ｘ(ｋ−１)を読み出す。次に、切り替え制御部１２７より、フィルタの切り替え時刻情報を得、切り替え時刻より前及び切り替え時刻より後の場合、数式(５４)のように、１時刻前の擬似平滑値(アンダーバー・ハット)ｘ’(ｋ−１)に、１時刻前の平滑値を設定する。
【０１３８】
【数３８】

【０１３９】
現時刻が切り替え時刻の場合、以下の数式(５５)、数式(５６)、数式(５７)、数式(５８)、数式(５９)に従い擬似平滑ベクトル(アンダーバー・ハット)ｘ’(ｋ−１)を算出する。
【０１４０】
【数３９】

【０１４１】
【数４０】

【０１４２】
ここで、数式(５５)は、距離変化率についての北基準直交座標系から極座標系への変換式であり、数式(５６)は、位置についての北基準直交座標系から、極座標系への変換式であり、数式(５８)は、極座標系から北基準直交座標系への変換式である。数式(５７)は、ドップラシフトの影響を受けた平滑値の距離成分に対し、ドップラシフト分を補正するための処理である。数式(５７)は、ドップラシフトの影響を受けた平滑値の距離成分に対し、ドップラシフトを考慮しない観測モデルで構成されたフィルタを用いることによる距離方向への平滑誤差を補正することを意味する。
【０１４３】
次に、以下の数式(６０)にしたがって予測ベクトルを算出する(予測ベクトルの算出ステップＳ３)。
【０１４４】
【数４１】

【０１４５】
状態推移行列は、数式(６１)の行列で与えられる。
【０１４６】
【数４２】

【０１４７】
次に、駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列Ｑ(ｋ−１)を設定する(駆動雑音共分散行列の設定ステップＳ４)。通常、この駆動雑音共分散行列の値は、目標追尾装置１０ａの運用上で想定される最大の目標機動能力を考慮して、サンプリング間隔ｔ(ｋ)−ｔ(ｋ−１)の大きさに応じて決定される。次に、予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３から、前時刻ｔ(ｋ−１)の平滑誤差共分散行列Ｐ(ｋ−１)を読み出し、上記駆動雑音共分散行列Ｑ(ｋ−１)を用いて、以下の数式(６２)に従い予測誤差共分散行列を算出する(予測誤差共分散行列の算出ステップＳ５)。
【０１４８】
【数４３】

【０１４９】
次に、観測行列の算出ステップＳ５ａａにおいて、切り替え制御部１２７からフィルタの切り替え時刻の情報を得、切り替え時刻より前については、観測行列を以下の数式(６４)として出力する。
【０１５０】
【数４４】

【０１５１】
ここで、Ｉ_3x3は３行３列の単位行列、０_3x3は３行３列の零行列である。切り替え時刻以後において、観測行列算出部１２５ａは、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ_h(ｋ)から、観測行列を算出する(観測行列の算出ステップＳ５ａａ)。
【０１５２】
【数４５】

【０１５３】
【数４６】

【０１５４】
上記、数式(６５)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップラシフトの影響を反映した観測値を記述するための行列であり、観測値が距離方向にドップラシフトの影響分であるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表している。数式(６５)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で与えられた予測ベクトル(上下バー)ｘ_h(ｋ)を極座標系に変換した変数(バー)Ｒ(ｋ)、(バー)Ｅ(ｋ)、(バー)Ａｚ(ｋ)、(バー・ドット)Ｒ(ｋ)、(バー・ドット)Ｅ(ｋ)、(バー・ドット)Ａｚ(ｋ)を用いて算出される。数式(６９)及び数式(７０)は、北基準直交座標系から極座標への変換の式を表している。数式(６６)におけるΔＴは、予め与えられる時定数である。
【０１５５】
ゲート用予測観測ベクトルの算出ステップ５ｂでは、ゲート用予測観測ベクトル算出部１２９は、予測ベクトル算出部１０３から入力された予測ベクトル(上下バー)ｘ_h(ｋ)と観測行列算出部１２５ａから入力された観測行列を用いて、以下の数式(７１)により予測観測ベクトル(上下バー)ｚ_g(ｋ)を算出する(予測観測ベクトルの算出ステップＳ５ｂ)。
【０１５６】
【数４７】

【０１５７】
さらに、観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)の観測雑音共分散行列Ｒ(ｋ)を設定し、残差共分散行列算出部１１１及び平滑誤差共分散行列算出部１２１に送出する(観測雑音共分散行列の設定ステップＳ７)。なお、観測雑音共分散行列設定部１１５は、観測ベクトル(アンダーバー)ｚ(ｋ)の精度評価に関連したパラメータを用いて観測雑音共分散行列Ｒ(ｋ)を設定する。例えば、目標観測装置２０が北基準直交座標系におけるｘ，ｙ，ｚ軸方向について目標を探知するレーダの場合、レーダの角軸の観測精度の情報に基づいて観測雑音共分散行列Ｒ(ｋ)を設定することができる。ここで、上記観測精度は、レーダが時刻ｔ(ｋ)に検出した目標信号の強度(または信号対雑音比)に基づいて推定することができる。
【０１５８】
次に残差共分散行列算出部１１１は、観測雑音共分散行列Ｒ(ｋ)、予測誤差共分散行列(バー)Ｐ(ｋ)、観測行列Ｈ(ｋ)とを読み込み、以下の数式(７２)にしたがって残差共分散行列Ｓ(ｋ)を算出し、ゲート判定部１０７およびゲイン行列算出部１１３に伝送する(残差共分散行列の算出ステップＳ８)。
【０１５９】
【数４８】

【０１６０】
ゲート判定部１０７は、予測観測ベクトル算出部１２９から入力された予測観測ベクトル(上下バー)ｚ_g(ｋ)を用いて、以下の数式(７３)により観測ベクトル(アンダーバー)ｚ(ｋ)に対する残差ベクトルを算出した後、残差共分散行列Ｓ(ｋ)を読み込み、以下の数式(７４)によりゲート判定処理を実行する(ゲート判定ステップＳ９)。
【０１６１】
【数４９】

【０１６２】
ただし、数式(７４)において、ｄはパラメータである。なお、ゲート判定部１０７により用いられるゲートは、予測観測ベクトル(上下バー)ｚ_g(ｋ)を中心として空間範囲であって、追尾目標からの観測ベクトルを所望の確率で捕捉可能な空間範囲である。上記のゲートの大きさや形状は、予測ベクトル誤差の評価値である予測誤差共分散行列(バー)Ｐ(ｋ)、観測誤差の評価値である観測雑音共分散行列Ｒ(ｋ)、及び観測行列算出部１２５ａで算出される観測行列Ｈ(ｋ)から計算される上記数式(７２)の残差共分散行列Ｓ(ｋ)によって決定される。上記数式(７４)は、上記ゲート内に目標観測装置２０からの観測ベクトルが存在するか否かを判定する基準条件である。たとえば、ゲート判定部１０７は、残差ベクトル(アンダーバー)ｒ(ｋ)が数式(７４)を満たす場合、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)が追尾目標の探知結果であることを判定し、追尾目標からのであると判定された観測べクトル(アンダーバー)ｚ(ｋ)を平滑ベクトル算出部１０９に送出する。一方、目標観測装置２０から数式(７４)を満たす観測ベクトルが得られなかった場合、ゲート判定部１０７は、追尾目標の観測ベクトルは探知されなかったものと見なし、平滑ベクトル算出部１０９への観測ベクトルの送出を実行しない。
【０１６３】
平滑ベクトル算出用予測観測ベクトルの算出ステップＳ９ａでは、平滑ベクトル算出部１０９へ出力する予測観測ベクトルを以下の数式(７５)及び数式(７６)で算出する。
【０１６４】
【数５０】

【０１６５】
平滑ベクトル算出用予測観測ベクトル(上下バー)ｚ_h(ｋ)は、平滑ベクトル算出部１０９において、観測値との差である残差を計算するために用いられる。
【０１６６】
次に、ゲイン行列算出部１１３は、予測誤差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、および観測行列Ｈ(ｋ)を読み込み、以下の数式(７７)にしたがって、フィルタゲインを算出する(ゲイン行列の算出ステップＳ１０)。
【０１６７】
【数５１】

【０１６８】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力された場合に、予測ベクトル(上下バー)ｘ_h(ｋ)、予測観測ベクトル(上下バー)ｚ_h(ｋ)、及びゲイン行列Ｋ(ｋ)とを読み込み、以下の数式(７８)にしたがって平滑ベクトルを算出する。
【０１６９】
【数５２】

【０１７０】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力されなかった場合は、以下の数式(７９)に示すように、予測ベクトル(上下バー)ｘ_h(ｋ)を平滑ベクトルとする(平滑ベクトルの算出ステップＳ１１)。
【０１７１】
【数５３】

【０１７２】
上記数式(７８)または数式(７９)により、平滑ベクトル算出部１０９で算出された平滑ベクトル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモリ１０５に格納されるとともに、航跡表示装置３０に伝送され、運用者に対して表示される。
【０１７３】
一方、平滑誤差共分散行列算出部１２１は、ゲート判定部１０７で追尾目標の観測ベクトルが得られたと判定された場合には、予測誤差共分散行列(バー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列Ｈ(ｋ)を用いて、以下の数式(８０)により平滑誤差共分散行列を算出し、ゲート判定部１０７で追尾目標の観測ベクトルが得られなかったと判定された場合には、以下の数式(８１)に示すように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行列の算出ステップＳ１２)。
【０１７４】
【数５４】

【０１７５】
数式(８０)及び数式(８１)に従う平滑誤差共分散行列の算出結果は、平滑誤差共分散行列用メモリ１２３に格納される。
【０１７６】
以上で時刻ｔ(ｋ)における処理が終了する。以降、追尾終了が判定されるまで、上記一連の処理ステップを繰り返し実行する(追尾終了ステップＳ１３)。
【０１７７】
図６に、実施の形態２を用いた平滑ベクトルの位置成分(平滑値)、予測観測ベクトル(ゲート中心)、および観測ベクトル(観測値)の時間推移を示す。図の実線は、状態ベクトルの位置成分の真の軌跡を表し、点線は、ドップラシフトの影響を受けた観測ベクトルの真値を示す。上記の処理ステップにより、図６のように、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、前時刻の平滑ベクトルに対し、予めドップラ補正を施して擬似平滑ベクトルを算出することにより、擬似平滑ベクトルを用いて算出したゲート用予測観測ベクトルと観測ベクトルとの距離が短くなる効果がある。さらに、平滑ベクトルを算出する際の予測観測ベクトルに、上記で算出したゲート用予測観測ベクトルと異なり、ドップラシフトの影響を受けた予測ベクトルから算出した予測観測ベクトル値を用いることにより、残差ベクトルが大きくなり、切り替え時における平滑ベクトルの過渡応答の収束速度が速くなる効果がある。
【０１７８】
なお、本実施の形態の考え方を用いれば、ドップラシフトを考慮した観測モデルで構成されたフィルタからドップラシフトを考慮しない観測モデルを用いたフィルタへの切り替えに対する対応も可能であることは明らかであるので、説明は省略する。
【０１７９】
実施の形態３．
図７はこの発明の実施の形態３による目標追尾装置を示すブロック構成図であり、また図８はその処理動作を示すフローチャートである。上記図４、図５と同様のものについては、詳述を省略する。また、図９はこの発明の実施の形態３における効果を示す概念図である。
【０１８０】
図７において、目標追尾装置１０ａは、実施の形態２を示すブロック構成図である図４と同様に、平滑ベクトル用メモリ１０５、予測ベクトル算出部１０３、ゲート用予測観測ベクトル算出部１２９、ゲート判定部１０７、平滑ベクトル算出部１０９、残差共分散行列算出部１１１、ゲイン行列算出部１１３、切り替え制御部１２７、観測雑音共分散行列設定部１１５、予測誤差共分散行列算出部１１７、駆動雑音共分散行列設定部１１９、平滑誤差共分散行列算出部１２１、平滑誤差共分散行列用メモリ１２３を備えている。
【０１８１】
また、目標追尾装置１０ａは、上記構成要素に加えて、観測行列算出部１２５ｂ(実施の形態２の図４におけるそれと動作が異なる)、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ(実施の形態２の図４におけるそれと動作が異なる)を備えている。
【０１８２】
観測行列算出部１２５ｂは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より後は、予測ベクトル算出部１０３より算出された予測値を用いて、ドップラシフトを考慮した観測行列を算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３に出力する。切り替わる時刻には、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１にドップラシフトを考慮しない観測モデルを算出及び出力し、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３にドップラシフトを考慮した観測行列を算出及び出力する。
平滑ベクトル算出用予測観測ベクトル算出部１３１ａは、予測ベクトル算出部１０３からの予測ベクトル及び観測行列算出部１２５ｂからの入力である観測行列より、平滑ベクトル算出部１０９で用いる予測観測ベクトルを算出する。
【０１８３】
図８において、実施の形態２の処理動作を示すフローチャートである図５と同様に、初期値の設定ステップＳ１、観測ベクトルの入力ステップＳ２、予測ベクトルの算出ステップＳ３、駆動雑音共分散行列の設定ステップＳ４、予測誤差共分散行列の算出ステップＳ５、ゲート用予測観測ベクトルの算出ステップＳ５ｂ、観測雑音共分散行列の設定ステップＳ７、残差共分散行列の算出ステップＳ８、ゲート判定ステップＳ９、ゲイン行列の算出ステップＳ１０、平滑ベクトルの算出ステップＳ１１、平滑誤差共分散行列の算出ステップＳ１２、を備えている。
【０１８４】
また、図８は上記処理ステップに加えて、平滑ベクトル算出用予測観測ベクトルの算出ステップＳ９ａａ(実施の形態２の処理動作を示す図５のそれと処理が異なる)、観測行列算出ステップＳ５ａａａ(実施の形態２の処理動作を示す図５のそれと処理が異なる)、が挿入されている。
【０１８５】
図８において、平滑ベクトル算出用予測観測ベクトルの算出ステップＳ９ａａは、予測ベクトル算出部１０３からの予測ベクトルと、観測行列算出部１２５ｂで算出された観測行列を用いて、平滑ベクトルの算出に用いる予測観測ベクトルを算出するステップである。
【０１８６】
観測行列算出ステップＳ５ａａａは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より後は、予測ベクトル算出部１０３より算出された予測値を用いて、ドップラシフトを考慮した観測行列を算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻には、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、にドップラシフトを考慮しない観測モデルを算出及び出力し、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３に、ドップラシフトを考慮した観測行列を算出及び出力するステップである。
【０１８７】
以下、図７および図８を参照しながら、実施の形態３による処理動作について詳細に説明する。まず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベクトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の設定ステップＳ１)。ここで、状態ベクトル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義される、数式(８２)の６次元ベクトルである。
【０１８８】
【数５５】

【０１８９】
また、平滑誤差共分散行列は６行６列の行列である。以降、ｋ＝1,2,・・・の各時刻毎の処理を実行する。時刻ｔ(ｋ)の処理において、目標観測装置２０から観測ベクトル(アンダーバー)ｚ(ｋ)が入力される(観測ベクトルの入力ステップＳ２)。ここで、観測ベクトル(アンダーバー)ｚ(ｋ)は、ドップラシフトの影響を受けた、北基準直交座標系で得られる3次元ベクトルである。まず、予測ベクトルの算出ステップＳ３では、平滑ベクトル用メモリ１０５から前時刻ｔ(ｋ−１)の平滑ベクトル(アンダーバー・ハット)ｘ(ｋ−１)を読み出し、以下の数式(８３)にしたがって予測ベクトルを算出する(予測ベクトルの算出ステップＳ３)。
【０１９０】
【数５６】

【０１９１】
状態推移行列は、数式(８４)の行列で与えられる。
【０１９２】
【数５７】

【０１９３】
次に、駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列Ｑ(ｋ−１)を設定する(駆動雑音共分散行列の設定ステップＳ４)。通常、この駆動雑音共分散行列の値は、目標追尾装置１０ａの運用上で想定される最大の目標機動能力を考慮して、サンプリング間隔ｔ(ｋ)−ｔ(ｋ−１)の大きさに応じて決定される。次に、予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３から、前時刻ｔ(ｋ−１)の平滑誤差共分散行列Ｐ(ｋ−１)を読み出し、上記駆動雑音共分散行列Ｑ(ｋ−１)を用いて、以下の数式(８５)に従い予測誤差共分散行列を算出する(予測誤差共分散行列の算出ステップＳ５)。
【０１９４】
【数５８】

【０１９５】
次に、観測行列の算出ステップＳ５ａａａにおいて、切り替え制御部１２７からフィルタの切り替え時刻の情報を得、切り替え時刻より前については、観測行列を以下の数式(８６)として、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３へ出力する。
【０１９６】
【数５９】

【０１９７】
ここで、Ｉ_3x3は３行３列の単位行列、０_3x3は３行３列の零行列である。切り替え時刻より後においては、観測雑音行列算出部は、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ_h(ｋ)から、以下の数式(８７)、数式(８８)、数式(８９)、数式(９０)、数式(９１)、数式(９２)により観測行列を算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３へ出力する。
【０１９８】
【数６０】

【０１９９】
【数６１】

【０２００】
上記、数式(８７)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップラシフトの影響を反映した観測値を記述するための行列であり、観測値が距離方向にドップラシフトの影響分であるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表している。数式(８７)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で与えられた予測ベクトル(上下バー)ｘ_h(ｋ)を極座標系に変換した変数(バー)Ｒ(ｋ)、(バー)Ｅ(ｋ)、(バー)Ａｚ(ｋ)、(バー・ドット)Ｒ(ｋ)、(バー・ドット)Ｅ(ｋ)、(バー・ドット)Ａｚ(ｋ)を用いて算出される。数式(９１)及び数式(９２)は、北基準直交座標系から極座標への変換の式を表している。数式(８８)におけるΔＴは、予め与えられる時定数である。
【０２０１】
切り替え時刻の場合、観測行列算出部１２５ｂは、観測行列を数式(８６)として、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１へ出力し、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ_h(ｋ)から、数式(８７)、数式(８８)、数式(８９)、数式(９０)、数式(９１)、数式(９２)により観測行列を算出し、平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３へ出力する。
【０２０２】
ゲート用予測観測ベクトルの算出ステップ５ｂでは、ゲート用予測観測ベクトル算出部１２９は、予測ベクトル算出部１０３から入力された予測ベクトル(上下バー)ｘ_h(ｋ)と観測行列算出部１２５ｂから入力された観測行列を用いて、以下の数式(９３)により予測観測ベクトル(上下バー)ｚ_g(ｋ)を算出する(予測観測ベクトルの算出ステップ５ｂ)。
【０２０３】
【数６２】

【０２０４】
さらに、観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)の観測雑音共分散行列Ｒ(ｋ)を設定し、残差共分散行列算出部１１１及び平滑誤差共分散行列算出部１２１に送出する(観測雑音共分散行列の設定ステップＳ７)。なお、観測雑音共分散行列設定部１１５は、観測ベクトル(アンダーバー)ｚ(ｋ)の精度評価に関連したパラメータを用いて観測雑音共分散行列Ｒ(ｋ)を設定する。例えば、目標観測装置２０が北基準直交座標系におけるｘ，ｙ，ｚ軸方向について目標を探知するレーダの場合、レーダの角軸の観測精度の情報に基づいて観測雑音共分散行列Ｒ(ｋ)を設定することができる。ここで、上記観測精度は、レーダが時刻ｔ(ｋ)に検出した目標信号の強度(または信号対雑音比)に基づいて推定することができる。
【０２０５】
次に残差共分散行列算出部１１１は、観測雑音共分散行列Ｒ(ｋ)、予測誤差共分散行列(バー)Ｐ(ｋ)、観測行列Ｈ(ｋ)とを読み込み、以下の数式(９４)にしたがって残差共分散行列Ｓ(ｋ)を算出し、ゲート判定部１０７およびゲイン行列算出部１１３に伝送する(残差共分散行列の算出ステップＳ８)。
【０２０６】
【数６３】

【０２０７】
ゲート判定部１０７は、予測観測ベクトル算出部１２９から入力された予測観測ベクトル(上下バー)ｚ(ｋ)を用いて、以下の数式(９５)により観測ベクトル(アンダーバー)ｚ(ｋ)に対する残差ベクトルを算出した後、残差共分散行列Ｓ(ｋ)を読み込み、以下の数式(９６)によりゲート判定処理を実行する(ゲート判定ステップＳ９)。
【０２０８】
【数６４】

【０２０９】
ただし、数式(９６)において、ｄはパラメータである。なお、ゲート判定部１０７により用いられるゲートは、予測観測ベクトル(上下バー)ｚ_g(ｋ)を中心として空間範囲であって、追尾目標からの観測ベクトルを所望の確率で捕捉可能な空間範囲である。上記のゲートの大きさや形状は、予測ベクトル誤差の評価値である予測誤差共分散行列(バー)Ｐ(ｋ)、観測誤差の評価値である観測雑音共分散行列Ｒ(ｋ)、及び観測行列算出部１２５ｂで算出される観測行列Ｈ(ｋ)から計算される上記数式(９４)の残差共分散行列Ｓ(ｋ)によって決定される。上記数式(９６)は、上記ゲート内に目標観測装置２０からの観測ベクトルが存在するか否かを判定する基準条件である。たとえば、ゲート判定部１０７は、残差ベクトル(アンダーバー)ｒ(ｋ)が数式(９６)を満たす場合、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)が追尾目標の探知結果であることを判定し、追尾目標からのであると判定された観測べクトル(アンダーバー)ｚ(ｋ)を平滑ベクトル算出部１０９に送出する。一方、目標観測装置２０から数式(９６)を満たす観測ベクトルが得られなかった場合、ゲート判定部１０７は、追尾目標の観測ベクトルは探知されなかったものと見なし、平滑ベクトル算出部１０９への観測ベクトルの送出を実行しない。
【０２１０】
平滑ベクトル算出用予測観測ベクトルの算出ステップＳ９ａａでは、平滑ベクトル算出部１０９へ出力する予測観測ベクトルを以下の数式(９７)で算出する。
【０２１１】
【数６５】

【０２１２】
平滑ベクトル算出用予測観測ベクトル(上下バー)ｚ_h(ｋ)は、平滑ベクトル算出部１０９において、観測値との差である残差を計算するために用いられる。
【０２１３】
次に、ゲイン行列算出部１１３は、予測誤差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、および観測行列Ｈ(ｋ)を読み込み、以下の数式(９８)にしたがって、フィルタゲインを算出する(ゲイン行列の算出ステップＳ１０)。
【０２１４】
【数６６】

【０２１５】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力された場合に、予測ベクトル(上下バー)ｘ_h(ｋ)、予測観測ベクトル(上下バー)ｚ_h(ｋ)、及びゲイン行列Ｋ(ｋ)とを読み込み、以下の数式(９９)にしたがって平滑ベクトルを算出する。
【０２１６】
【数６７】

【０２１７】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力されなかった場合は、以下の数式(１００)に示すように、予測ベクトル(上下バー)ｘ_h(ｋ)を平滑ベクトルとする(平滑ベクトルの算出ステップＳ１１)。
【０２１８】
【数６８】

【０２１９】
上記数式(９９)または数式(１００)により、平滑ベクトル算出部１０９で算出された平滑ベクトル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモリ１０５に格納されるとともに、航跡表示装置３０に伝送され、運用者に対して表示される。
【０２２０】
一方、平滑誤差共分散行列算出部１２１は、ゲート判定部１０７で追尾目標の観測ベクトルが得られたと判定された場合には、予測誤差共分散行列(バー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列Ｈ(ｋ)を用いて、以下の数式(１０１)により平滑誤差共分散行列を算出し、ゲート判定部１０７で追尾目標の観測ベクトルが得られなかったと判定された場合には、以下の数式(１０２)に示すように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行列の算出ステップＳ１２)。
【０２２１】
【数６９】

【０２２２】
数式(１０１)及び数式(１０２)に従う平滑誤差共分散行列の算出結果は、平滑誤差共分散行列用メモリ１２３に格納される。
【０２２３】
以上で時刻ｔ(ｋ)における処理が終了する。以降、追尾終了が判定されるまで、上記一連の処理ステップを繰り返し実行する(追尾終了ステップＳ１３)。
【０２２４】
図９に実施の形態３を用いた平滑ベクトルの位置成分(平滑値)、予測観測ベクトル(ゲート中心)、および観測ベクトル(観測値)の時間推移を示す。図の実線は、状態ベクトルの位置成分の真の軌跡を表し、点線は、ドップラシフトの影響を受けた観測ベクトルの真値を示す。上記の処理ステップにより、図９のように、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、ゲート用予測観測ベクトルの算出においてドップラシフトを考慮しない観測モデルを用いることにより、ゲート用予測観測ベクトルと観測ベクトルとの距離が短くなる効果がある。さらに、平滑ベクトルを算出する際の予測観測ベクトルに、上記で算出したゲート用予測観測ベクトルと異なり、ドップラシフトを考慮した観測モデルを用いることにより、残差ベクトルが大きくなり、切り替え時における平滑ベクトルの過渡応答の収束速度が速くなる効果がある。
【０２２５】
なお、本実施の形態の考え方を用いれば、ドップラシフトを考慮した観測モデルで構成されたフィルタからドップラシフトを考慮しない観測モデルを用いたフィルタへの切り替えに対する対応も可能であることは明らかであるので、説明は省略する。
【０２２６】
実施の形態４．
図１０はこの発明の実施の形態４による目標追尾装置を示すブロック構成図であり、また図１１はその処理動作を示すフローチャートである。上記図７、図８と同様のものについては、詳述を省略する。また、図１２はこの発明の実施の形態３における効果を示す概念図である。
【０２２７】
図１０において、目標追尾装置１０ａは実施の形態３を示すブロック構成図である図７と同様に、平滑ベクトル用メモリ１０５、予測ベクトル算出部１０３、ゲート用予測観測ベクトル算出部１２９、ゲート判定部１０７、平滑ベクトル算出部１０９、残差共分散行列算出部１１１、ゲイン行列算出部１１３、切り替え制御部１２７、観測雑音共分散行列設定部１１５、予測誤差共分散行列算出部１１７、駆動雑音共分散行列設定部１１９、平滑誤差共分散行列算出部１２１、平滑誤差共分散行列用メモリ１２３、を備えている。
【０２２８】
また目標追尾装置１０ａは、上記構成要素に加えて観測行列算出部１２５ｃ(実施の形態３の図７におけるそれと動作が異なる)、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ(実施の形態３の図７におけるそれと動作が異なる)、を備えている。
【０２２９】
観測行列算出部１２５ｃは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より後は、予測ベクトル算出部１０３より算出された予測値を用いて、ドップラシフトを考慮した観測行列を算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻には、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、にドップラシフトを考慮しない観測モデルを算出及び出力し、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、にドップラシフトを考慮した観測行列を算出及び出力する。
【０２３０】
平滑ベクトル算出用予測観測ベクトル算出部１３１ｂは、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より前及び後は、予測ベクトル算出部１０３からの予測ベクトル及び観測行列算出部１２５ｃからの入力である観測行列より、平滑ベクトル算出部１０９で用いる予測観測ベクトルを算出する。ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻では、予測ベクトル算出部１０３からの予測ベクトルより、平滑ベクトル算出部１０９で用いる予測観測ベクトルを算出する。
【０２３１】
図１１において、実施の形態３の処理動作を示すフローチャートである図８と同様に、初期値の設定ステップＳ１、観測ベクトルの入力ステップＳ２、予測ベクトルの算出ステップＳ３、駆動雑音共分散行列の設定ステップＳ４、予測誤差共分散行列の算出ステップＳ５、ゲート用予測観測ベクトルの算出ステップ５ｂ、観測雑音共分散行列の設定ステップＳ７、残差共分散行列の算出ステップＳ８、ゲート判定ステップＳ９、ゲイン行列の算出ステップＳ１０、平滑ベクトルの算出ステップＳ１１、平滑誤差共分散行列の算出ステップＳ１２、を備えている。
【０２３２】
また、図１１は上記処理ステップに加えて、平滑ベクトル算出用予測観測ベクトルの算出ステップＱ９ａａａ(実施の形態３の処理動作を示す図８のそれと処理が異なる)、観測行列算出ステップＳ５ａａａａ(実施の形態３の処理動作を示す図 14のそれと処理が異なる)、が挿入されている。
図 17において、平滑ベクトル算出用予測観測ベクトルの算出ステップは、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より前及び後は、予測ベクトル算出部１０３からの予測ベクトル及び観測行列算出部１２５ｃからの入力である観測行列より、平滑ベクトル算出部１０９で用いる予測観測ベクトルを算出する。ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻では、予測ベクトル算出部１０３からの予測ベクトルより、平滑ベクトル算出部１０９で用いる予測観測ベクトルを算出するステップである。
【０２３３】
観測行列算出ステップＳ５ａａａａは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より後は、予測ベクトル算出部１０３より算出された予測値を用いて、ドップラシフトを考慮した観測行列を算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、に出力する。切り替わる時刻には、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、にドップラシフトを考慮しない観測モデルを算出及び出力し、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３、にドップラシフトを考慮した観測行列を算出及び出力するステップである。
【０２３４】
以下、図１０、図１１を参照しながら、実施の形態４による処理動作について詳細に説明する。まず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベクトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の設定ステップＳ１)。ここで、状態ベクトル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義される、数式(１０３)の６次元ベクトルである。
【０２３５】
【数７０】

【０２３６】
また、平滑誤差共分散行列は６行６列の行列である。以降、k=1,2,・・・の各時刻毎の処理を実行する。時刻ｔ(ｋ)の処理において、目標観測装置２０から観測ベクトル(アンダーバー)ｚ(ｋ)が入力される(観測ベクトルの入力ステップＳ２)。ここで、観測ベクトル(アンダーバー)ｚ(ｋ)は、ドップラシフトの影響を受けた、北基準直交座標系で得られる３次元ベクトルである。まず、予測ベクトルの算出ステップＳ３では、平滑ベクトル用メモリ１０５から前時刻ｔ(ｋ−１)の平滑ベクトル(アンダーバー・ハット)ｘ(ｋ−１)を読み出し、以下の数式(１０４)にしたがって予測ベクトルを算出する(予測ベクトルの算出ステップＳ３)。
【０２３７】
【数７１】

【０２３８】
状態推移行列は、数式(１０５)の行列で与えられる。
【０２３９】
【数７２】

【０２４０】
次に、駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列Ｑ(ｋ−１)を設定する(駆動雑音共分散行列の設定ステップＳ４)。通常、この駆動雑音共分散行列の値は、目標追尾装置１０ａの運用上で想定される最大の目標機動能力を考慮して、サンプリング間隔ｔ(ｋ)−ｔ(ｋ−１)の大きさに応じて決定される。次に、予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３から、前時刻ｔ(ｋ−１)の平滑誤差共分散行列Ｐ(ｋ−１)を読み出し、上記駆動雑音共分散行列Ｑ(ｋ−１)を用いて、以下の数式(１０６)に従い予測誤差共分散行列を算出する(予測誤差共分散行列の算出ステップＳ５)。
【０２４１】
【数７３】

【０２４２】
次に、観測行列の算出ステップＳ５ａａａａにおいて、切り替え制御部１２７からフィルタの切り替え時刻の情報を得、切り替え時刻より前については、観測行列を以下の数式(１０７)として、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３へ出力する。
【０２４３】
【数７４】

【０２４４】
ここで、Ｉ_3x3は３行３列の単位行列、０_3x3は３行３列の零行列である。切り替え時刻より後においては、観測行列算出部１２５ｃは、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ_h(ｋ)から、以下の数式(１０８)、数式(１０９)、数式(１１０)、数式(１１１)、数式(１１２)、数式(１１３)により観測行列を算出し、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１、平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３へ出力する。
【０２４５】
【数７５】

【０２４６】
【数７６】

【０２４７】
上記、数式(１０８)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップラシフトの影響を反映した観測値を記述するための行列であり、観測値が距離方向にドップラシフトの影響分であるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表している。数式(１０８)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で与えられた予測ベクトル(上下バー)ｘ_h(ｋ)を極座標系に変換した変数(バー)Ｒ(ｋ)、(バー)Ｅ(ｋ)、(バー)Ａｚ(ｋ)、(バー・ドット)Ｒ(ｋ)、(バー・ドット)Ｅ(ｋ)、(バー・ドット)Ａｚ(ｋ)を用いて算出される。数式(１１２)及び数式(１１３)は、北基準直交座標系から極座標への変換の式を表している。数式(１０９)におけるΔＴは、予め与えられる時定数である。
【０２４８】
切り替え時刻の場合、観測行列算出部１２５ｃは、観測行列を数式(１０７)として、ゲート用予測観測ベクトル算出部１２９、残差共分散行列算出部１１１へ出力し、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ_h(ｋ)から、数式(１０８)、数式(１０９)、数式(１１０)、数式(１１１)、数式(１１２)、数式(１１３)により観測行列を算出し、平滑誤差共分散行列算出部１２１、ゲイン行列算出部１１３へ出力する。
【０２４９】
ゲート用予測観測ベクトルの算出ステップＳ５ｂでは、ゲート用予測観測ベクトル算出部１２９は、予測ベクトル算出部１０３から入力された予測ベクトル(上下バー)ｘ_h(ｋ)と観測行列算出部１２５ｃから入力された観測行列を用いて、以下の数式(１１４)により予測観測ベクトル(上下バー)ｚ_g(ｋ)を算出する(予測観測ベクトルの算出ステップＳ５ｂ)。
【０２５０】
【数７７】

【０２５１】
さらに、観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)の観測雑音共分散行列Ｒ(ｋ)を設定し、残差共分散行列算出部１１１及び平滑誤差共分散行列算出部１２１に送出する(観測雑音共分散行列の設定ステップＳ７)。なお、観測雑音共分散行列設定部１１５は、観測ベクトル(アンダーバー)ｚ(ｋ)の精度評価に関連したパラメータを用いて観測雑音共分散行列Ｒ(ｋ)を設定する。例えば、目標観測装置２０が北基準直交座標系におけるｘ，ｙ，ｚ軸方向について目標を探知するレーダの場合、レーダの角軸の観測精度の情報に基づいて観測雑音共分散行列Ｒ(ｋ)を設定することができる。ここで、上記観測精度は、レーダが時刻ｔ(ｋ)に検出した目標信号の強度(または信号対雑音比)に基づいて推定することができる。
【０２５２】
次に残差共分散行列算出部１１１は、観測雑音共分散行列Ｒ(ｋ)、予測誤差共分散行列(バー)Ｐ(ｋ)、観測行列Ｈ(ｋ)とを読み込み、以下の数式(１１５)にしたがって残差共分散行列Ｓ(ｋ)を算出し、ゲート判定部１０７およびゲイン行列算出部１１３に伝送する(残差共分散行列の算出ステップＳ８)。
【０２５３】
【数７８】

【０２５４】
ゲート判定部１０７は、予測観測ベクトル算出部１２９から入力された予測観測ベクトル(上下バー)ｚ(ｋ)を用いて、以下の数式(１１６)により観測ベクトル(アンダーバー)ｚ(ｋ)に対する残差ベクトルを算出した後、残差共分散行列Ｓ(ｋ)を読み込み、以下の数式(１１７)によりゲート判定処理を実行する(ゲート判定ステップＳ９)。
【０２５５】
【数７９】

【０２５６】
ただし、数式(１１７)において、ｄはパラメータである。なお、ゲート判定部１０７により用いられるゲートは、予測観測ベクトル(上下バー)ｚ_g(ｋ)を中心として空間範囲であって、追尾目標からの観測ベクトルを所望の確率で捕捉可能な空間範囲である。上記のゲートの大きさや形状は、予測ベクトル誤差の評価値である予測誤差共分散行列(バー)Ｐ(ｋ)、観測誤差の評価値である観測雑音共分散行列Ｒ(ｋ)、及び観測行列算出部１２５ｃで算出される観測行列Ｈ(ｋ)から計算される上記数式(９４)の残差共分散行列Ｓ(ｋ)によって決定される。上記数式(１１７)は、上記ゲート内に目標観測装置２０からの観測ベクトルが存在するか否かを判定する基準条件である。たとえば、ゲート判定部１０７は、残差ベクトル(アンダーバー)ｒ(ｋ)が数式(１１７)を満たす場合、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)が追尾目標の探知結果であることを判定し、追尾目標からのであると判定された観測べクトル(アンダーバー)ｚ(ｋ)を平滑ベクトル算出部１０９に送出する。一方、目標観測装置２０から数式(１１７)を満たす観測ベクトルが得られなかった場合、ゲート判定部１０７は、追尾目標の観測ベクトルは探知されなかったものと見なし、平滑ベクトル算出部１０９への観測ベクトルの送出を実行しない。
【０２５７】
平滑ベクトル算出用予測観測ベクトルの算出ステップＳ９ａａでは、現時刻が移管時刻より前またの場合、平滑ベクトル算出部１０９へ出力する予測観測ベクトルを以下の数式(１１８)で算出する。
【０２５８】
【数８０】

【０２５９】
現時刻が移管時刻の場合、以下の数式(１１９)、数式(１２０)、数式(１２１)、数式(１２２)、数式(１２３)で予測観測ベクトルを算出する。
【０２６０】
【数８１】

【０２６１】
数式(１２０)は、北基準直交座標系から極座標系への変換式であり、数式(１２２)は極座標系から北基準直交座標系での変換式である。数式(１２１)は、ドップラシフトの影響を受けた予測ベクトルの距離成分にドップラシフトを与える式である。こうすることにより、平滑ベクトルの算出ステップにおける残差を大きくすることが可能となる。
【０２６２】
次に、ゲイン行列算出部１１３は、予測誤差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、および観測行列Ｈ(ｋ)を読み込み、以下の数式(１２４)にしたがって、フィルタゲインを算出する(ゲイン行列の算出ステップＳ１０)。
【０２６３】
【数８２】

【０２６４】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力された場合に、予測ベクトル(上下バー)ｘ_h(ｋ)、予測観測ベクトル(上下バー)ｚ_h(ｋ)、及びゲイン行列Ｋ(ｋ)とを読み込み、以下の数式(１２５)にしたがって平滑ベクトルを算出する。
【０２６５】
【数８３】

【０２６６】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力されなかった場合は、以下の数式(１２６)に示すように、予測ベクトル(上下バー)ｘ_h(ｋ)を平滑ベクトルとする(平滑ベクトルの算出ステップＳ１１)。
【０２６７】
【数８４】

【０２６８】
上記数式(１２５)または数式(１２６)により、平滑ベクトル算出部１０９で算出された平滑ベクトル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモリ１０５に格納されるとともに、航跡表示装置３０に伝送され、運用者に対して表示される。
【０２６９】
一方、平滑誤差共分散行列算出部１２１は、ゲート判定部１０７で追尾目標の観測ベクトルが得られたと判定された場合には、予測誤差共分散行列(バー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列Ｈ(ｋ)を用いて、以下の数式(１２７)により平滑誤差共分散行列を算出し、ゲート判定部１０７で追尾目標の観測ベクトルが得られなかったと判定された場合には、以下の数式(１２８)に示すように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行列の算出ステップＳ１２)。
【０２７０】
【数８５】

【０２７１】
数式(１２７)及び数式(１２８)に従う平滑誤差共分散行列の算出結果は、平滑誤差共分散行列用メモリ１２３に格納される。
【０２７２】
以上で時刻ｔ(ｋ)における処理が終了する。以降、追尾終了が判定されるまで、上記一連の処理ステップを繰り返し実行する(追尾終了ステップＳ１３)。
【０２７３】
図１２に、実施の形態１を用いた平滑ベクトルの位置成分(平滑値)、予測観測ベクトル(ゲート中心)、および観測ベクトル(観測値)の時間推移を示す。図の実線は、状態ベクトルの位置成分の真の軌跡を表し、点線は、ドップラシフトの影響を受けた観測ベクトルの真値を示す。上記の処理ステップにより、図１２のように、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、ゲート用予測観測ベクトルの算出においてドップラシフトを考慮しない観測モデルを用いることにより、ゲート用予測観測ベクトルと観測ベクトルとの距離が短くなる効果がある。さらに、平滑ベクトルを算出する際の予測観測ベクトルに、上記で算出したゲート用予測観測ベクトルと異なり、ドップラシフトを考慮した観測モデルを用いることにより、残差ベクトルが大きくなり、切り替え時における平滑ベクトルの過渡応答の収束速度が速くなる効果がある。
【０２７４】
なお、本実施の形態の考え方を用いれば、ドップラシフトを考慮した観測モデルで構成されたフィルタからドップラシフトを考慮しない観測モデルを用いたフィルタへの切り替えに対する対応も可能であることは明らかであるので、説明は省略する。
【０２７５】
実施の形態５．
図１３はこの発明の実施の形態５による目標追尾装置を示すブロック構成図であり、また図１４はその処理動作を示すフローチャートである。上記図１、図２と同様のものについては、詳述を省略する。また、図１５はこの発明の実施の形態３における効果を示す概念図である。
【０２７６】
図１３において、目標追尾装置１０ａは、実施の形態１を示すブロック構成図である図１と同様に、平滑ベクトル用メモリ１０５、予測ベクトル算出部１０３、予測観測ベクトル算出部１０１、ゲート判定部１０７、平滑ベクトル算出部１０９、残差共分散行列算出部１１１、ゲイン行列算出部１１３、観測雑音共分散行列設定部１１５、予測誤差共分散行列算出部１１７、駆動雑音共分散行列設定部１１９、平滑誤差共分散行列算出部１２１、平滑誤差共分散行列用メモリ１２３、を備えている。
【０２７７】
また、目標追尾装置１０ａは、上記構成要素に加えて、切り替え制御部１２７、フィルタ用ドップラシフト時定数算出部１３５、観測行列算出部１２５ｄ(実施の形態１の図１におけるそれと動作が異なる)、を備えている。
【０２７８】
切り替え制御部１２７は、ドップラシフトを考慮しない観測モデルにより構成されたフィルタからドップラシフトを考慮した観測モデルにより構成されたフィルタへの切り替えタイミングをフィルタ用ドップラシフト時定数算出部１３５に出力する。
【０２７９】
フィルタ用ドップラシフト時定数算出部１３５は、切り替え制御部１２７によるフィルタの切り替え時刻を入力とし、フィルタに用いる観測モデルに設定するドップラシフト時定数を算出し、観測行列算出部１２５ｄへ出力する。
【０２８０】
観測行列算出部１２５ｄは、フィルタ用ドップラシフト時定数算出部１３５からのドップラシフト時定数を入力とし、予測ベクトル算出部１０３より算出された予測値を用いて、ドップラシフトを考慮した観測行列を算出する。
【０２８１】
図１４において、実施の形態１の処理動作を示すフローチャートである図２と同様に、初期値の設定ステップＳ１、観測ベクトルの入力ステップＳ２、予測ベクトルの算出ステップＳ３、駆動雑音共分散行列の設定ステップＳ４、予測誤差共分散行列の算出ステップＳ５、予測観測ベクトルの算出ステップＳ６、観測雑音共分散行列の設定ステップＳ７、残差共分散行列の算出ステップＳ８、ゲート判定ステップＳ９、ゲイン行列の算出ステップＳ１０、平滑ベクトルの算出ステップＳ１１、平滑誤差共分散行列の算出ステップＳ１２、を備えている。
【０２８２】
また、図１４は上記処理ステップに加えて、フィルタ用ドップラシフト時定数の算出ステップＳ５ｃ、観測行列算出ステップＳ５ａａａａａ(実施の形態１の処理動作を示す図２のそれと処理が異なる)、が挿入されている。
【０２８３】
図１４において、フィルタ用ドップラシフト時定数の算出ステップＳ５ｃは、切り替え制御部１２７によるフィルタの切り替え時刻を入力とし、フィルタに用いる観測モデルに設定するドップラシフト時定数を算出し、観測行列算出部１２５ｄへ出力するステップである。
【０２８４】
観測行列算出ステップＳ５ａａａａａは、切り替え制御部１２７からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻以後は、予測ベクトル算出部１０３より算出された予測値を用いて、観測行列を算出し、切り替わる以前は、ドップラシフトを考慮しない観測モデルを出力するステップである。
【０２８５】
以下、図１４を参照しながら、図１３に示したこの発明の実施の形態５よる処理動作について詳細に説明する。まず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベクトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の設定ステップＳ１)。ここで、状態ベクトル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義される、数式(１２９)の６次元ベクトルである。
【０２８６】
【数８６】

また、平滑誤差共分散行列は６行６列の行列である。以降、k=1,2,・・・の各時刻毎の処理を実行する。時刻ｔ(ｋ)の処理において、目標観測装置２０から観測ベクトル(アンダーバー)ｚ(ｋ)が入力される(観測ベクトルの入力ステップＳ２)と、予測ベクトル算出部１０３は、平滑ベクトル用メモリ１０５から前時刻ｔ(ｋ−１)の平滑ベクトル(アンダーバー・ハット)ｘ(ｋ−１)を読み出し、以下の数式(１３０)にしたがって予測ベクトルを算出する(予測ベクトルの算出ステップＳ３)。
【０２８７】
【数８７】

【０２８８】
ここで、観測ベクトル(アンダーバー)ｚ(ｋ)は、ドップラシフトの影響を受けた、北基準直交座標系で得られる３次元ベクトルである。状態推移行列は、数式(１３１)の行列で与えられる。
【０２８９】
【数８８】

【０２９０】
次に、駆動雑音共分散行列設定部１１９は、駆動雑音共分散行列Ｑ(ｋ−１)を設定する(駆動雑音共分散行列の設定ステップＳ４)。通常、この駆動雑音共分散行列の値は、目標追尾装置１０ａの運用上で想定される最大の目標機動能力を考慮して、サンプリング間隔ｔ(ｋ)−ｔ(ｋ−１)の大きさに応じて決定される。次に、予測誤差共分散行列算出部１１７は、平滑誤差共分散行列用メモリ１２３から、前時刻ｔ(ｋ−１)の平滑誤差共分散行列Ｐ(ｋ)を読み出し、上記駆動雑音共分散行列Ｑ(ｋ−１)を用いて、以下の数式(１３２)に従い予測誤差共分散行列を算出する(予測誤差共分散行列の算出ステップＳ５)。
【０２９１】
【数８９】

【０２９２】
次に、フィルタ用ドップラシフト時定数の算出ステップＳ５ｃにおいて、切り替え制御部１２７により切り替え時刻情報を得、現時刻が切り替え時刻の場合、その時刻を基点として、徐々に、所望のドップラシフト時定数になるように、観測モデルに与えるドップラシフト時定数を算出する(フィルタ用ドップラシフト時定数の算出ステップＳ５ｃ)。たとえば、フィルタ用の観測モデルを構成するドップラシフト時定数を以下の数式(１３３)で算出する。またこれを図１６に示す。
【０２９３】
【数９０】

【０２９４】
ここで、ΔＴ(ｋ)は観測モデルを算出する際に用いるドップラシフト時定数、ΔＴは所望のドップラシフト時定数、ｋ_aはフィルタ切り替え時刻、ｋ_bは切り替え開始後、観測モデル算出用の時定数を所望の値にする時刻である。ｋ_bは、設計者が定めるパラメータである。こうすることにより、予測観測ベクトルをはじめとする変数が、切り替え前と切り替え後で大きく変化する現象を防ぐことが可能である。
【０２９５】
次に、観測行列算出部１２５ｄは、予測ベクトル算出部１０３から出力された予測ベクトル(上下バー)ｘ(ｋ)及び、フィルタ用ドップラシフト時定数算出部１３５から出力されたドップラシフト時定数ΔＴ(ｋ)から、観測行列を算出する(観測行列の算出ステップＳ５ａａａａａ)。
【０２９６】
【数９１】

【０２９７】
【数９２】

【０２９８】
上記、数式(１３４)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップラシフトの影響を反映した観測値を記述するための行列であり、観測値が距離方向にドップラシフトの影響分であるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表している。数式(１３４)に示されている観測行列Ｈ(ｋ)は、北基準直交座標系で与えられた予測ベクトル(上下バー)ｘ(ｋ)を極座標系に変換した変数(バー)Ｒ(ｋ)、(バー)Ｅ(ｋ)、(バー)Ａｚ(ｋ)、(バー・ドット)Ｒ(ｋ)、(バー・ドット)Ｅ(ｋ)、(バー・ドット)Ａｚ(ｋ)を用いて算出される。数式(１３７)及び数式(１３８)は、北基準直交座標系から極座標への変換の式を表している。数式(１３５)におけるΔＴは、予め与えられる時定数である。
【０２９９】
予測観測ベクトル算出部１０１は、予測ベクトル算出部１０３から入力された予測ベクトル(上下バー)ｘ(ｋ)と観測行列算出部１２５ｄから入力された観測行列を用いて、以下の数式(１４０)により予測観測ベクトル(上下バー)ｚ(ｋ)を算出する(予測観測ベクトルの算出ステップＳ６)。
【０３００】
【数９３】

【０３０１】
さらに、観測雑音共分散行列設定部１１５は、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)の観測雑音共分散行列Ｒ(ｋ)を設定し、残差共分散行列算出部１１１及び平滑誤差共分散行列算出部１２１に送出する(観測雑音共分散行列の設定ステップＳ７)。なお、観測雑音共分散行列設定部１１５は、観測ベクトル(アンダーバー)ｚ(ｋ)の精度評価に関連したパラメータを用いて観測雑音共分散行列Ｒ(ｋ)を設定する。例えば、目標観測装置２０が北基準直交座標系におけるｘ，ｙ，ｚ軸方向について目標を探知するレーダの場合、レーダの角軸の観測精度の情報に基づいて観測雑音共分散行列Ｒ(ｋ)を設定することができる。ここで、上記観測精度は、レーダが時刻ｔ(ｋ)に検出した目標信号の強度(または信号対雑音比)に基づいて推定することができる。
【０３０２】
次に残差共分散行列算出部１１１は、観測雑音共分散行列Ｒ(ｋ)、予測誤差共分散行列(バー)Ｐ(ｋ)、観測行列Ｈ(ｋ)とを読み込み、以下の数式(１４１)にしたがって残差共分散行列Ｓ(ｋ)を算出し、ゲート判定部１０７およびゲイン行列算出部１１３に伝送する(残差共分散行列の算出ステップＳ８)。
【０３０３】
【数９４】

【０３０４】
ゲート判定部１０７は、予測観測ベクトル算出部１０１から入力された予測観測ベクトル(上下バー)ｚ(ｋ)を用いて、以下の数式(１４２)により観測ベクトル(アンダーバー)ｚ(ｋ)に対する残差ベクトルを算出した後、残差共分散行列Ｓ(ｋ)を読み込み、以下の数式(１４３)によりゲート判定処理を実行する(ゲート判定ステップＳ９)。
【０３０５】
【数９５】

【０３０６】
ただし、数式(１４３)において、ｄはパラメータである。なお、ゲート判定部１０７により用いられるゲートは、予測観測ベクトル(上下バー)ｚ(ｋ)を中心として空間範囲であって、追尾目標からの観測ベクトルを所望の確率で捕捉可能な空間範囲である。上記のゲートの大きさや形状は、予測ベクトル誤差の評価値である予測誤差共分散行列(バー)Ｐ(ｋ)、観測誤差の評価値である観測雑音共分散行列Ｒ(ｋ)、及び観測行列算出部１２５ｄで算出される観測行列Ｈ(ｋ)から計算される上記数式(４４)の残差共分散行列Ｓ(ｋ)によって決定される。上記数式(１４３)は、上記ゲート内に目標観測装置２０からの観測ベクトルが存在するか否かを判定する基準条件である。たとえば、ゲート判定部１０７は、残差ベクトル(アンダーバー)ｒ(ｋ)が数式(１４３)を満たす場合、目標観測装置２０から入力された観測ベクトル(アンダーバー)ｚ(ｋ)が追尾目標の探知結果であることを判定し、追尾目標からのであると判定された観測べクトル(アンダーバー)ｚ(ｋ)を平滑ベクトル算出部１０９に送出する。一方、目標観測装置２０から数式(４６)を満たす観測ベクトルが得られなかった場合、ゲート判定部１０７は、追尾目標の観測ベクトルは探知されなかったものと見なし、平滑ベクトル算出部１０９への観測ベクトルの送出を実行しない。
【０３０７】
次に、ゲイン行列算出部１１３は、予測誤差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、および観測行列Ｈ(ｋ)を読み込み、以下の数式(１４４)にしたがって、フィルタゲインを算出する(ゲイン行列の算出ステップＳ１０)。
【０３０８】
【数９６】

【０３０９】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力された場合に、予測ベクトル(上下バー)ｘ(ｋ)、予測観測ベクトル(上下バー)ｚ(ｋ)、及びゲイン行列Ｋ(ｋ)とを読み込み、以下の数式(１４５)にしたがって平滑ベクトルを算出する。
【０３１０】
【数９７】

【０３１１】
また、平滑ベクトル算出部１０９は、ゲート判定部１０７から観測ベクトル(アンダーバー)ｚ(ｋ)が入力されなかった場合は、以下の数式(１４６)に示すように、予測ベクトル(上下バー)ｘ(ｋ)を平滑ベクトルとする(平滑ベクトルの算出ステップＳ１１)。
【０３１２】
【数９８】

【０３１３】
上記数式(１４５)または数式(１４６)により、平滑ベクトル算出部１０９で算出された平滑ベクトル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモリ１０５に格納されるとともに、航跡表示装置３０に伝送され、運用者に対して表示される。
【０３１４】
一方、平滑誤差共分散行列算出部１２１は、ゲート判定部１０７で追尾目標の観測ベクトルが得られたと判定された場合には、予測誤差共分散行列(バー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列Ｈ(ｋ)を用いて、以下の数式(１４７)により平滑誤差共分散行列を算出し、ゲート判定部１０７で追尾目標の観測ベクトルが得られなかったと判定された場合には、以下の数式(１４８)に示すように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行列の算出ステップＳ１２)。
【０３１５】
【数９９】

【０３１６】
数式(１４７)及び数式(１４８)に従う平滑誤差共分散行列の算出結果は、平滑誤差共分散行列用メモリ１２３に格納される。
【０３１７】
以上で時刻ｔ(ｋ)における処理が終了する。以降、追尾終了が判定されるまで、上記一連の処理ステップを繰り返し実行する(追尾終了ステップＳ１３)。
【０３１８】
図１５に、実施の形態１を用いた平滑ベクトルの位置成分(平滑値)、予測観測ベクトル(ゲート中心)、および観測ベクトル(観測値)の時間推移を示す。図の実線は、状態ベクトルの位置成分の真の軌跡を表し、点線は、ドップラシフトの影響を受けた観測ベクトルの真値を示す。上記の処理ステップにより、図１５のように、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、観測モデルの用いるドップラシフト時定数を少しずつ変化させることにより、ゲート用予測観測ベクトルと観測ベクトルとの距離が、切り替え直前と切り替え直後において、急激に変化しなくなる効果がある。
【０３１９】
なお、本実施の形態の考え方を用いれば、ドップラシフトを考慮した観測モデルで構成されたフィルタからドップラシフトを考慮しない観測モデルを用いたフィルタへの切り替えに対する対応も可能であることは明らかであるので、説明は省略する。
【０３２０】
【発明の効果】
以上のようにこの発明によれば、目標観測装置から入力される観測ベクトルに基づいて追尾目標の航跡を算出するための目標追尾装置であって、平滑ベクトルの計算結果を格納するための平滑ベクトル用メモリと、平滑誤差共分散行列の計算結果を格納するための平滑誤差共分散行列用メモリと、前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出する予測ベクトル算出部と、駆動雑音共分散行列を設定する駆動雑音共分散行列設定部と、前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音共分散行列の値とを用いて現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出部と、前記観測ベクトルの精度評価に関連したパラメータを用いて観測雑音共分散行列を設定する観測雑音共分散行列設定部と、前記予測ベクトルを用いて現時刻の観測行列を算出する観測行列算出部と、前記予測誤差共分散行列、前記観測雑音共分散行列、及び前記観測行列を用いて残差共分散行列を算出する残差共分散行列算出部と、前記予測ベクトル及び前記観測行列から予測観測ベクトルを算出する予測観測ベクトル算出部と、前記予測観測ベクトル及び前記観測ベクトルを用いて残差ベクトルを算出するとともに、前記残差共分散行列を用いて、前記観測ベクトルが前記追尾目標の探知データであるか否かのゲート判定を行うゲート判定部と、前記予測誤差共分散行列、前記残差共分散行列、及び前記観測行列をもちいてフィルタのゲイン行列を算出するゲイン行列算出部と、前記残差ベクトル、前記予測ベクトルおよび前記ゲイン行列をもちいて前記平滑ベクトルを算出する平滑ベクトル算出部と、前記観測雑音共分散行列、前記予測誤差共分散行列、及び前記観測行列とを用いて前記平滑誤差共分散行列を算出する平滑誤差共分散行列算出部と、を備えたことを特徴とする目標追尾装置およびこれに基づく目標追尾方法としたので、ドップラシフト補正を考慮したフィルタを北基準直交座標系で構成するために、従来の技術で用いた極座標系の距離軸で表現した観測モデルではなく、北基準直交座標系における観測モデルを構成することにより、ドップラシフト補正を北基準直交座標系で行うことが可能となる。
【０３２１】
また、目標観測装置から入力される観測ベクトルに基づいて追尾目標の航跡を算出するための目標追尾装置であって、平滑ベクトルの計算結果を格納するための平滑ベクトル用メモリと、平滑誤差共分散行列の計算結果を格納するための平滑誤差共分散行列用メモリと、ドップラシフトを考慮しない観測モデルにより構成されたフィルタからドップラシフトを考慮した観測モデルにより構成されたフィルタへの切り替えタイミングを出力する切り替え制御部と、切り替え制御部よりフィルタの切り替え時刻情報を得、現時刻が切り替えを行う時刻の場合、前記平滑ベクトル用メモリからの１時刻前における平滑ベクトルを用いて、擬似平滑値を算出し、切り替えを行う時刻でない場合、１時刻前の平滑値をそのまま出力する切り替え直前の擬似平滑値算出部と、前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出する予測ベクトル算出部と、駆動雑音共分散行列を設定する駆動雑音共分散行列設定部と、前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音共分散行列の値とを用いて現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出部と、前記観測ベクトルの精度評価に関連したパラメータを用いて観測雑音共分散行列を設定する観測雑音共分散行列設定部と、前記切り替え制御部からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻以後は、前記予測ベクトル算出部より算出された予測値を用いて、ドップラシフトを考慮した現時刻の観測行列を算出し、切り替わる時刻より前は、ドップラシフトを考慮しない現時刻の観測モデルを算出する観測行列算出部と、前記予測誤差共分散行列、前記観測雑音共分散行列、及び前記観測行列を用いて残差共分散行列を算出する残差共分散行列算出部と、前記予測ベクトル及び前記観測行列を用いて、ゲート判定部で用いる予測観測ベクトルを算出するゲート用予測観測ベクトル算出部と、前記予測観測ベクトル及び前記観測ベクトルを用いて残差ベクトルを算出するとともに、前記残差共分散行列を用いて、前記観測ベクトルが前記追尾目標の探知データであるか否かのゲート判定を行うゲート判定部と、前記平滑ベクトル用メモリからの１時刻前における平滑ベクトル及び観測行列算出部からの入力である観測行列より、平滑ベクトル算出部で用いる予測観測ベクトルを算出する平滑ベクトル算出用予測観測ベクトル算出部と、前記予測誤差共分散行列、前記残差共分散行列、及び前記観測行列をもちいてフィルタのゲイン行列を算出するゲイン行列算出部と、前記残差ベクトル、前記予測ベクトルおよび前記ゲイン行列をもちいて前記平滑ベクトルを算出する前記平滑ベクトル算出部と、前記観測雑音共分散行列、前記予測誤差共分散行列、及び前記観測行列とを用いて前記平滑誤差共分散行列を算出する平滑誤差共分散行列算出部と、を備えたことを特徴とする目標追尾装置およびこれに基づく目標追尾方法としたので、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、前時刻の平滑ベクトルに対し、予めドップラ補正を施して擬似平滑ベクトルを算出することにより、擬似平滑ベクトルを用いて算出したゲート用予測観測ベクトルと観測ベクトルとの距離が短くなる効果がある。さらに、平滑ベクトルを算出する際の予測観測ベクトルに、上記で算出したゲート用予測観測ベクトルと異なり、ドップラシフトの影響を受けた予測ベクトルから算出した予測観測ベクトル値を用いることにより、残差ベクトルが大きくなり、切り替え時における平滑ベクトルの過渡応答の収束速度が速くなる効果がある。
【０３２２】
また、目標観測装置から入力される観測ベクトルに基づいて追尾目標の航跡を算出するための目標追尾装置であって、平滑ベクトルの計算結果を格納するための平滑ベクトル用メモリと、平滑誤差共分散行列の計算結果を格納するための平滑誤差共分散行列用メモリと、前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出する予測ベクトル算出部と、駆動雑音共分散行列を設定する駆動雑音共分散行列設定部と、前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音共分散行列の値とを用いて現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出部と、前記観測ベクトルの精度評価に関連したパラメータを用いて観測雑音共分散行列を設定する観測雑音共分散行列設定部と、ドップラシフトを考慮しない観測モデルにより構成されたフィルタからドップラシフトを考慮した観測モデルにより構成されたフィルタへの切り替えタイミングを出力する切り替え制御部と、この切り替え制御部からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より後は、前記予測値を用いて、ドップラシフトを考慮した観測行列を算出し、後記ゲート用予測観測ベクトル算出部、後記残差共分散行列算出部、後記平滑ベクトル算出用予測観測ベクトル算出部、後記平滑誤差共分散行列算出部、後記行列算出部、に出力し、切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出し、後記ゲート用予測観測ベクトル算出部、後記残差共分散行列算出部、後記平滑ベクトル算出用予測観測ベクトル算出部、後記平滑誤差共分散行列算出部、後記ゲイン行列算出部、に出力し、さらに、切り替わる時刻には、後記ゲート用予測観測ベクトル算出部、後記残差共分散行列算出部、にドップラシフトを考慮しない観測モデルを算出及び出力し、後記平滑ベクトル算出用予測観測ベクトル算出部、後記平滑誤差共分散行列算出部、後記ゲイン行列算出部にドップラシフトを考慮した観測行列を算出及び出力する観測行列算出部と、前記予測誤差共分散行列、前記観測雑音共分散行列、及び前記観測行列を用いて残差共分散行列を算出する前記残差共分散行列算出部と、前記予測ベクトル及び前記観測行列を用いて、ゲート判定部で用いる予測観測ベクトルを算出する前記ゲート用予測観測ベクトル算出部と、前記予測観測ベクトル及び前記観測ベクトルを用いて残差ベクトルを算出するとともに、前記残差共分散行列を用いて、前記観測ベクトルが前記追尾目標の探知データであるか否かのゲート判定を行う前記ゲート判定部と、前記予測ベクトル及び前記観測行列より、後記平滑ベクトル算出部で用いる予測観測ベクトルを算出する前記平滑ベクトル算出用予測観測ベクトル算出部と、前記予測誤差共分散行列、前記残差共分散行列、及び前記観測行列をもちいてフィルタのゲイン行列を算出する前記ゲイン行列算出部と、前記残差ベクトル、前記予測ベクトルおよび前記ゲイン行列をもちいて前記平滑ベクトルを算出する前記平滑ベクトル算出部と、前記観測雑音共分散行列、前記予測誤差共分散行列、及び前記観測行列とを用いて前記平滑誤差共分散行列を算出する前記平滑誤差共分散行列算出部と、を備えたことを特徴とする目標追尾装置およびこれに基づく目標追尾方法としたので、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、ゲート用予測観測ベクトルの算出においてドップラシフトを考慮しない観測モデルを用いることにより、ゲート用予測観測ベクトルと観測ベクトルとの距離が短くなる効果がある。さらに、平滑ベクトルを算出する際の予測観測ベクトルに、上記で算出したゲート用予測観測ベクトルと異なり、ドップラシフトを考慮した観測モデルを用いることにより、残差ベクトルが大きくなり、切り替え時における平滑ベクトルの過渡応答の収束速度が速くなる効果がある。
【０３２３】
また、目標観測装置から入力される観測ベクトルに基づいて追尾目標の航跡を算出するための目標追尾装置であって、平滑ベクトルの計算結果を格納するための平滑ベクトル用メモリと、平滑誤差共分散行列の計算結果を格納するための平滑誤差共分散行列用メモリと、前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出する予測ベクトル算出部と、駆動雑音共分散行列を設定する駆動雑音共分散行列設定部と、前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音共分散行列の値とを用いて現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出部と、前記観測ベクトルの精度評価に関連したパラメータを用いて観測雑音共分散行列を設定する観測雑音共分散行列設定部と、ドップラシフトを考慮しない観測モデルにより構成されたフィルタからドップラシフトを考慮した観測モデルにより構成されたフィルタへの切り替えタイミングを出力する切り替え制御部と、この切り替え制御部からのフィルタの切り替え時刻情報を入力とし、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より後は、前記予測値を用いて、ドップラシフトを考慮した観測行列を算出し、後記ゲート用予測観測ベクトル算出部、後記残差共分散行列算出部、後記平滑ベクトル算出用予測観測ベクトル算出部、後記平滑誤差共分散行列算出部、後記ゲイン行列算出部、に出力し、切り替わる時刻より前は、ドップラシフトを考慮しない観測モデルを算出し、後記ゲート用予測観測ベクトル算出部、後記残差共分散行列算出部、後記平滑ベクトル算出用予測観測ベクトル算出部、後記平滑ベクトル算出用予測観測ベクトル算出部、後記平滑誤差共分散行列算出部、後記ゲイン行列算出部、に出力し、切り替わる時刻には、後記ゲート用予測観測ベクトル算出部、後記残差共分散行列算出部、にドップラシフトを考慮しない観測モデルを算出及び出力し、後記ゲイン行列算出部、にドップラシフトを考慮した観測行列を算出及び出力する観測行列算出部と、前記予測誤差共分散行列、前記観測雑音共分散行列、及び前記観測行列を用いて残差共分散行列を算出する前記残差共分散行列算出部と、前記予測ベクトル及び前記観測行列を用いて、ゲート判定部で用いる予測観測ベクトルを算出する前記ゲート用予測観測ベクトル算出部と、前記予測観測ベクトル及び前記観測ベクトルを用いて残差ベクトルを算出するとともに、前記残差共分散行列を用いて、前記観測ベクトルが前記追尾目標の探知データであるか否かのゲート判定を行う前記ゲート判定部と、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻より前及び後は、予測ベクトル算出部からの予測ベクトル及び観測行列算出部からの入力である観測行列より、平滑ベクトル算出部で用いる予測観測ベクトルを算出し、ドップラシフトを考慮した観測モデルにより構成されたフィルタに切り替わる時刻では、予測ベクトル算出部からの予測ベクトルより、平滑ベクトル算出部で用いる予測観測ベクトルを算出する前記平滑ベクトル算出用予測観測ベクトル算出部と、前記予測誤差共分散行列、前記残差共分散行列、及び前記観測行列をもちいてフィルタのゲイン行列を算出する前記ゲイン行列算出部と、前記残差ベクトル、前記予測ベクトルおよび前記ゲイン行列をもちいて前記平滑ベクトルを算出する前記平滑ベクトル算出部と、前記観測雑音共分散行列、前記予測誤差共分散行列、及び前記観測行列とを用いて前記平滑誤差共分散行列を算出する前記平滑誤差共分散行列算出部と、を備えたことを特徴とする目標追尾装置およびこれに基づく目標追尾方法としたので、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、ゲート用予測観測ベクトルの算出においてドップラシフトを考慮しない観測モデルを用いることにより、ゲート用予測観測ベクトルと観測ベクトルとの距離が短くなる効果がある。さらに、平滑ベクトルを算出する際の予測観測ベクトルに、上記で算出したゲート用予測観測ベクトルと異なり、ドップラシフトを考慮した観測モデルを用いることにより、残差ベクトルが大きくなり、切り替え時における平滑ベクトルの過渡応答の収束速度が速くなる効果がある。
【０３２４】
また、目標観測装置から入力される観測ベクトルに基づいて追尾目標の航跡を算出するための目標追尾装置であって、平滑ベクトルの計算結果を格納するための平滑ベクトル用メモリと、平滑誤差共分散行列の計算結果を格納するための平滑誤差共分散行列用メモリと、ドップラシフトを考慮しない観測モデルにより構成されたフィルタからドップラシフトを考慮した観測モデルにより構成されたフィルタへの切り替えタイミングを出力する切り替え制御部と、前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出する予測ベクトル算出部と、駆動雑音共分散行列を設定する駆動雑音共分散行列設定部と、前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音共分散行列の値とを用いて現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出部と、前記観測ベクトルの精度評価に関連したパラメータを用いて観測雑音共分散行列を設定する観測雑音共分散行列設定部と、前記予測誤差共分散行列、前記観測雑音共分散行列、及び前記観測行列を用いて残差共分散行列を算出する残差共分散行列算出部と、前記切り替え制御部によるフィルタの切り替え時刻を入力とし、フィルタに用いる観測モデルに設定するドップラシフト時定数を算出し、後記観測行列算出部へ出力するフィルタ用ドップラシフト時定数算出部と、前記フィルタ用ドップラシフト時定数算出部からのドップラシフト時定数を入力とし、前記予測値を用いて、ドップラシフトを考慮した観測行列を算出する観測行列算出部と、前記予測ベクトル及び前記観測行列から予測観測ベクトルを算出する予測観測ベクトル算出部と、前記予測観測ベクトル及び前記観測ベクトルを用いて残差ベクトルを算出するとともに、前記残差共分散行列を用いて、前記観測ベクトルが前記追尾目標の探知データであるか否かのゲート判定を行うゲート判定部と、前記予測誤差共分散行列、前記残差共分散行列、及び前記観測行列をもちいてフィルタのゲイン行列を算出するゲイン行列算出部と、前記残差ベクトル、前記予測ベクトルおよび前記ゲイン行列をもちいて前記平滑ベクトルを算出する平滑ベクトル算出部と、前記観測雑音共分散行列、前記予測誤差共分散行列、及び前記観測行列とを用いて前記平滑誤差共分散行列を算出する平滑誤差共分散行列算出部と、を備えたことを特徴とする目標追尾装置およびこれに基づく目標追尾方法としたので、ドップラシフトを考慮しない観測モデルで構成されたフィルタから、ドップラシフトを考慮した観測モデルを考慮したフィルタへ切り替えを行う際に、ゲートの中心である予測観測ベクトルと観測ベクトルとの距離が離れてしまう問題を改善するため、切り替え時に、観測モデルの用いるドップラシフト時定数を少しずつ変化させることにより、ゲート用予測観測ベクトルと観測ベクトルとの距離が、切り替え直前と切り替え直後において、急激に変化しなくなる効果がある。
【図面の簡単な説明】
【図１】 この発明の実施の形態１による目標追尾装置を示すブロック構成図である。
【図２】 この発明の実施の形態１による目標追尾装置の処理動作を示すフローチャートである。
【図３】 この発明の実施の形態１における効果を示す概念図である。
【図４】 この発明の実施の形態２による目標追尾装置を示すブロック構成図である。
【図５】 この発明の実施の形態２による目標追尾装置の処理動作を示すフローチャートである。
【図６】 この発明の実施の形態２における効果を示す概念図である。
【図７】 この発明の実施の形態３による目標追尾装置を示すブロック構成図である。
【図８】 この発明の実施の形態３による目標追尾装置の処理動作を示すフローチャートである。
【図９】 この発明の実施の形態３における効果を示す概念図である。
【図１０】 この発明の実施の形態４による目標追尾装置を示すブロック構成図である。
【図１１】 この発明の実施の形態４による目標追尾装置の処理動作を示すフローチャートである。
【図１２】 この発明の実施の形態４における効果を示す概念図である。
【図１３】 この発明の実施の形態５による目標追尾装置を示すブロック構成図である。
【図１４】 この発明の実施の形態５による目標追尾装置の処理動作を示すフローチャートである。
【図１５】 この発明の実施の形態５における効果を示す概念図である。
【図１６】 この発明の実施の形態５におけるフィルタ用ドップラシフト時定数を説明するための図である。
【図１７】 従来のこの種の目標追尾装置を示すブロック構成図である。
【図１８】 従来の目標追尾装置の処理動作を示すフローチャートである。
【図１９】 この発明の実施の形態５における動作を説明するための概念図である。
【図２０】 北基準直交座標と極座標の定義を説明するための図である。
【図２１】 従来の技術におけるフィルタの切り替えを説明するための概念図である。
【符号の説明】
１０ａ 目標追尾装置、２０ 目標観測装置、３０ 航跡表示装置、１０１ 予測観測ベクトル算出部、１０３ 予測ベクトル算出部、１０５ 平滑ベクトル用メモリ、１０７ ゲート判定部、１０９ 平滑ベクトル算出部、１１１ 残差共分散行列算出部、１１３ ゲイン行列算出部、１１５ 観測雑音共分散行列設定部、１１７ 予測誤差共分散行列算出部、１１９ 駆動雑音共分散行列設定部、１２１ 平滑誤差共分散行列算出部、１２３ 平滑誤差共分散行列用メモリ、１２５，１２５ａ，１２５ｂ，１２５ｃ，１２５ｄ 観測行列算出部、１２７ 切り替え制御部、１２９ ゲート用予測観測ベクトル算出部、１３１，１３１ａ，１３１ｂ 平滑ベクトル算出用予測観測ベクトル算出部、１３３ 切り替え直前の擬似平滑値算出部、１３５ フィルタ用ドップラシフト時定数算出部。[0001]
BACKGROUND OF THE INVENTION
The present invention acquires a target position of a moving body such as an aircraft or a ship as an observation value (vector data) using a target observation device including a sensor such as a radar, and calculates a track of a tracking target from an observation vector. And the target tracking method.
[0002]
[Prior art]
"Fitzgerald RJ," 'Effects of Range-Doppler Coupling on Chirp Radar Tracking Accuracy "IEEE Transactions on, as shown in the prior art on target tracking methods in radar and other sensors using pulse compression based on linear frequency modulation Aerospace and Electronic Systems, Vol. AES-10, No.4, July 1974 ”.
[0003]
In sensors such as radar using pulse compression radar using linear frequency modulation, when the distance to the target is observed by the sensor, it is multiplied by a constant (called the Doppler shift time constant) in the distance change rate in addition to the distance component. The observation value added by the amount of the obtained is obtained. Therefore, only the distance component cannot be observed independently. If the true value of the distance between the sensor and the target is R, the distance change rate is (dot) R, the Doppler shift time constant is ΔT, and the observed noise value is v, the observed value is expressed by the following formula (1).
[0004]
[Expression 1]

[0005]
Here, ΔT is a constant determined by the linear frequency modulation method. The above document proposes a tracking method using a mathematical model in consideration of the added amount ΔT (dot) R.
[0006]
Hereinafter, conventional techniques will be described in detail with reference to the drawings. FIG. 17 is a block diagram of a conventional target tracking device of this type, FIG. 18 is a flowchart showing the processing operation, and FIG. 19 is a conceptual diagram for explaining the operation in the conventional technique.
[0007]
In FIG. 17, the target observation device 20 is a device comprising a sensor that observes a target position in space, the target tracking device 10 is a device that executes target tracking processing based on observation data from the target observation device 20, and a wake display device 30. Is a device that indicates to the operator the wake that is the processing result of the target tracking device 10.
[0008]
The target tracking device 10 includes a predicted observation vector calculation unit 101, a prediction vector calculation unit 103, a smooth vector memory 105, a gate determination unit 107, a smooth vector calculation unit 109, a residual covariance matrix calculation unit 111, and a gain matrix calculation unit 113. , An observation noise covariance matrix setting unit 115, a prediction error covariance matrix calculating unit 117, a driving noise covariance matrix setting unit 119, a smoothing error covariance matrix calculating unit 121, and a smoothing error covariance matrix memory 123.
[0009]
In the target tracking device 10, the smooth vector memory 105 stores a smooth vector calculation result by the smooth vector calculation unit 109. The smoothing error covariance matrix memory 123 stores the calculation result of the smoothing error covariance matrix by the smoothing error covariance matrix calculating unit 121. The prediction vector calculation unit 103 calculates a prediction vector at the current time from the smooth vector at the previous time stored in the smooth vector memory 105. The driving noise covariance matrix setting unit 119 sets a driving noise covariance matrix. The prediction error covariance matrix calculation unit 117 calculates the previous time smoothing error covariance matrix stored in the smoothing error covariance matrix memory 123 and the driving noise covariance matrix setting unit 119. A prediction error covariance matrix at the current time is calculated from the value of the variance matrix.
[0010]
The observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix based on the parameters related to the accuracy evaluation of the observation vector input from the target observation device 20. The residual covariance matrix calculation unit 111 calculates the residual from the prediction error covariance matrix calculated by the prediction error covariance matrix calculation unit 117 and the observation noise covariance matrix set by the observation noise covariance matrix setting unit 115. Calculate the difference covariance matrix. The predicted observation vector calculation unit 101 calculates a predicted observation vector from the prediction vector calculated by the prediction vector calculation unit 103. The gate determination unit 107 calculates a residual vector based on the predicted observation vector calculated by the predicted observation vector calculation unit 101 and the observation vector from the target observation device 20, and also calculates a residual covariance matrix calculation unit 111. Using the residual covariance matrix calculated by the above, a gate determination is made as to whether or not the observation vector is tracking target detection data.
[0011]
The gain matrix calculation unit 113 uses the prediction error covariance matrix calculated by the prediction error covariance matrix calculation unit 117 and the residual covariance matrix calculated by the residual covariance matrix calculation unit 111 to use a filter. Calculate the gain matrix. The smooth vector calculation unit 109 calculates the observation vector output from the gate determination unit 107, the prediction vector calculated by the prediction vector calculation unit 103, the prediction observation vector calculated by the prediction observation vector calculation unit 101, and the gain matrix calculation. A smooth vector is calculated using the gain matrix calculated by the unit 113. The smoothing error covariance matrix calculation unit 121 uses the prediction error covariance matrix calculated by the prediction error covariance matrix calculation unit 117 and the observation noise covariance matrix calculated by the observation noise covariance matrix setting unit 115. Then, a smooth error covariance matrix is calculated.
[0012]
In FIG. 18, an initial value setting step S1 sets initial values of a smooth vector and a smooth error covariance matrix. In the observation vector input step S <b> 2, the observation vector at the current time is read from the target observation device 20. In the prediction vector calculation step S3, a prediction vector is calculated from the smooth vector at the previous time. In the drive noise covariance matrix setting step S4, a drive noise covariance matrix is set.
[0013]
A prediction error covariance matrix calculation step S5 calculates a prediction error covariance matrix from the smoothing error covariance matrix and the drive noise covariance matrix at the previous time. The predicted observation vector calculation step S6 calculates a predicted observation vector from the prediction vector. In the observation noise covariance matrix setting step S7, an observation noise covariance matrix is set based on parameters related to the accuracy evaluation of the observation vector input from the target observation apparatus 20. In the residual covariance matrix calculation step S8, a residual covariance matrix is calculated using the prediction error covariance matrix and the observation noise covariance matrix.
[0014]
The gate determination step S9 calculates a residual vector using the predicted observation vector and the observation vector from the target observation apparatus 20, and uses the residual covariance matrix to determine whether the observation vector is tracking target detection data. Judgment of whether or not is made. The gain matrix calculation step S10 calculates a filter gain matrix using the prediction error covariance matrix and the residual covariance matrix. The smoothing vector calculation step S11 calculates a smoothing vector using the observation vector, the prediction vector, the prediction observation vector, and the gain matrix determined as the tracking target detection data in the gate determination. The smoothing error covariance matrix calculation step S12 calculates a smoothing error covariance matrix using the prediction error covariance matrix and the observation noise covariance matrix. In the tracking end determination step S13, it is determined whether or not the tracking process is ended. When the tracking process is continued, the process returns to the observation vector input step S1.
[0015]
Here, with respect to the conventional technique, a mathematical model of a target and a sensor assumed in the Kalman filter theory will be briefly described. In the Kalman filter theory, in order to estimate the target motion specifications, the target motion model and the sensor observation model are set as follows. First, the target motion model is expressed as the following formula (2).
[0016]
[Expression 2]

[0017]
In equation (2), (underbar) x (k) is an n-dimensional state vector representing the true value of the target motion specification at sampling time (hereinafter simply referred to as “time”) t (k), Ф (k−1) Is an n-by-n transition matrix representing the transition of the state vector from time t (k-1) to t (k). Further, (underbar) w (k) is an n-dimensional driving noise vector at time t (k), and is a white noise sequence according to an n-variate normal distribution of mean (underbar) 0 and covariance matrix Q (k).
[0018]
When defining the true value (underbar) x (k) of the target motion specification at time t (k), a coordinate system is required. FIG. 20 shows the north reference Cartesian coordinates and polar coordinates. In FIG. 20, the radar is located at the origin, the x-axis is east, the y-axis is north, and the vertical coordinates above the horizontal plane are the z-axis. A coordinate system in which the north reference orthogonal coordinates, the distance from the radar to the target is R, the elevation angle from the horizontal plane to the target is E, and the azimuth angle from the north direction to the target in the horizontal plane is Az is called a polar coordinate system. In the conventional technique, it is assumed that the basic motion of the target is constant speed motion in the distance direction, and the state vector and transition matrix in the motion model of Equation (2) in the polar coordinate system are expressed by the following Equation (3) and Equation (3) Set as shown in 4).
[0019]
[Equation 3]

[0020]
In Equations (3) and (4), the driving noise vector (underbar) w (k) represents an error term associated with approximating the actual target motion to a constant velocity motion in the distance direction. In the formula (3), “dot” on the character represents differentiation, and “T” represents transposition of the matrix.
[0021]
Next, an observation model of the sensor is expressed as the following formula (5).
[0022]
[Expression 4]

[0023]
In Equation (5), (underbar) z (k) is an m-dimensional observation vector at time t (k), and H (k) is an m-by-n observation matrix. (Under bar) v (k) is an m-dimensional observation noise vector at time t (k) corresponding to the observation vector (under bar) z (k), and the mean (under bar) 0 and m of the covariance matrix R (k). It is a white noise sequence according to the variable amount normal distribution. However, it is assumed that the observation noise vector (underbar) v (k) and the drive noise vector (underbar) w (k) are independent from each other.
[0024]
In the conventional technology, it is assumed that the observation result in the target distance direction in the polar coordinate system is obtained in a sensor such as a radar using pulse compression by the linear frequency modulation method, and the observation matrix and observation vector in the observation model are set as follows: To do.
[0025]
[Equation 5]

[0026]
Here, ΔT is a constant determined by the linear frequency modulation method. H (k) is a constant matrix. As in Equation (6) and Equation (7), the observed value (underbar) z (k) is the sum of the values obtained by multiplying the distance R (k) by the distance change rate (dot) R (k) by a constant ΔT. And observation noise (underbar) v (k). By using this observation model, it is possible to configure a Kalman filter taking into account the Doppler shift ΔT (dot) R (k).
[0027]
The entire observation vector (underbar) z (k) from the initial time t (0) to time t (k) is expressed as in the following formula (8).
[0028]
[Formula 6]

[0029]
As is well known, the Kalman filter is a sequential algorithm that uses the time series data of the observation vectors based on the motion model and the observation model to calculate an optimal state vector estimate in the sense that the variance of the estimation error is minimized. .
[0030]
The operation of the conventional target tracking device shown in FIG. 17 will be described below with reference to FIG. Here, in accordance with the Kalman filter theory, a predicted vector obtained by estimating the state vector (underbar) x (k) at time t (k) based on the observation results up to time t (k−1) is (upper and lower bars) x ( k), and a smooth vector obtained by estimating the state vector (underbar) x (k) at time t (k) based on the observation results up to time t (k) is represented as (underbar hat) x (k). In addition, a prediction error covariance matrix that is an error covariance matrix of a prediction vector is represented by (bar) P (k), and a smooth error covariance matrix that is an error covariance matrix of a smooth vector is represented by P (k).
[0031]
Furthermore, a predicted observation vector (upper / lower bar) z (k) obtained by estimating the observation vector (underbar) z (k) at time t (k) obtained from the predicted vector (upper / lower bar) x (k) is obtained from the sensor. The residual vector representing the error between the observed vector (underbar) z (k) and the predicted observed vector (upper and lower bars) z (k) is (underbar) r (k) and the residual vector is a covariance matrix of the residual vector. The difference covariance matrix is represented as S (k). Here, the prediction error covariance matrix and the smoothing error covariance matrix are defined by Equations (9) and (10) below.
[0032]
[Expression 7]

[0033]
In Equations (9) and (10), E [• | Z (k)] is a symbol that represents a conditional expected value when an observation vector up to time t (k) is obtained.
[0034]
First, in the smoothing vector memory 105 and the smoothing error covariance matrix memory 123, an initial value (underbar hat) x (0) of the smoothing vector and an initial value P (0) of the smoothing error covariance matrix are respectively stored. The tracking process is set (initial value setting step S1).
[0035]
Thereafter, processing at each time of k = 1, 2,... Is executed. In the process at time t (k), when an observation vector (underbar) z (k) is input from the target observation device 20 (observation vector input step S2), the prediction vector calculation unit 103 reads from the smooth vector memory 105. The smoothed vector (underbar hat) x (k−1) at the previous time t (k−1) is read, and the prediction vector is calculated according to the following equation (11) (prediction vector calculation step S3).
[0036]
[Equation 8]

[0037]
Next, the driving noise covariance matrix setting unit 119 sets the driving noise covariance matrix Q (k−1) (driving noise covariance matrix setting step S4). Normally, the value of the driving noise covariance matrix is set to the size of the sampling interval t (k) −t (k−1) in consideration of the maximum target maneuverability assumed in the operation of the target tracking device 10. Will be decided accordingly. Next, the prediction error covariance matrix calculation unit 117 reads the smoothing error covariance matrix P (k−1) at the previous time t (k−1) from the smoothing error covariance matrix memory 123, and the driving noise covariance matrix is read out. Using the variance matrix Q (k−1), a prediction error covariance matrix is calculated according to the following equation (12) (prediction error covariance matrix calculation step S5).
[0038]
[Equation 9]

[0039]
Further, the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) of the observation vector (underbar) z (k) input from the target observation device 20, and a residual covariance matrix calculation unit. 111 and the smoothing error covariance matrix calculation unit 121 (observation noise covariance matrix setting step S7). Note that the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) using parameters related to the accuracy evaluation of the observation vector (underbar) z (k). For example, when the target observation device 20 is a radar that detects a target based on a distance, the observation noise covariance matrix R (k) can be set based on information on the radar ranging accuracy. Here, the distance measurement accuracy can be estimated based on the intensity (or signal-to-noise ratio) of the target signal detected by the radar at time t (k). The prediction observation vector calculation unit 101 uses the prediction vector (upper and lower bars) x (k) input from the prediction vector calculation unit 103 to calculate the prediction observation vector (upper and lower bars) z (k) according to the following equation (13). Calculate (prediction observation vector calculation step S6).
[0040]
[Expression 10]

[0041]
Next, the residual covariance matrix calculation unit 111 reads the observation noise matrix R (k) and the prediction error covariance matrix (bar) P (k), and the residual covariance matrix according to the following equation (14). S (k) is calculated and transmitted to the gate determination unit 107 and the gain matrix calculation unit 113 (residual covariance matrix calculation step S8).
[0042]
## EQU11 ##

[0043]
The gate determination unit 107 calculates a residual vector for the observation vector (underbar) z (k) by the following equation (15), and then reads the residual covariance matrix S (k). A gate determination process is executed (gate determination step S9).
[0044]
[Expression 12]

[0045]
However, in Formula (16), d is a parameter. Note that the gate used by the gate determination unit 107 is a spatial range centered on the predicted observation vector (upper and lower bars) z (k), and is a spatial range in which the observation vector from the tracking target can be captured with a desired probability. . The size and shape of the gate are calculated from the prediction error covariance matrix (bar) P (k), which is the evaluation value of the prediction vector error, and the observation noise covariance matrix R (k), which is the evaluation value of the observation error. It is determined by the residual covariance matrix S (k) of the above equation (14). Equation (16) is a reference condition for determining whether or not an observation vector from the target observation apparatus 20 exists in the gate. For example, when the residual vector (underbar) r (k) satisfies Expression (16), the gate determination unit 107 uses the observation vector (underbar) z (k) input from the target observation device 20 as the tracking target detection result. And the observation vector (underbar) z (k) satisfying Expression (16) is sent to the smooth vector calculation unit 109. On the other hand, when an observation vector that satisfies Equation (16) is not obtained from the target observation device 20, the gate determination unit 107 regards that the observation target observation vector has not been detected, and the observation to the smooth vector calculation unit 109 is performed. Do not send vectors.
[0046]
Next, the gain matrix calculation unit 113 reads the prediction error covariance matrix (bar) P (k) and the residual covariance matrix S (k), and calculates the filter gain according to the following equation (17). (Gain matrix calculation step S10).
[0047]
[Formula 13]

[0048]
Further, when the observation vector (under bar) z (k) is input from the gate determination unit 107, the smooth vector calculation unit 109 receives the predicted observation vector (upper and lower bars) z (k) and the predicted vector (up and down bar) x ( k) and the gain matrix K (k) are read and a smooth vector is calculated according to the following equation (18).
[0049]
[Expression 14]

[0050]
Further, when the observation vector (under bar) z (k) is not input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x ( k) is a smooth vector (smooth vector calculation step S11).
[0051]
[Expression 15]

[0052]
The smoothing vector (underbar hat) x (k) calculated by the smoothing vector calculation unit 109 according to the above formula (18) or formula (19) is stored in the smooth vector memory 105 and also in the wake display device 30. Is transmitted and displayed to the operator.
[0053]
On the other hand, the smoothing error covariance matrix calculation unit 121, when the gate determination unit 107 determines that the tracking target observation vector has been obtained, the prediction error covariance matrix (bar) P (k) and the observation noise covariance. The smoothing error covariance matrix is calculated by the following equation (20) using the matrix R (k), and when the gate determination unit 107 determines that the tracking target observation vector has not been obtained, As shown in Equation (21), the prediction error covariance matrix (bar) P (k) is set as a smoothing error covariance matrix (smooth error covariance matrix calculation step S12).
[0054]
[Expression 16]

[0055]
The calculation result of the smoothing error covariance matrix according to Equation (20) and Equation (21) is stored in the smoothing error covariance matrix memory 123. This completes the process at time t (k). Thereafter, the series of processing steps are repeatedly executed until the end of tracking is determined (tracking end step 13).
[0056]
As described above, the conventional target tracking device 10 using the Kalman filter and describing the Doppler shift term ΔT (dot) R (k) in the polar coordinate system in the observation model uses the assumed equidistant change rate motion model, Calculate the tracking track. FIG. 19 shows time transitions of the distance component (smooth value) of the smooth vector, the predicted observation vector (gate center), and the observation vector (observation value) using the conventional technique. The solid line in the figure represents the true trajectory of the distance component of the state vector, and the dotted line represents the true value of the observed vector affected by the Doppler shift. As shown in the figure, the prediction observation vector is a value obtained by shifting the prediction vector by the Doppler shift by using an observation matrix in consideration of the Doppler shift, and is calculated near the observation value affected by the Doppler shift. The distance component of the smooth vector is calculated near the true locus of the state vector.
[0057]
As described above, the conventional technique is effective when tracking is performed on the assumption that the target moves only in the distance direction. However, when the target motion model covers not only the one-dimensional space in the distance direction but also the three-dimensional space, it is difficult to configure a filter with the motion model and the observation model assumed only in the distance direction. In particular, if the target motion is defined in the North Reference Cartesian coordinate system, conventional motion and observation models cannot be used.
[0058]
Furthermore, when the conventional filter taking into account the Doppler shift is used from the start of tracking, the estimated value of the distance change rate fluctuates, and the estimated value of the distance change rate deteriorates. The filter essentially corrects the Doppler shift included in the observed value using the estimated value of the distance change rate, and calculates the estimated distance value. Therefore, the deterioration of the distance change rate estimation accuracy leads to the deterioration of the distance estimation accuracy. For the above reason, in order to prevent the deterioration of the distance estimation accuracy, it is conceivable that a filter that does not consider Doppler shift is used in the initial tracking, and the filter is switched after the distance change rate estimation accuracy is stabilized. FIG. 21 shows a conceptual diagram regarding the transition of the distance smoothed value, the predicted observation value, and the observation value before and after switching. The filter that does not consider the Doppler shift is a Kalman filter in which the observation matrix is configured using the following formula (22) instead of the formula (7).
[0059]
[Expression 17]

[0060]
In this case, since the correction for the Doppler shift is not performed, the correction for the Doppler shift does not affect the distance accuracy due to the deterioration of the distance change rate accuracy at the start of the filter. Here, the problem is that, as indicated by an arrow A, the gate center at the time of switching is calculated at a location away from the observed value, and the observed value may not fit in the gate. As shown in this figure, first, if a filter that does not consider the Doppler shift is used with the observation value affected by the Doppler shift as an input, the smooth position of the filter is shifted from the target true position. Next, if the predicted observation value is calculated using the smoothed value immediately after switching the filter, the predicted observation value (gate center) is calculated at a position shifted by the Doppler shift, and as a result, the predicted observation value and the observation value are calculated. The distance of the value becomes long. When the distance becomes short, there is a risk that the observation value cannot be captured by the gate when determining the gate.
[0061]
[Problems to be solved by the invention]
As described above, the conventional target tracking device can configure the tracking device when the target motion model is described in the distance direction of the polar coordinate system and the observation model is configured in the distance direction of the polar coordinate system. When the target motion is defined in the Cartesian coordinate system and the observation model is the same, it is difficult to accurately track the target with the conventional motion and observation model.
[0062]
Furthermore, when switching from a filter that does not consider Doppler shift to a filter that considers Doppler shift, the distance between the predicted observation value (gate center) and the observation value becomes short, and the observation value may not be captured in the gate.
[0063]
The present invention was made to solve the above problems, and by configuring the motion model and the observation model in the north reference orthogonal coordinate system, it is possible to configure the target tracking device in the orthogonal coordinate system, and Target tracking device and target tracking method that solves the problem of increasing the distance between the observed value and the gate center when switching from a filter that uses an observation model that does not consider Doppler shift to a filter that uses Doppler shift correction The purpose is to provide.
[0064]
[Means for Solving the Problems]
  In view of the above object, the present inventionBased on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including a coordinate component of a distance, an elevation angle, an azimuth angle, a distance change rate, and an elevation angle change rate obtained by converting a prediction vector that is a predicted value of the state vector into a polar coordinate system An observation matrix that takes into account the distance observation error due to the Doppler shift using the value of the azimuth change rate and the value of the Doppler shift time constant that represents the ratio of the distance observation error due to the Doppler shift effect to the target distance change rate A target tracking device characterized by comprising an observation matrix calculating means for calculating a target tracking method and a target tracking method related thereto.
[0074]
DETAILED DESCRIPTION OF THE INVENTION
The present invention will be described below according to each embodiment.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a target tracking apparatus according to Embodiment 1 of the present invention, and FIG. 2 is a flowchart showing the processing operation thereof. In FIG. 1, the target tracking device 10a includes a predicted observation vector calculation unit 101, a prediction vector calculation unit 103, a smooth vector memory 105, a gate determination unit 107, Vector calculation unit 109, residual covariance matrix calculation unit 111, gain matrix calculation unit 113, observation noise covariance matrix setting unit 115, prediction error covariance matrix calculation unit 117, drive noise covariance matrix setting unit 119, smooth error covariance A variance matrix calculation unit 121 and a smooth error covariance matrix memory 123 are provided. The target tracking device 10a includes an observation matrix calculation unit 125 in addition to the above components. The observation matrix calculation unit 125 calculates an observation matrix using the prediction value calculated by the prediction vector calculation unit 103.
[0075]
In FIG. 2, an observation matrix calculation step S5a is a step of calculating an observation matrix using a prediction vector. However, each processing unit in FIG. 5, the memory, and the processing steps in FIG. 2 are defined in the north reference orthogonal coordinate system without using the two-dimensional state vector using the distance and the distance change rate described in the prior art. Since a 6-dimensional state vector is used, the order of the vector and the matrix is different from the conventional technique.
[0076]
In the first embodiment, a mathematical model of a target and a sensor assumed in the Kalman filter theory will be described. In the Kalman filter theory, in order to estimate the target motion specifications, the target motion model and the sensor observation model are set as follows. First, the target motion model is expressed as the following formula (23).
[0077]
[Formula 18]

[0078]
In equation (23), (underbar) x (k) is an n-dimensional state vector representing the true value of the target motion specification at sampling time (hereinafter simply referred to as “time”) t (k), Ф (k−1) Is an n-by-n transition matrix representing the transition of the state vector from t (k-1) to t (k). Further, (underbar) w (k) is an n-dimensional driving noise vector at time t (k), and is a white noise sequence according to an n-variate normal distribution of mean (underbar) 0 and covariance matrix Q (k). The true value (underbar) x (k) of the target motion specification at time t (k) is defined in the north reference orthogonal coordinates in FIG. Assuming that the basic motion of the target is constant velocity motion, the state vector and transition matrix in the motion model of Equation (23) in the north reference Cartesian coordinates are as shown in Equation (24) and Equation (25) below. Set.
[0079]
[Equation 19]

[0080]
In Equation (23), Equation (24), and Equation (25), the drive noise vector (underbar) w (k) represents an error term associated with approximating the actual target motion to straight ahead at constant speed. Next, an observation model of the sensor is expressed as the following formula (26).
[0081]
[Expression 20]

[0082]
In Equation (26), (underbar) z (k) is an m-dimensional observation vector at time t (k), and H (k) is an observation matrix of m rows and n columns. Further, (underbar) v (k) is an m-dimensional observation vector at time t (k) corresponding to the observation vector (underbar) z (k), and the mean (underbar) 0 and the m variation of the covariance matrix R (k). This is a white noise sequence according to the quantity normal distribution. However, it is assumed that the observation noise vector (underbar) v (k) and the drive noise vector (underbar) w (k) are independent from each other.
[0083]
When the observation result of the target position in the north reference orthogonal standard system is obtained by the sensor, the observation matrix in the observation model of Equation (26) is set as follows.
[0084]
[Expression 21]

[0085]
[Expression 22]

[0086]
The observation matrix H (k) shown in the equation (27) is a matrix for describing observation values reflecting the influence of Doppler shift that occurs in the distance direction in the north reference orthogonal coordinate system. This represents a shift in the distance direction by ΔT (dot) R (k), which is the effect of Doppler shift. The observation matrix H (k) shown in Equation (27) has variables R (k) and E (k) obtained by converting the state vector (underbar) x (k) given in the north reference orthogonal coordinate system into the polar coordinate system. ), Az (k), (dot) R (k), (dot) E (k), and (dot) Az (k). Equations (31) and (32) represent equations for conversion from the north reference orthogonal coordinate system to polar coordinates. ΔT in Equation (28) is a time constant given in advance. However, since the state vector (underbar) x (k), which is the true target motion specification, is unknown, the observation matrix is calculated using the predicted value (upper and lower bars) x (k). To do.
[0087]
Hereinafter, the processing operation according to the first embodiment of the present invention shown in FIG. 1 will be described in detail with reference to FIG. First, in the smoothing vector memory 105 and the smoothing error covariance matrix memory 123, an initial value (underbar hat) x (0) of the smoothing vector and an initial value P (0) of the smoothing error covariance matrix are respectively stored. The tracking process is set (initial value setting step S1). Here, the state vector (underbar) x (k) is a six-dimensional vector of Expression (33) defined in the north reference orthogonal coordinate system.
[0088]
[Expression 23]

[0089]
The smoothing error covariance matrix is a 6 × 6 matrix. Thereafter, processing at each time of k = 1, 2,... Is executed. In the process at time t (k), when an observation vector (underbar) z (k) is input from the target observation device 20 (observation vector input step S2), the prediction vector calculation unit 103 reads from the smooth vector memory 105. The smooth vector (underbar hat) x (k−1) at the previous time t (k−1) is read out, and the prediction vector is calculated according to the following equation (34) (prediction vector calculation step S3).
[0090]
[Expression 24]

[0091]
Here, the observation vector (underbar) z (k) is a three-dimensional vector obtained in the north reference orthogonal coordinate system affected by the Doppler shift. The state transition matrix is given by the matrix of Expression (35).
[0092]
[Expression 25]

[0093]
Next, the driving noise covariance matrix setting unit 119 sets the driving noise covariance matrix Q (k−1) (driving noise covariance matrix setting step S4). Normally, the value of the driving noise covariance matrix is set to the size of the sampling interval t (k) −t (k−1) in consideration of the maximum target maneuverability assumed in the operation of the target tracking device 10a. Will be decided accordingly. Next, the prediction error covariance matrix calculation unit 117 reads the smoothing error covariance matrix P (k−1) at the previous time t (k−1) from the smoothing error covariance matrix memory 123, and the driving noise covariance matrix is read out. Using the variance matrix Q (k−1), a prediction error covariance matrix is calculated according to the following equation (36) (prediction error covariance matrix calculation step S5).
[0094]
[Equation 26]

[0095]
Next, the observation matrix calculation unit 125 calculates an observation matrix from the prediction vector (upper and lower bars) x (k) output from the prediction vector calculation unit 103 (observation matrix calculation step S5a).
[0096]
[Expression 27]

[0097]
[Expression 28]

[0098]
The observation matrix H (k) shown in the equation (37) is a matrix for describing the observation values reflecting the influence of the Doppler shift occurring in the distance direction in the north reference orthogonal coordinate system. This represents a shift in the distance direction by ΔT (dot) R (k), which is the effect of Doppler shift. The observation matrix H (k) shown in Expression (37) is a variable (bar) R (k) obtained by converting a prediction vector (upper and lower bars) x (k) given in the north reference orthogonal coordinate system into a polar coordinate system. , (Bar) E (k), (bar) Az (k), (bar / dot) R (k), (bar / dot) E (k), (bar / dot) Az (k) Is done. Equations (41) and (42) represent equations for conversion from the north reference orthogonal coordinate system to polar coordinates. ΔT in Equation (38) is a time constant given in advance.
[0099]
The predicted observation vector calculation unit 101 uses the prediction vector (upper and lower bars) x (k) input from the prediction vector calculation unit 103 and the observation matrix input from the observation matrix calculation unit 125 according to the following equation (43). A predicted observation vector (upper and lower bars) z (k) is calculated (predicted observation vector calculation step S6).
[0100]
[Expression 29]

[0101]
Further, the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) of the observation vector (underbar) z (k) input from the target observation device 20, and a residual covariance matrix calculation unit. 111 and the smoothing error covariance matrix calculation unit 121 (observation noise covariance matrix setting step S7). Note that the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) using parameters related to the accuracy evaluation of the observation vector (underbar) z (k). For example, when the target observation device 20 is a radar that detects a target in the x-, y-, and z-axis directions in the north reference orthogonal coordinate system, the observation noise covariance matrix R (k) is based on information on the observation accuracy of the angular axis of the radar. Can be set. Here, the observation accuracy can be estimated based on the intensity (or signal-to-noise ratio) of the target signal detected by the radar at time t (k).
[0102]
Next, the residual covariance matrix calculation unit 111 reads the observation noise covariance matrix R (k), the prediction error covariance matrix (bar) P (k), and the observation matrix H (k), and the following equation (44) ), The residual covariance matrix S (k) is calculated and transmitted to the gate determination unit 107 and the gain matrix calculation unit 113 (residual covariance matrix calculation step S8).
[0103]
[30]

[0104]
The gate determination unit 107 uses the predicted observation vector (upper and lower bars) z (k) input from the predicted observation vector calculation unit 101 to calculate a residual for the observation vector (underbar) z (k) according to the following equation (45). After calculating the vector, the residual covariance matrix S (k) is read, and a gate determination process is executed according to the following equation (46) (gate determination step S9).
[0105]
[31]

[0106]
However, in Formula (46), d is a parameter. Note that the gate used by the gate determination unit 107 is a spatial range centered on the predicted observation vector (upper and lower bars) z (k), and is a spatial range in which the observation vector from the tracking target can be captured with a desired probability. . The size and shape of the gate include a prediction error covariance matrix (bar) P (k) that is an evaluation value of a prediction vector error, an observation noise covariance matrix R (k) that is an evaluation value of an observation error, and an observation matrix It is determined by the residual covariance matrix S (k) of the above equation (44) calculated from the observation matrix H (k) calculated by the calculation unit 125. Equation (46) is a reference condition for determining whether or not an observation vector from the target observation device 20 exists in the gate. For example, when the residual vector (underbar) r (k) satisfies Equation (46), the gate determination unit 107 uses the observation vector (underbar) z (k) input from the target observation device 20 as the tracking target detection result. The observation vector (underbar) z (k) determined to be from the tracking target is sent to the smooth vector calculation unit 109. On the other hand, when an observation vector that satisfies Equation (46) is not obtained from the target observation device 20, the gate determination unit 107 regards that the observation target observation vector has not been detected, and the observation to the smooth vector calculation unit 109 is performed. Do not send vectors.
[0107]
Next, the gain matrix calculation unit 113 reads the prediction error covariance matrix (bar) P (k), the residual covariance matrix S (k), and the observation matrix H (k), and the following equation (47) is read. Therefore, the filter gain is calculated (gain matrix calculation step S10).
[0108]
[Expression 32]

[0109]
In addition, when the observation vector (under bar) z (k) is input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x (k), predicted prediction vector (up and down bar) z ( k) and the gain matrix K (k) are read, and a smooth vector is calculated according to the following equation (48).
[0110]
[Expression 33]

[0111]
In addition, when the observation vector (underbar) z (k) is not input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the predicted vector (upper and lower bars) x ( k) is a smooth vector (smooth vector calculation step S11).
[0112]
[Expression 34]

[0113]
The smoothed vector (underbar hat) x (k) calculated by the smoothed vector calculating unit 109 according to the above formula (48) or formula (49) is stored in the smooth vector memory 105 and also stored in the wake display device 30. Is transmitted and displayed to the operator.
[0114]
On the other hand, when the gate determination unit 107 determines that the tracking target observation vector has been obtained, the smooth error covariance matrix calculation unit 121 determines the prediction error covariance matrix (bar) P (k) and the observation noise covariance. Using the matrix R (k) and the observation matrix H (k), a smooth error covariance matrix is calculated by the following equation (50), and it is determined by the gate determination unit 107 that the observation target observation vector has not been obtained. In this case, as shown in the following formula (51), the prediction error covariance matrix (bar) P (k) is set as a smoothing error covariance matrix (smooth error covariance matrix calculation step S12).
[0115]
[Expression 35]

[0116]
The calculation result of the smoothing error covariance matrix according to Equation (50) and Equation (51) is stored in the smoothing error covariance matrix memory 123.
[0117]
This completes the process at time t (k). Thereafter, the above series of processing steps are repeatedly executed until the end of tracking is determined (tracking end step S13).
[0118]
As described above, the target tracking device 10a according to the first embodiment that uses the Kalman filter and describes the Doppler shift term in the observation model in the north reference orthogonal coordinate system uses the equidistant linear motion model assumed in advance, calculate. FIG. 3 shows time transitions of the position component (smooth value) of the smooth vector, the predicted observation vector (gate center), and the observation vector (observation value) using the first embodiment. The solid line in the figure represents the true trajectory of the position component of the state vector, and the dotted line represents the true value of the observation vector affected by the Doppler shift. As shown in the figure, the prediction observation vector is a value obtained by shifting the prediction vector by the Doppler shift by using an observation matrix in consideration of the Doppler shift, and is calculated near the observation value affected by the Doppler shift. The distance component of the smooth vector is calculated near the true locus of the state vector.
[0119]
In this way, in order to configure a filter that takes into account Doppler shift correction in the North Reference Cartesian coordinate system, the observation model in the North Reference Cartesian coordinate system is used instead of the observation model expressed by the polar coordinate system distance axis used in the prior art. The Doppler shift correction can be performed in the north reference orthogonal coordinate system.
[0120]
Embodiment 2. FIG.
As shown in the first embodiment, the target tracking device that uses the Kalman filter and describes the Doppler shift term in the observation model in the north reference Cartesian coordinate system uses the constant velocity linear motion model assumed in advance, and uses the tracking track. calculate. However, if the filter considering the Doppler shift of the first embodiment is used from the start of tracking, the estimated values of speed (dot) x, (dot) y, and (dot) z fluctuate. Degradation of the distance change rate component obtained by displaying occurs. The filter essentially corrects the Doppler shift included in the observed value using the estimated value of the distance change rate, and calculates the estimated distance value. Therefore, the deterioration of the distance change rate estimation accuracy leads to the deterioration of the distance estimation accuracy. For the above reasons, in order to prevent the degradation of the distance estimation accuracy, a distance change rate obtained from speed (dot) x, (dot) y, (dot) z estimated values is used in the initial tracking period using a filter that does not consider Doppler shift. It is conceivable that the filter is switched after the estimation accuracy is stabilized. FIG. 21 shows a conceptual diagram regarding the transition of the position smooth value, the predicted observation value, and the observation value before and after switching. The filter that does not consider the Doppler shift is a Kalman filter in which the observation matrix is configured by using the following formula (52) instead of the formula (37).
[0121]
[Expression 36]

[0122]
In this case, since the correction for the Doppler shift is not performed, the correction for the Doppler shift does not affect the distance accuracy due to the deterioration of the distance change rate accuracy at the start of the filter. Here, the problem is that the gate center at the time of switching is calculated at a position away from the observed value, and the observed value may not fit in the gate. As shown in FIG. 21, first, when a filter that does not consider the Doppler shift is used with an observation value affected by the Doppler shift as an input, the smooth position of the filter is calculated with a deviation from the target true position. Next, if the predicted observation value is calculated using the smooth value immediately after the filter switching, the predicted observation value is calculated at a position shifted by the Doppler shift, and as a result, the distance between the predicted observation value and the observation value is It will be long. When the distance becomes short, there is a risk that the observation value cannot be captured by the gate when determining the gate. To solve this problem, solve the problem that the distance between the observed value and the gate center increases when switching from a filter that uses an observation model that does not consider Doppler shift to a filter that uses an observation model that considers Doppler shift correction. The purpose is to do.
[0123]
FIG. 4 is a block diagram showing a target tracking apparatus according to Embodiment 2 of the present invention, and FIG. 5 is a flowchart showing the processing operation. Detailed descriptions of those similar to those in FIGS. 1 and 2 are omitted. FIG. 6 is a conceptual diagram showing the effect in the second embodiment of the present invention.
[0124]
In FIG. 4, the target tracking device 10 a is similar to FIG. 1 illustrating the first embodiment, and includes a smooth vector memory 105, a prediction vector calculation unit 103, a gate determination unit 107, a smooth vector calculation unit 109, and a residual covariance matrix calculation. Unit 111, gain matrix calculation unit 113, observation noise covariance matrix setting unit 115, prediction error covariance matrix calculation unit 117, drive noise covariance matrix setting unit 119, smoothing error covariance matrix calculating unit 121, smoothing error covariance matrix A memory 123 is provided.
[0125]
In addition to the above-described components, the target tracking device 10a includes a switching control unit 127, an observation matrix calculation unit 125a (operation is different from that in FIG. 1 of Embodiment 1), and a predicted observation vector calculation unit 131 for smooth vector calculation. A predictive vector calculation unit for gate 129 and a pseudo smoothing value calculation unit 133 immediately before switching. The switching control unit 127 calculates the switching timing from the filter configured with the observation model that does not consider the Doppler shift to the filter configured with the observation model that considers the Doppler shift, and the pseudo-smooth value calculation immediately before the switching. Output to the unit 133.
[0126]
The observation matrix calculation unit 125a receives the filter switching time information from the switching control unit 127, and the prediction calculated by the prediction vector calculation unit 103 after the time of switching to the filter configured by the observation model considering the Doppler shift. Using the values, calculate an observation matrix considering the Doppler shift. Before the switching time, an observation model that does not consider Doppler shift is calculated.
The predicted observation vector calculation unit 131 for smooth vector calculation uses a predicted observation vector used by the smooth vector calculation unit 109 based on the smooth vector at one time from the smooth vector memory 105 and the observation matrix input from the observation matrix calculation unit 125a. Is calculated.
[0127]
The gate prediction observation vector calculation unit 129 calculates a prediction observation vector used by the gate determination unit 107 using the prediction vector that is an input from the prediction vector calculation unit 103 and the observation matrix that is an input from the observation matrix calculation unit 125a. To do.
[0128]
The pseudo-smooth value calculation unit 133 immediately before the switching obtains the filter switching time information from the switching control unit 127. When the current time is the time for switching, the pseudo-smooth value calculation unit 133 uses the smoothing vector one time before from the smoothing vector memory 105. Then, a pseudo smoothing value is calculated. If it is not time to switch, the smoothed value one hour before is output as it is.
[0129]
In FIG. 5, as in FIG. 2, which is a flowchart showing the processing operation of the first embodiment, initial value setting step S1, observation vector input step S2, prediction vector calculation step S3, drive noise covariance matrix setting Step S4, Prediction error covariance matrix calculation step S5, Observation noise covariance matrix setting step S7, Residual covariance matrix calculation step S8, Gate determination step S9, Gain matrix calculation step S10, Smooth vector calculation step S11, a smoothing error covariance matrix calculation step S12 is provided.
[0130]
FIG. 5 shows a pseudo-smooth value calculation step S2a immediately before switching, a gate predicted observation vector calculation step 5b, a smooth vector calculation predicted observation vector calculation step 9a, and an observation matrix calculation step 5aa in addition to the above processing steps. (The processing is different from that of FIG. 2 showing the processing operation of the first embodiment).
[0131]
In FIG. 5, the pseudo-smooth value calculation step S2a immediately before the switching obtains the filter switching time information from the switching control unit 127, and when the current time is the time for switching, the step S2a from the smoothing vector memory 105 is one time before. This is a step of calculating a pseudo-smooth value using a smooth vector. If the current time is not the time for switching, the smooth vector at one time from the smooth vector memory 105 is output as it is.
[0132]
Step S5b of calculating a predicted observation vector for gate is a step of calculating a predicted observation vector used for gate determination using the prediction vector calculated by the prediction vector calculation unit 103 and the observation matrix calculated by the observation matrix calculation unit 125a. It is.
[0133]
The prediction step S9a for calculating the smoothed vector calculation predicted observation vector uses the smoothed vector from the smoothed vector memory 105 at the previous time and the observation matrix calculated by the observation matrix calculating unit 125a to perform prediction for calculating the smooth vector. This is a step of calculating an observation vector.
[0134]
In the observation matrix calculation step S5aa, the filter switching time information from the switching control unit 127 is input, and the prediction calculated by the prediction vector calculation unit 103 is performed after the time when the filter is configured by the observation model considering the Doppler shift. Before the switching is performed by calculating the observation matrix using the values, this is a step of outputting an observation model that does not consider the Doppler shift.
[0135]
Hereinafter, the processing operation according to the second embodiment will be described in detail with reference to FIGS. 4 and 5. First, in the smoothing vector memory 105 and the smoothing error covariance matrix memory 123, an initial value (underbar hat) x (0) of the smoothing vector and an initial value P (0) of the smoothing error covariance matrix are respectively stored. The tracking process is set (initial value setting step S1). Here, the state vector (underbar) x (k) is a six-dimensional vector of Expression (53) defined in the north reference orthogonal coordinate system.
[0136]
[Expression 37]

[0137]
The smoothing error covariance matrix is a 6 × 6 matrix. Thereafter, processing at each time of k = 1, 2,... Is executed. In the process at time t (k), an observation vector (underbar) z (k) is input from the target observation device 20 (observation vector input step S2). Here, the observation vector (underbar) z (k) is a three-dimensional vector obtained in the north reference orthogonal coordinate system affected by the Doppler shift. Next, in the calculation step 2a of the pseudo smoothing value immediately before the switching, first, the smoothing vector (underbar hat) x (k-1) at the previous time t (k-1) is read from the smoothing vector memory 105. Next, filter switching time information is obtained from the switching control unit 127. When the filter switching time is before and after the switching time, the pseudo-smooth value (underbar hat) x one time before, as shown in Equation (54) '(k-1) is set to a smooth value one hour before.
[0138]
[Formula 38]

[0139]
When the current time is the switching time, pseudo-smooth vector (underbar hat) x ′ (k−1) according to the following formulas (55), (56), (57), (58), and (59) Is calculated.
[0140]
[39]

[0141]
[Formula 40]

[0142]
Here, Expression (55) is a conversion expression from the north reference orthogonal coordinate system to the polar coordinate system for the distance change rate, and Expression (56) is a conversion from the North reference orthogonal coordinate system to the polar coordinate system for the position. Equation (58) is a conversion equation from the polar coordinate system to the north reference orthogonal coordinate system. Equation (57) is a process for correcting the Doppler shift for the distance component of the smooth value affected by the Doppler shift. Equation (57) means that the smoothing error in the distance direction due to the use of a filter configured with an observation model that does not consider the Doppler shift is corrected for the distance component of the smooth value affected by the Doppler shift. .
[0143]
Next, a prediction vector is calculated according to the following formula (60) (prediction vector calculation step S3).
[0144]
[Expression 41]

[0145]
The state transition matrix is given by the matrix of Equation (61).
[0146]
[Expression 42]

[0147]
Next, the driving noise covariance matrix setting unit 119 sets the driving noise covariance matrix Q (k−1) (driving noise covariance matrix setting step S4). Normally, the value of the driving noise covariance matrix is set to the size of the sampling interval t (k) −t (k−1) in consideration of the maximum target maneuverability assumed in the operation of the target tracking device 10a. Will be decided accordingly. Next, the prediction error covariance matrix calculation unit 117 reads the smoothing error covariance matrix P (k−1) at the previous time t (k−1) from the smoothing error covariance matrix memory 123, and the driving noise covariance matrix is read out. Using the variance matrix Q (k−1), a prediction error covariance matrix is calculated according to the following formula (62) (prediction error covariance matrix calculation step S5).
[0148]
[Expression 43]

[0149]
Next, in the observation matrix calculation step S5aa, information on the filter switching time is obtained from the switching control unit 127, and the observation matrix is output as the following equation (64) before the switching time.
[0150]
(44)

[0151]
Where I_3x3Is a 3-by-3 identity matrix, 0_3x3Is a 3-by-3 zero matrix. After the switching time, the observation matrix calculation unit 125a uses the prediction vector (upper and lower bars) x output from the prediction vector calculation unit 103._hAn observation matrix is calculated from (k) (observation matrix calculation step S5aa).
[0152]
[Equation 45]

[0153]
[Equation 46]

[0154]
The observation matrix H (k) shown in the above equation (65) is a matrix for describing the observation values reflecting the influence of the Doppler shift occurring in the distance direction in the north reference orthogonal coordinate system. This represents a shift in the distance direction by ΔT (dot) R (k), which is the effect of Doppler shift. The observation matrix H (k) shown in Equation (65) is a prediction vector (upper and lower bars) x given by the north reference orthogonal coordinate system._hVariables (bar) R (k), (bar) E (k), (bar) Az (k), (bar dot) R (k), (bar dot) E converted from (k) to polar coordinate system It is calculated using (k), (bar dot) Az (k). Equations (69) and (70) represent equations for conversion from the north reference orthogonal coordinate system to polar coordinates. ΔT in the equation (66) is a time constant given in advance.
[0155]
In the gate predicted observation vector calculation step 5 b, the gate predicted observation vector calculation unit 129 includes the prediction vector (upper and lower bars) x input from the prediction vector calculation unit 103._hUsing (k) and the observation matrix input from the observation matrix calculation unit 125a, the predicted observation vector (upper and lower bars) z according to the following equation (71):_g(k) is calculated (prediction observation vector calculation step S5b).
[0156]
[Equation 47]

[0157]
Further, the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) of the observation vector (underbar) z (k) input from the target observation device 20, and a residual covariance matrix calculation unit. 111 and the smoothing error covariance matrix calculation unit 121 (observation noise covariance matrix setting step S7). Note that the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) using parameters related to the accuracy evaluation of the observation vector (underbar) z (k). For example, when the target observation device 20 is a radar that detects a target in the x-, y-, and z-axis directions in the north reference orthogonal coordinate system, the observation noise covariance matrix R (k) is based on information on the observation accuracy of the angular axis of the radar. Can be set. Here, the observation accuracy can be estimated based on the intensity (or signal-to-noise ratio) of the target signal detected by the radar at time t (k).
[0158]
Next, the residual covariance matrix calculation unit 111 reads the observation noise covariance matrix R (k), the prediction error covariance matrix (bar) P (k), and the observation matrix H (k), and the following equation (72) ), The residual covariance matrix S (k) is calculated and transmitted to the gate determination unit 107 and the gain matrix calculation unit 113 (residual covariance matrix calculation step S8).
[0159]
[Formula 48]

[0160]
The gate determination unit 107 includes a predicted observation vector (upper and lower bars) z input from the predicted observation vector calculation unit 129._g(k) is used to calculate a residual vector for the observed vector (underbar) z (k) by the following equation (73), and then the residual covariance matrix S (k) is read. Thus, a gate determination process is executed (gate determination step S9).
[0161]
[Equation 49]

[0162]
However, in Formula (74), d is a parameter. Note that the gate used by the gate determination unit 107 is the predicted observation vector (up / down bar) z._gIt is a spatial range centering on (k) and is a spatial range in which an observation vector from the tracking target can be captured with a desired probability. The size and shape of the gate include a prediction error covariance matrix (bar) P (k) that is an evaluation value of a prediction vector error, an observation noise covariance matrix R (k) that is an evaluation value of an observation error, and an observation matrix It is determined by the residual covariance matrix S (k) of the above equation (72) calculated from the observation matrix H (k) calculated by the calculation unit 125a. The mathematical formula (74) is a reference condition for determining whether or not an observation vector from the target observation device 20 exists in the gate. For example, when the residual vector (underbar) r (k) satisfies Equation (74), the gate determination unit 107 uses the observation vector (underbar) z (k) input from the target observation device 20 as the tracking target detection result. The observation vector (underbar) z (k) determined to be from the tracking target is sent to the smooth vector calculation unit 109. On the other hand, when the observation vector satisfying the equation (74) is not obtained from the target observation device 20, the gate determination unit 107 regards that the observation target observation vector has not been detected, and the observation to the smooth vector calculation unit 109 is performed. Do not send vectors.
[0163]
In the calculation step S9a of the predicted observation vector for smooth vector calculation, the predicted observation vector output to the smooth vector calculation unit 109 is calculated by the following formulas (75) and (76).
[0164]
[Equation 50]

[0165]
Predicted observation vector (upper and lower bars) z for smooth vector calculation_h(k) is used by the smooth vector calculation unit 109 to calculate a residual that is a difference from the observed value.
[0166]
Next, the gain matrix calculation unit 113 reads the prediction error covariance matrix (bar) P (k), the residual covariance matrix S (k), and the observation matrix H (k), and the following equation (77) is read. Therefore, the filter gain is calculated (gain matrix calculation step S10).
[0167]
[Formula 51]

[0168]
In addition, the smooth vector calculation unit 109 receives a prediction vector (upper and lower bars) x when an observation vector (underbar) z (k) is input from the gate determination unit 107._h(k), predicted observation vector (up / down bar) z_h(k) and the gain matrix K (k) are read, and a smooth vector is calculated according to the following equation (78).
[0169]
[Formula 52]

[0170]
In addition, when the observation vector (underbar) z (k) is not input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x as shown in the following equation (79)._hLet (k) be a smooth vector (smooth vector calculation step S11).
[0171]
[Equation 53]

[0172]
The smoothed vector (underbar hat) x (k) calculated by the smoothed vector calculating unit 109 according to the above formula (78) or formula (79) is stored in the smooth vector memory 105 and also stored in the wake display device 30. Is transmitted and displayed to the operator.
[0173]
On the other hand, when the gate determination unit 107 determines that the tracking target observation vector has been obtained, the smooth error covariance matrix calculation unit 121 determines the prediction error covariance matrix (bar) P (k) and the observation noise covariance. Using the matrix R (k) and the observation matrix H (k), a smooth error covariance matrix is calculated by the following equation (80), and it is determined by the gate determination unit 107 that the observation target observation vector has not been obtained. In this case, as shown in the following equation (81), the prediction error covariance matrix (bar) P (k) is set as a smooth error covariance matrix (smooth error covariance matrix calculation step S12).
[0174]
[Formula 54]

[0175]
The calculation result of the smoothing error covariance matrix according to Equation (80) and Equation (81) is stored in the smoothing error covariance matrix memory 123.
[0176]
This completes the process at time t (k). Thereafter, the above series of processing steps are repeatedly executed until the end of tracking is determined (tracking end step S13).
[0177]
FIG. 6 shows time transitions of the position component (smooth value) of the smooth vector, the predicted observation vector (gate center), and the observation vector (observation value) using the second embodiment. The solid line in the figure represents the true trajectory of the position component of the state vector, and the dotted line represents the true value of the observation vector affected by the Doppler shift. As a result of the above processing steps, as shown in FIG. 6, when switching from a filter composed of an observation model that does not consider Doppler shift to a filter that considers an observation model that considers Doppler shift, the prediction that is the center of the gate In order to improve the problem that the distance between the observation vector and the observation vector is increased, the pseudo smoothing vector is calculated by performing Doppler correction on the smoothing vector at the previous time and calculating the pseudo smoothing vector in advance at the time of switching. This has the effect of reducing the distance between the calculated predicted observation vector for gate and the observation vector. Further, unlike the predicted observation vector for gate calculated above, the predicted observation vector value calculated from the predicted vector affected by the Doppler shift is used as the predicted observation vector when calculating the smooth vector. Has an effect of increasing the convergence speed of the transient response of the smooth vector at the time of switching.
[0178]
Note that it is obvious that using the idea of the present embodiment, it is possible to cope with switching from a filter configured with an observation model considering Doppler shift to a filter using an observation model not considering Doppler shift. Therefore, explanation is omitted.
[0179]
Embodiment 3 FIG.
FIG. 7 is a block diagram showing a target tracking device according to Embodiment 3 of the present invention, and FIG. 8 is a flowchart showing the processing operation. Detailed descriptions of those similar to those in FIGS. 4 and 5 are omitted. FIG. 9 is a conceptual diagram showing the effect in the third embodiment of the present invention.
[0180]
In FIG. 7, the target tracking device 10 a is similar to FIG. 4, which is a block configuration diagram showing the second embodiment, and includes a smooth vector memory 105, a prediction vector calculation unit 103, a gate prediction observation vector calculation unit 129, and a gate determination. Unit 107, smooth vector calculation unit 109, residual covariance matrix calculation unit 111, gain matrix calculation unit 113, switching control unit 127, observation noise covariance matrix setting unit 115, prediction error covariance matrix calculation unit 117, drive noise covariance A variance matrix setting unit 119, a smoothing error covariance matrix calculating unit 121, and a smoothing error covariance matrix memory 123 are provided.
[0181]
In addition to the above components, the target tracking device 10a includes an observation matrix calculation unit 125b (operation is different from that in FIG. 4 of the second embodiment), a predicted observation vector calculation unit 131a for smooth vector calculation (second embodiment). The operation is different from that in FIG.
[0182]
The observation matrix calculation unit 125b receives the filter switching time information from the switching control unit 127, and is calculated by the prediction vector calculation unit 103 after the time of switching to the filter configured by the observation model considering the Doppler shift. By using the predicted value, an observation matrix taking into account the Doppler shift is calculated, and a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131a, and a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. Prior to the switching time, an observation model that does not consider Doppler shift is calculated, a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131a, and a smooth error covariance. The data is output to the matrix calculation unit 121 and the gain matrix calculation unit 113. At the time of switching, an observation model that does not consider the Doppler shift is calculated and output to the gate predicted observation vector calculation unit 129 and the residual covariance matrix calculation unit 111, and the smoothed vector calculation predicted observation vector calculation unit 131a An observation matrix considering the Doppler shift is calculated and output to the dispersion matrix calculation unit 121 and the gain matrix calculation unit 113.
The predicted observation vector calculation unit 131a for smooth vector calculation calculates a predicted observation vector used by the smooth vector calculation unit 109 from the prediction vector from the prediction vector calculation unit 103 and the observation matrix that is an input from the observation matrix calculation unit 125b.
[0183]
In FIG. 8, as in FIG. 5, which is a flowchart showing the processing operation of the second embodiment, initial value setting step S1, observation vector input step S2, prediction vector calculation step S3, and drive noise covariance matrix setting Step S4, prediction error covariance matrix calculation step S5, gate predicted observation vector calculation step S5b, observation noise covariance matrix setting step S7, residual covariance matrix calculation step S8, gate determination step S9, gain matrix Calculation step S10, a smooth vector calculation step S11, and a smooth error covariance matrix calculation step S12.
[0184]
In addition to the above processing steps, FIG. 8 is a step S9aa for calculating a predicted observation vector for smooth vector calculation (processing is different from that in FIG. 5 showing the processing operation of the second embodiment), and an observation matrix calculating step S5aaa (implementation) The processing operation of mode 2 is different from that of FIG. 5).
[0185]
In FIG. 8, a predicted observation vector calculation step S9aa for calculating a smooth vector uses the prediction vector from the prediction vector calculation unit 103 and the observation matrix calculated by the observation matrix calculation unit 125b to perform prediction used for calculating a smooth vector. This is a step of calculating an observation vector.
[0186]
The observation matrix calculation step S5aaa receives the filter switching time information from the switching control unit 127, and is calculated by the prediction vector calculation unit 103 after the time of switching to the filter configured by the observation model considering the Doppler shift. By using the predicted value, an observation matrix taking into account the Doppler shift is calculated, and a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131a, and a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. Prior to the switching time, an observation model that does not consider Doppler shift is calculated, a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131a, and a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. At the time of switching, an observation model that does not consider Doppler shift is calculated and output to the gate predicted observation vector calculation unit 129 and the residual covariance matrix calculation unit 111, and the smoothed vector calculation predicted observation vector calculation unit 131a, smoothing error is calculated. In this step, the covariance matrix calculation unit 121 and the gain matrix calculation unit 113 calculate and output an observation matrix considering the Doppler shift.
[0187]
Hereinafter, the processing operation according to the third embodiment will be described in detail with reference to FIGS. 7 and 8. First, in the smoothing vector memory 105 and the smoothing error covariance matrix memory 123, an initial value (underbar hat) x (0) of the smoothing vector and an initial value P (0) of the smoothing error covariance matrix are respectively stored. The tracking process is set (initial value setting step S1). Here, the state vector (underbar) x (k) is a six-dimensional vector of Formula (82) defined by the north reference orthogonal coordinate system.
[0188]
[Expression 55]

[0189]
The smoothing error covariance matrix is a 6 × 6 matrix. Thereafter, processing at each time of k = 1, 2,... Is executed. In the process at time t (k), an observation vector (underbar) z (k) is input from the target observation device 20 (observation vector input step S2). Here, the observation vector (underbar) z (k) is a three-dimensional vector obtained in the north reference orthogonal coordinate system affected by the Doppler shift. First, in the prediction vector calculation step S3, the smooth vector (underbar hat) x (k-1) at the previous time t (k-1) is read from the smooth vector memory 105 and predicted according to the following equation (83). A vector is calculated (prediction vector calculation step S3).
[0190]
[56]

[0191]
The state transition matrix is given by the matrix of Equation (84).
[0192]
[Equation 57]

[0193]
Next, the driving noise covariance matrix setting unit 119 sets the driving noise covariance matrix Q (k−1) (driving noise covariance matrix setting step S4). Normally, the value of the driving noise covariance matrix is set to the size of the sampling interval t (k) −t (k−1) in consideration of the maximum target maneuverability assumed in the operation of the target tracking device 10a. Will be decided accordingly. Next, the prediction error covariance matrix calculation unit 117 reads the smoothing error covariance matrix P (k−1) at the previous time t (k−1) from the smoothing error covariance matrix memory 123, and the driving noise covariance matrix is read out. Using the variance matrix Q (k−1), a prediction error covariance matrix is calculated according to the following equation (85) (prediction error covariance matrix calculation step S5).
[0194]
[Formula 58]

[0195]
Next, in the observation matrix calculation step S5aaa, information about the filter switching time is obtained from the switching control unit 127. Before the switching time, the observation matrix is set as the following mathematical formula (86), and the gate predicted observation vector calculation unit 129, the residual covariance matrix calculation unit 111, the predicted observation vector calculation unit 131 a for smooth vector calculation, the smooth error covariance matrix calculation unit 121, and the gain matrix calculation unit 113.
[0196]
[Formula 59]

[0197]
Where I_3x3Is a 3-by-3 identity matrix, 0_3x3Is a 3-by-3 zero matrix. After the switching time, the observation noise matrix calculation unit calculates the prediction vector (upper and lower bars) x output from the prediction vector calculation unit 103._hFrom (k), the following equation (87), equation (88), equation (89), equation (90), equation (91), and equation (92) are used to calculate an observation matrix, and a gate predicted observation vector calculation unit 129, the residual covariance matrix calculation unit 111, the predicted observation vector calculation unit 131 a for smooth vector calculation, the smooth error covariance matrix calculation unit 121, and the gain matrix calculation unit 113.
[0198]
[Expression 60]

[0199]
[Equation 61]

[0200]
The observation matrix H (k) shown in the equation (87) is a matrix for describing the observation value reflecting the influence of the Doppler shift occurring in the distance direction in the north reference orthogonal coordinate system. This represents a shift in the distance direction by ΔT (dot) R (k), which is the effect of Doppler shift. The observation matrix H (k) shown in Equation (87) is a prediction vector (upper and lower bars) x given by the north reference orthogonal coordinate system._hVariables (bar) R (k), (bar) E (k), (bar) Az (k), (bar dot) R (k), (bar dot) E converted from (k) to polar coordinate system It is calculated using (k), (bar dot) Az (k). Expressions (91) and (92) represent expressions for conversion from the north reference orthogonal coordinate system to the polar coordinates. ΔT in Equation (88) is a time constant given in advance.
[0201]
In the case of the switching time, the observation matrix calculation unit 125b outputs the observation matrix as the mathematical formula (86) to the gate predicted observation vector calculation unit 129 and the residual covariance matrix calculation unit 111, and is output from the prediction vector calculation unit 103. Predicted vector (up / down bar) x_hFrom (k), the observation matrix is calculated by the mathematical formula (87), the mathematical formula (88), the mathematical formula (89), the mathematical formula (90), the mathematical formula (91), and the mathematical formula (92). 131a, the smoothing error covariance matrix calculating unit 121, and the gain matrix calculating unit 113.
[0202]
In the gate predicted observation vector calculation step 5 b, the gate predicted observation vector calculation unit 129 includes the prediction vector (upper and lower bars) x input from the prediction vector calculation unit 103._hUsing (k) and the observation matrix input from the observation matrix calculation unit 125b, the predicted observation vector (upper and lower bars) z according to the following equation (93):_g(k) is calculated (prediction observation vector calculation step 5b).
[0203]
[62]

[0204]
Further, the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) of the observation vector (underbar) z (k) input from the target observation device 20, and a residual covariance matrix calculation unit. 111 and the smoothing error covariance matrix calculation unit 121 (observation noise covariance matrix setting step S7). Note that the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) using parameters related to the accuracy evaluation of the observation vector (underbar) z (k). For example, when the target observation device 20 is a radar that detects a target in the x-, y-, and z-axis directions in the north reference orthogonal coordinate system, the observation noise covariance matrix R (k) is based on information on the observation accuracy of the angular axis of the radar. Can be set. Here, the observation accuracy can be estimated based on the intensity (or signal-to-noise ratio) of the target signal detected by the radar at time t (k).
[0205]
Next, the residual covariance matrix calculation unit 111 reads the observation noise covariance matrix R (k), the prediction error covariance matrix (bar) P (k), and the observation matrix H (k), and the following equation (94) ), The residual covariance matrix S (k) is calculated and transmitted to the gate determination unit 107 and the gain matrix calculation unit 113 (residual covariance matrix calculation step S8).
[0206]
[Equation 63]

[0207]
The gate determination unit 107 uses the predicted observation vector (upper and lower bars) z (k) input from the predicted observation vector calculation unit 129, and calculates the residual with respect to the observation vector (underbar) z (k) according to the following equation (95). After calculating the vector, the residual covariance matrix S (k) is read, and gate determination processing is executed by the following equation (96) (gate determination step S9).
[0208]
[Expression 64]

[0209]
However, in Formula (96), d is a parameter. Note that the gate used by the gate determination unit 107 is the predicted observation vector (up / down bar) z._gIt is a spatial range centering on (k) and is a spatial range in which an observation vector from the tracking target can be captured with a desired probability. The size and shape of the gate include a prediction error covariance matrix (bar) P (k) that is an evaluation value of a prediction vector error, an observation noise covariance matrix R (k) that is an evaluation value of an observation error, and an observation matrix It is determined by the residual covariance matrix S (k) of the above equation (94) calculated from the observation matrix H (k) calculated by the calculation unit 125b. Equation (96) is a reference condition for determining whether or not an observation vector from the target observation device 20 exists in the gate. For example, when the residual vector (underbar) r (k) satisfies Equation (96), the gate determination unit 107 uses the observation vector (underbar) z (k) input from the target observation device 20 as the tracking target detection result. The observation vector (underbar) z (k) determined to be from the tracking target is sent to the smooth vector calculation unit 109. On the other hand, if an observation vector that satisfies Equation (96) is not obtained from the target observation device 20, the gate determination unit 107 regards that the observation target observation vector has not been detected, and the observation to the smooth vector calculation unit 109 is performed. Do not send vectors.
[0210]
In the calculation step S9aa of the predicted observation vector for smooth vector calculation, the predicted observation vector to be output to the smooth vector calculation unit 109 is calculated by the following equation (97).
[0211]
[Equation 65]

[0212]
Predicted observation vector (upper and lower bars) z for smooth vector calculation_h(k) is used by the smooth vector calculation unit 109 to calculate a residual that is a difference from the observed value.
[0213]
Next, the gain matrix calculation unit 113 reads the prediction error covariance matrix (bar) P (k), the residual covariance matrix S (k), and the observation matrix H (k), and the following equation (98) is read. Therefore, the filter gain is calculated (gain matrix calculation step S10).
[0214]
[Equation 66]

[0215]
In addition, the smooth vector calculation unit 109 receives a prediction vector (upper and lower bars) x when an observation vector (underbar) z (k) is input from the gate determination unit 107._h(k), predicted observation vector (up / down bar) z_h(k) and the gain matrix K (k) are read, and a smooth vector is calculated according to the following equation (99).
[0216]
[Expression 67]

[0217]
In addition, when the observation vector (underbar) z (k) is not input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x as shown in the following equation (100)._hLet (k) be a smooth vector (smooth vector calculation step S11).
[0218]
[Equation 68]

[0219]
The smoothing vector (underbar hat) x (k) calculated by the smoothing vector calculation unit 109 according to the above formula (99) or formula (100) is stored in the smooth vector memory 105 and also in the wake display device 30. Is transmitted and displayed to the operator.
[0220]
On the other hand, when the gate determination unit 107 determines that the tracking target observation vector has been obtained, the smooth error covariance matrix calculation unit 121 determines the prediction error covariance matrix (bar) P (k) and the observation noise covariance. Using the matrix R (k) and the observation matrix H (k), the smoothing error covariance matrix is calculated by the following equation (101), and the gate determination unit 107 determines that the observation target observation vector has not been obtained. In this case, as shown in the following formula (102), the prediction error covariance matrix (bar) P (k) is set as a smooth error covariance matrix (smooth error covariance matrix calculation step S12).
[0221]
[Equation 69]

[0222]
The calculation result of the smoothing error covariance matrix according to the equations (101) and (102) is stored in the smoothing error covariance matrix memory 123.
[0223]
This completes the process at time t (k). Thereafter, the above series of processing steps are repeatedly executed until the end of tracking is determined (tracking end step S13).
[0224]
FIG. 9 shows time transitions of the position component (smooth value) of the smooth vector, the predicted observation vector (gate center), and the observation vector (observation value) using the third embodiment. The solid line in the figure represents the true trajectory of the position component of the state vector, and the dotted line represents the true value of the observation vector affected by the Doppler shift. As a result of the above processing steps, as shown in FIG. 9, when switching from a filter composed of an observation model that does not consider Doppler shift to a filter that considers an observation model that considers Doppler shift, the prediction that is the center of the gate In order to improve the problem that the distance between the observation vector and the observation vector is increased, by using an observation model that does not consider Doppler shift in the calculation of the prediction observation vector for the gate at the time of switching, the prediction observation vector for the gate and the observation vector are This has the effect of shortening the distance. Furthermore, unlike the predicted observation vector for gates calculated above, the observation vector that takes into account the Doppler shift is used as the predicted observation vector for calculating the smooth vector. This has the effect of increasing the convergence speed of the transient response.
[0225]
Note that it is obvious that using the idea of the present embodiment, it is possible to cope with switching from a filter configured with an observation model considering Doppler shift to a filter using an observation model not considering Doppler shift. Therefore, explanation is omitted.
[0226]
Embodiment 4 FIG.
10 is a block diagram showing a target tracking apparatus according to Embodiment 4 of the present invention, and FIG. 11 is a flowchart showing the processing operation. Detailed description of the same components as those in FIGS. 7 and 8 is omitted. FIG. 12 is a conceptual diagram showing the effect in the third embodiment of the present invention.
[0227]
In FIG. 10, the target tracking device 10 a is similar to FIG. 7, which is a block configuration diagram showing the third embodiment, and includes a smooth vector memory 105, a prediction vector calculation unit 103, a gate prediction observation vector calculation unit 129, and a gate determination unit. 107, smoothing vector calculation unit 109, residual covariance matrix calculation unit 111, gain matrix calculation unit 113, switching control unit 127, observation noise covariance matrix setting unit 115, prediction error covariance matrix calculation unit 117, drive noise covariance A matrix setting unit 119, a smoothing error covariance matrix calculating unit 121, and a smoothing error covariance matrix memory 123 are provided.
[0228]
In addition to the above components, the target tracking device 10a includes an observation matrix calculation unit 125c (the operation is different from that in FIG. 7 of the third embodiment), a predicted observation vector calculation unit 131b for smooth vector calculation (the diagram of the third embodiment). 7 and the operation is different).
[0229]
The observation matrix calculation unit 125c receives the filter switching time information from the switching control unit 127, and is calculated by the prediction vector calculation unit 103 after the time of switching to the filter configured by the observation model considering the Doppler shift. By using the predicted value, an observation matrix taking into account the Doppler shift is calculated, a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131b, and a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. Prior to the switching time, an observation model that does not consider Doppler shift is calculated, a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131b, a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. At the switching time, an observation model that does not consider Doppler shift is calculated and output to the gate predicted observation vector calculation unit 129, the residual covariance matrix calculation unit 111, and a smoothing error covariance matrix calculation unit 121, a gain matrix calculation unit 113, and calculates and outputs an observation matrix considering the Doppler shift.
[0230]
The prediction vector calculation unit 131b for calculating a smooth vector calculates the prediction vector from the prediction vector calculation unit 103 and the observation matrix calculation unit 125c before and after the time when the filter is configured by the observation model considering the Doppler shift. A predicted observation vector used in the smooth vector calculation unit 109 is calculated from the observation matrix as an input. At the time of switching to the filter configured by the observation model considering the Doppler shift, the predicted observation vector used by the smooth vector calculation unit 109 is calculated from the prediction vector from the prediction vector calculation unit 103.
[0231]
In FIG. 11, the initial value setting step S1, the observation vector input step S2, the prediction vector calculation step S3, and the drive noise covariance matrix are set, as in FIG. 8 which is a flowchart showing the processing operation of the third embodiment. Step S4, prediction error covariance matrix calculation step S5, gate predicted observation vector calculation step 5b, observation noise covariance matrix setting step S7, residual covariance matrix calculation step S8, gate determination step S9, gain matrix Calculation step S10, a smooth vector calculation step S11, and a smooth error covariance matrix calculation step S12.
[0232]
In addition to the above processing steps, FIG. 11 includes a calculation step Q9aaa for predictive observation vector for smooth vector calculation (processing is different from that in FIG. 8 showing the processing operation of the third embodiment), an observation matrix calculation step S5aaaa (implementation step). The processing operation of the third embodiment is different from that of FIG. 14).
In FIG. 17, the calculation step of the predicted observation vector for smooth vector calculation is performed by calculating the prediction vector and the observation matrix from the prediction vector calculation unit 103 before and after the time of switching to the filter configured by the observation model considering the Doppler shift. A predicted observation vector used in the smooth vector calculation unit 109 is calculated from an observation matrix that is an input from the unit 125c. At the time of switching to the filter configured by the observation model considering the Doppler shift, this is a step of calculating the predicted observation vector used in the smooth vector calculation unit 109 from the prediction vector from the prediction vector calculation unit 103.
[0233]
The observation matrix calculation step S5aaaa receives the filter switching time information from the switching control unit 127, and is calculated by the prediction vector calculation unit 103 after the time of switching to the filter configured by the observation model considering the Doppler shift. By using the predicted value, an observation matrix taking into account the Doppler shift is calculated, a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131b, and a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. Prior to the switching time, an observation model that does not consider Doppler shift is calculated, a predicted observation vector calculation unit for gate 129, a residual covariance matrix calculation unit 111, a predicted observation vector calculation unit for smooth vector calculation 131b, a smooth error covariance. It outputs to the matrix calculation part 121 and the gain matrix calculation part 113. At the switching time, an observation model that does not consider Doppler shift is calculated and output to the gate predicted observation vector calculation unit 129, the residual covariance matrix calculation unit 111, and a smoothing error covariance matrix calculation unit 121, a gain matrix calculation unit 113 is a step of calculating and outputting an observation matrix considering the Doppler shift.
[0234]
Hereinafter, the processing operation according to the fourth embodiment will be described in detail with reference to FIGS. 10 and 11. First, in the smoothing vector memory 105 and the smoothing error covariance matrix memory 123, an initial value (underbar hat) x (0) of the smoothing vector and an initial value P (0) of the smoothing error covariance matrix are respectively stored. The tracking process is set (initial value setting step S1). Here, the state vector (underbar) x (k) is a six-dimensional vector of Formula (103) defined in the north reference orthogonal coordinate system.
[0235]
[Equation 70]

[0236]
The smoothing error covariance matrix is a 6 × 6 matrix. Thereafter, processing at each time of k = 1, 2,... Is executed. In the process at time t (k), an observation vector (underbar) z (k) is input from the target observation device 20 (observation vector input step S2). Here, the observation vector (underbar) z (k) is a three-dimensional vector obtained in the north reference orthogonal coordinate system affected by the Doppler shift. First, in the prediction vector calculation step S3, the smooth vector (underbar hat) x (k-1) at the previous time t (k-1) is read from the smooth vector memory 105 and predicted according to the following equation (104). A vector is calculated (prediction vector calculation step S3).
[0237]
[Equation 71]

[0238]
The state transition matrix is given by the matrix of Equation (105).
[0239]
[Equation 72]

[0240]
Next, the driving noise covariance matrix setting unit 119 sets the driving noise covariance matrix Q (k−1) (driving noise covariance matrix setting step S4). Normally, the value of the driving noise covariance matrix is set to the size of the sampling interval t (k) −t (k−1) in consideration of the maximum target maneuverability assumed in the operation of the target tracking device 10a. Will be decided accordingly. Next, the prediction error covariance matrix calculation unit 117 reads the smoothing error covariance matrix P (k−1) at the previous time t (k−1) from the smoothing error covariance matrix memory 123, and the driving noise covariance matrix is read out. Using the variance matrix Q (k−1), a prediction error covariance matrix is calculated according to the following formula (106) (prediction error covariance matrix calculation step S5).
[0241]
[Equation 73]

[0242]
Next, in the observation matrix calculation step S5aaaa, information on the filter switching time is obtained from the switching control unit 127. Before the switching time, the observation matrix is set as the following mathematical formula (107), and the predicted observation vector calculation unit for gate 129, the residual covariance matrix calculation unit 111, the prediction vector calculation unit for smooth vector calculation 131 b, the smoothing error covariance matrix calculation unit 121, and the gain matrix calculation unit 113.
[0243]
[Equation 74]

[0244]
Where I_3x3Is a 3-by-3 identity matrix, 0_3x3Is a 3-by-3 zero matrix. After the switching time, the observation matrix calculation unit 125c outputs the prediction vector (upper and lower bars) x output from the prediction vector calculation unit 103._hFrom (k), the following equation (108), equation (109), equation (110), equation (111), equation (112), and equation (113) are used to calculate an observation matrix, and a predicted observation vector calculation unit for gate 129, the residual covariance matrix calculation unit 111, the prediction vector calculation unit for smooth vector calculation 131 b, the smoothing error covariance matrix calculation unit 121, and the gain matrix calculation unit 113.
[0245]
[Expression 75]

[0246]
[76]

[0247]
The observation matrix H (k) shown in the above equation (108) is a matrix for describing observation values reflecting the influence of Doppler shift occurring in the distance direction in the north reference orthogonal coordinate system. This represents a shift in the distance direction by ΔT (dot) R (k), which is the effect of Doppler shift. The observation matrix H (k) shown in Equation (108) is a prediction vector (upper and lower bars) x given in the north reference orthogonal coordinate system._hVariables (bar) R (k), (bar) E (k), (bar) Az (k), (bar dot) R (k), (bar dot) E converted from (k) to polar coordinate system It is calculated using (k), (bar dot) Az (k). Expressions (112) and (113) represent expressions for conversion from the north reference orthogonal coordinate system to polar coordinates. ΔT in Equation (109) is a time constant given in advance.
[0248]
In the case of the switching time, the observation matrix calculation unit 125c outputs the observation matrix as the mathematical formula (107) to the gate predicted observation vector calculation unit 129 and the residual covariance matrix calculation unit 111, and is output from the prediction vector calculation unit 103. Predicted vector (up / down bar) x_hFrom (k), the observation matrix is calculated by Formula (108), Formula (109), Formula (110), Formula (111), Formula (112), and Formula (113), and a smoothing error covariance matrix calculation unit 121, The result is output to the gain matrix calculation unit 113.
[0249]
In the gate predicted observation vector calculation step S5b, the gate predicted observation vector calculation unit 129 uses the prediction vector (upper and lower bars) x input from the prediction vector calculation unit 103._hUsing (k) and the observation matrix input from the observation matrix calculation unit 125c, the predicted observation vector (upper and lower bars) z according to the following equation (114):_g(k) is calculated (prediction observation vector calculation step S5b).
[0250]
[77]

[0251]
Further, the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) of the observation vector (underbar) z (k) input from the target observation device 20, and a residual covariance matrix calculation unit. 111 and the smoothing error covariance matrix calculation unit 121 (observation noise covariance matrix setting step S7). Note that the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) using parameters related to the accuracy evaluation of the observation vector (underbar) z (k). For example, when the target observation device 20 is a radar that detects a target in the x-, y-, and z-axis directions in the north reference orthogonal coordinate system, the observation noise covariance matrix R (k) is based on information on the observation accuracy of the angular axis of the radar. Can be set. Here, the observation accuracy can be estimated based on the intensity (or signal-to-noise ratio) of the target signal detected by the radar at time t (k).
[0252]
Next, the residual covariance matrix calculation unit 111 reads the observation noise covariance matrix R (k), the prediction error covariance matrix (bar) P (k), and the observation matrix H (k), and the following mathematical formula (115 ), The residual covariance matrix S (k) is calculated and transmitted to the gate determination unit 107 and the gain matrix calculation unit 113 (residual covariance matrix calculation step S8).
[0253]
[Formula 78]

[0254]
The gate determination unit 107 uses the predicted observation vector (upper and lower bars) z (k) input from the predicted observation vector calculation unit 129, and calculates the residual with respect to the observation vector (underbar) z (k) by the following equation (116). After calculating the vector, the residual covariance matrix S (k) is read, and a gate determination process is executed according to the following equation (117) (gate determination step S9).
[0255]
[79]

[0256]
However, in Formula (117), d is a parameter. Note that the gate used by the gate determination unit 107 is the predicted observation vector (up / down bar) z._gIt is a spatial range centering on (k) and is a spatial range in which an observation vector from the tracking target can be captured with a desired probability. The size and shape of the gate include a prediction error covariance matrix (bar) P (k) that is an evaluation value of a prediction vector error, an observation noise covariance matrix R (k) that is an evaluation value of an observation error, and an observation matrix It is determined by the residual covariance matrix S (k) of the above equation (94) calculated from the observation matrix H (k) calculated by the calculation unit 125c. The equation (117) is a reference condition for determining whether or not an observation vector from the target observation device 20 exists in the gate. For example, when the residual vector (underbar) r (k) satisfies Equation (117), the gate determination unit 107 uses the observation vector (underbar) z (k) input from the target observation device 20 as the tracking target detection result. The observation vector (underbar) z (k) determined to be from the tracking target is sent to the smooth vector calculation unit 109. On the other hand, when the observation vector satisfying the equation (117) is not obtained from the target observation device 20, the gate determination unit 107 regards that the observation target observation vector has not been detected, and the observation to the smooth vector calculation unit 109 is performed. Do not send vectors.
[0257]
In the calculation step S9aa of the predicted observation vector for smooth vector calculation, when the current time is before the transfer time, the predicted observation vector output to the smooth vector calculation unit 109 is calculated by the following formula (118).
[0258]
[80]

[0259]
When the current time is the transfer time, the predicted observation vector is calculated by the following formula (119), formula (120), formula (121), formula (122), and formula (123).
[0260]
[Formula 81]

[0261]
Equation (120) is a conversion equation from the north reference orthogonal coordinate system to the polar coordinate system, and equation (122) is a conversion equation from the polar coordinate system to the north reference orthogonal coordinate system. Expression (121) is an expression that gives the Doppler shift to the distance component of the prediction vector affected by the Doppler shift. In this way, the residual in the smooth vector calculation step can be increased.
[0262]
Next, the gain matrix calculation unit 113 reads the prediction error covariance matrix (bar) P (k), the residual covariance matrix S (k), and the observation matrix H (k), and the following equation (124) is obtained. Therefore, the filter gain is calculated (gain matrix calculation step S10).
[0263]
[Formula 82]

[0264]
In addition, the smooth vector calculation unit 109 receives a prediction vector (upper and lower bars) x when an observation vector (underbar) z (k) is input from the gate determination unit 107._h(k), predicted observation vector (up / down bar) z_h(k) and the gain matrix K (k) are read, and a smooth vector is calculated according to the following equation (125).
[0265]
[Formula 83]

[0266]
Further, when the observation vector (underbar) z (k) is not input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x as shown in the following equation (126)._hLet (k) be a smooth vector (smooth vector calculation step S11).
[0267]
[Expression 84]

[0268]
The smoothing vector (underbar hat) x (k) calculated by the smoothing vector calculation unit 109 according to the above formula (125) or formula (126) is stored in the smooth vector memory 105 and also in the wake display device 30. Is transmitted and displayed to the operator.
[0269]
On the other hand, when the gate determination unit 107 determines that the tracking target observation vector has been obtained, the smooth error covariance matrix calculation unit 121 determines the prediction error covariance matrix (bar) P (k) and the observation noise covariance. Using the matrix R (k) and the observation matrix H (k), a smooth error covariance matrix is calculated by the following equation (127), and the gate determination unit 107 determines that the observation target observation vector has not been obtained. In this case, as shown in the following equation (128), the prediction error covariance matrix (bar) P (k) is set as a smooth error covariance matrix (smooth error covariance matrix calculation step S12).
[0270]
[Expression 85]

[0271]
The calculation result of the smoothing error covariance matrix according to Equation (127) and Equation (128) is stored in the smoothing error covariance matrix memory 123.
[0272]
This completes the process at time t (k). Thereafter, the above series of processing steps are repeatedly executed until the end of tracking is determined (tracking end step S13).
[0273]
FIG. 12 shows time transitions of the position component (smooth value) of the smooth vector, the predicted observation vector (gate center), and the observation vector (observation value) using the first embodiment. The solid line in the figure represents the true trajectory of the position component of the state vector, and the dotted line represents the true value of the observation vector affected by the Doppler shift. As a result of the above processing steps, as shown in FIG. 12, when switching from a filter configured with an observation model that does not consider Doppler shift to a filter that considers an observation model that considers Doppler shift, the prediction that is the center of the gate is performed. In order to improve the problem that the distance between the observation vector and the observation vector is increased, by using an observation model that does not consider Doppler shift in the calculation of the prediction observation vector for the gate at the time of switching, the prediction observation vector for the gate and the observation vector are This has the effect of shortening the distance. Furthermore, unlike the predicted observation vector for gates calculated above, the observation vector that takes into account the Doppler shift is used as the predicted observation vector for calculating the smooth vector. This has the effect of increasing the convergence speed of the transient response.
[0274]
Note that it is obvious that using the idea of the present embodiment, it is possible to cope with switching from a filter configured with an observation model considering Doppler shift to a filter using an observation model not considering Doppler shift. Therefore, explanation is omitted.
[0275]
Embodiment 5. FIG.
FIG. 13 is a block diagram showing a target tracking apparatus according to Embodiment 5 of the present invention, and FIG. 14 is a flowchart showing its processing operation. Detailed descriptions of those similar to those in FIGS. 1 and 2 are omitted. FIG. 15 is a conceptual diagram showing effects in the third embodiment of the present invention.
[0276]
In FIG. 13, the target tracking device 10 a is similar to FIG. 1, which is a block configuration diagram showing the first embodiment, and includes a smooth vector memory 105, a prediction vector calculation unit 103, a prediction observation vector calculation unit 101, and a gate determination unit 107. , Smoothing vector calculation unit 109, residual covariance matrix calculation unit 111, gain matrix calculation unit 113, observation noise covariance matrix setting unit 115, prediction error covariance matrix calculation unit 117, driving noise covariance matrix setting unit 119, smoothing An error covariance matrix calculation unit 121 and a smooth error covariance matrix memory 123 are provided.
[0277]
In addition to the above components, the target tracking device 10a includes a switching control unit 127, a filter Doppler shift time constant calculation unit 135, an observation matrix calculation unit 125d (operation differs from that in FIG. 1 of the first embodiment), It has.
[0278]
The switching control unit 127 outputs, to the filter Doppler shift time constant calculating unit 135, the switching timing from the filter configured by the observation model not considering the Doppler shift to the filter configured by the observation model considering the Doppler shift.
[0279]
The filter Doppler shift time constant calculation unit 135 receives the filter switching time by the switching control unit 127, calculates the Doppler shift time constant set in the observation model used for the filter, and outputs the calculated Doppler shift time constant to the observation matrix calculation unit 125d.
[0280]
The observation matrix calculation unit 125d receives the Doppler shift time constant from the filter Doppler shift time constant calculation unit 135 as an input, and uses the prediction value calculated by the prediction vector calculation unit 103 to calculate an observation matrix in consideration of the Doppler shift. To do.
[0281]
14, the initial value setting step S1, the observation vector input step S2, the prediction vector calculation step S3, and the drive noise covariance matrix are set, as in FIG. 2 which is a flowchart showing the processing operation of the first embodiment. Step S4, Prediction error covariance matrix calculation step S5, Prediction observation vector calculation step S6, Observation noise covariance matrix setting step S7, Residual covariance matrix calculation step S8, Gate determination step S9, Gain matrix calculation Step S10, a smoothing vector calculating step S11, and a smoothing error covariance matrix calculating step S12 are provided.
[0282]
Further, in FIG. 14, in addition to the above processing steps, a filter Doppler shift time constant calculation step S5c and an observation matrix calculation step S5aaaa (processing is different from that of FIG. 2 showing the processing operation of the first embodiment) are inserted. ing.
[0283]
In FIG. 14, the filter Doppler shift time constant calculation step S5c receives the filter switching time by the switching control unit 127, calculates the Doppler shift time constant set in the observation model used for the filter, and the observation matrix calculation unit 125d. It is a step to output to.
[0284]
The observation matrix calculation step S5aaaaaa receives the filter switching time information from the switching control unit 127, and after the time when switching to the filter configured by the observation model considering the Doppler shift, the prediction vector calculated by the prediction vector calculation unit 103 is used. Before the switching is performed by calculating the observation matrix using the values, this is a step of outputting an observation model that does not consider the Doppler shift.
[0285]
Hereinafter, the processing operation according to the fifth embodiment of the present invention shown in FIG. 13 will be described in detail with reference to FIG. First, in the smoothing vector memory 105 and the smoothing error covariance matrix memory 123, an initial value (underbar hat) x (0) of the smoothing vector and an initial value P (0) of the smoothing error covariance matrix are respectively stored. The tracking process is set (initial value setting step S1). Here, the state vector (underbar) x (k) is a six-dimensional vector of Expression (129) defined by the north reference orthogonal coordinate system.
[0286]
[86]

The smoothing error covariance matrix is a 6 × 6 matrix. Thereafter, processing at each time of k = 1, 2,... Is executed. In the process at time t (k), when an observation vector (underbar) z (k) is input from the target observation device 20 (observation vector input step S2), the prediction vector calculation unit 103 reads from the smooth vector memory 105. The smooth vector (underbar hat) x (k−1) at the previous time t (k−1) is read out, and the prediction vector is calculated according to the following equation (130) (prediction vector calculation step S3).
[0287]
[Expression 87]

[0288]
Here, the observation vector (underbar) z (k) is a three-dimensional vector obtained in the north reference orthogonal coordinate system affected by the Doppler shift. The state transition matrix is given by the matrix of Equation (131).
[0289]
[Equation 88]

[0290]
Next, the driving noise covariance matrix setting unit 119 sets the driving noise covariance matrix Q (k−1) (driving noise covariance matrix setting step S4). Normally, the value of the driving noise covariance matrix is set to the size of the sampling interval t (k) −t (k−1) in consideration of the maximum target maneuverability assumed in the operation of the target tracking device 10a. Will be decided accordingly. Next, the prediction error covariance matrix calculation unit 117 reads the smoothing error covariance matrix P (k) at the previous time t (k−1) from the smoothing error covariance matrix memory 123, and the driving noise covariance matrix. Using Q (k−1), a prediction error covariance matrix is calculated according to the following equation (132) (prediction error covariance matrix calculation step S5).
[0291]
[Expression 89]

[0292]
Next, in the calculation step S5c for the filter Doppler shift time constant, the switching control unit 127 obtains the switching time information. When the current time is the switching time, the desired Doppler shift time constant is gradually set from that time as a base point. Thus, the Doppler shift time constant to be given to the observation model is calculated (filtering Doppler shift time constant calculation step S5c). For example, the Doppler shift time constant constituting the observation model for the filter is calculated by the following equation (133). This is also shown in FIG.
[0293]
[90]

[0294]
Here, ΔT (k) is a Doppler shift time constant used when calculating the observation model, ΔT is a desired Doppler shift time constant, k_aIs the filter switching time, k_bIs the time when the observation model calculation time constant is set to a desired value after the start of switching. k_bAre parameters determined by the designer. By doing so, it is possible to prevent a phenomenon in which variables such as a predicted observation vector change greatly before and after switching.
[0295]
Next, the observation matrix calculation unit 125d outputs the prediction vector (upper and lower bars) x (k) output from the prediction vector calculation unit 103 and the Doppler shift time constant ΔT (output from the filter Doppler shift time constant calculation unit 135). k), an observation matrix is calculated (observation matrix calculation step S5aaaaaa).
[0296]
[91]

[0297]
[Equation 92]

[0298]
The observation matrix H (k) shown in the equation (134) is a matrix for describing observation values reflecting the influence of Doppler shift occurring in the distance direction in the north reference orthogonal coordinate system. This represents a shift in the distance direction by ΔT (dot) R (k), which is the effect of Doppler shift. The observation matrix H (k) shown in the equation (134) is a variable (bar) R (k) obtained by converting a prediction vector (upper and lower bars) x (k) given in the north reference orthogonal coordinate system into a polar coordinate system. , (Bar) E (k), (bar) Az (k), (bar / dot) R (k), (bar / dot) E (k), (bar / dot) Az (k) Is done. Equations (137) and (138) represent equations for conversion from the north reference orthogonal coordinate system to polar coordinates. ΔT in Equation (135) is a time constant given in advance.
[0299]
The predicted observation vector calculation unit 101 uses the prediction vector (upper and lower bars) x (k) input from the prediction vector calculation unit 103 and the observation matrix input from the observation matrix calculation unit 125d according to the following equation (140). A predicted observation vector (upper and lower bars) z (k) is calculated (predicted observation vector calculation step S6).
[0300]
[Equation 93]

[0301]
Further, the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) of the observation vector (underbar) z (k) input from the target observation device 20, and a residual covariance matrix calculation unit. 111 and the smoothing error covariance matrix calculation unit 121 (observation noise covariance matrix setting step S7). Note that the observation noise covariance matrix setting unit 115 sets the observation noise covariance matrix R (k) using parameters related to the accuracy evaluation of the observation vector (underbar) z (k). For example, when the target observation device 20 is a radar that detects a target in the x-, y-, and z-axis directions in the north reference orthogonal coordinate system, the observation noise covariance matrix R (k) is based on information on the observation accuracy of the angular axis of the radar. Can be set. Here, the observation accuracy can be estimated based on the intensity (or signal-to-noise ratio) of the target signal detected by the radar at time t (k).
[0302]
Next, the residual covariance matrix calculation unit 111 reads the observation noise covariance matrix R (k), the prediction error covariance matrix (bar) P (k), and the observation matrix H (k), and the following equation (141) ), The residual covariance matrix S (k) is calculated and transmitted to the gate determination unit 107 and the gain matrix calculation unit 113 (residual covariance matrix calculation step S8).
[0303]
[Equation 94]

[0304]
The gate determination unit 107 uses the predicted observation vector (upper and lower bars) z (k) input from the predicted observation vector calculation unit 101 and uses the following equation (142) to calculate the residual for the observation vector (underbar) z (k). After calculating the vector, the residual covariance matrix S (k) is read, and a gate determination process is executed by the following equation (143) (gate determination step S9).
[0305]
[95]

[0306]
However, in Formula (143), d is a parameter. Note that the gate used by the gate determination unit 107 is a spatial range centered on the predicted observation vector (upper and lower bars) z (k), and is a spatial range in which the observation vector from the tracking target can be captured with a desired probability. . The size and shape of the gate include a prediction error covariance matrix (bar) P (k) that is an evaluation value of a prediction vector error, an observation noise covariance matrix R (k) that is an evaluation value of an observation error, and an observation matrix It is determined by the residual covariance matrix S (k) of the above equation (44) calculated from the observation matrix H (k) calculated by the calculation unit 125d. The equation (143) is a reference condition for determining whether or not an observation vector from the target observation device 20 exists in the gate. For example, when the residual vector (underbar) r (k) satisfies Equation (143), the gate determination unit 107 uses the observation vector (underbar) z (k) input from the target observation device 20 as the tracking target detection result. The observation vector (underbar) z (k) determined to be from the tracking target is sent to the smooth vector calculation unit 109. On the other hand, when an observation vector that satisfies Equation (46) is not obtained from the target observation device 20, the gate determination unit 107 regards that the observation target observation vector has not been detected, and the observation to the smooth vector calculation unit 109 is performed. Do not send vectors.
[0307]
Next, the gain matrix calculation unit 113 reads the prediction error covariance matrix (bar) P (k), the residual covariance matrix S (k), and the observation matrix H (k), and the following equation (144) is read. Therefore, the filter gain is calculated (gain matrix calculation step S10).
[0308]
[Equation 96]

[0309]
In addition, when the observation vector (under bar) z (k) is input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x (k), predicted prediction vector (up and down bar) z ( k) and the gain matrix K (k) are read, and a smooth vector is calculated according to the following equation (145).
[0310]
[Equation 97]

[0311]
In addition, when the observation vector (underbar) z (k) is not input from the gate determination unit 107, the smooth vector calculation unit 109 predicts the prediction vector (upper and lower bars) x ( k) is a smooth vector (smooth vector calculation step S11).
[0312]
[Equation 98]

[0313]
The smoothing vector (underbar hat) x (k) calculated by the smoothing vector calculation unit 109 according to the above formula (145) or formula (146) is stored in the smooth vector memory 105 and also stored in the wake display device 30. Is transmitted and displayed to the operator.
[0314]
On the other hand, when the gate determination unit 107 determines that the tracking target observation vector has been obtained, the smooth error covariance matrix calculation unit 121 predicts the prediction error covariance matrix (bar) P (k) and the observation noise covariance. Using the matrix R (k) and the observation matrix H (k), the smoothing error covariance matrix is calculated by the following equation (147), and it is determined by the gate determination unit 107 that the observation target observation vector has not been obtained. In this case, as shown in the following formula (148), the prediction error covariance matrix (bar) P (k) is set as the smooth error covariance matrix (smooth error covariance matrix calculation step S12).
[0315]
[99]

[0316]
The calculation result of the smoothing error covariance matrix according to Equation (147) and Equation (148) is stored in the smoothing error covariance matrix memory 123.
[0317]
This completes the process at time t (k). Thereafter, the above series of processing steps are repeatedly executed until the end of tracking is determined (tracking end step S13).
[0318]
FIG. 15 shows time transitions of the position component (smooth value) of the smooth vector, the predicted observation vector (gate center), and the observation vector (observation value) using the first embodiment. The solid line in the figure represents the true trajectory of the position component of the state vector, and the dotted line represents the true value of the observation vector affected by the Doppler shift. As a result of the above processing steps, as shown in FIG. 15, when switching from a filter configured with an observation model that does not consider Doppler shift to a filter that considers an observation model that considers Doppler shift, the prediction that is the center of the gate is performed. In order to improve the problem that the distance between the observation vector and the observation vector increases, by changing the Doppler shift time constant used by the observation model little by little at the time of switching, the distance between the predicted observation vector for the gate and the observation vector becomes There is an effect that it does not change suddenly immediately before and after switching.
[0319]
Note that it is obvious that using the idea of the present embodiment, it is possible to cope with switching from a filter configured with an observation model considering Doppler shift to a filter using an observation model not considering Doppler shift. Therefore, explanation is omitted.
[0320]
【The invention's effect】
As described above, according to the present invention, a target tracking device for calculating a track of a tracking target based on an observation vector input from the target observation device, the smoothing vector for storing the smoothing vector calculation result Memory, a smoothing error covariance matrix memory for storing the calculation result of the smoothing error covariance matrix, a prediction vector calculation unit for calculating a prediction vector at the current time using the smoothing vector at the previous time, and driving noise Calculate the prediction error covariance matrix at the current time using the driving noise covariance matrix setting unit for setting the covariance matrix, the calculation result of the smoothing error covariance matrix at the previous time, and the value of the driving noise covariance matrix A prediction error covariance matrix calculation unit, an observation noise covariance matrix setting unit that sets an observation noise covariance matrix using parameters related to accuracy evaluation of the observation vector, and the prediction vector An observation matrix calculation unit that calculates an observation matrix at the current time using the prediction error covariance matrix, the observation noise covariance matrix, and a residual covariance matrix that calculates a residual covariance matrix using the observation matrix A calculation unit; a prediction observation vector calculation unit for calculating a prediction observation vector from the prediction vector and the observation matrix; a residual vector using the prediction observation vector and the observation vector; and the residual covariance matrix Using a gate determination unit that performs gate determination as to whether or not the observation vector is detection data of the tracking target, the prediction error covariance matrix, the residual covariance matrix, and the observation matrix The smoothing vector is calculated using a gain matrix calculating unit that calculates a gain matrix of the filter, the residual vector, the prediction vector, and the gain matrix. A smoothing vector calculation unit; and a smoothing error covariance matrix calculation unit that calculates the smoothing error covariance matrix using the observation noise covariance matrix, the prediction error covariance matrix, and the observation matrix. In order to construct a filter that considers Doppler shift correction in the north reference Cartesian coordinate system, it is expressed by the distance axis of the polar coordinate system used in the conventional technology. By configuring an observation model in the north reference orthogonal coordinate system instead of the observation model, the Doppler shift correction can be performed in the north reference orthogonal coordinate system.
[0321]
A target tracking device for calculating a track of a tracking target based on an observation vector input from the target observation device, comprising a smoothing vector memory for storing a smoothing vector calculation result, and a smoothing error covariance Output smoothing error covariance matrix memory for storing matrix calculation results, and switching timing from a filter configured with an observation model that does not consider Doppler shift to a filter configured with an observation model that considers Doppler shift The filter control time information is obtained from the switching control unit and the switching control unit, and when the current time is the time for switching, a pseudo smoothing value is calculated using the smoothing vector one time before from the smoothing vector memory. If it is not the time to switch, the smooth value immediately before one time is output as is. A smoothing value calculation unit, a prediction vector calculation unit that calculates a prediction vector at the current time using a smoothing vector at the previous time, a driving noise covariance matrix setting unit that sets a driving noise covariance matrix, and a smoothing error at the previous time A prediction error covariance matrix calculation unit that calculates a prediction error covariance matrix at the current time using the calculation result of the covariance matrix and the value of the driving noise covariance matrix, and parameters related to the accuracy evaluation of the observation vector Using the observation noise covariance matrix setting unit for setting the observation noise covariance matrix and the filter switching time information from the switching control unit as input, and after the time when switching to the filter configured by the observation model considering Doppler shift Uses the predicted value calculated by the predicted vector calculation unit to calculate the current time observation matrix considering the Doppler shift, and from the switching time Calculates a residual covariance matrix using an observation matrix calculation unit that calculates an observation model at the current time without considering Doppler shift, the prediction error covariance matrix, the observation noise covariance matrix, and the observation matrix Using a residual covariance matrix calculation unit, a predicted observation vector calculation unit for gate that calculates a predicted observation vector used in a gate determination unit using the prediction vector and the observation matrix, and using the predicted observation vector and the observation vector Calculating a residual vector and using the residual covariance matrix to determine whether the observation vector is detection data of the tracking target or not, and a smoothing vector memory The predicted observation vector used in the smooth vector calculation unit is calculated from the smooth vector and the observation matrix input from the observation matrix calculation unit one hour before A prediction vector calculation unit for calculating a smooth vector, a gain matrix calculation unit for calculating a gain matrix of a filter using the prediction error covariance matrix, the residual covariance matrix, and the observation matrix; and the residual The smoothing error using the smoothing vector calculation unit that calculates the smoothing vector using the vector, the prediction vector, and the gain matrix, the observation noise covariance matrix, the prediction error covariance matrix, and the observation matrix A smoothing error covariance matrix calculation unit for calculating a covariance matrix, and a target tracking method based on the target tracking device, and a target tracking method based on the target tracking device. When switching to a filter that takes into account an observation model that takes into account Doppler shift, the predicted observation vector that is the center of the gate In order to improve the problem that the distance between the observation vector and the observation vector is increased, the pseudo-smooth vector is calculated by performing Doppler correction on the smooth vector at the previous time in advance and calculating the pseudo-smooth vector at the time of switching. This has the effect of shortening the distance between the predicted observation vector for gate and the observation vector. Further, unlike the predicted observation vector for gate calculated above, the predicted observation vector value calculated from the predicted vector affected by the Doppler shift is used as the predicted observation vector when calculating the smooth vector. Has an effect of increasing the convergence speed of the transient response of the smooth vector at the time of switching.
[0322]
A target tracking device for calculating a track of a tracking target based on an observation vector input from the target observation device, comprising a smoothing vector memory for storing a smoothing vector calculation result, and a smoothing error covariance A smoothing error covariance matrix memory for storing matrix calculation results, a prediction vector calculation unit that calculates a prediction vector at the current time using the smoothing vector at the previous time, and driving noise for setting a driving noise covariance matrix A covariance matrix setting unit; a prediction error covariance matrix calculation unit that calculates a prediction error covariance matrix at the current time using the calculation result of the smoothing error covariance matrix at the previous time and the value of the driving noise covariance matrix; An observation noise covariance matrix setting unit that sets an observation noise covariance matrix using parameters related to the accuracy evaluation of the observation vector, and an observation model that does not consider Doppler shift A switching control unit that outputs the switching timing from the configured filter to a filter configured with an observation model that takes into account the Doppler shift, and an observation that takes into account the Doppler shift using the switching time information of the filter from this switching control unit as input. After the time of switching to the filter configured by the model, an observation matrix that takes into account the Doppler shift is calculated using the predicted value, and a predicted observation vector calculation unit for gates described later, a residual covariance matrix calculation unit described later, Output to the predicted observation vector calculation unit for smooth vector calculation, post-smooth error covariance matrix calculation unit, post-processing matrix calculation unit, and calculate the observation model that does not consider Doppler shift before the switching time, and predictive observation for gates described later Vector calculator, postscript residual covariance matrix calculator, predictive observation for postscript smooth vector calculation Output to the vector calculation unit, the post-smooth error covariance matrix calculation unit, and the post-processing gain matrix calculation unit. Calculate and output an observation model that does not consider shifts, and calculate and output an observation matrix that considers Doppler shifts to a predictive observation vector calculation unit for smoothing vector calculation, a smoothing error covariance matrix calculation unit, and a gain matrix calculation unit to be described later An observation matrix calculation unit, the prediction error covariance matrix, the observation noise covariance matrix, and a residual covariance matrix calculation unit that calculates a residual covariance matrix using the observation matrix, the prediction vector, and the prediction vector Using the observation matrix, the predicted observation vector calculation unit for the gate that calculates the predicted observation vector used in the gate determination unit, the predicted observation vector, and the Calculating a residual vector using an observation vector, and using the residual covariance matrix, performing the gate determination as to whether or not the observation vector is detection data of the tracking target; and The predicted vector calculation unit for smooth vector calculation that calculates a predicted observation vector used in the smoothing vector calculation unit described later from the prediction vector and the observation matrix, the prediction error covariance matrix, the residual covariance matrix, and the observation A gain matrix calculation unit that calculates a gain matrix of a filter using a matrix; a smooth vector calculation unit that calculates the smooth vector using the residual vector, the prediction vector, and the gain matrix; and the observation noise The smoothing error covariance matrix is calculated using the variance matrix, the prediction error covariance matrix, and the observation matrix. An error covariance matrix calculation unit, and a target tracking method and a target tracking method based on the target tracking device, so that an observation considering the Doppler shift is performed from a filter configured with an observation model not considering the Doppler shift. When switching to a filter that takes into account the model, in order to improve the problem that the distance between the predicted observation vector that is the center of the gate and the observation vector is increased, a Doppler shift is performed in the calculation of the predicted observation vector for the gate when switching. By using an observation model that is not considered, there is an effect that the distance between the predicted observation vector for the gate and the observation vector is shortened. Furthermore, unlike the predicted observation vector for gates calculated above, the observation vector that takes into account the Doppler shift is used as the predicted observation vector for calculating the smooth vector. This has the effect of increasing the convergence speed of the transient response.
[0323]
A target tracking device for calculating a track of a tracking target based on an observation vector input from the target observation device, comprising a smoothing vector memory for storing a smoothing vector calculation result, and a smoothing error covariance A smoothing error covariance matrix memory for storing matrix calculation results, a prediction vector calculation unit that calculates a prediction vector at the current time using the smoothing vector at the previous time, and driving noise for setting a driving noise covariance matrix A covariance matrix setting unit; a prediction error covariance matrix calculation unit that calculates a prediction error covariance matrix at the current time using the calculation result of the smoothing error covariance matrix at the previous time and the value of the driving noise covariance matrix; An observation noise covariance matrix setting unit that sets an observation noise covariance matrix using parameters related to the accuracy evaluation of the observation vector, and an observation model that does not consider Doppler shift A switching control unit that outputs the switching timing from the configured filter to a filter configured with an observation model that takes into account the Doppler shift, and an observation that takes into account the Doppler shift using the switching time information of the filter from this switching control unit as input. After the time of switching to the filter configured by the model, an observation matrix that takes into account the Doppler shift is calculated using the predicted value, and a predicted observation vector calculation unit for gates described later, a residual covariance matrix calculation unit described later, Predicted observation vector calculation unit for smooth vector calculation, postscript smoothing error covariance matrix calculation unit, postscript gain matrix calculation unit, outputs to the postscript switching time, calculates an observation model that does not consider Doppler shift, and postulates gate prediction Observation vector calculator, postscript residual covariance matrix calculator, postscript smooth vector calculation schedule Output to an observation vector calculation unit, a prediction vector calculation unit for smoothing vector calculation described later, a smoothing error covariance matrix calculation unit, a gain matrix calculation unit described later, and a gate prediction prediction vector calculation unit, An observation matrix calculation unit that calculates and outputs an observation model that does not consider Doppler shift to the residual covariance matrix calculation unit, and calculates and outputs an observation matrix that considers Doppler shift to the gain matrix calculation unit, and the prediction error A covariance matrix, the observation noise covariance matrix, the residual covariance matrix calculation unit that calculates a residual covariance matrix using the observation matrix, and a gate determination unit using the prediction vector and the observation matrix A predicted observation vector calculation unit for the gate that calculates a predicted observation vector to be used in the step, and a residual vector using the predicted observation vector and the observation vector. A gate determination unit that performs gate determination as to whether or not the observation vector is detection data of the tracking target using the residual covariance matrix and an observation model that considers Doppler shift Before and after the time of switching to the configured filter, the predicted observation vector used in the smooth vector calculation unit is calculated from the prediction vector from the prediction vector calculation unit and the observation matrix input from the observation matrix calculation unit, and the Doppler shift At the time of switching to the filter configured by the observation model taking into account the smoothed vector calculation prediction observation vector calculation unit for calculating the prediction observation vector used in the smooth vector calculation unit from the prediction vector from the prediction vector calculation unit, Fill using prediction error covariance matrix, residual covariance matrix, and observation matrix The gain matrix calculation unit for calculating the gain matrix, the smooth vector calculation unit for calculating the smooth vector using the residual vector, the prediction vector, and the gain matrix, the observation noise covariance matrix, and the prediction A target tracking apparatus comprising: an error covariance matrix; and the smoothing error covariance matrix calculating unit that calculates the smoothing error covariance matrix using the observation matrix, and a target tracking method based thereon Therefore, when switching from a filter configured with an observation model that does not consider Doppler shift to a filter that considers an observation model that considers Doppler shift, the distance between the predicted observation vector that is the center of the gate and the observation vector In order to improve the problem that the By using the observation model that does not consider the shift, the distance between the observation vector and the gate predicted observation vector is effective to shorten. Furthermore, unlike the predicted observation vector for gates calculated above, the observation vector that takes into account the Doppler shift is used as the predicted observation vector for calculating the smooth vector. This has the effect of increasing the convergence speed of the transient response.
[0324]
A target tracking device for calculating a track of a tracking target based on an observation vector input from the target observation device, comprising a smoothing vector memory for storing a smoothing vector calculation result, and a smoothing error covariance Output smoothing error covariance matrix memory for storing matrix calculation results, and switching timing from a filter configured with an observation model that does not consider Doppler shift to a filter configured with an observation model that considers Doppler shift A switching control unit, a prediction vector calculation unit that calculates a prediction vector at the current time using the smoothing vector at the previous time, a driving noise covariance matrix setting unit that sets a driving noise covariance matrix, and a smoothing error value at the previous time. A prediction error in which a prediction error covariance matrix at the current time is calculated using the calculation result of the variance matrix and the value of the driving noise covariance matrix. A covariance matrix calculation unit, an observation noise covariance matrix setting unit that sets an observation noise covariance matrix using parameters related to accuracy evaluation of the observation vector, the prediction error covariance matrix, and the observation noise covariance matrix , And a residual covariance matrix calculation unit that calculates a residual covariance matrix using the observation matrix, and a Doppler shift time constant that is set in an observation model used for the filter, with the filter switching time by the switching control unit as an input And a filter Doppler shift time constant calculation unit for outputting to the observation matrix calculation unit described later, and a Doppler shift time constant from the filter Doppler shift time constant calculation unit as inputs, and using the prediction value, a Doppler shift An observation matrix calculation unit that calculates an observation matrix taking into account the prediction vector and a prediction observation vector that is calculated from the prediction vector and the observation matrix An observation vector calculation unit, calculating a residual vector using the predicted observation vector and the observation vector, and using the residual covariance matrix, whether the observation vector is detection data of the tracking target A gate determination unit that performs a gate determination, a gain matrix calculation unit that calculates a gain matrix of a filter using the prediction error covariance matrix, the residual covariance matrix, and the observation matrix, the residual vector, The smoothing error covariance matrix is calculated using a smoothing vector calculation unit that calculates the smoothing vector using a prediction vector and the gain matrix, the observation noise covariance matrix, the prediction error covariance matrix, and the observation matrix. A smoothing error covariance matrix calculation unit for calculating the target tracking device and a target tracking method based on the target tracking device. When switching from a filter configured with an observation model that does not consider plastic shift to a filter that considers an observation model that considers Doppler shift, the distance between the predicted observation vector that is the center of the gate and the observation vector is separated. In order to improve the effect, the Doppler shift time constant used by the observation model is changed little by little at the time of switching, so that the distance between the predicted observation vector for gate and the observation vector does not change abruptly immediately before and immediately after switching. There is.
[Brief description of the drawings]
FIG. 1 is a block configuration diagram showing a target tracking device according to Embodiment 1 of the present invention;
FIG. 2 is a flowchart showing a processing operation of the target tracking device according to the first embodiment of the present invention.
FIG. 3 is a conceptual diagram showing an effect in the first embodiment of the present invention.
FIG. 4 is a block configuration diagram showing a target tracking device according to a second embodiment of the present invention.
FIG. 5 is a flowchart showing a processing operation of the target tracking device according to the second embodiment of the present invention.
FIG. 6 is a conceptual diagram showing an effect in the second embodiment of the present invention.
FIG. 7 is a block configuration diagram showing a target tracking device according to Embodiment 3 of the present invention.
FIG. 8 is a flowchart showing a processing operation of the target tracking device according to the third embodiment of the present invention.
FIG. 9 is a conceptual diagram showing an effect in the third embodiment of the present invention.
FIG. 10 is a block configuration diagram showing a target tracking device according to Embodiment 4 of the present invention;
FIG. 11 is a flowchart showing a processing operation of the target tracking device according to the fourth embodiment of the present invention.
FIG. 12 is a conceptual diagram showing an effect in the fourth embodiment of the present invention.
FIG. 13 is a block diagram showing a target tracking device according to Embodiment 5 of the present invention.
FIG. 14 is a flowchart showing a processing operation of the target tracking device according to the fifth embodiment of the present invention.
FIG. 15 is a conceptual diagram showing an effect in the fifth embodiment of the present invention.
FIG. 16 is a diagram for explaining a filter Doppler shift time constant according to the fifth embodiment of the present invention;
FIG. 17 is a block diagram showing a conventional target tracking device of this type.
FIG. 18 is a flowchart showing a processing operation of a conventional target tracking device.
FIG. 19 is a conceptual diagram for explaining an operation in the fifth embodiment of the present invention.
FIG. 20 is a diagram for explaining definitions of north reference orthogonal coordinates and polar coordinates.
FIG. 21 is a conceptual diagram for explaining filter switching in the prior art.
[Explanation of symbols]
10a target tracking device, 20 target observation device, 30 track display device, 101 prediction observation vector calculation unit, 103 prediction vector calculation unit, 105 smooth vector memory, 107 gate determination unit, 109 smooth vector calculation unit, 111 residual covariance Matrix calculation unit, 113 gain matrix calculation unit, 115 observation noise covariance matrix setting unit, 117 prediction error covariance matrix calculation unit, 119 driving noise covariance matrix setting unit, 121 smooth error covariance matrix calculation unit, 123 smooth error co Dispersion matrix memory, 125, 125a, 125b, 125c, 125d Observation matrix calculation unit, 127 switching control unit, 129 prediction observation vector calculation unit for gate, 131, 131a, 131b prediction observation vector calculation unit for smooth vector calculation, 133 switching Immediately preceding pseudo-smooth value calculation unit, 135 Doppler shift time constant calculation unit for data.

Claims (24)

Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
Distance, elevation angle, azimuth angle, distance change rate, elevation angle change rate, azimuth angle change rate value obtained by converting the predicted vector, which is the predicted value of the state vector, into the polar coordinate system, and distance observation error due to the effect of Doppler shift Target tracking feature that includes an observation matrix calculation means that calculates an observation matrix that takes into account the distance observation error due to the Doppler shift using the value of the Doppler shift time constant that represents the ratio of the distance change rate to the target apparatus. Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, and distance change obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation error due to Doppler shift using values of rate, elevation angle change rate, azimuth angle change rate, and the Doppler shift time constant value representing the ratio of the distance observation error due to the Doppler shift effect to the target distance change rate An observation matrix calculation means that calculates an observation matrix that does not consider a distance observation error due to Doppler shift, before calculating the observation matrix taking into account
A target tracking device comprising:   Obtain the filter switching time information from the switching control means, and if the current time is the time to switch, convert the smooth vector to the polar coordinate system for the smooth vector that is the smooth value of the state vector obtained at the previous time Using the distance, elevation angle, azimuth, distance change rate value, and Doppler shift time constant value obtained in this way, the correction to remove the error caused by the Doppler shift included in the distance direction component of the smooth vector is corrected. The target tracking device according to claim 2, further comprising a pseudo smoothing value calculation unit that does not perform correction when it is not time to perform switching. Predicted observation vector calculation means for gate that calculates a predicted observation vector for gate determination using the prediction vector at the current time calculated from the smooth vector from the pseudo-smooth value calculation means and the observation matrix from the observation matrix calculation means When,
Gate determination means for determining whether or not the observation vector is tracking target detection data using the predicted determination vector for gate determination,
Using the prediction vector at the current time calculated from the smooth vector at the previous time before correction by the pseudo smoothing value calculation means and the observation matrix from the observation matrix calculation means, the predicted observation vector for smooth vector calculation is A predicted observation vector calculation means for calculating a smooth vector to be calculated;
Smooth vector calculation for calculating a current time smooth vector using the prediction vector at the current time calculated from the smooth vector from the pseudo smooth value calculation means, the predicted observation vector for calculating the smooth vector, and the observation vector. Means,
The target tracking device according to claim 3, further comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe an observation matrix that considers the distance observation error due to the Doppler shift at the time of switching And an observation matrix calculation means for calculating both an observation matrix not considering the distance observation error due to the Doppler shift,
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means, After the time when the prediction observation vector for gate determination is calculated and switched, prediction for gate determination is performed using the prediction vector and an observation matrix in consideration of a distance observation error due to Doppler shift from the observation matrix calculation means. A predicted observation vector calculation means for a gate for calculating an observation vector;
Gate determination means for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation means for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means After calculating the predicted observation vector for smooth value calculation and after the switching time, use the prediction vector and the observation matrix taking into account the distance observation error due to the Doppler shift from the observation matrix calculation means. Smoothed vector calculation predicted observation vector calculation means for calculating a predicted observation vector of
Smoothing vector calculation means for calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for smooth value calculation from the predicted observation vector calculation means for smooth vector calculation, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means. After the time of calculating and switching, a residual covariance matrix calculating means for calculating a residual covariance matrix using an observation matrix taking into account a distance observation error due to a Doppler shift from the observation matrix calculating means;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the Kalman gain is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means. After the switching time, gain matrix calculation means for calculating Kalman gait using an observation matrix taking into account a distance observation error due to Doppler shift from the observation matrix calculation means;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the smoothing error covariance matrix using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means After the switching time, the smoothing error covariance matrix calculating means for calculating the smoothing error covariance matrix using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculating means,
A target tracking device comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that takes into account the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the switching time, and further observe the distance observation error due to the Doppler shift at the switching time An observation matrix calculation means for calculating both a matrix and an observation matrix that does not consider the distance observation error due to the Doppler shift;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means, After the time when the prediction observation vector for gate determination is calculated and switched, prediction for gate determination is performed using the prediction vector and an observation matrix in consideration of a distance observation error due to Doppler shift from the observation matrix calculation means. A predicted observation vector calculation means for a gate for calculating an observation vector;
Gate determination means for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation means for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means After the time when the predicted observation vector for calculating the smooth value is calculated and switched, the smoothed value is calculated using the prediction vector and the observation matrix in consideration of the distance observation error due to the Doppler shift from the observation matrix calculation means. The predicted observation vector is calculated, and at the time of switching, the distance, elevation angle, azimuth angle, distance change rate value obtained by converting the predicted vector into the polar coordinate system, the Doppler shift time constant value, and Is used to eliminate the error caused by the Doppler shift included in the distance direction component of the predicted position of the tracking target. A smoothing vector calculation predicted observation vector calculating means for calculating a predicted observation vector a smoothing vector calculating a target position of the orthogonal coordinate system obtained by performing,
Smoothing vector calculation means for calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for smooth vector calculation from the predicted observation vector calculation means for smooth vector calculation, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means. After the time of calculating and switching, a residual covariance matrix calculating means for calculating a residual covariance matrix using an observation matrix taking into account a distance observation error due to a Doppler shift from the observation matrix calculating means;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the Kalman gain is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means. After the switching time, gain matrix calculation means for calculating Kalman gain using an observation matrix taking into account distance observation errors due to Doppler shift from the observation matrix calculation means;
A target tracking device comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
The filter Doppler shift time constant that calculates the Doppler shift time constant for the filter that gradually increases with the passage of time after the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift A calculation means;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, and distance change obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Using the values of rate, elevation angle change rate, azimuth angle change rate and the Doppler shift time constant for the filter, an observation matrix is calculated that takes into account the distance observation error due to Doppler shift, and before the switching time, the Doppler shift An observation matrix calculation means for calculating an observation matrix that does not take into account distance observation errors;
A target tracking device comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe an observation matrix that considers the distance observation error due to the Doppler shift at the time of switching And an observation matrix calculation means for calculating both an observation matrix not considering the distance observation error due to the Doppler shift,
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means, A predicted observation vector calculation means for calculating a predicted observation vector for gate determination;
Gate determination means for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation means for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means Smoothed vector calculating predicted observation vector calculating means for calculating a predicted observation vector for calculating a smooth value;
Smoothing vector calculation means for calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for smooth value calculation from the predicted observation vector calculation means for smooth vector calculation, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means. A residual covariance matrix calculating means for calculating;
Calculate the Kalman gain using the observation matrix that does not take into account the distance observation error due to the Doppler shift from the observation matrix calculation means before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift A gain matrix calculating means;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the smoothing error covariance matrix using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means Smoothing error covariance matrix calculating means for calculating
A target tracking device comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe an observation matrix that considers the distance observation error due to the Doppler shift at the time of switching And an observation matrix calculation means for calculating both an observation matrix not considering the distance observation error due to the Doppler shift,
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means are used. , A predicted observation vector calculating means for calculating a predicted observation vector for gate determination,
Gate determination means for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation means for gate;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means, Smoothed vector calculating predicted observation vector calculating means for calculating a predicted observation vector for calculating a smooth value;
Smoothing vector calculation means for calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for smooth value calculation from the predicted observation vector calculation means for smooth vector calculation, and the observation vector;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means A residual covariance matrix calculating means for calculating
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the gain for calculating the Kalman gait using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means Matrix calculation means;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the smoothing error covariance matrix is calculated using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means. Smoothing error covariance matrix calculating means for calculating;
A target tracking device comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that takes into account the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the switching time, and further observe the distance observation error due to the Doppler shift at the switching time An observation matrix calculation means for calculating both a matrix and an observation matrix that does not consider the distance observation error due to the Doppler shift;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means, A predicted observation vector calculation means for calculating a predicted observation vector for gate determination;
Gate determination means for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation means for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation means Smoothed vector calculating predicted observation vector calculating means for calculating a predicted observation vector for calculating a smooth value;
Smoothing vector calculation means for calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for smooth vector calculation from the predicted observation vector calculation means for smooth vector calculation, and the observation vector;
Time Before switching to filter constituted by the observation model considering the distance measurement error due to the Doppler shift, using the observation matrix without considering the distance measurement error due to the Doppler shift from the observation matrix calculating means, the residual covariance matrix A residual covariance matrix calculating means for calculating;
Calculate the Kalman gain using the observation matrix that does not take into account the distance observation error due to the Doppler shift from the observation matrix calculation means before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift A gain matrix calculating means;
A target tracking device comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and velocity of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking device that estimates a state vector including the coordinate components of
A switching control means for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that takes into account the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the switching time, and further observe the distance observation error due to the Doppler shift at the switching time An observation matrix calculation means for calculating both a matrix and an observation matrix that does not consider the distance observation error due to the Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means are used. , A predicted observation vector calculating means for calculating a predicted observation vector for gate determination,
Gate determination means for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation means for gate;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means Smoothed vector calculating predicted observation vector calculating means for calculating a predicted observation vector for calculating a smooth value;
Smoothing vector calculation means for calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for smooth vector calculation from the predicted observation vector calculation means for smooth vector calculation, and the observation vector;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means A residual covariance matrix calculating means for calculating
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the gain for calculating the Kalman gain using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation means Matrix calculation means;
A target tracking device comprising:   The predicted observation vector calculation means for smooth vector calculation uses the distance, elevation angle, azimuth angle, distance change rate value obtained by converting the prediction vector into the polar coordinate system, and the Doppler shift time constant value at the time of switching. And calculating the target position of the orthogonal coordinate system obtained by performing correction for removing the error due to the influence of the Doppler shift included in the distance direction component of the predicted position of the tracking target as a predicted observation vector for calculating a smooth vector. The target tracking device according to claim 10 or 11, characterized in that: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
Distance, elevation angle, azimuth angle, distance change rate, elevation angle change rate, azimuth angle change rate value obtained by converting the predicted vector, which is the predicted value of the state vector, into the polar coordinate system, and distance observation error due to the effect of Doppler shift Target tracking feature that includes an observation matrix calculation step that calculates an observation matrix that takes into account the distance observation error due to the Doppler shift using the value of the Doppler shift time constant that represents the ratio between the target and the distance change rate of the target Method. Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, and distance change obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation error due to Doppler shift using values of rate, elevation angle change rate, azimuth angle change rate, and the Doppler shift time constant value representing the ratio of the distance observation error due to the Doppler shift effect to the target distance change rate An observation matrix calculation step that calculates an observation matrix that does not take into account a distance observation error due to Doppler shift before the time of switching,
A target tracking method characterized by comprising:   Obtain the filter switching time information from the switching control step, and if the current time is the time to switch, convert the smooth vector to the polar coordinate system for the smooth vector that is the smooth value of the state vector obtained at the previous time. Using the distance, elevation angle, azimuth, distance change rate value, and Doppler shift time constant value obtained in this way, the correction to remove the error caused by the Doppler shift included in the distance direction component of the smooth vector is corrected. The target tracking method according to claim 14, further comprising a pseudo smoothing value calculating step in which the correction is not performed when it is not time to perform switching. Predictive observation vector calculation step for gate that calculates a prediction observation vector for gate determination using the prediction vector at the current time calculated from the smooth vector from the pseudo smoothing value calculation step and the observation matrix from the observation matrix calculation step When,
Using the predicted observation vector for gate determination, a gate determination step of determining whether the observation vector is tracking target detection data;
Using the prediction vector at the current time calculated from the smoothing vector at the previous time before correction by the pseudo-smooth value calculation step and the observation matrix from the observation matrix calculation step, the predicted observation vector for calculating the smooth vector is obtained. A predicted observation vector calculation step for calculating a smooth vector to be calculated;
Smooth vector calculation for calculating a smooth vector at the current time using the predicted vector at the current time calculated from the smooth vector from the pseudo-smooth value calculating step, the predicted observation vector for calculating the smooth vector, and the observed vector. Steps,
The target tracking method according to claim 15, further comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe an observation matrix that considers the distance observation error due to the Doppler shift at the time of switching And an observation matrix calculation step for calculating both an observation matrix not considering the distance observation error due to the Doppler shift,
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step, After the time when the prediction observation vector for gate determination is calculated and switched, prediction for gate determination is performed using the prediction vector and an observation matrix in consideration of a distance observation error due to Doppler shift from the observation matrix calculation step. A predicted observation vector calculation step for a gate for calculating an observation vector;
A gate determination step for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. After calculating the predicted observation vector for smooth value calculation and after the switching time, use the predicted vector and the observation matrix taking into account the distance observation error due to the Doppler shift from the observation matrix calculation step. A predicted observation vector calculation step for calculating a smooth vector for calculating a predicted observation vector of
A smoothing vector calculation step of calculating a smoothing vector at a current time using the prediction vector, a predicted observation vector for calculating a smooth value from the predicted observation vector calculation step for calculating a smooth vector, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step. After the time of calculating and switching, a residual covariance matrix calculating step of calculating a residual covariance matrix using an observation matrix considering a distance observation error due to a Doppler shift from the observation matrix calculating step;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the Kalman gain is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step. After the switching time, using an observation matrix that takes into account a distance observation error due to Doppler shift from the observation matrix calculation step, a gain matrix calculation step for calculating Kalman gais,
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the smoothing error covariance matrix using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step After the switching time, a smoothing error covariance matrix calculating step for calculating a smoothing error covariance matrix using an observation matrix taking into account a distance observation error due to Doppler shift from the observation matrix calculating step;
A target tracking method characterized by comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe the distance observation error due to the Doppler shift at the time of switching An observation matrix calculation step for calculating both a matrix and an observation matrix that does not consider the distance observation error due to the Doppler shift;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step, After the time when the prediction observation vector for gate determination is calculated and switched, prediction for gate determination is performed using the prediction vector and an observation matrix in consideration of a distance observation error due to Doppler shift from the observation matrix calculation step. A predicted observation vector calculation step for a gate for calculating an observation vector;
A gate determination step for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. Calculate a smoothed value using the predicted vector and an observation matrix that takes into account the distance observation error due to the Doppler shift from the observation matrix calculation step after calculating the predicted observation vector for smoothing value calculation. The predicted observation vector is calculated, and at the time of switching, the distance, elevation angle, azimuth angle, distance change rate value obtained by converting the predicted vector into the polar coordinate system, the Doppler shift time constant value, and To eliminate the error caused by the Doppler shift included in the distance direction component of the predicted position of the tracking target. Correction and orthogonal coordinate system predicted observation smoothing vector calculation predicted observation vector calculation step of calculating a vector of the target position for calculating smoothing vector of obtained by performing the,
A smoothing vector calculation step of calculating a smoothing vector at a current time using the prediction vector, a predicted observation vector for calculating a smooth vector from the predicted observation vector calculation step for calculating the smooth vector, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step. After the time of calculating and switching, a residual covariance matrix calculating step of calculating a residual covariance matrix using an observation matrix considering a distance observation error due to a Doppler shift from the observation matrix calculating step;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the Kalman gain is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step. After the switching time, a gain matrix calculation step for calculating a Kalman gain using an observation matrix taking into account a distance observation error due to a Doppler shift from the observation matrix calculation step;
A target tracking method characterized by comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
The filter Doppler shift time constant that calculates the Doppler shift time constant for the filter that gradually increases with the passage of time after the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift A calculation step;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, and distance change obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Using the values of rate, elevation change rate, azimuth change rate and the Doppler shift time constant for the filter, an observation matrix is calculated that takes into account the distance observation error due to Doppler shift, and before the switching time, the Doppler shift An observation matrix calculation step for calculating an observation matrix that does not consider the distance observation error;
A target tracking method characterized by comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that takes into account the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe an observation matrix that takes into account the distance observation error due to the Doppler shift at the time of switching And an observation matrix calculation step for calculating both an observation matrix not considering the distance observation error due to the Doppler shift,
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step, A predicted observation vector calculation step for calculating a predicted observation vector for gate determination;
A gate determination step for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. A predicted observation vector calculation step for calculating a smooth vector to calculate a predicted observation vector for calculating a smooth value;
A smooth vector calculation step of calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for calculating a smooth value from the predicted observation vector calculation step for calculating the smooth vector, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step. Calculating a residual covariance matrix to be calculated;
Calculate the Kalman gain using an observation matrix that does not consider the distance observation error due to the Doppler shift from the observation matrix calculation step before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift A gain matrix calculation step;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the smoothing error covariance matrix using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step A smoothing error covariance matrix calculating step for calculating
A target tracking method characterized by comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe an observation matrix that considers the distance observation error due to the Doppler shift at the time of switching And an observation matrix calculation step for calculating both an observation matrix not considering the distance observation error due to the Doppler shift,
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. A predicted observation vector calculation step for calculating a predicted observation vector for gate determination;
A gate determination step for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation for gate;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step, A predicted observation vector calculation step for calculating a smooth vector and a predicted observation vector for calculating a smooth vector;
A smooth vector calculation step of calculating a smooth vector at the current time using the prediction vector, a predicted observation vector for calculating a smooth value from the predicted observation vector calculation step for calculating the smooth vector, and the observation vector;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step A residual covariance matrix calculating step for calculating
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the gain for calculating the Kalman gait using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step Matrix calculation step;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the smoothing error covariance matrix is calculated using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step. A smoothing error covariance matrix calculating step to calculate;
A target tracking method characterized by comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that takes into account the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the switching time, and further observe the distance observation error due to the Doppler shift at the switching time An observation matrix calculation step for calculating both a matrix and an observation matrix that does not consider the distance observation error due to the Doppler shift;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, using the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step, A predicted observation vector calculation step for calculating a predicted observation vector for gate determination;
A gate determination step for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation for gate;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. A predicted observation vector calculation step for calculating a smooth vector to calculate a predicted observation vector for calculating a smooth value;
A smoothing vector calculation step of calculating a smoothing vector at a current time using the prediction vector, a predicted observation vector for calculating a smooth vector from the predicted observation vector calculation step for calculating the smooth vector, and the observation vector;
Before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix is calculated using the observation matrix not considering the distance observation error due to the Doppler shift from the observation matrix calculation step. Calculating a residual covariance matrix to be calculated;
Calculate the Kalman gain using an observation matrix that does not take into account the distance observation error due to the Doppler shift from the observation matrix calculation step before the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift A gain matrix calculation step;
A target tracking method characterized by comprising: Based on the observation vector containing the coordinate component of the target position in the Cartesian coordinate system input from the target observation device using linear frequency modulation pulse compression, the position and speed of the tracking target in the Cartesian coordinate system using the Kalman filter A target tracking method for estimating a state vector including the coordinate components of
A switching control step for outputting a switching timing from a filter configured by an observation model not considering a distance observation error due to Doppler shift to a filter configured by an observation model considering a distance observation error due to Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to Doppler shift, the distance, elevation angle, azimuth angle, distance obtained by converting the prediction vector, which is the predicted value of the state vector, into the polar coordinate system Distance observation by Doppler shift using values of change rate, elevation angle change rate, azimuth angle change rate, and Doppler shift time constant representing the ratio of distance observation error due to Doppler shift effect and target distance change rate Calculate an observation matrix that considers the error, calculate an observation matrix that does not consider the distance observation error due to the Doppler shift before the time of switching, and further observe the distance observation error due to the Doppler shift at the time of switching An observation matrix calculation step for calculating both a matrix and an observation matrix that does not consider the distance observation error due to the Doppler shift;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. A predicted observation vector calculation step for calculating a predicted observation vector for gate determination;
A gate determination step for determining whether or not the observation vector is detection data of the tracking target using the predicted observation vector for gate determination from the predicted observation vector calculation for gate;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the prediction vector and the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step are used. A predicted observation vector calculation step for calculating a smooth vector to calculate a predicted observation vector for calculating a smooth value;
A smoothing vector calculation step of calculating a smoothing vector at a current time using the prediction vector, a predicted observation vector for smooth vector calculation from the predicted observation vector calculation step for smooth vector calculation, and the observation vector;
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the residual covariance matrix using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step A residual covariance matrix calculating step for calculating
After the time of switching to the filter configured by the observation model considering the distance observation error due to the Doppler shift, the gain for calculating the Kalman gain using the observation matrix considering the distance observation error due to the Doppler shift from the observation matrix calculation step Matrix calculation step;
A target tracking method characterized by comprising:   At the time when the predicted observation vector calculation step for smooth vector calculation is switched, the values of distance, elevation angle, azimuth angle, distance change rate obtained by converting the prediction vector into the polar coordinate system, and the value of the Doppler shift time constant are used. And calculating the target position of the orthogonal coordinate system obtained by performing correction for removing the error due to the influence of the Doppler shift included in the distance direction component of the predicted position of the tracking target as a predicted observation vector for calculating a smooth vector. 24. The target tracking method according to claim 22 or 23.

Images