JP4281473B2 - Target tracking device - Google Patents

Description

  The present invention relates to a target tracking device that predicts a motion specification of a tracking target based on an observation value of a motion specification of the tracking target, and particularly when the tracking target is meandering. The present invention relates to a technology for accurately predicting the above.

Conventional target tracking device, when the tracking target moves meandering, using a Kalman filter based on the motion model that represents the acceleration x "(t) is a sine function x" in _{(t) = n t sin (} ωt + φ) The motion specifications of the tracking target were predicted (for example, Patent Document 1 and Non-Patent Document 1). Here, x is the position of the tracking target, and x ″ represents the second derivative with respect to time t (x ″ = d ² x / dt ² ). Further, n _t is the maximum amplitude of the meandering motion, ω is the meander frequency, and φ is the phase difference.

Japanese Patent Laid-Open No. 2002-189519 “Tracking System and Tracking Method” Publication FIG. 4, paragraph 0038 "Kalman filter using multiple motion model for tracking (meandering) target tracking", IEICE General Conference, B-2-20, March 2003

  Since the conventional target tracking device is configured as described above, if one tries to handle the time displacement of the state variable between each sampling, the time change of the acceleration, which is one of the state variables, that is, the jerk term (acceleration It is necessary to handle up to the first derivative term by time). However, when the observation noise is large or the sampling interval is long, the estimation accuracy of the jerk term tends to deteriorate. In addition, when the meander frequency is small (when the meandering track has a long wavelength), the jerk term is inevitably estimated when the above motion model is used, even though it is not necessary to estimate the jerk term. As a result, useless processing is performed. Furthermore, when the meandering amplitude is large, the estimated value (position and velocity) at a certain time is adopted, and if the position prediction after several samplings is performed assuming that constant velocity linear motion is performed at the velocity at that time, the actual target is obtained. Deviation from the position will increase. The present invention aims to solve this problem.

The target tracking device according to the present invention is a target tracking device that calculates a motion specification smooth value at the current sample from a motion specification smooth value at the previous sample by a Kalman filter,
Based on the motion model of the tracking target that has a constant velocity linear motion component and a sine motion component, it is equipped with motion specification prediction means that calculates the motion specification smooth value at the current sample from the motion specification smooth value at the previous sample. It is a thing.

  The present invention adopts a motion model based on a position instead of the “motion model based on acceleration” used in the prior art, and adopts a motion model having not only a sine motion component but also a constant velocity linear motion component. As a result, the tracking process of the tracking target that performs the meandering motion can be performed with high accuracy, and the problem that the observed value and the predicted value are greatly shifted due to the tracking target meandering can be reduced.

Embodiments of the present invention will be described below with reference to the drawings.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a configuration of a target tracking device according to Embodiment 1 of the present invention. In the figure, a gate determination unit 10 is a part for determining whether or not an observation value is data effective for tracking. When there are a plurality of prediction vectors calculated by a motion specification predictor, which will be described later, the prediction specification integrator 11 integrates them to obtain an integrated prediction vector and an integrated prediction error covariance matrix that is an error covariance matrix. This is the part to be calculated. When there are a plurality of calculated smooth motion specifications, the smooth specification integrator 12 is a part that integrates them and calculates an integrated smooth value and an integrated smooth error covariance matrix. The integrated smooth value memory 13 is configured by a storage element or a circuit that stores an integrated smooth value, an integrated smooth error covariance matrix, and a posteriori probability of each motion model calculated by a motion specification prediction process described later.

  The motion model controller 14 is a part that determines the number of motion models to be employed and the meander frequency to be set for each motion model when performing the tracking process on the tracking target. The motion specification predictors 15-1 to 15-N (N is a natural number of 1 or more) are portions that predict the motion specification of the tracking target by executing a Kalman filter based on a predetermined motion model. In addition, the motion specification predictors 15-1 to 15-N calculate the likelihood that is an evaluation value of the motion model in order to evaluate the likelihood of each motion specification prediction value. When there are a plurality of motion specification predictors, that is, when N is 2 or more, the mixing smoother 16 predicts other motion specification with respect to the motion specification smooth value calculated by each motion specification predictor. It is a part which performs the adjustment process based on the movement specification smooth value calculated by the vessel. More specifically, the mixing smoother 16 performs a process called mixing on the smoothed value and smoothing error covariance matrix of each motion model. The model likelihood totalizer 17 is a part that calculates the sum of likelihoods used for evaluating the tracking results calculated by the motion specification predictors 15-1 to 15-N. The future position predictor 18 is a part that predicts the future position of the tracking target based on the motion specification smooth value calculated by the smooth specification integrator 12.

  Here, the configuration of the motion specification predictors 15-1 to 15-N will be supplemented. When N is 2 or more, that is, when a plurality of motion specification predictors are provided, each motion specification predictor should use a motion model different from other motion specification predictors. It is advantageous. Each of the motion specification predictors 15-1 to 15-N uses a plurality of different types of motion models, so that the difference between the tracking processing result of each motion model and the observed value of the actual tracking target can be increased or decreased. Will occur. Naturally, in this case, the processing accuracy of the motion model that calculates the tracking processing result close to the observed value is superior to the processing accuracy of the other motion models. By adopting a plurality of types of motion models, the processing accuracy can be improved over the tracking processing using a single motion model. Thus, the motion model controller 14 plays a role of determining which motion model each of the motion specification predictors 15-1 to 15-N performs the tracking process.

  However, it is not essential that the number of motion specification predictors be two or more. Even if at least one motion specification predictor is provided, the features of the present invention not found in the prior art can be achieved as will be apparent from the following description. It is. Therefore, the number of motion specification predictors, that is, the value of N is a design matter. That is, a simple configuration may be adopted as N = 1 (single motion model). In that case, the prediction value calculated by the motion specification predictor 15-1 may be output as the output value of the target tracking device according to Embodiment 1 of the present invention. The functions corresponding to the unit 12, the mixed smoother 16, and the model likelihood totalizer 17 are omitted.

  On the other hand, when it is assumed that the target to be tracked performs meandering motions with various meandering frequencies, it is advantageous to perform motion specification prediction based on a plurality of motion models. As the number of motion specification predictors, that is, the number of N is larger, the tracking process with higher accuracy can be performed and a wide range of meandering frequencies can be covered. Here, including N motion specification predictors means not only physically including N motion specification predictors that are physically separate, but also of less than N motion specification predictors. Each of them may time-divide one sample time and perform tracking processing based on a plurality of motion models, and as a result, tracking processing based on N motion models may be performed.

  FIG. 2 is a block diagram showing a detailed configuration of the motion specification predictor 15-1. As shown in the figure, the motion specification predictor 15-1 includes a smooth specification memory 21, a setter 22, a predictor 23, and a smoother 24. The motion model controller 14 is set by these components. A Kalman filter is executed based on the motion model. In general, the Kalman filter calculates a predicted value at a predetermined time (this is referred to as a sample here), samples and acquires an observed value at each time, obtains a smooth value from the predicted value and the observed value, and A loop process is performed in which the sample is subjected to prediction value calculation. The motion specification predictor 15-1 executes these loop processes in cooperation with the external prediction specification integrator 11, the smooth specification integrator 12, the mixed smoother 16, and the model likelihood aggregator 17. It is like that.

  The smoothing specification memory 21 is a storage circuit or element that stores the smoothing value and smoothing error covariance matrix at the previous sampling time for the motion model that the motion specification predictor 15-1 uses as the basis of the tracking process. Further, it may be constituted by a storage device using a storage medium such as a hard disk device or a CD-ROM drive device. Here, the smoothed value and smoothing error covariance matrix of the previous sample are calculated by the mixing smoother 16 and stored in the smoothing specification memory 21. The setter 22 is a part that calculates various parameters from the motion model assigned by the motion model controller 14 to the motion specification predictor 15-1. The various parameters are parameters including a state transition matrix, a meander frequency, and the like, and are parameters necessary for starting the tracking process.

  The predictor 23 is a part that calculates the motion specification prediction value in the current sample from the smooth value and smoothing error covariance matrix of the previous sample and the motion model. The motion specification prediction value calculated here is output to the prediction specification integrator 11. The smoothing value and smoothing error covariance matrix of the previous sample used here are stored in the smoothing specification memory 21, and the motion model is stored in the setting unit 22.

  The smoother 24 is a part that performs a smoothing process using the motion specification predicted value in the current sample calculated by the predictor 23 and the observed value of the tracking target that the gate determiner 10 determines to be correlated. . The motion specification smooth value calculated here is output to the smooth specification integrator 12.

  Further, the motion specification predictor 15-1 includes a probability memory 25, an a priori probability calculator 26, a likelihood calculator 27, and a posterior probability calculator 28, and the motion specification predictor 15- The evaluation value (prior probability, likelihood, posterior probability, etc.) of the exercise model on which 1 is based is calculated. The probability memory 25 is a storage circuit or element that stores the posterior probability in the previous sample. The prior probability calculator 26 is a part that calculates the prior probability of the current sample based on the posterior probability of the previous sample stored in the probability memory 25. The prior probabilities calculated here are output to the prediction data integrator 11. The likelihood calculator 27 includes the prior probability calculated by the prior probability calculator 26, the motion specification predicted value calculated by the predictor 23, the observed value of the tracking target output by the gate determiner 10 determined to be correlated, and This is a part for calculating the likelihood from. The likelihood calculated here is output to the model likelihood totalizer 17. The posterior probability calculator 28 is a part that calculates the posterior probability from the likelihood calculated by the likelihood calculator 27. The posterior probability calculated here is stored in the probability memory 25 as the posterior probability of the current sample, and is used for calculation of the prior probability, likelihood, and posterior probability in the next sample.

  Also in the motion specification predictors 15-2 to 15-N, the smooth specification memory 21, the setting device 22, the predictor 23, the smoother 24, the probability memory 25, the prior probability calculator 26, and the likelihood calculator 27. Although there are components corresponding to the posterior probability calculator 28, since these components are the same as those of the motion specification predictor 15-1, description thereof will be omitted. Further, in the following description, when particularly referring to the components of the motion specification predictors 15-2 to 15-N, for example, "smooth specification memory 21 of the motion specification predictor 15-2" Let us explicitly indicate the sign of the motion specification predictor. Further, when the “smooth specification memory 21” is simply specified without specifying the code of the motion specification predictor, the probability memory 25 in the motion specification predictor 15-1 is indicated. The same applies to the setting device 22, the predictor 23, the smoother 24, the probability memory 25, the prior probability calculator 26, the likelihood calculator 27, and the posterior probability calculator 28.

  Further, in the description so far, for example, a component that is a “part” such as “a part that calculates a predicted value” means that it is configured as a dedicated circuit or element for that purpose. However, a configuration in which a similar function is executed by a computer or a microcomputer chip having a CPU (Central Processing Unit) by a computer program may be employed.

  Furthermore, the gate determination unit 10 is an example of a gate determination unit, and the prediction item integration unit 11 is an example of a prediction item integration unit. Further, the smooth specification integrator 12 is an example of smooth specification integration means, and the integrated smooth value memory 13 is an example of integrated smooth value storage means. The motion model controller 14 is an example of motion model control means, and the motion specification predictors 15-1 to 15-N are examples of motion specification prediction means. The mixed smoother 16 is an example of a mixed smoothing unit, and the model likelihood totaling unit 17 is an example of a model likelihood totaling unit.

  Next, the operation of the target tracking device according to Embodiment 1 of the present invention will be described. As can be understood from the above description, this target tracking device assigns a motion model to each of the motion specification predictors 15-1 to 15-N, performs a tracking process using a Kalman filter, and calculates the motion calculated by them. Predicted specification values, motion specification smooth values and probabilities are used as prediction specification integrator 11, smooth specification integrator 12, integrated smooth value memory 13, motion model controller 14, mixed smoother 16, model likelihood aggregator. 17 is adjusted. Therefore, in the following description, the tracking process in the motion specification predictor 15-1 will be mainly described in order, and the prediction specification integrator 11, the smooth specification integrator 12, the integrated smooth value memory 13, the motion model. About the process in the controller 14, the mixing smoother 16, and the model likelihood totalizer 17, the motion specification and the probability value calculated by the motion specification predictor 15-1 and the motion specification predictors 15-2 to 15-N. It will be explained in a situation where adjustment with the motion specifications and probability values calculated by is required.

  First, when starting the tracking process, the initial value of the smooth value of each motion model and the initial value of the smooth error covariance matrix are set in the integrated smooth value memory 13. In addition, initial values of the posterior probabilities of the motion models assigned to the motion specification predictors are set in the probability memories 25 of the motion specification predictors 15-1 to 15-N. The tracking process starts with these preparations completed. Thereafter, each time a new observation value is obtained, the tracking process is executed while the sampling time is advanced one by one.

