Target tracking device and target tracking method

Abstract

The target tracking device is configured to include an initial value calculation unit (2) that acquires position observation information indicating each of the target observation positions at two different sampling times from a sensor that observes the position of the target, and calculates initial values ​​of smoothed values ​​including each of the target's position vector and target's velocity vector as initial values ​​of smoothed values ​​indicating the state of the target based on the position observation information. The target tracking device also includes a smoothing unit (3) that performs an estimation process of the smoothed value indicating the state of the target using the position observation information output from the sensor and a predicted value indicating the state of the target, and a prediction unit (4) that predicts the future state of the target using the initial value of the smoothed value calculated by the initial value calculation unit (2) or the smoothed value estimated by the smoothing unit (3), and outputs a predicted value indicating the prediction result of the future state of the target to the smoothing unit (3) as a predicted value to be used by the smoothing unit (3) for the next estimation process.

Description

[Technical Field]

[0001] This disclosure relates to a target tracking device and a target tracking method. [Background technology]

[0002] There is a target tracking device that tracks a target by predicting both the target's position and its speed. As an example of such a target tracking device, Non-Patent Document 1 discloses a target tracking device that uses a tracking filter to predict the position of a target, etc. [Prior art documents] [Non-patent literature]

[0003] [Non-Patent Document 1] “A Cost-Effective Tracking Algorithm for Hypersonic Glide Vehicle Maneuver Based on Modified Aerodynamic Model,” June 2016, Applied Sciences, [Retrieved December 27, 2022], Internet<URL:http: / / www.mdpi.com / journal / applsci> [Overview of the Initiative] [Problems that the invention aims to solve]

[0004] In the target tracking device disclosed in Non-Patent Document 1, in order to predict the target's position, etc., the tracking filter needs to be set with initial smoothing values ​​that include the target's position vector and the target's velocity vector, respectively, as initial values ​​for the smoothing values ​​that represent the target's state. If the initial values ​​for the smoothing values ​​deviate significantly from the original smoothing values, the accuracy of the position prediction by the target tracking device deteriorates. As a result, there was a problem that the target tracking device may become unable to track the target.

[0005] This disclosure was made to solve the above-mentioned problems, and aims to provide a target tracking device that can suppress the deterioration of prediction accuracy caused by the initial value of the smoothed value deviating significantly from the original smoothed value. [Means for solving the problem]

[0006] The target tracking device according to this disclosure includes an initial value calculation unit that acquires position observation information from a sensor that observes the position of a target, indicating the respective target observation positions at two different sampling times, and calculates initial smoothing values ​​that include the target's position vector and the target's velocity vector as initial smoothing values ​​indicating the state of the target, based on the position observation information. The target tracking device also includes a smoothing unit that performs estimation processing of smoothing values ​​indicating the state of the target using the position observation information output from the sensor and predicted values ​​indicating the state of the target, and a prediction unit that predicts the future state of the target using the initial smoothing values ​​calculated by the initial value calculation unit or the smoothing values ​​estimated by the smoothing unit, and outputs predicted values ​​indicating the predicted result of the future state of the target to the smoothing unit as predicted values ​​to be used in the next estimation process. Furthermore, the prediction unit includes a first coordinate transformation unit that transforms the coordinate system of the initial smoothing values ​​calculated by the initial value calculation unit, or the coordinate system of the smoothing values ​​estimated by the smoothing unit, into the target aircraft coordinate system; a prediction processing unit that uses the initial values ​​of the aircraft coordinate system after transformation by the first coordinate transformation unit, or the smoothing values ​​of the aircraft coordinate system after transformation by the first coordinate transformation unit, to predict the future state of the target and outputs a predicted value indicating the predicted result of the target's future state; and a second coordinate transformation unit that transforms the coordinate system of the predicted value output from the prediction processing unit into a North-referenced Cartesian coordinate system. The smoothing unit acquires the predicted value of the North-referenced Cartesian coordinate system after transformation by the second coordinate transformation unit as a predicted value to be used in the next estimation process, and performs estimation processing of smoothing values ​​indicating the state of the target using the position observation information output from the sensor and the predicted value of the North-referenced Cartesian coordinate system. [Effects of the Invention]

[0007] According to this disclosure, it is possible to suppress the deterioration of prediction accuracy that occurs when the initial value of the smoothed value deviates significantly from the original smoothed value. [Brief explanation of the drawing]

[0008] [Figure 1] This is a configuration diagram showing a target tracking device according to Embodiment 1. [Figure 2] This is a hardware configuration diagram showing the hardware of the target tracking device according to Embodiment 1. [Figure 3] This is a hardware configuration diagram of a computer when a target tracking device is implemented using software or firmware. [Figure 4] This flowchart shows the target tracking method, which is the processing procedure for the target tracking device. [Figure 5] This is a configuration diagram showing a target tracking device according to Embodiment 2. [Figure 6] This is a hardware configuration diagram showing the hardware of the target tracking device according to Embodiment 2. [Figure 7] This is an explanatory diagram showing the constant aerodynamic vector αn. [Figure 8] This flowchart shows the target tracking method, which is the processing procedure for the target tracking device. [Modes for carrying out the invention]

[0009] To provide a more detailed explanation of this disclosure, the forms for implementing this disclosure will be described below with reference to the attached drawings.

[0010] Embodiment 1. Figure 1 is a configuration diagram showing a target tracking device according to Embodiment 1. Figure 2 is a hardware configuration diagram showing the hardware of the target tracking device according to Embodiment 1. The target tracking device shown in Figure 1 comprises a tracking gate unit 1, an initial value calculation unit 2, a smoothing unit 3, and a prediction unit 4.

[0011] The tracking gate section 1 is implemented by the tracking gate circuit 11 shown in Figure 2. The tracking gate unit 1 acquires position observation information indicating the target observation position from a sensor (not shown) that observes the target's position. The tracking gate unit 1 outputs position observation information indicating the respective target observation positions at two different sampling times to the initial value calculation unit 2, for example, the position observation information for the first sampling time and the position observation information for the second sampling time. For position observation information from the third sampling time onward, the tracking gate unit 1 outputs the position observation information to the smoothing unit 3 only if the target observation position indicated by the position observation information is located within the tracking gate. Furthermore, if the target observation position indicated by the position observation information is within the tracking gate, the tracking gate unit 1 outputs the previously predicted value by the prediction unit 4 and the prediction error covariance matrix calculated by the prediction unit 4 to the smoothing unit 3. Here, the tracking gate unit 1 outputs position observation information for the first sampling time and position observation information for the second sampling time to the initial value calculation unit 2 as position observation information indicating the target observation positions at two different sampling times. However, this is just one example, and the tracking gate unit 1 may also output position observation information for the second sampling time and position observation information for the third sampling time to the initial value calculation unit 2 as position observation information indicating the target observation positions at two different sampling times, or it may output position observation information for the third sampling time and position observation information for the fourth sampling time to the initial value calculation unit 2.

[0012] The initial value calculation unit 2 is implemented by the initial value calculation circuit 12 shown in Figure 2. The initial value calculation unit 2 obtains, for example, position observation information for the first sampling time and position observation information for the second sampling time from the tracking gate unit 1. The initial value calculation unit 2 calculates initial values ​​for smoothed values ​​that represent the state of the target, including the target's position vector and the target's velocity vector, based on the position observation information from the first sampling time and the position observation information from the second sampling time. Furthermore, the initial value calculation unit 2 calculates the initial value of the smoothing error covariance matrix based on the observation error covariance matrix and the driving noise error covariance matrix, which will be described later. The initial value calculation unit 2 outputs the initial values ​​of the smoothed values ​​and the initial values ​​of the smoothing error covariance matrix to the prediction unit 4.

[0013] The smoothing section 3 is implemented by the smoothing circuit 13 shown in Figure 2. The smoothing unit 3 obtains position observation information, predicted values, and a prediction error covariance matrix from the tracking gate unit 1. The smoothing unit 3 uses position observation information, predicted values, and a prediction error covariance matrix to perform an estimation process for smooth values ​​that represent the target state. Specifically, the smoothing unit 3 estimates a smoothed value vector that includes the target's position vector, the target's velocity vector, and the target's aerodynamic coefficient, respectively, as smoothed values. For the target tracking device shown in Figure 1 to track a target, the target's position vector and target's velocity vector are essential as smoothing values ​​estimated by the smoothing unit 3. If the target's aerodynamic coefficient is not estimated as a smoothing value by the smoothing unit 3, the target tracking accuracy will decrease slightly, but target tracking is still possible even without the smoothing unit 3 estimating the target's aerodynamic coefficient. Furthermore, the smoothing unit 3 calculates a smoothing error covariance matrix based on the prediction error covariance matrix. The smoothing unit 3 outputs the smoothed value and the smoothing error covariance matrix to the prediction unit 4.

[0014] The prediction unit 4 is implemented by the prediction circuit 14 shown in Figure 2. The prediction unit 4 includes a first coordinate transformation unit 4a, a prediction processing unit 4b, a second coordinate transformation unit 4c, and a third coordinate transformation unit 4d. The prediction unit 4 obtains the initial value of the smoothed value from the initial value calculation unit 2, or the smoothed value from the smoothing unit 3. The prediction unit 4 predicts the target future state using the initial value of the smoothed value, or the smoothed value itself. The prediction unit 4 outputs a predicted value, which indicates the predicted future state of the target, to the tracking gate unit 1 as the predicted value that the smoothing unit 3 will use for the next estimation process. Furthermore, the prediction unit 4 obtains the initial value of the smoothing error covariance matrix from the initial value calculation unit 2, or the smoothing error covariance matrix from the smoothing unit 3. The prediction unit 4 calculates the prediction error covariance matrix using the initial values ​​of the smoothing error covariance matrix, or the smoothing error covariance matrix. The prediction unit 4 outputs the calculated prediction error covariance matrix to the tracking gate unit 1 as the prediction error covariance matrix that the smoothing unit 3 will use for the next estimation process.

