JPH0769419B2 - Tracking filter - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は観測雑音を含んだ目標位置情報を目標観測装
置より入力し，目標位置の真値および速度・加速度など
の目標運動諸元を推定する追尾フイルタの精度向上の方
法について提案するものである。DETAILED DESCRIPTION OF THE INVENTION [Industrial application] The present invention inputs target position information including observation noise from a target observation device, and estimates a true value of a target position and target motion parameters such as velocity and acceleration. We propose a method for improving the accuracy of the tracking filter.

〔従来の技術〕[Conventional technology]

第２図は例えば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）は第４の
遅延回路である。Figure 2 shows, for example, IEEE TRANSACTIONS ON AEROSPACE AND E
LECTRONIC SYSTEMS VOL.AES-13 No.3 `` MANEUVERING TA
RGET TRAC-KING USING ADAPTIVE STATE ESTIMATION "
It is a block diagram of the conventional tracking filter in the case of having a plurality of motion models shown in, where (1) is a target observation device for measuring a target position including observation noise, and (2)
Is an observation error evaluator for each motion model for evaluating the observation error for each of the motion models, and (3) is the reliability of each motion model for calculating the reliability of each of the motion models. A calculator, (4) a first delay circuit, (5) a gain matrix calculator for calculating a Kalman gain matrix used for calculating a smoothed value of target motion data, (6)
Is a smoother that calculates the smoothed value of the target motion data, and (7) is a predictive value calculator for each motion model that calculates the predicted value of the target motion data after one sampling for each of the multiple motion models. , (8) is the third delay circuit, (9)
Is a smooth error evaluator for each motion model that evaluates the smooth error which is the difference between the smooth value and the true value of each of the motion models, and (10) is the difference between the predicted value and the true value of each of the motion models. A prediction error evaluator for each motion model that evaluates a certain prediction error, (11) is a second delay circuit, and (12) is a fourth delay circuit.

従来の複数個の運動モデルを有する追尾フイルタは上記
のように構成され，たとえば固定値交座標を使用し，等
速直線運動モデルにサンプリング時刻によらない定数加
速度ベクトルが付加されたモデルを複数の異なつた加速
度レベルごとに有する場合が複数個の運動モデルとして
使用されていた。目標観測装置（１）では，極座標によ
る観測雑音を含んだ目標位置情報を直交座標に変換す
る。運動モデルごとの観測誤差評価器（２）では，運動
モデルごとの予測値算出器（７）より得られる現時点よ
り１サンプリング前に算出した目標位置予測ベクトルを
目標位置の平均ベクトルとし，運動モデルごとの予測誤
差評価器（10）より得られる現時点より１サンプリング
前に算出した予測誤差共分散行列およびあらかじめ設定
してある観測モデルより得られる観測雑音の共分散行列
より目標位置観測ベクトルと目標位置予測ベクトルの差
の共分散行列を求め，目標位置観測ベクトルが３次元正
規分布に従うとして複数個の運動モデルごとに，目標観
測装置より入力される目標観測位置の目標予測位置に対
する誤差を評価する。各運動モデルの信頼度算出器
（３）では，現時点より１サンプリング前に算出した複
数個の運動モデルおのおのの信頼度，運動モデルの推移
にマルコフ性を仮定し，あらかじめ設定された運動モデ
ルの推移確率及び運動モデルごとの観測誤差評価器
（２）より入力される複数個の運動モデルごとの観測誤
差評価値より，複数個の運動モデルおのおのの信頼度を
算出する。ゲイン行列算出器（５）では，あらかじめ設
定してある観測モデルより得られる観測雑音共分散行列
及び現時点より１サンプリング前に運動モデルごとの予
測誤差評価器（10）で算出しておいた運動モデルごとの
予測誤差共分散行列よりカルマンフイルタの理論よりゲ
イン行列を算出する。平滑器（６）では，現時点より１
サンプリング前に算出しておいた平滑値，目標観測装置
（１）より入力される目標観測位置，ゲイン行列算出器
（５）より入力されるゲイン行列及び各運動モデルの信
頼度算出器（３）で求めた複数個の運動モデルおのおの
の信頼度により重みづけした複数個の運動モデルを構成
している定数加速度ベクトルの総和により目標位置及び
目標速度の平滑値を算出する。運動モデルごとの予測値
算出器（７）では，平滑器（６）で算出した目標位置及
び目標速度と複数個の運動モデルごとの定数加速度ベク
トルにより現時点より１サンプリング後の目標位置及び
目標速度の予測値を算出する。運動モデルごとの平滑誤
差評価器（９）では，目標位置及び目標速度の平滑値と
真値の差の共分散行列をゲイン行列算出器（５）で算出
したゲイン行列及び現時点より１サンプリング前に運動
モデルごとの予測誤差評価器（10）で算出しておいた予
測誤差共分散行列により算出する。運動モデルごとの予
測誤差評価器（10）では，運動モデルごとの平滑誤差評
価器（９）より入力される平滑誤差共分散行列とあらか
じめ設定してある運動モデルより得られる駆動雑音共分
散行列より現時点より１サンプリング後の予測値と真値
との差の共分散行列を算出する。目標観測装置（１）で
は平滑器（６）より入力される目標位置及び目標速度の
平滑値をもとに目標が等速度直線運動を行うとして算出
した現時点より１サンプリング後の直交座標による目標
位置の予測値を中心に、運動モデルごとの予測誤差評価
器（10）で算出した運動モデルごとの予測誤差共分散行
列が運動モデルによらず一定となる性質を利用して目標
が存在する予測範囲を算出し、この範囲内の目標位置情
報のみを追尾目標から得られたものとして扱う。A conventional tracking filter having a plurality of motion models is configured as described above, and, for example, a model in which a constant velocity vector is added to a constant velocity linear motion model and a constant acceleration vector is added to the constant velocity linear motion model is used. The case of having different acceleration levels has been used as a plurality of motion models. The target observing device (1) converts target position information including observation noise in polar coordinates into Cartesian coordinates. In the observation error evaluator (2) for each motion model, the target position prediction vector calculated one sampling before the current time obtained from the prediction value calculator (7) for each motion model is set as the average vector of the target positions, and each motion model is calculated. Target position observation vector and target position prediction from the prediction error covariance matrix obtained from the prediction error evaluator (10) calculated 1 time before the present time and the observation noise covariance matrix obtained from the preset observation model The covariance matrix of the vector difference is obtained, and the error of the target observation position input from the target observation device with respect to the target predicted position is evaluated for each of a plurality of motion models assuming that the target position observation vector follows a three-dimensional normal distribution. In the reliability calculator (3) of each motion model, the reliability of each of the motion models calculated one sampling before the current time, Markov property in the motion model transition is assumed, and the motion model preset is changed. The reliability of each of the plurality of motion models is calculated from the probability and the observation error evaluation value of each of the motion models input from the observation error evaluator (2) for each motion model. In the gain matrix calculator (5), the observation noise covariance matrix obtained from the preset observation model and the motion model calculated by the prediction error evaluator (10) for each motion model one sampling before the present time The gain matrix is calculated from the Kalman filter theory from the prediction error covariance matrix for each. With the smoother (6), 1 from the present
Smoothed value calculated before sampling, target observation position input from target observation device (1), gain matrix input from gain matrix calculator (5), and reliability calculator (3) of each motion model The smoothed values of the target position and the target velocity are calculated from the sum of constant acceleration vectors constituting the plurality of motion models weighted by the reliability of each of the plurality of motion models obtained in step 1. The predicted value calculator (7) for each motion model uses the target position and target speed calculated by the smoother (6) and the constant acceleration vector for each motion model to calculate the target position and target speed one sampling after the current time. Calculate the predicted value. In the smoothing error evaluator (9) for each motion model, the covariance matrix of the difference between the smoothed value and the true value of the target position and the target velocity is calculated by the gain matrix calculator (5) and one sampling before the present time. It is calculated by the prediction error covariance matrix calculated by the prediction error evaluator (10) for each motion model. In the prediction error evaluator (10) for each motion model, the smoothing error covariance matrix input from the smoothing error evaluator (9) for each motion model and the driving noise covariance matrix obtained from the preset motion model are used. The covariance matrix of the difference between the predicted value and the true value after one sampling from the present time is calculated. In the target observing device (1), the target position based on the Cartesian coordinates after one sampling from the present time calculated as the target performing a uniform linear motion based on the smoothed values of the target position and the target speed input from the smoother (6). Prediction range in which the target exists by using the property that the prediction error covariance matrix for each motion model calculated by the prediction error evaluator (10) for each motion model is constant regardless of the motion model Is calculated, and only the target position information within this range is treated as being obtained from the tracking target.

