JP4176985B2 - Tracking device - Google Patents

Description

【００１０】
上式（１）において、ｏは観測値を表す添字であり、ｋはサンプリング時刻を表す添字であり、ｊは探知データの数を表す添字である。つまり探知データＤ_kjは、サンプリング時刻ｋにおけるｊ番目の探知データを表す。さらに、ｘ_okjは、探知データＤ_kjのＸ座標成分を表し、同様にｙ_okjは、探知データＤ_kjのＹ座標成分、ｚ_okjは、探知データＤ_kjのＺ座標成分をそれぞれ表す。また記号「′」は、ベクトルおよび行列の転置を表す。
【００１１】
観測雑音標準偏差は、Ｓ／Ｎ比ＳＮＲ_kjを用いて、下記の（２）式、（３）式、（４）式から求める。
【００１２】
【数２】

【００１３】
【数３】

【００１４】
【数４】

【００１５】
上式（２）、（３）及び（４）におけるσｒ_kj、σｅ_kj、σｂ_kjは、それぞれ、距離の観測雑音標準偏差、仰角の観測雑音標準偏差、方位角の観測雑音標準偏差を表す。また、上式（２）、（３）及び（４）におけるα_r、α_e、α_bは、レーダの送信信号の周波数、レーダの送信信号のバンド幅、レーダアンテナの有効開口径及び光速から決定される正の定数である。
【００１６】
ゲート判定諸元算出手段１２３では、観測手段１０１から、探知データ、距離の観測雑音標準偏差、仰角の観測雑音標準偏差および方位角の観測雑音標準偏差が入力されるとともに、後述の予測手段１０７から、１サンプリング遅延手段１０８を介して、予測ベクトル、予測誤差共分散行列が入力される。さらに、ゲート判定諸元算出手段１２３では、残差および残差共分散行列の算出を行う。
【００１７】
残差および残差共分散行列の説明を以下に行う。
【００１８】
以降、残差共分散行列はＳ_kj（ｔ）と表す。Ｓ_kj（ｔ）の添字ｋはサンプリング時刻を表し、ｊ番目の探知データからのものであることを表し、（ｔ）は追尾対象目標番号を表す。残差共分散行列は、カルマンフィルタの理論に基づき、次式（５）により算出される。
【００１９】
【数５】

【００２０】
（５）式において、Ｈは観測行列、Ｐ_pk（ｔ）は後述の予測手段１０７から得られる追尾対象目標ｔにおける予測誤差共分散行列Ｒ_kjは北基準直交座標における観測誤差共分散行列を表す。
【００２１】
（５）式における観測行列Ｈは次式（６）のように表せる。
【００２２】
【数６】

【００２３】
（６）式における北基準直交座標における観測誤差共分散行列Ｒ_kjは次式（７）で表せる。
【００２４】
【数７】

【００２５】
（７）式において、Λ_kjは極座標における観測誤差共分散行列であり、Γ_kj（ｔ）は、各追尾対象目標におけるΛ_kjの変換行列である。
【００２６】
（７）式において、極座標における観測誤差共分散行列Λ_kjは、（８）式で表せる。
【００２７】
【数８】

【００２８】
（８）式における記号「²（または、＾２）」は、２乗を表す。
【００２９】
（７）式における、各追尾対象目標におけるΛ_kjの変換行列Γ_kj（ｔ）は、次式（９）で表せる。
【００３０】
【数９】

【００３１】
（９）式におけるｒ_pk（ｔ）、ｅ_pk（ｔ）、ｂ_pk（ｔ）は、それぞれ、予測ベクトルＶＸ_pk（ｔ）の位置成分ｘ_pk（ｔ）、ｙ_pk（ｔ）、ｚ_pk（ｔ）の極座標成分で表したものである。
【００３２】
ｒ_pk（ｔ）、ｅ_pk（ｔ）、ｂ_pk（ｔ）とｘ_pk（ｔ）、ｙ_pk（ｔ）、ｚ_pk（ｔ）の関係は、次式（１０）で表される。
【００３３】
【数１０】

【００３４】
残差は、探知データと予測ベクトルの位置成分の差Ｄ_kj−ＶＺ_pk（ｔ）で表される。
【００３５】
したがって、ゲート判定諸元算出手段１２３では、残差、残差共分散行列、探知データ、予測ベクトルおよび予測誤差共分散行列を、後述のゲート判定手段１２４へ入力する。
【００３６】
ゲート判定手段１２４の説明のため、ゲートについて述べる。ゲートは次式（１１）で表される誤差楕円の領域である。
【００３７】
【数１１】

【００３８】
式（１１）において、ＶＺ_pk（ｔ）は予測ベクトルの位置成分を表す。式（１１）において、ｄ_εはゲートの大きさを決めるゲートサイズパラメータである。ここで、ゲートサイズパラメータは定数であり、ゲートサイズパラメータの下添字εは、ゲート内に目標が存在する確率を表す。このゲート内に目標が存在する確率を今後、ゲート内確率、又は、ゲート内目標存在期待確率と呼ぶ。ここで、ゲートサイズパラメータｄ_εは、ゲート内確率εの時のゲートの大きさを表す。ゲートサイズパラメータｄ_εは、統計学でいうカイ２乗分布により、自由度と自分が設定したいゲート内確率εを事前に決め、カイ２乗密度関数又は、カイ２乗分布表より求める。また、（１１）式における上添字の「−１」は、行列の逆行列を表す記号である。
【００３９】
予測ベクトルＶＸ_pk（ｔ）は、次式（１２）のように表される。
【００４０】
【数１２】

【００４１】
（１２）式において、ｘ_pk（ｔ），ｙ_pk（ｔ），ｚ_pk（ｔ）は、Ｘ、Ｙ、Ｚの予測位置、ｄｘ_pk（ｔ），ｄｙｐｋ（ｔ），ｄｚｐｋ（ｔ）は、Ｘ、Ｙ、Ｚの予測速度である。
【００４２】
（１１）式における予測ベクトルの位置成分ＶＺ_pk（ｔ）と式（１２）の予測ベクトルＶＸ_pk（ｔ）は、（６）式の観測行列Ｈを用いて、次式（１３）の関係がある。
【００４３】
【数１３】

【００４４】
（１１）式で表されるゲートの大きさは、上記ゲートサイズパラメータｄ_εと残差共分散行列Ｓ_kj（ｔ）により決まる。次に、何故、ゲートサイズパラメータｄ_εと残差共分散行列Ｓ_kj（ｔ）によりゲートの大きさが決まるのかを例を用いて以下に述べる。以下説明を簡便化するため、次式（１４）及び（１５）のように、行列およびベクトルを仮定する。そうすると、（１１）式は、次式（１４）および（１５）より、下記の（１６）式のように決まる。
【００４５】
【数１４】

【００４６】
【数１５】

【００４７】
【数１６】

【００４８】
ここで、（１４）式における行列Ａは３行３列の正値対称行列なので、次式（１７）及び（１８）が成立する。
【００４９】
【数１７】

【００５０】
【数１８】

【００５１】
ここで、行列Ａを次式（１９）のように、代数学による対角行列Ｂ、直交行列Ｃで対角化できたとする。
【００５２】
【数１９】

【００５３】
式（１９）における対角行列Ｂは、次式（２０）のように、行列Ａの固有値１／λ₁、１／λ₂、１／λ₃で表されるとする。ここで、式（１８）の関係より、固有値１／λ₁、１／λ₂、１／λ₃は、各々、１／λ₁＞０、１／λ₂＞０、１／λ₃＞０である。ここで、「／」は割り算を表す記号である。
【００５４】
【数２０】

【００５５】
（１９）式における直交行列Ｂは、次式（２１）のように行列Ａの固有ベクトルｖ１アンダーバー、ｖ２アンダーバー、ｖ３アンダーバーを用いて、（２１）式のように表される。ここで、ｖ１アンダーバー、ｖ２アンダーバー、ｖ３アンダーバーは各々３行１列のベクトルである。
【００５６】
【数２１】

【００５７】
（２１）式における行列Ａの固有ベクトルｖ１アンダーバー、ｖ２アンダーバー、ｖ３アンダーバーには、式（２２）の関係がある。
【００５８】
【数２２】

【００５９】
（２０）式は、（１９）式を用いて、（２３）式のように表される。
【００６０】
【数２３】

【００６１】
ここで、（２３）式におけるＣｘの成分を次式（２４）のようにした場合、（２３）式は、式（２０）、式（２４）を用いて、次式（２５）のように表される。
【００６２】
【数２４】

【００６３】
【数２５】

【００６４】
ここで、式（１４）、式（２０）より、行列Ａの固有値は１／λ₁、１／λ₂、１／λ₃なので、代数学により、残差共分散行列Ｓ_kj（ｔ）の固有値は、λ₁、λ₂、λ₃である。
【００６５】
よって、（２５）式は、誤差楕円の半径が、残差共分散行列の固有値λ₁、λ₂、λ₃および、ゲートサイズパラメータｄ_εにより決まることを表す。つまり、ゲートの大きさは、残差共分散行列とゲートサイズパラメータｄ_εにより決まる。
【００６６】
よって、ゲートの大きさは、残差共分散行列Ｓ_kj（ｔ）が大きいほど、又、残差共分散行列の逆行列が小さいほど、ゲートの大きさは大きくなる。また、ゲートサイズパラメータが大きいほど、ゲートの大きさは大きくなる。
【００６７】
（１１）式の予測ベクトルの位置成分ＶＺ_pk（ｔ）はゲートの中心を表す。（１１）式の左辺は、探知データＤ_kjとゲートの中心である予測ベクトルの位置成分ＶＺ_pk（ｔ）の残差を、残差共分散行列Ｓ_kj（ｔ）で正規化した距離を表す。
【００６８】
したがって、ゲート判定手段１２４では、探知データＤ_kjとゲートの中心である予測ベクトルの位置成分ＶＺ_pk（ｔ）の残差Ｄ_kj−ＶＺ_pk（ｔ）および残差共分散行列Ｓ_kj（ｔ）をゲート判定諸元算出手段１２３から入力を行い、ゲートである（１１）式を満たすかどうかの判定を行い、（１１）式を満たす探知データＤｋｊを目標信号の候補とする。また、（１１）を満たす探知データＤ_kjが複数あった場合、予測ベクトルに最も近い探知データ１つをその目標信号の候補ａＤ_kjとして出力する。さらに、その目標信号の候補をａＤ_kjに対応する残差共分散行列をａＳ_kj（ｔ）とする。つまり、データ更新手段１０６へは、ゲート内の探知データａＤ_kjと、ａＤ_kjに対応する残差共分散行列ａＳ_kj（ｔ）、予測誤差共分散行列、予測ベクトル、予測ベクトルの位置成分ＶＺ_pk（ｔ）を入力する。もし、ゲート内に探知データが入らない場合、つまりメモリトラックの場合には、ゲート内探知データａＤ_kjは、予測ベクトルの位置成分とし、ａＤ_kjに対応する残差共分散行列ａＳ_kj（ｔ）は、事前に設定したＳ／Ｎを用いて、算出を行なう。
【００６９】
メモリトラックとは、ゲート内に探知データが入らずにデータ更新を行っていく処理である。
【００７０】
データ更新手段１０６では、カルマンフィルタの理論に基づき、ゲイン行列Ｋ_k（ｔ）を次式（２６）から算出する。また、平滑ベクトルＶＸ_sk（ｔ）を次式（２６）のゲイン行列Ｋ_k（ｔ）を用いて、（２７）式により算出する。さらに、平滑誤差共分散行列Ｐ_sk（ｔ）を（２６）式のゲイン行列Ｋ_k（ｔ）を用いて、式（２８）により算出する。さらに、データ更新手段１０６では、（２７）式、（２８）式により算出された平滑ベクトルＶＸ_sk（ｔ）および平滑誤差共分散行列Ｐ_sk（ｔ）を出力する。
【００７１】
【数２６】

【００７２】
【数２７】

【００７３】
【数２８】

【００７４】
予測手段１０７では、カルマンフィルタの理論に基づき、次式（２９）により、現時刻ｋより１サンプリング後の時刻ｋ＋１における予測ベクトルＶＸｐｋ＋１（ｔ）を算出し、出力する。また、次式（３０）により、現時刻ｋより１サンプリング後の時刻ｋ＋１における予測誤差共分散行列Ｐ_pk（ｔ）を算出し、出力する。
【００７５】
【数２９】

【００７６】
【数３０】

【００７７】
（２９）式において、１サンプリング外挿を行う状態遷移行列Φｋは、（３１）式で表される。
【００７８】
【数３１】

【００７９】
（３１）式におけるΔｔは現時刻ｋと現時刻から１サンプリング後の時刻ｋ＋１までのサンプリング間隔を表す。
【００８０】
（３０）式における駆動雑音の共分散行列Ｑｋは、（３２）式で表される。
【００８１】
【数３２】

【００８２】
（３２）式におけるσ_aは駆動雑音の標準偏差を表す。駆動雑音の標準偏差はカルマンフィルタの目標の運動モデルの曖昧さを表す定数である。
【００８３】
１サンプリング遅延手段１０８は、予測手段１０７から入力された、サンプリング時刻ｋの予測ベクトルＶＸ_pk（ｔ）、予測誤差共分散行列Ｐ_pk（ｔ）を１サンプリング分遅延させ、表示手段１０９に入力する。
【００８４】
表示手段１０９では、データ更新手段１０６から入力された、平滑ベクトルを用いて、過去の数サンプリング分の平滑ベクトルの位置成分を線で結んで航跡として表示を行う。また、平滑ベクトルの速度成分を用いて、平滑ベクトルの位置成分を始点として、終点が次サンプリングの目標予測位置を示すように、速度の表示を行う。
【００８５】
従来の追尾装置の構成を示す概略図を図４６に示す。
【００８６】
上記のような追尾装置では、ゲートの大きさの調整が難しい。以下、その理由を示すために、図４７を用いて説明していく。
【００８７】
図４７は、ゲートと目標予測位置、目標観測位置、目標レンジ方向および目標クロスレンジ方向の関係図である。
【００８８】
図４７において、Ｇ１は時刻ｔ１におけるゲートを表す。また、Ｇ１’は時刻ｔ１におけるＧ１よりも大きいゲートサイズパラメータを設定しているゲートを表す。ここで、ゲートＧ１とゲートＧ１’のゲートの形状は同じである。さらに、Ｏ１は時刻ｔ１における目標観測位置、Ｐ１は時刻ｔ１における目標予測位置、ＴＧＴ１は時刻ｔ１における目標真位置、Ｃ１、１〜Ｃ１、９は、不要信号を表す。また、Ｏ２は時刻ｔ２における目標観測位置、Ｐ２は時刻ｔ２における目標予測位置、ＴＧＴ２は時刻ｔ２における目標真位置、Ｃ１、１〜Ｃ１、１１は、不要信号を表す。目標は、ＴＧＴ１からＴＧＴ２へと直進から旋回を行う。
【００８９】
図４７において、センサＳとＲ１を結んだ方向、あるいは、センサＳとＲ２を結んだ方向を目標レンジ方向と呼ぶ。また、目標レンジ方向と直交する軸を目標クロスレンジ方向と呼ぶ。
【００９０】
例えば、図４７において、初めに、図４７の実線の楕円のように、目標ｔ２におけるゲートＧ２をはった場合、目標観測位置Ｄ２は、レンジ方向に外している。そこで、ゲートＧ２を構成しているゲートサイズパラメータよりも大きいゲートサイズパラメータ設定して、目標観測位置Ｄ２が入るように、破線の楕円のように、ゲートＧ２’を構成する。そうすると、レンジ方向に外している目標観測位置Ｄ２は、ゲートＧ２’で補足できるものの、ゲートＧ２’は、ゲートＧ２と同じ形状を保ったまま大きくなるので、クロスレンジ方向のゲート半径が大きくなってしまう。
【００９１】
ここで、一般に、図４８のように、角度誤差に起因するクロスレンジ誤差は、センサ−目標間距離が遠くなる程、レンジ誤差に比べて、大きくなる。例えば、目標距離が数百キロメートルのオーダの場合、レンジ誤差とクロスレンジ誤差の比は、１対１０００から１００００程度の比率になる。したがって、ゲートサイズパラメータを大きくした場合、クロスレンジ方向のゲート半径の方が、レンジ方向のゲート半径よりも、かなり大きくなる。
【００９２】
そのため、図４７に示した例のように、ゲートの大きさが小さいゲートＧ２（図の実線）の場合は不要信号が入らないケースでも、ゲートの大きさが大きいゲートＧ２’（図の破線）内に不要信号が多数入ってしまうといった問題がある。
【００９３】
【発明が解決しようとする課題】
従来の追尾装置は以上のように構成されており、角度誤差に起因するクロスレンジ誤差は、センサ−目標間距離が遠くなる程、レンジ誤差に比べて、大きくなるので、ゲートサイズパラメータを大きくした場合、クロスレンジ方向のゲート半径の方が、レンジ方向のゲート半径よりも、かなり大きくなってしまい、大きさの小さいゲートの場合は不要信号が入らないケースでも、ゲートの大きさが大きいゲートＧ２’内には不要信号が多数入ってしまうという問題点があった。
【００９４】
この発明は、かかる問題点を解決するためになされたものであり、目標信号をとりやすく、不要信号を侵入させにくくするためのゲート判定を行うことが可能な追尾装置を得ることを目的とする。
【００９５】
【課題を解決するための手段】
この発明は、受信電力を元に、信号対雑音比であるＳ／Ｎ比を得て、それを距離、仰角および方位角の観測雑音標準偏差に変換し、前記観測雑音標準偏差とそれらに付随する位置を表す探知データとを出力する観測手段と、予測誤差共分散行列および前記探知データが入力され、それらから算出される残差共分散行列から、距離方向の固有値、仰角方向の固有値、方位角方向の固有値、距離方向の固有ベクトル、仰角方向の固有ベクトルおよび方位角方向の固有ベクトルを算出する対角化を行う第１のゲート判定諸元算出手段と、前記探知データが距離方向において所定次元の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力するレンジ方向のゲート判定手段と、前記観測手段及び前記第１のゲート判定諸元算出手段から出力されるデータを用いて、仰角方向及び方位角方向で、所定次元の楕円体ゲート判定を行うクロスレンジ方向のゲート判定手段と、前記距離方向での前記所定次元の楕円体ゲートに入っているか否かの判定結果を示す信号と、仰角方向および方位角方向での前記所定次元の楕円体ゲートに入っているか否かの判定結果を示す信号とから、距離方向、仰角方向および方位角方向のすべてにおいて前記所定次元の楕円体ゲート内に前記探知データが入っている場合に、目標信号の候補として、その探知データに付随する探知データおよび残差共分散行列を出力する第１のゲート判定手段と、前記第１のゲート判定手段から出力される前記探知データおよび前記残差共分散行列が入力され、カルマンフィルタの理論に基づいてデータの更新を行い、平滑誤差共分散行列および平滑ベクトルを出力する第１のデータ更新手段と、前記カルマンフィルタの理論に基づき、現時刻よりも１サンプリング後の予測誤差共分散行列および予測ベクトルを算出する予測手段と、前記予測手段から入力される予測誤差共分散行列および予測ベクトルを１サンプリング遅延させて、前記第１のゲート判定諸元算出手段に対して前記予測誤差共分散行列を出力する１サンプリング遅延手段と、前記第１のデータ更新手段から入力される平滑ベクトルから、目標航跡および目標の速度の表示を行う表示手段と、を備え、前記所定次元の楕円体ゲートは、前記残差共分散行列を元にした所定次元の楕円体ゲートであって、前記第１のゲート判定諸元算出手段は、前記楕円体ゲートにおいて、前記残差共分散行列の対角化によりレンジ方向とクロスレンジ方向とを決定し、前記レンジ方向のゲート判定手段および前記クロスレンジ方向のゲート判定手段は、前記第１のゲート判定諸元算出手段によって計算された前記レンジ方向および前記クロスレンジ方向に基づいて、前記レンジ方向および前記クロスレンジ方向のゲートサイズパラメータを設定することにより、前記楕円体ゲートのサイズを決定する追尾装置である。
【００９６】
また、前記クロスレンジ方向のゲート判定手段が、探知データが仰角方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力し、探知データが方位角方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力する第１のクロスレンジ方向のゲート判定手段から構成されている。
【００９７】
また、前記クロスレンジ方向のゲート判定手段が、仰角方向の固有ベクトル、仰角方向の固有値、方位角方向の固有ベクトルおよび方位角方向の固有値を用いて、クロスレンジ方向で、２次元のゲート判定を行う第２のクロスレンジ方向のゲート判定手段から構成されている。
【００９８】
また、前記第１のデータ更新手段から出力される前記平滑ベクトルの速度成分がある閾値より大きくなった場合、駆動雑音ベクトルの標準偏差を大きく設定して前記予測手段に出力する第１の駆動雑音制御手段をさらに備えている。
【００９９】
また、駆動雑音ベクトルの標準偏差の大きさを縦軸として、平滑ベクトルの速度成分を横軸としたロジスティック曲線に基づき、平滑ベクトルの速度成分の大きさに従って、駆動雑音ベクトルの標準偏差を大きく設定するように切り替える第２の駆動雑音制御手段をさらに備えている。
【０１００】
また、この発明は、受信電力を元に、信号対雑音比であるＳ／Ｎ比を得て、それを距離、仰角および方位角の観測雑音標準偏差に変換し、前記観測雑音標準偏差とそれらに付随する位置を表す探知データとを出力する観測手段と、予測誤差共分散行列および前記探知データが入力され、それらから算出される残差共分散行列から、距離方向の固有値、仰角方向の固有値、方位角方向の固有値、距離方向の固有ベクトル、仰角方向の固有ベクトルおよび方位角方向の固有ベクトルを算出する対角化を行う第１のゲート判定諸元算出手段と、前記探知データが距離方向の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力するレンジ方向のゲート判定手段と、前記探知データが仰角方向の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力し、探知データが方位角方向の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力する第１のクロスレンジ方向のゲート判定手段と、前記レンジ方向のゲート判定手段および前記第１のクロスレンジ方向のゲート判定手段における前記探知データのゲート判定の結果、ゲート内の探知データについて、ゲート中心と探知データの距離を正規分布の尤度に換算し、その尤度を用いて、データ更新を行い、平滑誤差共分散行列および平滑ベクトルを出力する第２のデータ更新手段と、カルマンフィルタの理論に基づき、現時刻よりも１サンプリング後の予測誤差共分散行列および予測ベクトルを算出する予測手段と、前記予測手段から入力される予測誤差共分散行列および予測ベクトルを１サンプリング遅延させて、前記第１のゲート判定諸元算出手段に対して出力する、１サンプリング遅延手段と、前記第２のデータ更新手段から入力される平滑ベクトルから、目標航跡および目標の速度の表示を行う表示手段と、を備え、前記距離方向の楕円体ゲートと、前記仰角方向の楕円体ゲートと、前記方位角方向の楕円体ゲートとは、組み合わされて、所定次元の楕円体ゲートを構成し、前記所定次元の楕円体ゲートは、前記残差共分散行列を元にした所定次元の楕円体ゲートであって、前記第１のゲート判定諸元算出手段は、前記楕円体ゲートにおいて、前記残差共分散行列の対角化によりレンジ方向とクロスレンジ方向とを決定し、前記レンジ方向のゲート判定手段および前記第１のクロスレンジ方向のゲート判定手段は、前記第１のゲート判定諸元算出手段によって計算された前記レンジ方向および前記クロスレンジ方向に基づいて、前記レンジ方向および前記クロスレンジ方向のゲートサイズパラメータを設定することにより、前記楕円体ゲートのサイズを決定する追尾装置である。
【０１０１】
また、サンプリング間隔により、ゲートサイズパラメータを設定する、第１のゲートサイズパラメータ設定手段をさらに備えている。
【０１０２】
また、距離の観測雑音標準偏差、仰角の観測雑音標準偏差および方位角の観測雑音標準偏差を固定の値に設定する観測誤差固定手段をさらに備えている。
【０１０３】
また、サンプリング間隔を固定する制御信号を予測手段に入力し、予測誤差共分散行列および予測ベクトルの算出の際のサンプリング間隔を固定するサンプリング間隔固定手段をさらに備えている。
【０１０４】
また、追尾初期から数サンプリング分、ゲートの中心を固定するゲート中心固定手段をさらに備えている。
【０１０５】
また、前記第１のデータ更新手段から得られる平滑ベクトルの速度成分の大きさを用いて、ゲートサイズパラメータの大きさを変える、ゲートサイズパラメータ可変手段をさらに備えている。
【０１０６】
また、前記第１のデータ更新手段から得られる平滑ベクトルを時定数を固定とした表示用フィルタに入力して、表示用平滑ベクトルを算出する、第１の表示用平滑手段をさらに備えている。
【０１０７】
また、前記第１のデータ更新手段から得られる平滑ベクトルを時定数を可変とした表示用フィルタに入力して、表示用平滑ベクトルを算出する、第２の表示用平滑手段をさらに備えている。
【０１０９】
また、前記第１のゲート判定諸元算出手段から入力される固有値の内、固有値の最大値と固有値の最小値の比である条件数を用いて、条件数がある閾値以下ならば、追尾の演算が不安定になっているとの危険信号を、前記表示手段に入力する条件数判定手段をさらに備えている。
【０１１０】
【発明の実施の形態】
実施の形態１．
この発明の実施の形態１に係わる追尾装置を図面を参照しながら説明する。図１は、この発明の実施の形態に係わる追尾装置の構成を示すブロック図である。なお、以下の各実施の形態において、図中の同一符号は、同一または相当部分を表す。
【０１１１】
図１において、１は、センサから得られる受信電力を元に、信号対雑音比であるＳ／Ｎ比を得て、それを距離、仰角および方位角の観測雑音標準偏差に変換し、当該観測雑音標準偏差とそれらに付随する位置を表す探知データとを出力する観測手段である。２は、後述する１サンプリング遅延手段８から予測誤差共分散行列および観測手段１から観測誤差共分散行列が入力され、それらから算出される残差共分散行列から、距離方向の固有値、仰角方向の固有値、方位角方向の固有値、距離方向の固有ベクトル、仰角方向の固有ベクトルおよび方位角方向の固有ベクトルを算出する第１のゲート判定諸元算出手段である。３は、探知データが距離方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力するレンジ方向のゲート判定手段である。４は、探知データが仰角方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力し、探知データが方位角方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力する第１のクロスレンジ方向のゲート判定手段である。５は、距離方向の１次元のゲートに入っているか否かの判定結果を示す信号と、仰角方向の１次元のゲートに入っているか否かの判定結果を示す信号と、方位角方向の１次元ゲートに入っているか否かの判定結果を示す信号とから、距離方向、仰角方向、方位角方向のすべての１次元のゲート内に前記探知データが入っている場合に、目標信号の候補として、その探知データに付随する探知データおよび残差共分散行列を出力する第１のゲート判定手段である。６は、カルマンフィルタの理論に基づいてデータの更新を行い、平滑誤差共分散行列および平滑ベクトルを出力する第１のデータ更新手段である。７は、カルマンフィルタの理論に基づき、現時刻よりも１サンプリング後の予測誤差共分散行列および予測ベクトルを算出する予測手段である。８は、予測手段７から入力される予測誤差共分散行列および予測ベクトルを１サンプリング遅延させて、第１のゲート判定諸元算出手段２に対して出力する、１サンプリング遅延手段である。９は、第１のデータ更新手段から入力される平滑ベクトルから、目標航跡および目標の速度の表示を行う表示手段である。なお、６，７，８，９は、それぞれ、上述した図４５の１０６，１０７，１０８，１０９に相当するものであるため、ここでは詳細な説明を省略する。
【０１１２】
第１のゲート判定諸元算出手段２の説明を以下に行う。
【０１１３】
第１のゲート判定諸元算出手段２では、観測手段１から、探知データ、距離の観測雑音標準偏差、仰角の観測雑音標準偏差および方位角の観測雑音標準偏差が入力され、予測手段７から、１サンプリング遅延手段８を介して、予測ベクトル及び予測誤差共分散行列が入力される。
【０１１４】
さらに、第１のゲート判定諸元算出手段２では、レンジ方向の固有ベクトル、クロスレンジ方向の固有ベクトルを算出するが、その算出方法を以下に示す。
【０１１５】
図４９に、レンジ方向の固有ベクトルとクロスレンジ方向の固有ベクトルの
関係図を示す。
【０１１６】
図４９における３次元楕円領域がゲートである。図４９において、Ａは予測位置、Ｂはゲート内の観測位置を表し、それらの位置ベクトルをＶＺ_pk（ｔ）、Ｄ_kjとする。また、距離方向（ＯＡ方向）の単位ベクトルをＵｒ、ｚ方向の単位ベクトルをＵｅｌ、水平面内の距離と直交するクロスレンジ横方向（ＨＩ方向）の単位ベクトルをＵｂｙとする。さらに、ゲート内の直交するＯＧ、ＣＤ、ＥＦをｘ１、ｘ２、ｘ３軸とする。ｘ１、ｘ２、ｘ３は、（２５）式におけるｘ₁、ｘ₂、ｘ₃と対応している。ｘ１、ｘ２、ｘ３の各軸の方向は、（２１）式における固有ベクトルｖ₁アンダーバー、ｖ₂アンダーバー、ｖ₃アンダーバーによりそれぞれ規定される。
【０１１７】
以下、固有ベクトルｖ₁アンダーバー、ｖ₂アンダーバー、ｖ₃アンダーバーを距離方向の固有ベクトルｖ_rアンダーバー、仰角方向の固有ベクトルｖ_el、方位角方向の固有ベクトルｖ_byとして割り当てる方法を示す。ここで、距離方向はレンジ方向、仰角方向はクロスレンジ縦方向、方位角方向は、クロスレンジ横方向と同じ意味である。
【０１１８】
固有ベクトルｖ₁アンダーバー、ｖ₂アンダーバー、ｖ₃アンダーバーの内、距離方向の単位Ｕ_rとの内積が一番大きい固有ベクトルを距離方向の固有ベクトルと定義する。また、その固有ベクトルに対応する固有値をλ_rとする。つまり、次式（３３）により決定する。
【０１１９】
【数３３】

