JP2006153736A - 目標追尾装置 - Google Patents
目標追尾装置 Download PDFInfo
- Publication number
- JP2006153736A JP2006153736A JP2004346794A JP2004346794A JP2006153736A JP 2006153736 A JP2006153736 A JP 2006153736A JP 2004346794 A JP2004346794 A JP 2004346794A JP 2004346794 A JP2004346794 A JP 2004346794A JP 2006153736 A JP2006153736 A JP 2006153736A
- Authority
- JP
- Japan
- Prior art keywords
- target
- reliability
- model
- threshold
- error
- 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.)
- Pending
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
【課題】 運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かない閾値を算出して、追尾精度を高めることができる目標追尾装置を得ることを目的とする。
【解決手段】 信頼度算出器１２により算出される等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値Ａを算出する。これにより、運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かない閾値が得られ、追尾精度を高めることができる。
【選択図】 図１
【解決手段】 信頼度算出器１２により算出される等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値Ａを算出する。これにより、運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かない閾値が得られ、追尾精度を高めることができる。
【選択図】 図１
Description
この発明は、目標の位置や速度などの目標運動諸元を推定する目標追尾装置に関するものである。
従来の目標追尾装置は、Ｎ個の運動モデルを持つカルマンフィルタを有し、その運動モデルの信頼度が発振を繰り返すことによる追尾精度の劣化や、各運動モデルの信頼度の均一化による観測雑音低減性能の劣化を抑制するため、誤差楕円体の重複度を判定する誤差楕円体重複度判定手段と、その誤差楕円体重複度判定手段により判定された誤差楕円体の重複度に基づいて、各誤差楕円体の間に空白域が生じないように上記誤差楕円体を拡大、あるいは、各誤差楕円体同士が重なり過ぎないように上記誤差楕円体を縮小させる拡大・縮小手段とを備えるようにしている。
即ち、各運動モデルを用いて算出された予測位置を中心とする誤差楕円体の重複度が低い場合、目標が旋回等を実施すると、既設定の運動モデルでは表現できない領域（誤差楕円体の領域外）に目標の観測位置が得られる場合がある。
この場合、既設定の運動モデルの尤度が低いにも拘わらず、その尤度に基づいて運動モデルの信頼度を計算するため、正しい信頼度が得られない。その結果、その信頼度が増減を繰り返すモデル信頼度の発振が生じて追尾精度が劣化することがある。
この場合、既設定の運動モデルの尤度が低いにも拘わらず、その尤度に基づいて運動モデルの信頼度を計算するため、正しい信頼度が得られない。その結果、その信頼度が増減を繰り返すモデル信頼度の発振が生じて追尾精度が劣化することがある。
そこで、従来の目標追尾装置は、残差共分散行列で各運動モデルの予測誤差を正規化した残差二次形式と、予め設定された閾値とを比較して、誤差楕円体の重複度を判定する。
そして、誤差楕円体の重複度が十分でないために、目標の観測位置が誤差楕円体の領域外に得られている場合、運動モデルの信頼度の発振を抑制するため、最も近い運動モデルの誤差楕円体の境界に観測位置が得られるように誤差楕円体を拡大させるようにする。
これにより、各運動モデルの尤度がそれぞれ上昇するため、各運動モデルの信頼度の推定精度が向上する（例えば、特許文献１参照）。
そして、誤差楕円体の重複度が十分でないために、目標の観測位置が誤差楕円体の領域外に得られている場合、運動モデルの信頼度の発振を抑制するため、最も近い運動モデルの誤差楕円体の境界に観測位置が得られるように誤差楕円体を拡大させるようにする。
これにより、各運動モデルの尤度がそれぞれ上昇するため、各運動モデルの信頼度の推定精度が向上する（例えば、特許文献１参照）。
ただし、誤差楕円体の重複度判定と誤差楕円体の拡大に用いる閾値は、運動モデルの信頼度の発振が生じず、かつ、不必要な誤差楕円体の拡大によって、観測雑音低減性能が劣化しないような値に設定する必要がある。
例えば、閾値を低めに設定しすぎた場合、等速直線運動を行っている目標の観測位置が、観測誤差の影響で、等速直線運動モデルの誤差楕円体の領域外に頻繁に現れることがある。
即ち、目標が等速直線運動を行っているにも拘わらず、運動モデルの領域外に観測位置が得られていると判定する誤判定確率が高くなり、等速直線運動を行っている目標に対しても不必要に誤差楕円体の拡大が行われる。その結果、運動モデルの信頼度の発振は生じないが、フィルタゲインの増大により観測誤差の抑圧性能が劣化する。
一方、閾値を高めに設定しすぎると、目標の観測位置が運動モデルの信頼度が発振する可能性の高い領域内に得られていても誤差楕円体の拡大が行われない。その結果、運動モデルの信頼度が発振して追尾精度が劣化する。
例えば、閾値を低めに設定しすぎた場合、等速直線運動を行っている目標の観測位置が、観測誤差の影響で、等速直線運動モデルの誤差楕円体の領域外に頻繁に現れることがある。
即ち、目標が等速直線運動を行っているにも拘わらず、運動モデルの領域外に観測位置が得られていると判定する誤判定確率が高くなり、等速直線運動を行っている目標に対しても不必要に誤差楕円体の拡大が行われる。その結果、運動モデルの信頼度の発振は生じないが、フィルタゲインの増大により観測誤差の抑圧性能が劣化する。
一方、閾値を高めに設定しすぎると、目標の観測位置が運動モデルの信頼度が発振する可能性の高い領域内に得られていても誤差楕円体の拡大が行われない。その結果、運動モデルの信頼度が発振して追尾精度が劣化する。
従来の目標追尾装置は以上のように構成されているので、運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かないように閾値を設定する必要がある。しかし、運動モデルの信頼度の発振抑制と観測誤差の低減がトレードオフの関係になるため、その都度、目標の観測条件を模擬して、経験的に閾値を見積って設定する必要があり、必ずしも性能を最大限引き出せず、追尾精度が劣化することがあるなどの課題があった。
この発明は上記のような課題を解決するためになされたもので、運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かない閾値を算出して、追尾精度を高めることができる目標追尾装置を得ることを目的とする。
この発明に係る目標追尾装置は、信頼度算出手段により算出される等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値を算出する閾値算出手段を設け、目標の観測位置と予測位置の残差二次形式を当該閾値と比較することによって、その予測位置を中心とする目標の存在期待確率を示す誤差楕円体の領域内に、目標の観測位置が存在しているか否かを判定するようにしたものである。
この発明によれば、信頼度算出手段により算出される等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値を算出するように構成したので、運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かない閾値が得られ、追尾精度を高めることができる効果がある。
実施の形態１．
図１はこの発明の実施の形態１による目標追尾装置を示す構成図であり、図において、目標観測装置１は例えば目標を探知するセンサが搭載され、そのセンサにより探知された目標の観測位置を示す目標位置観測情報を目標追尾装置に出力する。
目標追尾装置のゲート判定器２は目標観測装置１から出力された目標位置観測情報が示す目標の観測位置と、遅延回路１７に単位時間だけ遅延された目標の予測位置との残差二次形式を算出し、その残差二次形式が所定の閾値より小さければ、その目標の観測位置を平滑器３に出力する。
平滑器３はゲート判定器２から出力された目標の観測位置と、信頼度算出器１２により算出されたＮ個の運動モデルの信頼度と、入力ベクトル平滑器１８により算出された入力ベクトルの平滑値と、ゲイン行列算出器２０により算出されたゲイン行列と、モデル設定器５により設定された定数加速度ベクトルと、平滑器メモリ４に格納されている１サンプリング前の平滑値及び平滑誤差共分散行列とに基づいて、目標の位置を示す平滑値及び平滑誤差共分散行列を算出する。
平滑器メモリ４は平滑器３により算出された平滑値及び平滑誤差共分散行列を格納する。
図１はこの発明の実施の形態１による目標追尾装置を示す構成図であり、図において、目標観測装置１は例えば目標を探知するセンサが搭載され、そのセンサにより探知された目標の観測位置を示す目標位置観測情報を目標追尾装置に出力する。
目標追尾装置のゲート判定器２は目標観測装置１から出力された目標位置観測情報が示す目標の観測位置と、遅延回路１７に単位時間だけ遅延された目標の予測位置との残差二次形式を算出し、その残差二次形式が所定の閾値より小さければ、その目標の観測位置を平滑器３に出力する。
平滑器３はゲート判定器２から出力された目標の観測位置と、信頼度算出器１２により算出されたＮ個の運動モデルの信頼度と、入力ベクトル平滑器１８により算出された入力ベクトルの平滑値と、ゲイン行列算出器２０により算出されたゲイン行列と、モデル設定器５により設定された定数加速度ベクトルと、平滑器メモリ４に格納されている１サンプリング前の平滑値及び平滑誤差共分散行列とに基づいて、目標の位置を示す平滑値及び平滑誤差共分散行列を算出する。
平滑器メモリ４は平滑器３により算出された平滑値及び平滑誤差共分散行列を格納する。
モデル設定器５は定数加速度ベクトル（等速直線運動の零加速度ベクトルを含む）とモデル遷移行列を設定し、その定数加速度ベクトルを平滑器３、モデル毎予測器６、予測器１６及び入力ベクトル平滑器１８に出力し、そのモデル遷移行列を閾値設定器８、信頼度算出器１２及び入力ベクトル予測器１５に出力する。
モデル毎予測器６はモデル設定器５により設定された定数加速度ベクトル（零加速度ベクトルを含む）から等速直線運動の運動モデルを含むＮ個の運動モデルを設定し、運動モデル毎に、当該運動モデルを用いて目標の予測位置を算出し、平滑器３により算出された平滑誤差共分散行列と予め設定された駆動雑音共分散行列を用いて、運動モデル毎に予測誤差共分散行列を算出する。
遅延回路７はモデル毎予測器６により算出された目標の予測位置及び予測誤差共分散行列を１サンプリング時間だけ格納してから乗算器１１に出力する。
モデル毎予測器６はモデル設定器５により設定された定数加速度ベクトル（零加速度ベクトルを含む）から等速直線運動の運動モデルを含むＮ個の運動モデルを設定し、運動モデル毎に、当該運動モデルを用いて目標の予測位置を算出し、平滑器３により算出された平滑誤差共分散行列と予め設定された駆動雑音共分散行列を用いて、運動モデル毎に予測誤差共分散行列を算出する。
遅延回路７はモデル毎予測器６により算出された目標の予測位置及び予測誤差共分散行列を１サンプリング時間だけ格納してから乗算器１１に出力する。
閾値設定器８は信頼度算出器１２により算出された１サンプリング前のモデル信頼度と、モデル設定器５により設定されたモデル遷移行列と、モデル毎予測器６により算出された目標の予測位置及び予測誤差共分散行列と、予め設定された観測モデルから得られる観測誤差共分散行列とに基づいて、等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値Ａを算出する。
ただし、閾値設定器８は予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる理論的な確率を算出し、その理論確率が所定値より高い場合、その誤差楕円体の拡大・縮小に用いる閾値Ｂとして、その閾値Ａと同一の閾値を誤差共分散行列制御器１０に出力し、その理論確率が所定値より低い場合、その誤差楕円体の拡大・縮小に用いる閾値Ｂとして、χ自乗分布表を参照して目標の存在期待確率が得られる閾値を誤差共分散行列制御器１０に出力する。
ただし、閾値設定器８は予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる理論的な確率を算出し、その理論確率が所定値より高い場合、その誤差楕円体の拡大・縮小に用いる閾値Ｂとして、その閾値Ａと同一の閾値を誤差共分散行列制御器１０に出力し、その理論確率が所定値より低い場合、その誤差楕円体の拡大・縮小に用いる閾値Ｂとして、χ自乗分布表を参照して目標の存在期待確率が得られる閾値を誤差共分散行列制御器１０に出力する。
誤差楕円体重複判定器９はモデル毎予測器６により算出された目標の予測位置及び予測誤差共分散行列から、目標の観測位置と予測位置の残差二次形式を算出し、その残差二次形式の最小値が閾値設定器８により設定された閾値Ａより大きい場合、運動モデル毎の予測位置を中心とする目標の存在期待確率を示す誤差楕円体（等確率ライン）同士の重複が不十分であり、目標の観測位置が閾値Ａにより定義される誤差楕円体の領域外に存在している旨を示す重複判定情報を誤差共分散行列制御器１０に出力する。
誤差共分散行列制御器１０は誤差楕円体重複判定器９から出力された重複判定情報が、誤差楕円体の重複度が適当でないことを示す場合、閾値設定器８から出力された閾値Ｂに応じて誤差楕円体の拡大・縮小を決定する乗算係数を算出し、その乗算係数を乗算器１１に出力する。
誤差共分散行列制御器１０は誤差楕円体重複判定器９から出力された重複判定情報が、誤差楕円体の重複度が適当でないことを示す場合、閾値設定器８から出力された閾値Ｂに応じて誤差楕円体の拡大・縮小を決定する乗算係数を算出し、その乗算係数を乗算器１１に出力する。
乗算器１１は誤差共分散行列制御器１０から乗算係数が出力された場合、その乗算係数を遅延回路７から出力された予測誤差共分散行列に乗算し、乗算後の予測誤差共分散行列を信頼度算出器１２に出力する。
信頼度算出器１２はゲート判定器２から出力された目標の観測位置と、乗算器１１から出力された運動モデル毎の予測誤差共分散行列と、遅延回路７から出力された運動モデル毎の予測位置と、モデル設定器５により設定されたモデル遷移行列と、信頼度メモリ１３に格納されている１サンプリング前の各運動モデルの信頼度とに基づいて、Ｎ個の運動モデルの信頼度を算出する。
信頼度メモリ１３は信頼度算出器１２により算出されたＮ個の運動モデルの信頼度を格納する。
信頼度算出器１２はゲート判定器２から出力された目標の観測位置と、乗算器１１から出力された運動モデル毎の予測誤差共分散行列と、遅延回路７から出力された運動モデル毎の予測位置と、モデル設定器５により設定されたモデル遷移行列と、信頼度メモリ１３に格納されている１サンプリング前の各運動モデルの信頼度とに基づいて、Ｎ個の運動モデルの信頼度を算出する。
信頼度メモリ１３は信頼度算出器１２により算出されたＮ個の運動モデルの信頼度を格納する。
遅延回路１４は信頼度算出器１２により算出されたＮ個の運動モデルの信頼度を１サンプリング時間だけ格納してから入力ベクトル予測器１５及び予測器１６に出力する。
入力ベクトル予測器１５は遅延回路１４から出力されたＮ個の運動モデルの信頼度と、モデル設定器５により設定されたモデル遷移行列とに基づいて、入力ベクトルの予測値を算出する。
予測器１６は平滑器３により算出された平滑値と、遅延回路１４から出力されたＮ個の運動モデルの信頼度と、入力ベクトル予測器１５により算出された入力ベクトルの予測値と、モデル設定器５により設定された定数加速度ベクトルとに基づいて、目標の予測値及び予測誤差共分散行列を算出する。
遅延回路１７は予測器１６により算出された目標の予測値及び予測誤差共分散行列を１サンプリング時間だけ格納してからゲート判定器２に出力する。
入力ベクトル予測器１５は遅延回路１４から出力されたＮ個の運動モデルの信頼度と、モデル設定器５により設定されたモデル遷移行列とに基づいて、入力ベクトルの予測値を算出する。
予測器１６は平滑器３により算出された平滑値と、遅延回路１４から出力されたＮ個の運動モデルの信頼度と、入力ベクトル予測器１５により算出された入力ベクトルの予測値と、モデル設定器５により設定された定数加速度ベクトルとに基づいて、目標の予測値及び予測誤差共分散行列を算出する。
遅延回路１７は予測器１６により算出された目標の予測値及び予測誤差共分散行列を１サンプリング時間だけ格納してからゲート判定器２に出力する。
入力ベクトル平滑器１８は信頼度算出器１２により算出されたＮ個の運動モデルの信頼度とモデル設定器５により設定された定数加速度ベクトルとに基づいて入力ベクトルの平滑値を算出し、その入力ベクトルの平滑値を平滑器３に出力する。
遅延回路１９はモデル毎予測器６により算出された運動モデル毎の予測誤差共分散行列を１サンプリング時間だけ格納してからゲイン行列算出器２０に出力する。
ゲイン行列算出器２０は遅延回路１９から出力された運動モデル毎の予測誤差共分散行列と、予め設定された観測モデルから得られる観測誤差共分散行列とに基づいて、平滑値の算出に使用するゲイン行列を算出する。
表示装置２１は平滑器３により算出された平滑値を表示する。
遅延回路１９はモデル毎予測器６により算出された運動モデル毎の予測誤差共分散行列を１サンプリング時間だけ格納してからゲイン行列算出器２０に出力する。
ゲイン行列算出器２０は遅延回路１９から出力された運動モデル毎の予測誤差共分散行列と、予め設定された観測モデルから得られる観測誤差共分散行列とに基づいて、平滑値の算出に使用するゲイン行列を算出する。
表示装置２１は平滑器３により算出された平滑値を表示する。
図１の例では、ゲート判定器２、平滑器３、平滑器メモリ４、遅延回路１４、入力ベクトル予測器１５、予測器１６、遅延回路１７、入力ベクトル平滑器１８、遅延回路１９及びゲイン行列算出器２０は、単に観測値を選択するのではなく、航跡の平滑化や１サンプリング先の予測を行う追尾フィルタを構成している。
モデル設定器５、モデル毎予測器６及び遅延回路７から予測位置算出手段が構成されている。
閾値設定器８から閾値算出手段が構成され、誤差楕円体重複判定器９及び誤差共分散行列制御器１０から存在判定手段が構成されている。
乗算器１１、信頼度算出器１２及び信頼度メモリ１３から信頼度算出手段が構成されている。
モデル設定器５、モデル毎予測器６及び遅延回路７から予測位置算出手段が構成されている。
閾値設定器８から閾値算出手段が構成され、誤差楕円体重複判定器９及び誤差共分散行列制御器１０から存在判定手段が構成されている。
乗算器１１、信頼度算出器１２及び信頼度メモリ１３から信頼度算出手段が構成されている。
図２はこの発明の実施の形態１による目標追尾装置の閾値設定器８の処理内容を示すフローチャートである。
また、図３はこの発明の実施の形態１による目標追尾装置の誤差楕円体重複判定器９の処理内容を示すフローチャートであり、図４はこの発明の実施の形態１による目標追尾装置の誤差共分散行列制御器１０及び乗算器１１の処理内容を示すフローチャートである。
また、図３はこの発明の実施の形態１による目標追尾装置の誤差楕円体重複判定器９の処理内容を示すフローチャートであり、図４はこの発明の実施の形態１による目標追尾装置の誤差共分散行列制御器１０及び乗算器１１の処理内容を示すフローチャートである。
次に動作について説明する。
まず、目標観測装置１は、搭載しているセンサを用いて目標を探知し、そのセンサにより探知された目標の観測位置を示す目標位置観測情報を目標追尾装置に出力する。
目標追尾装置は、目標観測装置１から目標位置観測情報を受けると、その目標位置観測情報が示す目標の観測位置と、１サンプリング前に算出している目標の予測位置との残差二次形式を求め、その残差二次形式が所定の閾値より小さければ、その目標位置観測情報は真の目標の観測位置を示す情報であるものと判断して、追尾フィルタによる平滑・予測処理を行い、平滑値を表示装置２１に表示する。
以下、目標追尾装置の処理内容を具体的に説明する。
まず、目標観測装置１は、搭載しているセンサを用いて目標を探知し、そのセンサにより探知された目標の観測位置を示す目標位置観測情報を目標追尾装置に出力する。
目標追尾装置は、目標観測装置１から目標位置観測情報を受けると、その目標位置観測情報が示す目標の観測位置と、１サンプリング前に算出している目標の予測位置との残差二次形式を求め、その残差二次形式が所定の閾値より小さければ、その目標位置観測情報は真の目標の観測位置を示す情報であるものと判断して、追尾フィルタによる平滑・予測処理を行い、平滑値を表示装置２１に表示する。
以下、目標追尾装置の処理内容を具体的に説明する。
目標追尾装置のモデル設定器５は、予め、定数加速度ベクトル（零加速度ベクトルを含む）とモデル遷移行列を設定し、その定数加速度ベクトルを平滑器３、モデル毎予測器６、予測器１６及び入力ベクトル平滑器１８に出力する。
また、そのモデル遷移行列を閾値設定器８、信頼度算出器１２及び入力ベクトル予測器１５に出力する。
ここで、図５はモデル設定器５により設定される定数加速度ベクトルの一例を示す説明図である。図５の詳細は後述する。
また、そのモデル遷移行列を閾値設定器８、信頼度算出器１２及び入力ベクトル予測器１５に出力する。
ここで、図５はモデル設定器５により設定される定数加速度ベクトルの一例を示す説明図である。図５の詳細は後述する。
モデル毎予測器６は、モデル設定器５が定数加速度ベクトルを設定すると、その定数加速度ベクトルから等速直線運動の運動モデルを含むＮ個の運動モデルを設定する。
図６は航空機の運動をモデル化した運動モデルの一例を示す説明図であるが、この実施の形態１では、目標追尾装置の追尾フィルタが、Ｎ個の運動モデルを有する多重運動モデルを用いたカルマンフィルタである場合について説明する。
ここで、多重運動モデルを用いたカルマンフィルタの原理を説明する。
図６は航空機の運動をモデル化した運動モデルの一例を示す説明図であるが、この実施の形態１では、目標追尾装置の追尾フィルタが、Ｎ個の運動モデルを有する多重運動モデルを用いたカルマンフィルタである場合について説明する。
ここで、多重運動モデルを用いたカルマンフィルタの原理を説明する。
例えば、目標の運動モデルを下記の式（１）のように定義する。
ただし、Ｘkはサンプリング時刻ｔkにおける目標運動諸元の真値を表す状態ベクトルであり、例えば、ｘｙｚの直交座標系における目標位置ベクトルを下記の式（２）、直交座標系における目標速度ベクトルを下記の式（３）とすると、Ｘkは下記の式（４）のようになる。
また、Φk-1はサンプリング時刻ｔk-1からサンプリング時刻ｔkに遷移する際の状態ベクトルＸkの推移を表すモデル推移行列であり、目標が等速直線運動を行うと仮定すると、モデル推移行列Φk-1は下記の式（５）のようになる。ただし、Ｉは式（６）に示すような単位行列である。
また、ｗkはサンプリング時刻ｔkにおける駆動雑音ベクトルであり、Γkはサンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列である。
目標の運動モデルが等速直線運動と仮定し、その場合の打切り誤差項をΓk-1ｗk-1とみれば、ｗkは加速度ベクトルに相当し、Γk-1は下記の式（７）のようになる
ｗkは平均０の３次元正規分布白色雑音であり、下記の式（８）、式（９）とする。ここで、０は零ベクトルを表し、Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列である。
目標の運動モデルが等速直線運動と仮定し、その場合の打切り誤差項をΓk-1ｗk-1とみれば、ｗkは加速度ベクトルに相当し、Γk-1は下記の式（７）のようになる
また、ｕkはサンプリング時刻ｔkにおいて、Ｎ個の運動モデルを構成する定数加速度ベクトルであり、以下の式（１０）で表される。
Γ’はサンプリング時刻ｔkにおける定数加速度ベクトルの変換行列であり、以下の式（１１）のようになる。
Γ’はサンプリング時刻ｔkにおける定数加速度ベクトルの変換行列であり、以下の式（１１）のようになる。
図５において、Ｏは目標観測装置１を原点とする座標Ｏ−ｘｙｚの原点、Ｘは東方向をｘ軸の正とする座標Ｏ−ｘｙｚのｘ軸、Ｙは北方向をｙ軸の正とする座標Ｏ−ｘｙｚのｙ軸、Ｚは上方向をｚ軸の正とする座標Ｏ−ｘｙｚのｚ軸である。
また、α1はｙ軸正方向の定数加速度ベクトル、α2はｙ軸負方向の定数加速度ベクトル、α3はＸ軸正方向の定数ベクトル、α4はＸ軸負方向の定数加速度ベクトル、α5はＺ軸正方向の定数加速度ベクトル、α6はｚ軸負方向の定数加速度ベクトルである。
この他に、定数加速度ベクトルとして、α7＝０の運動モデルを想定すると、運動モデルのモデル数Ｎは７個となる。
定数加速度の１軸には、北基準直交座標系の北方向、推定速度ベクトル、ＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）などが考えられる。ここで、ＬＯＳは、レーダと目標を結んだベクトルである。
また、α1はｙ軸正方向の定数加速度ベクトル、α2はｙ軸負方向の定数加速度ベクトル、α3はＸ軸正方向の定数ベクトル、α4はＸ軸負方向の定数加速度ベクトル、α5はＺ軸正方向の定数加速度ベクトル、α6はｚ軸負方向の定数加速度ベクトルである。
この他に、定数加速度ベクトルとして、α7＝０の運動モデルを想定すると、運動モデルのモデル数Ｎは７個となる。
定数加速度の１軸には、北基準直交座標系の北方向、推定速度ベクトル、ＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）などが考えられる。ここで、ＬＯＳは、レーダと目標を結んだベクトルである。
次に、サンプリング時刻ｔkにおいて、以下の式（１２）が真であるとする仮説を式（１３）で表し、運動モデルの推移にマルコフ性を仮定する。
即ち、運動モデルΨk,αがサンプリング時刻ｔk-1の運動モデルにより決まり、サンプリング時刻ｔk-2までの運動モデルには依存しないものとする。
時刻ｔkにおける運動モデル間の推移確率を以下の式（１４）のように表し、例えば、最小サンプリング間隔をｔs＝ｔk−ｔk-1として、サンプリング間隔ｔs＝ｔk−ｔk-1の場合の運動モデル間推移確率を式（１４）と同様の意味でｐabとする。
さらに、Ｎ×Ｎ行列でａ行ｂ列の要素をｐabとしたものを運動モデル間推移確率行列Πと表し、サンプリング間隔がｔk−ｔk-1＝ｍ・ｔs、（ｍ＝１，２，…）と表せる場合、サンプリング時刻ｔkでの運動モデル間推移確率行列Πkは以下の式（１５）のように表される。このときの運動モデル間推移確率行列Πkのａ行ｂ列の要素が運動モデル間推移確率ｐk,abである。
時刻ｔkにおける運動モデル間の推移確率を以下の式（１４）のように表し、例えば、最小サンプリング間隔をｔs＝ｔk−ｔk-1として、サンプリング間隔ｔs＝ｔk−ｔk-1の場合の運動モデル間推移確率を式（１４）と同様の意味でｐabとする。
さらに、Ｎ×Ｎ行列でａ行ｂ列の要素をｐabとしたものを運動モデル間推移確率行列Πと表し、サンプリング間隔がｔk−ｔk-1＝ｍ・ｔs、（ｍ＝１，２，…）と表せる場合、サンプリング時刻ｔkでの運動モデル間推移確率行列Πkは以下の式（１５）のように表される。このときの運動モデル間推移確率行列Πkのａ行ｂ列の要素が運動モデル間推移確率ｐk,abである。
サンプリング時刻ｔkまでの観測情報Ｚkに基づくサンプリング時刻ｔkでの運動モデルの信頼度を、条件付き確率密度関数によって以下の式（１６）で定義すると、以下の式（１７）で計算される。
ただし、νk,aは、観測ベクトルの正規分布近似Ｐ［Ｚk｜Ψk,a，Ｚk-1］を多変量正規分布で近似したものであり、以下の式（１８）で表される。
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくサンプリング時刻ｔkでの運動モデルの事前信頼度を、条件付き確率密度関数によって以下の式（１９）と定義すると、以下の式（２０）で計算される。
サンプリング時刻ｔkまでの観測情報Ｚkに基づく定数加速度ベクトルの推定値を以下の式（２１）で定義すると、定数加速度ベクトルの推定値（推定加速度ベクトル）は、以下の式（２２）のように表される。
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づく定数加速度ベクトルの推定値を以下の式（２３）で定義すると、定数加速度ベクトルの推定値（予測加速度ベクトル）は、以下の式（２４）のように表される。
ここで、追尾対象の目標の観測値が、サンプリング時刻ｔkにおいて１つ得られるものとして、その観測系モデルを以下の式（２５）のように表されるものとする。
ただし、Ｚkはサンプリング時刻ｔkにおける位置情報の観測値より構成される直交座標の位置観測ベクトルであり、Ｈはサンプリング時刻ｔkにおける観測行列であり、上記の式（２６）で表される。
ｖkはサンプリング時刻ｔkにおける観測ベクトルＺkに対応する観測雑音ベクトルであって、平均０の３次元正規分布白色雑音であり、上記の式（２７）、式（２８）で表される。
また、Ｒkはサンプリング時刻ｔkにおける観測誤差共分散行列であり、運動モデルによらない値とする。また、駆動雑音ベクトルと観測雑音ベクトルは、互いに独立であるとする。
ｖkはサンプリング時刻ｔkにおける観測ベクトルＺkに対応する観測雑音ベクトルであって、平均０の３次元正規分布白色雑音であり、上記の式（２７）、式（２８）で表される。
また、Ｒkはサンプリング時刻ｔkにおける観測誤差共分散行列であり、運動モデルによらない値とする。また、駆動雑音ベクトルと観測雑音ベクトルは、互いに独立であるとする。
また、サンプリング時刻ｔ1からｔkまでの観測値ベクトルの集積を以下の式（２９）のように表すものとする。
カルマンフィルタの理論によれば、上記のモデルにしたがってサンプリング時刻ｔkで観測値が得られた場合の状態ベクトルＸkの推定値Ｘk（＋）は、以下の式（３０）〜（３８）によって計算される。
ここで、Ｘk,a（−）、Ｘk,a（＋）、Ｘk（−）、Ｘk（＋）、Ｐk,a（−）、Ｐk,a（＋）、Ｐk（−）、Ｐk（＋）は、以下の式（３９）〜（４６）のように定義される。
Ｘk,a（−）は、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値であり、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aに基づいて、サンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
Ｘk,a（＋）は、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値であり、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づいて、サンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
Ｘk,a（＋）は、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づくＸkの条件付き平均値であり、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aに基づいて、サンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
Ｘk（−）は、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくＸkの条件付き平均値であり、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
Ｘk（＋）は、サンプリング時刻ｔkまでの観測情報Ｚkに基づくＸkの条件付き平均値であり、サンプリング時刻ｔkまでの観測情報Ｚkに基づいて、サンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
Ｘk（＋）は、サンプリング時刻ｔkまでの観測情報Ｚkに基づくＸkの条件付き平均値であり、サンプリング時刻ｔkまでの観測情報Ｚkに基づいて、サンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
Ｐk,a（−）、Ｐk,a（＋）、Ｐk（−）、Ｐk（＋）は、それぞれＸk,a（−）、Ｘk,a（＋）、Ｘk（−）、Ｘk（＋）の誤差共分散行列を表す運動モデル毎の予測誤差共分散行列、運動モデル毎の平滑誤差共分散行列、Ｎ個の運動モデルによる予測誤差共分散行列、およびＮ個の運動モデルによる平滑誤差共分散行列である。
上記の式（３４）は、サンプリング時刻ｔkにおけるゲイン行列である。
また、カルマンフィルタを通常適用する場合と同様にして、初期値Ｘ0（＋）、Ｐ0（＋）が別途定まっているものとする。なお、式（３１）より、Ｐk,a（−）は、仮説Ψk,aによらない値なので、式（３４）よりＫk、式（３６）よりＰk,a（＋）も、同様に仮説Ψk,aによらない値となる。
上記の式（３４）は、サンプリング時刻ｔkにおけるゲイン行列である。
また、カルマンフィルタを通常適用する場合と同様にして、初期値Ｘ0（＋）、Ｐ0（＋）が別途定まっているものとする。なお、式（３１）より、Ｐk,a（−）は、仮説Ψk,aによらない値なので、式（３４）よりＫk、式（３６）よりＰk,a（＋）も、同様に仮説Ψk,aによらない値となる。
モデル毎予測器６は、上記のようにして、Ｎ個の運動モデルを設定すると、上記の原理に基づいて、運動モデル毎に、当該運動モデルを用いて目標の予測位置を算出する。
また、モデル毎予測器６は、後述する平滑器３により算出された平滑誤差共分散行列と、予め設定された駆動雑音共分散行列を用いて、運動モデル毎に予測誤差共分散行列を算出する。
遅延回路７は、モデル毎予測器６が目標の予測位置及び予測誤差共分散行列を算出すると、目標の予測位置及び予測誤差共分散行列を１サンプリング時間だけ格納してから乗算器１１に出力する。
また、モデル毎予測器６は、後述する平滑器３により算出された平滑誤差共分散行列と、予め設定された駆動雑音共分散行列を用いて、運動モデル毎に予測誤差共分散行列を算出する。
遅延回路７は、モデル毎予測器６が目標の予測位置及び予測誤差共分散行列を算出すると、目標の予測位置及び予測誤差共分散行列を１サンプリング時間だけ格納してから乗算器１１に出力する。
閾値設定器８は、モデル毎予測器６が目標の予測位置及び予測誤差共分散行列を算出すると、その目標の予測位置及び予測誤差共分散行列と、後述する信頼度算出器１２により算出された１サンプリング前のモデル信頼度と、モデル設定器５により設定されたモデル遷移行列と、予め設定された観測モデルから得られる観測誤差共分散行列とを以下の式（４７）に代入して、運動モデル覆域内における運動モデルの信頼度分布マップを作成する（ステップＳＴ１）。
ただし、νk,a はモデルの適合度を表す尤度を表し、Ｐabはモデル推移確率行列を表し、βk-1,b は１サンプル前のモデル信頼度を表している。
また、νk,aは、以下に示す多変量正規分布の式（４８）により表される。
擬似観測ベクトルzk は、運動モデルの覆域内において、一定間隔で一様に発生させ、各擬似観測ベクトルが得られた位置における上記モデル信頼度を計算する。
図６に示すような航空機の運動をモデル化した運動モデルを用いる場合、モデル信頼度の発振が生じる観測条件及び運動条件では、一例として、図７に示すような尤度分布、図８に示すようなモデル信頼度分布が得られる。誤差楕円体は、後述する閾値Ａにより決定される尤度分布の等確率ラインとなる。
図７において、中央の山が等速直線運動モデルの尤度分布であり、図８において、中央に位置しているモデル信頼度の高い長方形の領域が等速直線運動モデルのモデル信頼度が高い領域となる。
目標が図８の中央の長方形の領域内に存在していれば問題ないが、例えば、右旋回モデルの領域にわずかに入った場合、右旋回モデルに対するモデル信頼度が極めて高くなる。その結果、実際よりも旋回加速度が大きく推定され、モデル信頼度が発振して、追尾精度が劣化する。
図７において、中央の山が等速直線運動モデルの尤度分布であり、図８において、中央に位置しているモデル信頼度の高い長方形の領域が等速直線運動モデルのモデル信頼度が高い領域となる。
目標が図８の中央の長方形の領域内に存在していれば問題ないが、例えば、右旋回モデルの領域にわずかに入った場合、右旋回モデルに対するモデル信頼度が極めて高くなる。その結果、実際よりも旋回加速度が大きく推定され、モデル信頼度が発振して、追尾精度が劣化する。
したがって、閾値Ａを大きな値に設定した場合、等速直線運動モデルの誤差楕円体は、図８の中央の長方形の領域よりも大きくなるため、観測値が等速直線運動モデルの誤差楕円体内に得られても、加速度が実際より大きく推定され、モデル信頼度が発振する。
一方、閾値Ａを小さく設定した場合、逆に等速直線運動目標からの観測値が、観測雑音の影響で頻繁に誤差楕円体の領域外に得られるようになる。
このような場合、観測値が図８の長方形の領域内に存在していれば、モデル信頼度が発振しないが、不必要に誤差楕円体の拡大が行われ、却って観測誤差低減性能の劣化を招くことになる。
したがって、誤差楕円体重複度判定及び誤差楕円体の拡大に用いる閾値の最適化は、追尾フィルタの性能を最大限発揮させる上で重要である。
一方、閾値Ａを小さく設定した場合、逆に等速直線運動目標からの観測値が、観測雑音の影響で頻繁に誤差楕円体の領域外に得られるようになる。
このような場合、観測値が図８の長方形の領域内に存在していれば、モデル信頼度が発振しないが、不必要に誤差楕円体の拡大が行われ、却って観測誤差低減性能の劣化を招くことになる。
したがって、誤差楕円体重複度判定及び誤差楕円体の拡大に用いる閾値の最適化は、追尾フィルタの性能を最大限発揮させる上で重要である。
そこで、閾値設定器８は、運動モデル覆域内における運動モデルの信頼度分布マップを作成すると、その信頼度分布マップを参照して、等速直線運動の運動モデルの信頼度が一定値以上の地点を抽出する。即ち、信頼度の発振が生じない領域を特定する（ステップＳＴ２）。
図６に示すような二次元空間において、５つの運動モデル（例えば、等速直線運動、左右旋回、加減速モデル）が十字に配置されている場合、定数加速度の大きさが同一であれば、等速直線運動モデルのモデル信頼度の高い領域は正方形となる。一方、左右旋回や加減速モデルの定数加速度の大きさが異なれば、等速直線運動モデルのモデル信頼度の高い領域は長方形となる。観測値がモデル信頼度の高い領域に存在している限り、加速度を誤って大きく推定されることがなく、モデル信頼度は発振しない。
図６に示すような二次元空間において、５つの運動モデル（例えば、等速直線運動、左右旋回、加減速モデル）が十字に配置されている場合、定数加速度の大きさが同一であれば、等速直線運動モデルのモデル信頼度の高い領域は正方形となる。一方、左右旋回や加減速モデルの定数加速度の大きさが異なれば、等速直線運動モデルのモデル信頼度の高い領域は長方形となる。観測値がモデル信頼度の高い領域に存在している限り、加速度を誤って大きく推定されることがなく、モデル信頼度は発振しない。
閾値設定器８は、信頼度の発振が生じない領域を特定すると、その領域の各辺の長さを算出する（ステップＳＴ３）。即ち、１対の運動モデル（例えば、左右旋回の運動モデル）の予測値に最も近い地点同士を結んだ直線の長さを求める。
また、閾値設定器８は、式（４８）における残差共分散行列について、固有ベクトル方向の固有値λを算出する（ステップＳＴ４）。固有値の数は二次元空間では２個となる。
ここで、誤差楕円体の長径又は短径ｌk,i と、固有値λk,i と、誤差楕円体の等確率ラインを決定する閾値Ａ＝ｄとの関係は、以下の式（４９）から得られる。
ただし、
は２次元空間においては１，２となる。信頼度領域の各辺と２個の固有ベクトルの対応付けについては、固有ベクトルと一対の運動モデルの予測値同士を結んだベクトル同士のなす角度により決定される。
なお、固有値の算出方法は、特に限定するものではないが、例えば、以下の非特許文献に開示されている方法を使用すればよい。
・非特許文献
「ＮＵＭＥＲＩＣＡＬ ＲＥＣＩＰＥＳ ｉｎ Ｃ［日本語版］」技術評論社発行、第３３９頁から第３４５頁
また、閾値設定器８は、式（４８）における残差共分散行列について、固有ベクトル方向の固有値λを算出する（ステップＳＴ４）。固有値の数は二次元空間では２個となる。
ここで、誤差楕円体の長径又は短径ｌk,i と、固有値λk,i と、誤差楕円体の等確率ラインを決定する閾値Ａ＝ｄとの関係は、以下の式（４９）から得られる。
なお、固有値の算出方法は、特に限定するものではないが、例えば、以下の非特許文献に開示されている方法を使用すればよい。
・非特許文献
「ＮＵＭＥＲＩＣＡＬ ＲＥＣＩＰＥＳ ｉｎ Ｃ［日本語版］」技術評論社発行、第３３９頁から第３４５頁
閾値設定器８は、上記のようにして、信頼度の発振が生じない領域の各辺の長さと、固有ベクトル方向の固有値λを算出すると、その領域の各辺の長さと固有値λに基づいて、等速直線運動モデルに対する信頼度の高い領域に誤差楕円体が収まるような閾値Ａを決定する（ステップＳＴ５）。
即ち、信頼度領域の各辺SIk,1 及びSIk,2 と、誤差楕円体の長径又は半径が一致するような閾値d1 及びd2 をそれぞれ算出し、最も小さい閾値を選択することで、誤差楕円体をモデル信頼度の高い領域に収めることができる。
ただし、誤差楕円体同士の重複度が十分大きい場合、モデル信頼度が均一化しており、等速直線運動モデルに対するモデル信頼度が高い領域が得られない。この場合には、閾値Ａの自動算出は行わず、事前に設定した閾値を用いるようにする。
即ち、信頼度領域の各辺SIk,1 及びSIk,2 と、誤差楕円体の長径又は半径が一致するような閾値d1 及びd2 をそれぞれ算出し、最も小さい閾値を選択することで、誤差楕円体をモデル信頼度の高い領域に収めることができる。
閾値設定器８は、信頼度の高い領域に誤差楕円体が収まる閾値Ａを決定すると、その誤差楕円体の領域内に、目標の観測値が得られる理論的な確率を算出する（ステップＳＴ６）。
理論的な確率は、公知のχ自乗分布表を参照すれば、上記の閾値Ａから一意に決定することができる。
理論的な確率は、公知のχ自乗分布表を参照すれば、上記の閾値Ａから一意に決定することができる。
閾値設定器８は、理論的な確率が一定値以上であれば（ステップＳＴ７）、誤差楕円体内存在確率が高いので、閾値Ａを誤差共分散行列制御器１０に出力する。
一方、理論的な確率が一定値以下であれば（ステップＳＴ７）、閾値Ａを誤差共分散行列制御器１０に出力せずに、誤差楕円体の拡大に用いる閾値Ｂを求め、閾値Ｂを誤差共分散行列制御器１０に出力する。
誤差楕円体の拡大に用いる閾値Ｂは、公知のχ自乗分布表を参照すれば、目標の存在期待確率（誤差楕円体内存在確率）から一意に決定することができる。
なお、誤差楕円体の拡大に用いる閾値Ｂを求める理由は、信頼度が発振しない領域内に誤差楕円体を収めるために設定した閾値Ａでは、誤判定確率が高い場合、不必要な誤差楕円体の拡大によって観測雑音抑圧性能が劣化することがあるからである。
一方、理論的な確率が一定値以下であれば（ステップＳＴ７）、閾値Ａを誤差共分散行列制御器１０に出力せずに、誤差楕円体の拡大に用いる閾値Ｂを求め、閾値Ｂを誤差共分散行列制御器１０に出力する。
誤差楕円体の拡大に用いる閾値Ｂは、公知のχ自乗分布表を参照すれば、目標の存在期待確率（誤差楕円体内存在確率）から一意に決定することができる。
なお、誤差楕円体の拡大に用いる閾値Ｂを求める理由は、信頼度が発振しない領域内に誤差楕円体を収めるために設定した閾値Ａでは、誤判定確率が高い場合、不必要な誤差楕円体の拡大によって観測雑音抑圧性能が劣化することがあるからである。
ここで、例えば、閾値Ａの値が２．３６６である場合、誤差楕円体内に観測値が得られる理論的な確率は５０％となる。この場合、誤判定確率も５０％とみなすことができ、不必要な誤差楕円体の拡大が行われる可能性がある。
観測誤差の抑圧性能の劣化を防ぐため、後述する乗算係数を算出するに際して、観測値の誤差楕円体内存在確率を高く設定し、閾値Ｂを求める。
例えば、観測値が等速直線運動モデルの誤差楕円体内に存在する確率を、乗算係数の計算上９０％にしたい場合、閾値Ｂは、χ自乗分布表より６．２５１となる。
観測誤差の抑圧性能の劣化を防ぐため、後述する乗算係数を算出するに際して、観測値の誤差楕円体内存在確率を高く設定し、閾値Ｂを求める。
例えば、観測値が等速直線運動モデルの誤差楕円体内に存在する確率を、乗算係数の計算上９０％にしたい場合、閾値Ｂは、χ自乗分布表より６．２５１となる。
誤差楕円体重複判定器９は、上記のようにして、閾値設定器８が閾値Ａを設定すると、モデル毎予測器６から運動モデル毎の目標の予測値と予測誤差共分散行列を取得するとともに目標の観測値を取得し、下記の式（５２）に示すように、運動モデル毎に、その観測値と予測値の残差二次形式を算出する（ステップＳＴ１１）。なお、残差二次形式が閾値Ａと一致する場合、目標の観測値は誤差楕円体の境界上に存在することになる。
また、残差は３変量正規分布に従うため、その残差２次形式はχ自乗分布に従う。そのため、閾値Ａが決定されれば、目標の存在確率はχ自乗分布より求めることができる。逆に、所望の目標の存在確率が決定すれば、閾値Ａを求めることも可能である。この実施の形態１では、閾値Ａは閾値設定器８で決定するため、目標の存在確率が一意に求まる。
｛Ｚｋ−ＨＸｋ，ａ（−）｝’Ｓｋ，ａ −１｛Ｚｋ−ＨＸｋ，ａ（−）｝＜ｄ（５２）
Ｓｋ，ａ＝ＨＰｋ，ａ（−）Ｈ＋Ｒｋ （５３）
ただし、Ｓk,aは残差共分散行列であり、式（５２）を満たすある確率で、目標の存在が期待される領域が誤差楕円体となる。
また、残差は３変量正規分布に従うため、その残差２次形式はχ自乗分布に従う。そのため、閾値Ａが決定されれば、目標の存在確率はχ自乗分布より求めることができる。逆に、所望の目標の存在確率が決定すれば、閾値Ａを求めることも可能である。この実施の形態１では、閾値Ａは閾値設定器８で決定するため、目標の存在確率が一意に求まる。
｛Ｚｋ−ＨＸｋ，ａ（−）｝’Ｓｋ，ａ −１｛Ｚｋ−ＨＸｋ，ａ（−）｝＜ｄ（５２）
Ｓｋ，ａ＝ＨＰｋ，ａ（−）Ｈ＋Ｒｋ （５３）
ただし、Ｓk,aは残差共分散行列であり、式（５２）を満たすある確率で、目標の存在が期待される領域が誤差楕円体となる。
誤差楕円体重複判定器９は、残差二次形式を算出すると、その残差二次形式の最小値と閾値設定器８により設定された閾値Ａを比較する（ステップＳＴ１２）。
そして、その残差二次形式の最小値が閾値Ａより大きい場合、運動モデル毎の予測位置を中心とする目標の存在期待確率を示す誤差楕円体（等確率ライン）同士の重複が不十分であるので、目標の観測位置が閾値Ａにより定義される誤差楕円体の領域外に存在している旨を示す重複判定情報（判定係数ε＝１）を誤差共分散行列制御器１０に出力する（ステップＳＴ１３）。
そして、その残差二次形式の最小値が閾値Ａより大きい場合、運動モデル毎の予測位置を中心とする目標の存在期待確率を示す誤差楕円体（等確率ライン）同士の重複が不十分であるので、目標の観測位置が閾値Ａにより定義される誤差楕円体の領域外に存在している旨を示す重複判定情報（判定係数ε＝１）を誤差共分散行列制御器１０に出力する（ステップＳＴ１３）。
誤差楕円体重複判定器９は、残差二次形式の最小値が閾値Ａより小さい場合、その残差二次形式の分散と所定の閾値Ｃを比較する（ステップＳＴ１４）。
そして、その残差二次形式の分散が閾値Ｃより小さい場合、誤差楕円体同士が重なり過ぎているので、重なり過ぎている旨を示す重複判定情報（判定係数ε＝２）を誤差共分散行列制御器１０に出力する（ステップＳＴ１５）。
一方、その残差二次形式の分散が閾値Ｃより大きい場合、誤差楕円体が適度に重なっているので、適度に重なっている旨を示す重複判定情報（判定係数ε＝０）を誤差共分散行列制御器１０に出力する（ステップＳＴ１６）。
そして、その残差二次形式の分散が閾値Ｃより小さい場合、誤差楕円体同士が重なり過ぎているので、重なり過ぎている旨を示す重複判定情報（判定係数ε＝２）を誤差共分散行列制御器１０に出力する（ステップＳＴ１５）。
一方、その残差二次形式の分散が閾値Ｃより大きい場合、誤差楕円体が適度に重なっているので、適度に重なっている旨を示す重複判定情報（判定係数ε＝０）を誤差共分散行列制御器１０に出力する（ステップＳＴ１６）。
誤差共分散行列制御器１０は、誤差楕円体重複判定器９から重複判定情報を受けると、その重複判定情報を参照して、誤差楕円体が適度に重なっているか否かを判定する。
即ち、全ての運動モデルの判定係数がε＝０であるか否かを判定する（ステップＳＴ２１）。
全ての運動モデルの判定係数がε＝０であれば、誤差楕円体の拡大や縮小処理を実施する必要がないので、乗算係数を算出せずに処理を終了する。
即ち、全ての運動モデルの判定係数がε＝０であるか否かを判定する（ステップＳＴ２１）。
全ての運動モデルの判定係数がε＝０であれば、誤差楕円体の拡大や縮小処理を実施する必要がないので、乗算係数を算出せずに処理を終了する。
誤差共分散行列制御器１０は、判定係数がε＝１である場合、あるいは、判定係数がε＝２である場合、各運動モデルの予測位置と観測位置の残差二次形式を算出する（ステップＳＴ２２）。残差二次形式は式（５２）を使用して算出する。
誤差共分散行列制御器１０は、閾値設定器８から閾値Ａが出力された場合、その残差二次形式の最小値が閾値Ａと一致するような乗算係数を算出する（ステップＳＴ２３）。
一方、閾値設定器８から閾値Ｂが出力された場合、その残差二次形式の最小値が閾値Ｂと一致するような乗算係数を算出する（ステップＳＴ２３）。
ここで、乗算係数は、後述する乗算器１１が信頼度計算における残差共分散行列に乗算する係数であり、誤差楕円体の拡大や縮小処理を実施するためのものである。その乗算係数の算出方法は、特に限定するものではないが、例えば、下記の非特許文献に記載されている方法を使用すればよい。
・非特許文献
「Ｍ３ Ｆｉｌｔｅｒ ｗｉｔｈ Ｅｒｒｏｒ Ｅｌｌｉｐｓｅｓ Ｃｏｎｔｒｏｌ ｉｎ Ｍｏｄｅ Ｐｒｏｂａｂｉｌｉｔｉｅｓ Ｃａｌｃｕｌａｔｉｏｎ」 ２００３年 ８月 Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＳＩＣＥ ２００３ 第７７頁から第８１頁
誤差共分散行列制御器１０は、閾値設定器８から閾値Ａが出力された場合、その残差二次形式の最小値が閾値Ａと一致するような乗算係数を算出する（ステップＳＴ２３）。
一方、閾値設定器８から閾値Ｂが出力された場合、その残差二次形式の最小値が閾値Ｂと一致するような乗算係数を算出する（ステップＳＴ２３）。
ここで、乗算係数は、後述する乗算器１１が信頼度計算における残差共分散行列に乗算する係数であり、誤差楕円体の拡大や縮小処理を実施するためのものである。その乗算係数の算出方法は、特に限定するものではないが、例えば、下記の非特許文献に記載されている方法を使用すればよい。
・非特許文献
「Ｍ３ Ｆｉｌｔｅｒ ｗｉｔｈ Ｅｒｒｏｒ Ｅｌｌｉｐｓｅｓ Ｃｏｎｔｒｏｌ ｉｎ Ｍｏｄｅ Ｐｒｏｂａｂｉｌｉｔｉｅｓ Ｃａｌｃｕｌａｔｉｏｎ」 ２００３年 ８月 Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ＳＩＣＥ ２００３ 第７７頁から第８１頁
乗算器１１は、誤差共分散行列制御器１０から乗算係数を受けると、その乗算係数を遅延回路７から出力された予測誤差共分散行列に乗算し、乗算後の予測誤差共分散行列を信頼度算出器１２に出力する（ステップＳＴ２４）。
ただし、乗算器１１は、誤差共分散行列制御器１０が乗算係数を算出しない場合、遅延回路７から出力された予測誤差共分散行列を信頼度算出器１２に出力する。
ただし、乗算器１１は、誤差共分散行列制御器１０が乗算係数を算出しない場合、遅延回路７から出力された予測誤差共分散行列を信頼度算出器１２に出力する。
目標追尾装置の以降の処理内容は、上記特許文献１に開示されている目標追尾装置と同様であるため、処理内容の詳細は省略するが、簡単には次の通りである。
信頼度算出器１２は、ゲート判定器２から目標の観測位置が出力され、乗算器１１から運動モデル毎の予測誤差共分散行列が出力され、遅延回路７から運動モデル毎の予測位置が出力され、モデル設定器５からモデル遷移行列が出力され、信頼度メモリ１３に格納されている１サンプリング前の各運動モデルの信頼度を取得すると、その目標の観測位置と、運動モデル毎の予測誤差共分散行列と、運動モデル毎の予測位置と、モデル遷移行列と、１サンプリング前の各運動モデルの信頼度とに基づいて、Ｎ個の運動モデルの信頼度を算出する。
信頼度算出器１２により算出されたＮ個の運動モデルの信頼度は、信頼度メモリ１３に格納される。また、平滑器３及び遅延回路１４に出力される。
信頼度算出器１２により算出されたＮ個の運動モデルの信頼度は、信頼度メモリ１３に格納される。また、平滑器３及び遅延回路１４に出力される。
入力ベクトル予測器１５は、遅延回路１４により１サンプリング時間だけ遅延されたＮ個の運動モデルの信頼度と、モデル設定器５により設定されたモデル遷移行列とに基づいて、入力ベクトルの予測値を算出する。
予測器１６は、後述する平滑器３により算出された平滑値と、遅延回路１４により１サンプリング時間だけ遅延されたＮ個の運動モデルの信頼度と、入力ベクトル予測器１５により算出された入力ベクトルの予測値と、モデル設定器５により設定された定数加速度ベクトルとに基づいて、目標の予測値及び予測誤差共分散行列を算出する。
予測器１６により算出された目標の予測値及び予測誤差共分散行列は、遅延回路１７に格納される。
予測器１６は、後述する平滑器３により算出された平滑値と、遅延回路１４により１サンプリング時間だけ遅延されたＮ個の運動モデルの信頼度と、入力ベクトル予測器１５により算出された入力ベクトルの予測値と、モデル設定器５により設定された定数加速度ベクトルとに基づいて、目標の予測値及び予測誤差共分散行列を算出する。
予測器１６により算出された目標の予測値及び予測誤差共分散行列は、遅延回路１７に格納される。
ゲート判定器２は、目標観測装置１から出力された目標位置観測情報が示す目標の観測位置と、遅延回路１７により１サンプリング時間だけ遅延された目標の予測位置との残差二次形式を算出する。
そして、ゲート判定器２は、その残差二次形式が所定の閾値より大きい場合、その目標位置観測情報は真の目標の観測位置を示す情報ではないと判断して、その目標位置観測情報を廃棄する。
一方、その残差二次形式が所定の閾値より小さい場合、その目標位置観測情報は真の目標の観測位置を示す情報であると判断して、その目標の観測位置を平滑器３に出力する。
そして、ゲート判定器２は、その残差二次形式が所定の閾値より大きい場合、その目標位置観測情報は真の目標の観測位置を示す情報ではないと判断して、その目標位置観測情報を廃棄する。
一方、その残差二次形式が所定の閾値より小さい場合、その目標位置観測情報は真の目標の観測位置を示す情報であると判断して、その目標の観測位置を平滑器３に出力する。
入力ベクトル平滑器１８は、信頼度算出器１２からＮ個の運動モデルの信頼度を受けると、そのＮ個の運動モデルの信頼度と、モデル設定器５により設定された定数加速度ベクトルとに基づいて入力ベクトルの平滑値を算出し、その入力ベクトルの平滑値を平滑器３に出力する。
ゲイン行列算出器２０は、遅延回路１９により１サンプリング時間だけ遅延された運動モデル毎の予測誤差共分散行列と、予め設定された観測モデルから得られる観測誤差共分散行列とに基づいて、平滑値の算出に使用するゲイン行列を算出し、そのゲイン行列を平滑器３に出力する。
ゲイン行列算出器２０は、遅延回路１９により１サンプリング時間だけ遅延された運動モデル毎の予測誤差共分散行列と、予め設定された観測モデルから得られる観測誤差共分散行列とに基づいて、平滑値の算出に使用するゲイン行列を算出し、そのゲイン行列を平滑器３に出力する。
平滑器３は、ゲート判定器２から出力された目標の観測位置と、信頼度算出器１２により算出されたＮ個の運動モデルの信頼度と、入力ベクトル平滑器１８により算出された入力ベクトルの平滑値と、ゲイン行列算出器２０により算出されたゲイン行列と、モデル設定器５により設定された定数加速度ベクトルと、平滑器メモリ４に格納されている１サンプリング前の平滑値及び平滑誤差共分散行列とに基づいて、目標の位置を示す平滑値及び平滑誤差共分散行列を算出する。
表示装置２１は、平滑器３により算出された平滑値を表示する。
表示装置２１は、平滑器３により算出された平滑値を表示する。
以上で明らかなように、この実施の形態１によれば、信頼度算出器１２により算出される等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値Ａを算出するように構成したので、運動モデルの信頼度の発振や、観測雑音低減性能の劣化を招かない閾値が得られ、追尾精度を高めることができる効果を奏する。
また、この実施の形態１によれば、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる理論的な確率を算出し、その理論確率が一定値より高い場合、信頼度が高い領域内に誤差楕円体が収まる閾値Ａから乗算係数を算出し、その理論確率が一定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値Ｂを求め、その閾値Ｂから乗算係数を算出するように構成したので、誤判定確率が高い場合でも、不必要な誤差楕円体の拡大による観測雑音抑圧性能の劣化を防止することができる効果を奏する。
実施の形態２．
上記実施の形態１では、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる理論的な確率を算出し、その理論確率が一定値より高い場合、信頼度が高い領域内に誤差楕円体が収まる閾値Ａから乗算係数を算出し、その理論確率が一定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値Ｂを求め、その閾値Ｂから乗算係数を算出するものについて示したが、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる確率を実測し、その実測確率が一定値より高い場合、信頼度が高い領域内に誤差楕円体が収まる閾値Ａから乗算係数を算出し、その実測確率が一定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値Ｂを求め、その閾値Ｂから乗算係数を算出するようにしてもよく、上記実施の形態１と同様の効果を奏することができる。
上記実施の形態１では、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる理論的な確率を算出し、その理論確率が一定値より高い場合、信頼度が高い領域内に誤差楕円体が収まる閾値Ａから乗算係数を算出し、その理論確率が一定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値Ｂを求め、その閾値Ｂから乗算係数を算出するものについて示したが、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる確率を実測し、その実測確率が一定値より高い場合、信頼度が高い領域内に誤差楕円体が収まる閾値Ａから乗算係数を算出し、その実測確率が一定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値Ｂを求め、その閾値Ｂから乗算係数を算出するようにしてもよく、上記実施の形態１と同様の効果を奏することができる。
即ち、閾値設定器８が、予測位置を中心とする誤差楕円体の領域内に、目標の観測値が得られる理論的な確率を算出する代わりに、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる確率を実測し、その実測確率と一定値を比較するようにしてもよい。
なお、実測確率は、一定のサンプリング期間において、等速直線運動モデルの誤差楕円体内に目標の観測位置が得られる回数を計測することにより求めることができる。
なお、実測確率は、一定のサンプリング期間において、等速直線運動モデルの誤差楕円体内に目標の観測位置が得られる回数を計測することにより求めることができる。
１ 目標観測装置、２ ゲート判定器、３ 平滑器、４ 平滑器メモリ、５ モデル設定器（予測位置算出手段）、６ モデル毎予測器（予測位置算出手段）、７ 遅延回路（予測位置算出手段）、８ 閾値設定器（閾値算出手段）、９ 誤差楕円体重複判定器（存在判定手段）、１０ 誤差共分散行列制御器（存在判定手段）、１１ 乗算器（信頼度算出手段）、１２ 信頼度算出器（信頼度算出手段）、１３ 信頼度メモリ（信頼度算出手段）、１４ 遅延回路、１５ 入力ベクトル予測器、１６ 予測器、１７ 遅延回路、１８ 入力ベクトル平滑器、１９ 遅延回路、２０ ゲイン行列算出器、２１ 表示装置。
Claims (4)
- 等速直線運動の運動モデルを含む複数の運動モデルを追尾フィルタに設定し、各運動モデルを用いて目標の予測位置を算出する予測位置算出手段と、目標の観測位置と上記予測位置算出手段により算出された予測位置の残差二次形式を算出し、その残差二次形式を所定の閾値と比較することによって、上記予測位置を中心とする目標の存在期待確率を示す誤差楕円体の領域内に、目標の観測位置が存在しているか否かを判定する存在判定手段と、上記存在判定手段の判定結果に応じて誤差楕円体の拡大処理又は縮小処理を実施し、処理後の誤差楕円体を用いて複数の運動モデルの信頼度を算出する信頼度算出手段と、上記信頼度算出手段により算出される等速直線運動の運動モデルの信頼度が所定値より高い領域内に、その運動モデルを用いて算出された予測位置を中心とする誤差楕円体が収まる閾値を算出し、その閾値を上記存在判定手段に出力する閾値算出手段とを備えた目標追尾装置。
- 信頼度算出手段は、閾値算出手段により算出された閾値を用いて誤差楕円体の拡大処理又は縮小処理を実施することを特徴とする請求項１記載の目標追尾装置。
- 閾値算出手段は、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる理論的な確率を算出し、その理論確率が所定値より高い場合、存在判定手段に出力する閾値と同一の閾値を信頼度算出手段に出力し、その理論確率が所定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値を求め、その閾値を上記信頼度算出手段に出力することを特徴とする請求項２記載の目標追尾装置。
- 閾値算出手段は、予測位置を中心とする誤差楕円体の領域内に、目標の観測位置が得られる確率を実測し、その実測確率が所定値より高い場合、存在判定手段に出力する閾値と同一の閾値を信頼度算出手段に出力し、その実測確率が所定値より低い場合、χ自乗分布表を参照して目標の存在期待確率が得られる閾値を求め、その閾値を上記信頼度算出手段に出力することを特徴とする請求項２記載の目標追尾装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2004346794A JP2006153736A (ja) | 2004-11-30 | 2004-11-30 | 目標追尾装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2004346794A JP2006153736A (ja) | 2004-11-30 | 2004-11-30 | 目標追尾装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2006153736A true JP2006153736A (ja) | 2006-06-15 |
Family
ID=36632215
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2004346794A Pending JP2006153736A (ja) | 2004-11-30 | 2004-11-30 | 目標追尾装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2006153736A (ja) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011196940A (ja) * | 2010-03-23 | 2011-10-06 | Mitsubishi Electric Corp | 追尾装置 |
| US9240053B2 (en) | 2010-03-15 | 2016-01-19 | Bae Systems Plc | Target tracking |
| US9305244B2 (en) | 2010-03-15 | 2016-04-05 | Bae Systems Plc | Target tracking |
| WO2018211638A1 (ja) | 2017-05-17 | 2018-11-22 | 三菱電機株式会社 | 物体同定装置、路側装置および物体同定方法 |
-
2004
- 2004-11-30 JP JP2004346794A patent/JP2006153736A/ja active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9240053B2 (en) | 2010-03-15 | 2016-01-19 | Bae Systems Plc | Target tracking |
| US9305244B2 (en) | 2010-03-15 | 2016-04-05 | Bae Systems Plc | Target tracking |
| JP2011196940A (ja) * | 2010-03-23 | 2011-10-06 | Mitsubishi Electric Corp | 追尾装置 |
| WO2018211638A1 (ja) | 2017-05-17 | 2018-11-22 | 三菱電機株式会社 | 物体同定装置、路側装置および物体同定方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6736412B2 (ja) | 目標物監視システムおよび目標物監視方法 | |
| WO2018209997A1 (zh) | 用于目标跟踪的车载毫米波雷达滤波估计方法、装置及存储介质 | |
| RU2690305C1 (ru) | Способ определения демпфирующих гидродинамических характеристик корабля | |
| CA2524424A1 (en) | Multipath height finding method | |
| US11300663B2 (en) | Method for predicting a motion of an object | |
| Fanaswala et al. | Destination-aware target tracking via syntactic signal processing | |
| KR101982674B1 (ko) | 밀도 수정에 기초한 대체 트레이닝 분포 | |
| JP5692091B2 (ja) | 情報処理装置、情報処理方法およびコンピュータプログラム | |
| JP4866896B2 (ja) | 気象予測装置、気象予測方法および気象予測プログラム | |
| CN111678513A (zh) | 一种超宽带/惯导紧耦合的室内定位装置与系统 | |
| JP5230132B2 (ja) | 目標追尾装置及び目標追尾方法 | |
| JP5247066B2 (ja) | 目標追尾装置 | |
| JP2006153736A (ja) | 目標追尾装置 | |
| CN110442142B (zh) | 速度数据处理方法、装置、电子设备及计算机可读介质 | |
| JP4882544B2 (ja) | 追尾処理装置及びその方法並びにプログラム | |
| JP3926602B2 (ja) | 目標追尾装置及び方法 | |
| JP4817854B2 (ja) | 周辺物体追尾装置及び周辺物体位置予測方法 | |
| KR102469164B1 (ko) | 무인 수상선의 지구물리적 내비게이션 장치 및 방법 | |
| JP3792927B2 (ja) | 目標追尾装置 | |
| JP4977492B2 (ja) | 目標追尾装置及び目標追尾方法 | |
| JP2005077167A (ja) | 目標追尾装置 | |
| Havyarimana et al. | A novel probabilistic approach for vehicle position prediction in free, partial, and full GPS outages | |
| JPH0797136B2 (ja) | 多目標追尾方法及びその装置 | |
| JPWO2015178410A1 (ja) | 横メタセンタ高さ推定装置及び横メタセンタ高さ推定方法 | |
| JP3323415B2 (ja) | 目標追尾装置及びその目標追尾方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20071012 |