JPH0217485A - 追尾フイルタ - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は観測雑音を富んだ目標位置情報及び目標距離
変化率を目標観測装置より人力し、０徐位置の真値及び
速度などの目標運動諸元を推定する追尾フィルタの精匿
回上方法について提案するものである。

〔従来の技術〕

第４図は例えばＩＥＢＥ　ＴＲＡＮ８ＡＣ’ｒＩＯＮＳ
　０ＮＡＥ几０８ＰｈＣＥ　ＡＮＤ　ＥＩＪＯＴ）１．
０へＩＣＳＹＳＴＦＪＭ８ＶＯＬ　ＡＥＳ　−１３階３
．　ＭＡＹＬ９７７、　Ｐ３１０〜Ｐ　３１７１−Ｆｖ
ｌＡＮＢＵＹＥ几ＩＮＧ　Ｔ）ｔ）ＬＧＥＴ　ＴＨＡＯ
ＫＩへＧＵ８　ＩＮ（Ｊ　　ＡＩＪＡＰＴ　ｔｖｇ　　
５ＴＡＴＥ　　Ｅ８　Ｌ　ＩＭＡＩＯＮ　Ｊ　　で示さ
れた複数個の運動モデルを有する場合の従来の追尾フィ
ルタの構成図であり０図において＋１ｌＶｉ観測雑音を
含んだ目標位置及び目標距離変化率を計測する目標観測
装置、（２）は複数個の運動モデルおのおのに対して目
標位置観測誤差を評価するための運動モデルごとの位置
観測誤差評価器、（３）は複数個の運動モデルおのおの
の信頼度を算出するための各運動モデルの信頼度算出器
、（４）は第一の遅延回路、（５）は目標運動諸元の平
滑値算出に使用するカルマンゲイン行列算出のためのゲ
イン行列算出器、（６）は目標運動諸元の平滑値を算出
する平滑器、（７）は複数個の運動モデルおのおのにつ
いて現時点より１サンブルング後の目標運動諸元の予測
値を算出する運動モデルごとの予測値算出器、（８）は
第３の遅延回路、（９）は複数個の運動モデルおのおの
の平滑値と真値の差である平滑誤差を評価する運動モデ
ルごとの平滑誤差評価器、　Ｑｌは複数個の運動モデル
おのおのの予測値と真値の差である予測誤差を評価する
運動モデルごとの予測誤差評価器、ａυは第２の遅延回
路、α２は第４の遅延回路である。

従来の複数個の運動モデルを有する追尾フィルタは上記
のように構成され、たとえば固定直交座標を使用し１等
速直線運動モデルにサンプリング時刻によらない定数加
速度ベクトルが付加されたモデルを複数の異なった加速
度レベルごとに有する場合が複数個の運動モデルとして
使用されていた。目標観測装置（！）では、極座標によ
る観測雑音を含んだ目標位置情報を直交座標に変換する
。運動モデルごとの位置観測誤差評価器（２）では、運
動モデルごとの予測値算出器（７）より得られる現時点
より１サンプリング前に算出した目標位置予測ベクトル
を目標位置の平均ベクトルとし。

運動モデルごとの予測誤差評価器Ｑｌより得られる現時
点よｐｌサンプリング前に算出した予測誤差共分散行列
及びあらかじめ設定しである観測モデルより得られる観
測雑音の共分散行列より目標位置観測ベクトルと目標位
置予測ベクトルの差の共分散行列を求め、目標位置観測
ベクトルが３次元正規分布に従うとして複数個の運動モ
デルごとに、目標観測装置より入力される目標観測位置
の目標予測位置に対する誤差を評価する。各運動モデル
の信頼度算出器（３）では、現時点よりｌサンプリング
前に算出した複数個の運動モデルおのおのの信頼度、運
動モデルの推移にマルコフ性を仮定し、あらかじめ設定
された運動モデルの推移確率及び運動モデルごとの位置
観測誤差評価器（２）より人力される複数個の運動モデ
ルごとの目標位置観測誤差評価値より、複数個の運動モ
デルおのおのの信頼度を算出する。ゲイン行列算出器（
５）では、あらかじめ設定しである観測モデルより得ら
れる観測雑音共分散行列及び現時点より１サンプリング
前に運動モデルごとの予測誤差評価器Ｑｌで算出してお
い友運動モデルごとの予測誤差共分散行列ヨリカルマン
フィルタの理論よりケイン行列を算出する。平滑器（６
）では、現時点より１サンプリング前に算出しておいた
平滑値、目標観測装置１ｌｌ（１１より入力される目標
観測位置。