【０１２０】
（３３）式における、ｍａｘは、内積Ｕ_r・ｖ₁，Ｕ_r・ｖ₂、Ｕ_r・ｖ₃の内、最も大きいものを選ぶ働きをする。
【０１２１】
ただし、Ｕ_rは、次式（３４）で定義される。
【０１２２】
【数３４】

【０１２３】
ここで、（３４）式における記号「｜｜」は、代数学におけるユークリッドノルムを表す。
【０１２４】
次に、距離方向の固有ベクトルとして選択されたものを除いた、固有ベクトルｖ₁アンダーバー、ｖ₂アンダーバー、ｖ₃アンダーバーの内、残りの２つの固有ベクトルをそれぞれｖ₁’、ｖ₂’とする。ｖ１’ｖ２’の中で、仰角方向の単位ベクトルＵ_elとの内積が一番大きい固有ベクトルを、仰角方向の固有ベクトルと呼称し、ｖ_elとする。また、その固有値をλ_elとする。ここで、ｖ_elは、（３５）式により決定する。また、仰角方向の単位ベクトルＵ_elは、（３６）式で定義される。
【０１２５】
【数３５】

【０１２６】
【数３６】

【０１２７】
固有ベクトルｖ₁アンダーバー、ｖ₂アンダーバー、ｖ₃アンダーバーの内、距離方向の固有ベクトル、仰角方向の固有ベクトルとして、選択した２つを除いた、残り１つの固有ベクトルを、方位角方向の固有ベクトルと呼称し、ｖ_byとする。また、その固有値をλ_byとする。
【０１２８】
以上の処理により、距離方向の固有ベクトル、仰角方向の固有ベクトル、方位角方向の固有ベクトルを決定する。つまり、レンジ方向の固有ベクトルは、距離方向の固有ベクトル、クロスレンジ方向は、仰角方向の固有ベクトル、方位角方向の固有ベクトルである。よって、距離方向の固有ベクトル、方位角方向の固有ベクトル、仰角方向の固有ベクトルの各方向の残差は、それぞれ、ｖ_r・ｘ、ｖ_el・ｘ、ｖ_by・ｘで表される。
【０１２９】
したがって、第１のゲート判定諸元算出手段２では、観測手段１から得た探知データおよび予測手段７から１サンプリング遅延手段８を介して得た予測ベクトルを用いて、残差を（１５）式により算出し、残差共分散行列を（１６）式により算出を行う。さらに、距離方向の固有値λ_r、距離方向の固有ベクトルの残差ｖ_r・ｘ、また、予測ベクトル、予測誤差共分散行列、探知データ、残差および残差共分散行列をレンジ方向のゲート判定手段３に入力する。同様に、仰角方向の固有値λｅｌ、仰角方向の固有ベクトルの残差ｖ_el・ｘ、方位角方向の固有値λ_by、方位角方向の固有ベクトルの残差ｖ_by・ｘ、また、予測ベクトル、予測誤差共分散行列、探知データ、残差および残差共分散行列を第１のクロスレンジ方向のゲート判定手段４に入力する。
【０１３０】
次に、レンジ方向のゲート判定手段３の説明を行う。
【０１３１】
レンジ方向のゲート判定手段３では、（３７）式を用いて、探知データが、距離方向の１次元ゲートに入っているかどうかを判定する。ここで、（３７）式の右辺のｄ_rは、距離方向で事前に設定するゲートサイズパラメータである。
【０１３２】
【数３７】

【０１３３】
したがって、レンジ方向のゲート判定手段３では、（３７）式の距離ゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差共分散行列、探知データ、残差および残差共分散行列を第１のゲート判定手段５に入力する。
【０１３４】
次に、第１のクロスレンジ方向のゲート判定手段４の説明を行う。
【０１３５】
第１のクロスレンジ方向のゲート判定手段４では、次式（３８）を用いて、探知データが、仰角方向の１次元ゲートに入っているかどうかを判定する。ここで、（３８）式の右辺のｄ_elは、仰角方向で事前に設定するゲートサイズパラメータである。
【０１３６】
【数３８】

【０１３７】
また、第１のクロスレンジ方向のゲート判定手段４では、（３９）式を用いて、探知データが、方位角方向の１次元ゲートに入っているかどうかを判定する。ここで、（３９）式の右辺のｄ_byは、方位角方向で事前に設定するゲートサイズパラメータである。
【０１３８】
【数３９】

【０１３９】
したがって、第１のクロスレンジ方向のゲート判定手段４では、（３８）式の仰角ゲートに入っているか否かを示す判定信号、（３９）式の方位角ゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差共分散行列、探知データ、残差および残差共分散行列を第１のゲート判定手段５に入力する。
【０１４０】
第１のゲート判定手段５では、レンジ方向のゲート判定手段３から、（３７）式の距離ゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差共分散行列および探知データが入力される。また、第１のクロスレンジ方向のゲート判定手段４では、クロスレンジ方向のゲート判定手段４から、（３８）式の仰角ゲートに入っているか否かを示す判定信号、（３９）式の方位角ゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差共分散行列および探知データが入力される。
【０１４１】
第１のゲート判定手段５では、（３７）式の距離ゲートに入っているか否かを示す判定信号、（３８）式の仰角ゲートに入っているか否かを示す判定信号、（３９）式の方位角ゲートに入っているか否かを示す判定信号、の全てがゲートに入っているという信号である場合、これらの判定信号に対応する探知データＤ_kjを目標信号の候補とする。また、（３７）式の距離ゲートに入っているか否かを示す判定信号、（３８）式の仰角ゲートに入っているか否かを示す判定信号、（３９）式の方位角ゲートに入っているか否かを示す判定信号、の全てがゲートに入っているという信号が複数ある場合の、複数の探知データに関しては、予測ベクトルに最も近い探知データ１つをその目標信号の候補ａＤ_kjとして、出力する。また、探知データａＤ_kjに対応する残差共分散行列ａＳ_kj（ｔ）も出力する。また、予測ベクトルの位置成分は、（１３）式により算出する。
【０１４２】
したがって、第１のゲート判定手段５は、第１のデータ更新手段６へ、探知データの中の目標信号の候補ａＤ_kjと、ａＤ_kjに対応する残差共分散行列ａＳ_kj（ｔ）と、予測ベクトルの位置成分、予測誤差共分散行列、予測ベクトル、残差および残差共分散行列を入力する。
【０１４３】
したがって、従来の追尾装置では、（１１）式を用いて、ゲート判定処理を行っているので、図４７のように、レンジ方向の探知データを補足するため、ゲートサイズパラメータを大きくすると、ゲート全体が大きくなってしまい、特に、クロスレンジ方向で、ゲートが広がるため、不要信号を多数拾ってしまうといった問題があった。しかし、（３７）式、（３８）式、（３９）式のように、レンジ方向、クロスレンジ方向でそれぞれ、別々にゲート判定を行うことによって、レンジ方向、クロスレンジ方向独立にゲートの大きさを設定することができる。そのため、レンジ方向にだけゲートを広げたい時でも、クロスレンジ方向には影響がないので、目標信号を補足して、不要信号をなるべく補足しないという意味で最適に、ゲートを設定することが可能である。
【０１４４】
次に、図３及び図４は、この実施の形態１の動作を説明するフローチャートである。
【０１４５】
まず、ステップＳＴ１において、センサから得られるＳ／Ｎおよび探知データを入力する。
【０１４６】
次に、ステップＳＴ２において、残差および残差共分散行列を算出する。
【０１４７】
次に、ステップＳＴ３において、残差共分散行列から、固有値、固有ベクトルを算出する。
【０１４８】
次に、ステップＳＴ４において、残差共分散行列から算出される３つの固有ベクトルと、レンジ方向の単位ベクトルの内積を各々計算する。
【０１４９】
次に、ステップＳＴ５において、レンジ方向の単位ベクトルと固有ベクトルとの内積が一番大きいものをレンジ方向の固有ベクトルと決定する。レンジ方向の固有ベクトルとは、距離方向の固有ベクトルと同義である。３つの固有ベクトルの内、レンジ方向の固有ベクトルを除いた２つのクロスレンジ方向の固有ベクトルをステップＳＴ６に入力する。
【０１５０】
次に、ステップＳＴ６において、２つのクロスレンジ方向の固有ベクトルとｚ軸方向の単位ベクトルの内積を各々計算して、ステップＳＴ７に入力する。
【０１５１】
次に、ステップＳＴ７において、２つのクロスレンジ方向の固有ベクトルの内、ｚ軸方向の単位ベクトルと固有ベクトルの内積が一番大きいものをクロスレンジ縦方向の固有ベクトルと決定し、残りの１つの固有ベクトルをクロスレンジ横方向の固有ベクトルと決定する。クロスレンジ縦方向の固有ベクトルとは、仰角方向の固有ベクトルと同義である。同様にして、クロスレンジ横方向の固有ベクトルとは、方位角方向の固有ベクトルと同義である。
【０１５２】
次に、ステップＳＴ８において、レンジ方向とクロスレンジ方向の固有ベクトルから、レンジ方向、クロスレンジ縦方向、クロスレンジ横方向の残差を算出する。
【０１５３】
次に、ステップＳＴ９において、レンジ方向、クロスレンジ縦方向、クロスレンジ横方向のゲートサイズパラメータを各々設定する。
【０１５４】
次に、ステップＳＴ１０において、（３７）式、（３８）式、（３９）式を満たす場合、すなわち、レンジ方向、クロスレンジ縦方向、クロスレンジ横方向のすべてのゲートを満たす場合、ステップＳＴ１１において、探知データをゲート内探知データと判定する。また、（３７）式、（３８）式、（３９）式を満たさない場合、すなわち、レンジ方向、クロスレンジ縦方向、クロスレンジ横方向の中で、１つでもゲートを満たさない場合、ステップＳＴ１２において、探知データをゲート外探知データと判定する。
【０１５５】
次に、ステップＳＴ１３において、サンプリング時刻ｋにおける全探知データの個数Ｎｏｂｓ回、ＳＴ１〜ＳＴ１２までの処理を数えるカウンタｎがｎ＝Ｎｏｂｓになったら、ステップＳＴ１４に進む。もし、ｎ＝Ｎｏｂｓでなかったら、ＳＴ１〜ＳＴ１２の処理を継続する。
【０１５６】
次に、ステップＳＴ１４において、ゲート内探知データの内、ゲートの中心に一番近いものを１つ選択し、選択されたゲート内探知データとそれに付随する残差共分散行列を次のステップＳＴ１５に入力する。
【０１５７】
次に、ステップＳＴ１５において、カルマンフィルタの理論に基づき、データ更新を行ない、平滑ベクトル、平滑誤差共分散行列を算出する。さらに、その現時刻の平滑ベクトル、平滑誤差共分散行列を用いて、１サンプリング外挿を行い、現時刻より１サンプリング後の予測ベクトル、予測誤差共分散行列を算出する。
【０１５８】
次に、ＳＴ１６では、現時刻の平滑ベクトルを用いて、平滑位置および平滑速度の表示を行う。
【０１５９】
次に、ステップＳＴ１７では、サンプリング時刻ｋがサンプリング終了時刻ｋｅｎｄより大きくなったら、処理を中断する。また、サンプリング時刻ｋがサンプリング終了時刻ｋｅｎｄ以下の場合には、処理継続を継続する。
【０１６０】
以上のように、本実施の形態に係る目標追尾装置においては、、目標信号が入りやすく、不要信号を侵入させにくくするためのゲート判定を行うことが可能である。
【０１６１】
実施の形態２．
この発明の実施の形態２に係わる追尾装置を図面を参照しながら説明する。図５は、この発明の実施の形態２に係わる追尾装置の構成を示すブロック図である。図５において、１０は、第２のクロスレンジ方向のゲート判定手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０１６２】
第２のクロスレンジ方向のゲート判定手段１０を以下に説明する。第２のクロスレンジ方向のゲート判定手段１０では、第１のゲート判定諸元算出手段２から仰角方向の固有値λ_el、仰角の固有ベクトルの残差ｖ_el・ｘ、方位角方向の固有値λ_by、方位角の固有ベクトルの残差ｖ_by・ｘ、予測ベクトル、予測誤差共分散行列、探知データ、残差、残差共分散行列が入力される。さらに、次式（４０）を用いて、２次元のゲート判定を行う。
【０１６３】
【数４０】