〔発明が解決しようとする問題点〕[Problems to be solved by the invention]

上記のような従来の追尾フイルタでは，平滑器（６）で
算出する目標位置及び目標速度の平滑値が各運動モデル
の信頼度算出器（３）で求めた複数個の運動モデルのお
のおのに対する信頼度を反映した値にもかかわらず，目
標位置及び目標速度の予測値の誤差を評価する機構は各
運動モデルごとにしか存在しなかつたため予測誤差の評
価は従来のカルマンフイルタの手法によつており，運動
モデル一意的に定まるとして求めていたため，目標が旋
回したことによつて生じる予測誤差の増加が反映されず
現実よりはるかに良い評価結果を算出していた。このた
め，平面内を運動する目標追尾の場合，第４図に示すよ
うに目標予測位置を中心にして，たとえば99％の確率で
目標が存在する予測範囲は現実よりも小さくなつてい
た。ここで，第４図において,Pは追尾目標の予測位置,C
1は曲線C1で囲まれた範囲内に目標が99％の確率で存在
する従来の追尾フイルタによる予測範囲,C2は曲線C2で
囲まれた範囲内で目標が99％の確率で存在する現実の予
測範囲である。従つて，第４図における曲線C1内の探知
データが追尾対象目標からの探知データであるすると，
目標が旋回している場合，追尾対象目標からの探知デー
タを失うおそれがあつた。この対策として，目標の運動
状態によらず一律に目標の予測存在範囲を広げる手法も
あるが，追尾対象目標以外の目標あるいはクラツタが追
尾対象目標近くにある場合，追尾対象目標からの探知デ
ータとそれ以外の探知データが混在しやすくなるという
欠点があつた。In the conventional tracking filter as described above, the smoothed values of the target position and the target velocity calculated by the smoother (6) are reliable for each of the plurality of motion models obtained by the reliability calculator (3) of each motion model. Although there is only a mechanism for evaluating the error in the predicted value of the target position and target speed for each motion model, the prediction error is evaluated by the conventional Kalman filter method. Since the motion model was determined as being uniquely determined, an increase in prediction error caused by turning the target was not reflected and a much better evaluation result than in reality was calculated. For this reason, in the case of target tracking that moves in a plane, the prediction range in which the target exists with a probability of, for example, 99% with respect to the target predicted position is smaller than in reality, as shown in FIG. Here, in FIG. 4, P is the predicted position of the tracking target, and C is
1 is the range predicted by the conventional tracking filter in which the target exists with a probability of 99% within the range surrounded by the curve C1, C2 is the actual range in which the target exists within the range surrounded by the curve C2 with a probability of 99%. It is a prediction range. Therefore, if the detection data in the curve C1 in FIG. 4 is the detection data from the tracking target,
If the target is turning, the detection data from the tracking target could be lost. As a countermeasure for this, there is a method that uniformly expands the predicted existence range of the target regardless of the motion state of the target. However, when a target other than the tracking target target or clutter is near the tracking target target, the detection data from the tracking target target and There was a drawback that other detection data would easily be mixed.

この発明はこのような問題点を解決するためになされた
もので，旋回目標に対しても精度よく追尾できる追尾フ
イルタを構成することを目的とする。The present invention has been made to solve such a problem, and an object thereof is to construct a tracking filter capable of accurately tracking even a turning target.

〔問題点を解決するための手段〕[Means for solving problems]

の発明の追尾フイルタは，予測値と真値の差である予測
誤差の評価である予測誤差共分散行列の算出に複数個の
運動モデルおのおのの信頼度を使用するようにしたもの
である。The tracking filter of the invention described in (1) uses a reliability of each of a plurality of motion models for calculating a prediction error covariance matrix which is an evaluation of a prediction error which is a difference between a predicted value and a true value.

〔作用〕[Action]

