JP3333131B2 - Tracking device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a tracking device for inputting target position information including observation noise from observation means and estimating a true value of the target position information and target motion parameters such as speed, acceleration, and the like. The present invention relates to a tracking device with improved accuracy.

[0002]

2. Description of the Related Art Conventionally, as a tracking device of this type, FIG.
2 (the figure shown in JP-A-61-169786). FIG. 22 is a block diagram showing a configuration of a conventional tracking device. In FIG. 22, 31 is observation means for observing target position information, 32 is gain matrix calculation means for calculating a gain matrix, and 33 is a smoothing means for calculating smoothed values of target movement parameters such as target position, velocity, acceleration, and the like. Value calculating means, 34 is a predicted value calculating means for calculating a predicted value of the target motion data after one sampling from the current time, and 35 is a smoothing error calculation for calculating an error of the smoothed value calculated by the smoothing value calculating means 33. The means 36 is a prediction error calculating means for calculating the error of the predicted value calculated by the predicted value calculating means 34.

The prediction value calculation means 34 uses the preconditions that the angular velocity in the direction of the target velocity vector is 0, the angular velocity for rotating the target velocity vector is constant, and the acceleration in the target traveling direction is constant. A second angular velocity calculating means for calculating an angular velocity based on the calculated predicted value; a state transition matrix calculating means for calculating a state transition matrix using the angular velocity calculated by the second angular velocity calculating means;
9 is a first delay element, 40 is a second delay element, 41 is a third delay element, and 42 is a fourth delay element.

A coordinate conversion matrix calculation unit 44 calculates a conversion matrix from fixed rectangular coordinates to dynamic rectangular coordinates having a target velocity vector as one axis, using the predicted value calculated by the predicted value calculating means 34. Means 45 is a driving device for directing the observation means 1 to a target.

A conventional tracking device is configured as described above,
For example, as a coordinate system, dynamic rectangular coordinates having a target velocity vector as one axis are used. As a precondition for calculating the angular velocity, the angular velocity is calculated by keeping the acceleration in the target traveling direction constant. .

Next, with reference to FIG. 22, the operation of the conventional tracking device will be described by describing the operation of each of the above-mentioned conventional means. First, the observation means 31 observes a distance, an elevation angle, and an azimuth as target position information including observation noise expressed in polar coordinates.

The gain matrix calculating means 32 predicts a covariance matrix of observation noise in fixed rectangular coordinates calculated using target position information input from the observing means 31 by an observation system model set in advance and predicts it one sampling before. The prediction error covariance matrix calculated by the error calculation means 36 is input through the second delay element 40, and the coordinate conversion matrix calculated by the coordinate conversion matrix calculation means 44 one sample before is sampled by the fourth delay element 40. , And a gain matrix is calculated using these matrices according to the Kalman filter theory.

In the smoothing value calculating means 33, the current predicted value calculated by the predicted value calculating means 34 one sample before is input through the first delay element 39, and is also converted into fixed rectangular coordinates. The target position information input from the observation means 31 is input, and the coordinate conversion matrix calculated by the coordinate conversion matrix calculation means 44 one sample before is input through the fourth delay element 42, and these values are smoothed. Calculate the value.

The predicted value calculating means 34 assumes that the target performs a constant angular velocity motion, inputs the target position, the target speed, and the target acceleration calculated by the smoothed value calculating means 33, and furthermore, the state transition matrix calculating means. The state transition matrix calculated at 38 is input through the third delay element 41, and the target position, target speed, and target acceleration after one sampling are calculated based on these values.

The smoothing error calculating means 35 receives the gain matrix calculated by the gain matrix calculating means 32 and inputs the prediction error covariance matrix calculated by the prediction error calculating means 36 one sample before the second sampling. Input through the delay element 40, and further before one sampling, the coordinate transformation matrix calculating means 4
The coordinate conversion matrix calculated in step 4 is converted to a fourth delay element 42
, And a smooth error covariance matrix is calculated from these matrices.

The prediction error calculating means 36 inputs the smoothed error covariance matrix calculated by the smoothing error calculating means 35, and inputs the state transition matrix calculated by the state transition matrix calculating means 38 through a third delay element 41. Then, a prediction error covariance matrix is calculated from these matrices.

The state transition matrix calculating means 38 inputs the angular velocity calculated by the second angular velocity calculating means 43 and calculates a state transition matrix.

When calculating the angular velocity, the second angular velocity calculating means 43 inputs the predicted value calculated by the predicted value calculating means 34, sets the angular velocity in the direction of the target velocity vector to 0, and sets the angular velocity for rotating the target velocity vector. And the angular velocity is calculated on condition that the acceleration in the target traveling direction is constant.

The coordinate transformation matrix calculating means 44 inputs the predicted value calculated by the predicted value calculating means 34, and generates a coordinate transformation matrix for changing from fixed rectangular coordinates to dynamic rectangular coordinates according to the speed component of the predicted value. calculate.

The driving means 45 calculates the elevation angle and the azimuth to which the observation means 31 should point after one sampling from the current time based on the position information of the predicted value calculated by the predicted value calculation means 34, The observation means 31 is controlled so that 31 is directed in that direction.

[0016]

However, the above-mentioned conventional tracking device has a structure in which the tracking device is configured in such a manner that the acceleration in the target traveling direction is constant, so that the target device which flies while changing the direction of the velocity vector can be used. However, there is an advantage that tracking can be performed with high accuracy. However, according to this conventional method, since the coordinate transformation is required between the fixed rectangular coordinates and the dynamic rectangular coordinates, the algorithm becomes complicated,
Inevitably, there was a lot of computer resources. Therefore, as an example, when the sampling rate is very short, there is a problem that it is difficult to perform processing within a predetermined processing time.