【０１６４】
（４０）式において、ゲートサイズパラメータｄ_elbyは事前に設定しておく。
【０１６５】
したがって、第２のクロスレンジ方向のゲート判定手段１０では、（４０）式のゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差共分散行列、探知データ、残差および残差共分散行列を第１のゲート判定手段５に入力する。
【０１６６】
第１のゲート判定手段５では、レンジ方向のゲート判定手段３から、（３７）式の距離ゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差および共分散行列が入力される。また、第２のクロスレンジ方向のゲート判定手段１０から、（４０）式の距離ゲートに入っているか否かを示す判定信号、予測ベクトル、予測誤差および共分散行列が入力される。（３７）式のゲートに入っているか否かを示す判定信号、（４０）式のゲートに入っているか否かを示す判定信号の全てがゲートに入っているという信号である場合、これらの判定信号に対応する探知データＤｋｊを目標信号の候補とする。また、（３７）式の距離ゲートに入っているか否かを示す判定信号、（４０）式のゲートに入っているか否かを示す判定信号の全てがゲートに入っているという信号が複数ある場合の複数の探知データに関しては、予測ベクトルに最も近い探知データ１つをその目標信号の候補ａＤ_kjとして、出力する。また、探知データａＤ_kjに対応する残差共分散行列ａＳ_kj（ｔ）も出力する。また、予測ベクトルの位置成分は、（１３）式により算出する。
【０１６７】
（３７）式および（４０）式によりゲート判定を行うことにより、ゲート半径の比が大きく異なるレンジ方向とクロスレンジ方向で、独立にゲート判定が可能となり、図４７のように、レンジ方向のゲートを大きくした時、クロスレンジ方向も合わせて大きくなることが防げる。また、（４０）式により、ゲート判定を行うことにより、クロスレンジ方向縦方向のゲートサイズパラメータ、クロスレンジ横方向のゲートサイズパラメータ、すなわち、仰角方向のゲートサイズパラメータ、方位角方向のゲートサイズパラメータと２つ設定せずに、ゲートサイズパラメータの設定が１回で済む。
【０１６８】
次に図６及び図７に実施の形態２の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ここでは、ステップＳＴ８ｂおよびステップＳＴ９ｂについてのみ説明する。
【０１６９】
ステップＳＴ８ｂにおいて、クロスレンジ縦方向の残差をクロスレンジ縦方向の固有値で割ったものと、クロスレンジ横方向の残差をクロスレンジ横方向の固有値で割ったものを足したものを算出する。
【０１７０】
ステップＳＴ９ｂにおいて、（３７）式におけるレンジ方向のゲートに設定するゲートサイズパラメータｄ_rと、（４０）式における２次元のクロスレンジ方向のゲートに設定するゲートサイズパラメータｄ_elbyの設定を行う。
【０１７１】
したがって、以上のステップにより、（３７）式および（４０）式によりゲート判定を行うことにより、ゲート半径の比が大きく異なるレンジ方向とクロスレンジ方向で、独立にゲート判定が可能となるため、図５０のように、レンジ方向のゲートを大きくした時、クロスレンジ方向も合わせて大きくなることが防げる。また、（４０）式により、ゲート判定を行うことにより、クロスレンジ方向縦方向のゲートサイズパラメータ、クロスレンジ横方向のゲートサイズパラメータ、すなわち、仰角方向のゲートサイズパラメータ、方位角方向のゲートサイズパラメータと２つ設定せずに、ゲートサイズパラメータの設定が１回で済む。
【０１７２】
実施の形態３．
この発明の実施の形態３に係わる追尾装置を図面を参照しながら説明する。図８は、この発明の実施の形態３に係わる追尾装置の構成を示すブロック図である。図８において、１１は第１の駆動雑音制御手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０１７３】
カルマンフィルタの理論により、予測誤差共分散行列Ｐ_pk（ｔ）は、（３０）式により算出できる。
【０１７４】
第１の駆動雑音制御手段１１では、予測誤差共分散行列を算出の際の（３２）式における駆動雑音ベクトルの標準偏差σａを、第１のデータ更新手段６で得られる平滑ベクトルの速度成分の大きさがある閾値より大きい場合、大きく設定し、第１のデータ更新手段６で得られる平滑ベクトルの速度成分の大きさがある閾値より小さい場合、小さく設定する。
【０１７５】
（３２）式における駆動雑音ベクトルの標準偏差σａを大きくすることは、（３０）式により、予測誤差共分散行列が大きくなり、さらに（５）式により、残差共分散行列が大きくなるため、ゲートが大きくなる。したがって、目標速度が大きい場合に旋回する時、駆動雑音を小さく設定してある場合には、目標をゲート内に補足できない。しかし、第１の駆動雑音制御手段１１では、平滑ベクトルの速度成分の大きさがある閾値より大きくなった場合、駆動雑音ベクトルの標準偏差を大きくし、ゲートを広げるため、目標を補足しやすくなる。
【０１７６】
次に、図９及び図１０に実施の形態３の動作を説明するフローチャートを示す。ここでは、図３及び図４と同一符号のステップは省略する。すなわち、ステップＳＴ１５ｂおよびステップＳＴ１５ｃについて説明する。
【０１７７】
ステップＳＴ１５ｂにおいて、カルマンフィルタの理論に基づく、データ更新を行ない、平滑ベクトル、平滑誤差共分散行列を算出する。
【０１７８】
ステップＳＴ１５ｃにおいて、予測処理を行う前に、平滑ベクトルの速度成分の大きさがある閾値より大きくなった場合、駆動雑音ベクトルの標準偏差を大きくする。その後、カルマンフィルタの理論に基づく、予測処理を行ない、予測ベクトル、予測誤差共分散行列を算出する。
【０１７９】
したがって、以上のステップを行うことにより、平滑ベクトルの速度成分の大きさがある閾値より大きくなった場合、駆動雑音ベクトルの標準偏差を大きくし、ゲートを広げることで、目標を補足しやすくなる。
【０１８０】
実施の形態４．
この発明の実施の形態４に係わる追尾装置を図面を参照しながら説明する。
図１１は、この発明の実施の形態４に係わる追尾装置の構成を示すブロック図である。図８において、１２は、第２の駆動雑音制御手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０１８１】
第２の駆動雑音制御手段１２では、第１の平滑手段６で得られる平滑ベクトルの速度成分の大きさにより、駆動雑音標準偏差σａを（４１）式を用いて、切り替える。
【０１８２】
【数４１】

【０１８３】
（４１）式はロジスティック曲線である。ロジスティック曲線の概念図を図１２に示す。
【０１８４】
図１２の横軸は、平滑ベクトルの速度成分の大きさＶであり、縦軸は、駆動雑音の標準偏差σａである。ここで、Ｖ０は平滑ベクトルの位置成分の大きさＶの切り替えの閾値であり、σａ０は、Ｖ０に対応する駆動雑音の標準偏差である。
【０１８５】
（４１）式におけるａ、ｂ、ｃは、曲線の形状を決定するパラメータである。ここで、スケーリングパラメータａ、ｂ、ｃは、それぞれ、ａを大きくすると、図１２のように、曲線の縦幅が大きくなり、ｂを大きくすると、縦幅が小さく、勾配が滑らかになり、ｃを大きくすると、縦幅が同じで勾配が急になる性質を持つ。
【０１８６】
したがって、駆動雑音の標準偏差σａを急に大きくしたりすると、ゲインが急に大きく変動するため、平滑ベクトルおよび平滑誤差共分散行列の算出値がおかしくなる。しかし、（４１）式のように、駆動雑音の標準偏差σａを切り替えることにより、ゲインの推移が切り替える前と後で滑らかになるため、速度ベクトルの切り替えが安定してできるようになり、また、徐々に駆動雑音ベクトルの標準偏差を大きくし、ゲートを広げるため、目標を補足しやすくなる。
【０１８７】
次に、図１３及び図１４に実施の形態４の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ここではステップＳＴ１５ｄについてのみ説明する。
【０１８８】
ステップＳＴ１５ｄにおいて、事前にロジスティック曲線のスケーリングパラメータを決めた後、目標の平滑ベクトルの速度成分の大きさＶにより、駆動雑音の標準偏差を切り替える。
【０１８９】
したがって、以上のステップを行うことにより、駆動雑音の標準偏差σａを切り替えることにより、ゲインの推移が切り替える前と後で滑らかになり、かつ、速度ベクトルの切り替えが安定してできるようになる。また、徐々に駆動雑音ベクトルの標準偏差を大きくし、ゲートを広げるため、目標を補足しやすくなる。
【０１９０】
実施の形態５．
この発明の実施の形態５に係わる追尾装置を図面を参照しながら説明する。図１５は、この発明の実施の形態５に係わる追尾装置の構成を示すブロック図である。図１５において、１３は、第２のデータ更新手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０１９１】
第２のデータ更新手段１３は、図４６、図５０を用いて、説明する。図４６では、現時刻ｔｋにおける破線で囲まれた、予測ベクトルＶＸ_pk（ｔ）の位置成分を中心としたゲートの領域内に、探知データＤ_k1、Ｄ_k2、Ｄ_k3が存在している。重みづけを考慮したゲート判定およびデータ更新手段１３では、レンジ方向のゲート判定３手段から入力される（３７）式の距離ゲートに入っているか否かを示す判定信号、第１のクロスレンジ方向のゲート判定手段４から入力される（３８）式の仰角ゲートに入っているか否かを示す判定信号、（３９）式の方位角ゲートに入っているか否かを示す判定信号の全てがゲートに入っているという信号である場合、これらの判定信号に対応する探知データＤｋｊを目標信号の
候補とする。ここで、（３７）式の距離ゲートに入っているか否かを示す判定信号、（３８）式の仰角ゲートに入っているか否かを示す判定信号、（３９）式の方位角ゲートに入っているか否かを示す判定信号の全てがゲートに入っているという信号が複数ある場合を考える。この場合、第２のデータ更新手段１３では、複数の探知データに関しては、正規分布により、ゲート中心からの距離を換算した尤度により、ゲート中心間の距離を考慮した平滑ベクトルおよび平滑誤差共分散行列を算出する。
【０１９２】
ゲート中心からの距離を換算した尤度の概念図を図５０に示す。図５０中のＤ_k1、Ｄ_k2、Ｄ_k3、Ｄ_k4、Ｄ_k5は探知データであり、Ｄ_k1、Ｄ_k2、Ｄ_k3がゲート内に入っているとする。ここで、ゲート内に存在する探知データＤ_k1、Ｄ_k2、Ｄ_k3をゲート中心からの距離を換算した尤度は、図５０のように、正規分布の尤度γ_k1、γ_k2、γ_k3で表される。
【０１９３】
したがって、ゲート中心から１番近い探知データが不要信号の場合、平滑ベクトル、平滑誤差共分散行列を算出すると、次サンプリング以降のゲートの中心が、現時刻の不要信号に極端に引っ張られ、追尾を失敗することがある。しかし、重みづけを考慮したゲート判定およびデータ更新手段１３により、ゲート中心からの距離を換算した尤度により、ゲート中心間の距離を考慮した平滑ベクトルおよび平滑誤差共分散行列を算出することで、次サンプリング以降のゲートの中心が、現時刻の不要信号に極端に引っ張られことなく、不要信号中で安定した追尾を行うことができる。
【０１９４】
次に、図１６及び図１７に実施の形態５の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ１４ｂ及びステップＳＴ１５ｅについてのみ説明する。
【０１９５】
ステップＳＴ１４ｂにおいて、ゲート内の探知データの尤度を算出する。
【０１９６】
ステップＳＴ１５ｅにおいて、探知データの尤度を用いて、平滑ベクトルおよび平滑誤差共分散行列の算出を行う。また、現時刻ｋのゲート中心間の距離を考慮した平滑ベクトルを１サンプリング分外挿して、予測ベクトルおよび予測誤差共分散行列を算出する。
【０１９７】
以上のステップにより、したがって、ゲート中心から１番近い探知データが不要信号の場合、平滑ベクトル、平滑誤差共分散行列を算出すると、次サンプリング以降のゲートの中心が、現時刻の不要信号に極端に引っ張られ、追尾を失敗することがあるが、ゲート中心からの距離を換算した探知データの尤度により、ゲート中心間の距離を考慮した平滑ベクトルおよび平滑誤差共分散行列を算出することで、次サンプリング以降のゲートの中心が、現時刻の不要信号に極端に
引っ張られことなく、不要信号中で安定した追尾を行うことができる。
【０１９８】
実施の形態６．
この発明の実施の形態６に係わる追尾装置を図面を参照しながら説明する。図１８は、この発明の実施の形態６に係わる追尾装置の構成を示すブロック図である。図１３において、１４は、第１のゲートサイズパラメータ設定手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０１９９】
第１のゲートサイズパラメータ設定手段１４では、観測手段から得られる、現サンプリング時刻と前サンプリング時刻の時間間隔を見て、時間間隔が大きい場合、ゲートサイズパラメータを小さく設定するような信号をレンジ方向のゲート判定手段３、第１のクロスレンジ方向のゲート判定手段４にそれぞれ出力する。
【０２００】
ゲートの大きさを決めるゲート半径は、（２５）式の左辺の分母のように、固有値λ₁とｄ_εの積の０．５乗、λ₂とｄ_εの積の０．５乗、λ₃とｄ_εの積の０．５乗のように、固有値λ₁、λ₂、λ₃とゲートサイズパラメータｄ_εの積の０．５乗で決定される。
【０２０１】
ここで、サンプリング間隔の増加と共に、固有値λ₁、λ₂、λ₃は、大きくなるため、ゲート半径も大きくなる。しかし、サンプリング間隔が極端に長くなった場合、固有値も極端に大きくなるため、ゲートが非常に大きくなる。そのため、ゲート内には、目標信号のみならず、不要信号が大量に侵入してきて、ゲートをかける意味がなくなる。そのため、サンプリング間隔がある閾値より大きい場合は、ゲートサイズパラメータを小さく設定するようにして、適度な大きさのゲート半径を保つようにすることで、目標信号の補足および不要信号の侵入を防ぐようにする。
【０２０２】
次に、図１９及び図２０に実施の形態６の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ９ｂにつてのみ説明する。
【０２０３】
ステップＳＴ９ｂにおいて、サンプリング間隔がある閾値より大きい場合、レンジ方向とクロスレンジ横方向とクロスレンジ縦方向に設定するゲートサイズパラメータの値を小さく設定するように制御する。
【０２０４】
以上のステップにより、サンプリング間隔がある閾値より大きい場合は、ゲートサイズパラメータを小さく設定するようにして、適度な大きさのゲート半径を保つようにすることで、目標信号の補足および不要信号の侵入を防ぐようにする。
【０２０５】
実施の形態７．
この発明の実施の形態７に係わる追尾装置を図面を参照しながら説明する。図２１は、この発明の実施の形態７に係わる追尾装置の構成を示すブロック図である。図２１において、１５は、観測誤差固定手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０２０６】
観測誤差固定手段１５では、（８）式における距離の観測雑音標準偏差、仰角の観測雑音標準偏差、方位角の観測雑音標準偏差σｒ_kj、σｅ_kj、σｂ_kjに固定の値を設定する。
【０２０７】
（５）式の残差共分散行列Ｓ_kj（ｔ）におけるＲ_kjは、（７）式により計算され、（７）式のＲ_kj中のΛ_kjの計算は、（８）式により計算される。したがって、（８）式における距離の観測雑音標準偏差、仰角の観測雑音標準偏差、方位角の観測雑音標準偏差σｒ_kj、σｅ_kj、σｂ_kjに固定の値を設定することで、サンプリング時刻ごと、探知データごとに（８）式の計算が不要になるため、演算負荷が軽くなる。
【０２０８】
次に、図２２及び図２３に実施の形態７の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ２ｂについて説明する。
【０２０９】
ステップＳＴ２ｂにおいて、（１５）式により、残差を計算する。また、観測雑音の標準偏差を固定にして、（８）式からΛ_kjを計算し、Λ_kjを（７）式に代入し、Ｒ_kjを算出してから、残差共分散行列を算出する。
【０２１０】
以上のステップにより、（８）式における距離の観測雑音標準偏差、仰角の観測雑音標準偏差、方位角の観測雑音標準偏差σｒ_kj、σｅ_kj、σｂ_kjに固定の値を設定することで、サンプリング時刻ごと、探知データごとに、（８）式の計算が不要になるため、演算負荷が軽くなる。
【０２１１】
実施の形態８．
この発明の実施の形態８に係わる追尾装置を図面を参照しながら説明する。図２４は、この発明の実施の形態８に係わる追尾装置の構成を示すブロック図である。図１７において、１６は、サンプリング間隔固定手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０２１２】
サンプリング間隔固定手段１６では、サンプリング間隔を固定に制御する制御信号を予測手段７に入力を行う。そのため、（３１）式における状態遷移行列Φｋが固定値となるため、状態遷移行列Φｋを含む（２９）式の予測ベクトルの計算式および状態遷移行列Φｋを含む（３０）式の予測誤差共分散行列に関する演算負荷が軽減される。
【０２１３】
次に、図２５及び図２６に実施の形態８の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ１５ｆについてのみ説明する。
【０２１４】
ステップＳＴ１５ｆにおいて、サンプリング間隔固定とした状態遷移行列Φｋを用いて、（２９）式の予測ベクトルの計算式および（３０）式の予測誤差共分散行列を算出する。
【０２１５】
以上のステップにより、（３１）式における状態遷移行列Φｋを固定値として扱うことで、予測ベクトルおよび予測誤差共分散行列の演算負荷が軽減される。
【０２１６】
実施の形態９．
この発明の実施の形態９に係わる追尾装置を図面を参照しながら説明する。図２７は、この発明の実施の形態９に係わる追尾装置の構成を示すブロック図である。図１９において、１７は、ゲート中心固定手段である。なお、各図中の同一符号は、同一または相当部分を表す。
【０２１７】
ゲート中心固定手段１７では、追尾初期から数サンプリング分、（１５）式の右辺における予測ベクトルの位置成分ＶＺ_pk（ｔ）の値に代わりに、固定の第１サンプリングの探知データ使いつづける。
【０２１８】
ゲートを固定するサンプリング数は事前に設定しておく。
【０２１９】
したがって、追尾初期時は、ばらついた探知データから予測ベクトルを算出すると、速度ベクトルのばらつきが大きい。つまり、予測ベクトル算出の際、ばらついた速度ベクトルを使用すると、ゲートの中心が安定しないため、目標信号を補足できない可能性が高い。しかし、追尾初期時の数サンプリング分だけ、ゲートの中心である予測ベクトルの位置成分ＶＺ_pk（ｔ）の代わりに、固定の第１サンプリングの探知データ使いつづけることで、追尾初期時のばらつきが軽減される。
【０２２０】
次に、図２８及び図２９に実施の形態９の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ２ｃについてのみ説明する。
【０２２１】
ステップＳＴ２ｃにおいて、初期数サンプリング分、ゲート中心を固定する。
【０２２２】
以上のステップにより、追尾初期時の数サンプリング分だけ、ゲートの中心である予測ベクトルの位置成分ＶＺ_pk（ｔ）の代わりに、固定の第１サンプリングの探知データ使いつづけることで、追尾初期時のばらつきが軽減される。
【０２２３】
実施の形態１０．
この発明の実施の形態１０に係わる追尾装置を図面を参照しながら説明する。図３０は、この発明の実施の形態に係わる追尾装置の構成を示すブロック図である。図３０において、１８は、目標速度によるゲートサイズパラメータ可変手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０２２４】
目標速度によるゲートサイズパラメータ可変手段１８では、第１のデータ更新手段６から得られる１サンプリング前の平滑ベクトルから、平滑ベクトルの速度成分の大きさを算出する。その１サンプリング前の平滑ベクトルの速度成分の大きさが、ある閾値よりも大きい場合、レンジ方向のゲート判定手段３と第１のクロスレンジ方向のゲート判定手段４における現時刻のゲートサイズパラメータの大きさを大きくするような制御信号をレンジ方向のゲート判定手段３と第１のクロスレンジ方向のゲート判定手段４におけるそれぞれ入力する。レンジ方向のゲート判定手段３と第１のクロスレンジ方向のゲート判定手段４では、目標速度によるゲートサイズパラメータ可変手段１８から入力されたゲートサイズパラメータの大きさを大きくするような制御信号により、ゲートサイズパラメータを大きく設定する。
【０２２５】
したがって、目標速度によるゲートサイズパラメータ可変手段１８では、１サンプリング前の平滑ベクトルの速度成分の大きさが、ある閾値よりも大きい場合に、第１のクロスレンジ方向のゲート判定手段４における現時刻のゲートサイズパラメータの大きさと、レンジ方向のゲート判定手段３における現時刻のゲートサイズパラメータの大きさを大きくすることによって、目標信号を補足しやすくする。
【０２２６】
次に、図３１および図３２に実施の形態１０の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ９ｃについてのみ説明する。
【０２２７】
ステップＳＴ９ｃにおいて、１サンプリング前の平滑ベクトルの速度成分の大きさが、ある閾値よりも大きい場合に、レンジ方向とクロスレンジ横方向とクロスレンジ縦方向の現時刻のゲートサイズパラメータを大きく設定する。
【０２２８】
以上のステップにより、１サンプリング前の平滑ベクトルの速度成分の大きさが、ある閾値よりも大きい場合に、現時刻のゲートサイズパラメータの大きさを大きくすることによって、目標信号を補足しやすくする。
【０２２９】
実施の形態１１．
この発明の実施の形態１１に係わる追尾装置を図面を参照しながら説明する。図３３は、この発明の実施の形態１１に係わる追尾装置の構成を示すブロック図である。図３３において、１９は、第１の表示用平滑手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０２３０】
第１の表示用平滑手段１９では、第１のデータ更新手段６から入力された平滑ベクトルＶＸ_sk（ｔ）を、次式（４２）を用いて、表示用平滑ベクトルＶＸ_sk（ｔ＊）を求め、表示用平滑ベクトルＶＸ_sk（ｔ＊）を表示手段９に入力する。また、表示用平滑ベクトルを１サンプリング分保管しておく。
【０２３１】
【数４２】

【０２３２】
（４２）式において、ＶＸ_sk（ｔ）は、時刻ｋにおいて、第１のデータ更新手段６から入力される平滑ベクトル、ＶＸ_sk（ｔ＊）は、時刻ｋにおける表示用平滑ベクトル、ηｋは、平滑の重みを決める係数である。
【０２３３】
ηｋの算出は、次式（４３）により算出される。
【０２３４】
【数４３】

【０２３５】
（４３）式の右辺において、Δｔはサンプリング間隔、τは時定数である。
【０２３６】
時定数が大きい程、表示用平滑ベクトルは、現時刻の目標の探知データよりも遅れは大きくなるが、平滑効果は大きい、つまり見た目で滑らかに見える。
【０２３７】
したがって、第１の表示用平滑手段１９では、（４３）式の時定数を事前に決めておき、第１のデータ更新手段６から入力される平滑ベクトルＶＸ_sk（ｔ）から、（４２）式を用いて、表示用平滑ベクトルＶＸ_sk（ｔ＊）を求めることにより、オペレータへの表示が見やすくなる。
【０２３８】
次に、図３４及び図３５に実施の形態１１の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ１６ｂについてのみ説明する。
【０２３９】
ステップＳＴ１６ｂにおいて、平滑ベクトルより表示用平滑ベクトルを算出し、平滑位置平滑速度の表示を行う。
【０２４０】
ばらついている探知データにから求めた平滑ベクトルを表示すると、表示航跡のがたつきが起こるが、以上のステップにより、表示用平滑ベクトルの算出を行い、表示用平滑ベクトルから表示用航跡を表示すれば、オペレータに対する表示航跡が滑らかになり見やすくなる。
【０２４１】
実施の形態１２．
この発明の実施の形態１２に係わる追尾装置を図面を参照しながら説明する。図３６は、この発明の実施の形態に係わる追尾装置の構成を示すブロック図である。図３６において、２０は、第２の表示用平滑手段である。なお、各図中の同一符号は、同一または相当部分を表す。
【０２４２】
第２の表示用平滑手段２０では、第１のデータ更新手段６から入力された平滑ベクトルＶＸ_sk（ｔ）を、次式（４４）を用いて、表示用平滑ベクトルＶＸ_sk（ｔ＊＊）を求め、表示用平滑ベクトルＶＸ_sk（ｔ＊＊）を表示手段９に入力する。また、表示用平滑ベクトルを１サンプリング分保管しておく。
【０２４３】
【数４４】

【０２４４】
（４４）式において、ＶＸ_sk（ｔ）は、時刻ｋにおいて、第１のデータ更新手段６から入力される平滑ベクトル、ＶＸ_sk（ｔ＊＊）は、時刻ｋにおける表示用平滑ベクトル、θ_kは、平滑の重みを決める係数である。
【０２４５】
θ_kの算出は、（４５）式により算出する。
【０２４６】
【数４５】