ゲイン行列算出器（５）より入力されるゲイン行列及び
各運動モデルの信頼度算出器（３）で求めた複数個の運
動モデルおのおのの信頼度によシ重みづけした複数個の
運動モデルを構成している定数加速度ベクトルの総和に
よシ目標位置及び目標速度の平滑値を算出する。運動モ
デルごとの予測値算出器（７）では、平滑器（６）で算
出した目標位置及び目標速度と複数個の運動モデルごと
の定数加速度ベクトルにより現時点より１サンプリング
後の目標位置及び目標速度の予測値を算出する。運動モ
デルごとの平滑誤差評価器（９）では、目標位置及び目
標速度の平滑値と真値の差の共分散行列をゲイン行列算
出器（５）で算出したゲイン行列及び現時点より１サン
プリング前に運動モデルごとの予測誤差評価器Ｑｌで算
出しておい友予測誤差共分散行列により算出する。運動
モデルごとの予測誤差評価器ａ〔では、運動モデルごと
の平滑誤差評価器（９）よυ入力される平滑誤差共分散
行列とあらかじめ設定しである運動モデルより得られる
駆動雑音共分散行列より現時点よりｌサンプリング後の
予測値と真値との差の共分散行列を算出する。

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

上記のような従来の追尾フィルタでは、パルスドプラレ
ーダのように目標観測装Ｒ（１１で目標距離変化率が観
測される場合においても目標位置情報のみで追尾フィル
タが構成されていたため、目標旋回時に観測される目標
距離変化率の急激な変化が各運動モデルの信頼度算出器
（３）で求まる複数個の運動モデルおのおのの信頼度に
反映されず、また、平滑器（６）で算出する目標位置及
び目標速度の平滑値が各運動モデルの信頼度算出器（３
）で求めた複数個の運動モデルおのおのに対する信頼２
１−反映した値にもかかわらず、目標位置及び目標速度
の予測値は現時点よｊ５１−９−ンプリング間目標が完
全な等速直線運動を行うとして算出していた。このため
、旋回目標に対する追尾精度は劣化せざるをえなかった
。

この発明はこのような課題を解決するためになされたも
ので１旋回目標に対しても精度よく１０尾できる追尾フ
ィルタを得ることを目的とする。

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

この発明の追尾フィルタは、複数個の運動モデルおのお
のの信頼度探出に目標距離変化率を使用し、目標運動諸
元の予測値算出に複数個の運動モデルおのおのの信頼度
を使用するようにしたものでおる。

〔作　用〕

この発明においては、目標距離変化率の観測値と複数個
の運動モデルおのおのの距離変化率の予測値との差の確
率密度を複数個の運動モデルおのおのの予測値、ａ数個
の運動モデルおのおのの予測値と貞値の差である予測誤
差の評価である複数個の運動子デルごとの予測誤差共分
散行列及び目標距離変化率観測雑音より算出し、複数個
の運動モデルおのおのの信頼度算出の重みづけ係数とし
て使用し、旋回目標に対する複数個の運動モデルの合致
度である複数個の運動モデルおのおのに対する信頼度を
適正に算出するとともに、このようにして算出した複数
個の運動モデルおのおのに対する信頼度及び複数個の運
動モデルを構成している定数加速度ベクトルを使用し、
予測における加速度の影響項を算出し、この項に現時点
の平滑値をもとに等速直線運動予測により算出したｌサ
ンプリング後の予測値を加算することにより予測値を算
出するよう処している。

〔実施例〕

第１図はこの発明の一実施例を示す構成図であり、（１
）〜α２は上記従来装置と全く同一のものでおる。（１
３は予測値算出器、　ａ４１は運動モデルごとの距離変
化率銭測誤差評価器である複数個をＮ個とした場合の運
動モデルをｘｋ＝Φ−１、ｊｋ−１＋ρに−１ｗ、−、
＋ρに−１ｕｋ−１・・・・・・・・・・・・　（１１ とする。

ここで。

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

・Φｈ−ＩＩｄサンプリング時刻ｂ−ｔよりｔｋへの状
態ベクトルｘ１の推移行列で目標が等速直線運動を行う
と仮定した場合 ・ｕｋはサンプリング時刻１ｋにおいてＮ個の運動モデ
ルを構成する定数加速度ベクトルでｕ、　＝Ｑｌ又Ｃま
ら＝ａ２又は・・・・・・又は隻、＝５　　　　　　・
・・・・・・・・・・・　（２）であり ρ′にはサンプリング時刻ｔ、における定数加速度ベク
トルの変換行列で である。

・ちはサンプリング時刻１ｋにおける駆動雑音ベクトル 八はサンプリング時刻１ｋにおける駆動雑音ベクトルの
変換行列で たとえば、目標の運動モデルを等速直線運動と仮定した
ことによる打切り誤差項をＩ’に−ＩＷｋ−１とみれば
ＷｋＦｉ加速度ベクトル相当でろりである。

である。

第３図は水平面に平行な面内で定数加速度ベクトルを説
明する図でロシ９図において。

