JP2008076096A - Tracking method and its system - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a tracking method for accurately tracking a target under acceleration. <P>SOLUTION: A prediction error covariance matrix of the second time period is computed on the basis of a smoothing error covariance matrix of the first time period. A smoothing error covariance matrix of the second time period is computed on the basis of the prediction error covariance matrix of the second time period; a gain matrix of the second time period; and a prescribed observed noise covariance matrix. The gain matrix of the second time period including both a position gain, a gain on the position of the target included in a predicted value vector of the second time period, and a velocity gain, a gain on the velocity of the target included in a prediction value vector of the second time period, is computed on the basis of the prediction error covariance matrix of the second time period and the observed noise covariance matrix to acquire an observed value of the second time period. A smoothed value vector of the second time period is computed on the basis of the acquired observed value; the predicted value vector of the second time period; and the gain matrix of the second time period in the tracking method. In the tracking method, the gain matrix of the second time period including a fixed value gain as the speed gain is computed. <P>COPYRIGHT: (C)2008,JPO&INPIT

Description

  The present invention relates to a tracking method and apparatus for estimating a motion specification of a moving object based on observation values.

  Conventionally, a Kalman filter is generally used as a means for tracking a target using a target observation result by an observation device. In such a general tracking device, a motion model is described on the assumption that the basic motion of the target is constant velocity linear motion, and an acceleration term for expressing the deviation from the constant velocity linear motion of the target is set. Tracking processing is performed assuming white noise (for example, Patent Document 1).

Japanese Patent Laying-Open No. 2005-331248 “Target Tracking Device and Target Tracking Method”

  An actual target such as an aircraft, a ship, or a vehicle usually performs acceleration motion such as acceleration or deceleration while going straight, or turning while drawing an arc having a substantially constant radius. In such a case, the target acceleration vector is a constant value or a value that gradually changes over time, and it is difficult to model it as white noise. For this reason, the conventional tracking device has a problem that the optimum performance cannot be realized as a result.

  The present invention has been made for the purpose of solving such a problem, and obtains good estimation performance without assuming that the acceleration term in the target motion is white noise.

  In the tracking device of the present invention, the maximum value of the tracking delay when the target continuously moves at a given constant acceleration (determined from the assumed maximum motor capacity of the target) falls within a predetermined range. On the other hand, when the acceleration is zero, that is, when the target is performing a uniform linear motion, a new filter that minimizes the tracking error caused by the observation noise is used.

  As a result, it is possible to stabilize the tracking result during constant speed linear motion while keeping the tracking error for the maximum acceleration motion of the target within the allowable range, and as a result, easily realize the optimal tracking performance Can

Embodiments of the present invention will be described below with reference to the drawings.
Embodiment.
First, a general tracking process using a Kalman filter will be described. In tracking processing using a Kalman filter, a target motion model is assumed as shown in Equation (1). However, the sampling time (hereinafter simply referred to as time) is represented by t (k).

In a general tracking process using a Kalman filter, the motion model of Equation (1) is described assuming that the basic motion of the target is a constant velocity linear motion. For example, when estimating the target position and velocity in a three-dimensional Cartesian coordinate system of xyz, the state vector x (k) and the state transition matrix Φ are expressed as in equations (2), (3), and (4). Is defined.

However, T represents a vector, matrix transposition, and a dot represents time differentiation. Further, I _{n × n} and On _{× n} are an n-row n-column unit matrix and a zero matrix, respectively, and τ represents a constant sampling period.

  Furthermore, in the tracking process using the Kalman filter, w (k) in Expression (1) is x provided to express an error of x (k) generated by approximating the actual target motion with a constant velocity straight line. It is composed of noise components of acceleration terms in the y and z directions, and more specifically, it is assumed to be white noise that follows a mean zero vector and a trivariate normal distribution of a covariance matrix Q (k).

In addition, it is assumed that the observation model of the observation value supplied to the tracking process conforms to Equation (5).

For example, it is assumed that the observation result of the target position in the xyz three-dimensional orthogonal coordinate system is obtained by the observation device. At this time, the observation value vector z (k) is defined as in Equation (6), and the observation matrix H is defined as in Equation (7).

  In equation (5), v (k) is a noise component for representing the observation error of the observation device, and is assumed to be white noise according to a trivariate normal distribution of mean zero vector and covariance matrix R (k). To do.

  The state vector estimation processing by the Kalman filter algorithm based on the assumptions of the motion model and the observation model such as Equation (1) and Equation (5) will be described below.

  In the following description, a predicted value vector obtained by estimating the state vector at time t (k) based on the observation results up to time t (k−1) is represented as x ^ (k | k−1), and time t A smooth vector obtained by estimating the state vector at time t (k) based on the observation results up to (k) is represented as x ^ (k | k). Note that (variable) ^ represents a vector with ^ appended on (variable). That is, the relationship as shown in Expression (8) is established.