SUMMARY OF THE INVENTION The present invention has been made to solve the above-mentioned conventional problem. The speed in the target traveling direction is kept constant to eliminate the need for coordinate transformation with respect to dynamic Cartesian coordinates. It is intended to greatly reduce computer resources by simplifying.

[0018]

According to the first aspect of the present invention, there is provided a tracking apparatus for observing target position information, a gain matrix calculating unit for calculating a gain matrix, and a target position / velocity / acceleration. The smoothed value calculating means for calculating the smoothed value of the target motion data such as the target motion data, the predicted value calculating means for calculating the predicted value of the target motion data after one sampling from the current time, and the smoothed value calculating means. A smoothing error calculating means for calculating an error of the smoothed value, a prediction error calculating means for calculating an error of the predicted value calculated by the predicted value calculating means, an angular velocity in the target speed vector direction being 0, and rotating the target speed vector The angular velocity is calculated based on the predicted value calculated by the predicted value calculating means using the precondition that the angular velocity is constant and the velocity in the target traveling direction is constant, and further, the magnitude of the angular velocity vector and the target position vector are calculated. A first angular velocity calculating means for calculating the third derivative of the target position from the first derivative vector of Le,
State transition matrix calculation means for calculating a state transition matrix using the angular velocity calculated by the first angular velocity calculation means, wherein each of the means performs each operation in fixed rectangular coordinates.

According to a second aspect of the present invention, there is provided a tracking device, wherein a driving noise order switching means for switching an order of driving noise in fixed rectangular coordinates based on the angular velocity calculated by the first angular velocity calculating means is added. It was made.

According to a third aspect of the present invention, there is provided a tracking device for switching a state transition matrix in fixed rectangular coordinates based on the angular velocity calculated by the first angular velocity calculating means.
The state transition matrix switching means is added.

According to a fourth aspect of the present invention, in the tracking device, the tracking device further comprises a target existence expected area calculating means for calculating a target existence expected area based on the prediction error calculated by the prediction error calculating means. Target position information evaluation means for extracting target position information in a target existence expected area based on the target existence expected area calculated by the target existence expected area calculation means and the target position information observed by the observation means; and the target position information Reliability calculating means for calculating the reliability of the target position information in the target existence expected area extracted based on the target position information in the target existing expected area extracted by the evaluation means. Is performed.

According to a fifth aspect of the present invention, there is provided the tracking device, wherein a gain matrix switching means for switching a gain matrix in fixed rectangular coordinates is added based on the angular velocity calculated by the first angular velocity calculating means. It is.

According to a sixth aspect of the present invention, in the tracking apparatus, a first sampling control means for switching a sampling rate in fixed rectangular coordinates based on the angular velocity calculated by the first angular velocity calculation means is added. It was done.

According to a seventh aspect of the present invention, the tracking device further includes a driving noise amount switching unit for switching the magnitude of the driving noise in fixed rectangular coordinates based on the angular velocity calculated by the first angular velocity calculating unit. It is like that.

The tracking device according to the invention of claim 8 is based on the prediction error calculated by the prediction error calculation means.
A state transition matrix switching means for switching the second state transition matrix in fixed rectangular coordinates is added.

According to a ninth aspect of the present invention, a tracking device according to the present invention uses the prediction error calculated by the prediction error calculation means,
A second sampling control means for switching a sampling rate in fixed rectangular coordinates is added.

The present invention has been made in order to solve the above-mentioned conventional problems. By making the speed in the target traveling direction constant, coordinate conversion for dynamic rectangular coordinates is not required, and an algorithm is constituted by fixed rectangular coordinates. The algorithm can be simplified compared to the above conventional example,
Therefore, computer resources can be significantly reduced.

[0028]

BRIEF DESCRIPTION OF THE DRAWINGS FIG.
An embodiment of the present invention will be described in detail with reference to FIG. FIG.
FIG. 2 is a block diagram illustrating a configuration of a tracking device according to the first embodiment of the present invention. FIG. 2 is a diagram illustrating a fixed rectangular coordinate system used in the tracking device according to the embodiment of the present invention. FIG. 4 is a diagram showing a dynamic rectangular coordinate system used in the conventional tracking device, FIG. 4 is a diagram showing a target attitude angle of the dynamic rectangular coordinate system used in the conventional tracking device, and FIG. 5 shows an operation of the tracking device in the first embodiment of the present invention. FIG. 6 is a block diagram illustrating a configuration of a tracking device according to Embodiment 2 of the present invention. FIG. 7 is a flowchart illustrating an operation of the tracking device according to Embodiment 2 of the present invention. FIG. 9 is a block diagram illustrating a configuration of a tracking device according to a third embodiment of the present invention. FIG. 9 is a diagram illustrating a flowchart illustrating an operation of the tracking device according to the third embodiment of the present invention. Tracking Block diagram showing the configuration of a location, FIG. 11
FIG. 12 is a diagram showing a flowchart showing the operation of the tracking device in the fourth embodiment of the present invention, FIG. 12 is a block diagram showing the configuration of the tracking device in the fifth embodiment of the present invention, and FIG. 13 is a fifth embodiment of the present invention. FIG. 14 is a flowchart showing an operation of the tracking device in FIG.
15 is a block diagram illustrating the configuration of a tracking device according to the sixth embodiment of the present invention. FIG. 16 is a block diagram illustrating the configuration of the tracking device according to the seventh embodiment of the present invention. FIG. 17 is a flowchart showing the operation of the tracking device according to the seventh embodiment of the present invention. FIG. 18 is a block diagram showing the configuration of the tracking device according to the eighth embodiment of the present invention. FIG. 20 is a flowchart showing the operation of the tracking device in the eighth embodiment, FIG. 20 is a block diagram showing the configuration of the tracking device in the ninth embodiment of the present invention, and FIG. It is a figure showing the flow chart showing operation.

