JP2005274300A - Target tracking device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To predict a future position of a target from an observation value on the target by using a plurality of Kalman filters based on different motion models. <P>SOLUTION: This device is equipped with a Kalman filter 4 for calculating a first smoothed value and a first predictive value from observed values by using a Kalman filter based on a uniform rectilinear motion model, a Kalman filter 5 for calculating a second smoothed value and a second predictive value from the observed values by using a Kalman filter based on a meandering motion model, a motion determination processor 3 for determining the moving state of the target based on the first and second predictive values, a convergence determiner 7 for determining the converging state of the second smoothed value, and a filter output selector 8 for selecting either the first smoothed value or the second one based on the moving state and the converging state of the target. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a target tracking device that predicts a future position of a target, and more particularly to a technique for selecting an appropriate motion model from a plurality of motion models.

  As a target tracking filter, a Kalman filter (for example, Non-Patent Document 1) based on a constant velocity linear motion model is known. In such a target tracking filter, the future position after N samples is generally predicted from the smooth position vector and smooth velocity vector of the Kalman filter based on the constant velocity linear motion model.

  On the other hand, as a tracking filter for a meandering target, a Kalman filter based on a motion model in which acceleration x "(t) is expressed by a sine function x" (t) = ntsin (ωt + φ) when the tracking target moves meandering is used. Thus, the motion specifications of the tracking target have been predicted (for example, Patent Document 1 and Non-Patent Document 2). Here, x is the position of the tracking target, and x ″ represents the second derivative with respect to time t (x ″ = d2x / dt2). In the following description, x ′ represents a first derivative with respect to time t (x ′ = dx / dt). Also, nt is the maximum amplitude of the meandering motion, ω is the meandering frequency, and φ is the phase difference.

Japanese Patent Laid-Open No. 2002-189519 “Tracking System and Tracking Method” Publication FIG. 4, paragraph 0038 A. Gelb, ed., Applied Optimal Estimation, The M.I.T Press, Cambridge, 1974. "Kalman filter based on multiple motion model for turning (meandering) target tracking", IEICE General Conference, B-2-21, March 2003

When performing future position prediction using a conventional target tracking filter, the exterior position method is used to assume that the target motion follows a predetermined motion model based on the smooth position vector and smooth velocity vector of the tracking filter. Is calculated. Therefore, in order to obtain a good future position prediction result, it is required that the target motion model and the actual motion match. Otherwise, for example, when using a tracking filter that does not match the target motion or when the target motion changes midway, if the target motion model and the actual motion conflict, N samples The future future position prediction accuracy will deteriorate.

An object of the present invention is to perform future position prediction with higher accuracy by selecting an optimal motion model from a plurality of motion models.

The target tracking device according to the present invention calculates a plurality of smooth values from a target observed value using a plurality of Kalman filters based on different motion models, and selects an appropriate smooth value from these smooth values to select the target In the target tracking device that predicts the future position,
Constant velocity linear motion Kalman filter processing means for calculating a first smoothed value and a first predicted value from the observed value using a Kalman filter based on a constant velocity linear motion model;
Meandering Kalman filter processing means for calculating a second smoothed value and a second predicted value from the observed values using a Kalman filter based on a meandering motion model;
Motion discrimination processing means for discriminating the motion state of the target based on the first and second predicted values;
A convergence determining means for determining a convergence state of the second smooth value;
Based on the target motion state determined by the motion determination processing means and the convergence status determined by the convergence determination means, either the first smooth value or the second smooth value is set as the appropriate smooth value. Filter output selection means to select;
It is equipped with.

  As described above, according to the target tracking device of the present invention, the tracking filter that is most advantageous in performing future position prediction by combining the smooth velocity vectors obtained by the Kalman filter based on a plurality of motion models to perform target motion discrimination. Therefore, it is possible to improve the prediction accuracy of the future position.