Furthermore, the prediction error covariance matrix representing the error covariance matrix of x ^ (k | k-1) is P (k | k-1), and the smoothing error covariance representing the error covariance matrix of x ^ (k | k). The variance matrix is represented as P (k | k). That is, the relationship from Equation (9) to Equation (12) is established. Further, K (k) is a gain matrix of the filter, and is a 6 × 3 matrix when the equations (2) to (4), equations (6), and (7) are set. Furthermore, I is a 6 × 6 unit matrix.

  Note that the initial value x ^ (0 | 0) of x ^ (k | k) and the initial value P (0 | 0) of P (k | k) are separately calculated and given. The system noise covariance matrix Q (k) and the observation noise covariance matrix R (k) are given as parameters.

  The Kalman filter calculates an unbiased estimated value without an average bias using the equations (8) and (11), and calculates the gain matrix K (k) using the equation (10), thereby calculating the mean square error. It is known as an optimal filter that minimizes.

  The Kalman filter described so far can provide optimum estimation performance if the target motion model and the observation model of the observation apparatus are correctly expressed. However, in the Kalman filter, it is necessary to assume that the acceleration term w (k) for expressing the deviation from the target constant-velocity linear motion is white noise, as shown in Equation (1).

  However, when an actual target such as an aircraft, a ship, or a vehicle performs an acceleration motion, as shown in FIG. 3, the vehicle frequently accelerates or decelerates while moving straight, or turns while drawing an arc having a substantially constant radius.

  In these cases, the target acceleration vector is a constant value or a value that gradually changes over time, and cannot be modeled as white noise. As a result, in the conventional tracking device, it is difficult to set the value of the system noise covariance matrix Q (k) necessary for obtaining a desired tracking accuracy as a parameter. In other words, the conventional tracking device cannot accurately set the value of the parameter Q (k) based on the knowledge about the motion characteristic of the target to be tracked, so that the optimum performance cannot be realized as a result. was there.

  In the tracking device according to the embodiment of the present invention, in order to solve the above-described problem, it is assumed that the target acceleration term is white noise. The detailed method will be described below.

  Equations (8) and (11) are used as the state vector prediction equation and the update equation in the tracking filter calculation equation, as in the conventional Kalman filter. It is well known that by using the equations (8) and (11), an average bias does not occur in the tracking error during the constant velocity linear motion.

Next, it is assumed that the gain matrix K (k) in Expression (10) is expressed as follows.

Here, K ₁ (k) is a gain (position gain) for the target position estimation value, and K ₂ (k) is a gain (speed gain) for the speed estimation value. When the settings of Expression (2), Expression (3), Expression (6), and Expression (7) are performed, the position gain K ₁ (k) and the speed gain K ₂ (k) are all in a 3 × 3 matrix. is there.

In the tracking device according to the embodiment of the present invention, the speed gain K ₂ (k) is set to a fixed value that does not change with time. This is expressed as in equation (14).

Consider a case where the target moves with a constant acceleration vector a _c (determined from the maximum target exercise performance of the target). At this time, it was shown in the IEICE Transactions B, Vol.J86-B, No.11, pp.2397-2406, November 2003, "Steady tracking error in tracking three-dimensional space by linear filter". Thus, assuming an ideal condition where there is no observation error of the observation device, the error vector (steady tracking error vector) of the predicted position in the steady state after sufficient time has elapsed since the start of tracking is The finite value shown.

Thus, for maximum acceleration vector a _c target envisaged, be determined predicted position error vector e ~ _p tolerated, the relationship of equation (15), the gain (fixed gain) of a fixed value that does not change the time K ^- A value of ₂ can be determined. Note that (variable) ~ refers to the same thing as ~ added to (variable). The (variable) ^- on top of (variable) - refers to the same as that are indicated by the.

Next, the speed gain K of the fixed value defined in equation (14) ^- Using the ₂ in the equation (13), equation (8), uniform linear motion in the case of performing a filtering process using Equation (11) Consider the tracking error for. At uniform linear motion, since the corresponding when placed with a _c = 0 in equation (15), steady-state tracking error for the predicted position is e ~ _{p =} 0. However, a tracking error of the noise component due to the observation error of the observation apparatus occurs.