【０２４７】
（４５）式の右辺において、Δｔはサンプリング間隔、ρ_kはサンプリング時刻により可変する時定数である。
【０２４８】
ρ_kの算出は、次式（４６）により算出する。
【０２４９】
【数４６】

【０２５０】
（４６）において、ρ_kはサンプリング時刻により可変する時定数、φ_kは平滑の重みを決める係数である。また、ρ_infは、ρ_kの定常状態の固定値である。ρ_kは事前に設定する。（４６）式におけるφ_kは、次式（４７）により決める。
【０２５１】
【数４７】

【０２５２】
（４７）式における（４５）式の右辺において、Δｔはサンプリング間隔、τｃはサンプリング時刻により可変する時定数ρ_kを制御する時定数の時定数である。
【０２５３】
（４４）式、（４５）式、（４６）式、（４７）式を用いることで、初期状態では、平滑ベクトルのがたつきは、現時刻の目標の探知データよりも遅れよりも大きいため、表示用平滑ベクトルの時定数θｋを大きく設定し、オペレータへの表示用航跡を滑らかに見せることができる。また、初期状態が終わり定常状態に近くなると、平滑ベクトルのがたつきは小さくなるため、探知データとの遅れを小さくするため、時定数θ_kを小さく設定できる。したがって、オペレータへの表示が見やすくなる。
【０２５４】
次に、図３７及び図３８に実施の形態１２の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ１６ｃについてのみ説明する。
【０２５５】
ステップＳＴ１６ｃにおいて、平滑ベクトルより表示用平滑ベクトルを時定数を可変として算出し、平滑位置平滑速度の表示を行う。
【０２５６】
以上のステップにより、（４４）式、（４５）式、（４６）式、（４７）式を用いて、時定数を可変にして、表示用平滑ベクトルを算出することにより、表示用平滑ベクトルを求める前の平滑ベクトルの遅れと、平滑ベクトルの滑らかさの改善を行うことができる。
【０２５７】
実施の形態１３．
この発明の実施の形態１３に係わる追尾装置を図面を参照しながら説明する。図３９は、この発明の実施の形態に係わる追尾装置の構成を示すブロック図である。図３９において、２１は、マルチパス判定手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０２５８】
マルチパスとは、レーダから照射された電波が地面、海面、障害物に当って、散乱する反射電波である。
【０２５９】
マルチパス判定手段２１は、観測手段１から得られるある閾値よりもＳ／Ｎ比が大きい場合、（８）式における仰角の観測雑音標準偏差を事前に与える固定値に設定し、（１３）式を用いて残差共分散行列を算出するような制御信号を、第１のゲート判定諸元算出手段２に入力する。また、マルチパス判定手段２１は、観測手段１から得られるある閾値よりもＳ／Ｎ比が大きい場合、第１のクロスレンジ方向のゲート判定手段４におけるクロスレンジ縦方向のゲートサイズパラメータは、ゲート判定結果が必ずゲート内と判定するような十分に大きな値に設定するようにする制御信号を第１のクロスレンジ方向のゲート判定手段４に入力する。
【０２６０】
したがって、マルチパス判定手段２１により、高度方向の精度が良くないマルチパス環境下において、マルチパスの影響を一番受ける仰角の観測雑音標準偏差を使用しないことにより、マルチパス環境下での追尾性能を向上させる事ができる。
【０２６１】
次に、図４０及び図４１に実施の形態１３の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ２ｄ及びステップＳＴ９ｄについてのみ説明する。
【０２６２】
ステップＳＴ２ｄにおいて、Ｓ／Ｎ閾値による仰角観測雑音を固定すること、つまり、（８）式における仰角の観測雑音標準偏差を事前に与える固定値に設定した上で、残差共分散行列を（１３）式により算出する。
【０２６３】
ステップＳＴ９ｄにおいて、マルチパスと判定したら、マルチパスを想定したゲートサイズパラメータ設定、つまり、クロスレンジ縦方向のゲート判定において、すべての探知データがゲート内と判定するような、クロスレンジ縦方向のゲートサイズパラメータを設定する。
【０２６４】
以上のステップにより、高度方向の精度が良くないマルチパス環境下において、マルチパスの影響を一番受ける仰角の観測雑音標準偏差を使用しないことにより、マルチパス環境下での追尾性能を向上させる事ができる。
【０２６５】
実施の形態１４．
この発明の実施の形態１４に係わる追尾装置を図面を参照しながら説明する。図４２は、この発明の実施の形態１４に係わる追尾装置の構成を示すブロック図である。図４２において、２２は、条件数判定手段である。なお、図中の同一符号は、同一または相当部分を表す。
【０２６６】
第１のゲート判定諸元算出手段２から入力される固有値の内、固有値の最大値λｋｍａｘと固有値の最小値λｋｍｉｎの比を取り、次式（４８）における条件数ＣＯＮＤ_kを算出する。
【０２６７】
【数４８】

【０２６８】
ＣＯＮＤ_kが小さいと、行列演算において、桁落ちが起こり、逆行列演算ができないなどの問題が生じ、追尾の演算が不安定にある。
【０２６９】
条件数判定手段２２では、次式（４９）のように、条件数ＣＯＮＤ_kが事前に設定したある閾値ＣＯＮＤ_th以下の場合、オペレータに追尾の演算が不安定になっているという危険信号を表示手段９に入力し、オペレータの表示画面に出力させる。
【０２７０】
【数４９】

【０２７１】
したがって、条件数判定手段２２を用いる事によって、オペレータが追尾の演算が不安定になっているという危険信号を察知することができ、例えば、追尾演算をやり直したり、追尾演算を打ち切る判断ができる。
【０２７２】
次に、図４３及び図４４に実施の形態１４の動作を説明するフローチャートを示す。ここで、同一符号のステップは省略する。すなわち、ステップＳＴ１６ｄについてのみ説明する。
【０２７３】
ステップＳＴ１６ｄにおいて、（４８）式により、条件数を算出し、（４９）式が成り立つ時、オペレータに追尾の演算が不安定になっているという危険信号を表示手段９に入力し、オペレータの表示画面に出力させる。
【０２７４】
以上のステップにより、オペレータが追尾の演算が不安定になっているという危険信号を察知することができる。
【０２７５】
【発明の効果】
この発明は、受信電力を元に、信号対雑音比であるＳ／Ｎ比を得て、それを距離、仰角および方位角の観測雑音標準偏差に変換し、前記観測雑音標準偏差とそれらに付随する位置を表す探知データとを出力する観測手段と、予測誤差共分散行列および前記探知データが入力され、それらから算出される残差共分散行列から、距離方向の固有値、仰角方向の固有値、方位角方向の固有値、距離方向の固有ベクトル、仰角方向の固有ベクトルおよび方位角方向の固有ベクトルを算出する対角化を行う第１のゲート判定諸元算出手段と、前記探知データが距離方向において所定次元の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力するレンジ方向のゲート判定手段と、前記観測手段及び前記第１のゲート判定諸元算出手段から出力されるデータを用いて、仰角方向及び方位角方向で、所定次元の楕円体ゲート判定を行うクロスレンジ方向のゲート判定手段と、前記距離方向での前記所定次元の楕円体ゲートに入っているか否かの判定結果を示す信号と、仰角方向および方位角方向での前記所定次元の楕円体ゲートに入っているか否かの判定結果を示す信号とから、距離方向、仰角方向および方位角方向のすべてにおいて前記所定次元の楕円体ゲート内に前記探知データが入っている場合に、目標信号の候補として、その探知データに付随する探知データおよび残差共分散行列を出力する第１のゲート判定手段と、前記第１のゲート判定手段から出力される前記探知データおよび前記残差共分散行列が入力され、カルマンフィルタの理論に基づいてデータの更新を行い、平滑誤差共分散行列および平滑ベクトルを出力する第１のデータ更新手段と、前記カルマンフィルタの理論に基づき、現時刻よりも１サンプリング後の予測誤差共分散行列および予測ベクトルを算出する予測手段と、前記予測手段から入力される予測誤差共分散行列および予測ベクトルを１サンプリング遅延させて、前記第１のゲート判定諸元算出手段に対して前記予測誤差共分散行列を出力する１サンプリング遅延手段と、前記第１のデータ更新手段から入力される平滑ベクトルから、目標航跡および目標の速度の表示を行う表示手段と、を備え、前記所定次元の楕円体ゲートは、前記残差共分散行列を元にした所定次元の楕円体ゲートであって、前記第１のゲート判定諸元算出手段は、前記楕円体ゲートにおいて、前記残差共分散行列の対角化によりレンジ方向とクロスレンジ方向とを決定し、前記レンジ方向のゲート判定手段および前記クロスレンジ方向のゲート判定手段は、前記第１のゲート判定諸元算出手段によって計算された前記レンジ方向および前記クロスレンジ方向に基づいて、前記レンジ方向および前記クロスレンジ方向のゲートサイズパラメータを設定することにより、前記楕円体ゲートのサイズを決定する追尾装置であるので、目標信号が入りやすく、不要信号を侵入させにくくすることができる。
【０２７６】
また、前記クロスレンジ方向のゲート判定手段が、探知データが仰角方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力し、探知データが方位角方向の１次元ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力する第１のクロスレンジ方向のゲート判定手段から構成されているので、クロスレンジ方向のゲート判定を容易に行うことができる。
【０２７７】
また、前記クロスレンジ方向のゲート判定手段が、仰角方向の固有ベクトル、仰角方向の固有値、方位角方向の固有ベクトルおよび方位角方向の固有値を用いて、クロスレンジ方向で、２次元のゲート判定を行う第２のクロスレンジ方向のゲート判定手段から構成されているので、レンジ方向とクロスレンジ方向のでのゲート判定を独立に行うことができるため、レンジ方向のゲートを大きくした場合にもクロスレンジ方向も合わせて大きくなることを防ぐことができる。また、ゲートサイズパラメータの設定を１回で行うことができる。
【０２７８】
また、前記第１のデータ更新手段から出力される前記平滑ベクトルの速度成分がある閾値より大きくなった場合、駆動雑音ベクトルの標準偏差を大きく設定して前記予測手段に出力する第１の駆動雑音制御手段をさらに備えているので、標準偏差を大きくして、ゲートを広げることにより、目標を補足しやすくなる。
【０２７９】
また、駆動雑音ベクトルの標準偏差の大きさを縦軸として、平滑ベクトルの速度成分を横軸としたロジスティック曲線に基づき、平滑ベクトルの速度成分の大きさに従って、駆動雑音ベクトルの標準偏差を大きく設定するように切り替える第２の駆動雑音制御手段をさらに備えているので、駆動雑音ベクトルの標準偏差を徐々に大きくし、ゲートを広げるため、目標を補足しやすくなる。
【０２８０】
また、この発明は、受信電力を元に、信号対雑音比であるＳ／Ｎ比を得て、それを距離、仰角および方位角の観測雑音標準偏差に変換し、前記観測雑音標準偏差とそれらに付随する位置を表す探知データとを出力する観測手段と、予測誤差共分散行列および前記探知データが入力され、それらから算出される残差共分散行列から、距離方向の固有値、仰角方向の固有値、方位角方向の固有値、距離方向の固有ベクトル、仰角方向の固有ベクトルおよび方位角方向の固有ベクトルを算出する対角化を行う第１のゲート判定諸元算出手段と、前記探知データが距離方向の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力するレンジ方向のゲート判定手段と、前記探知データが仰角方向の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力し、探知データが方位角方向の楕円体ゲートに入っているか否かの判定を行い、その判定結果を示す信号を出力する第１のクロスレンジ方向のゲート判定手段と、前記レンジ方向のゲート判定手段および前記第１のクロスレンジ方向のゲート判定手段における前記探知データのゲート判定の結果、ゲート内の探知データについて、ゲート中心と探知データの距離を正規分布の尤度に換算し、その尤度を用いて、データ更新を行い、平滑誤差共分散行列および平滑ベクトルを出力する第２のデータ更新手段と、カルマンフィルタの理論に基づき、現時刻よりも１サンプリング後の予測誤差共分散行列および予測ベクトルを算出する予測手段と、前記予測手段から入力される予測誤差共分散行列および予測ベクトルを１サンプリング遅延させて、前記第１のゲート判定諸元算出手段に対して出力する、１サンプリング遅延手段と、前記第２のデータ更新手段から入力される平滑ベクトルから、目標航跡および目標の速度の表示を行う表示手段と、を備え、前記距離方向の楕円体ゲートと、前記仰角方向の楕円体ゲートと、前記方位角方向の楕円体ゲートとは、組み合わされて、所定次元の楕円体ゲートを構成し、前記所定次元の楕円体ゲートは、前記残差共分散行列を元にした所定次元の楕円体ゲートであって、前記第１のゲート判定諸元算出手段は、前記楕円体ゲートにおいて、前記残差共分散行列の対角化によりレンジ方向とクロスレンジ方向とを決定し、前記レンジ方向のゲート判定手段および前記第１のクロスレンジ方向のゲート判定手段は、前記第１のゲート判定諸元算出手段によって計算された前記レンジ方向および前記クロスレンジ方向に基づいて、前記レンジ方向および前記クロスレンジ方向のゲートサイズパラメータを設定することにより、前記楕円体ゲートのサイズを決定する追尾装置であるので、探知データの尤度により、ゲート中心間の距離を考慮した平滑ベクトルおよび平滑誤差共分散行列を算出することで、次サンプリング以降のゲートの中心が、現時刻の不要信号に極端に引っ張られることなく、不要信号中で安定した追尾を行うことができる。
【０２８１】
また、サンプリング間隔により、ゲートサイズパラメータを設定する、第１のゲートサイズパラメータ設定手段をさらに備えているので、サンプリング間隔が所定の閾値より大きい場合は、ゲートサイズパラメータを小さく設定するようにして、適度な大きさのゲート半径を保つようにすることで、目標信号の補足および不要信号の侵入を防ぐようにする。
【０２８２】
また、距離の観測雑音標準偏差、仰角の観測雑音標準偏差および方位角の観測雑音標準偏差を固定の値に設定する観測誤差固定手段をさらに備えているので、それらに固定の値を設定することで、サンプリング時刻ごと、探知データごとに極座標における観測誤差共分散行列の計算を行うことが不要になるため、演算負荷を軽減することができる。
【０２８３】
また、サンプリング間隔を固定する制御信号を予測手段に入力し、予測誤差共分散行列および予測ベクトルの算出の際のサンプリング間隔を固定するサンプリング間隔固定手段をさらに備えているので、状態遷移行列を固定値として扱うことで、予測ベクトルおよび予測誤差共分散行列の演算負荷を軽減することができる。
【０２８４】
また、追尾初期から数サンプリング分、ゲートの中心を固定するゲート中心固定手段をさらに備えているので、追尾初期時の数サンプリング分だけ、ゲートの中心である予測ベクトルの位置成分の代わりに、固定の第１サンプリングの探知データを使いつづけることで、追尾初期時のばらつきが軽減される。
【０２８５】
また、前記第１のデータ更新手段から得られる平滑ベクトルの速度成分の大きさを用いて、ゲートサイズパラメータの大きさを変える、ゲートサイズパラメータ可変手段をさらに備えているので、１サンプリング前の平滑ベクトルの速度成分の大きさが、所定の閾値よりも大きい場合に、現時刻のゲートサイズパラメータの大きさを大きくすることによって、目標信号を補足しやすくなる。
【０２８６】
また、前記第１のデータ更新手段から得られる平滑ベクトルを時定数を固定とした表示用フィルタに入力して、表示用平滑ベクトルを算出する、第１の表示用平滑手段をさらに備えているので、ばらついている探知データから求めた平滑ベクトルを表示すると、表示航跡のがたつきが起こるが、表示用平滑ベクトルの算出を行い、表示用平滑ベクトルから表示用航跡を表示すれば、オペレータに対する表示航跡が滑らかになり、見やすくなる。
【０２８７】
また、前記第１のデータ更新手段から得られる平滑ベクトルを時定数を可変とした表示用フィルタに入力して、表示用平滑ベクトルを算出する、第２の表示用平滑手段をさらに備えているので、時定数を可変にして、表示用平滑ベクトルを算出することにより、表示用平滑ベクトルを求める前の平滑ベクトルの遅れと、平滑ベクトルの滑らかさの改善を行うことができる。
【０２８９】
また、前記第１のゲート判定諸元算出手段から入力される固有値の内、固有値の最大値と固有値の最小値の比である条件数を用いて、条件数がある閾値以下ならば、追尾の演算が不安定になっているとの危険信号を、前記表示手段に入力する条件数判定手段をさらに備えているので、オペレータが追尾の演算が不安定になっているという危険信号を察知することができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態１における追尾装置の構成を示すブロック図である。
【図２】 追尾装置に設けられた観測手段に係わる座標系を示した説明図である。
【図３】 本発明の実施の形態１における追尾装置の処理の流れを示す流れ図である。
【図４】 本発明の実施の形態１における追尾装置の処理の流れを示す流れ図である。
【図５】 本発明の実施の形態２における追尾装置の構成を示すブロック図である。
【図６】 本発明の実施の形態２における追尾装置の処理の流れを示す流れ図である。
【図７】 本発明の実施の形態２における追尾装置の処理の流れを示す流れ図である。
【図８】 本発明の実施の形態３における追尾装置の構成を示すブロック図である。
【図９】 本発明の実施の形態３における追尾装置の処理の流れを示す流れ図である。
【図１０】 本発明の実施の形態３における追尾装置の処理の流れを示す流れ図である。
【図１１】 本発明の実施の形態４における追尾装置の構成を示すブロック図である。
【図１２】 ロジスティック曲線の概念図を示した説明図である。
【図１３】 本発明の実施の形態４における追尾装置の処理の流れを示す流れ図である。
【図１４】 本発明の実施の形態４における追尾装置の処理の流れを示す流れ図である。
【図１５】 本発明の実施の形態５における追尾装置の構成を示すブロック図である。
【図１６】 本発明の実施の形態５における追尾装置の処理の流れを示す流れ図である。
【図１７】 本発明の実施の形態５における追尾装置の処理の流れを示す流れ図である。
【図１８】 本発明の実施の形態６における追尾装置の構成を示すブロック図である。
【図１９】 本発明の実施の形態６における追尾装置の処理の流れを示す流れ図である。
【図２０】 本発明の実施の形態６における追尾装置の処理の流れを示す流れ図である。
【図２１】 本発明の実施の形態７における追尾装置の構成を示すブロック図である。
【図２２】 本発明の実施の形態７における追尾装置の処理の流れを示す流れ図である。
【図２３】 本発明の実施の形態７における追尾装置の処理の流れを示す流れ図である。
【図２４】 本発明の実施の形態８における追尾装置の構成を示すブロック図である。
【図２５】 本発明の実施の形態８における追尾装置の処理の流れを示す流れ図である。
【図２６】 本発明の実施の形態８における追尾装置の処理の流れを示す流れ図である。
【図２７】 本発明の実施の形態９における追尾装置の構成を示すブロック図である。
【図２８】 本発明の実施の形態９における追尾装置の処理の流れを示す流れ図である。
【図２９】 本発明の実施の形態９における追尾装置の処理の流れを示す流れ図である。
【図３０】 本発明の実施の形態１０における追尾装置の構成を示すブロック図である。
【図３１】 本発明の実施の形態１０における追尾装置の処理の流れを示す流れ図である。
【図３２】 本発明の実施の形態１０における追尾装置の処理の流れを示す流れ図である。
【図３３】 本発明の実施の形態１１における追尾装置の構成を示すブロック図である。
【図３４】 本発明の実施の形態１１における追尾装置の処理の流れを示す流れ図である。
【図３５】 本発明の実施の形態１１における追尾装置の処理の流れを示す流れ図である。
【図３６】 本発明の実施の形態１２における追尾装置の構成を示すブロック図である。
【図３７】 本発明の実施の形態１２における追尾装置の処理の流れを示す流れ図である。
【図３８】 本発明の実施の形態１２における追尾装置の処理の流れを示す流れ図である。
【図３９】 本発明の実施の形態１３における追尾装置の構成を示すブロック図である。
【図４０】 本発明の実施の形態１３における追尾装置の処理の流れを示す流れ図である。
【図４１】 本発明の実施の形態１３における追尾装置の処理の流れを示す流れ図である。
【図４２】 本発明の実施の形態１４における追尾装置の構成を示すブロック図である。
【図４３】 本発明の実施の形態１４における追尾装置の処理の流れを示す流れ図である。
【図４４】 本発明の実施の形態１４における追尾装置の処理の流れを示す流れ図である。
【図４５】 従来の追尾装置の構成を示すブロック図である。
【図４６】 従来の追尾装置の構成を示す説明図である。
【図４７】 ゲートと目標予測位置、目標観測位置、目標レンジ方向および目標クロスレンジ方向の関係図である。
【図４８】 角度誤差に対するクロスレンジ誤差およびレンジ誤差の関係を示す説明図である。
【図４９】 レンジ方向の固有ベクトルとクロスレンジ方向の固有ベクトルの関係を示した説明図である。
【図５０】 ゲート中心からの距離を換算した尤度の概念図である。
【符号の説明】
１ 観測手段、２ 第１のゲート判定諸元算出手段、３ レンジ方向のゲート判定手段、４ 第１のクロスレンジ方向のゲート判定手段、５ 第１のゲート判定手段、６ 第１のデータ更新手段、７ 予測手段、８ １サンプリング遅延手段、９ 表示手段、１０ 第２のクロスレンジ方向のゲート判定手段、１１ 第１の駆動雑音制御手段、１２ 第２の駆動雑音制御手段、１３ 第２のデータ更新手段、１４ 第１のゲートサイズパラメータ設定手段、１５ 観測誤差固定手段、１６ サンプリング間隔固定手段、１７ ゲート中心固定手段、１８ 目標速度によるゲートサイズパラメータ可変手段、１９ 第１の表示用平滑手段、２０ 第２の表示用平滑手段、２１ マルチパス判定手段、２２ 条件数判定手段，１０１ 観測手段、１０６ データ更新手段、１０７ 予測手段、１０８ １サンプリング遅延手段、１０９ 表示手段、１２３ ゲート判定諸元算出手段、１２４ ゲート判定手段。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a tracking device, and more particularly to a target tracking device used for target tracking in a high-density environment.
[0002]
[Prior art]
A conventional tracking device will be described below with reference to the drawings. FIG. 45 shows, for example, “Samuel S. Blackman, Multiple-Target Tracking with Radar Applications, Artech House, Dedham, 1986” p. 83-p. 92, especially p. It is a block diagram which shows the structure of the conventional target tracking apparatus which has the gate determination method shown by 88, 4.2.2 (4.6) type | formula.
[0003]
In FIG. 45, 101 is observation means, 106 is data update means, 107 is prediction means, 108 is 1 sampling delay means, 109 is display means, 123 is gate determination specification calculation means, and 124 is gate determination means.
[0004]
FIG. 2 is a diagram for explaining a coordinate system related to the observation means 101 in FIG. In FIG. 2, O is a sensor, T is a tracking target, R is a distance between the tracking target T and the sensor O, E is an elevation angle formed by a line segment OT connecting the sensor O and the tracking target T with the XY plane, B Is the azimuth angle formed by the orthogonal projection vector of the line segment OT connecting the sensor O and the tracking target T onto the XY plane and the X axis. Furthermore, [R, E, B] represent polar coordinates, and [X, Y, Z] represent north reference orthogonal coordinates. From now on, when there are no particular notices, the coordinates indicate the north reference orthogonal coordinates.
[0005]
The observation means 101 obtains the detection data and the S / N ratio which is a signal (noise) to noise ratio (noise) from the sensor, and the S / N ratio is the observation noise standard deviation of the distance, the observation noise standard deviation of the elevation angle, The azimuth angle observation noise standard deviation is converted, and the detection data, distance observation noise standard deviation, elevation angle observation noise standard deviation, and azimuth angle observation noise standard deviation are input to the gate determination specification calculation means 123.
[0006]
Note that the types of detection data generally include target signals for tracking target targets and other unnecessary signals.
[0007]
The detection data is D _kj , S / N ratio is SNR _kj Each is represented by a symbol. Here, the subscript k represents the sampling time, and the subscript j represents the number of detection data at the sampling time k.
[0008]
Detection data D _kj Is represented by the following equation (1).
[0009]
[Expression 1]