Ｏは目標観測装置ｔを原点とした座標０−ｘｙの原点、
又は東方向をＸ＠を正とした座標ＯｘｙのＸ軸、　Ｙは
北方向をｙ軸の正とした座標０−ｘｙのｙ軸、ＡＩはｙ
軸の正方向の定数加速度ベクトル、Ａ２／ｒｉ、ｘ軸の
正の方向の定数加速度ベクトル、Ａ３はｙ軸の負の方向
の定数加速度ベクトル、　　ＡＪＵＸ軸の負の方向の定
数加速度ベク る定数加速度ペク 重力加速度とする の定数加速度ベク 場合 Ｎ　＝　５ であり９式（２）は トルである。第３図におけ トルの大きさをｌｏｇ＜ｇは ）とし、この他に加速度Ｏ トルを考えた運動モデルの ・・・・・・・・・・・・・・・　（３）００３次元正
規分布白色雑音で Ｅ［Ｗ、］　＝０　　　　　　　・・・・・・・・・・
・・・・・（７）”ＣＱｋＷＬ：）　＝Ｑ＞　　（ｋ＝
ｚのとき）。

０Ｉ（ｋ←ｔのとき）・・・・・・・・・　（８）とす
る。

ここで。

・０は零ベクトル ａ　Ｑ、はサンプリング時刻ｔ、における駆動雑音共分
散行列 であり。

とサンプリング時亥目、に無関係に書ける。

次に、サンプリング時側型、において ｕ、−１＝α”　　”＝１＋　　２＋　・・”’−＋　
Ｎ）　−・”　＜５）が真であるとの仮説を 中に、　ａ　　　　（ａ＝１．２．−−、　Ｎ　）　−
（６１と書く。

Ｅを平均をめられす記号として、　Ｗ、は平均目標位置
観測モデルを 久＝　）ｌｋｂ＋５　　　　　・・・・・・・・・・・
・・・・（９）とする。

ここで。

・Ｚｋはサンプリング時刻ｔ、における目標位置情報の
観測値よシ構成される直交座標による目標位置観測ベク
トル ・Ｈ５はサンプリング時刻ｔ、における観測行列で Ｈｋ＝（Ｉ　　ｏＩ） である。

一ヱ、はサンプリング時刻１．における目標位置観測雑
音ベクトルであり、平均Ｏの３次元正規分布白色雑音で ＥＣｖｋ〕＝旦 Ｊｖｋｖｔ）＝ｌＬ、（ｋ＝ｔの時）、０ｆ（ｋ＝Ｑｔ
の寺）・・・・・・・・・・・・・・・　Ｑｌである。

なお、　　Ｒｋｕサンプリング時刻１ｋにおける目標位
置観測雑音共分散行列で、運動モデルによらない値とす
る。

目標距離変化率観測モデルを 曳（ｋ）　＝　Ｒ（ｋｌ　＋ｖ、’　（ｋ）　　　　　
　・・・・・・・・・αυとする。

ここで。

拳几。（ｋ）はサンプリング時刻１ｋにおける目標距離
変化率の観測値 ０几（ｋ）はサンプリング時刻ｔｋにおける目標距離変
化率の真値 ・Ｖ・（ｋ）はサンプリング時刻１．における目標孔 距離変化率観測雑音であり、平均Ｏの１次元正規分布白
色雑音で Ｂ　［ｖ−（ｋｒ〕＝　Ｏ・・・・・・・・川・・・・
　α２ＥＣｖ−６ｄｖ−Ｃａ〕＝”ｒ警ｋＪｃｋ＝１の
時）。

凰　　　　凰　　　　　　　　　凡 ０（ｋ〜Ｌの時）・・・・・・　（１３である。

ム２．τ？・ωはサンプリング時刻ｔ、におけ凰 る目標距離変化率の分散で、運動モデルによらない値と
する。

また、目標距離をＲ（ｋｌ すなわち Ｒ”（ｋ）＝＝　ｘ、”　＋　ｙ、　＋　ｚ、”　　　
・・・曲曲・・・・Ｉとずれば０式α尋の両辺を微分し
て でめる。

サンプリング時刻ｔ！からｔ、までの目標位置観測Ｊベ
クトルの全体ｉ２−目標距原変化率の観測値の全体をＺ
ｋ、すなわち ｚｋ＝Ｃ４１１各２．・・・・・・、４ｋ〕　　・・・
・・・・・・αＧｚ　＝　［ａｏｔｏ、　椀（２ｒ、・
・・・・・、４＋ｈ＋１−・・・・−ａｎと書く。

以上から通常のカルマンフィルタの理論より ↑ａ（−１＝Φ　？　（→十へ−１仝８　・・・・・・
Ｑｌｌ−一ト１−に−１ ）”’ｌ−１＝Φに−Ｉ　Ｐｋ−１’（１）Φに−１”
　Ｆｋ−１’Ｊｋ−ｓ　Ｆす。

