JP2003329771A

JP2003329771A - 追尾装置および追尾処理方法

Info

Publication number: JP2003329771A
Application number: JP2002142031A
Authority: JP
Inventors: Takashi Matsuzaki; 貴史松崎; Masayoshi Ito; 正義系; Yoshio Kosuge; 義夫小菅
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2002-05-16
Filing date: 2002-05-16
Publication date: 2003-11-19

Abstract

(57)【要約】【課題】着弾点予測の初期値の精度を向上させ、さら
に、着弾点予測を精度良く行う追尾装置を得る。【解決手段】第２の遅延手段１０、第２の予測手段１
１、および第２の平滑手段１２により、平滑ベクトルお
よび共分散行列を過去Ｌ時刻分遡って計算することによ
って、着弾点予測の初期値の精度が向上する。また、運
動座標への座標変換行列算出手段８、および運動座標で
の駆動雑音設定手段９により、駆動雑音ベクトルの共分
散行列を運動座標で定義を行い、それを北基準直交座標
に変換して、駆動雑音ベクトルの共分散行列を与えるこ
とにより、初期値の水平方向のふらつきが抑えられる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、目標の着弾点予
測を行う際の初期値の精度を向上させる追尾装置および
追尾処理方法に関するものである。

【０００２】図３０は例えば特開平８−１７８５９５号
公報に示された従来の追尾装置を示す構成図であり、図
において、１は観測手段、２は第１の遅延手段、３は第
１の予測手段、４は第１の平滑手段、５は第１の着弾点
予測手段、６は表示手段、７は北基準直交座標での駆動
雑音設定手段である。

【０００３】次に動作について説明する。図２は観測手
段に関する座標系を示す説明図であり、図において、Ｏ
はセンサ、Ｔは追尾目標、Ｒは追尾目標ＴとセンサＯと
の間の距離、Ｅ１はセンサＯと追尾目標Ｔとを結ぶ線分
ＯＴがＸ−Ｙ平面となす仰角、ＡｚはセンサＯと追尾目
標Ｔとを結ぶ線分ＯＴのＸ−Ｙ平面への正射影ベクトル
がＸ軸となす方位角である。さらに、［Ｒ，Ｅ１，Ａ
ｚ］は極座標を表し、［Ｘ，Ｙ，Ｚ］は北基準直交座標
を表す。観測手段１では、レーダにより、目標からの位
置を極座標で受信する。この時の目標の観測位置は、距
離、仰角、方位角の真値にそれぞれ観測雑音が乗ってい
る。観測手段１では、観測雑音が乗った距離、仰角、方
位角の目標の観測位置を、式（１）により、座標変換を
行い、北基準直交座標の観測位置へと座標変換を行う。
そして、北基準直交座標における目標の観測位置、およ
び距離、仰角、方位角の観測雑音の標準偏差を第１の平
滑手段４へそれぞれ入力する。ここで、北基準直交座標
における目標の観測位置は、特に断りがなければ、以
降、北基準直交座標における目標の観測位置ベクトルと
呼ぶ。

【数１】

【０００４】また、観測手段１、第１の遅延手段２、第
１の予測手段３、第１の平滑手段４、北基準直交座標で
の駆動雑音設定手段７の処理において、カルマンフィル
タを使用する。カルマンフィルタの使用の際の運動モデ
ルは、式（２）を使用する。

【数２】式（２）において、Ｘ（ｋ）はサンプリング時刻ｋの時
の北基準直交座標における位置、速度、加速度等を表す
状態変数ベクトルであり、Φ（ｋ）は状態遷移行列、ｗ
（ｋ）は目標の運動の曖昧さを表す駆動雑音ベクトルで
ある。カルマンフィルタの使用の際の観測モデルは、式
（３）を使用する。

【数３】式（３）において、ｚ（ｋ）はサンプリング時刻ｋにお
ける北基準直交座標における位置を表す観測ベクトルで
あり、Ｈ（ｋ）は状態変数ベクトルＸ（ｋ）から、位置
成分を抽出する観測行列であり、ｖ（ｋ）は北基準直交
座標で定義される観測雑音ベクトルである。以降、特に
断りがなければ、時刻ｋ＋１における予測ベクトルをｘ
（ｋ＋１｜ｋ）と表し、時刻ｋにおける平滑ベクトルを
ｘ（ｋ｜ｋ）と表し、時刻ｋ＋１における予測誤差共分
散行列をＰ（ｋ＋１｜ｋ）と表し、時刻ｋにおける平滑
誤差共分散行列をＰ（ｋ｜ｋ）と表し、時刻ｋにおける
ゲイン行列をＫ（ｋ）と表し、時刻ｋにおける駆動雑音
ベクトルの共分散行列をＱ（ｋ）と表し、時刻ｋにおけ
る観測誤差共分散行列をＲ（ｋ）と表し、行列およびベ
クトルの右上添字「Ｔ」は、行列およびベクトルの転置
を表すこととする。また、行列の右上添字「−１」は、
行列の逆行列を表すこととする。

【０００５】第１の遅延手段２では、後述の第１の平滑
手段４から入力される時刻ｋにおける平滑ベクトルと平
滑誤差共分散行列を１サンプリング分遅延し、時刻ｋ−
１における平滑ベクトルと平滑誤差共分散行列を第１の
予測手段３に出力する。第１の予測手段３では、第１の
遅延手段２から入力される時刻ｋ−１における平滑ベク
トルと平滑誤差共分散行列とを用い、さらに、後述で説
明する北基準直交座標での駆動雑音設定手段７から出力
される駆動雑音ベクトルの共分散行列を用いて、１サン
プリング分外挿を行った、時刻ｋにおける予測ベクトル
と予測誤差共分散行列とを算出する。さらに、時刻ｋに
おける予測ベクトルと予測誤差共分散行列とを第１の平
滑手段４に出力する。予測ベクトルの算出は、式（４）
により算出し、予測誤差共分散行列の算出は、式（５）
により算出する。

【数４】

【数５】第１の平滑手段４では、第１の予測手段３から出力され
る時刻ｋにおける予測ベクトルと予測誤差共分散行列と
を用いて、時刻ｋにおけるゲイン行列と平滑ベクトルと
平滑誤差共分散行列とを算出する。ゲイン行列は、式
（６）により算出を行い、平滑ベクトルは、式（７）に
より算出を行い、平滑誤差共分散行列は、式（８）によ
り算出を行う。

【数６】

【数７】

【数８】

【０００６】第１の着弾点予測手段５では、第１の平滑
手段４から出力される時刻ｋにおける平滑ベクトルと平
滑誤差共分散行列とを用いて、事前に登録してあるデー
タベースにより、着弾点計算を行い、その着弾点計算の
出力結果である、位置、速度等を持つ予測ベクトル、予
測誤差共分散行列を表示手段６へ出力する。表示手段６
では、予測ベクトルを用いて、予測位置の表示を行い、
予測誤差共分散行列を用いて、着弾点の範囲の表示を行
う。北基準直交座標での駆動雑音設定手段７では、図２
の北基準直交座標で定義される駆動雑音ベクトルの共分
散行列を事前に設定し、その駆動雑音ベクトルの共分散
行列を第１の予測手段３に出力する。

【０００７】図５は目標の真航跡と現時刻の平滑値を初
期値として着弾点予測を行った予測航跡とを示す関係図
であり、従来の追尾装置の説明を行うために、この図５
を用いる。ここで、黒塗りの丸印は各時刻の真値、白抜
きの丸印は各時刻の平滑値、白抜きの三角印は再計算し
た平滑値である。白抜きの三角印である、再計算した平
滑値は、後述の実施の形態の説明の際に使用する。ここ
で、Ｓ１［ｋ］は、サンプリング時刻ｋの時の平滑値、
Ｓ２［ｋ］は、サンプリング時刻ｋの時の再計算した平
滑値、Ｔ［ｋ］は、サンプリング時刻ｋの時の真値であ
る。また、実線は真航跡、一点鎖線は着弾点予測の計算
を行う際、現時刻の平滑値を初期値とした予測航跡、点
線は着弾点予測の計算を行う際、再計算した平滑値を初
期値とした予測航跡である。再計算した平滑値について
の説明は、後述の実施の形態の説明の際に使用する。こ
こで、以降、特に断りがなければ、サンプリング時刻ｋ
を現時刻として説明を行う。図５において、従来の追尾
装置では、目標を追跡している初期時間帯の場合、現時
刻の目標平滑位置および現時刻の平滑速度の誤差が大き
い。そのため、例えば、Ｓ１［ｋ］の平滑位置および速
度を使用して、Ｎ時刻後の目標の予測着弾位置を計算す
る場合、実際の真着弾位置Ｔ［ｋ＋Ｎ］と予測着弾位置
Ｓ１［ｋ＋Ｎ］のばらつきは大きくなってしまうといっ
た課題がある。

