JPH0769419B2 - 追尾フイルタ - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は観測雑音を含んだ目標位置情報を目標観測装
置より入力し，目標位置の真値および速度・加速度など
の目標運動諸元を推定する追尾フイルタの精度向上の方
法について提案するものである。

〔従来の技術〕

第２図は例えばIEEE TRANSACTIONS ON AEROSPACE AND E
LECTRONIC SYSTEMS VOL.AES−13 No.3「MANEUVERING TA
RGET TRAC−KING USING ADAPTIVE STATE ESTIMATION」
で示された複数個の運動モデルを有する場合の従来の追
尾フイルタの構成図であり，図において，（１）は観測
雑音を含んだ目標位置を計測する目標観測装置，（２）
は複数個の運動モデルおのおのに対して観測誤差を評価
するための運動モデルごとの観測誤差評価器，（３）は
複数個の運動モデルおのおのの信頼度を算出するための
各運動モデルの信頼度算出器，（４）は第１の遅延回
路，（５）は目標運動諸元の平滑値算出に使用するカル
マンゲイン行列算出のためのゲイン行列算出器，（６）
は目標運動諸元の平滑値を算出する平滑器，（７）は複
数個の運動モデルおのおのについて現時点より１サンプ
ルング後の目標運動諸元の予測値を算出する運動モデル
ごとの予測値算出器，（８）は第３の遅延回路，（９）
は複数個の運動モデルおのおのの平滑値と真値の差であ
る平滑誤差を評価する運動モデルごとの平滑誤差評価
器，（10）は複数個の運動モデルおのおのの予測値と真
値の差である予測誤差を評価する運動モデルごとの予測
誤差評価器，（11）は第２の遅延回路，（12）は第４の
遅延回路である。

従来の複数個の運動モデルを有する追尾フイルタは上記
のように構成され，たとえば固定値交座標を使用し，等
速直線運動モデルにサンプリング時刻によらない定数加
速度ベクトルが付加されたモデルを複数の異なつた加速
度レベルごとに有する場合が複数個の運動モデルとして
使用されていた。目標観測装置（１）では，極座標によ
る観測雑音を含んだ目標位置情報を直交座標に変換す
る。運動モデルごとの観測誤差評価器（２）では，運動
モデルごとの予測値算出器（７）より得られる現時点よ
り１サンプリング前に算出した目標位置予測ベクトルを
目標位置の平均ベクトルとし，運動モデルごとの予測誤
差評価器（10）より得られる現時点より１サンプリング
前に算出した予測誤差共分散行列およびあらかじめ設定
してある観測モデルより得られる観測雑音の共分散行列
より目標位置観測ベクトルと目標位置予測ベクトルの差
の共分散行列を求め，目標位置観測ベクトルが３次元正
規分布に従うとして複数個の運動モデルごとに，目標観
測装置より入力される目標観測位置の目標予測位置に対
する誤差を評価する。各運動モデルの信頼度算出器
（３）では，現時点より１サンプリング前に算出した複
数個の運動モデルおのおのの信頼度，運動モデルの推移
にマルコフ性を仮定し，あらかじめ設定された運動モデ
ルの推移確率及び運動モデルごとの観測誤差評価器
（２）より入力される複数個の運動モデルごとの観測誤
差評価値より，複数個の運動モデルおのおのの信頼度を
算出する。ゲイン行列算出器（５）では，あらかじめ設
定してある観測モデルより得られる観測雑音共分散行列
及び現時点より１サンプリング前に運動モデルごとの予
測誤差評価器（10）で算出しておいた運動モデルごとの
予測誤差共分散行列よりカルマンフイルタの理論よりゲ
イン行列を算出する。平滑器（６）では，現時点より１
サンプリング前に算出しておいた平滑値，目標観測装置
（１）より入力される目標観測位置，ゲイン行列算出器
（５）より入力されるゲイン行列及び各運動モデルの信
頼度算出器（３）で求めた複数個の運動モデルおのおの
の信頼度により重みづけした複数個の運動モデルを構成
している定数加速度ベクトルの総和により目標位置及び
目標速度の平滑値を算出する。運動モデルごとの予測値
算出器（７）では，平滑器（６）で算出した目標位置及
び目標速度と複数個の運動モデルごとの定数加速度ベク
トルにより現時点より１サンプリング後の目標位置及び
目標速度の予測値を算出する。運動モデルごとの平滑誤
差評価器（９）では，目標位置及び目標速度の平滑値と
真値の差の共分散行列をゲイン行列算出器（５）で算出
したゲイン行列及び現時点より１サンプリング前に運動
モデルごとの予測誤差評価器（10）で算出しておいた予
測誤差共分散行列により算出する。運動モデルごとの予
測誤差評価器（10）では，運動モデルごとの平滑誤差評
価器（９）より入力される平滑誤差共分散行列とあらか
じめ設定してある運動モデルより得られる駆動雑音共分
散行列より現時点より１サンプリング後の予測値と真値
との差の共分散行列を算出する。目標観測装置（１）で
は平滑器（６）より入力される目標位置及び目標速度の
平滑値をもとに目標が等速度直線運動を行うとして算出
した現時点より１サンプリング後の直交座標による目標
位置の予測値を中心に、運動モデルごとの予測誤差評価
器（10）で算出した運動モデルごとの予測誤差共分散行
列が運動モデルによらず一定となる性質を利用して目標
が存在する予測範囲を算出し、この範囲内の目標位置情
報のみを追尾目標から得られたものとして扱う。