・・・・・・・・・・・・・・・　ｑ９！ｋ（ホ）＝王
、←）十に、　（りｙ　　Ｈ；、　’Ｅ：’−’　）　
　・・・・・・　■札、二Ｐテ←）Ｈτ（ＨｋＰ”　（
−１Ｈτ十町）−１・・・・・・　Ｑυｐａｌ−１１＝
　、ｐ”ｔ−＋　−Ｋ　ＨＰａＨｋｋｋｋ ・・・・・・・・・・・・・・・　（２）（ａ＝１，２
．　　・・・・・・、Ｎ）でめる。

ここで ・’ｉ、”ｆ−１［仮説Ｇ＋ａ　のもとてのサンプリン
グ時刻ｔ、に対するムの予測ペクトツノで１条件つき平
均ベクトルで書Ｖアはカルマンフィルタの理論より ’；’：ｆ−１＝＝Ｅ［−ｘ、　１４−、．３ｚ’−’
　ｚ’−’］　・・・（ｌｉｌ　　　。

でおる。

・ｈ←）はサンプリング時刻ｔ、に対する玉、の平滑ベ
クトルで ｘ、ｔｅｌ　＝　　Ｅ［ｘ、　　ｌ　　Ｚ’、　　　Ｚ
Ｄ　　）　　　　　　−−＝　　　Ｑ４Ｊでめる。

ｅ　ｘ　ｋｔＨｕ仮説＋に＋ａ　のもとてのサンプリン
グ時刻ｔｋに対するＸ、の平滑ベクトルでｚｋ　　″ ＥＬｈ”＝”［王ｈｌ’ｈ　ｇ＋　　ｌ　　ｚＤｌ’Ｈ
ＨＨ＋　　Ｇ５である。

＠ｐｈ…はサンプリング時開直、における平滑誤差共分
散行列でるり、カルマンフィルタの理論より Ｐ、ｔｅｌ　＝　Ｍ　［（ｘ、−”；ｊ　ｔｅｌ　）　
（ｘ　　−ｘ　←すＴ＋　ｚｋ、　　呪］−ｋ　　　　
　　　−ｋ　　−ｋ （至） である。

・Ｐ；←）はサンプリング時刻ｔ、における仮説へ、ａ
　のもとての予測誤差共分散行列でＰ”、ｉ−１＝　Ｊ
（ｘ　−ｒ＋−＋）（ｘ−、ａ、−、）’ｒ、牛。

−ｋ　　　−ｋ　　　　　　　−一転　　−ｋＺ゛人Ｚ
Ｄ’、１ ・中・・・・・・・・・Ｑ６ｔ である。

・Ｐ、ｌ＋ｊｉｆｆサンプリング時刻【、における仮説
中、８　のもとての平滑誤差共分散行列でＰａｌ→＝　
Ｊ（ｘ　　−’；Ｘｆ＋＋）（ｘ、−ｘ：ｉ＋す７ト↓
−２，ａ。

−ｋ　　　−ｋ Ｚｋ、　Ｚ＋Ｄ］　　　　　　　・・・・・・・・・・
・・・・・　（支）でめる。

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

なお、Ａは行列人の転置行列をあられす。

また、カルマンフィルタを通常適用する場合と同様にし
て、初期値邑。ｌ＋１．Ｐｏ１＋１ハ別途定まっている
とする。

式（９）よりＰＨ（−１は仮説４−ｋａによらない値な
ので６式Ｑυ工υに５０式＠よりべ（ト）も同様に仮説
＋ｋａ　　によらない値とする。

サンプリング時刻ｔ、までの観測情報による仮説中０，
３の信頼度βに、ａを条件つき確率密凝関銭により ρ　−ｐ（＋、、ａ　ｌ　Ｚ“、Ｚい　・・−・・・・
・・（Ｊｋ、ａ Ｌａ＝１．礼・・・・・・、へ− と亨く。

運動モデルの推移にマルコフ性を仮定する。すなわち運
動モデル輸、３はザ、ブリ、り時刻ＩＫ−２までの運動
モデルには依存しないとする。この時、運動モーＹルの
推移確率ＰａｂをＰａｂ＝Ｐ［４−Ｋａ　　牛に−１、
ｂ　〕−Ｃ３ｆ）（ａ＝１．２．・・・・・・ＩＮ＋り
＝１．２．・・・・・・、へ）と指゛く。

仮ＡＱ　４−ｘ、　ａのもとての目標位Ｗ観６１１１誤
遇−の評価 Ｊａ（６）（ａ＝１．２．・・・・・・、Ｎ）　　・・
・・・・　０υを３次元正規分邪で−ｈφ、すｆば７Ｊ
ルマンフイルタの理論よυ ｊａＮ　二９（Ｚ　ＨＨ”　［−１、３４Ｐ”ｆ−１）
Ｊ”十Ｒ）−ＫＫ−ＫＫＮＫＫ Ｃつ である。

ここで。