【０００８】図６は駆動雑音を大きく設定したときと小
さく設定したときの初期値のふらつきが予測着弾点に及
ぼす影響を示す関係図であり、図６において、点線は速
度ベクトルに対し直交する水平軸方向の駆動雑音設定を
大きくした場合の初期値のふらつきと、そのときの着弾
予測を行ったときの予測航跡を表し、太実線は速度ベク
トルに対し直交する水平軸方向の駆動雑音設定を小さく
した場合の初期値のふらつきと、そのときの着弾予測を
行ったときの予測航跡をそれぞれ表す。細実線は目標の
真航跡を表す。図６において、速度ベクトルに対し直交
する水平軸方向の駆動雑音設定を大きくした場合、速度
ベクトルに対し直交する水平軸方向の駆動雑音設定を小
さくした場合に比べ、着弾予測位置が大きくふらつく。
ここで、第１の予測手段３で設定する駆動雑音ベクトル
の共分散行列は、目標の速度が高速かつ非線形な運動を
行う影響で、式（２）で定義される運動モデルが目標の
実際の運動と合っていないと判断されるか事前に既知の
場合、目標への追従性を高めるため、駆動雑音ベクトル
の共分散行列を大きめに設定する必要がある。しかしな
がら、従来の追尾装置では、駆動雑音ベクトルの共分散
行列が北基準直交座標で定義されているため、駆動雑音
ベクトルの共分散行列を大きめに設定すると、その駆動
雑音ベクトルの共分散行列を、速度ベクトルの方向と、
速度ベクトルに対し直交する水平軸方向、速度ベクトル
に対し直交する鉛直軸方向の３方向に分解した時、速度
ベクトルに対し直交する水平軸方向の駆動雑音ベクトル
が大きいため、着弾点の予測に使用する初期値の誤差が
大きくなってしまう。そのため、予測着弾点の誤差が特
に速度ベクトル直交する水平方向において大きくなって
しまうといった課題がある。

【０００９】

【発明が解決しようとする課題】従来の追尾装置は以上
のように構成されているので、現時刻の平滑値を初期値
として予測着弾位置を計算する場合、ばらつきは大きく
なってしまうといった課題があった。また、駆動雑音ベ
クトルの共分散行列が北基準直交座標で定義されている
ため、駆動雑音ベクトルの共分散行列を大きめに設定す
ると、予測着弾点の誤差が特に速度ベクトル直交する水
平方向において大きくなってしまうといった課題があっ
た。

【００１０】この発明は上記のような課題を解決するた
めになされたもので、着弾点予測の初期値の精度を向上
させ、さらに、着弾点予測を精度良く行う追尾装置およ
び追尾処理方法を得ることを目的とする。

【００１１】

【課題を解決するための手段】この発明に係る追尾装置
は、第１の遅延手段からの１サンプリング前の平滑ベク
トルの速度が北基準直交座標において、水平面と鉛直面
でなす角を算出して、運動座標から北基準直交座標への
座標変換行列を算出し、出力する運動座標への座標変換
行列算出手段と、運動座標において駆動雑音ベクトルの
共分散行列を設定する運動座標での駆動雑音設定手段
と、現時刻までの観測値を用いて、現時刻より数サンプ
リング前の平滑ベクトルおよび平滑誤差共分散行列を再
計算したものを１サンプリング遅延させ出力する第２の
遅延手段と、再計算された平滑誤差共分散行列を用い
て、１サンプリング分外挿を行う第２の予測手段と、現
時刻までの観測値を用いて、過去数サンプリング遡っ
て、平滑ベクトルおよび平滑誤差共分散行列を算出し、
第１の着弾点予測手段に出力する第２の平滑手段とを備
えたものである。

【００１２】この発明に係る追尾装置は、第１の着弾点
予測手段からの予測ベクトルおよび予測誤差共分散行列
を、ある一定サンプリング分保持しておき、その一定サ
ンプリングの間の予測ベクトルおよび予測誤差共分散行
列について加重平均を行い、その加重平均した予測ベク
トルおよび加重平均した予測誤差共分散行列を表示手段
に出力する第１の予測諸元平均手段を備えたものであ
る。

【００１３】この発明に係る追尾装置は、第１の着弾点
予測手段からの予測ベクトルおよび予測誤差共分散行列
を、ある一定サンプリング分保持しておき、その一定サ
ンプリングの間の予測ベクトルおよび予測誤差共分散行
列について、予測誤差共分散行列のノルムにより、重み
づけ平均を行い、その重みづけ平均した予測ベクトルお
よび重みづけ平均した予測誤差共分散行列を表示手段に
出力する第２の予測諸元平均手段を備えたものである。

【００１４】この発明に係る追尾装置は、第１の着弾点
予測手段からの予測ベクトルおよび予測誤差共分散行列
を、ある一定サンプリング分保持しておき、その一定サ
ンプリングの間の予測ベクトルおよび予測誤差共分散行
列について、着弾点における予測誤差共分散行列を水平
面上に射影した面積を重みとして、重みづけ平均を行
い、その重みづけ平均した予測ベクトルおよび重みづけ
平均した予測誤差共分散行列を表示手段に出力する第３
の予測諸元平均手段を備えたものである。

【００１５】この発明に係る追尾装置は、観測開始時刻
からある事前に決めた時刻よりも一定時間が経過した数
サンプリング後に着弾点予測を開始する制御信号を第１
の着弾点予測手段に出力する第１の着弾点予測開始制御
手段を備えたものである。

【００１６】この発明に係る追尾装置は、平滑誤差共分
散行列のノルムが事前に決めたある定数より小さくなっ
た時、着弾点予測を開始する制御信号を第１の着弾点予
測手段に出力する第２の着弾点予測開始制御手段を備え
たものである。

【００１７】この発明に係る追尾装置は、観測手段から
得られる現時刻と１サンプリング前の時刻との間のサン
プリング間隔がある事前に決められた閾値より小さい場
合、運動座標における駆動雑音ベクトルの共分散行列を
小さく設定するように運動座標での駆動雑音設定手段を
制御する第１の駆動雑音制御手段を備えたものである。

【００１８】この発明に係る追尾装置は、第１の遅延手
段から得られる１サンプリング前の速度ベクトルの大き
さがある事前に決められた閾値より大きい場合、運動座
標における駆動雑音ベクトルの共分散行列の速度ベクト
ル方向の成分を大きくするように運動座標での駆動雑音
設定手段を制御する第２の駆動雑音制御手段を備えたも
のである。

【００１９】この発明に係る追尾装置は、第１の遅延手
段から得られる１サンプリング前の速度ベクトルの大き
さがある事前に決められた閾値より大きい場合が、過去
数回連続で起こった場合、運動座標における駆動雑音ベ
クトルの共分散行列の速度ベクトル方向の成分を大きく
するように運動座標での駆動雑音設定手段を制御する第
３の駆動雑音制御手段を備えたものである。

【００２０】この発明に係る追尾装置は、第１の着弾点
予測手段からの予測ベクトルおよび予測誤差共分散行列
を、制御理論に基づく１次のフィルタ処理により、表示
用の予測ベクトルおよび表示用の予測誤差共分散行列に
変換し、表示手段に出力する第３の平滑手段を備えたも
のである。

【００２１】この発明に係る追尾装置は、運動座標への
座標変換行列算出手段における運動座標から北基準直交
座標への座標変換行列を算出する際に、その座標変換行
列に必要な第１の遅延手段から出力される１サンプリン
グ前の平滑ベクトルの速度が、北基準直交座標における
水平面となす角と、その平滑ベクトルが鉛直面となす角
とを、それぞれ１サンプリング前の平滑ベクトルの位置
から求まる仰角と方位角とに置き換える制御信号をユー
ザーの判断により運動座標への座標変換行列算出手段に
出力する距離基準の座標変換行列算出制御手段を備えた
ものである。

【００２２】この発明に係る追尾装置は、運動座標への
座標変換行列算出手段における運動座標から北基準直交
座標への座標変換行列を算出する際に、その座標変換行
列に必要な第１の遅延手段から出力される１サンプリン
グ前の平滑ベクトルの速度が、北基準直交座標における
水平面となす角と、その平滑ベクトルが鉛直面となす角
とを、それぞれ過去数サンプリング分の平滑ベクトルか
ら算出される推定面から求まる法線ベクトルが、北基準
直交座標における水平面となす角と、その法線ベクトル
が鉛直面とそれぞれなす角とにそれぞれ置き換える制御
信号をユーザーの判断により運動座標への座標変換行列
算出手段に出力する推定面基準の座標変換行列算出制御
手段を備えたものである。

【００２３】この発明に係る追尾処理方法は、運動座標
における駆動雑音ベクトルの共分散行列を設定する共分
散行列設定工程と、運動座標から北基準直交座標への座
標変換行列を平滑ベクトルの速度成分により算出する座
標変換行列算出工程と、現時刻までの観測値、平滑ベク
トルおよび平滑誤差共分散行列を用いて、事前に決めら
れた時刻より前の平滑ベクトルおよび平滑誤差共分散行
列を再算出する第２の平滑算出工程とを備えたものであ
る。

【００２４】

【発明の実施の形態】以下、この発明の実施の一形態を
説明する。実施の形態１．図１はこの発明の実施の形態１による追
尾装置を示す構成図であり、図において、１は観測手
段、２は第１の遅延手段、３は第１の予測手段、４は第
１の平滑手段、５は第１の着弾点予測手段、６は表示手
段、８は運動座標への座標変換行列算出手段、９は運動
座標での駆動雑音設定手段、１０は第２の遅延手段、１
１は第２の予測手段、１２は第２の平滑手段である。

