JPH0769419B2 - Tracking filter - Google Patents
Tracking filterInfo
- Publication number
- JPH0769419B2 JPH0769419B2 JP22323687A JP22323687A JPH0769419B2 JP H0769419 B2 JPH0769419 B2 JP H0769419B2 JP 22323687 A JP22323687 A JP 22323687A JP 22323687 A JP22323687 A JP 22323687A JP H0769419 B2 JPH0769419 B2 JP H0769419B2
- Authority
- JP
- Japan
- Prior art keywords
- motion
- target
- error
- model
- observation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【発明の詳細な説明】 〔産業上の利用分野〕 この発明は観測雑音を含んだ目標位置情報を目標観測装
置より入力し,目標位置の真値および速度・加速度など
の目標運動諸元を推定する追尾フイルタの精度向上の方
法について提案するものである。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.
第2図は例えばIEEE TRANSACTIONS ON AEROSPACE AND E
LECTRONIC SYSTEMS VOL.AES−13 No.3「MANEUVERING TA
RGET TRAC−KING USING ADAPTIVE STATE ESTIMATION」
で示された複数個の運動モデルを有する場合の従来の追
尾フイルタの構成図であり,図において,(1)は観測
雑音を含んだ目標位置を計測する目標観測装置,(2)
は複数個の運動モデルおのおのに対して観測誤差を評価
するための運動モデルごとの観測誤差評価器,(3)は
複数個の運動モデルおのおのの信頼度を算出するための
各運動モデルの信頼度算出器,(4)は第1の遅延回
路,(5)は目標運動諸元の平滑値算出に使用するカル
マンゲイン行列算出のためのゲイン行列算出器,(6)
は目標運動諸元の平滑値を算出する平滑器,(7)は複
数個の運動モデルおのおのについて現時点より1サンプ
ルング後の目標運動諸元の予測値を算出する運動モデル
ごとの予測値算出器,(8)は第3の遅延回路,(9)
は複数個の運動モデルおのおのの平滑値と真値の差であ
る平滑誤差を評価する運動モデルごとの平滑誤差評価
器,(10)は複数個の運動モデルおのおのの予測値と真
値の差である予測誤差を評価する運動モデルごとの予測
誤差評価器,(11)は第2の遅延回路,(12)は第4の
遅延回路である。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.
従来の複数個の運動モデルを有する追尾フイルタは上記
のように構成され,たとえば固定値交座標を使用し,等
速直線運動モデルにサンプリング時刻によらない定数加
速度ベクトルが付加されたモデルを複数の異なつた加速
度レベルごとに有する場合が複数個の運動モデルとして
使用されていた。目標観測装置(1)では,極座標によ
る観測雑音を含んだ目標位置情報を直交座標に変換す
る。運動モデルごとの観測誤差評価器(2)では,運動
モデルごとの予測値算出器(7)より得られる現時点よ
り1サンプリング前に算出した目標位置予測ベクトルを
目標位置の平均ベクトルとし,運動モデルごとの予測誤
差評価器(10)より得られる現時点より1サンプリング
前に算出した予測誤差共分散行列およびあらかじめ設定
してある観測モデルより得られる観測雑音の共分散行列
より目標位置観測ベクトルと目標位置予測ベクトルの差
の共分散行列を求め,目標位置観測ベクトルが3次元正
規分布に従うとして複数個の運動モデルごとに,目標観
測装置より入力される目標観測位置の目標予測位置に対
する誤差を評価する。各運動モデルの信頼度算出器
(3)では,現時点より1サンプリング前に算出した複
数個の運動モデルおのおのの信頼度,運動モデルの推移
にマルコフ性を仮定し,あらかじめ設定された運動モデ
ルの推移確率及び運動モデルごとの観測誤差評価器
(2)より入力される複数個の運動モデルごとの観測誤
差評価値より,複数個の運動モデルおのおのの信頼度を
算出する。ゲイン行列算出器(5)では,あらかじめ設
定してある観測モデルより得られる観測雑音共分散行列
及び現時点より1サンプリング前に運動モデルごとの予
測誤差評価器(10)で算出しておいた運動モデルごとの
予測誤差共分散行列よりカルマンフイルタの理論よりゲ
イン行列を算出する。平滑器(6)では,現時点より1
サンプリング前に算出しておいた平滑値,目標観測装置
(1)より入力される目標観測位置,ゲイン行列算出器
(5)より入力されるゲイン行列及び各運動モデルの信
頼度算出器(3)で求めた複数個の運動モデルおのおの
の信頼度により重みづけした複数個の運動モデルを構成
している定数加速度ベクトルの総和により目標位置及び
目標速度の平滑値を算出する。運動モデルごとの予測値
算出器(7)では,平滑器(6)で算出した目標位置及
び目標速度と複数個の運動モデルごとの定数加速度ベク
トルにより現時点より1サンプリング後の目標位置及び
目標速度の予測値を算出する。運動モデルごとの平滑誤
差評価器(9)では,目標位置及び目標速度の平滑値と
真値の差の共分散行列をゲイン行列算出器(5)で算出
したゲイン行列及び現時点より1サンプリング前に運動
モデルごとの予測誤差評価器(10)で算出しておいた予
測誤差共分散行列により算出する。運動モデルごとの予
測誤差評価器(10)では,運動モデルごとの平滑誤差評
価器(9)より入力される平滑誤差共分散行列とあらか
じめ設定してある運動モデルより得られる駆動雑音共分
散行列より現時点より1サンプリング後の予測値と真値
との差の共分散行列を算出する。目標観測装置(1)で
は平滑器(6)より入力される目標位置及び目標速度の
平滑値をもとに目標が等速度直線運動を行うとして算出
した現時点より1サンプリング後の直交座標による目標
位置の予測値を中心に、運動モデルごとの予測誤差評価
器(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.
上記のような従来の追尾フイルタでは,平滑器(6)で
算出する目標位置及び目標速度の平滑値が各運動モデル
の信頼度算出器(3)で求めた複数個の運動モデルのお
のおのに対する信頼度を反映した値にもかかわらず,目
標位置及び目標速度の予測値の誤差を評価する機構は各
運動モデルごとにしか存在しなかつたため予測誤差の評
価は従来のカルマンフイルタの手法によつており,運動
モデル一意的に定まるとして求めていたため,目標が旋
回したことによつて生じる予測誤差の増加が反映されず
現実よりはるかに良い評価結果を算出していた。このた
め,平面内を運動する目標追尾の場合,第4図に示すよ
うに目標予測位置を中心にして,たとえば99%の確率で
目標が存在する予測範囲は現実よりも小さくなつてい
た。ここで,第4図において,Pは追尾目標の予測位置,C
1は曲線C1で囲まれた範囲内に目標が99%の確率で存在
する従来の追尾フイルタによる予測範囲,C2は曲線C2で
囲まれた範囲内で目標が99%の確率で存在する現実の予
測範囲である。従つて,第4図における曲線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.
の発明の追尾フイルタは,予測値と真値の差である予測
誤差の評価である予測誤差共分散行列の算出に複数個の
運動モデルおのおのの信頼度を使用するようにしたもの
である。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.
この発明においては,複数個の運動モデルおのおののに
対する信頼度及び複数個の運動モデルを構成する定数加
速度ベクトルを使用し運動モデルが一意的に定まらない
ことにより平滑誤差の評価である予測誤差共分散行列を
増加させる諸元を算出し,この諸元を複数個の運動モデ
ルごとの平滑誤差共分散行列に加算することにより予測
誤差共分散行列を算出するようにしている。すなわち、
この発明による予測誤差共分散行列PK(−)は、従来の
追尾フィルタによって求まる値である運動モデルごとの
予測誤差共分散行列▲Pa K▼(−)に半正値対称行列で
ある式(51)の右辺の最終項を加算して式(51)によっ
て求まる。なお、式(13)に示したように運動モデルご
との予測誤差共分散行列▲Pa 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.
第1図はこの発明の一実施例を示す構成図であり,
(1)〜(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.
複数個をN個とした場合の運動モデルをx k =Φk-1 x k-1+Γk-1 x k-1+Γ′k-1 u k-1 ……
(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)
ここで, ・x kはサンプリング時刻tkにおける目標運動諸元の真
値をあらわす状態ベクトルであり,直交座標における目
標位置ベクトルを 直交座標における目標速度ベクトルを とした時, である。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はサンプリング時刻tk-1よりtKへの状態ベクト
ルx 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.
・w kはサンプリング時刻tkにおける駆動雑音ベクトル Γkはサンプリング時刻tkにおける駆動雑音ベクトルの
変換行列で たとえば,目標の運動モデルを等速直線運動と仮定した
ことによる打切り誤差項Γk-1 w k-1とみればw k は加速度ベクトル相当であり である。· 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.
・u kはサンプリング時刻tkにおいてN個の運動モデル
を構成する定数加速度ベクトルでu k =α 1又はu k=α 2又は……又はu k=α N……
(2) であり, Γ′kはサンプリング時刻tkにおける定数加速度ベクト
ルの変換行列で である。 -U k is a constant acceleration vector forming N motion models at the sampling time t k , and u k = α 1 or u k = α 2 or ... or u k = α N ...
(2) and Γ ′ k is the transformation matrix of the constant acceleration vector at the sampling time t k . Is.
第3図は水平面に平行な面内で定数加速度ベクトルを説
明する図であり,図において,Oは目標観測装置を原点と
した座標O−xyの原点,Xは東方向のx軸の正とした座標
O−xyのx軸,Yは北方向をy軸の正とした座標O−xyの
y軸,A1はy軸の正方向の定数加速度ベクトル,A2はx軸
の正の方向の定数加速度ベクトル,A3はy軸の負の方向
の定数加速度ベクトル,A4はx軸の負の方向の定数加速
度ベクトルである。第3図における定数加速度ベクトル
の大きさを10g(gは重力加速度とする)とし,この他
に加速度0の定数加速度ベクトルを考えた運動モデルの
場合 N=5 (3) であり,式(2)は とサンプリング時刻tkに無関係に書ける。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 .
次に、サンプリング時刻tkにおいてu k-1 =α a(a=1,2,…,N) (5) が真であるとの仮説を Ψk,a(a=1,2,…,N) (6) と書く。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.
Eを平均をあらわす記号として,w kは平均0の3次元
正規分布白色雑音で E〔w k〕=0 (7) E〔w k w l T〕=Qk(k=1の時),OI(k≠lの時)
(8) とする。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)
ここで, ・0は零ベクトル ・Qkはサンプリング時刻tkにおける駆動雑音共分散行列 であり,a T はベクトルaの転置ベクトルをあらわす。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.
観測モデルをz k =Hk x k+v k (9) とする。The observation model is z k = H k x k + v k (9).
ここで, ・z kはサンプリング時刻tkにおける目標位置情報の観
測値より構成される直交座標による観測ベクトル ・Hkはサンプリング時刻tkにおける観測行列で Hk=(I 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.
・v kはサンプリング時刻tkにおける観測雑音ベクトル
であり,平均0の3次元正規分布白色雑音で, E〔v k〕=0 E〔v k v l T〕=Rk(k=1の時), 0I(k≠lの時) (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).
なお,Rkはサンプリング時刻tkにおける観測雑音共分散
行列で,運動モデルによらない値とする。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.
また,サンプリング時刻t1からtkまでの観測ベクトル全
体をZk,すなわち Zk=〔z 1,z 2,…,z k〕 (11) と書く。The entire observation vector from sampling time t 1 to t k is written as Z k , that is, Z k = [ z 1 , z 2 , ..., Z k ] (11).
以上のことより通常のカルマンフイルタの理論により, k a (−)=Φk-1 k-1(+)+Γ′k-1 α a (12) Pk a(-)=Φk-1Pk-1(+)Φk-1T+Γk-1Qk-1Γk-1 TT (13) k a (+)= k a(−)+Kk(z K−Hk k a(−))
(14) Kk=Pk a(−)Hk T(HkPk a(−)Hk T+Rk)-1 (15) Pk a(+)=Pk a(−)−KkHkPk a(−) (16) (a=1,2,…,N) である。From the above, according to the usual theory of Kalman filter, k a (−) = Φk-1 k-1(+) + Γ ′k-1 α a (12) Pk a(-) = Φk-1Pk-1(+) Φk-1T+ Γk-1Qk-1Γk-1 TT (13) k a (+) = k a(-) + Kk(z K−Hk k a(-))
(14) Kk= Pk a(−) Hk T(HkPk a(−) Hk T+ Rk)-1 (15) Pk a(+) = Pk a(−) − KkHkPk a(−) (16) (a = 1,2, ..., N).
ここで ・ k a(−)は仮説Ψk,aのもとでの観測ベクトルz 1,
z 2…z k-1のよりのx kの予測ベクトルで,条件つき
平均ベクトルで書けばカルマンフイルタの理論より k a (−)=(E)〔x k|Ψk,a,Zk-1〕 (17) である。here · k a(−) Is hypothesis Ψk, aObservation vector underz 1,
z Two…z k-1Thanx kThe prediction vector of
If you write in the average vector k a (-) = (E) [x k| Ψk, a, Zk-1] (17)
・ k(+)は観測ベクトルz 1,z 2…,z kよりのx
kの平滑ベクトルで k (+)=E〔x k|Zk〕 (18) である。・ k(+) Is the observation vectorz 1,z Two… 、z kThanx
kWith the smooth vector of k (+) = E [x k| Zk] (18)
・ k a(+)は仮説Ψk,aのもとでの観測ベクトルz 1,
z 2…z kよりのx kの平滑ベクトルで k a (+)=E〔x k|Ψk,a,Zk〕 (19) である。・ k a(+) Is hypothesis Ψk, aObservation vector underz 1,
z Two…z kThanx kWith the smooth vector of k a (+) = E [x k| Ψk, a, Zk] (19)
・Pk(+)はサンプリング時刻tkにおける平滑誤差共分
散行列であり,カルマンフイルタの理論より Pk(+)=E〔(x k- k(+))(x k- k(+))T|Zk〕 (20) である。・ Pk(+) Is sampling time tkCovariance of the smooth error at
It is a dispersion matrix, and from the theory of Kalman filter Pk(+) = E 〔(x k- k(+)) (x k- k(+))T| Zk] (20).
・Pk a(−)はサンプリング時刻tkにおける仮説Ψk,aの
もとで予測誤差共分散行列で Pk a(-)=E〔(x k- k a(-))(x k- k a(-))T|Ψk,a,Zk-1]
(21) である。・ Pk a(-) Is the sampling time tkHypothesis Ψ atk, aof
The prediction error covariance matrix Pk a(-) = E 〔(x k- k a(-)) (x k- k a(-))T| Ψk, a, Zk-1]
(21).
・Pk a(+)はサンプリング時刻tkにおける仮説Ψk,aの
もとでの平滑誤差分散行列で Pk a(+)=E〔(x k- k a(+))(x k- k a(+))T|Ψk,a,Zk]
(22) である。・ Pk a(+) Is sampling time tkHypothesis Ψ atk, aof
The original smooth error variance matrix Pk a(+) = E 〔(x k- k a(+)) (x k- k a(+))T| Ψk, a, Zk]
(22)
・Kkはサンプリグ時刻tkにおけるゲイン行列である。・ K k is the gain matrix at the sampling time t k .
なお,ATは行列Aの転置行列をあらわす。Note that AT represents the transposed matrix of matrix A.
また,カルマンフイルタを通常適用する場合と同様にし
て,初期値 O(+),PO(+)は別途定まつていると
する。Also, in the same way as when applying the Kalman filter normally.
Initial value O(+), PO(+) Is determined separately
To do.
式(13)よりPk a(−)は仮説Ψk,aによらない値なの
で、式(15)よりKk,式(16)よりPk 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.
観測ベクトルz 1,z 2,…,z kが得られた時の仮説Ψ
k,aの信頼度βk,aを条件つき確率密度関数により βk,a=P〔Ψk,a|Zk〕 (23) (a=1,2,…,N) と書く。Hypothesis Ψ when the observation vectors z 1 , z 2 , ..., 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はサンプリング時刻tk-1の運動モデルよ
り決まりサンプリング時刻tk-2までの運動モデルには依
存しないとする。この時,運動モデルの推移確率Pabを Pab=P〔Ψ|k,a|Ψk−1,b〕 (24) (a=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のもとでの観測誤差の評価 fa(k)(a=1,2,…,N) (25) を3次元正規分布で近似すればカルマンフイルタの理論
より fa(k)=g(z k;Hk k a(-),HkPk a(-)Hk T+Rk) (26) である。Hypothesis Ψk, aEvaluation of observation error undera(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; Hk k a(-), HkPk a(-) Hk T+ Rk) (26).
ここで, g(z k;a k,Ak)は平均ベクトルa k,共分散行列Akの3
次元正規分布のz 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)よりわかるように,目標位置の観測ベクトルz
kが目標位置の予測ベクトルHk k a(−)に近いほどfa
(k)の値は大きくなり目標が仮説Ψk,aのもとで運動
したとの信頼度が高く評価される。第5図は観測誤差の
評価値fa(k)を1次元において説明する図であり,図
において,Gは目標位置をあらわす横軸,Fは確率密度をあ
らわす縦軸,Zは観測ベクトルz kの示す目標位置,PAは
仮説Ψk,aのもとでの目標予測位置,FAは確率密度fa
(k),Nは平均Hk k a(−),分散HkPk a(−)Hk T+Rk
によつて定まる正規分布曲線である。観測ベクトルz k
が得られていない時点での仮説Ψk,aの信頼度P{Ψk,a
|Zk-1}は,マルコフ性より,仮説Ψk,aが1サンプリン
グ前の各仮説Ψk−1,b(b=1,2,…,N)より推移して
得られるので,Ψk−1,bの信頼度である式(23)のβ
k−1,b及び推移確率である式(24)のPabより として求まる。As can be seen from equation (26), the observation vector of the target positionz
kIs the target position prediction vector Hk k aThe closer to (-), the more fa
The value of (k) becomes large and the goal is hypothesis Ψ.k, aExercise under
It is highly evaluated for its reliability. Figure 5 shows the observation error
Evaluation value faIt 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, aTarget predicted position under F, and FA is probability density fa
(K), N is the average Hk k a(-), Variance HkPk a(−) Hk T+ Rk
It is a normal distribution curve determined by. Observation vectorz k
Hypothesis Ψ at the time when is not obtainedk, aReliability P {Ψ ofk, a
| Zk-1} Is the hypothesis Ψ from the Markov property.k, aThere 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, bAnd P in equation (24), which is the transition probabilityafrom bIs obtained as.
観測ベクトルz kが得られた時点での仮説Ψk,aの信頼
度βk,εは,観測ベクトルz kが得られない時点の仮説
Ψk,aの信頼度P〔Ψk,a|Zk-1〕に仮説Ψk,aの観測ベク
トルz kによる評価値fa(k)を乗算した値,すなわ
ち式(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 (+)=Φk-1 k-1(+)+Γ′k-1 k-1+Kk〔Z
k −Hk(Φ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+ Kk〔Z
k −Hk(Φk-1 k-1(+) + Γ ′k-1 k-1)] (33).
ここで、 である。here, Is.
複数個の運動モデルを構成している定数加速度ベクトル
α a(a=1,2,…N)の条件つき確率密度関数を離散系
における確率論に従い とする。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
ここで,デルタ関数δ(x)は ∫…∫G(x)(xdx=G(o) (37) の性質を有している。Here, delta function [delta] (x) has the property of ∫ ... ∫G (x) (x dx = G (o) (37).
式(35)及び式(37)より E〔u k-1|Ψk,a,Zk-1〕=∫…∫u k-1P〔u k-1 |Ψk,a,ZK-1〕du k-1=∫…∫u k-1δ(u k-1 α adu
k-1=α a (38) すなわち ▲ a k-1▼=E〔u k-1|Ψk,a,Zk-1〕 (39) とした時 ▲ a k-1▼=α a (40) である。From equation (35) and equation (37), E [u k-1| Ψk, a, Zk-1] = ∫ ... ∫u k-1P 〔u k-1 │Ψk, a, ZK-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, Zk-1] (39) When ▲ a k-1▼ =α a (40)
式(36)及び式(37)より である。From equation (36) and equation (37) Is.
すなわち, k-1 =E〔u k-1|Zk-1〕 (42) とした時 である。That is, k-1 = E [u k-1| Zk-1] (42)Is.
式(35),式(40)及び式(37)より E〔(u k-1−▲ a k-1▼(u k-1−▲ a k-1▼)T|Ψ
k,a,Zk-1〕 =∫…∫(u k-1−α a)(u k-1−α a)TP〔u k-1| Ψk,a,Zk-1〕du k-1 =∫…∫(u k-1-α a)(u k-1-α a)Tδ〔u k-1-α a)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, Zk-1] = ∫… ∫ (u k-1−α a) (u k-1−α a) TP [u k-1| Ψk, a, Zk-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 (−)を観測ベクトルz 1,z 2,…,z K-1よりのx
Kの予測ベクトル,すなわち k (−)=E〔x K|ZK-1〕 (46) とする。From equation (36) and equation (37)Is. k (-) Is the observation vectorz 1,z 2, ...,z K-1Thanx
KThe prediction vector of, ie k (-) = E [x K| ZK-1] (46)
式(1)に式(46),式(18),式(7)及び式(42)
を適用して k (−)=Φ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).
式(1)及び式(47)よりx k − k(−)=Φk-1(x k-1− k-1(+))+Γ
k-1 w k-1 +Γ′k-1(u 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).
Pk(−)をサンプリング時刻tkにおける予測誤差共分散
行列,すなわち Pk(−)=E〔(x k− k(−))(x k−
k(−))T|Zk-1〕 (49) とする。Pk(-) Is the sampling time tkPrediction error covariance in
Matrix, ie Pk(-) = E [(x k− k(-)) (x k−
k(-))T| Zk-1] (49)
式(49)に式(48)を代入し,式(20),式(8)及び
式(45)を適用し,x k-1,w k-1,u 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のもとでの平滑誤差
共分散行列Pk a(+)は仮説Ψk,aに無関係に定まる値な
ので Pk(+)=Pk 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)
次にこの発明の追尾フイルタの動作を第1図に従つて説
明する。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.
目標観測装置(1)では極座標で目標位置情報を観測
し,結果を直交座標による目標位置情報に変換し,式
(9)の観測ベクトルz kを出力する。運動モデルごと
の観測誤差評価器(2)では,運動モデルごとの予測値
算出器(7)より得られる現時点より1サンプリング前
に算出した式(12)の運動モデルごとの予測ベクトル
k a(−)を第4の遅延回路(12)を通して入力し,目標
位置予測ベクトルHk k a(−)を算出し運動モデルごと
の予測誤差評価器(10)より得られる現時点より1サン
プリング前に算出した式(13)の運動モデルごとの予測
誤差共分散行列Pk a(−)を第2の遅延回路(11)を通
し入力し運動モデルごとの予測位置誤差の共分散行列Hk
Pk a(−)Hk Tを算出し,これとあらかじめ設定してある
観測モデルより得られる式(10)観測雑音共分散行列Rk
より観測ベクトルz kと運動モデルごとの目標位置予測
ベクトルHk k a(−)の差の共分散行列を算出し,目標
観測装置(1)より入力される簡素ベクトルz kの複数
個の運動モデルおのおのに対する合致度である観測誤差
の評価値である式(25)のfa(k)を式(26)に従い算
出する。各運動モデルの信頼度算出器(3)では,サン
プリング時刻tk-1で算出した複数個の運動モデルおのお
のに対する信頼度である式(23)のβk−1,aを第1の
遅延回路(4)を通して入力し,これとあらかじめ設定
された式(24)の推移確率Pabより観測ベクトルz kが
得られない時点での各運動モデルの信頼度である式(2
7)の値を算出し,式(27)の値に観測ベクトルz kの
各運動モデルに対する合致度である運動モデルごとの観
測誤差評価器(2)より入力される式(25)のfa(k)
を重みづけし、式(30)に従い観測ベクトルz kが得ら
れた時点での複数個の運動モデルおのおのの信頼度であ
る式(23)のβk,aを算出する。ゲイン行列算出器
(5)では,あらかじめ設定された観測モデルより得ら
れる式(10)の観測雑音共分散列及び1サンプリング前
に算出しておいた運動モデルごとの予測誤差評価器(1
0)より第2の遅延回路(11)を通して入力される運動
モデルごとの予測誤差共分散行列Pk a(−)とにより,
式(15)に従い平滑ベクトル k(+)算出に使用する
ゲイン行列Kkを算出する。なお、式(13)より運動モデ
ルごとの予測誤差共分散行列▲Pa K▼(−)は運動モデ
ルによらない値となるため、式(15)よりゲイン行列KK
も運度モデルによらない値となる。平滑器(6)では,1
サンプリング前に算出しておいた式(18)の平滑ベクト
ル k-1(+)を第3の遅延回路(18)を通し入力し,
等速直線運動予測によりΦk-1 k-1(+)を算出し,各
運動モデルの信頼度算出器(3)より得られる式(30)
の各運動モデルの信頼度βk,aとあらかじめ設定された
式(5)の複数個の運動モデルを構成している定数加速
度ベクトルα aより式(34)に従い目標の加速度 k-1
を推定し, k-1が予測に影響する項Γ′k-1 k-1を
算出し,これらと目標観測装置(1)より得られる式
(9)の観測ベクトルz kに対し,平滑ベクトル
k(+)算出において観測ベクトルz kが寄与する度合
を決定するゲイン行列算出器(5)より得られるゲイン
行列Kkを式(33)に従い適用し平滑ベクトル k(+)
を算出する。運動モデルごとの予測値算出器(7)で
は,平滑器(6)より得られる平滑ベクトル k(+)
より等速直線運動予測によりΦk k(+)を算出し,
あらかじめ設定された複数個の運動モデルを構成する定
数加速度ベクトルα aが予測に影響する項Γ′k α aを
算出し,現時点より1サンプリング後のサンプリング時
刻tk+1に対する複数個の運動モデルごとの予測ベクトル
▲ a k+1▼(−)を式(12)に従い算出する。運動モデ
ルごとの平滑誤差評価器(9)では,1サンプリング前に
算出しておいた式(21)の運動モデルごとの予測誤差共
分散行列Pk a(−)を運動モデルごとの予測誤差評価器
(10)より第2の遅延回路(11)を通して入力し,ゲイ
ン行列算出器(5)より得られるゲイン行列Kkとによ
り,式(19)の運動モデルごとの平滑ベクトル
k a(+)の平滑誤差の評価値である複数個の運動モデル
ごとの平滑誤差共分散行列Pk a(+)を式(16)に従い
算出する。なお、式(13)、式(15)、式(16)より運
動モデルごとの平滑誤差共分散列▲Pa 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 Hk 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 Pk a(-) Through the second delay circuit (11)
Input and then the covariance matrix H of the predicted position error for each motion modelk
Pk a(−) Hk TIs calculated and preset with this
Equation (10) obtained from the observation model Observation noise covariance matrix Rk
More observation vectorz kAnd target position prediction for each motion model
Vector hk 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 ofaCalculate (k) according to equation (26)
Put out. In the reliability calculator (3) for each motion model,
Pulling time tk-1Each of the multiple motion models calculated in
Β in equation (23), which is the reliability fork-1, aThe 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, aTo 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 modelk aBy (-),
Smooth vector according to equation (15) k(+) Used for calculation
Gain matrix KkTo calculate. In addition, from the equation (13)
Prediction error covariance matrix for each rule ▲ Pa K▼ (-) is the exercise model
Since it is a value that does not depend onK
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 motionk-1 k-1(+) Is calculated and
Equation (30) obtained from the motion model reliability calculator (3)
Β of each motion model ofk, aAnd 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 predictionk-1 k-1To
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 KkIs 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 predictionk α aTo
Calculated and sampled one sampling after the current time
Tick tk + 1Prediction 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 Pk 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 eachk a(+) According to equation (16)
calculate. Note that from equation (13), equation (15), and equation (16),
Smooth error covariance sequence for each dynamic model ▲ Pa k▼ (+) is exercise
The value does not depend on the model.
運動モデルごとの予測誤差評価器(10)では,運動モデ
ルごとの平滑誤差評価器(9)より得られる式(22)の
運動モデルごとの平滑誤差共分散行列Pk a(+)を式(5
3)に従い式(20)の平滑誤差共分散行列Pk(+)とみ
なし,あらかじめ設定された運動モデルより得られる式
(8)の駆動雑音共分散行列Qkとにより式(13)に従い
現時点より1サンプリング後のサンプリング時刻tk+1に
対する複数個の運動モデルごとの予測ベクトル
k+1 a(−)の予測誤差の評価である運動モデルごとの予
測誤差共分散行列Pk+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)では,各運動モデルの信頼度算出
器(3)より得られるβk,b,あらかじめ設定された式
(24)の推移確率Pab及びあらかじめ設定された複数個
の運動モデルを構成している式(5)の定数加速度ベク
トルα aより式(52)を使用し現時点より1サンプリン
グ後の予測ベクトル k+1(−)を大きくする項 を算出し,これに運動モデルごとの予測誤差評価器(1
0)より得られる運動モデルごとの平滑誤差共分散行列P
k+1 a(−)を加算し予測誤差の評価である予測誤差共分
散行列Pk+1(−)を式(51)に従い算出する。なお、目
標観測装置(1)では平滑器(6)より入力される目標
位置及び目標速度の平滑値をもとに目標が等速直線運動
を行うとして現時点より1サンプリング後の直交座標の
目標位置の予測値Z K+1(−)=HK+1ΦK K(+)を
算出し、予測誤差評価器(13)より入力される予測誤差
共分散行列PK+1(−)及びあらかじめ設定された観測系
モデルより(10)の観測雑音共分散行列RK+1より正値対
称行列SK+1=HK+1PK+1(−)▲HT K+1▼+RK+1を算出
し、Z K+1を中心にした目標の予測存在範囲を算出し、
この範囲内の目標位置情報のみを追尾目標から得られた
もとして扱う。すなわち、観測ベクトルZ K+1が(Z K+1
−Z K+1(−))TSK -1(Z K+1−Z K+1(−))≦d を満たす時のみ、観測ベクトルZ K+1を追尾目標から得
られたものとして扱う。ここでdはあらかじめ設定され
た正のパラメータである。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 + 1The 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 Pk + 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(-) = HK + 1ΦK K(+)
Prediction error calculated and input from the prediction error evaluator (13)
Covariance matrix PK + 1(-) And preset observation system
From the model, the observation noise covariance matrix R of (10)K + 1More positive pair
Nominal matrix SK + 1= HK + 1PK + 1(-) ▲ HT K + 1▼ + RK + 1Calculate
ThenZ K + 1Calculate 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 + 1But(Z K + 1
−Z K + 1(-))TSK -1(Z K + 1−Z K + 1Only when (-)) ≤ d, the observation vectorZ K + 1Get from the tracking target
Treated as Where d is preset
It is a positive parameter.
以上のようにこの発明によれば通常の目標自動追尾装置
に特別の付加装置を付けることなく安価に目標運動諸元
算出精度を向上させることができる。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.
第1図はこの発明の一実施例の構成を説明する図,第2
図は従来の追尾フイルタを説明する図,第3図は定数加
速度ベクトルを説明する図,第4図は目標予測存在範囲
を説明する図,第5図は観測誤差評価値を説明する図で
ある。 図において,(1)は目標観測装置,(2)は運動モデ
ルごとの観測誤差評価器,(3)は各運動モデルの信頼
度算出器,(4)は第1の遅延回路,(5)はゲイン行
列算出器,(6)は平滑器,(7)は運動モデルごとの
予測値算出器,(8)は第3の遅延回路,(9)は運動
モデルごとの平滑誤差評価器,(10)は運動モデルごと
の予測誤差評価器,(11)は第2の遅延回路,(12)は
第4の遅延回路,(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)
同一次元の複数個の定数ベクトルより構成される異なる
複数個の運動モデルおのおのと上記目標観測装置より入
力される目標位置情報との合致度である観測誤差評価値
を算出する運動モデルごとの観測誤差評価器と、上記運
動モデルごとの観測誤差評価器より上記各運動モデルお
のおのに対する観測誤差評価値を入力し、上記各運動モ
デルおのおのの信頼度を算出する各運動モデルの信頼度
算出器と、上記各運動モデルで共通のゲイン行列を算出
するゲイン行列算出器と、上記目標観測装置より入力さ
れる最新の目標位置情報、上記各運動モデルの信頼度算
出器で算出した各運動モデルおのおのの信頼度及び上記
ゲイン行列算出器で算出したゲイン行列とを用いて上記
各運動モデルの影響を統合して得られる目標位置、速
度、加速度などの目標運動諸元の平滑値を算出する平滑
器と、上記平滑器で算出した目標運動諸元の平滑値を入
力し、上記各運動モデルおのおのに対する目標運動諸元
の1サンプリング後の予測値を算出し、その予測値を上
記運動モデルごとの観測誤差評価器へ出力する運動モデ
ルごとの予測値算出器と、上記ゲイン行列算出器よりゲ
イン行列を入力し、上記各運動モデルおのおのに対する
目標運動諸元の平滑値の誤差を評価する運動モデルごと
の平滑誤差評価器と、上記運動モデルごとの平滑誤差評
価器の評価結果を用いて上記各運動モデルおのおのに対
する目標運動諸元の予測値の誤差を評価し、その予測誤
差の評価値を上記ゲイン行列算出器、上記運動モデルご
との平滑誤差評価器及び上記運動モデルごとの観測誤差
評価器へ出力する運動モデルごとの予測誤差評価器と、
上記各運動モデルの信頼度算出器で算出した各運動モデ
ルの信頼度及び上記運動モデルごとの予測誤差評価器で
算出した各運動モデルおのおのに対する予測誤差の評価
値とを入力し、上記各運動モデルの影響を統合して得ら
れる予測値の誤差を評価する予測誤差評価器とを備えた
ことを特徴とする追尾フィルタ。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.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP22323687A JPH0769419B2 (en) | 1987-09-07 | 1987-09-07 | Tracking filter |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP22323687A JPH0769419B2 (en) | 1987-09-07 | 1987-09-07 | Tracking filter |
Publications (2)
Publication Number | Publication Date |
---|---|
JPS6465476A JPS6465476A (en) | 1989-03-10 |
JPH0769419B2 true JPH0769419B2 (en) | 1995-07-31 |
Family
ID=16794930
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP22323687A Expired - Lifetime JPH0769419B2 (en) | 1987-09-07 | 1987-09-07 | Tracking filter |
Country Status (1)
Country | Link |
---|---|
JP (1) | JPH0769419B2 (en) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB2448599B (en) * | 2007-04-18 | 2012-02-15 | Lfk Lenkflugka Rpersysteme Gmbh | Ascertaining and predicting the motion of a target |
RU2760951C1 (en) * | 2021-03-22 | 2021-12-01 | Федеральное государственное казённое военное образовательное учреждение высшего образования "Военная академия воздушно-космической обороны имени Маршала Советского Союза Г.К. Жукова" Министерства обороны Российской Федерации | Method for tracking a cruise missile when rounding the terrain in various tactical situations |
-
1987
- 1987-09-07 JP JP22323687A patent/JPH0769419B2/en not_active Expired - Lifetime
Also Published As
Publication number | Publication date |
---|---|
JPS6465476A (en) | 1989-03-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Kim | Development of track to track fusion algorithms | |
US8374624B2 (en) | Location measurement method based on predictive filter | |
CN111985093A (en) | Adaptive unscented Kalman filtering state estimation method with noise estimator | |
Pfaff et al. | Robust monte-carlo localization using adaptive likelihood models | |
CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
Vivone et al. | Multiple sensor measurement updates for the extended target tracking random matrix model | |
Dang et al. | Cubature Kalman filter under minimum error entropy with fiducial points for INS/GPS integration | |
Yoon et al. | Object tracking from image sequences using adaptive models in fuzzy particle filter | |
Baum et al. | Extended object tracking based on combined set-theoretic and stochastic fusion | |
JPH0769419B2 (en) | Tracking filter | |
CN115578425A (en) | Dynamic tracking method applied to fry counter and based on unscented Kalman filtering | |
JPH0797136B2 (en) | Multi-target tracking method and apparatus | |
CN114061592A (en) | Adaptive robust AUV navigation method based on multiple models | |
CN109035301B (en) | Group target tracking method based on repulsion model modified random matrix algorithm | |
KR20220027832A (en) | State determination method and device, robot, storage medium and computer program | |
JPH0797135B2 (en) | Multi-target tracking method and apparatus | |
Beutler et al. | Semi-analytic stochastic linearization for range-based pose tracking | |
JPH0769417B2 (en) | Tracking filter | |
JPH0695138B2 (en) | Tracking filter | |
JPH0695137B2 (en) | Tracking filter | |
JPH0797134B2 (en) | Tracking filter | |
Rodríguez et al. | Kalman filters based on multibody models with colored noise | |
JP3302870B2 (en) | Sensor bias error estimation device | |
JP3411485B2 (en) | Target tracking device | |
JPH0690279B2 (en) | Tracking filter |