、９（Ｚ、ａ、Ａ）は平均ベクトルａ　、共分−Ｋ　　
　　　Ｋ　　　　　Ｋ　　　　　　　　　　　　　　　
　　　　　　　　Ｋ散行列人の３次元正規分布のＺにお
ける確Ｋ　　　　　　　　　　　　　　　　　　　　　
　　　　　Ｋ重密度である。

式■よりわかるように、目標位置の観測ベクトル２が目
標位置の予測ベクトルＨｘ　ｆ−ＩＫ　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　Ｋ−Ｋ
に近いほどｆａ’ｙ’ｒの値は大きくなり目標が仮説４
−Ｋａのもとで運動したとの信頼度が高く評価される。

第４図は目標位置観測誤差の評価値ｆａ（転）を１次元
において説明する図でめり。

図において、Ｇは目標位置をめられす横軸。

Ｆは確率密度をめられす縦軸、Ｚは観測ベクトル矛、の
示す目標位置、ＰＡは仮説Φに、ａのもとての目標予測
位置、ＦＡは確率密度ｆ、（６）Ｎ　１４平均Ｈ”　（
−）　分散Ｈｐａ（−１Ｈ’十ＲＫ　Ｌ　ッに−Ｋ　　
　　　　　　　に　に　　　　Ｋ　　　　Ｋて定まる正
規分布曲線である。

仮説中に３のもとてのサンプリング時側輸に対する目標
距離変化率の予測値を九二（６）。

すなわち １４ｏｏ＝＝ｎ（メｍｌ　４−ｃ、　ａ＋　　”−一　
”Ａ−”　　”・・”　　　”と書けば６式ａ５よシ ・・・・・・・・・・・・　（ロ） である。

ここで でろる。

なお１式ａ４１よシ でるる。

また、全微分の性質より一次近似を行えば一４←：　ｈ
Ｋ（”；：　ｆ−１）　（五−に（−））・・・罰であ
る。

ここで。

ｈＫ（ｘ　Ｌ−１）は −に δＸ　　δｘＫ　”ＫδｚＫａｘＫδｙ３δｚ３（至） である。

式Ｑｌｌ及び式ｃ３ηより 煽凶−４（転）弁ｈＫ（ｘ二←ｌ　）　（Ｘｘ　　？コ
Ｈ）　＋ｖ　ｉ、％（至） である。

式（至）０式＠１式α２及びＶ−が白色雑音の仮定より Ｅ（（ＲｐＨＲ論）　Ｉ　４”Ｋ＋ａ＋　ｚＫ−、ｚ、
−１〕＋・＋　”Ｑだから１式（至）より Ｅ　（（Ｒｏｔ’ｌ　ＩΦに、ａ、　Ｚ””？　Ｚ’０
−’ｌ　＝＝ｇ＋ＩＱ−・−（４１）である。

式（３９）、式（４１）、式（２７）、式（１３）及び
ｖ、Ｌ９Ｑが白色雑音の仮定より ［（Ａｏｅｏ　−Ｅ［）ＱＩＫＩＩ　Ｊｒｉ、ｈ　Ｚ’
でｚＩ＆１〕、２．　＋に、、　、　ｔ−，１ＺＫ−１
＝ｈに（Ｋｔ−１）理（−）ｈンヌンー））＋２・（転
）　・・・・・・（４２）である。

式（４１）及び式（４２）より仮説＋に＋ａのもとての
目標距離変化率観測誤差の評価 ｆ２Ｅ　　（ａ＝１．２．・・・、Ｎ）　　　・・・・
・・・・・（匂を１次元正規分布で近似すれば 九（ト）＝ｈ（Ｈｃ＋閃；糾ト屹りえ（号（−））ぺ（
−）号（ｇ、−、、十泗）几 ・・・・・・・・・（４４） である。

ここで＊　＆ｚ（ｔ；ｍ、ｎ）は平均ｍ１分散ｎの１次
元正規分布の！、における薙率蓄此でめる。

サンプリング時刻ｔＫの観測情報ＺＫ及びｎｏ（ト）が
得られない時点での仮説４−に＋ａの信頼度ＰＣ’Ｋ　
１ａＩ　ＺＫｌ　”ｏ−’−’はマ／Ｌ／：Ｉ７性ヨリ
。

仮説＋ｉｃ４が１サンプリング前の各仮説＋に−１，ｂ
ｂ（ｂ＝１．　２．・・・、Ｎ）より推移して得られる
ので＋、４−に−１＋ｂ　　の信頼度である式（支）の
／’ｘ−１−ｂ及び推移確率である式（１）のＰａｂよ
りＰ　［４−ｓｃ、１１　Ｚ’−、’　Ｚｊ−’）　＝
１’　／”ＩＧ−１、ｂ　Ｐａｂ　−（４５）として求
まる。