Embodiment 1 First, the configuration of the tracking device according to the first embodiment of the present invention will be described with reference to FIG. In FIG. 1, reference numeral 1 denotes observation means for observing target position information in polar coordinates, converts the target position information into fixed rectangular coordinates and outputs the result, 2 denotes gain matrix calculation means for calculating a gain matrix in fixed rectangular coordinates, and 3 Is a smoothed value calculating means for calculating a smoothed value vector of a target motion specification such as a target position, velocity and acceleration in fixed rectangular coordinates, and 4 is a target motion specification after one sampling from the current time in fixed rectangular coordinates. Predicted value calculating means 5 for calculating the predicted value vector are smoothed error calculating means for calculating the error of the smoothed value calculated by the smoothed value calculating means 3 in fixed rectangular coordinates.

6 is a prediction error calculating means for calculating the error of the predicted value calculated by the predicted value calculating means 4 in fixed rectangular coordinates, and 7 is an angular velocity in the direction of the target speed vector.
Based on the assumption that the angular velocity for rotating the target velocity vector is constant and the velocity in the target traveling direction is constant, the angular velocity in the fixed orthogonal coordinates is calculated based on the predicted value calculated by the predicted value calculating means 4. This is a first angular velocity calculating means for calculating and further calculating the third derivative of the target position vector from the magnitude of the angular velocity vector and the first derivative of the target position vector.

Reference numeral 8 denotes a state transition matrix calculating means for calculating a state transition matrix using the angular velocity calculated by the first angular velocity calculating means 7 in fixed rectangular coordinates, and 9 denotes a predicted value calculating means before one sampling. A first delay element for inputting the prediction value calculated in 4 is a second delay element for inputting the prediction error calculated by the prediction error calculation means 6 one sampling before, and 11 is a state transition matrix calculation before one sampling. This is a third delay element for inputting the state transition matrix calculated by the means 6.

Next, the operation of the tracking device according to the first embodiment of the present invention will be described with reference to FIGS. FIG.
4 to 4 show a coordinate system necessary for explaining the first angular velocity calculating means. Figure 2 shows the observation device at the origin O and the eastward direction at x
Fixed orthogonal coordinates O-xyz, where X is the axis, Y is the Y axis in the north direction, and Z is the Z axis in the vertical direction.

FIG. 3 shows that the tracking center of the target is the origin O,
Dynamic orthogonal coordinates in which the target velocity vector is a positive U on the u-axis, a right-hand direction perpendicular to the velocity vector in the plane parallel to the horizontal plane is a positive V on the v-axis, and a downward W perpendicular to the velocity vector is a positive W on the w-axis. It shows O-uvw. In this figure, P is the angular velocity of the dynamic rectangular coordinates O-uvw around the u-axis, Q is the angular velocity of the dynamic rectangular coordinates O-uvw around the v-axis, and R is the dynamic rectangular coordinate O-uvw around the w-axis. Angular velocity.

FIG. 4 shows a target attitude angle required for explaining the first angular velocity calculating means. In the figure, H is a unit vector on the velocity vector, I is a vector obtained by projecting the vector H on a horizontal plane, and J Is the target attitude angle on the horizontal plane, the angle from Y to vector I on the y-axis of the fixed orthogonal coordinates O-xyz, K is the target attitude angle on the vertical plane, the angle from vector I to vector H, and O is the fixed orthogonal. Coordinates O-x
The origin of yz, X is the x axis, Y is the y axis, and Z is the z axis.

The angles of the target attitude angles J and K are α and β, respectively.
Then, there is a relationship of [Equation 1]. Where Vx, V
y and Vz are velocity components in the x, y, and z-axis directions of the target velocity vector V at the fixed rectangular coordinates O-xyz.

[0036]

(Equation 1)

The relationship [Equation 4] exists between the position [Equation 2] of the fixed rectangular coordinates O-xyz and the position [Equation 3] of the dynamic rectangular coordinates O-uvw. Dynamic Cartesian coordinates O− in [Equation 4]
A coordinate conversion matrix F for converting uvw to fixed rectangular coordinates O-xyz is [Equation 5].

[0038]

(Equation 2)

[0039]

(Equation 3)

[0040]

(Equation 4)

[0041]

(Equation 5)

[0042]

[Outside 1]

Then, according to the vector analysis formula,
[Equation 6], [Equation 7], [Equation 8], [Equation 9], [Equation 1]
0] and [Equation 11].

[0044]

(Equation 6)

[0045]

(Equation 7)

[0046]

(Equation 8)

[0047]

(Equation 9)

[0048]

(Equation 10)

[0049]

[Equation 11]

As a precondition, it is assumed that the magnitude of the target velocity vector is fixed and [Equation 12] holds. Also, the angular velocity that rotates the velocity vector is regarded as constant,
It is assumed that [Equation 13] holds. Further, the angular velocity P about the u-axis in the target traveling direction irrelevant to the transition of the target position is regarded as zero as shown in [Equation 14].

[0051]

(Equation 12)

[0052]

(Equation 13)

[0053]

[Equation 14]