この発明においては，複数個の運動モデルおのおののに
対する信頼度及び複数個の運動モデルを構成する定数加
速度ベクトルを使用し運動モデルが一意的に定まらない
ことにより平滑誤差の評価である予測誤差共分散行列を
増加させる諸元を算出し，この諸元を複数個の運動モデ
ルごとの平滑誤差共分散行列に加算することにより予測
誤差共分散行列を算出するようにしている。すなわち、
この発明による予測誤差共分散行列P_K（−）は、従来の
追尾フィルタによって求まる値である運動モデルごとの
予測誤差共分散行列▲Ｐ^a _K▼（−）に半正値対称行列で
ある式（51）の右辺の最終項を加算して式（51）によっ
て求まる。なお、式（13）に示したように運動モデルご
との予測誤差共分散行列▲Ｐ^a _K▼（−）は仮説によらな
い値となる。In the present invention, the reliability of each of the plurality of motion models and the constant acceleration vector forming the plurality of motion models are used, and the motion model is not uniquely determined. The prediction error covariance matrix is calculated by calculating the parameters that increase the matrix and adding these parameters to the smoothing error covariance matrix for each of the multiple motion models. That is,
The prediction error covariance matrix P _K (−) according to the present invention is a semi-positive symmetric matrix in the prediction error covariance matrix ▲ P ^a _K ▼ (−) for each motion model, which is a value obtained by the conventional tracking filter. The last term on the right-hand side of (51) is added to obtain by equation (51). Note that, as shown in Expression (13), the prediction error covariance matrix ▲ P ^a _K ▼ (-) for each motion model has a value that does not depend on the hypothesis.

〔実施例〕〔Example〕

第１図はこの発明の一実施例を示す構成図であり，
（１）〜（12）は上記従来装置と全く同一のものであ
る。（13）は予測値の予測誤差を評価する予測誤差評価
器である。FIG. 1 is a block diagram showing an embodiment of the present invention,
(1) to (12) are exactly the same as the above conventional device. (13) is a prediction error evaluator that evaluates the prediction error of the prediction value.

複数個をＮ個とした場合の運動モデルをｘ _ｋ ＝Φ_k-1 ｘ _k-1＋Γ_k-1 ｘ _k-1＋Γ′_k-1 ｕ _k-1 ……
（１） とする。The motion model when the number of the plurality is N is x _k = Φ _k-1 x _k-1 + Γ _k-1 x _k-1 + Γ ′ _k-1 u _k-1.
(1)

ここで， ・ｘ _ｋはサンプリング時刻t_kにおける目標運動諸元の真
値をあらわす状態ベクトルであり，直交座標における目
標位置ベクトルを 直交座標における目標速度ベクトルを とした時， である。Where: • x _k is a state vector that represents the true value of the target motion specifications at the sampling time t _k , and the target position vector in Cartesian coordinates is The target velocity vector in Cartesian coordinates And when Is.

・Φ_k-1はサンプリング時刻t_k-1よりt_Kへの状態ベクト
ルｘ _ｋの推移行列で目標が等速直線運動を行うと仮定し
た場合， である。Φ _k-1 is a transition matrix of the state vector x _k from the sampling time t _k-1 to t _K , and it is assumed that the target makes a uniform linear motion, Is.

・ｗ _ｋはサンプリング時刻t_kにおける駆動雑音ベクトル Γ_ｋはサンプリング時刻t_kにおける駆動雑音ベクトルの
変換行列で たとえば，目標の運動モデルを等速直線運動と仮定した
ことによる打切り誤差項Γ_k-1 ｗ _k-1とみればｗ _ｋ は加速度ベクトル相当であり である。· W _k is a sampling time t drive noise vector gamma _k in _k is for example a transformation matrix driving noise vector at sampling time t _k, truncation error term due to the assumed constant velocity linear motion target motion model gamma _k-1 If we look at w _k-1 , w _k is equivalent to the acceleration vector. Is.

・ｕ _ｋはサンプリング時刻t_kにおいてＮ個の運動モデル
を構成する定数加速度ベクトルでｕ _ｋ ＝α _１又はｕ _ｋ＝α _２又は……又はｕ _ｋ＝α _Ｎ……
（２） であり， Γ′_ｋはサンプリング時刻t_kにおける定数加速度ベクト
ルの変換行列で である。 -U _k is a constant acceleration vector forming N motion models at the sampling time t _k , and u _k = α ₁ or u _k = α ₂ or ... or u _k = α _N ...
(2) and Γ ′ _k is the transformation matrix of the constant acceleration vector at the sampling time t _k . Is.

第３図は水平面に平行な面内で定数加速度ベクトルを説
明する図であり，図において,Oは目標観測装置を原点と
した座標Ｏ−xyの原点,Xは東方向のｘ軸の正とした座標
Ｏ−xyのｘ軸,Yは北方向をｙ軸の正とした座標Ｏ−xyの
ｙ軸,A1はｙ軸の正方向の定数加速度ベクトル,A2はｘ軸
の正の方向の定数加速度ベクトル,A3はｙ軸の負の方向
の定数加速度ベクトル,A4はｘ軸の負の方向の定数加速
度ベクトルである。第３図における定数加速度ベクトル
の大きさを10g（ｇは重力加速度とする）とし，この他
に加速度０の定数加速度ベクトルを考えた運動モデルの
場合 Ｎ＝５ （３） であり，式（２）は とサンプリング時刻t_kに無関係に書ける。Fig. 3 is a diagram for explaining the constant acceleration vector in the plane parallel to the horizontal plane. In the figure, O is the origin of the coordinate O-xy with the target observation device as the origin, and X is the positive x-axis in the east direction. X-axis of coordinate O-xy, Y is the y-axis of coordinate O-xy with the north direction as the positive y-axis, A1 is a constant acceleration vector in the positive direction of the y-axis, and A2 is a constant in the positive direction of the x-axis. An acceleration vector, A3 is a constant acceleration vector in the negative direction of the y-axis, and A4 is a constant acceleration vector in the negative direction of the x-axis. In the case of a motion model in which the constant acceleration vector in FIG. 3 has a magnitude of 10 g (g is the gravitational acceleration) and a constant acceleration vector with zero acceleration is considered, N = 5 (3) ) Is And can be written regardless of the sampling time t _k .

次に、サンプリング時刻t_kにおいてｕ _k-1 ＝α _ａ（ａ＝1,2,…,N） （５） が真であるとの仮説を Ψ_k,a（ａ＝1,2,…,N） （６） と書く。Next, at the sampling time t _k , a hypothesis that u _k−1 = α _a (a = 1,2, ..., N) (5) is true is given by Ψ _{k, a} (a = 1,2, ..., N) (6) Write.