目標位ｍ観測ベクトルＺＫ及び目標距離変化率の観測値
几。輪が得られた時点での仮説軸、ａの信頼度／Ｋ　、
ａ　Ｆｉ、観測ベクトルムが得られない時点の仮説の信
頼度Ｐ〔輸＋ａ　’　”Ｋ−＋　ｚ：’　］に仮説輸、
ａの目標位置観測ベクトルζによる評価値ｙ−及び目標
距離変化率の観測値視（ト）による評価値Ｋｆ：”Ｊ’
ｉ乗算した値、すムわち式（４５）、式（３１）及び式
（４３）より、ソ、β、−１１，Ｐａｂｆ−ｆ：ｖ・・
・・・・・・・・・・（４６）確率の性質よシ Σ入、ａ＝１ ａ＝１ ・・・・・・・・・・・・・・・（４７）でめる。

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

式３０１式［有］及び式（４９）より Ｂ、（＋１＝＝ｈＬ　ＸＫＩ　Ｚ’、　ｚ’ｖ　：］　
＝ｆ・・ｆｘＫＰ（ｘ　、ｌ　Ｚ’、Ｚ−）ｄｘ　ｘ＝
Σβに＋ａ　ｆ＝Ｊｘ　ＰＣｘ　１４に、、Ｚ　、　Ｚ
Ｄ　〕〕ｄｘａ−１−鳥ζ−に−４ だから に比列すると考えてよい。

である。

式（５０）及び式■１式１１８１式（４７）より”’ｉ
、Ｈ＝の、−１，に−、Ｈ＋ｌ’、’−１Ｅｘ−１　＋
ＫＫ　［ＺＫ−〇にである。

ここで ベイズの定理を使用し ・・・・・・・・・・・・・・・　（５３）である。

ｘ−１−１ｋサンプリング時刻１ｋに対するＸの−Ｋ　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　−に予測ベクトル、すなわち ｘＫＨ＝　Ｅ　［ｘ、　ｌ　ＺＫ−、’　ｚ、−１］　
　−”−”・−・（５４）とすれば１式（５３）及び式
（２３）より１Ｋｊ−１＝　ｆ　−ｆ　ｘ、）’［ｘＫ
ｌ　ＺＫ−、’　Ｚｐ−’　！　ｄｘＫ＝Σｐ［’ｉｘ
、ａ ａ＝１ ｚＫ−、Ｉ　ＺＫ−１〕ｑａ、。

−に だ力・ら Δ　　　　　Ｎ ｘｔ−１＝ΣＰローに＋ａ −に　ａ＝１ でるる。

ＺＫ−ｊｚ低−”］”ｔ−＋ Ｋ ・・・・・・（５５） 確率の性質より ＪＰ〔φに＋ａ　、　ＺＫ−１〕＝　、　　　□・・・
・・・・・・・・・・・・（５６）ａ＝ｌ だから１式（５５）　Ｋ式（１８）を代入してｘ　ｔ−
１＝Φ　　ｘ　　　Ｌｎ −Ｋ　　　　　　　Ｋ−１−に−１ ＋／”　（Ｊ　Ｐｒ４−ｘ、ａＩＺＩｃ−、’　Ｚ’、
−”］ａ　）Ｋ−１ａ＝ｌ　　　　　　　　　　　　　
　　−ｎを得る。

式（４５）及び式（５７）より ｘ　ｉ−１＝Φ　ｘ（４Ｍ −Ｋ　　　　　　　Ｋ−１−に−１ 十）；−１ζそ”１仏’に−１１ｂ　Ｐａｂ　’ａ　）
である。

なお、先ｌこ述べたように仮説φに＋ａのもとての平滑
誤差共分散行列１．’％ｌは仮説輸、ａに無関係に定ま
る値なので ｐ、ｔ＋１＝＝　Ｐｐｓ　　　　　　　　　　・・・・
・・・・・・・・・・・（５９）とする。

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

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