[0010]
In the above formula (1), o is a subscript representing the observation value, k is a subscript representing the sampling time, and j is a subscript representing the number of detection data. In other words, detection data D _kj Represents the j-th detection data at the sampling time k. X _okj Is detection data D _kj Represents the X coordinate component of y _okj Is detection data D _kj Y coordinate component of z _okj Is detection data D _kj Represents the Z coordinate component of each. The symbol “′” represents transposition of vectors and matrices.
[0011]
Observation noise standard deviation is S / N ratio SNR _kj Is obtained from the following equations (2), (3), and (4).
[0012]
[Expression 2]

[0013]
[Equation 3]

[0014]
[Expression 4]

[0015]
Σr in the above equations (2), (3) and (4) _kj , Σe _kj , Σb _kj Represents the observation noise standard deviation of the distance, the observation noise standard deviation of the elevation angle, and the observation noise standard deviation of the azimuth angle, respectively. Further, α in the above formulas (2), (3) and (4) _r , Α _e , Α _b Is a positive constant determined from the frequency of the radar transmission signal, the bandwidth of the radar transmission signal, the effective aperture diameter of the radar antenna, and the speed of light.
[0016]
In the gate determination specification calculation means 123, detection data, observation noise standard deviation of distance, observation noise standard deviation of elevation angle, and observation noise standard deviation of azimuth are input from the observation means 101, and from the prediction means 107 described later. The prediction vector and the prediction error covariance matrix are input via the 1 sampling delay means 108. Further, the gate determination specification calculation unit 123 calculates a residual and a residual covariance matrix.
[0017]
The residual and residual covariance matrix will be described below.
[0018]
Hereinafter, the residual covariance matrix is S _kj (T). S _kj The subscript k of (t) represents the sampling time, indicating that it is from the jth detection data, and (t) represents the tracking target target number. The residual covariance matrix is calculated by the following equation (5) based on the Kalman filter theory.
[0019]
[Equation 5]

[0020]
In Equation (5), H is an observation matrix and P _pk (T) is a prediction error covariance matrix R for the tracking target t obtained from the prediction means 107 described later. _kj Represents the observation error covariance matrix in the north reference Cartesian coordinates.
[0021]
The observation matrix H in the equation (5) can be expressed as the following equation (6).
[0022]
[Formula 6]

[0023]
Observation error covariance matrix R in north reference Cartesian coordinates in equation (6) _kj Can be expressed by the following equation (7).
[0024]
[Expression 7]

[0025]
In equation (7), Λ _kj Is the observation error covariance matrix in polar coordinates and Γ _kj (T) is the Λ for each tracking target _kj Is the transformation matrix.
[0026]
In equation (7), the observation error covariance matrix Λ in polar coordinates _kj Can be expressed by equation (8).
[0027]
[Equation 8]

[0028]
Symbol “8” ² (Or ^ 2) "represents square.
[0029]
Λ in each target to be tracked in equation (7) _kj Transformation matrix Γ _kj (T) can be expressed by the following equation (9).
[0030]
[Equation 9]

[0031]
R in equation (9) _pk (T), e _pk (T), b _pk (T) is the prediction vector VX, respectively. _pk Position component x of (t) _pk (T), y _pk (T), z _pk This is represented by the polar coordinate component of (t).
[0032]
r _pk (T), e _pk (T), b _pk (T) and x _pk (T), y _pk (T), z _pk The relationship (t) is expressed by the following equation (10).
[0033]
[Expression 10]

[0034]
The residual is the difference D between the position data of the detection data and the prediction vector. _kj -VZ _pk It is represented by (t).
[0035]
Therefore, the gate determination specification calculation unit 123 inputs the residual, residual covariance matrix, detection data, prediction vector, and prediction error covariance matrix to the gate determination unit 124 described later.
[0036]
For description of the gate determination means 124, the gate will be described. The gate is an error ellipse area expressed by the following equation (11).
[0037]
## EQU11 ##

[0038]
In Formula (11), VZ _pk (T) represents the position component of the prediction vector. In equation (11), d _ε Is a gate size parameter that determines the size of the gate. Here, the gate size parameter is a constant, and the subscript ε of the gate size parameter represents the probability that the target exists in the gate. The probability that the target exists in the gate will be referred to as the in-gate probability or the in-gate target existence probability in the future. Where the gate size parameter d _ε Represents the size of the gate when the in-gate probability ε. Gate size parameter d _ε Is determined in advance by the chi-square distribution in the statistics, and determines the degree of freedom and the in-gate probability ε to be set by the chi-square density function or the chi-square distribution table. In addition, the superscript “−1” in the expression (11) is a symbol representing an inverse matrix.
[0039]
Prediction vector VX _pk (T) is expressed as the following equation (12).
[0040]
[Expression 12]

[0041]
In equation (12), x _pk (T), y _pk (T), z _pk (T) is the predicted position of X, Y, Z, dx _pk (T), dypk (t), and dzpk (t) are the predicted speeds of X, Y, and Z.
[0042]
Position component VZ of the prediction vector in equation (11) _pk (T) and prediction vector VX of equation (12) _pk (T) has the relationship of the following equation (13) using the observation matrix H of equation (6).
[0043]
[Formula 13]

[0044]
The size of the gate represented by the equation (11) is the gate size parameter d. _ε And residual covariance matrix S _kj Determined by (t). Next, why gate size parameter d _ε And residual covariance matrix S _kj An example of whether the gate size is determined by (t) will be described below. In order to simplify the description below, a matrix and a vector are assumed as in the following equations (14) and (15). Then, the expression (11) is determined as the following expression (16) from the following expressions (14) and (15).
[0045]
[Expression 14]

[0046]
[Expression 15]

[0047]
[Expression 16]

[0048]
Here, since the matrix A in the equation (14) is a positive symmetric matrix of 3 rows and 3 columns, the following equations (17) and (18) hold.
[0049]
[Expression 17]

[0050]
[Expression 18]

[0051]
Here, it is assumed that the matrix A can be diagonalized with a diagonal matrix B and an orthogonal matrix C by algebra as shown in the following equation (19).
[0052]
[Equation 19]

[0053]
The diagonal matrix B in Equation (19) is the eigenvalue 1 / λ of the matrix A as shown in Equation (20) below. ₁ , 1 / λ ₂ , 1 / λ _Three It is assumed that Here, from the relationship of the equation (18), the eigenvalue 1 / λ ₁ , 1 / λ ₂ , 1 / λ _Three Are respectively 1 / λ ₁ > 0, 1 / λ ₂ > 0, 1 / λ _Three > 0. Here, “/” is a symbol representing division.
[0054]
[Expression 20]

[0055]
The orthogonal matrix B in the equation (19) is expressed as the following equation (21) using the eigenvectors v1 underbar, v2 underbar, and v3 underbar of the matrix A as in the following equation (21). Here, v1 underbar, v2 underbar, and v3 underbar are vectors of 3 rows and 1 column, respectively.
[0056]
[Expression 21]

[0057]
The eigenvectors v1 underbar, v2 underbar, and v3 underbar of the matrix A in Equation (21) have the relationship of Equation (22).
[0058]
[Expression 22]

[0059]
The expression (20) is expressed as the expression (23) using the expression (19).
[0060]
[Expression 23]

[0061]
Here, when the component of Cx in the equation (23) is expressed by the following equation (24), the equation (23) is expressed by the following equation (25) using the equations (20) and (24). expressed.
[0062]
[Expression 24]

[0063]
[Expression 25]

[0064]
Here, from the equations (14) and (20), the eigenvalue of the matrix A is 1 / λ. ₁ , 1 / λ ₂ , 1 / λ _Three So, by algebra, the residual covariance matrix S _kj The eigenvalue of (t) is λ ₁ , Λ ₂ , Λ _Three It is.
[0065]
Therefore, equation (25) indicates that the radius of the error ellipse is the eigenvalue λ of the residual covariance matrix. ₁ , Λ ₂ , Λ _Three And the gate size parameter d _ε It is determined by. That is, the size of the gate is the residual covariance matrix and the gate size parameter d. _ε It depends on.
[0066]
Thus, the size of the gate is the residual covariance matrix S _kj The larger the (t) and the smaller the inverse of the residual covariance matrix, the larger the gate size. Also, the larger the gate size parameter, the larger the gate size.
[0067]
Position component VZ of prediction vector of equation (11) _pk (T) represents the center of the gate. The left side of equation (11) is detection data D _kj And the position component VZ of the prediction vector which is the center of the gate _pk Let the residual of (t) be the residual covariance matrix S _kj The distance normalized by (t) is represented.
[0068]
Accordingly, the gate determination unit 124 detects the detection data D. _kj And the position component VZ of the prediction vector which is the center of the gate _pk Residual D of (t) _kj -VZ _pk (T) and residual covariance matrix S _kj (T) is input from the gate determination specification calculation means 123, it is determined whether or not the expression (11) as a gate is satisfied, and the detection data Dkj satisfying the expression (11) is set as a candidate for a target signal. Further, detection data D satisfying (11) _kj If there are a plurality of detection data, one detection data closest to the prediction vector is selected as a target signal candidate aD. _kj Output as. Further, the target signal candidate is aD _kj The residual covariance matrix corresponding to _kj (T). That is, the detection data aD in the gate is sent to the data updating means 106. _kj And aD _kj Residual covariance matrix aS corresponding to _kj (T), prediction error covariance matrix, prediction vector, position component VZ of prediction vector _pk Enter (t). If no detection data enters the gate, that is, in the case of a memory track, the detection data aD in the gate _kj Is the position component of the prediction vector and aD _kj Residual covariance matrix aS corresponding to _kj (T) is calculated using S / N set in advance.
[0069]
The memory track is a process of updating data without detecting data entering the gate.
[0070]
In the data updating means 106, the gain matrix K is based on the Kalman filter theory. _k (T) is calculated from the following equation (26). Also, the smooth vector VX _sk (T) is the gain matrix K of the following equation (26) _k Using (t), calculation is made according to equation (27). Furthermore, the smooth error covariance matrix P _sk (T) is the gain matrix K of equation (26) _k Using (t), calculation is made according to equation (28). Further, in the data updating means 106, the smooth vector VX calculated by the equations (27) and (28). _sk (T) and smoothing error covariance matrix P _sk (T) is output.
[0071]
[Equation 26]

[0072]
[Expression 27]

[0073]
[Expression 28]

[0074]
Based on the theory of the Kalman filter, the prediction means 107 calculates and outputs a prediction vector VXpk + 1 (t) at time k + 1 one sampling after the current time k according to the following equation (29). Also, according to the following equation (30), the prediction error covariance matrix P at time k + 1 after one sampling from the current time k _pk (T) is calculated and output.
[0075]
[Expression 29]

[0076]
[30]

[0077]
In equation (29), a state transition matrix Φk for performing one sampling extrapolation is represented by equation (31).
[0078]
[31]

[0079]
In Expression (31), Δt represents the current time k and the sampling interval from the current time to time k + 1 after one sampling.
[0080]
The drive noise covariance matrix Qk in equation (30) is represented by equation (32).
[0081]
[Expression 32]

[0082]
Σ in equation (32) _a Represents the standard deviation of the driving noise. The standard deviation of the driving noise is a constant representing the ambiguity of the target motion model of the Kalman filter.
[0083]
The one sampling delay means 108 receives the prediction vector VX at the sampling time k input from the prediction means 107. _pk (T), prediction error covariance matrix P _pk (T) is delayed by one sampling and input to the display means 109.
[0084]
In the display means 109, using the smooth vector input from the data update means 106, the position components of the smooth vectors for the past several samplings are connected by lines and displayed as a wake. Also, using the velocity component of the smooth vector, the velocity is displayed such that the position component of the smooth vector is the starting point and the end point indicates the target predicted position of the next sampling.
[0085]
A schematic diagram showing the configuration of a conventional tracking device is shown in FIG.
[0086]
In the tracking device as described above, it is difficult to adjust the size of the gate. Hereinafter, the reason will be described with reference to FIG.
[0087]
FIG. 47 is a diagram showing the relationship between the gate, the target predicted position, the target observation position, the target range direction, and the target cross range direction.
[0088]
In FIG. 47, G1 represents a gate at time t1. G1 ′ represents a gate for which a larger gate size parameter than G1 at time t1 is set. Here, the gate shapes of the gate G1 and the gate G1 ′ are the same. Further, O1 is a target observation position at time t1, P1 is a target predicted position at time t1, TGT1 is a target true position at time t1, and C1, 1 to C1, and 9 are unnecessary signals. Further, O2 is a target observation position at time t2, P2 is a target predicted position at time t2, TGT2 is a target true position at time t2, and C1, 1 to C1, and 11 are unnecessary signals. The target turns straight from TGT1 to TGT2.
[0089]
In FIG. 47, the direction connecting sensors S and R1 or the direction connecting sensors S and R2 is referred to as a target range direction. An axis orthogonal to the target range direction is referred to as a target cross range direction.
[0090]
For example, in FIG. 47, when the gate G2 at the target t2 is first set as shown by the solid-line ellipse in FIG. 47, the target observation position D2 is out of the range direction. Therefore, a gate size parameter larger than the gate size parameter constituting the gate G2 is set, and the gate G2 ′ is configured like a dashed ellipse so that the target observation position D2 enters. Then, although the target observation position D2 removed in the range direction can be supplemented by the gate G2 ′, the gate G2 ′ becomes large while maintaining the same shape as the gate G2, so that the gate radius in the cross range direction becomes large. End up.
[0091]
Here, generally, as shown in FIG. 48, the cross range error due to the angle error becomes larger as the distance between the sensor and the target becomes longer than the range error. For example, when the target distance is on the order of several hundred kilometers, the ratio of the range error to the cross range error is about 1: 1000 to 10,000. Therefore, when the gate size parameter is increased, the gate radius in the cross range direction is considerably larger than the gate radius in the range direction.
[0092]
Therefore, as in the example shown in FIG. 47, in the case of the gate G2 having a small gate size (solid line in the figure), the gate G2 ′ having a large gate size (broken line in the figure) even in the case where no unnecessary signal is input There is a problem that a large number of unnecessary signals are contained within.
[0093]
[Problems to be solved by the invention]
The conventional tracking device is configured as described above, and the cross range error due to the angle error increases as the distance between the sensor and the target increases, so the gate size parameter is increased. In this case, the gate radius in the cross range direction is considerably larger than the gate radius in the range direction, and the gate G2 having a large gate size is used even in the case where a small gate does not receive an unnecessary signal. 'There was a problem that many unnecessary signals entered.
[0094]
The present invention has been made to solve such problems, and an object of the present invention is to provide a tracking device that can easily obtain a target signal and can perform gate determination for making it difficult for an unnecessary signal to enter. .
[0095]
[Means for Solving the Problems]
The present invention obtains an S / N ratio, which is a signal-to-noise ratio, based on received power, converts it into distance, elevation angle, and azimuth angle observation noise standard deviations, and associates the observation noise standard deviations with them. An observation means for outputting detection data representing the position to be detected, a prediction error covariance matrix and the detection data are inputted, and from the residual covariance matrix calculated from them, the eigenvalue in the distance direction, the eigenvalue in the elevation direction, the azimuth Calculate eigenvalues in the angular direction, eigenvectors in the distance direction, eigenvectors in the elevation direction, and eigenvectors in the azimuth direction Perform diagonalization First gate determination specification calculation means and the detection data are in the distance direction In Of a given dimension Ellipsoid A determination is made as to whether or not the gate is entered and a signal indicating the result of the determination is output using the range direction gate determination means, and the data output from the observation means and the first gate determination specification calculation means. In the elevation direction and azimuth direction, Ellipsoid Cross range direction gate determination means for performing gate determination, and the distance direction so Of the predetermined dimension Ellipsoid A signal indicating the result of determining whether or not the gate is entered, and the elevation direction Direction and direction Azimuth direction so The predetermined dimension of Ellipsoid From the signal indicating the determination result of whether it is in the gate, distance direction, elevation angle direction and Azimuth all In The predetermined dimension Ellipsoid A first gate determination means for outputting detection data and a residual covariance matrix associated with the detection data as candidates for a target signal when the detection data is contained in the gate; and the first gate determination First detection means for inputting the detection data and the residual covariance matrix outputted from the means, updating the data based on a Kalman filter theory, and outputting a smooth error covariance matrix and a smooth vector; Based on the theory of the Kalman filter, a prediction means for calculating a prediction error covariance matrix and a prediction vector after one sampling from the current time, and a prediction error covariance matrix and a prediction vector input from the prediction means are delayed by one sampling. A one sampling delay for outputting the prediction error covariance matrix to the first gate determination specification calculation means. Means, from the smoothing vector received as input from the first data updating means, and display means for displaying the target track and the target speed comprises, of predetermined dimensions Ellipsoid The gate is an ellipsoidal gate of a predetermined dimension based on the residual covariance matrix, and the first gate determination specification calculating means is configured to obtain a diagonal of the residual covariance matrix in the ellipsoidal gate. Range direction and cross-range direction Decision Then, the range direction gate determination means and the cross range direction gate determination means are based on the range direction and the cross range direction calculated by the first gate determination specification calculation means. The gate size parameter in the cross range direction Setting Thus, the tracking device determines the size of the ellipsoidal gate.
[0096]
The cross range direction gate determination means determines whether or not the detection data is in a one-dimensional gate in the elevation angle direction, outputs a signal indicating the determination result, and the detection data is 1 in the azimuth direction. It is comprised from the gate determination means of the 1st cross-range direction which determines whether it is in the dimension gate, and outputs the signal which shows the determination result.
[0097]
The cross range direction gate determination means performs two-dimensional gate determination in the cross range direction using the eigenvector in the elevation angle direction, the eigenvalue in the elevation angle direction, the eigenvector in the azimuth direction, and the eigenvalue in the azimuth direction. 2 gate determination means in the cross range direction.
[0098]
Also, The output from the first data update means If the smooth vector velocity component exceeds a certain threshold, set the standard deviation of the drive noise vector to a large value. And output to the prediction means First drive noise control means is further provided.
[0099]
A logistic curve with the standard deviation of the drive noise vector as the vertical axis and the velocity component of the smooth vector as the horizontal axis. On the line Based on the velocity component of the smooth vector According to the size of Set a large standard deviation of the driving noise vector To switch Second drive noise control means is further provided.
[0100]
In addition, the present invention obtains an S / N ratio that is a signal-to-noise ratio based on the received power and converts it into observation noise standard deviations of distance, elevation angle, and azimuth, and the observation noise standard deviations and those An observation means for outputting detection data representing a position associated with the position, a prediction error covariance matrix and the detection data are input, and from the residual covariance matrix calculated from them, eigenvalues in the distance direction and eigenvalues in the elevation direction Calculate eigenvalues in azimuth direction, eigenvectors in distance direction, eigenvectors in elevation direction, and eigenvectors in azimuth direction Perform diagonalization A first gate determination specification calculation means and the detection data in the distance direction; Ellipsoid It is determined whether or not it is in the gate, and a gate determination means in the range direction that outputs a signal indicating the determination result, and the detection data is in the elevation direction Ellipsoid It determines whether or not it is in the gate, outputs a signal indicating the determination result, and the detection data is in the azimuth direction Ellipsoid First cross range direction gate determination means for determining whether or not the gate is entered and outputting a signal indicating the determination result; the range direction gate determination means; and the first cross range direction gate. As a result of the gate determination of the detection data in the determination means, for the detection data in the gate, the distance between the gate center and the detection data is converted into the likelihood of the normal distribution, the data is updated using the likelihood, smoothing error A second data updating unit that outputs a covariance matrix and a smooth vector, a prediction unit that calculates a prediction error covariance matrix and a prediction vector after one sampling from the current time based on the theory of the Kalman filter, and the prediction unit The input prediction error covariance matrix and the prediction vector are delayed by one sampling, and the first gate determination specification calculation means And outputs, includes a 1 sample delay means, a smoothing vector inputted from said second data updating means, and display means for displaying the target track and the target speed, and The ellipsoidal gate in the distance direction, the ellipsoidal gate in the elevation angle direction, and the ellipsoidal gate in the azimuth direction are combined to form a predetermined dimension ellipsoidal gate, The predetermined dimension Ellipsoid The gate is an ellipsoidal gate of a predetermined dimension based on the residual covariance matrix, and the first gate determination specification calculating means is configured to obtain a diagonal of the residual covariance matrix in the ellipsoidal gate. Range direction and cross-range direction Decision And the range direction gate determination means and the range direction First The gate determination means for the cross range direction is configured to determine the gate size parameter for the range direction and the cross range direction based on the range direction and the cross range direction calculated by the first gate determination specification calculation means. Setting Thus, the tracking device determines the size of the ellipsoidal gate.
[0101]
In addition, first gate size parameter setting means for setting a gate size parameter according to the sampling interval is further provided.
[0102]
Further, an observation error fixing means for setting the observation noise standard deviation of the distance, the observation noise standard deviation of the elevation angle, and the observation noise standard deviation of the azimuth angle to fixed values is further provided.
[0103]
Further, the apparatus further includes sampling interval fixing means for inputting a control signal for fixing the sampling interval to the prediction means and fixing the sampling interval when calculating the prediction error covariance matrix and the prediction vector.
[0104]
Further, gate center fixing means for fixing the center of the gate for several samplings from the initial tracking is further provided.
[0105]
Also, the above First Gate size parameter variable means is further provided for changing the size of the gate size parameter using the magnitude of the velocity component of the smooth vector obtained from the data update means.
[0106]
Also, the above First First smoothing means for displaying is further provided for calculating a smoothing vector for display by inputting the smoothing vector obtained from the data updating means to a display filter having a fixed time constant.
[0107]
Also, the above First Second display smoothing means is further provided for inputting a smooth vector obtained from the data updating means to a display filter having a variable time constant and calculating a display smooth vector.
[0109]
If the condition number that is the ratio of the maximum value of the eigenvalue to the minimum value of the eigenvalue is used among the eigenvalues input from the first gate determination specification calculation means, The apparatus further includes condition number determination means for inputting a danger signal indicating that the calculation is unstable to the display means.
[0110]
DETAILED DESCRIPTION OF THE INVENTION
Embodiment 1 FIG.
A tracking device according to Embodiment 1 of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram showing a configuration of a tracking device according to an embodiment of the present invention. In the following embodiments, the same reference numerals in the drawings represent the same or corresponding parts.
[0111]
In FIG. 1, 1 obtains an S / N ratio that is a signal-to-noise ratio based on the received power obtained from the sensor, converts it to an observation noise standard deviation of distance, elevation angle, and azimuth angle, and This is observation means for outputting noise standard deviations and detection data representing the positions associated therewith. 2 is inputted with a prediction error covariance matrix from the 1 sampling delay means 8 described later and an observation error covariance matrix from the observation means 1, and from the residual covariance matrix calculated from them, the eigenvalue in the distance direction and the elevation direction It is a first gate determination specification calculation means for calculating an eigenvalue, an eigenvalue in an azimuth direction, an eigenvector in a distance direction, an eigenvector in an elevation direction, and an eigenvector in an azimuth direction. Reference numeral 3 denotes range direction gate determination means for determining whether or not the detection data is in a one-dimensional gate in the distance direction and outputting a signal indicating the determination result. 4 determines whether the detection data is in the one-dimensional gate in the elevation direction, outputs a signal indicating the determination result, and determines whether the detection data is in the one-dimensional gate in the azimuth direction. This is a first cross range direction gate determination means for performing a determination and outputting a signal indicating the determination result. Reference numeral 5 denotes a signal indicating a determination result of whether or not a one-dimensional gate in the distance direction, a signal indicating a determination result of whether or not a one-dimensional gate in the elevation direction, and 1 in the azimuth direction As a target signal candidate when the detection data is contained in all the one-dimensional gates in the distance direction, the elevation angle direction, and the azimuth direction based on the signal indicating the determination result of whether or not it is in the dimension gate. , First gate determination means for outputting detection data and residual covariance matrix associated with the detection data. Reference numeral 6 denotes first data updating means for updating data based on the Kalman filter theory and outputting a smoothing error covariance matrix and a smoothing vector. Reference numeral 7 denotes prediction means for calculating a prediction error covariance matrix and a prediction vector after one sampling from the current time based on the Kalman filter theory. Reference numeral 8 denotes a one-sampling delay unit that delays the prediction error covariance matrix and the prediction vector input from the prediction unit 7 by one sampling and outputs them to the first gate determination specification calculation unit 2. Reference numeral 9 denotes display means for displaying the target track and the target speed from the smooth vector input from the first data updating means. 6, 7, 8, and 9 correspond to the above-described 106, 107, 108, and 109 in FIG. 45, and detailed description thereof is omitted here.
[0112]
The first gate determination specification calculation means 2 will be described below.
[0113]
The first gate determination specification calculation means 2 receives detection data, distance observation noise standard deviation, elevation observation noise standard deviation and azimuth angle observation noise standard deviation from the observation means 1, and from the prediction means 7, A prediction vector and a prediction error covariance matrix are input via the one sampling delay means 8.
[0114]
Further, the first gate determination specification calculation means 2 calculates eigenvectors in the range direction and eigenvectors in the cross range direction, and the calculation method is described below.
[0115]
Figure 49 shows the eigenvectors in the range direction and the eigenvectors in the cross range direction.
The relationship diagram is shown.
[0116]
A three-dimensional elliptical region in FIG. 49 is a gate. In FIG. 49, A represents the predicted position, B represents the observed position in the gate, and these position vectors are represented by VZ. _pk (T), D _kj And The unit vector in the distance direction (OA direction) is Ur, the unit vector in the z direction is Uel, and the unit vector in the cross range lateral direction (HI direction) orthogonal to the distance in the horizontal plane is Uby. Further, OG, CD, and EF orthogonal to each other in the gate are set as x1, x2, and x3 axes. x1, x2, and x3 are x in the equation (25). ₁ , X ₂ , X _Three It corresponds to. The direction of each axis of x1, x2, and x3 is the eigenvector v in the equation (21). ₁ Underbar, v ₂ Underbar, v _Three Each is defined by an underbar.
[0117]
Hereafter, the eigenvector v ₁ Underbar, v ₂ Underbar, v _Three Underbar is the eigenvector v in the distance direction _r Underbar, eigenvector v in the elevation direction _el , Azimuthal eigenvector v _by Shows how to assign as. Here, the distance direction is the same as the range direction, the elevation direction is the vertical direction of the cross range, and the azimuth direction is the same as the horizontal direction of the cross range.
[0118]
Eigenvector v ₁ Underbar, v ₂ Underbar, v _Three Unit U in the distance direction of the underbar _r Is defined as the eigenvector in the distance direction. The eigenvalue corresponding to the eigenvector is λ _r And That is, it determines with following Formula (33).
[0119]
[Expression 33]

