JPH0429034B2 - - Google Patents

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. [Prior Art] Fig. 2 is a diagram of a tracking filter configured with a normal Kalman filter. In the figure, 1 is a target observation device that measures a target position including observation noise, and 2 is a target observation device that measures the target position and target movement. A gain matrix calculator for calculating the Kalman gain matrix used to calculate the smoothed values of the specifications, 3 a smoother for calculating the smoothed values of the target position and target motion specifications, 4 the target position after one sampling from the current point. and a predicted value calculator that calculates predicted values of target motion specifications; 5 is a first delay circuit; 8 is a smoothing error evaluation device that evaluates errors in smoothed values; and 11 is a prediction error evaluation unit that evaluates errors in predicted values. The device 12 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 converts the covariance matrix of observation noise in polar coordinates obtained from a preset observation system model into the covariance matrix of observation noise in fixed orthogonal coordinates using the target position information in polar coordinates input from the target observation device 1. It is converted into a matrix, and a gain matrix is calculated according to the Kalman filter theory using this matrix and a covariance matrix of prediction errors calculated by the prediction error evaluation device one sampling ago. In the smoother 3, the predicted value calculator 4
The predicted values of the target position and target speed in fixed orthogonal coordinates with respect to the present moment calculated one sampling ago, which are inputted by The target position is corrected by multiplying the difference between the predicted value of the target position for the above-mentioned current point in the fixed orthogonal coordinates inputted from the predicted value calculator 4 by the gain matrix inputted from the gain matrix calculator 2, and calculates the target position. and calculate the smoothed value of the target speed. The predicted value calculator 4 calculates predicted values of the target position and target speed after one sampling based on the target position and target speed calculated by the smoother 3 assuming that the target performs uniform linear motion. The smoothing error evaluation device 8 calculates the difference between the smoothed value and the true value, which are calculated 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. Compute the covariance matrix of differences. The prediction error evaluation device 11 calculates, based on the covariance matrix input by the smoothing error evaluation device 8, the difference between the predicted value and the true value that will occur after one sampling from the current point when the target moves according to the motion system model. Calculate the covariance matrix. [Problems to be Solved by the Invention] In the conventional tracking filter as described above, even when the target is making a sharp turn, the prediction error evaluation device 11 calculates the prediction error based on the assumption that the target is in uniform linear motion. An error occurs because the covariance matrix is calculated, and the result is transmitted to the gain matrix being calculated by the gain matrix calculator 2. Similarly, errors occur in the predicted value calculated by the predicted value calculator 4. Since the smoother 3 calculates the smoothed value using the gain matrix and the predicted value, an error occurs every sampling, and as the error accumulates, the smoothed value will eventually become a value that is completely different from the true value. There was a problem that. There is also a method of constructing a tracking filter using a Kalman filter by adopting a motion system model in which the target performs uniform acceleration motion in fixed orthogonal coordinates, but as shown in Figure 3, the target performs uniform circular motion. In this case, the magnitude of the 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. Therefore, the calculation accuracy was not improved much. Here, in FIG. 3, A is the rotation center of the uniform circular motion 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 has been 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 the Problems] The tracking filter of the present invention smoothes the values observed by the target observation device and calculates motion parameters such as target position, velocity, and acceleration using the tracking filter using a Kalman filter. In order to calculate the transition matrix that expresses the state in which the motion specifications change from one sampling time to the next sampling time, we use dynamic orthogonal coordinates that change with the movement of the target with the direction of the velocity vector as one axis. The angular velocity of the target is calculated from the velocity and acceleration of the target, and the angular velocity is used. [Operation] In this invention, the target observation device as shown in FIG.
Instead of fixed orthogonal coordinates 0-xyz where the positive Y axis is the positive Z axis and the vertical upward direction is the positive Z axis, the tracking center is the origin 0' and the target velocity vector is the positive U axis of the u axis as shown in Figure 5. , dynamic orthogonal coordinates 0'-uvw are adopted in which the positive V of the v-axis is perpendicular to the velocity vector and in the right-hand direction in a plane parallel to the horizontal plane, and the positive W of the w-axis is perpendicular to the velocity vector and downward. Here, in Fig. 4, 0 is the coordinate 0
-xyz origin, X is the x-axis with coordinates 0-xyz, Y is the y-axis with coordinates 0-xyz, Z is the z-axis with coordinates 0-xyz, and in Figure 5, 0' is the coordinate 0'-uvw. The origin of
U is the u axis with coordinates 0'-uvw, V is v with coordinates 0'-uvw
The axis, 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 the angular velocity around the w-axis. Furthermore, the angular velocity of the target that occurs when the direction of the target's velocity vector changes every moment is calculated from the target's velocity and acceleration, and the dynamic orthogonal coordinate 0'-
This is an attempt to improve calculation accuracy by calculating the motion specifications of the target using velocity, acceleration, and angular velocity using UVW. [Embodiment] FIG. 1 is a block diagram showing the main parts of an embodiment of an automatic target tracking device in which the present invention can be most effectively utilized. In the figure, 1 is a target observation device, 2 is a gain matrix calculator, and 3 is a smoother, 4 is a predicted value calculator,
5 is a first delay circuit, 6 is a coordinate transformation matrix calculator that calculates a coordinate transformation matrix between orthogonal coordinates 0-xyz and orthogonal coordinates 0-uvw, 7 is a second delay circuit, 8 is a smoothing error evaluation device, 9 is a transition matrix calculator that calculates a transition matrix used to calculate the target position and target motion specifications after one sampling; 10 is a third delay circuit; 11 is a prediction error evaluation device; 12 is a fourth The delay circuit 13 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, I is a vector obtained by projecting vector H onto the horizontal plane, and J is the attitude angle of the target on the horizontal plane, with orthogonal coordinates 0. - Angle from the y-axis Y of xyz to the vector I, K
is the attitude of the target in the vertical plane, and is the angle from vector I to vector H, 0 is the origin of Cartesian coordinates 0-xyz, X is the x-axis, Y is the y-axis, and Z is the z-axis. 6th
From the figure, if the component related to the orthogonal coordinates 0-xyz of the velocity vector is V _x V _y V _z , then J=tan ^-1 V _x /V _y ……(1) It is. Note that the orthogonal coordinates 0− of any vector in space
Between the component x y z related to xyz and the component u v w related to the orthogonal coordinate 0-uvw, if we use the orthogonal matrix F by the target attitude angles J and K and its transposed matrix F ^T , we can calculate x y by linear algebra. z=Fx v w...(3) u v w=F ^T x yz...(4) There is a relationship between In other words, for example, the Cartesian coordinate of the acceleration vector is 0
If the components related to -xyz and 0-uvw are respectively A _x A _y A _z = A _w , then A _u A _v A _w = F ^T A _x A _y A _z ...(7) A _x A _y A _z = F There is the following relationship: ^T A _u A _v A _z ……(8). Also, if the unit vectors in the positive direction on each axis in the orthogonal coordinates 0-uvw are _u , _v , _w , then using the formula of vector analysis, using the angular velocities P, Q, and R in Figure 5, , d/dt _u =R _v -Q _w ...(9) d/dt _v = -R _u +Q _w ...(10) d/dt _w =Q _u -P _v ...(11) . However, since the angular velocity P around the u-axis is irrelevant to target tracking, P=0...(12). Also, the angular velocity of the tracking target is constant,
It is also assumed that the time differential value of the acceleration in the direction of travel is constant. That is, d/dtQ=0, d/dtR=0, d/dtA _u =0 (13). ^The _position vector of ^the target _at ^the orthogonal coordinates _{0 - uvw} ^is given ^by _the ^coordinates From the way the system is taken, V _u = f, V _v =
When V _w = 0, dp/dt = f・_u = V _u・_u (15). Therefore, the acceleration vector is d ² p/dt ² = df/dt· _u + f·de _u /dt (16). By applying equation (9) to equation (16), we obtain d ² p/dt ² = df/dt・_u + (R・f)・_v + (−Q・f)・_w (17). On the other hand, the orthogonal coordinates of the acceleration vector 0-
Since the component related to uvw is A _u A _v A _w , A _u = df/dt (18) A _v = R・f (19) A _w = −Q・f (20). Therefore, the angular velocities R and Q are determined by R=A _v /f (21) Q=-A _w /f (22). From equation (18), (17) becomes d ² p/dt ² =A _u・_u + (R・f)・_v + (−Q・f)・_w (23). Furthermore, since the component of the acceleration vector d ₂ P/dt ² with respect to the orthogonal coordinate 0− uvw is A _u v A _w , d ² p/dt ² = A _u・_u + A _u・_v + A _w・_w ……(24 ) can be written. From formula (23), d ³ p/dt ³ = dA _u /dt・_u +A _u・de _u /dt+(dR/dt
・f)・_v + (R・df/dt)・_v +(R・f)・_dev
/dt +(-d _Q /dt・f) _w +(-Q・df/dt)・_w
+(-Q・f)・de _w /dt...(25) Applying (9) to (13) and equation (18) to equation (25), d ³ p/dt ³ = (−Q ²・f−R ²・f)・_u +2R・df/d
Obtain t・_v −2Q・df/dt・_w ……(26). Furthermore, by applying equation (18) to equation (26), d ³ p/dt ³ = −(Q ² + R ² )・f・_u +2R・A _u・_v −
2Q・A _u・_w …(27) is obtained. The target position prediction vector t seconds after the current time is
If we write _F (t), we get (t)=∞ 〓 n±0P ⁽ⁿ⁾ (0)/n〓t ⁿ ……(28). Here, ⁽ⁿ⁾ (0) is the value of d ⁿ P/dt· at time 0. Although it depends on the accuracy required of the automatic target tracking device, here, if we discontinue the terms of fourth order or higher in equation (28), P _F (t) = ₃ 〓 ^n2o P ⁽ⁿ⁾ (0)/n〓t ⁿ ... …(29) becomes. In equation (29), if ⁽⁰⁾ (0), that is, the component related to the orthogonal coordinates 0-uvw of the target position vector is P _u P _v P _w , then ⁽¹⁾ (0), that is, the orthogonal component of the target velocity vector at the current time The component related to the coordinate 0-uvw is V _u A _v A _w , and P ⁽²⁾ (0), that is, the component related to the orthogonal coordinate 0-uvw of the target acceleration vector at the current time is A _u A _u A _w , and P ^{( 3)} (0) From the way the coordinate system is taken, V _u = f,
Since V _v = V _w = 0, use equation (27) and calculate 3×3
If we write the unit matrix of as I, then _F (t) = P _u P _v P _w + (tI + t ³ /3〓C3) V _u V _v V _w + (t ² /2〓I + t ³ /3〓D3) A _u A _v A _w ……(30). Here, C ₃ =−Q ² +R ² 0 0 0 0 0 0 0 0 (31) D ₃ =0 0 0 2R 0 0 −2Q 0 0 (32). The target speed prediction vector t seconds after the current time is
If we write _F (t), by differentiating equation (30), we get _F (t) = (I + t ² /2〓C3)V _u V _v V _w + (tI + t ² /2〓D3)A _u A _v A _w …(33) is obtained. If we write the target acceleration prediction vector t seconds from the current moment as V _F (t), then by differentiating equation (33), we get _F (t) = tC ₃ V _u V _v V _w + (I + tD ₃ ) A _u A _v A _w ……(34) is obtained. From Equations (30), Equations (33) and Equations (34), using Cartesian coordinates 0-uvw at sampling time tK _-1 ,
The _{motion system} model _{can be} written _as Here, X _K is the sampling time t _K
is the _state vector _that is _the true value of the _target motion specifications _at This is the transition matrix of the state vector to _K , and if I is a 3 x 3 unit matrix (OI is a 3 x 3 zero matrix). , W _K is the equation (29) at sampling time t _K
is the driving noise vector corresponding to the fourth-order term of the truncation error when calculating , where E is the symbol representing the average, and W K follows a multivariate normal distribution with mean 0. E[ W _K ] = 0 ... (40) E [ W _K W ^T _j = Q _K K = j) , OI (when K≠j)
...(41) (A ^T represents the transposed matrix of matrix A). 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, 0 is the origin of coordinates 0-xyz, and X is the coordinate system 0.
-xyz x-axis, Y is the y-axis, Z is the z-axis, L is the distance to the target, M is the azimuth angle of the target, and N is the high angle of the target. From Fig. 7, P _x = L・cosN・sinM ……(42) P _y = L・cosN・sinM ……(43) P _z = L・sinN ……(44). Here, the component related to the orthogonal coordinates 0-xyz of the target position vector is defined as P _x P _y P _z . Using equations (42) to (44), if the infinitesimal quantity in the orthogonal coordinates 0-xyz is dPx dPy dPz, and the infinitesimal quantity in the polar coordinates is dL dN dM, and the relationship between the two infinitesimal quantities is linearly approximated, then from the total differential formula, dPx dPy dPz＝Γ ₂ dL dN dM ……(45) becomes. Therefore, the observation system model for Cartesian _coordinates 0-uvw _{is as follows: G K F K 0I 0I} 0I F _K 0I 0I 0I F _K ……(47) Then, the orthogonal coordinates 0− at sampling time t _K
Since _the true value at uvw _is G ^T _{K G K - 1} The state vector regarding the coordinates 0−xyz is G _K ^T
_{G K -} 1 It can be written as Z _K = HG ^T _K G _K-1 F ^T _K Γ ₂ (K) v _K ……(48). Here, z _K is the observation vector at sampling time t _K , H is the observation matrix, H = (I 0I 0I) ... (49) v _K is the observation noise vector expressed in polar coordinates at sampling time t _K , and Γ ₂ (k ) V _K is the formula (45)
It becomes observation noise regarding orthogonal coordinates 0-xyz,
F _K ^T Γ ₂ (k) From equation (4), V _K is the observation noise with respect to the orthogonal coordinate 0-uvw at sampling time t _K , and V _K follows a multivariate normal distribution with mean 0. E[ V _K ] = 0... (50) E[ V _K V ^T _j ]=R _K (when K≠j), 0I (when K≠j) ...(51). Also, V ^j and W _K are phaseless with each other, that is, E[ V _K W ^T _j ]=0I (for all k and j)
...(52). From the above _, using the orthogonal coordinates 0 - uvw at the sampling time _{t K} , in the same way as the normal Kalman filter _theory , P _K+1 (−)=Φ _K・P _K (+)・Φ ^T _K +Γ ₁ (K)・Q _x・
Γ ₁
(K) ^T ...(54) X _K (+)=G ^T _K G _K-1 X _K (-)+K _k・[ Z _K −H・
_G ^T _{K ・} G _{K -} ¹ _{_ _ _} (−)G ^T _K-1 G _K ……(56) is obtained. Here, X _K (-) is the observation vector Z ₀ , Z ₁ , ...,
The predicted vector X _K (+) of X _K from Z _K-1 is the observed vector Z ₀ , Z ₁ ,...,
The smooth vector P _K (+) for X _K from Z _K is the smooth error covariance matrix at sampling time t _K , that is, P _K (+) = E [( X _K (+) − G ^T _K G _K-1 X _K ) ( X _K
⁽ + ₎ − _G ^T _{K G K} - _{1 K} ( _- ) ₋ _ _{_} ^_ _{_ _ _} ^_ _{_ _ _} (−)G ^T _K-1 G _K H ^T (HG ^T _K G _K-1 P _K (−)G ^T _K-
1 G _K H ^T +F ^T _K Γ ₂ (K) R _K Γ ₂ (K) ^T F _K ) ^-1 ...(59). Next, the operation of the tracking filter of the present invention will be explained with reference to FIG. In addition, in the same way as when applying the Kalman filter normally, the initial values of sampling time t ₀ ,
Assuming that the value at t ₁ is fixed, the case after sampling time t ₂ will be explained. 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 calculates the distance L to the target observed by the target observation device 1, the height angle N,
Sampling time t _K according to equation (46) according to azimuth M
The value Γ ₂ (K) of Γ ₂ at is calculated, R _K of equation (51) set in advance, F _K of equation (5) at sampling time t _K predicted at sampling time T _K-1, and ( 47 ₎ is inputted from the coordinate transformation matrix calculator 6 through the second delay circuit 7, and P _K (-) in equation (54) for the sampling time t _K predicted at the sampling time t _K-1 is inputted as the second delay circuit 7. The signal is inputted from the prediction error evaluation device 11 through the delay circuit 12 of No. 4, and the Kalman gain matrix K _K is calculated according to equation (59) using H in equation (49). In the smoother 3, _GK of (47) is inputted from the coordinate transformation matrix calculator through the second delay circuit 7, and the target observation position regarding the orthogonal coordinates 0-xyz inputted from the target observation device 1 is determined at the sampling time using the above _GK .
Z _x in equation (48), which is the value converted to the target observation position with respect to the orthogonal coordinate 0-uvw at t _K , is predicted at the sampling time t _K-1 input from the predicted value calculator 4 through the first delay circuit 5. sampling time t _K
X _K (-) of equation (53), which is the predicted value for , K _K of equation (59) input from gain matrix calculator 2, and equation (4)
9), the smooth vector X _K according to equation (55)
Calculate (+). In the predicted value calculator 4, the smoother 3
X _K (+) and the third delay circuit 1
Prediction for sampling time tK ₊₁ at sampling time _tK according to equation (53) using Φ _K of equation (37) for sampling time _tK calculated at sampling time _tK-1 input from the transition matrix calculator 9 through 0 Calculate the vector X _K+1 (-). In the coordinate _{transformation} matrix calculator _{6 , the} predicted vector After converting to the velocity component, F _K+ 1 for the sampling time t _K+1 of Equation (5) and Equation (47) are calculated using Equation (1) and Equation (2).
Calculate G _K+1 of . In the smoothing error evaluation device 8, K _K of equation (59) inputted from the gain matrix calculator 2, the second
G _K in Equation (47) for the sampling time t _K calculated at the sampling time t _K-1 input from the coordinate transformation matrix calculator 6 through the delay circuit 7 , and from the predicted value error evaluation device 11 through the fourth delay circuit 12 A smoothed error covariance matrix P _K (+) is calculated according to equation (56) using P _K (-) for sampling time t _K calculated at input sampling time t _{K-1 and} H in equation (49). The transition matrix calculator 9 uses the X _K+1 (-) input from the predicted value calculator 4 to calculate the speed f of equation (14) with respect to sampling time t _K+1 , and equation (21)
and the angular velocities R, Q of equation (22), equation (31) and equation (32)
The matrices C ₃ and D ₃ of are calculated and the sampling time of equation (37) is calculated.
Calculate Φ _K+1 for t _K+1 . The prediction error evaluation device 11 uses the transition matrix Φ _K of equation (37) for the sampling time t _K inputted from the transition matrix calculator 9 through the third delay circuit 10, and the equation (56) inputted from the smoothing error evaluation device 8. smoothed error covariance matrix P _K (+)
and equation (41) calculated in advance on paper.
Prediction error covariance matrix P K+1 for sampling time t K _{+ 1} according to Equation (54) using Q _K and Γ ₁ (K) of Equation (39).
Calculate (-). The drive device 13 calculates the high angle and azimuth angle at which the target observation device 1 should aim at sampling time t K+1 based on the position information of the predicted vector X K _{+ 1} (−) inputted from the predicted value calculator 4. is calculated and the target observation device 1 is controlled to point in that direction. [Effects of the Invention] As described above, according to the present invention, it is possible to improve the calculation accuracy of target motion parameters at low cost without adding any special additional device to a normal automatic target tracking device. Although the case of the 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 for explaining the configuration of an embodiment of the present invention, FIG. 2 is a diagram for explaining the configuration of a conventional tracking filter, FIG. 3 is a diagram for explaining a uniform circular motion target, and FIG. 4 is a diagram for explaining the configuration of a conventional tracking filter. Diagram explaining fixed orthogonal coordinates 0-xyz, 5th
The figures are diagrams for explaining dynamic orthogonal coordinates 0'-uvw, FIG. 6 is a diagram for explaining the target posture angle, and FIG. 7 is a diagram for explaining polar coordinates. In the figure, 1 is the target observation device;
2 is a gain matrix calculator, 3 is a smoother, 4 is a predicted value calculator, 5 is a first delay circuit, 6 is a coordinate transformation matrix calculator, 7 is a second delay circuit, 8 is a smoothing error evaluation device, 9 is a transition matrix calculator, 10 is a third delay circuit, 11 is a prediction error evaluation device, 12 is a fourth delay circuit, and 13 is a drive device. Note that the same or corresponding parts in each figure are designated by the same reference numerals.

Claims (1)

[Claims] 1. A target observation device that observes target position information, a gain matrix calculator that calculates a gain matrix in orthogonal coordinates with the velocity vector as one axis, and a target movement such as target position, velocity, acceleration, etc. in orthogonal coordinates with the velocity vector as one axis. A smoother that calculates the smoothed values of the specifications, a predicted value calculator that calculates the predicted values of the target motion specifications after one sampling from the current time in orthogonal coordinates with the velocity vector as one axis, and a smoother that calculates the smoothed values of the specifications. A smoothing error evaluation device that calculates a value error in orthogonal coordinates with the speed vector as one axis, and a prediction error evaluation device that calculates the error of the predicted value calculated by the predicted value calculator in orthogonal coordinates with the speed vector as one axis. , a coordinate transformation matrix calculator that calculates a transformation matrix from fixed orthogonal coordinates to orthogonal coordinates with the velocity vector as one axis using the predicted value calculated by the predicted value calculator, a predicted value calculated by the predicted value calculator 1. A tracking filter comprising a transition matrix calculator that calculates a transition matrix of a target using an angular velocity of the target calculated based on the angular velocity of the target.