目標＃　＃ｌ装置（ｔｌでは目標距離変化率及び極座標
で目標位置観測誤差ｆｌｌｌｌｔ、、　　目標位置情報
金直交座標に変換し１式ｒｔｕの目標距離変化率の観８
Ｉｌｌ値視（６）及び式（９）の目標位置親御１ベクト
ルＺ　を出力する。運動モデルごとの位置観−に 側誤差評価器（２）では、運動モデルごとの予測値算出
器（７）より得られる現時点より１サンプリング前に算
出した式（１１のＪｉ１！動モデルごとの予測ベクトル
ｘ１−）を第４の遅延回路ａ２を通し−Ｈに −Ｃ人力し目標位置予測ベクトルＨ”’Ｈを算出に−に し、運動モデルごとの予測誤差評価器Ｉｌｌより得られ
る現時点より１サンプリング前に算出した弐〇の運動モ
デルごとの予測誤差共分散行列Ｐ、７＋−１を第２の遅
延回路Ｑｌｌ全通し入力し運動モデルごとの予測位置誤
差の共分散行列ＨＫＰ：１−ＩＨ″Ｋ　を算出しこれと
めらかじめ設定してめる観測モデルより得られる式Ｑｌ
の目標位置観測雑音共分散行列へより観測ベクトルＺＫ
と運動モデルごとの目標位置予測ベクトルＨＫＫ＋−１
の差の共分散行列ＨＫｐ：ｔ−ｎｄ”ａ、を算出し目標
観測装置＋１１より入力される目標位置観測ベクトルタ
にの複数個の運動モデルおのおのに対する合致度である
目標位置観測誤差の評価値である式ＣＩＯのｆａｏＯを
式（至）に従い算出する。運動モデルごとの距離変化率
観測誤差評価器Ｉでは、運動モデルごとの予測値算出器
（７）より得られる現時点より１サンプリング前に算出
した式ａ８の運動モデルごとの予測ベクトル”；”ｌ−
１を第４の遅延回路Ｑ２を通し入力し式（ロ）Ｋ 及び式關に従い目標距離変化率の予測値ｊＸＱＱを求め
さらに式（至）１式伺及び式（至）に変換行列りに（↑
ａ１−１）　’Ｑ算出し、運動モデルごとの予測誤−に 差計１ｄｌ＋　ｉｓ　＋１（Ｉより得られる現時点より
１サンプリング前に算出した式ａ９の運動モデルごとの
予測誤差共分散行列Ｐａ１−１を第２の遅延回路αυに 全通し入力しこれとあらかじめ設定しである観測モデル
より得られる式αＪの目標距離変化率観測雑音２・＋Ｋ
ｌより目標距離変化率の観測値曳Ｎと予測値ｌ′Ｌ−の
差の分散である式（４２）のｈＫ（♀シー１）Ｐ二（−
）ｈン↑か一〇＋１（ト）を算出し、目標観測装置（１
）より入力される目標距離変化率の観測値Ｒ謬の複数制
の運動モデルおのおのに対する合致度である目標距離変
化率観測誤差の評価値である式（４３）のｆ畑を式（４
４）に従い算出する。各運動モデルの信頼度算出器（３
）では現時点より１サンプリング前に算出した複数個の
運動モデルおのおのに対する信頼度である式（至）の７
に−ｔ＋ａを第１の遅延回路（４）を通して入力しこれ
とあらかじめ設定された式■の推移確率Ｐａｂより目標
観測情報省え及びＲ８（イ）が得られない時点での各運
動モデルの信頼度でめる式（４５）の値全算出し９式（
４５）の値に目標位置観測ベクトルｚＫの各運動モデル
に対する合致度である運動モデルごとの位置観測誤差評
価器（２）より入力される式Ｃ３１１の九（転）及び目
標距離変化重視（イ）の各運動モデルに対する合致度で
ある運動モデルごとの距離変化率観測誤差評価器α４よ
り入力される式（４３）のｆ２ＫＪＫよジ重みづけし式
（４８）に従い目標位置観測ベクトル４Ｋ及び目標距離
変化率ル。（転）が得られた時点での複数個の運動モデ
ルおのおのの信頼度である式のの入、ａ　を算出する。

ゲイン行列算出器（５）では、めらかじめ設定された観
測モデルより得られる式α１の目標位置観測雑音共分散
行列〜及びｌサンプリング前に算出しておいた運動モデ
ルごとの予測誤差評価器Ｑｌより第２の遅延回路ａｌｌ
を通して入力される運動モデルごとの予測誤差共分散行
列Ｐン−１とにより式３υに従い平滑ベクトルへ（（１
）算出に使用するゲイン行列ＫＫ’を算出する。平滑器
（６）では、ｌサンプリング前に算出しておいた式＠の
平滑ベクトルｘ　　ｆＨを第３の遅延回路（８）を通し
入力し等遠方線運動予測によりΦ？　（ホ）に−１−Ｋ
−１ を算出し、各運動モデルの信頼度へ、ａ　とあらかじめ
設定された式（５）の複数個の運動モデルを構成してい
る定数加速度ベクトルらにより式（５２）に従い目標の
加速度Ｕ　を推定し−に−１ Ｕ　が予測に影響する項／’ｕ　　を算出し。

−に−Ｉ　　　　　　　　　　　　　　　　　　　　　
　　　　　Ｋ−１−に−１これらと目標観測装置（１）
より得られる式（９）の目標立１ｑ観測ベクトルＺＫに
対し、平滑ベクトル王−算出において観測ベクトル迄が
寄与する度合を決定するゲイン行列算出器（５）より得
られるゲイン行列ＫＫを式（５１）に従い適用し平滑ベ
クトルＸ…を算出する。運動モデルごと−に の予測値算出器（７）では、平滑Ｗ　ｆ６＋より得られ
る平滑ベクトルｘＫ（＋１より等遠方線運動予測により
Φに’ｉＫ’→を算出し、めらかじめ設定された複数個
の連動モデルを構成する逆数加速度ベクトルαが予測に
影響する項Ｐ′αを算出し。