[0120]
In Equation (33), max is the inner product U _r ・ V ₁ , U _r ・ V ₂ , U _r ・ V _Three Of these, it works to choose the largest one.
[0121]
However, U _r Is defined by the following equation (34).
[0122]
[Expression 34]

[0123]
Here, the symbol “||” in Equation (34) represents the Euclidean norm in algebra.
[0124]
Next, the eigenvector v excluding those selected as eigenvectors in the distance direction ₁ Underbar, v ₂ Underbar, v _Three The remaining two eigenvectors of the underbar are v ₁ ', V ₂ 'And. In v1′v2 ′, the unit vector U in the elevation direction _el Is called the eigenvector in the elevation direction, and v _el And And its eigenvalue is λ _el And Where v _el Is determined by equation (35). Also, the unit vector U in the elevation direction _el Is defined by equation (36).
[0125]
[Expression 35]

[0126]
[Expression 36]

[0127]
Eigenvector v ₁ Underbar, v ₂ Underbar, v _Three Among the underbars, the remaining one eigenvector excluding the selected two as eigenvectors in the distance direction and eigenvectors in the elevation angle direction is called an eigenvector in the azimuth direction, and v _by And And its eigenvalue is λ _by And
[0128]
Through the above processing, the eigenvector in the distance direction, the eigenvector in the elevation direction, and the eigenvector in the azimuth direction are determined. That is, the eigenvector in the range direction is an eigenvector in the distance direction, and the cross range direction is an eigenvector in the elevation direction and an eigenvector in the azimuth direction. Therefore, the residuals in each direction of the eigenvector in the distance direction, the eigenvector in the azimuth direction, and the eigenvector in the elevation direction are respectively v _r X, v _el X, v _by • Represented by x.
[0129]
Therefore, the first gate determination specification calculation unit 2 uses the detection data obtained from the observation unit 1 and the prediction vector obtained from the prediction unit 7 via the one sampling delay unit 8 to calculate the residual by the equation (15). And the residual covariance matrix is calculated using equation (16). Furthermore, the eigenvalue λ in the distance direction _r , Residual eigenvector in distance direction v _r X, and a prediction vector, a prediction error covariance matrix, detection data, a residual, and a residual covariance matrix are input to the gate determination means 3 in the range direction. Similarly, the eigenvalue λel in the elevation direction and the residual v of the eigenvector in the elevation direction _el X, eigenvalue λ in azimuth direction _by , Eigenvector residual in azimuth direction v _by X, and a prediction vector, a prediction error covariance matrix, detection data, a residual, and a residual covariance matrix are input to the gate determination means 4 in the first cross range direction.
[0130]
Next, the gate determination unit 3 in the range direction will be described.
[0131]
The gate determination means 3 in the range direction determines whether or not the detection data is in the one-dimensional gate in the distance direction using the equation (37). Here, d on the right side of the equation (37) _r Is a gate size parameter set in advance in the distance direction.
[0132]
[Expression 37]

[0133]
Therefore, in the range direction gate determination means 3, a determination signal, prediction vector, prediction error covariance matrix, detection data, residual data and residual covariance matrix indicating whether or not the distance gate of equation (37) is entered. Input to the first gate determination means 5.
[0134]
Next, the gate determination means 4 in the first cross range direction will be described.
[0135]
The first cross range direction gate determination means 4 determines whether the detection data is in the one-dimensional gate in the elevation angle direction using the following equation (38). Here, d on the right side of the equation (38) _el Is a gate size parameter set in advance in the elevation direction.
[0136]
[Formula 38]

[0137]
Further, the first cross range direction gate determination means 4 determines whether or not the detection data is in the one-dimensional gate in the azimuth direction using the equation (39). Here, d on the right side of the equation (39) _by Is a gate size parameter set in advance in the azimuth direction.
[0138]
[39]

[0139]
Therefore, in the first cross range direction gate determination means 4, a determination signal indicating whether or not the vehicle is in the elevation gate of equation (38), and a determination that indicates whether or not the vehicle is in the azimuth gate of equation (39). A signal, a prediction vector, a prediction error covariance matrix, detection data, a residual, and a residual covariance matrix are input to the first gate determination unit 5.
[0140]
In the first gate determination means 5, a determination signal, a prediction vector, a prediction error covariance matrix, and detection data indicating whether or not the distance gate of equation (37) is entered are input from the range direction gate determination means 3. The In the first cross range direction gate determination means 4, the cross range direction gate determination means 4 determines whether or not it is in the elevation gate of equation (38), and the azimuth angle of equation (39). A determination signal indicating whether or not the gate is entered, a prediction vector, a prediction error covariance matrix, and detection data are input.
[0141]
In the first gate determining means 5, a determination signal indicating whether or not the vehicle is in the distance gate of the equation (37), a determination signal indicating whether or not the vehicle is in the elevation gate of the equation (38), When all of the determination signals indicating whether or not they are in the azimuth gate are signals indicating that they are in the gate, the detection data D corresponding to these determination signals _kj Is a candidate for the target signal. Also, a determination signal indicating whether or not it is in the distance gate of the equation (37), a determination signal indicating whether or not it is in the elevation gate of the equation (38), and whether it is in the azimuth gate of the equation (39) For a plurality of detection data when there are a plurality of signals indicating that all of the determination signals indicating whether or not are in the gate, one detection data closest to the prediction vector is selected as the target signal candidate aD. _kj As output. Also, detection data aD _kj Residual covariance matrix aS corresponding to _kj (T) is also output. Further, the position component of the prediction vector is calculated by equation (13).
[0142]
Therefore, the first gate determination means 5 sends the target signal candidate aD in the detection data to the first data update means 6. _kj And aD _kj Residual covariance matrix aS corresponding to _kj (T), the position component of the prediction vector, the prediction error covariance matrix, the prediction vector, the residual, and the residual covariance matrix are input.
[0143]
Therefore, in the conventional tracking device, the gate determination process is performed using the equation (11). Therefore, as shown in FIG. 47, if the gate size parameter is increased to increase the detection data in the range direction, In particular, there is a problem that a large number of unnecessary signals are picked up because the gate spreads in the cross range direction. However, the gate size is independently determined in the range direction and the cross range direction by performing the gate determination separately in the range direction and the cross range direction as in the expressions (37), (38), and (39). Can be set. Therefore, even when you want to expand the gate only in the range direction, there is no effect on the cross range direction, so it is possible to set the gate optimally in the sense that the target signal is supplemented and unnecessary signals are supplemented as little as possible. is there.
[0144]
Next, FIGS. 3 and 4 are flowcharts for explaining the operation of the first embodiment.
[0145]
First, in step ST1, S / N and detection data obtained from the sensor are input.
[0146]
Next, in step ST2, a residual and a residual covariance matrix are calculated.
[0147]
Next, in step ST3, eigenvalues and eigenvectors are calculated from the residual covariance matrix.
[0148]
Next, in step ST4, inner products of the three eigenvectors calculated from the residual covariance matrix and the unit vector in the range direction are calculated.
[0149]
Next, in step ST5, the largest inner product of the unit vector in the range direction and the eigenvector is determined as the eigenvector in the range direction. The eigenvector in the range direction is synonymous with the eigenvector in the distance direction. Of the three eigenvectors, two eigenvectors in the cross range direction excluding the eigenvectors in the range direction are input to step ST6.
[0150]
Next, in step ST6, inner products of the two eigenvectors in the cross range direction and the unit vector in the z-axis direction are respectively calculated and input to step ST7.
[0151]
Next, in step ST7, among the two cross range direction eigenvectors, the one having the largest inner product of the z-axis direction unit vector and the eigenvector is determined as the cross range vertical eigenvector, and the remaining one eigenvector is crossed. Determine the eigenvector in the horizontal direction of the range. The eigenvector in the vertical direction of the cross range is synonymous with the eigenvector in the elevation direction. Similarly, the eigenvector in the cross range lateral direction is synonymous with the eigenvector in the azimuth direction.
[0152]
Next, in step ST8, residuals in the range direction, the cross range vertical direction, and the cross range horizontal direction are calculated from the eigenvectors in the range direction and the cross range direction.
[0153]
Next, in step ST9, gate size parameters in the range direction, cross range vertical direction, and cross range horizontal direction are set.
[0154]
Next, in step ST10, when the expressions (37), (38), and (39) are satisfied, that is, when all the gates in the range direction, the cross range vertical direction, and the cross range horizontal direction are satisfied, in step ST11 The detection data is determined as in-gate detection data. Further, when the equations (37), (38), and (39) are not satisfied, that is, when at least one gate is not satisfied in the range direction, the cross range vertical direction, and the cross range horizontal direction, step ST12. The detection data is determined as out-gate detection data.
[0155]
Next, in step ST13, when the number n of the total number of detection data at the sampling time k becomes n = Nobs when the counter n for counting the processes from ST1 to ST12 is reached, the process proceeds to step ST14. If not n = Nobs, the processing of ST1 to ST12 is continued.
[0156]
Next, in step ST14, one of the in-gate detection data that is closest to the center of the gate is selected, and the selected in-gate detection data and its associated residual covariance matrix are transferred to the next step ST15. input.
[0157]
Next, in step ST15, data is updated based on the theory of Kalman filter, and a smooth vector and a smooth error covariance matrix are calculated. Further, one sampling extrapolation is performed using the smooth vector and smooth error covariance matrix at the current time, and a prediction vector and prediction error covariance matrix after one sampling from the current time are calculated.
[0158]
Next, in ST16, the smooth position and the smooth speed are displayed using the smooth vector at the current time.
[0159]
Next, in step ST17, when the sampling time k becomes larger than the sampling end time kend, the processing is interrupted. If the sampling time k is less than or equal to the sampling end time kend, the processing is continued.
[0160]
As described above, in the target tracking device according to the present embodiment, it is possible to perform gate determination to make it easier for a target signal to enter and to prevent an unnecessary signal from entering.
[0161]
Embodiment 2. FIG.
A tracking device according to Embodiment 2 of the present invention will be described with reference to the drawings. FIG. 5 is a block diagram showing the configuration of the tracking device according to Embodiment 2 of the present invention. In FIG. 5, reference numeral 10 denotes a second gate determination means in the cross range direction. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0162]
The gate determination means 10 in the second cross range direction will be described below. In the second cross range direction gate determination means 10, the eigenvalue λ in the elevation angle direction from the first gate determination specification calculation means 2. _el , Residual eigenvector of elevation angle v _el X, eigenvalue λ in azimuth direction _by , The residual of the eigenvector of the azimuth v _by X, prediction vector, prediction error covariance matrix, detection data, residual, residual covariance matrix are input. Furthermore, a two-dimensional gate determination is performed using the following equation (40).
[0163]
[Formula 40]

[0164]
In equation (40), the gate size parameter d _elby Is set in advance.
[0165]
Therefore, in the second cross range direction gate determination means 10, a determination signal indicating whether or not the gate of the equation (40) is entered, a prediction vector, a prediction error covariance matrix, detection data, a residual and a residual are shared. The variance matrix is input to the first gate determination means 5.
[0166]
In the first gate determination means 5, a determination signal, a prediction vector, a prediction error, and a covariance matrix indicating whether or not the distance gate of Expression (37) is entered are input from the range direction gate determination means 3. In addition, a determination signal, a prediction vector, a prediction error, and a covariance matrix that indicate whether or not the distance gate of Expression (40) is entered are input from the second cross range direction gate determination means 10. These determinations are made when all of the determination signal indicating whether or not the gate of the equation (37) is entered and the determination signal indicating whether or not the gate of the equation (40) are all included in the gate. The detection data Dkj corresponding to the signal is set as a target signal candidate. In addition, when there are a plurality of signals indicating that all of the determination signals indicating whether or not they are in the distance gate of Expression (37) and the determination signals indicating whether or not they are in the gate of Expression (40) are in the gate As for the plurality of detection data, the detection data closest to the prediction vector is selected as the target signal candidate aD. _kj As output. Also, detection data aD _kj Residual covariance matrix aS corresponding to _kj (T) is also output. Further, the position component of the prediction vector is calculated by equation (13).
[0167]
By performing gate determination according to equations (37) and (40), gate determination can be made independently in the range direction and the cross range direction in which the ratio of the gate radii is greatly different. As shown in FIG. When is increased, it can be prevented that the cross range direction also increases. Further, by performing gate determination according to equation (40), the gate size parameter in the vertical direction of the cross range, the gate size parameter in the horizontal direction of the cross range, that is, the gate size parameter in the elevation angle direction, and the gate size parameter in the azimuth direction. And setting the gate size parameter only once.
[0168]
Next, FIGS. 6 and 7 are flowcharts for explaining the operation of the second embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST8b and step ST9b will be described here.
[0169]
In step ST8b, the sum of the cross range vertical residual divided by the cross range vertical eigenvalue and the cross range horizontal residual divided by the cross range horizontal eigenvalue is calculated.
[0170]
In step ST9b, the gate size parameter d set to the gate in the range direction in the equation (37) _r And a gate size parameter d set to the gate in the two-dimensional cross-range direction in the equation (40) _elby Set up.
[0171]
Therefore, by performing the gate determination according to the equations (37) and (40) according to the above steps, the gate determination can be performed independently in the range direction and the cross range direction in which the ratio of the gate radii is greatly different. As in the case of 50, when the gate in the range direction is enlarged, it is possible to prevent the cross range direction from being increased. Further, by performing gate determination according to equation (40), the gate size parameter in the vertical direction of the cross range, the gate size parameter in the horizontal direction of the cross range, that is, the gate size parameter in the elevation angle direction, and the gate size parameter in the azimuth direction. And setting the gate size parameter only once.
[0172]
Embodiment 3 FIG.
A tracking device according to Embodiment 3 of the present invention will be described with reference to the drawings. FIG. 8 is a block diagram showing the configuration of the tracking device according to Embodiment 3 of the present invention. In FIG. 8, 11 is a first drive noise control means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0173]
Prediction error covariance matrix P based on Kalman filter theory _pk (T) can be calculated by equation (30).
[0174]
In the first drive noise control means 11, the standard deviation σa of the drive noise vector in the equation (32) when calculating the prediction error covariance matrix is used as the velocity component of the smooth vector obtained by the first data update means 6. If the magnitude is larger than a certain threshold, it is set larger, and if the magnitude of the velocity component of the smooth vector obtained by the first data updating means 6 is smaller than a certain threshold, it is set smaller.
[0175]
Increasing the standard deviation σa of the drive noise vector in equation (32) increases the prediction error covariance matrix by equation (30), and further increases the residual covariance matrix by equation (5). The gate becomes larger. Therefore, when turning when the target speed is high, the target cannot be captured in the gate if the drive noise is set small. However, in the first driving noise control means 11, when the magnitude of the velocity component of the smooth vector becomes larger than a certain threshold, the standard deviation of the driving noise vector is increased and the gate is expanded, so that the target can be easily supplemented. .
[0176]
Next, FIGS. 9 and 10 are flowcharts for explaining the operation of the third embodiment. Here, steps having the same reference numerals as those in FIGS. 3 and 4 are omitted. That is, step ST15b and step ST15c will be described.
[0177]
In step ST15b, data is updated based on the Kalman filter theory, and a smooth vector and a smooth error covariance matrix are calculated.
[0178]
In step ST15c, if the magnitude of the velocity component of the smooth vector exceeds a certain threshold before performing the prediction process, the standard deviation of the drive noise vector is increased. Thereafter, a prediction process based on the Kalman filter theory is performed to calculate a prediction vector and a prediction error covariance matrix.
[0179]
Therefore, by performing the above steps, when the magnitude of the velocity component of the smooth vector becomes larger than a certain threshold value, it becomes easy to supplement the target by increasing the standard deviation of the drive noise vector and widening the gate.
[0180]
Embodiment 4 FIG.
A tracking device according to Embodiment 4 of the present invention will be described with reference to the drawings.
FIG. 11 is a block diagram showing the configuration of the tracking device according to Embodiment 4 of the present invention. In FIG. 8, 12 is a 2nd drive noise control means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0181]
In the second driving noise control means 12, the driving noise standard deviation σa is switched using the equation (41) according to the magnitude of the velocity component of the smooth vector obtained by the first smoothing means 6.
[0182]
[Expression 41]