〔発明が解決しようとする問題点〕

上記のような従来の追尾フイルタでは，平滑器（６）で
算出する目標位置及び目標速度の平滑値が各運動モデル
の信頼度算出器（３）で求めた複数個の運動モデルのお
のおのに対する信頼度を反映した値にもかかわらず，目
標位置及び目標速度の予測値の誤差を評価する機構は各
運動モデルごとにしか存在しなかつたため予測誤差の評
価は従来のカルマンフイルタの手法によつており，運動
モデル一意的に定まるとして求めていたため，目標が旋
回したことによつて生じる予測誤差の増加が反映されず
現実よりはるかに良い評価結果を算出していた。このた
め，平面内を運動する目標追尾の場合，第４図に示すよ
うに目標予測位置を中心にして，たとえば99％の確率で
目標が存在する予測範囲は現実よりも小さくなつてい
た。ここで，第４図において,Pは追尾目標の予測位置,C
1は曲線C1で囲まれた範囲内に目標が99％の確率で存在
する従来の追尾フイルタによる予測範囲,C2は曲線C2で
囲まれた範囲内で目標が99％の確率で存在する現実の予
測範囲である。従つて，第４図における曲線C1内の探知
データが追尾対象目標からの探知データであるすると，
目標が旋回している場合，追尾対象目標からの探知デー
タを失うおそれがあつた。この対策として，目標の運動
状態によらず一律に目標の予測存在範囲を広げる手法も
あるが，追尾対象目標以外の目標あるいはクラツタが追
尾対象目標近くにある場合，追尾対象目標からの探知デ
ータとそれ以外の探知データが混在しやすくなるという
欠点があつた。

この発明はこのような問題点を解決するためになされた
もので，旋回目標に対しても精度よく追尾できる追尾フ
イルタを構成することを目的とする。

〔問題点を解決するための手段〕

の発明の追尾フイルタは，予測値と真値の差である予測
誤差の評価である予測誤差共分散行列の算出に複数個の
運動モデルおのおのの信頼度を使用するようにしたもの
である。

〔作用〕

この発明においては，複数個の運動モデルおのおののに
対する信頼度及び複数個の運動モデルを構成する定数加
速度ベクトルを使用し運動モデルが一意的に定まらない
ことにより平滑誤差の評価である予測誤差共分散行列を
増加させる諸元を算出し，この諸元を複数個の運動モデ
ルごとの平滑誤差共分散行列に加算することにより予測
誤差共分散行列を算出するようにしている。すなわち、
この発明による予測誤差共分散行列P_K（−）は、従来の
追尾フィルタによって求まる値である運動モデルごとの
予測誤差共分散行列▲Ｐ^a _K▼（−）に半正値対称行列で
ある式（51）の右辺の最終項を加算して式（51）によっ
て求まる。なお、式（13）に示したように運動モデルご
との予測誤差共分散行列▲Ｐ^a _K▼（−）は仮説によらな
い値となる。

〔実施例〕

第１図はこの発明の一実施例を示す構成図であり，
（１）〜（12）は上記従来装置と全く同一のものであ
る。（13）は予測値の予測誤差を評価する予測誤差評価
器である。