The motion model controller 14 acquires the tracking target state variables and model probabilities from the smooth specification integrator 12 via the integrated smooth value memory 13. Then, the number of models necessary for obtaining stable tracking accuracy and the meandering frequency to be set are determined, and a motion model is set in the setting unit 22 of the motion specification predictors 15-1 to 15-N. Here, the motion model set by the motion model controller 14 is a motion model x (t) having a sine motion component and a constant velocity linear motion component according to equations (1) to (3).

As shown in Equation (1), x (t) has a component x ₁ (t) and a component x ₂ (t). The component x ₁ (t) is a component representing sinusoidal motion, and x ₂ (t) is a component representing constant velocity linear motion. n _t is the maximum amplitude of sinusoidal motion and ω is the meander frequency of sinusoidal motion. Further, a is the velocity in the constant velocity linear motion component, and b is the initial velocity at t = 0.

  Equation (1) expresses a motion model in one axis. However, in the case of an actual tracking target, a two-dimensional motion is obtained when a plane motion is performed, such as an automobile or a ship. Is a three-dimensional motion. Therefore, the motion model given by equation (1) is set independently for each dimension. However, here, in order to facilitate understanding, description will be made based on a one-dimensional motion model.

  When there are a plurality of motion specification predictors, the motion model controller 14 sets a plurality of motion models in which the value of ω in Equation (2) is changed as setters of the motion specification predictors 15-1 to 15-N. Set to 22 respectively. For example, when the meandering cycle that can be taken by the target tracking target is 10 to 60 seconds, the meandering frequency is 1/10 to 1/60 Hz. Therefore, when N = 11 (when 11 motion parameter predictors are provided), 1/10, 1/15, 1/20, 1/25, 1/30, 1/35, 1 / What is necessary is just to set (omega) like 40, 1/45, 1/50, 1/55, 1/60 to each setting device 22 of motion specification predictors 15-1 to 15-11. However, other meandering frequencies may be selected. For example, 1/10, 1/20, 1/25, 1/30, 1/33, 1/36, 1/39, 1/42 , 1/45, 1/50, 1/60, etc., the resolution of some frequency sections may be made finer.

Next, the exercise model will be described. The motion model according to Equation (1) is a motion model in continuous time, but it is more appropriate to set a motion model for each sample time, that is, in discrete time, in the Kalman filter. Therefore, a motion model in discrete time is derived from Equation (1). From equations (2) and (3), differential equations of equations (4) and (5) are obtained.

等速直線運動成分については、

となる。ここで、ｘ₁'(t)、ｘ₂'(t)はそれぞれｘ₁(t)、ｘ₂(t)の時間による１階微分を表す。さらにｘ ₁(t)、ｘ ₂(t)は正弦運動成分と等速直線運動成分それぞれの状態変数を表しており、ｘ ₁'(t)、ｘ ₂'(t)はｘ ₁(t)、ｘ ₂(t)の各成分を時間微分したものである。Ｆ₁とＦ₂はそれぞれの成分の連続時間における状態遷移行列を表しており、Ｄ₁とＤ₂は駆動雑音の変換行列を表している。ｗ₁(t)とｗ₂(t)はそれぞれの成分の駆動雑音である。 When these differential equations are expressed in the state space,

For constant velocity linear motion components,

It becomes. Here, x ₁ ′ (t) and x ₂ ′ (t) represent first-order derivatives with respect to time of x ₁ (t) and x ₂ (t), respectively. Furthermore, x ₁ (t) and x ₂ (t) represent the state variables of the sinusoidal motion component and the constant velocity linear motion component, respectively, and x ₁ '(t) and x ₂ ' (t) are x ₁ (t) , X ₂ (t) are time differentiated components. F ₁ and F ₂ represent state transition matrices of the respective components in continuous time, and D ₁ and D ₂ represent drive noise conversion matrices. w ₁ (t) and w ₂ (t) are drive noises of the respective components.

ただし、
ｗ(t)＝[０ ｗ₁(t) ０ ｗ₂(t)]^Ｔ
であって、ｗ(t)は駆動雑音であり、式（１７）、式（１８）を満たす連続時間の白色雑音過程である。

