JP2843910B2 - 多目標追尾装置 - Google Patents
多目標追尾装置Info
- Publication number
- JP2843910B2 JP2843910B2 JP8321266A JP32126696A JP2843910B2 JP 2843910 B2 JP2843910 B2 JP 2843910B2 JP 8321266 A JP8321266 A JP 8321266A JP 32126696 A JP32126696 A JP 32126696A JP 2843910 B2 JP2843910 B2 JP 2843910B2
- Authority
- JP
- Japan
- Prior art keywords
- target
- motion
- motion models
- equation
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【0001】
【発明の属する技術分野】この発明は航空機、飛翔体等
の移動物体を目標とし、レーダ等の電波センサや赤外線
カメラ等の光学センサに代表される目標観測装置を用
い、目標観測装置による複数の目標および目標以外のク
ラッタ等からの信号検出結果に基づき、目標の位置や速
度等の真値を推定していくことにより、複数の目標の運
動を追尾する多目標追尾装置に関するものである。
の移動物体を目標とし、レーダ等の電波センサや赤外線
カメラ等の光学センサに代表される目標観測装置を用
い、目標観測装置による複数の目標および目標以外のク
ラッタ等からの信号検出結果に基づき、目標の位置や速
度等の真値を推定していくことにより、複数の目標の運
動を追尾する多目標追尾装置に関するものである。
【0002】
【従来の技術】図13は例えばIEEE TRANSA
CTIONS ON AUTOMATIC CONTR
OL VOL.AC−23,AUGUST 1978,
P618−626 「Tracking Method
s in a Multitarget Enviro
nment」の中で「Probabilistic D
ata Association Filter」とし
て示された従来の多目標追尾方法を示す処理手順、図1
4は図13の従来の多目標追尾方法に対応した従来の多
目標追尾装置の構成図である。
CTIONS ON AUTOMATIC CONTR
OL VOL.AC−23,AUGUST 1978,
P618−626 「Tracking Method
s in a Multitarget Enviro
nment」の中で「Probabilistic D
ata Association Filter」とし
て示された従来の多目標追尾方法を示す処理手順、図1
4は図13の従来の多目標追尾方法に対応した従来の多
目標追尾装置の構成図である。
【0003】図13において、従来の多目標追尾方法
は、ステップ1で目標位置の観測結果をもとに通常のカ
ルマンフィルタ理論に基づき目標位置、速度の平滑値お
よび平滑誤差共分散行列の初期値を設定し、ステップ1
5で例えば目標の運動モデルを等速直進運動モデルで設
定したのち、ステップ16で等速直進運動モデルにより
現時刻より1サンプリング後の目標位置、速度の予測値
を算出し、ステップ17で上記等速直進運動モデルによ
る予測値の誤差を推定した等速直進運動モデルによる予
測誤差共分散行列を算出し、ステップ6で目標観測位置
を信号検出結果である探知データとして入力し、ステッ
プ7で追尾目標と相関のある探知データとして、等速直
進運動モデルによる目標予測位置を中心に等速直進運動
モデルによる予測誤差共分散行列を使用して定まる空間
のある領域内に存在する探知データを選択し、ステップ
18でステップ7において選択された探知データが追尾
対象目標からの探知データであるか否かの信頼度を等速
直進運動モデルによる予測値および等速直進運動モデル
による予測誤差共分散行列を使用して算出し、ステップ
9で目標運動諸元の平滑に使用するゲイン行列を算出
し、ステップ19で等速直進運動モデルによる目標位
置、速度の平滑値および等速直進運動モデルによる平滑
誤差共分散行列を算出し、ステップ14において追尾終
了になるまでこの一連の流れを繰り返すようになってい
た。
は、ステップ1で目標位置の観測結果をもとに通常のカ
ルマンフィルタ理論に基づき目標位置、速度の平滑値お
よび平滑誤差共分散行列の初期値を設定し、ステップ1
5で例えば目標の運動モデルを等速直進運動モデルで設
定したのち、ステップ16で等速直進運動モデルにより
現時刻より1サンプリング後の目標位置、速度の予測値
を算出し、ステップ17で上記等速直進運動モデルによ
る予測値の誤差を推定した等速直進運動モデルによる予
測誤差共分散行列を算出し、ステップ6で目標観測位置
を信号検出結果である探知データとして入力し、ステッ
プ7で追尾目標と相関のある探知データとして、等速直
進運動モデルによる目標予測位置を中心に等速直進運動
モデルによる予測誤差共分散行列を使用して定まる空間
のある領域内に存在する探知データを選択し、ステップ
18でステップ7において選択された探知データが追尾
対象目標からの探知データであるか否かの信頼度を等速
直進運動モデルによる予測値および等速直進運動モデル
による予測誤差共分散行列を使用して算出し、ステップ
9で目標運動諸元の平滑に使用するゲイン行列を算出
し、ステップ19で等速直進運動モデルによる目標位
置、速度の平滑値および等速直進運動モデルによる平滑
誤差共分散行列を算出し、ステップ14において追尾終
了になるまでこの一連の流れを繰り返すようになってい
た。
【0004】図14は従来の多目標追尾装置の構成図で
あり、図14において、51は追尾目標および追尾目標
以外のクラッタ等からの信号検出結果である目標観測位
置を探知データとして出力する目標観測装置、56は等
速直進運動モデルにより現時刻より1サンプリング前に
算出しておいた目標予測位置を中心に等速直進運動モデ
ルにより現時刻より1サンプリング前に算出しておいた
予測誤差共分散行列を使用して求まる空間のある領域内
に存在する探知データを選択する相関器、58はゲイン
行列算出器、81は等速直進運動モデルによる予測器、
82は等速直進運動モデルにより現時刻より1サンプリ
ング前に算出しておいた目標予測位置と等速直進運動モ
デルにより現時刻より1サンプリング前に算出しておい
た予測誤差共分散行列を用い、相関器56で選択した探
知データが追尾対象目標からの探知データであるか否か
の信頼度を算出する探知データの信頼度算出器、83は
等速直進運動モデルによる平滑器、84は等速直進運動
モデルによる平滑誤差評価器、85は等速直進運動モデ
ルによる予測誤差評価器、86,87は遅延回路であ
る。
あり、図14において、51は追尾目標および追尾目標
以外のクラッタ等からの信号検出結果である目標観測位
置を探知データとして出力する目標観測装置、56は等
速直進運動モデルにより現時刻より1サンプリング前に
算出しておいた目標予測位置を中心に等速直進運動モデ
ルにより現時刻より1サンプリング前に算出しておいた
予測誤差共分散行列を使用して求まる空間のある領域内
に存在する探知データを選択する相関器、58はゲイン
行列算出器、81は等速直進運動モデルによる予測器、
82は等速直進運動モデルにより現時刻より1サンプリ
ング前に算出しておいた目標予測位置と等速直進運動モ
デルにより現時刻より1サンプリング前に算出しておい
た予測誤差共分散行列を用い、相関器56で選択した探
知データが追尾対象目標からの探知データであるか否か
の信頼度を算出する探知データの信頼度算出器、83は
等速直進運動モデルによる平滑器、84は等速直進運動
モデルによる平滑誤差評価器、85は等速直進運動モデ
ルによる予測誤差評価器、86,87は遅延回路であ
る。
【0005】上記ゲイン行列算出器58は等速直進運動
モデルにより現時刻より1サンプリング前に算出してお
いた予測誤差共分散行列より目標運動諸元の平滑に使用
するゲイン行列を算出する。等速直進運動モデルによる
予測器81は等速直進運動モデルによる平滑器83で算
出した平滑値をもとに等速直進運動モデルにより現時刻
より1サンプリング後の目標位置、速度の予測値を算出
する。等速直進運動モデルによる平滑器83は相関器5
6で選択された探知データと探知データの信頼度算出器
82で算出した探知データの信頼度と等速直進運動モデ
ルによる現時刻より1サンプリング前に算出しておいた
予測値およびゲイン行列算出器58で算出したゲイン行
列を使用して目標位置、速度の平滑値を算出する。
モデルにより現時刻より1サンプリング前に算出してお
いた予測誤差共分散行列より目標運動諸元の平滑に使用
するゲイン行列を算出する。等速直進運動モデルによる
予測器81は等速直進運動モデルによる平滑器83で算
出した平滑値をもとに等速直進運動モデルにより現時刻
より1サンプリング後の目標位置、速度の予測値を算出
する。等速直進運動モデルによる平滑器83は相関器5
6で選択された探知データと探知データの信頼度算出器
82で算出した探知データの信頼度と等速直進運動モデ
ルによる現時刻より1サンプリング前に算出しておいた
予測値およびゲイン行列算出器58で算出したゲイン行
列を使用して目標位置、速度の平滑値を算出する。
【0006】次に、等速直進運動モデルによる平滑誤差
評価器84は等速直進運動モデルによる予測器81で現
時刻より1サンプリング前に算出しておいた予測値、ゲ
イン行列算出器58で算出したゲイン行列、相関器56
で選択された探知データ、探知データの信頼度算出器8
2で算出した探知データの信頼度および等速直進運動モ
デルにより現時点より1サンプリング前に算出しておい
た予測誤差共分散行列を用いて目標位置、速度の平滑誤
差の評価値を算出する。また、等速直進運動モデルによ
る予測誤差評価器85は等速直進運動モデルによる平滑
誤差評価器84で算出した平滑誤差共分散行列をもとに
目標位置、速度の予測誤差の評価値を算出する。遅延回
路86は等速直進運動モデルによる予測器81で算出し
た予測値を1サンプリング遅延させる。置換回路87は
等速直進運動モデルによる予測誤差評価器85で算出し
た予測誤差共分散行列を1サンプリング遅延させる。
評価器84は等速直進運動モデルによる予測器81で現
時刻より1サンプリング前に算出しておいた予測値、ゲ
イン行列算出器58で算出したゲイン行列、相関器56
で選択された探知データ、探知データの信頼度算出器8
2で算出した探知データの信頼度および等速直進運動モ
デルにより現時点より1サンプリング前に算出しておい
た予測誤差共分散行列を用いて目標位置、速度の平滑誤
差の評価値を算出する。また、等速直進運動モデルによ
る予測誤差評価器85は等速直進運動モデルによる平滑
誤差評価器84で算出した平滑誤差共分散行列をもとに
目標位置、速度の予測誤差の評価値を算出する。遅延回
路86は等速直進運動モデルによる予測器81で算出し
た予測値を1サンプリング遅延させる。置換回路87は
等速直進運動モデルによる予測誤差評価器85で算出し
た予測誤差共分散行列を1サンプリング遅延させる。
【0007】
【発明が解決しようとする課題】上記のような従来の多
目標追尾方法およびその装置においては、相関器56
は、追尾目標が等速直進運動モデルに従うとして等速直
進運動モデルによる予測器81で算出した追尾目標の予
測位置を中心とし、等速直進運動モデルによる予測誤差
評価器85で算出した予測誤差共分散行列を使用して得
られる空間のある領域内に目標観測装置51より入力さ
れた探知データが存在した場合に、上記探知データを追
尾目標より探知された可能性がある探知データであると
判定していたため、目標が旋回運動を行うと相関範囲を
決める中心点が大きくずれる、あるいは相関をとるべき
空間の領域の大きさが目標の旋回状況を考慮しないため
に不当に小さく評価されるなどの問題が発生し、追尾性
能は劣化せざるを得なかった。
目標追尾方法およびその装置においては、相関器56
は、追尾目標が等速直進運動モデルに従うとして等速直
進運動モデルによる予測器81で算出した追尾目標の予
測位置を中心とし、等速直進運動モデルによる予測誤差
評価器85で算出した予測誤差共分散行列を使用して得
られる空間のある領域内に目標観測装置51より入力さ
れた探知データが存在した場合に、上記探知データを追
尾目標より探知された可能性がある探知データであると
判定していたため、目標が旋回運動を行うと相関範囲を
決める中心点が大きくずれる、あるいは相関をとるべき
空間の領域の大きさが目標の旋回状況を考慮しないため
に不当に小さく評価されるなどの問題が発生し、追尾性
能は劣化せざるを得なかった。
【0008】この発明はこのような課題を解決するため
になされたもので、複数の目標およびクラッタ等の不要
信号から、位置および距離変化率等の信号検出結果が探
知データとして得られる環境下において、旋回目標に対
しても精度よく追尾できる多目標追尾装置を提供するも
のである。
になされたもので、複数の目標およびクラッタ等の不要
信号から、位置および距離変化率等の信号検出結果が探
知データとして得られる環境下において、旋回目標に対
しても精度よく追尾できる多目標追尾装置を提供するも
のである。
【0009】
【課題を解決するための手段】この発明の第1の発明
は、同一次元の複数のn個の定数ベクトルより構成され
る目標のn個の運動モデルによる目標位置、速度などの
予測値を算出するn個の運動モデルによる予測器と、上
記n個の運動モデルによる予測値により定まる空間の領
域を相関範囲とし、目標観測位置、目標観測距離変化率
等を用いて追尾目標と相関の可能性のある探知データを
選択する相関器と、上記複数のn個の運動モデルおよび
上記相関器で選択された探知データの信頼度を算出する
n個の運動モデルによる信頼度算出器と、上記n個の運
動モデルを構成する定数ベクトルの推定値を算出する定
数ベクトル推定器と、上記定数ベクトル推定値に基づき
目標が旋回しているか否か等の目標の運動状態を判定す
る定数ベクトル推定値による目標運動判定器と、上記目
標運動状態の判定結果に基づき、設定するn個の運動モ
デルの組み合わせを選択的に切り替えつつ、n個の運動
モデルを用いることによる目標位置、速度などの平滑値
を算出するn個の運動モデルによる平滑器とこの平滑値
の誤差を推定した平滑誤差共分散行列を算出するn個の
運動モデルによる平滑誤差評価器とを設けたものであ
る。
は、同一次元の複数のn個の定数ベクトルより構成され
る目標のn個の運動モデルによる目標位置、速度などの
予測値を算出するn個の運動モデルによる予測器と、上
記n個の運動モデルによる予測値により定まる空間の領
域を相関範囲とし、目標観測位置、目標観測距離変化率
等を用いて追尾目標と相関の可能性のある探知データを
選択する相関器と、上記複数のn個の運動モデルおよび
上記相関器で選択された探知データの信頼度を算出する
n個の運動モデルによる信頼度算出器と、上記n個の運
動モデルを構成する定数ベクトルの推定値を算出する定
数ベクトル推定器と、上記定数ベクトル推定値に基づき
目標が旋回しているか否か等の目標の運動状態を判定す
る定数ベクトル推定値による目標運動判定器と、上記目
標運動状態の判定結果に基づき、設定するn個の運動モ
デルの組み合わせを選択的に切り替えつつ、n個の運動
モデルを用いることによる目標位置、速度などの平滑値
を算出するn個の運動モデルによる平滑器とこの平滑値
の誤差を推定した平滑誤差共分散行列を算出するn個の
運動モデルによる平滑誤差評価器とを設けたものであ
る。
【0010】この発明の第2の発明は、同一次元の複数
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、上記n個の運動モデル
による予測値により定まる空間の領域を相関範囲とし、
目標観測位置、目標観測距離変化率等を用いて追尾目標
と相関の可能性のある探知データを選択する相関器と、
上記複数のn個の運動モデルおよび上記相関器で選択さ
れた探知データの信頼度を算出するn個の運動モデルに
よる信頼度算出器と、上記n個の運動モデルを構成する
定数ベクトルの推定値を算出する定数ベクトル推定器
と、上記n個の運動モデルを用いることによる目標距離
変化率の予測値を算出した後、この予測値および目標距
離変化率観測値に基づき目標が旋回しているか否か等の
目標の運動状態を判定する目標距離変化率予測値による
目標運動判定器と、上記目標運動状態の判定結果に基づ
き、設定するn個の運動モデルの組み合わせを選択的に
切り替えつつ、n個の運動モデルを用いることによる目
標位置、速度などの平滑値を算出するn個の運動モデル
による平滑器とこの平滑値の誤差を推定した平滑誤差共
分散行列を算出するn個の運動モデルによる平滑誤差評
価器とを設けたものである。
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、上記n個の運動モデル
による予測値により定まる空間の領域を相関範囲とし、
目標観測位置、目標観測距離変化率等を用いて追尾目標
と相関の可能性のある探知データを選択する相関器と、
上記複数のn個の運動モデルおよび上記相関器で選択さ
れた探知データの信頼度を算出するn個の運動モデルに
よる信頼度算出器と、上記n個の運動モデルを構成する
定数ベクトルの推定値を算出する定数ベクトル推定器
と、上記n個の運動モデルを用いることによる目標距離
変化率の予測値を算出した後、この予測値および目標距
離変化率観測値に基づき目標が旋回しているか否か等の
目標の運動状態を判定する目標距離変化率予測値による
目標運動判定器と、上記目標運動状態の判定結果に基づ
き、設定するn個の運動モデルの組み合わせを選択的に
切り替えつつ、n個の運動モデルを用いることによる目
標位置、速度などの平滑値を算出するn個の運動モデル
による平滑器とこの平滑値の誤差を推定した平滑誤差共
分散行列を算出するn個の運動モデルによる平滑誤差評
価器とを設けたものである。
【0011】この発明の第3の発明は、同一次元の複数
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測器の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測誤差共分散行列により定ま
る空間の領域を相関範囲とし、目標観測位置、目標観測
距離変化率等を用いて追尾目標と相関の可能性のある探
知データを選択する相関器と、上記複数のn個の運動モ
デルおよび上記相関器で選択された探知データの信頼度
を算出するn個の運動モデルによる信頼度算出器と、上
記n個の運動モデルを構成する定数ベクトルの推定値を
算出する定数ベクトル推定器と、上記定数ベクトル推定
値に基づき目標が旋回しているか否か等の目標の運動状
態を判定する定数ベクトル推定値による目標運動判定器
と、上記目標運動状態の判定結果に基づき、設定するn
個の運動モデルの組み合わせを選択的に切り替えつつ、
n個の運動モデルを用いることによる目標位置、速度な
どの平滑値を算出するn個の運動モデルによる平滑器と
この平滑値の誤差を推定した平滑誤差共分散行列を算出
するn個の運動モデルによる平滑誤差評価器とを設けた
ものである。
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測器の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測誤差共分散行列により定ま
る空間の領域を相関範囲とし、目標観測位置、目標観測
距離変化率等を用いて追尾目標と相関の可能性のある探
知データを選択する相関器と、上記複数のn個の運動モ
デルおよび上記相関器で選択された探知データの信頼度
を算出するn個の運動モデルによる信頼度算出器と、上
記n個の運動モデルを構成する定数ベクトルの推定値を
算出する定数ベクトル推定器と、上記定数ベクトル推定
値に基づき目標が旋回しているか否か等の目標の運動状
態を判定する定数ベクトル推定値による目標運動判定器
と、上記目標運動状態の判定結果に基づき、設定するn
個の運動モデルの組み合わせを選択的に切り替えつつ、
n個の運動モデルを用いることによる目標位置、速度な
どの平滑値を算出するn個の運動モデルによる平滑器と
この平滑値の誤差を推定した平滑誤差共分散行列を算出
するn個の運動モデルによる平滑誤差評価器とを設けた
ものである。
【0012】この発明の第4の発明は、同一次元の複数
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測値の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測誤差共分散行列により定ま
る空間の領域を相関範囲とし、目標観測位置、目標観測
距離変化率等を用いて追尾目標と相関の可能性のある探
知データを選択する相関器と、上記複数のn個の運動モ
デルおよび上記相関器で選択された探知データの信頼度
を算出するn個の運動モデルによる信頼度算出器と、上
記n個の運動モデルを構成する定数ベクトルの推定値を
算出する定数ベクトル推定器と、上記n個の運動モデル
を用いることによる目標距離変化率の予測値を算出した
後、この予測値および目標距離変化率観測値に基づき目
標が旋回しているか否か等の目標の運動状態を判定する
目標距離変化率予測値による目標運動判定器と、上記目
標運動状態の判定結果に基づき、設定するn個の運動モ
デルの組み合わせを選択的に切り替えつつ、n個の運動
モデルを用いることによる目標位置、速度などの平滑値
を算出するn個の運動モデルによる平滑器とこの平滑値
の誤差を推定した平滑誤差共分散行列を算出するn個の
運動モデルによる平滑誤差評価器とを設けたものであ
る。
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測値の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測誤差共分散行列により定ま
る空間の領域を相関範囲とし、目標観測位置、目標観測
距離変化率等を用いて追尾目標と相関の可能性のある探
知データを選択する相関器と、上記複数のn個の運動モ
デルおよび上記相関器で選択された探知データの信頼度
を算出するn個の運動モデルによる信頼度算出器と、上
記n個の運動モデルを構成する定数ベクトルの推定値を
算出する定数ベクトル推定器と、上記n個の運動モデル
を用いることによる目標距離変化率の予測値を算出した
後、この予測値および目標距離変化率観測値に基づき目
標が旋回しているか否か等の目標の運動状態を判定する
目標距離変化率予測値による目標運動判定器と、上記目
標運動状態の判定結果に基づき、設定するn個の運動モ
デルの組み合わせを選択的に切り替えつつ、n個の運動
モデルを用いることによる目標位置、速度などの平滑値
を算出するn個の運動モデルによる平滑器とこの平滑値
の誤差を推定した平滑誤差共分散行列を算出するn個の
運動モデルによる平滑誤差評価器とを設けたものであ
る。
【0013】この発明の第5の発明は、同一次元の複数
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測値の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測値および上記n個の運動モ
デルによる予測誤差共分散行列により定まる空間の領域
を相関範囲とし、目標観測位置、目標観測距離変化率等
を用いて追尾目標と相関の可能性のある探知データを選
択する相関器と、上記複数のn個の運動モデルおよび上
記相関器で選択された探知データの信頼度を算出するn
個の運動モデルによる信頼度算出器と、上記n個の運動
モデルを構成する定数ベクトルの推定値を算出する定数
ベクトル推定器と、上記定数ベクトル推定値に基づき目
標が旋回しているか否か等の目標の運動状態を判定する
定数ベクトル推定値による目標運動判定器と、上記目標
運動状態の判定結果に基づき、設定するn個の運動モデ
ルの組み合わせを選択的に切り替えつつ、n個の運動モ
デルを用いることによる目標位置、速度などの平滑値を
算出するn個の運動モデルによる平滑器とこの平滑値の
誤差を推定した平滑誤差共分散行列を算出するn個の運
動モデルによる平滑誤差評価器とを設けたものである。
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測値の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測値および上記n個の運動モ
デルによる予測誤差共分散行列により定まる空間の領域
を相関範囲とし、目標観測位置、目標観測距離変化率等
を用いて追尾目標と相関の可能性のある探知データを選
択する相関器と、上記複数のn個の運動モデルおよび上
記相関器で選択された探知データの信頼度を算出するn
個の運動モデルによる信頼度算出器と、上記n個の運動
モデルを構成する定数ベクトルの推定値を算出する定数
ベクトル推定器と、上記定数ベクトル推定値に基づき目
標が旋回しているか否か等の目標の運動状態を判定する
定数ベクトル推定値による目標運動判定器と、上記目標
運動状態の判定結果に基づき、設定するn個の運動モデ
ルの組み合わせを選択的に切り替えつつ、n個の運動モ
デルを用いることによる目標位置、速度などの平滑値を
算出するn個の運動モデルによる平滑器とこの平滑値の
誤差を推定した平滑誤差共分散行列を算出するn個の運
動モデルによる平滑誤差評価器とを設けたものである。
【0014】この発明の第6の発明は、同一次元の複数
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測値の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測値および上記n個の運動モ
デルによる予測誤差共分散行列により定まる空間の領域
を相関範囲とし、目標観測位置、目標観測距離変化率等
を用いて追尾目標と相関の可能性のある探知データを選
択する相関器と、上記複数のn個の運動モデルおよび上
記相関器で選択された探知データの信頼度を算出するn
個の運動モデルによる信頼度算出器と、上記n個の運動
モデルを構成する定数ベクトルの推定値を算出する定数
ベクトル推定器と、上記n個の運動モデルを用いること
による目標距離変化率の予測値を算出した後、この予測
値および目標距離変化率観測値に基づき目標が旋回して
いるか否か等の目標の運動状態を判定する目標距離変化
率予測値による目標運動判定器と、上記目標運動状態の
判定結果に基づき、設定するn個の運動モデルの組み合
わせを選択的に切り替えつつ、n個の運動モデルを用い
ることによる目標位置、速度などの平滑値を算出するn
個の運動モデルによる平滑器とこの平滑値の誤差を推定
した平滑誤差共分散行列を算出するn個の運動モデルに
よる平滑誤差評価器とを設けたものである。
のn個の定数ベクトルより構成される目標のn個の運動
モデルによる目標位置、速度などの予測値を算出するn
個の運動モデルによる予測器と、この予測値の誤差を推
定したn個の運動モデルによる予測誤差共分散行列を算
出するn個の運動モデルによる予測誤差評価器と、上記
n個の運動モデルによる予測値および上記n個の運動モ
デルによる予測誤差共分散行列により定まる空間の領域
を相関範囲とし、目標観測位置、目標観測距離変化率等
を用いて追尾目標と相関の可能性のある探知データを選
択する相関器と、上記複数のn個の運動モデルおよび上
記相関器で選択された探知データの信頼度を算出するn
個の運動モデルによる信頼度算出器と、上記n個の運動
モデルを構成する定数ベクトルの推定値を算出する定数
ベクトル推定器と、上記n個の運動モデルを用いること
による目標距離変化率の予測値を算出した後、この予測
値および目標距離変化率観測値に基づき目標が旋回して
いるか否か等の目標の運動状態を判定する目標距離変化
率予測値による目標運動判定器と、上記目標運動状態の
判定結果に基づき、設定するn個の運動モデルの組み合
わせを選択的に切り替えつつ、n個の運動モデルを用い
ることによる目標位置、速度などの平滑値を算出するn
個の運動モデルによる平滑器とこの平滑値の誤差を推定
した平滑誤差共分散行列を算出するn個の運動モデルに
よる平滑誤差評価器とを設けたものである。
【0015】この発明においては、例えば複数のn個の
目標の運動モデルとして等速直進運動モデルに零ベクト
ルを含むn個の定数加速度ベクトルを付加した運動モデ
ルを使用し、n個の運動モデルによる目標位置、速度の
予測値を、複数個の運動モデルおのおのの信頼度および
複数個の運動モデルを構成する定数加速度ベクトルを使
用して予測における加速度の影響項を算出し、この諸元
に現時刻の平滑値をもとに等速直進運動予測により算出
した1サンプリング後の予測値を加算することにより算
出し、複数個の運動モデルおのおのの信頼度および複数
個の運動モデルを構成する定数加速度ベクトルを使用し
運動モデルが一意的に定まらないことにより予測誤差の
評価である予測誤差共分散行列を増大させる諸元を算出
し、この諸元に複数個の運動モデルごとの予測誤差共分
散行列を加算することによりn個の運動モデルによる予
測誤差共分散行列を算出している。
目標の運動モデルとして等速直進運動モデルに零ベクト
ルを含むn個の定数加速度ベクトルを付加した運動モデ
ルを使用し、n個の運動モデルによる目標位置、速度の
予測値を、複数個の運動モデルおのおのの信頼度および
複数個の運動モデルを構成する定数加速度ベクトルを使
用して予測における加速度の影響項を算出し、この諸元
に現時刻の平滑値をもとに等速直進運動予測により算出
した1サンプリング後の予測値を加算することにより算
出し、複数個の運動モデルおのおのの信頼度および複数
個の運動モデルを構成する定数加速度ベクトルを使用し
運動モデルが一意的に定まらないことにより予測誤差の
評価である予測誤差共分散行列を増大させる諸元を算出
し、この諸元に複数個の運動モデルごとの予測誤差共分
散行列を加算することによりn個の運動モデルによる予
測誤差共分散行列を算出している。
【0016】さらにこの発明においては、例えば等速直
進運動のように目標の運動が安定し、その運動モデルが
一意的に定まるような状態にある場合に、n個の運動モ
デルを用いることにより不当に観測精度の悪さを追尾の
がたつきに影響させることを防ぐために、算出される定
数加速度ベクトルの推定値あるいは目標距離変化率の予
測値に基づき目標の運動状態を判定し、例えば目標が旋
回していると判定した場合にはn個の運動モデルを用い
ることにより平滑値および平滑誤差共分散行列を算出
し、目標が等速直進していると判定した場合にはn個の
運動モデルを構成しているn個の定数ベクトルをすべて
零ベクトルに置き換え等価的に等速直進モデルとして、
平滑値および平滑誤差共分散行列を算出している。
進運動のように目標の運動が安定し、その運動モデルが
一意的に定まるような状態にある場合に、n個の運動モ
デルを用いることにより不当に観測精度の悪さを追尾の
がたつきに影響させることを防ぐために、算出される定
数加速度ベクトルの推定値あるいは目標距離変化率の予
測値に基づき目標の運動状態を判定し、例えば目標が旋
回していると判定した場合にはn個の運動モデルを用い
ることにより平滑値および平滑誤差共分散行列を算出
し、目標が等速直進していると判定した場合にはn個の
運動モデルを構成しているn個の定数ベクトルをすべて
零ベクトルに置き換え等価的に等速直進モデルとして、
平滑値および平滑誤差共分散行列を算出している。
【0017】この発明の第1および第4の発明において
は、多数の複数目標を同時に追尾しクラッタよりの探知
データも多い場合に対処可能とするため、追尾目標間で
相関をとるべき領域が重ならないよう、また、相関をと
るべき領域内の探知データを増加させないように、相関
をとるべき領域を広げずに計算負荷の増大を抑えつつ旋
回目標対処能力を向上させるため、相関をとるべき中心
点の算出にn個の運動モデルによる予測値、相関をとる
べき空間の領域の大きさ算出にn個の運動モデルによる
予測誤差共分散行列ではなく運動モデルごとの予測誤差
共分散行列を使用するようにしている。
は、多数の複数目標を同時に追尾しクラッタよりの探知
データも多い場合に対処可能とするため、追尾目標間で
相関をとるべき領域が重ならないよう、また、相関をと
るべき領域内の探知データを増加させないように、相関
をとるべき領域を広げずに計算負荷の増大を抑えつつ旋
回目標対処能力を向上させるため、相関をとるべき中心
点の算出にn個の運動モデルによる予測値、相関をとる
べき空間の領域の大きさ算出にn個の運動モデルによる
予測誤差共分散行列ではなく運動モデルごとの予測誤差
共分散行列を使用するようにしている。
【0018】この発明の第2および第5の発明において
は、追尾目標数が少なく、クラッタも少ない環境下での
サンプリング間隔が長く観測精度の悪い目標観測装置に
よる多目標追尾を想定し、目標予測値の算出に加速度項
を付加した場合に観測精度の悪さに起因する追尾のがた
つきを表面化させずに旋回目標処理能力を向上させるた
め、相関をとるべき中心点の算出にはn個の運動モデル
による予測値ではなく、運動モデルごとの予測値の一つ
である定数加速度ベクトルが零ベクトルの場合の等速直
進運動モデルによる予測値を使用し、相関をとるべき空
間の領域の大きさ算出にn個の運動モデルによる予測誤
差共分散行列を使用している。
は、追尾目標数が少なく、クラッタも少ない環境下での
サンプリング間隔が長く観測精度の悪い目標観測装置に
よる多目標追尾を想定し、目標予測値の算出に加速度項
を付加した場合に観測精度の悪さに起因する追尾のがた
つきを表面化させずに旋回目標処理能力を向上させるた
め、相関をとるべき中心点の算出にはn個の運動モデル
による予測値ではなく、運動モデルごとの予測値の一つ
である定数加速度ベクトルが零ベクトルの場合の等速直
進運動モデルによる予測値を使用し、相関をとるべき空
間の領域の大きさ算出にn個の運動モデルによる予測誤
差共分散行列を使用している。
【0019】この発明の第3および第6の発明において
は、きわめて精度の高い追尾が要求され、目標観測装置
の精度が高く、サンプリング間隔が短く、高性能の計算
機システムを使用する場合の多目標追尾を想定し、相関
をとるべき中心点の算出にn個の運動モデルによる予測
値、相関をとるべき空間の領域の大きさ算出にn個の運
動モデルによる予測誤差共分散行列を使用するようにし
ている。
は、きわめて精度の高い追尾が要求され、目標観測装置
の精度が高く、サンプリング間隔が短く、高性能の計算
機システムを使用する場合の多目標追尾を想定し、相関
をとるべき中心点の算出にn個の運動モデルによる予測
値、相関をとるべき空間の領域の大きさ算出にn個の運
動モデルによる予測誤差共分散行列を使用するようにし
ている。
【0020】
【発明の実施の形態】この発明による多目標追尾装置の
一実施例について説明する。図1および図2はこの発明
の多目標追尾装置の実施例1についてそれぞれ処理手順
および構成を示す図、図3および図4はこの発明の多目
標追尾装置の実施例2についてそれぞれ処理手順および
構成を示す図である。図5はこの発明の多目標追尾装置
の実施例3,5について処理手順を示す図、図6はこの
発明の多目標追尾装置の実施例3について構成を示す
図、図7はこの発明の多目標追尾装置の一実施例4,6
について処理手順を示す図、図8はこの発明の多目標追
尾装置の実施例4について構成を示す図、図9はこの発
明の多目標追尾装置の実施例5について構成を示す図、
図10はこの発明の多目標追尾装置の実施例6について
構成を示す図である。
一実施例について説明する。図1および図2はこの発明
の多目標追尾装置の実施例1についてそれぞれ処理手順
および構成を示す図、図3および図4はこの発明の多目
標追尾装置の実施例2についてそれぞれ処理手順および
構成を示す図である。図5はこの発明の多目標追尾装置
の実施例3,5について処理手順を示す図、図6はこの
発明の多目標追尾装置の実施例3について構成を示す
図、図7はこの発明の多目標追尾装置の一実施例4,6
について処理手順を示す図、図8はこの発明の多目標追
尾装置の実施例4について構成を示す図、図9はこの発
明の多目標追尾装置の実施例5について構成を示す図、
図10はこの発明の多目標追尾装置の実施例6について
構成を示す図である。
【0021】以下、この発明の一実施例を図1〜図10
にしたがい説明する前に、この発明の根拠となる理論の
骨子を説明する。なお以下ベクトルを表わす記号につい
ては、コード処理部では例えばベクトルXの様に、記号
の前に“ベクトル”を付し、イメージ処理部では記号に
下線を付して示す。
にしたがい説明する前に、この発明の根拠となる理論の
骨子を説明する。なお以下ベクトルを表わす記号につい
ては、コード処理部では例えばベクトルXの様に、記号
の前に“ベクトル”を付し、イメージ処理部では記号に
下線を付して示す。
【0022】この発明の多目標追尾装置による目標追尾
処理開始時刻をt1 とし、離散時間的に目標追尾処理の
動作をする各サンプリング時刻をtk (k=1,2,
…)と書く。また、サンプリング時刻tk-1 とサンプリ
ング時刻tk の間のサンプリング間隔をτk とする。す
なわち、式(1)とする。
処理開始時刻をt1 とし、離散時間的に目標追尾処理の
動作をする各サンプリング時刻をtk (k=1,2,
…)と書く。また、サンプリング時刻tk-1 とサンプリ
ング時刻tk の間のサンプリング間隔をτk とする。す
なわち、式(1)とする。
【0023】
【数1】
【0024】目標の運動モデルについて次のように定義
する。すなわち、複数個をn個とした場合のサンプリン
グ時刻tk における目標の運動モデルを式(2)とす
る。なお、以降ベクトルを表す記号にはアンダーライン
を付す。式(2)において、ベクトルXk はサンプリン
グ時刻tk における目標運動諸元の真値を表す状態ベク
トルであり、式(3)に示すように直交座標における目
標位置、および直交座標における目標速度を成分とする
ベクトルとする。なお、記号Tはベクトルの転置を表
す。Φk-1 はサンプリング時刻tk-1 よりtk への状態
ベクトルΧk の推移行列で目標が等速直進運動を行うと
仮定した場合、式(4)および式(5)で与えられる。
ベクトルwk はサンプリング時刻tk における駆動雑音
ベクトルであり、平均ベクトル0の3次元正規分布白色
雑音で、Eを平均を表す記号として式(6)、式(7)
を満たすものとする。ここで、Qk はサンプリング時刻
tk における駆動雑音共分散行列で運動モデルに拠らな
い値とする。Γk はサンプリング時刻tk における駆動
雑音ベクトルの変換行列で、例えば、目標の運動モデル
を等速直進運動と仮定したことにより打ち切り誤差を式
(2)の右辺第2項とみればベクトルwk は加速度ベク
トル相当であり、Γk は式(8)となる。ベクトルuk
はサンプリング時刻tk においてn個の運動モデルを構
成する定数加速度ベクトルで式(9)で表され、Γ’k
はサンプリング時刻tk における定数加速度ベクトルの
変換行列であり、式(10)となる。
する。すなわち、複数個をn個とした場合のサンプリン
グ時刻tk における目標の運動モデルを式(2)とす
る。なお、以降ベクトルを表す記号にはアンダーライン
を付す。式(2)において、ベクトルXk はサンプリン
グ時刻tk における目標運動諸元の真値を表す状態ベク
トルであり、式(3)に示すように直交座標における目
標位置、および直交座標における目標速度を成分とする
ベクトルとする。なお、記号Tはベクトルの転置を表
す。Φk-1 はサンプリング時刻tk-1 よりtk への状態
ベクトルΧk の推移行列で目標が等速直進運動を行うと
仮定した場合、式(4)および式(5)で与えられる。
ベクトルwk はサンプリング時刻tk における駆動雑音
ベクトルであり、平均ベクトル0の3次元正規分布白色
雑音で、Eを平均を表す記号として式(6)、式(7)
を満たすものとする。ここで、Qk はサンプリング時刻
tk における駆動雑音共分散行列で運動モデルに拠らな
い値とする。Γk はサンプリング時刻tk における駆動
雑音ベクトルの変換行列で、例えば、目標の運動モデル
を等速直進運動と仮定したことにより打ち切り誤差を式
(2)の右辺第2項とみればベクトルwk は加速度ベク
トル相当であり、Γk は式(8)となる。ベクトルuk
はサンプリング時刻tk においてn個の運動モデルを構
成する定数加速度ベクトルで式(9)で表され、Γ’k
はサンプリング時刻tk における定数加速度ベクトルの
変換行列であり、式(10)となる。
【0025】
【数2】
【0026】図11は平面に平行な面内で定数加速度ベ
クトルを説明する図であり、図において、Oは目標観測
装置を原点とした座標O−xyの原点、Xは東方向をx
軸の正とした座標O−xyのx軸、Yは北方向をy軸の
正とした座標O−xyのy軸、A1はy軸の正方向の定
数加速度ベクトル、A2はx軸の正方向の定数加速度ベ
クトル、A3はy軸の負方向の定数加速度ベクトル、A
4はx軸の負方向の定数加速度ベクトルである。図11
における定数加速度ベクトルの大きさを10g(gは重
力加速度とする)とし、この他に加速度ベクトル0の定
数加速度ベクトルを考えた運動モデルの場合、n=5で
あり、式(9)は式(10)のようにサンプリング時刻
に無関係に書ける。
クトルを説明する図であり、図において、Oは目標観測
装置を原点とした座標O−xyの原点、Xは東方向をx
軸の正とした座標O−xyのx軸、Yは北方向をy軸の
正とした座標O−xyのy軸、A1はy軸の正方向の定
数加速度ベクトル、A2はx軸の正方向の定数加速度ベ
クトル、A3はy軸の負方向の定数加速度ベクトル、A
4はx軸の負方向の定数加速度ベクトルである。図11
における定数加速度ベクトルの大きさを10g(gは重
力加速度とする)とし、この他に加速度ベクトル0の定
数加速度ベクトルを考えた運動モデルの場合、n=5で
あり、式(9)は式(10)のようにサンプリング時刻
に無関係に書ける。
【0027】
【数3】
【0028】サンプリング時刻tk において式(12)
が真であるとの仮説をΨk,L (L=1,2,…n)と書
く。運動モデルの時間的推移にはマルコフ性を仮定し、
運動モデルΨk,L はサンプリング時刻tk-1 の運動モデ
ルより決まりサンプリング時刻tk-2 までの運動モデル
には依存しないものとする。この時運動モデルの推移確
率PLkLk-1を式(13)とする。
が真であるとの仮説をΨk,L (L=1,2,…n)と書
く。運動モデルの時間的推移にはマルコフ性を仮定し、
運動モデルΨk,L はサンプリング時刻tk-1 の運動モデ
ルより決まりサンプリング時刻tk-2 までの運動モデル
には依存しないものとする。この時運動モデルの推移確
率PLkLk-1を式(13)とする。
【0029】
【数4】
【0030】次に観測系モデルを以下のように定義す
る。追尾対象目標よりの探知データはサンプリング時刻
tk において高々1つ得られるとし、その観測系モデル
を式(14)とする。ただし、ベクトルzk は式(1
5)に示すようにサンプリング時刻tk において直交座
標による目標観測位置と目標距離変化率の観測値で構成
される目標観測ベクトルであり、またF(ベクトル
xk )は式(16)、式(17)で表される非線形関数
である。ベクトルvk はサンプリング時刻tk における
目標観測ベクトルに対応した観測雑音ベクトルで、平均
ベクトル0の4次元正規白色雑音であり式(18)、式
(19)を満たすものとする。なお、Rk はサンプリン
グ時刻tk における観測雑音共分散行列で運動モデルに
よらない値とする。
る。追尾対象目標よりの探知データはサンプリング時刻
tk において高々1つ得られるとし、その観測系モデル
を式(14)とする。ただし、ベクトルzk は式(1
5)に示すようにサンプリング時刻tk において直交座
標による目標観測位置と目標距離変化率の観測値で構成
される目標観測ベクトルであり、またF(ベクトル
xk )は式(16)、式(17)で表される非線形関数
である。ベクトルvk はサンプリング時刻tk における
目標観測ベクトルに対応した観測雑音ベクトルで、平均
ベクトル0の4次元正規白色雑音であり式(18)、式
(19)を満たすものとする。なお、Rk はサンプリン
グ時刻tk における観測雑音共分散行列で運動モデルに
よらない値とする。
【0031】
【数5】
【0032】追尾対象目標以外からの探知データ、すな
わち誤探知データは空間に一様に分布しているとし、サ
ンプリング時刻tk における単位体積あたりの発生頻度
をλ k FTとし、追尾目標と相関をとるべき空間の領域の
体積をVGkとしたとき、追尾対象目標以外からの探知デ
ータが相関をとるべき領域内に存在する総数は平均λ k
FTVGkのポアソン分布に従うとする。なお、ここでVGk
は探知データが4次元であることから4次元空間の体積
である。
わち誤探知データは空間に一様に分布しているとし、サ
ンプリング時刻tk における単位体積あたりの発生頻度
をλ k FTとし、追尾目標と相関をとるべき空間の領域の
体積をVGkとしたとき、追尾対象目標以外からの探知デ
ータが相関をとるべき領域内に存在する総数は平均λ k
FTVGkのポアソン分布に従うとする。なお、ここでVGk
は探知データが4次元であることから4次元空間の体積
である。
【0033】サンプリング時刻tk において相関をとる
べき領域内に入った探知データの総数をmk 個とし、そ
の探知データの全体をベクトルzk,1 ,ベクトル
zk,2 ,…,ベクトルzk,mkとし、式(20)のように
書く。さらにサンプリング時刻t1からtk までの探知
データ数全体の情報をMk すなわち式(21)と書き、
またサンプリング時刻t1 からtk までの探知データの
全体をZk すなわち、式(22)と書く。
べき領域内に入った探知データの総数をmk 個とし、そ
の探知データの全体をベクトルzk,1 ,ベクトル
zk,2 ,…,ベクトルzk,mkとし、式(20)のように
書く。さらにサンプリング時刻t1からtk までの探知
データ数全体の情報をMk すなわち式(21)と書き、
またサンプリング時刻t1 からtk までの探知データの
全体をZk すなわち、式(22)と書く。
【0034】
【数6】
【0035】次に、目標の運動モデルについて若干の補
足を行う。複数個の運動モデルを構成している定数加速
度ベクトルベクトルαL (L=1,2,…,n)の条件
付き確率密度関数を離散値型の確率論に従い式(2
3)、式(24)とする。ここで、デルタ関数δ(ベク
トルx)は式(25)の性質を有している。
足を行う。複数個の運動モデルを構成している定数加速
度ベクトルベクトルαL (L=1,2,…,n)の条件
付き確率密度関数を離散値型の確率論に従い式(2
3)、式(24)とする。ここで、デルタ関数δ(ベク
トルx)は式(25)の性質を有している。
【0036】
【数7】
【0037】式(24)および式(25)より式(2
6)となる。すなわち、式(27)としたとき、式(2
8)である。また、式(24)および式(25)より式
(29)となる。
6)となる。すなわち、式(27)としたとき、式(2
8)である。また、式(24)および式(25)より式
(29)となる。
【0038】
【数8】
【0039】次に、サンプリング時刻tk までの探知デ
ータが得られた時点における、複数の各運動モデルおよ
びサンプリング時刻tk に得られた複数の各探知データ
の仮説に対する信頼度について定義する。ベクトルz
k,i (i=1,2,…,mk )が追尾対象目標からの探
知データであるとの仮説をχk,i (i=1,2,…,m
k )と書き、追尾対象目標から探知データが得られない
との仮説をχk,o と書く。サンプリング時刻tk までの
探知データの情報による仮説の信頼度を条件付き確率密
度関数により、式(30)〜式(33)のように定義す
る。
ータが得られた時点における、複数の各運動モデルおよ
びサンプリング時刻tk に得られた複数の各探知データ
の仮説に対する信頼度について定義する。ベクトルz
k,i (i=1,2,…,mk )が追尾対象目標からの探
知データであるとの仮説をχk,i (i=1,2,…,m
k )と書き、追尾対象目標から探知データが得られない
との仮説をχk,o と書く。サンプリング時刻tk までの
探知データの情報による仮説の信頼度を条件付き確率密
度関数により、式(30)〜式(33)のように定義す
る。
【0040】
【数9】
【0041】ここで、式(34)、式(35)の仮定を
おく。このとき、式(13)および式(35)より式
(36)となる。また確率論より式(37)〜式(4
5)が成立する。
おく。このとき、式(13)および式(35)より式
(36)となる。また確率論より式(37)〜式(4
5)が成立する。
【0042】
【数10】
【0043】以下、上記のように目標の運動モデル、観
測系モデル、および仮説とその信頼度が定義された場合
の、目標追尾処理手順について説明する。
測系モデル、および仮説とその信頼度が定義された場合
の、目標追尾処理手順について説明する。
【0044】まず、各仮説のもとでの平滑、予測処理の
方法について示す。すなわち、通常のカルマンフィルタ
の理論より、拡張カルマンフィルタを適用すると式(4
6)〜式(55)となる。
方法について示す。すなわち、通常のカルマンフィルタ
の理論より、拡張カルマンフィルタを適用すると式(4
6)〜式(55)となる。
【0045】
【数11】
【0046】ここで、ベクトルx^LK k (−)は仮説Ψ
k,lkのもとでのサンプリング時刻t k に対するベクトル
xk の予測ベクトルで、条件付き平均ベクトルで書けば
カルマンフィルタの理論より式(56)である。また、
ベクトルx^k (−)は、サンプリング時刻tk に対す
るベクトルxk の予測ベクトルで式(57)である。ベ
クトルx^k (+)はサンプリング時刻tk に対するベ
クトルxk の平滑ベクトルで式(58)である。ベクト
ルx^k,i,Lk(+)は仮説χk,i および仮説Ψ k,lkのも
とでのサンプリング時刻tk に対するベクトルxk の平
滑ベクトルで式(59)で表される。さらに、P
k (+)はサンプリング時刻tk における平滑誤差共分
散行列で式(60)である。また、Pk Lk(−)はサン
プリング時刻t k における仮説Ψk,lkのもとでの予測誤
差共分散行列で式(61)である。Pk, i,Lk(+)はサ
ンプリング時刻tk における仮説χk,i およびΨk,lkの
もとでの平滑誤差共分散行列で式(62)である。
k,lkのもとでのサンプリング時刻t k に対するベクトル
xk の予測ベクトルで、条件付き平均ベクトルで書けば
カルマンフィルタの理論より式(56)である。また、
ベクトルx^k (−)は、サンプリング時刻tk に対す
るベクトルxk の予測ベクトルで式(57)である。ベ
クトルx^k (+)はサンプリング時刻tk に対するベ
クトルxk の平滑ベクトルで式(58)である。ベクト
ルx^k,i,Lk(+)は仮説χk,i および仮説Ψ k,lkのも
とでのサンプリング時刻tk に対するベクトルxk の平
滑ベクトルで式(59)で表される。さらに、P
k (+)はサンプリング時刻tk における平滑誤差共分
散行列で式(60)である。また、Pk Lk(−)はサン
プリング時刻t k における仮説Ψk,lkのもとでの予測誤
差共分散行列で式(61)である。Pk, i,Lk(+)はサ
ンプリング時刻tk における仮説χk,i およびΨk,lkの
もとでの平滑誤差共分散行列で式(62)である。
【0047】
【数12】
【0048】Kk はサンプリング時刻tk におけるゲイ
ン行列である。また、拡張カルマンフィルタを通常適用
する場合と同様にして初期値ベクトルx^0 (+)、P
0 (+)は別途定まっているとする。なお、式(47)
よりPLk k (−)は仮説Ψk, lkによらない値となるの
で、式(48)よりKk 、式(52)よりP
k,i,Lk(+)(i=1,2,…mk )も仮説Ψk,1kによ
らない値となる。
ン行列である。また、拡張カルマンフィルタを通常適用
する場合と同様にして初期値ベクトルx^0 (+)、P
0 (+)は別途定まっているとする。なお、式(47)
よりPLk k (−)は仮説Ψk, lkによらない値となるの
で、式(48)よりKk 、式(52)よりP
k,i,Lk(+)(i=1,2,…mk )も仮説Ψk,1kによ
らない値となる。
【0049】次に、探知データと追尾目標の相関方法に
ついて示す。探知データベクトルzがdをパラメータと
して式(63)を満たすとき、探知データベクトルzは
追尾目標と相関があると判定される。ここでベクトルz
k (−)は式(64)で与えられる。またSk は式(6
5)で与えられる。ただし、Pk (−)はサンプリング
時刻tk における予測誤差共分散行列であり、式(6
6)で表される。
ついて示す。探知データベクトルzがdをパラメータと
して式(63)を満たすとき、探知データベクトルzは
追尾目標と相関があると判定される。ここでベクトルz
k (−)は式(64)で与えられる。またSk は式(6
5)で与えられる。ただし、Pk (−)はサンプリング
時刻tk における予測誤差共分散行列であり、式(6
6)で表される。
【0050】
【数13】
【0051】図12は簡単な例として探知データの次元
が2次元の場合について式(63)による探知データと
追尾目標との相関を説明する図であり、図においてPは
追尾目標からの観測が予測される点である式(64)の
ベクトルzk (−)、Qは相関をとるべき範囲の内外を
定める境界線でパラメータdおよび式(65)のSkよ
り線形代数学より算出され、D1〜D4は探知データで
ある。
が2次元の場合について式(63)による探知データと
追尾目標との相関を説明する図であり、図においてPは
追尾目標からの観測が予測される点である式(64)の
ベクトルzk (−)、Qは相関をとるべき範囲の内外を
定める境界線でパラメータdおよび式(65)のSkよ
り線形代数学より算出され、D1〜D4は探知データで
ある。
【0052】追尾対象目標が相関をとるべき領域内に存
在する確率をPGkと書く。すなわち式(67)とする。
ここで領域Gk は式(68)で表され、またN(ベクト
ルz k ;ak ,Ak )は平均ベクトルak 、共分散行列
Ak の4次元正規分布のベクトルzk における確率密度
関数である。なお、確率論よりPGkはdの値によって一
意的に定まる。
在する確率をPGkと書く。すなわち式(67)とする。
ここで領域Gk は式(68)で表され、またN(ベクト
ルz k ;ak ,Ak )は平均ベクトルak 、共分散行列
Ak の4次元正規分布のベクトルzk における確率密度
関数である。なお、確率論よりPGkはdの値によって一
意的に定まる。
【0053】
【数14】
【0054】次に、各仮説の信頼度の算出方法について
示す。仮説Ψk,lkのもとでの探知データベクトルzk,i
の確率密度関数lLk(ベクトルZk,i )(Lk =1,
2,…n;i=1,2,…mk )を4次元正規分布で近
似すればカルマンフィルタの理論より式(69)であ
る。式(69)よりわかるように、探知データベクトル
z k,i が観測諸元の予測ベクトルF(ベクトルx^Lk k
(−))に近いほどlLk(ベクトルzk,i )の値は大き
くなり、ベクトルzk,i は目標が仮説Ψk,1kのもとで運
動したとした場合に追尾目標から得られた探知データで
あるとの信頼度が高く評価される。サンプリング時刻t
k における探知情報が得られない時点での仮説Ψ
k-1,lk-1かつ仮説Ψk,1kが真である信頼度は、仮説Ψ
k-1,lk-1の信頼度である式(32)のβLk-1 k-1 および
推移確率である式(13)のPLkLk-1より式(70)に
よりもとまる。また、目標が探知される確率をPD とす
れば、追尾対象目標が相関をとるべき領域内に存在して
探知される確率はPD PGkである。したがってサンプリ
ング時刻tk の探知情報が得られた時点での仮説Ψ
k-1,lk-1,Ψ k,lkおよび仮説χk,0 が正しいとの信頼度
は、サンプリング時刻tk における探知情報が得られな
い時点での信頼度である式(70)に追尾対象目標より
相関をとるべき領域内から探知データが得られない確率
l−PD PGkおよびmk 個の探知データすべてが追尾対
象目標以外からの探知データであるとの信頼度
(λk PT)mkを乗算した値、すなわち式(71)に比例
すると考えてよい。
示す。仮説Ψk,lkのもとでの探知データベクトルzk,i
の確率密度関数lLk(ベクトルZk,i )(Lk =1,
2,…n;i=1,2,…mk )を4次元正規分布で近
似すればカルマンフィルタの理論より式(69)であ
る。式(69)よりわかるように、探知データベクトル
z k,i が観測諸元の予測ベクトルF(ベクトルx^Lk k
(−))に近いほどlLk(ベクトルzk,i )の値は大き
くなり、ベクトルzk,i は目標が仮説Ψk,1kのもとで運
動したとした場合に追尾目標から得られた探知データで
あるとの信頼度が高く評価される。サンプリング時刻t
k における探知情報が得られない時点での仮説Ψ
k-1,lk-1かつ仮説Ψk,1kが真である信頼度は、仮説Ψ
k-1,lk-1の信頼度である式(32)のβLk-1 k-1 および
推移確率である式(13)のPLkLk-1より式(70)に
よりもとまる。また、目標が探知される確率をPD とす
れば、追尾対象目標が相関をとるべき領域内に存在して
探知される確率はPD PGkである。したがってサンプリ
ング時刻tk の探知情報が得られた時点での仮説Ψ
k-1,lk-1,Ψ k,lkおよび仮説χk,0 が正しいとの信頼度
は、サンプリング時刻tk における探知情報が得られな
い時点での信頼度である式(70)に追尾対象目標より
相関をとるべき領域内から探知データが得られない確率
l−PD PGkおよびmk 個の探知データすべてが追尾対
象目標以外からの探知データであるとの信頼度
(λk PT)mkを乗算した値、すなわち式(71)に比例
すると考えてよい。
【0055】また、サンプリング時刻tk の探知情報が
得られた時点での仮説Ψk-1,lk-1,Ψk,lkおよび仮説χ
k,i (i=1,2,…、mk )が正しいとの信頼度は、
サンプリング時刻tk における探知情報が得られない時
点での信頼度である式(70)に追尾対象目標が探知さ
れる確率PD 、仮説Ψk,lkのもとでの探知データベクト
ルzk,i の評価値lLk(ベクトルzk,i )およびmk −
1個の探知データが追尾対象目標以外から得られている
との信頼度(λk PT)mk-1を乗算した値、すなわち式
(72)に比例すると考えてよい。したがって、式(3
0)のβk,i,Lk,L k-1 は、式(45)を使用し、式(7
1)および式(72)を正規化して式(73)〜式(7
5)の通り算出される。なお、式(31)のβk,i 、式
(32)のβLk k ,式(33)のβk,i,Lkは式(73)
〜式(75)で算出されたβk,i,Lk ,Lk-1 をそれぞれ式
(37)、式(39)、式(41)に代入することによ
り算出される。
得られた時点での仮説Ψk-1,lk-1,Ψk,lkおよび仮説χ
k,i (i=1,2,…、mk )が正しいとの信頼度は、
サンプリング時刻tk における探知情報が得られない時
点での信頼度である式(70)に追尾対象目標が探知さ
れる確率PD 、仮説Ψk,lkのもとでの探知データベクト
ルzk,i の評価値lLk(ベクトルzk,i )およびmk −
1個の探知データが追尾対象目標以外から得られている
との信頼度(λk PT)mk-1を乗算した値、すなわち式
(72)に比例すると考えてよい。したがって、式(3
0)のβk,i,Lk,L k-1 は、式(45)を使用し、式(7
1)および式(72)を正規化して式(73)〜式(7
5)の通り算出される。なお、式(31)のβk,i 、式
(32)のβLk k ,式(33)のβk,i,Lkは式(73)
〜式(75)で算出されたβk,i,Lk ,Lk-1 をそれぞれ式
(37)、式(39)、式(41)に代入することによ
り算出される。
【0056】
【数15】
【0057】次に、加速度ベクトル推定値の算出方法に
ついて示す。サンプリング時刻tkまでの探知データが
得られた時点における加速度ベクトルの推定値を式(7
6)で定義する。式(27)、式(28)の場合と同様
にして、式(76)より式(77)となる。したがって
式(77)に式(32)を適用して式(78)を得る。
ついて示す。サンプリング時刻tkまでの探知データが
得られた時点における加速度ベクトルの推定値を式(7
6)で定義する。式(27)、式(28)の場合と同様
にして、式(76)より式(77)となる。したがって
式(77)に式(32)を適用して式(78)を得る。
【0058】
【数16】
【0059】すべての仮説のもとでの目標位置、速度の
平滑ベクトル、すなわち式(58)のベクトルx^
k (+)は、各仮説のもとでもとめた平滑ベクトルを各
仮説の信頼度を用いて統合することによって算出され
る。まず、式(33)およびベイズの定理より式(7
9)が得られる。式(58)、式(59)、式(79)
より式(80)である。式(80)および式(49)、
式(51)、式(46)、式(78)、式(42)、式
(44)より式(81)〜式(83)である。
平滑ベクトル、すなわち式(58)のベクトルx^
k (+)は、各仮説のもとでもとめた平滑ベクトルを各
仮説の信頼度を用いて統合することによって算出され
る。まず、式(33)およびベイズの定理より式(7
9)が得られる。式(58)、式(59)、式(79)
より式(80)である。式(80)および式(49)、
式(51)、式(46)、式(78)、式(42)、式
(44)より式(81)〜式(83)である。
【0060】
【数17】
【0061】上記、すべての仮説のもとでの目標位置、
速度の平滑ベクトルの平滑誤差共分散行列、すなわち式
(60)のPk (+)は以下のように算出される。すな
わち、まず式(60)、式(58)、式(59)、式
(62)および式(79)より式(84)である。式
(42)および式(80)より式(85)であるから、
式(84)および式(85)より式(86)となる。式
(86)に式(50)、式(52)、式(38)、式
(43)、式(49)、式(51)、式(46)、式
(78)、式(81)〜式(83)および式(42)、
式(44)を代入し、さらにPLk k (−)およびP
k,i,Lk(+)がΨk,lkに無関係なことに注意すれば式
(87)が得られる。
速度の平滑ベクトルの平滑誤差共分散行列、すなわち式
(60)のPk (+)は以下のように算出される。すな
わち、まず式(60)、式(58)、式(59)、式
(62)および式(79)より式(84)である。式
(42)および式(80)より式(85)であるから、
式(84)および式(85)より式(86)となる。式
(86)に式(50)、式(52)、式(38)、式
(43)、式(49)、式(51)、式(46)、式
(78)、式(81)〜式(83)および式(42)、
式(44)を代入し、さらにPLk k (−)およびP
k,i,Lk(+)がΨk,lkに無関係なことに注意すれば式
(87)が得られる。
【0062】
【数18】
【0063】上記すべての仮説のもとでの平滑ベクトル
に基づく目標位置、速度の予測ベクトル、すなわち式
(57)のベクトルx^k (−)、およびベクトルx^
k (−)の予測誤差共分散行列、すなわち式(66)の
Pk (−)は各々次のように算出される。まず、予測ベ
クトルx^k (−)について、式(57)に式(2)を
代入し、式(58)、式(6)および式(27)を適用
して式(88)を得る。ここで式(70)より確率論に
したがい式(89)となるから、式(28)、式(8
9)より式(90)である。次に予測誤差共分散行列P
k (−)について、式(2)および式(88)より式
(91)であるから、式(66)に式(91)を代入
し、式(60)、式(7)および式(29)を適用しベ
クトルxk-1 、ベクトルwk-1 ,ベクトルuk-1 が互い
に無相関とすれば式(92)を得る。したがって、式
(92)、式(47)および式(89)より式(93)
である。
に基づく目標位置、速度の予測ベクトル、すなわち式
(57)のベクトルx^k (−)、およびベクトルx^
k (−)の予測誤差共分散行列、すなわち式(66)の
Pk (−)は各々次のように算出される。まず、予測ベ
クトルx^k (−)について、式(57)に式(2)を
代入し、式(58)、式(6)および式(27)を適用
して式(88)を得る。ここで式(70)より確率論に
したがい式(89)となるから、式(28)、式(8
9)より式(90)である。次に予測誤差共分散行列P
k (−)について、式(2)および式(88)より式
(91)であるから、式(66)に式(91)を代入
し、式(60)、式(7)および式(29)を適用しベ
クトルxk-1 、ベクトルwk-1 ,ベクトルuk-1 が互い
に無相関とすれば式(92)を得る。したがって、式
(92)、式(47)および式(89)より式(93)
である。
【0064】
【数19】
【0065】なお、上記一連の多目標追尾方法は、その
特別な場合として式(12)のn個の定数加速度ベクト
ルをすべて零ベクトルと設定する、すなわち式(94)
とすることにより、等速直進運動モデルによる多目標追
尾方法を含む。式(94)とした場合、特に平滑ベクト
ル算出式および平滑誤差共分散行列算出式について、式
(46)、式(78)に式(94)を適用し、式(4
3)を使用することにより、式(81)〜式(83)は
式(95)〜式(97)となり、式(87)は式(9
8)となる。
特別な場合として式(12)のn個の定数加速度ベクト
ルをすべて零ベクトルと設定する、すなわち式(94)
とすることにより、等速直進運動モデルによる多目標追
尾方法を含む。式(94)とした場合、特に平滑ベクト
ル算出式および平滑誤差共分散行列算出式について、式
(46)、式(78)に式(94)を適用し、式(4
3)を使用することにより、式(81)〜式(83)は
式(95)〜式(97)となり、式(87)は式(9
8)となる。
【0066】
【数20】
【0067】次に、上記の多目標追尾方法の処理過程に
おいて算出される諸量に基づき、目標運動の直進/曲進
を判定する方法について説明する。サンプリング時刻t
k における目標運動の直進/曲進の判定結果はフラグF
MDk で表し、判定結果が直進の場合式(98)、判定
結果が曲進の場合式(99)と定義する。また、追尾開
始時刻における判定結果フラグの初期値は0(直進)に
設定する。
おいて算出される諸量に基づき、目標運動の直進/曲進
を判定する方法について説明する。サンプリング時刻t
k における目標運動の直進/曲進の判定結果はフラグF
MDk で表し、判定結果が直進の場合式(98)、判定
結果が曲進の場合式(99)と定義する。また、追尾開
始時刻における判定結果フラグの初期値は0(直進)に
設定する。
【0068】
【数21】
【0069】まず第一に、加速度ベクトル推定値に基づ
き、目標運動の直進/曲進を判定する方法を示す。式
(78)の加速度ベクトル推定値uk-1 のx成分、y成
分、z成分をそれぞれ推定値uk-1 (X)、u
k-1 (y)、uk-1 (z)と書き、式(100)とす
る。加速度ベクトル推定値uk-1 を各成分ごとに一次フ
ィルタによって平滑したベクトル推定値をus k-1 と
し、そのx成分、y成分、z成分をそれぞれ推定値us
k-1 (x)、us k-1 (y)、us k-1 (z)と書く。
すなわち式(101)〜式(104)とする。ここで、
μは一次フィルタのゲインであり、x,y,zの各成分
で同一の値である。また、追尾開始時刻における推定値
us k-1 (x),us k-1 (y),us k-1 (z)の初
期値はすべて0に設定する。ベクトル推定値us k-1 の
ユークリッドノルム推定値‖us k-1 ‖は式(105)
となる。
き、目標運動の直進/曲進を判定する方法を示す。式
(78)の加速度ベクトル推定値uk-1 のx成分、y成
分、z成分をそれぞれ推定値uk-1 (X)、u
k-1 (y)、uk-1 (z)と書き、式(100)とす
る。加速度ベクトル推定値uk-1 を各成分ごとに一次フ
ィルタによって平滑したベクトル推定値をus k-1 と
し、そのx成分、y成分、z成分をそれぞれ推定値us
k-1 (x)、us k-1 (y)、us k-1 (z)と書く。
すなわち式(101)〜式(104)とする。ここで、
μは一次フィルタのゲインであり、x,y,zの各成分
で同一の値である。また、追尾開始時刻における推定値
us k-1 (x),us k-1 (y),us k-1 (z)の初
期値はすべて0に設定する。ベクトル推定値us k-1 の
ユークリッドノルム推定値‖us k-1 ‖は式(105)
となる。
【0070】サンプリング時刻tk における目標運動の
直進/曲進は次のように判定する。すなわち、式(10
6)が成立する場合、目標運動は直進と判定し式(9
8)とする。式(107)が成立する場合、目標運動は
曲進と判定し式(99)とする。式(108)が成立す
る場合は、サンプリング時刻tk-1 の判定結果を保持し
式(109)とする。ここで、ζ1 、ζ2 はサンプリン
グ時刻によらない定数で式(110)を満たすものとす
る。
直進/曲進は次のように判定する。すなわち、式(10
6)が成立する場合、目標運動は直進と判定し式(9
8)とする。式(107)が成立する場合、目標運動は
曲進と判定し式(99)とする。式(108)が成立す
る場合は、サンプリング時刻tk-1 の判定結果を保持し
式(109)とする。ここで、ζ1 、ζ2 はサンプリン
グ時刻によらない定数で式(110)を満たすものとす
る。
【0071】
【数22】
【0072】第二に目標距離変化率の予測値に基づき、
目標運動の直進/曲進を判定する方法を示す。R. P k
をサンプリング時刻tk に対する目標距離変化率の予測
値とし式(112)で定義すれば、式(68)より式
(17)を用い式(113)である。また、式(65)
のSk の第4行第4列要素をSk (4,4)とする。す
なわち、式(114)とする。サンプリング時刻tk に
おける探知データベクトルzk,i (i=1,2,…,m
k )の目標距離変化率成分をそれぞれR. o k,i(i=
1,2,…,mk )とし、サンプリング時刻tk までの
探知データが得られた時点でのR. o k,i の確率密度関
数を式(115)の1次元正規分布で近似すれば、式
(115)で定義される目標距離変化率R. o k,i (i
=1,2,…,mk )の加重平均推定値R. o k の確率
密度関数は、式(38)を用い、またR. o k,i (i=
1,2,…,mk )が互いに独立であるとして、式(1
17)の1次元正規分布となる。したがって、スカラ量
εk を式(118)で定義すれば、εk は自由度1のカ
イ2乗分布にしたがう。
目標運動の直進/曲進を判定する方法を示す。R. P k
をサンプリング時刻tk に対する目標距離変化率の予測
値とし式(112)で定義すれば、式(68)より式
(17)を用い式(113)である。また、式(65)
のSk の第4行第4列要素をSk (4,4)とする。す
なわち、式(114)とする。サンプリング時刻tk に
おける探知データベクトルzk,i (i=1,2,…,m
k )の目標距離変化率成分をそれぞれR. o k,i(i=
1,2,…,mk )とし、サンプリング時刻tk までの
探知データが得られた時点でのR. o k,i の確率密度関
数を式(115)の1次元正規分布で近似すれば、式
(115)で定義される目標距離変化率R. o k,i (i
=1,2,…,mk )の加重平均推定値R. o k の確率
密度関数は、式(38)を用い、またR. o k,i (i=
1,2,…,mk )が互いに独立であるとして、式(1
17)の1次元正規分布となる。したがって、スカラ量
εk を式(118)で定義すれば、εk は自由度1のカ
イ2乗分布にしたがう。
【0073】サンプリング時刻における目標運動の直進
/曲進は次のように判定する。すなわち、式(119)
が成立する場合、目標運動は直進と判定し式(99)と
する。式(120)が成立する場合、目標運動は曲進と
判定し式(100)とする。式(121)が成立する場
合は、サンプリング時刻tk-1 の判定結果を保持する。
すなわち式(109)とする。ここで、η1 ,η2 はサ
ンプリング時刻によらない定数で式(121)を満たす
ものとする。
/曲進は次のように判定する。すなわち、式(119)
が成立する場合、目標運動は直進と判定し式(99)と
する。式(120)が成立する場合、目標運動は曲進と
判定し式(100)とする。式(121)が成立する場
合は、サンプリング時刻tk-1 の判定結果を保持する。
すなわち式(109)とする。ここで、η1 ,η2 はサ
ンプリング時刻によらない定数で式(121)を満たす
ものとする。
【0074】
【数23】
【0075】次にこの発明の実施例1を図1および図2
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
【0076】以下、ループ内の処理の流れを示す。説明
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^k-1 (+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^Lk
k(−)を算出し、また運動モデルごとの予測誤差評価
器53において、n個の運動モデルによる平滑誤差評価
器63より入力されるn個の運動モデルにより平滑誤差
共分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。次にn個の運動
モデルによる予測器54において、n個の運動モデルに
よる平滑器62より式(58)のn個の運動モデルによ
る平滑値ベクトルx^K-1 (+)を入力して等速直進運
動予測によりΦk-1 ベクトルx^K-1 (+)を算出し、
n個の運動モデルによる信頼度算出器57より得られる
式(32)の各運動モデルの信頼度βLk-1 k-1 、あらか
じめ設定された式(13)の推移確率PLkLk-1および式
(12)の定数加速度ベクトルαLkより式(90)にし
たがいサンプリング時刻t k における探知情報が得られ
ない時点での加速度の影響項を推定して、式(88)に
したがいn個の運動モデルによる予測値ベクトルx^k
(−)を算出する(ステップ4)。
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^k-1 (+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^Lk
k(−)を算出し、また運動モデルごとの予測誤差評価
器53において、n個の運動モデルによる平滑誤差評価
器63より入力されるn個の運動モデルにより平滑誤差
共分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。次にn個の運動
モデルによる予測器54において、n個の運動モデルに
よる平滑器62より式(58)のn個の運動モデルによ
る平滑値ベクトルx^K-1 (+)を入力して等速直進運
動予測によりΦk-1 ベクトルx^K-1 (+)を算出し、
n個の運動モデルによる信頼度算出器57より得られる
式(32)の各運動モデルの信頼度βLk-1 k-1 、あらか
じめ設定された式(13)の推移確率PLkLk-1および式
(12)の定数加速度ベクトルαLkより式(90)にし
たがいサンプリング時刻t k における探知情報が得られ
ない時点での加速度の影響項を推定して、式(88)に
したがいn個の運動モデルによる予測値ベクトルx^k
(−)を算出する(ステップ4)。
【0077】次に時刻がサンプリング時刻tk-1 からサ
ンプリング時刻tk に経過したのち、目標観測装置51
より目標およびクラッタ等からの位置および距離変化率
の検出結果をサンプリング時刻tk の探知データとして
入力する(ステップ6)。次に相関器56において、サ
ンプリング時刻tk-1 に算出した式(57)のn個の運
動モデルによる予測値ベクトルx^k (−)を第1の遅
延回路64を通しn個の運動モデルによる予測器54よ
り入力して、相関をとるべき中心位置である式(64)
のベクトルzk (−)を式(16)により算出し、サン
プリング時刻に算出した式(61)の運動モデルごとの
予測誤差共分散行列PLk k (−)を第2の遅延回路65
を通し運動モデルごとの予測誤差評価器53より入力し
て、これとあらかじめ設定された式(19)の観測雑音
共分散行列Rk および上記第1の遅延回路64を通して
n個の運動モデルによる予測器54より入力したベクト
ルxk (−)より、相関をとるべき領域の大きさ算出に
使用する式(65)のSkをPk (−)の代わりにPLk
k (−)を使用することにより求め、式(63)により
追尾対象目標と相関の可能性のあるmk 個の探知データ
を選択し、これを式(20)のZk とする(ステップ
7)。
ンプリング時刻tk に経過したのち、目標観測装置51
より目標およびクラッタ等からの位置および距離変化率
の検出結果をサンプリング時刻tk の探知データとして
入力する(ステップ6)。次に相関器56において、サ
ンプリング時刻tk-1 に算出した式(57)のn個の運
動モデルによる予測値ベクトルx^k (−)を第1の遅
延回路64を通しn個の運動モデルによる予測器54よ
り入力して、相関をとるべき中心位置である式(64)
のベクトルzk (−)を式(16)により算出し、サン
プリング時刻に算出した式(61)の運動モデルごとの
予測誤差共分散行列PLk k (−)を第2の遅延回路65
を通し運動モデルごとの予測誤差評価器53より入力し
て、これとあらかじめ設定された式(19)の観測雑音
共分散行列Rk および上記第1の遅延回路64を通して
n個の運動モデルによる予測器54より入力したベクト
ルxk (−)より、相関をとるべき領域の大きさ算出に
使用する式(65)のSkをPk (−)の代わりにPLk
k (−)を使用することにより求め、式(63)により
追尾対象目標と相関の可能性のあるmk 個の探知データ
を選択し、これを式(20)のZk とする(ステップ
7)。
【0078】次にn個の運動モデルによる信頼度算出器
57において、サンプリング時刻t k-1 に式(39)に
より算出した各運動モデルの信頼度βLk-1 k-1 を第3の
遅延回路66を通して入力し、サンプリング時刻tk-1
に算出した式(56)の運動モデルごとの予測値ベクト
ルx^Lk k(−)を第4の遅延回路67を通し運動モデ
ルごとの予測器52より入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通して
運動モデルごとの予測誤差評価器53より入力して、あ
らかじめ設定された式(19)の観測雑音共分散行列R
k より式(69)、式(16)、式(17)および式
(53)、式(54)により仮説Ψk,lkのもとでの相関
器56よりの探知データベクトルzk,i の信頼度l
Lk(ベクトルzk,i )を求め、あらかじめ設定された探
知確率PD 、式(67)の追尾対象目標が相関をとるべ
き領域内に存在する確率PGkおよび式(13)の運動モ
デルの推移確率PLkLK-1により式(73)〜式(75)
および式(39)、式(41)、式(43)にしたがい
運動モデルおよび探知データの信頼度βk,i,Lk,Lk-1 β
Lk k 、βk,i,Lk,βk,i を算出する(ステップ8)。
57において、サンプリング時刻t k-1 に式(39)に
より算出した各運動モデルの信頼度βLk-1 k-1 を第3の
遅延回路66を通して入力し、サンプリング時刻tk-1
に算出した式(56)の運動モデルごとの予測値ベクト
ルx^Lk k(−)を第4の遅延回路67を通し運動モデ
ルごとの予測器52より入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通して
運動モデルごとの予測誤差評価器53より入力して、あ
らかじめ設定された式(19)の観測雑音共分散行列R
k より式(69)、式(16)、式(17)および式
(53)、式(54)により仮説Ψk,lkのもとでの相関
器56よりの探知データベクトルzk,i の信頼度l
Lk(ベクトルzk,i )を求め、あらかじめ設定された探
知確率PD 、式(67)の追尾対象目標が相関をとるべ
き領域内に存在する確率PGkおよび式(13)の運動モ
デルの推移確率PLkLK-1により式(73)〜式(75)
および式(39)、式(41)、式(43)にしたがい
運動モデルおよび探知データの信頼度βk,i,Lk,Lk-1 β
Lk k 、βk,i,Lk,βk,i を算出する(ステップ8)。
【0079】次にゲイン行列算出器58において、サン
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値べくとるx^k (−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルベクトルαLkを用い、式(7
8)にしたがい式(76)の加速度ベクトル推定値を算
出する(ステップ10)。
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値べくとるx^k (−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルベクトルαLkを用い、式(7
8)にしたがい式(76)の加速度ベクトル推定値を算
出する(ステップ10)。
【0080】次に加速度ベクトル推定値による目標運動
判定器60において、加速度ベクトル推定器59より式
(76)の加速度ベクトル推定値uk-1 を入力し、目標
運動判定器60で得られたサンプリング時刻tk-1 にお
ける目標運動判定結果FMD k-1 を第5の遅延回路68
を通して入力し、式(103)〜式(105)にしたが
い加速度ベクトル推定値の平滑ベクトル推定値uS k-1
を算出し、さらに式(106)にしたがい加速度ベクト
ル推定値の平滑ベクトル推定値uS k-1 のユークリッド
ノルム推定値‖us k-1 ‖を算出した後、目標運動判定
結果FMDk を、式(107)が成立する場合には式
(99)のように設定し、また式(108)が成立する
場合には式(100)のように設定し、また式(10
9)が成立する場合には式(110)のように設定する
(ステップ11)。
判定器60において、加速度ベクトル推定器59より式
(76)の加速度ベクトル推定値uk-1 を入力し、目標
運動判定器60で得られたサンプリング時刻tk-1 にお
ける目標運動判定結果FMD k-1 を第5の遅延回路68
を通して入力し、式(103)〜式(105)にしたが
い加速度ベクトル推定値の平滑ベクトル推定値uS k-1
を算出し、さらに式(106)にしたがい加速度ベクト
ル推定値の平滑ベクトル推定値uS k-1 のユークリッド
ノルム推定値‖us k-1 ‖を算出した後、目標運動判定
結果FMDk を、式(107)が成立する場合には式
(99)のように設定し、また式(108)が成立する
場合には式(100)のように設定し、また式(10
9)が成立する場合には式(110)のように設定する
(ステップ11)。
【0081】次にn個の運動モデルによる平滑器62に
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1 (+)を第6の遅延回路69を通して入力し、式
(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,i
の信頼度βk,i,Lkをn個の運動モデルによる信頼度算出
器57より入力し、相関器56より探知データベクトル
zk,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値x1k k (−)を第4の遅
延回路67を通し運動モデルごとの予測器52より入力
し、式(48)のゲイン行列Kk をゲイン行列算出器5
8より入力し、式(76)の加速度ベクトル推定値u
k-1 を加速度ベクトル推定器59より入力し、目標運動
判定結果FMDk を加速度ベクトル推定値による目標運
動判定器60より入力し、目標運動判定結果FMDk が
1(曲進)である場合には、式(81)〜式(83)に
したがいn個の運動モデルによる平滑値ベクトルx^k
(+)を算出し、目標運動判定結果FMDk が0(直
進)である場合には、式(95)および式(96)、式
(97)にしたがいn個の運動モデルによる平滑値ベク
トルx^k (+)を算出する。
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1 (+)を第6の遅延回路69を通して入力し、式
(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,i
の信頼度βk,i,Lkをn個の運動モデルによる信頼度算出
器57より入力し、相関器56より探知データベクトル
zk,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値x1k k (−)を第4の遅
延回路67を通し運動モデルごとの予測器52より入力
し、式(48)のゲイン行列Kk をゲイン行列算出器5
8より入力し、式(76)の加速度ベクトル推定値u
k-1 を加速度ベクトル推定器59より入力し、目標運動
判定結果FMDk を加速度ベクトル推定値による目標運
動判定器60より入力し、目標運動判定結果FMDk が
1(曲進)である場合には、式(81)〜式(83)に
したがいn個の運動モデルによる平滑値ベクトルx^k
(+)を算出し、目標運動判定結果FMDk が0(直
進)である場合には、式(95)および式(96)、式
(97)にしたがいn個の運動モデルによる平滑値ベク
トルx^k (+)を算出する。
【0082】n個の運動モデルによる平滑誤差評価器6
3において、n個の運動モデルによる信頼度算出器57
より式(31)のβk,i 、式(32)のβLk k および式
(33)のβk,i,Lkを入力し、サンプリング時刻tk-1
に算出した式(61)の運動モデルごとの予測誤差共分
散行列PLk k (−)を第2の遅延回路65を通し運動モ
デルごとの予測誤差評価器53より入力し、ゲイン行列
算出器58より式(48)のゲイン行列Kk を入力し、
サンプリング時刻tk-1 に算出しておいた式(56)の
運動モデルごとの予測値ベクトルxLk k (−)を第4の
遅延回路67を通し運動モデルごとの予測器52より入
力し、サンプリング時刻tk-1 に算出したn個の運動モ
デルによる予測値ベクトルx^k (−)を第1の遅延回
路64を通しn個の運動モデルによる予測器54より入
力し、相関器56より探知データベクトルzk,1 ,ベク
トルzk,2 ,…,ベクトルzk,mkを入力し、式(76)
の加速度ベクトル推定値uk-1 を加速度ベクトル推定器
59より入力し、目標運動判定結果FMDk を加速度ベ
クトル推定値による目標運動判定器60より入力し、目
標運動判定結果FMDk が1(曲進)である場合には、
式(87)におよび式(82)、式(83)にしたがい
式(60)のn個の運動モデルによる平滑誤差共分散行
列Pk (+)を算出し、目標運動判定結果FMDk が0
(直進)である場合には、式(98)および式(9
6)、式(97)にしたがい式(60)のn個の運動モ
デルによる平滑誤差共分散行列Pk (+)を算出する
(ステップ13)。追尾終了になるまでループ内のこの
一連の流れを繰り返す(ステップ14)。
3において、n個の運動モデルによる信頼度算出器57
より式(31)のβk,i 、式(32)のβLk k および式
(33)のβk,i,Lkを入力し、サンプリング時刻tk-1
に算出した式(61)の運動モデルごとの予測誤差共分
散行列PLk k (−)を第2の遅延回路65を通し運動モ
デルごとの予測誤差評価器53より入力し、ゲイン行列
算出器58より式(48)のゲイン行列Kk を入力し、
サンプリング時刻tk-1 に算出しておいた式(56)の
運動モデルごとの予測値ベクトルxLk k (−)を第4の
遅延回路67を通し運動モデルごとの予測器52より入
力し、サンプリング時刻tk-1 に算出したn個の運動モ
デルによる予測値ベクトルx^k (−)を第1の遅延回
路64を通しn個の運動モデルによる予測器54より入
力し、相関器56より探知データベクトルzk,1 ,ベク
トルzk,2 ,…,ベクトルzk,mkを入力し、式(76)
の加速度ベクトル推定値uk-1 を加速度ベクトル推定器
59より入力し、目標運動判定結果FMDk を加速度ベ
クトル推定値による目標運動判定器60より入力し、目
標運動判定結果FMDk が1(曲進)である場合には、
式(87)におよび式(82)、式(83)にしたがい
式(60)のn個の運動モデルによる平滑誤差共分散行
列Pk (+)を算出し、目標運動判定結果FMDk が0
(直進)である場合には、式(98)および式(9
6)、式(97)にしたがい式(60)のn個の運動モ
デルによる平滑誤差共分散行列Pk (+)を算出する
(ステップ13)。追尾終了になるまでループ内のこの
一連の流れを繰り返す(ステップ14)。
【0083】次にこの発明の実施例2を図3および図4
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
【0084】以下、ループ内の処理の流れを示す。説明
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^k-1 (+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^Lk
k(−)を算出し、また運動モデルごとの予測誤差評価
器53において、n個の運動モデルによる平滑誤差評価
器63より入力されるn個の運動モデルによる平滑誤差
共分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^k-1 (+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^Lk
k(−)を算出し、また運動モデルごとの予測誤差評価
器53において、n個の運動モデルによる平滑誤差評価
器63より入力されるn個の運動モデルによる平滑誤差
共分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。
【0085】次にn個の運動モデルによる予測器54に
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx
k-1 (+)を入力して等速直進運動予測によりΦk-1 ベ
クトルx^K-1 (+)を算出し、n個の運動モデルによ
る信頼度算出器57より得られる式(32)の各運動モ
デルの信頼度βLk-1 k-1 、あらかじめ設定された式(1
3)の推移確率PLkLk-1および式(12)の定数加速度
ベクトルαLkより式(90)にしたがいサンプリング時
刻tk における探知情報が得られない時点での加速度の
影響項を推定して、式(88)にしたがいn個の運動モ
デルによる予測値ベクトルx^k (−)を算出する(ス
テップ4)。
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx
k-1 (+)を入力して等速直進運動予測によりΦk-1 ベ
クトルx^K-1 (+)を算出し、n個の運動モデルによ
る信頼度算出器57より得られる式(32)の各運動モ
デルの信頼度βLk-1 k-1 、あらかじめ設定された式(1
3)の推移確率PLkLk-1および式(12)の定数加速度
ベクトルαLkより式(90)にしたがいサンプリング時
刻tk における探知情報が得られない時点での加速度の
影響項を推定して、式(88)にしたがいn個の運動モ
デルによる予測値ベクトルx^k (−)を算出する(ス
テップ4)。
【0086】次に時刻がサンプリング時刻tk-1 からサ
ンプリング時刻tk に経過したのち、目標観測装置51
より目標およびクラッタ等からの位置および距離変化率
の検出結果をサンプリング時刻tk の探知データとして
入力する(ステップ6)。
ンプリング時刻tk に経過したのち、目標観測装置51
より目標およびクラッタ等からの位置および距離変化率
の検出結果をサンプリング時刻tk の探知データとして
入力する(ステップ6)。
【0087】次に相関器56において、サンプリング時
刻tk-1 に算出した式(57)のn個の運動モデルによ
る予測値ベクトルx^K (−)を第1の遅延回路64を
通しn個の運動モデルによる予測器54より入力して、
相関をとるべき中心位置である式(64)のベクトルz
k (−)を式(16)により算出し、サンプリング時刻
に算出した式(61)の運動モデルごとの予測誤差共分
散行列PLk k (−)を第2の遅延回路65を通し運動モ
デルごとの予測誤差評価器53より入力して、これとあ
らかじめ設定された式(19)の観測雑音共分散行列R
k および上記第1の遅延回路64を通してn個の運動モ
デルによる予測器54より入力したベクトルx^
k (−)より、相関をとるべき領域の大きさ算出に使用
する式(65)のSをPk (−)の代わりにP
Lk k (−)を使用することにより求め、式(63)によ
り追尾対象目標と相関の可能性のあるmk 個の探知デー
タを選択し、これを式(20)のZk とする(ステップ
7)。
刻tk-1 に算出した式(57)のn個の運動モデルによ
る予測値ベクトルx^K (−)を第1の遅延回路64を
通しn個の運動モデルによる予測器54より入力して、
相関をとるべき中心位置である式(64)のベクトルz
k (−)を式(16)により算出し、サンプリング時刻
に算出した式(61)の運動モデルごとの予測誤差共分
散行列PLk k (−)を第2の遅延回路65を通し運動モ
デルごとの予測誤差評価器53より入力して、これとあ
らかじめ設定された式(19)の観測雑音共分散行列R
k および上記第1の遅延回路64を通してn個の運動モ
デルによる予測器54より入力したベクトルx^
k (−)より、相関をとるべき領域の大きさ算出に使用
する式(65)のSをPk (−)の代わりにP
Lk k (−)を使用することにより求め、式(63)によ
り追尾対象目標と相関の可能性のあるmk 個の探知デー
タを選択し、これを式(20)のZk とする(ステップ
7)。
【0088】次にn個の運動モデルによる信頼度算出器
57において、サンプリング時刻t k-1 に式(39)に
より算出した各運動モデルの信頼度βLk-1 k-1 を第3の
遅延回路66を通して入力し、サンプリング時刻tk-1
に算出した式(56)の運動モデルごとの予測値ベクト
ルx^Lk k(−)を第4の遅延回路67を通し運動モデ
ルごとの予測器52より入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通して
運動モデルごとの予測誤差評価器53より入力して、あ
らかじめ設定された式(19)の観測雑音共分散行列R
k より式(69)、式(16)、式(17)および式
(53)、式(54)により仮説Ψk,lkのもとでの相関
器56よりの探知データベクトルzk,i の信頼度l
Lk(ベクトルzk,i )を求め、あらかじめ設定された探
知確率PD 、式(67)の追尾対象目標が相関をとるべ
き領域内に存在する確率PGkおよび式(13)の運動モ
デルの推移確率PLkLk-1により式(73)〜式(75)
および式(39)、式(41)、式(43)にしたがい
運動モデルおよび探知データの信頼度βk,i,Lk,Lk-1 、
βLk k 、βk,i,Lk、βk,i を算出する(ステップ8)。
57において、サンプリング時刻t k-1 に式(39)に
より算出した各運動モデルの信頼度βLk-1 k-1 を第3の
遅延回路66を通して入力し、サンプリング時刻tk-1
に算出した式(56)の運動モデルごとの予測値ベクト
ルx^Lk k(−)を第4の遅延回路67を通し運動モデ
ルごとの予測器52より入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通して
運動モデルごとの予測誤差評価器53より入力して、あ
らかじめ設定された式(19)の観測雑音共分散行列R
k より式(69)、式(16)、式(17)および式
(53)、式(54)により仮説Ψk,lkのもとでの相関
器56よりの探知データベクトルzk,i の信頼度l
Lk(ベクトルzk,i )を求め、あらかじめ設定された探
知確率PD 、式(67)の追尾対象目標が相関をとるべ
き領域内に存在する確率PGkおよび式(13)の運動モ
デルの推移確率PLkLk-1により式(73)〜式(75)
および式(39)、式(41)、式(43)にしたがい
運動モデルおよび探知データの信頼度βk,i,Lk,Lk-1 、
βLk k 、βk,i,Lk、βk,i を算出する(ステップ8)。
【0089】次にゲイン行列算出器58において、サン
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^k (−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^k (−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
【0090】次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
【0091】次に目標距離変化率の予測値による目標運
動判定器61において、サンプリング時刻tk-1 に算出
した式(57)のn個の運動モデルによる予測値ベクト
ルx^k (−)を第1の遅延回路64を通しn個の運動
モデルによる予測器54より入力し、探知データの信頼
度βk,i をn個の運動モデルによる信頼度算出器57よ
り入力し、目標運動判定器61で得られるサンプリング
時刻tk-1 における目標運動判定結果FMDk-1 を第5
の遅延回路68を通して入力し、相関器56より探知デ
ータの相関をとるべき領域の大きさ算出のために使用し
た式(65)のSk を入力し、さらに相関器56より探
知データベクトルzk,1 ,ベクトルzk, 2 , …,ベクト
ルzk,mkを入力してその距離変化率成分をR. O k,1 ,
R. O k, 2 , …,R. O k,mkとし、式(113)、式
(17)により目標距離変化率の予測値R. P k 、式
(114)によりSk の第4行第4列要素Sk (4,
4)、式(116)により目標距離変化率の加重平均推
定値R. o k をそれぞれ求め、さらに式(118)にし
たがいスカラ量εk を算出した後、目標運動判定結果F
MDk を、式(119)が成立する場合には式(99)
のように設置し、また式(120)が成立する場合には
式(100)のように設定し、また式(121)が成立
する場合には式(110)のように設定する(ステップ
12)。
動判定器61において、サンプリング時刻tk-1 に算出
した式(57)のn個の運動モデルによる予測値ベクト
ルx^k (−)を第1の遅延回路64を通しn個の運動
モデルによる予測器54より入力し、探知データの信頼
度βk,i をn個の運動モデルによる信頼度算出器57よ
り入力し、目標運動判定器61で得られるサンプリング
時刻tk-1 における目標運動判定結果FMDk-1 を第5
の遅延回路68を通して入力し、相関器56より探知デ
ータの相関をとるべき領域の大きさ算出のために使用し
た式(65)のSk を入力し、さらに相関器56より探
知データベクトルzk,1 ,ベクトルzk, 2 , …,ベクト
ルzk,mkを入力してその距離変化率成分をR. O k,1 ,
R. O k, 2 , …,R. O k,mkとし、式(113)、式
(17)により目標距離変化率の予測値R. P k 、式
(114)によりSk の第4行第4列要素Sk (4,
4)、式(116)により目標距離変化率の加重平均推
定値R. o k をそれぞれ求め、さらに式(118)にし
たがいスカラ量εk を算出した後、目標運動判定結果F
MDk を、式(119)が成立する場合には式(99)
のように設置し、また式(120)が成立する場合には
式(100)のように設定し、また式(121)が成立
する場合には式(110)のように設定する(ステップ
12)。
【0092】次にn個の運動モデルによる平滑器62に
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1 (+)を第6の遅延回路69を通して入力し、式
(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,i
の信頼度βk,i,LKをn個の運動モデルによる信頼度算出
器57より入力し、相関器56より探知データベクトル
zk,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、式(48)のゲイン行列Kk をゲイン行
列算出器58より入力し、式(76)の加速度ベクトル
推定値uk-1 を加速度ベクトル推定器59より入力し、
目標運動判定結果FMDk を目標距離変化率の予測値に
よる目標運動判定器61より入力し、目標運動判定結果
FMDk が1(曲進)である場合には、式(81)〜式
(83)にしたがいn個の運動モデルによる平滑値ベク
トルx^k (+)を算出し、目標運動判定結果FMDk
が0(直進)である場合には、式(95)および式(9
6)、式(97)にしたがいn個の運動モデルによる平
滑値ベクトルx^k (+)を算出する。
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1 (+)を第6の遅延回路69を通して入力し、式
(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,i
の信頼度βk,i,LKをn個の運動モデルによる信頼度算出
器57より入力し、相関器56より探知データベクトル
zk,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、式(48)のゲイン行列Kk をゲイン行
列算出器58より入力し、式(76)の加速度ベクトル
推定値uk-1 を加速度ベクトル推定器59より入力し、
目標運動判定結果FMDk を目標距離変化率の予測値に
よる目標運動判定器61より入力し、目標運動判定結果
FMDk が1(曲進)である場合には、式(81)〜式
(83)にしたがいn個の運動モデルによる平滑値ベク
トルx^k (+)を算出し、目標運動判定結果FMDk
が0(直進)である場合には、式(95)および式(9
6)、式(97)にしたがいn個の運動モデルによる平
滑値ベクトルx^k (+)を算出する。
【0093】またn個の運動モデルによる平滑誤差評価
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^k (−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を目標距離変化率の予測値による目標運動判定器61よ
り入力し、目標運動判定結果FMDk が1(曲進)であ
る場合には、式(87)および式(82)、式(83)
にしたがい式(60)のn個の運動モデルによる平滑誤
差共分散行列Pk (+)を算出し、目標運動判定結果F
MDk が0(直進)である場合には、式(98)および
式(96)、式(97)にしたがい式(60)のn個の
運動モデルによる平滑誤差共分散行列Pk (+)を算出
する(ステップ13)。追尾終了になるまでループ内の
この一連の流れを繰り返す(ステップ14)。
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^k (−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を目標距離変化率の予測値による目標運動判定器61よ
り入力し、目標運動判定結果FMDk が1(曲進)であ
る場合には、式(87)および式(82)、式(83)
にしたがい式(60)のn個の運動モデルによる平滑誤
差共分散行列Pk (+)を算出し、目標運動判定結果F
MDk が0(直進)である場合には、式(98)および
式(96)、式(97)にしたがい式(60)のn個の
運動モデルによる平滑誤差共分散行列Pk (+)を算出
する(ステップ13)。追尾終了になるまでループ内の
この一連の流れを繰り返す(ステップ14)。
【0094】次にこの発明の実施例3を図5および図6
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
【0095】以下、ループ内の処理の流れを示す。説明
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^k-1 (+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^k
(−)を算出し、また運動モデルごとの予測誤差評価器
53において、n個の運動モデルによる平滑誤差評価器
63より入力されるn個の運動モデルによる平滑誤差共
分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^k-1 (+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^k
(−)を算出し、また運動モデルごとの予測誤差評価器
53において、n個の運動モデルによる平滑誤差評価器
63より入力されるn個の運動モデルによる平滑誤差共
分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。
【0096】次にn個の運動モデルによる予測器54に
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^k-1 (+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαlkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^k (−)を算出する(ステッ
プ4)。
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^k-1 (+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαlkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^k (−)を算出する(ステッ
プ4)。
【0097】次にn個の運動モデルによる予測誤差評価
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLkLk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトルu^k- 1 よりPk (−)の予測誤差を大
きく評価する項
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLkLk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトルu^k- 1 よりPk (−)の予測誤差を大
きく評価する項
【0098】
【数24】
【0099】を算出し、式(93)にしたがいn個の運
動モデルによる予測誤差共分散行列Pk (−)を算出す
る(ステップ5)。次に時刻がサンプリング時刻tk-1
からサンプリング時刻tk に経過したのち、目標観測装
置51より目標およびクラッタ等からの位置および距離
変化率の検出結果をサンプリング時刻tk の探知データ
として入力する(ステップ6)。
動モデルによる予測誤差共分散行列Pk (−)を算出す
る(ステップ5)。次に時刻がサンプリング時刻tk-1
からサンプリング時刻tk に経過したのち、目標観測装
置51より目標およびクラッタ等からの位置および距離
変化率の検出結果をサンプリング時刻tk の探知データ
として入力する(ステップ6)。
【0100】次に相関器56において、サンプリング時
刻tk-1 に算出した式(56)の運動モデルごとの予測
値ベクトルx^Lk k (−)を第4の遅延回路67を通し
運動モデルごとの予測器52より入力して、相関をとる
べき中心位置である式(64)のベクトルzk (−)を
式(16)においてベクトルx^k (−)の代わりに定
数加速度ベクトルαLkが零ベクトルの場合のベクトルx
^LK k (−)を使用して算出し、サンプリング時刻t
k-1 に算出した式(66)のn個の運動モデルによる予
測誤差共分散行列Pk (−)を第7の遅延回路70を通
しn個の運動モデルによる予測誤差評価器55より入力
して、これとあらかじめ設定された式(19)の観測雑
音共分散行列Rk および第1の遅延回路64を通してn
個の運動モデルによる予測器54より入力したベクトル
x^k (−)より、相関をとるべき領域の大きさ算出に
使用する式(65)のSk を求め、式(63)により追
尾対象目標と相関の可能性のあるmk 個の探知データを
選択し、これを式(20)のZk とする(ステップ
7)。
刻tk-1 に算出した式(56)の運動モデルごとの予測
値ベクトルx^Lk k (−)を第4の遅延回路67を通し
運動モデルごとの予測器52より入力して、相関をとる
べき中心位置である式(64)のベクトルzk (−)を
式(16)においてベクトルx^k (−)の代わりに定
数加速度ベクトルαLkが零ベクトルの場合のベクトルx
^LK k (−)を使用して算出し、サンプリング時刻t
k-1 に算出した式(66)のn個の運動モデルによる予
測誤差共分散行列Pk (−)を第7の遅延回路70を通
しn個の運動モデルによる予測誤差評価器55より入力
して、これとあらかじめ設定された式(19)の観測雑
音共分散行列Rk および第1の遅延回路64を通してn
個の運動モデルによる予測器54より入力したベクトル
x^k (−)より、相関をとるべき領域の大きさ算出に
使用する式(65)のSk を求め、式(63)により追
尾対象目標と相関の可能性のあるmk 個の探知データを
選択し、これを式(20)のZk とする(ステップ
7)。
【0101】次にn個の運動モデルによる信頼度算出器
57において、信頼度算出器57でサンプリング時刻t
k-1 に式(39)により算出した各運動モデルの信頼度
βLk -1 k-1 を第3の遅延回路66を通して入力し、サン
プリング時刻tk-1 に算出した式(56)の運動モデル
ごとの予測値ベクトルx^Lk k(−)を第4の遅延回路
67を通し運動モデルごとの予測器52より入力し、サ
ンプリング時刻tk-1に算出した式(61)の運動モデ
ルごとの予測誤差共分散行列PLk k (−)を第2の遅延
回路65を通して運動モデルごとの予測誤差評価器53
より入力して、あらかじめ設定された式(19)の観測
雑音共分散行列Rk より式(69)、式(16)、式
(17)および式(53)、式(54)により仮説Ψ
k,lkのもとでの相関器56よりの探知データベクトルz
k,i の信頼度lLk(ベクトルzk,i )を求め、あらかじ
め設定された探知確率PD 、式(67)の追尾対象目標
が相関をとるべき領域内に存在する確率PGkおよび式
(13)の運動モデルの推移確率PLkLk-1により式(7
3)〜式(75)および式(39)、式(41)、式
(43)にしたがい運動モデルおよび探知データの信頼
度βk,i,Lk,Lk-1 、βLk k 、βk,i,Lk,βk,i を算出す
る(ステップ8)。
57において、信頼度算出器57でサンプリング時刻t
k-1 に式(39)により算出した各運動モデルの信頼度
βLk -1 k-1 を第3の遅延回路66を通して入力し、サン
プリング時刻tk-1 に算出した式(56)の運動モデル
ごとの予測値ベクトルx^Lk k(−)を第4の遅延回路
67を通し運動モデルごとの予測器52より入力し、サ
ンプリング時刻tk-1に算出した式(61)の運動モデ
ルごとの予測誤差共分散行列PLk k (−)を第2の遅延
回路65を通して運動モデルごとの予測誤差評価器53
より入力して、あらかじめ設定された式(19)の観測
雑音共分散行列Rk より式(69)、式(16)、式
(17)および式(53)、式(54)により仮説Ψ
k,lkのもとでの相関器56よりの探知データベクトルz
k,i の信頼度lLk(ベクトルzk,i )を求め、あらかじ
め設定された探知確率PD 、式(67)の追尾対象目標
が相関をとるべき領域内に存在する確率PGkおよび式
(13)の運動モデルの推移確率PLkLk-1により式(7
3)〜式(75)および式(39)、式(41)、式
(43)にしたがい運動モデルおよび探知データの信頼
度βk,i,Lk,Lk-1 、βLk k 、βk,i,Lk,βk,i を算出す
る(ステップ8)。
【0102】次にゲイン行列算出器58において、サン
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^k (−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^k (−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
【0103】次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)を加速度ベクトル推定値を算出する
(ステップ10)。
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)を加速度ベクトル推定値を算出する
(ステップ10)。
【0104】次に加速度ベクトル推定値による目標運動
判定器60において、式(76)の加速度ベクトル推定
値uk-1 を加速度ベクトル推定器59より入力し、推定
器59で得られたサンプリング時刻tk-1 における目標
運動判定結果FMDk-1 を第5の遅延回路68を通して
入力し、式(103)〜式(105)にしたがい加速度
ベクトル推定値の平滑ベクトル推定値uS k-1 を算出
し、さらに式(106)にしたがい加速度ベクトル推定
値の平滑ベクトル推定値uS K-1 のユークリッドノルム
推定値‖uS k-1 ‖を算出した後、目標運動判定結果F
MDk を、式(107)が成立する場合には式(99)
のように設定し、また式(108)が成立する場合には
式(100)のように設定し、また式(109)が成立
する場合には式(110)のように設定する(ステップ
11)。
判定器60において、式(76)の加速度ベクトル推定
値uk-1 を加速度ベクトル推定器59より入力し、推定
器59で得られたサンプリング時刻tk-1 における目標
運動判定結果FMDk-1 を第5の遅延回路68を通して
入力し、式(103)〜式(105)にしたがい加速度
ベクトル推定値の平滑ベクトル推定値uS k-1 を算出
し、さらに式(106)にしたがい加速度ベクトル推定
値の平滑ベクトル推定値uS K-1 のユークリッドノルム
推定値‖uS k-1 ‖を算出した後、目標運動判定結果F
MDk を、式(107)が成立する場合には式(99)
のように設定し、また式(108)が成立する場合には
式(100)のように設定し、また式(109)が成立
する場合には式(110)のように設定する(ステップ
11)。
【0105】次にn個の運動モデルによる平滑器62に
おいて、平滑器62によりサンプリング時刻tk-1 に算
出しておいた式(58)のn個の運動モデルによる平滑
値ベクトルx^k-1 (+)を第6の遅延回路69を通し
入力し、n個の運動モデルによる信頼度算出器57より
式(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,1
の信頼度βk,i,Lkを入力し、相関器56より探知データ
ベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルzk,mk
を入力し、サンプリング時刻tk-1 に算出しておいた式
(56)の運動モデルごとの予測値ベクトルx^
Lk k(−)を第4の遅延回路67を通し運動モデルごと
の予測器52より入力し、ゲイン行列算出器58より式
(48)のゲイン行列Kk を入力し、式(76)の加速
度ベクトル推定値uk-1 を加速度ベクトル推定器59よ
り入力し、目標運動判定結果FMDk を加速度ベクトル
推定値による目標運動判定器60より入力し、目標運動
判定結果FMDk が1(曲進)である場合には、式(8
1)〜式(83)にしたがいn個の運動モデルによる平
滑値ベクトルx^k (+)を算出し、目標運動判定結果
FMDk が0(直進)である場合には、式(95)およ
び式(96)、式(97)にしたがいn個の運動モデル
による平滑値ベクトルxk (+)を算出する。
おいて、平滑器62によりサンプリング時刻tk-1 に算
出しておいた式(58)のn個の運動モデルによる平滑
値ベクトルx^k-1 (+)を第6の遅延回路69を通し
入力し、n個の運動モデルによる信頼度算出器57より
式(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,1
の信頼度βk,i,Lkを入力し、相関器56より探知データ
ベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルzk,mk
を入力し、サンプリング時刻tk-1 に算出しておいた式
(56)の運動モデルごとの予測値ベクトルx^
Lk k(−)を第4の遅延回路67を通し運動モデルごと
の予測器52より入力し、ゲイン行列算出器58より式
(48)のゲイン行列Kk を入力し、式(76)の加速
度ベクトル推定値uk-1 を加速度ベクトル推定器59よ
り入力し、目標運動判定結果FMDk を加速度ベクトル
推定値による目標運動判定器60より入力し、目標運動
判定結果FMDk が1(曲進)である場合には、式(8
1)〜式(83)にしたがいn個の運動モデルによる平
滑値ベクトルx^k (+)を算出し、目標運動判定結果
FMDk が0(直進)である場合には、式(95)およ
び式(96)、式(97)にしたがいn個の運動モデル
による平滑値ベクトルxk (+)を算出する。
【0106】またn個の運動モデルによる平滑誤差評価
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を加速度ベクトル推定値による目標運動判定器60より
入力し、目標運動判定結果FMDk が1(曲進)である
場合には、式(87)および式(82)、式(83)に
したがい式(60)のn個の運動モデルによる平滑誤差
共分散行列Pk (+)を算出し、目標運動判定結果FM
Dk が0(直進)である場合には、式(98)および式
(96)、式(97)にしたがい式(60)のn個の運
動モデルによる平滑誤差共分散行列Pk (+)を算出す
る(ステップ13)。追尾終了になるまでルーウ内のこ
と一連の流れを繰り返す(ステップ14)。
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を加速度ベクトル推定値による目標運動判定器60より
入力し、目標運動判定結果FMDk が1(曲進)である
場合には、式(87)および式(82)、式(83)に
したがい式(60)のn個の運動モデルによる平滑誤差
共分散行列Pk (+)を算出し、目標運動判定結果FM
Dk が0(直進)である場合には、式(98)および式
(96)、式(97)にしたがい式(60)のn個の運
動モデルによる平滑誤差共分散行列Pk (+)を算出す
る(ステップ13)。追尾終了になるまでルーウ内のこ
と一連の流れを繰り返す(ステップ14)。
【0107】次にこの発明の実施例4を図7および図8
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
【0108】以下、ループ内の処理の流れを示す。説明
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^ k-1(+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^ K
(−)を算出する。
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^ k-1(+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^ K
(−)を算出する。
【0109】運動モデルごとの予測誤差評価器53は、
n個の運動モデルによる平滑誤差評価器63より入力さ
れるn個の運動モデルによる平滑誤差共分散行列Pk-1
(+)およびあらかじめ設定された式(7)の駆動雑音
共分散行列QK-1 より式(47)にしたがい運動モデル
ごとの予測誤差共分散行列PLk k (−)を算出する(ス
テップ3)。
n個の運動モデルによる平滑誤差評価器63より入力さ
れるn個の運動モデルによる平滑誤差共分散行列Pk-1
(+)およびあらかじめ設定された式(7)の駆動雑音
共分散行列QK-1 より式(47)にしたがい運動モデル
ごとの予測誤差共分散行列PLk k (−)を算出する(ス
テップ3)。
【0110】次にn個の運動モデルによる予測器54に
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^ k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^ k-1(+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαLkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^ k(−)を算出する(ステッ
プ4)。
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^ k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^ k-1(+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαLkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^ k(−)を算出する(ステッ
プ4)。
【0111】次にn個の運動モデルによる予測誤差評価
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLklk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトルu^k- 1 よりPk (−)の予測誤差を大
きく評価する項、式(123)を算出し、式(93)に
したがいn個の運動モデルによる予測誤差共分散行列P
k (−)を算出する(ステップ5)。
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLklk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトルu^k- 1 よりPk (−)の予測誤差を大
きく評価する項、式(123)を算出し、式(93)に
したがいn個の運動モデルによる予測誤差共分散行列P
k (−)を算出する(ステップ5)。
【0112】次に相関器56は時刻がサンプリング時刻
tk-1 からサンプリング時刻tk に経過したのち、目標
観測装置51より目標およびクラッタ等からの位置およ
び距離変化率の検出結果をサンプリング時刻tk の探知
データとして入力する(ステップ6)。そして相関器5
6において、サンプリング時刻tk-1 に算出した式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力して、相関をとるべき中心位置である式(6
4)のベクトルzk (−)を式(16)においてベクト
ルx^ k(−)の代わりに定数加速度ベクトルαLkが零
ベクトルの場合のベクトルx^Lk k(−)を使用して算
出し、サンプリング時刻tk-1 に算出した式(66)の
n個の運動モデルによる予測誤差共分散行列P
Lk k (−)を第7の遅延回路70を通しn個の運動モデ
ルによる予測誤差評価器55より入力して、これとあら
かじめ設定された式(19)の観測雑音共分散行列Rk
および第1の遅延回路64を通してn個の運動モデルに
よる予測器54より入力してベクトルx^ K(−)よ
り、相関をとるべき領域の大きさ算出に使用する式(6
5)のSk を求め、式(63)により追尾対象目標と相
関の可能性のあるmk 個の探知データを選択し、これを
式(20)のZk とする(ステップ7)。
tk-1 からサンプリング時刻tk に経過したのち、目標
観測装置51より目標およびクラッタ等からの位置およ
び距離変化率の検出結果をサンプリング時刻tk の探知
データとして入力する(ステップ6)。そして相関器5
6において、サンプリング時刻tk-1 に算出した式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力して、相関をとるべき中心位置である式(6
4)のベクトルzk (−)を式(16)においてベクト
ルx^ k(−)の代わりに定数加速度ベクトルαLkが零
ベクトルの場合のベクトルx^Lk k(−)を使用して算
出し、サンプリング時刻tk-1 に算出した式(66)の
n個の運動モデルによる予測誤差共分散行列P
Lk k (−)を第7の遅延回路70を通しn個の運動モデ
ルによる予測誤差評価器55より入力して、これとあら
かじめ設定された式(19)の観測雑音共分散行列Rk
および第1の遅延回路64を通してn個の運動モデルに
よる予測器54より入力してベクトルx^ K(−)よ
り、相関をとるべき領域の大きさ算出に使用する式(6
5)のSk を求め、式(63)により追尾対象目標と相
関の可能性のあるmk 個の探知データを選択し、これを
式(20)のZk とする(ステップ7)。
【0113】次にn個の運動モデルによる信頼度算出器
57は、信頼度算出器57でサンプリング時刻tk-1 に
式(39)により算出した各運動モデルの信頼度βLk-1
k-1を第3の遅延回路66を通して入力し、サンプリン
グ時刻tk-1 に算出した式(56)の運動モデルごとの
予測値ベクトルx^Lk k(−)を第4の遅延回路67を
通し運動モデルごとの予測器52より入力し、サンプリ
ング時刻tk-1 に算出した式(61)の運動モデルごと
の予測誤差共分散行列PLk k (−)を第2の遅延回路6
5を通して運動モデルごとの予測誤差評価器53より入
力して、あらかじめ設定された式(19)の観測雑音共
分散行列Rk より式(69)、式(16)、式(17)
および式(53)、式(54)により仮説Ψk,lkのもと
での相関器56よりの探知データベクトルzk,i の信頼
度lLk(ベクトルzk,i )を求め、あらかじめ設定され
た探知確率PD 、式(67)の追尾対象目標が相関をと
るべき領域内に存在する確率PGkおよび式(13)の運
動モデルの推移確率PLklk -1により式(73)〜式(7
5)および式(39)、式(41)、式(43)にした
がい運動モデルおよび探知データの信頼度β
k,i,Lk,Lk-1 、βLk k 、βk,i, Lk、βk,i を算出する
(ステップ8)。
57は、信頼度算出器57でサンプリング時刻tk-1 に
式(39)により算出した各運動モデルの信頼度βLk-1
k-1を第3の遅延回路66を通して入力し、サンプリン
グ時刻tk-1 に算出した式(56)の運動モデルごとの
予測値ベクトルx^Lk k(−)を第4の遅延回路67を
通し運動モデルごとの予測器52より入力し、サンプリ
ング時刻tk-1 に算出した式(61)の運動モデルごと
の予測誤差共分散行列PLk k (−)を第2の遅延回路6
5を通して運動モデルごとの予測誤差評価器53より入
力して、あらかじめ設定された式(19)の観測雑音共
分散行列Rk より式(69)、式(16)、式(17)
および式(53)、式(54)により仮説Ψk,lkのもと
での相関器56よりの探知データベクトルzk,i の信頼
度lLk(ベクトルzk,i )を求め、あらかじめ設定され
た探知確率PD 、式(67)の追尾対象目標が相関をと
るべき領域内に存在する確率PGkおよび式(13)の運
動モデルの推移確率PLklk -1により式(73)〜式(7
5)および式(39)、式(41)、式(43)にした
がい運動モデルおよび探知データの信頼度β
k,i,Lk,Lk-1 、βLk k 、βk,i, Lk、βk,i を算出する
(ステップ8)。
【0114】次にゲイン行列算出器58において、サン
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^ k(−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^ k(−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
【0115】次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLK K を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
n個の運動モデルによる信頼度算出器57より式(3
2)のβLK K を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
【0116】次に目標距離変化率の予測値による目標運
動判定器61において、サンプリング時刻tk-1 に算出
した式(57)のn個の運動モデルによる予測値ベクト
ルx^ k(−)を第1の遅延回路64を通しn個の運動
モデルによる予測器54より入力し、探知データの信頼
度βk,i をn個の運動モデルによる信頼度算出器57よ
り入力し、目標運動判定器61でのサンプリング時刻t
k-1 における目標運動判定結果FMDk-1 を第5の遅延
回路68を通して入力し、相関器56より探知データの
相関をとるべき領域の大きさ算出のために使用した式
(65)のSk を入力し、さらに相関器56より探知デ
ータベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルz
k,mkを入力してその距離変化率成分をR. o k,1 ,R
. o k,2 ,…,R. o k,mkとし、式(113)、式(1
7)により目標距離変化率の予測値R . P K 、式(11
4)によりSk の第4行第4列要素Sk (4,4)、式
(116)により目標距離変化率の加重平均推定値R
. o k をそれぞれ求め、さらに式(118)にしたがい
スカラ量εk を算出した後、目標運動判定結果FMDk
を、式(119)が成立する場合には式(99)のよう
に設定し、また式(120)が成立する場合には式(1
00)のように設定し、また式(121)が成立する場
合には式(110)のように設定する(ステップ1
2)。
動判定器61において、サンプリング時刻tk-1 に算出
した式(57)のn個の運動モデルによる予測値ベクト
ルx^ k(−)を第1の遅延回路64を通しn個の運動
モデルによる予測器54より入力し、探知データの信頼
度βk,i をn個の運動モデルによる信頼度算出器57よ
り入力し、目標運動判定器61でのサンプリング時刻t
k-1 における目標運動判定結果FMDk-1 を第5の遅延
回路68を通して入力し、相関器56より探知データの
相関をとるべき領域の大きさ算出のために使用した式
(65)のSk を入力し、さらに相関器56より探知デ
ータベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルz
k,mkを入力してその距離変化率成分をR. o k,1 ,R
. o k,2 ,…,R. o k,mkとし、式(113)、式(1
7)により目標距離変化率の予測値R . P K 、式(11
4)によりSk の第4行第4列要素Sk (4,4)、式
(116)により目標距離変化率の加重平均推定値R
. o k をそれぞれ求め、さらに式(118)にしたがい
スカラ量εk を算出した後、目標運動判定結果FMDk
を、式(119)が成立する場合には式(99)のよう
に設定し、また式(120)が成立する場合には式(1
00)のように設定し、また式(121)が成立する場
合には式(110)のように設定する(ステップ1
2)。
【0117】次にn個の運動モデルによる平滑器62に
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1(+)を第6の遅延回路69を通し入力し、n個の
運動モデルによる信頼度算出器57より式(32)の各
運動モデルの信頼度βLk k および式(33)の仮説Ψk,
lkのもとでの探知データベクトルzk,i の信頼度β
k,i,Lkを入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
サンプリング時刻tk-1 に算出しておいた式(56)の
運動モデルごとの予測値ベクトルx^Lk k(−)を第4
の遅延回路67を通し運動モデルごとの予測器52より
入力し、ゲイン行列算出器58より式(48)のゲイン
行列Kk を入力し、式(76)の加速度ベクトル推定値
uk-1 を加速度ベクトル推定器59より入力し、目標運
動判定結果FMDk を目標距離変化率の予測値による目
標運動判定器61より入力し、目標運動判定結果FMD
k が1(曲進)である場合には、式(81)〜式(8
3)にしたがいn個の運動モデルによる平滑値ベクトル
xk(+)を算出し、目標運動判定結果FMDk が0
(直進)である場合には、式(95)および式(9
6)、式(97)にしたがいn個の運動モデルによる平
滑値ベクトルx^ k(+)を算出する。
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1(+)を第6の遅延回路69を通し入力し、n個の
運動モデルによる信頼度算出器57より式(32)の各
運動モデルの信頼度βLk k および式(33)の仮説Ψk,
lkのもとでの探知データベクトルzk,i の信頼度β
k,i,Lkを入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
サンプリング時刻tk-1 に算出しておいた式(56)の
運動モデルごとの予測値ベクトルx^Lk k(−)を第4
の遅延回路67を通し運動モデルごとの予測器52より
入力し、ゲイン行列算出器58より式(48)のゲイン
行列Kk を入力し、式(76)の加速度ベクトル推定値
uk-1 を加速度ベクトル推定器59より入力し、目標運
動判定結果FMDk を目標距離変化率の予測値による目
標運動判定器61より入力し、目標運動判定結果FMD
k が1(曲進)である場合には、式(81)〜式(8
3)にしたがいn個の運動モデルによる平滑値ベクトル
xk(+)を算出し、目標運動判定結果FMDk が0
(直進)である場合には、式(95)および式(9
6)、式(97)にしたがいn個の運動モデルによる平
滑値ベクトルx^ k(+)を算出する。
【0118】またn個の運動モデルにより平滑誤差評価
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を目標距離変化率の予測値による目標運動判定器61よ
り入力し、目標運動判定結果FMDk が1(曲進)であ
る場合には、式(87)および式(82)、式(83)
にしたがい式(60)のn個の運動モデルによる平滑誤
差共分散行列Pk (+)を算出し、目標運動判定結果F
MDk が0(直進)である場合には、式(98)および
式(96)、式(97)にしたがい式(60)のn個の
運動モデルによる平滑誤差共分散行列Pk (+)を算出
する(ステップ13)。追尾終了になるまでループ内の
この一連の流れを繰り返す(ステップ14)。
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を目標距離変化率の予測値による目標運動判定器61よ
り入力し、目標運動判定結果FMDk が1(曲進)であ
る場合には、式(87)および式(82)、式(83)
にしたがい式(60)のn個の運動モデルによる平滑誤
差共分散行列Pk (+)を算出し、目標運動判定結果F
MDk が0(直進)である場合には、式(98)および
式(96)、式(97)にしたがい式(60)のn個の
運動モデルによる平滑誤差共分散行列Pk (+)を算出
する(ステップ13)。追尾終了になるまでループ内の
この一連の流れを繰り返す(ステップ14)。
【0119】次にこの発明の実施例5を図5および図9
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
にしたがって説明する。目標観測装置51より得られる
目標位置および目標距離変化率の情報をもとに通常のカ
ルマンフィルタの理論に基づき、目標位置、速度の平滑
値および平滑誤差共分散行列の初期値を設定し(ステッ
プ1)、式(11)の例のように零加速度ベクトルを含
む式(12)のn個の定数加速度ベクトルによって構成
される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
【0120】以下、ループ内の処理の流れを示す。説明
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^ k-1(+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測ベクトルx^
K(−)を算出する。
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^ k-1(+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測ベクトルx^
K(−)を算出する。
【0121】また運動モデルごとの予測誤差評価器53
において、n個の運動モデルによる平滑誤差評価器63
より入力されるn個の運動モデルによる平滑誤差共分散
行列Pk-1 (+)およびあらかじめ設定された式(7)
の駆動雑音共分散行列Qk-1より式(47)にしたがい
運動モデルごとの予測誤差共分散行列PLk k (−)を算
出する(ステップ3)。
において、n個の運動モデルによる平滑誤差評価器63
より入力されるn個の運動モデルによる平滑誤差共分散
行列Pk-1 (+)およびあらかじめ設定された式(7)
の駆動雑音共分散行列Qk-1より式(47)にしたがい
運動モデルごとの予測誤差共分散行列PLk k (−)を算
出する(ステップ3)。
【0122】次にn個の運動モデルによる予測器54に
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^ k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^ k-1(+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαLkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^ k(−)を算出する(ステッ
プ4)。
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^ k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^ k-1(+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαLkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^ k(−)を算出する(ステッ
プ4)。
【0123】次にn個の運動モデルによる予測誤差評価
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLkLk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトルu^K- 1 よりPk (−)の予測誤差を大
きく評価する項、式(123)を算出し、式(93)に
したがいn個の運動モデルによる予測誤差共分散行列P
k (−)を算出する(ステップ5)。
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLkLk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトルu^K- 1 よりPk (−)の予測誤差を大
きく評価する項、式(123)を算出し、式(93)に
したがいn個の運動モデルによる予測誤差共分散行列P
k (−)を算出する(ステップ5)。
【0124】次に相関器56は時刻がサンプリング時刻
tk-1 からサンプリング時刻tk に経過したのち、目標
観測装置51より目標およびクラッタ等からの位置およ
び距離変化率の検出結果をサンプリング時刻tk の探知
データとして入力する(ステップ6)。次に相関器56
において、サンプリング時刻tk-1 に算出した式(5
7)のn個の運動モデルによる予測値ベクトルx^
k(−)を第1の遅延回路64を通しn個の運動モデル
による予測器54より入力して、相関をとるべき中心位
置である式(64)のベクトルzk (−)を式(16)
により算出し、サンプリング時刻tk-1 に算出した式
(61)のn個のモデルによる予測誤差共分散行列Pk
(−)を第7の遅延回路70を通しn個の運動モデルに
よる予測誤差評価器55より入力して、これとあらかじ
め設定された式(19)の観測雑音共分散行列Rk およ
び第1の遅延回路64を通してn個の運動モデルによる
予測器54より入力したベクトルx^ k(−)より、相
関をとるべき領域の大きさ算出に使用する式(65)の
Sk を求め、式(63)により追尾対象目標と相関の可
能性のあるmk 個の探知データを選択し、これを式(2
0)のzk とする(ステップ7)。
tk-1 からサンプリング時刻tk に経過したのち、目標
観測装置51より目標およびクラッタ等からの位置およ
び距離変化率の検出結果をサンプリング時刻tk の探知
データとして入力する(ステップ6)。次に相関器56
において、サンプリング時刻tk-1 に算出した式(5
7)のn個の運動モデルによる予測値ベクトルx^
k(−)を第1の遅延回路64を通しn個の運動モデル
による予測器54より入力して、相関をとるべき中心位
置である式(64)のベクトルzk (−)を式(16)
により算出し、サンプリング時刻tk-1 に算出した式
(61)のn個のモデルによる予測誤差共分散行列Pk
(−)を第7の遅延回路70を通しn個の運動モデルに
よる予測誤差評価器55より入力して、これとあらかじ
め設定された式(19)の観測雑音共分散行列Rk およ
び第1の遅延回路64を通してn個の運動モデルによる
予測器54より入力したベクトルx^ k(−)より、相
関をとるべき領域の大きさ算出に使用する式(65)の
Sk を求め、式(63)により追尾対象目標と相関の可
能性のあるmk 個の探知データを選択し、これを式(2
0)のzk とする(ステップ7)。
【0125】次にn個の運動モデルによる信頼度算出器
57において、信頼度算出器57でサンプリング時刻t
k-1 に式(39)により算出した各運動モデルの信頼度
βLk -1 k-1 を第3の遅延回路66を通して入力し、サン
プリング時刻tk-1 に算出した式(56)の運動モデル
ごとの予測値ベクトルx^Lk k(−)を第4の遅延回路
67を通し運動モデルごとの予測器52より入力し、サ
ンプリング時刻tk-1に算出した式(61)の運動モデ
ルごとの予測誤差共分散行列PLk k (−)を第2の遅延
回路65を通して運動モデルごとの予測誤差評価器53
より入力して、あらかじめ設定された式(19)の観測
雑音共分散行列Rk より式(69)、式(16)、式
(17)および式(53)、式(54)により仮説Ψ
k,lkのもとでの相関器56よりの探知データベクトルz
k,i の信頼度lLk(ベクトルzk,i )を求め、あらかじ
め設定された探知確率PD 、式(67)の追尾対象目標
が相関をとるべき領域内に存在する確率PGkおよび式
(13)の運動モデルの推移確率PLkLk-1により式(7
3)〜式(75)および式(39)、式(41)、式
(43)にしたがい運動モデルおよび探知データの信頼
度βk,i,Lk,Lk-1 、βLk k 、βk,i,Lk、βk,i を算出す
る(ステップ8)。
57において、信頼度算出器57でサンプリング時刻t
k-1 に式(39)により算出した各運動モデルの信頼度
βLk -1 k-1 を第3の遅延回路66を通して入力し、サン
プリング時刻tk-1 に算出した式(56)の運動モデル
ごとの予測値ベクトルx^Lk k(−)を第4の遅延回路
67を通し運動モデルごとの予測器52より入力し、サ
ンプリング時刻tk-1に算出した式(61)の運動モデ
ルごとの予測誤差共分散行列PLk k (−)を第2の遅延
回路65を通して運動モデルごとの予測誤差評価器53
より入力して、あらかじめ設定された式(19)の観測
雑音共分散行列Rk より式(69)、式(16)、式
(17)および式(53)、式(54)により仮説Ψ
k,lkのもとでの相関器56よりの探知データベクトルz
k,i の信頼度lLk(ベクトルzk,i )を求め、あらかじ
め設定された探知確率PD 、式(67)の追尾対象目標
が相関をとるべき領域内に存在する確率PGkおよび式
(13)の運動モデルの推移確率PLkLk-1により式(7
3)〜式(75)および式(39)、式(41)、式
(43)にしたがい運動モデルおよび探知データの信頼
度βk,i,Lk,Lk-1 、βLk k 、βk,i,Lk、βk,i を算出す
る(ステップ8)。
【0126】次にゲイン行列算出器58において、サン
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^ k(−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^ k(−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
【0127】次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
【0128】次に加速度ベクトル推定値による目標運動
判定器60において、式(76)の加速度ベクトル推定
値uk-1 を加速度ベクトル推定器59より入力し、判定
器60でのサンプリング時刻tk-1 における目標運動判
定結果FMDk-1 を第5の遅延回路68を通して入力
し、式(103)〜式(105)にしたがい加速度ベク
トル推定値の平滑ベクトル推定値uS K-1 を算出し、さ
らに式(106)にしたがい加速度ベクトル推定値の平
滑ベクトル推定値uS K-1 のユークリッドノルムベクト
ル推定値‖uS k-1 ‖を算出した後、目標運動判定結果
FMDk を、式(107)が成立する場合には式(9
9)のように設定し、また式(108)が成立する場合
には式(100)のように設定し、また式(109)が
成立する場合には式(110)のように設定する(ステ
ップ11)。
判定器60において、式(76)の加速度ベクトル推定
値uk-1 を加速度ベクトル推定器59より入力し、判定
器60でのサンプリング時刻tk-1 における目標運動判
定結果FMDk-1 を第5の遅延回路68を通して入力
し、式(103)〜式(105)にしたがい加速度ベク
トル推定値の平滑ベクトル推定値uS K-1 を算出し、さ
らに式(106)にしたがい加速度ベクトル推定値の平
滑ベクトル推定値uS K-1 のユークリッドノルムベクト
ル推定値‖uS k-1 ‖を算出した後、目標運動判定結果
FMDk を、式(107)が成立する場合には式(9
9)のように設定し、また式(108)が成立する場合
には式(100)のように設定し、また式(109)が
成立する場合には式(110)のように設定する(ステ
ップ11)。
【0129】次にn個の運動モデルによる平滑器62に
おいて、平滑器62でサンプリング時刻tk-1 に算出し
ておいた式(58)のn個の運動モデルによる平滑値ベ
クトルx^ k-1(+)を第6の遅延回路69を通し入力
し、n個の運動モデルによる信頼度算出器57より式
(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,i
の信頼度βk,i,Lkを入力し、相関器56より探知データ
ベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルz k,mk
を入力し、サンプリング時刻tk-1 に算出しておいた式
(56)の運動モデルごとの予測値ベクトルx^
Lk k(−)を第4の遅延回路67を通し運動モデルごと
の予測器52より入力し、ゲイン行列算出器58より式
(48)のゲイン行列Kk を入力し、式(76)の加速
度ベクトル推定値uk-1 を加速度ベクトル推定器59よ
り入力し、目標運動判定結果FMDk を加速度ベクトル
推定値による目標運動判定器60より入力し、目標運動
判定結果FMDk が1(曲進)である場合には、式(8
1)〜式(83)にしたがいn個の運動モデルによる平
滑値ベクトルx^ k(+)を算出し、目標運動判定結果
FMDk が0(直進)である場合には、式(95)およ
び式(96)、式(97)にしたがいn個の運動モデル
による平滑値ベクトルxk (+)を算出する。
おいて、平滑器62でサンプリング時刻tk-1 に算出し
ておいた式(58)のn個の運動モデルによる平滑値ベ
クトルx^ k-1(+)を第6の遅延回路69を通し入力
し、n個の運動モデルによる信頼度算出器57より式
(32)の各運動モデルの信頼度βLk k および式(3
3)の仮説Ψk,lkのもとでの探知データベクトルzk,i
の信頼度βk,i,Lkを入力し、相関器56より探知データ
ベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルz k,mk
を入力し、サンプリング時刻tk-1 に算出しておいた式
(56)の運動モデルごとの予測値ベクトルx^
Lk k(−)を第4の遅延回路67を通し運動モデルごと
の予測器52より入力し、ゲイン行列算出器58より式
(48)のゲイン行列Kk を入力し、式(76)の加速
度ベクトル推定値uk-1 を加速度ベクトル推定器59よ
り入力し、目標運動判定結果FMDk を加速度ベクトル
推定値による目標運動判定器60より入力し、目標運動
判定結果FMDk が1(曲進)である場合には、式(8
1)〜式(83)にしたがいn個の運動モデルによる平
滑値ベクトルx^ k(+)を算出し、目標運動判定結果
FMDk が0(直進)である場合には、式(95)およ
び式(96)、式(97)にしたがいn個の運動モデル
による平滑値ベクトルxk (+)を算出する。
【0130】またn個の運動モデルによる平滑誤差評価
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を加速度ベクトル推定値による目標運動判定器60より
入力し、目標運動判定結果FMDk が1(曲進)である
場合には、式(87)および式(82)、式(83)に
したがい式(60)のn個の運動モデルによる平滑誤差
共分散行列Pk (+)を算出し、目標運動判定結果FM
Dk が0(直進)である場合には、式(98)および式
(96)、式(97)にしたがい式(60)のn個の運
動モデルによる平滑誤差共分散行列Pk (+)を算出す
る(ステップ13)。追尾終了になるまでループ内のこ
の一連の流れを繰り返す(ステップ14)。
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を加速度ベクトル推定値による目標運動判定器60より
入力し、目標運動判定結果FMDk が1(曲進)である
場合には、式(87)および式(82)、式(83)に
したがい式(60)のn個の運動モデルによる平滑誤差
共分散行列Pk (+)を算出し、目標運動判定結果FM
Dk が0(直進)である場合には、式(98)および式
(96)、式(97)にしたがい式(60)のn個の運
動モデルによる平滑誤差共分散行列Pk (+)を算出す
る(ステップ13)。追尾終了になるまでループ内のこ
の一連の流れを繰り返す(ステップ14)。
【0131】次にこの発明の実施例6を図7および図1
0にしたがって説明する。目標観測装置51より得られ
る目標位置および目標距離変化率の情報をもとに通常の
カルマンフィルタの理論に基づき、目標位置、速度の平
滑値および平滑誤差共分散行列の初期値を設定し(ステ
ップ1)、式(11)の例のように零加速度ベクトルを
含む式(12)のn個の定数加速度ベクトルによって構
成される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
0にしたがって説明する。目標観測装置51より得られ
る目標位置および目標距離変化率の情報をもとに通常の
カルマンフィルタの理論に基づき、目標位置、速度の平
滑値および平滑誤差共分散行列の初期値を設定し(ステ
ップ1)、式(11)の例のように零加速度ベクトルを
含む式(12)のn個の定数加速度ベクトルによって構
成される目標のn個の運動モデルを設定した(ステップ
2)のち、追尾を継続するための処理のループに入る。
【0132】以下、ループ内の処理の流れを示す。説明
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^ k-1(+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^ k
(−)を算出し、また運動モデルごとの予測誤差評価器
53において、n個の運動モデルによる平滑誤差評価器
63より入力されるn個の運動モデルによる平滑誤差共
分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。
の便宜上ループ先頭においてのサンプリング時刻がt
k-1 であったものとする。運動モデルごとの予測器52
において、n個の運動モデルによる平滑器62より入力
される式(58)のn個の運動モデルによる平滑値ベク
トルx^ k-1(+)およびn個の運動モデルを構成して
いる式(12)の定数加速度ベクトルαLkより式(4
6)にしたがい運動モデルごとの予測値ベクトルx^ k
(−)を算出し、また運動モデルごとの予測誤差評価器
53において、n個の運動モデルによる平滑誤差評価器
63より入力されるn個の運動モデルによる平滑誤差共
分散行列Pk-1 (+)およびあらかじめ設定された式
(7)の駆動雑音共分散行列Qk-1 より式(47)にし
たがい運動モデルごとの予測誤差共分散行列P
Lk k (−)を算出する(ステップ3)。
【0133】次にn個の運動モデルによる予測器54に
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^ k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^ k-1(+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαLkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^ k(−)を算出する(ステッ
プ4)。
おいて、n個の運動モデルによる平滑器62より式(5
8)のn個の運動モデルによる平滑値ベクトルx^ k-1
(+)を入力して等速直進運動予測によりΦk-1 ベクト
ルx^ k-1(+)を算出し、n個の運動モデルによる信
頼度算出器57より得られる式(32)の各運動モデル
の信頼度βLk-1 k-1 、あらかじめ設定された式(13)
の推移確率PLkLk-1および式(12)の定数加速度ベク
トルαLkより式(90)にしたがいサンプリング時刻t
k における探知情報が得られない時点での加速度の影響
項を推定して、式(88)にしたがいn個の運動モデル
による予測値ベクトルx^ k(−)を算出する(ステッ
プ4)。
【0134】次にn個の運動モデルによる予測誤差評価
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLkLk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトル推定値uk-1 よりPk (−)の予測誤差
を大きく評価する項、式(123)を算出し、式(9
3)にしたがいn個の運動モデルによる予測誤差共分散
行列Pk (−)を算出する(ステップ5)。
器55において、運動モデルごとの予測誤差評価器53
より式(61)の運動モデルごとの予測誤差共分散行列
PLk k (−)を入力し、n個の運動モデルによる信頼度
算出器57より得られる式(32)のβLk-1 k-1 、あら
かじめ設定された式(13)の推移確率PLkLk-1、あら
かじめ設定された定数加速度ベクトルαLkおよび式(9
0)のベクトル推定値uk-1 よりPk (−)の予測誤差
を大きく評価する項、式(123)を算出し、式(9
3)にしたがいn個の運動モデルによる予測誤差共分散
行列Pk (−)を算出する(ステップ5)。
【0135】次に時刻がサンプリング時刻tk-1 からサ
ンプリング時刻tk に経過したのち、目標観測装置51
より目標およびクラッタ等からの位置および距離変化率
の検出結果をサンプリング時刻tk の探知データとして
入力する(ステップ6)。
ンプリング時刻tk に経過したのち、目標観測装置51
より目標およびクラッタ等からの位置および距離変化率
の検出結果をサンプリング時刻tk の探知データとして
入力する(ステップ6)。
【0136】次に相関器56において、サンプリング時
刻tk-1 に算出した式(57)のn個の運動モデルによ
る予測値ベクトルx^ k(−)を第1の遅延回路64を
通しn個の運動モデルによる予測器54より入力して、
相関をとるべき中心位置である式(64)のベクトルz
k (−)を式(16)により算出し、サンプリング時刻
tk-1 に算出した式(61)の運動モデルごとの予測誤
差共分散行列PLk k (−)を第2の遅延回路65を通し
運動モデルごとの予測誤差評価器53より入力して、こ
れとあらかじめ設定された式(19)の観測雑音共分散
行列Rk および第1の遅延回路64を通してn個の運動
モデルによる予測器54より入力したベクトルx^
k(−)より、相関をとるべき領域の大きさ算出に使用
する式(65)のSk を求め、式(63)により追尾対
象目標と相関の可能性のあるmk 個の探知データを選択
し、これを式(20)のZk とする(ステップ7)。
刻tk-1 に算出した式(57)のn個の運動モデルによ
る予測値ベクトルx^ k(−)を第1の遅延回路64を
通しn個の運動モデルによる予測器54より入力して、
相関をとるべき中心位置である式(64)のベクトルz
k (−)を式(16)により算出し、サンプリング時刻
tk-1 に算出した式(61)の運動モデルごとの予測誤
差共分散行列PLk k (−)を第2の遅延回路65を通し
運動モデルごとの予測誤差評価器53より入力して、こ
れとあらかじめ設定された式(19)の観測雑音共分散
行列Rk および第1の遅延回路64を通してn個の運動
モデルによる予測器54より入力したベクトルx^
k(−)より、相関をとるべき領域の大きさ算出に使用
する式(65)のSk を求め、式(63)により追尾対
象目標と相関の可能性のあるmk 個の探知データを選択
し、これを式(20)のZk とする(ステップ7)。
【0137】次にn個の運動モデルによる信頼度算出器
57において、信頼度算出器57でサンプリング時刻t
k-1 に式(39)により算出した各運動モデルの信頼度
βLk -1 k-1 を第3の遅延回路66を通して入力し、サン
プリング時刻tk-1 に算出した式(56)の運動モデル
ごとの予測値ベクトルx^Lk k(−)を第4の遅延回路
67を通し運動モデルごとの予測器52より入力し、サ
ンプリング時刻tk-1に算出した式(61)の運動モデ
ルごとの予測誤差共分散行列PLk k (−)を第2の遅延
回路65を通して運動モデルごとの予測誤差評価器53
より入力して、あらかじめ設定された式(19)の観測
雑音共分散行列Rk より式(69)、式(16)、式
(17)および式(53)、式(54)により仮説Ψ
k,lkのもとでの相関器56よりの探知データベクトルz
k,i の信頼度1Lk(ベクトルzk,i )を求め、あらかじ
め設定された探知確率PD 、式(67)の追尾対象目標
が相関をとるべき領域内に存在する確率PGkおよび式
(13)の運動モデルの推移確率PLkLk-1により式(7
3)〜式(75)および式(39)、式(41)、式
(43)にしたがい運動モデルおよび探知データの信頼
度βk,i,Lk,Lk-1 、βLk k 、βk,i,Lk、βk,i を算出す
る(ステップ8)。
57において、信頼度算出器57でサンプリング時刻t
k-1 に式(39)により算出した各運動モデルの信頼度
βLk -1 k-1 を第3の遅延回路66を通して入力し、サン
プリング時刻tk-1 に算出した式(56)の運動モデル
ごとの予測値ベクトルx^Lk k(−)を第4の遅延回路
67を通し運動モデルごとの予測器52より入力し、サ
ンプリング時刻tk-1に算出した式(61)の運動モデ
ルごとの予測誤差共分散行列PLk k (−)を第2の遅延
回路65を通して運動モデルごとの予測誤差評価器53
より入力して、あらかじめ設定された式(19)の観測
雑音共分散行列Rk より式(69)、式(16)、式
(17)および式(53)、式(54)により仮説Ψ
k,lkのもとでの相関器56よりの探知データベクトルz
k,i の信頼度1Lk(ベクトルzk,i )を求め、あらかじ
め設定された探知確率PD 、式(67)の追尾対象目標
が相関をとるべき領域内に存在する確率PGkおよび式
(13)の運動モデルの推移確率PLkLk-1により式(7
3)〜式(75)および式(39)、式(41)、式
(43)にしたがい運動モデルおよび探知データの信頼
度βk,i,Lk,Lk-1 、βLk k 、βk,i,Lk、βk,i を算出す
る(ステップ8)。
【0138】次にゲイン行列算出器58において、サン
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^ k(−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
プリング時刻tk-1 に算出した式(61)の運動モデル
ごとの予測誤差共分散行列PLk k (−)を第2の遅延回
路65を通し運動モデルごとの予測誤差評価器53より
入力し、サンプリング時刻t k-1 に算出した式(57)
のn個の運動モデルによる予測値ベクトルx^ k(−)
を第1の遅延回路64を通しn個の運動モデルによる予
測器54より入力し、あらかじめ設定された式(19)
の観測雑音共分散行列Rk より式(48)および式(5
3)、式(54)にしたがいゲイン行列を算出する(ス
テップ9)。
【0139】次に加速度ベクトル推定器59において、
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
n個の運動モデルによる信頼度算出器57より式(3
2)のβLk k を入力し、あらかじめ設定された式(1
2)の定数加速度ベクトルαLkを用い、式(78)にし
たがい式(76)の加速度ベクトル推定値を算出する
(ステップ10)。
【0140】次に目標距離変化率の予測値による目標運
動判定器61において、サンプリング時刻tk-1 に算出
した式(57)のn個の運動モデルによる予測値ベクト
ルx^ k(−)を第1の遅延回路64を通しn個の運動
モデルによる予測器54より入力し、探知データの信頼
度βk,i をn個の運動モデルによる信頼度算出器57よ
り入力し、目標運動判定器61でのサンプリング時刻t
k-1 における目標運動判定結果FMDk-1 を第5の遅延
回路68を通して入力し、相関器56より探知データの
相関をとるべき領域の大きさ算出のために使用した式
(65)のSk を入力し、さらに相関器56より探知デ
ータベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルz
k,mkを入力してその距離変化率成分をR. o k,1 ,R
. o k,2 ,…,R. o k,mkとし、式(113)、式(1
7)により目標距離変化率の予測値R . P k 、式(11
4)によりSk の第4行第4列要素Sk (4,4)、式
(116)により目標距離変化率の加重平均推定値R
. o k をそれぞれ求め、さらに式(118)にしたがい
スカラ量εk を算出した後、目標運動判定結果FMDk
を、式(119)が成立する場合には式(99)のよう
に設定し、また式(120)が成立する場合には式(1
00)のように設定し、また式(121)が成立する場
合には式(110)のように設定する(ステップ1
1)。
動判定器61において、サンプリング時刻tk-1 に算出
した式(57)のn個の運動モデルによる予測値ベクト
ルx^ k(−)を第1の遅延回路64を通しn個の運動
モデルによる予測器54より入力し、探知データの信頼
度βk,i をn個の運動モデルによる信頼度算出器57よ
り入力し、目標運動判定器61でのサンプリング時刻t
k-1 における目標運動判定結果FMDk-1 を第5の遅延
回路68を通して入力し、相関器56より探知データの
相関をとるべき領域の大きさ算出のために使用した式
(65)のSk を入力し、さらに相関器56より探知デ
ータベクトルzk,1 ,ベクトルzk,2 ,…,ベクトルz
k,mkを入力してその距離変化率成分をR. o k,1 ,R
. o k,2 ,…,R. o k,mkとし、式(113)、式(1
7)により目標距離変化率の予測値R . P k 、式(11
4)によりSk の第4行第4列要素Sk (4,4)、式
(116)により目標距離変化率の加重平均推定値R
. o k をそれぞれ求め、さらに式(118)にしたがい
スカラ量εk を算出した後、目標運動判定結果FMDk
を、式(119)が成立する場合には式(99)のよう
に設定し、また式(120)が成立する場合には式(1
00)のように設定し、また式(121)が成立する場
合には式(110)のように設定する(ステップ1
1)。
【0141】次にn個の運動モデルによる平滑器62に
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1(+)を第6の遅延回路69を通し入力し、n個の
運動モデルによる信頼度算出器57より式(32)の各
運動モデルの信頼度βLk k および式(33)の仮説Ψk,
lkのもとでの探知データベクトルzk,i の信頼度β
k,i,Lkを入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
サンプリング時刻tk-1 に算出しておいた式(56)の
運動モデルごとの予測値ベクトルx^Lk k(−)を第4
の遅延回路67を通し運動モデルごとの予測器52より
入力し、ゲイン行列算出器58より式(48)のゲイン
行列Kk を入力し、式(76)の加速度ベクトル推定値
uk-1 を加速度ベクトル推定器59より入力し、目標運
動判定結果FMDk を目標距離変化率の予測値による目
標運動判定器61より入力し、目標運動判定結果FMD
k が1(曲進)である場合には、式(81)〜式(8
3)にしたがいn個の運動モデルによる平滑値ベクトル
xk(+)を算出し、目標運動判定結果FMDk が0
(直進)である場合には、式(95)および式(9
6)、式(97)にしたがいn個の運動モデルによる平
滑値ベクトルx^ k(+)を算出する。
おいて、サンプリング時刻tk-1 に算出しておいた式
(58)のn個の運動モデルによる平滑値ベクトルx^
k-1(+)を第6の遅延回路69を通し入力し、n個の
運動モデルによる信頼度算出器57より式(32)の各
運動モデルの信頼度βLk k および式(33)の仮説Ψk,
lkのもとでの探知データベクトルzk,i の信頼度β
k,i,Lkを入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
サンプリング時刻tk-1 に算出しておいた式(56)の
運動モデルごとの予測値ベクトルx^Lk k(−)を第4
の遅延回路67を通し運動モデルごとの予測器52より
入力し、ゲイン行列算出器58より式(48)のゲイン
行列Kk を入力し、式(76)の加速度ベクトル推定値
uk-1 を加速度ベクトル推定器59より入力し、目標運
動判定結果FMDk を目標距離変化率の予測値による目
標運動判定器61より入力し、目標運動判定結果FMD
k が1(曲進)である場合には、式(81)〜式(8
3)にしたがいn個の運動モデルによる平滑値ベクトル
xk(+)を算出し、目標運動判定結果FMDk が0
(直進)である場合には、式(95)および式(9
6)、式(97)にしたがいn個の運動モデルによる平
滑値ベクトルx^ k(+)を算出する。
【0142】またn個の運動モデルによる平滑誤差評価
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を目標距離変化率の予測値による目標運動判定器61よ
り入力し、目標運動判定結果FMDk が1(曲進)であ
る場合には、式(87)および式(82)、式(83)
にしたがい式(60)のn個の運動モデルによる平滑誤
差共分散行列Pk (+)を算出、目標運動判定結果FM
Dk が0(直進)である場合には、式(98)および式
(96)、式(97)にしたがい式(60)のn個の運
動モデルによる平滑誤差共分散行列Pk (+)を算出す
る(ステップ13)。追尾終了になるまでループ内のこ
の一連の流れを繰り返す(ステップ14)。
器63において、n個の運動モデルによる信頼度算出器
57より式(31)のβk,i 、式(32)のβLk k およ
び式(33)のβk,i,Lkを入力し、サンプリング時刻t
k-1 に算出した式(61)の運動モデルごとの予測誤差
共分散行列PLk k (−)を第2の遅延回路65を通し運
動モデルごとの予測誤差評価器53より入力し、ゲイン
行列算出器58より式(48)のゲイン行列Kk を入力
し、サンプリング時刻tk-1 に算出しておいた式(5
6)の運動モデルごとの予測値ベクトルx^Lk k(−)
を第4の遅延回路67を通し運動モデルごとの予測器5
2より入力し、サンプリング時刻tk-1 に算出したn個
の運動モデルによる予測値ベクトルx^ k(−)を第1
の遅延回路64を通しn個の運動モデルによる予測器5
4より入力し、相関器56より探知データベクトルz
k,1 ,ベクトルzk,2 ,…,ベクトルzk,mkを入力し、
式(76)の加速度ベクトル推定値uk-1 を加速度ベク
トル推定器59より入力し、目標運動判定結果FMDk
を目標距離変化率の予測値による目標運動判定器61よ
り入力し、目標運動判定結果FMDk が1(曲進)であ
る場合には、式(87)および式(82)、式(83)
にしたがい式(60)のn個の運動モデルによる平滑誤
差共分散行列Pk (+)を算出、目標運動判定結果FM
Dk が0(直進)である場合には、式(98)および式
(96)、式(97)にしたがい式(60)のn個の運
動モデルによる平滑誤差共分散行列Pk (+)を算出す
る(ステップ13)。追尾終了になるまでループ内のこ
の一連の流れを繰り返す(ステップ14)。
【0143】
【発明の効果】以上のようにこの発明によれば、通常の
目標自動追尾装置に特別の付加装置を付けることもな
く、目標運動諸元算出精度を向上させることができる。
なお、以上は等速直進運動モデルに定数加速度ベクトル
が付加されたモデルの場合について説明したが、これ以
外の複数個の運動モデルを有して目標観測装置の情報よ
り目標運動諸元を算出する多目標追尾方法および多目標
追尾装置に適用できる。
目標自動追尾装置に特別の付加装置を付けることもな
く、目標運動諸元算出精度を向上させることができる。
なお、以上は等速直進運動モデルに定数加速度ベクトル
が付加されたモデルの場合について説明したが、これ以
外の複数個の運動モデルを有して目標観測装置の情報よ
り目標運動諸元を算出する多目標追尾方法および多目標
追尾装置に適用できる。
【図1】この発明の多目標追尾装置の実施例1の処理手
順を示す図である。
順を示す図である。
【図2】この発明の多目標追尾装置の実施例1の構成を
示す図である。
示す図である。
【図3】この発明の多目標追尾装置の実施例2の処理手
順を示す図である。
順を示す図である。
【図4】この発明の多目標追尾装置の実施例2の構成を
示す図である。
示す図である。
【図5】この発明の多目標追尾装置の実施例3,5の処
理手順を示す図である。
理手順を示す図である。
【図6】この発明の多目標追尾装置の実施例3の構成を
示す図である。
示す図である。
【図7】この発明の多目標追尾装置の実施例4,6の処
理手順を示す図である。
理手順を示す図である。
【図8】この発明の多目標追尾装置の実施例4の構成を
示す図である。
示す図である。
【図9】この発明の多目標追尾装置の実施例5の構成を
示す図である。
示す図である。
【図10】この発明の多目標追尾装置の実施例6の構成
を示す図である。
を示す図である。
【図11】定数加速度ベクトルを説明する図である。
【図12】追尾目標と探知データの相関を説明する図で
ある。
ある。
【図13】従来の多目標追尾方法の一実施例の処理手順
を示す図である。
を示す図である。
【図14】従来の多目標追尾装置の一実施例の構成を示
す図である。
す図である。
1 初期値設定ステップ 2 n個の運動モデル設定ステップ 3 運動モデルごとの予測値および予測誤差共分散行列
算出ステップ 4 n個の運動モデルによる予測値算出ステップ 5 n個の運動モデルによる予測誤差共分散行列算出ス
テップ 6 探知データ入力ステップ 7 探知データ選択ステップ 8 運動モデルおよび探知データの信頼度算出ステップ 9 ゲイン行列算出ステップ 10 加速度ベクトル推定ステップ 11 加速度ベクトル推定値による目標運動判定ステッ
プ 12 目標距離変化率の予測値による目標運動判定ステ
ップ 13 n個の運動モデルによる平滑値および平滑誤差共
分散行列算出ステップ 14 終了判定ステップ 15 等速直進運動モデル設定ステップ 16 等速直進運動モデルによる予測値算出ステップ 17 等速直進運動モデルによる予測誤差共分散行列算
出ステップ 18 探知データの信頼度算出ステップ 19 等速直進運動モデルによる平滑値および平滑誤差
共分散行列算出ステップ 51 目標観測装置 52 運動モデルごとの予測器 53 運動モデルごとの予測誤差評価器 54 n個の運動モデルによる予測器 55 n個の運動モデルによる予測誤差評価器 56 相関器 57 n個の運動モデルによる信頼度算出器 58 ゲイン行列算出器 59 加速度ベクトル推定器 60 加速度ベクトル推定値による目標運動判定器 61 目標距離変化率の予測値による目標運動判定器 62 n個の運動モデルによる平滑器 63 n個の運動モデルによる平滑誤差評価器 64 第1の遅延回路 65 第2の遅延回路 66 第3の遅延回路 67 第4の遅延回路 68 第5の遅延回路 69 第6の遅延回路 70 第7の遅延回路 81 等速直進運動モデルによる予測器 82 探知データの信頼度算出器 83 等速直進運動モデルによる平滑器 84 等速直進運動モデルによる平滑誤差評価器 85 等速直進運動モデルによる予測誤差評価器 86 第8の遅延回路 87 第9の遅延回路
算出ステップ 4 n個の運動モデルによる予測値算出ステップ 5 n個の運動モデルによる予測誤差共分散行列算出ス
テップ 6 探知データ入力ステップ 7 探知データ選択ステップ 8 運動モデルおよび探知データの信頼度算出ステップ 9 ゲイン行列算出ステップ 10 加速度ベクトル推定ステップ 11 加速度ベクトル推定値による目標運動判定ステッ
プ 12 目標距離変化率の予測値による目標運動判定ステ
ップ 13 n個の運動モデルによる平滑値および平滑誤差共
分散行列算出ステップ 14 終了判定ステップ 15 等速直進運動モデル設定ステップ 16 等速直進運動モデルによる予測値算出ステップ 17 等速直進運動モデルによる予測誤差共分散行列算
出ステップ 18 探知データの信頼度算出ステップ 19 等速直進運動モデルによる平滑値および平滑誤差
共分散行列算出ステップ 51 目標観測装置 52 運動モデルごとの予測器 53 運動モデルごとの予測誤差評価器 54 n個の運動モデルによる予測器 55 n個の運動モデルによる予測誤差評価器 56 相関器 57 n個の運動モデルによる信頼度算出器 58 ゲイン行列算出器 59 加速度ベクトル推定器 60 加速度ベクトル推定値による目標運動判定器 61 目標距離変化率の予測値による目標運動判定器 62 n個の運動モデルによる平滑器 63 n個の運動モデルによる平滑誤差評価器 64 第1の遅延回路 65 第2の遅延回路 66 第3の遅延回路 67 第4の遅延回路 68 第5の遅延回路 69 第6の遅延回路 70 第7の遅延回路 81 等速直進運動モデルによる予測器 82 探知データの信頼度算出器 83 等速直進運動モデルによる平滑器 84 等速直進運動モデルによる平滑誤差評価器 85 等速直進運動モデルによる予測誤差評価器 86 第8の遅延回路 87 第9の遅延回路
フロントページの続き (56)参考文献 特開 平6−43241(JP,A) 特開 平5−288840(JP,A) 特公 平7−97136(JP,B2) 特公 平7−97135(JP,B2) 特公 平7−69417(JP,B2) 特公 平6−95138(JP,B2) (58)調査した分野(Int.Cl.6,DB名) G01S 13/66
Claims (6)
- 【請求項1】 目標位置、速度等の平滑値および平滑値
の誤差を推定した平滑誤差共分散行列の初期値を設定す
る手段、同一次元の複数のn個の定数ベクトルを用いる
ことによる目標のn個の運動モデルを設定する手段、n
個の各運動モデルごとに目標位置、速度等の予測値およ
びその誤差を推定した予測誤差共分散行列を算出する手
段、n個の運動モデル全体による目標位置、速度等の予
測値を算出する手段、探知データとして目標位置および
目標距離変化率の観測値を誤探知データを含め一般に複
数個入力する手段、上記n個の運動モデル全体による予
測値および上記n個の各運動モデルごとの予測誤差共分
散行列を使用して上記複数個の探知データの中から追尾
目標と相関の可能性のある探知データのみをいくつか選
択する手段、複数のn個の各運動モデルおよび選択され
た各探知データの信頼度を算出する手段、上記各運動モ
デルで同一のゲイン行列を算出する手段、上記n個の運
動モデルを構成する定数ベクトルの推定値を算出する手
段、上記定数ベクトルの推定値に基づき目標運動を判定
する手段、上記目標運動の判定結果に基づきn個の運動
モデルの設定方法を複数の場合から選択しつつn個の運
動モデル全体による目標位置、速度等の平滑値およびそ
の誤差を推定した平滑誤差共分散行列を算出する手段と
を具備したことを特徴とする多目標追尾装置。 - 【請求項2】 目標位置、速度等の平滑値および平滑値
の誤差を推定した平滑誤差共分散行列の初期値を設定す
る手段、同一次元の複数のn個の定数ベクトルを用いる
ことによる目標のn個の運動モデルを設定する手段、n
個の各運動モデルごとに目標位置、速度等の予測値およ
びその誤差を推定した予測誤差共分散行列を算出する手
段、n個の運動モデル全体による目標位置、速度等の予
測値を算出する手段、探知データとして目標位置および
目標距離変化率の観測値を誤探知データを含め一般に複
数個入力する手段、上記n個の運動モデル全体による予
測値および上記n個の各運動モデルごとの予測誤差共分
散行列を使用して上記複数個の探知データの中から追尾
目標と相関の可能性のある探知データのみをいくつか選
択する手段、複数のn個の各運動モデルおよび選択され
た各探知データの信頼度を算出する手段、上記各運動モ
デルで同一のゲイン行列を算出する手段、上記n個の運
動モデルを構成する定数ベクトルの推定値を算出する手
段、目標距離変化率の予測値を算出しこの値に基づき目
標運動を判定する手段、上記目標運動の判定結果に基づ
きn個の運動モデルの設定方法を複数の場合から選択し
つつn個の運動モデル全体による目標位置、速度等の平
滑値およびその誤差を推定した平滑誤差共分散行列を算
出する手段とを具備したことを特徴とする多目標追尾装
置。 - 【請求項3】 目標位置、速度等の平滑値および平滑値
の誤差を推定した平滑誤差共分散行列の初期値を設定す
る手段、同一次元の複数のn個の定数ベクトルを用いる
ことによる目標のn個の運動モデルを設定する手段、n
個の各運動モデルごとに目標位置、速度等の予測値およ
びその誤差を推定した予測誤差共分散行列を算出する手
段、n個の運動モデル全体を用いることによる目標位
置、速度等の予測値を算出する手段、n個の運動モデル
全体による予測値の誤差を推定した予測誤差共分散行列
を算出する手段、探知データとして目標位置および目標
距離変化率の観測値を誤探知データを含め一般に複数個
入力する手段、上記n個の各運動モデルごとの予測値お
よび上記n個の運動モデル全体による予測誤差共分散行
列を使用して、上記複数個の探知データの中から追尾目
標と相関の可能性のある探知データのみをいくつか選択
する手段、複数のn個の各運動モデルおよび選択された
各探知データの信頼度を算出する手段、上記各運動モデ
ルで同一のゲイン行列を算出する手段、上記n個の運動
モデルを構成する定数ベクトルの推定値を算出する手
段、上記定数ベクトルの推定値に基づき目標運動を判定
する手段、上記目標運動の判定結果に基づきn個の運動
モデルの設定方法を複数の場合から選択しつつn個の運
動モデル全体による目標位置、速度等の平滑値およびそ
の誤差を推定した平滑誤差共分散行列を算出する手段と
を具備したことを特徴とする多目標追尾装置。 - 【請求項4】 目標位置、速度等の平滑値および平滑値
の誤差を推定した平滑誤差共分散行列の初期値を設定す
る手段、同一次元の複数のn個の定数ベクトルを用いる
ことによる目標のn個の運動モデルを設定する手段、n
個の各運動モデルごとに目標位置、速度等の予測値およ
びその誤差を推定した予測誤差共分散行列を算出する手
段、n個の運動モデル全体を用いることによる目標位
置、速度等の予測値を算出する手段、n個の運動モデル
全体による予測値の誤差を推定した予測誤差共分散行列
を算出する手段、探知データとして目標位置および目標
距離変化率の観測値を誤探知データを含め一般に複数個
入力する手段、上記n個の各運動モデルごとの予測値お
よび上記n個の運動モデル全体による予測誤差共分散行
列を使用して、上記複数個の探知データの中から追尾目
標と相関の可能性のある探知データのみをいくつか選択
する手段、複数のn個の各運動モデルおよび選択された
各探知データの信頼度を算出する手段、上記各運動モデ
ルで同一のゲイン行列を算出する手段、上記n個の運動
モデルを構成する定数ベクトルの推定値を算出する手
段、目標距離変化率の予測値を算出しこの値に基づき目
標運動を判定する手段、上記目標運動の判定結果に基づ
きn個の運動モデルの設定方法を複数の場合から選択し
つつn個の運動モデル全体による目標位置、速度等の平
滑値およびその誤差を推定した平滑誤差共分散行列を算
出する手段とを具備したことを特徴とする多目標追尾装
置。 - 【請求項5】 目標位置、速度等の平滑値および平滑値
の誤差を推定した平滑誤差共分散行列の初期値を設定す
る手段、同一次元の複数のn個の定数ベクトルを用いる
ことによる目標のn個の運動モデルを設定する手段、n
個の各運動モデルごとに目標位置、速度等の予測値およ
びその誤差を推定した予測誤差共分散行列を算出する手
段、n個の運動モデル全体を用いることによる目標位
置、速度等の予測値を算出する手段、n個の運動モデル
全体による予測値の誤差を推定した予備誤差共分散行列
を算出する手段、探知データとして目標位置および目標
距離変化率の観測値を誤探知データを含め一般に複数個
入力する手段、上記n個の運動モデル全体の予測値およ
び上記n個の運動モデル全体による予測誤差共分散行列
を使用して、上記複数個の探知データの中から追尾目標
と相関の可能性のある探知データのみをいくつか選択す
る手段、複数のn個の各運動モデルおよび選択された各
探知データの信頼度を算出する手段、上記各運動モデル
で同一のゲイン行列を算出する手段、上記n個の運動モ
デルを構成する定数ベクトルの推定値を算出する手段、
上記定数ベクトルの推定値に基づき目標運動を判定する
手段、上記目標運動の判定結果に基づきn個の運動モデ
ルの設定方法を複数の場合から選択しつつn個の運動モ
デル全体による目標位置、速度等の平滑値およびその誤
差を推定した平滑誤差共分散行列を算出する手段とを具
備したことを特徴とする多目標追尾装置。 - 【請求項6】 目標位置、速度等の平滑値および平滑値
の誤差を推定した平滑誤差共分散行列の初期値を設定す
る手段、同一次元の複数のn個の定数ベクトルを用いる
ことによる目標のn個の運動モデルを設定する手段、n
個の各運動モデルごとに目標位置、速度等の予測値およ
びその誤差を推定した予測誤差共分散行列を算出する手
段、n個の運動モデル全体を用いることによる目標位
置、速度等の予測値を算出する手段、n個の運動モデル
全体による予測値の誤差を推定した予測誤差共分散行列
を算出する手段、探知データとして目標位置および目標
距離変化率の観測値を誤探知データを含め一般に複数個
入力する手段、上記n個の運動モデル全体の予測値およ
び上記n個の運動モデル全体による予測誤差共分散行列
を使用して、上記複数個の探知データの中から追尾目標
と相関の可能性のある探知データのみをいくつか選択す
る手段、複数のn個の各運動モデルおよび選択された各
探知データの信頼度を算出する手段、上記各運動モデル
で同一のゲイン行列を算出する手段、上記n個の運動モ
デルを構成する定数ベクトルの推定値を算出する手段、
目標距離変化率の予測値を算出しこの値に基づき目標運
動を判定する手段、上記目標運動の判定結果に基づきn
個の運動モデルの設定方法を複数の場合から選択しつつ
n個の運動モデル全体による目標位置、速度等の平滑値
およびその誤差を推定した平滑誤差共分散行列を算出す
る手段とを具備したことを特徴とする多目標追尾装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP8321266A JP2843910B2 (ja) | 1996-12-02 | 1996-12-02 | 多目標追尾装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP8321266A JP2843910B2 (ja) | 1996-12-02 | 1996-12-02 | 多目標追尾装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH10160831A JPH10160831A (ja) | 1998-06-19 |
JP2843910B2 true JP2843910B2 (ja) | 1999-01-06 |
Family
ID=18130658
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP8321266A Expired - Lifetime JP2843910B2 (ja) | 1996-12-02 | 1996-12-02 | 多目標追尾装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2843910B2 (ja) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5011904B2 (ja) * | 2006-09-19 | 2012-08-29 | 三菱電機株式会社 | 追尾方法及びその装置 |
-
1996
- 1996-12-02 JP JP8321266A patent/JP2843910B2/ja not_active Expired - Lifetime
Also Published As
Publication number | Publication date |
---|---|
JPH10160831A (ja) | 1998-06-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Lu et al. | Heterogeneous multi-task learning for multiple pseudo-measurement estimation to bridge GPS outages | |
JP4116898B2 (ja) | 目標追尾装置 | |
US5877998A (en) | Recursive method for target motion analysis | |
CN111007880B (zh) | 一种基于汽车雷达的扩展目标跟踪方法 | |
Mählisch et al. | De-cluttering with integrated probabilistic data association for multisensor multitarget ACC vehicle tracking | |
JP2843910B2 (ja) | 多目標追尾装置 | |
JP3996080B2 (ja) | 目標追尾装置 | |
JPH0797136B2 (ja) | 多目標追尾方法及びその装置 | |
JP2893176B2 (ja) | 多目標追尾装置 | |
JP2004219300A (ja) | 目標追尾装置 | |
JP3926602B2 (ja) | 目標追尾装置及び方法 | |
JPH0643241A (ja) | 多目標追尾方法及びその装置 | |
JP3440010B2 (ja) | 目標追尾装置 | |
JPH05297947A (ja) | 多目標追尾装置 | |
JP2003149328A (ja) | レーダの目標相関装置 | |
JPH05288840A (ja) | 多目標追尾装置 | |
JPH0797135B2 (ja) | 多目標追尾方法及びその装置 | |
JP2001228245A (ja) | 目標追尾装置および目標追尾方法 | |
JP2002341024A (ja) | 多目標追尾装置 | |
JP3411485B2 (ja) | 目標追尾装置 | |
JP3970585B2 (ja) | 目標追尾装置及び方法 | |
JP2002286838A (ja) | 目標追尾装置及び目標追尾方法 | |
JP3447587B2 (ja) | 追尾装置及び追尾方法 | |
CN117934549B (zh) | 一种基于概率分布指导数据关联的3d多目标跟踪方法 | |
JP3044296B2 (ja) | 目標相関装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
S533 | Written request for registration of change of name |
Free format text: JAPANESE INTERMEDIATE CODE: R313533 |
|
R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
S533 | Written request for registration of change of name |
Free format text: JAPANESE INTERMEDIATE CODE: R313533 |
|
R350 | Written notification of registration of transfer |
Free format text: JAPANESE INTERMEDIATE CODE: R350 |
|
EXPY | Cancellation because of completion of term |