複数個をＮ個とした場合の運動モデルをｘ _ｋ ＝Φ_k-1 ｘ _k-1＋Γ_k-1 ｘ _k-1＋Γ′_k-1 ｕ _k-1 ……
（１） とする。

ここで， ・ｘ _ｋはサンプリング時刻t_kにおける目標運動諸元の真
値をあらわす状態ベクトルであり，直交座標における目
標位置ベクトルを 直交座標における目標速度ベクトルを とした時， である。

・Φ_k-1はサンプリング時刻t_k-1よりt_Kへの状態ベクト
ルｘ _ｋの推移行列で目標が等速直線運動を行うと仮定し
た場合， である。

・ｗ _ｋはサンプリング時刻t_kにおける駆動雑音ベクトル Γ_ｋはサンプリング時刻t_kにおける駆動雑音ベクトルの
変換行列で たとえば，目標の運動モデルを等速直線運動と仮定した
ことによる打切り誤差項Γ_k-1 ｗ _k-1とみればｗ _ｋ は加速度ベクトル相当であり である。

・ｕ _ｋはサンプリング時刻t_kにおいてＮ個の運動モデル
を構成する定数加速度ベクトルでｕ _ｋ ＝α _１又はｕ _ｋ＝α _２又は……又はｕ _ｋ＝α _Ｎ……
（２） であり， Γ′_ｋはサンプリング時刻t_kにおける定数加速度ベクト
ルの変換行列で である。

第３図は水平面に平行な面内で定数加速度ベクトルを説
明する図であり，図において,Oは目標観測装置を原点と
した座標Ｏ−xyの原点,Xは東方向のｘ軸の正とした座標
Ｏ−xyのｘ軸,Yは北方向をｙ軸の正とした座標Ｏ−xyの
ｙ軸,A1はｙ軸の正方向の定数加速度ベクトル,A2はｘ軸
の正の方向の定数加速度ベクトル,A3はｙ軸の負の方向
の定数加速度ベクトル,A4はｘ軸の負の方向の定数加速
度ベクトルである。第３図における定数加速度ベクトル
の大きさを10g（ｇは重力加速度とする）とし，この他
に加速度０の定数加速度ベクトルを考えた運動モデルの
場合 Ｎ＝５ （３） であり，式（２）は とサンプリング時刻t_kに無関係に書ける。

次に、サンプリング時刻t_kにおいてｕ _k-1 ＝α _ａ（ａ＝1,2,…,N） （５） が真であるとの仮説を Ψ_k,a（ａ＝1,2,…,N） （６） と書く。

Ｅを平均をあらわす記号として，ｗ _ｋは平均０の３次元
正規分布白色雑音で Ｅ〔ｗ _ｋ〕＝０ （７） Ｅ〔ｗ _ｋ ｗ _l ^T〕＝Q_k（ｋ＝１の時）,OI（ｋ≠ｌの時）
（８） とする。

ここで， ・０は零ベクトル ・Q_kはサンプリング時刻t_kにおける駆動雑音共分散行列 であり，ａ ^Ｔ はベクトルａの転置ベクトルをあらわす。

観測モデルをｚ _ｋ ＝H_k ｘ _ｋ＋ｖ _ｋ （９） とする。

ここで， ・ｚ _ｋはサンプリング時刻t_kにおける目標位置情報の観
測値より構成される直交座標による観測ベクトル ・H_kはサンプリング時刻t_kにおける観測行列で H_k＝（Ｉ 0I） である。

・ｖ _ｋはサンプリング時刻t_kにおける観測雑音ベクトル
であり，平均０の３次元正規分布白色雑音で， Ｅ〔ｖ _ｋ〕＝０ Ｅ〔ｖ _ｋ ｖ _l ^T〕＝R_k（ｋ＝１の時）， 0I（ｋ≠ｌの時） （10） である。

なお,R_kはサンプリング時刻t_kにおける観測雑音共分散
行列で，運動モデルによらない値とする。

また，サンプリング時刻t₁からt_kまでの観測ベクトル全
体をZ_k,すなわち Z_k＝〔ｚ ₁,ｚ ₂,…，ｚ _ｋ〕 （11） と書く。