Based on the prerequisite [Equation 12], [Equation 9] and [Equation 10] are expressed as [Equation 15] and [Equation 16], respectively.

[0055]

(Equation 15)

[0056]

(Equation 16)

The two sides of [Equation 6] are differentiated with respect to time.
Using the preconditions [Equation 12] and [Equation 8], [Equation 7] becomes [Equation 17].

[0058]

[Equation 17]

The two sides of [Equation 17] are differentiated with respect to time, and preconditions [Equation 12], [Equation 13], [Equation 15], [Equation 1]
6] is used to obtain [Equation 18].

[0060]

(Equation 18)

Further, by substituting [Equation 6] into [Equation 18],
[Equation 19] is obtained.

[0062]

[Equation 19]

From [Equation 6] and [Equation 17], [Equation 2]
0].

[0064]

(Equation 20)

Based on [Equation 20] and the prerequisite [Equation 14], the target angular velocity vector magnitude ω of [Equation 11] is obtained by [Equation 21]. The first angular velocity calculating means in FIG. 1 uses [Equation 21].

[0066]

(Equation 21)

[0067]

[Outside 2]

[0068]

(Equation 22)

[0069]

(Equation 23)

Next, in order to explain the state transition matrix calculation means of FIG. 1, the state transition matrix will be derived. The k sampling th time and t _k, a sampling interval T _k and [Equation 24], the position vector of the target at time t _k [Equation 25], the velocity vector [Expression 26], the acceleration vector [ Equation 27]

[0071]

[Outside 3]

[0072]

(Equation 24)

[0073]

(Equation 25)

[0074]

(Equation 26)

[0075]

[Equation 27]

[0076]

[Equation 28]

Around time t _k−1 , the position vector [Equation 2]
5], the velocity vector [Equation 26], and the acceleration vector [Equation 27] are respectively developed by analytical formulas.
When terminating the fifth or higher order term, [Equation 29], [Equation 30],
[Equation 31] is obtained.

[0078]

(Equation 29)

[0079]

[Equation 30]

[0080]

(Equation 31)

The magnitude of the angular velocity at the sampling time t _k is ω _k−1, and [Equation 29], [Equation 30], [Equation 31]
Can be expressed by using [Equation 25], [Equation 26], [Equation 27] and [Equation 22], respectively.
3] and [Equation 34].

[0082]

(Equation 32)

[0083]

[Equation 33]

[0084]

(Equation 34)

When Ι is a unit matrix of 3 rows and 3 columns, [Equation 3
2], [Equation 33] and [Equation 34] are represented as [Equation 35] as a motion model using the state variable vector [Equation 28].

[0086]

[Outside 4]

[0087]

(Equation 35)

[0088]

[Equation 36]

[0089]

(37)

[0090]

(38)

[0091]

[Equation 39]

[0092]

[Outside 5]

[0093]

(Equation 40)

[0094]

[Equation 41]

[0095]

(Equation 42)

[0096]

[Outside 6]

[0097]

[Equation 43]

As described above, based on the theory of the Kalman filter, [Equation 44], [Equation 45], [Equation 46], [Equation 4]
7], and performs a prediction process and a smoothing process from [Equation 48].

[0099]

[Outside 7]

[0100]

[Equation 44]

[0101]

[Equation 45]

[0102]

[Equation 46]

[0103]

[Equation 47]

[0104]

[Equation 48]

Next, the processing procedure of the tracking device according to the first embodiment of the present invention will be described with reference to FIGS.
In the flowchart shown in FIG. 5, step ST is started, and the process proceeds to observation value input in step ST1, where an observation value vector obtained by converting target position information given in polar coordinates into fixed rectangular coordinates is obtained.

In the gain calculation in step ST2,
A gain matrix is calculated based on [Equation 46] using the observation value vector calculated by the observation value input in step ST1 and the observation error covariance matrix calculated in advance. Next, the process proceeds to the calculation of the smoothed value in step ST3, and the smoothed value vector is calculated using the gain matrix obtained in the gain calculation in step ST2 and the observation value vector and predicted value vector obtained in advance (Equation 47).

In the calculation of the smoothing error in step ST4, the equation (48) is used to calculate the smoothing error covariance matrix from the gain matrix obtained in the gain calculation in step ST2 and the prediction error covariance matrix calculated in advance. I do. Step ST
In the predicted value calculation of 5, the predicted value vector after one sampling is calculated according to [Equation 44] using the smoothed value vector obtained in the smoothed value calculation of step ST3.

Next, in calculating the angular velocity in step ST6,
Using the predicted value vector calculated in the predicted value calculation in step ST5, the magnitude of the angular velocity vector is obtained according to [Equation 21], and the process proceeds to step ST7.

In calculating the state transition matrix in step ST7, [Equation 36] is prepared in advance, and the state transition matrix is calculated according to [Equation 23] by using the magnitude of the angular velocity vector calculated in the angular velocity calculation in step ST6. . That is, the magnitude of the angular velocity vector calculated in the angular velocity calculation in step ST6 is sequentially substituted into [Equation 36], and a state transition matrix is obtained.

Next, in the prediction error calculation in step ST8, the state transition matrix calculated in the state transition matrix calculation in step ST7 and the error covariance matrix of the observation noise calculated in advance are used to calculate [Equation 45]. Based on this, a prediction error covariance matrix is calculated. In step ST9, in order to perform the smoothing process, when the process is further continued, the process returns to the observation value input process in step ST1 due to the delay element.

In the present embodiment described above, in order to reflect the angular velocity in the state transition matrix, the target movement in the three-dimensional space includes the constant velocity linear movement and the constant velocity circular movement in a plane parallel to the horizontal plane. Can be faithfully reflected.