【００２５】次に動作について説明する。図２は観測手
段に関する座標系を示す説明図であり、図において、Ｏ
はセンサ、Ｔは追尾目標、Ｒは追尾目標ＴとセンサＯと
の間の距離、Ｅ１はセンサＯと追尾目標Ｔとを結ぶ線分
ＯＴがＸ−Ｙ平面となす仰角、ＡｚはセンサＯと追尾目
標Ｔとを結ぶ線分ＯＴのＸ−Ｙ平面への正射影ベクトル
がＸ軸となす方位角である。さらに、［Ｒ，Ｅ１，Ａ
ｚ］は極座標を表し、［Ｘ，Ｙ，Ｚ］は北基準直交座標
を表す。観測手段１では、レーダにより、目標からの位
置を極座標で受信する。この時の目標の観測位置は、距
離、仰角、方位角の真値にそれぞれ観測雑音が乗ってい
る。観測手段１では、観測雑音が乗った距離、仰角、方
位角の目標の観測位置を、式（１）により、座標変換を
行い、北基準直交座標の観測位置へと座標変換を行う。
そして、北基準直交座標における目標の観測位置、およ
び距離、仰角、方位角の観測雑音の標準偏差を第１の平
滑手段４へそれぞれ入力する。ここで、北基準直交座標
における目標の観測位置は、特に断りがなければ、以
降、北基準直交座標における目標の観測位置ベクトルと
呼ぶ。

【数９】

【００２６】また、観測手段１、第１の遅延手段２、第
１の予測手段３、第１の平滑手段４、運動座標への座標
変換行列算出手段８、運動座標での駆動雑音設定手段
９、第２の遅延手段１０、第２の予測手段１１、第２の
平滑手段１２の処理において、カルマンフィルタを使用
する。カルマンフィルタの使用の際の運動モデルは、式
（２）を使用する。

【数１０】式（２）において、Ｘ（ｋ）はサンプリング時刻ｋの時
の北基準直交座標における位置、速度、加速度等を表す
状態変数ベクトルであり、Φ（ｋ）は状態遷移行列、ｗ
（ｋ）は目標の運動の曖昧さを表す駆動雑音ベクトルで
ある。カルマンフィルタの使用の際の観測モデルは、式
（３）を使用する。

【数１１】式（３）において、ｚ（ｋ）はサンプリング時刻ｋにお
ける北基準直交座標における位置を表す観測ベクトルで
あり、Ｈ（ｋ）は状態変数ベクトルＸ（ｋ）から、位置
成分を抽出する観測行列であり、ｖ（ｋ）は北基準直交
座標で定義される観測雑音ベクトルである。以降、特に
断りがなければ、時刻ｋ＋１における予測ベクトルをｘ
（ｋ＋１｜ｋ）と表し、時刻ｋにおける平滑ベクトルを
ｘ（ｋ｜ｋ）と表し、時刻ｋ＋１における予測誤差共分
散行列をＰ（ｋ＋１｜ｋ）と表し、時刻ｋにおける平滑
誤差共分散行列をＰ（ｋ｜ｋ）と表し、時刻ｋにおける
ゲイン行列をＫ（ｋ）と表し、時刻ｋにおける駆動雑音
ベクトルの共分散行列をＱ（ｋ）と表し、時刻ｋにおけ
る観測誤差共分散行列をＲ（ｋ）と表し、行列およびベ
クトルの右上添字「Ｔ」は、行列およびベクトルの転置
を表すこととする。また、行列の右上添字「−１」は、
行列の逆行列を表すこととする。

【００２７】第１の遅延手段２では、後述の第１の平滑
手段４から入力される時刻ｋにおける平滑ベクトルと平
滑誤差共分散行列を１サンプリング分遅延し、時刻ｋ−
１における平滑ベクトルと平滑誤差共分散行列を第１の
予測手段３に出力する。第１の予測手段３では、第１の
遅延手段２から入力される時刻ｋ−１における平滑ベク
トルと平滑誤差共分散行列とを用い、さらに、後述で説
明する運動座標での駆動雑音設定手段９から出力される
駆動雑音ベクトルの共分散行列を用いて、１サンプリン
グ分外挿を行った、時刻ｋにおける予測ベクトルと予測
誤差共分散行列とを算出する。さらに、時刻ｋにおける
予測ベクトルと予測誤差共分散行列とを第１の平滑手段
４に出力する。予測ベクトルの算出は、式（４）により
算出し、予測誤差共分散行列の算出は、式（５）により
算出する。

【数１２】

【数１３】第１の平滑手段４では、第１の予測手段３から出力され
る時刻ｋにおける予測ベクトルと予測誤差共分散行列と
を用いて、時刻ｋにおけるゲイン行列と平滑ベクトルと
平滑誤差共分散行列とを算出する。ゲイン行列は、式
（６）により算出を行い、平滑ベクトルは、式（７）に
より算出を行い、平滑誤差共分散行列は、式（８）によ
り算出を行う。

【数１４】

【数１５】

【数１６】

【００２８】次に、運動座標への座標変換行列算出手段
８を説明するために、北基準直交座標と運動座標の関係
を説明する。図３は北基準直交座標を示す説明図であ
り、図において、ＯＴは目標の速度ベクトルを表し、
α，βは北基準直交座標における速度ベクトルのなす角
を表す目標姿勢角である。図４は運動座標を示す説明図
であり、図において、運動座標は、追尾目標の中心を原
点Ｔ、追尾目標の速度ベクトル方向をＵ軸の正、水平面
に平行な面内で速度ベクトルに垂直で右手方向をＶ軸の
正、速度ベクトルに垂直で下向きをＷ軸の正とした座標
である。北基準直交座標における［ｘ，ｙ，ｚ］^Ｔと運
動座標における［ｕ，ｖ，ｗ］ ^Ｔの間の関係式は、式
（９）、式（１０）である。式（９）における座標変換
行列Ｆは、運動座標［ｕ，ｖ，ｗ］^Ｔから、北基準直交
座標［ｘ，ｙ，ｚ］^Ｔへの座標変換行列を表す。ここ
で、座標変換行列Ｆは、式（１１）により算出される。
式（１１）におけるα，βは、速度ベクトルの向きから
算出される。

【数１７】

【数１８】

【数１９】したがって、運動座標への座標変換行列算出手段８で
は、第１の遅延手段２から入力される平滑ベクトルを使
用して、平滑ベクトルの速度成分が北基準直交座標にお
いてなす角α，βを算出して、式（１１）の関係によ
り、平滑ベクトルが持つ時刻と同時刻を持つ、運動座標
から北基準直交座標への座標変換行列を算出し、それを
後述の運動座標での駆動雑音設定手段９に出力する。

【００２９】次に、運動座標での駆動雑音設定手段９に
ついて説明する。ここで、運動座標での駆動雑音設定手
段９は、式（１２）の運動モデルを基に設定を行う。式
（１２）における、ｗ２（ｋ−１）はサンプリング時刻
ｋ−１の時の運動座標における駆動雑音ベクトルであ
る。また、サンプリング時刻ｋの時の運動座標における
駆動雑音ベクトルの共分散行列をＱ２（ｋ）と定義す
る。式（１２）における、Ｆ（ｋ−１）は座標変換行列
Ｆの時刻ｋ−１の時を表し、時刻ｋ−１の時の平滑ベク
トルの速度成分から算出を行う。

【数２０】したがって、運動座標での駆動雑音設定手段９では、ま
ず、運動座標における駆動雑音ベクトルの共分散行列Ｑ
２（ｋ）の成分である、運動座標におけるＶ軸成分を、
Ｕ軸およびＷ軸よりも小さめに設定を行う。次に、式
（１１）の算出式により算出したＦ（ｋ）を用いて、Ｖ
軸成分を小さめに設定した運動座標における駆動雑音ベ
クトルの共分散行列Ｑ２（ｋ）、式（１３）の右辺のよ
うに、北基準直交座標に座標変換を行い、式（１３）に
おける駆動雑音ベクトルの共分散行列Ｑ（ｋ）を後述の
第１の予測手段３に出力する。

【数２１】

【００３０】次に、第２の平滑手段１２について説明す
る。第２の平滑手段１２では、平滑ベクトルの再計算を
行う回数Ｌを事前に決めておく。そして、各時刻ごと
に、Ｌ回分、式（１４）、式（１５）、式（１６）の処
理を行う。但し、Ｌは正の整数である。ここで、式（１
４）において、ｘ（ｋ−ｊ｜ｋ）は、サンプリング時刻
ｋにおけるｊサンプリング前の再計算した平滑ベクトル
であり、式（１５）において、Ｋ（ｋ，ｊ）は、サンプ
リング時刻ｋにおけるｊサンプリング前の再計算したゲ
イン行列であり、式（１６）において、Ｐ（ｋ−ｊ，ｋ
｜ｋ）は、サンプリング時刻ｋにおけるｊサンプリング
前の再計算した共分散行列である。また、ｊは１からＬ
の間の値である。さらに、式（１６）に関連して、例え
ば、式（１７）の関係が成立する。したがって、第２の
平滑手段１２では、Ｌサンプリング前の再計算した平滑
ベクトルおよびＬサンプリング前の再計算した共分散行
列を第１の着弾点予測手段５に出力する。また、サンプ
リング時刻ｋにおける、ｊサンプリング前（ｊ＝１，・
・・，Ｌ）の再計算した平滑ベクトルおよびサンプリン
グ時刻ｋにおけるｊサンプリング前（ｊ＝１，・・・，
Ｌ）の再計算した共分散行列を後述の第２の遅延手段１
０に出力する。例えば、ｊ＝２の時を考えて、ｘ（ｋ−
１｜ｋ），ｘ（ｋ−２｜ｋ）およびＰ（ｋ−１，ｋ｜
ｋ），Ｐ（ｋ−２，ｋ｜ｋ）を後述の第２の遅延手段１
０に出力する。

【数２２】

【数２３】

【数２４】

【数２５】