Embodiments of the present invention will be described below with reference to the drawings.
Embodiment 1 FIG.
FIG. 1 is a block diagram showing the configuration of the target tracking device according to Embodiment 1 of the present invention. In the figure, the observation apparatus 1 detects a signal emitted or reflected by a target for which the system performs future position prediction by a sensor such as an antenna, and performs signal processing on the signal detected by the sensor. It is a device that outputs as detection data. An example of such a device is a radar device that processes radio waves, but may be a sonar device or other device that processes sound wave signals.

  The tracking maintenance device 2 extracts data suitable as an observation value from the target from the detection data obtained by the observation device 1 by gate determination processing, and performs estimation and prediction of motion parameters using a tracking filter, for example, a Kalman filter. This is a part that is output as a motion specification prediction value. In this description and the following description, the term “part” is assumed to be constituted by a dedicated circuit or element for performing such processing, but a general-purpose computer and a computer program for causing the computer to execute predetermined processing are used. You may make it comprise combining.

  The motion specification prediction values output from the tracking maintenance device 2 are a motion discrimination processor 3, a Kalman filter 4 based on a constant velocity linear motion model (hereinafter referred to as Kalman filter 4), and a Kalman filter 5 based on a meandering motion model (hereinafter referred to as Kalman filter 5). Are output to the Kalman filter 6 based on the multiple motion model (hereinafter referred to as the Kalman filter 6).

  The motion discrimination processor 3 is a part that classifies the target motion state based on the processing results output by the Kalman filter 4 based on the constant velocity linear motion model and the Kalman filter 5 based on the meandering motion model. The motion discrimination processor 3 is an example of motion discrimination processing means. FIG. 2 is a block diagram showing a detailed configuration of the motion discrimination processor 3. In the figure, a residual quadratic form calculator 31-1 is a prediction value (which may actually be a vector value composed of a combination of a plurality of values) that is a processing result of the Kalman filter 4, and a prediction error covariance matrix. This is a part for calculating the residual quadratic form from the observation error covariance matrix. Similarly, the residual secondary form calculator 31-2 is a part that calculates the residual secondary form from the processing result of the Kalman filter 5. The determination evaluation function calculators 32-1 and 32-2 are portions that calculate a determination evaluation function that is an average value during a certain sampling based on the calculated residual quadratic form. The exercise determination unit 33 is a part that determines the target exercise state based on the determination evaluation function obtained from the determination evaluation function calculators 32-1 and 32-2. More specifically, the determination evaluation function obtained from the determination evaluation function calculators 32-1 and 32-2 is compared with a predetermined threshold, and the target state is determined by combining the threshold determination results. “Constant linear motion”, “meandering motion”, and “maneuver motion other than meandering” are discriminated.

  Subsequently, the components shown in FIG. 1 will be described. The Kalman filter 4 is an example of a constant velocity linear motion Kalman filter processing means. Based on the constant velocity linear motion model, the predicted value and the prediction error are calculated for each sampling from the motion specification predicted value output by the tracking maintaining device 2. This is a part for calculating the variance matrix and the observation error covariance matrix. FIG. 3 is a block diagram showing a detailed configuration of the Kalman filter 4. As illustrated, the Kalman filter 4 includes a delay device 41, a predictor 42, and a smoother 43. The delay unit 41 is a part that holds the processing result of the previous sampling for one sampling and outputs it for the current sampling. Such processing results include a smooth value and a smooth error covariance matrix. The predictor 42 is a part that calculates a prediction value and a prediction error covariance matrix based on a constant velocity linear motion model using the smoothing value and the smoothing error covariance matrix before one sampling obtained from the delay unit 41. The predicted value and the prediction error covariance matrix calculated here are output to the smoother 43 described later together with the motion discrimination processor 3. The smoother 43 is a part that obtains a motion specification prediction value from the tracking maintenance device 2 and calculates a filter gain, a smooth value, and a smooth error covariance matrix. In calculating the filter gain, smooth value, and smooth error covariance matrix, in addition to the motion specification prediction value, the processing result of the predictor 42 is provided, and the processing result of the smoother 43 is returned to the delay unit 41. It is supposed to be.

  Next, the configuration of the Kalman filter 5 will be described. The Kalman filter 5 is an example of meandering motion Kalman filter processing means, and is a part for processing a Kalman filter based on a meandering motion model. Here, the meandering motion is a motion that moves in the vicinity of a certain straight line (referred to as a reference straight line), and means a motion that crosses the reference straight line at a predetermined cycle. Therefore, the meandering motion is expressed by the frequency (meandering frequency) crossing the reference line, and the inclination and intercept of the reference line. In particular, the meandering motion model on which the Kalman filter 5 is based is assumed to include a constant velocity linear motion component and a sinusoidal motion component.

  As a meandering motion model used for target tracking, it is realistic to prepare a plurality of meandering frequencies in advance. Accordingly, the Kalman filter 5 outputs the tracking maintaining device 2 based on a meandering motion model meandering at meander frequencies ω1 to ωN (N is a natural number of 1 or more) (hereinafter, each motion model is referred to as model 1 to model N). A predicted value, a prediction error covariance matrix, and an observation error covariance matrix are calculated for each sampling from the motion specification predicted value. FIG. 4 is a block diagram showing a detailed configuration of the Kalman filter 4. As shown in the figure, the Kalman filter 5 has a system (delayer, mixer, predictor, smoother) that processes the Kalman filter for each motion model having a predetermined meandering frequency. For example, the processing system 5-N for the motion model with the meander frequency ωN includes a delay unit 51-N, a mixer 52-N, a predictor 53-N, and a smoother 54-N (N is a natural number of 1 or more). ing.

  Among them, the delay unit 51 -N is a part that holds the processing result of the previous sampling and outputs it to the current sampling, like the delay unit 41 in the Kalman filter 4. The mixer 52-N follows the assumption that the meander frequency of the target meander motion transitions to the meander frequency of the other model during one sampling based on the first order Markov process and This is a part for calculating the mixed smoothing value and the mixed smoothing error covariance matrix from the smoothing error covariance matrix and the posterior reliability obtained from the reliability calculator 57. The predictor 53-N uses the mixed smoothing value and the mixed smoothing error covariance matrix calculated by the mixer 52-N based on the motion model represented by the synthesis of the constant velocity linear motion and the sine function motion. And a part for calculating a prediction error covariance matrix. Similar to the smoother 43 of the Kalman filter 4, the smoother 54 -N is a part that acquires a motion specification prediction value from the tracking maintaining device 2 and calculates a filter gain, a smooth value, and a smooth error covariance matrix.

  In the Kalman filter 5 shown in FIG. 4, the delay unit 51 -N, the mixer 52 -N, the predictor 53 -N, and the smoother 54 -N are arranged separately for each individual meander frequency. Although it is configured, a part or all of these parts may be shared between the motion models with meandering frequencies ω1 to ωN. In the case where these parts are shared among the motion models with the meandering frequencies ω1 to ωN, for example, a selector or a switch may be provided to switch the input / output of the parts between the respective motion models at high speed.

  Likelihood calculator 56 is the likelihood from model 1 to model N from the prediction value and prediction error covariance matrix output from predictors 53-1 to N and the motion specification prediction value output from tracking maintenance device 2. It is a part which calculates. The reliability calculator 57 is a part that calculates the posterior reliability from the model 1 to the model N based on the likelihood from the model 1 to the model N and the posterior reliability before one sampling. The integrated smoother 59 is based on the smoothed value and smoothing error covariance matrix for each motion model output from the smoothers 54-1 to N and the posterior reliability obtained from the reliability calculator, and the integrated smoothing value and the integrated smoothing error. This is a part for calculating the covariance matrix. The integrated predictor 58 uses the prediction value and prediction error covariance matrix for each motion model output from the predictors 53-1 to 53 -N, the posterior reliability obtained from the reliability calculator, and the predetermined model transition probability, and the prior reliability. This is a part for calculating the integrated prediction value and the integrated prediction error covariance matrix. Since the Kalman filter based on the meandering motion model processes meandering motion on one axis (for example, the x coordinate axis), two or three such tracking filters are used for a two-dimensional space or a three-dimensional space. It will be.

  Next, the configuration of the Kalman filter 6 will be described. The Kalman filter 6 is an example of multi-motion Kalman filter processing means, and is a part for processing a Kalman filter based on a multi-motion model. That is, the Kalman filter 6 calculates a predicted value for each acceleration using a constant acceleration model with one or more accelerations α1 to αN (N is a natural number of 1 or more), and estimates the turning acceleration based on the model reliability. It has become. By adopting such a Kalman filter based on the multiple motion model, it is possible to improve the follow-up performance for maneuver motion such as turning.

  FIG. 5 is a block diagram showing a detailed configuration of the Kalman filter 6. In the figure, a delay unit 6 is a part that holds the processing result of the previous sampling and outputs it to the current sampling. The Kalman filter 6 performs a prediction process and a smoothing process using a motion model (model 1 to model N) with accelerations α1 to αN, respectively, but the delay device 61 of the Kalman filter 6 is shared by a plurality of motion models. ing. The previous sampling processing results such as the smoothed value and smoothing error covariance matrix output from the delay device 61 are output to the processing systems 62-1 to 62-N for each motion model of the accelerations α1 to αN. Each processing system 62-1 to N is provided with predictors 63-1 to N, and is a part for calculating a prediction value and a prediction error covariance matrix based on a motion model represented by each acceleration.

  The likelihood calculator 64 is the likelihood of the models 1 to N based on the prediction values and prediction error covariance matrices output from the predictors 63-1 to 63-N and the motion specification prediction values output from the tracking maintenance device 2. It is a part which calculates. The reliability calculator 65 is a part that calculates the posterior reliability for each motion model based on the likelihood for each motion model and the posterior reliability before one sampling. The integrated predictor 66 calculates the integrated predictor and the integrated prediction error based on the integrated smoothing value before sampling obtained by the delay unit 61, the integrated smoothing error covariance matrix, and the prior reliability calculated by the reliability calculator 65. This is a part for calculating the covariance matrix.

  The integrated smoother 67 uses the integrated prediction value calculated by the integrated predictor 66, the integrated prediction error covariance matrix, and the posterior reliability calculated by the reliability calculator 65, so that the filter gain, the integrated smooth vector, and the integrated smooth error covariance matrix. It is a part which calculates. The calculated integrated smoothing vector and integrated smoothing error covariance matrix are output to the delay unit 61 as a calculation result of the current sampling and output to the outside as an output result of the Kalman filter 6.

  The convergence determination unit 7 is an example of a convergence determination unit, and is a part for determining a stable state of a smooth value calculated by the Kalman filter 5 based on a meandering motion model (for example, whether a smooth velocity vector is stable). . When the smooth velocity vector of the Kalman filter 4 based on the constant velocity linear motion model is compared with the smooth velocity vector of the constant velocity linear motion model in the Kalman filter 5 based on the meandering motion model, the constant velocity in the Kalman filter 5 based on the meandering motion model immediately after the start of tracking is compared. It tends to take time for the smooth velocity vector of the linear motion model to stabilize. For this reason, if the smooth velocity vector of the constant velocity linear motion model is used for the future position prediction at the unconvergence stage, the future position prediction accuracy deteriorates. In order to avoid such a problem, the convergence determination unit 7 is provided to determine the convergence of the smoothing velocity vector calculated by the Kalman filter 5 and when the smoothing velocity vector calculated by the Kalman filter 5 is not used, the smoothing velocity vector is not used. I am doing so.

  FIG. 6 is a block diagram illustrating a detailed configuration of the convergence determination unit 7. In the figure, a velocity vector angle calculator 71 is a part that calculates an angle formed by the smooth velocity vectors of the constant velocity linear motion model at two adjacent sampling times k and k−1. The angle evaluation function calculator 72 calculates an angle evaluation function that is an average value of angles at a constant sampling number. The threshold determination unit 73 is a part that determines that the smooth velocity vector of the constant velocity linear motion model has converged when the angle evaluation function falls below the threshold. The determination result memory 74 is a storage element or circuit that stores an angle evaluation function and a convergence determination result.

  The filter output selector 8 is an example of filter output selection means, and is a part that selects an appropriate smooth value based on the motion discrimination result output by the motion discrimination processor 3. Specifically, when the motion discriminating processor 3 determines “constant linear motion”, the smooth value of the Kalman filter 4 based on the constant linear motion model is used. When the smooth value of the Kalman filter 5 is determined as “maneuver motion other than meandering motion”, the smooth value of the Kalman filter 6 based on the multiple motion model is selected. Note that the filter output selector 8 operates by incorporating the determination result of the convergence determiner 7 until the convergence determiner 7 determines that the smooth value of the Kalman filter 5 has converged after the start of tracking. Does not adopt the calculation result of the Kalman filter 5. The future position predictor 9 is a part that calculates the future position after N sampling by the extrapolation method from the smooth position and smooth velocity vector obtained from the filter output selector 8.

  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 performs tracking processing by the Kalman filter 4 based on the constant velocity linear motion model, the Kalman filter 5 based on the meandering motion model, and the Kalman filter 6 based on the multiple motion model. Based on the result of the motion discrimination process using the difference and the convergence determination result of the Kalman filter based on the meandering motion model, the filter output is selected and the future position is predicted. The operation of each filter and convergence determination unit 7 and filter output selector 8 will be described below.