以上のことより通常のカルマンフイルタの理論により， _k ^a （−）＝Φ_{k-1 k-1}（＋）＋Γ′_k-1 α _ａ （12） P_k ^a(-)=Φ_k-1P_k-1(+)Φk-1^Ｔ+Γ_k-1Q_k-1Γ_k-1 ^TT (13) _k ^a （＋）＝ _k ^a（−）＋K_k（ｚ _Ｋ−H_{k k} ^a（−））
（14） K_k＝P_k ^a（−）H_k ^T（H_kP_k ^a（−）H_k ^T＋R_k）^-1 （15） P_k ^a（＋）＝P_k ^a（−）−K_kH_kP_k ^a（−） （16） （ａ＝1,2,…,N） である。

ここで ・ _k ^a（−）は仮説Ψ_k,aのもとでの観測ベクトルｚ ₁,
ｚ _２…ｚ _k-1のよりのｘ _ｋの予測ベクトルで，条件つき
平均ベクトルで書けばカルマンフイルタの理論より _k ^a （−）＝（Ｅ）〔ｘ _k|Ψ_k,a,Z^k-1〕 （17） である。

・ _ｋ（＋）は観測ベクトルｚ ₁,ｚ _２…，ｚ _ｋよりのｘ
_ｋの平滑ベクトルで _ｋ （＋）＝Ｅ〔ｘ _k|Z^k〕 （18） である。

・ _k ^a（＋）は仮説Ψ_k,aのもとでの観測ベクトルｚ ₁,
ｚ _２…ｚ _ｋよりのｘ _ｋの平滑ベクトルで _k ^a （＋）＝Ｅ〔ｘ _k|Ψ_k,a,Z^k〕 （19） である。

・P_k（＋）はサンプリング時刻t_kにおける平滑誤差共分
散行列であり，カルマンフイルタの理論より P_ｋ(+)=E〔(x _ｋ- _ｋ(+))(x _ｋ- _ｋ(+))^Ｔ|Z^ｋ〕 (20) である。

・P_k ^a（−）はサンプリング時刻t_kにおける仮説Ψ_k,aの
もとで予測誤差共分散行列で P_k ^a(-)=E〔(x _ｋ- _k ^a(-))(x _ｋ- _k ^a(-))^Ｔ|Ψ_k,a,Z^k-1]
(21) である。

・P_k ^a（＋）はサンプリング時刻t_kにおける仮説Ψ_k,aの
もとでの平滑誤差分散行列で P_k ^a(+)=E〔(x _ｋ- _k ^a(+))(x _ｋ- _k ^a(+))^Ｔ|Ψ_k,a,Z^ｋ]
(22) である。

・K_kはサンプリグ時刻t_kにおけるゲイン行列である。

なお,A^Tは行列Ａの転置行列をあらわす。

また，カルマンフイルタを通常適用する場合と同様にし
て，初期値 _Ｏ（＋）,P_O（＋）は別途定まつていると
する。

式（13）よりP_k ^a（−）は仮説Ψ_k,aによらない値なの
で、式（15）よりK_k,式（16）よりP_k ^a（＋）も同様に仮
説Ψ_k,aによらない値となる。

観測ベクトルｚ ₁,ｚ ₂,…，ｚ _ｋが得られた時の仮説Ψ
_k,aの信頼度β_k,aを条件つき確率密度関数により β_k,a＝Ｐ〔Ψ_k,a|Z^k〕 （23） （ａ＝1,2,…,N） と書く。

運動モデルの推移にマルコフ性を仮定する。すなわち運
動モデルΨ_k,aはサンプリング時刻t_k-1の運動モデルよ
り決まりサンプリング時刻t_k-2までの運動モデルには依
存しないとする。この時，運動モデルの推移確率P_abを P_ab＝Ｐ〔Ψ｜_k,a｜Ψ_ｋ−１,^b〕 （24） （ａ＝1,2,…,N;b＝1,2,…,N） と書く。

仮説Ψ_k,aのもとでの観測誤差の評価 f_a（ｋ）（ａ＝1,2,…,N） （25） を３次元正規分布で近似すればカルマンフイルタの理論
より fa(k)=g(z _ｋ;H_{ｋ k} ^a(-),H_ｋP_k ^a(-)H_k ^T+R_ｋ) (26) である。

ここで， ｇ（ｚ _k;ａ _k,A_k）は平均ベクトルａ _k,共分散行列A_kの３
次元正規分布のｚ _ｋにおける確率密度である。