Embodiment 2 Next, with reference to FIG. 6, the configuration of the tracking device according to the second embodiment of the present invention will be described. 6, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 6, components different from those in FIG.
Is a motion model [Equation 3] according to the accuracy required by the tracking device.
6] is a drive noise order switching means for determining the order of the drive noise conversion matrix Γ ₂ (k), and uses the following expression [49] instead of [37].

[0114]

[Outside 8]

[0115]

[Equation 49]

The driving noise order switching means 16, which is a characteristic element of FIG. 6, calculates the driving noise conversion matrix using the driving noise conversion matrix [Equation 49] according to the required accuracy of the driving noise.
This is input to the prediction error calculation means 6.

Next, a processing procedure of the tracking device according to the first embodiment of the present invention will be described with reference to FIGS.
In the flowchart shown in FIG. 7, step ST is started, and the process proceeds to the input of the observation value in step ST1.
The same operation parts as those in the first embodiment are denoted by the same reference symbols up to T8, and the description thereof will not be repeated.

In FIG. 7, in step ST8a, which is the characteristic step, the driving noise order is set in accordance with the required accuracy, a driving noise conversion matrix is calculated using [Equation 49], and the calculated driving noise conversion matrix is calculated. Is input to the prediction error calculation in step ST8, thereby calculating a prediction error covariance matrix.

In the present embodiment described above, the calculation of the driving noise conversion matrix portion is not equivalent to the fourth derivative of the position vector, but is equivalent to the third derivative or the second derivative lower than the fourth derivative. Can be reduced.

Embodiment 3 Next, a configuration of a tracking device according to Embodiment 3 of the present invention will be described with reference to FIG. 8, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 8, components different from those in FIG.
Is a state transition matrix switching unit that determines the target motion state based on the magnitude ω of the angular velocity vector calculated by the first angular velocity calculation unit 7 and changes the shape of the state transition matrix Φ. The matrix is calculated by the state transition matrix calculation means 8, and the state transition matrix in the case of constant acceleration motion and constant speed linear motion is calculated in advance. Then, the target motion state is determined from the magnitude of the angular velocity, and as shown in [Equation 50], the target motion state obtained from the determination result, that is, the constant angular velocity motion, the constant acceleration motion, or the constant velocity linear motion To change the state transition matrix Φ and input it to the third delay element 11.

[0122]

[Equation 50]

Next, a processing procedure of the tracking device according to the third embodiment of the present invention will be described with reference to FIGS.
In the flowchart shown in FIG. 9, step ST is started, and the process proceeds to input of an observation value in step ST1. The same reference numerals are given to the same operation parts as in the first embodiment, and the description thereof will not be repeated.

In FIG. 9, in step ST7a, which is the characteristic step, state transition matrices for constant angular velocity motion, constant acceleration motion, and constant velocity linear motion are prepared in advance as shown in [Equation 50]. The target motion is determined based on the magnitude of the angular velocity vector calculated in the angular velocity calculation in step ST6, and one of the three equations shown in [Equation 50] is selected based on the target motion.
Input to the calculation of the prediction error.

In the present embodiment described above, by switching the state transition matrix according to the magnitude of the angular velocity, the mismatch between the motion models is reduced, and the target accuracy is improved. Further, by switching from a constant angular velocity motion to a state transition matrix such as a constant velocity linear motion or a constant acceleration motion, the amount of calculation can be reduced.

Embodiment 4 Next, referring to FIG.
A configuration of a tracking device according to Embodiment 4 of the present invention will be described. 10, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted. Hereinafter, FIG.
Only the components different from those in FIG. 1 will be described.

[0127]

[Outside 9]

[0128]

(Equation 51)

[0129]

(Equation 52)

[0130]

(Equation 53)

[0131]

[Outside 10]

This is target position information evaluation means for judging whether or not the target existence expected area [Equation 53] calculated by the target existence expected area calculation means 18 is present. Reference numeral 21 denotes a target existence expected area determined by the target position information evaluation means 20 [Equation 53].
The observation vector existing within the target expected region [Equation 5]
3]

[0133]

[Outside 11]

Next, a processing procedure of the tracking device according to the fourth embodiment of the present invention will be described with reference to FIGS. In the flowchart shown in FIG.
After starting T, the process proceeds to the input of observation values in step ST1, and the same operation parts as those in the first embodiment are denoted by the same reference numerals.
The description will not be repeated.

In FIG. 11, the characteristic steps are step ST1a, step ST1b and step ST8b.
It is. Hereinafter, each of these steps will be mainly described. In step ST8b, an observation error covariance matrix is calculated from the prediction error covariance matrix calculated from the prediction error calculation in step ST8 by using [Equation 52].
From the predicted value vector calculated from the predicted value calculation at T5, a predicted observed value vector is calculated using [Equation 52], and a target existence expected area [Equation 53] is calculated from the observed error covariance matrix and the predicted observed value vector. calculate.

In step ST1a, the observation value vector calculated in step ST1 is calculated using the target existence expected area [Equation 5] calculated from the target existence expected area calculation in step ST8b.
Step ST for evaluating whether or not the area exists in the area [3]
In step 1b, the reliability of the observation value vector in the target existence expected area calculated as a result of the target position information evaluation in step ST1a is determined according to the distance of the predicted observation vector that is the center of the target existence expected area [Equation 53]. It is calculated from the probability density, and its reliability is calculated by the gain calculation in step ST2 or step ST3
To calculate the smoothed value of.