上記において、δ(t)はディラックのデルタ関数であり、τは次の条件を満たす時刻である。すなわち、ｔ≠τの場合にディラックのデルタ関数の値が０となり、ｔ＝τの場合にディラックのデルタ関数の値が無限大となる。また式（１９）において、ｑ_１、ｑ_２は白色雑音過程のパワースペクトル密度であり、［ｍ^２／ｓ^３］の次元を持つ（ｍは距離の次元であり、ｓは時間の次元である）。 Here, summarized in sinusoidal motion component of the state variable x ₁ (t) and a constant speed linear motion component of the state variable x ₂ (t) and one of the state variables x (t). Ie
x (t) = [ x ₁ (t) x ₁ '(t) x ₂ (t) x ₂ ' (t)] ^T
It is. Then, from Equation (6) to Equation (13), the following continuous differential equation of state for x (t) is obtained.

However,
w (t) = [0 w ₁ (t) 0 w ₂ (t)] ^T
Where w (t) is drive noise, and is a continuous-time white noise process that satisfies Equations (17) and (18).

In the above, δ (t) is a Dirac delta function, and τ is a time that satisfies the following condition. That is, the value of the Dirac delta function is 0 when t ≠ τ, and the value of the Dirac delta function is infinite when t = τ. In Equation (19), q ₁ and q ₂ are power spectral densities of the white noise process, and have a dimension of [m ² / s ³ ] (m is a dimension of distance, and s is a dimension of time. ).

ここで、Ｈは観測行列を、Ｇ_２は観測が行われる極座標から北基準直交座標への座標変換行列を表している。またｚ(t)は観測値である。ｖ(t)は観測雑音ベクトルであって、平均０、共分散Ｒの白色ガウス雑音である。 On the other hand, the observation equation of the tracking target in continuous time is given by Equation (20) and Equation (21).

Here, H is the observation matrix, G ₂ denotes a coordinate transformation matrix to the north reference orthogonal coordinates from polar coordinates observation is performed. Z (t) is an observed value. v (t) is an observation noise vector, which is white Gaussian noise having an average of 0 and a covariance R.

Expressions (14) to (21) are represented by a discrete time block diagram as shown in FIG. 3, for example. In FIG. 3, s represents a complex variable in Laplace transform, that is, a differential operator. The y _sin position of the tracking target in the sine motion component, y _st the position of the tracking target in the uniform linear motion components, v _n is a measurement noise, v _sin, v _st is sinusoidal motion component and a constant speed rectilinear motion It represents the drive noise in each of the components. Furthermore, z is an observed value. As is clear from this figure, by expressing the time variation of the position of the meandering target by a motion model having a constant velocity linear motion component and a sinusoidal motion component, it is not necessary to estimate acceleration and jerk terms. The meander frequency can be estimated. Further, by using the estimated value (position and velocity) of the constant velocity linear motion component, the position of the tracking target after K samples (K is a natural number) can be estimated on the central axis of the meandering locus.

となる。なお式（２２）、（２４）において、Φは離散時間における状態遷移行列である。またｘ_1,k、ｘ_2,kは、それぞれ正弦運動成分ｘ₁と等速直線運動成分ｘ₂のｋサンプル目の状態変数を表している。さらにｗ _kは、４次元の離散時間白色雑音系列であって、以下の性質を有する。

ここで、δ_ｋｌはクロネッカーのデルタ関数であって、ｋ＝ｌの場合に値が１となり、ｋ≠ｌで値が０となる。 The k-th sample (k is a natural number) in a discrete time motion model is obtained by finding the solutions of the differential equations of Equations (4) and (5).

It becomes. In equations (22) and (24), Φ is a state transition matrix in discrete time. X _{1, k} and x _{2, k} represent state variables of the kth sample of the sine motion component x ₁ and the constant velocity linear motion component x ₂ , respectively. W _k is a four-dimensional discrete-time white noise sequence and has the following properties.

Here, δ _kl is a Kronecker delta function, and the value is 1 when k = 1 and 0 when k ≠ l.

となる。ここで、ｖ _kは観測雑音ベクトルであり、平均０、共分散Ｒ_kの白色ガウス雑音となる。またＧ_2,kは観測雑音の極座標から北基準直交座標への座標変換行列を表す。以上により、離散時間における運動モデルが導出された。 Moreover, when the observation equation according to the equations (20) and (21) is expressed in discrete time,

It becomes. Here, v _k is an observation noise vector, which is white Gaussian noise having an average of 0 and a covariance R _k . G _{2, k} represents a coordinate transformation matrix from polar coordinates of observation noise to north reference orthogonal coordinates. From the above, a motion model in discrete time was derived.

  Next, the setter 22 calculates a state transition matrix, an error covariance matrix of drive noise, and a meander frequency based on the motion model set by the motion model controller 14 and outputs the calculated state transition matrix to the predictor 23.

The predictor 23 reads the mixed smoothed value and the mixed smoothed error covariance matrix stored in the smoothing specification memory 21, these values and the state transition matrix calculated by the setter 22, the error covariance matrix of the drive noise, and based on the winding frequency, the predicted vector x ^ k of the current motion model, _{a (-)} and the prediction error covariance matrix P _{k, a (-)} is calculated based on the equation (31) and (30) a . Here, the right shoulder subscript ^ of x ^ is represented by the superscript of x on the mathematical expression. The subscript a indicates that it is a state variable for the motion model a.

  The smoothing specification memory 21 stores the initial value of the mixed smoothing value and the mixed smoothing error covariance matrix in the initial state, and in the steady state (the state in which the observation value of at least one sample is processed), The mixed smoothing value of the sample and the mixed smoothing error covariance matrix are stored. The prediction vector and the prediction error covariance matrix obtained here are output to the prediction specification integrator 11, the smoother 24, and the likelihood calculator 27, respectively.

ここで、μ_k,a(−)はｋサンプル目の運動モデルａについての事前確率であり、μ_k,a(＋)はｋサンプル目の運動モデルａについての事後確率であって、Ｐ_abは運動モデルｂから運動モデルａに推移する確率を表す。 Next, the prior probability calculator 26 reads the posterior probability at the previous sample from the probability memory 25, calculates the prior probability Pr [Ψ _{k, a} | Z ^k-1 ] of the current sample by the equation (32), The result is output to the prediction data integrator 11 and the likelihood calculator 27.

Here, μ _{k, a} (−) is the prior probability for the k-th motion model a, μ _{k, a} (+) is the posterior probability for the k-th motion model a, and P _ab Represents the probability of transition from motion model b to motion model a.

Subsequently, the prediction specification integrator 11 includes a motion specification prediction value for each motion model calculated by the predictor 23 and the prior probability calculator 26 of the motion specification predictors 15-1 to 15-N, A prediction error covariance matrix and prior probabilities are acquired, and a prediction vector and a prediction error covariance matrix for each hypothesis of each motion model are integrated, and an integrated prediction vector x ^ _k (−) based on the observation values before one sampling. And the integrated prediction error covariance matrix P _k (−) are calculated by Expression (33) and Expression (34), and these are output to the gate determiner 10.

Next, the gate determiner 10 extracts the integrated prediction vector x ^ _k (−) and the integrated prediction error covariance matrix P _k (in order to exclude data with extremely low observation accuracy from observation values acquired by a sensor (not shown). From-), it is determined whether the observed value is valid data using the residual quadratic form. When the gate determiner 10 determines that the observed value is valid data, the gate determiner 10 outputs the k-th sample observed value z _k to the likelihood calculator 27 and the smoother 24.

ここで、ｇ（ｚ；ａｖ，Ａ）は平均ａｖ、共分散行列Ａの３変量正規分布のｚにおける確率密度関数を表す。またＲ_kは観測雑音ベクトルの共分散行列である。 When the likelihood calculator 27 obtains the observation value z _k of the _k-th sample, the prior probability Pr [Ψ _{k, a} | Z ^k-1 ] of the current sample calculated by the prior probability calculator 26 and the prediction vector x ^ _{Using k, a} (−) and the prediction error covariance matrix P _{k, a} (−), the likelihood of the motion model is calculated from equation (35), and the posterior probability calculator 28 and the model likelihood aggregator 17 are calculated. Output.

Here, g (z; av, A) represents a probability density function at z of the trivariate normal distribution of mean av and covariance matrix A. R _k is a covariance matrix of observation noise vectors.