【００３１】第２の遅延手段１０では、前述の第２の平
滑手段１２により得られたサンプリング時刻ｋにおけ
る、ｊサンプリング前（ｊ＝１，・・・，Ｌ）の再計算
した平滑ベクトル、およびサンプリング時刻ｋにおける
ｊサンプリング前（ｊ＝１，・・・，Ｌ）の再計算した
共分散行列が入力され、それらを１サンプリング遅延さ
せ、第２の予測手段１１へ出力する。例えば、ｘ（ｋ−
ｊ｜ｋ）が入力された場合、ｘ（ｋ−ｊ｜ｋ−１）を出
力し、Ｐ（ｋ，ｋ−ｊ｜ｋ）が入力された場合、Ｐ（ｋ
−１，ｋ−ｊ｜ｋ−１）が出力される。

【００３２】第２の予測手段１１では、第２の遅延手段
１０から得られた再計算した共分散行列を用いて、式
（１８）の算出式により、１サンプリング外挿を行った
共分散行列を第２の平滑手段１２に出力する。

【数２６】

【００３３】第１の着弾点予測手段５では、第２の平滑
手段１２から出力されるＬサンプリング前の再計算した
平滑ベクトルおよびＬサンプリング前の再計算した平滑
誤差共分散行列を用いて、目標の推定軌道角、目標の推
定高度を算出し、事前に用意した、各機種毎の目標軌道
算出用パラメータである軌道角、高度、運動特性を示す
推力のパラメータおよび空力パラメータからなるデータ
ベースから、推定軌道角、推定高度を検索キーとして、
目標軌道のパラメータを決定し、これらのパラメータを
用いて、各機種毎の目標の運動方程式と飛行時間とを考
慮して、事前に登録してあるデータベースにより、着弾
点計算を行い、その着弾点計算の出力結果である、位
置、速度等を持つ予測ベクトル、予測誤差共分散行列を
表示手段６へ出力する。表示手段６では、予測ベクトル
を用いて、予測位置の表示を行い、予測誤差共分散行列
を用いて、着弾点の範囲の表示を行う。

【００３４】したがって、従来の追尾装置では、図５の
一点鎖線のように、着弾点予測を行う際の初期値の誤差
が大きいため、真の着弾点に比べ、誤差が大きいといっ
た課題があり、また、図６の点線のように、目標の真航
跡の軌道面に対し、駆動雑音の調整ができないため、水
平方向の予測着弾点の真の着弾点との誤差が大きくなっ
てしまうといった課題があった。しかし、式（１４）〜
式（１６）のように、平滑ベクトルおよび共分散行列を
過去Ｌ時刻分遡って計算することによって、着弾点予測
の初期値の精度が向上する。また、駆動雑音ベクトルの
共分散行列を運動座標で定義を行い、それを北基準直交
座標に変換して、駆動雑音ベクトルの共分散行列を式
（１３）のように、与えることにより、初期値の水平方
向のふらつきが抑えられる。

【００３５】図７はこの発明の実施の形態１による追尾
処理方法を示すフローチャートであり、まず、運動座標
への座標変換行列算出手段８および運動座標での駆動雑
音設定手段９において、運動座標による駆動雑音ベクト
ルの共分散行列の設定を行い（ステップＳＴ１：共分散
行列設定工程）、運動座標から北基準直交座標への座標
変換行列を平滑ベクトルの速度成分より算出を行う（ス
テップＳＴ２：座標変換行列算出工程）。次に、第１の
予測手段３において、ステップＳＴ１において設定され
た運動座標による駆動雑音ベクトルの共分散行列を、ス
テップＳＴ２において算出された運動座標から北基準直
交座標への座標変換行列を用いて、式（１３）のように
運動座標による駆動雑音ベクトルの共分散行列を北基準
直交座標へ座標変換を行い、式（４）、式（５）を用い
て、予測ベクトルおよび予測誤差共分散行列を算出する
（ステップＳＴ３：予測算出工程）。次に、観測手段１
において、北基準直交座標における目標の観測位置およ
び距離、仰角、方位角の観測雑音の標準偏差を第１の平
滑手段４へそれぞれ出力する（ステップＳＴ４：観測値
取得工程）。次に、第１の平滑手段４において、平滑ベ
クトル、平滑誤差共分散行列を、式（６）、式（７）、
式（８）を使用して算出する（ステップＳＴ５：第１の
平滑算出工程）。次に、第２の遅延手段１０、第２の予
測手段１１、第２の平滑手段１２において、式（１４）
〜式（１８）を使用して、現時刻よりＬ時刻前の平滑ベ
クトル、平滑誤差共分散行列を再計算する（ステップＳ
Ｔ６：第２の平滑算出工程）。次に、第１の着弾点予測
手段５において、再計算したＬ時刻前の平滑ベクトル、
平滑誤差共分散行列を用いて、予測航跡、予測着弾点の
計算を行う（ステップＳＴ７：予測着弾点算出工程）。
次に、表示手段６において、ステップＳＴ７で算出した
予測着弾点および予測航跡をオペレータへ表示する（ス
テップＳＴ８）。次に、サンプリング時刻ｋがサンプリ
ング終了時刻ｋｅｎｄより大きくなったら、処理を中断
する。また、サンプリング時刻ｋがサンプリング時刻ｋ
ｅｎｄ以下の場合には、処理を継続する（ステップＳＴ
９，ＳＴ１０）。

【００３６】以上のように、この実施の形態１によれ
ば、運動座標から北基準直交座標への座標変換行列を算
出すると共に、事前に決められた時刻より前の平滑ベク
トルおよび平滑誤差共分散行列を再算出することによ
り、着弾点予測の初期値の精度を向上させ、さらに、着
弾点予測を精度良く行うことができる。

【００３７】実施の形態２．図８はこの発明の実施の形
態２による追尾装置を示す構成図であり、図において、
１３は第１の予測諸元平均手段である。なお、各図の同
一符号は、同一または相当部分を表す。

【００３８】次に動作について説明する。第１の予測諸
元平均手段１３では、第１の着弾点予測手段５から出力
される位置、速度等を持つ予測ベクトル、予測誤差共分
散行列を、ある一定サンプリング分保持しておき、一定
サンプリング分の予測ベクトルおよび予測誤差共分散行
列を加重平均する。そして、その加重平均した予測ベク
トルおよび予測誤差共分散行列を表示手段６に出力す
る。第１の予測諸元平均手段１３を用いることにより、
オペレータへの表示する際の位置および航跡の表示がば
らつかず、安定して表示される。

【００３９】図９はこの発明の実施の形態２による追尾
処理方法を示すフローチャートであり、ここで、同一ス
テップは説明を省略する。第１の予測諸元平均手段１３
において、ステップＳＴ７において算出された予測ベク
トル、予測誤差共分散行列を、ある一定サンプリング分
保持しておき、一定サンプリング分の予測ベクトルおよ
び予測誤差共分散行列を加重平均する（ステップＳＴ７
ａ）。そして、その加重平均した予測ベクトルおよび予
測誤差共分散行列を出力する。したがって、以上のステ
ップにより、オペレータへの表示する際の位置および航
跡の表示がばらつかず、安定して表示される。

【００４０】以上のように、この実施の形態２によれ
ば、表示する際の位置および航跡がばらつかず、安定し
た表示を行うことができる。

【００４１】実施の形態３．図１０はこの発明の実施の
形態３による追尾装置を示す構成図であり、図におい
て、１４は第２の予測諸元平均手段である。なお、各図
の同一符号は、同一または相当部分を表す。

【００４２】次に動作について説明する。第２の予測諸
元平均手段１４では、第１の着弾点予測手段５から出力
される位置、速度等を持つ予測ベクトル、予測誤差共分
散行列を、ある一定サンプリング分保持しておき、代数
学におけるノルムを利用して、一定サンプリング分の予
測ベクトルおよび予測誤差共分散行列を予測誤差共分散
行列のノルムにより、重みづけ平均を行う。そして、そ
の重みづけ平均した予測ベクトルｎｈおよび予測誤差共
分散行列を表示手段６に出力する。第２の予測諸元平均
手段１４のような誤差を反映した重みづけ平均を行うこ
とにより、オペレータへの表示する際の位置および航跡
の表示がばらつかず、安定して表示される。

【００４３】図１１はこの発明の実施の形態３による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第２の予測諸元平均手段１
４において、ステップＳＴ７において算出された予測ベ
クトル、予測誤差共分散行列を、ある一定サンプリング
分保持しておき、一定サンプリング分の予測ベクトルお
よび予測誤差共分散行列を予測誤差共分散行列のノルム
により、重みづけ平均する（ステップＳＴ７ｂ）。そし
て、その重みづけ平均した予測ベクトルおよび予測誤差
共分散行列を出力する。したがって、以上のステップに
より、誤差を反映した重みづけを行っているので、オペ
レータへの表示する際の位置および航跡の表示がばらつ
かず、安定して表示される。

【００４４】以上のように、この実施の形態３によれ
ば、誤差を反映した重みづけを行っているので、表示す
る際の位置および航跡がばらつかず、安定した表示を行
うことができる。

【００４５】実施の形態４．図１２はこの発明の実施の
形態４による追尾装置を示す構成図であり、図におい
て、１５は第３の予測諸元平均手段である。なお、各図
の同一符号は、同一または相当部分を表す。