[0183]
Equation (41) is a logistic curve. A conceptual diagram of the logistic curve is shown in FIG.
[0184]
The horizontal axis of FIG. 12 is the magnitude V of the velocity component of the smooth vector, and the vertical axis is the standard deviation σa of the drive noise. Here, V0 is a threshold value for switching the magnitude V of the position component of the smooth vector, and σa0 is a standard deviation of drive noise corresponding to V0.
[0185]
In Equation (41), a, b, and c are parameters that determine the shape of the curve. Here, in the scaling parameters a, b, and c, when a is increased, the vertical width of the curve is increased as shown in FIG. 12, and when b is increased, the vertical width is decreased and the gradient is smoothed. When is increased, the vertical width is the same and the gradient is steep.
[0186]
Therefore, if the standard deviation σa of the drive noise is suddenly increased, the gains will fluctuate greatly and the calculated values of the smoothing vector and the smoothing error covariance matrix will be strange. However, by switching the standard deviation σa of the driving noise as in the equation (41), the transition of the gain becomes smooth before and after switching, so that the speed vector can be switched stably, Gradually increase the standard deviation of the drive noise vector and widen the gate, making it easier to supplement the target.
[0187]
Next, FIGS. 13 and 14 are flowcharts for explaining the operation of the fourth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST15d will be described here.
[0188]
In step ST15d, after the scaling parameter of the logistic curve is determined in advance, the standard deviation of the driving noise is switched according to the velocity component magnitude V of the target smooth vector.
[0189]
Therefore, by performing the above steps, by switching the standard deviation σa of the driving noise, the transition of the gain becomes smooth before and after switching, and the speed vector can be switched stably. In addition, since the standard deviation of the drive noise vector is gradually increased and the gate is expanded, the target can be easily supplemented.
[0190]
Embodiment 5 FIG.
A tracking apparatus according to Embodiment 5 of the present invention will be described with reference to the drawings. FIG. 15 is a block diagram showing the configuration of the tracking device according to Embodiment 5 of the present invention. In FIG. 15, reference numeral 13 denotes a second data update unit. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0191]
The second data updating unit 13 will be described with reference to FIGS. In FIG. 46, the prediction vector VX surrounded by a broken line at the current time tk. _pk In the gate area centering on the position component of (t), the detection data D _k1 , D _k2 , D _k3 Is present. In the gate determination and data update means 13 in consideration of the weighting, a determination signal indicating whether or not the distance gate of the expression (37) is inputted from the range direction gate determination 3 means, the first cross range direction All of the determination signal indicating whether or not it is in the elevation gate of the equation (38) and the determination signal indicating whether or not it is in the azimuth gate of the equation (39) inputted from the gate determination means 4 enter the gate. Is detected signal Dkj corresponding to these determination signals, the target signal
Candidate. Here, a determination signal indicating whether or not it is in the distance gate of the equation (37), a determination signal indicating whether or not it is in the elevation gate of the equation (38), and an azimuth gate of the equation (39) Consider a case where there are a plurality of signals that all of the determination signals indicating whether or not there are gates. In this case, in the second data updating unit 13, for a plurality of detection data, a smooth vector and a smooth error covariance in consideration of the distance between the gate centers based on the likelihood obtained by converting the distance from the gate center by a normal distribution. Calculate the matrix.
[0192]
FIG. 50 shows a conceptual diagram of likelihood obtained by converting the distance from the gate center. D in FIG. _k1 , D _k2 , D _k3 , D _k4 , D _k5 Is detection data, D _k1 , D _k2 , D _k3 Is in the gate. Here, detection data D existing in the gate _k1 , D _k2 , D _k3 Is the likelihood γ of the normal distribution as shown in FIG. _k1 , Γ _k2 , Γ _k3 It is represented by
[0193]
Therefore, if the detection data closest to the gate center is an unnecessary signal, calculating the smoothing vector and smoothing error covariance matrix causes the center of the gate after the next sampling to be extremely pulled by the unnecessary signal at the current time, and tracking is performed. May fail. However, by calculating the smoothing vector and the smoothing error covariance matrix in consideration of the distance between the gate centers, the gate determination and data updating means 13 in consideration of the weighting, and the likelihood obtained by converting the distance from the gate center, The center of the gate after the next sampling is not extremely pulled by the unnecessary signal at the current time, and stable tracking can be performed in the unnecessary signal.
[0194]
Next, FIGS. 16 and 17 are flowcharts for explaining the operation of the fifth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST14b and step ST15e will be described.
[0195]
In step ST14b, the likelihood of the detection data in the gate is calculated.
[0196]
In step ST15e, a smooth vector and a smooth error covariance matrix are calculated using the likelihood of the detection data. Also, a prediction vector and a prediction error covariance matrix are calculated by extrapolating one smoothing vector in consideration of the distance between gate centers at the current time k.
[0197]
Therefore, if the detection data closest to the gate center is an unnecessary signal, the smoothing vector and the smoothing error covariance matrix are calculated, and the center of the gate after the next sampling is extremely reduced to the unnecessary signal at the current time. The tracking may fail, but by calculating the smoothing vector and smoothing error covariance matrix considering the distance between the gate centers based on the likelihood of the detection data converted from the distance from the gate center, The center of the gate after sampling becomes extremely unnecessary at the current time.
Stable tracking can be performed in unnecessary signals without being pulled.
[0198]
Embodiment 6 FIG.
A tracking apparatus according to Embodiment 6 of the present invention will be described with reference to the drawings. FIG. 18 is a block diagram showing the configuration of the tracking device according to Embodiment 6 of the present invention. In FIG. 13, reference numeral 14 denotes first gate size parameter setting means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0199]
The first gate size parameter setting means 14 looks at the time interval between the current sampling time and the previous sampling time obtained from the observation means, and if the time interval is large, a signal for setting the gate size parameter to be small is displayed in the range direction. Are output to the gate determination means 3 and the gate determination means 4 in the first cross-range direction.
[0200]
The gate radius that determines the size of the gate is the eigenvalue λ as in the denominator of the left side of equation (25). ₁ And d _ε The product of the power of 0.5, λ ₂ And d _ε The product of the power of 0.5, λ _Three And d _ε The eigenvalue λ ₁ , Λ ₂ , Λ _Three And gate size parameter d _ε Determined by the power of 0.5.
[0201]
Here, as the sampling interval increases, the eigenvalue λ ₁ , Λ ₂ , Λ _Three Increases the gate radius. However, when the sampling interval becomes extremely long, the eigenvalue becomes extremely large, so that the gate becomes very large. For this reason, not only the target signal but also a large amount of unnecessary signals have entered the gate, making it meaningless to apply the gate. Therefore, when the sampling interval is larger than a certain threshold, the gate size parameter is set to be small so that the gate radius of an appropriate size is maintained to prevent the target signal from being captured and unnecessary signals from entering. To.
[0202]
Next, FIGS. 19 and 20 are flowcharts for explaining the operation of the sixth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST9b will be described.
[0203]
In step ST9b, when the sampling interval is larger than a certain threshold value, control is performed so that the gate size parameter values set in the range direction, the cross range horizontal direction, and the cross range vertical direction are set small.
[0204]
By the above steps, when the sampling interval is larger than a certain threshold, the gate size parameter is set to be small so that an appropriate gate radius is maintained, so that the target signal can be captured and unnecessary signals can be introduced. To prevent.
[0205]
Embodiment 7 FIG.
A tracking apparatus according to Embodiment 7 of the present invention will be described with reference to the drawings. FIG. 21 is a block diagram showing the configuration of the tracking device according to Embodiment 7 of the present invention. In FIG. 21, 15 is an observation error fixing means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0206]
In the observation error fixing means 15, the observation noise standard deviation of the distance, the observation noise standard deviation of the elevation angle, and the observation noise standard deviation σr of the azimuth angle in the equation (8). _kj , Σe _kj , Σb _kj Set a fixed value to.
[0207]
Residual covariance matrix S in equation (5) _kj R in (t) _kj Is calculated by equation (7), and R in equation (7) _kj Λ in _kj Is calculated by equation (8). Therefore, the distance observation noise standard deviation, the elevation angle observation noise standard deviation, and the azimuth angle observation noise standard deviation σr in equation (8) _kj , Σe _kj , Σb _kj By setting a fixed value to, the calculation of equation (8) is not required for each sampling time and for each detection data, thereby reducing the calculation load.
[0208]
Next, FIGS. 22 and 23 are flowcharts for explaining the operation of the seventh embodiment. Here, steps with the same reference numerals are omitted. That is, step ST2b will be described.
[0209]
In step ST2b, the residual is calculated by equation (15). In addition, the standard deviation of the observation noise is fixed and Λ _kj And Λ _kj Is substituted into equation (7) and R _kj After calculating the residual covariance matrix.
[0210]
By the above steps, the distance observation noise standard deviation, the elevation angle observation noise standard deviation, and the azimuth angle observation noise standard deviation σr in equation (8) _kj , Σe _kj , Σb _kj By setting a fixed value to, the calculation of equation (8) is not required for each sampling time and for each detection data, thereby reducing the calculation load.
[0211]
Embodiment 8 FIG.
A tracking device according to Embodiment 8 of the present invention will be described with reference to the drawings. FIG. 24 is a block diagram showing the structure of the tracking device according to Embodiment 8 of the present invention. In FIG. 17, 16 is a sampling interval fixing means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0212]
The sampling interval fixing means 16 inputs a control signal for controlling the sampling interval to the prediction means 7. Therefore, since the state transition matrix Φk in the equation (31) has a fixed value, the calculation formula of the prediction vector of the equation (29) including the state transition matrix Φk and the prediction error covariance of the equation (30) including the state transition matrix Φk. The calculation load related to the matrix is reduced.
[0213]
Next, FIGS. 25 and 26 are flowcharts for explaining the operation of the eighth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST15f will be described.
[0214]
In step ST15f, using the state transition matrix Φk with a fixed sampling interval, the calculation formula of the prediction vector of Expression (29) and the prediction error covariance matrix of Expression (30) are calculated.
[0215]
By handling the state transition matrix Φk in the equation (31) as a fixed value by the above steps, the calculation load of the prediction vector and the prediction error covariance matrix is reduced.
[0216]
Embodiment 9 FIG.
A tracking device according to Embodiment 9 of the present invention will be described with reference to the drawings. FIG. 27 is a block diagram showing the configuration of the tracking device according to Embodiment 9 of the present invention. In FIG. 19, 17 is a gate center fixing means. In addition, the same code | symbol in each figure represents the same or an equivalent part.
[0217]
In the gate center fixing means 17, the position component VZ of the prediction vector on the right side of the equation (15) for several samplings from the initial tracking. _pk Instead of the value of (t), the detection data of the fixed first sampling is continuously used.
[0218]
The sampling number for fixing the gate is set in advance.
[0219]
Therefore, when the prediction vector is calculated from the detected data that varies in the initial stage of tracking, the variation of the velocity vector is large. In other words, if a variable velocity vector is used when calculating a prediction vector, the center of the gate is not stable, and there is a high possibility that the target signal cannot be supplemented. However, the position component VZ of the prediction vector that is the center of the gate by the number of samplings at the beginning of tracking. _pk By continuing to use the fixed first sampling detection data instead of (t), variations at the beginning of tracking can be reduced.
[0220]
Next, FIGS. 28 and 29 are flowcharts for explaining the operation of the ninth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST2c will be described.
[0221]
In step ST2c, the gate center is fixed for the initial sampling.
[0222]
With the above steps, the position component VZ of the prediction vector that is the center of the gate is the number of samplings at the initial tracking. _pk By continuing to use the fixed first sampling detection data instead of (t), variations at the beginning of tracking can be reduced.
[0223]
Embodiment 10 FIG.
A tracking device according to Embodiment 10 of the present invention will be described with reference to the drawings. FIG. 30 is a block diagram showing the configuration of the tracking device according to the embodiment of the present invention. In FIG. 30, reference numeral 18 denotes gate size parameter variable means based on the target speed. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0224]
The gate size parameter variable means 18 based on the target speed calculates the magnitude of the velocity component of the smooth vector from the smooth vector before one sampling obtained from the first data update means 6. When the velocity component of the smooth vector before one sampling is larger than a certain threshold value, the size of the gate size parameter at the current time in the gate determination unit 3 in the range direction and the gate determination unit 4 in the first cross range direction A control signal for increasing the length is input to the gate determining unit 3 in the range direction and the gate determining unit 4 in the first cross range direction. The gate determining unit 3 in the range direction and the gate determining unit 4 in the first cross-range direction use a control signal that increases the size of the gate size parameter input from the gate size parameter varying unit 18 according to the target speed, Increase the size parameter.
[0225]
Therefore, in the gate size parameter varying means 18 based on the target speed, when the magnitude of the speed component of the smooth vector before one sampling is larger than a certain threshold, the current time in the first cross range direction gate determination means 4 The target signal can be easily supplemented by increasing the size of the gate size parameter and the size of the gate size parameter at the current time in the gate determination means 3 in the range direction.
[0226]
Next, FIGS. 31 and 32 are flowcharts for explaining the operation of the tenth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST9c will be described.
[0227]
In step ST9c, if the magnitude of the velocity component of the smooth vector before one sampling is larger than a certain threshold value, the gate size parameter at the current time in the range direction, the cross range horizontal direction, and the cross range vertical direction is set large.
[0228]
By the above steps, the target signal can be easily supplemented by increasing the size of the gate size parameter at the current time when the magnitude of the velocity component of the smooth vector before one sampling is larger than a certain threshold value.
[0229]
Embodiment 11 FIG.
A tracking device according to Embodiment 11 of the present invention will be described with reference to the drawings. FIG. 33 is a block diagram showing the structure of the tracking device according to Embodiment 11 of the present invention. In FIG. 33, reference numeral 19 denotes first display smoothing means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0230]
In the first display smoothing means 19, the smoothing vector VX input from the first data updating means 6 _sk (T) is converted into a smoothing vector VX for display using the following equation (42). _sk (T *) is obtained, and the display smoothing vector VX _sk (T *) is input to the display means 9. Further, the display smooth vector is stored for one sampling.
[0231]
[Expression 42]

[0232]
In the formula (42), VX _sk (T) is a smooth vector VX input from the first data updating means 6 at time k, VX _sk (T *) is a smoothing vector for display at time k, and ηk is a coefficient that determines a smoothing weight.
[0233]
ηk is calculated by the following equation (43).
[0234]
[Equation 43]

[0235]
In the right side of the equation (43), Δt is a sampling interval, and τ is a time constant.
[0236]
The larger the time constant, the larger the delay of the display smooth vector than the target detection data at the current time, but the greater the smoothing effect, that is, it looks smooth.
[0237]
Therefore, in the first display smoothing means 19, the time constant of the equation (43) is determined in advance, and the smoothing vector VX input from the first data updating means 6 is determined. _sk From (t), using the expression (42), the display smooth vector VX _sk By obtaining (t *), the display to the operator is easy to see.
[0238]
Next, FIGS. 34 and 35 are flowcharts for explaining the operation of the eleventh embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST16b will be described.
[0239]
In step ST16b, a display smooth vector is calculated from the smooth vector, and the smooth position smoothing speed is displayed.
[0240]
If the smoothed vector obtained from the scattered detection data is displayed, the display track will become unstable, but the display smooth vector is calculated by the above steps and the display track is displayed from the display smooth vector. As a result, the display track for the operator is smooth and easy to see.
[0241]
Embodiment 12 FIG.
A tracking apparatus according to Embodiment 12 of the present invention will be described with reference to the drawings. FIG. 36 is a block diagram showing the configuration of the tracking device according to the embodiment of the present invention. In FIG. 36, reference numeral 20 denotes a second display smoothing means. In addition, the same code | symbol in each figure represents the same or an equivalent part.
[0242]
In the second display smoothing means 20, the smoothing vector VX input from the first data updating means 6 is used. _sk (T) is expressed using the following equation (44), and the display smoothing vector VX _sk (T **) is obtained and the smoothing vector VX for display _sk (T **) is input to the display means 9. Further, the display smooth vector is stored for one sampling.
[0243]
(44)

[0244]
In equation (44), VX _sk (T) is a smooth vector VX input from the first data updating means 6 at time k, VX _sk (T **) is the display smooth vector at time k, θ _k Is a coefficient that determines the smoothing weight.
[0245]
θ _k Is calculated by equation (45).
[0246]
[Equation 45]

[0247]
In the right side of equation (45), Δt is the sampling interval, ρ _k Is a time constant that varies depending on the sampling time.
[0248]
ρ _k Is calculated by the following equation (46).
[0249]
[Equation 46]

[0250]
In (46), ρ _k Is a time constant that varies according to the sampling time, φ _k Is a coefficient that determines the smoothing weight. Ρ _inf Is ρ _k Is a fixed value in the steady state. ρ _k Is set in advance. Φ in equation (46) _k Is determined by the following equation (47).
[0251]
[Equation 47]

[0252]
In the right side of equation (45) in equation (47), Δt is a sampling interval, and τc is a time constant ρ that varies depending on the sampling time. _k Is the time constant of the time constant that controls
[0253]
By using the equations (44), (45), (46), and (47), the smooth vector shakiness is larger than the delay of the target detection data at the current time in the initial state. The time constant θk of the display smooth vector can be set large, and the display wake for the operator can be shown smoothly. Also, when the initial state is over and the steady state is approached, the smooth vector shakiness decreases, so that the time constant θ is reduced in order to reduce the delay from the detection data. _k Can be set small. Therefore, the display to the operator is easy to see.
[0254]
Next, FIGS. 37 and 38 are flowcharts for explaining the operation of the twelfth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST16c will be described.
[0255]
In step ST16c, a smoothing vector for display is calculated from the smoothing vector with a variable time constant, and the smoothed position smoothing speed is displayed.
[0256]
By the above steps, the display smooth vector is calculated by calculating the display smooth vector using the equations (44), (45), (46), and (47), while varying the time constant. It is possible to improve the delay of the smooth vector before obtaining and the smoothness of the smooth vector.
[0257]
Embodiment 13 FIG.
A tracking apparatus according to Embodiment 13 of the present invention will be described with reference to the drawings. FIG. 39 is a block diagram showing the configuration of the tracking device according to the embodiment of the present invention. In FIG. 39, 21 is a multipath determination means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0258]
Multipath is reflected radio waves that scatter when radio waves emitted from a radar hit the ground, sea surface, and obstacles.
[0259]
When the S / N ratio is larger than a certain threshold obtained from the observation unit 1, the multipath determination unit 21 sets the elevation noise observation noise standard deviation in the equation (8) to a fixed value that gives in advance, and the equation (13) A control signal for calculating the residual covariance matrix using is input to the first gate determination specification calculation means 2. In addition, when the S / N ratio is larger than a certain threshold obtained from the observation unit 1, the multipath determination unit 21 determines that the gate size parameter in the cross range longitudinal direction in the gate determination unit 4 in the first cross range direction is the gate A control signal is input to the first cross range direction gate determination means 4 so that the determination result is set to a sufficiently large value so that the determination result is always within the gate.
[0260]
Accordingly, the multipath determination means 21 does not use the observation noise standard deviation of the elevation angle that is most affected by the multipath in a multipath environment where the accuracy in the altitude direction is not good, thereby tracking performance in the multipath environment. Can be improved.
[0261]
Next, FIGS. 40 and 41 are flowcharts for explaining the operation of the thirteenth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST2d and step ST9d will be described.
[0262]
In step ST2d, the elevation observation noise due to the S / N threshold is fixed, that is, after setting the observation noise standard deviation of the elevation angle in equation (8) to a fixed value that gives in advance, the residual covariance matrix is (13 ).
[0263]
If it is determined in step ST9d that the multipath is used, the gate size parameter setting assuming multipath, that is, the cross range vertical direction gate in which all detection data is determined to be in the gate in the cross range vertical direction gate determination. Set the size parameter.
[0264]
The above steps improve tracking performance in a multipath environment by not using the observation noise standard deviation of the elevation angle that is most affected by the multipath in a multipath environment where accuracy in the altitude direction is not good. Can do.
[0265]
Embodiment 14 FIG.
A tracking device according to Embodiment 14 of the present invention will be described with reference to the drawings. FIG. 42 is a block diagram showing the structure of the tracking device according to Embodiment 14 of the present invention. In FIG. 42, 22 is a condition number determination means. In addition, the same code | symbol in a figure represents the same or an equivalent part.
[0266]
Of the eigenvalues input from the first gate determination specification calculation means 2, the ratio of the maximum eigenvalue λkmax to the minimum eigenvalue λkmin is taken, and the condition number COND in the following equation (48) _k Is calculated.
[0267]
[Formula 48]

[0268]
COND _k If the value is small, digits are lost in the matrix operation, causing problems such as inability to perform the inverse matrix operation, and the tracking operation is unstable.
[0269]
In the condition number determination means 22, the condition number COND is given as in the following equation (49). _k A certain threshold value COND set in advance _th In the following cases, the operator inputs a danger signal that the tracking calculation is unstable to the display means 9 and outputs it to the operator's display screen.
[0270]
[Equation 49]