ここで、右辺の分母はモデル尤度算出器１７が算出した尤度合計値を表しており、また右辺の分子は尤度算出器２７が算出した運動モデルａについてのｋサンプル目の尤度である。 Next, the model likelihood totalizer 17 sums the likelihood of each motion model calculated by the motion specification predictors 15-1 to 15-N to obtain a likelihood total value, and the motion specification predictor 15- 1 to 15-N is output to each posterior probability calculator 28. The posterior probability calculator 28 calculates the posterior probability by the equation (36), outputs the posterior probability to the smoothing specification integrator 12 and the mixing smoother 16, and stores it in the probability memory 25.

Here, the denominator on the right side represents the total likelihood value calculated by the model likelihood calculator 17, and the numerator on the right side is the likelihood of the kth sample for the motion model a calculated by the likelihood calculator 27. is there.

On the other hand, the smoother 24 uses the prediction vector x ^ _ka (−) calculated by the predictor 23 and the prediction error covariance matrix P _{k, a} (−), the smooth vector of the current motion model, the smooth error covariance matrix, and The Kalman gain is calculated by the equations (37), (38), and (39), and is output to the smoothing specification integrator 12 and the mixing smoother 16.

Next, the mixing smoother 16 includes the smoothing vector and the smoothing error covariance matrix calculated by the respective smoothers 24 of the motion specification predictors 15-1 to 15-N, and the motion specification predictors 15-1 to 15-. From the posterior probabilities calculated by the respective N posterior probability calculators 28, the k-1 sample mixed smoothing vector x ^ _{k-1, mix (+)} and the mixed smoothing based on the equations (40) and (41) The error covariance matrix P _{k−1, mix (+)} is calculated and stored in the smooth specification memory 21 of each of the motion specification predictors 15-1 to 15-N.

  The k-1 sample mixed smooth vector and the mixed smoothing error covariance matrix are used to calculate a prediction vector and a prediction error covariance matrix in the k sample loop. In this way, the mixing smoother 16 adjusts the smooth value of the (k−1) th sample by the motion model a using the smooth value of the motion model b other than the motion model a, and the motion model a is based on this smooth value. By calculating the prediction value and smooth value of the k-th sample according to, the likelihood of the prediction process in the other motion model is reflected in the prediction process of the motion model a, and the prediction process by the motion model a is optimized. Can be achieved. However, such processing is not essential, and a configuration may be adopted in which the smoothing vector and smoothing error covariance matrix calculated by the smoother 24 are directly written in the smoothing specification memory.

The smooth specification integrator 12 acquires the smooth vector, the smooth error covariance matrix, and the posterior probability for each of the motion models of the motion specification predictors 15-1 to 15-N, and integrates them according to the equation (43). A smooth vector and an integrated smooth error covariance matrix are calculated.

In addition, the smooth specification integrator 12 uses the posterior probability calculated by the posterior probability calculator 28 of the motion item predictors 15-1 to 15-N and the meander frequency given as a constant in each motion model, You may make it calculate a meandering frequency estimated value from Formula (45).

The integrated smooth vector x ^ _k (+) and the integrated smooth error covariance matrix P _k (+) do not perform estimation of the jerk term, so that sufficient estimation accuracy is already secured. Therefore, these values may be output values of the target tracking device according to the first embodiment of the present invention. In applications where the tracking of the target is maintained and the tracking target is moved to one sample ahead and the radar beam is irradiated, the integrated smoothing vector x ^ k (+) and the integrated smoothing error covariance matrix It is sufficient to use Pk (+) as the output value.

  However, the future position predictor 18 is further provided here to predict the position after K samples to avoid a collision with the tracking target, or to introduce a guidance object (for example, flight) to the predicted position of the tracking target after K samples. It can be used for the purpose of inducing the body. In other words, this target tracking device performs target tracking based on a motion model having a constant velocity linear motion component and a sinusoidal motion component, so it is assumed that the tracking target exists on the central axis of the meandering motion depending on conditions. The target tracking can be performed on the assumption that the target tracking is performed or that the target tracking is present on a meandering locus.

Therefore, for example, when a predetermined point is away from the tracking target, that is, when the distance between the predetermined point and the tracking target is a certain value or more, the point on the central axis of the meandering locus is set as the predicted position of the tracking target. To do. In this way, in the case of an aircraft, collision can be avoided while maintaining the current flight and suppressing time and fuel loss. Specifically: That is, the state variable of the tracked target has a sinusoidal motion component x ₁ and constant-velocity rectilinear motion component x ₂ as given by equation (23), From this, the smoothing position of k-th sample of the tracking target x ^ _{tgt, k} (+) can be expressed by equation (46).

なお、式（４７）において、Ｔはサンプリング間隔であり、またｘ＾_ｋ(＋)とｙ＾_ｋ(＋)はそれぞれ、式（４３）で与えられる統合平滑値のｘ及びｙ座標成分を表している。 When the distance between the predetermined point and the tracking target is a certain value or more, the predicted position after K samples of the tracking target is set as a point on the central axis of the meandering locus, so tracking is performed in two-dimensional tracking on the xy plane. Predicted positions (x ^ _{c, k + K} (−), y ^ _{c, k + K} (−)) after the target K samples are obtained by ignoring the sine motion component, thereby obtaining the equations (47) to (49). ).

In Expression (47), T is a sampling interval, and x ^ _k (+) and y ^ _k (+) represent the x and y coordinate components of the integrated smooth value given by Expression (43), respectively. ing.

On the other hand, when the predetermined point and the tracking target are close to each other, that is, when the distance between the predetermined point and the tracking target is less than a certain value, the future position of the tracking target is calculated as a point on the meandering axis. Also,
It is desirable to calculate as a point on a meandering locus. This is because, for example, when a predetermined point is an obstacle to the movement of the tracking target, a more detailed tracking track of the tracking target is required to predict the danger of collision. For example, when the tracking target is another aircraft or ship and the predetermined point is the own aircraft or own ship, this is useful for predicting the risk of collision and avoiding the collision.

ここで、ｎ＾_px及びｎ＾_pyは正弦運動成分推定値の最大振幅を、ω_x及びω_yはこの目標追尾装置により推定された蛇行周波数を表す。各座標軸で動作するフィルタの正弦運動成分推定値の最大振幅ｎ＾_px及びｎ＾_pyは、蛇行運動の１周期の間（蛇行周期は推定蛇行数波数より換算してＴ＝１／ωとなる）における正弦運動成分の推定位置の最大値が、蛇行運動の最大振幅となる。したがって、最大振幅は蛇行運動の１周期毎に更新され、更新後は１周期の間、その大きさが不変であるものと仮定する。
また、φ_k,x及びφ_k,yはｋサンプル目の正弦関数の位相であって、式（５１）より算出する。

In such a case, it is assumed that the meandering frequency and maximum acceleration of the tracking target are constant until after K samples, and the predicted value of the future position existing on the meandering locus (x ^ _{w, k + K} (−), y ^ _{w, k + K} (−)) is calculated by the equation (50).

Here, n ^ _px and n ^ _py represent the maximum amplitude of the estimated sinusoidal component, and ω _x and ω _y represent the meander frequency estimated by the target tracking device. The maximum amplitudes n ^ _px and n ^ _py of the estimated sinusoidal component of the filter operating on each coordinate axis are for one period of the meandering motion (the meandering period is T = 1 / ω converted from the estimated meandering wave number). ) Is the maximum amplitude of the meandering motion. Therefore, it is assumed that the maximum amplitude is updated every cycle of the meandering motion, and that the magnitude remains unchanged for one cycle after the update.
Φ _{k, x} and φ _{k, y} are the phases of the sine function of the k-th sample _{, and} are calculated from equation (51).

  As is clear from the above, according to the target tracking device of the first embodiment of the present invention, the target tracking is performed based on the motion model having the sine motion component and the constant velocity linear motion component. It is possible to accurately predict the tracking process of the tracking target to be performed.

Embodiment 2. FIG.
Paul Zarchan "Tactical and Strategic Missile Guidance Third Edition" Volume 176, American Institute of Aeronautics and Astronauticsn, Inc..p.12 ~ p.29 p.433 ~ p.439 On the other hand, a model in which an acceleration expressed by a sine function is applied in the vertical direction has been studied. Therefore, let us consider a tracking target that moves by a two-dimensional velocity vector having (v _x , v _y ) as components as shown in FIG. Here, as shown in the figure, it is assumed that tan θ = v _y / v _x and an acceleration vector having a magnitude “a” perpendicular to the vector (v _x , v _y ) is acting. Further, the magnitude of the velocity vector (v _x , v _y ) is assumed to be constant.