【００４６】次に動作について説明する。第３の予測諸
元平均手段１５では、第１の着弾点予測手段５から出力
される位置、速度等を持つ予測ベクトル、予測誤差共分
散行列を、ある一定サンプリング分保持しておき、着弾
点における予測誤差共分散行列を水平上に射影した面の
面積を重みとして、一定サンプリング分の予測ベクトル
および予測誤差共分散行列の重みづけ平均を行う。そし
て、その着弾点における予測誤差共分散行列を水平上に
射影した面の面積で、重みづけ平均した予測ベクトルお
よび予測誤差共分散行列を表示手段６に出力する。着弾
点における予測誤差共分散行列を水平上に射影した面の
面積は、着弾範囲の面積に相当する。第３の予測諸元平
均手段１５のような誤差を反映した重みづけ平均を行う
ことにより、オペレータへの表示する際の位置および航
跡の表示がばらつかず、安定して表示される。

【００４７】図１３はこの発明の実施の形態４による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第３の予測諸元平均手段１
５において、ステップＳＴ７において算出された予測ベ
クトル、予測誤差共分散行列を、ある一定サンプリング
分保持しておき、一定サンプリング分の予測ベクトルお
よび予測誤差共分散行列をその着弾点における予測誤差
共分散行列を水平上に射影した面の面積で、重みづけ平
均する（ステップＳＴ７ｃ）。そして、その重みづけ平
均した予測ベクトルおよび予測誤差共分散行列を出力す
る。したがって、以上のステップにより、着弾範囲を反
映した重みづけを行っているので、オペレータへの表示
する際の位置および航跡の表示がばらつかず、安定して
表示される。

【００４８】以上のように、この実施の形態４によれ
ば、着弾範囲を反映した重みづけを行っているので、表
示する際の位置および航跡がばらつかず、安定した表示
を行うことができる。

【００４９】実施の形態５．図１４はこの発明の実施の
形態５による追尾装置を示す構成図であり、図におい
て、１６は第１の着弾点予測開始制御手段である。な
お、各図の同一符号は、同一または相当部分を表す。

【００５０】次に動作について説明する。第１の着弾点
予測開始制御手段１６では、時刻のしきい値Ｎを事前に
決めておき、観測開始からＮ秒後に着弾点予測を開始す
るような制御信号を第１の着弾点予測手段５に出力し、
第１の着弾点予測手段５において、初期値の誤差が収束
すると考えられるＮ秒後から着弾点予測を行うように制
御する。観測開始時刻から数サンプリングでは着弾点予
測の初期値の誤差が大きいが、第１の着弾点予測開始制
御手段１６を用いることにより、オペレータへ表示する
際の位置および航跡の表示がばらつかず、安定して表示
される。

【００５１】図１５はこの発明の実施の形態５による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第１の着弾点予測開始制御
手段１６において、ステップＳＴ６において算出された
着弾点予測の初期値となる、現時刻よりＬ時刻分遡って
再計算した平滑ベクトル、平滑誤差共分散行列を、事前
に決めた時刻Ｎより後で渡すようにする（ステップＳＴ
６ａ）。したがって、以上のステップにより、初期値の
誤差が収束すると考えられるＮ秒後から着弾点予測を行
うため、オペレータへの表示する際の位置および航跡の
表示がばらつかず、安定して表示される。

【００５２】以上のように、この実施の形態５によれ
ば、初期値の誤差が収束すると考えられるＮ秒後から着
弾点予測を行うため、表示する際の位置および航跡がば
らつかず、安定した表示を行うことができる。

【００５３】実施の形態６．図１６はこの発明の実施の
形態６による追尾装置を示す構成図であり、図におい
て、１７は第２の着弾点予測開始制御手段である。な
お、各図の同一符号は、同一または相当部分を表す。

【００５４】次に動作について説明する。第２の着弾点
予測開始制御手段１７では、平滑誤差共分散行列のノル
ムが事前に決めたある定数より小さくなった時、着弾点
予測を開始するような制御信号を第１の着弾点予測手段
５に出力し、第１の着弾点予測手段５において、初期値
の誤差が収束すると考えられる平滑誤差共分散行列のノ
ルムが事前に決めたある閾値より小さくなった時から、
着弾点予測を行うように制御する。観測開始時刻近辺で
は、着弾点予測の初期値の誤差が大きいが、第２の着弾
点予測開始制御手段１７を用いることによって、オペレ
ータへ表示する際の位置および航跡の表示がばらつか
ず、安定して表示される。

【００５５】図１７はこの発明の実施の形態６による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第２の着弾点予測開始制御
手段１７において、ステップＳＴ６において算出された
着弾点予測の初期値となる、現時刻よりＬ時刻分遡って
再計算した平滑ベクトル、平滑誤差共分散行列を、初期
値の誤差が収束すると考えられる平滑誤差共分散行列の
ノルムが事前に決めたある閾値より小さくなった時か
ら、渡すようにする（ステップＳＴ６ｂ）。したがっ
て、以上のステップにより、初期値の誤差が収束すると
考えられる、平滑誤差共分散行列のノルムが事前に決め
たある閾値より小さくなった時から着弾点予測を行うた
め、オペレータへ表示する際の位置および航跡の表示が
ばらつかず、安定して表示される。

【００５６】以上のように、この実施の形態６によれ
ば、初期値の誤差が収束すると考えられる平滑誤差共分
散行列のノルムが事前に決めたある閾値より小さくなっ
た時から着弾点予測を行うため、表示する際の位置およ
び航跡の表示がばらつかず、安定した表示を行うことが
できる。

【００５７】実施の形態７．図１８はこの発明の実施の
形態７による追尾装置を示す構成図であり、図におい
て、１８は第１の駆動雑音制御手段である。なお、各図
の同一符号は、同一または相当部分を表す。

【００５８】次に動作について説明する。第１の駆動雑
音制御手段１８では、観測手段１から得られる現時刻と
１サンプリング前の時刻のサンプリング間隔が、ある事
前に決められた閾値より小さい場合、運動座標における
駆動雑音共分散行列を小さくするように制御する制御信
号を、運動座標での駆動雑音設定手段９に出力する。サ
ンプリング間隔が短い場合、駆動雑音ベクトルの共分散
行列を大きく設定すると、平滑ベクトルの誤差が大きく
なってしまう。そのため、第１の駆動雑音制御手段１８
を用いることにより、平滑ベクトルの誤差を小さくする
ことが可能である。

【００５９】図１９はこの発明の実施の形態７による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第１の駆動雑音制御手段１
８において、サンプリング間隔がある事前に決められた
閾値より小さい場合、運動座標における駆動雑音共分散
行列を小さくするように制御する制御信号を、出力する
（ステップＳＴ１０ａ）。したがって、以上のステップ
により、平滑ベクトルの誤差を小さくすることが可能と
なる。

【００６０】以上のように、この実施の形態７によれ
ば、平滑ベクトルの誤差を小さくすることができる。

【００６１】実施の形態８．図２０はこの発明の実施の
形態８による追尾装置を示す構成図であり、図におい
て、１９は第２の駆動雑音制御手段である。なお、各図
の同一符号は、同一または相当部分を表す。

【００６２】次に動作について説明する。第２の駆動雑
音制御手段１９では、第１の遅延手段２から得られる現
時刻より１サンプリング前の時刻の速度ベクトルの大き
さがある事前に決められた閾値より大きい場合、運動座
標における駆動雑音ベクトルの共分散行列の速度ベクト
ル方向のＵ軸成分を大きくするように制御する制御信号
を、運動座標での駆動雑音設定手段９に出力する。速度
ベクトルが大きい場合、システムで立てた運動モデルと
実際の目標の運動モデルが大きく異なる場合がある。そ
のような場合、追従遅れが起こり、平滑ベクトルの誤差
が大きくなる可能性がある。第２の駆動雑音制御手段１
９を設けることにより、目標への追従遅れが少なくな
り、平滑ベクトルの誤差を小さくすることが可能であ
る。

【００６３】図２１はこの発明の実施の形態８による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第２の駆動雑音制御手段１
９において、現時刻より１サンプリング前の速度ベクト
ルの大きさが、ある事前に決められた閾値よりも大きい
場合、運動座標における駆動雑音共分散行列の速度ベク
トル方向のＵ軸成分が大きくなるように制御する制御信
号を、出力する（ステップＳＴ１０ｂ）。したがって、
以上のステップにより、目標への追従遅れが少なくな
り、平滑ベクトルの誤差を小さくすることが可能であ
る。

【００６４】以上のように、この実施の形態８によれ
ば、目標への追従遅れが少なくなり、平滑ベクトルの誤
差を小さくすることができる。

【００６５】実施の形態９．図２２はこの発明の実施の
形態９による追尾装置を示す構成図であり、図におい
て、２０は第３の駆動雑音制御手段である。なお、各図
の同一符号は、同一または相当部分を表す。

【００６６】次に動作について説明する。第３の駆動雑
音制御手段２０では、第１の遅延手段２から得られる現
時刻より１サンプリング前の時刻の速度ベクトルの大き
さがある事前に決められた閾値より大きい場合が過去に
数回連続で起こった場合、運動座標における駆動雑音共
分散行列の速度ベクトル方向のＵ軸成分を大きくするよ
うに制御する制御信号を、運動座標での駆動雑音設定手
段９に出力する。速度ベクトルが大きい場合、システム
で立てた運動モデルと実際の目標の運動モデルが大きく
異なる場合がある。そのような場合、追従遅れが起こ
り、平滑ベクトルの誤差が大きくなる可能性がある。ま
た、１回の速度ベクトルの大きさで、実際の目標の速度
が大きいことを判断し、その判断が誤っていた場合、却
って、平滑ベクトルの誤差を大きくすることがある。そ
のため、第３の駆動雑音制御手段２０を用いることによ
り、速度ベクトルの大きさの判断の確度が向上し、その
結果、平滑ベクトルの誤差が誤って大きくなることが防
止できる。