[0015] The first coordinate transformation unit 4a obtains the initial value of the smoothed value from the initial value calculation unit 2, or the smoothed value from the smoothing unit 3. The first coordinate transformation unit 4a transforms the coordinate system of the initial smoothed value, or the coordinate system of the smoothed value, from the North-referenced Cartesian coordinate system to the target aircraft coordinate system. The first coordinate transformation unit 4a outputs an initial value of the smoothed value of the aircraft coordinate system, or the smoothed value of the aircraft coordinate system, to the prediction processing unit 4b. Furthermore, the first coordinate transformation unit 4a obtains the initial value of the smoothing error covariance matrix from the initial value calculation unit 2, or the smoothing error covariance matrix from the smoothing unit 3. The first coordinate transformation unit 4a transforms the coordinate system of the initial values ​​of the smoothing error covariance matrix, or the coordinate system of the smoothing error covariance matrix, from the North-referenced Cartesian coordinate system to the target aircraft coordinate system. The first coordinate transformation unit 4a outputs the initial value of the smoothing error covariance matrix of the aircraft coordinate system, or the smoothing error covariance matrix of the aircraft coordinate system, to the prediction processing unit 4b.

[0016] The prediction processing unit 4b obtains the initial value of the smoothed value of the aircraft coordinate system, or the smoothed value of the aircraft coordinate system, from the first coordinate transformation unit 4a. The prediction processing unit 4b predicts the future state of the target using the initial value of the smoothed value of the aircraft coordinate system, or the smoothed value of the aircraft coordinate system. The prediction processing unit 4b outputs a predicted value indicating the predicted future state of the target to the second coordinate transformation unit 4c. Furthermore, the prediction processing unit 4b obtains the initial value of the smoothing error covariance matrix of the aircraft coordinate system, or the smoothing error covariance matrix of the aircraft coordinate system. The prediction processing unit 4b calculates the prediction error covariance matrix using the initial value of the smoothing error covariance matrix of the aircraft coordinate system, or the smoothing error covariance matrix of the aircraft coordinate system. The prediction processing unit 4b outputs the prediction error covariance matrix of the aircraft coordinate system to the second coordinate transformation unit 4c.

[0017] The second coordinate transformation unit 4c obtains the predicted values ​​of the aircraft coordinate system and the prediction error covariance matrix of the aircraft coordinate system from the prediction processing unit 4b. The second coordinate transformation unit 4c transforms the coordinate system of the predicted values ​​from the aircraft coordinate system to the North-referenced Cartesian coordinate system. The second coordinate transformation unit 4c outputs the predicted values ​​in the North reference Cartesian coordinate system and the predicted error covariance matrix of the aircraft coordinate system to the third coordinate transformation unit 4d.

[0018] The third coordinate transformation unit 4d obtains the predicted values ​​of the North-reference Cartesian coordinate system and the predicted error covariance matrix of the aircraft coordinate system from the second coordinate transformation unit 4c. The third coordinate transformation unit 4d transforms the coordinate system of the prediction error covariance matrix from the aircraft coordinate system to the North-referenced Cartesian coordinate system, and adds driving noise to the transformed prediction error covariance matrix. The third coordinate transformation unit 4d outputs the predicted values ​​in the North reference Cartesian coordinate system and the prediction error covariance matrix with driving noise to the tracking gate unit 1. In the target tracking device shown in Figure 1, the prediction unit 4 is equipped with a third coordinate transformation unit 4d. If the prediction unit 4 is not equipped with the third coordinate transformation unit 4d, the target tracking accuracy will be slightly reduced, but target tracking is still possible even without the prediction unit 4 being equipped with the third coordinate transformation unit 4d. If the prediction unit 4 is not equipped with the third coordinate transformation unit 4d, the second coordinate transformation unit 4c transforms the coordinate system of the prediction error covariance matrix from the aircraft coordinate system to the North-referenced Cartesian coordinate system, and outputs the transformed prediction error covariance matrix to the tracking gate unit 1.

[0019] In Figure 1, it is assumed that the tracking gate unit 1, initial value calculation unit 2, smoothing unit 3, and prediction unit 4, which are components of the target tracking device, are each implemented by dedicated hardware as shown in Figure 2. That is, the target tracking device is assumed to be implemented by a tracking gate circuit 11, an initial value calculation circuit 12, a smoothing circuit 13, and a prediction circuit 14. Each of the tracking gate circuit 11, initial value calculation circuit 12, smoothing circuit 13, and prediction circuit 14 can be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field-Programmable Gate Array), or a combination thereof.

[0020] The components of a target tracking device are not limited to those implemented by dedicated hardware; the target tracking device may also be implemented by software, firmware, or a combination of software and firmware. Software or firmware is stored as a program in the computer's memory. A computer refers to the hardware that executes programs, and includes, for example, a CPU (Central Processing Unit), GPU (Graphics Processing Unit), central processing unit, processing unit, arithmetic unit, microprocessor, microcomputer, processor, or DSP (Digital Signal Processor).

[0021] Figure 3 is a hardware diagram of a computer when the target tracking device is implemented by software or firmware. When the target tracking device is implemented using software or firmware, a program is stored in memory 21 that instructs the computer to execute the respective processing procedures in the tracking gate unit 1, initial value calculation unit 2, smoothing unit 3, and prediction unit 4. The computer's processor 22 then executes the program stored in memory 21.

[0022] Furthermore, Figure 2 shows an example where each component of the target tracking device is implemented by dedicated hardware, and Figure 3 shows an example where the target tracking device is implemented by software or firmware, etc. However, this is only one example, and some components of the target tracking device may be implemented by dedicated hardware, while the remaining components may be implemented by software or firmware, etc.

[0023] Next, we will explain the operating principle of the target tracking device shown in Figure 1. In the target tracking device shown in Figure 1, the state vector X of the tracking filter k However, it is assumed that it can be expressed as shown in equation (1) below. The observation value vector Z, which is position observation information given from the sensor to the target tracking device k shall be expressed as in the following formula (2).

[0024] In formulas (1) and (2), T is a mathematical symbol indicating transposition.

[0025] The state vector X k contains x k , y k , z k which is a position vector indicating the position of the target. The state vector X k contains x k dot, y k dot, z k dot is a velocity vector indicating the velocity of the target. In the text of the specification, due to the relationship of electronic applications, the symbol "·" cannot be attached above the characters, so it is expressed as x k dot. The position vector x k , y k , z k and the velocity vector x k dot, y k dot, z k dot are each represented in the north reference orthogonal coordinate system. The state vector X k contains α L1 which corresponds to the aerodynamic coefficient related to the component that raises the target aircraft by lift. Specifically, α L1 is the result of dividing the aerodynamic coefficient × reference area by the mass. Hereinafter, for the sake of convenience of explanation, α L1 will be explained as the aerodynamic coefficient of the target. The state vector X k contains α L2 which corresponds to the aerodynamic coefficient related to the component that maneuvers the target aircraft in the horizontal direction. Hereinafter, α L2 will also be explained as the aerodynamic coefficient of the target. The state vector X kThe β included in this expression corresponds to the aerodynamic coefficient related to air resistance. Hereafter, β will also be explained assuming it represents the target aerodynamic coefficient. Aerodynamic coefficient α L1 ,α L2 β is expressed in the target aircraft coordinate system. Therefore, the state vector X k The coordinate system is defined as a mixture of the North-referenced Cartesian coordinate system and the aircraft coordinate system.

[0026] Furthermore, the state vector of the tracking filter is given by Y as shown in equation (3) below. k It can also be expressed as follows.

[0027] TIFF0007395079000002.tif13166

[0028] The state vector X shown in equation (1) k And the state vector Y shown in equation (3) k There are differences in 4 to 6 items. State vector Y k V included k This can be expressed as shown in equation (4) below: State vector Y k gamma contained in k This can be expressed as shown in equation (5) below: State vector Y k η included k This can be expressed as shown in equation (6) below.

[0029] TIFF0007395079000003.tif77166

[0030] In the target tracking device shown in Figure 1, the state vector X shown in equation (1) is used. k And the state vector Y shown in equation (3) k The relationship between the two can be expressed as shown in equation (7) below.

[0031] In equation (7) of TIFF0007395079000004.tif14166, g() is the state vector Xk state vector Y k This is a function for converting to [a specific format].

[0032] In the target tracking device shown in Figure 1, the motion model of the target is assumed to be represented by a first-order differential equation, as shown in equation (8) below. Equation (9) below represents the state vector Y k V is included in k The first derivative of γ k The first derivative and η k This shows the first-order derivatives of each of them.

[0033] In equation (9) of TIFF0007395079000005.tif59166, g is the acceleration due to gravity. γ k This can be expressed as shown in equation (10) below. L is the lift force acting vertically along the aircraft's axis and is expressed as shown in equation (11) below. Y is the lift force acting horizontally along the aircraft axis and is expressed as shown in equation (12) below. D is the component of the aerodynamic force due to the drag force acting in the opposite direction to the target velocity vector, and is expressed as shown in equation (13) below.

[0034] TIFF0007395079000006.tif81166

[0035] In equations (11) to (13), ρ is the air density, which changes with the target altitude h as shown in equation (14) below.

[0036] In equation (14) of TIFF0007395079000007.tif21166, ρ0 is the atmospheric density at the Earth's surface. ρ0 = 1.3906 [kg / m³] 3 ] h s h represents the height of the scale. s = 7.163 × 10 3 It is [m].

[0037] Non-patent document 1 describes equations (11) to (13) based on the aircraft's attitude information, namely the aircraft's bank angle and angle of attack. The aircraft's attitude information is unknown. Therefore, in the target tracking device shown in Figure 1, the state vector Y k The aerodynamic coefficient α is an element of this. L1 This includes the lift L and the aerodynamic coefficient α. L2 Assuming that the lift force Y is included in the state vector Y and the aerodynamic coefficient β contains component D, k This is defined.

[0038] The state vector X shown in equation (7) k and state vector Y k Based on the relationship, when the coordinate system of the target motion model shown in equation (8) is transformed into a North-referenced Cartesian coordinate system, the target motion model is expressed as shown in equation (15) below.

[0039] In formula (15) TIFF0007395079000008.tif13166, W k This is the drive noise vector.

[0040] In the target tracking device shown in Figure 1, the drive noise vector W k Assume that it has the properties shown in equation (16) below. Equation (17) below is the drive noise error covariance matrix.

[0041] TIFF0007395079000009.tif27166

[0042] If we discretize the state variable changes during the interval Δt between two sampling times by truncating the Taylor expansion to the first order, the target motion model shown in equation (8) can be expressed as equation (18) below.

[0043] TIFF0007395079000010.tif14166

[0044] Observed value vector Z provided from the sensor to the target tracking device k However, it is expressed as shown in equation (2), and the observed value vector Z k If the coordinate system is represented in a North-referenced Cartesian coordinate system, the target observation model can be defined as shown in equation (19) below.