In this case, if the components of the acceleration vector are (a _x (t), a _y (t)),
a _x (t) = − n _t cos ωt sin θ (t)
a _y (t) = − n _t cos ωt cos θ (t)
Given in. Where n _t is the maximum amplitude. The components (v _x (t), v _y (t)) when the velocity vector is regarded as a function of time are derived from the definition of θ in FIG.
v _x (t) = v cos θ (t)
v _y (t) = v sin θ (t)
Therefore, when these equations are differentiated with respect to time t,
a _x (t) = v _x '(t) = − v sin θ (t) · θ ′ (t)
a _y (t) = v _y ′ (t) = − v cos θ (t) · θ ′ (t)
It becomes. Therefore, the first-order differential value θ ′ (t) of θ (t) with respect to time t is given by Expression (53).

ここで、φは初期位相である。初期位相φ＝０とした場合、ｖ_ｘ(t)とｖ_ｙ(t)は式（５５）によって与えられる。

By integrating equation (53) at time t, the angle θ (t) is

Here, φ is the initial phase. When the initial phase φ = 0, v _x (t) and v _y (t) are given by equation (55).

From the equation (54), in understanding the relationship between the angle θ (t) and the time variation of v _x , v _y , rather than further expansion by the Fourier series or the like, the concept of FIG. It is easier to understand the relationship. In the figure, the upper graph is a representation of how the time variation of theta (t), the middle graph shows the time variation of v _x, lower graph illustrates a time variation of v _y. As is clear from comparison between the middle and lower graphs, the speed change in the x-axis direction is a second harmonic in the y-axis direction. This is because when the velocity vector swings in the vertical direction in FIG. 4 by a change in the acceleration, double v _x (t) is v _y (t) while the v _y (t) is repeatedly increases and decreases around the zero speed This is because it fluctuates at a frequency of.

  From the above, it is understood that when the acceleration meander frequency is ω, the speed fluctuation of the wavy meander target has a sine motion component due to two frequencies ω and 2ω. Since the sinusoidal motion component of the meandering frequency 2ω gives a change in position in the traveling direction, the influence is small even if the amplitude is ignored when the meandering period is short. However, if the meandering cycle becomes longer, the amplitude also increases, so the influence cannot be ignored, and a motion model having a sinusoidal motion component with a meandering frequency of 2ω is required. The target tracking device according to the second embodiment of the present invention is supposed to track a tracking target that moves by such a motion model, and uses a motion model having a sinusoidal motion component of a meandering frequency 2ω. Is.

  As a block diagram showing the configuration of the target tracking device according to the second embodiment of the present invention, FIG. 1 is used similarly to the first embodiment, but the target tracking device according to the second embodiment of the present invention is the same as that of the first embodiment. Since only the motion model is different, the description of the configuration of each component is omitted.

式（５６）〜式（５９）において、ｘ₁(t)は蛇行周波数ωの正弦運動成分、ｘ₂(t)は蛇行周波数２ωの正弦運動成分、ｘ₃(t)は等速直線運動成分を表しており、ｎ_t1、ｎ_t2はそれぞれｘ₁(t)、ｘ₂(t)の最大振幅を表すものである。またａは等速直線運動の速度成分（傾き）であり、ｂは初期速度（切片）である。 Next, the operation of the target tracking device according to Embodiment 2 of the present invention will be described. Also in the target tracking device according to the second embodiment of the present invention, the same initial processing as that in the first embodiment is performed, and the motion model controller 14 sets the setters 22 of the motion specification predictors 15-1 to 15-N. Set an exercise model. As already described, this target tracking device uses a motion model having a sinusoidal motion component having a meandering frequency of 2ω. This motion model is expressed by equation (56).

In Expressions (56) to (59), x ₁ (t) is a sinusoidal motion component of meander frequency ω, x ₂ (t) is a sinusoidal motion component of meander frequency 2ω, and x ₃ (t) is a constant velocity linear motion component. N _t1 and n _t2 represent the maximum amplitudes of x ₁ (t) and x ₂ (t), respectively. Further, a is a velocity component (slope) of constant velocity linear motion, and b is an initial velocity (intercept).

となる。これらの微分方程式を状態空間において表現すると、

となる。ここで、Ｆ_１、Ｆ_２及びＦ_３はそれぞれ蛇行周波数ωの正弦運動成分、蛇行周波数２ωの正弦運動成分、等速直線運動成分の連続時間における状態遷移行列を、Ｄ_１、Ｄ_２及びＤ_３は駆動雑音の変換行列を表す。またｗ₁(t)、ｗ₂(t)及びｗ₃(t)は、各成分における駆動雑音である。 Next, similarly to Embodiment 1, also in Embodiment 2 of the present invention, a motion model in discrete time is derived from Expressions (56) to (59). When Expressions (56) to (59) are expressed as differential equations, Expressions (60) to (62) are obtained.

It becomes. Expressing these differential equations in state space,

It becomes. Here, F ₁ , F _2, and F ₃ are state transition matrices in continuous time of a sinusoidal motion component of meander frequency ω, a sinusoidal motion component of meander frequency 2ω, and a constant velocity linear motion component, respectively, D ₁ , D _2, and D 3 ₃ represents a drive noise conversion matrix. Further, w ₁ (t), w ₂ (t) and w ₃ (t) are drive noises in each component.

The following continuous differential equation is obtained by combining the equations (63) to (71) into one.

ここで、δ(t)はディラックのデルタ関数である。また、ｑ_１、ｑ_２、ｑ_３は白色雑音過程のパワースペクトル密度で［ｍ^２／ｓ^３］（ｍは距離、ｓは時間）の次元を持つ。 w (t) is drive noise in each component, and is a continuous-time white noise process that satisfies the properties of equations (77) to (79).

Here, δ (t) is a Dirac delta function. Further, q ₁ , q ₂ , and q ₃ are power spectral densities of the white noise process and have dimensions [m ² / s ³ ] (m is a distance and s is a time).

ここで、Ｈは観測行列、Ｇ_２は観測が行われる極座標から北基準直交座標への座標変換行列を表している。またｙ(t)は観測値である。ｖ(t)は観測雑音ベクトルであって、平均０、共分散Ｒの白色ガウス雑音である。 On the other hand, the observation equation of the tracking target in continuous time is given by Equation (80) and Equation (81).

Here, H is the observation matrix, G ₂ denotes a coordinate transformation matrix to the north reference orthogonal coordinates from polar coordinates observation is performed. Y (t) is an observed value. v (t) is an observation noise vector, which is white Gaussian noise having an average of 0 and a covariance R.

Expressions (63) to (81) are represented by a discrete-time block diagram as shown in FIG. 6, for example. In FIG. 6, the same reference numerals or variables as those in the block diagram (FIG. 3) of the first embodiment are the same as those in FIG. y _sin2 represents the position of the tracking target in the sine motion component of the meandering frequency 2ω. As can be seen from this figure, the processing for estimating the acceleration and jerk terms is unnecessary as in the first embodiment, and by using the estimated values (position and velocity) of the constant velocity linear motion component, The position of the tracking target can be estimated on the central axis of the meandering locus.

となる。なお式（８２）、（８４）において、Φは離散時間における状態遷移行列である。またｘ_1,k、ｘ_2,k、ｘ_3,kは、それぞれ正弦運動成分ｘ₁、ｘ₂と等速直線運動成分ｘ₃のｋサンプル目の状態変数を表している。さらにｗ _kは、４次元の離散時間白色雑音系列であって、以下の性質を有する。

The motion model in the discrete time of the kth sample is obtained by finding the solutions of the differential equations of Equations (60) to (62).

It becomes. In equations (82) and (84), Φ is a state transition matrix in discrete time. The _{x 1, k, x 2,} k, x 3, k is each represent a sine motion components x _1, x ₂ and k-th sample of the state variables of the constant velocity linear motion components x _3. W _k is a four-dimensional discrete-time white noise sequence and has the following properties.

となる。ここで、ｖ _ｋは観測雑音ベクトルであり、平均０、共分散Ｒ_ｋの白色ガウス雑音となる。またＧ_2,kは観測雑音の極座標から北基準直交座標への座標変換行列を表す。以上により離散時間における運動モデルが導出された。 In addition, when the observation equations of Equation (80) and Equation (81) are expressed in discrete time,

