JPS61169786A - Tracking filter - Google Patents

Abstract

PURPOSE:To enable accurate tracking of a target flying varying the direction of velocity vector by using a dynamically rectangular coordinate with the velocity vector of the target being observed as one axis while the angular speed of the target involved is calculated. CONSTITUTION:Information on target position is observed on a polar coordinate with a target observing device 1 to be converted into information based on the rectangular coordinate to calculate the Karman's gain matrix with a gain matrix calculator 2 or smoothing vector with a smoothing device 3. A forecast vector is calculated with a forecast value calculator 4 from inputs of the smoothing device 3 and a transition matrix calculator 9 and converted to the expression on the rectangular coordinate with a coordinate conversion matrix calculator 6 to be inputted into a smoothing error evaluator 8 and the gain matrix calculator 2. On the other hand, the transition matrix is determined by calculating the speed and angular speed of the target with the transition matrix calculator 9 for the forecast vector to be inputted into a forecast error evaluator 11, where a forecast error covariance matrix is calculated from the input from the unit 8 to inputted into the calculator 2. The high angle and azimuth of the target observing device 1 are controlled with a driver 13 based on positional information among forecast vectors inputted from the forecast value calculator 4.

Description

[Detailed Description of the Invention] [Field of Industrial Application] This invention inputs target position information containing observation noise from a target observation device, and estimates the true value of the target position and target motion specifications such as velocity and acceleration. This paper proposes a method for improving the accuracy of tracking filters.

[Conventional technology]

Fig. 2 is a diagram of a tracking filter configured with a normal Kalman filter. In Fig. 1, (1) is a target observation device that measures a target position including observation noise.

(2) is a gain matrix calculator for calculating the Kalman gain matrix used to calculate the smoothed values of the target position and target motion specifications; (3) is the smoother for calculating the smoothed values of the target position and target motion specifications. , (4) is a predicted value calculator that calculates the predicted values of the target position and target motion specifications after one sampling at the current time, (5) is the first delay circuit, and (8) is the evaluation of the error of the smoothed value. (111 is a prediction error evaluation device that evaluates the error of a predicted value, and α is a fourth delay circuit.

A conventional tracking filter is configured as described above.

For example, fixed orthogonal coordinates were used as the coordinate system, and a uniform linear motion model was used as the motion system model. The target observation device (1) observes distance, height angle, and azimuth including observation noise, which is target position information in polar coordinates. The gain matrix calculator (2) calculates the observation noise covariance matrix in polar coordinates obtained from a preset observation system model using the target position information in polar coordinates input from the target observation device (1) for observation in fixed orthogonal coordinates. It is converted into a noise covariance matrix, and a gain matrix is calculated according to the Kalman filter theory using this matrix and the prediction error covariance matrix calculated by the prediction error evaluation device one sampling ago. The smoother (3) uses the predicted values of the target position and target speed in the fixed orthogonal coordinates for the current moment calculated one sampling ago, which are input from the predicted value calculator (4), from the target observation device (1). A gain matrix is applied to the difference between the observed value of the target position after conversion into the input fixed orthogonal coordinates and the predicted value of the target position for the above-mentioned current point in the fixed orthogonal coordinates inputted by the predicted value calculator (4). The gain matrix inputted from the calculator (2) is corrected by the multiplied value, and smoothed values of the target position and target velocity are calculated. Predicted value calculator (4)
Now, assuming that the target performs uniform linear motion, predicted values of the target position and target speed after one sampling are calculated based on the target position and target speed calculated by the smoother (3). The smoothing error evaluation device (8) calculates the smoothed value and true value when the target moves according to the preset motion system model and the observed value of the target position is obtained according to the preset observation system model. Calculate the covariance matrix of the difference between The prediction error evaluation device calculates the predicted value and true value that occur after sampling based on the covariance matrix inputted to the smoothing error evaluation device (8). Calculate the covariance matrix of the difference between

[Problem that the invention seeks to solve]

In the conventional tracking filter as described above, even when the target is making a sharp turn, the prediction error evaluation device @ calculates the covariance matrix of the prediction error, assuming that the target is moving in a straight line at a constant velocity. occurs.

The result is transmitted to the gain matrix being calculated by the gain matrix calculator (2). Similarly, an error occurs in the predicted value calculated by the predicted value calculator (4). Since the smoother (3) calculates a smoothed value using the gain matrix and the predicted value, an error occurs every 1 samplings.

As the errors accumulate, there is a problem in that the smoothed value K becomes a value that is completely different from the true value.

There is also a method of configuring a tracking filter using a Kalman filter by adopting a motion system model in which the target moves with constant acceleration in fixed orthogonal coordinates, but as shown in Figure 3, when the target moves evenly every day, Also, the magnitude of velocity and acceleration is constant, but their direction changes from moment to moment, so the velocity and acceleration considered on each axis of the fixed orthogonal coordinates change in a sinusoidal manner, so the calculation accuracy is poor. was not improved much. here.

In Figure 3, A is the rotation center of the equal daily exercise target.

B is the position of the target at a certain time, C is the velocity vector with respect to the target position B, and D is the acceleration vector with respect to the target position B.

The present invention was made to solve these problems, and it is an object of the present invention to construct a tracking filter that can accurately track a flying target while changing the direction of its velocity vector.

[Means for solving problems]

The tracking filter of this invention uses the direction of the velocity vector as one axis when observing with a target observation device, smoothing the predetermined value, and calculating motion specifications such as the position, velocity, and acceleration of the target using a tracking filter using a Kalman filter. In addition to using dynamic orthogonal coordinates that change with the target's motion, we calculate the target's angular velocity from the target's velocity and acceleration in order to calculate the transition matrix that expresses the transition of motion parameters from one sampling time to the next sampling time. It is calculated using angular velocity.

[Effect]

In this invention, the target observation device as shown in FIG. 4, which has been conventionally used to calculate the motion specifications of the target, is used at the origin 0.
Set the tracking center in seconds instead of 0-xyz in fixed orthogonal coordinates, with the east direction being the positive X of the X axis, the north direction being the positive Y of the y axis, and the vertical upward direction being the positive 2 of the two axes. The origin is 0', the target velocity vector is the positive U of the U axis, the direction perpendicular to the velocity vector and to the right in a plane parallel to the horizontal plane is the positive V of the V axis, and the downward direction perpendicular to the velocity vector is the positive W of the W axis. Dynamic orthogonal coordinates 0'-uV
W was adopted. Here, in FIG. 4, 0 is the coordinate 0-
Origin of xyz, X is coordinate 0 - X axis of xyz, Y is coordinate 0
-xyz y-axis, 2 is the coordinate 0-xyz two axes, and in FIG. 5, 0' is the origin of the coordinate 0'-uvw.

U is the U axis with coordinates o'-uvw, ■ is the V-axis with coordinates 0'-uvw, W is the W-axis with coordinates 0' uvw, P is the angular velocity around the U-axis, Q is the angular velocity around the V-axis, and R is This is the angular velocity around the W axis. Furthermore, the angular velocity of the target that occurs when the direction of the velocity vector of the target changes every moment is calculated from the velocity and acceleration of the target, and the motion parameters of the target are calculated using the velocity, acceleration, and angular velocity according to the dynamic orthogonal coordinates 0'-uvw. This is an attempt to improve calculation accuracy by calculating the original.

〔Example〕

FIG. 1 is a block diagram showing the main parts of an embodiment of an automatic target tracking device that can most effectively utilize the present invention. In FIG. 0, (1) is a target observation device, (2) is a gain matrix calculator, ( 3) is a smoother, and (4) is a predicted value calculator.

(5) is the first delay circuit, (6) is the orthogonal coordinate 0-xyz
(7) is the second delay circuit, (8) is the smoothing error evaluation device, (9) is the target position after l sampling and A transition matrix calculator that calculates the transition matrix used to calculate the target motion specifications, αG is the third delay circuit, (
1υ is a prediction error evaluation device, (12 is a fourth delay circuit, α
J is a drive device for directing the target observation device (1) to the target.

FIG. 6 is a diagram for explaining the target attitude angle.

In the figure, H is a unit vector on the velocity vector.

■ is the vector obtained by projecting the vector H onto the horizontal plane, J is the attitude angle of the target on the horizontal plane, and the angle of the vector It from the y-axis Y of Cartesian coordinates 0-xyz is the attitude angle of the target on the vertical plane. 0 is the origin of the orthogonal coordinates 0-xyz, X is the X axis, Y is the y axis, and 2 is the z axis. From Figure 6, the orthogonal coordinates of the velocity vector 0-x
Here is the component related to yz.

Note that the orthogonal coordinates of any vector in space are the components [
uW], there is a relationship based on linear algebra if an orthogonal matrix F based on the target attitude angles J and K and its transposed matrix F1 are used.

That is, for example, the orthogonal coordinates of the acceleration vector 0-xyz
and O-uvw, respectively.

Also, if the unit vectors in the positive direction on each axis in the orthogonal coordinates 0-uvw are 100 u and e ma, respectively, then
According to the formula of vector analysis, the angular velocities P, Q,
using R.

100 Ma=4Nll+p~ ・-11111 Shout t -ew = Q iFw -P My
There is a relationship of ----------αDt. However, since the angular velocity P around the U axis is irrelevant to target tracking, p = o
-------- (2). Also, the angular velocity of the tracking target is constant.

The acceleration in the direction of travel is also constant. That is to say.

Change the target position vector in Cartesian coordinates 0-uvw,
If the speed of the target is f, that is, f = r animals y" - f - Vl = W onion 7 - Q4), then from the way the coordinate system is set, e Vu = f , Vv =
=Vw=0 and next = f−ε・=−・τII −m−−−(
to) t.

Therefore, the acceleration vector becomes. By applying equation (9) to equation αG, we obtain. On the other hand, the orthogonal coordinates of the acceleration vector 0−uvwf Au== −−−−−−−−One stop t Av = R−f −−−−−
-1 hit Aw = -Q-f -1---■. Therefore, the angular velocities R and Q are determined by Av R = -------- (211 V Q = ----m------■).

From the formula α barrel, the formula α is −”’ = Au−Mu −1−(R, f ), ew
+(Q,f)''---63dt'.

Furthermore, 1 type @ is given by the formula and the formula α mark. It can be written as

This is based on the formula. Apply equations (9) to (to) to the weapon to obtain. Furthermore, apply the formula to 2■ to obtain.

The target position prediction vector after t seconds from the current time is P, (t)
If you write it, it is a value.

Although it depends on the accuracy required of the automatic target tracking device, here, it is necessary to truncate the terms of the fourth order or higher in the equation (support).

Nojo in formula (2), that is, the target position vector formula α91 (2) and formula (2), the 3×3 unit matrix can be written as I. Here it is.

If we write the target speed prediction vector t seconds after the current time as false, we can obtain by differentiating the equation 1.

From the present moment, the target acceleration prediction vector of the sand diameter is set as A-(0
By writing, we obtain by differentiating the equation 1 country.

From formula ■1 formula■ and formula (to), sampling time ti
, using the orthogonal coordinates Q-uvv in
1--1-----CtS. Here, x is a state vector that is the true value of the target motion specifications at sampling time t.

That is, 0m-1 is the sampling time t, from -6 to 1.
is the transition matrix of the state vector to , and if I is a 3×3 unit matrix, then B! is the sampling time t. The driving noise vector corresponding to the fourth order term of the truncation error when calculating the equation (support) in K is WEI = 舊'Uw ) ' ---
------- (to), and let E be the symbol representing the mean, and wK follows the mean-dominant multivariate normal distribution. B [! , ] = and , ------11 decoy E
[Radio-concave] = QK (when K = j), O (when K ~ j) -
, (4υ.

It is also assumed that target position information is obtained from the target observation device in a polar coordinate system as shown in FIG. Here, in FIG. 7, B is the target position at a certain time, and 0 is the coordinate 0-
The origin of xyz, X is the X axis of the coordinate system o-xyz, YVi
, axis, z'fiz axis, L is the distance to the target, 1M is the azimuth angle of the target, and N is the high angle of the target. Figure 7: Px = L -ctsN -sinM
−−−−−−−(BPy = L−■N−(2)M
−−−−−−1 關P, = L −sin
M -------14). Here, if the relationship between the two minute amounts in the orthogonal coordinates o-xyz is linearly approximated using the orthogonal coordinate equations of the target position vector p, the total differential formula is obtained.

Therefore, the observation system model regarding the orthogonal coordinates 0- u vwK is 8 at sampling time t1 and F of equation (5)
are 7', (x), and Pk, respectively, then the orthogonal coordinates 0-uv at sampling time t,
The true value at w is "ZG*, Xi, so Z-=HG: G -x-+ F:nQ4 v----
---(It can be written as 481. Here, ILz is the sampling time t, and the observation vector H is the observation matrix H-(I OI OI) ---1--
-f41!1 is the observation noise vector expressed in polar coordinates at sampling time t! 1 follows the multivariate normal distribution with mean q, and g[ylI] = standing -1-----
Group E[yxys=P, (when K~j), 0I (when K~j) - (goods). Also, yl and w are mutually phaseless, that is, E[y iris! r, 1 = OI (for all, j) - 〇 埠.

From the above, in the same way as the normal Kalman filter theory, the orthogonal coordinates 0-Of at sampling time t,
Using f, 翫@(→=Φ1k(→ −−−−−−6
JP,, (→=Φ筐・p, (→・Φ:+Hahatmo・Kimi (T) 1----1 (Wealth) request←ααg XK(→+4・(To)
−H・((・へ、翫←)l −−−=(to) is obtained. Here, 1, (→ is the predicted vector xi( → is the observation vector!@I !l I...
, h for 6 such that P, (→ is the smoothed error covariance matrix at sampling time −, i.e., p, (→=B [(y, (+3−C1;
J→- to '4-cxJ'] ----------(5'eP
, 0 is the prediction error covariance matrix at the sampling time t3, that is, P, (→=E[(址)Xt)(b(−)−x工+”]
−−−−−−−(2) Shizuku is the sampling time t
It is a gain matrix at 1, and it is H' to 1 (H' to 0. & (→ to GL
+y: r, cqB, eight (19'Ftl'
・-----H.

Next, the operation of this nail-shaped tracking filter will be explained with reference to FIG.

Note that, in the same way as when the Kalman filter is normally applied, it is assumed that the initial value at sampling time -1t, K is fixed, and the case after sampling time t will be described.

The target observation device (1) observes target position information using polar coordinates, and converts the result to target position information regarding orthogonal coordinates 0-xyz. The gain matrix calculator (2) uses the target observation device (1
), the value of n at sampling time t according to Shikune is 7:
Calculate (K), preset the formula (goods) bird, sampling time 1. F3 of equation (5) at sampling time t1 predicted by
The coordinate transformation matrix calculator (61 is input through the delay circuit (7) of Input from error evaluation device αD and 0
The Kalman gain matrix 4 is calculated according to the formula (4!H of J). In the smoother (3), the second delay circuit (7
), the orthogonal coordinates 0-xy input from the target observation device (1) to the formula (factor) input from the coordinate transformation matrix calculator
The equation is a value obtained by converting the target observation position with respect to z into the target observation position with respect to the orthogonal coordinates 0-+rvw at the sampling times t and l' (at G).

Gain matrix calculator (2 ) to input the equation (to) K and the equation (H and K in 41, the smooth vector
The predicted value calculator (4) calculates X1K (→ and the sampling time tl- input from the transition matrix calculator (9) through the third delay circuit) The predicted vector X*, r←) for the sampling time t, , is calculated at the sampling time t1 according to the equation (to) using Φ in equation (5) for the sampling time t calculated in step 4.The coordinate transformation matrix calculator ( 6), the predicted vector input from the predicted value calculator (4) is -s
Expression of -xyz 0 section
Calculate F, l, and G of the equation (equation) for the ring time t-. In the smoothing error evaluation device (8), the equation inputted from the gain matrix calculator (2) is input to the coordinate transformation matrix calculator (6) through the second delay circuit (71), and the sampling time t, - Pl for the sampling time t1 calculated at the sampling time 'E-+ input from the prediction error evaluation device αD through the fourth delay circuit α (Calculate the smoothed error covariance matrix p, (→ by H in 49 according to formula -. Transition matrix calculator (9
), the predicted value calculator (4) inputs the velocity magnitude f1 of the equation circle for +1 at sampling time 1 using (→), equation 12D, and the angular velocities R and Q of equation (2).

Calculate the matrices C, , D of Equation C311 and Equation (2), and calculate Φ and - for the sampling time t of Equation (B).

In the prediction error evaluation device αυ, transition matrix Φ8. The smoothing error covariance matrix P of the equation (b) inputted from the smoothing error evaluation unit (8), (→ and the equation CD calculated on the desk in advance)
According to equation (b), the prediction error covariance matrix P for the sampling time tE is obtained by
(Calculate →.

The drive device 03 calculates the height angle and azimuth at which the target observation device (1) should point at sampling time t based on the position information among the predicted vectors 3 (←) input from the predicted value calculator (4). The target observation device (1) is controlled to point in that direction.

〔Effect of the invention〕

As described above, according to the present invention, it is possible to improve the target motion specification calculation accuracy at low cost without adding a special additional device to a normal automatic target tracking device.

Note that although the case of an automatic target tracking device has been described above, the present invention can be applied to a normal tracking filter that calculates target motion specifications from a target observation device.

[Brief explanation of drawings]

Fig. 1 is a diagram illustrating the configuration of an embodiment of the present invention;
The figure is a diagram explaining the configuration of a conventional tracking filter, Figure 3 is a diagram explaining an equal daily exercise goal, and Figure 4 is a diagram explaining the fixed orthogonal coordinate 0.
-ryz, Figure 5 is a diagram to explain the dynamic orthogonal coordinate 0-' stomach, Figure 6 is a diagram to explain the target posture angle, and Figure 7 is a diagram to explain the target posture angle.
The figure is a diagram explaining polar coordinates. In the figure, (1) is the target observation device, (2) is the gain matrix calculator, (3) is the smoother, (4) is the predicted value calculator, and (5) is the 1 delay circuit,
(6) is a coordinate transformation matrix calculator, C7) is a second delay circuit, and (8) is a smoothing error evaluation device. (9) is a transition matrix calculator, C1 is the third delay circuit, aυ
is a prediction error evaluation device, C2 is a fourth delay circuit, and Ql is a drive device. In addition, the same reference numerals are attached to the middle or corresponding portions in each figure.

Claims (1)

[Claims] The device consists of a target observation device, a smoother, and a predicted value calculator, and smoothes the observed value of the target position obtained from the target observation device to calculate the target motion specifications in fixed orthogonal coordinates such as the target position, velocity, and acceleration. A tracking filter configured to use dynamic orthogonal coordinates with one axis being the velocity vector of the target being observed by the target observation device, and to calculate and use the angular velocity of the target being observed. filter.