JP2003130949A - 目標追尾装置及び目標追尾方法 - Google Patents

Abstract

(57)【要約】 【課題】 運動モデル及び観測モデルを北基準直交座標
系で構成して、直交座標系での目標追尾装置の構成を可
能とした目標追尾装置および方法を提供する。 【解決手段】 前時刻平滑誤差共分散行列と駆動雑音共
分散行列値１１９から現時刻予測誤差共分散行列を算出
する１１７、予測ベクトル１０３から現時刻観測行列を
算出する１２５、１１７と１１５の行列と観測行列から
残差共分散行列を算出する１１１、１０３と１２５から
予測観測ベクトルを算出する１０１、１０１、１０３か
ら残差ベクトルを算出かつ１１１から１０３が追尾目標
の探知データか否かのゲート判定を行う１０７、１１７
と１１１の行列と観測行列からフィルタのゲイン行列を
算出する１１３、残差ベクトル、予測ベクトル、ゲイン
行列から平滑ベクトルを算出する１０９、１１５と１１
７の行列と観測行列から平滑誤差共分散行列を算出する
１２１で成る。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、レーダなどのセ
ンサからなる目標観測装置を用いて航空機や船舶などの
移動体の目標位置を観測値(ベクトルデータ)として取得
し、観測ベクトルから追尾目標の航跡を算出する目標追
尾装置及び目標追尾方法に関わるものである。

【０００２】

【従来の技術】直線状周波数変調方式によるパルス圧縮
を用いたレーダなどのセンサにおける目標追尾方法に関
する従来の技術に示すものとして、「Fitzgerald R.J
著、”'Effects of Range-Doppler Coupling on Chirp
Radar Tracking Accuracy”IEEETransactions on Aeros
pace and Electronic Systems, Vol. AES-10, No.4, Ju
ly 1974」という文献がある。

【０００３】直線状周波数変調方式によるパルス圧縮レ
ーダを用いたレーダなどのセンサでは、センサにより目
標までの距離を観測した場合、距離成分に加え、距離変
化率にある定数(ドップラシフト時定数と呼ぶ)を乗算し
た分だけ加算された観測値が得られる。よって、距離成
分だけを独立に観測することができない。センサと目標
間の距離の真値をＲ、距離変化率を(ドット)Ｒ、ドップ
ラシフト時定数をΔＴ、観測雑音値をｖとすると、観測
値は以下の数式(１)で表される。

【０００４】

【数１】

【０００５】ここで、ΔＴは、直線状周波数変調方式で
決まる定数である。上記文献では、この加算分ΔＴ(ド
ット)Ｒを考慮した数式モデルを用いた追尾方法を提案
している。

【０００６】以下、図面を参照しながら、従来の技術に
ついて詳細に説明する。図１７は従来のこの種の目標追
尾装置のブロック構成図であり、図１８はその処理動作
を示すフローチャート、図１９は従来の技術における動
作を説明するための概念図である。

【０００７】図１７において、目標観測装置２０は空間
中の目標位置を観測するセンサからなる装置、目標追尾
装置１０は目標観測装置２０からの観測データに基づい
て目標追尾処理を実行する装置、航跡表示装置３０は目
標追尾装置１０の処理結果である航跡を運用者に示す装
置である。

【０００８】目標追尾装置１０は、予測観測ベクトル算
出部１０１、予測ベクトル算出部１０３、平滑ベクトル
用メモリ１０５、ゲート判定部１０７、平滑ベクトル算
出部１０９、残差共分散行列算出部１１１、ゲイン行列
算出部１１３、観測雑音共分散行列設定部１１５、予測
誤差共分散行列算出部１１７、駆動雑音共分散行列設定
部１１９、平滑誤差共分散行列算出部１２１、平滑誤差
共分散行列用メモリ１２３を備えている。

【０００９】目標追尾装置１０内において、平滑ベクト
ル用メモリ１０５は、平滑ベクトル算出部１０９による
平滑ベクトルの計算結果を格納する。また、平滑誤差共
分散行列用メモリ１２３は、平滑誤差共分散行列算出部
１２１による平滑誤差共分散行列の計算結果を格納す
る。予測ベクトル算出部１０３は、平滑ベクトル用メモ
リ１０５に格納された前時刻の平滑ベクトルから現時刻
の予測ベクトルを算出する。駆動雑音共分散行列設定部
１１９は、駆動雑音共分散行列を設定する。予測誤差共
分散行列算出部１１７は、平滑誤差共分散行列用メモリ
１２３に格納された前時刻の平滑誤差共分散行列の計算
結果と、駆動雑音共分散行列設定部１１９により設定さ
れた駆動雑音共分散行列の値とから、現時刻の予測誤差
共分散行列を算出する。

【００１０】観測雑音共分散行列設定部１１５は、目標
観測装置２０から入力された観測ベクトルの精度評価に
関連したパラメータに基づいて、観測雑音共分散行列を
設定する。残差共分散行列算出部１１１は、予測誤差共
分散行列算出部１１７により算出された予測誤差共分散
行列と、観測雑音共分散行列設定部１１５により設定さ
れた観測雑音共分散行列とから、残差共分散行列を算出
する。予測観測ベクトル算出部１０１は、予測ベクトル
算出部１０３により算出された予測ベクトルから予測観
測ベクトルを算出する。ゲート判定部１０７は、予測観
測ベクトル算出部１０１により算出された予測観測ベク
トルと、目標観測装置２０からの観測ベクトルとに基づ
いて、残差ベクトルを算出するとともに、残差共分散行
列算出部１１１により算出された残差共分散行列を用い
て、観測ベクトルが追尾目標の探知データであるか否か
のゲート判定を行う。

【００１１】ゲイン行列算出部１１３は、予測誤差共分
散行列算出部１１７により算出された予測誤差共分散行
列と、残差共分散行列算出部１１１により算出された残
差共分散行列とを用いて、フィルタのゲイン行列を算出
する。平滑ベクトル算出部１０９は、ゲート判定部１０
７により出力された観測ベクトルと、予測ベクトル算出
部１０３より算出された予測ベクトルと、予測観測ベク
トル算出部１０１より算出された予測観測ベクトルと、
ゲイン行列算出部１１３により算出されたゲイン行列と
を用いて、平滑ベクトルを算出する。平滑誤差共分散行
列算出部１２１は、予測誤差共分散行列算出部１１７に
より算出された予測誤差共分散行列と、観測雑音共分散
行列設定部１１５により算出された観測雑音共分散行列
とを用いて、平滑誤差共分散行列を算出する。

【００１２】図１８において、初期値設定ステップＳ１
は、平滑ベクトルと平滑誤差共分散行列の初期値を設定
する。観測ベクトルの入力ステップＳ２は、目標観測装
置２０から現時刻の観測ベクトルを読み込む。予測ベク
トルの算出ステップＳ３は、前時刻の平滑ベクトルから
予測ベクトルを算出する。駆動雑音共分散行列の設定ス
テップＳ４は、駆動雑音共分散行列を設定する。

【００１３】予測誤差共分散行列の算出ステップＳ５
は、前時刻の平滑誤差共分散行列と駆動雑音共分散行列
とから予測誤差共分散行列を算出する。予測観測ベクト
ルの算出ステップＳ６は、予測ベクトルから予測観測ベ
クトルを算出する。観測雑音共分散行列の設定ステップ
Ｓ７は、目標観測装置２０から入力された観測ベクトル
の精度評価に関連したパラメータに基づいて観測雑音共
分散行列を設定する。残差共分散行列の算出ステップＳ
８は、予測誤差共分散行列と観測雑音共分散行列とを用
いて残差共分散行列を算出する。

【００１４】ゲート判定ステップＳ９は、予測観測ベク
トルと目標観測装置２０からの観測ベクトルとを用いて
残差ベクトルを算出するとともに、残差共分散行列を用
いて、観測ベクトルが追尾目標の探知データであるか否
かのゲート判定を行う。ゲイン行列の算出ステップＳ１
０は、予測誤差共分散行列と残差共分散行列とを用い
て、フィルタゲイン行列を算出する。平滑ベクトルの算
出ステップＳ１１は、ゲート判定で追尾目標の探知デー
タと判定された観測ベクトル、予測ベクトル、予測観測
ベクトル、及びゲイン行列を用いて、平滑ベクトルを算
出する。平滑誤差共分散行列の算出ステップＳ１２は、
予測誤差共分散行列と観測雑音共分散行列とを用いて、
平滑誤差共分散行列を算出する。追尾終了判定ステップ
Ｓ１３は、追尾処理終了か否かを判定して、追尾処理を
続行する場合には、観測ベクトルの入力ステップＳ１に
戻る。

【００１５】ここで、従来の技術について、カルマンフ
ィルタの理論で仮定される目標及びセンサの数式モデル
について簡単に説明する。カルマンフィルタの理論で
は、目標運動諸元の推定を行うために、目標の運動モデ
ルとセンサの観測モデルを以下のように設定する。ま
ず、目標の運動モデルを以下の数式(２)のように表す。

【００１６】

【数２】

【００１７】数式(２)において、(アンダーバー)ｘ(ｋ)
はサンプリング時刻(以下、単に「時刻」という)ｔ(ｋ)
における目標運動諸元の真値を表すｎ次元状態ベクト
ル、Ф(ｋ−１)は時刻ｔ(ｋ−１)からｔ(ｋ)への状態ベ
クトルの推移を表すｎ行ｎ列の推移行列である。また、
(アンダーバー)ｗ(ｋ)は時刻ｔ(ｋ)におけるｎ次元駆動
雑音ベクトルであり、平均(アンダーバー)０、共分散行
列Ｑ(ｋ)のｎ変量正規分布に従う白色雑音系列である。

【００１８】時刻ｔ(ｋ)における目標運動諸元の真値
(アンダーバー)ｘ(ｋ)を定義する際、座標系が必要とな
る。図２０は北基準直交座標と極座標を示し、図２０で
レーダを原点に配置し、ｘ軸を東(east)、ｙ軸を北(nor
th)、水平面鉛直上方をｚ軸にとった直交座標を北基準
直交座標、また、レーダから目標までの距離をＲ、水平
面より目標までの仰角をＥ、水平面内で北方向より目標
までの方位角をＡzとした座標系を極座標系と呼ぶ。従
来の技術では、目標の基本的な運動を、距離方向につい
て等速運動すると仮定し、極座標系における数式(２)の
運動モデルにおける状態ベクトルと推移行列を、以下の
数式(３)及び数式(４)に示すように設定する。

【００１９】

【数３】

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

【００２１】次に、センサの観測モデルを以下の数式
(５)のように表す。

【００２２】

【数４】

【００２３】数式(５)において、(アンダーバー)ｚ(ｋ)
は時刻ｔ(ｋ)におけるｍ次元観測ベクトル、Ｈ(ｋ)はｍ
行ｎ列の観測行列である。また、(アンダーバー)ｖ(ｋ)
は観測ベクトル(アンダーバー)ｚ(ｋ)に対応した時刻ｔ
(ｋ)のｍ次元観測雑音ベクトルで、平均(アンダーバー)
０、共分散行列Ｒ(ｋ)のｍ変数量正規分布にしたがう白
色雑音系列である。但し、観測雑音ベクトル(アンダー
バー)ｖ(ｋ)と駆動雑音ベクトル(アンダーバー)ｗ(ｋ)
は互いに独立であるとする。

【００２４】従来の技術では、直線状周波数変調方式に
よるパルス圧縮を用いたレーダなどのセンサにおいて、
極座標系における目標距離方向の観測結果が得られると
し、観測モデルにおける観測行列と観測ベクトルを以下
の通りに設定する。

【００２５】

【数５】

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

【００２７】初期時刻ｔ(０)より時刻ｔ(ｋ)までの観測
ベクトル(アンダーバー)ｚ(ｋ)の全体を、以下の数式
(８)のように表す。

【００２８】

【数６】

【００２９】周知のように、カルマンフィルタは、上記
運動モデル及び観測モデルに基づき、観測ベクトルの時
系列データを用いて、推定誤差の分散が最小となる意味
で最適な状態ベクトル推定値を算出する逐次アルゴリズ
ムである。

【００３０】以下、図１８を参考にしながら、図１７に
示した従来の目標追尾装置による動作について説明す
る。なお、ここでは、カルマンフィルタの理論に従い、
時刻ｔ(ｋ−１)までの観測結果に基づき時刻ｔ(ｋ)の状
態ベクトル(アンダーバー)ｘ(ｋ)を推定した予測ベクト
ルを(上下バー)ｘ(ｋ)と表し、時刻ｔ(ｋ)までの観測結
果に基づき時刻ｔ(ｋ)の状態ベクトル(アンダーバー)ｘ
(ｋ)を推定した平滑ベクトルを(アンダーバー・ハット)
ｘ(ｋ)と表す。また、予測ベクトルの誤差共分散行列で
ある予測誤差共分散行列を(バー)Ｐ(ｋ)、平滑ベクトル
の誤差共分散行列である平滑誤差共分散行列をＰ(ｋ)と
表す。

【００３１】さらに、予測ベクトル(上下バー)ｘ(ｋ)よ
り得られる時刻ｔ(ｋ)の観測ベクトル(アンダーバー)ｚ
(ｋ)を推定した予測観測ベクトルを(上下バー)ｚ(ｋ)、
センサから得られた観測ベクトル(アンダーバー)ｚ(ｋ)
と上記予測観測ベクトル(上下バー)ｚ(ｋ)との誤差を表
す残差ベクトルを(アンダーバー)ｒ(ｋ)、残差ベクトル
の共分散行列である残差共分散行列をＳ(ｋ)と表す。こ
こで、予測誤差共分散行列及び平滑誤差共分散行列は、
以下の数式(９)及び数式(１０)で定義される。

【００３２】

【数７】

【００３３】数式(９)及び数式(１０)において、Ｅ[・|
Ｚ(ｋ)]は、時刻ｔ(ｋ)までの観測ベクトルを得たもと
での条件付き期待値を表す記号である。

【００３４】まず、平滑ベクトル用メモリ１０５と平滑
誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベ
クトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤
差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開
始する(初期値の設定ステップＳ１)。

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

【００３６】

【数８】

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

【００３８】

【数９】

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

【００４０】

【数１０】

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

【００４２】

【数１１】

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

【００４４】

【数１２】

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

【００４６】次に、ゲイン行列算出部１１３は、予測誤
差共分散行列(バー)Ｐ(ｋ)と残差共分散行列Ｓ(ｋ)とを
読み込み、以下の数式(１７)にしたがって、フィルタゲ
インを算出する(ゲイン行列の算出ステップＳ１０)。

【００４７】

【数１３】

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

【００４９】

【数１４】

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

【００５１】

【数１５】