(Kalman filter based on constant velocity linear motion model)
First, processing in the Kalman filter 4 will be described. The Kalman filter 4 based on the constant velocity linear motion model calculates a smooth and prediction vector and a smooth and prediction error covariance matrix at each sampling time. The motion model of the Kalman filter 4 based on the constant velocity linear motion model in the north reference orthogonal coordinate system is expressed by the equations (1) to (4).

Where x _k is a state vector at sampling time t _k , Φ _i is a state transition matrix from sampling time t _k to t _{k + 1} , w _k is a driving noise vector at sampling time t _k , and Γ _k is driving noise conversion A matrix, τ _k is a sampling interval, and I _{3 × 3} is a 3 × 3 unit matrix. Note that the driving noise vector w _k is assumed to be white noise according to an n-variable normal distribution with an average of 0 and a covariance matrix Q _i .

Next, the radar observation model in the north reference orthogonal coordinate system is expressed by Expression (5) and Expression (6).

Here, z _k is an observation vector at sampling time t _k , H is an observation matrix, Λ _k is a conversion matrix from the radar observation coordinate system to the filter coordinate system, and ν _k is an observation noise vector at sampling time t _k. .

なお、文章による説明で用いるｘ^という表記の^という記号は、数式上はｘの上に表示されるものとする。 From the motion model and the observation model shown in the equations (1) to (6), the Riccati equation for the prediction process and the smoothing process shown below is derived. When the smoothing vector x ^ _k-1 (+) and the smoothing error covariance matrix Pk (+) are obtained by the tracking process up to the sampling time t _k−1 , the predictor 42 calculates the equation (7). And based on (8), the prediction vector x ^ _k (−) calculates the prediction error covariance matrix P _k (−).

It should be noted that the symbol “^” in the notation “x ^” used in the explanation by text is displayed above “x” in the mathematical expression.

なお、Ｒkは観測誤差共分散行列である。以上が等速直線運動モデルに基づくカルマンフィルタ４における処理である。 Subsequently, the smoother 43 uses the observed value z _k obtained at the sampling time t _k , the prediction vector, and the prediction error covariance matrix to calculate the smooth vector x ^ _k (−) based on the equations (9) and (10). The smooth error covariance matrix P _k (+) calculates the Kalman gain K _k .

Rk is an observation error covariance matrix. The above is the processing in the Kalman filter 4 based on the constant velocity linear motion model.

(Kalman filter based on meandering motion model)
The Kalman filter 5 based on the meandering motion model expresses the target position by meandering motion obtained as the sum of constant velocity linear motion and sinusoidal motion with the motion straight line as an axis. FIG. 7 is a diagram showing the relationship between a sinusoidal motion model, a constant velocity linear motion model, and a meandering motion model in the xt plane. Reference numeral 101 indicates a curve indicating a locus of a target position by a sine function motion model. Reference numeral 102 is a straight line indicating the locus of the target position by the constant velocity linear motion model. Reference numeral 103 is a curve showing a locus by meandering motion obtained by synthesizing a sine function motion model and a constant velocity linear motion model. By expressing the target position by meandering motion obtained by synthesizing the sine function motion model and the constant velocity linear motion model in this way, the position of the target whose average velocity with respect to the x axis is not zero can be expressed accurately. At the same time, even vibrational motion that cannot be expressed by constant velocity linear motion can be expressed with high accuracy.

ここで、ｘ₁(t)は正弦関数運動モデルによる位置成分を、ｘ₂(t)は等速直線運動モデルによる位置成分を、ｎ_tは正弦波の最大振幅を、ωは正弦波の蛇行周波数を表す。また、ａは軸の時間軸に対する傾きを、ｂは切片を表す。 Such a motion model in the continuous time of the meandering motion is expressed by equations (12) to (14).

Where x ₁ (t) is the position component based on the sinusoidal motion model, x ₂ (t) is the position component based on the constant velocity linear motion model, n _t is the maximum amplitude of the sine wave, and ω is the meander of the sine wave. Represents the frequency. Further, a represents the inclination of the axis with respect to the time axis, and b represents the intercept.

  FIG. 8 is a block diagram showing the meandering motion model expressed by the equations (12) to (14). The upper part of this block diagram represents a sine function motion model, and the lower part represents a constant velocity linear motion model. In the meandering function motion model, the time to start meandering is random and is represented by white Gaussian noise.

When the motion model according to the equations (12) to (14) is expressed as a differential equation, equations (15) and (16) are obtained.

ここで、ｘ(t)は等速直線運動成分と正弦関数運動成分の位置及び速度についての状態変数ベクトルを表す。 Further, when the differential equations (15) and (16) are expressed in the state space, the equations (17) to (21) are obtained.

Here, x (t) represents a state variable vector regarding the position and velocity of the constant velocity linear motion component and the sinusoidal motion component.

ここで、ｘ_kはサンプリング時刻ｔ_kの状態ベクトル、Φ_iはサンプリング時刻ｔ_kからｔ_k+1への状態遷移行列、ｗ_kはサンプリング時刻ｔ_kの駆動雑音ベクトル、Γ_kは駆動雑音変換行列である。なお、駆動雑音ベクトルｗ_kは、平均０、共分散行列Ｑ_iのｎ変量正規分布に従う白色雑音と仮定する。 The sampling period is T, the value of x (t) at the sampling time t = kT (k = 0,1,2 ‥ ) and x _k. By setting t ₀ = kT and t = (k + 1) T, the discrete-time motion model in the north reference orthogonal coordinate system is expressed by equations (22) to (24).

Where x _k is a state vector at sampling time t _k , Φ _i is a state transition matrix from sampling time t _k to t _{k + 1} , w _k is a driving noise vector at sampling time t _k , and Γ _k is driving noise conversion It is a matrix. Note that the driving noise vector w _k is assumed to be white noise according to an n-variable normal distribution with an average of 0 and a covariance matrix Q _i .

Next, the target observation equation in discrete time is expressed by equations (25) and (26).

Here, H ₁ is an observation matrix, Λ _k is a coordinate transformation matrix of observation noise, ν _k is an observation noise vector, white Gaussian noise with mean 0 and covariance R _k . When the observation noises of the distance, the elevation angle, and the azimuth at the sampling time t _k are ν _{R, k} , ν _{E, k} and ν _{By, k} , ν _k is given by the equation (27).