[0271]
Therefore, by using the condition number determination means 22, it is possible for the operator to detect a danger signal that the tracking calculation is unstable. For example, it is possible to perform the tracking calculation again or to cancel the tracking calculation.
[0272]
Next, FIGS. 43 and 44 are flowcharts for explaining the operation of the fourteenth embodiment. Here, steps with the same reference numerals are omitted. That is, only step ST16d will be described.
[0273]
In step ST16d, the condition number is calculated from the equation (48), and when the equation (49) is established, a danger signal that the tracking calculation is unstable is input to the display means 9 to the operator, and the operator's display Output to the screen.
[0274]
Through the above steps, the operator can detect a danger signal that the tracking calculation is unstable.
[0275]
【The invention's effect】
The present invention obtains an S / N ratio, which is a signal-to-noise ratio, based on received power, converts it into distance, elevation angle, and azimuth angle observation noise standard deviations, and associates the observation noise standard deviations with them. An observation means for outputting detection data representing the position to be detected, a prediction error covariance matrix and the detection data are inputted, and from the residual covariance matrix calculated from them, the eigenvalue in the distance direction, the eigenvalue in the elevation direction, the azimuth Calculate eigenvalues in the angular direction, eigenvectors in the distance direction, eigenvectors in the elevation direction, and eigenvectors in the azimuth direction Perform diagonalization First gate determination specification calculation means and the detection data are in the distance direction In Of a given dimension Ellipsoid A determination is made as to whether or not the gate is entered and a signal indicating the result of the determination is output using the range direction gate determination means, and the data output from the observation means and the first gate determination specification calculation means. In the elevation direction and azimuth direction, Ellipsoid Cross range direction gate determination means for performing gate determination, and the distance direction so Of the predetermined dimension Ellipsoid A signal indicating the result of determining whether or not the gate is entered, and the elevation direction Direction and direction Azimuth direction so The predetermined dimension of Ellipsoid From the signal indicating the determination result of whether it is in the gate, distance direction, elevation angle direction and Azimuth all In The predetermined dimension Ellipsoid A first gate determination means for outputting detection data and a residual covariance matrix associated with the detection data as candidates for a target signal when the detection data is contained in the gate; and the first gate determination First detection means for inputting the detection data and the residual covariance matrix outputted from the means, updating the data based on a Kalman filter theory, and outputting a smooth error covariance matrix and a smooth vector; Based on the theory of the Kalman filter, a prediction means for calculating a prediction error covariance matrix and a prediction vector after one sampling from the current time, and a prediction error covariance matrix and a prediction vector input from the prediction means are delayed by one sampling. A one sampling delay for outputting the prediction error covariance matrix to the first gate determination specification calculation means. Means, from the smoothing vector received as input from the first data updating means, and display means for displaying the target track and the target speed comprises, of predetermined dimensions Ellipsoid The gate is an ellipsoidal gate of a predetermined dimension based on the residual covariance matrix, and the first gate determination specification calculating means is configured to obtain a diagonal of the residual covariance matrix in the ellipsoidal gate. Range direction and cross-range direction Decision Then, the range direction gate determination means and the cross range direction gate determination means are based on the range direction and the cross range direction calculated by the first gate determination specification calculation means. The gate size parameter in the cross range direction Setting By doing so, since the tracking device determines the size of the ellipsoidal gate, the target signal can easily enter and it is possible to make it difficult for an unnecessary signal to enter.
[0276]
The cross range direction gate determination means determines whether or not the detection data is in a one-dimensional gate in the elevation angle direction, outputs a signal indicating the determination result, and the detection data is 1 in the azimuth direction. Since it is composed of the first cross range direction gate determination means for determining whether or not it is in the dimension gate and outputting a signal indicating the determination result, it is easy to perform the cross range direction gate determination. Can do.
[0277]
The cross range direction gate determination means performs two-dimensional gate determination in the cross range direction using the eigenvector in the elevation angle direction, the eigenvalue in the elevation angle direction, the eigenvector in the azimuth direction, and the eigenvalue in the azimuth direction. Because it consists of two cross range direction gate judgment means, the gate judgment in the range direction and cross range direction can be performed independently, so even when the range direction gate is enlarged, the cross range direction is also matched Can be prevented. Also, the gate size parameter can be set once.
[0278]
Also, The output from the first data update means If the smooth vector velocity component exceeds a certain threshold, set the standard deviation of the drive noise vector to a large value. And output to the prediction means Since the first drive noise control means is further provided, the target can be easily supplemented by increasing the standard deviation and widening the gate.
[0279]
A logistic curve with the standard deviation of the drive noise vector as the vertical axis and the velocity component of the smooth vector as the horizontal axis. On the line Based on the velocity component of the smooth vector According to the size of Set a large standard deviation of the driving noise vector To switch Since the second drive noise control means is further provided, the standard deviation of the drive noise vector is gradually increased and the gate is expanded, so that the target can be easily supplemented.
[0280]
In addition, the present invention obtains an S / N ratio that is a signal-to-noise ratio based on the received power and converts it into observation noise standard deviations of distance, elevation angle, and azimuth, and the observation noise standard deviations and those An observation means for outputting detection data representing a position associated with the position, a prediction error covariance matrix and the detection data are input, and from the residual covariance matrix calculated from them, eigenvalues in the distance direction and eigenvalues in the elevation direction Calculate eigenvalues in azimuth direction, eigenvectors in distance direction, eigenvectors in elevation direction, and eigenvectors in azimuth direction Perform diagonalization A first gate determination specification calculation means and the detection data in the distance direction; Ellipsoid It is determined whether or not it is in the gate, and a gate determination means in the range direction that outputs a signal indicating the determination result, and the detection data is in the elevation direction Ellipsoid It determines whether or not it is in the gate, outputs a signal indicating the determination result, and the detection data is in the azimuth direction Ellipsoid First cross range direction gate determination means for determining whether or not the gate is entered and outputting a signal indicating the determination result; the range direction gate determination means; and the first cross range direction gate. As a result of the gate determination of the detection data in the determination means, for the detection data in the gate, the distance between the gate center and the detection data is converted into the likelihood of the normal distribution, the data is updated using the likelihood, smoothing error A second data updating unit that outputs a covariance matrix and a smooth vector, a prediction unit that calculates a prediction error covariance matrix and a prediction vector after one sampling from the current time based on the theory of the Kalman filter, and the prediction unit The input prediction error covariance matrix and the prediction vector are delayed by one sampling, and the first gate determination specification calculation means And outputs, includes a 1 sample delay means, a smoothing vector inputted from said second data updating means, and display means for displaying the target track and the target speed, and The ellipsoidal gate in the distance direction, the ellipsoidal gate in the elevation angle direction, and the ellipsoidal gate in the azimuth direction are combined to form a predetermined dimension ellipsoidal gate, The predetermined dimension Ellipsoid The gate is an ellipsoidal gate of a predetermined dimension based on the residual covariance matrix, and the first gate determination specification calculating means is configured to obtain a diagonal of the residual covariance matrix in the ellipsoidal gate. Range direction and cross-range direction Decision And the range direction gate determination means and the range direction First The gate determination means for the cross range direction is configured to determine the gate size parameter for the range direction and the cross range direction based on the range direction and the cross range direction calculated by the first gate determination specification calculation means. Setting Since the tracking device determines the size of the ellipsoidal gate, the next sampling is performed by calculating a smooth vector and a smooth error covariance matrix in consideration of the distance between the gate centers based on the likelihood of the detection data. The center of the subsequent gate can be stably tracked in the unnecessary signal without being extremely pulled by the unnecessary signal at the current time.
[0281]
In addition, since it further includes a first gate size parameter setting means for setting the gate size parameter according to the sampling interval, when the sampling interval is larger than a predetermined threshold, the gate size parameter is set to be small, By keeping an appropriate gate radius, the target signal is prevented from being captured and unwanted signals are prevented from entering.
[0282]
In addition, there is an observation error fixing means that sets the observation noise standard deviation for distance, the observation noise standard deviation for elevation angle, and the observation noise standard deviation for azimuth angle to fixed values. Thus, it is not necessary to calculate the observation error covariance matrix in polar coordinates for each sampling time and each detection data, so that the calculation load can be reduced.
[0283]
In addition, a control signal that fixes the sampling interval is input to the prediction means, and further equipped with a sampling interval fixing means that fixes the sampling interval when calculating the prediction error covariance matrix and prediction vector, the state transition matrix is fixed. By treating it as a value, it is possible to reduce the calculation load of the prediction vector and the prediction error covariance matrix.
[0284]
In addition, it is equipped with a gate center fixing means that fixes the center of the gate for several samplings from the beginning of tracking, so it is fixed instead of the position component of the prediction vector, which is the center of the gate, for several samplings at the beginning of tracking. By continuing to use the detection data of the first sampling, variation at the beginning of tracking is reduced.
[0285]
Also, the above First Since the gate size parameter variable means for changing the size of the gate size parameter using the magnitude of the velocity component of the smooth vector obtained from the data updating means is further provided, the magnitude of the velocity component of the smooth vector before one sampling is provided. Is larger than a predetermined threshold, the target signal can be easily supplemented by increasing the size of the gate size parameter at the current time.
[0286]
Also, the above First Since the first smoothing means for calculating the display smoothing vector is input by inputting the smoothing vector obtained from the data updating means to the display filter with a fixed time constant, the detection data varies. When the smooth vector obtained from the above is displayed, the display wakes up and down, but if the display smooth vector is calculated and the display wake is displayed from the display smooth vector, the display wake for the operator becomes smooth, It becomes easy to see.
[0287]
Also, the above First Since the smoothing vector obtained from the data updating means is further input to the display filter having a variable time constant, and the second smoothing means for calculating the display smoothing vector is further provided, the time constant is made variable. Thus, by calculating the display smooth vector, it is possible to improve the delay of the smooth vector before obtaining the display smooth vector and the smoothness of the smooth vector.
[0289]
If the condition number that is the ratio of the maximum value of the eigenvalue to the minimum value of the eigenvalue is used among the eigenvalues input from the first gate determination specification calculation means, Since it is further provided with a condition number judgment means for inputting a danger signal that the calculation is unstable to the display means, the operator detects the danger signal that the tracking calculation is unstable. Can do.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of a tracking device according to Embodiment 1 of the present invention.
FIG. 2 is an explanatory diagram showing a coordinate system related to observation means provided in the tracking device.
FIG. 3 is a flowchart showing a process flow of the tracking device according to the first embodiment of the present invention.
FIG. 4 is a flowchart showing a process flow of the tracking device according to the first embodiment of the present invention.
FIG. 5 is a block diagram showing a configuration of a tracking device according to Embodiment 2 of the present invention.
FIG. 6 is a flowchart showing a process flow of the tracking device according to the second embodiment of the present invention.
FIG. 7 is a flowchart showing a process flow of the tracking device according to the second embodiment of the present invention.
FIG. 8 is a block diagram showing a configuration of a tracking device according to Embodiment 3 of the present invention.
FIG. 9 is a flowchart showing a process flow of the tracking device according to the third embodiment of the present invention.
FIG. 10 is a flowchart showing a process flow of the tracking device according to the third embodiment of the present invention.
FIG. 11 is a block diagram showing a configuration of a tracking device according to Embodiment 4 of the present invention.
FIG. 12 is an explanatory diagram showing a conceptual diagram of a logistic curve.
FIG. 13 is a flowchart showing a process flow of the tracking device according to the fourth embodiment of the present invention.
FIG. 14 is a flowchart showing a process flow of the tracking device according to the fourth embodiment of the present invention.
FIG. 15 is a block diagram showing a configuration of a tracking device according to Embodiment 5 of the present invention.
FIG. 16 is a flowchart showing a process flow of the tracking device according to the fifth embodiment of the present invention.
FIG. 17 is a flowchart showing a process flow of the tracking device according to the fifth embodiment of the present invention.
FIG. 18 is a block diagram showing a configuration of a tracking device according to Embodiment 6 of the present invention.
FIG. 19 is a flowchart showing a process flow of the tracking device according to the sixth embodiment of the present invention.
FIG. 20 is a flowchart showing a process flow of the tracking device according to the sixth embodiment of the present invention.
FIG. 21 is a block diagram showing a configuration of a tracking device according to Embodiment 7 of the present invention.
FIG. 22 is a flowchart showing a process flow of the tracking device according to the seventh embodiment of the present invention.
FIG. 23 is a flowchart showing a process flow of the tracking device according to the seventh embodiment of the present invention.
FIG. 24 is a block diagram showing a configuration of a tracking device according to Embodiment 8 of the present invention.
FIG. 25 is a flowchart showing a process flow of the tracking device according to the eighth embodiment of the present invention.
FIG. 26 is a flowchart showing a process flow of the tracking device according to the eighth embodiment of the present invention.
FIG. 27 is a block diagram showing a configuration of a tracking device according to Embodiment 9 of the present invention.
FIG. 28 is a flowchart showing a process flow of the tracking device according to the ninth embodiment of the present invention.
FIG. 29 is a flowchart showing a process flow of the tracking device according to the ninth embodiment of the present invention.
FIG. 30 is a block diagram showing a configuration of a tracking device according to Embodiment 10 of the present invention.
FIG. 31 is a flowchart showing a process flow of the tracking device according to the tenth embodiment of the present invention.
FIG. 32 is a flowchart showing a process flow of the tracking device according to the tenth embodiment of the present invention.
FIG. 33 is a block diagram showing a configuration of a tracking device according to Embodiment 11 of the present invention.
FIG. 34 is a flowchart showing a process flow of the tracking device according to the eleventh embodiment of the present invention.
FIG. 35 is a flowchart showing a process flow of the tracking device according to the eleventh embodiment of the present invention.
FIG. 36 is a block diagram showing a configuration of a tracking apparatus according to Embodiment 12 of the present invention.
FIG. 37 is a flowchart showing a process flow of the tracking device according to the twelfth embodiment of the present invention.
FIG. 38 is a flowchart showing a process flow of the tracking device according to the twelfth embodiment of the present invention.
FIG. 39 is a block diagram showing a configuration of a tracking apparatus according to Embodiment 13 of the present invention.
FIG. 40 is a flowchart showing a process flow of the tracking device according to the thirteenth embodiment of the present invention.
FIG. 41 is a flowchart showing a process flow of the tracking device according to the thirteenth embodiment of the present invention.
FIG. 42 is a block diagram showing a configuration of a tracking apparatus according to Embodiment 14 of the present invention.
FIG. 43 is a flowchart showing a process flow of the tracking device according to the fourteenth embodiment of the present invention.
FIG. 44 is a flowchart showing a process flow of the tracking device according to the fourteenth embodiment of the present invention.
FIG. 45 is a block diagram showing a configuration of a conventional tracking device.
FIG. 46 is an explanatory diagram showing a configuration of a conventional tracking device.
FIG. 47 is a diagram showing the relationship between a gate, a target predicted position, a target observation position, a target range direction, and a target cross range direction.
FIG. 48 is an explanatory diagram showing a relationship between a cross range error and a range error with respect to an angle error.
FIG. 49 is an explanatory diagram showing the relationship between eigenvectors in the range direction and eigenvectors in the cross range direction.
FIG. 50 is a conceptual diagram of likelihood obtained by converting the distance from the gate center.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Observation means, 1st gate determination specification calculation means, 3 range direction gate determination means, 4th cross range direction gate determination means, 5 1st gate determination means, 6 1st data update means , 7 prediction means, 8 1 sampling delay means, 9 display means, 10 second cross range direction gate determination means, 11 first drive noise control means, 12 second drive noise control means, 13 second data Updating means, 14 first gate size parameter setting means, 15 observation error fixing means, 16 sampling interval fixing means, 17 gate center fixing means, 18 gate size parameter variable means according to target speed, 19 first display smoothing means, 20 second display smoothing means, 21 multipath judging means, 22 condition number judging means, 101 observing means, 106 data updating means, 107 Measuring means, 108 1 sampling delay means, 109 display means, 123 gate determination specification calculation means, 124 gate determination means.

Claims (14)

Based on the received power, an S / N ratio, which is a signal-to-noise ratio, is obtained and converted into observation noise standard deviations of distance, elevation angle, and azimuth, and the observation noise standard deviations and their associated positions are represented. Observation means for outputting detection data;
The prediction error covariance matrix and the detection data are input, and the eigenvalue in the distance direction, the eigenvalue in the elevation angle direction, the eigenvalue in the azimuth direction, the eigenvector in the distance direction, and the eigenvector in the elevation angle direction are calculated from them. And first gate determination specification calculation means for performing diagonalization for calculating eigenvectors in the azimuth direction,
Determining whether or not the detection data is in an ellipsoidal gate of a predetermined dimension in the distance direction , and a gate determination means in a range direction for outputting a signal indicating the determination result;
Using the data output from the observation means and the first gate determination specification calculation means, the gate determination means in the cross range direction for performing ellipsoidal gate determination in a predetermined dimension in the elevation angle direction and the azimuth angle direction,
A signal indicative of the whether the determination result is within a predetermined dimension of the ellipsoid gate in the distance direction, whether has entered the predetermined dimension of the ellipsoid gate in elevation Direction and how position angle direction When the detection data is included in the ellipsoid gate of the predetermined dimension in all of the distance direction, the elevation angle direction, and the azimuth angle direction from the signal indicating the determination result, the detection data is attached to the detection data as a target signal candidate. First detection means for outputting detection data and a residual covariance matrix,
The detection data output from the first gate determination means and the residual covariance matrix are input, the data is updated based on the theory of the Kalman filter, and a smooth error covariance matrix and a smooth vector are output. Data update means,
Prediction means for calculating a prediction error covariance matrix and a prediction vector after one sampling from the current time based on the theory of the Kalman filter;
1 sampling delay means for delaying the prediction error covariance matrix and the prediction vector input from the prediction means by 1 sampling, and outputting the prediction error covariance matrix to the first gate determination specification calculation means;
Display means for displaying the target track and the target speed from the smooth vector input from the first data updating means;
With
The predetermined dimension ellipsoidal gate is a predetermined dimension ellipsoidal gate based on the residual covariance matrix,
It said first gate determining specifications calculating means, in the ellipsoid gate, determines the range direction and cross range direction by diagonalization of the residual covariance matrix,
The range direction gate determination means and the cross range direction gate determination means are configured to generate the range direction and the cross range direction based on the range direction and the cross range direction calculated by the first gate determination specification calculation means. A tracking device characterized by determining a size of the ellipsoidal gate by setting a gate size parameter in a range direction. The gate determination means in the cross range direction,
It is determined whether the detection data is in the one-dimensional gate in the elevation angle direction, a signal indicating the determination result is output, and it is determined whether the detection data is in the one-dimensional gate in the azimuth direction. The tracking device according to claim 1, further comprising: a first cross range direction gate determination unit that outputs a signal indicating the determination result. The gate determination means in the cross range direction,
It is composed of second cross range direction gate determination means for performing two-dimensional gate determination in the cross range direction using the eigen vector in the elevation angle direction, the eigen value in the elevation angle direction, the eigen vector in the azimuth angle direction, and the eigen value in the azimuth angle direction. The tracking device according to claim 1, wherein:   The first driving noise control means for setting the standard deviation of the driving noise vector to be large and outputting it to the prediction means when the velocity component of the smooth vector outputted from the first data updating means becomes larger than a certain threshold value. The tracking device according to claim 1, further comprising:   Based on a logistic curve with the vertical axis representing the magnitude of the standard deviation of the driving noise vector and the horizontal axis representing the velocity component of the smooth vector, the standard deviation of the driving noise vector is set to be large according to the magnitude of the velocity component of the smooth vector. The tracking device according to any one of claims 1 to 3, further comprising a second drive noise control means for switching to the above. Based on the received power, an S / N ratio, which is a signal-to-noise ratio, is obtained and converted into observation noise standard deviations of distance, elevation angle, and azimuth, and the observation noise standard deviations and their associated positions are represented. Observation means for outputting detection data;
The prediction error covariance matrix and the detection data are input, and the eigenvalue in the distance direction, the eigenvalue in the elevation angle direction, the eigenvalue in the azimuth direction, the eigenvector in the distance direction, and the eigenvector in the elevation angle direction are calculated from them. And first gate determination specification calculation means for performing diagonalization for calculating eigenvectors in the azimuth direction,
Determining whether or not the detection data is in the ellipsoidal gate in the distance direction, and a gate determination means in the range direction for outputting a signal indicating the determination result;
It is determined whether or not the detection data is in the ellipsoidal gate in the elevation angle direction, a signal indicating the determination result is output, and it is determined whether or not the detection data is in the ellipsoidal gate in the azimuth direction. Performing a first cross range direction gate determination means for outputting a signal indicating the determination result; and
As a result of the gate determination of the detection data in the gate determination unit in the range direction and the gate determination unit in the first cross range direction, the distance between the gate center and the detection data for the detection data in the gate is set to the likelihood of the normal distribution. A second data updating means for converting, performing data updating using the likelihood, and outputting a smoothing error covariance matrix and a smoothing vector;
Prediction means for calculating a prediction error covariance matrix and a prediction vector after one sampling from the current time based on the Kalman filter theory;
1 sampling delay means for delaying the prediction error covariance matrix and the prediction vector input from the prediction means by 1 sampling and outputting to the first gate determination specification calculation means;
Display means for displaying the target track and the target speed from the smooth vector input from the second data updating means;
With
The ellipsoidal gate in the distance direction, the ellipsoidal gate in the elevation angle direction, and the ellipsoidal gate in the azimuth direction are combined to form a predetermined dimension ellipsoidal gate,
The predetermined dimension ellipsoidal gate is a predetermined dimension ellipsoidal gate based on the residual covariance matrix,
It said first gate determining specifications calculating means, in the ellipsoid gate, determines the range direction and cross range direction by diagonalization of the residual covariance matrix,
The range direction gate determination means and the first cross range direction gate determination means are configured to generate the range direction based on the range direction and the cross range direction calculated by the first gate determination specification calculation means. And the size of the ellipsoidal gate is determined by setting a gate size parameter in the cross-range direction.   4. The tracking device according to claim 1, further comprising first gate size parameter setting means for setting a gate size parameter according to a sampling interval.   The observation error fixing means for setting the observation noise standard deviation of the distance, the observation noise standard deviation of the elevation angle, and the observation noise standard deviation of the azimuth angle to fixed values is further provided. The tracking device described.   4. A sampling interval fixing means for inputting a control signal for fixing a sampling interval to the prediction means and fixing a sampling interval for calculating a prediction error covariance matrix and a prediction vector. The tracking device according to any one of the above.   4. The tracking device according to claim 1, further comprising gate center fixing means for fixing the center of the gate for several samplings from the initial stage of tracking.   4. A gate size parameter variable means for changing the size of the gate size parameter using the magnitude of the velocity component of the smooth vector obtained from the first data updating means. The tracking device according to any one of the above.   It further comprises first display smoothing means for inputting a smooth vector obtained from the first data updating means to a display filter having a fixed time constant and calculating a display smooth vector. The tracking device according to any one of claims 1 to 3.   It further comprises second display smoothing means for inputting a smooth vector obtained from the first data updating means to a display filter having a variable time constant and calculating a display smooth vector. The tracking device according to any one of claims 1 to 3.   Of the eigenvalues input from the first gate determination specification calculation means, using the condition number that is the ratio of the maximum eigenvalue to the minimum eigenvalue, and if the condition number is less than a certain threshold, the tracking operation The tracking device according to any one of claims 1 to 3, further comprising a condition number determining means for inputting a danger signal indicating instability to the display means.

Images