【００６７】図２３はこの発明の実施の形態９による追
尾処理方法を示すフローチャートであり、ここで、同一
ステップは説明を省略する。第３の駆動雑音制御手段２
０において、現時刻より１サンプリング前の速度ベクト
ルの大きさが、ある事前に決められた閾値よりも大きい
場合が、過去に数回ある場合、運動座標における駆動雑
音共分散行列の速度ベクトル方向のＵ軸成分が大きくな
るように制御する制御信号を、出力する（ステップＳＴ
１０ｃ）。したがって、以上のステップを行うことによ
り、速度ベクトルの大きさの判断の確度が向上し、その
結果、平滑ベクトルの誤差が誤って大きくなることが防
止できる。

【００６８】以上のように、この実施の形態９によれ
ば、速度ベクトルの大きさの判断の確度が向上し、その
結果、平滑ベクトルの誤差が誤って大きくなることが防
止できる。

【００６９】実施の形態１０．図２４はこの発明の実施
の形態１０による追尾装置を示す構成図であり、図にお
いて、２１は第３の平滑手段である。なお、各図の同一
符号は、同一または相当部分を表す。

【００７０】次に動作について説明する。第３の平滑手
段２１では、第１の着弾点予測手段５から得られる予測
ベクトルを式（１９）、式（２０）を用いて表示用に計
算する。

【数２７】

【数２８】式（１９）において、ｍは表示する時刻であり、ｘｘ
（ｍ）は時刻ｍにおける予測ベクトルであり、ｙｙ
（ｍ）は表示用に算出する予測ベクトルである。式（２
０）において、Ｔ（ｍ）は時刻ｍと時刻ｍ−１のサンプ
リング間隔であり、μは事前に決められた時定数であ
る。また、λ（ｍ）は時刻ｍにおいて、式（１９）にお
ける表示用に算出する予測ベクトルｙｙ（ｍ）を算出す
る際に、時刻ｍの時の予測ベクトルｘｘ（ｍ）と時刻ｍ
−１の時の予測ベクトルｘｘ（ｍ−１）の予測ベクトル
のどちらを重く用いるかを決める重みである。表示用の
予測誤差共分散行列も式（１９）における予測ベクトル
を予測誤差共分散行列と置き換えることで計算できる。
したがって、第３の平滑手段２１では、式（１９）およ
び式（２０）の計算方法、つまり制御理論に基づく１次
のフィルタ処理により、表示用の予測ベクトルおよび表
示用の予測誤差共分散行列を算出し、表示手段６にそれ
らを出力する。したがって、第３の平滑手段２１を用い
ることで、表示用の予測ベクトルおよび予測誤差共分散
行列を式（１９）および式（２０）の計算方法を用いて
計算することによって、オペレータへの表示が滑らかに
なり、表示が見やすくなる。

【００７１】図２５はこの発明の実施の形態１０による
追尾処理方法を示すフローチャートであり、ここで、同
一ステップは説明を省略する。第３の平滑手段２１にお
いて、式（１９）および式（２０）の計算方法を用い
て、表示用の予測ベクトルおよび予測誤差共分散行列を
算出し、それらを出力する（ステップＳＴ７ｄ）。した
がって、以上のステップを行うことにより、予測ベクト
ルおよび予測誤差共分散行列を式（１９）および式（２
０）の計算方法を用いて計算することによって、オペレ
ータへの表示が滑らかになり、表示が見やすくなる。

【００７２】以上のように、この実施の形態１０によれ
ば、表示用の予測ベクトルおよび表示用の予測誤差共分
散行列を、制御理論に基づく１次フィルタ処理を用いて
計算することによって、表示が滑らかになり、表示が見
やすくなる。

【００７３】実施の形態１１．図２６はこの発明の実施
の形態１１による追尾装置を示す構成図であり、図にお
いて、２２は距離基準の座標変換行列算出制御手段であ
る。なお、各図の同一符号は、同一または相当部分を表
す。

【００７４】次に動作について説明する。距離基準の座
標変換行列算出制御手段２２では、第１の遅延手段２か
ら運動座標への座標変換行列算出手段８に出力される１
サンプリング前の平滑ベクトルを用いて、その１サンプ
リング前の平滑ベクトルから算出される仰角と方位角と
を、式（１１）における目標姿勢角α，βへそれぞれ代
入するという制御信号を、ユーザーの判断により、運動
座標への座標変換行列算出手段８に出力する。したがっ
て、距離基準の座標変換行列算出制御手段２２を用いる
ことで、観測精度が悪い影響で、平滑ベクトルの精度が
悪くなる場合でも、駆動雑音を適正に設定することが可
能となるため、平滑ベクトルの誤差を小さくすることが
できる。

【００７５】図２７はこの発明の実施の形態１１による
追尾処理方法を示すフローチャートであり、ここで、同
一ステップは説明を省略する。距離基準の座標変換行列
算出制御手段２２において、１サンプリング前の平滑ベ
クトルから算出される仰角と方位角とを、座標変換行列
算出のための目標姿勢角α，βへそれぞれ代入するとい
う制御信号を、出力する（ステップＳＴ２ａ）。したが
って、以上のステップを行うことにより、観測精度が悪
い影響で、平滑ベクトルの精度が悪くなる場合でも、駆
動雑音を適正に設定することが可能となり、平滑ベクト
ルの誤差を小さくすることができる。

【００７６】以上のように、この実施の形態１１によれ
ば、観測精度が悪い影響で、平滑ベクトルの精度が悪く
なる場合でも、駆動雑音を適正に設定することが可能と
なり、平滑ベクトルの誤差を小さくすることができる。

【００７７】実施の形態１２．図２８はこの発明の実施
の形態１２による追尾装置を示す構成図であり、図にお
いて、２３は推定面基準の座標変換行列算出制御手段で
ある。なお、各図の同一符号は、同一または相当部分を
表す。

【００７８】次に動作について説明する。推定面基準の
座標変換行列算出制御手段２３では、第１の遅延手段２
から出力される１サンプリング前の平滑ベクトルを過去
数サンプリング分蓄えておき、その過去数サンプリング
分の平滑ベクトルから算出される推定面から求まる法線
ベクトルを用いて、その法線ベクトルが北基準直交座標
における水平面となす角、また、その法線ベクトルが鉛
直面となす角をそれぞれ算出し、法線ベクトルが北基準
直交座標における水平面となす角と法線ベクトルが鉛直
面となす角とを式（１１）における目標姿勢角α，βに
それぞれ代入するという制御信号を、ユーザーの判断に
より、運動座標への座標変換行列算出手段８に出力す
る。したがって、推定面基準の座標変換行列算出制御手
段２３を用いることで、観測精度が悪い影響で、平滑ベ
クトルの精度が悪くなる場合でも、駆動雑音を適正に設
定することが可能となるため、平滑ベクトルの誤差を小
さくすることができる。

【００７９】図２９はこの発明の実施の形態１２による
追尾処理方法を示すフローチャートであり、ここで、同
一ステップは説明を省略する。推定面基準の座標変換行
列算出制御手段２３において、過去数サンプリング分の
平滑ベクトルから算出される推定面から求まる法線ベク
トルを用いて、その法線ベクトルが北基準直交座標にお
ける水平面となす角、その法線ベクトルが鉛直面となす
角をそれぞれ算出し、式（１１）における目標姿勢角
α，βにそれぞれ代入するという制御信号を出力する
（ステップＳＴ２ｂ）。したがって、以上のステップを
行うことにより、観測精度が悪い影響で、平滑ベクトル
の精度が悪くなる場合でも、駆動雑音を適正に設定する
ことが可能となり、平滑ベクトルの誤差を小さくするこ
とができる。

【００８０】以上のように、この実施の形態１２によれ
ば、観測精度が悪い影響で、平滑ベクトルの精度が悪く
なる場合でも、駆動雑音を適正に設定することが可能と
なり、平滑ベクトルの誤差を小さくすることができる。

【００８１】

【発明の効果】以上のように、この発明によれば、第１
の遅延手段からの１サンプリング前の平滑ベクトルの速
度が北基準直交座標において、水平面と鉛直面でなす角
を算出して、運動座標から北基準直交座標への座標変換
行列を算出し、出力する運動座標への座標変換行列算出
手段と、運動座標において駆動雑音ベクトルの共分散行
列を設定する運動座標での駆動雑音設定手段と、現時刻
までの観測値を用いて、現時刻より数サンプリング前の
平滑ベクトルおよび平滑誤差共分散行列を再計算したも
のを１サンプリング遅延させ出力する第２の遅延手段
と、再計算された平滑誤差共分散行列を用いて、１サン
プリング分外挿を行う第２の予測手段と、現時刻までの
観測値を用いて、過去数サンプリング遡って、平滑ベク
トルおよび平滑誤差共分散行列を算出し、第１の着弾点
予測手段に出力する第２の平滑手段とを備えるように構
成したので、運動座標から北基準直交座標への座標変換
行列を算出すると共に、所定時間前の平滑ベクトルおよ
び平滑誤差共分散行列を再算出することにより、着弾点
予測の初期値の精度を向上させ、さらに、着弾点予測を
精度良く行うことができる効果がある。

【００８２】この発明によれば、第１の着弾点予測手段
からの予測ベクトルおよび予測誤差共分散行列を、ある
一定サンプリング分保持しておき、その一定サンプリン
グの間の予測ベクトルおよび予測誤差共分散行列につい
て加重平均を行い、その加重平均した予測ベクトルおよ
び加重平均した予測誤差共分散行列を表示手段に出力す
る第１の予測諸元平均手段を備えるように構成したの
で、表示する際の位置および航跡がばらつかず、安定し
た表示を行うことができる効果がある。