Ｅを平均をあらわす記号として，ｗ _ｋは平均０の３次元
正規分布白色雑音で Ｅ〔ｗ _ｋ〕＝０ （７） Ｅ〔ｗ _ｋ ｗ _l ^T〕＝Q_k（ｋ＝１の時）,OI（ｋ≠ｌの時）
（８） とする。Let E be a symbol representing the mean, and w _k is a three-dimensional normally distributed white noise with mean 0 E [ w _k ] = 0 (7) E [ w _k w _L ^T ] = Q _k (when k = 1), OI (when k ≠ l)
(8)

ここで， ・０は零ベクトル ・Q_kはサンプリング時刻t_kにおける駆動雑音共分散行列 であり，ａ ^Ｔ はベクトルａの転置ベクトルをあらわす。Here, 0 is a zero vector, Q _k is a driving noise covariance matrix at sampling time t _k , and a ^T represents a transposed vector of vector a.

観測モデルをｚ _ｋ ＝H_k ｘ _ｋ＋ｖ _ｋ （９） とする。The observation model is z _k = H _k x _k + v _k (9).

ここで， ・ｚ _ｋはサンプリング時刻t_kにおける目標位置情報の観
測値より構成される直交座標による観測ベクトル ・H_kはサンプリング時刻t_kにおける観測行列で H_k＝（Ｉ 0I） である。Here, · z _k observation vector · H _k by configured orthogonal coordinate than the observed value of the target position information at sampling time t _k is H _k = (I 0I) in the observation matrix at sampling time t _k.

・ｖ _ｋはサンプリング時刻t_kにおける観測雑音ベクトル
であり，平均０の３次元正規分布白色雑音で， Ｅ〔ｖ _ｋ〕＝０ Ｅ〔ｖ _ｋ ｖ _l ^T〕＝R_k（ｋ＝１の時）， 0I（ｋ≠ｌの時） （10） である。• v _k is the observation noise vector at the sampling time t _k , which is a three-dimensional normally distributed white noise with an average of 0 , and E [ v _k ] = 0 E [ v _k v _l ^T ] = R _k (when k = 1 ), 0I (when k ≠ l) (10).

なお,R_kはサンプリング時刻t_kにおける観測雑音共分散
行列で，運動モデルによらない値とする。Note that R _k is the observation noise covariance matrix at the sampling time t _k and is a value that does not depend on the motion model.

また，サンプリング時刻t₁からt_kまでの観測ベクトル全
体をZ_k,すなわち Z_k＝〔ｚ ₁,ｚ ₂,…，ｚ _ｋ〕 （11） と書く。The entire observation vector from sampling time t ₁ to t _k is written as Z _k , that is, Z _k = [ z ₁ , z ₂ , ..., 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） である。From the above, according to the usual theory of Kalman filter, _k ^a (−) = Φ_{k-1 k-1}(+) + Γ ′_k-1 α _a (12) P_k ^a(-) = Φ_k-1P_k-1(+) Φk-1^T+ Γ_k-1Q_k-1Γ_k-1 ^TT (13) _k ^a (+) = _k ^a(-) + K_k(z _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) (a = 1,2, ..., N).

ここで ・ _k ^a（−）は仮説Ψ_k,aのもとでの観測ベクトルｚ ₁,
ｚ _２…ｚ _k-1のよりのｘ _ｋの予測ベクトルで，条件つき
平均ベクトルで書けばカルマンフイルタの理論より _k ^a （−）＝（Ｅ）〔ｘ _k|Ψ_k,a,Z^k-1〕 （17） である。here · _k ^a(−) Is hypothesis Ψ_{k, a}Observation vector underz ₁,
z _Two…z _k-1Thanx _kThe prediction vector of
If you write in the average vector _k ^a (-) = (E) [x _k| Ψ_{k, a}, Z^k-1] (17)

・ _ｋ（＋）は観測ベクトルｚ ₁,ｚ _２…，ｚ _ｋよりのｘ
_ｋの平滑ベクトルで _ｋ （＋）＝Ｅ〔ｘ _k|Z^k〕 （18） である。・ _k(+) Is the observation vectorz ₁,z _Two… 、z _kThanx
_kWith the smooth vector of _k (+) = E [x _k| Z^k] (18)

・ _k ^a（＋）は仮説Ψ_k,aのもとでの観測ベクトルｚ ₁,
ｚ _２…ｚ _ｋよりのｘ _ｋの平滑ベクトルで _k ^a （＋）＝Ｅ〔ｘ _k|Ψ_k,a,Z^k〕 （19） である。・ _k ^a(+) Is hypothesis Ψ_{k, a}Observation vector underz ₁,
z _Two…z _kThanx _kWith the smooth vector of _k ^a (+) = E [x _k| Ψ_{k, a}, Z^k] (19)

・P_k（＋）はサンプリング時刻t_kにおける平滑誤差共分
散行列であり，カルマンフイルタの理論より P_ｋ(+)=E〔(x _ｋ- _ｋ(+))(x _ｋ- _ｋ(+))^Ｔ|Z^ｋ〕 (20) である。・ P_k(+) Is sampling time t_kCovariance of the smooth error at
It is a dispersion matrix, and from the theory of Kalman filter P_k(+) = E 〔(x _k- _k(+)) (x _k- _k(+))^T| Z^k] (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(-) Is the sampling time t_kHypothesis Ψ at_{k, a}of
The prediction error covariance matrix P_k ^a(-) = E 〔(x _k- _k ^a(-)) (x _k- _k ^a(-))^T| Ψ_{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) である。・ P_k ^a(+) Is sampling time t_kHypothesis Ψ at_{k, a}of
The original smooth error variance matrix P_k ^a(+) = E 〔(x _k- _k ^a(+)) (x _k- _k ^a(+))^T| Ψ_{k, a}, Z^k]
 (22)

・K_kはサンプリグ時刻t_kにおけるゲイン行列である。・ K _k is the gain matrix at the sampling time t _k .

なお,A^Tは行列Ａの転置行列をあらわす。Note that ^AT represents the transposed matrix of matrix A.

また，カルマンフイルタを通常適用する場合と同様にし
て，初期値 _Ｏ（＋）,P_O（＋）は別途定まつていると
する。Also, in the same way as when applying the Kalman filter normally.
Initial value _O(+), P_O(+) Is determined separately
To do.