ここで、Ｋ_k,aはカルマンゲイン、Ｈ₁は観測行列、Ｒ_kは観測誤差共分散行列、Λ_kはレーダの観測座標系からフィルタの座標系への座標変換行列である。平滑器５４−１〜５４−Ｎによって算出されたモデル１〜モデルＮの平滑ベクトルｘ^_k,a(-)及び平滑誤差共分散行列Ｐ_k,a(-)は、それぞれ遅延器５１−１〜５１−Ｎを経由して、混合器５２−１〜５２−Ｎに出力される一方で、統合平滑器５５に出力する。 From the motion model and the observation model given by the equations (12) to (17), the Riccati equations for prediction processing and smoothing processing in the models 1 to N of the Kalman filter 5 are derived. First, the smoothers 54-1 to 54-N obtain the smoothing vector x ^ _{k, a} (+) and the smoothing error covariance matrix P _{k, a} (+) for each motion model at the sampling k from the equations (28) to (30). ).

Here, K _{k, a} is a Kalman gain, H ₁ is an observation matrix, R _k is an observation error covariance matrix, and Λ _k is a coordinate transformation matrix from a radar observation coordinate system to a filter coordinate system. The smooth vectors x ^ _{k, a} (−) and the smooth error covariance matrix P _{k, a} (−) of the models 1 to N calculated by the smoothers 54-1 to 54-N are the delay units 51-1, respectively. Are output to the mixers 52-1 to 52-N via the -51-N and output to the integrated smoother 55.

ここで、β_k,b|aは逆推移確率であり、式（３３）により与えられる。

このようにして算出されたモデル１〜モデルＮの混合平滑ベクトル及び混合平滑誤差共分散行列は、モデル１〜モデルＮの予測器５３−１〜５３−Ｎに出力される。 The mixers 52-1 to 52-N use the smoothing vector x ^ _{k, a} (+) and the smoothing error covariance matrix P _{k, a} (+) obtained through the delay units 51-1 to 51-N. Calculate mixed smooth vector x ^ ^mix _{k-1, a} (+) and mixed smooth error covariance matrix P ^mix _{k, a} (+) based on equations (31) and (32) considering transition between models To do.

Here, β _{k, b | a} is the reverse transition probability and is given by equation (33).

The mixed smooth vector and the mixed smoothing error covariance matrix of the models 1 to N calculated in this way are output to the predictors 53-1 to 53-N of the models 1 to N.

ここで、Φ_k-1,aは運動モデル毎の状態遷移行列、Ｑ_k-1,aは運動モデル毎の駆動雑音の誤差共分散行列を表す。Φ_k-1,aは、式（２４）にモデル毎に設定した蛇行周波数ωを代入することにより得られる。 The predictors 53-1 to 53-N are configured to obtain the mixed smooth vector x ^ ^mix _{k-1, a} (+) and the mixed smooth error covariance matrix P ^mix _k, obtained by the tracking process up to the sampling time t _{k-1 .} Based on _a (+), prediction vectors x ^ _{k, a} (-) and prediction error covariance matrix P _{k, a} (-) of model 1 to model N at sampling k are obtained from equations (34) and (35). calculate.

Here, Φ _{k-1, a} represents a state transition matrix for each motion model, and Q _{k-1, a} represents an error covariance matrix of drive noise for each motion model. Φ _{k−1, a} can be obtained by substituting the meander frequency ω set for each model into the equation (24).

ここで、ｇ(ｚ；ａ，Ａ)は平均ａ、共分散行列Ａの３変量正規分布のｚにおける確率密度関数を表す。また、Ｒ_kは観測雑音ベクトルの共分散行列である。 The likelihood calculator 56 calculates the prediction vector for each motion model calculated by the predictors 53-1 to 53-N, the prediction error covariance matrix, the observation value output from the tracking maintaining device 2, and the likelihood calculator 56. Based on the observed error covariance matrix, the likelihood of each model is calculated based on Equation (36).

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

Further, the reliability calculator 57 calculates the posterior reliability β _{k, a} of the motion model from the equation (37).

Here, ν _{k, a} is the likelihood for each filter, and β ^ _{k, a} (−) is the prior reliability obtained from the equation (38).

上記において、Ｐ_a|bは状態遷移確率、β_k-1,bは1サンプリング前の事後信頼度である。このようにして算出された事後信頼度は統合平滑器５５及び混合器５２−１〜５２−Ｎに出力され、事前信頼度は統合予測器５８に出力される。

In the above, P _{a | b} is the state transition probability, and β _{k−1, b} is the posterior reliability before one sampling. The posterior reliability calculated in this way is output to the integrated smoother 55 and the mixers 52-1 to 52-N, and the prior reliability is output to the integrated predictor 58.

ここで、β_k,aは事後信頼度を表す。 The integrated smoother 55 has a smoothing value x ^ _k (+) and a smoothing error covariance matrix P _k (+) calculated by the smoothers 54-1 to 54-N at the sampling k, and a posteriori confidence obtained from the reliability calculator. From the degree β _{k, a} , the integrated smooth vector x ^ _k (+) and the integrated smooth error covariance matrix P _k (+) at the sampling k are calculated based on the equations (39) and (40).

Here, β _{k, a} represents the posterior reliability.

また、推定蛇行周波数は式（４２）より算出される。

ここで、ω_aは各フィルタに事前に設定される蛇行周波数である。 Since the integrated smooth vector x ^ _k (+) is composed of an integrated smooth vector of a constant velocity linear motion model and a sine function motion model, x ^ _{int, k} obtained by adding both vectors as shown in Equation (41). (+) Is the actual target smooth position and smooth velocity vector calculated by the integrated smoother 55.

Further, the estimated meander frequency is calculated from the equation (42).

Here, ω _a is a meander frequency set in advance for each filter.

ここで、ｘ^_k(-)は統合予測ベクトルを、Ｐ_k(-)は統合予測誤差共分散行列を、β^_k,a (-)は運動モデル毎の事前信頼度を表す。以上が蛇行運動モデルに基づくカルマンフィルタ５における処理である。 The integrated predictor 58 includes a prediction vector x ^ _{k, a} (−) and a prediction error covariance matrix P _{k, a} (−) for each motion model calculated by the predictors 53-1 to 53-N _{, a} reliability calculator. From the prior reliability β ^ _{k, a} (−) calculated by 57, the integrated prediction vector x ^ _k (−) and the integrated prediction error covariance matrix P _k (−) are based on the equations (43) and (44). calculate.

Here, x ^ _k (−) represents the integrated prediction vector, P _k (−) represents the integrated prediction error covariance matrix, and β ^ _{k, a} (−) represents the prior reliability for each motion model. The above is the processing in the Kalman filter 5 based on the meandering motion model.

ここで、ｘ_kはサンプリング時刻ｔ_kの状態ベクトル、Φ_kはサンプリング時刻ｔ_kからｔ_k+1への状態遷移行列、ｗ_kはサンプリング時刻ｔ_kの駆動雑音ベクトル、ｕ_kはサンプリング時刻ｔ_kの定数加速度ベクトル、Γ_kは駆動雑音変換行列、Γ'_kは加速度ベクトル変換行列である。なお、駆動雑音ベクトルｗ_kは、平均０、共分散行列Ｑ_kのｎ変量正規分布に従う白色雑音と仮定する。 The Kalman filter 6 based on a multiple motion model calculates predicted values based on a plurality of constant acceleration models, estimates acceleration from the reliability of each motion model, and calculates an integrated smooth value. By performing the acceleration estimation, there is a feature that the tracking performance with respect to maneuver motion such as turning is particularly high as compared with the Kalman filter based on the constant velocity linear motion model. The target motion model in the Kalman filter 6 is calculated based on the equation (45).