式（26）よりわかるように，目標位置の観測ベクトルｚ
_ｋが目標位置の予測ベクトルH_{k k} ^a（−）に近いほどf_a
（ｋ）の値は大きくなり目標が仮説Ψ_k,aのもとで運動
したとの信頼度が高く評価される。第５図は観測誤差の
評価値f_a（ｋ）を１次元において説明する図であり，図
において,Gは目標位置をあらわす横軸,Fは確率密度をあ
らわす縦軸,Zは観測ベクトルｚ _ｋの示す目標位置,PAは
仮説Ψ_k,aのもとでの目標予測位置,FAは確率密度ｆａ
（ｋ）,Nは平均H_{k k} ^a（−），分散H_kP_k ^a（−）H_k ^T＋R_k
によつて定まる正規分布曲線である。観測ベクトルｚ _ｋ
が得られていない時点での仮説Ψ_k,aの信頼度Ｐ｛Ψ_k,a
|Z^k-1｝は，マルコフ性より，仮説Ψ_k,aが１サンプリン
グ前の各仮説Ψ_ｋ−1,b（ｂ＝1,2,…,N）より推移して
得られるので，Ψ_ｋ−1,bの信頼度である式（23）のβ
_ｋ−1,b及び推移確率である式（24）のＰａｂより として求まる。

観測ベクトルｚ _ｋが得られた時点での仮説Ψ_k,aの信頼
度β_k,εは，観測ベクトルｚ _ｋが得られない時点の仮説
Ψ_k,aの信頼度Ｐ〔Ψ_k,a|Z^k-1〕に仮説Ψ_k,aの観測ベク
トルｚ _ｋによる評価値ｆａ（ｋ）を乗算した値，すなわ
ち式（27）および式（25）より， に比例すると考えてよい。

確率の性質より だから，式（28）を正規化して である。

式（23）及びベイズの定理より である。

式（18），式（19）及び式（31）より だから， である。

式（32）及び式（14），式（12），式（29）より， _ｋ （＋）＝Φ_{k-1 k-1}（＋）＋Γ′_{k-1 k-1}＋K_k〔Ｚ
_ｋ −H_k（Φ_{k-1 k-1}（＋）＋Γ′_{k-1 k-1}）〕 （33） である。

ここで、 である。

複数個の運動モデルを構成している定数加速度ベクトル
α _ａ（ａ＝1,2,…Ｎ）の条件つき確率密度関数を離散系
における確率論に従い とする。

ここで，デルタ関数δ（ｘ）は ∫…∫Ｇ（ｘ）（ｘdx＝Ｇ（ｏ） （37） の性質を有している。

式（35）及び式（37）より Ｅ〔ｕ _k-1|Ψ_k,a,Z^k-1〕＝∫…∫ｕ _k-1P〔ｕ _k-1 ｜Ψ_k,a,Z^K-1〕ｄｕ _k-1＝∫…∫ｕ _k-1δ（ｕ _k-1 α _adｕ
_k-1＝α _ａ （38） すなわち ▲ ^a _k-1▼＝Ｅ〔ｕ _k-1|Ψ_k,a,Z^k-1〕 （39） とした時 ▲ ^a _k-1▼＝α _ａ （40） である。

式（36）及び式（37）より である。

すなわち， _k-1 ＝Ｅ〔ｕ _k-1|Z^k-1〕 （42） とした時 である。

式（35），式（40）及び式（37）より Ｅ〔（ｕ _k-1−▲ ^a _k-1▼（ｕ _k-1−▲ ^a _k-1▼）T|Ψ
_k,a,Z^k-1〕 ＝∫…∫（ｕ _k-1−α _ａ）（ｕ _k-1−α _ａ）TP〔ｕ _k-1| Ψ_k,a,Z^k-1〕ｄｕ _k-1 =∫…∫(u _k-1-α _ａ)(u _k-1-α _ａ)Tδ〔u _k-1-α _ａ)du _k-1=
OI (44) である。

式（36）及び式（37）より である。 _ｋ （−）を観測ベクトルｚ ₁,ｚ ₂,…，ｚ _K-1よりのｘ
_Ｋの予測ベクトル，すなわち _ｋ （−）＝Ｅ〔ｘ _K|Z_K-1〕 （46） とする。