ａＫ−ａ 現時点より１サンプリング後のサンプリング時刻−ヤ、
に対する複数個の運動モデルごとの予測ベクトルｘ（−
ｌ１式ｕ８に従い算出する。

−に＋１ 運動モデルごとの平滑課差計ｌｌ１ｌＩ器（９）では、
ｌサンプリング前に算出しておいた式面の運動モデルご
との予測誤差共分散行列ｐａ←）を運動に モデルごとの予浪（１誤差計価器１１１ｍより第２の遅
延回路αυ全全通て入力し、ゲイン行列算出器（５）よ
り得られるゲイン行列へとにより式□□□の運動モデル
ごとの平滑ベクトル宅；の平滑誤差の評価値である複数
個の運動モデルごとの平滑誤差共分散行列Ｐ二（→を式
＠に従い算出する。運動モデルごとの予測誤差評価器（
９）より得られる式（至）の運動モデルごとの平滑誤差
共分散行列ｐａ（ト）を式（５３）に従い式（至）の平
滑誤差に 共分散行列Ｐ−とみなし、あらかじめ設定された運動モ
デルよシ得られる式（８）の駆動雑音共分散行列（とに
より式０９に従い現時点より１サンプリング後のサンプ
リング時刻ｔＫヤ、に対する複数個の運動モデルごとの
予測ベクトルＸ　（−）の予測誤差の評価でるる予測誤
差−に十ｌ 共分散行列ｐ　　ｉ−１を算出する。

に＋１ 予測値算出器Ｑ３では、平滑器（６）より式（至）の平
滑ベクトル五−を入力し等速直巌運動予測によりΦに／
：ＫＬ＋１を算出し、各運動モデルの信頼度算出器（３
）工り得られる式（４８）の各運動モデルの１８幀度／
’に＋ａとあらかじめ設定された式（７）の推ｙ確率Ｐ
ａｂより目標位置観測ベクトルｚＫ＋１及び目標船離変
化率１帖（Ｋ＋１　）が得られない時点での式（４５）
の各運動モデルの信頼度ΣＡ、ｂｐａ１．　ｋ算出し、
これとあらかじめ設定ｂツｌ された複数個の運動モデルを構成している式（５）の定
数加速度ベクトル（ＩａよりＺＫ＋１　及ヒ椀（ＩＣ＋
１）が得られない時点での目標加速度を算出することに
より式（５８）に従い現時点より１サンプリング後の予
測ベクトル−４（−）を算出する。

〔発明の効果〕

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

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

【図面の簡単な説明】

第１図はこの発明の一実施例の構成を説明する図、第２
図は従来の追尾フィルタを説明する図、第３図１ユ定数
加速島二ベクトルを説明する図、第４図は目標位置鋭測
誤差訂価値を説明する図である。 図におい−Ｃ，（１１は目標観測装置、（２）は運動モ
デルごとの位置観測誤差評価器、（３）は各連動モデル
の信頼度算出器、（４）は第１の遅延回路、（５）はゲ
イン行列算出器、（６）は平滑器、（７）は連動モデル
ごとの予測値算出器、（８）は第３の遅延回路、　ｆ９
１１ｄ運拗モデルごとの平滑誤差評価器、ＯＩは運動モ
デルごとの予測誤差評価器、　ＱＤは第２の遅延回路、
　ｎａは第４の遅延回路、（１３は予測値算出器、ｌＮ
４１は運動モデルごとの距離変化率観測誤差評価器であ
る。 なお、谷図中同−あるいハイ−白部分分には同一符号を
付して示しである。 代−人　　大　岩　Ｑ　　雄 第３図

Claims (1)

【特許請求の範囲】
1. 目標観測装置より目標位置及び目標距離変化率を入力し
   、目標位置、速度などの平滑値を算出する平滑器と、現
   時点より１サンプリング後の目標位置、速度などの予測
   値を算出する予測値算出器とを有する追尾フィルタにお
   いて、同一次元の複数個の定数ベクトルより構成される
   複数個の運動モデルおのおのに対する目標位置観測誤差
   を評価する運動モデルごとの位置観測誤差評価器と、上
   記各運動モデルおのおのに対する目標距離変化率観測誤
   差を評価する運動モデルごとの距離変化率観測誤差評価
   器と、上記各運動モデルで同一のゲイン行列算出器と、
   上記運動モデルごとの位置観測誤差評価器で算出した各
   運動モデルおのおのに対する目標位置観測誤差及び上記
   運動モデルごとの距離変化率観測誤差評価器で算出した
   各運動モデルおのおのに対する目標距離変化率観測誤差
   を使用して上記各運動モデルおのおのの信頼度を算出す
   る各運動モデルの信頼度算出器と、上記各運動モデルの
   信頼度算出器で算出した各運動モデルの信頼度を使用し
   て目標運動諸元の予測量を算出する予測値算出器とる具
   備することを特徴とする追尾フィルタ。