In the present embodiment described above, the target existence expected area is calculated, it is determined whether or not the observed value vector exists in the target existence expected area, and the reliability is calculated. This can be dealt with even in a signal environment.

Embodiment 5 FIG. Next, referring to FIG.
A configuration of a tracking device according to Embodiment 5 of the present invention will be described. 12, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 12, components different from those in FIG.
Reference numeral 2 denotes a gain matrix switching unit that determines a target motion state based on the magnitude ω of the angular velocity vector calculated by the first angular velocity calculation unit 7 and switches a gain matrix.

Next, the processing procedure of the tracking device according to the fifth embodiment of the present invention will be described with reference to FIGS. In the flowchart shown in FIG.
After starting T, the process proceeds to the input of observation values in step ST1, and the same operation parts as those in the first embodiment are denoted by the same reference numerals.
The description will not be repeated.

In FIG. 13, in step ST2a, which is the characteristic step, the target motion state is determined from the magnitude of the angular velocity vector calculated from the angular velocity calculation in step ST6. , The gains of the three motion states are calculated in the same procedure as in the first embodiment, and the gain matrices of the three motions are switched according to the required accuracy of the target. And input to step ST4.

In the present embodiment described above, the target motion state is determined from the magnitude of the angular velocity vector, and the gain matrix is switched. Achieve the objective sufficiently by using the state transition matrix of constant velocity linear motion and constant acceleration motion as the state transition matrix used for matrix calculation, and using the state transition matrix of constant angular velocity motion only in the prediction vector calculation part. Can be.

Embodiment 6 FIG. Next, referring to FIG.
A configuration of a tracking device according to Embodiment 6 of the present invention will be described. 14, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 14, components different from those in FIG.
Reference numeral 3 denotes first sampling control means for determining a target motion state based on the magnitude ω of the angular velocity vector calculated by the first angular velocity calculation means 7 and changing a sampling rate.

Next, a processing procedure of the tracking device according to the sixth embodiment of the present invention will be described with reference to FIGS. In the flowchart shown in FIG.
After starting T, the process proceeds to the input of observation values in step ST1, and the same operation parts as those in the first embodiment are denoted by the same reference numerals.
The description will not be repeated.

In FIG. 15, in step ST1c, which is the characteristic step, the target motion state is determined from the magnitude of the angular velocity vector calculated from the angular velocity calculation in step ST7. If the sampling rate from ST1c to ST8 is roughened and the target exercise state is unknown, the sampling rate is controlled by reducing the sampling rate from step ST1c to step ST8.

In the present embodiment described above, the target motion state is determined from the magnitude of the angular velocity vector.
When the target motion state is known, the sampling rate is roughened to reduce the amount of calculation, and when the target motion state is not known, the sampling rate is changed so that the sampling rate is reduced. Clarify the motion state of the target and improve the target accuracy.

Embodiment 7 FIG. Next, referring to FIG.
A configuration of a tracking device according to Embodiment 7 of the present invention will be described. 16, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 16, components different from those in FIG.
Reference numeral 4 denotes a driving noise amount switching unit that determines a target motion state based on the magnitude ω of the angular velocity vector calculated by the first angular velocity calculating unit 7 and changes the driving noise amount.

Next, a processing procedure of the tracking device according to the seventh embodiment of the present invention will be described with reference to FIGS. In the flowchart shown in FIG.
After starting T, the process proceeds to the input of observation values in step ST1, and the same operation parts as those in the first embodiment are denoted by the same reference numerals.
The description will not be repeated.

In FIG. 17, in step ST8c, which is the characteristic step, the target motion state is determined from the magnitude of the angular velocity vector calculated from the angular velocity calculation in step ST6, and the amount of driving noise is determined.
8 to the prediction error calculation.

In the present embodiment described above, the motion state of the target is determined based on the magnitude of the angular velocity vector, and the followability to the target can be enhanced by changing the amount of driving noise.

Embodiment 8 FIG. Next, referring to FIG.
A configuration of a tracking device according to Embodiment 8 of the present invention will be described. 18, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 18, components different from those in FIG.
Reference numeral 5 denotes a state transition matrix switching unit that determines a target motion state based on the prediction error calculated by the prediction error calculation unit 6 and thereby switches the state transition matrix from the state transition matrix calculation unit 8.

Next, a processing procedure of the tracking device according to the eighth embodiment of the present invention will be described with reference to FIGS. In the flowchart shown in FIG.
After starting T, the process proceeds to the input of observation values in step ST1, and the same operation parts as those in the first embodiment are denoted by the same reference numerals.
The description will not be repeated.

In FIG. 19, in step ST7b, which is the characteristic step, the target motion state is determined from the prediction error calculation in step ST8 based on the prediction error, and the constant angular velocity motion of [Formula 50] prepared in advance is used. One of the state transition matrices at the time of the acceleration motion and the constant velocity linear motion is selected, and the selected expression is input to the prediction error calculation of step ST8.

In the present embodiment described above, the target motion state is determined from the prediction error, and the state transition matrix is switched according to the determination, thereby reducing the mismatch between the motion models and improving the target accuracy. Further, by switching from the constant angular velocity motion to the state transition matrix of the constant velocity linear motion or the constant acceleration motion, the calculation amount can be reduced.

Embodiment 9 FIG. Next, referring to FIG.
The configuration of a tracking device according to Embodiment 9 of the present invention will be described. 20, the same components as those in FIG. 1 are denoted by the same reference numerals, and description thereof will be omitted.

In FIG. 20, components different from those in FIG.
Reference numeral 6 denotes second sampling control means for determining a target exercise state based on the prediction error calculated by the prediction error calculation means 6 and changing the sampling rate according to the determination.