This tracking error occurs in both the predicted value vector x ^ (k | k-1) and the smoothed value vector x ^ (k | k). Therefore, in the embodiment of the present invention, the position gain K is set so as to minimize the evaluation value obtained by weighting the mean square error of the predicted value vector and the mean square error of the smooth value vector in this case, that is, the equation (16). ₁ (k) is determined.

Here, if c = 1, K ₁ (k) can be determined so as to minimize the mean square error of the smooth value vector, and if c = 0, the mean square error of the predicted value vector can be determined. K ₁ (k) can be determined to minimize.

A formula for calculating K ₁ (k) that minimizes formula (16) is given by formula (17).

また、式(８)、式（１１）のフィルタ処理式において、式（１３）、式（１４）、式（１７）のゲイン行列Ｋ(ｋ)を使用した場合、予測誤差共分散行列Ｐ(ｋ｜ｋ−１)、平滑誤差共分散行列Ｐ(ｋ｜ｋ)の逐次計算式は式（１９）、式（２０）で与えられる。

Here, P ¹¹ (k | k−1) and P ¹² (k | k−1) are the blocks in the case where the prediction error covariance matrix is represented by a 2-by-2 block matrix shown in Expression (18). P ¹¹ (k | k−1) corresponds to a covariance matrix of predicted position errors, and P ¹² (k | k−1) corresponds to a correlation matrix of predicted position errors and predicted speed errors.

Further, when the gain matrix K (k) of the equations (13), (14), and (17) is used in the filter processing equations of the equations (8) and (11), the prediction error covariance matrix P ( k | k-1) and the smoothing error covariance matrix P (k | k) are sequentially given by equations (19) and (20).

  Expression (20) is the same as Expression (12) of the conventional Kalman filter. On the other hand, when Equation (19) is compared with Equation (9) in the case of the conventional Kalman filter, it can be seen that Equation (19) does not have a term of the covariance matrix Q (k) of system noise. This is an effect brought about by the fact that the filtering method according to the embodiment of the present invention has stopped assuming the target acceleration term as white noise.

  Next, the configuration and operation of the tracking device according to the embodiment of the present invention will be described. FIG. 1 is a diagram showing the configuration of the tracking device of the present invention. In the figure, the observation apparatus 1 that acquires the observation value is also illustrated. In the tracking device 2 in the figure, the smooth value vector storage means 11 is a part for storing a smooth value vector, and the smooth value vector initial value setting means 12 sets the initial value of the smooth value vector in the smooth value vector storage means 11. It is a part.

The prediction means 13 is a part that calculates a predicted value vector at the current time from a smooth value vector at the previous time. The gain matrix setting means 14 is a part for setting the gain matrix K (k) using the position gain K ₁ (k) and the speed gain K ₂ (k).

  The smoothing unit 15 inputs the prediction value vector calculated by the prediction unit 13 and the observation value vector from the observation apparatus 1, and also inputs the gain matrix K (k) from the gain matrix setting unit 14 to obtain the smoothed value vector. This is the part to be calculated.

  The smoothing error covariance matrix initial value storage means 16 is a part for storing a smooth value vector for storing the values of the smoothing error covariance matrix P (k | k), and is a smoothing error covariance matrix initial value setting means. Reference numeral 17 denotes a part for setting the initial value of the smoothing error covariance matrix P (k | k) in the smoothing error covariance matrix initial value storage means 16.

  The prediction error covariance matrix calculating means 18 uses the smoothing error covariance matrix P (k | k) of the previous time stored by the smoothing error covariance matrix initial value storage means 16 to predict the prediction error covariance matrix P (k | K-1) is a part to be calculated.

  The smoothing error covariance matrix calculating means 19 inputs the prediction error covariance matrix P (k | k−1) at the current time and the gain matrix K (k) from the gain matrix setting means, and the observation noise from the observation device. This is a part for calculating the smoothing error covariance matrix P (k | k) by inputting the variance matrix R (k).

The speed gain storage means 20 is a part for storing the speed gain K ₂ (k), and the speed gain setting means 21 is a part for setting the speed gain K ₂ (k) in the speed gain storage means.

The position gain calculation means 22 inputs the value of the prediction error covariance matrix P (k | k−1) and the observation noise covariance matrix R (k) from the observation device, and calculates the position gain K ₁ (k). It is a part.

  The gain matrix setting means 14, speed gain storage means 20, speed gain setting means 21, and position gain calculation means 22 are collectively referred to as gain matrix calculation means 23.

  In each of the above-described constituent parts, the part used for storing vector values and numerical values is configured using a storage device or storage element such as a magnetic disk device, a random access memory, or a flash memory. The other parts may be configured as dedicated circuits or DSPs (Digital Signal Processors) that perform the above-described functions, or functions of software programs processed by one or more CPUs (Central Processing Units). You may implement as.