式（13）よりP_k ^a（−）は仮説Ψ_k,aによらない値なの
で、式（15）よりK_k,式（16）よりP_k ^a（＋）も同様に仮
説Ψ_k,aによらない値となる。Equation (13) from P _k ^a (-) hypothesis [psi _k, since the value that does not depend on _a, K _k from equation ^{_{(15), P k a (}} +) likewise hypothesis from the equation (16) Ψ _{k, a} It does not depend on the value.

観測ベクトルｚ ₁,ｚ ₂,…，ｚ _ｋが得られた時の仮説Ψ
_k,aの信頼度β_k,aを条件つき確率密度関数により β_k,a＝Ｐ〔Ψ_k,a|Z^k〕 （23） （ａ＝1,2,…,N） と書く。Hypothesis Ψ when the observation vectors z ₁ , z ₂ , ..., z _k are obtained
_k, by the reliability β _{k, a} conditional probability density function of _{a β k, a} = P [Ψ _{^k, a} | Z _k] (23) (a = 1,2, ..., N) and write.

運動モデルの推移にマルコフ性を仮定する。すなわち運
動モデルΨ_k,aはサンプリング時刻t_k-1の運動モデルよ
り決まりサンプリング時刻t_k-2までの運動モデルには依
存しないとする。この時，運動モデルの推移確率P_abを P_ab＝Ｐ〔Ψ｜_k,a｜Ψ_ｋ−１,^b〕 （24） （ａ＝1,2,…,N;b＝1,2,…,N） と書く。Markov property is assumed for the transition of the motion model. That is, the motion model Ψ _{k, a} is determined from the motion model at the sampling time t _k-1 and does not depend on the motion model up to the sampling time t _k-2 . At this time, the transition probability P _a b of the motion model is P _a b = P [Ψ | _{k, a} | Ψ _k−1 , ^b ] (24) (a = 1,2, ..., N; b = 1,2 ,…, N) is written.

仮説Ψ_k,aのもとでの観測誤差の評価 f_a（ｋ）（ａ＝1,2,…,N） （25） を３次元正規分布で近似すればカルマンフイルタの理論
より fa(k)=g(z _ｋ;H_{ｋ k} ^a(-),H_ｋP_k ^a(-)H_k ^T+R_ｋ) (26) である。Hypothesis Ψ_{k, a}Evaluation of observation error under_a(K) (a = 1,2, ..., N) (25) can be approximated by a three-dimensional normal distribution, the theory of Kalman filter
More fa (k) = g (z _k; H_{k k} ^a(-), H_kP_k ^a(-) H_k ^T+ R_k) (26).

ここで， ｇ（ｚ _k;ａ _k,A_k）は平均ベクトルａ _k,共分散行列A_kの３
次元正規分布のｚ _ｋにおける確率密度である。Here, g ( z _k ; a _k , A _k ) is 3 of the mean vector a _k and the covariance matrix A _k .
It is the probability density in z _k of the dimensional normal distribution.

式（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）のＰａｂより として求まる。As can be seen from equation (26), the observation vector of the target positionz
_kIs the target position prediction vector H_{k k} ^aThe closer to (-), the more f_a
The value of (k) becomes large and the goal is hypothesis Ψ._{k, a}Exercise under
It is highly evaluated for its reliability. Figure 5 shows the observation error
Evaluation value f_aIt is a figure explaining (k) in one dimension, and is a figure.
, G is the horizontal axis representing the target position, and F is the probability density.
Vertical axis, Z is the observation vectorz _kThe target position, PA is
Hypothesis Ψ_{k, a}Target predicted position under F, and FA is probability density fa
(K), N is the average H_{k k} ^a(-), Variance H_kP_k ^a(−) H_k ^T+ R_k
It is a normal distribution curve determined by. Observation vectorz _k
Hypothesis Ψ at the time when is not obtained_{k, a}Reliability P {Ψ of_{k, a}
| Z^k-1} Is the hypothesis Ψ from the Markov property._{k, a}There is one sample
Before each hypothesis Ψ_{k-1, b}(B = 1,2, ..., N)
Ψ_{k-1, b}Β in equation (23), which is the reliability of
_{k-1, b}And P in equation (24), which is the transition probabilityafrom bIs obtained as.

観測ベクトルｚ _ｋが得られた時点での仮説Ψ_k,aの信頼
度β_k,εは，観測ベクトルｚ _ｋが得られない時点の仮説
Ψ_k,aの信頼度Ｐ〔Ψ_k,a|Z^k-1〕に仮説Ψ_k,aの観測ベク
トルｚ _ｋによる評価値ｆａ（ｋ）を乗算した値，すなわ
ち式（27）および式（25）より， に比例すると考えてよい。Reliability beta _k hypothesis [psi _{k, a} at the time of the observation vector z _k is _{obtained, epsilon} is reliability P hypothesis [psi _{k, a} point where the observation vector z _k is not obtained [[psi _{k, a} | Z ^k-1 ] is multiplied by the evaluation value f a (k) of the observation vector z _k of the hypothesis Ψ _{k, a} , that is, from Equation (27) and Equation (25), It can be considered to be proportional to.

確率の性質より だから，式（28）を正規化して である。From the nature of probability Therefore, normalize equation (28) Is.

式（23）及びベイズの定理より である。From equation (23) and Bayes' theorem Is.

式（18），式（19）及び式（31）より だから， である。From equation (18), equation (19) and equation (31) So Is.

式（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） である。From equation (32) and equation (14), equation (12), and equation (29), _k (+) = Φ_{k-1 k-1}(+) + Γ ′_{k-1 k-1}＋ K_k〔Z
_k −H_k(Φ_{k-1 k-1}(+) + Γ ′_{k-1 k-1})] (33).

ここで、 である。here, Is.

複数個の運動モデルを構成している定数加速度ベクトル
α _ａ（ａ＝1,2,…Ｎ）の条件つき確率密度関数を離散系
における確率論に従い とする。Constant acceleration vector that composes multiple motion models
The conditional probability density function of α _a (a = 1,2, ... N) follows the probability theory in the discrete system. And

ここで，デルタ関数δ（ｘ）は ∫…∫Ｇ（ｘ）（ｘdx＝Ｇ（ｏ） （37） の性質を有している。Here, delta function [delta] (x) has the property of ∫ ... ∫G (x) (x dx = G (o) (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） である。From equation (35) and equation (37), E [u _k-1| Ψ_{k, a}, Z^k-1] = ∫ ... ∫u _k-1P 〔u _k-1 │Ψ_{k, a}, Z^K-1] Du _k-1= ∫ ... ∫u _k-1δ (u _k-1 α _adu
_k-1=α _a (38) That is ▲ ^a _k-1▼ = E [u _k-1| Ψ_{k, a}, Z^k-1] (39) When ▲ ^a _k-1▼ ＝α _a (40)

式（36）及び式（37）より である。From equation (36) and equation (37) Is.

すなわち， _k-1 ＝Ｅ〔ｕ _k-1|Z^k-1〕 （42） とした時 である。That is, _k-1 = E [u _k-1| Z^k-1] (42)Is.

式（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) である。From equation (35), equation (40) and equation (37), E [(u _k-1-▲ ^a _k-1▼ (u _k-1-▲ ^a _k-1▼) T | Ψ
_{k, a}, Z^k-1] = ∫… ∫ (u _k-1−α _a) (u _k-1−α _a) TP [u _k-1| Ψ_{k, a}, Z^k-1] Du _k-1 = ∫… ∫ (u _k-1-α _a) (u _k-1-α _a) Tδ (u _k-1-α _a) du _k-1=
This is OI (44).

式（36）及び式（37）より である。 _ｋ （−）を観測ベクトルｚ ₁,ｚ ₂,…，ｚ _K-1よりのｘ
_Ｋの予測ベクトル，すなわち _ｋ （−）＝Ｅ〔ｘ _K|Z_K-1〕 （46） とする。From equation (36) and equation (37)Is. _k (-) Is the observation vectorz ₁,z ₂, ...,z _K-1Thanx
_KThe prediction vector of, ie _k (-) = E [x _K| Z_K-1] (46)

式（１）に式（46），式（18），式（７）及び式（42）
を適用して _ｋ （−）＝Φ_{k-1 k1}（＋）＋Γ′_{k-1 k-1} （47） を得る。In equation (1), equation (46), equation (18), equation (7) and equation (42)
Apply _k (−) = Φ_{k-1 k1}(+) + Γ ′_{k-1 k-1} Get (47).

式（１）及び式（47）よりｘ _ｋ − _ｋ（−）＝Φ_k-1（ｘ _k-1− _k-1（＋））＋Γ
_k-1 ｗ _k-1 ＋Γ′_k-1（ｕ _k-1− _k-1） （48） である。From equation (1) and equation (47)x _k − _k(−) = Φ_k-1(x _k-1− _k-1(+)) + Γ
_k-1 w _k-1 + Γ ′_k-1(u _k-1− _k-1) (48).

P_k（−）をサンプリング時刻t_kにおける予測誤差共分散
行列，すなわち P_k（−）＝Ｅ〔（ｘ _ｋ− _ｋ（−））（ｘ _ｋ−
_ｋ（−））^T|Z^k-1〕 （49） とする。P_k(-) Is the sampling time t_kPrediction error covariance in
Matrix, ie P_k(-) = E [(x _k− _k(-)) (x _k−
_k(-))^T| Z^k-1] (49)

式（49）に式（48）を代入し，式（20），式（８）及び
式（45）を適用し，ｘ _k-1,ｗ _k-1,ｕ _k-1が互いに無相関
とすれば， を得る。Substituting equation (48) into equation (49) and applying equation (20), equation (8), and equation (45), x _k-1 , w _k-1 , and u _k-1 are uncorrelated with each other. if, To get

従つて，式（27），式（13）及び式（50）より である。Therefore, from equation (27), equation (13) and equation (50) Is.

ここで、式（43）及び式（27）より である。Here, from equation (43) and equation (27) Is.

なお，先に述べたように仮説Ψ_k,aのもとでの平滑誤差
共分散行列P_k ^a（＋）は仮説Ψ_k,aに無関係に定まる値な
ので P_k（＋）＝P_k ^a（＋） （53） とする。Note that error covariance matrix under the assumption as mentioned above ^{_{Ψ k, a P k a (}} +) Since the values determined independent of the hypothesis _{Ψ k, a P k (+} ) = P k a (+) (53)

次にこの発明の追尾フイルタの動作を第１図に従つて説
明する。Next, the operation of the tracking filter of the present invention will be described with reference to FIG.

なお，カルマンフイルタを追尾フイルタに通常適用する
場合と同様にして，初期値はあらかじめ定まつていると
する。It is assumed that the initial value is previously determined in the same manner as when the Kalman filter is normally applied to the tracking filter.

目標観測装置（１）では極座標で目標位置情報を観測
し，結果を直交座標による目標位置情報に変換し，式
（９）の観測ベクトルｚ _ｋを出力する。運動モデルごと
の観測誤差評価器（２）では，運動モデルごとの予測値
算出器（７）より得られる現時点より１サンプリング前
に算出した式（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▼（＋）は運動
モデルによらない値となる。The target observation device (1) observes the target position information in polar coordinates.
Then, the result is converted into target position information in Cartesian coordinates,
Observation vector of (9)z _kIs output. For each exercise model
In the observation error evaluator (2), the predicted value for each motion model
One sampling before the current time obtained from the calculator (7)
Prediction vector for each motion model of equation (12) calculated in
_k ^a(-) Is input through the fourth delay circuit (12) and the target
Position prediction vector H_{k k} ^a(-) Is calculated for each motion model
1 sun from the present time obtained from the prediction error evaluator (10)
Prediction for each motion model of equation (13) calculated before pulling
Error covariance matrix P_k ^a(-) Through the second delay circuit (11)
Input and then the covariance matrix H of the predicted position error for each motion model_k
P_k ^a(−) H_k ^TIs calculated and preset with this
Equation (10) obtained from the observation model Observation noise covariance matrix R_k
More observation vectorz _kAnd target position prediction for each motion model
Vector h_{k k} ^aCalculate the difference covariance matrix of (-)
Simple vector input from observation device (1)z _kMultiple of
Observation error, which is the degree of agreement with each motion model
F in Expression (25), which is the evaluation value of_aCalculate (k) according to equation (26)
Put out. In the reliability calculator (3) for each motion model,
Pulling time t_k-1Each of the multiple motion models calculated in
Β in equation (23), which is the reliability for_{k-1, a}The first
Input through the delay circuit (4) and preset with this
Transition probability P of the given equation (24)_aobservation vector from bz _kBut
The formula (2
Calculate the value of 7) and use the value of Equation (27) as the observation vector.z _kof
The view for each motion model, which is the degree of agreement with each motion model
F of the equation (25) input from the measurement error evaluator (2)_a(K)
And the observation vector according to equation (30)z _kGot
The reliability of each of the multiple motion models at the time
Β in Equation (23)_{k, a}To calculate. Gain matrix calculator
In (5), it is obtained from the preset observation model.
(10) observed noise covariance sequence and one sampling before
Prediction error evaluator (1
Motion input from 0) through the second delay circuit (11)
Prediction error covariance matrix P for each model_k ^aBy (-),
Smooth vector according to equation (15) _k(+) Used for calculation
Gain matrix K_kTo calculate. In addition, from the equation (13)
Prediction error covariance matrix for each rule ▲ P^a _K▼ (-) is the exercise model
Since it is a value that does not depend on_K
Also does not depend on the luck model. In the smoother (6), 1
Smooth vector of equation (18) calculated before sampling
Le _k-1Input (+) through the third delay circuit (18),
Φ by prediction of uniform linear motion_{k-1 k-1}(+) Is calculated and
Equation (30) obtained from the motion model reliability calculator (3)
Β of each motion model of_{k, a}And preset
Constant acceleration that composes multiple motion models in equation (5)
Degree vectorα _aAccording to equation (34), the target acceleration _k-1
Estimate  _k-1Is the term Γ ′ that affects the prediction_{k-1 k-1}To
Formulas calculated and obtained from these and the target observation device (1)
Observation vector of (9)z _kFor a smooth vector
_kObservation vector in (+) calculationz _kDegree of contribution
Gain obtained from the gain matrix calculator (5) that determines
Matrix K_kIs applied according to equation (33) and the smooth vector _k(+)
To calculate. Prediction value calculator (7) for each motion model
Is a smooth vector obtained from the smoother (6) _k(+)
More uniform linear motion prediction Φ_{k k}(+) Is calculated,
A set of preset motion models.
Number acceleration vectorα _aIs the term Γ ′ that affects the prediction_k α _aTo
Calculated and sampled one sampling after the current time
Tick t_{k + 1}Prediction vector for multiple motion models for
▲ ^a _{k + 1}▼ (-) is calculated according to equation (12). Exercise model
The smoothing error evaluator (9) for each
The prediction error for each motion model in equation (21)
Covariance matrix P_k ^a(-) Is a prediction error evaluator for each motion model
Input from (10) through the second delay circuit (11)
Gain matrix K obtained from the matrix calculator (5)_kAnd by
The smooth vector for each motion model in Eq. (19)
_k ^aMultiple motion models that are evaluation values of (+) smoothing error
Smooth error covariance matrix P for each_k ^a(+) According to equation (16)
calculate. Note that from equation (13), equation (15), and equation (16),
Smooth error covariance sequence for each dynamic model ▲ P^a _k▼ (+) is exercise
The value does not depend on the model.

運動モデルごとの予測誤差評価器（10）では，運動モデ
ルごとの平滑誤差評価器（９）より得られる式（22）の
運動モデルごとの平滑誤差共分散行列P_k ^a（＋）を式（5
3）に従い式（20）の平滑誤差共分散行列P_k（＋）とみ
なし，あらかじめ設定された運動モデルより得られる式
（８）の駆動雑音共分散行列Q_kとにより式（13）に従い
現時点より１サンプリング後のサンプリング時刻t_k+1に
対する複数個の運動モデルごとの予測ベクトル
_k+1 ^a（−）の予測誤差の評価である運動モデルごとの予
測誤差共分散行列P_k+1 ^a（−）を算出する。In the prediction error evaluator (10) for each motion model, the smoothing error covariance matrix P _k ^a (+) for each motion model of the formula (22) obtained from the smoothing error evaluator (9) for each motion model is expressed by the formula ( Five
It is regarded as the smooth error covariance matrix P _k (+) of Eq. (20) according to 3) and the driving noise covariance matrix Q _{k of} Eq. (8) obtained from the preset motion model Prediction vector for each motion model for sampling time t _{k + 1} after one sampling
_{k + 1} ^a (-) of the prediction prediction error covariance matrix for each evaluation in a motion model of the error P _{k + 1} ^a (-) is calculated.

予測誤差評価器（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を追尾目標から得
られたものとして扱う。ここでｄはあらかじめ設定され
た正のパラメータである。The prediction error evaluator (13) calculates the reliability of each motion model.
Β obtained from vessel (3)_k,b, preset expression
Transition probability P of (24)_ab and a preset number
Constant acceleration vector of equation (5) that constitutes the motion model of
Tollα _aFrom equation (52), use one sample
Prediction vector after _{k + 1}The term to increase (-)And the prediction error evaluator (1
0) Smooth error covariance matrix P for each motion model
_{k + 1} ^a(-) Is added to predict the prediction error
Matrix P_{k + 1}(₋) Is calculated according to the equation (51). The eyes
In the target observation device (1), the target input from the smoother (6)
The target moves at a uniform speed based on the position and the smoothed value of the target speed.
Of the Cartesian coordinates after 1 sampling from the current time.
Predicted value of target positionZ _{K + 1}(-) = H_{K + 1}Φ_{K K}(+)
Prediction error calculated and input from the prediction error evaluator (13)
Covariance matrix P_{K + 1}(-) And preset observation system
From the model, the observation noise covariance matrix R of (10)_{K + 1}More positive pair
Nominal matrix S_{K + 1}= H_{K + 1}P_{K + 1}(-) ▲ H^T _{K + 1}▼ + R_{K + 1}Calculate
ThenZ _{K + 1}Calculate the predicted existence range of the target centered on
Only target position information within this range was obtained from the tracking target
Treat as That is, the observation vectorZ _{K + 1}But(Z _{K + 1}
−Z _{K + 1}(-))^TS_K ^-1(Z _{K + 1}−Z _{K + 1}Only when (-)) ≤ d, the observation vectorZ _{K + 1}Get from the tracking target
Treated as Where d is preset
It is a positive parameter.

〔発明の効果〕〔The invention's effect〕

以上のようにこの発明によれば通常の目標自動追尾装置
に特別の付加装置を付けることなく安価に目標運動諸元
算出精度を向上させることができる。As described above, according to the present invention, it is possible to improve the accuracy of target motion parameter calculation at low cost without attaching a special additional device to the normal target automatic tracking device.

なお以上は等速直線運動モデルに定数加速度ベクトルが
付加されたモデルの場合について説明したが，これ以外
の複数個の運動モデルを有して目標観測装置の情報より
目標運動諸元を算出する追尾フイルタに適用できる。In the above, the case of a model in which a constant acceleration vector is added to the constant velocity linear motion model has been described. However, tracking that calculates the target motion parameters from the information of the target observation device with multiple motion models other than this Applicable to filters.

【図面の簡単な説明】[Brief description of drawings]

第１図はこの発明の一実施例の構成を説明する図，第２
図は従来の追尾フイルタを説明する図，第３図は定数加
速度ベクトルを説明する図，第４図は目標予測存在範囲
を説明する図，第５図は観測誤差評価値を説明する図で
ある。 図において，（１）は目標観測装置，（２）は運動モデ
ルごとの観測誤差評価器，（３）は各運動モデルの信頼
度算出器，（４）は第１の遅延回路，（５）はゲイン行
列算出器，（６）は平滑器，（７）は運動モデルごとの
予測値算出器，（８）は第３の遅延回路，（９）は運動
モデルごとの平滑誤差評価器，（10）は運動モデルごと
の予測誤差評価器，（11）は第２の遅延回路，（12）は
第４の遅延回路，（13）は平滑誤差評価器である。 なお，各図中同一あるいは相当部分には同一符号を付し
て示してある。FIG. 1 is a diagram for explaining the configuration of an embodiment of the present invention, and FIG.
FIG. 4 is a diagram for explaining a conventional tracking filter, FIG. 3 is a diagram for explaining a constant acceleration vector, FIG. 4 is a diagram for explaining a target predicted existence range, and FIG. 5 is a diagram for explaining an observation error evaluation value. . In the figure, (1) is a target observation device, (2) is an observation error evaluator for each motion model, (3) is a reliability calculator of each motion model, (4) is a first delay circuit, (5) Is a gain matrix calculator, (6) is a smoother, (7) is a predicted value calculator for each motion model, (8) is a third delay circuit, (9) is a smoothing error evaluator for each motion model, ( 10) is a prediction error evaluator for each motion model, (11) is a second delay circuit, (12) is a fourth delay circuit, and (13) is a smoothing error evaluator. In the drawings, the same or corresponding parts are designated by the same reference numerals.

Claims (1)

【特許請求の範囲】[Claims] 【請求項１】目標位置情報を観測する目標観測装置と、
同一次元の複数個の定数ベクトルより構成される異なる
複数個の運動モデルおのおのと上記目標観測装置より入
力される目標位置情報との合致度である観測誤差評価値
を算出する運動モデルごとの観測誤差評価器と、上記運
動モデルごとの観測誤差評価器より上記各運動モデルお
のおのに対する観測誤差評価値を入力し、上記各運動モ
デルおのおのの信頼度を算出する各運動モデルの信頼度
算出器と、上記各運動モデルで共通のゲイン行列を算出
するゲイン行列算出器と、上記目標観測装置より入力さ
れる最新の目標位置情報、上記各運動モデルの信頼度算
出器で算出した各運動モデルおのおのの信頼度及び上記
ゲイン行列算出器で算出したゲイン行列とを用いて上記
各運動モデルの影響を統合して得られる目標位置、速
度、加速度などの目標運動諸元の平滑値を算出する平滑
器と、上記平滑器で算出した目標運動諸元の平滑値を入
力し、上記各運動モデルおのおのに対する目標運動諸元
の１サンプリング後の予測値を算出し、その予測値を上
記運動モデルごとの観測誤差評価器へ出力する運動モデ
ルごとの予測値算出器と、上記ゲイン行列算出器よりゲ
イン行列を入力し、上記各運動モデルおのおのに対する
目標運動諸元の平滑値の誤差を評価する運動モデルごと
の平滑誤差評価器と、上記運動モデルごとの平滑誤差評
価器の評価結果を用いて上記各運動モデルおのおのに対
する目標運動諸元の予測値の誤差を評価し、その予測誤
差の評価値を上記ゲイン行列算出器、上記運動モデルご
との平滑誤差評価器及び上記運動モデルごとの観測誤差
評価器へ出力する運動モデルごとの予測誤差評価器と、
上記各運動モデルの信頼度算出器で算出した各運動モデ
ルの信頼度及び上記運動モデルごとの予測誤差評価器で
算出した各運動モデルおのおのに対する予測誤差の評価
値とを入力し、上記各運動モデルの影響を統合して得ら
れる予測値の誤差を評価する予測誤差評価器とを備えた
ことを特徴とする追尾フィルタ。1. A target observing device for observing target position information,
An observation error for each motion model that calculates an observation error evaluation value that is the degree of agreement between each of a plurality of different motion models composed of a plurality of constant vectors of the same dimension and the target position information input from the above target observation device Input the observation error evaluation value for each of the above motion models from the evaluator and the observation error evaluator for each of the motion models, and calculate the reliability of each of the motion models, and the reliability of each motion model, and the above. A gain matrix calculator that calculates a common gain matrix for each motion model, the latest target position information input from the target observation device, and the reliability of each motion model calculated by the reliability calculator of each motion model And a target position, velocity, acceleration, etc. obtained by integrating the influence of each motion model using the gain matrix calculated by the gain matrix calculator. Input the smoother for calculating the smoothed value of the motion parameters and the smoothed value of the target motion parameter calculated by the smoother, and calculate the predicted value of the target motion parameter for each of the above motion models after one sampling. , The predicted value calculator for each motion model that outputs the predicted value to the observation error evaluator for each motion model, and the gain matrix is input from the gain matrix calculator, and the target motion parameters for each of the motion models are input. Using the smoothing error evaluator for each motion model that evaluates the error of the smoothed value and the evaluation result of the smoothing error evaluator for each of the above motion models, evaluate the error of the predicted value of the target motion parameters for each of the above motion models. , A prediction error for each motion model that outputs the evaluation value of the prediction error to the gain matrix calculator, the smoothing error evaluator for each motion model, and the observation error evaluator for each motion model. And the error estimator,
The reliability of each exercise model calculated by the reliability calculator for each exercise model and the evaluation value of the prediction error for each exercise model calculated by the prediction error evaluator for each exercise model are input, and each exercise model is entered. And a prediction error evaluator that evaluates the error of the predicted value obtained by integrating the effects of the above.