【００５２】上記数式(１８)または数式(１９)により、
平滑ベクトル算出部１０９で算出された平滑ベクトル
(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモ
リ１０５に格納されるとともに、航跡表示装置３０に伝
送され、運用者に対して表示される。

【００５３】一方、平滑誤差共分散行列算出部１２１
は、ゲート判定部１０７で追尾目標の観測ベクトルが得
られたと判定された場合には、予測誤差共分散行列(バ
ー)Ｐ(ｋ)と観測雑音共分散行列Ｒ(ｋ)とを用いて、以
下の数式(２０)により平滑誤差共分散行列を算出し、ゲ
ート判定部１０７で追尾目標の観測ベクトルが得られな
かったと判定された場合には、以下の数式(２１)に示す
ように、予測誤差共分散行列(バー)Ｐ(ｋ)を平滑誤差共
分散行列とする(平滑誤差共分散行列の算出ステップＳ
１２)。

【００５４】

【数１６】

【００５５】数式(２０)及び数式(２１)に従う平滑誤差
共分散行列の算出結果は、平滑誤差共分散行列用メモリ
１２３に格納される。以上で時刻ｔ(ｋ)における処理が
終了する。以降、追尾終了が判定されるまで、上記一連
の処理ステップを繰り返し実行する(追尾終了ステップ
１３)。

【００５６】このように、カルマンフィルタを用い、極
座標系でドップラシフト項ΔＴ(ドット)Ｒ(ｋ)を観測モ
デルに記述した従来の目標追尾装置１０は、事前に仮定
された等距離変化率運動モデルを用い、追尾航跡を算出
する。図１９に、従来の技術を用いた平滑ベクトルの距
離成分(平滑値)、予測観測ベクトル(ゲート中心)、およ
び観測ベクトル(観測値)の時間推移を示す。図の実線
は、状態ベクトルの距離成分の真の軌跡を表し、点線
は、ドップラシフトの影響を受けた観測ベクトルの真値
を示す。図のように、予測観測ベクトルは、ドップラシ
フトを考慮した観測行列を用いることにより、予測ベク
トルをドップラシフト分だけシフトした値となり、ドッ
プラシフトの影響を受けた観測値の近くに算出される。
平滑ベクトルの距離成分は状態ベクトルの真の軌跡付近
に算出される。

【００５７】以上のように、従来の技術では、目標が距
離方向のみに運動すると仮定し追尾を行う場合には有効
である。しかしながら、目標の運動モデルが距離方向の
１次元空間のみでなく、３次元空間に及ぶ場合、距離方
向のみで仮定された運動モデル及び観測モデルでは、フ
ィルタを構成することが困難である。特に、目標の運動
が北基準直交座標系で定義される場合、従来の運動およ
び観測モデルを用いることはできない。

【００５８】さらに、上記従来の技術のドップラシフト
を考慮したフィルタを追尾開始時から用いると、距離変
化率の推定値が揺らぐため、距離変化率推定値の劣化が
生じる。フィルタは、本質的に、距離変化率の推定値を
用いて、観測値に含まれているドップラシフト分を補正
し、距離推定値を算出する。よって、距離変化率推定精
度の劣化が距離推定精度の劣化につながる。上記の理由
から、距離推定精度の劣化を防ぐため、追尾初期は、ド
ップラシフトを考慮しないフィルタを用い、距離変化率
推定精度が安定後、フィルタの切り替えを行うことが考
えられる。図２１に、距離平滑値、予測観測値、および
観測値の、切り替え前と切り替え後の推移に関する概念
図を示す。ドップラシフトを考慮しないフィルタとは、
観測行列を、数式(７)の代わりに、以下の数式(２２)を
用いて構成されたカルマンフィルタである。

【００５９】

【数１７】

【００６０】この場合、ドップラシフト分の補正を行わ
ないため、ドップラシフト分の補正による、フィルタ開
始時の距離変化率精度劣化の距離精度に対する影響がな
い。ここで、問題となるのは、矢印Ａで示すように切り
替え時のゲート中心が観測値と離れた所に算出され、観
測値がゲートに収まらない可能性があることである。こ
の図に示すように、まず、ドップラシフトの影響を受け
た観測値を入力としてドップラシフトを考慮しないフィ
ルタを用いると、フィルタの平滑位置が目標の真の位置
からずれて算出されてしまう。次に、フィルタの切り替
え直後にその平滑値を用いて予測観測値を算出すると、
ドップラシフト分だけずれた位置に予測観測値(ゲート
中心)が算出されてしまい、結果的に予測観測値と観測
値の距離が長くなってしまう。距離がながくなると、ゲ
ート判定の際に、観測値をゲートに捕捉できない危険性
が生じる。

【００６１】

【発明が解決しようとする課題】従来の目標追尾装置は
以上のように、目標の運動モデルが極座標系の距離方向
で記述され、観測モデルが極座標系の距離方向で構成さ
れる場合、追尾装置を構成することが可能であるが、目
標の運動が直交座標系で定義され、観測モデルもそうで
ある場合、従来の運動及び観測モデルでは正確な目標追
尾が困難であった。

【００６２】さらに、ドップラシフトを考慮しないフィ
ルタからドップラシフトを考慮したフィルタへの切り替
え時に、予測観測値(ゲート中心)と観測値の距離がなが
くなり、観測値がゲート内に捕捉できない可能性があっ
た。

【００６３】本発明は、上記の問題点を解決するために
なされたもので、運動モデル及び観測モデルを北基準直
交座標系で構成することにより、直交座標系での目標追
尾装置の構成を可能とし、さらに、ドップラシフトを考
慮しない観測モデルを用いたフィルタからドップラシフ
ト補正を考慮した観測モデルを用いたフィルタの切り替
え時に、観測値とゲート中心の距離が増大する問題を解
消した目標追尾装置および目標追尾方法を提供すること
を目的とする。

【００６４】

【課題を解決するための手段】上記の目的に鑑み、この
発明は、目標観測装置から入力される観測ベクトルに基
づいて追尾目標の航跡を算出するための目標追尾装置で
あって、平滑ベクトルの計算結果を格納するための平滑
ベクトル用メモリと、平滑誤差共分散行列の計算結果を
格納するための平滑誤差共分散行列用メモリと、前時刻
の平滑ベクトルを用いて現時刻の予測ベクトルを算出す
る予測ベクトル算出部と、駆動雑音共分散行列を設定す
る駆動雑音共分散行列設定部と、前時刻の平滑誤差共分
散行列の計算結果と前記駆動雑音共分散行列の値とを用
いて現時刻の予測誤差共分散行列を算出する予測誤差共
分散行列算出部と、前記観測ベクトルの精度評価に関連
したパラメータを用いて観測雑音共分散行列を設定する
観測雑音共分散行列設定部と、前記予測ベクトルを用い
て現時刻の観測行列を算出する観測行列算出部と、前記
予測誤差共分散行列、前記観測雑音共分散行列、及び前
記観測行列を用いて残差共分散行列を算出する残差共分
散行列算出部と、前記予測ベクトル及び前記観測行列か
ら予測観測ベクトルを算出する予測観測ベクトル算出部
と、前記予測観測ベクトル及び前記観測ベクトルを用い
て残差ベクトルを算出するとともに、前記残差共分散行
列を用いて、前記観測ベクトルが前記追尾目標の探知デ
ータであるか否かのゲート判定を行うゲート判定部と、
前記予測誤差共分散行列、前記残差共分散行列、及び前
記観測行列をもちいてフィルタのゲイン行列を算出する
ゲイン行列算出部と、前記残差ベクトル、前記予測ベク
トルおよび前記ゲイン行列をもちいて前記平滑ベクトル
を算出する平滑ベクトル算出部と、前記観測雑音共分散
行列、前記予測誤差共分散行列、及び前記観測行列とを
用いて前記平滑誤差共分散行列を算出する平滑誤差共分
散行列算出部と、を備えたことを特徴とする目標追尾装
置にある。

【００６５】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、ドップラシフトを考慮しない観測モデルにより構成
されたフィルタからドップラシフトを考慮した観測モデ
ルにより構成されたフィルタへの切り替えタイミングを
出力する切り替え制御部と、切り替え制御部よりフィル
タの切り替え時刻情報を得、現時刻が切り替えを行う時
刻の場合、前記平滑ベクトル用メモリからの１時刻前に
おける平滑ベクトルを用いて、擬似平滑値を算出し、切
り替えを行う時刻でない場合、１時刻前の平滑値をその
まま出力する切り替え直前の擬似平滑値算出部と、前時
刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出
する予測ベクトル算出部と、駆動雑音共分散行列を設定
する駆動雑音共分散行列設定部と、前時刻の平滑誤差共
分散行列の計算結果と前記駆動雑音共分散行列の値とを
用いて現時刻の予測誤差共分散行列を算出する予測誤差
共分散行列算出部と、前記観測ベクトルの精度評価に関
連したパラメータを用いて観測雑音共分散行列を設定す
る観測雑音共分散行列設定部と、前記切り替え制御部か
らのフィルタの切り替え時刻情報を入力とし、ドップラ
シフトを考慮した観測モデルにより構成されたフィルタ
に切り替わる時刻以後は、前記予測ベクトル算出部より
算出された予測値を用いて、ドップラシフトを考慮した
現時刻の観測行列を算出し、切り替わる時刻より前は、
ドップラシフトを考慮しない現時刻の観測モデルを算出
する観測行列算出部と、前記予測誤差共分散行列、前記
観測雑音共分散行列、及び前記観測行列を用いて残差共
分散行列を算出する残差共分散行列算出部と、前記予測
ベクトル及び前記観測行列を用いて、ゲート判定部で用
いる予測観測ベクトルを算出するゲート用予測観測ベク
トル算出部と、前記予測観測ベクトル及び前記観測ベク
トルを用いて残差ベクトルを算出するとともに、前記残
差共分散行列を用いて、前記観測ベクトルが前記追尾目
標の探知データであるか否かのゲート判定を行うゲート
判定部と、前記平滑ベクトル用メモリからの１時刻前に
おける平滑ベクトル及び観測行列算出部からの入力であ
る観測行列より、平滑ベクトル算出部で用いる予測観測
ベクトルを算出する平滑ベクトル算出用予測観測ベクト
ル算出部と、前記予測誤差共分散行列、前記残差共分散
行列、及び前記観測行列をもちいてフィルタのゲイン行
列を算出するゲイン行列算出部と、前記残差ベクトル、
前記予測ベクトルおよび前記ゲイン行列をもちいて前記
平滑ベクトルを算出する前記平滑ベクトル算出部と、前
記観測雑音共分散行列、前記予測誤差共分散行列、及び
前記観測行列とを用いて前記平滑誤差共分散行列を算出
する平滑誤差共分散行列算出部と、を備えたことを特徴
とする目標追尾装置にある。

【００６６】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、前時刻の平滑ベクトルを用いて現時刻の予測ベクト
ルを算出する予測ベクトル算出部と、駆動雑音共分散行
列を設定する駆動雑音共分散行列設定部と、前時刻の平
滑誤差共分散行列の計算結果と前記駆動雑音共分散行列
の値とを用いて現時刻の予測誤差共分散行列を算出する
予測誤差共分散行列算出部と、前記観測ベクトルの精度
評価に関連したパラメータを用いて観測雑音共分散行列
を設定する観測雑音共分散行列設定部と、ドップラシフ
トを考慮しない観測モデルにより構成されたフィルタか
らドップラシフトを考慮した観測モデルにより構成され
たフィルタへの切り替えタイミングを出力する切り替え
制御部と、この切り替え制御部からのフィルタの切り替
え時刻情報を入力とし、ドップラシフトを考慮した観測
モデルにより構成されたフィルタに切り替わる時刻より
後は、前記予測値を用いて、ドップラシフトを考慮した
観測行列を算出し、後記ゲート用予測観測ベクトル算出
部、後記残差共分散行列算出部、後記平滑ベクトル算出
用予測観測ベクトル算出部、後記平滑誤差共分散行列算
出部、後記行列算出部、に出力し、切り替わる時刻より
前は、ドップラシフトを考慮しない観測モデルを算出
し、後記ゲート用予測観測ベクトル算出部、後記残差共
分散行列算出部、後記平滑ベクトル算出用予測観測ベク
トル算出部、後記平滑誤差共分散行列算出部、後記ゲイ
ン行列算出部、に出力し、さらに、切り替わる時刻に
は、後記ゲート用予測観測ベクトル算出部、後記残差共
分散行列算出部、にドップラシフトを考慮しない観測モ
デルを算出及び出力し、後記平滑ベクトル算出用予測観
測ベクトル算出部、後記平滑誤差共分散行列算出部、後
記ゲイン行列算出部にドップラシフトを考慮した観測行
列を算出及び出力する観測行列算出部と、前記予測誤差
共分散行列、前記観測雑音共分散行列、及び前記観測行
列を用いて残差共分散行列を算出する前記残差共分散行
列算出部と、前記予測ベクトル及び前記観測行列を用い
て、ゲート判定部で用いる予測観測ベクトルを算出する
前記ゲート用予測観測ベクトル算出部と、前記予測観測
ベクトル及び前記観測ベクトルを用いて残差ベクトルを
算出するとともに、前記残差共分散行列を用いて、前記
観測ベクトルが前記追尾目標の探知データであるか否か
のゲート判定を行う前記ゲート判定部と、前記予測ベク
トル及び前記観測行列より、後記平滑ベクトル算出部で
用いる予測観測ベクトルを算出する前記平滑ベクトル算
出用予測観測ベクトル算出部と、前記予測誤差共分散行
列、前記残差共分散行列、及び前記観測行列をもちいて
フィルタのゲイン行列を算出する前記ゲイン行列算出部
と、前記残差ベクトル、前記予測ベクトルおよび前記ゲ
イン行列をもちいて前記平滑ベクトルを算出する前記平
滑ベクトル算出部と、前記観測雑音共分散行列、前記予
測誤差共分散行列、及び前記観測行列とを用いて前記平
滑誤差共分散行列を算出する前記平滑誤差共分散行列算
出部と、を備えたことを特徴とする目標追尾装置にあ
る。

【００６７】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、前時刻の平滑ベクトルを用いて現時刻の予測ベクト
ルを算出する予測ベクトル算出部と、駆動雑音共分散行
列を設定する駆動雑音共分散行列設定部と、前時刻の平
滑誤差共分散行列の計算結果と前記駆動雑音共分散行列
の値とを用いて現時刻の予測誤差共分散行列を算出する
予測誤差共分散行列算出部と、前記観測ベクトルの精度
評価に関連したパラメータを用いて観測雑音共分散行列
を設定する観測雑音共分散行列設定部と、ドップラシフ
トを考慮しない観測モデルにより構成されたフィルタか
らドップラシフトを考慮した観測モデルにより構成され
たフィルタへの切り替えタイミングを出力する切り替え
制御部と、この切り替え制御部からのフィルタの切り替
え時刻情報を入力とし、ドップラシフトを考慮した観測
モデルにより構成されたフィルタに切り替わる時刻より
後は、前記予測値を用いて、ドップラシフトを考慮した
観測行列を算出し、後記ゲート用予測観測ベクトル算出
部、後記残差共分散行列算出部、後記平滑ベクトル算出
用予測観測ベクトル算出部、後記平滑誤差共分散行列算
出部、後記ゲイン行列算出部、に出力し、切り替わる時
刻より前は、ドップラシフトを考慮しない観測モデルを
算出し、後記ゲート用予測観測ベクトル算出部、後記残
差共分散行列算出部、後記平滑ベクトル算出用予測観測
ベクトル算出部、後記平滑ベクトル算出用予測観測ベク
トル算出部、後記平滑誤差共分散行列算出部、後記ゲイ
ン行列算出部、に出力し、切り替わる時刻には、後記ゲ
ート用予測観測ベクトル算出部、後記残差共分散行列算
出部、にドップラシフトを考慮しない観測モデルを算出
及び出力し、後記ゲイン行列算出部、にドップラシフト
を考慮した観測行列を算出及び出力する観測行列算出部
と、前記予測誤差共分散行列、前記観測雑音共分散行
列、及び前記観測行列を用いて残差共分散行列を算出す
る前記残差共分散行列算出部と、前記予測ベクトル及び
前記観測行列を用いて、ゲート判定部で用いる予測観測
ベクトルを算出する前記ゲート用予測観測ベクトル算出
部と、前記予測観測ベクトル及び前記観測ベクトルを用
いて残差ベクトルを算出するとともに、前記残差共分散
行列を用いて、前記観測ベクトルが前記追尾目標の探知
データであるか否かのゲート判定を行う前記ゲート判定
部と、ドップラシフトを考慮した観測モデルにより構成
されたフィルタに切り替わる時刻より前及び後は、予測
ベクトル算出部からの予測ベクトル及び観測行列算出部
からの入力である観測行列より、平滑ベクトル算出部で
用いる予測観測ベクトルを算出し、ドップラシフトを考
慮した観測モデルにより構成されたフィルタに切り替わ
る時刻では、予測ベクトル算出部からの予測ベクトルよ
り、平滑ベクトル算出部で用いる予測観測ベクトルを算
出する前記平滑ベクトル算出用予測観測ベクトル算出部
と、前記予測誤差共分散行列、前記残差共分散行列、及
び前記観測行列をもちいてフィルタのゲイン行列を算出
する前記ゲイン行列算出部と、前記残差ベクトル、前記
予測ベクトルおよび前記ゲイン行列をもちいて前記平滑
ベクトルを算出する前記平滑ベクトル算出部と、前記観
測雑音共分散行列、前記予測誤差共分散行列、及び前記
観測行列とを用いて前記平滑誤差共分散行列を算出する
前記平滑誤差共分散行列算出部と、を備えたことを特徴
とする目標追尾装置にある。

【００６８】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、ドップラシフトを考慮しない観測モデルにより構成
されたフィルタからドップラシフトを考慮した観測モデ
ルにより構成されたフィルタへの切り替えタイミングを
出力する切り替え制御部と、前時刻の平滑ベクトルを用
いて現時刻の予測ベクトルを算出する予測ベクトル算出
部と、駆動雑音共分散行列を設定する駆動雑音共分散行
列設定部と、前時刻の平滑誤差共分散行列の計算結果と
前記駆動雑音共分散行列の値とを用いて現時刻の予測誤
差共分散行列を算出する予測誤差共分散行列算出部と、
前記観測ベクトルの精度評価に関連したパラメータを用
いて観測雑音共分散行列を設定する観測雑音共分散行列
設定部と、前記予測誤差共分散行列、前記観測雑音共分
散行列、及び前記観測行列を用いて残差共分散行列を算
出する残差共分散行列算出部と、前記切り替え制御部に
よるフィルタの切り替え時刻を入力とし、フィルタに用
いる観測モデルに設定するドップラシフト時定数を算出
し、後記観測行列算出部へ出力するフィルタ用ドップラ
シフト時定数算出部と、前記フィルタ用ドップラシフト
時定数算出部からのドップラシフト時定数を入力とし、
前記予測値を用いて、ドップラシフトを考慮した観測行
列を算出する観測行列算出部と、前記予測ベクトル及び
前記観測行列から予測観測ベクトルを算出する予測観測
ベクトル算出部と、前記予測観測ベクトル及び前記観測
ベクトルを用いて残差ベクトルを算出するとともに、前
記残差共分散行列を用いて、前記観測ベクトルが前記追
尾目標の探知データであるか否かのゲート判定を行うゲ
ート判定部と、前記予測誤差共分散行列、前記残差共分
散行列、及び前記観測行列をもちいてフィルタのゲイン
行列を算出するゲイン行列算出部と、前記残差ベクト
ル、前記予測ベクトルおよび前記ゲイン行列をもちいて
前記平滑ベクトルを算出する平滑ベクトル算出部と、前
記観測雑音共分散行列、前記予測誤差共分散行列、及び
前記観測行列とを用いて前記平滑誤差共分散行列を算出
する平滑誤差共分散行列算出部と、を備えたことを特徴
とする目標追尾装置にある。

【００６９】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾方法であって、平滑ベクトルと平滑誤差共分散行列
の初期値を設定する初期値の設定ステップと、前記目標
観測装置から現時刻の観測ベクトルを読み込む観測ベク
トルの入力ステップと、前時刻の平滑ベクトルから予測
ベクトルを算出する予測ベクトルの算出ステップと、駆
動雑音共分散行列を設定する駆動雑音共分散行列の設定
ステップと、前時刻の平滑誤差共分散行列と前記駆動雑
音共分散行列とから予測誤差共分散行列を算出する予測
誤差共分散行列の算出ステップと、前記予測ベクトルか
ら観測行列を算出する観測行列の算出ステップと、前記
予測ベクトル及び前記観測行列から予測観測ベクトルを
算出する予測観測ベクトルの算出ステップと、前記観測
ベクトルの精度評価に関連したパラメータに基づいて観
測雑音共分散行列を設定する観測雑音共分散行列の設定
ステップと、前記予測誤差共分散行列、前記観測雑音共
分散行列、及び前記観測行列とを用いて残差共分散行列
を算出する残差共分散行列の算出ステップと、前記予測
観測ベクトルと前記観測ベクトルとを用いて残差ベクト
ルを算出するとともに、前記残差共分散行列を用いて、
前記観測ベクトルが追尾目標の探知データであるか否か
のゲート判定を行うゲート判定ステップと、前記予測誤
差共分散行列、前記残差共分散行列、及び前記観測行列
とを用いて、フィルタゲイン行列を算出するゲイン行列
の算出ステップと、前記ゲート判定で追尾目標の探知デ
ータと判定された観測ベクトル、前記予測ベクトル、前
記予測観測ベクトル、及び前記ゲイン行列を用いて、平
滑ベクトルを算出する平滑ベクトルの算出ステップと、
前記予測誤差共分散行列、前記観測雑音共分散行列、及
び前記観測行列とを用いて、前記平滑誤差共分散行列を
算出する平滑誤差共分散行列の算出ステップと、追尾処
理終了か否かを判定して、追尾処理を続行する場合に
は、前記観測ベクトルの入力ステップに戻る追尾終了判
定ステップと、を備えたことを特徴とする目標追尾方法
にある。

【００７０】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾方法であって、平滑ベクトルと平滑誤差共分散行列
の初期値を設定する初期値の設定ステップと、前記目標
観測装置から現時刻の観測ベクトルを読み込む観測ベク
トルの入力ステップと、現時刻が切り替えを行う時刻の
場合、１時刻前における平滑ベクトルを用いて、擬似平
滑値を算出し、現時刻が切り替えを行う時刻でない場
合、１時刻前における後記平滑ベクトルをそのまま出力
とする切り替え直前の擬似平滑値の算出ステップと、前
時刻の後記平滑ベクトルから予測ベクトルを算出する予
測ベクトルの算出ステップと、駆動雑音共分散行列を設
定する駆動雑音共分散行列の設定ステップと、前時刻の
後記平滑誤差共分散行列と前記駆動雑音共分散行列とか
ら予測誤差共分散行列を算出する予測誤差共分散行列の
算出ステップと、ドップラシフトを考慮した観測モデル
により構成されたフィルタに切り替わる時刻以後は、前
記予測値を用いて、観測行列を算出し、切り替わる以前
は、ドップラシフトを考慮しない観測行列を出力する観
測行列算出ステップと、前記予測ベクトルと、前記観測
行列を用いて、ゲート判定に用いる予測観測ベクトルを
算出するゲート用予測観測ベクトルの算出ステップと、
前記観測ベクトルの精度評価に関連したパラメータに基
づいて観測雑音共分散行列を設定する観測雑音共分散行
列の設定ステップと、前記予測誤差共分散行列、前記観
測雑音共分散行列、及び前記観測行列とを用いて残差共
分散行列を算出する残差共分散行列の算出ステップと、
前記予測観測ベクトルと前記観測ベクトルとを用いて残
差ベクトルを算出するとともに、前記残差共分散行列を
用いて、前記観測ベクトルが追尾目標の探知データであ
るか否かのゲート判定を行うゲート判定ステップと、１
時刻前における後記平滑ベクトルと、前記観測行列を用
いて、後記平滑ベクトルの算出に用いる予測観測ベクト
ルを算出する平滑ベクトル算出用予測観測ベクトルの算
出ステップと、前記予測誤差共分散行列、前記残差共分
散行列、及び前記観測行列とを用いて、フィルタゲイン
行列を算出するゲイン行列の算出ステップと、ゲート判
定で追尾目標の探知データと判定された前記観測ベクト
ル、前記予測ベクトル、前記予測観測ベクトル、及び前
記ゲイン行列を用いて、平滑ベクトルを算出する平滑ベ
クトルの算出ステップと、前記予測誤差共分散行列、前
記観測雑音共分散行列、及び前記観測行列とを用いて、
平滑誤差共分散行列を算出する平滑誤差共分散行列の算
出ステップと、追尾処理終了か否かを判定して、追尾処
理を続行する場合には、前記観測ベクトルの入力ステッ
プに戻る追尾終了判定ステップと、を備えたことを特徴
とする目標追尾方法にある。

【００７１】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾方法であって、後記平滑ベクトルと後記平滑誤差共
分散行列の初期値を設定する初期値の設定ステップと、
前記目標観測装置から現時刻の観測ベクトルを読み込む
観測ベクトルの入力ステップと、前時刻の後記平滑ベク
トルから予測ベクトルを算出する予測ベクトルの算出ス
テップと、駆動雑音共分散行列を設定する駆動雑音共分
散行列の設定ステップと、前時刻の後記平滑誤差共分散
行列と前記駆動雑音共分散行列とから予測誤差共分散行
列を算出する予測誤差共分散行列の算出ステップと、フ
ィルタの切り替え時刻情報を入力とし、ドップラシフト
を考慮した観測モデルにより構成されたフィルタに切り
替わる時刻より後は、後記予測値を用いて、ドップラシ
フトを考慮した観測行列を算出し、ゲート用予測観測ベ
クトル算出ステップ、残差共分散行列算出ステップ、平
滑ベクトル算出用予測観測ベクトル算出ステップ、平滑
誤差共分散行列算出ステップ、ゲイン行列算出ステッ
プ、に出力し、切り替わる時刻より前は、ドップラシフ
トを考慮しない観測モデルを算出し、ゲート用予測観測
ベクトル算出ステップ、残差共分散行列算出ステップ、
平滑ベクトル算出用予測観測ベクトル算出ステップ、平
滑誤差共分散行列算出ステップ、ゲイン行列算出ステッ
プ、に出力し、さらに、切り替わる時刻には、ゲート用
予測観測ベクトル算出ステップ、残差共分散行列算出ス
テップ、にドップラシフトを考慮しない観測モデルを算
出及び出力し、平滑ベクトル算出用予測観測ベクトル算
出ステップ、平滑誤差共分散行列算出ステップ、ゲイン
行列算出ステップに、ドップラシフトを考慮した観測行
列を算出及び出力する観測行列算出ステップと、前記予
測ベクトルと、前記観測行列を用いて、ゲート判定に用
いる予測観測ベクトルを算出するゲート用予測観測ベク
トルの算出ステップと、前記観測ベクトルの精度評価に
関連したパラメータに基づいて観測雑音共分散行列を設
定する観測雑音共分散行列の設定ステップと、前記予測
誤差共分散行列、前記観測雑音共分散行列、及び前記観
測行列とを用いて残差共分散行列を算出する残差共分散
行列の算出ステップと、前記予測観測ベクトルと前記観
測ベクトルとを用いて残差ベクトルを算出するととも
に、前記残差共分散行列を用いて、前記観測ベクトルが
追尾目標の探知データであるか否かのゲート判定を行う
ゲート判定ステップと、前記予測ベクトルと、前記観測
行列を用いて、後記平滑ベクトルの算出に用いる予測観
測ベクトルを算出する平滑ベクトル算出用予測観測ベク
トルの算出ステップと、前記予測誤差共分散行列、前記
残差共分散行列、及び前記観測行列とを用いて、フィル
タゲイン行列を算出するゲイン行列の算出ステップと、
ゲート判定で追尾目標の探知データと判定された前記観
測ベクトル、前記予測ベクトル、前記予測観測ベクト
ル、及び前記ゲイン行列を用いて、平滑ベクトルを算出
する平滑ベクトルの算出ステップと、前記予測誤差共分
散行列、前記観測雑音共分散行列、及び前記観測行列と
を用いて、平滑誤差共分散行列を算出する平滑誤差共分
散行列の算出ステップと、追尾処理終了か否かを判定し
て、追尾処理を続行する場合には、前記観測ベクトルの
入力ステップに戻る追尾終了判定ステップと、を備えた
ことを特徴とする目標追尾方法にある。

【００７２】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾方法であって、後記平滑ベクトルと後記平滑誤差共
分散行列の初期値を設定する初期値の設定ステップと、
前記目標観測装置から現時刻の観測ベクトルを読み込む
観測ベクトルの入力ステップと、前時刻の後記平滑ベク
トルから予測ベクトルを算出する予測ベクトルの算出ス
テップと、駆動雑音共分散行列を設定する駆動雑音共分
散行列の設定ステップと、前時刻の後記平滑誤差共分散
行列と前記駆動雑音共分散行列とから予測誤差共分散行
列を算出する予測誤差共分散行列の算出ステップと、フ
ィルタの切り替え時刻情報を入力とし、ドップラシフト
を考慮した観測モデルにより構成されたフィルタに切り
替わる時刻より後は、前記予測値を用いて、ドップラシ
フトを考慮した観測行列を算出し、後記ゲート用予測観
測ベクトル算出ステップ、後記残差共分散行列算出ステ
ップ、後記平滑ベクトル算出用予測観測ベクトル算出ス
テップ、後記平滑誤差共分散行列算出ステップ、後記ゲ
イン行列算出ステップ、に出力し、切り替わる時刻より
前は、ドップラシフトを考慮しない観測モデルを算出
し、後記ゲート用予測観測ベクトル算出ステップ、後記
残差共分散行列算出ステップ、後記平滑ベクトル算出用
予測観測ベクトル算出ステップ、後記平滑ベクトル算出
用予測観測ベクトル算出ステップ、後記平滑誤差共分散
行列算出ステップ、後記ゲイン行列算出ステップ、に出
力し、切り替わる時刻には、後記ゲート用予測観測ベク
トル算出ステップ、後記残差共分散行列算出ステップ、
にドップラシフトを考慮しない観測モデルを算出及び出
力し、後記ゲイン行列算出ステップ、にドップラシフト
を考慮した観測行列を算出及び出力する観測行列算出ス
テップと、前記予測ベクトルと、前記観測行列を用い
て、ゲート判定に用いる予測観測ベクトルを算出するゲ
ート用予測観測ベクトルの算出ステップと、前記観測ベ
クトルの精度評価に関連したパラメータに基づいて観測
雑音共分散行列を設定する観測雑音共分散行列の設定ス
テップと、前記予測誤差共分散行列、前記観測雑音共分
散行列、及び前記観測行列とを用いて残差共分散行列を
算出する残差共分散行列の算出ステップと、前記予測観
測ベクトルと前記観測ベクトルとを用いて残差ベクトル
を算出するとともに、前記残差共分散行列を用いて、前
記観測ベクトルが追尾目標の探知データであるか否かの
ゲート判定を行うゲート判定ステップと、ドップラシフ
トを考慮した観測モデルにより構成されたフィルタに切
り替わる時刻より前及び後は、前記予測ベクトル及び前
記観測行列より、後記平滑ベクトル算出ステップで用い
る予測観測ベクトルを算出し、ドップラシフトを考慮し
た観測モデルにより構成されたフィルタに切り替わる時
刻では、前記予測ベクトルより、後記平滑ベクトル算出
ステップで用いる予測観測ベクトルを算出する平滑ベク
トル算出用予測観測ベクトルの算出ステップと、前記予
測誤差共分散行列、前記残差共分散行列、及び前記観測
行列とを用いて、フィルタゲイン行列を算出するゲイン
行列の算出ステップと、ゲート判定で追尾目標の探知デ
ータと判定された前記観測ベクトル、前記予測ベクト
ル、前記予測観測ベクトル、及び前記ゲイン行列を用い
て、平滑ベクトルを算出する平滑ベクトルの算出ステッ
プと、前記予測誤差共分散行列、前記観測雑音共分散行
列、及び前記観測行列とを用いて、平滑誤差共分散行列
を算出する平滑誤差共分散行列の算出ステップと、追尾
処理終了か否かを判定して、追尾処理を続行する場合に
は、前記観測ベクトルの入力ステップに戻る追尾終了判
定ステップと、を備えたことを特徴とする目標追尾方法
にある。

【００７３】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾方法であって、後記平滑ベクトルと後記平滑誤差共
分散行列の初期値を設定する初期値の設定ステップと、
前記目標観測装置から現時刻の観測ベクトルを読み込む
観測ベクトルの入力ステップと、前時刻の平滑ベクトル
から予測ベクトルを算出する予測ベクトルの算出ステッ
プと、駆動雑音共分散行列を設定する駆動雑音共分散行
列の設定ステップと、前時刻の平滑誤差共分散行列と前
記駆動雑音共分散行列とから予測誤差共分散行列を算出
する予測誤差共分散行列の算出ステップと、フィルタの
切り替え時刻を入力とし、フィルタに用いる観測モデル
に設定するドップラシフト時定数を算出するフィルタ用
ドップラシフト時定数の算出ステップと、フィルタの切
り替え時刻情報を入力とし、ドップラシフトを考慮した
観測モデルにより構成されたフィルタに切り替わる時刻
以後は、前記予測値を用いて、観測行列を算出し、切り
替わる以前は、ドップラシフトを考慮しない観測モデル
を出力する観測行列算出ステップと、前記予測ベクトル
及び前記観測行列から予測観測ベクトルを算出する予測
観測ベクトルの算出ステップと、前記観測ベクトルの精
度評価に関連したパラメータに基づいて観測雑音共分散
行列を設定する観測雑音共分散行列の設定ステップと、
前記予測誤差共分散行列、前記観測雑音共分散行列、及
び前記観測行列とを用いて残差共分散行列を算出する残
差共分散行列の算出ステップと、前記予測観測ベクトル
と前記観測ベクトルとを用いて残差ベクトルを算出する
とともに、前記残差共分散行列を用いて、前記観測ベク
トルが追尾目標の探知データであるか否かのゲート判定
を行うゲート判定ステップと、前記予測誤差共分散行
列、前記残差共分散行列、及び前記観測行列とを用い
て、フィルタゲイン行列を算出するゲイン行列の算出ス
テップと、ゲート判定で追尾目標の探知データと判定さ
れた観測ベクトル、前記予測ベクトル、前記予測観測ベ
クトル、及び前記ゲイン行列を用いて、平滑ベクトルを
算出する平滑ベクトルの算出ステップと、前記予測誤差
共分散行列、前記観測雑音共分散行列、及び前記観測行
列とを用いて、前記平滑誤差共分散行列を算出する平滑
誤差共分散行列の算出ステップと、追尾処理終了か否か
を判定して、追尾処理を続行する場合には、前記観測ベ
クトルの入力ステップに戻る追尾終了判定ステップと、
を備えたことを特徴とする目標追尾方法にある。

【００７４】

【発明の実施の形態】以下この発明を各実施の形態に従
って説明する。 実施の形態１．図１はこの発明の実施の形態１による目
標追尾装置を示すブロック構成図であり、図２はその処
理動作を示すフローチャートである。図１において、目
標追尾装置１０ａは、従来の技術を示す図２のブロック
構成図と同様に、予測観測ベクトル算出部１０１、予測
ベクトル算出部１０３、平滑ベクトル用メモリ１０５、
ゲート判定部１０７、平滑ベクトル算出部１０９、残差
共分散行列算出部１１１、ゲイン行列算出部１１３、観
測雑音共分散行列設定部１１５、予測誤差共分散行列算
出部１１７、駆動雑音共分散行列設定部１１９、平滑誤
差共分散行列算出部１２１、平滑誤差共分散行列用メモ
リ１２３、を備えている。また、目標追尾装置１０ａ
は、上記構成要素に加えて、観測行列算出部１２５を備
えている。観測行列算出部１２５は、予測ベクトル算出
部１０３より算出された予測値を用いて、観測行列を算
出する。

【００７５】図２において、観測行列算出ステップＳ５
ａは、予測ベクトルを用いて、観測行列を算出するステ
ップである。但し、図５の各処理部、メモリ、及び図２
の処理ステップについて、従来の技術で説明した距離と
距離変化率を用いた二次元の状態ベクトルを用いず、北
基準直交座標系で定義した６次元の状態ベクトルを用い
るため、従来の技術とベクトル及び行列の次数が異な
る。

【００７６】実施の形態１において、カルマンフィルタ
の理論で仮定される目標及びセンサの数式モデルについ
て説明する。カルマンフィルタの理論では、目標運動諸
元の推定を行うために、目標の運動モデルとセンサの観
測モデルを以下のように設定する。まず、目標の運動モ
デルを以下の数式(２３)のように表す。

【００７７】

【数１８】

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

【００７９】

【数１９】

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

【００８１】

【数２０】

【００８２】数式(２６)において、(アンダーバー)ｚ
(ｋ)は時刻ｔ(ｋ)におけるｍ次元観測ベクトル、Ｈ(ｋ)
はｍ行ｎ列の観測行列である。また、(アンダーバー)ｖ
(ｋ)は観測ベクトル(アンダーバー)ｚ(ｋ)に対応した時
刻ｔ(ｋ)のｍ次元観測ベクトルで、平均(アンダーバー)
０、共分散行列Ｒ(ｋ)のｍ変数量正規分布にしたがう白
色雑音系列である。但し、観測雑音ベクトル(アンダー
バー)ｖ(ｋ)と駆動雑音ベクトル(アンダーバー)ｗ(ｋ)
は互いに独立であるとする。

【００８３】センサにより、北基準直交標系における目
標の位置の観測結果が得られる場合、数式(２６)の観測
モデルにおける観測行列を以下の通りに設定する。

【００８４】

【数２１】

【００８５】

【数２２】

【００８６】上記、数式(２７)に示されている観測行列
Ｈ(ｋ)は、北基準直交座標系で距離方向に生じるドップ
ラシフトの影響を反映した観測値を記述するための行列
であり、観測値が距離方向にドップラシフトの影響分で
あるΔＴ(ドット)Ｒ(ｋ)だけシフトすることを表してい
る。数式(２７)に示されている観測行列Ｈ(ｋ)は、北基
準直交座標系で与えられた状態ベクトル(アンダーバー)
ｘ(ｋ)を極座標系に変換した変数Ｒ(ｋ)、Ｅ(ｋ)、Ａｚ
(ｋ)、(ドット)Ｒ(ｋ)、(ドット)Ｅ(ｋ)、(ドット)Ａｚ
(ｋ)を用いて算出される。数式(３１)及び数式(３２)
は、北基準直交座標系から極座標への変換の式を表して
いる。数式(２８)におけるΔＴは、予め与えられる時定
数である。但し、実際には、真の目標の運動諸元である
状態ベクトル(アンダーバー)ｘ(ｋ)は未知であるため、
その予測値である(上下バー)ｘ(ｋ)を用いて観測行列を
算出する。

【００８７】以下、図２を参照しながら、図１に示した
この発明の実施の形態１による処理動作について詳細に
説明する。まず、平滑ベクトル用メモリ１０５と平滑誤
差共分散行列用メモリ１２３とに、それぞれ、平滑ベク
トルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤差
共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開始
する(初期値の設定ステップＳ１)。ここで、状態ベクト
ル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義さ
れる、数式(３３)の６次元ベクトルである。

【００８８】

【数２３】

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

【００９０】

【数２４】

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

【００９２】

【数２５】

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

【００９４】

【数２６】

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

【００９６】

【数２７】

【００９７】

【数２８】

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

【００９９】予測観測ベクトル算出部１０１は、予測ベ
クトル算出部１０３から入力された予測ベクトル(上下
バー)ｘ(ｋ)と観測行列算出部１２５から入力された観
測行列を用いて、以下の数式(４３)により予測観測ベク
トル(上下バー)ｚ(ｋ)を算出する(予測観測ベクトルの
算出ステップＳ６)。

【０１００】

【数２９】

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

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

【０１０３】

【数３０】

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

【０１０５】

【数３１】

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

【０１０７】次に、ゲイン行列算出部１１３は、予測誤
差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、お
よび観測行列Ｈ(ｋ)を読み込み、以下の数式(４７)にし
たがって、フィルタゲインを算出する(ゲイン行列の算
出ステップＳ１０)。

【０１０８】

【数３２】

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

【０１１０】

【数３３】

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

【０１１２】

【数３４】

【０１１３】上記数式(４８)または数式(４９)により、
平滑ベクトル算出部１０９で算出された平滑ベクトル
(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモ
リ１０５に格納されるとともに、航跡表示装置３０に伝
送され、運用者に対して表示される。

【０１１４】一方、平滑誤差共分散行列算出部１２１
は、ゲート判定部１０７で追尾目標の観測ベクトルが得
られたと判定された場合には、予測誤差共分散行列(バ
ー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列
Ｈ(ｋ)を用いて、以下の数式(５０)により平滑誤差共分
散行列を算出し、ゲート判定部１０７で追尾目標の観測
ベクトルが得られなかったと判定された場合には、以下
の数式(５１)に示すように、予測誤差共分散行列(バー)
Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行
列の算出ステップＳ１２)。

【０１１５】

【数３５】

【０１１６】数式(５０)及び数式(５１)に従う平滑誤差
共分散行列の算出結果は、平滑誤差共分散行列用メモリ
１２３に格納される。

【０１１７】以上で時刻ｔ(ｋ)における処理が終了す
る。以降、追尾終了が判定されるまで、上記一連の処理
ステップを繰り返し実行する(追尾終了ステップＳ１
３)。

【０１１８】このように、カルマンフィルタを用い、北
基準直交座標系でドップラシフト項を観測モデルに記述
した実施の形態１の目標追尾装置１０ａは、事前に仮定
された等距離直線運動モデルを用い、追尾航跡を算出す
る。図３に実施の形態１を用いた平滑ベクトルの位置成
分(平滑値)、予測観測ベクトル(ゲート中心)、および観
測ベクトル(観測値)の時間推移を示す。図の実線は、状
態ベクトルの位置成分の真の軌跡を表し、点線は、ドッ
プラシフトの影響を受けた観測ベクトルの真値を示す。
図のように、予測観測ベクトルは、ドップラシフトを考
慮した観測行列を用いることにより、予測ベクトルをド
ップラシフト分だけシフトした値となり、ドップラシフ
トの影響を受けた観測値の近くに算出される。平滑ベク
トルの距離成分は状態ベクトル真の軌跡付近に算出され
る。

【０１１９】このように、ドップラシフト補正を考慮し
たフィルタを北基準直交座標系で構成するために、従来
の技術で用いた極座標系の距離軸で表現した観測モデル
ではなく、北基準直交座標系における観測モデルを構成
することにより、ドップラシフト補正を北基準直交座標
系で行うことが可能となる。

【０１２０】実施の形態２．実施の形態１で示したよう
に、カルマンフィルタを用い、北基準直交座標系でドッ
プラシフト項を観測モデルに記述した目標追尾装置は、
事前に仮定された等速直線運動モデルを用い、追尾航跡
を算出する。しかし、上記実施の形態１のドップラシフ
トを考慮したフィルタを追尾開始時から用いると、速度
(ドット)ｘ，(ドット)ｙ，(ドット)ｚの推定値が揺らぐ
ため、これらの推定値を極座標表示することによって得
られる距離変化率成分の劣化が生じる。フィルタは、本
質的に、距離変化率の推定値を用いて、観測値に含まれ
ているドップラシフト分を補正し、距離推定値を算出す
る。よって、距離変化率推定精度の劣化が距離推定精度
の劣化につながる。上記の理由から、距離推定精度の劣
化を防ぐため、追尾初期は、ドップラシフトを考慮しな
いフィルタを用い、速度(ドット)ｘ，(ドット)ｙ，(ド
ット)ｚ推定値より得られる距離変化率推定精度が安定
後、フィルタの切り替えを行うことが考えられる。図２
１に、位置平滑値、予測観測値、および観測値の、切り
替え前と切り替え後の推移に関する概念図が示されてい
る。ドップラシフトを考慮しないフィルタとは、観測行
列を、数式(３７)の代わりに、以下の数式(５２)を用い
て構成されたカルマンフィルタである。

【０１２１】

【数３６】

【０１２２】この場合、ドップラシフト分の補正を行わ
ないため、ドップラシフト分の補正による、フィルタ開
始時の距離変化率精度劣化の距離精度に対する影響がな
い。ここで、問題となるのは、切り替え時のゲート中心
が観測値と離れた所に算出され、観測値がゲートに収ま
らない可能性があることである。図２１に示すように、
まず、ドップラシフトの影響を受けた観測値を入力とし
てドップラシフトを考慮しないフィルタを用いると、フ
ィルタの平滑位置が目標の真の位置からずれて算出され
てしまう。次に、フィルタの切り替え直後にその平滑値
を用いて予測観測値を算出すると、ドップラシフト分だ
けずれた位置に予測観測値が算出されてしまい、結果的
に予測観測値と観測値の距離が長くなってしまう。距離
がながくなると、ゲート判定の際に、観測値をゲートに
捕捉できない危険性が生じる。この問題を解決するた
め、以後、ドップラシフトを考慮しない観測モデルを用
いたフィルタからドップラシフト補正を考慮した観測モ
デルを用いたフィルタの切り替え時に、観測値とゲート
中心の距離が増大する問題を解決することを目的とす
る。

【０１２３】図４はこの発明の実施の形態２による目標
追尾装置を示すブロック構成図であり、また図５はその
処理動作を示すフローチャートである。上記図１、図２
と同様のものについては、詳述を省略する。また、図６
はこの発明の実施の形態２における効果を示す概念図で
ある。

【０１２４】図４において目標追尾装置１０ａは、実施
の形態１を示す図１と同様に、平滑ベクトル用メモリ１
０５、予測ベクトル算出部１０３、ゲート判定部１０
７、平滑ベクトル算出部１０９、残差共分散行列算出部
１１１、ゲイン行列算出部１１３、観測雑音共分散行列
設定部１１５、予測誤差共分散行列算出部１１７、駆動
雑音共分散行列設定部１１９、平滑誤差共分散行列算出
部１２１、平滑誤差共分散行列用メモリ１２３を備えて
いる。

【０１２５】また、目標追尾装置１０ａは、上記構成要
素に加えて、切り替え制御部１２７、観測行列算出部１
２５ａ(実施の形態１の図１におけるそれと動作が異な
る)、平滑ベクトル算出用予測観測ベクトル算出部１３
１、ゲート用予測観測ベクトル算出部１２９、切り替え
直前の擬似平滑値算出部１３３を備えている。切り替え
制御部１２７は、ドップラシフトを考慮しない観測モデ
ルにより構成されたフィルタからドップラシフトを考慮
した観測モデルにより構成されたフィルタへの切り替え
タイミングを、観測行列算出部１２５ａおよび切り替え
直前の擬似平滑値算出部１３３に出力する。

【０１２６】観測行列算出部１２５ａは、切り替え制御
部１２７からのフィルタの切り替え時刻情報を入力と
し、ドップラシフトを考慮した観測モデルにより構成さ
れたフィルタに切り替わる時刻以後は、予測ベクトル算
出部１０３より算出された予測値を用いて、ドップラシ
フトを考慮した観測行列を算出する。切り替わる時刻よ
り前は、ドップラシフトを考慮しない観測モデルを算出
する。平滑ベクトル算出用予測観測ベクトル算出部１３
１は、平滑ベクトル用メモリ１０５からの1時刻前にお
ける平滑ベクトル及び観測行列算出部１２５ａからの入
力である観測行列より、平滑ベクトル算出部１０９で用
いる予測観測ベクトルを算出する。

【０１２７】ゲート用予測観測ベクトル算出部１２９
は、予測ベクトル算出部１０３からの入力である予測ベ
クトル及び観測行列算出部１２５ａからの入力である観
測行列を用いて、ゲート判定部１０７で用いる予測観測
ベクトルを算出する。

【０１２８】切り替え直前の擬似平滑値算出部１３３
は、切り替え制御部１２７よりフィルタの切り替え時刻
情報を得、現時刻が切り替えを行う時刻の場合、平滑ベ
クトル用メモリ１０５からの１時刻前における平滑ベク
トルを用いて、擬似平滑値を算出する。切り替えを行う
時刻でない場合、1時刻前の平滑値をそのまま出力す
る。

【０１２９】図５において、実施の形態１の処理動作を
示すフローチャートである図２と同様に、初期値の設定
ステップＳ１、観測ベクトルの入力ステップＳ２、予測
ベクトルの算出ステップＳ３、駆動雑音共分散行列の設
定ステップＳ４、予測誤差共分散行列の算出ステップＳ
５、観測雑音共分散行列の設定ステップＳ７、残差共分
散行列の算出ステップＳ８、ゲート判定ステップＳ９、
ゲイン行列の算出ステップＳ１０、平滑ベクトルの算出
ステップＳ１１、平滑誤差共分散行列の算出ステップＳ
１２を備えている。

【０１３０】また、図５は上記処理ステップに加えて、
切り替え直前の擬似平滑値の算出ステップＳ２ａ、ゲー
ト用予測観測ベクトルの算出ステップ５ｂ、平滑ベクト
ル算出用予測観測ベクトルの算出ステップ９ａ、観測行
列算出ステップ５ａａ(実施の形態１の処理動作を示す
図２のそれと処理が異なる)、が挿入されている。

【０１３１】図５において、切り替え直前の擬似平滑値
の算出ステップＳ２ａは、切り替え制御部１２７よりフ
ィルタの切り替え時刻情報を得、現時刻が切り替えを行
う時刻の場合、平滑ベクトル用メモリ１０５からの１時
刻前における平滑ベクトルを用いて、擬似平滑値を算出
するステップである。現時刻が切り替えを行う時刻でな
い場合、平滑ベクトル用メモリ１０５からの1時刻前に
おける平滑ベクトルをそのまま出力とする。

【０１３２】ゲート用予測観測ベクトルの算出ステップ
Ｓ５ｂは、予測ベクトル算出部１０３で算出された予測
ベクトルと、観測行列算出部１２５ａで算出された観測
行列を用いて、ゲート判定に用いる予測観測ベクトルを
算出するステップである。

【０１３３】平滑ベクトル算出用予測観測ベクトルの算
出ステップＳ９ａは、平滑ベクトル用メモリ１０５から
の１時刻前における平滑ベクトルと、観測行列算出部１
２５ａで算出された観測行列を用いて、平滑ベクトルの
算出に用いる予測観測ベクトルを算出するステップであ
る。

【０１３４】観測行列算出ステップＳ５ａａは、切り替
え制御部１２７からのフィルタの切り替え時刻情報を入
力とし、ドップラシフトを考慮した観測モデルにより構
成されたフィルタに切り替わる時刻以後は、予測ベクト
ル算出部１０３より算出された予測値を用いて、観測行
列を算出し、切り替わる以前は、ドップラシフトを考慮
しない観測モデルを出力するステップである。

【０１３５】以下、図４、図５を参照しながら、実施の
形態２による処理動作について詳細に説明する。まず、
平滑ベクトル用メモリ１０５と平滑誤差共分散行列用メ
モリ１２３とに、それぞれ、平滑ベクトルの初期値(ア
ンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の初
期値Ｐ(０)とを設定し、追尾処理を開始する(初期値の
設定ステップＳ１)。ここで、状態ベクトル(アンダーバ
ー)ｘ(ｋ)は、北基準直交座標系で定義される、数式(５
３)の６次元ベクトルである。

【０１３６】

【数３７】

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

【０１３８】

【数３８】

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

【０１４０】

【数３９】

【０１４１】

【数４０】

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

【０１４３】次に、以下の数式(６０)にしたがって予測
ベクトルを算出する(予測ベクトルの算出ステップＳ
３)。

【０１４４】

【数４１】

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

【０１４６】

【数４２】

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

【０１４８】

【数４３】

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

【０１５０】

【数４４】

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

【０１５２】

【数４５】

【０１５３】

【数４６】

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

【０１５５】ゲート用予測観測ベクトルの算出ステップ
５ｂでは、ゲート用予測観測ベクトル算出部１２９は、
予測ベクトル算出部１０３から入力された予測ベクトル
(上下バー)ｘ_h(ｋ)と観測行列算出部１２５ａから入力
された観測行列を用いて、以下の数式(７１)により予測
観測ベクトル(上下バー)ｚ_g(ｋ)を算出する(予測観測ベ
クトルの算出ステップＳ５ｂ)。

【０１５６】

【数４７】

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

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

【０１５９】

【数４８】

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

【０１６１】

【数４９】

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

【０１６３】平滑ベクトル算出用予測観測ベクトルの算
出ステップＳ９ａでは、平滑ベクトル算出部１０９へ出
力する予測観測ベクトルを以下の数式(７５)及び数式
(７６)で算出する。

【０１６４】

【数５０】

【０１６５】平滑ベクトル算出用予測観測ベクトル(上
下バー)ｚ_h(ｋ)は、平滑ベクトル算出部１０９におい
て、観測値との差である残差を計算するために用いられ
る。

【０１６６】次に、ゲイン行列算出部１１３は、予測誤
差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、お
よび観測行列Ｈ(ｋ)を読み込み、以下の数式(７７)にし
たがって、フィルタゲインを算出する(ゲイン行列の算
出ステップＳ１０)。

【０１６７】

【数５１】

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

【０１６９】

【数５２】

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

【０１７１】

【数５３】

【０１７２】上記数式(７８)または数式(７９)により、
平滑ベクトル算出部１０９で算出された平滑ベクトル
(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メモ
リ１０５に格納されるとともに、航跡表示装置３０に伝
送され、運用者に対して表示される。

【０１７３】一方、平滑誤差共分散行列算出部１２１
は、ゲート判定部１０７で追尾目標の観測ベクトルが得
られたと判定された場合には、予測誤差共分散行列(バ
ー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列
Ｈ(ｋ)を用いて、以下の数式(８０)により平滑誤差共分
散行列を算出し、ゲート判定部１０７で追尾目標の観測
ベクトルが得られなかったと判定された場合には、以下
の数式(８１)に示すように、予測誤差共分散行列(バー)
Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共分散行
列の算出ステップＳ１２)。

【０１７４】

【数５４】

【０１７５】数式(８０)及び数式(８１)に従う平滑誤差
共分散行列の算出結果は、平滑誤差共分散行列用メモリ
１２３に格納される。

【０１７６】以上で時刻ｔ(ｋ)における処理が終了す
る。以降、追尾終了が判定されるまで、上記一連の処理
ステップを繰り返し実行する(追尾終了ステップＳ１
３)。

【０１７７】図６に、実施の形態２を用いた平滑ベクト
ルの位置成分(平滑値)、予測観測ベクトル(ゲート中
心)、および観測ベクトル(観測値)の時間推移を示す。
図の実線は、状態ベクトルの位置成分の真の軌跡を表
し、点線は、ドップラシフトの影響を受けた観測ベクト
ルの真値を示す。上記の処理ステップにより、図６のよ
うに、ドップラシフトを考慮しない観測モデルで構成さ
れたフィルタから、ドップラシフトを考慮した観測モデ
ルを考慮したフィルタへ切り替えを行う際に、ゲートの
中心である予測観測ベクトルと観測ベクトルとの距離が
離れてしまう問題を改善するため、切り替え時に、前時
刻の平滑ベクトルに対し、予めドップラ補正を施して擬
似平滑ベクトルを算出することにより、擬似平滑ベクト
ルを用いて算出したゲート用予測観測ベクトルと観測ベ
クトルとの距離が短くなる効果がある。さらに、平滑ベ
クトルを算出する際の予測観測ベクトルに、上記で算出
したゲート用予測観測ベクトルと異なり、ドップラシフ
トの影響を受けた予測ベクトルから算出した予測観測ベ
クトル値を用いることにより、残差ベクトルが大きくな
り、切り替え時における平滑ベクトルの過渡応答の収束
速度が速くなる効果がある。

【０１７８】なお、本実施の形態の考え方を用いれば、
ドップラシフトを考慮した観測モデルで構成されたフィ
ルタからドップラシフトを考慮しない観測モデルを用い
たフィルタへの切り替えに対する対応も可能であること
は明らかであるので、説明は省略する。

【０１７９】実施の形態３．図７はこの発明の実施の形
態３による目標追尾装置を示すブロック構成図であり、
また図８はその処理動作を示すフローチャートである。
上記図４、図５と同様のものについては、詳述を省略す
る。また、図９はこの発明の実施の形態３における効果
を示す概念図である。

【０１８０】図７において、目標追尾装置１０ａは、実
施の形態２を示すブロック構成図である図４と同様に、
平滑ベクトル用メモリ１０５、予測ベクトル算出部１０
３、ゲート用予測観測ベクトル算出部１２９、ゲート判
定部１０７、平滑ベクトル算出部１０９、残差共分散行
列算出部１１１、ゲイン行列算出部１１３、切り替え制
御部１２７、観測雑音共分散行列設定部１１５、予測誤
差共分散行列算出部１１７、駆動雑音共分散行列設定部
１１９、平滑誤差共分散行列算出部１２１、平滑誤差共
分散行列用メモリ１２３を備えている。

【０１８１】また、目標追尾装置１０ａは、上記構成要
素に加えて、観測行列算出部１２５ｂ(実施の形態２の
図４におけるそれと動作が異なる)、平滑ベクトル算出
用予測観測ベクトル算出部１３１ａ(実施の形態２の図
４におけるそれと動作が異なる)を備えている。

【０１８２】観測行列算出部１２５ｂは、切り替え制御
部１２７からのフィルタの切り替え時刻情報を入力と
し、ドップラシフトを考慮した観測モデルにより構成さ
れたフィルタに切り替わる時刻より後は、予測ベクトル
算出部１０３より算出された予測値を用いて、ドップラ
シフトを考慮した観測行列を算出し、ゲート用予測観測
ベクトル算出部１２９、残差共分散行列算出部１１１、
平滑ベクトル算出用予測観測ベクトル算出部１３１ａ、
平滑誤差共分散行列算出部１２１、ゲイン行列算出部１
１３、に出力する。切り替わる時刻より前は、ドップラ
シフトを考慮しない観測モデルを算出し、ゲート用予測
観測ベクトル算出部１２９、残差共分散行列算出部１１
１、平滑ベクトル算出用予測観測ベクトル算出部１３１
ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算出
部１１３に出力する。切り替わる時刻には、ゲート用予
測観測ベクトル算出部１２９、残差共分散行列算出部１
１１にドップラシフトを考慮しない観測モデルを算出及
び出力し、平滑ベクトル算出用予測観測ベクトル算出部
１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行
列算出部１１３にドップラシフトを考慮した観測行列を
算出及び出力する。平滑ベクトル算出用予測観測ベクト
ル算出部１３１ａは、予測ベクトル算出部１０３からの
予測ベクトル及び観測行列算出部１２５ｂからの入力で
ある観測行列より、平滑ベクトル算出部１０９で用いる
予測観測ベクトルを算出する。

【０１８３】図８において、実施の形態２の処理動作を
示すフローチャートである図５と同様に、初期値の設定
ステップＳ１、観測ベクトルの入力ステップＳ２、予測
ベクトルの算出ステップＳ３、駆動雑音共分散行列の設
定ステップＳ４、予測誤差共分散行列の算出ステップＳ
５、ゲート用予測観測ベクトルの算出ステップＳ５ｂ、
観測雑音共分散行列の設定ステップＳ７、残差共分散行
列の算出ステップＳ８、ゲート判定ステップＳ９、ゲイ
ン行列の算出ステップＳ１０、平滑ベクトルの算出ステ
ップＳ１１、平滑誤差共分散行列の算出ステップＳ１
２、を備えている。

【０１８４】また、図８は上記処理ステップに加えて、
平滑ベクトル算出用予測観測ベクトルの算出ステップＳ
９ａａ(実施の形態２の処理動作を示す図５のそれと処
理が異なる)、観測行列算出ステップＳ５ａａａ(実施の
形態２の処理動作を示す図５のそれと処理が異なる)、
が挿入されている。

【０１８５】図８において、平滑ベクトル算出用予測観
測ベクトルの算出ステップＳ９ａａは、予測ベクトル算
出部１０３からの予測ベクトルと、観測行列算出部１２
５ｂで算出された観測行列を用いて、平滑ベクトルの算
出に用いる予測観測ベクトルを算出するステップであ
る。

【０１８６】観測行列算出ステップＳ５ａａａは、切り
替え制御部１２７からのフィルタの切り替え時刻情報を
入力とし、ドップラシフトを考慮した観測モデルにより
構成されたフィルタに切り替わる時刻より後は、予測ベ
クトル算出部１０３より算出された予測値を用いて、ド
ップラシフトを考慮した観測行列を算出し、ゲート用予
測観測ベクトル算出部１２９、残差共分散行列算出部１
１１、平滑ベクトル算出用予測観測ベクトル算出部１３
１ａ、平滑誤差共分散行列算出部１２１、ゲイン行列算
出部１１３、に出力する。切り替わる時刻より前は、ド
ップラシフトを考慮しない観測モデルを算出し、ゲート
用予測観測ベクトル算出部１２９、残差共分散行列算出
部１１１、平滑ベクトル算出用予測観測ベクトル算出部
１３１ａ、平滑誤差共分散行列算出部１２１、ゲイン行
列算出部１１３、に出力する。切り替わる時刻には、ゲ
ート用予測観測ベクトル算出部１２９、残差共分散行列
算出部１１１、にドップラシフトを考慮しない観測モデ
ルを算出及び出力し、平滑ベクトル算出用予測観測ベク
トル算出部１３１ａ、平滑誤差共分散行列算出部１２
１、ゲイン行列算出部１１３に、ドップラシフトを考慮
した観測行列を算出及び出力するステップである。

【０１８７】以下、図７および図８を参照しながら、実
施の形態３による処理動作について詳細に説明する。ま
ず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列
用メモリ１２３とに、それぞれ、平滑ベクトルの初期値
(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の
初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値
の設定ステップＳ１)。ここで、状態ベクトル(アンダー
バー)ｘ(ｋ)は、北基準直交座標系で定義される、数式
(８２)の６次元ベクトルである。

【０１８８】

【数５５】

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

【０１９０】

【数５６】

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

【０１９２】

【数５７】

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

【０１９４】

【数５８】

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

【０１９６】

【数５９】

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

【０１９８】

【数６０】

【０１９９】

【数６１】

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

【０２０１】切り替え時刻の場合、観測行列算出部１２
５ｂは、観測行列を数式(８６)として、ゲート用予測観
測ベクトル算出部１２９、残差共分散行列算出部１１１
へ出力し、予測ベクトル算出部１０３から出力された予
測ベクトル(上下バー)ｘ_h(ｋ)から、数式(８７)、数式
(８８)、数式(８９)、数式(９０)、数式(９１)、数式
(９２)により観測行列を算出し、平滑ベクトル算出用予
測観測ベクトル算出部１３１ａ、平滑誤差共分散行列算
出部１２１、ゲイン行列算出部１１３へ出力する。

【０２０２】ゲート用予測観測ベクトルの算出ステップ
５ｂでは、ゲート用予測観測ベクトル算出部１２９は、
予測ベクトル算出部１０３から入力された予測ベクトル
(上下バー)ｘ_h(ｋ)と観測行列算出部１２５ｂから入力
された観測行列を用いて、以下の数式(９３)により予測
観測ベクトル(上下バー)ｚ_g(ｋ)を算出する(予測観測ベ
クトルの算出ステップ５ｂ)。

【０２０３】

【数６２】

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

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

【０２０６】

【数６３】

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

【０２０８】

【数６４】

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

【０２１０】平滑ベクトル算出用予測観測ベクトルの算
出ステップＳ９ａａでは、平滑ベクトル算出部１０９へ
出力する予測観測ベクトルを以下の数式(９７)で算出す
る。

【０２１１】

【数６５】

【０２１２】平滑ベクトル算出用予測観測ベクトル(上
下バー)ｚ_h(ｋ)は、平滑ベクトル算出部１０９におい
て、観測値との差である残差を計算するために用いられ
る。

【０２１３】次に、ゲイン行列算出部１１３は、予測誤
差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、お
よび観測行列Ｈ(ｋ)を読み込み、以下の数式(９８)にし
たがって、フィルタゲインを算出する(ゲイン行列の算
出ステップＳ１０)。

【０２１４】

【数６６】

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

【０２１６】

【数６７】

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

【０２１８】

【数６８】

【０２１９】上記数式(９９)または数式(１００)によ
り、平滑ベクトル算出部１０９で算出された平滑ベクト
ル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メ
モリ１０５に格納されるとともに、航跡表示装置３０に
伝送され、運用者に対して表示される。

【０２２０】一方、平滑誤差共分散行列算出部１２１
は、ゲート判定部１０７で追尾目標の観測ベクトルが得
られたと判定された場合には、予測誤差共分散行列(バ
ー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列
Ｈ(ｋ)を用いて、以下の数式(１０１)により平滑誤差共
分散行列を算出し、ゲート判定部１０７で追尾目標の観
測ベクトルが得られなかったと判定された場合には、以
下の数式(１０２)に示すように、予測誤差共分散行列
(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共
分散行列の算出ステップＳ１２)。

【０２２１】

【数６９】

【０２２２】数式(１０１)及び数式(１０２)に従う平滑
誤差共分散行列の算出結果は、平滑誤差共分散行列用メ
モリ１２３に格納される。

【０２２３】以上で時刻ｔ(ｋ)における処理が終了す
る。以降、追尾終了が判定されるまで、上記一連の処理
ステップを繰り返し実行する(追尾終了ステップＳ１
３)。

【０２２４】図９に実施の形態３を用いた平滑ベクトル
の位置成分(平滑値)、予測観測ベクトル(ゲート中心)、
および観測ベクトル(観測値)の時間推移を示す。図の実
線は、状態ベクトルの位置成分の真の軌跡を表し、点線
は、ドップラシフトの影響を受けた観測ベクトルの真値
を示す。上記の処理ステップにより、図９のように、ド
ップラシフトを考慮しない観測モデルで構成されたフィ
ルタから、ドップラシフトを考慮した観測モデルを考慮
したフィルタへ切り替えを行う際に、ゲートの中心であ
る予測観測ベクトルと観測ベクトルとの距離が離れてし
まう問題を改善するため、切り替え時に、ゲート用予測
観測ベクトルの算出においてドップラシフトを考慮しな
い観測モデルを用いることにより、ゲート用予測観測ベ
クトルと観測ベクトルとの距離が短くなる効果がある。
さらに、平滑ベクトルを算出する際の予測観測ベクトル
に、上記で算出したゲート用予測観測ベクトルと異な
り、ドップラシフトを考慮した観測モデルを用いること
により、残差ベクトルが大きくなり、切り替え時におけ
る平滑ベクトルの過渡応答の収束速度が速くなる効果が
ある。

【０２２５】なお、本実施の形態の考え方を用いれば、
ドップラシフトを考慮した観測モデルで構成されたフィ
ルタからドップラシフトを考慮しない観測モデルを用い
たフィルタへの切り替えに対する対応も可能であること
は明らかであるので、説明は省略する。

【０２２６】実施の形態４．図１０はこの発明の実施の
形態４による目標追尾装置を示すブロック構成図であ
り、また図１１はその処理動作を示すフローチャートで
ある。上記図７、図８と同様のものについては、詳述を
省略する。また、図１２はこの発明の実施の形態３にお
ける効果を示す概念図である。

【０２２７】図１０において、目標追尾装置１０ａは実
施の形態３を示すブロック構成図である図７と同様に、
平滑ベクトル用メモリ１０５、予測ベクトル算出部１０
３、ゲート用予測観測ベクトル算出部１２９、ゲート判
定部１０７、平滑ベクトル算出部１０９、残差共分散行
列算出部１１１、ゲイン行列算出部１１３、切り替え制
御部１２７、観測雑音共分散行列設定部１１５、予測誤
差共分散行列算出部１１７、駆動雑音共分散行列設定部
１１９、平滑誤差共分散行列算出部１２１、平滑誤差共
分散行列用メモリ１２３、を備えている。

【０２２８】また目標追尾装置１０ａは、上記構成要素
に加えて観測行列算出部１２５ｃ(実施の形態３の図７
におけるそれと動作が異なる)、平滑ベクトル算出用予
測観測ベクトル算出部１３１ｂ(実施の形態３の図７に
おけるそれと動作が異なる)、を備えている。

【０２２９】観測行列算出部１２５ｃは、切り替え制御
部１２７からのフィルタの切り替え時刻情報を入力と
し、ドップラシフトを考慮した観測モデルにより構成さ
れたフィルタに切り替わる時刻より後は、予測ベクトル
算出部１０３より算出された予測値を用いて、ドップラ
シフトを考慮した観測行列を算出し、ゲート用予測観測
ベクトル算出部１２９、残差共分散行列算出部１１１、
平滑ベクトル算出用予測観測ベクトル算出部１３１ｂ、
平滑誤差共分散行列算出部１２１、ゲイン行列算出部１
１３、に出力する。切り替わる時刻より前は、ドップラ
シフトを考慮しない観測モデルを算出し、ゲート用予測
観測ベクトル算出部１２９、残差共分散行列算出部１１
１、平滑ベクトル算出用予測観測ベクトル算出部１３１
ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列算出
部１１３、に出力する。切り替わる時刻には、ゲート用
予測観測ベクトル算出部１２９、残差共分散行列算出部
１１１、にドップラシフトを考慮しない観測モデルを算
出及び出力し、平滑誤差共分散行列算出部１２１、ゲイ
ン行列算出部１１３、にドップラシフトを考慮した観測
行列を算出及び出力する。

【０２３０】平滑ベクトル算出用予測観測ベクトル算出
部１３１ｂは、ドップラシフトを考慮した観測モデルに
より構成されたフィルタに切り替わる時刻より前及び後
は、予測ベクトル算出部１０３からの予測ベクトル及び
観測行列算出部１２５ｃからの入力である観測行列よ
り、平滑ベクトル算出部１０９で用いる予測観測ベクト
ルを算出する。ドップラシフトを考慮した観測モデルに
より構成されたフィルタに切り替わる時刻では、予測ベ
クトル算出部１０３からの予測ベクトルより、平滑ベク
トル算出部１０９で用いる予測観測ベクトルを算出す
る。

【０２３１】図１１において、実施の形態３の処理動作
を示すフローチャートである図８と同様に、初期値の設
定ステップＳ１、観測ベクトルの入力ステップＳ２、予
測ベクトルの算出ステップＳ３、駆動雑音共分散行列の
設定ステップＳ４、予測誤差共分散行列の算出ステップ
Ｓ５、ゲート用予測観測ベクトルの算出ステップ５ｂ、
観測雑音共分散行列の設定ステップＳ７、残差共分散行
列の算出ステップＳ８、ゲート判定ステップＳ９、ゲイ
ン行列の算出ステップＳ１０、平滑ベクトルの算出ステ
ップＳ１１、平滑誤差共分散行列の算出ステップＳ１
２、を備えている。

【０２３２】また、図１１は上記処理ステップに加え
て、平滑ベクトル算出用予測観測ベクトルの算出ステッ
プＱ９ａａａ(実施の形態３の処理動作を示す図８のそ
れと処理が異なる)、観測行列算出ステップＳ５ａａａ
ａ(実施の形態３の処理動作を示す図 14のそれと処理が
異なる)、が挿入されている。図 17において、平滑ベク
トル算出用予測観測ベクトルの算出ステップは、ドップ
ラシフトを考慮した観測モデルにより構成されたフィル
タに切り替わる時刻より前及び後は、予測ベクトル算出
部１０３からの予測ベクトル及び観測行列算出部１２５
ｃからの入力である観測行列より、平滑ベクトル算出部
１０９で用いる予測観測ベクトルを算出する。ドップラ
シフトを考慮した観測モデルにより構成されたフィルタ
に切り替わる時刻では、予測ベクトル算出部１０３から
の予測ベクトルより、平滑ベクトル算出部１０９で用い
る予測観測ベクトルを算出するステップである。

【０２３３】観測行列算出ステップＳ５ａａａａは、切
り替え制御部１２７からのフィルタの切り替え時刻情報
を入力とし、ドップラシフトを考慮した観測モデルによ
り構成されたフィルタに切り替わる時刻より後は、予測
ベクトル算出部１０３より算出された予測値を用いて、
ドップラシフトを考慮した観測行列を算出し、ゲート用
予測観測ベクトル算出部１２９、残差共分散行列算出部
１１１、平滑ベクトル算出用予測観測ベクトル算出部１
３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン行列
算出部１１３、に出力する。切り替わる時刻より前は、
ドップラシフトを考慮しない観測モデルを算出し、ゲー
ト用予測観測ベクトル算出部１２９、残差共分散行列算
出部１１１、平滑ベクトル算出用予測観測ベクトル算出
部１３１ｂ、平滑誤差共分散行列算出部１２１、ゲイン
行列算出部１１３、に出力する。切り替わる時刻には、
ゲート用予測観測ベクトル算出部１２９、残差共分散行
列算出部１１１、にドップラシフトを考慮しない観測モ
デルを算出及び出力し、平滑誤差共分散行列算出部１２
１、ゲイン行列算出部１１３、にドップラシフトを考慮
した観測行列を算出及び出力するステップである。

【０２３４】以下、図１０、図１１を参照しながら、実
施の形態４による処理動作について詳細に説明する。ま
ず、平滑ベクトル用メモリ１０５と平滑誤差共分散行列
用メモリ１２３とに、それぞれ、平滑ベクトルの初期値
(アンダーバー・ハット)ｘ(０)と平滑誤差共分散行列の
初期値Ｐ(０)とを設定し、追尾処理を開始する(初期値
の設定ステップＳ１)。ここで、状態ベクトル(アンダー
バー)ｘ(ｋ)は、北基準直交座標系で定義される、数式
(１０３)の６次元ベクトルである。

【０２３５】

【数７０】

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

【０２３７】

【数７１】

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

【０２３９】

【数７２】

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

【０２４１】

【数７３】

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

【０２４３】

【数７４】

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

【０２４５】

【数７５】

【０２４６】

【数７６】

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

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

【０２４９】ゲート用予測観測ベクトルの算出ステップ
Ｓ５ｂでは、ゲート用予測観測ベクトル算出部１２９
は、予測ベクトル算出部１０３から入力された予測ベク
トル(上下バー)ｘ_h(ｋ)と観測行列算出部１２５ｃから
入力された観測行列を用いて、以下の数式(１１４)によ
り予測観測ベクトル(上下バー)ｚ_g(ｋ)を算出する(予測
観測ベクトルの算出ステップＳ５ｂ)。

【０２５０】

【数７７】

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

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

【０２５３】

【数７８】

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

【０２５５】

【数７９】

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

【０２５７】平滑ベクトル算出用予測観測ベクトルの算
出ステップＳ９ａａでは、現時刻が移管時刻より前また
の場合、平滑ベクトル算出部１０９へ出力する予測観測
ベクトルを以下の数式(１１８)で算出する。

【０２５８】

【数８０】

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

【０２６０】

【数８１】

【０２６１】数式(１２０)は、北基準直交座標系から極
座標系への変換式であり、数式(１２２)は極座標系から
北基準直交座標系での変換式である。数式(１２１)は、
ドップラシフトの影響を受けた予測ベクトルの距離成分
にドップラシフトを与える式である。こうすることによ
り、平滑ベクトルの算出ステップにおける残差を大きく
することが可能となる。

【０２６２】次に、ゲイン行列算出部１１３は、予測誤
差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、お
よび観測行列Ｈ(ｋ)を読み込み、以下の数式(１２４)に
したがって、フィルタゲインを算出する(ゲイン行列の
算出ステップＳ１０)。

【０２６３】

【数８２】

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

【０２６５】

【数８３】

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

【０２６７】

【数８４】

【０２６８】上記数式(１２５)または数式(１２６)によ
り、平滑ベクトル算出部１０９で算出された平滑ベクト
ル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メ
モリ１０５に格納されるとともに、航跡表示装置３０に
伝送され、運用者に対して表示される。

【０２６９】一方、平滑誤差共分散行列算出部１２１
は、ゲート判定部１０７で追尾目標の観測ベクトルが得
られたと判定された場合には、予測誤差共分散行列(バ
ー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列
Ｈ(ｋ)を用いて、以下の数式(１２７)により平滑誤差共
分散行列を算出し、ゲート判定部１０７で追尾目標の観
測ベクトルが得られなかったと判定された場合には、以
下の数式(１２８)に示すように、予測誤差共分散行列
(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共
分散行列の算出ステップＳ１２)。

【０２７０】

【数８５】

【０２７１】数式(１２７)及び数式(１２８)に従う平滑
誤差共分散行列の算出結果は、平滑誤差共分散行列用メ
モリ１２３に格納される。

【０２７２】以上で時刻ｔ(ｋ)における処理が終了す
る。以降、追尾終了が判定されるまで、上記一連の処理
ステップを繰り返し実行する(追尾終了ステップＳ１
３)。

【０２７３】図１２に、実施の形態１を用いた平滑ベク
トルの位置成分(平滑値)、予測観測ベクトル(ゲート中
心)、および観測ベクトル(観測値)の時間推移を示す。
図の実線は、状態ベクトルの位置成分の真の軌跡を表
し、点線は、ドップラシフトの影響を受けた観測ベクト
ルの真値を示す。上記の処理ステップにより、図１２の
ように、ドップラシフトを考慮しない観測モデルで構成
されたフィルタから、ドップラシフトを考慮した観測モ
デルを考慮したフィルタへ切り替えを行う際に、ゲート
の中心である予測観測ベクトルと観測ベクトルとの距離
が離れてしまう問題を改善するため、切り替え時に、ゲ
ート用予測観測ベクトルの算出においてドップラシフト
を考慮しない観測モデルを用いることにより、ゲート用
予測観測ベクトルと観測ベクトルとの距離が短くなる効
果がある。さらに、平滑ベクトルを算出する際の予測観
測ベクトルに、上記で算出したゲート用予測観測ベクト
ルと異なり、ドップラシフトを考慮した観測モデルを用
いることにより、残差ベクトルが大きくなり、切り替え
時における平滑ベクトルの過渡応答の収束速度が速くな
る効果がある。

【０２７４】なお、本実施の形態の考え方を用いれば、
ドップラシフトを考慮した観測モデルで構成されたフィ
ルタからドップラシフトを考慮しない観測モデルを用い
たフィルタへの切り替えに対する対応も可能であること
は明らかであるので、説明は省略する。

【０２７５】実施の形態５．図１３はこの発明の実施の
形態５による目標追尾装置を示すブロック構成図であ
り、また図１４はその処理動作を示すフローチャートで
ある。上記図１、図２と同様のものについては、詳述を
省略する。また、図１５はこの発明の実施の形態３にお
ける効果を示す概念図である。

【０２７６】図１３において、目標追尾装置１０ａは、
実施の形態１を示すブロック構成図である図１と同様
に、平滑ベクトル用メモリ１０５、予測ベクトル算出部
１０３、予測観測ベクトル算出部１０１、ゲート判定部
１０７、平滑ベクトル算出部１０９、残差共分散行列算
出部１１１、ゲイン行列算出部１１３、観測雑音共分散
行列設定部１１５、予測誤差共分散行列算出部１１７、
駆動雑音共分散行列設定部１１９、平滑誤差共分散行列
算出部１２１、平滑誤差共分散行列用メモリ１２３、を
備えている。

【０２７７】また、目標追尾装置１０ａは、上記構成要
素に加えて、切り替え制御部１２７、フィルタ用ドップ
ラシフト時定数算出部１３５、観測行列算出部１２５ｄ
(実施の形態１の図１におけるそれと動作が異なる)、を
備えている。

【０２７８】切り替え制御部１２７は、ドップラシフト
を考慮しない観測モデルにより構成されたフィルタから
ドップラシフトを考慮した観測モデルにより構成された
フィルタへの切り替えタイミングをフィルタ用ドップラ
シフト時定数算出部１３５に出力する。

【０２７９】フィルタ用ドップラシフト時定数算出部１
３５は、切り替え制御部１２７によるフィルタの切り替
え時刻を入力とし、フィルタに用いる観測モデルに設定
するドップラシフト時定数を算出し、観測行列算出部１
２５ｄへ出力する。

【０２８０】観測行列算出部１２５ｄは、フィルタ用ド
ップラシフト時定数算出部１３５からのドップラシフト
時定数を入力とし、予測ベクトル算出部１０３より算出
された予測値を用いて、ドップラシフトを考慮した観測
行列を算出する。

【０２８１】図１４において、実施の形態１の処理動作
を示すフローチャートである図２と同様に、初期値の設
定ステップＳ１、観測ベクトルの入力ステップＳ２、予
測ベクトルの算出ステップＳ３、駆動雑音共分散行列の
設定ステップＳ４、予測誤差共分散行列の算出ステップ
Ｓ５、予測観測ベクトルの算出ステップＳ６、観測雑音
共分散行列の設定ステップＳ７、残差共分散行列の算出
ステップＳ８、ゲート判定ステップＳ９、ゲイン行列の
算出ステップＳ１０、平滑ベクトルの算出ステップＳ１
１、平滑誤差共分散行列の算出ステップＳ１２、を備え
ている。

【０２８２】また、図１４は上記処理ステップに加え
て、フィルタ用ドップラシフト時定数の算出ステップＳ
５ｃ、観測行列算出ステップＳ５ａａａａａ(実施の形
態１の処理動作を示す図２のそれと処理が異なる)、が
挿入されている。

【０２８３】図１４において、フィルタ用ドップラシフ
ト時定数の算出ステップＳ５ｃは、切り替え制御部１２
７によるフィルタの切り替え時刻を入力とし、フィルタ
に用いる観測モデルに設定するドップラシフト時定数を
算出し、観測行列算出部１２５ｄへ出力するステップで
ある。

【０２８４】観測行列算出ステップＳ５ａａａａａは、
切り替え制御部１２７からのフィルタの切り替え時刻情
報を入力とし、ドップラシフトを考慮した観測モデルに
より構成されたフィルタに切り替わる時刻以後は、予測
ベクトル算出部１０３より算出された予測値を用いて、
観測行列を算出し、切り替わる以前は、ドップラシフト
を考慮しない観測モデルを出力するステップである。

【０２８５】以下、図１４を参照しながら、図１３に示
したこの発明の実施の形態５よる処理動作について詳細
に説明する。まず、平滑ベクトル用メモリ１０５と平滑
誤差共分散行列用メモリ１２３とに、それぞれ、平滑ベ
クトルの初期値(アンダーバー・ハット)ｘ(０)と平滑誤
差共分散行列の初期値Ｐ(０)とを設定し、追尾処理を開
始する(初期値の設定ステップＳ１)。ここで、状態ベク
トル(アンダーバー)ｘ(ｋ)は、北基準直交座標系で定義
される、数式(１２９)の６次元ベクトルである。

【０２８６】

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

【０２８７】

【数８７】

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

【０２８９】

【数８８】

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

【０２９１】

【数８９】

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

【０２９３】

【数９０】

【０２９４】ここで、ΔＴ(ｋ)は観測モデルを算出する
際に用いるドップラシフト時定数、ΔＴは所望のドップ
ラシフト時定数、ｋ_aはフィルタ切り替え時刻、ｋ_bは切
り替え開始後、観測モデル算出用の時定数を所望の値に
する時刻である。ｋ_bは、設計者が定めるパラメータで
ある。こうすることにより、予測観測ベクトルをはじめ
とする変数が、切り替え前と切り替え後で大きく変化す
る現象を防ぐことが可能である。

【０２９５】次に、観測行列算出部１２５ｄは、予測ベ
クトル算出部１０３から出力された予測ベクトル(上下
バー)ｘ(ｋ)及び、フィルタ用ドップラシフト時定数算
出部１３５から出力されたドップラシフト時定数ΔＴ
(ｋ)から、観測行列を算出する(観測行列の算出ステッ
プＳ５ａａａａａ)。

【０２９６】

【数９１】

【０２９７】

【数９２】

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

【０２９９】予測観測ベクトル算出部１０１は、予測ベ
クトル算出部１０３から入力された予測ベクトル(上下
バー)ｘ(ｋ)と観測行列算出部１２５ｄから入力された
観測行列を用いて、以下の数式(１４０)により予測観測
ベクトル(上下バー)ｚ(ｋ)を算出する(予測観測ベクト
ルの算出ステップＳ６)。

【０３００】

【数９３】

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

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

【０３０３】

【数９４】

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

【０３０５】

【数９５】

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

【０３０７】次に、ゲイン行列算出部１１３は、予測誤
差共分散行列(バー)Ｐ(ｋ)、残差共分散行列Ｓ(ｋ)、お
よび観測行列Ｈ(ｋ)を読み込み、以下の数式(１４４)に
したがって、フィルタゲインを算出する(ゲイン行列の
算出ステップＳ１０)。

【０３０８】

【数９６】

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

【０３１０】

【数９７】

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

【０３１２】

【数９８】

【０３１３】上記数式(１４５)または数式(１４６)によ
り、平滑ベクトル算出部１０９で算出された平滑ベクト
ル(アンダーバー・ハット)ｘ(ｋ)は、平滑ベクトル用メ
モリ１０５に格納されるとともに、航跡表示装置３０に
伝送され、運用者に対して表示される。

【０３１４】一方、平滑誤差共分散行列算出部１２１
は、ゲート判定部１０７で追尾目標の観測ベクトルが得
られたと判定された場合には、予測誤差共分散行列(バ
ー)Ｐ(ｋ)、観測雑音共分散行列Ｒ(ｋ)、及び観測行列
Ｈ(ｋ)を用いて、以下の数式(１４７)により平滑誤差共
分散行列を算出し、ゲート判定部１０７で追尾目標の観
測ベクトルが得られなかったと判定された場合には、以
下の数式(１４８)に示すように、予測誤差共分散行列
(バー)Ｐ(ｋ)を平滑誤差共分散行列とする(平滑誤差共
分散行列の算出ステップＳ１２)。

【０３１５】

【数９９】

【０３１６】数式(１４７)及び数式(１４８)に従う平滑
誤差共分散行列の算出結果は、平滑誤差共分散行列用メ
モリ１２３に格納される。

【０３１７】以上で時刻ｔ(ｋ)における処理が終了す
る。以降、追尾終了が判定されるまで、上記一連の処理
ステップを繰り返し実行する(追尾終了ステップＳ１
３)。

【０３１８】図１５に、実施の形態１を用いた平滑ベク
トルの位置成分(平滑値)、予測観測ベクトル(ゲート中
心)、および観測ベクトル(観測値)の時間推移を示す。
図の実線は、状態ベクトルの位置成分の真の軌跡を表
し、点線は、ドップラシフトの影響を受けた観測ベクト
ルの真値を示す。上記の処理ステップにより、図１５の
ように、ドップラシフトを考慮しない観測モデルで構成
されたフィルタから、ドップラシフトを考慮した観測モ
デルを考慮したフィルタへ切り替えを行う際に、ゲート
の中心である予測観測ベクトルと観測ベクトルとの距離
が離れてしまう問題を改善するため、切り替え時に、観
測モデルの用いるドップラシフト時定数を少しずつ変化
させることにより、ゲート用予測観測ベクトルと観測ベ
クトルとの距離が、切り替え直前と切り替え直後におい
て、急激に変化しなくなる効果がある。

【０３１９】なお、本実施の形態の考え方を用いれば、
ドップラシフトを考慮した観測モデルで構成されたフィ
ルタからドップラシフトを考慮しない観測モデルを用い
たフィルタへの切り替えに対する対応も可能であること
は明らかであるので、説明は省略する。

【０３２０】

【発明の効果】以上のようにこの発明によれば、目標観
測装置から入力される観測ベクトルに基づいて追尾目標
の航跡を算出するための目標追尾装置であって、平滑ベ
クトルの計算結果を格納するための平滑ベクトル用メモ
リと、平滑誤差共分散行列の計算結果を格納するための
平滑誤差共分散行列用メモリと、前時刻の平滑ベクトル
を用いて現時刻の予測ベクトルを算出する予測ベクトル
算出部と、駆動雑音共分散行列を設定する駆動雑音共分
散行列設定部と、前時刻の平滑誤差共分散行列の計算結
果と前記駆動雑音共分散行列の値とを用いて現時刻の予
測誤差共分散行列を算出する予測誤差共分散行列算出部
と、前記観測ベクトルの精度評価に関連したパラメータ
を用いて観測雑音共分散行列を設定する観測雑音共分散
行列設定部と、前記予測ベクトルを用いて現時刻の観測
行列を算出する観測行列算出部と、前記予測誤差共分散
行列、前記観測雑音共分散行列、及び前記観測行列を用
いて残差共分散行列を算出する残差共分散行列算出部
と、前記予測ベクトル及び前記観測行列から予測観測ベ
クトルを算出する予測観測ベクトル算出部と、前記予測
観測ベクトル及び前記観測ベクトルを用いて残差ベクト
ルを算出するとともに、前記残差共分散行列を用いて、
前記観測ベクトルが前記追尾目標の探知データであるか
否かのゲート判定を行うゲート判定部と、前記予測誤差
共分散行列、前記残差共分散行列、及び前記観測行列を
もちいてフィルタのゲイン行列を算出するゲイン行列算
出部と、前記残差ベクトル、前記予測ベクトルおよび前
記ゲイン行列をもちいて前記平滑ベクトルを算出する平
滑ベクトル算出部と、前記観測雑音共分散行列、前記予
測誤差共分散行列、及び前記観測行列とを用いて前記平
滑誤差共分散行列を算出する平滑誤差共分散行列算出部
と、を備えたことを特徴とする目標追尾装置およびこれ
に基づく目標追尾方法としたので、ドップラシフト補正
を考慮したフィルタを北基準直交座標系で構成するため
に、従来の技術で用いた極座標系の距離軸で表現した観
測モデルではなく、北基準直交座標系における観測モデ
ルを構成することにより、ドップラシフト補正を北基準
直交座標系で行うことが可能となる。

【０３２１】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、ドップラシフトを考慮しない観測モデルにより構成
されたフィルタからドップラシフトを考慮した観測モデ
ルにより構成されたフィルタへの切り替えタイミングを
出力する切り替え制御部と、切り替え制御部よりフィル
タの切り替え時刻情報を得、現時刻が切り替えを行う時
刻の場合、前記平滑ベクトル用メモリからの１時刻前に
おける平滑ベクトルを用いて、擬似平滑値を算出し、切
り替えを行う時刻でない場合、１時刻前の平滑値をその
まま出力する切り替え直前の擬似平滑値算出部と、前時
刻の平滑ベクトルを用いて現時刻の予測ベクトルを算出
する予測ベクトル算出部と、駆動雑音共分散行列を設定
する駆動雑音共分散行列設定部と、前時刻の平滑誤差共
分散行列の計算結果と前記駆動雑音共分散行列の値とを
用いて現時刻の予測誤差共分散行列を算出する予測誤差
共分散行列算出部と、前記観測ベクトルの精度評価に関
連したパラメータを用いて観測雑音共分散行列を設定す
る観測雑音共分散行列設定部と、前記切り替え制御部か
らのフィルタの切り替え時刻情報を入力とし、ドップラ
シフトを考慮した観測モデルにより構成されたフィルタ
に切り替わる時刻以後は、前記予測ベクトル算出部より
算出された予測値を用いて、ドップラシフトを考慮した
現時刻の観測行列を算出し、切り替わる時刻より前は、
ドップラシフトを考慮しない現時刻の観測モデルを算出
する観測行列算出部と、前記予測誤差共分散行列、前記
観測雑音共分散行列、及び前記観測行列を用いて残差共
分散行列を算出する残差共分散行列算出部と、前記予測
ベクトル及び前記観測行列を用いて、ゲート判定部で用
いる予測観測ベクトルを算出するゲート用予測観測ベク
トル算出部と、前記予測観測ベクトル及び前記観測ベク
トルを用いて残差ベクトルを算出するとともに、前記残
差共分散行列を用いて、前記観測ベクトルが前記追尾目
標の探知データであるか否かのゲート判定を行うゲート
判定部と、前記平滑ベクトル用メモリからの１時刻前に
おける平滑ベクトル及び観測行列算出部からの入力であ
る観測行列より、平滑ベクトル算出部で用いる予測観測
ベクトルを算出する平滑ベクトル算出用予測観測ベクト
ル算出部と、前記予測誤差共分散行列、前記残差共分散
行列、及び前記観測行列をもちいてフィルタのゲイン行
列を算出するゲイン行列算出部と、前記残差ベクトル、
前記予測ベクトルおよび前記ゲイン行列をもちいて前記
平滑ベクトルを算出する前記平滑ベクトル算出部と、前
記観測雑音共分散行列、前記予測誤差共分散行列、及び
前記観測行列とを用いて前記平滑誤差共分散行列を算出
する平滑誤差共分散行列算出部と、を備えたことを特徴
とする目標追尾装置およびこれに基づく目標追尾方法と
したので、ドップラシフトを考慮しない観測モデルで構
成されたフィルタから、ドップラシフトを考慮した観測
モデルを考慮したフィルタへ切り替えを行う際に、ゲー
トの中心である予測観測ベクトルと観測ベクトルとの距
離が離れてしまう問題を改善するため、切り替え時に、
前時刻の平滑ベクトルに対し、予めドップラ補正を施し
て擬似平滑ベクトルを算出することにより、擬似平滑ベ
クトルを用いて算出したゲート用予測観測ベクトルと観
測ベクトルとの距離が短くなる効果がある。さらに、平
滑ベクトルを算出する際の予測観測ベクトルに、上記で
算出したゲート用予測観測ベクトルと異なり、ドップラ
シフトの影響を受けた予測ベクトルから算出した予測観
測ベクトル値を用いることにより、残差ベクトルが大き
くなり、切り替え時における平滑ベクトルの過渡応答の
収束速度が速くなる効果がある。

【０３２２】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、前時刻の平滑ベクトルを用いて現時刻の予測ベクト
ルを算出する予測ベクトル算出部と、駆動雑音共分散行
列を設定する駆動雑音共分散行列設定部と、前時刻の平
滑誤差共分散行列の計算結果と前記駆動雑音共分散行列
の値とを用いて現時刻の予測誤差共分散行列を算出する
予測誤差共分散行列算出部と、前記観測ベクトルの精度
評価に関連したパラメータを用いて観測雑音共分散行列
を設定する観測雑音共分散行列設定部と、ドップラシフ
トを考慮しない観測モデルにより構成されたフィルタか
らドップラシフトを考慮した観測モデルにより構成され
たフィルタへの切り替えタイミングを出力する切り替え
制御部と、この切り替え制御部からのフィルタの切り替
え時刻情報を入力とし、ドップラシフトを考慮した観測
モデルにより構成されたフィルタに切り替わる時刻より
後は、前記予測値を用いて、ドップラシフトを考慮した
観測行列を算出し、後記ゲート用予測観測ベクトル算出
部、後記残差共分散行列算出部、後記平滑ベクトル算出
用予測観測ベクトル算出部、後記平滑誤差共分散行列算
出部、後記行列算出部、に出力し、切り替わる時刻より
前は、ドップラシフトを考慮しない観測モデルを算出
し、後記ゲート用予測観測ベクトル算出部、後記残差共
分散行列算出部、後記平滑ベクトル算出用予測観測ベク
トル算出部、後記平滑誤差共分散行列算出部、後記ゲイ
ン行列算出部、に出力し、さらに、切り替わる時刻に
は、後記ゲート用予測観測ベクトル算出部、後記残差共
分散行列算出部、にドップラシフトを考慮しない観測モ
デルを算出及び出力し、後記平滑ベクトル算出用予測観
測ベクトル算出部、後記平滑誤差共分散行列算出部、後
記ゲイン行列算出部にドップラシフトを考慮した観測行
列を算出及び出力する観測行列算出部と、前記予測誤差
共分散行列、前記観測雑音共分散行列、及び前記観測行
列を用いて残差共分散行列を算出する前記残差共分散行
列算出部と、前記予測ベクトル及び前記観測行列を用い
て、ゲート判定部で用いる予測観測ベクトルを算出する
前記ゲート用予測観測ベクトル算出部と、前記予測観測
ベクトル及び前記観測ベクトルを用いて残差ベクトルを
算出するとともに、前記残差共分散行列を用いて、前記
観測ベクトルが前記追尾目標の探知データであるか否か
のゲート判定を行う前記ゲート判定部と、前記予測ベク
トル及び前記観測行列より、後記平滑ベクトル算出部で
用いる予測観測ベクトルを算出する前記平滑ベクトル算
出用予測観測ベクトル算出部と、前記予測誤差共分散行
列、前記残差共分散行列、及び前記観測行列をもちいて
フィルタのゲイン行列を算出する前記ゲイン行列算出部
と、前記残差ベクトル、前記予測ベクトルおよび前記ゲ
イン行列をもちいて前記平滑ベクトルを算出する前記平
滑ベクトル算出部と、前記観測雑音共分散行列、前記予
測誤差共分散行列、及び前記観測行列とを用いて前記平
滑誤差共分散行列を算出する前記平滑誤差共分散行列算
出部と、を備えたことを特徴とする目標追尾装置および
これに基づく目標追尾方法としたので、ドップラシフト
を考慮しない観測モデルで構成されたフィルタから、ド
ップラシフトを考慮した観測モデルを考慮したフィルタ
へ切り替えを行う際に、ゲートの中心である予測観測ベ
クトルと観測ベクトルとの距離が離れてしまう問題を改
善するため、切り替え時に、ゲート用予測観測ベクトル
の算出においてドップラシフトを考慮しない観測モデル
を用いることにより、ゲート用予測観測ベクトルと観測
ベクトルとの距離が短くなる効果がある。さらに、平滑
ベクトルを算出する際の予測観測ベクトルに、上記で算
出したゲート用予測観測ベクトルと異なり、ドップラシ
フトを考慮した観測モデルを用いることにより、残差ベ
クトルが大きくなり、切り替え時における平滑ベクトル
の過渡応答の収束速度が速くなる効果がある。

【０３２３】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、前時刻の平滑ベクトルを用いて現時刻の予測ベクト
ルを算出する予測ベクトル算出部と、駆動雑音共分散行
列を設定する駆動雑音共分散行列設定部と、前時刻の平
滑誤差共分散行列の計算結果と前記駆動雑音共分散行列
の値とを用いて現時刻の予測誤差共分散行列を算出する
予測誤差共分散行列算出部と、前記観測ベクトルの精度
評価に関連したパラメータを用いて観測雑音共分散行列
を設定する観測雑音共分散行列設定部と、ドップラシフ
トを考慮しない観測モデルにより構成されたフィルタか
らドップラシフトを考慮した観測モデルにより構成され
たフィルタへの切り替えタイミングを出力する切り替え
制御部と、この切り替え制御部からのフィルタの切り替
え時刻情報を入力とし、ドップラシフトを考慮した観測
モデルにより構成されたフィルタに切り替わる時刻より
後は、前記予測値を用いて、ドップラシフトを考慮した
観測行列を算出し、後記ゲート用予測観測ベクトル算出
部、後記残差共分散行列算出部、後記平滑ベクトル算出
用予測観測ベクトル算出部、後記平滑誤差共分散行列算
出部、後記ゲイン行列算出部、に出力し、切り替わる時
刻より前は、ドップラシフトを考慮しない観測モデルを
算出し、後記ゲート用予測観測ベクトル算出部、後記残
差共分散行列算出部、後記平滑ベクトル算出用予測観測
ベクトル算出部、後記平滑ベクトル算出用予測観測ベク
トル算出部、後記平滑誤差共分散行列算出部、後記ゲイ
ン行列算出部、に出力し、切り替わる時刻には、後記ゲ
ート用予測観測ベクトル算出部、後記残差共分散行列算
出部、にドップラシフトを考慮しない観測モデルを算出
及び出力し、後記ゲイン行列算出部、にドップラシフト
を考慮した観測行列を算出及び出力する観測行列算出部
と、前記予測誤差共分散行列、前記観測雑音共分散行
列、及び前記観測行列を用いて残差共分散行列を算出す
る前記残差共分散行列算出部と、前記予測ベクトル及び
前記観測行列を用いて、ゲート判定部で用いる予測観測
ベクトルを算出する前記ゲート用予測観測ベクトル算出
部と、前記予測観測ベクトル及び前記観測ベクトルを用
いて残差ベクトルを算出するとともに、前記残差共分散
行列を用いて、前記観測ベクトルが前記追尾目標の探知
データであるか否かのゲート判定を行う前記ゲート判定
部と、ドップラシフトを考慮した観測モデルにより構成
されたフィルタに切り替わる時刻より前及び後は、予測
ベクトル算出部からの予測ベクトル及び観測行列算出部
からの入力である観測行列より、平滑ベクトル算出部で
用いる予測観測ベクトルを算出し、ドップラシフトを考
慮した観測モデルにより構成されたフィルタに切り替わ
る時刻では、予測ベクトル算出部からの予測ベクトルよ
り、平滑ベクトル算出部で用いる予測観測ベクトルを算
出する前記平滑ベクトル算出用予測観測ベクトル算出部
と、前記予測誤差共分散行列、前記残差共分散行列、及
び前記観測行列をもちいてフィルタのゲイン行列を算出
する前記ゲイン行列算出部と、前記残差ベクトル、前記
予測ベクトルおよび前記ゲイン行列をもちいて前記平滑
ベクトルを算出する前記平滑ベクトル算出部と、前記観
測雑音共分散行列、前記予測誤差共分散行列、及び前記
観測行列とを用いて前記平滑誤差共分散行列を算出する
前記平滑誤差共分散行列算出部と、を備えたことを特徴
とする目標追尾装置およびこれに基づく目標追尾方法と
したので、ドップラシフトを考慮しない観測モデルで構
成されたフィルタから、ドップラシフトを考慮した観測
モデルを考慮したフィルタへ切り替えを行う際に、ゲー
トの中心である予測観測ベクトルと観測ベクトルとの距
離が離れてしまう問題を改善するため、切り替え時に、
ゲート用予測観測ベクトルの算出においてドップラシフ
トを考慮しない観測モデルを用いることにより、ゲート
用予測観測ベクトルと観測ベクトルとの距離が短くなる
効果がある。さらに、平滑ベクトルを算出する際の予測
観測ベクトルに、上記で算出したゲート用予測観測ベク
トルと異なり、ドップラシフトを考慮した観測モデルを
用いることにより、残差ベクトルが大きくなり、切り替
え時における平滑ベクトルの過渡応答の収束速度が速く
なる効果がある。

【０３２４】また、目標観測装置から入力される観測ベ
クトルに基づいて追尾目標の航跡を算出するための目標
追尾装置であって、平滑ベクトルの計算結果を格納する
ための平滑ベクトル用メモリと、平滑誤差共分散行列の
計算結果を格納するための平滑誤差共分散行列用メモリ
と、ドップラシフトを考慮しない観測モデルにより構成
されたフィルタからドップラシフトを考慮した観測モデ
ルにより構成されたフィルタへの切り替えタイミングを
出力する切り替え制御部と、前時刻の平滑ベクトルを用
いて現時刻の予測ベクトルを算出する予測ベクトル算出
部と、駆動雑音共分散行列を設定する駆動雑音共分散行
列設定部と、前時刻の平滑誤差共分散行列の計算結果と
前記駆動雑音共分散行列の値とを用いて現時刻の予測誤
差共分散行列を算出する予測誤差共分散行列算出部と、
前記観測ベクトルの精度評価に関連したパラメータを用
いて観測雑音共分散行列を設定する観測雑音共分散行列
設定部と、前記予測誤差共分散行列、前記観測雑音共分
散行列、及び前記観測行列を用いて残差共分散行列を算
出する残差共分散行列算出部と、前記切り替え制御部に
よるフィルタの切り替え時刻を入力とし、フィルタに用
いる観測モデルに設定するドップラシフト時定数を算出
し、後記観測行列算出部へ出力するフィルタ用ドップラ
シフト時定数算出部と、前記フィルタ用ドップラシフト
時定数算出部からのドップラシフト時定数を入力とし、
前記予測値を用いて、ドップラシフトを考慮した観測行
列を算出する観測行列算出部と、前記予測ベクトル及び
前記観測行列から予測観測ベクトルを算出する予測観測
ベクトル算出部と、前記予測観測ベクトル及び前記観測
ベクトルを用いて残差ベクトルを算出するとともに、前
記残差共分散行列を用いて、前記観測ベクトルが前記追
尾目標の探知データであるか否かのゲート判定を行うゲ
ート判定部と、前記予測誤差共分散行列、前記残差共分
散行列、及び前記観測行列をもちいてフィルタのゲイン
行列を算出するゲイン行列算出部と、前記残差ベクト
ル、前記予測ベクトルおよび前記ゲイン行列をもちいて
前記平滑ベクトルを算出する平滑ベクトル算出部と、前
記観測雑音共分散行列、前記予測誤差共分散行列、及び
前記観測行列とを用いて前記平滑誤差共分散行列を算出
する平滑誤差共分散行列算出部と、を備えたことを特徴
とする目標追尾装置およびこれに基づく目標追尾方法と
したので、ドップラシフトを考慮しない観測モデルで構
成されたフィルタから、ドップラシフトを考慮した観測
モデルを考慮したフィルタへ切り替えを行う際に、ゲー
トの中心である予測観測ベクトルと観測ベクトルとの距
離が離れてしまう問題を改善するため、切り替え時に、
観測モデルの用いるドップラシフト時定数を少しずつ変
化させることにより、ゲート用予測観測ベクトルと観測
ベクトルとの距離が、切り替え直前と切り替え直後にお
いて、急激に変化しなくなる効果がある。

【図面の簡単な説明】

【図１】 この発明の実施の形態１による目標追尾装置
を示すブロック構成図である。

【図２】 この発明の実施の形態１による目標追尾装置
の処理動作を示すフローチャートである。

【図３】 この発明の実施の形態１における効果を示す
概念図である。

【図４】 この発明の実施の形態２による目標追尾装置
を示すブロック構成図である。

【図５】 この発明の実施の形態２による目標追尾装置
の処理動作を示すフローチャートである。

【図６】 この発明の実施の形態２における効果を示す
概念図である。

【図７】 この発明の実施の形態３による目標追尾装置
を示すブロック構成図である。

【図８】 この発明の実施の形態３による目標追尾装置
の処理動作を示すフローチャートである。

【図９】 この発明の実施の形態３における効果を示す
概念図である。

【図１０】 この発明の実施の形態４による目標追尾装
置を示すブロック構成図である。

【図１１】 この発明の実施の形態４による目標追尾装
置の処理動作を示すフローチャートである。

【図１２】 この発明の実施の形態４における効果を示
す概念図である。

【図１３】 この発明の実施の形態５による目標追尾装
置を示すブロック構成図である。

【図１４】 この発明の実施の形態５による目標追尾装
置の処理動作を示すフローチャートである。

【図１５】 この発明の実施の形態５における効果を示
す概念図である。

【図１６】 この発明の実施の形態５におけるフィルタ
用ドップラシフト時定数を説明するための図である。

【図１７】 従来のこの種の目標追尾装置を示すブロッ
ク構成図である。

【図１８】 従来の目標追尾装置の処理動作を示すフロ
ーチャートである。

【図１９】 この発明の実施の形態５における動作を説
明するための概念図である。

【図２０】 北基準直交座標と極座標の定義を説明する
ための図である。

【図２１】 従来の技術におけるフィルタの切り替えを
説明するための概念図である。

【符号の説明】

１０ａ 目標追尾装置、２０ 目標観測装置、３０ 航
跡表示装置、１０１予測観測ベクトル算出部、１０３
予測ベクトル算出部、１０５ 平滑ベクトル用メモリ、
１０７ ゲート判定部、１０９ 平滑ベクトル算出部、
１１１ 残差共分散行列算出部、１１３ ゲイン行列算
出部、１１５ 観測雑音共分散行列設定部、１１７ 予
測誤差共分散行列算出部、１１９ 駆動雑音共分散行列
設定部、１２１ 平滑誤差共分散行列算出部、１２３
平滑誤差共分散行列用メモリ、１２５，１２５ａ，１２
５ｂ，１２５ｃ，１２５ｄ 観測行列算出部、１２７切
り替え制御部、１２９ ゲート用予測観測ベクトル算出
部、１３１，１３１ａ，１３１ｂ 平滑ベクトル算出用
予測観測ベクトル算出部、１３３ 切り替え直前の擬似
平滑値算出部、１３５ フィルタ用ドップラシフト時定
数算出部。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 小菅 義夫 東京都千代田区丸の内二丁目２番３号 三 菱電機株式会社内 Ｆターム(参考） 5J070 AB08 AC01 AE02 AE04 AH19 AK14 AK22 BA01 BB06

Claims (10)

【特許請求の範囲】
1. 【請求項１】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   装置であって、 平滑ベクトルの計算結果を格納するための平滑ベクトル
   用メモリと、 平滑誤差共分散行列の計算結果を格納するための平滑誤
   差共分散行列用メモリと、 前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを
   算出する予測ベクトル算出部と、 駆動雑音共分散行列を設定する駆動雑音共分散行列設定
   部と、 前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音
   共分散行列の値とを用いて現時刻の予測誤差共分散行列
   を算出する予測誤差共分散行列算出部と、 前記観測ベクトルの精度評価に関連したパラメータを用
   いて観測雑音共分散行列を設定する観測雑音共分散行列
   設定部と、 前記予測ベクトルを用いて現時刻の観測行列を算出する
   観測行列算出部と、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列を用いて残差共分散行列を算出する残差
   共分散行列算出部と、 前記予測ベクトル及び前記観測行列から予測観測ベクト
   ルを算出する予測観測ベクトル算出部と、 前記予測観測ベクトル及び前記観測ベクトルを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが前記追尾目標の探知データ
   であるか否かのゲート判定を行うゲート判定部と、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列をもちいてフィルタのゲイン行列を算出する
   ゲイン行列算出部と、 前記残差ベクトル、前記予測ベクトルおよび前記ゲイン
   行列をもちいて前記平滑ベクトルを算出する平滑ベクト
   ル算出部と、 前記観測雑音共分散行列、前記予測誤差共分散行列、及
   び前記観測行列とを用いて前記平滑誤差共分散行列を算
   出する平滑誤差共分散行列算出部と、 を備えたことを特徴とする目標追尾装置。
2. 【請求項２】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   装置であって、 平滑ベクトルの計算結果を格納するための平滑ベクトル
   用メモリと、 平滑誤差共分散行列の計算結果を格納するための平滑誤
   差共分散行列用メモリと、 ドップラシフトを考慮しない観測モデルにより構成され
   たフィルタからドップラシフトを考慮した観測モデルに
   より構成されたフィルタへの切り替えタイミングを出力
   する切り替え制御部と、 切り替え制御部よりフィルタの切り替え時刻情報を得、
   現時刻が切り替えを行う時刻の場合、前記平滑ベクトル
   用メモリからの１時刻前における平滑ベクトルを用い
   て、擬似平滑値を算出し、切り替えを行う時刻でない場
   合、１時刻前の平滑値をそのまま出力する切り替え直前
   の擬似平滑値算出部と、 前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを
   算出する予測ベクトル算出部と、 駆動雑音共分散行列を設定する駆動雑音共分散行列設定
   部と、 前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音
   共分散行列の値とを用いて現時刻の予測誤差共分散行列
   を算出する予測誤差共分散行列算出部と、 前記観測ベクトルの精度評価に関連したパラメータを用
   いて観測雑音共分散行列を設定する観測雑音共分散行列
   設定部と、 前記切り替え制御部からのフィルタの切り替え時刻情報
   を入力とし、ドップラシフトを考慮した観測モデルによ
   り構成されたフィルタに切り替わる時刻以後は、前記予
   測ベクトル算出部より算出された予測値を用いて、ドッ
   プラシフトを考慮した現時刻の観測行列を算出し、切り
   替わる時刻より前は、ドップラシフトを考慮しない現時
   刻の観測モデルを算出する観測行列算出部と、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列を用いて残差共分散行列を算出する残差
   共分散行列算出部と、 前記予測ベクトル及び前記観測行列を用いて、ゲート判
   定部で用いる予測観測ベクトルを算出するゲート用予測
   観測ベクトル算出部と、 前記予測観測ベクトル及び前記観測ベクトルを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが前記追尾目標の探知データ
   であるか否かのゲート判定を行うゲート判定部と、 前記平滑ベクトル用メモリからの１時刻前における平滑
   ベクトル及び観測行列算出部からの入力である観測行列
   より、平滑ベクトル算出部で用いる予測観測ベクトルを
   算出する平滑ベクトル算出用予測観測ベクトル算出部
   と、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列をもちいてフィルタのゲイン行列を算出する
   ゲイン行列算出部と、 前記残差ベクトル、前記予測ベクトルおよび前記ゲイン
   行列をもちいて前記平滑ベクトルを算出する前記平滑ベ
   クトル算出部と、 前記観測雑音共分散行列、前記予測誤差共分散行列、及
   び前記観測行列とを用いて前記平滑誤差共分散行列を算
   出する平滑誤差共分散行列算出部と、 を備えたことを特徴とする目標追尾装置。
3. 【請求項３】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   装置であって、 平滑ベクトルの計算結果を格納するための平滑ベクトル
   用メモリと、 平滑誤差共分散行列の計算結果を格納するための平滑誤
   差共分散行列用メモリと、 前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを
   算出する予測ベクトル算出部と、 駆動雑音共分散行列を設定する駆動雑音共分散行列設定
   部と、 前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音
   共分散行列の値とを用いて現時刻の予測誤差共分散行列
   を算出する予測誤差共分散行列算出部と、 前記観測ベクトルの精度評価に関連したパラメータを用
   いて観測雑音共分散行列を設定する観測雑音共分散行列
   設定部と、 ドップラシフトを考慮しない観測モデルにより構成され
   たフィルタからドップラシフトを考慮した観測モデルに
   より構成されたフィルタへの切り替えタイミングを出力
   する切り替え制御部と、 この切り替え制御部からのフィルタの切り替え時刻情報
   を入力とし、ドップラシフトを考慮した観測モデルによ
   り構成されたフィルタに切り替わる時刻より後は、前記
   予測値を用いて、ドップラシフトを考慮した観測行列を
   算出し、後記ゲート用予測観測ベクトル算出部、後記残
   差共分散行列算出部、後記平滑ベクトル算出用予測観測
   ベクトル算出部、後記平滑誤差共分散行列算出部、後記
   行列算出部、に出力し、切り替わる時刻より前は、ドッ
   プラシフトを考慮しない観測モデルを算出し、後記ゲー
   ト用予測観測ベクトル算出部、後記残差共分散行列算出
   部、後記平滑ベクトル算出用予測観測ベクトル算出部、
   後記平滑誤差共分散行列算出部、後記ゲイン行列算出
   部、に出力し、さらに、切り替わる時刻には、後記ゲー
   ト用予測観測ベクトル算出部、後記残差共分散行列算出
   部、にドップラシフトを考慮しない観測モデルを算出及
   び出力し、後記平滑ベクトル算出用予測観測ベクトル算
   出部、後記平滑誤差共分散行列算出部、後記ゲイン行列
   算出部にドップラシフトを考慮した観測行列を算出及び
   出力する観測行列算出部と、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列を用いて残差共分散行列を算出する前記
   残差共分散行列算出部と、 前記予測ベクトル及び前記観測行列を用いて、ゲート判
   定部で用いる予測観測ベクトルを算出する前記ゲート用
   予測観測ベクトル算出部と、 前記予測観測ベクトル及び前記観測ベクトルを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが前記追尾目標の探知データ
   であるか否かのゲート判定を行う前記ゲート判定部と、 前記予測ベクトル及び前記観測行列より、後記平滑ベク
   トル算出部で用いる予測観測ベクトルを算出する前記平
   滑ベクトル算出用予測観測ベクトル算出部と、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列をもちいてフィルタのゲイン行列を算出する
   前記ゲイン行列算出部と、 前記残差ベクトル、前記予測ベクトルおよび前記ゲイン
   行列をもちいて前記平滑ベクトルを算出する前記平滑ベ
   クトル算出部と、 前記観測雑音共分散行列、前記予測誤差共分散行列、及
   び前記観測行列とを用いて前記平滑誤差共分散行列を算
   出する前記平滑誤差共分散行列算出部と、 を備えたことを特徴とする目標追尾装置。
4. 【請求項４】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   装置であって、 平滑ベクトルの計算結果を格納するための平滑ベクトル
   用メモリと、 平滑誤差共分散行列の計算結果を格納するための平滑誤
   差共分散行列用メモリと、 前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを
   算出する予測ベクトル算出部と、 駆動雑音共分散行列を設定する駆動雑音共分散行列設定
   部と、 前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音
   共分散行列の値とを用いて現時刻の予測誤差共分散行列
   を算出する予測誤差共分散行列算出部と、 前記観測ベクトルの精度評価に関連したパラメータを用
   いて観測雑音共分散行列を設定する観測雑音共分散行列
   設定部と、 ドップラシフトを考慮しない観測モデルにより構成され
   たフィルタからドップラシフトを考慮した観測モデルに
   より構成されたフィルタへの切り替えタイミングを出力
   する切り替え制御部と、 この切り替え制御部からのフィルタの切り替え時刻情報
   を入力とし、ドップラシフトを考慮した観測モデルによ
   り構成されたフィルタに切り替わる時刻より後は、前記
   予測値を用いて、ドップラシフトを考慮した観測行列を
   算出し、後記ゲート用予測観測ベクトル算出部、後記残
   差共分散行列算出部、後記平滑ベクトル算出用予測観測
   ベクトル算出部、後記平滑誤差共分散行列算出部、後記
   ゲイン行列算出部、に出力し、切り替わる時刻より前
   は、ドップラシフトを考慮しない観測モデルを算出し、
   後記ゲート用予測観測ベクトル算出部、後記残差共分散
   行列算出部、後記平滑ベクトル算出用予測観測ベクトル
   算出部、後記平滑ベクトル算出用予測観測ベクトル算出
   部、後記平滑誤差共分散行列算出部、後記ゲイン行列算
   出部、に出力し、切り替わる時刻には、後記ゲート用予
   測観測ベクトル算出部、後記残差共分散行列算出部、に
   ドップラシフトを考慮しない観測モデルを算出及び出力
   し、後記ゲイン行列算出部、にドップラシフトを考慮し
   た観測行列を算出及び出力する観測行列算出部と、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列を用いて残差共分散行列を算出する前記
   残差共分散行列算出部と、 前記予測ベクトル及び前記観測行列を用いて、ゲート判
   定部で用いる予測観測ベクトルを算出する前記ゲート用
   予測観測ベクトル算出部と、 前記予測観測ベクトル及び前記観測ベクトルを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが前記追尾目標の探知データ
   であるか否かのゲート判定を行う前記ゲート判定部と、 ドップラシフトを考慮した観測モデルにより構成された
   フィルタに切り替わる時刻より前及び後は、予測ベクト
   ル算出部からの予測ベクトル及び観測行列算出部からの
   入力である観測行列より、平滑ベクトル算出部で用いる
   予測観測ベクトルを算出し、ドップラシフトを考慮した
   観測モデルにより構成されたフィルタに切り替わる時刻
   では、予測ベクトル算出部からの予測ベクトルより、平
   滑ベクトル算出部で用いる予測観測ベクトルを算出する
   前記平滑ベクトル算出用予測観測ベクトル算出部と、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列をもちいてフィルタのゲイン行列を算出する
   前記ゲイン行列算出部と、 前記残差ベクトル、前記予測ベクトルおよび前記ゲイン
   行列をもちいて前記平滑ベクトルを算出する前記平滑ベ
   クトル算出部と、 前記観測雑音共分散行列、前記予測誤差共分散行列、及
   び前記観測行列とを用いて前記平滑誤差共分散行列を算
   出する前記平滑誤差共分散行列算出部と、 を備えたことを特徴とする目標追尾装置。
5. 【請求項５】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   装置であって、 平滑ベクトルの計算結果を格納するための平滑ベクトル
   用メモリと、 平滑誤差共分散行列の計算結果を格納するための平滑誤
   差共分散行列用メモリと、 ドップラシフトを考慮しない観測モデルにより構成され
   たフィルタからドップラシフトを考慮した観測モデルに
   より構成されたフィルタへの切り替えタイミングを出力
   する切り替え制御部と、 前時刻の平滑ベクトルを用いて現時刻の予測ベクトルを
   算出する予測ベクトル算出部と、 駆動雑音共分散行列を設定する駆動雑音共分散行列設定
   部と、 前時刻の平滑誤差共分散行列の計算結果と前記駆動雑音
   共分散行列の値とを用いて現時刻の予測誤差共分散行列
   を算出する予測誤差共分散行列算出部と、 前記観測ベクトルの精度評価に関連したパラメータを用
   いて観測雑音共分散行列を設定する観測雑音共分散行列
   設定部と、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列を用いて残差共分散行列を算出する残差
   共分散行列算出部と、 前記切り替え制御部によるフィルタの切り替え時刻を入
   力とし、フィルタに用いる観測モデルに設定するドップ
   ラシフト時定数を算出し、後記観測行列算出部へ出力す
   るフィルタ用ドップラシフト時定数算出部と、 前記フィルタ用ドップラシフト時定数算出部からのドッ
   プラシフト時定数を入力とし、前記予測値を用いて、ド
   ップラシフトを考慮した観測行列を算出する観測行列算
   出部と、 前記予測ベクトル及び前記観測行列から予測観測ベクト
   ルを算出する予測観測ベクトル算出部と、 前記予測観測ベクトル及び前記観測ベクトルを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが前記追尾目標の探知データ
   であるか否かのゲート判定を行うゲート判定部と、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列をもちいてフィルタのゲイン行列を算出する
   ゲイン行列算出部と、 前記残差ベクトル、前記予測ベクトルおよび前記ゲイン
   行列をもちいて前記平滑ベクトルを算出する平滑ベクト
   ル算出部と、 前記観測雑音共分散行列、前記予測誤差共分散行列、及
   び前記観測行列とを用いて前記平滑誤差共分散行列を算
   出する平滑誤差共分散行列算出部と、 を備えたことを特徴とする目標追尾装置。
6. 【請求項６】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   方法であって、 平滑ベクトルと平滑誤差共分散行列の初期値を設定する
   初期値の設定ステップと、 前記目標観測装置から現時刻の観測ベクトルを読み込む
   観測ベクトルの入力ステップと、 前時刻の平滑ベクトルから予測ベクトルを算出する予測
   ベクトルの算出ステップと、 駆動雑音共分散行列を設定する駆動雑音共分散行列の設
   定ステップと、 前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列
   とから予測誤差共分散行列を算出する予測誤差共分散行
   列の算出ステップと、 前記予測ベクトルから観測行列を算出する観測行列の算
   出ステップと、 前記予測ベクトル及び前記観測行列から予測観測ベクト
   ルを算出する予測観測ベクトルの算出ステップと、 前記観測ベクトルの精度評価に関連したパラメータに基
   づいて観測雑音共分散行列を設定する観測雑音共分散行
   列の設定ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて残差共分散行列を算出する残
   差共分散行列の算出ステップと、 前記予測観測ベクトルと前記観測ベクトルとを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが追尾目標の探知データであ
   るか否かのゲート判定を行うゲート判定ステップと、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列とを用いて、フィルタゲイン行列を算出する
   ゲイン行列の算出ステップと、 前記ゲート判定で追尾目標の探知データと判定された観
   測ベクトル、前記予測ベクトル、前記予測観測ベクト
   ル、及び前記ゲイン行列を用いて、平滑ベクトルを算出
   する平滑ベクトルの算出ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて、前記平滑誤差共分散行列を
   算出する平滑誤差共分散行列の算出ステップと、 追尾処理終了か否かを判定して、追尾処理を続行する場
   合には、前記観測ベクトルの入力ステップに戻る追尾終
   了判定ステップと、 を備えたことを特徴とする目標追尾方法。
7. 【請求項７】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   方法であって、 平滑ベクトルと平滑誤差共分散行列の初期値を設定する
   初期値の設定ステップと、 前記目標観測装置から現時刻の観測ベクトルを読み込む
   観測ベクトルの入力ステップと、 現時刻が切り替えを行う時刻の場合、１時刻前における
   平滑ベクトルを用いて、擬似平滑値を算出し、現時刻が
   切り替えを行う時刻でない場合、１時刻前における後記
   平滑ベクトルをそのまま出力とする切り替え直前の擬似
   平滑値の算出ステップと、 前時刻の後記平滑ベクトルから予測ベクトルを算出する
   予測ベクトルの算出ステップと、 駆動雑音共分散行列を設定する駆動雑音共分散行列の設
   定ステップと、 前時刻の後記平滑誤差共分散行列と前記駆動雑音共分散
   行列とから予測誤差共分散行列を算出する予測誤差共分
   散行列の算出ステップと、 ドップラシフトを考慮した観測モデルにより構成された
   フィルタに切り替わる時刻以後は、前記予測値を用い
   て、観測行列を算出し、切り替わる以前は、ドップラシ
   フトを考慮しない観測行列を出力する観測行列算出ステ
   ップと、 前記予測ベクトルと、前記観測行列を用いて、ゲート判
   定に用いる予測観測ベクトルを算出するゲート用予測観
   測ベクトルの算出ステップと、 前記観測ベクトルの精度評価に関連したパラメータに基
   づいて観測雑音共分散行列を設定する観測雑音共分散行
   列の設定ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて残差共分散行列を算出する残
   差共分散行列の算出ステップと、 前記予測観測ベクトルと前記観測ベクトルとを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが追尾目標の探知データであ
   るか否かのゲート判定を行うゲート判定ステップと、 １時刻前における後記平滑ベクトルと、前記観測行列を
   用いて、後記平滑ベクトルの算出に用いる予測観測ベク
   トルを算出する平滑ベクトル算出用予測観測ベクトルの
   算出ステップと、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列とを用いて、フィルタゲイン行列を算出する
   ゲイン行列の算出ステップと、 ゲート判定で追尾目標の探知データと判定された前記観
   測ベクトル、前記予測ベクトル、前記予測観測ベクト
   ル、及び前記ゲイン行列を用いて、平滑ベクトルを算出
   する平滑ベクトルの算出ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて、平滑誤差共分散行列を算出
   する平滑誤差共分散行列の算出ステップと、 追尾処理終了か否かを判定して、追尾処理を続行する場
   合には、前記観測ベクトルの入力ステップに戻る追尾終
   了判定ステップと、 を備えたことを特徴とする目標追尾方法。
8. 【請求項８】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   方法であって、 後記平滑ベクトルと後記平滑誤差共分散行列の初期値を
   設定する初期値の設定ステップと、 前記目標観測装置から現時刻の観測ベクトルを読み込む
   観測ベクトルの入力ステップと、 前時刻の後記平滑ベクトルから予測ベクトルを算出する
   予測ベクトルの算出ステップと、 駆動雑音共分散行列を設定する駆動雑音共分散行列の設
   定ステップと、 前時刻の後記平滑誤差共分散行列と前記駆動雑音共分散
   行列とから予測誤差共分散行列を算出する予測誤差共分
   散行列の算出ステップと、 フィルタの切り替え時刻情報を入力とし、ドップラシフ
   トを考慮した観測モデルにより構成されたフィルタに切
   り替わる時刻より後は、後記予測値を用いて、ドップラ
   シフトを考慮した観測行列を算出し、ゲート用予測観測
   ベクトル算出ステップ、残差共分散行列算出ステップ、
   平滑ベクトル算出用予測観測ベクトル算出ステップ、平
   滑誤差共分散行列算出ステップ、ゲイン行列算出ステッ
   プ、に出力し、切り替わる時刻より前は、ドップラシフ
   トを考慮しない観測モデルを算出し、ゲート用予測観測
   ベクトル算出ステップ、残差共分散行列算出ステップ、
   平滑ベクトル算出用予測観測ベクトル算出ステップ、平
   滑誤差共分散行列算出ステップ、ゲイン行列算出ステッ
   プ、に出力し、さらに、切り替わる時刻には、ゲート用
   予測観測ベクトル算出ステップ、残差共分散行列算出ス
   テップ、にドップラシフトを考慮しない観測モデルを算
   出及び出力し、平滑ベクトル算出用予測観測ベクトル算
   出ステップ、平滑誤差共分散行列算出ステップ、ゲイン
   行列算出ステップに、ドップラシフトを考慮した観測行
   列を算出及び出力する観測行列算出ステップと、 前記予測ベクトルと、前記観測行列を用いて、ゲート判
   定に用いる予測観測ベクトルを算出するゲート用予測観
   測ベクトルの算出ステップと、 前記観測ベクトルの精度評価に関連したパラメータに基
   づいて観測雑音共分散行列を設定する観測雑音共分散行
   列の設定ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて残差共分散行列を算出する残
   差共分散行列の算出ステップと、 前記予測観測ベクトルと前記観測ベクトルとを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが追尾目標の探知データであ
   るか否かのゲート判定を行うゲート判定ステップと、 前記予測ベクトルと、前記観測行列を用いて、後記平滑
   ベクトルの算出に用いる予測観測ベクトルを算出する平
   滑ベクトル算出用予測観測ベクトルの算出ステップと、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列とを用いて、フィルタゲイン行列を算出する
   ゲイン行列の算出ステップと、 ゲート判定で追尾目標の探知データと判定された前記観
   測ベクトル、前記予測ベクトル、前記予測観測ベクト
   ル、及び前記ゲイン行列を用いて、平滑ベクトルを算出
   する平滑ベクトルの算出ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて、平滑誤差共分散行列を算出
   する平滑誤差共分散行列の算出ステップと、 追尾処理終了か否かを判定して、追尾処理を続行する場
   合には、前記観測ベクトルの入力ステップに戻る追尾終
   了判定ステップと、 を備えたことを特徴とする目標追尾方法。
9. 【請求項９】 目標観測装置から入力される観測ベクト
   ルに基づいて追尾目標の航跡を算出するための目標追尾
   方法であって、 後記平滑ベクトルと後記平滑誤差共分散行列の初期値を
   設定する初期値の設定ステップと、 前記目標観測装置から現時刻の観測ベクトルを読み込む
   観測ベクトルの入力ステップと、 前時刻の後記平滑ベクトルから予測ベクトルを算出する
   予測ベクトルの算出ステップと、 駆動雑音共分散行列を設定する駆動雑音共分散行列の設
   定ステップと、 前時刻の後記平滑誤差共分散行列と前記駆動雑音共分散
   行列とから予測誤差共分散行列を算出する予測誤差共分
   散行列の算出ステップと、 フィルタの切り替え時刻情報を入力とし、ドップラシフ
   トを考慮した観測モデルにより構成されたフィルタに切
   り替わる時刻より後は、前記予測値を用いて、ドップラ
   シフトを考慮した観測行列を算出し、後記ゲート用予測
   観測ベクトル算出ステップ、後記残差共分散行列算出ス
   テップ、後記平滑ベクトル算出用予測観測ベクトル算出
   ステップ、後記平滑誤差共分散行列算出ステップ、後記
   ゲイン行列算出ステップ、に出力し、切り替わる時刻よ
   り前は、ドップラシフトを考慮しない観測モデルを算出
   し、後記ゲート用予測観測ベクトル算出ステップ、後記
   残差共分散行列算出ステップ、後記平滑ベクトル算出用
   予測観測ベクトル算出ステップ、後記平滑ベクトル算出
   用予測観測ベクトル算出ステップ、後記平滑誤差共分散
   行列算出ステップ、後記ゲイン行列算出ステップ、に出
   力し、切り替わる時刻には、後記ゲート用予測観測ベク
   トル算出ステップ、後記残差共分散行列算出ステップ、
   にドップラシフトを考慮しない観測モデルを算出及び出
   力し、後記ゲイン行列算出ステップ、にドップラシフト
   を考慮した観測行列を算出及び出力する観測行列算出ス
   テップと、 前記予測ベクトルと、前記観測行列を用いて、ゲート判
   定に用いる予測観測ベクトルを算出するゲート用予測観
   測ベクトルの算出ステップと、 前記観測ベクトルの精度評価に関連したパラメータに基
   づいて観測雑音共分散行列を設定する観測雑音共分散行
   列の設定ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて残差共分散行列を算出する残
   差共分散行列の算出ステップと、 前記予測観測ベクトルと前記観測ベクトルとを用いて残
   差ベクトルを算出するとともに、前記残差共分散行列を
   用いて、前記観測ベクトルが追尾目標の探知データであ
   るか否かのゲート判定を行うゲート判定ステップと、 ドップラシフトを考慮した観測モデルにより構成された
   フィルタに切り替わる時刻より前及び後は、前記予測ベ
   クトル及び前記観測行列より、後記平滑ベクトル算出ス
   テップで用いる予測観測ベクトルを算出し、ドップラシ
   フトを考慮した観測モデルにより構成されたフィルタに
   切り替わる時刻では、前記予測ベクトルより、後記平滑
   ベクトル算出ステップで用いる予測観測ベクトルを算出
   する平滑ベクトル算出用予測観測ベクトルの算出ステッ
   プと、 前記予測誤差共分散行列、前記残差共分散行列、及び前
   記観測行列とを用いて、フィルタゲイン行列を算出する
   ゲイン行列の算出ステップと、 ゲート判定で追尾目標の探知データと判定された前記観
   測ベクトル、前記予測ベクトル、前記予測観測ベクト
   ル、及び前記ゲイン行列を用いて、平滑ベクトルを算出
   する平滑ベクトルの算出ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
   び前記観測行列とを用いて、平滑誤差共分散行列を算出
   する平滑誤差共分散行列の算出ステップと、 追尾処理終了か否かを判定して、追尾処理を続行する場
   合には、前記観測ベクトルの入力ステップに戻る追尾終
   了判定ステップと、 を備えたことを特徴とする目標追尾方法。
10. 【請求項１０】 目標観測装置から入力される観測ベク
    トルに基づいて追尾目標の航跡を算出するための目標追
    尾方法であって、 後記平滑ベクトルと後記平滑誤差共分散行列の初期値を
    設定する初期値の設定ステップと、 前記目標観測装置から現時刻の観測ベクトルを読み込む
    観測ベクトルの入力ステップと、 前時刻の平滑ベクトルから予測ベクトルを算出する予測
    ベクトルの算出ステップと、 駆動雑音共分散行列を設定する駆動雑音共分散行列の設
    定ステップと、 前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列
    とから予測誤差共分散行列を算出する予測誤差共分散行
    列の算出ステップと、 フィルタの切り替え時刻を入力とし、フィルタに用いる
    観測モデルに設定するドップラシフト時定数を算出する
    フィルタ用ドップラシフト時定数の算出ステップと、 フィルタの切り替え時刻情報を入力とし、ドップラシフ
    トを考慮した観測モデルにより構成されたフィルタに切
    り替わる時刻以後は、前記予測値を用いて、観測行列を
    算出し、切り替わる以前は、ドップラシフトを考慮しな
    い観測モデルを出力する観測行列算出ステップと、 前記予測ベクトル及び前記観測行列から予測観測ベクト
    ルを算出する予測観測ベクトルの算出ステップと、 前記観測ベクトルの精度評価に関連したパラメータに基
    づいて観測雑音共分散行列を設定する観測雑音共分散行
    列の設定ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
    び前記観測行列とを用いて残差共分散行列を算出する残
    差共分散行列の算出ステップと、 前記予測観測ベクトルと前記観測ベクトルとを用いて残
    差ベクトルを算出するとともに、前記残差共分散行列を
    用いて、前記観測ベクトルが追尾目標の探知データであ
    るか否かのゲート判定を行うゲート判定ステップと、 前記予測誤差共分散行列、前記残差共分散行列、及び前
    記観測行列とを用いて、フィルタゲイン行列を算出する
    ゲイン行列の算出ステップと、 ゲート判定で追尾目標の探知データと判定された観測ベ
    クトル、前記予測ベクトル、前記予測観測ベクトル、及
    び前記ゲイン行列を用いて、平滑ベクトルを算出する平
    滑ベクトルの算出ステップと、 前記予測誤差共分散行列、前記観測雑音共分散行列、及
    び前記観測行列とを用いて、前記平滑誤差共分散行列を
    算出する平滑誤差共分散行列の算出ステップと、 追尾処理終了か否かを判定して、追尾処理を続行する場
    合には、前記観測ベクトルの入力ステップに戻る追尾終
    了判定ステップと、 を備えたことを特徴とする目標追尾方法。