FIG. 2 is a flowchart of the tracking method of the present invention. First, in step S101, the smooth value vector initial value setting means 12 sets the smooth value vector initial value x ^ (0 | 0) in the smooth value vector storage means 11, and the smoothing error covariance matrix initial value setting means 17 The initial value P (0 | 0) of the smoothing error covariance matrix is set in the smoothing error covariance matrix storage means 16.

In step S102, the speed gain setting means 21 sets a fixed speed gain K ^- ₂ in the speed gain storage means.

In step S103, the prediction means 13 uses the previous time smooth value vector x ^ (k-1 | k-1) input from the smooth value vector storage means 11, and according to the equation (8), predicts the current time. Vector x ^ (k | k-1) is calculated.
At the same time, the prediction error covariance matrix calculation means 19 uses the previous time smoothing error covariance matrix P (k−1 | k−1) input from the smoothing error covariance matrix storage means, and follows equation (19). The prediction error covariance matrix P (k | k−1) at the current time is calculated.

Prediction means 13, smoothing means 15, position gain calculating means 22, matrix gain setting means 14,
The tracking is ended by any part of the smooth error covariance matrix calculating means 19.

Hereinafter, description will be made assuming that each sampling time is k. When the position gain calculating means 22 receives the observation value vector z (k) and the observation noise covariance matrix R (k) from the observation device (step S104), the position gain calculating means 22 follows the equation (17) and the position gain is calculated. K ₁ (k) is calculated (step S105).

In step S106, the matrix gain setting means 14, the position gain K ₁ (k) and speed gain K ^- with _2, according to equation (21), to set the gain matrix K (k).

  In step S107, the smoothing means 15 uses the predicted value vector x ^ (k | k-1), the observed value vector z (k), and the gain matrix K (k) according to the equation (11) to determine the current time. The smooth value vector x ^ (k | k) is calculated. This smooth value vector is stored in the smooth value vector storage means 11.

  On the other hand, the smoothing error covariance matrix calculating means 19 calculates the prediction error covariance matrix P (k | k−1), the observation noise covariance matrix R (k), and the gain matrix K (k) according to the equation (20). The smoothing error covariance matrix P (k | k) at the current time is calculated. This smoothing error covariance matrix P (k | k) is stored in the smoothing error covariance matrix storage means.

The above is a series of processes at time t (k). Subsequently, it is determined whether or not the tracking is finished. If the tracking is not finished (S108: No), the value of k is incremented, and the process is re-executed from step S104. In the case of tracking end (S108: Yes), the processing ends.
Until this is done, the series of processing is repeated.

  In the equation (17), when the parameter of the weighting factor is set to c = 1, a search type observation device (search radar or the like) that emphasizes the tracking accuracy of the smooth value vector in order to monitor the target motion at the current time. ) Is suitable for application. On the other hand, when c = 0 is set, it is suitable for application to a tracking type observation apparatus (such as a tracking radar) that places importance on the tracking accuracy of the predicted value vector in order to determine the observation area at the next time.

Moreover, in the description so far, the case where the target position and velocity in the xyz three-dimensional space are estimated has been taken as an example. However, in other cases, the method of applying the present invention can be easily analogized. For example, in order to estimate the target position and velocity in the two-dimensional space of xy, the state vector is expressed by equation (22), the state transition matrix is expressed by equation (23), the observed value vector is expressed by equation (24), and the observation matrix is expressed by If it puts like Formula (25), the same tracking device and the tracking method can be obtained.

At this time, the position gain K ₁ (k) and the speed gain K ^- ₂ are both 2 × 2 matrices.
Furthermore, since the tracking in the two-dimensional space or the tracking in the three-dimensional space is executed independently for each coordinate axis, a filter in the one-dimensional space can be configured. In this case, the state vector is set as in Expression (26), the state transition matrix is set as Expression (27), the observed value vector is set as Expression (28), and the observation matrix is set as shown in Expression (29). The same apparatus and method as the tracking apparatus and tracking method can be obtained.

Expressions (26) to (29) represent cases where the tracking process in the x-axis direction is performed as an example. In this case, both the position gain K ₁ (k) and the speed gain K ^- ₂ are scalars.

  As described above, according to the tracking device and the tracking method of the present invention, since the target acceleration is not modeled as the system noise of the white noise unlike the case of the Kalman filter, it is necessary to set the noise covariance matrix as a parameter. Absent. Instead, a fixed speed gain value is determined by designating a steady tracking error of the predicted position that is allowed for the assumed maximum acceleration of the target. Further, when the determined velocity gain is used, a position gain is automatically calculated so as to minimize the noise component of the tracking error caused by the observation noise during the constant velocity linear motion.

  As a result, it is possible to stabilize the tracking result during constant speed linear motion while keeping the tracking error for the maximum acceleration motion of the target within the allowable range, and as a result, easily realize the optimal tracking performance Has the advantage of being able to.

  The present invention can be applied to, for example, a tracking method and apparatus for estimating a motion specification of a moving object based on observed values.