【００８３】この発明によれば、第１の着弾点予測手段
からの予測ベクトルおよび予測誤差共分散行列を、ある
一定サンプリング分保持しておき、その一定サンプリン
グの間の予測ベクトルおよび予測誤差共分散行列につい
て、予測誤差共分散行列のノルムにより、重みづけ平均
を行い、その重みづけ平均した予測ベクトルおよび重み
づけ平均した予測誤差共分散行列を表示手段に出力する
第２の予測諸元平均手段を備えるように構成したので、
誤差を反映した重みづけを行っているので、表示する際
の位置および航跡がばらつかず、安定した表示を行うこ
とができる効果がある。

【００８４】この発明によれば、第１の着弾点予測手段
からの予測ベクトルおよび予測誤差共分散行列を、ある
一定サンプリング分保持しておき、その一定サンプリン
グの間の予測ベクトルおよび予測誤差共分散行列につい
て、着弾点における予測誤差共分散行列を水平面上に射
影した面積を重みとして、重みづけ平均を行い、その重
みづけ平均した予測ベクトルおよび重みづけ平均した予
測誤差共分散行列を表示手段に出力する第３の予測諸元
平均手段を備えるように構成したので、着弾範囲を反映
した重みづけを行っているので、表示する際の位置およ
び航跡がばらつかず、安定した表示を行うことができる
効果がある。

【００８５】この発明によれば、観測開始時刻からある
事前に決めた時刻よりも一定時間が経過した数サンプリ
ング後に着弾点予測を開始する制御信号を第１の着弾点
予測手段に出力する第１の着弾点予測開始制御手段を備
えるように構成したので、初期値の誤差が収束すると考
えられる一定時間後から着弾点予測を行うため、表示す
る際の位置および航跡がばらつかず、安定した表示を行
うことができる効果がある。

【００８６】この発明によれば、平滑誤差共分散行列の
ノルムが事前に決めたある定数より小さくなった時、着
弾点予測を開始する制御信号を第１の着弾点予測手段に
出力する第２の着弾点予測開始制御手段を備えるように
構成したので、初期値の誤差が収束すると考えられる平
滑誤差共分散行列のノルムが事前に決めたある定数より
小さくなった時から着弾点予測を行うため、表示する際
の位置および航跡がばらつかず、安定した表示を行うこ
とができる効果がある。

【００８７】この発明によれば、観測手段から得られる
現時刻と１サンプリング前の時刻との間のサンプリング
間隔がある事前に決められた閾値より小さい場合、運動
座標における駆動雑音ベクトルの共分散行列を小さく設
定するように運動座標での駆動雑音設定手段を制御する
第１の駆動雑音制御手段を備えるように構成したので、
平滑ベクトルの誤差を小さくすることができる効果があ
る。

【００８８】この発明によれば、第１の遅延手段から得
られる１サンプリング前の速度ベクトルの大きさがある
事前に決められた閾値より大きい場合、運動座標におけ
る駆動雑音ベクトルの共分散行列の速度ベクトル方向の
成分を大きくするように運動座標での駆動雑音設定手段
を制御する第２の駆動雑音制御手段を備えるように構成
したので、目標への追従遅れが少なくなり、平滑ベクト
ルの誤差を小さくすることができる効果がある。

【００８９】この発明によれば、第１の遅延手段から得
られる１サンプリング前の速度ベクトルの大きさがある
事前に決められた閾値より大きい場合が、過去数回連続
で起こった場合、運動座標における駆動雑音ベクトルの
共分散行列の速度ベクトル方向の成分を大きくするよう
に運動座標での駆動雑音設定手段を制御する第３の駆動
雑音制御手段を備えるように構成したので、速度ベクト
ルの大きさの判断の確度が向上し、その結果、平滑ベク
トルの誤差が誤って大きくなることを防止できる効果が
ある。

【００９０】この発明によれば、第１の着弾点予測手段
からの予測ベクトルおよび予測誤差共分散行列を、制御
理論に基づく１次のフィルタ処理により、表示用の予測
ベクトルおよび表示用の予測誤差共分散行列に変換し、
表示手段に出力する第３の平滑手段を備えるように構成
したので、表示用の予測ベクトルおよび表示用の予測誤
差共分散行列を、制御理論に基づく１次のフィルタ処理
を用いて計算することによって、表示が滑らかになり、
表示が見やすくなる効果がある。

【００９１】この発明によれば、運動座標への座標変換
行列算出手段における運動座標から北基準直交座標への
座標変換行列を算出する際に、その座標変換行列に必要
な第１の遅延手段から出力される１サンプリング前の平
滑ベクトルの速度が、北基準直交座標における水平面と
なす角と、その平滑ベクトルが鉛直面となす角とを、そ
れぞれ１サンプリング前の平滑ベクトルの位置から求ま
る仰角と方位角とに置き換える制御信号をユーザーの判
断により運動座標への座標変換行列算出手段に出力する
距離基準の座標変換行列算出制御手段を備えるように構
成したので、観測精度が悪い影響で、平滑ベクトルの精
度が悪くなる場合でも、駆動雑音を適正に設定すること
が可能となり、平滑ベクトルの誤差を小さくすることが
できる効果がある。

【００９２】この発明によれば、運動座標への座標変換
行列算出手段における運動座標から北基準直交座標への
座標変換行列を算出する際に、その座標変換行列に必要
な第１の遅延手段から出力される１サンプリング前の平
滑ベクトルの速度が、北基準直交座標における水平面と
なす角と、その平滑ベクトルが鉛直面となす角とを、そ
れぞれ過去数サンプリング分の平滑ベクトルから算出さ
れる推定面から求まる法線ベクトルが、北基準直交座標
における水平面となす角と、その法線ベクトルが鉛直面
とそれぞれなす角とにそれぞれ置き換える制御信号をユ
ーザーの判断により運動座標への座標変換行列算出手段
に出力する推定面基準の座標変換行列算出制御手段を備
えるように構成したので、観測精度が悪い影響で、平滑
ベクトルの精度が悪くなる場合でも、駆動雑音を適正に
設定することが可能となり、平滑ベクトルの誤差を小さ
くすることができる効果がある。

【００９３】この発明によれば、運動座標における駆動
雑音ベクトルの共分散行列を設定する共分散行列設定工
程と、運動座標から北基準直交座標への座標変換行列を
平滑ベクトルの速度成分により算出する座標変換行列算
出工程と、現時刻までの観測値、平滑ベクトルおよび平
滑誤差共分散行列を用いて、事前に決められた時刻より
前の平滑ベクトルおよび平滑誤差共分散行列を再算出す
る第２の平滑算出工程とを備えるように構成したので、
運動座標から北基準直交座標への座標変換行列を算出す
ると共に、事前に決められた時刻より前の平滑ベクトル
および平滑誤差共分散行列を再算出することにより、着
弾点予測の初期値の精度を向上させ、さらに、着弾点予
測を精度良く行うことができる効果がある。

【図面の簡単な説明】

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

【図２】観測手段に関する座標系を示す説明図であ
る。

【図３】北基準直交座標を示す説明図である。

【図４】運動座標を示す説明図である。

【図５】目標の真航跡と現時刻の平滑値を初期値とし
て着弾点予測を行った予測航跡とを示す関係図である。

【図６】駆動雑音を大きく設定したときと小さく設定
したときの初期値のふらつきが予測着弾点に及ぼす影響
を示す関係図である。

【図７】この発明の実施の形態１による追尾処理方法
を示すフローチャートである。

【図８】この発明の実施の形態２による追尾装置を示
す構成図である。

【図９】この発明の実施の形態２による追尾処理方法
を示すフローチャートである。

【図１０】この発明の実施の形態３による追尾装置を
示す構成図である。

【図１１】この発明の実施の形態３による追尾処理方
法を示すフローチャートである。

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

【図１３】この発明の実施の形態４による追尾処理方
法を示すフローチャートである。

【図１４】この発明の実施の形態５による追尾装置を
示す構成図である。

【図１５】この発明の実施の形態５による追尾処理方
法を示すフローチャートである。

【図１６】この発明の実施の形態６による追尾装置を
示す構成図である。

【図１７】この発明の実施の形態６による追尾処理方
法を示すフローチャートである。

【図１８】この発明の実施の形態７による追尾装置を
示す構成図である。

【図１９】この発明の実施の形態７による追尾処理方
法を示すフローチャートである。

【図２０】この発明の実施の形態８による追尾装置を
示す構成図である。

【図２１】この発明の実施の形態８による追尾処理方
法を示すフローチャートである。

【図２２】この発明の実施の形態９による追尾装置を
示す構成図である。

【図２３】この発明の実施の形態９による追尾処理方
法を示すフローチャートである。

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

【図２５】この発明の実施の形態１０による追尾処理
方法を示すフローチャートである。

【図２６】この発明の実施の形態１１による追尾装置
を示す構成図である。

【図２７】この発明の実施の形態１１による追尾処理
方法を示すフローチャートである。

【図２８】この発明の実施の形態１２による追尾装置
を示す構成図である。

【図２９】この発明の実施の形態１２による追尾処理
方法を示すフローチャートである。

【図３０】従来の追尾装置を示す構成図である。

【符号の説明】

