JP5709651B2 - Tracking device - Google Patents
Tracking device Download PDFInfo
- Publication number
- JP5709651B2 JP5709651B2 JP2011125469A JP2011125469A JP5709651B2 JP 5709651 B2 JP5709651 B2 JP 5709651B2 JP 2011125469 A JP2011125469 A JP 2011125469A JP 2011125469 A JP2011125469 A JP 2011125469A JP 5709651 B2 JP5709651 B2 JP 5709651B2
- Authority
- JP
- Japan
- Prior art keywords
- motion
- value
- kalman filter
- covariance matrix
- linear motion
- 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.)
- Active
Links
- 230000033001 locomotion Effects 0.000 claims description 426
- 239000011159 matrix material Substances 0.000 claims description 111
- 230000007704 transition Effects 0.000 claims description 61
- 238000012545 processing Methods 0.000 claims description 39
- 238000004364 calculation method Methods 0.000 claims description 37
- 238000012937 correction Methods 0.000 claims description 11
- 238000011156 evaluation Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 description 67
- 238000010586 diagram Methods 0.000 description 20
- 238000009499 grossing Methods 0.000 description 16
- 239000013598 vector Substances 0.000 description 14
- 238000012423 maintenance Methods 0.000 description 13
- 230000014509 gene expression Effects 0.000 description 7
- 238000000034 method Methods 0.000 description 7
- 238000009825 accumulation Methods 0.000 description 4
- 238000001514 detection method Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 239000000284 extract Substances 0.000 description 3
- 238000013213 extrapolation Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 102100036848 C-C motif chemokine 20 Human genes 0.000 description 1
- 101000713099 Homo sapiens C-C motif chemokine 20 Proteins 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
この発明は、異なる運動モデルに基づく複数のカルマンフィルタを用いて目標の多様な運動に対処する追尾装置に関するものである。 The present invention relates to a tracking device that copes with various movements of a target by using a plurality of Kalman filters based on different movement models.
追尾装置による目標追尾においては、等速直線運動、蛇行運動、蛇行以外(旋廻等)のマニューバ運動等、多様な運動に対処する必要がある。そこで、等速直線運動モデルに基づくカルマンフィルタ、蛇行運動モデルに基づくカルマンフィルタ、および多重運動モデルに基づくカルマンフィルタを用いて、フィルタバンクによる並列処理を行い、目標の運動状態を判別して、目標に対する適切な平滑値を選択(フィルタ選択)する方法がある(例えば特許文献1参照)。 In target tracking by a tracking device, it is necessary to cope with various motions such as a constant velocity linear motion, a meandering motion, and a maneuver motion other than meandering (turning, etc.). Therefore, the Kalman filter based on the constant-velocity linear motion model, the Kalman filter based on the meandering motion model, and the Kalman filter based on the multiple motion model are used for parallel processing by the filter bank to determine the motion state of the target, and There is a method of selecting (filter selection) a smooth value (see, for example, Patent Document 1).
この特許文献1に開示された追尾装置では、取得した目標に対する観測値に基づいて、等速直線運動カルマンフィルタ処理器で、等速直線運動モデルに基づくカルマンフィルタを用いて、平滑値(第1の平滑値)および予測値(第1の予測値)を算出し、蛇行運動カルマンフィルタ処理器で、蛇行運動モデルに基づくカルマンフィルタを用いて、平滑値(第2の平滑値)および予測値(第2の予測値)を算出している。なお、蛇行運動モデルは、等速直線運動モデルおよび正弦関数運動モデルから構成される。
そして、運動判別処理器で、第1,2の予測値に基づいて目標の運動状態を判別し、収束判定器で、第2の平滑値の収束状況を判定している。なお、運動判別処理器では、第1,2の予測値から残差2次形式をそれぞれ算出するとともに、各残差2次形式から各運動モデルの判定評価関数値を求め、これらの判定評価関数値と所定のしきい値とを比較することによって、目標の運動状態を判別している。
In the tracking device disclosed in Patent Literature 1, a smooth value (first smoothing) is obtained by using a Kalman filter based on a constant velocity linear motion model in a constant velocity linear motion Kalman filter processor based on an observed value for an acquired target. Value) and a predicted value (first predicted value), and a meandering motion Kalman filter processor uses a Kalman filter based on the meandering motion model to calculate a smooth value (second smoothed value) and a predicted value (second predicted value). Value). The meandering motion model is composed of a constant velocity linear motion model and a sine function motion model.
The motion discrimination processor determines the target motion state based on the first and second predicted values, and the convergence determiner determines the convergence state of the second smooth value. The motion discriminating processor calculates the residual quadratic form from the first and second predicted values, obtains a judgment evaluation function value of each motion model from each residual quadratic form, and uses these judgment evaluation functions. The target motion state is determined by comparing the value with a predetermined threshold value.
そして、フィルタ出力選択器で、運動判別処理器が判別した目標の運動状態、および、収束判定器が判定した収束状況に基づいて、第1の平滑値および第2の平滑値のうちの一方を適切な平滑値として選択する。また、第2の平滑値が収束したと判定した後に、蛇行運動モデルを構成する等速直線運動モデルの平滑速度ベクトルの方向が所定値を超えて変化した場合には、第2の平滑値が未収束であると判定し、多重運動モデルに基づくカルマンフィルタを用いて算出した平滑値を適切な平滑値として選択する。 Then, the filter output selector selects one of the first smooth value and the second smooth value based on the target motion state determined by the motion determination processor and the convergence status determined by the convergence determiner. Select an appropriate smooth value. In addition, when it is determined that the second smooth value has converged and the direction of the smooth velocity vector of the constant velocity linear motion model constituting the meandering motion model changes beyond a predetermined value, the second smooth value is It determines with having not converged, and selects the smooth value calculated using the Kalman filter based on a multiple motion model as an appropriate smooth value.
上述したように、特許文献1で開示された方法では、第1,2の予測値から残差2次形式をそれぞれ算出し、各残差2次形式から各運動モデルの判定評価関数値を求め、判定評価関数値と所定のしきい値とを比較することで、目標の運動状態を判別している。また、蛇行運動モデルに基づくカルマンフィルタを用いて算出した第2の平滑値に対する収束判定を行っている。 As described above, in the method disclosed in Patent Document 1, the residual quadratic form is calculated from the first and second predicted values, and the determination evaluation function value of each motion model is obtained from each residual quadratic form. The target motion state is determined by comparing the determination evaluation function value with a predetermined threshold value. Moreover, the convergence determination with respect to the 2nd smooth value calculated using the Kalman filter based on a meandering motion model is performed.
しかしながら、追尾開始直後に観測雑音が大きい場合、蛇行運動モデルでの平滑誤差が小さくても、この蛇行運動モデルを構成する等速直線運動モデルと正弦関数運動モデルでの平滑値は安定せず、目標運動を等速直線運動成分と正弦関数運動成分とに分離するのに時間を要する。このような場合、等速直線運動モデルの平滑速度ベクトルが安定しないため、この平滑値を用いて目標の未来位置予測を行った際に、未来予測位置が大きく変動して誤差が大きくなってしまう。そのため、特許文献1では、目標が蛇行運動をしている場合であっても、蛇行運動モデルを構成する等速直線運動モデルの平滑速度ベクトルが安定するまでは、等速直線運動モデルに基づくカルマンフィルタを選択している。そのため、誤差が大きくなってしまうという課題がある。 However, if the observation noise is large immediately after the start of tracking, even if the smoothing error in the meandering motion model is small, the smooth value in the constant velocity linear motion model and sine function motion model that constitute this meandering motion model is not stable, It takes time to separate the target motion into a constant velocity linear motion component and a sinusoidal motion component. In such a case, since the smooth velocity vector of the constant velocity linear motion model is not stable, when the target future position prediction is performed using this smooth value, the future prediction position largely fluctuates and the error becomes large. . Therefore, in Patent Document 1, even if the target is in a meandering motion, the Kalman filter based on the constant velocity linear motion model is used until the smooth velocity vector of the constant velocity linear motion model constituting the meandering motion model is stabilized. Is selected. Therefore, there is a problem that the error becomes large.
また、判定評価関数値と所定のしきい値とを比較することによる目標の運動状態の判別は処理が複雑であるという課題がある。さらに、等速直線運動モデルに基づくカルマンフィルタ、蛇行運動モデルに基づくカルマンフィルタおよび多重運動モデルに基づくカルマンフィルタを用いたフィルタバンク方式は計算負荷が大きいという課題がある。 Further, there is a problem that the determination of the target motion state by comparing the judgment evaluation function value with a predetermined threshold value is complicated. Furthermore, the filter bank method using the Kalman filter based on the constant velocity linear motion model, the Kalman filter based on the meandering motion model, and the Kalman filter based on the multiple motion model has a problem that the calculation load is large.
この発明は、上記のような課題を解決するためになされたもので、フィルタを簡易に切り替えることで早期に目標に追従可能であり、また、計算量の削減を図ることができる追尾装置を提供することを目的としている。 The present invention has been made to solve the above-described problems, and provides a tracking device that can quickly follow a target by simply switching filters and can reduce the amount of calculation. The purpose is to do.
この発明に係る追尾装置は、等速直線運動モデルに基づくカルマンフィルタを用いて、取得した目標に対する観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する等速直線運動カルマンフィルタ処理手段と、蛇行運動モデルに基づくカルマンフィルタを用いて、観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する蛇行運動カルマンフィルタ処理手段と、多重運動モデルに基づくカルマンフィルタを用いて、観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する多重運動カルマンフィルタ処理手段と、観測値、および、等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるか等速直線運動でないかを判定する等速直線運動判定手段と、観測値、および、各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度を算出する尤度計算手段と、尤度計算手段により算出された各運動モデルに対する尤度および等速直線運動判定手段による判定結果に基づき、当該判定結果に応じて各運動モデルに対する信頼度を算出する信頼度計算手段と、信頼度計算手段により算出された各運動モデルに対する信頼度を用いて、各カルマンフィルタ処理手段によりそれぞれ算出された平滑値の加重平均値を算出する加重平均計算手段とを備えたものである。 The tracking device according to the present invention calculates a predicted value, a prediction error covariance matrix, an observation error covariance matrix, and a smooth value based on the observed values for the acquired target using a Kalman filter based on a constant velocity linear motion model. Fast linear motion Kalman filter processing means, meandering motion Kalman filter processing means for calculating a predicted value, a prediction error covariance matrix, an observation error covariance matrix and a smooth value based on the observed value using a Kalman filter based on a meandering motion model; A multi-motion Kalman filter processing means for calculating a prediction value, a prediction error covariance matrix, an observation error covariance matrix and a smooth value based on an observation value using a Kalman filter based on a multi-motion model, an observation value, and a constant velocity line Prediction value, prediction error covariance matrix and observation error calculated by the motion Kalman filter processing means Based on the variance matrix, and determines a constant speed linear motion determining means for determining target motion state is not or uniform linear motion is uniform linear motion, observations, and predictions are calculated respectively by each Kalman filter processing means Likelihood calculation means for calculating the likelihood for each motion model based on the value, prediction error covariance matrix and observation error covariance matrix, and the likelihood and constant velocity line for each motion model calculated by the likelihood calculation means -out based on the determination result by the motion decision means, a reliability calculating means for calculating a reliability of each motion model according to the determination result, by using the reliability of each motion model calculated by the reliability calculation unit, Weighted average calculating means for calculating a weighted average value of smooth values calculated by each Kalman filter processing means.
この発明によれば、上記のように構成したので、フィルタを簡易に切り替えることで早期に目標に追従可能であり、また、計算量の削減を図ることができる。 According to this invention, since it comprised as mentioned above, it can follow a target at an early stage by changing a filter simply, and can aim at reduction of calculation amount.
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態1.
図1はこの発明の実施の形態1に係る追尾装置の構成を示すブロック図である。
追尾装置は、図1に示すように、観測装置1、追尾維持装置2、等速直線運動カルマンフィルタ処理器(等速直線運動カルマンフィルタ処理手段)3、蛇行運動カルマンフィルタ処理器(蛇行運動カルマンフィルタ処理手段)4、多重運動カルマンフィルタ処理器(多重運動カルマンフィルタ処理手段)5、尤度計算器(尤度計算手段)6、等速直線運動判定器(等速直線運動判定手段)7、信頼度計算器(信頼度計算手段)8、加重平均計算器(加重平均計算手段)9および未来位置予測器10から構成されている。
Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1 FIG.
1 is a block diagram showing the configuration of a tracking device according to Embodiment 1 of the present invention.
As shown in FIG. 1, the tracking device includes an observation device 1, a
観測装置1は、アンテナ、赤外線センサや光学カメラ等を用いて、対象となる目標が放射または反射する信号を検出し、この信号に対して信号処理を行って探知データとして出力するものである。 The observation device 1 uses an antenna, an infrared sensor, an optical camera, or the like to detect a signal that is emitted or reflected from a target target, performs signal processing on the signal, and outputs it as detection data.
追尾維持装置2は、観測装置1により出力された探知データの中から目標に対する観測値として適したデータをゲート判定処理により抽出し、追尾フィルタ(例えばカルマンフィルタ)による運動諸元の推定および予測を行って、運動諸元予測値(以下、観測値と称す)として出力するものである。
The
等速直線運動カルマンフィルタ処理器3(以下、カルマンフィルタ処理器3と称す)は、等速直線運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻(観測時刻)での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。
The constant velocity linear motion Kalman filter processor 3 (hereinafter referred to as the Kalman filter processor 3) uses a Kalman filter based on the constant velocity linear motion model, and each sampling time (observation) based on the observation value output by the
蛇行運動カルマンフィルタ処理器4(以下、カルマンフィルタ処理器4と称す)は、蛇行運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。ここで蛇行運動とは、所定の基準直線(主軸成分)の近傍を移動する運動であって、基準直線を所定の周期で横切るような運動をいう。したがって蛇行運動は基準直線を横切る周波数(蛇行周波数)と基準直線の傾きおよび切片とで表現される。特にカルマンフィルタが基づいている蛇行運動モデルは、等速直線運動モデルと正弦関数運動モデルとから構成されているものを仮定する。
The meandering motion Kalman filter processor 4 (hereinafter referred to as the Kalman filter processor 4) uses a Kalman filter based on the meandering motion model, based on the observation value output by the
多重運動カルマンフィルタ処理器5(以下、カルマンフィルタ処理器5と称す)は、蛇行以外(旋廻等)のマニューバ運動に対処する多重運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。
The multi-motion Kalman filter processor 5 (hereinafter referred to as “Kalman
尤度計算器6は、追尾維持装置2により出力された観測値、および、各カルマンフィルタ処理器3〜5によりそれぞれ算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデル(等速直線運動モデル、蛇行運動モデルおよび多重運動モデル)に対する尤度をそれぞれ算出するものである。
The
等速直線運動判定器7は、追尾装置2により出力された観測値、および、カルマンフィルタ処理器3により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定し、その判定結果を信頼度計算器8に出力するものである。この等速直線運動判定器7の構成については後述する。
The constant-velocity linear motion determiner 7 determines the target based on the observed value output from the
信頼度計算器8は、尤度計算器6により算出された各運動モデルに対する各尤度および等速直線運動判定器7による判定結果に基づいて、各運動モデルに対する信頼度を算出するものである。ここで、信頼度計算器8は、等速直線運動判定器7により目標の運動状態が等速直線運動であると判定された場合には、等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にする。
The
加重平均計算器9は、信頼度計算器8により算出された各運動モデルに対する信頼度に基づいて、各カルマンフィルタ処理器3〜5によりそれぞれ算出された平滑値の加重平均値(統合平滑値)を算出するものである。
未来位置予測器10は、加重平均計算器9により算出された加重平均値に基づいて、外挿法により、Nサンプリング時刻後の目標の未来位置を予測するものである。
The weighted
The
次に、等速直線運動判定器7の構成について説明する。図2はこの発明の実施の形態1における等速直線運動判定器7の構成を示すブロック図である。
等速直線運動判定器7は、図2に示すように、残差2次形式算出器71、時間平均計算器(時間平均計算手段)72、判定器(判定手段)73およびしきい値設定器74から構成されている。
Next, the configuration of the constant velocity
As shown in FIG. 2, the constant velocity
残差2次形式算出器71は、追尾維持装置2により出力された観測値、および、カルマンフィルタ処理器3により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差2次形式を算出するものである。
時間平均計算器72は、残差2次形式算出器71により算出された残差2次形式の所定期間分の平均値を算出するものである。
The residual quadratic form calculator 71 is based on the observation value output by the tracking
The time
判定器73は、時間平均計算器72により算出された残差2次形式の平均値としきい値設定器74により設定されたしきい値とを比較し、目標の運動状態が等速直線運動であるかを判定するものである。ここで、判定器73は、残差2次形式の平均値がしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器8に判定結果を出力する。
しきい値設定器74は、判定器73で残差2次形式の平均値と比較する際に用いるしきい値を設定するものである。なお、しきい値は任意に設定可能である。
The
The threshold
次に、上記のように構成された追尾装置の一連の動作について説明する。図3はこの発明の実施の形態1に係る追尾装置の動作を示すフローチャートである。なお、観測装置1、追尾維持装置2、各カルマンフィルタ処理器3〜5および未来位置予測器10の動作は従来技術(例えば特許文献1)における動作と同様であるため、その説明を簡略化する。
追尾装置の動作では、図3に示すように、まず、観測装置1は、アンテナ、赤外線センサや光学カメラ等を用いて、対象となる目標が放射又は反射する信号を検出し、この信号に対して信号処理を行って探知データとして出力する(ステップST31)。
Next, a series of operations of the tracking device configured as described above will be described. FIG. 3 is a flowchart showing the operation of the tracking device according to Embodiment 1 of the present invention. The operations of the observation device 1, the tracking
In the operation of the tracking device, as shown in FIG. 3, first, the observation device 1 detects a signal radiated or reflected by a target target using an antenna, an infrared sensor, an optical camera, etc. The signal is processed and output as detection data (step ST31).
次いで、追尾維持装置2は、観測装置1により出力された探知データの中から目標に対する観測値として適したデータをゲート判定処理により抽出し、追尾フィルタによる運動諸元の推定および予測を行って観測値として出力する(ステップST32)。
Next, the tracking
次いで、カルマンフィルタ処理器3は、等速直線運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する(ステップST33)。
Next, the
また、カルマンフィルタ処理器4は、蛇行運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する(ステップST34)。
Further, the
また、カルマンフィルタ処理器5は、多重運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する(ステップST35)。
Further, the
次いで、尤度計算器6は、追尾維持装置2により出力された観測値、および、各カルマンフィルタ処理器3〜5によりそれぞれ算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度をそれぞれ算出する(ステップST36)。
Next, the
具体的には、サンプリング時刻をk、各運動モデルをa(=1〜3)、観測値(観測ベクトル)をzk、予測値(予測ベクトル)をx(ハット)k,a(−)、予測誤差共分散行列をPk,a(−)、観測誤差共分散行列をRkとした場合、尤度計算器6は式(1)を用いて尤度νk,aを算出する。
ここで、g(zk;m,A)は、平均m、共分散行列Aの3変量正規分布の観測値zkにおける確率密度関数を示している。また、Haは各カルマンフィルタ処理器3〜5の観測行列、Λkはレーダの観測座標系からフィルタの座標系への変換行列を示している。
Specifically, the sampling time is k, each motion model is a (= 1 to 3), the observed value (observed vector) is z k , and the predicted value (predicted vector) is x (hat) k, a (−), When the prediction error covariance matrix is P k, a (−) and the observation error covariance matrix is R k , the
Here, g (z k ; m, A) represents a probability density function in the observed value z k of the trivariate normal distribution of mean m and covariance matrix A. H a represents an observation matrix of each
次いで、等速直線運動判定器7は、追尾装置2により出力された観測値、および、カルマンフィルタ処理器3により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定する(ステップST37)。
Next, the constant velocity linear
具体的には、カルマンフィルタ処理器3により算出された予測値(予測ベクトル)をx(ハット)k,cv(−)、予測誤差共分散行列をPk,cv(−)とした場合、まず、等速直線運動判定器7内の残差2次形式算出器71は式(2)〜(4)を用いて残差2次形式δk,cvを算出する。
ここで、Hcvはカルマンフィルタ処理器3の観測行列を示している。
Specifically, when the prediction value (prediction vector) calculated by the
Here, H cv indicates an observation matrix of the
そして、時間平均計算器72は、式(5)を用いて、サンプリング時刻kからT時刻前までの残差2次形式の平均値δ(バー)k,cvを算出することで、過去の残差2次形式の時間変化を観測する。
Then, the time
ここで、目標が等速直線運動を行っている場合、等速直線運動モデルに基づく残差2次形式の平均値δ(バー)k,cvは小さくなる。
そこで、判定器73は、時間平均計算器72により算出された残差2次形式の平均値δ(バー)k,cvと、しきい値設定器74により設定された所定のしきい値とを比較する。そして、平均値δ(バー)k,cvがしきい値以下であれば目標の運動状態が等速直線運動であると判定し、平均値δ(バー)k,cvがしきい値よりも大きければ目標の運動状態が等速直線運動ではないと判定し、その判定結果を信頼度計算器8に出力する。
Here, when the target is performing constant velocity linear motion, the average value δ (bar) k, cv of the residual quadratic form based on the constant velocity linear motion model becomes small.
Therefore, the
次いで、信頼度計算器8は、尤度計算器6により算出された各運動モデルに対する尤度および等速直線運動判定器7による判定結果に基づいて、各運動モデルに対する信頼度を算出する(ステップST38)。
具体的には、信頼度計算器8は式(6)を用いて信頼度μk,aを算出する。
ここで、μk,1は等速直線運動モデルに対する信頼度、μk,2は蛇行運動モデルに対する信頼度、μk,3は多重運動モデルに対する信頼度を示している。
Next, the
Specifically, the
Here, μ k, 1 represents the reliability for the constant velocity linear motion model, μ k, 2 represents the reliability for the meandering motion model, and μ k, 3 represents the reliability for the multiple motion model.
なお、信頼度計算器8は、等速直線運動判定器7による判定結果が、目標の運動状態が等速直線運動であることを示している場合には、式(6)を用いて算出した信頼度ではなく式(7)に示す信頼度を用いる。
すなわち、等速直線運動モデルに対する信頼度μk,1を最大値(=1)にし、蛇行運動モデルおよび多重運動モデルに対する信頼度μk,2,μk,3を最小値(=0)にする。
The
That is, the reliability μ k, 1 for the constant velocity linear motion model is set to the maximum value (= 1), and the reliability μ k, 2 , μ k, 3 for the meandering motion model and the multiple motion model is set to the minimum value (= 0). To do.
次いで、加重平均計算器9は、信頼度計算器8により計算された信頼度に基づいて、各カルマンフィルタ処理器3〜5によりそれぞれ算出された平滑値の加重平均値を算出する(ステップST39)。
具体的には、加重平均計算器9は式(8)を用いて加重平均値x(ハット)k(+)を算出する。
Next, the weighted
Specifically, the weighted
次いで、未来位置予測器10は、加重平均計算器9により算出された加重平均値に基づいて、外挿法により、Nサンプリング時刻後の目標の未来位置を算出する(ステップST40)。
Next, the
ここで、実施の形態1に係る追尾装置では、従来の追尾装置のような運動状態の判別によるフィルタ選択ではなく、各運動モデルに対する信頼度に基づいて、各運動モデルによる平滑値の加重平均値を算出している。そのため、簡易なフィルタ切り替えが可能となり、早期に目標の運動に対し追従できる。
すなわち、例えば等速直線運動を行っている目標が急に旋廻した場合、等速直線運動モデルに基づくカルマンフィルタから多重運動モデルに基づくカルマンフィルタにフィルタを切り替える必要がある。しかしながら、従来の追尾装置では、運動状態が確定してからフィルタが切り替わるため、旋回直後では等速直線運動に基づくカルマンフィルタを選択してしまい、追尾誤差が大きくなってしまう。それに対して、実施の形態1に係る追尾装置では、各運動モデルに対する信頼度に基づいた各平滑値の加重平均値を算出することで、ある程度追尾できているフィルタの出力結果を使用することができ、追尾誤差の増大を抑えることができる。
Here, in the tracking device according to the first embodiment, the weighted average value of the smooth values by each motion model is not based on the filter selection based on the determination of the motion state as in the conventional tracking device, but based on the reliability for each motion model. Is calculated. Therefore, simple filter switching is possible, and it is possible to follow the target motion at an early stage.
That is, for example, when a target performing constant velocity linear motion suddenly turns, it is necessary to switch the filter from a Kalman filter based on a constant velocity linear motion model to a Kalman filter based on a multiple motion model. However, in the conventional tracking device, since the filter is switched after the motion state is determined, the Kalman filter based on the constant velocity linear motion is selected immediately after the turn, and the tracking error becomes large. On the other hand, in the tracking device according to the first embodiment, it is possible to use the output result of a filter that can be tracked to some extent by calculating the weighted average value of each smooth value based on the reliability of each motion model. And increase in tracking error can be suppressed.
一方、各運動モデルに対する信頼度を用いる場合、目標が等速直線運動を行っているときには各運動モデル(等速直線運動モデル、蛇行運動モデルおよび多重運動モデル)に対する信頼度に差がなくなり、カルマンフィルタ処理器4およびカルマンフィルタ処理器5により算出された平滑値も加重平均されて、等速直線運動を行っている目標に対して追尾誤差が大きくなってしまう。そこで、実施の形態1に係る追尾装置では、等速直線運動判定器7を設けて、目標の運動状態が等速直線運動であると判定した場合には等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にしている。これにより、等速直線運動を行っている目標に対しても精度よく追尾可能となる。
On the other hand, when the reliability for each motion model is used, there is no difference in the reliability for each motion model (constant linear motion model, meandering motion model and multiple motion model) when the target is performing constant velocity linear motion, and the Kalman filter The smoothed values calculated by the
以上のように、この実施の形態1によれば、運動状態の判別によってフィルタ選択を行うのではなく、各運動モデルに対する信頼度に基づいて、各運動モデルによる平滑値の加重平均値を算出するように構成したので、簡易にフィルタを切り替えることができ、早期に目標に追従可能となる。また、目標の運動状態が等速直線運動の場合には、等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にすることで早期に目標に追従可能となるため、多様な運動に対して早期に追従することができる。 As described above, according to the first embodiment, the weighted average value of the smooth values of each motion model is calculated based on the reliability of each motion model, instead of performing filter selection by determining the motion state. Since it comprised so, a filter can be switched easily and it becomes possible to follow a target at an early stage. If the target motion state is constant-velocity linear motion, the reliability for the constant-velocity linear motion model is maximized, and the reliability for the meandering motion model and multi-motion model is minimized, so that the target can be achieved early. Since it is possible to follow, it is possible to quickly follow various movements.
実施の形態2.
図4はこの発明の実施の形態2に係る追尾装置の構成を示すブロック図であり、図5は蛇行運動カルマンフィルタ処理器4b(以下、カルマンフィルタ処理器4bと称す)の構成を示すブロック図である。
図4に示す実施の形態2に係る追尾装置は、図1に示す実施の形態1に係る追尾装置からカルマンフィルタ処理器3を削除し、カルマンフィルタ処理器4をカルマンフィルタ処理器4bに変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。
4 is a block diagram showing a configuration of a tracking device according to
The tracking device according to
カルマンフィルタ処理器4bは、実施の形態1におけるカルマンフィルタ処理器4による蛇行運動モデル(等速直線運動モデルおよび正弦関数運動モデル)に基づく目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値の算出・出力処理に加えて、これらの各値の等速直線運動成分(主軸成分)も出力する。
このカルマンフィルタ処理器4bは、図5に示すように、複数の周波数別カルマンフィルタ処理器(周波数別カルマンフィルタ処理手段)41−1〜41−N、信頼度計算器42、統合予測器43および統合平滑器44から構成されている。
The
As shown in FIG. 5, the
各周波数別カルマンフィルタ処理器41−1〜41−Nは、それぞれ異なる所定の蛇行周波数ω1〜ωNで蛇行する蛇行運動モデル1〜Nに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。この各周波数別カルマンフィルタ処理器41−1〜41−Nはそれぞれ、遅延器45−1〜45−N、混合器46−1〜46−N、予測器47−1〜47−N、平滑器48−1〜48−Nおよび尤度計算器49−1〜49−Nから構成されている。
以下では、周波数別カルマンフィルタ処理器41−1内の各機能部についてのみ説明を行うが、それ以外の周波数別カルマンフィルタ処理器41−2〜41−N内の各機能部についても同様である。
Each Kalman filter processor 41-1 to 41-N for each frequency uses the Kalman filter based on the meandering motion models 1 to N meandering at different predetermined meandering frequencies ω1 to ωN, and the observed values output from the
In the following, only the function units in the frequency-specific Kalman filter processor 41-1 will be described, but the same applies to the other function units in the frequency-specific Kalman filter processors 41-2 to 41-N.
遅延器45−1は、平滑器48−1により算出された前回サンプリング時刻でのフィルタゲイン、平滑値および平滑誤差共分散行列を1サンプリング期間保持した後、次回サンプリング時刻にて混合器46−1に出力するものである。 The delay unit 45-1 holds the filter gain, the smoothed value, and the smoothing error covariance matrix at the previous sampling time calculated by the smoother 48-1 for one sampling period, and then the mixer 46-1 at the next sampling time. Is output.
混合器46−1は、目標の蛇行運動の蛇行周波数が1次マルコフ過程に基づいて1サンプリング時刻の間に他の蛇行周波数に遷移するとの仮定に従って、各蛇行運動モデル1〜Nに対応した遅延器45−1〜45−Nにより出力された平滑値および平滑誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、混合平滑値および混合平滑誤差共分散行列を算出するものである。
The mixer 46-1 delays corresponding to each of the meandering motion models 1 to N according to the assumption that the meandering frequency of the target meandering motion transitions to another meandering frequency during one sampling time based on the first-order Markov process. The smoothed values and smoothing error covariance matrices output by the units 45-1 to 45-N, and the posterior reliability for each of the meandering motion models 1 to N calculated by the
予測器47−1は、等速直線運動モデルおよび正弦関数運動モデルの統合により表される蛇行運動モデルに従って、混合器46−1により算出された混合平滑値および混合平滑誤差共分散行列に基づいて、予測値および予測誤差共分散行列を算出するものである。 The predictor 47-1 is based on the mixed smoothing value and the mixed smoothing error covariance matrix calculated by the mixer 46-1 according to the meandering motion model represented by the integration of the constant velocity linear motion model and the sine function motion model. The prediction value and the prediction error covariance matrix are calculated.
平滑器48−1は、追尾維持装置2により出力された観測値、および、予測器47−1により算出された予測値および予測誤差共分散行列に基づいて、フィルタゲイン、平滑値および平滑誤差共分散行列を算出するものである。
Based on the observation value output from the
尤度計算器49−1は、追尾維持装置2により出力された観測値、および、予測器47−1により算出された予測値および予測誤差共分散行列に基づいて、蛇行運動モデル1に対する尤度を算出するものである。
The likelihood calculator 49-1 is a likelihood for the meandering motion model 1 based on the observation value output by the tracking
また、信頼度計算器42は、各蛇行運動モデル1〜Nに対応した尤度計算器49−1〜49−Nにより算出された尤度および前回サンプリング時刻での事後信頼度に基づいて、各蛇行運動モデル1〜Nに対する事後信頼度を算出するものである。
In addition, the
統合予測器43は、各蛇行運動モデル1〜Nに対応した予測器47−1〜47−Nにより算出された予測値および予測誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出するものである。この演算結果は、カルマンフィルタ4bにより算出された予測値および予測誤差共分散行列として尤度計算器6に出力され、また、この演算結果の等速直線運動成分(主軸成分)は、尤度計算器6および等速直線運動判定器7に出力される。
The
統合平滑器44は、各蛇行運動モデル1〜Nに対応した平滑器48−1〜48−Nにより算出された平滑値および平滑誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出するものである。この演算結果は、カルマンフィルタ4bにより算出された平滑値および平滑誤差共分散行列として加重平均計算器9に出力され、また、この演算結果の等速直線運動成分(主軸成分)は、加重平均計算器9に出力される。
The integrated smoother 44 includes smooth values and smooth error covariance matrices calculated by the smoothers 48-1 to 48 -N corresponding to the meandering motion models 1 to N, and the
次に、上記のように構成された追尾装置の一連の動作について説明する。なお、図6に示す実施の形態2に係る追尾装置の動作は、図3に示す実施の形態1に係る追尾装観測装置1のステップST33,34をステップST61,62に変更したものであり、それ以外の動作は同様であるため、このステップST61,62の動作についてのみ説明を行う。また以下では、蛇行運動モデルや観測方程式は従来技術(例えば特許文献1)と同様であり、関係する式のみを用いて説明を行う。
なお、追尾維持装置2により出力されたサンプリング時刻kでの観測値の状態ベクトルxkは式(9)のように定義される。
ここで、xkはサンプリング時刻kでの状態ベクトル、x1,kは正弦関数運動成分の位置、x1,kは正弦関数運動成分の速度、x2,kは等速直線運動成分の位置、x2,kは等速直線運動成分の速度を示している。
Next, a series of operations of the tracking device configured as described above will be described. In addition, operation | movement of the tracking apparatus which concerns on
Note that the state vector x k of the observed value at the sampling time k output by the
Here, x k is a state vector at the sampling time k, x 1, k is the position of the sine function motion component, x 1, k is the speed of the sine function motion component, and x 2, k is the position of the constant velocity linear motion component. , X 2, k indicate the velocity of the constant velocity linear motion component.
ステップST61において、カルマンフィルタ処理器4bは、蛇行運動モデルに基づくカルマンフィルタ処理を行う。具体的には、まず、各周波数別カルマンフィルタ処理器41−1〜41−Nは、それぞれ異なる所定の蛇行周波数ω1〜ωNで蛇行する蛇行運動モデル1〜Nに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づいて、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する。そして、信頼度計算器42は、各蛇行運動モデル1〜Nに対応した各尤度計算器49−1〜49−Nにより算出された尤度および前回サンプリング時刻での各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、各蛇行運動モデル1〜Nに対する事後信頼度を算出する。
In step ST61, the
そして、統合予測器43は、各蛇行運動モデル1〜Nに対応した予測器47−1〜47−Nにより算出された予測値および予測誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出する。
具体的には、予測値をx(ハット)k,a(−)、予測誤差共分散行列をP(ハット)k,a(−)、事前信頼度をβ(ハット)k,a(−)とした場合、統合予測器43は式(10)および(11)を用いて統合予測ベクトルx(ハット)k(−)および統合予測誤差共分散行列Pk(−)を算出する。
この統合予測器43により算出された統合予測値および統合予測誤差共分散行列は、カルマンフィルタ4bによる予測値および予測誤差共分散行列として尤度計算器6に出力される。
The
Specifically, the predicted value is x (hat) k, a (−), the prediction error covariance matrix is P (hat) k, a (−), and the prior reliability is β (hat) k, a (−). In this case, the
The integrated prediction value and the integrated prediction error covariance matrix calculated by the
そして、統合平滑器44は、各蛇行運動モデル1〜Nに対応した平滑器48−1〜48−Nにより算出された平滑値および平滑誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出する。
具体的には、平滑値をx(ハット)k,a(+)、平滑誤差共分散行列をP(ハット)k,a(+)とした場合、統合平滑器44は式(12)および(13)を用いて統合平滑ベクトルx(ハット)k(+)および統合平滑誤差共分散行列Pk(+)を算出する。
The integrated smoother 44 is calculated by the smoothing value and smoothing error covariance matrix calculated by the smoothers 48-1 to 48 -N corresponding to the meandering motion models 1 to N, and the
Specifically, when the smooth value is x (hat) k, a (+) and the smoothing error covariance matrix is P (hat) k, a (+), the integrated smoother 44 uses the equations (12) and (12). 13) is used to calculate the integrated smooth vector x (hat) k (+) and the integrated smooth error covariance matrix P k (+).
なお、カルマンフィルタ処理器4bの平滑値としては、正弦関数運動モデルおよび等速直線運動モデルの統合平滑値として構成されるため、式(14)を用いて両ベクトルを加算したx(ハット)int,a(+)が実際の目標に対する平滑位置および平滑速度ベクトルとなる。
この統合平滑器44により算出された統合平滑値および統合平滑誤差共分散行列は、カルマンフィルタ4bによる平滑値および平滑誤差共分散行列として加重平均計算器9に出力される。
Note that the smooth value of the
The integrated smooth value and integrated smooth error covariance matrix calculated by the integrated smoother 44 are output to the weighted
次いで、ステップST62において、カルマンフィルタ処理器4b(統合予測器43および統合平滑器44)は、蛇行運動モデルを構成する等速直線運動モデルに基づくカルマンフィルタ処理の結果を出力する。
ここで、主軸成分である等速直線運動成分の予測ベクトルx(ハット)2_cv,k(−)および平滑ベクトルx(ハット)2_cv,k(+)は式(15)および式(16)のように示される。
Next, in step ST62, the
Here, the prediction vector x (hat) 2_cv, k (−) and the smooth vector x (hat) 2_cv, k (+) of the constant velocity linear motion component, which is the main axis component, are expressed by the following equations (15) and (16). Shown in
以上のように、この実施の形態2によれば、カルマンフィルタ処理器4bにて、蛇行運動モデルを構成する等速直線運動モデルに基づくカルマンフィルタ処理の結果をカルマンフィルタ処理器3の代わりに用いるように構成したので、カルマンフィルタ処理器3は不要となり、このカルマンフィルタ処理器3による計算負荷を削減できる。
As described above, according to the second embodiment, the
実施の形態3.
図7はこの発明の実施の形態3に係る追尾装置の構成を示すブロック図であり、図8は等速直線運動判定器7bの構成を示すブロック図であり、図9は等速直線運動判定を示す図である。
図7に示す実施の形態3に係る追尾装置は、図4に示す実施の形態2における等速直線運動判定器7を等速直線運動判定器7bに変更し、カルマンフィルタ処理器4bからの蛇行運動モデルの出力と、その等速直線運動成分(主軸成分)の出力とが等速直線運動判定器7bへ入力するように変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。
7 is a block diagram showing a configuration of a tracking device according to
The tracking device according to the third embodiment shown in FIG. 7 changes the constant velocity
ここで、図9(a)に示すように、目標の運動状態が蛇行運動の場合では、蛇行運動モデルの平滑値(等速直線運動成分(主軸成分)と正弦関数運動成分とを統合した統合平滑値)と、蛇行運動モデルの等速直線運動成分の平滑値との差は大きい。一方、図9(b)に示すように、目標の運動状態が等速直線運動の場合では、その差は小さい。
そこで、等速直線運動判定器7bによる等速直線運動判定では、上記特性を利用して、上記差分値が所定のしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器8に判定結果を出力する。
Here, as shown in FIG. 9A, when the target motion state is a meandering motion, the smooth value (constant linear motion component (main axis component)) and the sinusoidal motion component of the meandering motion model are integrated. The difference between the smooth value) and the smooth value of the constant velocity linear motion component of the meandering motion model is large. On the other hand, as shown in FIG. 9B, when the target motion state is a constant velocity linear motion, the difference is small.
Therefore, in the constant velocity linear motion determination by the constant velocity
図8に示す等速直線運動判定器7bは、図2に示す実施の形態2における等速直線運動判定器7の残差2次形式算出器71を差分計算器(差分計算手段)71bに変更し、時間平均計算器72を削除したものである。
The constant velocity
差分計算器71bは、カルマンフィルタ処理器4bにより算出された目標に対する平滑値と、この平滑値の等速直線運動成分(主軸成分)との差分値を算出するものである。
The difference calculator 71b calculates a difference value between the smooth value for the target calculated by the
また、判定器73は、差分計算器71bにより算出された差分値としきい値設定器74により設定されたしきい値とを比較し、目標の運動状態が等速直線運動であるかを判定する。ここで、判定器73は、差分値がしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器8に判定結果を出力する。
また、しきい値設定器74は、判定器73で差分値と比較する際に用いるしきい値を設定する。なお、しきい値は任意に設定可能である。
Further, the
The
以上のように、この実施の形態3によれば、等速直線運動判定器7bにて、カルマンフィルタ処理器4bからの蛇行運動モデルの平滑値とその等速直線成分(主軸成分)との差分値がしきい値以下の場合に、目標の運動状態が等速直線運動であると見なすように構成したので、運動状態が等速直線運動である目標に対して、より精度のよい追尾が可能となる。
As described above, according to the third embodiment, the constant velocity
実施の形態4.
図10はこの発明の実施の形態4に係る追尾装置の構成を示すブロック図であり、図11は蛇行運動カルマンフィルタ処理器4c(以下、カルマンフィルタ処理器4cと称す)の構成を示すブロック図である。
図10に示す実施の形態4に係る追尾装置は、図4に示す実施の形態2に係る追尾装置のカルマンフィルタ処理器4bをカルマンフィルタ処理器4cに変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。
10 is a block diagram showing the configuration of a tracking device according to
The tracking device according to
実施の形態2におけるカルマンフィルタ処理器4bでは、等速直線運動成分(主軸成分)を統合予測器43および統合平滑器44から出力している。この場合、等速直線運動モデルおよび正弦関数運動モデルから構成される蛇行運動モデルに基づいて処理を行うことになる。ここで、追尾開始直後に観測雑音が大きい場合、両モデルを合わせた統合平滑値に係る平滑誤差は小さくても、モデルごとの平滑値は安定しない。そのため、追尾初期において、等速直線運動成分を取り出した際に追尾誤差が大きくなってしまう。
In the
そこで、図10,11に示すように、実施の形態4におけるカルマンフィルタ処理器4cでは、蛇行運動モデルとして用意されている複数の蛇行周波数ω1〜ωNの中で最も高い周波数(図11では、周波数別カルマンフィルタ処理器41−Nの蛇行周波数)の等速直線運動成分(主軸成分)のみを取り出す。すなわち、高周波の蛇行周波数の振幅は低周波の蛇行周波数の振幅よりも小さいため、観測雑音に埋もれると、統合平滑としてはほとんど主軸成分しか残らないことを利用する。
Therefore, as shown in FIGS. 10 and 11, in the
このカルマンフィルタ処理器4cでは、蛇行周波数ω1〜ωNで蛇行する蛇行運動モデル1〜Nに基づいて、蛇行運動モデル1〜Nごとの予測値および予測誤差共分散行列を算出し、各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出する。この統合予測値および統合予測誤差共分散行列は、カルマンフィルタ4cによる予測値および予測誤差共分散行列として尤度計算器6に出力される。同様に、蛇行運動モデル1〜Nごとの平滑値、平滑誤差共分散行列および事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出する。この統合平滑値および統合平滑誤差共分散行列は、カルマンフィルタ4cによる平滑値および平滑誤差共分散行列として加重平均計算器9に出力される。
さらに、カルマンフィルタ処理器4cは、蛇行運動モデル1〜Nが蛇行周波数ごとのモデルであるとした場合、最も高い蛇行周波数である蛇行運動モデルの等速直線運動成分(主軸成分)を取り出す。この取り出された予測値および予測誤差共分散行列の等速直線運動成分は、尤度計算器6および等速直線運動判定器7に出力され、平滑値および統合平滑誤差共分散行列の等速直線運動成分は、加重平均計算器9に出力される。
The
Furthermore, when the meandering motion models 1 to N are models for each meandering frequency, the
以上のように、この実施の形態4によれば、カルマンフィルタ処理器4cにて、最も高い蛇行周波数である蛇行運動モデルの等速直線運動成分をカルマンフィルタ処理器3の代わりに用いるように構成したので、追尾初期において等速直線運動成分が十分に安定しない場合であっても、高周波である蛇行運動モデルの等速直線運動成分を取り出すことで、運動状態が等速直線運動である目標に対して、より精度のよい追尾が可能となる。
As described above, according to the fourth embodiment, the
実施の形態5.
実施の形態1〜4では、各運動モデルに対する信頼度に基づいて各平滑値の加重平均を算出するものについて示したが、実施の形態5では、非特許文献1で用いられているトラックスコア(track score)なる指標値を用いて、各サンプリング時刻での各運動状態(等速直線運動状態、蛇行運動状態または多重運動状態)に対する評価(重み付け)を行い、これに基づいて各平滑値の加重平均を算出するものについて示す。
[非特許文献1]
J.Arnold、S.Shaw,H. Pasternack “Efficient target tracking using dynamic programming”,IEEE Trans.Aerospace and Electronic System,Vol.29,No.1,pp.4566(1993)
In the first to fourth embodiments, the weighted average of each smoothed value is calculated based on the reliability for each motion model. However, in the fifth embodiment, the track score (used in Non-Patent Document 1) Using the index value of track score), each motion state (constant linear motion state, meandering motion state or multiple motion state) at each sampling time is evaluated (weighted), and the smoothing value is weighted based on this. It shows about what calculates an average.
[Non-Patent Document 1]
J. et al. Arnold, S.M. Shaw, H .; Pasternack “Efficient target tracking using dynamic programming”, IEEE Trans. Aerospace and Electronic System, Vol. 29, no. 1, pp. 4566 (1993)
図12はこの発明の実施の形態5に係る追尾装置の構成を示すブロック図であり、図13は重み係数設定器11の構成を示すブロック図である。
図12に示す実施の形態5に係る追尾装置は、図4に示す実施の形態2に係る追尾装置から等速直線運動判定器7を削除し、尤度計算器6および信頼度計算器8を重み係数設定器(重み係数設定手段)11に変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。なお、実施の形態5に係る追尾装置は、図1に示す実施の形態1の追尾装置に対しても同様に適応可能である。
FIG. 12 is a block diagram showing the configuration of the tracking device according to the fifth embodiment of the present invention, and FIG. 13 is a block diagram showing the configuration of the weighting
The tracking device according to the fifth embodiment shown in FIG. 12 deletes the constant velocity
重み係数設定器11は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分(図1に示す追尾装置に実施の形態5の追尾装置の構成を適用した場合にはカルマンフィルタ処理器3により算出された各値)に基づいて、各運動モデルに対する重み係数を算出するものである。この重み係数設定器11は、図13に示すように、尤度計算器111、推移確率設定器(推移確率設定手段)112、トラックスコア算出器(トラックスコア算出手段)113、重み係数算出器(重み係数算出手段)114、残差2次形式算出器(残差2次形式算出手段)115、推移確率補正器(推移確率補正手段)116および遅延器117から構成されている。
The weighting
尤度計算器111は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、今回サンプリング時刻での各運動モデルに対する尤度をそれぞれ計算するものである。
The
推移確率設定器112は、目標が前回サンプリング時刻において所定の運動状態(各推移元運動状態)であった場合に、この推移元運動状態から今回サンプリング時刻において各運動状態へ推移する推移確率を設定するものである。なおこの際、推移確率設定器112は、遅延器117により出力された補正値に基づいて、推移確率を補正する。
The
トラックスコア算出器113は、尤度計算器111により算出された各運動モデルに対する尤度および推移確率設定器112により設定された推移確率に基づいて、今回サンプリング時刻での各運動状態に対する評価値(トラックスコア)を算出するものである。このトラックスコア算出器113では、まず、今回サンプリング時刻での各運動状態に対して、前回サンプリング時刻での運動状態に対するトラックスコアと、推移確率設定器112により設定された前回サンプリング時刻での運動状態から今回サンプリング時刻での運動状態への推移確率との加算値が最大となる前回サンプリング時刻での運動状態(推移元運動状態)を選択する。そして、この推移元運動状態に対するトラックスコア、推移元運動状態からの推移確率および尤度計算器111により算出された各運動モデルに対する尤度に基づいて、今回サンプリング時刻での各運動状態に対するトラックスコアを算出する。
Based on the likelihood for each motion model calculated by the
重み係数算出器114は、トラックスコア算出器113により算出された各運動状態に対するトラックスコアに基づいて、各運動モデルに対する重み係数を算出するものである。この重み係数算出器114により算出された重み係数は信頼度として加重平均計算器9に出力される。
The
残差2次形式算出器115は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する残差2次形式をそれぞれ算出するものである。
The residual
推移確率補正器116は、残差2次形式算出器115により算出された各運動モデルに対する残差2次形式に基づいて、推移確率に対する補正値を算出するものである。
遅延器117は、推移確率補正器116により算出された前回サンプリング時刻での補正値を1サンプリング期間保持して、今回サンプリング時刻にて推移確率設定器112に出力するものである。
The
The
次に、上記のように構成された追尾装置の一連の動作について説明する。なお、図14に示す実施の形態5に係る追尾装置の動作は、図6に示す実施の形態2に係る追尾装観測装置1のステップST36〜38をステップST111に変更したものであり、それ以外の動作は同様であるため、このステップST111についてのみ説明を行う。 Next, a series of operations of the tracking device configured as described above will be described. The operation of the tracking device according to the fifth embodiment shown in FIG. 14 is obtained by changing steps ST36 to 38 of the tracking device observation device 1 according to the second embodiment shown in FIG. 6 to step ST111. Since this operation is the same, only step ST111 will be described.
ステップST111において、重み係数設定器11は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する重み係数を算出する。
In step ST111, the weight
具体的には、まず、観測値をzk、予測値をx(ハット)k(−)、予測誤差共分散行列をPk(−)、観測誤差共分散行列をRkとした場合、重み係数設定器11内の尤度計算器111は、式(1)を用いて各運動モデルに対する尤度をそれぞれ算出する。
Specifically, first, when the observation value is z k , the prediction value is x (hat) k (−), the prediction error covariance matrix is P k (−), and the observation error covariance matrix is R k , the weight The
そして、トラックスコア算出器113は、各運動モデルに基づくカルマンフィルタごとに、式(17)を用いて、今回サンプリング時刻kでの各運動状態に対するトラックスコアを算出する。
ここで、Xk−1,bは前回サンプリング時刻k−1での運動状態に対するトラックスコアであり、P(θk,a|θk,b)は前回サンプリング時刻k−1での運動状態から今回サンプリング時刻kでの運動状態への推移確率、νk,aは各運動モデルに対する尤度である。
Then, the
Here, X k−1, b is a track score for the motion state at the previous sampling time k−1, and P (θ k, a | θ k, b ) is from the motion state at the previous sampling time k−1. The transition probability to the motion state at this sampling time k, ν k, a is the likelihood for each motion model.
式(17)に示すトラックスコアの算出式は、従来技術(非特許文献1)と基本的な動作は同じであり、前回サンプリング時刻k−1での推移元となる各運動状態の中から、今回サンプリング時刻kでの運動状態に対するトラックスコアを最大にする運動状態を選択し、これを推移元運動状態とする。そして、推移元運動状態に対するトラックスコアと、推移元運動状態から今回サンプリング時刻kでの運動状態への推移確率と、今回サンプリング時刻kでの運動モデルに対する尤度とから今回サンプリング時刻kでの運動状態に対するトラックスコアを求める。 The calculation formula of the track score shown in Expression (17) has the same basic operation as that of the conventional technique (Non-Patent Document 1), and from each motion state that becomes a transition source at the previous sampling time k−1, This time, the exercise state that maximizes the track score for the exercise state at the sampling time k is selected, and this is set as the transition source exercise state. The motion at the current sampling time k is calculated from the track score for the transitional motion state, the transition probability from the transitional motion state to the motion state at the current sampling time k, and the likelihood for the motion model at the current sampling time k. Find the track score for the state.
図15は式(17)の第1項目が示すサンプリングごとのトラックスコアの積みあがりを示したものである。
図15に示すように、サンプリング時刻0の場合には、各運動状態に対するトラックスコアはすべて0である。また、サンプリング時刻0からサンプリング時刻1となるとき、等速直線運動状態から等速直線運動状態への推移確率は3、等速直線運動状態から蛇行運動状態への推移確率は1、等速直線運動状態から多重運動状態への推移確率は0である。なおこの推移確率は推移確率設定器112により設定される。
そして、例えば、サンプリング時刻1での等速直線運動状態に対するトラックスコアを最大にするサンプリング時刻0での運動状態は、推移確率が最も高い等速直線運動状態となる。そして、サンプリング時刻1での等速直線運動状態に対するトラックスコアは、サンプリング時刻0での等速直線運動状態に対するトラックスコアである0と、サンプリング時刻0での等速直線運動状態からサンプリング時刻1での等速直線運動状態への推移確率である3との加算値である3となる。
同様にして、サンプリング時刻1での蛇行運動状態および多重運動状態に対するトラックスコアも算出される。
FIG. 15 shows the accumulation of track scores for each sampling indicated by the first item of equation (17).
As shown in FIG. 15, when the sampling time is 0, the track scores for each motion state are all 0. When the sampling time is 0 to the sampling time 1, the transition probability from the constant velocity linear motion state to the constant velocity linear motion state is 3, the transition probability from the constant velocity linear motion state to the meandering motion state is 1, and the constant velocity straight line The transition probability from the motion state to the multiple motion state is zero. This transition probability is set by the transition
For example, the motion state at the
Similarly, track scores for the meandering motion state and the multiple motion state at the sampling time 1 are also calculated.
また、例えば、サンプリング時刻2での蛇行運動状態に対してでは、推移元運動状態としてサンプリング時刻1の等速直線運動状態を選択する。そして、サンプリング時刻2での蛇行運動状態に対するトラックスコアは、サンプリング時刻1での等速直線運動状態に対するトラックスコアである3と、サンプリング時刻1での等速直線運動状態からサンプリング時刻2での蛇行運動状態への推移確率である2との加算値である5となる。
同様にして、サンプリング時刻2での等速直線運動状態および多重運動状態に対するトラックスコアも算出される。
なお図15は、式(17)の第1項目の推移確率を考慮したトラックスコアの積みあがりを示したものであるので、実際には第1項目の値と第2項目の尤度との加算値が各運動状態に対するトラックスコアとなる。
For example, for the meandering motion state at
Similarly, track scores for the constant velocity linear motion state and the multiple motion state at the
Note that FIG. 15 shows the accumulation of the track score in consideration of the transition probability of the first item in Expression (17), so in actuality, the value of the first item and the likelihood of the second item are added. The value is the track score for each exercise state.
そして、重み係数算出器114は、トラックスコア算出器113により算出された各運動状態に対するトラックスコアに基づいて、式(18)を用いて各運動モデルに対する重み係数を算出する。
この重み係数算出器114により算出された重み係数は信頼度として加重平均計算器9に出力される。
Then, the
The weighting coefficient calculated by the
一方、残差2次形式算出器115は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する残差2次形式をそれぞれ算出する。そして、推移確率補正器116は、残差2次形式算出器115により算出された各運動モデルに対する残差2次形式に基づいて、推移確率に対する補正値を算出し、遅延器117で1サンプリング期間保持された後、推移確率設定器112に出力される。
On the other hand, the residual
例えば、目標が等速直線運動を継続して行っている場合には、等速直線運動状態に対するトラックスコアはサンプリング時刻ごとに積みあがっていき、等速直線運動モデルに対する重み係数が大きくなっていく。その後、目標が急に旋回した場合、等速直線運動状態に対するトラックスコアは大きく積みあがっているため、多重運動モデルに対する重み係数と比べて等速直線運動モデルに対する重み係数が大きくなってしまう。
一方、多重運動状態に対するトラックスコアが大きくならない場合、等速直線運動モデルに基づくカルマンフィルタの残差2次形式は多重運動モデルに基づくカルマンフィルタの残差2次形式と比べると値が非常に大きくなる。そこで、それぞれのカルマンフィルタの残差2次形式の値の差を利用し、多重運動状態のトラックスコアが大きく積み上がるように推移確率に対する補正を行う。これによって、多重運動モデルに対する重み係数を大きくすることができ、目標が急旋回する場合であっても追従できることが可能となる。
For example, if the target continues constant linear motion, the track score for the constant linear motion state increases at each sampling time, and the weighting factor for the constant linear motion model increases. . Thereafter, when the target turns suddenly, the track score for the constant-velocity linear motion state is greatly increased, so that the weighting factor for the constant-velocity linear motion model becomes larger than the weighting factor for the multiple motion model.
On the other hand, when the track score for the multiple motion state does not increase, the Kalman filter residual quadratic form based on the constant-velocity linear motion model has a much larger value than the Kalman filter residual quadratic form based on the multi-motion model. Therefore, using the difference between the values of the residual quadratic form of each Kalman filter, the transition probability is corrected so that the track score in the multiple motion state is greatly accumulated. As a result, the weighting coefficient for the multiple motion model can be increased, and it is possible to follow even when the target turns sharply.
以上のように、この実施の形態5によれば、運動状態の判別ではなく、トラックスコアを用いて、前回サンプリング時刻での推移元の運動状態の中から今回サンプリング時刻での運動状態に対するトラックスコアを最大にする運動状態を選択し、この推移元運動状態に対するトラックスコア、推移元運動状態からの推移確率および今回サンプリング時刻での運動モデルに対する尤度に基づいて、今回サンプリング時刻での運動状態に対するトラックスコアを算出し、各運動モデルに対する重み係数を算出するように構成したので、簡易にフィルタを切り替えることができ、早期に目標に追従可能となる。また、目標の運動状態が等速直線運動であり、各運動モデルに対する信頼度に差がなくなる場合であっても、等速直線運動状態に対するトラックスコアの積みあがりによって等速直線モデルに対する重み係数を大きくすることができる。そのため、早期に目標に追従することができる。さらに、目標が等速直線運動から急に旋廻した場合であっても、推移確率を補正することで多重運動状態に対するトラックスコアを積み上げることができ、多重運動モデルに対する重み係数を大きくすることができ、多様な運動に対しても早期に追従することができる。 As described above, according to the fifth embodiment, the track score for the motion state at the current sampling time is selected from the transitional motion states at the previous sampling time using the track score instead of the determination of the motion state. Is selected based on the track score for the transition source motion state, the transition probability from the transition source motion state, and the likelihood for the motion model at the current sampling time. Since the track score is calculated and the weight coefficient for each motion model is calculated, the filter can be easily switched, and the target can be followed quickly. In addition, even if the target motion state is constant velocity linear motion and there is no difference in the reliability for each motion model, the weight coefficient for the constant velocity linear model is set by the accumulation of the track score for the constant velocity linear motion state. Can be bigger. Therefore, it is possible to follow the target early. Furthermore, even if the target turns suddenly from a uniform linear motion, the track score for multiple motion states can be accumulated by correcting the transition probability, and the weight coefficient for the multiple motion model can be increased. , Can follow a variety of exercises early.
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。 In the present invention, within the scope of the invention, any combination of the embodiments, or any modification of any component in each embodiment, or omission of any component in each embodiment is possible. .
1 観測装置、2 追尾維持装置、3 等速直線運動カルマンフィルタ処理器(等速直線運動カルマンフィルタ処理手段)、4,4b,4c 蛇行運動カルマンフィルタ処理器(蛇行運動カルマンフィルタ処理手段)、5 多重運動カルマンフィルタ処理器(多重運動カルマンフィルタ処理手段)、6 尤度計算器(尤度計算手段)、7,7b 等速直線運動判定器(等速直線運動判定手段)、8 信頼度計算器(信頼度計算手段)、9 加重平均計算器(加重平均計算手段)、10 未来位置予測器、11 重み係数設定器、41−1〜41−N 周波数別カルマンフィルタ処理器(周波数別カルマンフィルタ処理手段)、42 信頼度計算器、43 統合予測器、44 統合平滑器、45−1〜45−N 遅延器、46−1〜46−N 混合器、47−1〜47−N 予測器、48−1〜48−N 平滑器、49−1〜49−N 尤度計算器、71 残差2次形式算出器(残差2次形式算出手段)、71b 差分計算器(差分計算手段)、72 時間平均計算器(時間平均計算手段)、73 判定器(判定手段)、74 しきい値設定器、111 尤度計算器、112 推移確率設定器(推移確率設定手段)、113 トラックスコア算出器(トラックスコア算出手段)、114 重み係数算出器(重み係数算出手段)、115 残差2次形式算出器(残差2次形式算出手段)、116 推移確率補正器(推移確率補正手段)、117 遅延器。 DESCRIPTION OF SYMBOLS 1 Observation apparatus, 2 tracking maintenance apparatus, 3 constant velocity linear motion Kalman filter processor (constant velocity linear motion Kalman filter processing means), 4, 4b, 4c meandering motion Kalman filter processing device (meandering motion Kalman filter processing means), 5 multiple motion Kalman filter processing (Multi-motion Kalman filter processing means), 6 likelihood calculator (likelihood calculation means), 7, 7b constant velocity linear motion determiner (constant velocity linear motion determination means), 8 reliability calculator (reliability calculation means) , 9 Weighted average calculator (weighted average calculating means), 10 Future position predictor, 11 Weight coefficient setter, 41-1 to 41-N Frequency-specific Kalman filter processor (frequency-specific Kalman filter processing means), 42 Reliability calculator 43 integrated predictor, 44 integrated smoother, 45-1 to 45-N delay unit, 46-1 to 46-N mixer, 47 -1 to 47-N predictor, 48-1 to 48-N smoother, 49-1 to 49-N likelihood calculator, 71 residual quadratic form calculator (residual quadratic form calculating means), 71b Difference calculator (difference calculation means), 72 time average calculator (time average calculation means), 73 decision unit (determination means), 74 threshold value setter, 111 likelihood calculator, 112 transition probability setter (transition probability) Setting means), 113 track score calculator (track score calculation means), 114 weight coefficient calculator (weight coefficient calculation means), 115 residual quadratic form calculator (residual secondary form calculating means), 116 transition probability correction (Transition probability correction means), 117 delay device.
Claims (7)
蛇行運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する蛇行運動カルマンフィルタ処理手段と、
多重運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する多重運動カルマンフィルタ処理手段と、
前記観測値、および、前記等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、前記目標の運動状態が等速直線運動であるか等速直線運動でないかを判定する等速直線運動判定手段と、
前記観測値、および、前記各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度を算出する尤度計算手段と、
前記尤度計算手段により算出された各運動モデルに対する尤度および前記等速直線運動判定手段による判定結果に基づき、当該判定結果に応じて各運動モデルに対する信頼度を算出する信頼度計算手段と、
前記信頼度計算手段により算出された各運動モデルに対する信頼度を用いて、前記各カルマンフィルタ処理手段によりそれぞれ算出された平滑値の加重平均値を算出する加重平均計算手段と
を備えた追尾装置。 Constant-velocity linear motion Kalman filter processing means for calculating a predicted value, a prediction error covariance matrix, an observation error covariance matrix and a smooth value based on an observed value for an acquired target using a Kalman filter based on a constant velocity linear motion model;
A meandering motion Kalman filter processing means for calculating a predicted value, a prediction error covariance matrix, an observation error covariance matrix and a smooth value based on the observed value using a Kalman filter based on a meandering motion model;
A multi-motion Kalman filter processing means for calculating a predicted value, a prediction error covariance matrix, an observation error covariance matrix and a smooth value based on the observed value using a Kalman filter based on a multi-motion model;
Based on the observed value and the predicted value, the prediction error covariance matrix and the observation error covariance matrix calculated by the constant velocity linear motion Kalman filter processing means, whether the target motion state is constant velocity linear motion, etc. Constant velocity linear motion judging means for judging whether it is a fast linear motion,
Likelihood calculation means for calculating a likelihood for each motion model based on the observed value and the predicted value, prediction error covariance matrix and observation error covariance matrix calculated by each Kalman filter processing means;
Based-out in accordance with the determination result likelihood and the constant velocity linear motion determining means for each motion model calculated by the likelihood calculating means, the reliability calculation means calculates the reliability of each motion model according to the determination result When,
A tracking device comprising weighted average calculation means for calculating a weighted average value of smooth values calculated by the Kalman filter processing means using the reliability of each motion model calculated by the reliability calculation means.
前記観測値、および、前記等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差2次形式を算出する残差2次形式算出手段と、
前記残差2次形式算出手段により算出された残差2次形式の所定観測分の平均値を算出する時間平均計算手段と、
前記時間平均計算手段により算出された平均値が所定のしきい値以下の場合に、前記目標の運動状態が等速直線運動であると判定する判定手段とを有し、
前記信頼度計算手段は、前記等速直線運動判定手段により前記目標の運動状態が等速直線運動であると判定された場合に、前記等速直線運動モデルに対する信頼度を最大値にし、前記蛇行運動モデルおよび前記多重運動モデルに対する信頼度を最小値にする
ことを特徴とする請求項1記載の追尾装置。 The constant velocity linear motion determination means includes:
Residual quadratic form calculation for calculating a residual quadratic form based on the observed value and the predicted value, prediction error covariance matrix and observation error covariance matrix calculated by the constant velocity linear motion Kalman filter processing means Means,
A time average calculating means for calculating an average value of predetermined observations in the residual secondary form calculated by the residual secondary form calculating means;
Determination means for determining that the target motion state is constant-velocity linear motion when the average value calculated by the time average calculation means is equal to or less than a predetermined threshold;
The reliability calculation means maximizes the reliability of the constant velocity linear motion model when the target motion state is determined to be constant velocity linear motion by the constant velocity linear motion determination means, and the meandering The tracking device according to claim 1, wherein the reliability of the motion model and the multiple motion model is minimized.
前記蛇行運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値のそれぞれの等速直線運動成分を用いる
ことを特徴とする請求項1または請求項2記載の追尾装置。 Instead of the constant velocity linear motion Kalman filter processing means,
3. The constant velocity linear motion component of each of a predicted value, a prediction error covariance matrix, an observation error covariance matrix and a smooth value calculated by the meandering Kalman filter processing means is used. Tracking device.
前記蛇行運動カルマンフィルタ処理手段により算出された平滑値および当該平滑値の等速直線成分の差分値を算出する差分計算手段と、
前記差分計算手段により算出された差分値が所定のしきい値以下の場合に、前記目標の運動状態が等速直線運動であると判定する判定手段とを有し、
前記信頼度計算手段は、前記等速直線運動判定手段により前記目標の運動状態が等速直線運動であると判定された場合に、前記等速直線運動モデルに対する信頼度を最大値にし、前記蛇行運動モデルおよび前記多重運動モデルに対する信頼度を最小値にする
ことを特徴とする請求項1または請求項3記載の追尾装置。 The constant velocity linear motion determination means includes:
A difference calculation means for calculating a difference between a smooth value calculated by the meandering Kalman filter processing means and a constant-velocity linear component of the smooth value;
Determining means for determining that the movement state of the target is constant-velocity linear movement when the difference value calculated by the difference calculation means is equal to or less than a predetermined threshold value;
The reliability calculation means maximizes the reliability of the constant velocity linear motion model when the target motion state is determined to be constant velocity linear motion by the constant velocity linear motion determination means, and the meandering 4. The tracking device according to claim 1, wherein the reliability of the motion model and the multiple motion model is minimized.
前記等速直線運動カルマンフィルタ処理手段に代えて、
前記各周波数別カルマンフィルタ処理手段のうち、最も高い蛇行周波数に対応した周波数別カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値のそれぞれの等速直線運動成分を用いる
ことを特徴とする請求項1から請求項4のうちのいずれか1項記載の追尾装置。 The meandering motion Kalman filter processing means uses a Kalman filter based on a motion model meandering at different meandering frequencies, and based on the observation values, predicts a prediction value, a prediction error covariance matrix, an observation error covariance matrix, and a smooth value. A plurality of frequency-specific Kalman filter processing means for calculating,
Instead of the constant velocity linear motion Kalman filter processing means,
The constant-velocity linear motions of the predicted value, the prediction error covariance matrix, the observation error covariance matrix, and the smooth value calculated by the frequency-specific Kalman filter processing means corresponding to the highest meander frequency among the frequency-specific Kalman filter processing means The tracking device according to any one of claims 1 to 4, wherein a component is used.
前記目標が前回観測時刻において各運動状態であった場合に、当該各運動状態から今回観測時刻において各運動状態へ推移する推移確率を設定する推移確率設定手段と、
今回観測時刻での各運動状態に対して、前回観測時刻での運動状態に対する評価値であるトラックスコアと、前記推移確率設定手段により設定された当該前回観測時刻での運動状態から今回観測時刻での運動状態への推移確率との加算値が最大となる前回観測時刻での運動状態を選択し、当該選択した運動状態に対するトラックスコア、当該選択した運動状態からの推移確率および前記各運動モデルに対する尤度に基づいて、当該今回観測時刻での各運動状態に対するトラックスコアを算出するトラックスコア算出手段と、
前記トラックスコア算出手段により算出された各運動状態に対するトラックスコアに基づいて各運動モデルに対する重み係数を算出し、各運動モデルに対する信頼度として前記加重平均計算手段に出力する重み係数算出手段と
を有する重み係数設定手段を備えた
ことを特徴とする請求項1から請求項5のうちのいずれか1項記載の追尾装置。 Instead of the uniform linear motion determining means and the reliability calculation unit,
A transition probability setting means for setting a transition probability of transition from each motion state to each motion state at the current observation time when the target is each motion state at the previous observation time;
For each movement state at the current observation time, the track score, which is an evaluation value for the movement state at the previous observation time, and the movement state at the previous observation time set by the transition probability setting means, Select the motion state at the previous observation time at which the added value with the transition probability to the motion state is the maximum, track score for the selected motion state, transition probability from the selected motion state, and each motion model Track score calculating means for calculating a track score for each motion state at the current observation time based on the likelihood;
A weighting factor calculating unit that calculates a weighting factor for each motion model based on the track score for each motion state calculated by the track score calculating unit, and outputs the weighting factor to the weighted average calculating unit as a reliability for each motion model. 6. The tracking device according to claim 1, further comprising a weighting coefficient setting unit.
前記観測値、および、前記各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差2次形式を算出する残差2次形式算出手段と、
前記残差2次形式算出手段により算出された残差2次形式に基づいて、推移確率に対する補正値を算出する推移確率補正手段とを有し、
前記推移確率設定手段は、前記推移確率補正手段により算出された補正値に基づいて、前記推移確率を補正する
ことを特徴とする請求項6記載の追尾装置。 The weighting factor setting means includes
Residual quadratic form calculating means for calculating a residual quadratic form based on the observed value and the predicted value, prediction error covariance matrix and observation error covariance matrix respectively calculated by the Kalman filter processing means; ,
Transition probability correction means for calculating a correction value for the transition probability based on the residual quadratic form calculated by the residual secondary form calculation means;
The tracking apparatus according to claim 6, wherein the transition probability setting unit corrects the transition probability based on a correction value calculated by the transition probability correction unit.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011125469A JP5709651B2 (en) | 2011-06-03 | 2011-06-03 | Tracking device |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011125469A JP5709651B2 (en) | 2011-06-03 | 2011-06-03 | Tracking device |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2012251904A JP2012251904A (en) | 2012-12-20 |
JP5709651B2 true JP5709651B2 (en) | 2015-04-30 |
Family
ID=47524845
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011125469A Active JP5709651B2 (en) | 2011-06-03 | 2011-06-03 | Tracking device |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5709651B2 (en) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111710192A (en) * | 2020-06-18 | 2020-09-25 | 武汉理工大学 | Ship bridge collision accident early warning and recording method, device and system |
WO2023181354A1 (en) * | 2022-03-25 | 2023-09-28 | 日本電気株式会社 | Information processing device, calculation method, and storage medium |
WO2023181355A1 (en) * | 2022-03-25 | 2023-09-28 | 日本電気株式会社 | Information processing device, detection method, and storage medium |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6795453B2 (en) * | 2017-05-22 | 2020-12-02 | 日本電信電話株式会社 | Measuring device and measuring method |
CN110007298B (en) * | 2018-01-04 | 2023-04-07 | 武汉科技大学 | Target advanced prediction tracking method |
WO2020148894A1 (en) * | 2019-01-18 | 2020-07-23 | 三菱電機株式会社 | Motion state assessment device |
CN110261859B (en) * | 2019-06-25 | 2023-10-31 | 北京中科海讯数字科技股份有限公司 | Underwater maneuvering static alternating state target tracking method |
CN111537950B (en) * | 2020-04-14 | 2023-04-21 | 哈尔滨工业大学 | Satellite position prediction tracking method based on position fingerprint and two-step polynomial fitting |
CN112134544B (en) * | 2020-09-25 | 2024-03-19 | 郑州佛光发电设备股份有限公司 | Filtering method, device, equipment and computer readable storage medium |
CN113865659B (en) * | 2021-09-17 | 2024-03-22 | 湖南威铭能源科技有限公司 | High-precision household ultrasonic water meter anti-interference filtering method and equipment |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6710743B2 (en) * | 2001-05-04 | 2004-03-23 | Lockheed Martin Corporation | System and method for central association and tracking in passive coherent location applications |
JP4348535B2 (en) * | 2004-03-24 | 2009-10-21 | 三菱電機株式会社 | Target tracking device |
JP2007183112A (en) * | 2006-01-04 | 2007-07-19 | Mitsubishi Electric Corp | Target tracking device |
JP5424662B2 (en) * | 2009-01-30 | 2014-02-26 | 三菱電機株式会社 | Target tracking device |
-
2011
- 2011-06-03 JP JP2011125469A patent/JP5709651B2/en active Active
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111710192A (en) * | 2020-06-18 | 2020-09-25 | 武汉理工大学 | Ship bridge collision accident early warning and recording method, device and system |
WO2023181354A1 (en) * | 2022-03-25 | 2023-09-28 | 日本電気株式会社 | Information processing device, calculation method, and storage medium |
WO2023181355A1 (en) * | 2022-03-25 | 2023-09-28 | 日本電気株式会社 | Information processing device, detection method, and storage medium |
Also Published As
Publication number | Publication date |
---|---|
JP2012251904A (en) | 2012-12-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5709651B2 (en) | Tracking device | |
US9646389B2 (en) | Systems and methods for image scanning | |
JP4348535B2 (en) | Target tracking device | |
US9911191B2 (en) | State estimation apparatus, state estimation method, and integrated circuit with calculation of likelihood data and estimation of posterior probability distribution data | |
US11394870B2 (en) | Main subject determining apparatus, image capturing apparatus, main subject determining method, and storage medium | |
JP6541538B2 (en) | Target tracking device and radar device | |
CN104820997A (en) | Target tracking method based on block sparse expression and HSV feature fusion | |
US8654187B2 (en) | Work recognition system, work recognition device, and work recognition method | |
WO2020262070A1 (en) | Tracking device | |
CN106846373A (en) | A kind of mutual occlusion handling method of video object for merging target appearance model and game theory | |
CN115019128A (en) | Image generation model training method, image generation method and related device | |
CN111523619B (en) | Target existence probability calculation method and device, electronic equipment and storage medium | |
JP5452307B2 (en) | Tracking device | |
JP2012181710A (en) | Object tracking device, method and program | |
JP5078669B2 (en) | Target detection apparatus, target detection method, and target detection program | |
CN110414130B (en) | Variable structure multi-model maneuvering target tracking method based on error-fuzzy decomposition | |
JP3926602B2 (en) | Target tracking apparatus and method | |
JP2003149328A (en) | Target correlation device of radar | |
JP5675273B2 (en) | Target detection apparatus and target detection method | |
RU2778355C1 (en) | Device and method for prediction autofocus for an object | |
US20240013403A1 (en) | Information processing apparatus, control method thereof, and storage medium | |
US20220343511A1 (en) | Information processing apparatus, information processing method, and storage medium | |
JP2006279890A (en) | Method and device for tracking correlation | |
JP2011028375A (en) | Image processor, image processing method and image processing program | |
Zhang et al. | A method of adaptive learning rate tracking for embedded device based correlation surface evaluation |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20131126 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20140609 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20140624 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20140806 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20150203 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150303 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5709651 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |