JP5230132B2 - Target tracking device and target tracking method - Google Patents
Target tracking device and target tracking method Download PDFInfo
- Publication number
- JP5230132B2 JP5230132B2 JP2007178317A JP2007178317A JP5230132B2 JP 5230132 B2 JP5230132 B2 JP 5230132B2 JP 2007178317 A JP2007178317 A JP 2007178317A JP 2007178317 A JP2007178317 A JP 2007178317A JP 5230132 B2 JP5230132 B2 JP 5230132B2
- Authority
- JP
- Japan
- Prior art keywords
- covariance matrix
- calculating
- vector
- matrix
- calculated
- 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
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
本発明は目標追尾装置及び目標追尾方法に関し、特に、レーダなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動体の位置を観測し、その観測結果に基づいて移動体の真の位置、速度などの運動諸元を高精度に推定する追尾装置および追尾方法に関するものである。 The present invention relates to a target tracking device and a target tracking method, and in particular, using an observation device including a sensor such as a radar, observes the position of a moving body such as an aircraft, a ship, or a vehicle, and based on the observation result, The present invention relates to a tracking device and a tracking method for accurately estimating motion specifications such as a true position and speed.
一般に、観測装置による目標観測結果を用いた目標追尾処理では、カルマンフィルタを使用する(例えば、特許文献1参照)。 In general, a Kalman filter is used in the target tracking process using the target observation result by the observation apparatus (see, for example, Patent Document 1).
従来技術においては、3次元空間用の等速直線運動モデルや等加速度運動モデルを使用し、目標運動のあいまいさを表す駆動雑音ベクトルをx,y,zの各方向の加速度項で定義している。以下、等速直線運動モデルを使用した場合を例に、従来の目標追尾方法を説明する。 In the prior art, a constant velocity linear motion model or a constant acceleration motion model for a three-dimensional space is used, and a drive noise vector representing the ambiguity of the target motion is defined by acceleration terms in the x, y, and z directions. Yes. Hereinafter, the conventional target tracking method will be described by taking as an example the case of using a constant velocity linear motion model.
カルマンフィルタを使用した追尾処理では、設定する座標系を、図8のようにセンサを原点、東方向をx軸の正、北方向をy軸の正、水平面(x−y面)上方向をz軸の正に取った北基準直交座標とする。つぎに、目標の運動モデルを以下のように仮定する。ただし、サンプリング時刻(以下、単に時刻)をt(k)で表す。 In the tracking process using the Kalman filter, the coordinate system to be set is as shown in FIG. 8, where the sensor is the origin, the east direction is positive on the x axis, the north direction is positive on the y axis, and the horizontal plane (xy plane) is z The north reference Cartesian coordinates taken on the positive axis. Next, the target motion model is assumed as follows. However, the sampling time (hereinafter simply referred to as time) is represented by t (k).
但し、
x(k):時刻t(k)における目標運動諸元の真値を表す状態変数ベクトル
Φ:時刻t(k−1)から時刻t(k)への状態ベクトルの既知の推移行列
w(k):北基準直交座標の時刻t(k)における駆動雑音ベクトル
Γ:駆動雑音ベクトルの変換行列
とする。
However,
x (k): state variable vector representing the true value of the target motion specification at time t (k) Φ: known transition matrix of the state vector from time t (k−1) to time t (k) w (k ): Drive noise vector at time t (k) in the north reference Cartesian coordinates Γ: Drive noise vector conversion matrix.
従来の一般的な追尾装置では、目標の基本的な運動をx−y−zの3次元空間における北基準直交座標の等速直線運動と仮定して上記運動モデルを記述する。たとえば、目標の位置と速度を推定する場合、状態ベクトルx(k)、状態推移行列Φを次式のように定義する。 In the conventional general tracking device, the motion model is described on the assumption that the basic motion of the target is constant velocity linear motion of north reference orthogonal coordinates in a three-dimensional space of xyz. For example, when estimating the target position and velocity, the state vector x (k) and the state transition matrix Φ are defined as follows:
但し、Tはベクトル及び行列の転置、ドットは時間微分を表す。また、In×n及び0n×nはそれぞれn行n列の単位行列及び零行列であり、τは一定のサンプリング周期を表す。 However, T represents transposition of a vector and a matrix, and a dot represents time differentiation. Further, I n × n and 0 n × n are a unit matrix and a zero matrix of n rows and n columns, respectively, and τ represents a constant sampling period.
さらに、w(k)は現実の目標運動を北基準直交座標の等速直線で近似したことによるx(k)の誤差を表現するために設けられたx,y,zの各方向の加速度項の雑音成分で構成されており、平均零ベクトルで駆動雑音共分散行列Q(k)の3変量白色正規分布に従う駆動雑音ベクトルであると仮定する。 Further, w (k) is an acceleration term in each direction of x, y, and z provided to express an error of x (k) by approximating the actual target motion with a constant velocity straight line of north reference orthogonal coordinates. It is assumed that the driving noise vector has a mean zero vector and follows the trivariate white normal distribution of the driving noise covariance matrix Q (k).
つぎに、状態ベクトル及び観測雑音ベクトルを使用して目標位置の観測ベクトルを表現する際の、観測装置の観測モデルを以下のように仮定する。 Next, the observation model of the observation apparatus when expressing the observation vector at the target position using the state vector and the observation noise vector is assumed as follows.
但し、
z(k):時刻t(k)における目標位置の観測ベクトル
H:観測行列
v(k):時刻t(k)における北基準直交座標による観測雑音ベクトル
とする。
However,
z (k): observation vector of target position at time t (k) H: observation matrix v (k): observation noise vector based on north reference orthogonal coordinates at time t (k).
たとえば、観測装置によりx−y−zの3次元空間の北基準直交座標系における目標位置の観測結果が得られるとする。このとき、観測ベクトルz(k)、観測行列Hを次式のように定義する。 For example, it is assumed that the observation result of the target position in the north reference orthogonal coordinate system in the xyz three-dimensional space is obtained by the observation device. At this time, the observation vector z (k) and the observation matrix H are defined as follows.
また、v(k)は観測装置の観測誤差を表すための雑音成分であり、平均零ベクトルで観測雑音共分散行列B(k)の3変量白色正規分布に従う北基準直交座標による観測雑音ベクトルであると仮定する。 Further, v (k) is a noise component for representing the observation error of the observation device, and is an observation noise vector by the north reference orthogonal coordinate according to the trivariate white normal distribution of the observation noise covariance matrix B (k) with a mean zero vector. Assume that there is.
上記のような運動モデル、観測モデルの仮定のもと、カルマンフィルタは以下に示すアルゴリズムで、状態ベクトルの予測を行う。時刻t(k−1)までの観測結果に基づいて時刻t(k)の状態ベクトルを推定した予測値ベクトルをxハット(k|k−1)と表し、また、xハット(k|k−1)の誤差共分散行列を表す予測誤差共分散行列をP(k|k−1)と表す。 Under the assumption of the motion model and the observation model as described above, the Kalman filter predicts the state vector by the following algorithm. A predicted value vector obtained by estimating the state vector at time t (k) based on the observation results up to time t (k−1) is represented as x hat (k | k−1), and x hat (k | k− The prediction error covariance matrix representing the error covariance matrix of 1) is represented as P (k | k−1).
なお、駆動雑音共分散行列Q(k)はパラメータとして与える。 The drive noise covariance matrix Q (k) is given as a parameter.
つぎに、ゲートの設定を以下のように仮定する。ゲートとは、図9に示すように、追尾目標が存在すると予測される範囲であり、ゲート内の観測ベクトルを追尾に使用するものである。ゲートの中心は、1サンプリング前までの観測ベクトルの情報に基づく、時刻tkの観測ベクトルz(k)の推定値である。これを考慮し、上記式(5)に、予測ベクトルの定義及び観測雑音ベクトルは平均零ベクトルの白色雑音であるとの仮定を使用することで、以下の式が得られる。 Next, the gate setting is assumed as follows. As shown in FIG. 9, the gate is a range where a tracking target is predicted to exist, and the observation vector in the gate is used for tracking. The center of the gate, based on the information of the observation vector to one sampling before, an observation estimate of vector z (k) at time t k. Considering this, the following equation is obtained by using the assumption that the definition of the prediction vector and the observation noise vector are white noise of the mean zero vector in the above equation (5).
但し、
S(k):残差ベクトルz(k)−z(k−1)の共分散行列
d:ゲートの大きさを決めるパラメータ
とする。
However,
S (k): Covariance matrix of residual vector z (k) -z (k-1) d: A parameter for determining the gate size.
式(10)がゲートの中心、式(11)がゲートの広がり、式(12)がゲートの判定を示す。ここで、式(7),(4),(9)及び(11)より、次式(13)を得る。 Expression (10) indicates the center of the gate, Expression (11) indicates the spread of the gate, and Expression (12) indicates determination of the gate. Here, the following equation (13) is obtained from the equations (7), (4), (9), and (11).
式(13)の右辺の第2項では、ゲートの広がりを判定する要素の一つが駆動雑音共分散行列のサンプリング間隔の影響であるということを示している。 The second term on the right side of Equation (13) indicates that one of the factors for determining the gate spread is the influence of the sampling interval of the drive noise covariance matrix.
ゲート内に追尾目標の観測ベクトルが得られると、つぎに、状態ベクトルの更新を行う。時刻t(k)までの観測結果に基づいて時刻t(k)の状態ベクトルを推定した平滑ベクトルをxハット(k|k)と表し、xハット(k|k)の誤差共分散行列を表す平滑誤差共分散行列をP(k|k)と表す。さらに、K(k)はフィルタのゲイン行列であり、上記式(2)〜(4),(6),(7)の設定を行った場合、6行3列の行列である。 Once the tracking target observation vector is obtained in the gate, the state vector is then updated. A smooth vector obtained by estimating the state vector at time t (k) based on the observation results up to time t (k) is represented as x hat (k | k), and represents an error covariance matrix of x hat (k | k). The smooth error covariance matrix is represented as P (k | k). Further, K (k) is a gain matrix of the filter, and is a 6 × 3 matrix when the above formulas (2) to (4), (6), and (7) are set.
なお、xハット(k|k)の初期値xハット(0|0)と、P(k|k)の初期値P(0|0)が別途算出され、与えられるものとする。また、駆動雑音共分散行列Q(k)、観測雑音共分散行列B(k)はパラメータとして与える。 Note that an initial value x hat (0 | 0) of x hat (k | k) and an initial value P (0 | 0) of P (k | k) are separately calculated and given. The driving noise covariance matrix Q (k) and the observation noise covariance matrix B (k) are given as parameters.
カルマンフィルタは、式(8),(15)により、平均的な偏りのない不偏推定値を算出するとともに、式(14)でゲイン行列を算出することにより、平均2乗誤差を最小化する最適なフィルタとして知られている。 The Kalman filter calculates an unbiased estimated value without an average bias using the equations (8) and (15) and calculates a gain matrix using the equation (14), thereby minimizing the mean square error. Known as a filter.
尚、従来技術による一般的な追尾装置の構成を図10に示す。
図10において、1は平滑値ベクトル記憶手段に、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段、2は平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段、3は前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段、4は前記予測値ベクトルと観測装置からの観測値ベクトルを入力し、ゲートを設定するゲート範囲設定手段、5は前記ゲートと予測値ベクトルとゲイン行列設定手段よりゲイン行列を入力して、平滑値ベクトルを算出する平滑手段である。
6は目標速度の分散値を設定する駆動雑音分散設定手段、7は前記駆動雑音分散より駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段、8は平滑誤差共分散行列記憶手段に、平滑値ベクトルの誤差を推定した平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段、9は平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段、10は前時刻の平滑誤差共分散行列と駆動雑音共分散行列より現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段、11は前記予測誤差共分散行列とゲイン行列設定手段よりのゲイン行列を入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段である。
また、12は観測雑音共分散行列が与えられる観測雑音共分散行列算出手段、13は予測誤差共分散行列の値と観測雑音共分散行列より残差共分散行列を算出する残差共分散行列算出手段、14は前記残差共分散行列の値と予測誤差共分散行列の値よりゲイン行列が設定されるゲイン行列設定手段である。
FIG. 10 shows the configuration of a general tracking device according to the prior art.
In FIG. 10, 1 is a smooth value vector initial value setting means for setting an initial value of a smooth value vector of a state vector composed of motion parameters such as a target position and speed in a smooth value vector storage means, and 2 is a smooth value. Smoothing value vector storage means for storing the value vector value, 3 is a prediction means for calculating a prediction value vector at the current time from the smoothing value vector at the previous time, and 4 is input with the prediction value vector and the observation value vector from the observation device Gate range setting means 5 for setting a gate is a smoothing means for calculating a smooth value vector by inputting a gain matrix from the gate, predicted value vector, and gain matrix setting means.
6 is a driving noise variance setting means for setting a variance value of the target speed, 7 is a driving noise covariance matrix calculating means for calculating a driving noise covariance matrix from the driving noise variance, and 8 is a smoothing error covariance matrix storage means. Smoothing error covariance matrix initial value setting means for setting an initial value of a smoothing error covariance matrix that estimates the error of the smoothing vector, 9 is a smoothing error covariance matrix storage means for storing the value of the smoothing error covariance matrix, 10 Is a prediction error covariance matrix calculating means for calculating a prediction error covariance matrix at the current time from the smoothing error covariance matrix and the driving noise covariance matrix at the previous time, and 11 is a prediction error covariance matrix and gain matrix setting means from the above Smoothing error covariance matrix calculating means for inputting a gain matrix and calculating a smoothing error covariance matrix.
Further, 12 is an observation noise covariance matrix calculation means to which an observation noise covariance matrix is given, and 13 is a residual covariance matrix calculation for calculating a residual covariance matrix from the prediction error covariance matrix value and the observation noise covariance matrix.
上述したように、カルマンフィルタは、適度なサンプリング間隔で目標の運動モデルと観測装置の観測モデルが正しく表現されていれば、最適なゲートの広がりの設定が行われ、追尾性能を発揮することができる。しかしながら、式(13)の右辺の第2項では、ゲートの広がりを決める要素の一つが駆動雑音共分散行列のサンプリング間隔の影響であるということを示したように、ここでの値(τ4/4)×Q(k−1)は、目標速度、速度ベクトルの向きに関係無く、サンプリング間隔τが大きくなれば、ゲートが極端に拡大していくことを表している。 As described above, if the target motion model and the observation model of the observation device are correctly expressed at an appropriate sampling interval, the Kalman filter can set the optimal gate spread and exhibit the tracking performance. . However, in the second term on the right-hand side of Equation (13), the value (τ 4) shown here indicates that one of the factors that determine the gate spread is the influence of the sampling interval of the drive noise covariance matrix. / 4) × Q (k−1) represents that the gate expands extremely as the sampling interval τ increases, regardless of the target speed and the direction of the velocity vector.
このため、たとえば等速直線運動を行っていた目標が等速円運動で90度旋回したのち、再び等速直線運動を行う場合でも、x,y,z方向おのおのにおいて、サンプリング間隔が大きくなるほど、ゲートは拡大していく。また、単純に目標が等速直線運動を行っている時でも、サンプリング間隔が大きくなると、非現実的な大きさにゲートが拡大してしまう。ゲートが不必要に拡大してしまうと、不要信号を受信する確率が高まり、目標の追尾を外す可能性が高くなるという問題点がある。 For this reason, for example, even when a target that has been performing constant-velocity linear motion turns 90 degrees in constant-speed circular motion and then performs constant-velocity linear motion again, as the sampling interval increases in each of the x, y, and z directions, The gate will expand. Even when the target is simply performing a uniform linear motion, if the sampling interval increases, the gate expands to an unrealistic size. If the gate expands unnecessarily, there is a problem that the probability of receiving an unnecessary signal increases, and the possibility of losing the target tracking increases.
さらに、目標の加速度ベクトルを駆動雑音として設定するため、速度成分を引用しない一定の大きさのゲート設定となり、目標速度が大きい時にその速度に適したゲートの範囲が割り当てられず、高速移動目標に対してゲート内に目標を捕捉できず、追尾を外してしまう危険性を含むという問題点があった。 Furthermore, because the target acceleration vector is set as the drive noise, the gate setting is a constant size that does not quote the speed component, and when the target speed is large, the gate range suitable for that speed is not allocated, making it a fast moving target. On the other hand, there was a problem that the target could not be captured in the gate, and there was a risk of losing tracking.
本発明は、かかる問題点を解決するためになされたものであり、サンプリング間隔が大きい時にゲートが不必要に拡大するのを防ぎ、また、目標速度の大小に合わせたゲート設定により、高速移動目標に対しても適切なゲートが設定される目標追尾装置および目標追尾方法を得ることを目的としている。 The present invention has been made to solve such a problem, and prevents the gate from unnecessarily expanding when the sampling interval is large, and the high-speed moving target by setting the gate according to the target speed. It is also an object of the present invention to obtain a target tracking device and a target tracking method in which an appropriate gate is set.
この発明は、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段とを備えた目標追尾装置である。 According to the present invention, smooth value vector initial value setting means for setting an initial value of a smooth value vector of a state vector composed of motion parameters such as a target position and speed, an initial value of the smooth value vector, and calculation are calculated. Smooth value vector storage means for storing the value of the smooth value vector, prediction means for calculating a predicted value vector at the current time from the smooth value vector at the previous time stored in the smooth value vector storage means, and the smooth value vector Speed norm calculation means for calculating a speed norm based on the smooth value vector stored in the storage means, and input the calculated speed norm and the smooth value vector stored in the smooth value vector storage means And a driving noise vector coordinate transformation matrix calculating means for calculating a driving noise vector coordinate transformation matrix and a target speed variance as a driving noise variance. Driving noise variance setting means, and the driving noise covariance matrix calculated by the set driving noise variance and the driving noise vector coordinate transformation matrix computing means calculated by the driving noise vector coordinate transformation matrix computing means A covariance matrix calculating means; a smoothing error covariance matrix initial value setting means for setting an initial value of a smoothing error covariance matrix for estimating an error of a smoothing vector; an initial value of the smoothing error covariance matrix and a calculated smoothing Calculated by the smoothing error covariance matrix storage means for storing the value of the error covariance matrix, the smoothing error covariance matrix at the previous time stored in the smoothing error covariance matrix storage means, and the driving noise covariance matrix calculation means A prediction error covariance matrix calculating means for calculating a prediction error covariance matrix at the current time from the driving noise covariance matrix, and the calculated prediction error covariance matrix Residual covariance matrix calculating means for calculating a residual covariance matrix based on the matrix value and the observation noise covariance matrix obtained from the observation device; the calculated residual covariance matrix value and the prediction error Gain matrix setting means for setting a gain matrix based on the value of the prediction error covariance matrix calculated by the variance matrix calculating means, the predicted value vector calculated by the prediction means, and an observed value vector obtained from the observation device The gate range setting means for setting the gate, the set gate, the predicted value vector calculated by the prediction means, and the gain matrix set by the gain matrix setting means Smoothing means for calculating a smooth value vector, and the prediction error covariance matrix calculated by the prediction error covariance matrix calculating means and the gain matrix setting means. The target tracking device includes a smoothing error covariance matrix calculating unit that inputs a set gain matrix and calculates a smoothing error covariance matrix.
この発明は、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段とを備えた目標追尾装置であるので、サンプリング間隔が大きい時にゲートが不必要に拡大するのを防ぐとともに、高速移動目標に対しても速度に応じた最適なゲート設定をすることにより正確にゲート内に目標を捕捉することができる。 According to the present invention, smooth value vector initial value setting means for setting an initial value of a smooth value vector of a state vector composed of motion parameters such as a target position and speed, an initial value of the smooth value vector, and calculation are calculated. Smooth value vector storage means for storing the value of the smooth value vector, prediction means for calculating a predicted value vector at the current time from the smooth value vector at the previous time stored in the smooth value vector storage means, and the smooth value vector Speed norm calculation means for calculating a speed norm based on the smooth value vector stored in the storage means, and input the calculated speed norm and the smooth value vector stored in the smooth value vector storage means And a driving noise vector coordinate transformation matrix calculating means for calculating a driving noise vector coordinate transformation matrix and a target speed variance as a driving noise variance. Driving noise variance setting means, and the driving noise covariance matrix calculated by the set driving noise variance and the driving noise vector coordinate transformation matrix computing means calculated by the driving noise vector coordinate transformation matrix computing means A covariance matrix calculating means; a smoothing error covariance matrix initial value setting means for setting an initial value of a smoothing error covariance matrix for estimating an error of a smoothing vector; an initial value of the smoothing error covariance matrix and a calculated smoothing Calculated by the smoothing error covariance matrix storage means for storing the value of the error covariance matrix, the smoothing error covariance matrix at the previous time stored in the smoothing error covariance matrix storage means, and the driving noise covariance matrix calculation means A prediction error covariance matrix calculating means for calculating a prediction error covariance matrix at the current time from the driving noise covariance matrix, and the calculated prediction error covariance matrix Residual covariance matrix calculating means for calculating a residual covariance matrix based on the matrix value and the observation noise covariance matrix obtained from the observation device; the calculated residual covariance matrix value and the prediction error Gain matrix setting means for setting a gain matrix based on the value of the prediction error covariance matrix calculated by the variance matrix calculating means, the predicted value vector calculated by the prediction means, and an observed value vector obtained from the observation device The gate range setting means for setting the gate, the set gate, the predicted value vector calculated by the prediction means, and the gain matrix set by the gain matrix setting means Smoothing means for calculating a smooth value vector, and the prediction error covariance matrix calculated by the prediction error covariance matrix calculating means and the gain matrix setting means. Since the target tracking device includes a smoothing error covariance matrix calculating unit that inputs a set gain matrix and calculates a smoothing error covariance matrix, the gate is unnecessarily enlarged when the sampling interval is large. In addition, it is possible to accurately capture the target in the gate by setting an optimum gate corresponding to the speed even for the high-speed moving target.
実施の形態1.
以下、図面に基づき、本発明の実施の形態1に係る目標追尾装置について以下に説明する。図1は、本実施の形態1に係る目標追尾装置の構成を示した図である。
図1において、15は平滑値ベクトルより速度ノルムを計算する速度ノルム計算手段、16は前記速度ノルムと平滑値ベクトルを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段、17は駆動雑音共分散行列算出手段に対する目標速度の分散を設定する駆動雑音分散設定手段、18は前記駆動雑音分散と駆動雑音ベクトルの座標変換行列より駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段である。他の構成については、上述の図10と同じであるため、ここでは、同一符号を付して示し、それらの説明については省略する。なお、図10との相違点としては、図1の構成においては、図10の符号6,7で示す構成が無くなり、代わりに、符号15〜18で示す構成が設けられている。
Embodiment 1 FIG.
Hereinafter, the target tracking device according to the first embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a diagram showing a configuration of the target tracking device according to the first embodiment.
In FIG. 1, 15 is a velocity norm calculation means for calculating a velocity norm from a smooth value vector, and 16 is a coordinate conversion matrix of a drive noise vector for inputting the velocity norm and the smooth value vector and calculating a coordinate conversion matrix of the drive noise vector. A calculation means, 17 is a drive noise variance setting means for setting a target speed variance for the drive noise covariance matrix calculation means, and 18 is a drive for calculating a drive noise covariance matrix from the drive noise variance and the coordinate transformation matrix of the drive noise vector. It is a noise covariance matrix calculation means. Since other configurations are the same as those in FIG. 10 described above, the same reference numerals are given here, and descriptions thereof are omitted. 10 differs from FIG. 10 in the configuration of FIG. 1 in that the configurations indicated by
本発明の追尾装置では、極座標での目標速度ベクトルの大きさと向きのふらつきを駆動雑音とする等速直線運動モデルを用いた新たなカルマンフィルタを使用している。極座標とは、図2に示すように、センサより目標までの距離をR、水平面より目標までの仰角をE、水平面内で北方向より目標までの方位角をByとした座標のことを示す。 The tracking device of the present invention uses a new Kalman filter using a constant velocity linear motion model in which the fluctuation of the target velocity vector in polar coordinates and the fluctuation of the direction are driving noise. As shown in FIG. 2, the polar coordinates indicate coordinates in which the distance from the sensor to the target is R, the elevation angle from the horizontal plane to the target is E, and the azimuth angle from the north direction to the target is By in the horizontal plane.
ここで、本発明において使用する座標系の説明を図3で示す。本発明での座標系では、目標速度ベクトルの向きを示す角度α,βと、速度ベクトルの大きさVを使用し、システム雑音を設定する。なお、北基準直交座標表示と極座標表示の間には、次の関係が成り立つ。 Here, an explanation of the coordinate system used in the present invention is shown in FIG. In the coordinate system in the present invention, the angles α and β indicating the direction of the target velocity vector and the velocity vector magnitude V are used to set the system noise. Note that the following relationship holds between the north reference orthogonal coordinate display and the polar coordinate display.
つぎに、速度ベクトルのふらつきの北基準直交座標表示を式(18)で、極座標表示を式(19)で定義する。 Next, the north reference orthogonal coordinate display of the fluctuation of the velocity vector is defined by Expression (18), and the polar coordinate display is defined by Expression (19).
すると、ふらつきが微少量であると仮定すれば、線形近似により、式(17)をTailor展開して、2次以上の項を無視すれば、式(18)及び式(19)より、式(20)及び式(21)を得る。 Then, assuming that the wobbling is very small, Equation (17) is expanded in Taylor by linear approximation, and if the second-order or higher terms are ignored, Equation (18) and Equation (19) give 20) and (21) are obtained.
ここで、Gは駆動雑音ベクトルの座標変換行列である。本発明では、駆動雑音設定時は極座標系を使用するが、カルマンフィルタ内での各計算時は北基準直交座標系のパラメータとして引用するため、このような座標変換行列が必要となる。なお、この駆動雑音ベクトルの座標変換行列こそが、本発明の特徴である。 Here, G is a coordinate transformation matrix of a driving noise vector. In the present invention, a polar coordinate system is used when driving noise is set, but such a coordinate transformation matrix is necessary because each calculation in the Kalman filter is cited as a parameter of the north reference orthogonal coordinate system. Note that this drive noise vector coordinate transformation matrix is the feature of the present invention.
式(21)及び式(17)より、次式(22)を得る。 From the equations (21) and (17), the following equation (22) is obtained.
目標速度は式(23)で表され、また、式(17),(20)及び水平面内の速度の定義の式(24)より、次式(25),(26),(27)及び(28)を得る。 The target speed is expressed by Expression (23), and from Expressions (17) and (20) and Expression (24) of the definition of the speed in the horizontal plane, the following Expressions (25), (26), (27) and ( 28) is obtained.
式(22)に式(25)〜(28)を代入し、駆動雑音ベクトルの座標変換行列として、式(29)を得る。 Expressions (25) to (28) are substituted into Expression (22), and Expression (29) is obtained as a coordinate transformation matrix of the drive noise vector.
つぎに、運動モデルの定義を説明する。
式(1)では、駆動雑音ベクトルとして北基準直交座標の加速度ベクトルを使用しているが、本発明においては、駆動雑音ベクトルとして極座標の速度ベクトルのふらつきを使用し、それを北基準直交座標に座標変換を行い、処理を行うものである。そこで、運動モデルは次式(30)となる。
Next, the definition of the motion model will be described.
In equation (1), the acceleration vector of the north reference orthogonal coordinate is used as the driving noise vector. In the present invention, the fluctuation of the velocity vector of the polar coordinate is used as the driving noise vector, and this is converted into the north reference orthogonal coordinate. Coordinate conversion is performed and processing is performed. Therefore, the motion model is expressed by the following equation (30).
ここで、Γは駆動雑音ベクトルの変換行列で、北基準直交座標系の駆動雑音ベクトルw(k−1|vc)が速度のディメンションのため、式(31)となり、式(30)に式(20)を代入し、近似誤差を無視すると、式(32)となる。 Here, Γ is a drive noise vector transformation matrix, and the drive noise vector w (k−1 | vc) of the north reference Cartesian coordinate system is a velocity dimension, and therefore, Equation (31) is obtained. Substituting 20) and ignoring the approximation error yields equation (32).
但し、
Wvp(k):平均零ベクトル駆動雑音共分散行列Qvp(k)の3変量白色正規分布に従う極座標系駆動雑音ベクトル
とする。
However,
Wvp (k): A polar coordinate system drive noise vector according to the trivariate white normal distribution of the mean zero vector drive noise covariance matrix Qvp (k).
上記のような運動モデルと、式(5)の観測モデルのもと、カルマンフィルタは式(9)を次式(34)に変更することにより状態ベクトルの予測を行う。 Based on the above motion model and the observation model of equation (5), the Kalman filter predicts the state vector by changing equation (9) to the following equation (34).
なお、上述した従来方式による式(9)は駆動雑音ベクトルを北基準直交座標の加速度ベクトルとした場合の予測誤差共分散行列であり、また、式(34)は駆動雑音ベクトルを極座標の速度ベクトルのふらつきとした場合の予測誤差共分散行列である。ここで、従来方式である式(9)はあらかじめ設定されたパラメータのみで算出されるのに対し、本発明の方式である式(34)は、式(29)からもわかるように、追尾フィルタに算出すべき目標速度が反映されている。 Equation (9) according to the conventional method described above is a prediction error covariance matrix when the driving noise vector is an acceleration vector of the north reference orthogonal coordinate, and Equation (34) is a velocity vector of polar coordinates. This is a prediction error covariance matrix when wandering. Here, equation (9), which is the conventional method, is calculated using only parameters set in advance, whereas equation (34), which is the method of the present invention, can be seen from equation (29) as shown in FIG. The target speed to be calculated is reflected in.
つぎに、ゲートの算出式は、駆動雑音ベクトルを極座標の速度ベクトルのふらつきとしても、駆動雑音ベクトルを北基準直交座標の加速度ベクトルとした場合と同一である。ただし、予測誤差共分散行列が式(34)で算出されるため、式(13)は、式(7),(31),(34)及び(11)より、式(35)となる。 Next, the calculation formula of the gate is the same as when the driving noise vector is the acceleration vector of the north reference orthogonal coordinate even if the driving noise vector is the fluctuation of the velocity vector of the polar coordinate. However, since the prediction error covariance matrix is calculated by Expression (34), Expression (13) becomes Expression (35) from Expressions (7), (31), (34), and (11).
式(35)の右辺の第2項は、ゲートの広がりを決める要素の一つであり、駆動雑音共分散行列のサンプリング間での影響であることを示す。なお、この値τ2GQvp(k−1)GTは、式(29),(19)、及び、(33)より、目標速度のあいまいさであるΔV及び速度ベクトルの向きのあいまいさΔα、Δβを反映している。また、サンプリング間隔τが大きくなれば、算出した目標速度ベクトルに応じて大きくなっていく。 The second term on the right side of Equation (35) is one of the factors that determine the spread of the gate, and indicates that it is an influence between sampling of the drive noise covariance matrix. Incidentally, the value τ 2 GQ vp (k-1 ) G T has the formula (29), (19), and, (33) than the ambiguity of orientation of ΔV and the velocity vector is ambiguity target speed Δα , Δβ is reflected. Further, when the sampling interval τ is increased, the sampling interval τ is increased according to the calculated target speed vector.
つぎに、駆動雑音ベクトルを極座標の速度ベクトルのふらつきとする本発明の追尾フィルタの各要素の特徴を述べる。 Next, the characteristics of each element of the tracking filter according to the present invention in which the drive noise vector is a fluctuation of the polar coordinate velocity vector will be described.
予測誤差共分散行列の導出について、次式(36)が定義される。 The following equation (36) is defined for derivation of the prediction error covariance matrix.
ここで、従来方式と比較し、本発明の特徴が、Γの項(駆動雑音ベクトルの変換行列)と、Qの項(誤差共分散行列)に含まれる。 Here, compared with the conventional method, the features of the present invention are included in the term of Γ (transformation matrix of driving noise vector) and the term of Q (error covariance matrix).
まず、変換行列Γの項の特徴を表示する。 First, the characteristics of the terms of the transformation matrix Γ are displayed.
従来方式では、式(4)が示すように、式(9)における6×3の変換行列Γの内の上部の3×3の行列部は、サンプリング間隔の二乗τ2に比例しており、また変換行列Γの内の下部の3×3の行列部は、サンプリング間隔τに比例している。これに対し、本発明の方式では、式(31)が示すように、式(36)における6×3の変換行列Γの内の上部の3×3の行列部は、サンプリング間隔τに比例しており、また、変換行列Γの内の下部の3×3の行列部は、サンプリング間隔に無関係な定数であることがわかる。 In the conventional method, as shown in Equation (4), the upper 3 × 3 matrix portion of the 6 × 3 transformation matrix Γ in Equation (9) is proportional to the square τ 2 of the sampling interval, The lower 3 × 3 matrix portion of the transformation matrix Γ is proportional to the sampling interval τ. On the other hand, in the method of the present invention, as shown in Equation (31), the upper 3 × 3 matrix portion of the 6 × 3 transformation matrix Γ in Equation (36) is proportional to the sampling interval τ. In addition, it can be seen that the lower 3 × 3 matrix portion of the transformation matrix Γ is a constant unrelated to the sampling interval.
すなわち、従来方式ではサンプリング間隔の二乗の値に比例するゲート設定となっており、サンプリング間隔が広がると極端に大きなゲートが設定されていたが、本発明の方式では、サンプリング間隔の一乗の値に比例するゲート設定であり、サンプリング間隔が広がってもゲートの設定は線形に拡大するので、従来方式よりもサンプリング間隔の大きさの変化を受けにくいことを示している(図4参照)。これにより、サンプリング間隔が大きくなっても極端にゲートが拡大することはなく、適度な大きさのゲート設定が保たれるため、不要信号の受信確率が低くなり、追尾を外す危険性が少なくなる。 That is, in the conventional method, the gate setting is proportional to the square value of the sampling interval, and when the sampling interval is widened, an extremely large gate is set, but in the method of the present invention, the sampling interval is raised to the square value. This is a proportional gate setting, and even if the sampling interval is widened, the gate setting is linearly expanded, indicating that it is less susceptible to changes in the sampling interval than the conventional method (see FIG. 4). As a result, even if the sampling interval is increased, the gate is not extremely expanded, and a moderately-sized gate setting is maintained, so that the reception probability of unnecessary signals is lowered and the risk of removing tracking is reduced. .
つぎに、Qの項(誤差共分散行列)の特徴を表示する。 Next, the characteristics of the Q term (error covariance matrix) are displayed.
従来方式では、式(9)に示すように平均零ベクトルの駆動雑音共分散行列であり、速度に無関係なパラメータである。これに対し、本発明の方式では、式(29)の座標変換行列が組み込まれるため、次式(37)で表される。 In the conventional method, as shown in Equation (9), the driving noise covariance matrix has a mean zero vector and is a parameter unrelated to speed. On the other hand, in the system of the present invention, since the coordinate transformation matrix of Expression (29) is incorporated, it is expressed by the following Expression (37).
ここで、式(37)の各要素は、次式(38)〜(43)で表す。 Here, each element of the formula (37) is represented by the following formulas (38) to (43).
上下方向の速度zドットは微少で、速度ベクトルはx軸上にあるとすると、次式(44)〜(46)も仮定される。 If the vertical speed z dot is very small and the speed vector is on the x-axis, the following equations (44) to (46) are also assumed.
ここで、式(44)〜(46)を式(37)〜(43)に代入したのち、式(23)及び(24)を使用し、次式(47)が得られる。 Here, after substituting Expressions (44) to (46) into Expressions (37) to (43), Expressions (23) and (24) are used to obtain the following Expression (47).
但し、
qv:目標速度のふらつきの分散
V2qα:水平面内の速度ベクトルの向きのふらつきの分散に速度の二乗が乗算された値
V2qβ:鉛直面内の速度ベクトルの向きのふらつきの分散に速度の二乗が乗算された値
とする。
However,
q v : dispersion of fluctuation of target speed V 2 q α : value obtained by multiplying dispersion of fluctuation of velocity vector direction in horizontal plane by square of velocity V 2 q β : fluctuation of velocity vector direction in vertical plane The variance is multiplied by the square of speed.
ここで、(23)成分及び(34)成分に速度の二乗が組み込まれることになる。すなわち、従来方式では目標速度の大小にかかわらず、一定の大きさのゲートが設定されていたため、速度が大きな目標に対しても、速度の小さい目標と同じ範囲のゲートが設定され、適切な範囲のゲート設定とはならず、追尾を外す可能性が大きかった。本発明の方式では、1サンプリング前の目標速度がパラメータとして毎回引用されるため、目標速度の大小に比例したゲート設定が可能となり、速度が大きな目標に対してはゲート範囲を拡大することが可能となった。すなわち、高速移動目標に対して適切なゲート設定が可能となり、追尾を外す可能性を小さくすることが可能となる(図5参照)。 Here, the square of the speed is incorporated into the (23) component and the (34) component. In other words, in the conventional method, a gate with a constant size was set regardless of the target speed, so a gate with the same range as a target with a low speed is set for a target with a high speed, and an appropriate range is set. It was not possible to set the gate, and the possibility to remove the tracking was great. In the method of the present invention, since the target speed before one sampling is quoted as a parameter every time, it is possible to set a gate proportional to the target speed, and it is possible to expand the gate range for a target with a high speed. It became. That is, it is possible to set an appropriate gate for a high-speed movement target, and to reduce the possibility of removing tracking (see FIG. 5).
次に、図6のフローチャートに基づいて、本発明の追尾方法について説明する。
図6に示すように、初期化ステップであるステップS1において、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルとその誤差を推定した平滑誤差共分散行列の初期値を設定する。以降、ステップS2〜ステップS12までの各サンプリング時刻における追尾処理ステップに入る。まず、ステップS2の速度ノルム算出ステップにおいて、目標速度のノルムを算出する。
次に、ステップS3の駆動雑音の座標変換行列算出ステップにおいて、駆動雑音ベクトルの座標変換行列を算出する。次に、ステップS4の駆動雑音分散設定ステップにおいて、駆動雑音分散を設定する。次に、ステップS5の駆動雑音共分散行列算出ステップにおいて、駆動雑音共分散行列を算出する。次に、ステップS6の予測処理ステップにおいて、予測値ベクトルと予測誤差共分散行列を算出する。次に、ステップS7の観測諸元入力ステップにおいて、観測値ベクトルと観測雑音共分散行列を入力する。次に、ステップS8の残差共分散行列算出ステップにおいて、残差共分散行列を算出する。次に、ステップS9のゲート設定ステップにおいて、ゲートの設定を行う。次に、ステップS10のゲイン行列設定ステップにおいて、ゲイン行列を設定する。次に、ステップS11の平滑処理ステップにおいて、平滑値ベクトルと平滑誤差共分散行列を算出する。最後に、ステップS12の追尾終了判定ステップにおいて、追尾終了か否かを判断し、追尾終了の場合は処理を終了し、そうでなければ、ステップS2の処理に戻る。
Next, the tracking method of the present invention will be described based on the flowchart of FIG.
As shown in FIG. 6, in step S1, which is an initialization step, the initial value of the smoothing error covariance matrix in which the smooth value vector of the state vector composed of the motion parameters such as the target position and velocity and the error thereof are estimated. Set. Thereafter, the process enters a tracking process step at each sampling time from step S2 to step S12. First, in the speed norm calculation step of step S2, the norm of the target speed is calculated.
Next, in a driving noise coordinate transformation matrix calculation step in step S3, a driving noise vector coordinate transformation matrix is calculated. Next, in the drive noise variance setting step in step S4, the drive noise variance is set. Next, a drive noise covariance matrix is calculated in a drive noise covariance matrix calculation step in step S5. Next, in the prediction processing step of step S6, a prediction value vector and a prediction error covariance matrix are calculated. Next, an observation value vector and an observation noise covariance matrix are input in an observation specification input step of step S7. Next, a residual covariance matrix is calculated in a residual covariance matrix calculation step in step S8. Next, in the gate setting step of step S9, the gate is set. Next, a gain matrix is set in the gain matrix setting step of step S10. Next, in the smoothing step of step S11, a smooth value vector and a smooth error covariance matrix are calculated. Finally, in the tracking end determination step of step S12, it is determined whether or not the tracking is ended. If the tracking is ended, the processing is ended. Otherwise, the processing returns to step S2.
以下、図1および図6を用いて、本発明の追尾装置と追尾方法の動作手順をさらに詳細に説明する。
まず、平滑値ベクトル初期値設定手段1が、平滑値ベクトル記憶手段2に、平滑値ベクトルの初期値xハット(0|0)を設定し、また、平滑誤差共分散行列初期値設定手段8が、平滑誤差共分散行列記憶手段9に、平滑誤差共分散行列の初期値P(0|0)を設定する(ステップS1の初期化ステップ)。以下、各サンプリング時刻の追尾処理のループに入る。
Hereinafter, the operation procedure of the tracking device and the tracking method of the present invention will be described in more detail with reference to FIGS.
First, the smooth value vector initial value setting means 1 sets the smooth value vector initial value x hat (0 | 0) in the smooth value vector storage means 2, and the smooth error covariance matrix initial value setting means 8 The initial value P (0 | 0) of the smoothing error covariance matrix is set in the smoothing error covariance matrix storage means 9 (initialization step of step S1). Thereafter, the process enters a tracking process loop at each sampling time.
サンプリング時刻t(k)における一連の処理では、まず、速度ノルム算出手段15が平滑値ベクトル記憶手段2から入力した前時刻の平滑値ベクトルxハット(k−1|k−1)を使用し、速度ベクトルのふらつきを算出し(ステップS2の速度ノルム算出ステップ)、その値と、前時刻の平滑値ベクトルxハット(k−1|k−1)とを使用し、駆動雑音ベクトルの座標変換行列算出手段16により、式(29)の駆動雑音ベクトルの座標変換行列を算出する(ステップS3の駆動雑音ベクトルの座標変換行列算出ステップ)。
In a series of processes at the sampling time t (k), first, the smoothing value vector x hat (k−1 | k−1) of the previous time input from the smoothing value
つぎに、駆動雑音分散設定手段17が、式(33)で示される駆動雑音分散を設定する(ステップS4の駆動雑音分散設定ステップ)。つぎに、駆動雑音共分散行列算出手段18が、駆動雑音ベクトルの座標変換行列算出手段16で算出した駆動雑音ベクトルの座標変換行列Gと、駆動雑音分散設定手段17で設定された式(33)で示される駆動雑音分散を使用し、式(47)で示す駆動雑音共分散行列を算出する(ステップS5の駆動雑音共分散行列算出ステップ)。 Next, the drive noise variance setting means 17 sets the drive noise variance represented by the equation (33) (drive noise variance setting step in step S4). Next, the driving noise covariance matrix calculating means 18 uses the driving noise vector coordinate transformation matrix G calculated by the driving noise vector coordinate transformation matrix calculating means 16 and the equation (33) set by the driving noise variance setting means 17. Is used to calculate the driving noise covariance matrix expressed by the equation (47) (driving noise covariance matrix calculating step in step S5).
つぎに、予測手段3が平滑値ベクトル記憶手段2から入力した前時刻の平滑値ベクトルxハット(k−1|k−1)を使用し、式(8)に従って、現時刻の予測値ベクトルxハット(k|k−1)を算出する。
Next, using the smoothed value vector x hat (k−1 | k−1) of the previous time input from the smoothed value
また、予測誤差共分散行列算出手段10が、平滑誤差共分散行列記憶手段9から入力した前時刻の平滑誤差共分散行列P(k−1|k−1)と、前時刻に算出した式(47)の駆動雑音共分散行列を使用し、式(34)に従って、現時刻の予測誤差共分散行列P(k|k−1)を算出する(ステップS6の予測処理ステップ)。
Further, the prediction error covariance
つぎに、観測装置102より観測雑音共分散行列算出手段12を介して観測値ベクトルz(k)と観測雑音共分散行列B(k)が入力されると(ステップS7の観測諸元入力ステップ)、残差共分散行列算出手段13が、予測誤差共分散行列算出手段10で前時刻に算出した予測誤差共分散行列P(k|k−1)と、入力された観測雑音共分散行列B(k)より、残差共分散行列を算出し(ステップS8の残差共分散行列算出ステップ)、その残差共分散行列と予測誤差共分散行列により、ゲイン行列設定手段14によってゲイン行列が設定される(ステップS10のゲイン行列設定ステップ)。
Next, when the observation value vector z (k) and the observation noise covariance matrix B (k) are input from the
つぎに、ゲート範囲設定手段4が、予測手段3が算出した予測値ベクトルxハット(k|k−1)と、観測装置102より入力された観測値ベクトルz(k)を使用し、式(13)に示すゲート範囲を設定した(ステップS9のゲート設定ステップ)後、平滑手段5が、式(15)に従って、前期予測値ベクトルxハット(k|k−1)、ゲート範囲S(k)、ゲイン行列K(k)を使用して、現時刻の平滑値ベクトルxハット(k|k)を算出する。この平滑値ベクトルは平滑値ベクトル記憶手段2に格納する(ステップS11の平滑処理ステップの一部)。
Next, the gate
また、平滑誤差共分散行列算出手段11は、式(16)に従って、前記予測誤差共分散行列P(k|k−1)、ゲイン行列K(k)及び観測行列Hを使用して、現時刻の平滑誤差共分散行列P(k|k)を算出する。この平滑誤差共分散行列は平滑誤差共分散行列記憶手段9に格納する(ステップS11の平滑処理ステップの残りの部分)。 The smoothing error covariance matrix calculating means 11 uses the prediction error covariance matrix P (k | k−1), the gain matrix K (k), and the observation matrix H according to the equation (16) to The smoothing error covariance matrix P (k | k) is calculated. This smoothing error covariance matrix is stored in the smoothing error covariance matrix storage means 9 (the remaining part of the smoothing processing step in step S11).
以上で、時刻t(k)における一連の処理が終了する。追尾終了判定ステップにおいて追尾終了との判定がなされる(ステップS12の追尾終了判定ステップ)まで、これら一連の処理を繰り返して実行する。 Thus, a series of processes at time t (k) is completed. These series of processes are repeated until the tracking end determination step determines that the tracking is ended (tracking end determination step in step S12).
本発明の追尾装置及び追尾方法を使用した極座標表示の速度のふらつきを駆動雑音とするカルマンフィルタによれば、従来のカルマンフィルタのように、サンプリング間隔の大きさに比例してゲートが爆発的に拡大することがないため、それぞれのサンプリング間隔に応じた適度な大きさのゲートが設定され、不要信号の受信率を少なくし、追尾を外す危険性を低くすることが可能となる利点を有する。さらに、速度ベクトルの北基準直交座標表示と極座標表示の変換行列内に、1サンプリング前の速度要因が引用されることとなり、従来方式ではゲートが目標速度の大小に関係なく一定の大きさで設定されていたものが、本発明方式では、目標速度の大小に応じてゲートの範囲が変動し、設定されることとなった。これにより、目標速度が大きければ、その速度に対応した大きさのゲートが設定されることになり、目標速度が大きい時に不釣合いに小さいゲートを設定し、目標の追尾を外してしまうような危険性を低くすることが可能となる利点を有する。 According to the Kalman filter using the fluctuation in the speed of polar coordinate display using the tracking device and the tracking method of the present invention as driving noise, the gate expands in proportion to the size of the sampling interval as in the conventional Kalman filter. Therefore, there is an advantage that a gate having an appropriate size corresponding to each sampling interval is set, the reception rate of unnecessary signals can be reduced, and the risk of removing tracking can be reduced. In addition, the speed factor before one sampling is quoted in the conversion matrix of the north reference Cartesian coordinate display and polar coordinate display of the speed vector. In the conventional method, the gate is set at a constant size regardless of the target speed. However, in the method of the present invention, the gate range fluctuates and is set according to the target speed. As a result, if the target speed is high, a gate with a size corresponding to that speed will be set. If the target speed is high, a small gate may be set disproportionately and the target tracking may be removed. There is an advantage that it is possible to lower the performance.
実施の形態2.
以下、本発明の実施の形態2に係る目標追尾装置および目標追尾方法について説明する。本実施の形態2においては、構成および動作は、基本的に、図1及び図6に示した実施の形態1と同じであるため、それらを参照することとし、以下の説明においては、実施の形態1と異なる部分のみを説明することとする。
Hereinafter, a target tracking device and a target tracking method according to
図7は、本実施の形態2にかかる目標追尾装置の駆動雑音分散設定手段17の内部構成を示したブロック図である。図7に示すように、本実施の形態2にかかる駆動雑音分散設定手段17には、目標別の最大加速度を算出する最大加速度算出手段20と、目標別の最小速度を算出する最小速度算出手段21と、算出された最大加速度及び最小速度より示される最大角加速度を算出する最大角加速度算出手段22と、算出された最大角加速度により示される最大旋回半径を算出する最大旋回半径算出手段23と、算出された最大旋回半径より駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出手段24とが設けられている。
FIG. 7 is a block diagram showing an internal configuration of the drive noise variance setting means 17 of the target tracking device according to the second embodiment. As shown in FIG. 7, the drive noise
本実施の形態においては、速度ベクトルを駆動雑音とすることにより、目標の最大旋回範囲が機体別(目標別)によって導かれる。まず、目標の機種別により、式(48)に示す速度の最大値と最小値及び加速度の最大値と最小値が与えられるとする。 In the present embodiment, the target maximum turning range is derived by machine body (by target) by using the speed vector as drive noise. First, it is assumed that the maximum and minimum values of speed and the maximum and minimum values of acceleration shown in Expression (48) are given depending on the target model.
ここで、式(49)の定義より、角加速度の最大値と最小値が式(50)で求められる。 Here, the maximum value and the minimum value of the angular acceleration are obtained by the formula (50) from the definition of the formula (49).
式(50)で求められた最大角加速度を使用し、式(51)で示す最大旋回半径が求められる。 Using the maximum angular acceleration obtained by Equation (50), the maximum turning radius shown by Equation (51) is obtained.
但し、
Δβ:旋回半径
T:サンプリングレート
とする。
However,
Δβ: turning radius T: sampling rate.
つまり、本実施の形態においては、駆動雑音分散設定手段17において、図7に示すように、目標別の最大加速度および最小速度を、それぞれ、最大加速度算出手段20と最小速度算出手段21とにより求め、それらの値を用いて、最大角加速度を最大角加速度算出手段22より算出する。算出された最大角加速度により最大旋回半径を最大旋回半径算出手段23により算出し、その値に基づいて駆動雑音共分散行列内のパラメータを駆動雑音共分散行列内パラメータ算出手段24より算出する。 That is, in the present embodiment, as shown in FIG. 7, the drive noise variance setting means 17 obtains the maximum acceleration and the minimum speed for each target by the maximum acceleration calculation means 20 and the minimum speed calculation means 21, respectively. The maximum angular acceleration is calculated by the maximum angular acceleration calculating means 22 using these values. Based on the calculated maximum angular acceleration, the maximum turning radius is calculated by the maximum turning radius calculating means 23, and the parameters in the driving noise covariance matrix are calculated by the driving noise covariance matrix parameter calculating means 24 based on the value.
以上のように、本実施の形態2においては、上述の実施の形態1と同様の効果が得られるとともに、さらに、速度ベクトルを駆動雑音とすることにより、目標の最大旋回範囲が目標別によって導かれ、その値に基づいて駆動雑音共分散行列内のパラメータを算出する。それにより、たとえば等速直線運動を行っていた目標が等速円運動で90度旋回したのち、再び等速直線運動を行う場合でも、x,y,z方向おのおのにおいて、サンプリング間隔が大きくなるほど、ゲートが不必要に拡大してしまうことを防止し、適度な大きさのゲートの設定を可能として、効率よく目標の追尾を行うことができるという効果が得られる。 As described above, in the second embodiment, the same effect as in the first embodiment described above can be obtained, and further, the target maximum turning range can be derived by target by using the speed vector as the driving noise. The parameter in the driving noise covariance matrix is calculated based on the value. Thereby, for example, even when a target that has been performing constant-velocity linear motion turns 90 degrees in constant-speed circular motion and then performs constant-velocity linear motion again, as the sampling interval increases in each of the x, y, and z directions, It is possible to prevent the gate from unnecessarily expanding and to set a gate having an appropriate size, and to obtain an effect that the target can be tracked efficiently.
1 平滑値ベクトル初期値設定手段、2 平滑値ベクトル記憶手段、3 予測手段、4 ゲート範囲設定手段、5 平滑手段、6 駆動雑音分散設定手段、7 駆動雑音共分散行列算出手段、8 平滑誤差共分散行列初期値設定手段、9 平滑誤差共分散行列記憶手段、10 予測誤差共分散行列算出手段、11 平滑誤差共分散行列算出手段、12 観測雑音共分散行列算出手段、13 残差共分散行列算出手段、14 ゲイン行列設定手段、15 速度ノルム計算手段、16 駆動雑音ベクトルの座標変換行列算出手段、17 駆動雑音分散設定手段、18 駆動雑音共分散行列算出手段、20 最大加速度算出手段、21 最小速度算出手段、22 最大角加速度算出手段、23 最大旋回半径算出手段、24 駆動雑音共分散行列内パラメータ算出手段。
1 smoothing value vector initial value setting means, 2 smoothing value vector storage means, 3 prediction means, 4 gate range setting means, 5 smoothing means, 6 driving noise variance setting means, 7 driving noise covariance matrix calculating means, 8 smoothing error sharing Variance matrix initial value setting means, 9 smoothing error covariance matrix storage means, 10 prediction error covariance matrix calculating means, 11 smoothing error covariance matrix calculating means, 12 observation noise covariance matrix calculating means, 13 residual covariance
Claims (4)
前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、
前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、
前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、
算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、
駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、
設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、
平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、
前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、
前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、
算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、
算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、
前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、
設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、
前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段と
を備えたことを特徴とする目標追尾装置。 Smooth value vector initial value setting means for setting an initial value of a smooth value vector of a state vector composed of motion parameters such as a target position and speed;
Smooth value vector storage means for storing an initial value of the smooth value vector and a value of the calculated smooth value vector;
Prediction means for calculating a predicted value vector at the current time from a smooth value vector at the previous time stored in the smooth value vector storage means;
Speed norm calculation means for calculating a speed norm based on the smooth value vector stored in the smooth value vector storage means;
A driving noise vector coordinate transformation matrix calculation means for inputting the calculated velocity norm and the smoothing value vector stored in the smoothing value vector storage means, and calculating a driving noise vector coordinate transformation matrix;
Driving noise dispersion setting means for setting the dispersion of the target speed as the driving noise dispersion;
A driving noise covariance matrix calculating means for calculating a driving noise covariance matrix from the set driving noise variance and a coordinate conversion matrix of the driving noise vector calculated by the coordinate conversion matrix calculating means of the driving noise vector;
Smoothing error covariance matrix initial value setting means for setting an initial value of a smoothing error covariance matrix for estimating an error of a smoothing vector;
Smoothing error covariance matrix storage means for storing an initial value of the smoothing error covariance matrix and a value of the calculated smoothing error covariance matrix;
Based on the smoothing error covariance matrix at the previous time stored in the smoothing error covariance matrix storage means and the driving noise covariance matrix calculated by the driving noise covariance matrix calculating means, the prediction error covariance at the current time A prediction error covariance matrix calculating means for calculating a matrix;
A residual covariance matrix calculating means for calculating a residual covariance matrix from the calculated value of the prediction error covariance matrix and an observation noise covariance matrix obtained from the observation device;
A gain matrix setting means for setting a gain matrix based on the calculated value of the residual covariance matrix and the value of the prediction error covariance matrix calculated by the prediction error covariance matrix calculation means;
Gate range setting means for inputting the predicted value vector calculated by the prediction means and the observed value vector obtained from the observation device, and setting a gate;
Smoothing means for inputting a predicted value vector calculated by the set gate and the prediction means and the gain matrix set by the gain matrix setting means, and calculating a smooth value vector;
Smoothing error covariance matrix calculating means for inputting the prediction error covariance matrix calculated by the prediction error covariance matrix calculating means and the gain matrix set by the gain matrix setting means and calculating a smoothing error covariance matrix And a target tracking device.
目標別の最大加速度を算出する最大加速度算出手段と、
前記目標別の最小速度を算出する最小速度算出手段と、
算出された前記最大加速度及び前記最小速度に基づいて、前記目標別の最大角加速度を算出する最大角加速度算出手段と、
算出された前記最大角加速度に基づいて、前記目標別の最大旋回半径を算出する最大旋回半径算出手段と、
算出された前記最大旋回半径に基づいて、前記駆動雑音共分散行列算出手段における前記駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出手段と
を備えていることを特徴とする請求項1に記載の目標追尾装置。 The driving noise variance setting means includes
Maximum acceleration calculating means for calculating the maximum acceleration for each target;
Minimum speed calculating means for calculating the minimum speed for each target;
Maximum angular acceleration calculating means for calculating the maximum angular acceleration for each target based on the calculated maximum acceleration and the minimum speed;
A maximum turning radius calculating means for calculating a maximum turning radius for each target based on the calculated maximum angular acceleration;
Drive noise covariance matrix parameter calculation means for calculating parameters in the drive noise covariance matrix in the drive noise covariance matrix calculation means based on the calculated maximum turning radius, The target tracking device according to claim 1.
前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶ステップと、
前記平滑値ベクトル記憶ステップにより記憶された前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測ステップと、
前記平滑値ベクトル記憶ステップにより記憶された前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算ステップと、
算出された前記速度ノルムと、前記平滑値ベクトル記憶ステップで記憶された前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出ステップと、
駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定ステップと、
設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出ステップにより算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出ステップと、
平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定ステップと、
前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶ステップと、
前記平滑誤差共分散行列記憶ステップにより記憶された前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出ステップにより算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出ステップと、
算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出ステップと、
算出された前記残差共分散行列の値と前記予測誤差共分散行列算出ステップにより算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定ステップと、
前記予測ステップにより算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定ステップと、
設定された前記ゲートと前記予測ステップにより算出された前記予測値ベクトルと前記ゲイン行列設定ステップにより設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑ステップと、
前記予測誤差共分散行列算出ステップにより算出された前記予測誤差共分散行列と前記ゲイン行列設定ステップにより設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出ステップと
を備えたことを特徴とする目標追尾方法。 A smooth value vector initial value setting step for setting an initial value of a smooth value vector of a state vector composed of motion parameters such as a target position and speed;
A smooth value vector storage step for storing an initial value of the smooth value vector and a value of the calculated smooth value vector;
A prediction step of calculating a prediction value vector at the current time from the smoothing value vector at the previous time stored in the smoothing value vector storage step;
A speed norm calculation step for calculating a speed norm based on the smooth value vector stored by the smooth value vector storage step;
A driving noise vector coordinate transformation matrix calculation step of inputting the calculated velocity norm and the smoothing value vector stored in the smoothing value vector storage step to calculate a driving noise vector coordinate transformation matrix;
A drive noise dispersion setting step for setting the dispersion of the target speed as the drive noise dispersion;
A driving noise covariance matrix calculating step for calculating a driving noise covariance matrix from the set driving noise variance and the driving noise vector coordinate conversion matrix calculating step;
A smoothing error covariance matrix initial value setting step for setting an initial value of a smoothing error covariance matrix for estimating an error of the smoothing vector;
A smoothing error covariance matrix storing step for storing an initial value of the smoothing error covariance matrix and a value of the calculated smoothing error covariance matrix;
The prediction error covariance matrix at the current time is calculated from the smoothing error covariance matrix at the previous time stored in the smoothing error covariance matrix storing step and the driving noise covariance matrix calculated in the driving noise covariance matrix calculating step. A prediction error covariance matrix calculating step for calculating
A residual covariance matrix calculating step of calculating a residual covariance matrix from the calculated value of the prediction error covariance matrix and the observation noise covariance matrix obtained from the observation device;
A gain matrix setting step of setting a gain matrix by the calculated value of the residual covariance matrix and the value of the prediction error covariance matrix calculated by the prediction error covariance matrix calculation step;
A gate range setting step of setting a gate by inputting the prediction value vector calculated by the prediction step and an observation value vector obtained from the observation device;
A smoothing step of calculating a smoothing value vector by inputting the set gate and the prediction value vector calculated by the prediction step and the gain matrix set by the gain matrix setting step;
A smoothing error covariance matrix calculating step for inputting the prediction error covariance matrix calculated by the prediction error covariance matrix calculating step and the gain matrix set by the gain matrix setting step and calculating a smoothing error covariance matrix A target tracking method comprising: and.
目標別の最大加速度を算出する最大加速度算出ステップと、
前記目標別の最小速度を算出する最小速度算出ステップと、
算出された前記最大加速度及び前記最小速度に基づいて、前記目標別の最大角加速度を算出する最大角加速度算出ステップと、
算出された前記最大角加速度に基づいて、前記目標別の最大旋回半径を算出する最大旋回半径算出ステップと、
算出された前記最大旋回半径に基づいて、前記駆動雑音共分散行列算出ステップにおける前記駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出ステップと
を備えていることを特徴とする請求項3に記載の目標追尾方法。 The drive noise variance setting step includes:
A maximum acceleration calculating step for calculating a maximum acceleration for each target;
A minimum speed calculating step for calculating a minimum speed for each target;
A maximum angular acceleration calculating step of calculating a maximum angular acceleration for each target based on the calculated maximum acceleration and the minimum speed;
A maximum turning radius calculating step for calculating a maximum turning radius for each target based on the calculated maximum angular acceleration;
A driving noise covariance matrix parameter calculating step for calculating a parameter in the driving noise covariance matrix in the driving noise covariance matrix calculating step based on the calculated maximum turning radius. The target tracking method according to claim 3.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2007178317A JP5230132B2 (en) | 2007-07-06 | 2007-07-06 | Target tracking device and target tracking method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2007178317A JP5230132B2 (en) | 2007-07-06 | 2007-07-06 | Target tracking device and target tracking method |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2009014596A JP2009014596A (en) | 2009-01-22 |
JP5230132B2 true JP5230132B2 (en) | 2013-07-10 |
Family
ID=40355666
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2007178317A Active JP5230132B2 (en) | 2007-07-06 | 2007-07-06 | Target tracking device and target tracking method |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5230132B2 (en) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5748946B2 (en) * | 2009-08-28 | 2015-07-15 | 株式会社東芝 | Target tracking device |
CN109959907B (en) * | 2017-12-25 | 2023-10-20 | 大连楼兰科技股份有限公司 | Method for processing height data |
CN110442142B (en) * | 2018-05-02 | 2022-12-27 | 北京京东尚科信息技术有限公司 | Speed data processing method and device, electronic equipment and computer readable medium |
RU2707581C1 (en) * | 2019-05-28 | 2019-11-28 | Федеральный научно-производственный центр акционерное общество "Научно-производственное объединение "Марс" | Method for adaptive tracking of targets with formation of a gate in a coordinate system associated with the target |
CN113030959B (en) * | 2021-04-01 | 2023-07-04 | 成都汇蓉国科微***技术有限公司 | Radar and photoelectric linkage anti-low small-target 3D map picture jitter eliminating method |
CN114018250B (en) * | 2021-10-18 | 2024-05-03 | 杭州鸿泉物联网技术股份有限公司 | Inertial navigation method, electronic device, storage medium and computer program product |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3097471B2 (en) * | 1994-11-16 | 2000-10-10 | 三菱電機株式会社 | Tracking device |
JPH10246778A (en) * | 1997-03-04 | 1998-09-14 | Mitsubishi Electric Corp | Target search tracking device |
JP2893176B2 (en) * | 1997-05-08 | 1999-05-17 | 防衛庁技術研究本部長 | Multi-target tracking device |
JP3330852B2 (en) * | 1997-07-29 | 2002-09-30 | 三菱電機株式会社 | Target tracking method and device |
JP2002131423A (en) * | 2000-10-18 | 2002-05-09 | Mitsubishi Electric Corp | Radar device |
JP4080722B2 (en) * | 2001-10-25 | 2008-04-23 | 三菱電機株式会社 | Target tracking device and target tracking method |
JP4176985B2 (en) * | 2001-11-07 | 2008-11-05 | 三菱電機株式会社 | Tracking device |
JP2003329771A (en) * | 2002-05-16 | 2003-11-19 | Mitsubishi Electric Corp | Tracking apparatus and tracking processing method |
JP4196684B2 (en) * | 2003-01-16 | 2008-12-17 | 三菱電機株式会社 | Target tracking device |
JP4281473B2 (en) * | 2003-08-29 | 2009-06-17 | 三菱電機株式会社 | Target tracking device |
-
2007
- 2007-07-06 JP JP2007178317A patent/JP5230132B2/en active Active
Also Published As
Publication number | Publication date |
---|---|
JP2009014596A (en) | 2009-01-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5230132B2 (en) | Target tracking device and target tracking method | |
Kaulbersch et al. | A Cartesian B-spline vehicle model for extended object tracking | |
US11199850B2 (en) | Estimation device, control method, program and storage medium | |
EP1835463A2 (en) | Obstacle tracking apparatus and method | |
WO2018221455A1 (en) | Update device, control method, program, and storage medium | |
JP2011064484A (en) | Sensor bias estimation device | |
EP4160271A1 (en) | Method and apparatus for processing data for autonomous vehicle, electronic device, and storage medium | |
JP6882243B2 (en) | Avoidance support device | |
JP2020118575A (en) | Inter-vehicle distance measurement device, error model generation device, learning model generation device, and method and program thereof | |
JP2000241542A (en) | Movable body-tracking device | |
JP2003329771A (en) | Tracking apparatus and tracking processing method | |
JP4817854B2 (en) | Peripheral object tracking device and peripheral object position prediction method | |
CN113763434B (en) | Target track prediction method based on Kalman filtering multi-motion model switching | |
JP5315585B2 (en) | Wake generation system, error covariance matrix initial value setting device, wake generation method, and error covariance matrix initial value setting method | |
JP3926602B2 (en) | Target tracking apparatus and method | |
JP4080722B2 (en) | Target tracking device and target tracking method | |
CN115171384A (en) | Key vehicle position delay compensation method and device in vehicle-mounted display process | |
JP3739622B2 (en) | Target tracking device and target tracking method | |
Klein et al. | Observability analysis for tracking of coordinated turn maneuvers | |
JP5264416B2 (en) | Target tracking device | |
JP2001091622A (en) | Target position observing device | |
JP3792927B2 (en) | Target tracking device | |
WO2021112177A1 (en) | Information processing apparatus, control method, program, and storage medium | |
CN115096328B (en) | Positioning method and device of vehicle, electronic equipment and storage medium | |
JP7069371B2 (en) | Evasion support device |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20100412 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20111130 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20111206 |
|
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: 20130219 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20130319 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20160329 Year of fee payment: 3 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5230132 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 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 |