[0045] In equation (19) of TIFF0007395079000011.tif27166, H is the observation matrix, and V k This is the observed noise vector.

[0046] Observed noise vector V k We assume that it has the properties shown in equation (21) below. Equation (22) below is the observation error covariance matrix R k That is the case.

[0047] In formula (23) of TIFF0007395079000012.tif81166, r is the distance from the radar device to the target, E is the elevation angle of the target relative to the radar device, and A is the azimuth angle of the target relative to the radar device.

[0048] The tracking filter processing based on the state vector, motion model, and observation model described above is performed as follows. First, the initial value X of the smoothed value vector. 0,1 As shown in equation (24) below, it is calculated from the observed value vector Z0, which is the position observation information at the first sampling time t0, and the observed value vector Z1 at the second sampling time t1. The observed value vectors Z1 and Z2 are observed value vectors in the North-referenced Cartesian coordinate system.

[0049] In equation (24) of TIFF0007395079000013.tif23166, Δt is the sampling interval between sampling time t0 and sampling time t1.

[0050] Initial value P of the smoothing error covariance matrix 0,1It is expressed as in the following formula (25). Each of formula (24) and formula (25) is applied to six dimensions up to the target position velocity. The initial value P of the smoothing error covariance matrix 0,1 In P 0,1(+) (7:9,7:9) corresponds to the terms of the aerodynamic coefficients in the 7th to 9th dimensions, and the error covariance matrix Q of the driving noise aero is given.

[0051] TIFF0007395079000014.tif33166

[0052] The error covariance matrix Q of the driving noise aero is expressed as in the following formula (29).

[0053] TIFF0007395079000015.tif89166

[0054] In formula (28), σ 2 accx , σ 2 accy , σ 2 accz each indicates the variance of the system noise acting on the target position and velocity respectively. In particular, σ 2 accx is a parameter indicating the fluctuation of the acceleration in the x coordinate, and σ 2 accy is a parameter indicating the fluctuation of the acceleration in the y coordinate, and σ 2 accz is a parameter indicating the fluctuation of the acceleration in the z coordinate. In formula (29), σ 2 αL1 , σ 2 αL2 , σ 2 β each indicates the variance of the system noise acting on the aerodynamic force. In particular, σ 2 αL1 is a parameter indicating the fluctuation of the aerodynamic force in the x coordinate, and σ 2 αL2is a parameter indicating the fluctuation of aerodynamic force in the y coordinate, and σ 2 β is a parameter indicating the fluctuation of aerodynamic force in the z coordinate.

[0055] Smoothing value vector X of the north reference orthogonal coordinate system k|k is converted to the smoothing value vector Y of the body coordinate system according to Equation (7) k|k Smoothing error covariance matrix P according to Equation (25) k|k or the smoothing error covariance matrix P according to Equation (67) described later k|k is converted to the smoothing error covariance matrix V of the body coordinate system according to the following Equation (30) k|k

[0056] TIFF0007395079000016.tif65166 Matrix A is the Jacobian matrix.

[0057] TIFF​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​ TIFF0007395079000022.tif131166

[0064] Predicted vector Y in the aircraft coordinate system k+1|k The predicted value vector X in the North reference Cartesian coordinate system is given by equation (53) below. k+1|k It will be converted. Prediction error covariance matrix V of the aircraft coordinate system k+1|k The prediction error covariance matrix P in the northern reference orthogonal coordinate system is given by the following equation (54): k+1|k It is converted to the prediction error covariance matrix P in the northern reference orthogonal coordinate system. k+1|k The driving noise error covariance matrix Q k It is added.

[0065] TIFF0007395079000023.tif78166

[0066] TIFF0007395079000024.tif106166

[0067] The tracking gate is set to the sampling time t. k This indicates the area where the target may exist. The tracking gate size is set to, for example, the beam width of a sensor (not shown). The beam width is determined by the predicted angle calculated from the position vector in the North-reference Cartesian coordinate system. The predicted angle includes both the predicted elevation angle and the predicted azimuth angle.

[0068] Smoothed vector X for each motion model k|k As shown in equation (66) below, the predicted value vector X of the North reference Cartesian coordinate system shown in equation (53) is represented by equation (53). k|k-1 And the observed vector Z in the North-referenced Cartesian coordinate system. k And the Kalman gain K for each motion model k This is calculated using the observation matrix H for each motion model in the North-referenced Cartesian coordinate system. Smoothing error covariance matrix P for each motion model k|kAs shown in equation (67) below, the prediction error covariance matrix P of the northern reference orthogonal coordinate system shown in equation (54) is k|k-1 And the Kalman gain K for each motion model k This is calculated using the observation matrix H for each motion model in the North-referenced Cartesian coordinate system.

[0069] In formula (68) of TIFF0007395079000025.tif48166, R k This is the observation error covariance matrix shown in equation (23).

[0070] Next, we will explain the operation of the target tracking device shown in Figure 1. Figure 4 is a flowchart showing the target tracking method, which is the processing procedure of the target tracking device. The tracking gate unit 1 receives the observed value vector Z, which is position observation information indicating the target observation position, from the sensor that observes the target's position. k Obtain it. The tracking gate unit 1 outputs the acquired position observation information to the initial value calculation unit 2 if, for example, the acquired position observation information is the position observation information for the first sampling time t0, or the position observation information for the second sampling time t1. The position observation information at sampling time t0 is the observed value vector Z0, and the position observation information at sampling time t1 is the observed value vector Z1.

[0071] The initial value calculation unit 2 obtains the observed value vector Z0 at the first sampling time t0 and the observed value vector Z1 at the second sampling time t1 from the tracking gate unit 1. The initial value calculation unit 2 calculates the smoothed value vector X of the North reference Cartesian coordinate system by substituting the observed value vector Z0 and the observed value vector Z1 into equation (24). k|k Initial value X 0,1 Calculate. Furthermore, the initial value calculation unit 2 calculates the smoothing error covariance matrix P by substituting the observation error covariance matrices R0, R1 and the driving noise error covariance matrix Q1 into equation (25). k|k Initial value P 0,1 Calculate (Step ST1 in Figure 4). The initial value calculation unit 2 calculates the smoothed value vector X k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Each of these is output to the prediction unit 4. Initial value calculation unit 2 generates the smoothed value vector X k|k Initial value X 0,1 The timing for calculating these factors includes, for example, the timing when a new target is observed, as well as the timing when the same target is observed again after the tracked target has disappeared. In the target tracking device shown in Figure 1, the initial value calculation unit 2 calculates a smoothed value vector X based on the observed value vector Z0 at the first sampling time t0 and the observed value vector Z1 at the second sampling time t1. k|k Initial value X 0,1 This calculates the following. However, this is just one example, and the initial value calculation unit 2 calculates the smoothed value vector X based on, for example, the observed value vector Z1 at the second sampling time t1 and the observed value vector Z2 at the third sampling time t2. k|k Alternatively, the initial value of X can be calculated based on the observed value vector Z2 at the third sampling time t2 and the observed value vector Z3 at the fourth sampling time t3. k|k You may also calculate the initial value of [the function].

[0072] The prediction unit 4 receives the smoothed value vector X from the initial value calculation unit 2. k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Get each of them. The prediction unit 4 generates the smoothed value vector X k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Each of these is used to predict the target state (step ST2 in Figure 4). The target state predicted by the prediction unit 4 is the future state of the target, and the predicted value indicating the future state of the target is the predicted value that the smoothing unit 3 uses for the next estimation process. The prediction process of prediction unit 4 will be explained in detail below.