It becomes. Here, v _k is an observation noise vector, which is white Gaussian noise having an average of 0 and a covariance R _k . G _{2, k} represents a coordinate transformation matrix from polar coordinates of observation noise to north reference orthogonal coordinates. Thus, a motion model in discrete time was derived.

  Subsequently, the motion specification predictors 15-1 to 15-N execute the tracking process using the Kalman filter as in the first embodiment. The prediction specification integrator 11, the smoothing specification integrator 12, the integrated smoothing value memory 13, the motion model controller 14, the mixed smoother 16, and the model likelihood totalizer 17 are motion specification predictors 15-1 to 15-15. The point that the prediction vector, the prediction error covariance matrix, the smoothing vector, the smoothing error covariance matrix, the prior probability, the likelihood, and the posterior probability are adjusted and weighted is calculated by −N. .

ここで、ｘ＾_1,k(＋)及びｘ＾'_1,k(＋)は蛇行周波数ωの正弦運動成分の位置と速度の統合平滑値を表している。また、ｘ＾_2,k(＋)及びｘ＾'_2,k(＋)は蛇行周波数２ωの正弦運動成分の位置と速度の統合平滑値を表している。ｘ＾_3,k(＋)及びｘ＾'_3,k(＋)は等速直線運動成分の位置と速度の統合平滑値を表すものである。 Next, the future position predictor 18 sets the point on the central axis of the meandering locus as the predicted position of the tracking target when the predetermined point and the tracking target are apart from each other by a predetermined value or more. When the distance is less than a certain value, the point on the meandering locus is set as the predicted position of the tracking target. If the motion model according to the second embodiment is used, the smoothing position x ^ _{tgt, k} (+) of the k-th sample of the tracking target is expressed by Expression (89).

Here, x ^ _{1, k} (+) and x ^ ' _{1, k} (+) represent the integrated smooth value of the position and velocity of the sinusoidal motion component of the meander frequency ω. Moreover, x ^ _{2, k} (+) and x ^ ' _{2, k} (+) represent the integrated smooth value of the position and velocity of the sinusoidal motion component of the meandering frequency 2ω. x ^ _{3, k} (+) and x ^ ' _{3, k} (+) represent the integrated smooth value of the position and velocity of the constant velocity linear motion component.

なお、式（９１）において、Ｔはサンプリング間隔であり、またｘ＾_k(＋)とｙ＾_k(＋)はそれぞれ、式（４３）で与えられる統合平滑値のｘ及びｙ座標成分を表している。 When the distance between the predetermined point and the tracking target is a certain value or more, the predicted position (x ^ _{c, k + K} (−), y ^ _c, after the K samples of the tracking target in two-dimensional tracking on the xy plane) _{k + K} (−)) is given by the equations (91) to (93) by ignoring the sine motion component as in the first embodiment.

In Expression (91), T is a sampling interval, and x ^ _k (+) and y ^ _k (+) represent the x and y coordinate components of the integrated smooth value given by Expression (43), respectively. ing.

ここで、ｎ＾_1p,x及びｎ＾_1p,yは蛇行周波数ωの正弦運動成分についての推定値の最大振幅を表す。また、ｎ＾_2p,x及びｎ＾_2p,yは蛇行周波数２ωの正弦運動成分についての推定値の最大振幅を表す。ω_ｘ及びω_ｙはこの目標追尾装置により推定された蛇行周波数を表す。そしてφ_k,x及びφ_k,yはｋサンプル目の正弦関数の位相であって、式（９５）より算出する。

When the distance between the predetermined point and the tracking target is less than a predetermined value, the predicted position (x ^ _{w, k + K} (−), y ^) after K samples of the tracking target in two-dimensional tracking on the xy plane _{w, k + K} (−)) is calculated by the equation (94).

Here, n ^ _{1p, x} and n ^ _{1p, y} represent the maximum amplitude of the estimated value for the sinusoidal motion component of the meander frequency ω. N ^ _{2p, x} and n ^ _{2p, y} represent the maximum amplitude of the estimated value for the sinusoidal motion component of the meandering frequency 2ω. ω _x and ω _y represent meandering frequencies estimated by the target tracking device. Φ _{k, x} and φ _{k, y} are the phases of the sine function of the k-th sample _{, and} are calculated from equation (95).

  As apparent from the above, according to the target tracking device of the second embodiment of the present invention, in addition to the sine motion component (meander frequency ω) and the constant velocity linear motion component, the motion having the sine motion component of the meander frequency 2ω. Since the target tracking is performed based on the model, it is possible to accurately predict the tracking process of the tracking target that performs the wavy meandering motion.

  Note that the target tracking device according to the second embodiment may adopt a configuration in which the output of the smoothing data integrator 12 is used as the output of the target tracking device without going through the future position predictor 18. This target tracking device accurately performs wave-like meandering motion by performing a tracking process based on a motion model that also has a sinusoidal motion component (meander frequency ω), a constant velocity linear motion component, and a sinusoidal motion component of meander frequency 2ω. It has a feature that it can be tracked, and the effect is already secured even when the smoothing specification integrator 12 outputs the integrated smoothing value.

Embodiment 3 FIG.
In the target tracking device according to the first and second embodiments, when performing three-dimensional tracking, the motion model controller 14 determines the motion model predictors 15-1 to 15-N so as to correspond to the respective axes. It was set in each of the setting devices 22. However, by including the inclination and intercept of the center axis of the meandering target in the component of the state variable vector and performing coordinate conversion from the x-axis to the meandering center axis, the motion specifications of the meandering target are estimated and future position prediction is performed. You may do it. The target tracking device according to Embodiment 3 of the present invention has such features.

  The configuration of the target tracking device according to the third embodiment of the present invention is shown in the block diagram of FIG. 1 as in the first embodiment. The motion specification predictors 15-1 to 15-N of the target tracking device according to the third embodiment are different from the first embodiment in that tracking processing is performed by an extended Kalman filter derived from a nonlinear motion model. . However, since the other points are the same as those of the first embodiment, description of the configuration will be omitted.

Next, the operation of the target tracking device according to Embodiment 3 of the present invention will be described. Here, the same initial processing as in the first embodiment is performed, and the motion model is set in the setting units 22 of the motion specification predictors 15-1 to 15-N by the motion model controller 14. Here, assuming that the meandering target is moving on the meandering axis with the inclination θ (t) and the intercept ρ (t) with respect to the x-axis, the position time variation in the direction perpendicular to the meandering central axis (center axis) If the oscillating component with respect to is expressed by a sine function, the continuous differential equation of the motion model is given by the equations (98) to (102).

ここで、δ(t)はディラックのデルタ関数、Ｑは駆動雑音の誤差共分散行列を表す。 w (t) is drive noise, which is a continuous-time white noise process that satisfies the equations (103) and (104).

Here, δ (t) is a Dirac delta function, and Q is an error covariance matrix of drive noise.

ここで、Ｈは観測行列、Ｇ_２は観測が行われる極座標から北基準直交座標への座標変換行列を表している。またｙ(t)は観測値である。ｖ(t)は観測雑音ベクトルであって、平均０、共分散Ｒの白色ガウス雑音である。 On the other hand, the observation equation of the tracking target in continuous time is given by Equation (105) and Equation (106).

Here, H is the observation matrix, G ₂ denotes a coordinate transformation matrix to the north reference orthogonal coordinates from polar coordinates observation is performed. Y (t) is an observed value. v (t) is an observation noise vector, which is white Gaussian noise having an average of 0 and a covariance R.

In the continuous differential equations, the sampling period T, the sampling time t = kT (k = 0,1,2, ...) when the value of x (t) and x _k in discrete-time motion model of the formula (107) Led.

Here, the state variable vector x _k and the state transition matrix Φ in Expression (107) are obtained by the following expressions.

W _{k in} equation (110) is a four-dimensional discrete-time white noise sequence and has the following properties.

ここでＡは以下の座標変換行列を表す。