式（１）に式（46），式（18），式（７）及び式（42）
を適用して _ｋ （−）＝Φ_{k-1 k1}（＋）＋Γ′_{k-1 k-1} （47） を得る。

式（１）及び式（47）よりｘ _ｋ − _ｋ（−）＝Φ_k-1（ｘ _k-1− _k-1（＋））＋Γ
_k-1 ｗ _k-1 ＋Γ′_k-1（ｕ _k-1− _k-1） （48） である。

P_k（−）をサンプリング時刻t_kにおける予測誤差共分散
行列，すなわち P_k（−）＝Ｅ〔（ｘ _ｋ− _ｋ（−））（ｘ _ｋ−
_ｋ（−））^T|Z^k-1〕 （49） とする。

式（49）に式（48）を代入し，式（20），式（８）及び
式（45）を適用し，ｘ _k-1,ｗ _k-1,ｕ _k-1が互いに無相関
とすれば， を得る。

従つて，式（27），式（13）及び式（50）より である。

ここで、式（43）及び式（27）より である。

なお，先に述べたように仮説Ψ_k,aのもとでの平滑誤差
共分散行列P_k ^a（＋）は仮説Ψ_k,aに無関係に定まる値な
ので P_k（＋）＝P_k ^a（＋） （53） とする。

次にこの発明の追尾フイルタの動作を第１図に従つて説
明する。

なお，カルマンフイルタを追尾フイルタに通常適用する
場合と同様にして，初期値はあらかじめ定まつていると
する。

目標観測装置（１）では極座標で目標位置情報を観測
し，結果を直交座標による目標位置情報に変換し，式
（９）の観測ベクトルｚ _ｋを出力する。運動モデルごと
の観測誤差評価器（２）では，運動モデルごとの予測値
算出器（７）より得られる現時点より１サンプリング前
に算出した式（12）の運動モデルごとの予測ベクトル
_k ^a（−）を第４の遅延回路（12）を通して入力し，目標
位置予測ベクトルH_{k k} ^a（−）を算出し運動モデルごと
の予測誤差評価器（10）より得られる現時点より１サン
プリング前に算出した式（13）の運動モデルごとの予測
誤差共分散行列P_k ^a（−）を第２の遅延回路（11）を通
し入力し運動モデルごとの予測位置誤差の共分散行列H_k
P_k ^a（−）H_k ^Tを算出し，これとあらかじめ設定してある
観測モデルより得られる式（10）観測雑音共分散行列R_k
より観測ベクトルｚ _ｋと運動モデルごとの目標位置予測
ベクトルH_{k k} ^a（−）の差の共分散行列を算出し，目標
観測装置（１）より入力される簡素ベクトルｚ _ｋの複数
個の運動モデルおのおのに対する合致度である観測誤差
の評価値である式（25）のf_a（ｋ）を式（26）に従い算
出する。各運動モデルの信頼度算出器（３）では，サン
プリング時刻t_k-1で算出した複数個の運動モデルおのお
のに対する信頼度である式（23）のβ_ｋ−1,aを第１の
遅延回路（４）を通して入力し，これとあらかじめ設定
された式（24）の推移確率P_abより観測ベクトルｚ _ｋが
得られない時点での各運動モデルの信頼度である式（2
7）の値を算出し，式（27）の値に観測ベクトルｚ _ｋの
各運動モデルに対する合致度である運動モデルごとの観
測誤差評価器（２）より入力される式（25）のf_a（ｋ）
を重みづけし、式（30）に従い観測ベクトルｚ _ｋが得ら
れた時点での複数個の運動モデルおのおのの信頼度であ
る式（23）のβ_k,aを算出する。ゲイン行列算出器
（５）では，あらかじめ設定された観測モデルより得ら
れる式（10）の観測雑音共分散列及び１サンプリング前
に算出しておいた運動モデルごとの予測誤差評価器（1
0）より第２の遅延回路（11）を通して入力される運動
モデルごとの予測誤差共分散行列P_k ^a（−）とにより，
式（15）に従い平滑ベクトル _ｋ（＋）算出に使用する
ゲイン行列K_kを算出する。なお、式（13）より運動モデ
ルごとの予測誤差共分散行列▲Ｐ^a _K▼（−）は運動モデ
ルによらない値となるため、式（15）よりゲイン行列K_K
も運度モデルによらない値となる。平滑器（６）では,1
サンプリング前に算出しておいた式（18）の平滑ベクト
ル _k-1（＋）を第３の遅延回路（18）を通し入力し，
等速直線運動予測によりΦ_{k-1 k-1}（＋）を算出し，各
運動モデルの信頼度算出器（３）より得られる式（30）
の各運動モデルの信頼度β_k,aとあらかじめ設定された
式（５）の複数個の運動モデルを構成している定数加速
度ベクトルα _ａより式（34）に従い目標の加速度 _k-1
を推定し， _k-1が予測に影響する項Γ′_{k-1 k-1}を
算出し，これらと目標観測装置（１）より得られる式
（９）の観測ベクトルｚ _ｋに対し，平滑ベクトル
_ｋ（＋）算出において観測ベクトルｚ _ｋが寄与する度合
を決定するゲイン行列算出器（５）より得られるゲイン
行列K_kを式（33）に従い適用し平滑ベクトル _ｋ（＋）
を算出する。運動モデルごとの予測値算出器（７）で
は，平滑器（６）より得られる平滑ベクトル _ｋ（＋）
より等速直線運動予測によりΦ_{ｋ ｋ}（＋）を算出し，
あらかじめ設定された複数個の運動モデルを構成する定
数加速度ベクトルα _ａが予測に影響する項Γ′_ｋ α _ａを
算出し，現時点より１サンプリング後のサンプリング時
刻t_k+1に対する複数個の運動モデルごとの予測ベクトル
▲ ^a _k+1▼（−）を式（12）に従い算出する。運動モデ
ルごとの平滑誤差評価器（９）では,1サンプリング前に
算出しておいた式（21）の運動モデルごとの予測誤差共
分散行列P_k ^a（−）を運動モデルごとの予測誤差評価器
（10）より第２の遅延回路（11）を通して入力し，ゲイ
ン行列算出器（５）より得られるゲイン行列K_kとによ
り，式（19）の運動モデルごとの平滑ベクトル
_k ^a（＋）の平滑誤差の評価値である複数個の運動モデル
ごとの平滑誤差共分散行列P_k ^a（＋）を式（16）に従い
算出する。なお、式（13）、式（15）、式（16）より運
動モデルごとの平滑誤差共分散列▲Ｐ^a _k▼（＋）は運動
モデルによらない値となる。