It is a block diagram which shows the structure of the tracking apparatus by embodiment of this invention. It is a flowchart of the process of the tracking apparatus by embodiment of this invention. It is a figure which shows the example of a target exercise | movement of the tracking apparatus by embodiment of this invention.

Explanation of symbols

13 prediction means,
18 prediction error covariance matrix calculating means,
19 Smoothing error covariance matrix calculating means,
23 gain matrix calculation means,
15 Smoothing means.

Claims (8)

A tracking method for estimating a target position and velocity,
A prediction step of calculating a second time prediction value vector from the first time smooth value vector;
A prediction error covariance matrix calculating step of calculating a second time prediction error covariance matrix from the first time smoothing error covariance matrix;
A smoothing error covariance matrix calculating step of calculating a second time smoothing error covariance matrix based on a second time prediction error covariance matrix, a second time gain matrix, and a predetermined observation noise covariance matrix When,
From a second time prediction error covariance matrix and the predetermined observed noise covariance matrix, a position gain that is a gain for a target position included in a second time predicted value vector and a second time predicted value A gain matrix calculating step for calculating a gain matrix for a second time including a speed gain that is a gain for a target speed included in the vector;
A smoothing step of acquiring an observed value of the second time, and calculating a smoothed value vector of the second time based on the acquired observed value, a predicted value vector of the second time, and a gain matrix of the second time; ,
In the tracking method having
A tracking method characterized in that, in the gain matrix calculation step, a gain matrix for a second time including a fixed value gain is calculated as the speed gain. In the gain matrix calculation step, a position gain is calculated so as to minimize the mean square error of the smooth value vector of the second time when the target moves at a constant linear velocity, and the second time including the position gain is calculated. The tracking method according to claim 1, wherein a gain matrix is calculated. In the gain matrix calculation step, a position gain is calculated so as to minimize the mean square error of the predicted value vector of the second time when the target moves at a constant linear velocity, and the second time including the position gain is calculated. The tracking method according to claim 1, wherein a gain matrix is calculated. In the gain matrix calculation step, the mean square error of the smooth vector of the second time when the target moves at a constant linear velocity and the average 2 of the predicted value vector of the second time when the target moves at a constant linear velocity The tracking method according to claim 1, wherein a position gain that minimizes a weighted average with a multiplication error is calculated. A tracking device that estimates the position and speed of a target,
Prediction means for calculating a second time prediction value vector from the first time smooth value vector;
A prediction error covariance matrix calculating means for calculating a second time prediction error covariance matrix from the first time smoothing error covariance matrix;
Smoothing error covariance matrix calculating means for calculating a second time smoothing error covariance matrix based on a second time prediction error covariance matrix, a second time gain matrix, and a predetermined observation noise covariance matrix When,
From a second time prediction error covariance matrix and the predetermined observed noise covariance matrix, a position gain that is a gain for a target position included in a second time predicted value vector and a second time predicted value A gain matrix calculating means for calculating a gain matrix for a second time including a speed gain that is a gain for a target speed included in the vector;
Smoothing means for acquiring an observed value at a second time and calculating a smoothed value vector at a second time based on the acquired observed value, a predicted value vector at the second time, and a gain matrix at the second time; ,
In a tracking device with
The tracking device according to claim 1, wherein the gain matrix calculating means calculates a gain matrix for a second time including a fixed gain as the speed gain. The gain matrix calculation means calculates a position gain that minimizes the mean square error of the smooth value vector of the second time when the target moves linearly at a constant speed, and the gain matrix calculating means calculates the second time including the position gain. The tracking device according to claim 5, wherein a gain matrix is calculated. The gain matrix calculating means calculates a position gain that minimizes the mean square error of the predicted value vector of the second time when the target moves at a constant linear velocity, and calculates the second time including the position gain. The tracking device according to claim 5, wherein a gain matrix is calculated. The gain matrix calculating means calculates the mean square error of the second time smooth value vector when the target moves at a constant linear velocity and the average 2 of the predicted value vector at the second time when the target moves at a constant linear velocity. 6. The tracking device according to claim 5, wherein a position gain that minimizes a weighted average with a multiplication error is calculated.

Images