Since the motion equation of the formula (107) assumes that the target is moving the x-axis, after the state variable vector x _k is the angle -θ rotated in the x-axis direction, when subtracting the sections [rho, The following formula is obtained:

Here, A represents the following coordinate transformation matrix.

となる。ここでｘ _k＝ｇ(ｘ _k-1)とすれば、予測ベクトルｘ＾_k+1(−)及び予測誤差共分散行列Ｐ_k(−)は、

と表される。ただしｘ＾_k-1(＋)はｋ−１サンプル目の平滑ベクトルであって、Ｐ_k(−)は同じくｋ−１サンプル目の平滑誤差共分散行列、Ｑ_k-1は駆動雑音共分散行列を表している。また、Φ_p,k-1は座標変換後の状態遷移行列であり、関数ｇ(ｘ＾_k(＋))の傾きを表すために、以下の式より求められる。

From equation (113)

It becomes. If x _k = g ( x _k-1 ), the prediction vector x ^ _{k + 1} (−) and the prediction error covariance matrix P _k (−) are

It is expressed. Where x ^ _k-1 (+) is the smooth vector of the k-1 sample, P _k (-) is the smooth error covariance matrix of the k-1 sample, and Q _k-1 is the drive noise covariance. Represents a matrix. Further, Φ _{p, k−1} is a state transition matrix after coordinate conversion, and is obtained from the following equation in order to represent the gradient of the function g ( x ^ _k (+)).

となる。ここで、ｖ _ｋは観測雑音ベクトルであり、平均０、共分散Ｒ_ｋの白色ガウス雑音である。 Moreover, when the observation equation according to equation (105) is expressed in discrete time,

It becomes. Here, v _k is an observation noise vector, which is white Gaussian noise having an average of 0 and a covariance R _k .

となる。予測器２３は、実施の形態１の式（３０）と式（３１）に替えて、式（１２０）と式（１２１）を用いて予測ベクトルと予測誤差共分散行列とを算出する。 Based on the above equation of motion and observation equation, an extended Kalman filter used in the target tracking device according to the third embodiment of the present invention is derived. The prediction vector and prediction error covariance matrix of each motion model at time k are

It becomes. The predictor 23 calculates a prediction vector and a prediction error covariance matrix using the equations (120) and (121) instead of the equations (30) and (31) of the first embodiment.

となる。 Next, the smoother 24 calculates a smooth vector, a smooth error covariance matrix, and a Kalman gain. The smooth value, smooth error covariance matrix, and Kalman gain are

It becomes.

なお、Ｔはサンプリング間隔である。 As described above, the smooth value calculated by the smoother 24 may be used as the output value of the target tracking device according to the third embodiment. However, as in the first and second embodiments, the future position predictor 18 is used for tracking. Assuming that the target is on the center axis of the meandering locus or on the meandering locus, the position after K samples is calculated. First, assuming that it exists on the central axis of the meandering locus, the predicted position (x ^ _{c, k + K} (−), y ^ _{c, k + K} (−)) is given by the equations (123) to (126) by ignoring the sine motion component.

T is a sampling interval.

ここで、ｎ＾_pyは正弦運動成分推定値の最大振幅であって、ω＾_yはこの目標追尾装置によって推定された蛇行周波数を表している。またφ_k,yはｋサンプル目の正弦関数の位相であって、式（１２４）から算出される。

Also, assuming that the tracking target is on a meandering locus, the predicted value (x ^ _{c, k + K} (−), y ^ _{c, k + K} (−)) of the future position after K samples (127).

Here, n ^ _py is the maximum amplitude of the estimated sinusoidal component, and ω ^ _y represents the meander frequency estimated by the target tracking device. Φ _{k, y} is the phase of the sine function of the kth sample _{, and} is calculated from the equation (124).

  As is clear from the above, according to the target tracking device according to the third embodiment of the present invention, by using the extended Kalman filter, the filter is not operated independently for each coordinate axis as in the first and second embodiments. However, it is possible to perform three-dimensional tracking and future position prediction in consideration of the correlation of observation noise and the like of each axis.

Embodiment 4 FIG.
Although the target tracking device of the first to third embodiments is based on a motion model having a constant velocity linear motion component and a sine motion component, a dedicated motion specification for performing tracking processing based only on the constant velocity linear motion model. A predictor is newly provided, and when the reliability of the smooth value calculated by the motion specification predictor is equal to or higher than a predetermined value, the motion specification predictors 15-1 to 15-N are stopped or the calculation result is obtained. May not be used. The target tracking device according to the fourth embodiment has such a feature.

  FIG. 8 is a block diagram showing the structure of the target tracking device according to Embodiment 4 of the present invention. In the figure, a constant velocity linear motion specification predictor 19 is a part that performs tracking processing based on a constant velocity linear motion model. The result of the tracking process (predicted value and smooth value) by the constant velocity linear motion parameter predictor 19 is output to the predictive parameter integrator 11, the smooth parameter integrator 12, and the mixed smoother 16, respectively. Yes.

  The reliability determination unit 20 is a part that performs reliability determination based on the posterior probabilities calculated by the constant velocity linear motion specification predictor 19 and other motion specification predictors. The other components having the same reference numerals as those in FIG. 1 are the same as those in the first embodiment and will not be described. However, the posterior probabilities calculated by the motion specification predictors 15-1 to 15-N are reliable. The second embodiment is different from the first embodiment in that it is also output to the degree determiner 20 and the determination result by the reliability determiner is output to the smoothing specification integrator 12.

  Next, detailed configurations of the motion specification predictors 15-1 to 15-N and the constant velocity linear motion specification predictor 19 of the target tracking device according to Embodiment 4 of the present invention will be described. Here, since the motion specification predictors 15-1 to 15-N and the constant velocity linear motion specification predictor 19 are different only in the motion model on which they depend, the components are the same. The motion specification predictor 15-1 will be described as a representative. FIG. 9 is a block diagram showing a detailed configuration of the motion specification predictor 15-1 of the target tracking device according to Embodiment 4 of the present invention.

  In the figure, the posterior probability calculator 28 calculates the posterior probability in the same manner as in the first embodiment. The posterior probability calculated here is added to the smoothing specification integrator 12, the mixed smoother 16, and the probability memory 25. Thus, it is also output to the reliability determination unit 20. The other components having the same reference numerals as those in FIG. 2 are the same as those in the first embodiment, and the description thereof is omitted.

  Next, the operation of the target tracking device according to Embodiment 4 of the present invention will be described. The operation of the motion specification predictors 15-1 to 15-N is the same as that of the first embodiment, and the constant velocity linear motion specification predictor 19 uses a Kalman filter based on a constant velocity linear motion model. Since the target tracking device is already known, the description is omitted. Here, it is assumed that the posterior probabilities are calculated by the respective motion element predictors (including the constant velocity linear motion element predictor 19) by the same processing as in the first embodiment, and the posterior probabilities are used as the reliability. The reliability determination unit 20 determines whether or not the output of the constant velocity linear motion specification predictor 19 is adopted.

For example, when the a posteriori probability calculated by the constant velocity linear motion specification predictor 19 is μ _{k, 1} (+), between M samples (M is a natural number, preferably an integer of 2 or more), When continuously exceeding the threshold value _dμ , the predicted value and smooth value output from the constant velocity linear motion specification predictor 19 are adopted as the output of the target tracking device. Here, to the threshold d _mu may be constant value, it may be calculated based on the posterior probability calculating motion specifications predictor 151 to 15-N.

  In this case, the prediction specification integrator 11 acquires the prediction value of the constant velocity linear motion specification predictor 19 without waiting for the prediction value calculation processing of the motion specification predictors 15-1 to 15-N to be completed. As soon as the operation starts. In addition, the smooth specification integrator 12 does not wait for the output of the smooth values of the motion specification predictors 15-1 to 15-N, and as soon as the constant velocity linear motion specification predictor 19 calculates the smooth value, The output of the smooth value and the future position prediction process are started. By doing so, the time required for the calculation processing of the motion specification predictors 15-1 to 15-N is not required, so that the throughput of the entire target tracking device is improved. Furthermore, since the sampling time can be shortened from this, it becomes possible to process many observation values, and the prediction accuracy is improved.

  Moreover, when the conditions for employing the output result of the constant velocity linear motion specification predictor 19 are satisfied, control may be performed to stop the operation of the motion specification predictors 15-1 to 15-N. . By doing so, for example, when the tracking processing based on a plurality of motion models is performed by time division using a smaller number of central processing units than the number of motion specification predictors, the load on the central processing unit is reduced. Can be reduced.

  In the fourth embodiment, a motion model having two sinusoidal motion components of meandering frequencies ω and 2ω used in the second embodiment is used as a motion model assigned to the motion specification predictors 15-1 to 15-N. May be.

  Needless to say, in the fourth embodiment, the extended Kalman filter used in the third embodiment may be used.

Embodiment 5 FIG.
In the fourth embodiment, the processing of the motion specification predictors 15-1 to 15-N is omitted based on the reliability of the constant velocity linear motion specification predictor 19 that performs the tracking process using the constant speed linear motion model. It was decided to. However, in addition to this, when the maximum amplitude of the tracking target that performs the meandering motion is equal to or smaller than a predetermined value, the processing of the motion specification predictors 15-1 to 15-N is omitted, and the constant velocity linear motion specification predictor. You may make it employ | adopt 19 output values. The target tracking device according to Embodiment 5 of the present invention has such a feature.

  FIG. 10 is a block diagram showing the structure of the target tracking device according to Embodiment 5 of the present invention. As shown in the figure, the target tracking device according to the fifth embodiment of the present invention eliminates the reliability determination device 20 in the fourth embodiment and provides an amplitude determination device 31 instead. The amplitude determiner 31 is a part that determines whether or not the maximum amplitude of the meandering locus among the integrated smooth values calculated by the smoothing specification integrator 12 is equal to or less than a predetermined value. When the amplitude determination unit 31 determines that the maximum amplitude of the meandering trajectory is equal to or less than a predetermined value, the smooth specification integrator 12 adopts the smooth value calculated by the constant velocity linear motion specification predictor 19 as an integrated smooth value. It has become.

  Next, the operation of the target tracking device according to the fifth embodiment of the present invention will be described. Also in the fifth embodiment, the calculation of the smooth value and the calculation of the posterior probability in the motion specification predictors 15-1 to 15-N are the same as those in the fourth embodiment, and thus the description thereof is omitted. When the smooth specification integrator 12 calculates the maximum amplitude of the integrated estimated value from the smooth values calculated by the motion specification predictors 15-1 to 15-N using, for example, Equation (45), this value is calculated. Output to the amplitude determiner 31.

The amplitude determiner 31 determines, for example, whether or not n ^ _{p, x} (meander amplitude in the x-axis direction) and n ^ _{p, y} (meander amplitude in the y-axis direction) satisfy the following conditional expression, and the result is The result is output to the smoothing specification integrator 12.

  When the smooth specification integrator 12 receives the determination result that satisfies the equation (126) from the amplitude determiner 31, the smooth value calculated by the constant velocity linear motion specification predictor 19 is the same as in the fourth embodiment. And the calculation results of the motion specification predictors 15-1 to 15-N are omitted. In that case, the operation of the motion specification predictors 15-1 to 15-N may be stopped as in the fourth embodiment.

  As apparent from the above, according to the target tracking device of the fifth embodiment of the present invention, when the motion of the tracking target can be approximated to the constant velocity linear motion, the constant velocity linear motion model is adopted, and the sine motion Since the tracking process by the motion model having components is omitted, the calculation load can be reduced.

  The present invention can be applied to an apparatus or system that tracks a moving body such as an aircraft, a ship, or an automobile.

It is a block diagram which shows the structure of the target tracking apparatus by Embodiment 1 of this invention. It is a block diagram which shows the detailed structure of the target tracking apparatus by Embodiment 1 of this invention. It is a block diagram by the discrete time of the motion model of the target tracking apparatus by Embodiment 1 of this invention. It is explanatory drawing explaining the relationship between the speed and acceleration of a tracking target made into the object of the target tracking apparatus by Embodiment 2 of this invention. It is explanatory drawing explaining the relationship of the time fluctuation of each speed component of the tracking target used as the object of the target tracking apparatus by Embodiment 2 of this invention of the x-axis direction and the y-axis direction. It is a block diagram by the discrete time of the motion model of the target tracking apparatus by Embodiment 2 of this invention. It is a conceptual diagram which shows the example of the track of a tracking target in Embodiment 3 of this invention. It is a block diagram which shows the structure of the target tracking apparatus by Embodiment 4 of this invention. It is a block diagram which shows the detailed structure of the target tracking apparatus by Embodiment 4 of this invention. It is a block diagram which shows the structure of the target tracking apparatus by Embodiment 5 of this invention.

Explanation of symbols

10 Gate judgment device,
11 Predictive data integration unit,
12 Smoothing specification integrator,
13 Integrated smoothing value memory,
14 motion model controller,
15-1 to 15-N motion specification predictor,
16 mixing smoother,
17 Model likelihood counter,
18 Future position predictor,
19 Constant velocity linear motion specification predictor,
21 Smooth specification memory,
22 Setter,
23 Predictor,
24 smoother,
25 probability memory,
26 Prior probability calculator,
27 Likelihood calculator,
28 posterior probability calculator,
31 Amplitude determiner.

Claims (11)

In the target tracking device that calculates the motion specification smooth value at the current sample from the motion specification smooth value at the previous sample by a Kalman filter,
Based on the motion model of the tracking target that has a constant velocity linear motion component and a sine motion component, it is equipped with motion specification prediction means that calculates the motion specification smooth value at the current sample from the motion specification smooth value at the previous sample. A target tracking device characterized by that. The motion specification prediction means calculates the motion specification smooth value based on the motion model in which the position of the tracking target is represented by the sum of a constant velocity linear motion component and a sine motion component. The target tracking device according to claim 1. The motion specification prediction means calculates the motion specification smooth value based on the motion model further having a sine motion component having a frequency of 2ω, where ω is the frequency of the sine motion component. The target tracking device according to claim 1 or 2, characterized in that In the target tracking device that calculates the motion specification smooth value at the current sample from the motion specification smooth value at the previous sample by the extended Kalman filter,
The motion of the tracking target is expressed by approximating the oscillating component of the meandering locus with respect to the central axis to a sine motion, and based on a motion model using the inclination angle and intercept of the central axis as state variable vectors, A target tracking device comprising a motion specification predicting means for calculating a motion specification smooth value at the time of the current sampling from the motion specification smooth value of. 5. The motion specification prediction unit calculates the motion specification smooth value based on a plurality of the motion models having different frequencies of sinusoidal motion components. 6. Target tracking device. 6. The target tracking device according to claim 5, further comprising smooth feature integration means for integrating a plurality of motion specification smooth values calculated for each motion model by the motion specification prediction means. Model reliability calculation means for calculating the reliability of each of the plurality of motion models used by the motion specification prediction means,
The target tracking device according to claim 6, wherein the smooth specification integration unit integrates the plurality of motion specification smooth values based on the reliability calculated by the model reliability calculation unit. A smoothing unit that adjusts the motion specification smooth value calculated by the motion specification prediction unit at the time of the previous sample based on a motion specification smooth value based on another motion model integrated by the smooth specification integration unit;
The exercise specifications predicting means, that in the calculation of the current sample during exercise specifications smoothed value, instead of the exercise specifications smoothed value of the previous sample, using the motion specifications smoothed value the mixed-smoothing means is adjusted The target tracking device according to claim 6 or 7, wherein A constant velocity linear motion specification predicting means for calculating a smooth motion value of the tracking target based on a constant velocity linear motion model;
The model reliability calculation means calculates the reliability of the motion specification smooth value calculated by the constant velocity linear motion specification prediction means,
The smooth feature integrating means is the motion immediately before the constant velocity when the reliability related to the smooth motion value calculated by the constant velocity linear motion feature predicting means calculated by the model reliability calculating means is a predetermined value or more. 8. The target tracking device according to claim 7, wherein the motion specification smooth value calculated by the specification prediction means is used as an integration result. A constant velocity linear motion specification predicting means for calculating a smooth motion value of the tracking target based on a constant velocity linear motion model;
When the amplitude of the sine motion component included in the track of the tracking target is equal to or less than a predetermined value, the smooth feature integration unit sets the motion specification smooth value of the immediately preceding constant motion specification prediction unit as an integration result. The target tracking device according to any one of claims 6 to 8, wherein the target tracking device is characterized. Position between K samplings by multiplying the time displacement of the position component by constant velocity linear motion included in the motion specification smooth value of the current sample calculated by the motion specification prediction means by a sampling interval of K times (K is a natural number). While obtaining the component time displacement,
Calculating the predicted position after K sampling by adding the position component time displacement during the K sampling to the position of the tracking target of the sample this time; and
When the distance between the position component due to the constant velocity linear motion included in the motion specification smooth value and the tracking target is less than a certain value, the frequency and phase of the sine motion included in the motion specification smooth value 2. A future position prediction means for obtaining a time displacement after K sampling of a sine motion and adding the predicted position after the K sampling as a predicted position after the K sampling. 10. The target tracking device according to any one of 10.

Images