運動モデルごとの予測誤差評価器（10）では，運動モデ
ルごとの平滑誤差評価器（９）より得られる式（22）の
運動モデルごとの平滑誤差共分散行列P_k ^a（＋）を式（5
3）に従い式（20）の平滑誤差共分散行列P_k（＋）とみ
なし，あらかじめ設定された運動モデルより得られる式
（８）の駆動雑音共分散行列Q_kとにより式（13）に従い
現時点より１サンプリング後のサンプリング時刻t_k+1に
対する複数個の運動モデルごとの予測ベクトル
_k+1 ^a（−）の予測誤差の評価である運動モデルごとの予
測誤差共分散行列P_k+1 ^a（−）を算出する。

予測誤差評価器（13）では，各運動モデルの信頼度算出
器（３）より得られるβ_k,b,あらかじめ設定された式
（24）の推移確率P_ab及びあらかじめ設定された複数個
の運動モデルを構成している式（５）の定数加速度ベク
トルα _ａより式（52）を使用し現時点より１サンプリン
グ後の予測ベクトル _k+1（−）を大きくする項 を算出し，これに運動モデルごとの予測誤差評価器（1
0）より得られる運動モデルごとの平滑誤差共分散行列P
_k+1 ^a（−）を加算し予測誤差の評価である予測誤差共分
散行列P_k+1（₋）を式（51）に従い算出する。なお、目
標観測装置（１）では平滑器（６）より入力される目標
位置及び目標速度の平滑値をもとに目標が等速直線運動
を行うとして現時点より１サンプリング後の直交座標の
目標位置の予測値Ｚ _K+1（−）＝H_K+1Φ_{Ｋ Ｋ}（＋）を
算出し、予測誤差評価器（13）より入力される予測誤差
共分散行列P_K+1（−）及びあらかじめ設定された観測系
モデルより（10）の観測雑音共分散行列R_K+1より正値対
称行列S_K+1＝H_K+1P_K+1（−）▲Ｈ^T _K+1▼＋R_K+1を算出
し、Ｚ _K+1を中心にした目標の予測存在範囲を算出し、
この範囲内の目標位置情報のみを追尾目標から得られた
もとして扱う。すなわち、観測ベクトルＺ _K+1が（Ｚ _K+1
−Ｚ _K+1（−））^TS_K ^-1（Ｚ _K+1−Ｚ _K+1（−））≦ｄ を満たす時のみ、観測ベクトルＺ _K+1を追尾目標から得
られたものとして扱う。ここでｄはあらかじめ設定され
た正のパラメータである。