Here, x _k the state vector of the sampling time t _k, [Phi _k is the state transition matrix to t _{k + 1} from the sampling time t _k, w _k is the driving noise vector sampling time t _k, u _k is a sampling time t _{k is} a constant acceleration vector, Γ _k is a drive noise conversion matrix, and Γ ′ _k is an acceleration vector conversion matrix. Note that the driving noise vector w _k is assumed to be white noise according to an n-variate normal distribution with an average of 0 and a covariance matrix Q _k .

観測モデルについては、等速直線運動モデルに基づくカルマンフィルタ４と同様である。 Further, the state vector and the state transition matrix are set based on the equations (46) and (47).

The observation model is the same as the Kalman filter 4 based on the constant velocity linear motion model.

ここで、α_aは運動モデル毎の定数加速度ベクトルである。なお、Ｐ_k,a(-)はモデルによらず、統合予測誤差共分散行列Ｐ_k(-)と同じである。このようにして算出された運動モデル毎の予測ベクトル及び予測誤差共分散行列は、尤度計算器６４に出力される。 From these motion model and observation model, the Riccati equations for prediction processing and smoothing processing in the models 1 to N of the Kalman filter 6 are derived. First, the predictors 63-1 to 63-N use the integrated smooth vector x ^ _k-1 (+) and the integrated smooth error covariance matrix P _k (+) calculated by the tracking process up to the sampling time t _k-1. Based on the equations (49) and (50), a prediction vector x ^ _{k, a} (−) for each motion model and a prediction error covariance matrix P _{k, a} (−) for each motion model are calculated.

Here, α _a is a constant acceleration vector for each motion model. Note that P _{k, a} (−) is the same as the integrated prediction error covariance matrix P _k (−) regardless of the model. The prediction vector and the prediction error covariance matrix for each motion model calculated in this way are output to the likelihood calculator 64.

ここで、ｇ(ｚ；ａ，Ａ)は平均ａ、共分散行列Ａの３変量正規分布のｚにおける確率密度関数を表す。また、Ｒ_kは観測雑音ベクトルの共分散行列である。 Next, the likelihood calculator 64 calculates each model from the prediction vector calculated by the predictors 63-1 to 63-N, the prediction error covariance matrix, and the observed value output from the tracking maintaining device 2 according to the equation (51). The likelihood of is calculated.

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

ここで、ν_k,aはモデル１〜モデルＮの尤度、β^_k,a(-)は式（５３）より得られる事前信頼度である。

ここで、Ｐ_a|bは状態遷移確率、β_k-1,bは１サンプリング前の事後信頼度である。ここで算出された事後信頼度は統合平滑器６７に出力され、事前信頼度は統合予測器６６に出力される。 The reliability calculator 65 calculates the posterior reliability of the motion model based on the equation (52) based on the likelihood of each model calculated by the likelihood calculator 64.

Here, ν _{k, a} is the likelihood of model 1 to model N, and β ^ _{k, a} (−) is the prior reliability obtained from equation (53).

Here, P _{a | b} is the state transition probability, and β _{k−1, b} is the posterior reliability before one sampling. The posterior reliability calculated here is output to the integrated smoother 67, and the prior reliability is output to the integrated predictor 66.

ここで、Ｐ_abはモデル遷移確率、β^_k,aは事前信頼度、ｕ^_k-1(-)は予測加速度を表す。 The integrated predictor 66 uses the integrated smoothing value of one sample before obtained through the delay unit 61, the integrated smoothing error covariance matrix, and the prior reliability calculated by the reliability calculator 65 to perform integrated prediction at sampling k. The vector x ^ _k (−) and the integrated prediction error covariance matrix P _k (−) are calculated based on the equations (54) to (56).

Here, P _ab represents the model transition probability, β ^ _{k, a} represents the prior reliability, and u ^ _k-1 (-) represents the predicted acceleration.

ここで、Ｋ_kはカルマンゲイン、Ｈは観測行列、Ｒ_kは観測誤差共分散行列、Λ_kはレーダの観測座標系からフィルタの座標系への変換行列、ｕ^_k-1(+)は加速度推定値を表す。以上が多重運動モデルに基づくカルマンフィルタ６における処理である。 The integrated smoother 67 includes an integrated prediction vector x ^ _k (−) and an integrated prediction error covariance matrix P _k (−) calculated by the integrated predictor 66, an observed value z _k calculated by the tracking maintaining device 2, and a reliability. From the posterior reliability β _{k, a} calculated by the calculator 65, smoothing processing is performed based on the equations (57) to (60).

Where K _k is the Kalman gain, H is the observation matrix, R _k is the observation error covariance matrix, Λ _k is the transformation matrix from the radar observation coordinate system to the filter coordinate system, and u ^ _k-1 (+) is Represents acceleration estimates. The above is the processing in the Kalman filter 6 based on the multiple motion model.

(Exercise judgment processing)
Next, the operation of the motion discrimination processor 3 will be described. The predicted value calculated by the Kalman filter 4 based on the constant velocity linear motion model is output to the residual quadratic form calculator 31-1 of the motion discrimination processor 3. Here, the prediction values calculated by the Kalman filter 4 based on the constant velocity linear motion model include a prediction vector and a prediction error covariance matrix. The observation error covariance matrix is also output to the residual quadratic form calculator 31-1.

ここで、ｚ_kは観測ベクトル、Ｒ_kは観測誤差共分散行列、Λ_kはレーダの観測座標系からフィルタの座標系への変換行列を表す。また、Ｈは観測行列、ｘ^_k,cv(-)は等速直線運動モデルに基づくカルマンフィルタの統合予測ベクトル、Ｐ_k,cv(-)は統合予測誤差共分散行列を表す。 Then, the residual quadratic form calculator 31-1 calculates the residual quadratic form δ _{k, cv} from the observation error covariance matrix based on the equations (61) to (63).

Here, z _k represents an observation vector, R _k represents an observation error covariance matrix, and Λ _k represents a conversion matrix from the radar observation coordinate system to the filter coordinate system. H represents an observation matrix, x ^ _{k, cv} (-) represents an integrated prediction vector of a Kalman filter based on a constant velocity linear motion model, and _{Pk, cv} (-) represents an integrated prediction error covariance matrix.

ここで、ｚ_kは観測ベクトル、Ｒ_kは観測誤差共分散行列、Λ_kはレーダの観測座標系からフィルタの座標系への変換行列を表す。また、Ｈ₁は観測行列、ｘ^_k,wv(-)は蛇行運動モデルに基づくカルマンフィルタの統合予測ベクトル、Ｐ_k,wv(-)は統合予測誤差共分散行列を表す。 Similarly, the predicted value, prediction error covariance matrix, and observation error covariance matrix of the Kalman filter 5 based on the meandering motion model are output to the residual quadratic form calculator 31-2 of the motion discriminator 3. The residual quadratic form δ _{k, wv} of the Kalman filter based on the meandering motion model is calculated based on the equations (64) to (66).

Here, z _k represents an observation vector, R _k represents an observation error covariance matrix, and Λ _k represents a conversion matrix from the radar observation coordinate system to the filter coordinate system. H ₁ represents an observation matrix, x ^ _{k, wv} (−) represents an integrated prediction vector of a Kalman filter based on a meandering motion model, and P _{k, wv} (−) represents an integrated prediction error covariance matrix.

Since the Kalman filter based on the meandering motion model operates on a certain coordinate axis, when performing two-dimensional or three-dimensional tracking, the residual vector ε _{k, wv} is calculated for each coordinate axis, and each residual covariance is used as a diagonal element. A residual quadratic form is calculated from Equation (66) using the residual covariance matrix.

ここで、αは忘却係数であって、Δとの関係として式（６９）を満たす。

The residual quadratic form from the Kalman filter based on the calculated constant velocity linear motion model and the Kalman filter based on the meandering motion model is output to the respective judgment evaluation function calculators 32-1 and 32-2, and at a constant sample number Δ. Judgment evaluation functions μ ^cv _k and μ ^wv _k which are average values of the residual quadratic form are calculated by the equations (67) and (68).

Here, α is a forgetting factor, and satisfies Equation (69) as a relationship with Δ.

The motion determiner 33 acquires the determination evaluation function calculated from the expressions (67) and (68), and performs threshold determination. Specifically, as shown in FIG. 9, two threshold values having different sizes are set, and it is determined whether or not the determination evaluation function exceeds the threshold value. Here, a value larger than h ₂ is set as the threshold value h ₁ , and a value larger than h ₄ is set as the threshold value h ₃ . This is to prevent frequent switching because the judgment evaluation function calculated from the equations (67) and (68) fluctuates in the vicinity of the threshold value.

  As shown in FIG. 10, the evaluation function of the Kalman filter 4 based on the constant-velocity linear motion model is small for constant-velocity linear motion targets and has large characteristics for maneuver motions such as meandering and turning targets. . On the other hand, the Kalman filter 5 based on the meandering motion model has a characteristic that it is small for constant velocity linear motion and meandering motion and increases for other motions such as turning.

  Thus, paying attention to the property that the residual quadratic form of the “Kalman filter using a meandering motion model” is small for constant speed and meandering motion and large for maneuver motions other than meandering, By combining with the result of the residual quadratic form of “Kalman filter using a fast linear motion model”, it is possible to determine “constant velocity”, “meandering”, and “maneuver motion other than meandering”.

  FIG. 11 is a state transition diagram for motion discrimination. In this way, using the characteristics of the evaluation function for the target motion of each filter of FIG. 10, the target motion state is changed to “constant linear motion”, “meander motion”, “maneuver motion other than meander motion”. Determine. The discrimination result is output to the filter output selector 8.

  As described above, the motion discrimination processor 3 calculates the residual quadratic form of each motion model, obtains a determination evaluation function value for each motion model from each of the residual quadratic forms, and determines these motion models. Since the evaluation function and a predetermined threshold value are compared to determine the target motion state, it is equivalent to the process of accumulating the values of each sample and calculating the average by sequential processing. Thus, erroneous determination can be suppressed.

(Convergence judgment processing)
Next, the operation of the convergence determination unit 7 will be described. The convergence determination unit 7 performs convergence determination on the smooth velocity vector output from the Kalman filter 4 based on the meandering motion model. The Kalman filter 4 performs processing based on the meandering motion derived from the constant velocity linear motion model and the sinusoidal motion model. If the observation noise is large immediately after the start of tracking, the smoothing error related to the integrated smooth value combining both models is performed. Even if is small, the smooth value for each model is not stable, and it takes time to separate the constant velocity component and the sine function component of the target motion. In such a case, the smooth velocity vector of the constant velocity linear motion model is not stable. Therefore, if the future position prediction is performed based on such a smooth value, the future prediction position largely fluctuates and the error becomes large.

  Therefore, the convergence determination unit 7 calculates an evaluation function of an angle (time displacement of the direction component of the smoothing velocity vector) formed by the two smoothing velocity vectors calculated at the sampling time k and the time k−1 for the constant velocity linear motion model. By calculating and comparing with a threshold value, a convergence determination is performed on the smooth velocity vector of the constant velocity linear motion model.

As shown in FIG. 12, the velocity vector angle calculator 71 calculates the angle formed by the smooth velocity vector of the constant velocity linear motion model at the sampling time k and the smooth velocity vector of the constant velocity linear motion model at the sampling time k−1. . If the smooth velocity vector of the constant velocity linear motion model at the sampling time k is ν ^ _k and the smooth velocity vector of the constant velocity linear motion model at the sampling time k−1 is ν ^ _k-1 , then the smooth velocity vector ν ^ _k And the angle angle θ _k [°] formed by ν ^ _k−1 is calculated from the equation (70).

Subsequently, the angle evaluation function calculator 72 averages the angle θ _k , calculates the angle evaluation function μ _k ^θ from Expression (71), and outputs the angle evaluation function μ _k ^θ to the threshold value determiner 73.

  FIG. 13 shows an example of a simulation result of the smooth velocity vector convergence determination evaluation function of the constant velocity linear motion model when the meandering target is tracked by the Kalman filter based on the meandering motion model. The horizontal axis represents time, and the vertical axis represents the convergence evaluation function. The evaluation function immediately after the start of tracking is 100 ° or more, and the smoothing speed vector fluctuates significantly. However, it decreases rapidly with the passage of time, and the smooth velocity vector becomes stable to the extent that it can be used for future position prediction.

Therefore, the threshold value determination unit 73 determines that the smooth velocity vector of the constant velocity linear motion model has converged when the angle evaluation function μ ^θ _k is lower than the threshold value h ₅ . Thereafter, when the threshold value h ₆ is exceeded, it is determined again that the smooth velocity vector of the constant velocity linear motion model has not converged. The threshold value h ₅ is set to a value smaller than the threshold value h ₆ . This is to prevent frequent different determinations from being repeated when the above-described evaluation value fluctuates in the vicinity of the threshold value.

  Since the convergence determination unit 7 is thus provided to determine the degree of stability of the meandering motion model, it is possible to avoid performing future position prediction based on the smooth value when the smooth value of the Kalman filter 5 is unstable. As a result, stable future position prediction can be performed. Further, since the degree of stability of the meandering motion is determined based on the time displacement in the direction of the smooth velocity vector of the constant velocity linear motion component constituting the meandering motion model, the convergence state of the smooth value of the Kalman filter 5 is quantified. It was possible to make an objective and accurate judgment.

  In the above-described processing example, the processing is performed based on the change (time displacement) between the previous sample and the current sample in the direction of the smooth velocity vector of the constant velocity linear motion component constituting the meandering motion model. did. However, it goes without saying that the sampling interval is not limited to adjacent samples such as the previous sample and the current sample.

(Filter output selection processing)
The filter output selector 8 is one of the smooth values output by the Kalman filters 4 to 6 based on the motion determination result output by the motion determination processor 3 and the determination result regarding the convergence state output by the convergence determiner 7. One is selected and output to the future position predictor 9. FIG. 14 shows which Kalman filter value the filter output selector 8 selects. As shown in the figure, when the determination result of the motion discrimination processor 3 is constant velocity motion, a smooth value by the Kalman filter 4 based on the constant velocity linear motion model is selected. When the determination result of the motion discrimination processor 3 is a meandering motion, the determination result of the convergence determiner 7 is referred to, and when it is not converged, the Kalman filter 4 based on the constant velocity linear motion model is used. Select a smooth value. In the case of convergence, the smooth value of the Kalman filter 5 based on the meandering motion model is selected. When the determination result of the motion discrimination processor 3 is a maneuver other than meandering, a smooth value by the Kalman filter 6 based on the multiple motion model is selected.

  As described above, when the smoothing velocity vector of the Kalman filter 5 based on the meandering motion model is not determined to converge even after the start of tracking, the smoothing value of the Kalman filter 5 based on the meandering motion model is obtained even if the motion discrimination processor 3 determines that the target meanders. Instead, the smooth value of the Kalman filter 4 based on the constant velocity linear motion model is output to the future position predictor 9 instead.

ここで、ｘ^_k+N(-)は未来予測値を表す。また、ｘ^^int _k-1(+)はフィルタ出力選択器８により選択された追尾フィルタの平滑値を表す。さらには、Ｔ_Nはサンプリング時刻ｋからｋ＋Ｎまでの時間を表す。蛇行運動モデルに基づくカルマンフィルタ５の場合、各座標軸において得られた等速直線運動モデルの統合平滑値がｘ^^int _k-1(+)となる。一方、等速直線運動モデルに基づくカルマンフィルタ４、及び多重運動モデルに基づくカルマンフィルタ６は、それぞれ統合平滑値がｘ^^int _k-1(+)となる。 (Future position prediction)
The future position predictor 9 calculates the future position after N sampling from the equations (72) and (73) from the smooth position and smooth velocity vector obtained from the filter output selector.

Here, x ^ _{k + N} (-) represents a predicted future value. X ^ ^int _k-1 (+) represents the smoothing value of the tracking filter selected by the filter output selector 8. Furthermore, T _N represents the time from sampling time k to k + N. In the case of the Kalman filter 5 based on the meandering motion model, the integrated smooth value of the constant velocity linear motion model obtained on each coordinate axis is x ^ ^int _k-1 (+). On the other hand, the Kalman filter 4 based on the constant velocity linear motion model and the Kalman filter 6 based on the multiple motion model each have an integrated smooth value of x ^ ^int _k-1 (+).

  As is apparent from the above, according to the target tracking device in the first embodiment of the present invention, the future position prediction is performed by determining the target motion and determining the smooth velocity vector convergence of the constant velocity linear motion model in the Kalman filter based on the meandering motion model. It is possible to select the output of the tracking filter that is most advantageous for performing the prediction, and to predict the future position after N samples based on the smooth value at the sampling time k.

  Note that the motion discriminating processor 3 and the convergence discriminator 7 which are a part of the features of the target tracking device according to Embodiment 1 of the present invention are effective even when there is no Kalman filter 6 based on the multiple motion model. It is clear to do.

Embodiment 2. FIG.
In the target tracking device according to the first embodiment, the convergence determination unit 7 determines the convergence of the smooth velocity vector of the constant velocity linear motion component in the Kalman filter 5 based on the meandering motion model. Since the smooth velocity vector of the constant velocity linear motion model takes time to converge after the start of tracking, the filter selector 8 does not select the output of the Kalman filter based on the unconvergent meandering motion model, and the motion discrimination processor 3 Even if it is determined as “meandering motion”, the Kalman filter 4 based on the constant velocity linear motion model is selected. On the other hand, the direction of the constant-velocity linear motion component of the meandering motion model may change greatly due to a maneuver such as target turning. In this case, the angle evaluation function once increases while increasing again. In such a state, even if it is determined as “meandering motion” by the motion discrimination processor 3, the smooth velocity vector has not converged. Therefore, when the filter output selector 8 selects the output of the Kalman filter 5 based on the meandering motion model, good future position prediction accuracy cannot be obtained.

  Therefore, in order to avoid such a problem, when the angle evaluation function increases rapidly after convergence as shown in FIG. 15, the convergence determination unit 7 causes the constant-velocity linear motion component in the Kalman filter 5 based on the meandering motion model. The smooth velocity vector may be determined to be in an unconverged state again. The target tracking device according to Embodiment 2 of the present invention has such characteristics. FIG. 16 is a block diagram showing the configuration of the convergence determination unit 7 in the target tracking device according to Embodiment 2 of the present invention. In the figure, a threshold value setter 75 is a part for setting a new threshold value. The other components denoted by the same reference numerals as those in FIG. 6 are the same as those in the first embodiment, and thus the description thereof is omitted. By the action of the threshold setting unit 75, the threshold determining unit 73 does not perform the convergence determination of the smooth velocity vector in the constant velocity linear motion model until the threshold setting unit 75 falls below the threshold again.

ｈ_７は現在のしきい値である。 Here, the threshold value determination unit 73 performs the non-convergence determination by the angle evaluation function when, for example, the inequality shown in Expression (74) is satisfied.

h ₇ is the current threshold value.

When the non-convergence determination is made by the threshold value determiner 73, the threshold value setter 75 resets the new threshold value to the threshold value determiner 73 as shown in the equations (75) and (76). To do.

The threshold determination unit 73 determines that the smooth velocity vector of the constant velocity linear motion model has converged when the angle evaluation function μ ^θ _k is lower than the threshold h ′ ₅ . Thereafter, when the threshold value h ′ ₆ is exceeded, it is determined again that the smooth velocity vector of the constant velocity linear motion model has not converged. Further, since the non-convergence determination again is caused by maneuver motion such as turning, even if the motion discrimination processor 3 determines that the “meandering motion”, the filter output selector 8 uses the multiple motion model. Based on the Kalman filter 6 is selected. FIG. 17 is a diagram showing which Kalman filter smooth value is selected by the filter output selector 8 based on the determination results of the motion determination processor 3 and the convergence determiner 7. The configuration of this figure is the same as that of FIG.

  As described above, the motion discriminating processor 3 determines “meandering motion” and the smooth velocity vector has not yet converged, and the angle evaluation function increases again after convergence by the maneuver such as target turning. Even in this case, it is possible to select a motion model suitable for the target motion by appropriately setting the threshold value in the convergence determiner 7 and devising the convergence determination. It can be improved.

Embodiment 3 FIG.
In addition, in the convergence determination process in the convergence determination unit 7, in addition to the smooth value calculated by the Kalman filter 5 based on the meandering motion model, the smooth value calculated by the Kalman filter 4 based on the constant velocity linear motion model may be considered. The target tracking device according to Embodiment 3 of the present invention has such features. FIG. 18 is a block diagram showing the structure of the target tracking device according to Embodiment 3 of the present invention. 18 differs from FIG. 1 in that the processing result of the Kalman filter 4 based on the constant velocity linear motion model is also output to the convergence determination unit 7 in FIG. 18, and the other points are the same.

  FIG. 19 is a block diagram showing a detailed configuration of convergence determination unit 7 of the target tracking device according to Embodiment 3 of the present invention. In the figure, velocity vector angle calculators 71-1 and 71-2 correspond to the velocity vector angle calculator 71 in FIG. 6, and are based on the smooth value of the Kalman filter based on the constant velocity linear motion model and the meandering motion model, respectively. The smoothing value of the Kalman filter is acquired and processed. The angle evaluation function calculators 72-1 and 72-2 correspond to the angle evaluation function calculator 72 in FIG. 6, and are respectively for the Kalman filter based on the constant velocity linear motion model and the Kalman filter based on the meandering motion model. Process. The other components denoted by the same reference numerals as those in FIG. 6 are the same as those in the first embodiment, and thus the description thereof is omitted.

  Next, the operation of the convergence determination unit 7 of the target tracking device according to Embodiment 3 of the present invention will be described. The velocity vector angle calculator 71-1 acquires the smooth velocity vector of the Kalman filter 4 based on the constant velocity linear motion model. The velocity vector angle calculator 71-2 obtains a smooth velocity vector of the constant velocity linear motion component in the Kalman filter 5 based on the meandering motion model. In addition to evaluating the time displacement of the direction component of the smooth velocity vector of the constant velocity linear motion component that constitutes the meandering motion model, the direction component of the smooth velocity vector and the direction component of the constant velocity linear motion model are compared. .

Specifically, the angle evaluation function calculators 72-1 and 72-2 are angle evaluation functions μ ^θ _{k, cv} and μ ^θ _k for the smooth velocity vector shown in the equation (71) as in the first embodiment. _{, wv} are calculated respectively. Then, when determining that the smooth value by the meandering motion model has converged, the threshold value determiner 73 further compares the direction component of the smooth velocity vector and the direction component of the constant velocity linear motion model, Relative determination of the smooth velocity vector of the constant velocity linear motion component in the Kalman filter 5 based on the meandering motion model is performed.

  Then, the filter output selector 8 uses the Kalman filter angle evaluation function based on the meandering motion model even if the “meandering motion” is judged by the motion discrimination processor 3 based on FIG. If the angle evaluation function of the Kalman filter based on the model does not fall below, the output of the Kalman filter 4 based on the constant velocity linear motion model is selected.

  As described above, according to the third embodiment of the present invention, the angle evaluation function of the Kalman filter based on the constant velocity linear motion model is also used for the convergence determination. As a result, the reference line of the meandering motion (straight line represented by the slope and intercept of the constant velocity linear motion component) fluctuates and the smooth value is not sufficiently stable. Since the time displacement is small, even if it is determined that the smooth value alone of the Kalman filter 5 based on the meandering motion model has converged, it can be determined that the smooth value has not converged. Therefore, it is possible to avoid future position prediction based on an unstable smooth value and to perform future position prediction with higher accuracy.

Embodiment 4 FIG.
In addition, the future position prediction up to N samples ahead is performed inside the convergence determination unit 7, the distance between the future position prediction value of N samples ahead and the future position prediction value of N−1 samples ahead is calculated, and this distance is calculated. A convergence determination may be made based on this. The target tracking device according to Embodiment 4 of the present invention has such features.

  FIG. 20 is a block diagram showing a detailed configuration of convergence determination unit 7 of the target tracking device according to Embodiment 4 of the present invention. As shown in the figure, the convergence determination unit 7 of the target tracking device according to the fourth embodiment of the present invention includes a future position predictor 75, a future position predicted value distance calculator 76, a future position predicted value memory 77, and a future predicted value interval. The distance determination unit 78 is configured.

  The operation of each of these components will be described below. The future position predictor 75 acquires the smooth value of the constant velocity linear motion component in the Kalman filter based on the meandering motion model, and calculates the future predicted value after N samples in advance. The future predicted value calculated here is output to the future position predicted value distance calculator 76. The future position predicted value distance calculator 76 stores the future predicted value of N samples ahead acquired from the future position predictor 75 in the future position predicted value memory 77 and also stores the future position predicted value of N-1 samples ahead in the future position. Obtained from the predicted value memory 77, the Euclidean distance between the future position predicted values at the N sample destination and the N-1 sample destination is calculated.

  Subsequently, the future position predicted value distance determination unit 78 compares the Euclidean distance between the future position predicted values with a threshold value. If this distance exceeds the threshold value, it is determined that the smooth velocity vector of the constant velocity linear motion model in the Kalman filter based on the meandering motion model has not converged. As a result, the filter output selector 8 determines which Kalman filter to select based on each determination result as shown in FIG.

  Thus, according to the target tracking device according to Embodiment 4 of the present invention, even if the Euclidean distance has never fallen below the threshold even after the tracking start, even if it is determined as the meandering motion by the motion discrimination processor 3, It is determined that the smooth velocity vector of the constant velocity linear motion model in the Kalman filter 5 based on the meandering motion model has not converged, and the filter output selector 8 selects the output of the Kalman filter 4 based on the constant velocity linear motion model. .

  In the above-described processing, the future position in the adjacent sample such as N sample destination and N-1 sample destination is used as the future position prediction value used for calculating the distance. However, the present invention is not limited to the case where the sample points are adjacent to each other.

  The present invention is particularly useful for improving the processing accuracy of a target tracking system that prepares various motion models for targets that perform various motions and performs target tracking.

It is a block diagram which shows the structure of the target tracking apparatus in Embodiment 1 of this invention. It is a block diagram which shows the detailed structure of the target tracking apparatus in Embodiment 1 of this invention. It is a block diagram which shows another detailed structure of the target tracking apparatus in Embodiment 1 of this invention. It is a block diagram which shows another detailed structure of the target tracking apparatus in Embodiment 1 of this invention. It is a block diagram which shows another detailed structure of the target tracking apparatus in Embodiment 1 of this invention. It is a block diagram which shows another detailed structure of the target tracking apparatus in Embodiment 1 of this invention. It is a figure which shows the relationship between the sine function motion model in an xt plane, a constant velocity linear motion model, and a meandering motion model. It is the figure which expressed the meandering movement model as a block diagram. It is the table | surface showing the relationship between each Kalman filter, an evaluation function, and a threshold value. It is the table | surface which showed the characteristic which a determination evaluation function shows with respect to each exercise | movement of a target. It is a state transition diagram of the exercise | movement discrimination | determination by the target tracking apparatus in Embodiment 1 of this invention. It is a figure for demonstrating the method to calculate the angle | corner which the smooth velocity vector of the constant velocity linear motion model between each sampling by the target tracking apparatus in Embodiment 1 of this invention makes. It is a figure which shows the example of the simulation result of the smooth velocity vector convergence determination evaluation function of the constant velocity linear motion model at the time of tracking a meandering target by the Kalman filter based on a meandering motion model. It is a figure which shows the relationship between the exercise | movement determination process result in the target tracking apparatus in Embodiment 1 of this invention, and the selected Kalman filter. It is a figure which shows another example of the simulation result of the smooth velocity vector convergence determination evaluation function of the constant velocity linear motion model at the time of tracking a meandering target by the Kalman filter based on a meandering motion model. It is a block diagram which shows the detailed structure of the target tracking apparatus in Embodiment 2 of this invention. It is a figure which shows the relationship between the exercise | movement determination process result in the target tracking apparatus in Embodiment 2 of this invention, and the selected Kalman filter. It is a block diagram which shows the structure of the target tracking apparatus in Embodiment 3 of this invention. It is a block diagram which shows the detailed structure of the target tracking apparatus in Embodiment 3 of this invention. It is a block diagram which shows the detailed structure of the target tracking apparatus in Embodiment 4 of this invention.

Explanation of symbols

4 Kalman filter based on constant velocity linear motion model,
5 Kalman filter based on the meandering motion model,
6 Kalman filter based on multiple motion model,
7 Convergence determiner,
8 Filter output selector.

Claims (7)

In a target tracking device that calculates a plurality of smooth values from observed values of a target using a plurality of Kalman filters based on different motion models and predicts a future position of the target by selecting an appropriate smooth value from these smooth values ,
Constant velocity linear motion Kalman filter processing means for calculating a first smoothed value and a first predicted value from the observed value using a Kalman filter based on a constant velocity linear motion model;
Meandering Kalman filter processing means for calculating a second smoothed value and a second predicted value from the observed values using a Kalman filter based on a meandering motion model;
Motion discrimination processing means for discriminating the motion state of the target based on the first and second predicted values;
A convergence determining means for determining a convergence state of the second smooth value;
Based on the target motion state determined by the motion determination processing means and the convergence status determined by the convergence determination means, either the first smooth value or the second smooth value is set as the appropriate smooth value. Filter output selection means to select;
A target tracking device comprising: The motion discrimination processing means calculates a residual quadratic form from the first and second predicted values, calculates a determination evaluation function value of each motion model from each of the residual quadratic forms, and Comparing the evaluation function of the movement model of the movement model and a predetermined threshold value to determine the movement state of the target.
The target tracking device according to claim 1. The meandering Kalman filter processing means calculates the smooth value and the predicted value using a Kalman filter based on a meandering motion model including a sinusoidal motion component and a constant velocity linear motion component,
The convergence determination means determines a convergence state of the second smooth value based on a time displacement between predetermined samples in a direction of a smooth velocity vector of a constant velocity linear motion component constituting the meandering motion model;
The target tracking device according to claim 1. In the case where it is determined that the second smooth value has converged, the convergence determination means further acquires the first smooth value to configure the direction of the constant velocity linear motion model and the meandering motion model, etc. Performing a comparison with the direction of the smooth velocity vector of the fast linear motion component to determine a convergence state of the second smooth value;
The target tracking device according to claim 3. The convergence determination means predicts a first future position and a second future position at different sample times from the second smooth value, and calculates a distance between the first future position and the second future position. Calculating and determining a convergence state of the second smooth value based on the distance;
The target tracking device according to claim 1. A multi-motion Kalman filter processing means for calculating a smoothed value and a predicted value from the observed value using a Kalman filter based on a multi-motion model assuming a motion having a plurality of constant accelerations;
The target tracking device according to claim 1, further comprising: The convergence determining means determines that the second smoothing value has converged and then the first smoothing motion vector is changed when the direction of the smoothing velocity vector of the constant velocity linear motion component constituting the meandering motion model changes beyond a predetermined value. It is determined that the smooth value of 2 has not converged,
When it is determined that the second smooth value has not converged as a result of a change in the smoothing speed vector after the filter output selection unit determines that the second smooth value has converged, Selecting a smooth value of a multi-motion Kalman filter as the optimal smooth value;
The target tracking device according to claim 6.

Images