[0073] The first coordinate conversion unit 4a obtains, from the initial value calculation unit 2, the initial value X k|k of 0,1 the smoothed value vector X k|k and the initial value P 0,1 of the smoothed error covariance matrix P The first coordinate conversion unit 4a converts the coordinate system of the initial value X 0,1 represented by the smoothed value vector X k|k into the aircraft coordinate system as shown in Equation (3). In Equation (3), the state vector Y k corresponds to the smoothed value vector Y k|k . That is, the first coordinate conversion unit 4a converts the smoothed value vector X k|k in the north reference orthogonal coordinate system into the smoothed value vector Y k|k in the aircraft coordinate system. The state vector Y k in the aircraft coordinate system is a vector based on the velocity vector including the magnitude and direction of the velocity. Also, the first coordinate conversion unit 4a converts the coordinate system of the initial value P 0,1 represented by the smoothed error covariance matrix P k|k into the aircraft coordinate system.<( That is, the first coordinate conversion unit 4a converts the smoothed error covariance matrix P k|k in the north reference orthogonal coordinate system into the smoothed error covariance matrix V<( k|k in the aircraft coordinate system. The first coordinate conversion unit 4a outputs the smoothed value vector Y k|k in the aircraft coordinate system and the smoothed error covariance matrix V k|k in the aircraft coordinate system to the prediction processing unit 4b respectively.

[0074] The prediction processing unit 4b obtains, from the first coordinate conversion unit 4a, the smoothed value vector Y k|k in the aircraft coordinate system and the smoothed error covariance matrix V k|k in the aircraft coordinate system respectively. The prediction processing unit 4b calculates the predicted value vector Y k|k in the aircraft coordinate system by substituting the smoothed value vector Y k+1|k in the aircraft coordinate system into Equation (42). Further, the prediction processing unit 4b substitutes the smoothing error covariance matrix V in the body coordinate system k|k into Equation (43) to calculate the predicted error covariance matrix V in the body coordinate system k+1|k . The prediction processing unit 4b outputs the predicted value vector Y in the body coordinate system k+1|k and the predicted error covariance matrix V in the body coordinate system k+1|k to the second coordinate conversion unit 4c respectively.

[0075] The second coordinate conversion unit 4c obtains the predicted value vector Y in the body coordinate system k+1|k and the predicted error covariance matrix V in the body coordinate system k+1|k from the prediction processing unit 4b respectively. The second coordinate conversion unit 4c converts the predicted value vector Y in the body coordinate system k+1|k to the predicted value vector X in the north-reference orthogonal coordinate system k+1|k according to Equation (53). The second coordinate conversion unit 4c outputs the predicted value vector X in the north-reference orthogonal coordinate system k+1|k and the predicted error covariance matrix V in the body coordinate system k+1|k to the third coordinate conversion unit 4d respectively.

[0076] The third coordinate conversion unit 4d obtains the predicted value vector X in the north-reference orthogonal coordinate system k+1|k and the predicted error covariance matrix V in the body coordinate system k+1|k from the second coordinate conversion unit 4c respectively. The third coordinate conversion unit 4d substitutes the predicted error covariance matrix V in the body coordinate system k+1|k and the drive noise error covariance matrix Q k into Equation (53) to convert the predicted error covariance matrix V in the body coordinate system k+1|k to the predicted error covariance matrix P in the north-reference orthogonal coordinate system k+1|k . The third coordinate conversion unit 4d outputs the predicted value vector X in the north-reference orthogonal coordinate system k+1|k and the predicted error covariance matrix P in the north-reference orthogonal coordinate system k+1|k to the tracking gate unit 1 respectively.

[0077] The tracking gate unit 1 receives the observed value vector Z, which is position observation information indicating the target observation position, from the sensor that observes the target's position. k Obtain the observed vector Z. k This represents the position observation information for the third and subsequent sampling times. The tracking gate unit 1 receives the predicted value vector X of the North reference Cartesian coordinate system from the third coordinate transformation unit 4d. k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Get each of them. The tracking gate unit 1 controls the observed value vector Z k The position vector x included in k ,y k ,z k Based on this, it is determined whether the target's position is within the tracking gate. If the target position is within the tracking gate (step ST3: YES in Figure 4), the tracking gate unit 1 will generate the observed value vector Z k Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Each of these is output to the smoothing unit 3. If the target position is not within the tracking gate (step ST3:NO in Figure 4), the tracking gate unit 1 will return the observed value vector Z k Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k None of these are output to the smoothing unit 3.

[0078] The smoothing unit 3 receives the observed value vector Z from the tracking gate unit 1. k Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Get each of them. The smoothing section 3 is the observed value vector Z k and the predicted value vector X in the North-referenced Cartesian coordinate system k+1|k By substituting and into equation (66), the smoothed vector X k|k The result is calculated (step ST4 in Figure 4). Equation (66) contains the predicted value vector Xk+1|k However, the predicted value vector X k|k-1 It is substituted as follows. Furthermore, the smoothing section 3 is the prediction error covariance matrix P of the North reference Cartesian coordinate system. k+1|k By substituting this into equation (67), the smoothing error covariance matrix P k|k Calculate the following (step ST4 in Figure 4). Equation (67) includes the prediction error covariance matrix P k+1|k However, the prediction error covariance matrix P k|k-1 It is substituted as follows. The smoothing section 3 is the smoothed value vector X k|k and the smoothing error covariance matrix P k|k Each of these is output to the prediction unit 4. Furthermore, the smoothing unit 3 generates a smoothed value vector X k|k and the smoothing error covariance matrix P k|k Each of these is output to, for example, a display device (not shown) or a radar device (not shown).

[0079] The first coordinate transformation unit 4a receives the smoothed value vector X from the smoothing unit 3. k|k and the smoothing error covariance matrix P k|k Get each of them. The first coordinate transformation unit 4a calculates the smoothed value vector X according to equations (4) to (6). k|k The coordinate system is transformed into the aircraft coordinate system shown in equation (3) (step ST5 in Figure 4). That is, the first coordinate transformation unit 4a transforms the smoothed value vector X of the North reference Cartesian coordinate system. k|k The smoothed vector Y in the aircraft coordinate system k|k Convert to. Furthermore, the first coordinate transformation unit 4a calculates the smoothing error covariance matrix P according to equation (30). k|k Convert the coordinate system to the aircraft coordinate system (step ST5 in Figure 4). That is, the first coordinate transformation unit 4a is the smoothing error covariance matrix P of the North reference Cartesian coordinate system. k|k The smoothing error covariance matrix V in the aircraft coordinate system k|k Convert to. The first coordinate transformation unit 4a processes the state vector Y of the aircraft coordinate system. k and the smoothing error covariance matrix V of the aircraft coordinate system k|k Each of these is output to the prediction processing unit 4b.

[0080] The prediction processing unit 4b receives the smoothed value vector Y of the aircraft coordinate system from the first coordinate transformation unit 4a. k|k and the smoothing error covariance matrix V of the aircraft coordinate system k|k Get each of them. The prediction processing unit 4b generates a smoothed vector Y in the aircraft coordinate system. k|k By substituting this into equation (42), we obtain the predicted vector Y in the aircraft coordinate system. k+1|k Calculate (Step ST6 in Figure 4). Furthermore, the prediction processing unit 4b calculates the smoothing error covariance matrix V of the aircraft coordinate system. k|k By substituting this into equation (43), the prediction error covariance matrix V of the aircraft coordinate system can be obtained. k+1|k Calculate (Step ST6 in Figure 4). The prediction processing unit 4b generates the predicted value vector Y in the aircraft coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the second coordinate transformation unit 4c.

[0081] The second coordinate transformation unit 4c receives the predicted value vector Y of the aircraft coordinate system from the prediction processing unit 4b. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Get each of them. The second coordinate transformation unit 4c calculates the predicted value vector Y of the aircraft coordinate system according to equation (53). k+1|k Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k Convert to (Step ST7 in Figure 4). The second coordinate transformation unit 4c generates a predicted value vector X in the North reference Cartesian coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the third coordinate transformation unit 4d.

[0082] The third coordinate transformation unit 4d receives the predicted value vector X of the North-reference Cartesian coordinate system from the second coordinate transformation unit 4c. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Get each of them. The third coordinate transformation unit 4d is the prediction error covariance matrix V of the aircraft coordinate system. k+1|kand the drive noise error covariance matrix Q k By substituting and into equation (53), the prediction error covariance matrix V of the aircraft coordinate system can be obtained. k+1|k The prediction error covariance matrix P in the North-referenced Cartesian coordinate system. k+1|k Convert to (Step ST8 in Figure 4). The third coordinate transformation unit 4d generates the predicted value vector X in the North-reference Cartesian coordinate system. k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Each of these is output to the tracking gate unit 1.

[0083] If the target tracking device has not received a request to terminate the target tracking process (in the case of step ST9:NO in Figure 4), the processes in steps ST3 to ST8 are repeated. If a request to terminate the target tracking process is given to the target tracking device (in step ST9: YES in Figure 4), the series of processes of the target tracking device will end.

[0084] In the above embodiment 1, the target tracking device is configured to include an initial value calculation unit 2 that acquires position observation information from a sensor that observes the target's position, indicating the target's observation position at two different sampling times, and calculates initial values ​​of smoothed values ​​that include the target's position vector and the target's velocity vector, respectively, as initial values ​​of smoothed values ​​that indicate the target's state, based on the position observation information. The target tracking device also includes a smoothing unit 3 that performs estimation processing of smoothed values ​​indicating the target's state using the position observation information output from the sensor and predicted values ​​indicating the target's state, and a prediction unit 4 that predicts the target's future state using the initial values ​​of smoothed values ​​calculated by the initial value calculation unit 2 or the smoothed values ​​estimated by the smoothing unit 3, and outputs predicted values ​​indicating the predicted result of the target's future state to the smoothing unit 3 as predicted values ​​to be used in the next estimation process.Therefore, the target tracking device can suppress the deterioration of prediction accuracy that occurs when the initial values ​​of smoothed values ​​deviate significantly from the original smoothed values.

[0085] Embodiment 2. In Embodiment 2, a target tracking device including a prediction unit 6 that performs prediction processing based on each of a plurality of motion hypotheses will be described. The motion hypotheses correspond to motion models.

[0086] FIG. 5 is a configuration diagram showing the target tracking device according to Embodiment 2. In FIG. 5, the same reference numerals as those in FIG. 1 denote the same or corresponding parts, and thus detailed description thereof will be omitted. FIG. 6 is a hardware configuration diagram showing the hardware of the target tracking device according to Embodiment 2. In FIG. 6, the same reference numerals as those in FIG. 2 denote the same or corresponding parts, and thus detailed description thereof will be omitted. The target tracking device shown in FIG. 5 includes a tracking gate unit 1, an initial value calculation unit 2, a smoothing unit 5, and a prediction unit 6.

[0087] The smoothing unit 5 is realized by a smoothing circuit 15 shown in FIG. 6. The smoothing unit 5 acquires position observation information from the tracking gate unit 1, and acquires a plurality of predicted values and a plurality of prediction error covariance matrices from the prediction unit 6. The smoothing unit 5 performs an estimation process of a smoothed value indicating the state of the target using the position observation information, each predicted value, and each prediction error covariance matrix. Further, the smoothing unit 5 calculates a smoothing error covariance matrix based on each prediction error covariance matrix. The smoothing unit 5 outputs each of the plurality of smoothed values and the plurality of smoothing error covariance matrices to the prediction unit 6.

[0088] The prediction unit 6 is realized by a prediction circuit 16 shown in FIG. 6. The prediction unit 6 includes a first coordinate conversion unit 6a, prediction processing units 6b-1 to 6b-N, a predicted value integration unit 6c, second coordinate conversion units 6d-1 to 6d-N, 6e, and third coordinate conversion units 6f-1 to 6f-N, 6g. The prediction unit 6 acquires an initial value of the smoothed value from the initial value calculation unit 2 or the smoothed value from the smoothing unit 5. The prediction unit 6 performs prediction processing based on each of a plurality of motion hypotheses of the target using the initial value of the smoothed value or the smoothed value, and calculates a plurality of predicted values. The prediction unit 6 outputs multiple predicted values ​​to the smoothing unit 5 as predicted values ​​to be used by the smoothing unit 5 in the next estimation process. The prediction unit 6 also outputs some of the predicted values ​​to the tracking gate unit 1. Furthermore, the prediction unit 6 obtains the initial value of the smoothing error covariance matrix from the initial value calculation unit 2, or multiple smoothing error covariance matrices from the smoothing unit 6. The prediction unit 6 calculates the prediction error covariance matrix using the initial values ​​of the smoothing error covariance matrix, or the respective smoothing error covariance matrices. The prediction unit 4 outputs multiple prediction error covariance matrices to the smoothing unit 5.

[0089] The first coordinate transformation unit 6a obtains an initial value for smoothing from the initial value calculation unit 2, or multiple smoothing values ​​from the smoothing unit 5. The first coordinate transformation unit 6a transforms the coordinate system of the initial smoothed values, or the coordinate system of each smoothed value, from the North-referenced Cartesian coordinate system to the target aircraft coordinate system. The first coordinate transformation unit 6a outputs initial values ​​of the smoothed values ​​of the aircraft coordinate system to the prediction processing units 6b-1 to 6b-N and the prediction value integration unit 6c, respectively. Furthermore, the first coordinate transformation unit 6a outputs one of the multiple smoothed values ​​of the aircraft coordinate system to the prediction processing unit 6b-n (n=1,2,···,N), and outputs the multiple smoothed values ​​of the aircraft coordinate system to the prediction value integration unit 6c. Furthermore, the first coordinate transformation unit 6a obtains the initial value of the smoothing error covariance matrix from the initial value calculation unit 2, or multiple smoothing error covariance matrices from the smoothing unit 5. The first coordinate transformation unit 6a transforms the coordinate system of the initial values ​​of the smoothing error covariance matrix, or the coordinate system of each smoothing error covariance matrix, from the North-referenced Cartesian coordinate system to the aircraft coordinate system. The first coordinate transformation unit 6a outputs the initial value of the smoothing error covariance matrix of the aircraft coordinate system to the prediction processing units 6b-1 to 6b-N and the prediction value integration unit 6c, respectively. The first coordinate transformation unit 6a outputs one of the multiple smoothing error covariance matrices of the aircraft coordinate system to the prediction processing units 6b-n, and outputs the multiple smoothing error covariance matrices of the aircraft coordinate system to the prediction value integration unit 6c.

[0090] The prediction processing units 6b-n (n=1,2,···,N) obtain from the first coordinate transformation unit 6a either an initial value for the smoothed value of the aircraft coordinate system, or one of several smoothed values ​​of the aircraft coordinate system. N is an integer greater than or equal to 2. The prediction processing units 6b-n calculate predicted values ​​by performing prediction processing based on the nth motion hypothesis using the initial values ​​of the smoothed values ​​of the aircraft coordinate system, or the smoothed values ​​of the aircraft coordinate system. The prediction processing units 6b-n output the predicted values ​​of the aircraft coordinate system to the second coordinate transformation unit 6d-n. Furthermore, the prediction processing units 6b-n obtain from the first coordinate transformation unit 6a either the initial value of the smoothing error covariance matrix of the aircraft coordinate system, or one of the multiple smoothing error covariance matrices of the aircraft coordinate system. The prediction processing units 6b-n calculate the prediction error covariance matrix using the initial values ​​of the smoothing error covariance matrix of the aircraft coordinate system, or the smoothing error covariance matrix of the aircraft coordinate system. The prediction processing units 6b-n output the prediction error covariance matrix of the aircraft coordinate system to the second coordinate transformation unit 6d-n.

[0091] The predicted value integration unit 6c obtains an initial value of the smoothed value of the aircraft coordinate system, or multiple smoothed values ​​of the aircraft coordinate system, from the first coordinate transformation unit 6a. The prediction value integration unit 6c uses the initial value of the smoothed value of the aircraft coordinate system, or multiple smoothed values ​​of the aircraft coordinate system, to perform prediction processing based on the first to the Nth motion hypotheses as N motion hypotheses and calculates predicted values. Furthermore, the prediction value integration unit 6c obtains multiple smoothing error covariance matrices of the aircraft coordinate system from the first coordinate transformation unit 6a. The prediction value integration unit 6c calculates the prediction error covariance matrix using multiple smoothing error covariance matrices of the aircraft coordinate system. The prediction value integration unit 6c outputs the predicted values ​​of the aircraft coordinate system and the prediction error covariance matrix of the aircraft coordinate system to the second coordinate transformation unit 6e.

[0092] The second coordinate transformation unit 6d-n (n=1,2,···,N) obtains the predicted values ​​of the aircraft coordinate system and the prediction error covariance matrix of the aircraft coordinate system from the prediction processing unit 6b-n. The second coordinate transformation unit 6d-n converts the predicted values ​​in the aircraft coordinate system to predicted values ​​in the North-referenced Cartesian coordinate system. The second coordinate transformation unit 6d-n outputs the predicted values ​​for the North reference Cartesian coordinate system and the predicted error covariance matrix for the aircraft coordinate system to the third coordinate transformation unit 6f-n.

[0093] The second coordinate transformation unit 6e obtains the predicted values ​​of the aircraft coordinate system and the prediction error covariance matrix of the aircraft coordinate system from the predicted value integration unit 6c. The second coordinate transformation unit 6e converts the predicted values ​​in the aircraft coordinate system to predicted values ​​in the North-referenced Cartesian coordinate system. The second coordinate transformation unit 6e outputs the predicted values ​​in the North reference Cartesian coordinate system and the predicted error covariance matrix of the aircraft coordinate system to the third coordinate transformation unit 6g.

[0094] The third coordinate transformation unit 6f-n (n=1,2,···,N) obtains the predicted values ​​for the North-reference Cartesian coordinate system and the predicted error covariance matrix for the aircraft coordinate system from the second coordinate transformation unit 6d-n. The third coordinate transformation unit 6f-n transforms the prediction error covariance matrix of the aircraft coordinate system into the prediction error covariance matrix of the North-referenced Cartesian coordinate system, and adds driving noise to the transformed prediction error covariance matrix. The third coordinate transformation unit 6f-n outputs predicted values ​​with driving noise to the smoothing unit 5.

[0095] The third coordinate transformation unit 6g obtains the predicted values ​​for the North-reference Cartesian coordinate system and the predicted error covariance matrix for the aircraft coordinate system from the second coordinate transformation unit 6e. The third coordinate transformation unit 6g converts the predicted values ​​in the North-referenced Cartesian coordinate system into a prediction error covariance matrix of the North-referenced Cartesian coordinate system, and adds driving noise to the converted prediction error covariance matrix. The third coordinate transformation unit 6g outputs predicted values ​​in the North reference Cartesian coordinate system to the tracking gate unit 1. Furthermore, the third coordinate transformation unit 6g outputs the predicted values ​​in the North reference Cartesian coordinate system and the predicted error covariance matrix with driving noise to the smoothing unit 5.

[0096] In Figure 5, it is assumed that the tracking gate unit 1, initial value calculation unit 2, smoothing unit 5, and prediction unit 6, which are components of the target tracking device, are each implemented by dedicated hardware as shown in Figure 6. That is, the target tracking device is assumed to be implemented by a tracking gate circuit 11, an initial value calculation circuit 12, a smoothing circuit 15, and a prediction circuit 16. Each of the tracking gate circuit 11, initial value calculation circuit 12, smoothing circuit 15, and prediction circuit 16 can be, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC, an FPGA, or a combination thereof.

[0097] The components of a target tracking device are not limited to those implemented by dedicated hardware; the target tracking device may also be implemented by software, firmware, or a combination of software and firmware. When the target tracking device is implemented using software or firmware, a program is stored in the memory 21 shown in Figure 3 that instructs a computer to execute the respective processing procedures in the tracking gate unit 1, initial value calculation unit 2, smoothing unit 5, and prediction unit 6. Then, the processor 22 shown in Figure 3 executes the program stored in the memory 21.

[0098] Furthermore, Figure 6 shows an example where each component of the target tracking device is implemented by dedicated hardware, while Figure 3 shows an example where the target tracking device is implemented by software or firmware. However, this is merely one example, and some components of the target tracking device may be implemented by dedicated hardware, while the remaining components may be implemented by software or firmware.

[0099] The motion model of the target tracking device shown in Figure 5 is obtained by adding a constant aerodynamic vector to the motion model of the target tracking device shown in Figure 1, as shown in equation (69) below.

[0100] TIFF0007395079000026.tif29166u k The sampling time t k These are the constant aerodynamic vectors that constitute each of the N motion models.

[0101] Figure 7 shows the constant aerodynamic vector α n This is an explanatory diagram illustrating the concept. Constant aerodynamic vector α n As shown in equation (71) below, the vertical component of the lift is α of the aircraft. L1 , the horizontal component of the lift α L2 It also includes a drag component β in the opposite direction to the velocity vector V.

[0102] TIFF0007395079000027.tif34166

[0103] In equation (69), Γ k The sampling time t k This is the transformation matrix for the constant aerodynamic vectors in the system, and is expressed as shown in equation (72) below.

[0104] TIFF0007395079000028.tif30166

[0105] The constant aerodynamic vector α is assumed as shown in equation (70). n The state vector Y k α included L1 ,α L2 β is added. Sampling time t k In this case, when an example of a combination of constant aerodynamic vectors is described as shown in equation (73) below, the motion hypothesis when this description is true is expressed as shown in equation (74) below.

[0106] TIFF0007395079000029.tif27166

[0107] Sampling time t k The probability of transition between motion models pk,ab This can be expressed as shown in equation (75) below: Inter-motion model transition probability p k,ab The sampling time t k and sampling time t k-1 The sampling interval is t S =t k -t k-1 This is the probability of transition between motion models in the given case.

[0108] TIFF0007395079000030.tif19166

[0109] Sampling time t k Observed vector Z up to this point k Based on sampling time t k The confidence level of the motion hypothesis can be calculated as shown in equation (77) below, by defining the following equation (76) using the conditional probability density function.

[0110] In formula (77) TIFF0007395079000031.tif37166, v k,a This is the normal distribution approximation P[Z|Ψ] of the observed vector. k,a, Z k-1 This is an approximation using a multivariate normal distribution, and is calculated based on the predicted values ​​in the northern reference Cartesian coordinate system as shown in equation (78) below.

[0111] TIFF0007395079000032.tif15166

[0112] Sampling time t k-1 Observed vector Z up to this point k-1 Based on sampling time t k The prior confidence level of the motion hypothesis can be calculated as shown in equation (80) below, by defining the following equation (79) using the conditional probability density function.

[0113] TIFF0007395079000033.tif42166

[0114] Sampling time t k Observed vector Z up to this point k Estimated constant acceleration vector u based on k-1 If (+) is defined as in equation (81), then the estimated value of the constant acceleration vector u k-1 (+) is expressed as shown in equation (82) below. Below are the estimated values ​​of the constant acceleration vector u k-1 The (+) symbol is called the estimated aerodynamic vector.

[0115] TIFF0007395079000034.tif40166

[0116] Sampling time t k-1 Observed vector Z up to this point k-1 Estimated constant acceleration vector u based on k-1 If (-) is defined as in equation (83), then the estimated value of the constant acceleration vector u k-1 (-) is expressed as shown in equation (84) below. Below are the estimated values ​​of the constant acceleration vector u k-1 The (-) is called the predicted aerodynamic vector.

[0117] TIFF0007395079000035.tif41166

[0118] Based on the theory of the Kalman filter, the sampling time t is determined according to the motion model and the observation model. k Observed value vector Z k If obtained, the state vector X k Estimated value X k|k It is calculated as follows: The process of calculating the initial values ​​of the smoothed vector and the process of converting the coordinate system to the aircraft coordinate system are the same as in Embodiment 1.

[0119] (1a) Prediction processing based on the nth (n=1,2,···,N) motion hypothesis In the prediction process based on the nth motion hypothesis, the predicted value vector Y is as shown in equation (85) below. k|k-1,aThe following equation (86) is used to calculate the prediction error covariance matrix V k|k-1,a This is calculated.

[0120] TIFF0007395079000036.tif45166

[0121] Predicted value vector Y k|k-1,a The coordinate system is transformed from the aircraft coordinate system to the North-referenced Cartesian coordinate system. That is, the predicted vector Y in the aircraft coordinate system. k|k-1,a This is the predicted value vector X in the North-referenced Cartesian coordinate system. k|k-1,a It is converted to the following equation (87): The predicted value vector Y k+1|k,a This represents a coordinate transformation. Prediction error covariance matrix V k|k-1,a The coordinate system is transformed from the aircraft coordinate system to the North-referenced Cartesian coordinate system. That is, the prediction error covariance matrix V of the aircraft coordinate system. k|k-1,a This is the prediction error covariance matrix P in the North-referenced Cartesian coordinate system. k|k-1,a This is converted to the following equation (88): The prediction error covariance matrix V k+1|k,a This represents the coordinate transformation. Prediction error covariance matrix P k+1|k,a The driving noise error covariance matrix Q k It has been added.

[0122] In TIFF0007395079000037.tif28166 (88), Υ k This is the Jacobian matrix shown in equation (55).

[0123] (2a) Prediction process based on N motion hypotheses Assuming N motion hypotheses, the prediction process based on the 1st to Nth motion hypotheses is as follows: Predicted value vector Y k|k-1 The following equation (90) is used to calculate the prediction error covariance matrix V k|k-1 This is calculated. Predicted aerodynamic vector u k-1 (-) is calculated by formula (84).

[0124] TIFF0007395079000038.tif58166

[0125] Predicted value vector Y k|k-1 The coordinate system is transformed from the aircraft coordinate system to the North-referenced Cartesian coordinate system. That is, the predicted vector Y in the aircraft coordinate system. k|k-1 This is the predicted value vector X in the North-referenced Cartesian coordinate system. k|k-1,a It is converted to the following equation (91): The predicted value vector Y k+1|k This represents a coordinate transformation. Prediction error covariance matrix V k|k-1 The coordinate system is transformed from the aircraft coordinate system to the North-referenced Cartesian coordinate system. That is, the prediction error covariance matrix V of the aircraft coordinate system. k|k-1 This is the prediction error covariance matrix P in the North-referenced Cartesian coordinate system. k|k-1 Equation (92) is the prediction error covariance matrix V k+1|k This represents the coordinate transformation. Prediction error covariance matrix P k+1|k The driving noise error covariance matrix Q k It has been added.

[0126] TIFF0007395079000039.tif30166

[0127] (1b) Smoothing based on the nth (n=1,2,···,N) motion hypothesis In the smoothing process based on the nth motion hypothesis, the smoothed value vector X is as shown in equation (93) below. k|k,a The following equation (94) is used to calculate the smoothing error covariance matrix P k|k,a This is calculated.

[0128] TIFF0007395079000040.tif57166

[0129] (2b) Smoothing based on N motion hypotheses In the smoothing process based on N motion hypotheses, the smoothed value vector X is obtained as shown in equation (96) below. k|kThe smoothing error covariance matrix P is calculated as shown in equation (97) below. k|k,a This is calculated.

[0130] TIFF0007395079000041.tif83166

[0131] Next, we will explain the operation of the target tracking device shown in Figure 5. Figure 8 is a flowchart showing the target tracking method, which is the processing procedure of the target tracking device. The tracking gate unit 1 receives the observed value vector Z, which is position observation information indicating the target observation position, from the sensor that observes the target's position. k Obtain it. The tracking gate unit 1 outputs the acquired position observation information to the initial value calculation unit 2 if the acquired position observation information is the position observation information for the first sampling time t0 from the beginning, or the position observation information for the second sampling time t1 from the beginning. The position observation information at sampling time t0 is the observed value vector Z0, and the position observation information at sampling time t1 is the observed value vector Z1.

[0132] The initial value calculation unit 2 obtains the observed value vector Z0 at the first sampling time t0 and the observed value vector Z1 at the second sampling time t1 from the tracking gate unit 1. The initial value calculation unit 2 calculates the smoothed value vector X of the North reference Cartesian coordinate system by substituting the observed value vector Z0 and the observed value vector Z1 into equation (24). k|k Initial value X 0,1 Calculate. Furthermore, the initial value calculation unit 2 calculates the initial value P of the smoothing error covariance matrix by substituting the observation error covariance matrices R0, R1 and the driving noise error covariance matrix Q1 into equation (25). 0,1 Calculate (Step ST11 in Figure 8). The initial value calculation unit 2 calculates the smoothed value vector X k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Each of these is output to the prediction unit 6.

[0133] The prediction unit 6 receives the smoothed value vector X from the initial value calculation unit 2. k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Get each of them. The prediction unit 6 generates a smoothed value vector X k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Each of these is used to predict the target state (step ST12 in Figure 8). The target state predicted by the prediction unit 6 is the future state of the target, and the predicted value indicating the future state of the target is the predicted value that the smoothing unit 5 uses for the next estimation process. The prediction process of the prediction unit 6 will be explained in detail below.

[0134] The first coordinate transformation unit 6a receives the smoothed value vector X from the initial value calculation unit 2. k|k Initial value X 0,1 and the smoothing error covariance matrix P k|k Initial value P 0,1 Get each of them. The first coordinate transformation unit 6a performs the following according to equations (4) to (6): 0,1 Smoothed vector X that shows k|k The coordinate system is transformed into the aircraft coordinate system shown in equation (3). That is, the first coordinate transformation unit 6a transforms the smoothed value vector X of the North reference Cartesian coordinate system. k|k The smoothed vector Y in the aircraft coordinate system k|k Convert to the aircraft coordinate system state vector Y. k This is a vector that is based on a velocity vector, which includes the magnitude and direction of the velocity. Furthermore, the first coordinate transformation unit 6a sets the initial value P according to equation (30). 0,1 The smoothed error covariance matrix P shown k|k Convert the coordinate system to the aircraft's coordinate system. That is, the first coordinate transformation unit 6a is the smoothing error covariance matrix P of the North reference Cartesian coordinate system. k|k The smoothing error covariance matrix V in the aircraft coordinate system k|k Convert to. The first coordinate transformation unit 6a generates a smoothed vector Y of the aircraft coordinate system. k|k and the smoothing error covariance matrix V of the aircraft coordinate system k|k Each of these is output to the prediction processing units 6b-1 to 6b-N and the prediction value integration unit 6c, respectively.

[0135] The prediction processing units 6b-n (n=1,···,N) receive the smoothed vector Y of the aircraft coordinate system from the first coordinate transformation unit 6a. k|k and the smoothing error covariance matrix V of the aircraft coordinate system k|k Get each of them. The prediction processing units 6b-n generate the smoothed vector Y of the aircraft coordinate system. k|k By substituting this into equation (85), we obtain the predicted vector Y in the aircraft coordinate system. k+1|k,a Calculate Y. In equation (85), k-1|k-1 Y k|k This corresponds to Y k|k-1,a Y k+1|k,a It corresponds to this. Furthermore, the prediction processing units 6b-n calculate the smoothing error covariance matrix V of the aircraft coordinate system. k|k By substituting this into equation (86), the prediction error covariance matrix V of the aircraft coordinate system is obtained. k+1|k,a Calculate V. In equation (86), k-1|k-1 V k|k This corresponds to V k|k-1,a V k+1|k,a It corresponds to this. The prediction processing units 6b-n generate the predicted value vector Y in the aircraft coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the second coordinate transformation unit 6d-n.

[0136] The predicted value integration unit 6c receives the smoothed value vector Y of the aircraft coordinate system from the first coordinate transformation unit 6a. k|k and the smoothing error covariance matrix V of the aircraft coordinate system k|k Get each of them. The predicted value integration unit 6c generates the smoothed value vector Y of the aircraft coordinate system. k|k By substituting this into equation (89), we obtain the predicted vector Y in the aircraft coordinate system. k+1|k Calculate Y. In equation (89), k-1|k-1Y k|k This corresponds to Y k|k-1 Y k+1|k It corresponds to this. Furthermore, the prediction value integration unit 6c uses the smoothing error covariance matrix V of the aircraft coordinate system. k|k By substituting this into equation (90), the prediction error covariance matrix V of the aircraft coordinate system is obtained. k+1|k Calculate V. In equation (90), k|k-1,a V k|k This corresponds to V k|k-1 V k+1|k It corresponds to this. The prediction value integration unit 6c generates the predicted value vector Y in the aircraft coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the second coordinate transformation unit 6e.

[0137] The second coordinate transformation unit 6d-n (n=1,···,N) receives the predicted value vector Y of the aircraft coordinate system from the prediction processing unit 6b-n. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,a Get each of them. The second coordinate transformation unit 6d-n calculates the predicted value vector Y of the aircraft coordinate system according to equation (87). k+1|k,a Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k,a Convert to. The second coordinate transformation unit 6d-n is the predicted value vector X in the North reference Cartesian coordinate system. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,a Each of these is output to the third coordinate transformation unit 6f-n.

[0138] The third coordinate transformation unit 6f-n converts the second coordinate transformation unit 6d-n to the predicted value vector X of the North reference Cartesian coordinate system. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,a Get each of them. The third coordinate transformation unit 6f-n is the prediction error covariance matrix V of the aircraft coordinate system. k+1|k,a and the drive noise error covariance matrix Q k By substituting and into equation (88), the prediction error covariance matrix V of the aircraft coordinate system can be obtained. k+1|k,a The prediction error covariance matrix P in the North-referenced Cartesian coordinate system.k+1|k,a Convert to. The third coordinate transformation unit 6f-n calculates the predicted value vector X in the North-reference Cartesian coordinate system. k+1|k,a and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k,a Each of these is output to the smoothing unit 5.

[0139] The second coordinate transformation unit 6e receives the predicted value vector Y of the aircraft coordinate system from the predicted value integration unit 6c. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Get each of them. The second coordinate transformation unit 6e calculates the predicted value vector Y of the aircraft coordinate system according to equation (91). k+1|k Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k Convert to. The second coordinate transformation unit 6e generates a predicted value vector X in the North reference Cartesian coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the third coordinate transformation unit 6g.

[0140] The third coordinate transformation unit 6g receives the predicted value vector X of the North reference Cartesian coordinate system from the second coordinate transformation unit 6e. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Get each of them. The third coordinate transformation unit 6g is the prediction error covariance matrix V of the aircraft coordinate system. k+1|k and the drive noise error covariance matrix Q k By substituting and into equation (92), the prediction error covariance matrix V of the aircraft coordinate system can be obtained. k+1|k The prediction error covariance matrix P in the North-referenced Cartesian coordinate system. k+1|k Convert to. The third coordinate transformation unit 6g generates the predicted value vector X in the North reference Cartesian coordinate system. k+1|k This is output to the tracking gate unit 1. Furthermore, the third coordinate transformation unit 6g calculates the predicted value vector X in the North reference Cartesian coordinate system. k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Each of these is output to the smoothing unit 5.

[0141] The tracking gate unit 1 receives the observed value vector Z, which is position observation information indicating the target observation position, from the sensor that observes the target's position. k Obtain it. The tracking gate unit 1 receives the predicted value vector X of the North reference Cartesian coordinate system from the third coordinate transformation unit 6g. k+1|k Obtain it. The tracking gate unit 1 controls the observed value vector Z k The position vector x included in k ,y k ,z k Based on this, it is determined whether the target's position is within the tracking gate. If the tracking gate unit 1 is located within the tracking gate (in step ST13: YES in Figure 8), it notifies the smoothing unit 5 that the target is located within the tracking gate. If the tracking gate unit 1 does not have the target position within the tracking gate (in step ST13:NO in Figure 8), it notifies the smoothing unit 5 that the target position is not within the tracking gate.

[0142] When the smoothing unit 5 receives notification from the tracking gate unit 1 that the target position is within the tracking gate, it receives the predicted value vector X of the North reference Cartesian coordinate system from each of the third coordinate transformation units 6f-1 to 6f-N. k+1|k,a and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k,a Get each of them. Furthermore, the smoothing unit 5 receives the predicted value vector X of the North reference Cartesian coordinate system from the third coordinate transformation unit 6g. k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Get each of them. The smoothing section 5 is the observed value vector Z k and the predicted value vector X in the North-referenced Cartesian coordinate system k+1|k,a By substituting and into equation (93), the smoothed vector X k|k,a Calculate (Step ST14 in Figure 8). Equation (93) contains the predicted value vector X k+1|k,a However, the predicted value vector X k|k-1,a It is substituted as follows. Furthermore, the smoothing section 5 is the prediction error covariance matrix P of the North reference Cartesian coordinate system.k+1|k,a By substituting this into equation (94), the smoothing error covariance matrix P k|k,a The following is calculated (step ST14 in Figure 8). Equation (94) includes the prediction error covariance matrix P k+1|k,a However, the prediction error covariance matrix P k|k-1,a It is substituted as follows. The smoothing section 5 is the smoothed value vector X k|k,a and the smoothing error covariance matrix P k|k,a Each of these is output to the prediction unit 6. Furthermore, the smoothing unit 5 generates a smoothed value vector X k|k,a and the smoothing error covariance matrix P k|k,a Each of these is output to, for example, a display device (not shown) or a radar device (not shown).

[0143] The smoothing section 5 generates the predicted value vector X k+1|k By substituting this into equation (7), the predicted vector Y k+1|k Calculate the observed value vector Z k and the predicted value vector Y k+1|k By substituting and into equation (96), the smoothed vector X k|k Calculate (Step ST14 in Figure 8). Equation (96) contains the predicted value vector Y k+1|k However, the predicted value vector Y k|k-1 It is substituted as follows. Furthermore, the smoothing section 5 is the prediction error covariance matrix P of the North reference Cartesian coordinate system. k+1|k By substituting this into equation (97), the smoothing error covariance matrix P k|k The following is calculated (step ST14 in Figure 8). Equation (97) includes the prediction error covariance matrix P k+1|k However, the prediction error covariance matrix P k|k,a It is substituted as follows. The smoothing section 5 is the smoothed value vector X k|k and the smoothing error covariance matrix P k|k Each of these is output to the prediction unit 6. Furthermore, the smoothing unit 5 generates a smoothed value vector X k|k and the smoothing error covariance matrix P k|k Each of these is output to, for example, a display device (not shown) or a radar device (not shown). In the target tracking device shown in Figure 5, the smoothing unit 5 generates a smoothed value vector X k|k,a and smoothed vector X k|k Each of these is output to a display device, etc. However, this is just one example, and the smoothing unit 5 also outputs, for example, the smoothed value vector X k|k,a and the smoothed vector X k|k The average value of these values ​​may be output to a display device or similar.

[0144] The first coordinate transformation unit 6a receives the smoothed value vector X from the smoothing unit 5. k|k,a and the smoothing error covariance matrix P k|k,a Get each of them. Furthermore, the first coordinate transformation unit 6a converts the smoothed value vector X from the smoothing unit 5. k|k and the smoothing error covariance matrix P k|k Get each of them. The first coordinate transformation unit 6a calculates the smoothed value vector X according to equations (4) to (6). k|k,a and smoothed vector X k|k Each coordinate system is transformed into the aircraft coordinate system shown in equation (3) (step ST15 in Figure 8). That is, the first coordinate transformation unit 6a transforms the smoothed value vector X of the North reference Cartesian coordinate system. k|k,a The smoothed vector Y in the aircraft coordinate system k|k,a Convert to the smoothed vector X in the North-referenced Cartesian coordinate system. k|k The smoothed vector Y in the aircraft coordinate system k|k Convert to. The first coordinate transformation unit 6a generates a smoothed vector Y of the aircraft coordinate system. k|k,a The coordinate system (a=1,···,N) is output to the prediction processing unit 6b-n(n=1,···,N), and the smoothed vector Y of the aircraft coordinate system is output. k|k The result is output to the predicted value integration unit 6c.

[0145] Furthermore, the first coordinate transformation unit 6a calculates the smoothing error covariance matrix P according to equation (30). k|k,a and the smoothing error covariance matrix P k|k Convert each coordinate system to the aircraft coordinate system (step ST15 in Figure 8). That is, the first coordinate transformation unit 6a is the smoothing error covariance matrix P of the North reference Cartesian coordinate system.k|k,a The smoothing error covariance matrix V in the aircraft coordinate system k|k,a Convert to the smoothing error covariance matrix P in the northern reference orthogonal coordinate system. k|k The smoothing error covariance matrix V in the aircraft coordinate system k|k Convert to (Step ST15 in Figure 8). The first coordinate transformation unit 6a calculates the smoothing error covariance matrix V of the aircraft coordinate system. k|k,a The (a=1,···,N) is output to the prediction processing unit 6b-n(n=1,···,N), and the smoothing error covariance matrix V of the aircraft coordinate system is output. k|k The result is output to the predicted value integration unit 6c.

[0146] The prediction processing units 6b-n (n=1,···,N) receive the smoothed vector Y of the aircraft coordinate system from the first coordinate transformation unit 6a. k|k,a (a=1,···,N) and the smoothing error covariance matrix V of the aircraft coordinate system k|k,a Get each of the values ​​(a=1,···,N). The prediction processing units 6b-n generate the smoothed vector Y of the aircraft coordinate system. k|k As such, the smoothed vector Y in the aircraft coordinate system k|k,a By substituting this into equation (85), we obtain the predicted vector Y in the aircraft coordinate system. k+1|k,a Calculate (step ST16a in Figure 8). In equation (85), Y k-1|k-1 Y k|k This corresponds to Y k|k-1,a Y k+1|k,a It corresponds to this. Furthermore, the prediction processing units 6b-n calculate the smoothing error covariance matrix V of the aircraft coordinate system. k|k The smoothing error covariance matrix V of the aircraft coordinate system k|k,a By substituting this into equation (86), the prediction error covariance matrix V of the aircraft coordinate system is obtained. k+1|k,a Calculate (step ST16a in Figure 8). In equation (86), V k-1|k-1 V k|k This corresponds to V k|k-1,a V k+1|k,a It corresponds to this. The prediction processing units 6b-n generate the predicted value vector Y in the aircraft coordinate system. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,a Each of these is output to the second coordinate transformation unit 6e.

[0147] The predicted value integration unit 6c receives the smoothed value vector Y of the aircraft coordinate system from the first coordinate transformation unit 6a. k|k and the smoothing error covariance matrix V of the aircraft coordinate system k|k Get each of them. The predicted value integration unit 6c generates the smoothed value vector Y of the aircraft coordinate system. k|k By substituting this into equation (89), we obtain the predicted vector Y in the aircraft coordinate system. k+1|k Calculate (step ST16b in Figure 8). In equation (89), Y k-1|k-1 Y k|k This corresponds to Y k|k-1 Y k+1|k It corresponds to this. Furthermore, the prediction value integration unit 6c uses the smoothing error covariance matrix V of the aircraft coordinate system. k|k By substituting this into equation (90), the prediction error covariance matrix V of the aircraft coordinate system is obtained. k+1|k Calculate (step ST16b in Figure 8). In equation (90), V k-1|k-1 V k|k This corresponds to V k|k-1 V k+1|k It corresponds to this. The prediction value integration unit 6c generates the predicted value vector Y in the aircraft coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the second coordinate transformation unit 6d-n.

[0148] The second coordinate transformation unit 6d-n (n=1,···,N) receives the predicted value vector Y of the aircraft coordinate system from the prediction processing unit 6b-n. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,a Get each of them. The second coordinate transformation unit 6d-n calculates the predicted value vector Y of the aircraft coordinate system according to equation (87). k+1|k,a Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k,a Convert to (Step ST17 in Figure 8). The second coordinate transformation unit 4c generates a predicted value vector X in the North reference Cartesian coordinate system. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,aEach of these is output to the third coordinate transformation unit 6f-n.

[0149] The third coordinate transformation unit 6f-n converts the second coordinate transformation unit 6d-n to the predicted value vector X of the North reference Cartesian coordinate system. k+1|k,a and the aircraft coordinate system prediction error covariance matrix V k+1|k,a Get each of them. The third coordinate transformation unit 6f-n is the prediction error covariance matrix V of the aircraft coordinate system. k+1|k,a and the drive noise error covariance matrix Q k By substituting and into equation (88), the prediction error covariance matrix V of the aircraft coordinate system can be obtained. k+1|k,a The prediction error covariance matrix P in the North-referenced Cartesian coordinate system. k+1|k,a Convert to (Step ST18 in Figure 8). The third coordinate transformation unit 6f-n calculates the predicted value vector X in the North-reference Cartesian coordinate system. k+1|k,a and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k,a Each of these is output to the smoothing unit 5.

[0150] The second coordinate transformation unit 6e receives the predicted value vector Y of the aircraft coordinate system from the predicted value integration unit 6c. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Get each of them. The second coordinate transformation unit 6e calculates the predicted value vector Y of the aircraft coordinate system according to equation (91). k+1|k Predicted value vector X in the North-referenced Cartesian coordinate system k+1|k Convert to (Step ST17 in Figure 8). The second coordinate transformation unit 6e generates a predicted value vector X in the North reference Cartesian coordinate system. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Each of these is output to the third coordinate transformation unit 6g.

[0151] The third coordinate transformation unit 6g receives the predicted value vector X of the North reference Cartesian coordinate system from the second coordinate transformation unit 6e. k+1|k and the aircraft coordinate system prediction error covariance matrix V k+1|k Get each of them. The third coordinate transformation unit 6g is the prediction error covariance matrix V of the aircraft coordinate system. k+1|kand the drive noise error covariance matrix Q k By substituting and into equation (92), the prediction error covariance matrix V of the aircraft coordinate system can be obtained. k+1|k The prediction error covariance matrix P in the North-referenced Cartesian coordinate system. k+1|k Convert to (Step ST18 in Figure 8). The third coordinate transformation unit 6g generates the predicted value vector X in the North reference Cartesian coordinate system. k+1|k This is output to the tracking gate unit 1. Furthermore, the third coordinate transformation unit 6g calculates the predicted value vector X in the North reference Cartesian coordinate system. k+1|k and the prediction error covariance matrix P of the North-referenced Cartesian coordinate system k+1|k Each of these is output to the smoothing unit 5.

[0152] If the target tracking device has not received a request to terminate the target tracking process (in the case of step ST19:NO in Figure 8), the processes in steps ST13 to ST18 are repeated. If a request to terminate the target tracking process is given to the target tracking device (step ST19: YES in Figure 8), the series of processes of the target tracking device will end.

[0153] In the above embodiment 2, the prediction unit 6 uses the initial value of the smoothed value calculated by the initial value calculation unit 2, or the smoothed value estimated by the smoothing unit 5, to perform prediction processing based on each of the multiple motion hypotheses at the target and calculate multiple predicted values. The target tracking device shown in Figure 5 is configured such that the smoothing unit 5 uses the position observation information output from the sensor and each predicted value calculated by the prediction unit 6 to estimate a smoothed value indicating the smooth state of the target. Therefore, the target tracking device shown in Figure 5, like the target tracking device shown in Figure 1, can suppress the deterioration of prediction accuracy due to the initial value of the smoothed value deviating significantly from the original smoothed value, and can also improve tracking accuracy compared to the target tracking device shown in Figure 1.

[0154] In the target tracking device shown in Figure 5, the smoothing unit 5 generates N smoothed value vectors X k|k,a And a smoothed value vector X k|kThe output is displayed on a display device, etc., not shown in the figure. However, this is just one example, and the smoothing unit 5 may, for example, output N smoothed value vectors X k|k,a and one smoothed value vector X k|k The average value of, or N smoothed vector X k|k,a and one smoothed value vector X k|k The weighted sum of these values ​​may be calculated and the average value, or the weighted sum, may be output to a display device (not shown).

[0155] Furthermore, this disclosure allows for free combination of each embodiment, modification of any component in each embodiment, or omission of any component in each embodiment. [Industrial applicability]

[0156] This disclosure is suitable for target tracking devices and target tracking methods. [Explanation of Symbols]

[0157] 1 Tracking gate unit, 2 Initial value calculation unit, 3 Smoothing unit, 4 Prediction unit, 4a First coordinate transformation unit, 4b Prediction processing unit, 4c Second coordinate transformation unit, 4d Third coordinate transformation unit, 5 Smoothing unit, 6 Prediction unit, 6a First coordinate transformation unit, 6b-1 to 6b-N Prediction processing unit, 6c Prediction value integration unit, 6d-1 to 6d-N, 6e Second coordinate transformation unit, 6f-1 to 6f-N, 6g Third coordinate transformation unit, 11 Tracking gate circuit, 12 Initial value calculation circuit, 13 Smoothing circuit, 14 Prediction circuit, 15 Smoothing circuit, 16 Prediction circuit, 21 Memory, 22 Processor.

Claims

1. an initial value calculation unit that acquires position observation information indicating each of the target observation positions at two different sampling times from a sensor that observes the position of the target, and calculates initial values ​​of smoothed values ​​including a position vector of the target and a velocity vector of the target as initial values ​​of smoothed values ​​indicating a state of the target based on the position observation information; a smoothing unit that performs an estimation process of a smoothed value indicating a state of the target using position observation information output from the sensor and a predicted value indicating a state of the target; a prediction unit that predicts a future state of the target by using the initial value of the smoothed value calculated by the initial value calculation unit or the smoothed value estimated by the smoothing unit, and outputs a predicted value indicating a prediction result of the future state of the target to the smoothing unit as a predicted value to be used by the smoothing unit in a next estimation process; The prediction unit is a first coordinate conversion unit that converts a coordinate system of an initial value of the smoothed value calculated by the initial value calculation unit or a coordinate system of the smoothed value estimated by the smoothing unit into an aircraft coordinate system of the target; a prediction processing unit that predicts a future state of the target using an initial value of the aircraft body coordinate system after transformation by the first coordinate transformation unit or a smoothed value of the aircraft body coordinate system after transformation by the first coordinate transformation unit, and outputs a prediction value indicating a prediction result of the future state of the target; a second coordinate conversion unit that converts a coordinate system of the predicted value output from the prediction processing unit into a north-based rectangular coordinate system; The smooth portion is a target tracking device characterized in that a predicted value in the north-based orthogonal coordinate system after transformation by the second coordinate transformation unit is obtained as a predicted value to be used in the next estimation process, and a smoothed value indicating the state of the target is estimated using position observation information output from the sensor and the predicted value in the north-based orthogonal coordinate system.

2. The smooth portion is using position observation information output from the sensor and a previously predicted predicted value, estimating smoothed values ​​indicating a state of the target, the smoothed values ​​including a position vector of the target, a velocity vector of the target, and an aerodynamic coefficient of the target; The prediction unit is 2. The target tracking device according to claim 1, further comprising: a first smoothing unit that calculates a smoothed value for each of the target positions; a first smoothing unit that calculates a smoothed value for each of the target positions; a second smoothing unit that calculates a smoothed value for each of the target positions;

3. The smooth portion is performing an estimation process of a smoothed value indicating a state of the target using the position observation information output from the sensor, the predicted value output from the prediction unit, and a prediction error covariance matrix, and calculating a smoothed error covariance matrix based on the prediction error covariance matrix; 2. The target tracking device according to claim 1, further comprising: a smoothing unit that calculates a prediction error covariance matrix using the smoothing error covariance matrix and the drive noise error covariance matrix calculated by the smoothing unit; and outputs the prediction error covariance matrix to the smoothing unit as a prediction error covariance matrix to be used by the smoothing unit in a next estimation process.

4. 2. The target tracking device according to claim 1, further comprising a tracking gate unit which outputs the position observation information output from the sensor to the smoothing unit only when the target observation position indicated by the position observation information is within the tracking gate.

5. The prediction unit is performing a prediction process based on each of a plurality of motion hypotheses of the target using the initial value of the smoothed value calculated by the initial value calculation unit or the smoothed value estimated by the smoothing unit to calculate a plurality of predicted values; The smooth portion is 2. A target tracking device according to claim 1, further comprising: a prediction unit for predicting a state of the target based on position observation information output from the sensor and each predicted value calculated by the prediction unit;

6. The prediction unit is 6. The target tracking device according to claim 5, further comprising: a prediction process based on the plurality of motion hypotheses to calculate a predicted value in addition to the prediction process based on each motion hypothesis; and a prediction value based on the plurality of motion hypotheses is output to the smoothing unit in addition to the plurality of predicted values ​​based on each motion hypothesis.

7. an initial value calculation unit acquires position observation information indicating each of the target observation positions at two different sampling times from a sensor observing the position of the target, and calculates initial values ​​of smoothed values ​​including a position vector of the target and a velocity vector of the target as initial values ​​of a smoothed value indicating a state of the target based on the position observation information; a smoothing unit, using the position observation information output from the sensor and a predicted value indicating a state of the target, performing an estimation process of a smoothed value indicating the state of the target; a prediction unit predicts a future state of the target using the initial value of the smoothed value calculated by the initial value calculation unit or the smoothed value estimated by the smoothing unit, and outputs a predicted value indicating a prediction result of the future state of the target to the smoothing unit as a predicted value to be used by the smoothing unit in a next estimation process; The prediction unit is a first coordinate transformation unit transforms a coordinate system of the initial value of the smoothed value calculated by the initial value calculation unit or a coordinate system of the smoothed value estimated by the smoothing unit into an aircraft coordinate system of the target; a prediction processing unit predicts a future state of the target using an initial value of the aircraft coordinate system after transformation by the first coordinate transformation unit or a smoothed value of the aircraft coordinate system after transformation by the first coordinate transformation unit, and outputs a prediction value indicating a prediction result of the future state of the target; a second coordinate transformation unit transforms a coordinate system of the predicted value output from the prediction processing unit into a north-based rectangular coordinate system; The smooth portion is a predicted value in the north-referenced orthogonal coordinate system after transformation by the second coordinate transformation unit is obtained as a predicted value to be used in the next estimation process, and a smoothed value indicating the state of the target is estimated using position observation information output from the sensor and the predicted value in the north-referenced orthogonal coordinate system.

Images