Next, a processing procedure of the tracking device according to the ninth embodiment of the present invention will be described with reference to FIGS. In the flowchart shown in FIG.
After starting T, the process proceeds to the input of observation values in step ST1, and the same operation parts as those in the first embodiment are denoted by the same reference numerals.
The description will not be repeated.

In FIG. 21, in step ST1d, which is the characteristic step, the target exercise state is determined from the prediction error calculated from the prediction error calculation in step ST8.
When the target exercise state is known, the sampling rate from step ST1d to step ST8 is increased,
When the target exercise state is not known, the sampling rate is controlled so as to make the sampling rate from step ST1d to step ST8 fine.

In the present embodiment described above, the target motion state is determined from the prediction error, and when the target motion state is known, the sampling rate is roughened to reduce the amount of calculation, and the target motion state is reduced. If you do not know the state, by making the sampling rate finer,
Clarify the motion state of the target and improve the target accuracy.

Incidentally, the tracking device in the embodiment of the present invention may be constituted by hardware or software, and in any case, it operates as shown in the flowchart of the attached drawings. I just need.

[0164]

The tracking device according to the first aspect of the present invention is configured as described above, and in particular, makes the speed in the target traveling direction constant, eliminates the need for coordinate transformation for dynamic rectangular coordinates, and uses a fixed orthogonal algorithm. By simplifying the configuration by using only coordinates, computer resources can be greatly reduced.In addition, in order to reflect the angular velocity in the state transition matrix, constant velocity linear motion and in a plane parallel to the horizontal plane It is possible to faithfully reflect the motion of the target in the three-dimensional space, including the fast-circle motion.

The tracking device according to the second aspect of the present invention is configured as described above. In particular, the calculation of the driving noise conversion matrix portion is not equivalent to the fourth derivative of the position vector, but is lower than the third derivative or the third derivative. By taking the second derivative equivalent,
The amount of calculation can be reduced.

The tracking device according to the third aspect of the present invention is configured as described above. In particular, by switching the state transition matrix according to the magnitude of the angular velocity, the mismatch between the motion models is reduced, and the target accuracy is improved. . Further, by switching from a constant angular velocity motion to a state transition matrix such as a constant velocity linear motion or a constant acceleration motion, the amount of calculation can be reduced.

The tracking device according to a fourth aspect of the present invention is configured as described above, and particularly calculates a target existence expected area to determine whether or not the observed value vector exists in the target existence expected area. However, by calculating the reliability, it is possible to cope with the unnecessary signal environment.

The tracking device according to the fifth aspect of the present invention is configured as described above, and particularly, by judging the target motion state from the magnitude of the angular velocity vector and switching the gain matrix, the prediction vector If it is desired to improve only the accuracy of the motion vector, the state transition matrix used for calculating the gain matrix should be a constant velocity linear motion or a constant acceleration motion. This can be easily realized by using a matrix.

The tracking device according to the invention according to claim 6 is configured as described above, and in particular, determines the motion state of the target from the magnitude of the angular velocity vector, and performs sampling when the motion state of the target is known. By reducing the amount of calculation by roughing the rate, and when the target motion state is not known, the sampling rate is changed to make the sampling rate finer, so that the target motion state becomes clear and the target accuracy is also improved. improves.

The tracking device according to the invention of claim 7 is configured as described above, and in particular, determines the motion state of the target based on the magnitude of the angular velocity vector, and changes the amount of drive noise to follow the target. Higher.

The tracking device according to the eighth aspect of the present invention is configured as described above, and in particular, determines the target motion state from the prediction error, and switches the state transition matrix in accordance with the judgment, thereby obtaining a discrepancy between the motion models. Is reduced, the target accuracy is improved, and the amount of calculation can be reduced by switching from a constant angular velocity motion to a constant speed linear motion or constant acceleration motion state transition matrix.

The tracking device according to the ninth aspect of the present invention is configured as described above, and particularly, the motion state of the target is determined from the prediction error, and the motion state of the target is determined by changing the sampling rate. When the target is in motion, the sampling rate can be reduced and the amount of calculation can be reduced. Is improved.

[Brief description of the drawings]

FIG. 1 is a block diagram illustrating a configuration of a tracking device according to a first embodiment of the present invention;

FIG. 2 is a diagram showing a fixed rectangular coordinate system used in the tracking device according to the embodiment of the present invention;

FIG. 3 is a diagram showing a dynamic rectangular coordinate system used in a conventional tracking device;

FIG. 4 is a diagram showing a target attitude angle in a dynamic rectangular coordinate system used in a conventional tracking device;

FIG. 5 is a diagram showing a flowchart representing an operation of the tracking device according to the first embodiment of the present invention;

FIG. 6 is a block diagram showing a configuration of a tracking device according to Embodiment 2 of the present invention;

FIG. 7 is a diagram showing a flowchart illustrating an operation of the tracking device according to the second embodiment of the present invention;

FIG. 8 is a block diagram showing a configuration of a tracking device according to a third embodiment of the present invention;

FIG. 9 is a flowchart showing an operation of the tracking device according to the third embodiment of the present invention;

FIG. 10 is a block diagram showing a configuration of a tracking device according to a fourth embodiment of the present invention;

FIG. 11 is a flowchart showing an operation of the tracking device according to the fourth embodiment of the present invention;

FIG. 12 is a block diagram showing a configuration of a tracking device according to Embodiment 5 of the present invention;

FIG. 13 is a flowchart showing an operation of the tracking device according to the fifth embodiment of the present invention;