〔発明の効果〕

以上のようにこの発明によれば通常の目標自動追尾装置
に特別の付加装置を付けることなく安価に目標運動諸元
算出精度を向上させることができる。

なお以上は等速直線運動モデルに定数加速度ベクトルが
付加されたモデルの場合について説明したが，これ以外
の複数個の運動モデルを有して目標観測装置の情報より
目標運動諸元を算出する追尾フイルタに適用できる。

【図面の簡単な説明】

第１図はこの発明の一実施例の構成を説明する図，第２
図は従来の追尾フイルタを説明する図，第３図は定数加
速度ベクトルを説明する図，第４図は目標予測存在範囲
を説明する図，第５図は観測誤差評価値を説明する図で
ある。 図において，（１）は目標観測装置，（２）は運動モデ
ルごとの観測誤差評価器，（３）は各運動モデルの信頼
度算出器，（４）は第１の遅延回路，（５）はゲイン行
列算出器，（６）は平滑器，（７）は運動モデルごとの
予測値算出器，（８）は第３の遅延回路，（９）は運動
モデルごとの平滑誤差評価器，（10）は運動モデルごと
の予測誤差評価器，（11）は第２の遅延回路，（12）は
第４の遅延回路，（13）は平滑誤差評価器である。 なお，各図中同一あるいは相当部分には同一符号を付し
て示してある。

Claims (1)

【特許請求の範囲】
1. 【請求項１】目標位置情報を観測する目標観測装置と、
   同一次元の複数個の定数ベクトルより構成される異なる
   複数個の運動モデルおのおのと上記目標観測装置より入
   力される目標位置情報との合致度である観測誤差評価値
   を算出する運動モデルごとの観測誤差評価器と、上記運
   動モデルごとの観測誤差評価器より上記各運動モデルお
   のおのに対する観測誤差評価値を入力し、上記各運動モ
   デルおのおのの信頼度を算出する各運動モデルの信頼度
   算出器と、上記各運動モデルで共通のゲイン行列を算出
   するゲイン行列算出器と、上記目標観測装置より入力さ
   れる最新の目標位置情報、上記各運動モデルの信頼度算
   出器で算出した各運動モデルおのおのの信頼度及び上記
   ゲイン行列算出器で算出したゲイン行列とを用いて上記
   各運動モデルの影響を統合して得られる目標位置、速
   度、加速度などの目標運動諸元の平滑値を算出する平滑
   器と、上記平滑器で算出した目標運動諸元の平滑値を入
   力し、上記各運動モデルおのおのに対する目標運動諸元
   の１サンプリング後の予測値を算出し、その予測値を上
   記運動モデルごとの観測誤差評価器へ出力する運動モデ
   ルごとの予測値算出器と、上記ゲイン行列算出器よりゲ
   イン行列を入力し、上記各運動モデルおのおのに対する
   目標運動諸元の平滑値の誤差を評価する運動モデルごと
   の平滑誤差評価器と、上記運動モデルごとの平滑誤差評
   価器の評価結果を用いて上記各運動モデルおのおのに対
   する目標運動諸元の予測値の誤差を評価し、その予測誤
   差の評価値を上記ゲイン行列算出器、上記運動モデルご
   との平滑誤差評価器及び上記運動モデルごとの観測誤差
   評価器へ出力する運動モデルごとの予測誤差評価器と、
   上記各運動モデルの信頼度算出器で算出した各運動モデ
   ルの信頼度及び上記運動モデルごとの予測誤差評価器で
   算出した各運動モデルおのおのに対する予測誤差の評価
   値とを入力し、上記各運動モデルの影響を統合して得ら
   れる予測値の誤差を評価する予測誤差評価器とを備えた
   ことを特徴とする追尾フィルタ。