１観測手段、２第１の遅延手段、３第１の予測手
段、４第１の平滑手段、５第１の着弾点予測手段、
６表示手段、８運動座標への座標変換行列算出手
段、９運動座標での駆動雑音設定手段、１０第２の
遅延手段、１１第２の予測手段、１２第２の平滑手
段、１３第１の予測諸元平均手段、１４第２の予測諸
元平均手段、１５第３の予測諸元平均手段、１６第
１の着弾点予測開始制御手段、１７第２の着弾点予測
開始制御手段、１８第１の駆動雑音制御手段、１９
第２の駆動雑音制御手段、２０第３の駆動雑音制御手
段、２１第３の平滑手段、２２距離基準の座標変換
行列算出制御手段、２３推定面基準の座標変換行列算出
制御手段、ステップＳＴ１共分散行列設定工程、ステ
ップＳＴ２座標変換行列算出工程、ステップＳＴ３
予測算出工程、ステップＳＴ４観測値取得工程、ステ
ップＳＴ５第１の平滑算出工程、ステップＳＴ６第
２の平滑算出工程、ステップＳＴ７予測着弾点算出工
程、Ｏセンサ、Ｔ追尾目標、Ｒ距離、Ａｚ方位
角、Ｅ１仰角、ＯＴ速度ベクトル、α，β 目標姿
勢角。

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

Claims

【特許請求の範囲】

【請求項１】北基準直交座標における目標の観測位置
を出力する観測手段と、現時刻よりも１サンプリング分遅延した平滑ベクトルお
よび平滑誤差共分散行列を出力する第１の遅延手段と、上記第１の遅延手段からの平滑ベクトルおよび平滑誤差
共分散行列を１サンプリング分外挿計算を行い、予測ベ
クトルおよび予測誤差共分散行列を算出し、出力する第
１の予測手段と、現時刻の観測値、上記第１の予測手段からの予測ベクト
ルおよび予測誤差共分散行列を用いて、平滑ベクトルお
よび平滑誤差共分散行列を算出し、出力する第１の平滑
手段と、過去数サンプリング溯って算出された平滑ベクトルおよ
び平滑誤差共分散行列を用いて、事前に登録してあるデ
ータベースにより、その着弾点計算を行い、その着弾点
計算の出力結果として、位置、速度を持つ予測ベクト
ル、予測誤差共分散行列を出力する第１の着弾点予測手
段と、上記第１の着弾点予測手段からの予測ベクトルを用い
て、予測位置の表示を行い、予測誤差共分散行列を用い
て、着弾点の範囲の表示を行う表示手段と、上記第１の遅延手段からの１サンプリング前の平滑ベク
トルの速度が北基準直交座標において、水平面と鉛直面
でなす角を算出して、運動座標から北基準直交座標への
座標変換行列を算出し、出力する運動座標への座標変換
行列算出手段と、上記運動座標への座標変換行列算出手段からの座標変換
行列に基づいて、運動座標において駆動雑音ベクトルの
共分散行列を設定する運動座標での駆動雑音設定手段
と、現時刻までの観測値を用いて、現時刻より数サンプリン
グ前の平滑ベクトルおよび平滑誤差共分散行列を再計算
したものを１サンプリング遅延させ出力する第２の遅延
手段と、上記第２の遅延手段からの再計算された平滑誤差共分散
行列を用いて、１サンプリング分外挿を行う第２の予測
手段と、現時刻までの観測値を用いて、過去数サンプリング遡っ
て、平滑ベクトルおよび平滑誤差共分散行列を算出し、
上記第１の着弾点予測手段に出力する第２の平滑手段と
を備えた追尾装置。
【請求項２】第１の着弾点予測手段からの予測ベクト
ルおよび予測誤差共分散行列を、ある一定サンプリング
分保持しておき、その一定サンプリングの間の予測ベク
トルおよび予測誤差共分散行列について加重平均を行
い、その加重平均した予測ベクトルおよび加重平均した
予測誤差共分散行列を表示手段に出力する第１の予測諸
元平均手段を備えたことを特徴とする請求項１記載の追
尾装置。
【請求項３】第１の着弾点予測手段からの予測ベクト
ルおよび予測誤差共分散行列を、ある一定サンプリング
分保持しておき、その一定サンプリングの間の予測ベク
トルおよび予測誤差共分散行列について、予測誤差共分
散行列のノルムにより、重みづけ平均を行い、その重み
づけ平均した予測ベクトルおよび重みづけ平均した予測
誤差共分散行列を表示手段に出力する第２の予測諸元平
均手段を備えたことを特徴とする請求項１記載の追尾装
置。
【請求項４】第１の着弾点予測手段からの予測ベクト
ルおよび予測誤差共分散行列を、ある一定サンプリング
分保持しておき、その一定サンプリングの間の予測ベク
トルおよび予測誤差共分散行列について、着弾点におけ
る予測誤差共分散行列を水平面上に射影した面積を重み
として、重みづけ平均を行い、その重みづけ平均した予
測ベクトルおよび重みづけ平均した予測誤差共分散行列
を表示手段に出力する第３の予測諸元平均手段を備えた
ことを特徴とする請求項１記載の追尾装置。
【請求項５】観測開始時刻からある事前に決めた時刻
よりも一定時間が経過した数サンプリング後に着弾点予
測を開始する制御信号を第１の着弾点予測手段に出力す
る第１の着弾点予測開始制御手段を備えたことを特徴と
する請求項１記載の追尾装置。
【請求項６】平滑誤差共分散行列のノルムが事前に決
めたある定数より小さくなった時、着弾点予測を開始す
る制御信号を第１の着弾点予測手段に出力する第２の着
弾点予測開始制御手段を備えたことを特徴とする請求項
１記載の追尾装置。
【請求項７】観測手段から得られる現時刻と１サンプ
リング前の時刻との間のサンプリング間隔がある事前に
決められた閾値より小さい場合、運動座標における駆動
雑音ベクトルの共分散行列を小さく設定するように運動
座標での駆動雑音設定手段を制御する第１の駆動雑音制
御手段を備えたことを特徴とする請求項１記載の追尾装
置。
【請求項８】第１の遅延手段から得られる１サンプリ
ング前の速度ベクトルの大きさがある事前に決められた
閾値より大きい場合、運動座標における駆動雑音ベクト
ルの共分散行列の速度ベクトル方向の成分を大きくする
ように運動座標での駆動雑音設定手段を制御する第２の
駆動雑音制御手段を備えたことを特徴とする請求項１記
載の追尾装置。
【請求項９】第１の遅延手段から得られる１サンプリ
ング前の速度ベクトルの大きさがある事前に決められた
閾値より大きい場合が、過去数回連続で起こった場合、
運動座標における駆動雑音ベクトルの共分散行列の速度
ベクトル方向の成分を大きくするように運動座標での駆
動雑音設定手段を制御する第３の駆動雑音制御手段を備
えたことを特徴とする請求項１記載の追尾装置。
【請求項１０】第１の着弾点予測手段からの予測ベク
トルおよび予測誤差共分散行列を、制御理論に基づく１
次のフィルタ処理により、表示用の予測ベクトルおよび
表示用の予測誤差共分散行列に変換し、表示手段に出力
する第３の平滑手段を備えたことを特徴とする請求項１
記載の追尾装置。
【請求項１１】運動座標への座標変換行列算出手段に
おける運動座標から北基準直交座標への座標変換行列を
算出する際に、その座標変換行列に必要な第１の遅延手
段から出力される１サンプリング前の平滑ベクトルの速
度が、北基準直交座標における水平面となす角と、その
平滑ベクトルが鉛直面となす角とを、それぞれ１サンプ
リング前の平滑ベクトルの位置から求まる仰角と方位角
とに置き換える制御信号をユーザーの判断により運動座
標への座標変換行列算出手段に出力する距離基準の座標
変換行列算出制御手段を備えたことを特徴とする請求項
１記載の追尾装置。
【請求項１２】運動座標への座標変換行列算出手段に
おける運動座標から北基準直交座標への座標変換行列を
算出する際に、その座標変換行列に必要な第１の遅延手
段から出力される１サンプリング前の平滑ベクトルの速
度が、北基準直交座標における水平面となす角と、その
平滑ベクトルが鉛直面となす角とを、それぞれ過去数サ
ンプリング分の平滑ベクトルから算出される推定面から
求まる法線ベクトルが、北基準直交座標における水平面
となす角と、その法線ベクトルが鉛直面とそれぞれなす
角とにそれぞれ置き換える制御信号をユーザーの判断に
より運動座標への座標変換行列算出手段に出力する推定
面基準の座標変換行列算出制御手段を備えたことを特徴
とする請求項１記載の追尾装置。
【請求項１３】運動座標における駆動雑音ベクトルの
共分散行列を設定する共分散行列設定工程と、運動座標から北基準直交座標への座標変換行列を平滑ベ
クトルの速度成分により算出する座標変換行列算出工程
と、運動座標による駆動雑音ベクトルの共分散行列を北基準
直交座標に座標変換し、予測ベクトルおよび予測誤差共
分散行列を算出する予測算出工程と、北基準直交座標における目標の観測位置を取得する観測
値取得工程と、現時刻の観測値、予測ベクトルおよび予測誤差共分散行
列を用いて、平滑ベクトルおよび平滑誤差共分散行列を
算出する第１の平滑算出工程と、現時刻までの観測値、平滑ベクトルおよび平滑誤差共分
散行列を用いて、所定時間前の平滑ベクトルおよび平滑
誤差共分散行列を再算出する第２の平滑算出工程と、事前に決められた時刻より前の平滑ベクトルおよび平滑
誤差共分散行列を用いて、予測着弾点を算出する予測着
弾点算出工程とを備えた追尾処理方法。