FIG. 14 is a block diagram illustrating a configuration of a tracking device according to a sixth embodiment of the present invention;

FIG. 15 is a flowchart showing an operation of the tracking device according to the sixth embodiment of the present invention;

FIG. 16 is a block diagram showing a configuration of a tracking device according to a seventh embodiment of the present invention;

FIG. 17 is a diagram showing a flowchart representing an operation of the tracking device according to the seventh embodiment of the present invention;

FIG. 18 is a block diagram showing a configuration of a tracking device according to Embodiment 8 of the present invention;

FIG. 19 is a diagram showing a flowchart representing an operation of the tracking device according to the eighth embodiment of the present invention;

FIG. 20 is a block diagram showing a configuration of a tracking device according to Embodiment 9 of the present invention;

FIG. 21 is a view showing a flowchart representing an operation of the tracking device according to the ninth embodiment of the present invention;

FIG. 22 is a block diagram illustrating a configuration of a conventional tracking device.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 Observation means, 2 Gain matrix calculation means, 3 Smooth value calculation means, 4 Prediction value calculation means, 5 Smooth error calculation means, 6 Prediction error calculation means, 7 First angular velocity calculation means, 8 First state transition matrix calculation Means, 9 first delay element, 10 second delay element, 11 third delay element, 12 fourth delay element, 13 second angular velocity calculating means, 14 coordinate transformation matrix calculating means, 15 driving means, 16 Driving noise order switching means 17 State transition matrix switching means, 18 Target existence expected area calculation means, 19 Fifth delay element, 20 Target position information evaluation means, 21 Reliability calculation means, 22 Gain matrix switching means, 23 First Sampling control means, 24
Drive noise amount switching means, 25 second state transition matrix switching means, 26 second sampling control means, 31
Observation means, 32 gain matrix calculation means, 33 smooth value calculation means, 34 prediction value calculation means 35 smoothing error calculation means, 36 prediction error calculation means,
38 state transition matrix calculating means, 39 first delay element,
40 second delay element 41 third delay element, 42 fourth delay element, 43
Second angular velocity calculating means 44 coordinate transformation matrix calculating means, 45 driving means.

──────────────────────────────────────────────────続 き Continuation of the front page Examiner Tetsunobu Miyagawa (56) References JP-A-61-169786 (JP, A) JP-A-9-297176 (JP, A) JP-A-2-85788 (JP, A) JP-A-2-53317 (JP, A) JP-A-2-17485 (JP, A) JP-A-5-126951 (JP, A) (58) Fields investigated (Int. Cl. ⁷ , DB name) G01S 7/00-7/42 G01S 13/00-13/95

Claims (9)

(57) [Claims] 1. Observing means for observing target position information, gain matrix calculating means for calculating a gain matrix, and smoothing value calculating means for calculating smoothed values of target motion parameters such as a target position, velocity, and acceleration. Prediction value calculation means for calculating a prediction value of a target motion specification after one sampling from the current time; smoothing error calculation means for calculating an error of a smoothed value calculated by the smoothing value calculation means; A prediction error calculating means for calculating an error of the predicted value calculated by the means, and a precondition that an angular velocity in a target velocity vector direction is 0, an angular velocity for rotating a target velocity vector is constant, and a velocity in a target traveling direction is constant. Calculating an angular velocity based on the predicted value calculated by the predicted value calculating means, and further calculating a third derivative of the target position vector from the magnitude of the angular velocity vector and the first differential vector of the target position. First angular velocity calculating means;
A state transition matrix calculating means for calculating a state transition matrix using the angular velocity calculated by the angular velocity calculating means, wherein each of the means performs each operation in fixed rectangular coordinates. . 2. A tracking device according to claim 1, further comprising a drive noise order switching means for switching the order of drive noise in fixed rectangular coordinates based on the angular velocity calculated by said first angular velocity calculation means. . 3. The apparatus according to claim 1, further comprising a first state transition matrix switching unit that switches a state transition matrix in fixed rectangular coordinates based on the angular velocity calculated by the first angular velocity calculation unit. Tracking device. 4. The tracking device further comprises: a target existence expected area calculating means for calculating a target existence expected area based on the prediction error calculated by the prediction error calculating means; and a target calculated by the target existence expected area calculating means. Target position information evaluation means for extracting target position information in the target presence expected area based on the presence expected area and the target position information observed by the observation means, and a target position in the target existence expected area extracted by the target position information evaluation means And a reliability calculating means for calculating the reliability of the target position information in the target existence expected area based on the information, wherein each means performs each operation in fixed rectangular coordinates. 2. The tracking device according to 1. 5. The tracking device according to claim 1, further comprising gain matrix switching means for switching a gain matrix in fixed rectangular coordinates based on the angular velocity calculated by said first angular velocity calculation means. 6. A tracking device according to claim 1, further comprising a first sampling control means for switching a sampling rate in fixed rectangular coordinates based on the angular velocity calculated by said first angular velocity calculation means. 7. The tracking device according to claim 1, further comprising a drive noise amount switching unit for switching the magnitude of the drive noise in fixed rectangular coordinates based on the angular velocity calculated by the first angular velocity calculation unit. apparatus. 8. The tracking system according to claim 1, further comprising a state transition matrix switching unit that switches a second state transition matrix in fixed rectangular coordinates based on the prediction error calculated by the prediction error calculation unit. apparatus. 9. The tracking device according to claim 1, further comprising a second sampling control unit that switches a sampling rate in fixed rectangular coordinates based on the prediction error calculated by the prediction error calculation unit.