Target state estimation device and target state estimation method

Abstract

The target state estimation device is configured to include an orientation calculation value acquisition unit (11) that acquires an orientation calculation value indicating the orientation of the target each time a time corresponding to a time window elapses, a state estimation value calculation unit (14) that estimates a state of the target using the orientation calculation value acquired by the orientation calculation value acquisition unit (11) and outputs a state estimation value indicating a result of the estimation of the state, an accuracy calculation unit (15) that calculates accuracy of the state estimation value output from the state estimation value calculation unit (14), and a smoothed value calculation unit (16) that predicts the state of the target by performing Kalman filter processing on the state estimation value output from the state estimation value calculation unit (14) using the accuracy calculated by the accuracy calculation unit (15), and calculates a smoothed value indicating the state of the target using a state prediction value indicating a result of the prediction of the state.

Description

[Technical field]

[0001] The present disclosure relates to a target state estimation device and a target state estimation method. [Background technology]

[0002] BO-TMA (Bearing Only Target Motion Analysis) is a technology for estimating the state of a moving target. Examples of the state of a target include the target's position and speed. BO-TMA estimates the state of a target using only the direction measurement values ​​from a direction sensor that measures the target's direction at each of multiple points. Since the target's direction and position have a nonlinear relationship, BO-TMA deals with a nonlinear problem. For this reason, BO-TMA requires iterative calculations when estimating the state of the target. Therefore, one of the issues with BO-TMA is that the amount of calculation required to estimate the state increases. Another issue is that the stability of the solution indicating the estimated value of the state may be impaired. In order to solve the problems caused by dealing with nonlinear problems, for example, Non-Patent Document 1 discloses a technique for estimating the state of a target after replacing the nonlinear problem with a pseudo-linear problem by using the geometric relationship between the target's orientation, target position, and the position of a direction sensor. [Prior art documents] [Non-patent literature]

[0003] [Non-Patent Document 1] L. Badriasl, S. Arulampalam and A. Finn, “A Novel Batch Bayesian WIV Estimator for Three-Dimensional TMA Using Bearing and Elevation Measurements,” IEEE Transactions on Signal Processing, vol. 66, no. 4, pp. 1023-1036, 2018. Summary of the Invention [Problem to be solved by the invention]

[0004] The technology disclosed in Non-Patent Document 1 is based on the premise that the motion of the target is a uniform linear motion. Therefore, when the motion of the target is a non-uniform linear motion, the technology has a problem that the state of the target cannot be estimated.

[0005] The present disclosure has been made to solve the above-mentioned problems, and aims to provide a target state estimation device that can estimate the state of a target regardless of whether the target's motion is constant-velocity rectilinear motion or not. [Means for solving the problem]

[0006] The target state estimation device according to the present disclosure performs the following each time a time corresponding to a time window elapses: Based on the sensor signals from the sensors observing the target The system includes an azimuth calculation value acquisition unit that acquires an azimuth calculation value indicating the azimuth angle and elevation angle of the target, a state estimation value calculation unit that constructs a positioning equation using the azimuth calculation value acquired by the azimuth calculation value acquisition unit and outputs a 6-dimensional state estimation value consisting of the position and velocity of the target by solving the positioning equation using SBWIV, an accuracy calculation unit that calculates the accuracy of the 6-dimensional state estimation value output from the state estimation value calculation unit, and a smoothing value calculation unit that smoothes the 6-dimensional state estimation value by performing Kalman filter processing with the 6-dimensional state estimation value as an input using the accuracy calculated by the accuracy calculation unit. Effect of the Invention

[0007] According to the present disclosure, the state of a target can be estimated regardless of whether the target's motion is a uniform linear motion or not. [Brief description of the drawings]

[0008] [Figure 1] 1 is a configuration diagram showing a target state estimating device according to a first embodiment. [Diagram 2]1 is a hardware configuration diagram showing hardware of a target state estimating device according to a first embodiment. [Diagram 3] FIG. 2 is a hardware configuration diagram of a computer when the target state estimation device is realized by software, firmware, or the like. [Figure 4] 4 is a flowchart showing a processing procedure of a target state estimation unit 5. [Diagram 5] FIG. 11 is a configuration diagram showing a target state estimating device according to a second embodiment. [Figure 6] FIG. 11 is a hardware configuration diagram showing hardware of a target state estimating device according to a second embodiment. [Figure 7] FIG. 11 is an explanatory diagram showing the concept of determining an abnormality in a direction measurement value. [Figure 8] FIG. 11 is a configuration diagram showing a target state estimating device according to a third embodiment. [Figure 9] FIG. 11 is a hardware configuration diagram showing hardware of a target state estimating device according to a third embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0009] In order to describe the present disclosure in more detail, embodiments of the present disclosure will be described below with reference to the accompanying drawings.

[0010] Embodiment 1 FIG. 1 is a configuration diagram showing a target state estimating device according to the first embodiment. FIG. 2 is a hardware configuration diagram showing hardware of the target state estimating device according to the first embodiment. The target state estimation device shown in FIG. 1 includes a sensor signal processing unit 1, a direction calculation value storage unit 4, and a target state estimation unit 5.

[0011] The sensor signal processing unit 1 includes a sensor signal acquiring unit 2 and a direction calculating unit 3. The sensor signal processing unit 1 acquires a sensor signal from a sensor observing a target, and calculates the azimuth of the target based on the sensor signal. The sensor signal processing unit 1 outputs to the azimuth calculation value storage unit 4 an azimuth calculation value indicating the calculation result of the azimuth of the target.

[0012] The sensor signal acquiring unit 2 is realized by, for example, a sensor signal acquiring circuit 21 shown in FIG. The sensor signal acquisition unit 2 acquires a sensor signal from a sensor observing a target, and outputs the sensor signal to the orientation calculation unit 3.

[0013] The direction calculation unit 3 is realized by, for example, a direction calculation circuit 22 shown in FIG. The direction calculation unit 3 calculates the direction of the target based on the sensor signal output from the sensor signal acquisition unit 2. The direction calculation unit 3 outputs a direction calculation value indicating the calculation result of the direction of the target to the direction calculation value storage unit 4.

[0014] The azimuth calculation value storage unit 4 is realized by, for example, an azimuth calculation value storage circuit 23 shown in FIG. The direction calculation value storage unit 4 stores the direction calculation value output from the direction calculation unit 3.

[0015] The target state estimation unit 5 includes an orientation calculation value acquisition unit 11, an error setting unit 12, a prior information setting unit 13, a state estimation value calculation unit 14, an accuracy calculation unit 15, a smoothed value calculation unit 16, and a result output unit 17. The azimuth calculation value acquisition unit 11 is realized by, for example, an azimuth calculation value acquisition circuit 24 shown in FIG. The azimuth calculation value acquisition unit 11 acquires an azimuth calculation value indicating the azimuth calculated by the sensor signal processing unit 1 every time a time corresponding to the time window has elapsed. That is, the azimuth calculation value acquisition unit 11 acquires the azimuth calculation value stored in the azimuth calculation value storage unit 4 every time a time corresponding to the time window elapses. The direction calculation value acquisition unit 11 outputs the direction calculation value to the state estimate value calculation unit 14.

[0016] The error setting unit 12 is realized by, for example, an error setting circuit 25 shown in FIG. The error setting unit 12 sets the measurement error variance of the azimuth calculation value acquired by the azimuth calculation value acquisition unit 11 . The error setting unit 12 outputs the measurement error variance of the azimuth calculation value to the state estimate value calculation unit 14 .

[0017] The advance information setting unit 13 is realized by, for example, the advance information setting circuit 26 shown in FIG. The prior information setting unit 13 sets prior information regarding the state of the goal. The prior information setting unit 13 outputs the prior information to the state estimate value calculation unit 14 .

[0018] The state estimate value calculation unit 14 is realized by, for example, a state estimate value calculation circuit 27 shown in FIG. The state estimated value calculation unit 14 acquires the calculated heading value from the calculated heading value acquisition unit 11 , acquires the measurement error variance of the calculated heading value from the error setting unit 12 , and acquires prior information from the prior information setting unit 13 . A state estimate calculation unit 14 estimates the state of the target using the direction calculation value, the measurement error variance, and the prior information. The state estimate value calculation unit 14 outputs a state estimate value indicating the estimation result of the target state to each of the accuracy calculation unit 15 and the smoothed value calculation unit 16.

[0019] The accuracy calculation unit 15 is realized by, for example, an accuracy calculation circuit 28 shown in FIG. The accuracy calculation unit 15 acquires the state estimate value from the state estimate calculation unit 14 . The accuracy calculation unit 15 calculates the accuracy of the state estimate value, and outputs the accuracy of the state estimate value to the smoothed value calculation unit 16 .

[0020] The smoothed value calculation unit 16 is realized by, for example, a smoothed value calculation circuit 29 shown in FIG. The smoothed value calculation unit 16 obtains the state estimate value from the state estimate value calculation unit 14 and obtains the accuracy of the state estimate value from the accuracy calculation unit 15 . The smoothed value calculation unit 16 predicts the state of the target by performing Kalman filter processing on the state estimated value using the accuracy of the state estimated value. The smoothed value calculation unit 16 calculates a smoothed value indicating the state of the target by using a state predicted value indicating the predicted result of the state. The smoothed value calculation unit 16 outputs the smoothed value indicating the target state to the result output unit 17 .

[0021] The result output unit 17 is realized by, for example, a result output circuit 30 shown in FIG. The result output unit 17 obtains the smoothed value indicating the target state from the smoothed value calculation unit 16 . The result output unit 17 outputs the smoothed value to, for example, a radar device (not shown) or a display device (not shown).

[0022] In Fig. 1, it is assumed that each of the components of the target state estimation device, that is, the sensor signal acquisition unit 2, the direction calculation unit 3, the direction calculation value storage unit 4, the direction calculation value acquisition unit 11, the error setting unit 12, the prior information setting unit 13, the state estimation value calculation unit 14, the accuracy calculation unit 15, the smoothed value calculation unit 16, and the result output unit 17, is realized by dedicated hardware as shown in Fig. 2. That is, it is assumed that the target state estimation device is realized by the sensor signal acquisition circuit 21, the direction calculation circuit 22, the direction calculation value storage circuit 23, the direction calculation value acquisition circuit 24, the error setting circuit 25, the prior information setting circuit 26, the state estimation value calculation circuit 27, the accuracy calculation circuit 28, the smoothed value calculation circuit 29, and the result output circuit 30.

[0023] Here, the orientation calculation value memory circuit 23 may be, for example, a non-volatile or volatile semiconductor memory such as a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read Only Memory), an EEPROM (Electrically Erasable Programmable Read Only Memory), a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, or a DVD (Digital Versatile Disc). In addition, each of the sensor signal acquisition circuit 21, the orientation calculation circuit 22, the orientation calculation value acquisition circuit 24, the error setting circuit 25, the prior information setting circuit 26, the state estimation value calculation circuit 27, the accuracy calculation circuit 28, the smoothed value calculation circuit 29 and the result output circuit 30 corresponds to, 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 of these.

[0024] The components of the target state estimation device are not limited to those realized by dedicated hardware, and the target state estimation device may be realized by software, firmware, or a combination of software and firmware. The software or firmware is stored as a program in the memory of a computer. The computer means hardware that executes the program, and includes, for example, a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), a central processing unit, a processing unit, an arithmetic unit, a microprocessor, a microcomputer, a processor, or a DSP (Digital Signal Processor).

[0025] FIG. 3 is a hardware configuration diagram of a computer in the case where the target state estimation device is realized by software, firmware, or the like. When the target state estimation device is realized by software, firmware, or the like, the orientation calculation value storage unit 4 is configured on a computer memory 41. Programs for causing a computer to execute the respective processing procedures in the sensor signal acquisition unit 2, the orientation calculation unit 3, the orientation calculation value acquisition unit 11, the error setting unit 12, the prior information setting unit 13, the state estimation value calculation unit 14, the accuracy calculation unit 15, the smoothed value calculation unit 16, and the result output unit 17 are stored in the memory 41. Then, a processor 42 of the computer executes the programs stored in the memory 41.

[0026] 2 shows an example in which each of the components of the target state estimation device is realized by dedicated hardware, and Fig. 3 shows an example in which the target state estimation device is realized by software, firmware, etc. However, this is merely one example, and some of the components in the target state estimation device may be realized by dedicated hardware, and the remaining components may be realized by software, firmware, etc.

[0027] Next, the operation of the target state estimating device shown in FIG. 1 will be described. 4 is a flowchart showing the processing procedure of the target state estimation unit 5. The processing procedure of the target state estimation unit 5 shows a part of the target state estimation method. The sensor signal acquisition unit 2 acquires a sensor signal from a sensor observing a target. The number of sensors observing a target may be one or more. The sensor signal output from the sensor includes the target observation time t and the sensor coordinates indicating the position of the sensor. The sensor coordinates are, for example, the sensor position vector r k where k is a variable that identifies the sensor, and k = 1, . . . , K. K is an integer equal to or greater than 1. The sensor signal acquisition unit 2 outputs the sensor signal to the direction calculation unit 3.

[0028] The direction calculation unit 3 acquires the sensor signal from the sensor signal acquisition unit 2 . The direction calculation unit 3 calculates the direction of the target based on the sensor signal. The process of calculating the target direction based on the sensor signal is a known technique, and therefore a detailed description thereof will be omitted. The direction calculation unit 3 calculates a direction calculated value indicating the calculation result of the direction of the target, the target observation time t included in the sensor signal, and the sensor position vector r included in the sensor signal. k The calculated azimuth value is output to the azimuth calculation value storage unit 4. The calculated azimuth value is the azimuth angle direction calculated azimuth value θ tilde k and the calculated azimuth value in the elevation angle direction φ tilde kIn the text of the specification, because of electronic filing, the symbol "~" cannot be placed above the letters, so the tilde symbol is used instead. k , φ tilde k The calculated azimuth is expressed as follows. For example, the azimuth calculation value is expressed as a vector c k and the target position vector p k It is represented by: As a result, the azimuth calculation value storage unit 4 stores the azimuth calculation value, the target observation time t, and the sensor position vector r k The pair of is stored. Below, the calculated direction, the target observation time t, and the sensor position vector r k The set of these is called the orientation data set DS.

[0029] Orientation calculation value acquisition unit 11 waits until the time corresponding to the time window has elapsed (if step ST1 in FIG. 4: NO). Each time a time corresponding to the time window elapses (step ST1 in FIG. 4: YES), the direction calculation value acquisition unit 11 acquires the direction data set DS from the direction calculation value storage unit 4 (step ST2 in FIG. 4). Specifically, it is assumed that the time corresponding to the time window is tw, and the observation time t included in the orientation data set DS previously acquired by the orientation calculation value acquisition unit 11 from the orientation calculation value storage unit 4 is t1. In this case, the orientation calculation value acquisition unit 11 acquires an orientation data set DS with the first observation time t among the orientation data sets stored in the orientation calculation value storage unit 4, the observation time t being later than t1+tw. The time tw corresponding to the time window is set according to the motion of the target. That is, even if the motion of the target is non-uniform linear motion, if the time tw corresponding to the time window is sufficiently short compared to the time required for the target's speed to change, the motion of the target can be regarded as uniform motion. Also, even if the motion of the target is non-uniform linear motion, if the time tw corresponding to the time window is sufficiently short compared to the time required for the target's traveling direction to change, the motion of the target can be regarded as linear motion. Therefore, the time tw corresponding to the time window is set to a time that allows the motion of the target to be regarded as uniform linear motion even if the motion of the target is non-uniform linear motion. Note that the time tw corresponding to the time window may change over time, and multiple adjacent time windows may overlap each other in part. The direction calculation value acquisition unit 11 outputs the direction data set DS acquired from the direction calculation value storage unit 4 to the state estimate value calculation unit 14.

[0030] The error setting unit 12 sets the measurement error variance of the orientation calculation value included in the orientation data set DS acquired by the orientation calculation value acquisition unit 11 (step ST3 in FIG. 4). Any method may be used to set the measurement error variance by the error setting unit 12, but if some additional accuracy information is added to the azimuth measurement value, the measurement error variance may be set based on the additional accuracy information. For example, if the additional accuracy information indicates the accuracy of the azimuth calculation value, a larger measurement error variance may be set as the accuracy of the azimuth calculation value decreases. The error setting unit 12 outputs the measurement error variance of the azimuth calculation value to the state estimate value calculation unit 14 .

[0031] The prior information setting unit 13 sets prior information regarding the state of the goal (step ST4 in FIG. 4). Specifically, the prior information setting unit 13 sets, as the prior information regarding the state of the goal, for example, an expectation value of the prior probability regarding the state of the goal and a covariance matrix of the prior probability regarding the state of the goal. Any method may be used for setting the prior information set by the prior information setting unit 13, and for example, the prior information may be set by using the predictive distribution of the Kalman filter used by the smoothed value calculation unit 16 as a prior distribution. When it is difficult for the prior information setting unit 13 to set the prior information regarding the state of the target, the influence of the prior information may be made negligible by setting a sufficiently large value to the covariance matrix of the prior probability. The prior information setting unit 13 outputs the prior information to the state estimate value calculation unit 14 .

[0032] The state estimated value calculation unit 14 acquires the orientation data set DS from the orientation calculated value acquisition unit 11 , acquires the measurement error variance from the error setting unit 12 , and acquires prior information from the prior information setting unit 13 . The state estimation value calculation unit 14 estimates the state of the target using the orientation data set DS, the measurement error variance, and the prior information (step ST5 in FIG. 4). The state estimation value indicating the state estimation result by the state estimation value calculation unit 14 is, for example, a Short-time Bayesian Weighted Instrumental Variable (SBWIV) estimation value. The state estimate value calculation unit 14 outputs a state estimate value indicating the estimation result of the target state to each of the accuracy calculation unit 15 and the smoothed value calculation unit 16.

[0033] Hereinafter, a specific description will be given of the target state estimation process performed by the state estimated value calculation unit 14. Details of the target state estimation process are disclosed in Non-Patent Document 1. First, the state estimated value calculation unit 14 formulates a positioning equation as shown in the following equation (1) as a positioning equation that expresses the relationship between the target direction and the target position in a pseudo-linear manner.

[0034] TIFF0007415090000001.tif12166In equation (1), T is the mathematical symbol for transposition.

[0035] The state estimate calculation unit 14 calculates a Bayesian Weighted Pseudo-Linear Estimator (BWPLE) that is an estimator by performing Maximum A Posteriori (MAP) estimation, which is a maximum a posteriori estimation, using the measurement error covariance and prior information. Next, the state estimate calculation unit 14 calculates an estimator called BBCPLE (Bayesian Bias Compensated weighted Pseudo-Linear Estimator) by correcting a bias error caused by the pseudo-linearization included in BWPLE. Finally, the state estimate calculation unit 14 calculates the SBWIV estimate, which is a state estimate with the bias corrected, by applying the instrumental variable method using the direction estimate calculated back from the BBCPLE. When the motion of the target is non-uniform linear motion, the SBWIV estimated value may contain an error. However, by setting the time tw corresponding to the time window to a time that can be regarded as uniform linear motion without any problem, the error in the SBWIV estimated value can be made to a size that is practically problem-free. On the other hand, by shortening the time tw corresponding to the time window, the estimation accuracy of the SBWIV estimated value may decrease. In such a case, the smoothed value calculation unit 16 in the subsequent stage predicts the state of the target by performing Kalman filter processing on the SBWIV estimated value using an error covariance matrix, which is the accuracy of the SBWIV estimated value. Then, the smoothed value calculation unit 16 calculates the state prediction value x m|m-1 and the prediction error covariance matrix P m|m-1 By performing smoothing processing using m|m Therefore, even if the target motion is a non-uniform linear motion, the target state estimate x m|m The error contained in is of a size that does not cause any practical problems.

[0036] The accuracy calculation unit 15 acquires the SBWIV estimated value, which is the state estimated value, from the state estimated value calculation unit 14. The accuracy calculation unit 15 calculates the accuracy of the SBWIV estimated value (step ST6 in FIG. 4). The accuracy calculation unit 15 outputs the accuracy of the SBWIV estimated value to the smoothed value calculation unit 16.

[0037] Hereinafter, the process of calculating the accuracy of the SBWIV estimated value by the accuracy calculation unit 15 will be specifically described. The ideal accuracy of the SBWIV estimated value asymptotically approaches the Bayesian Cramer-Rao Bound (BCRB). Therefore, the accuracy calculation unit 15 calculates the BCRB at the start time of the time window as shown in the following formula (2).

[0038] TIFF0007415090000002.tif102166

[0039] TIFF0007415090000003.tif19166In equation (3), Δt m is the time difference between the representative time of the m-th time window and the start time of the time window.

[0040] TIFF0007415090000004.tif97166In equation (5), M k is the time difference Δt from the beginning of the time window to the kth observation time. k Using the above, the transformation matrix is ​​expressed by the following equation (8).

[0041] TIFF0007415090000005.tif8166

[0042] Next, the accuracy calculation unit 15 calculates the R hat which is the BCRB at the start time of the time window. BCRM,m,0 At time t m In order to convert the value into a value equivalent to R hat, as shown in the following equation (9), BCRM,m,0 is divided into four submatrices of 3 rows and 3 columns. In the text of the specification, the symbol “^” cannot be placed above the letters due to electronic filing, so R hat BCRM,m,0 It is written as follows.

[0043] TIFF0007415090000006.tif15166

[0044] B CRB corresponding to the representative time of the m-th time window is expressed as in the following formula (10). B CRB is the theoretical lower limit of accuracy. For this reason, the accuracy calculation unit 15 calculates an arbitrary real number, which is one or more adjustment parameters, as R hat. BCRM,m The error covariance matrix R of the SBWIV estimate is m BWIV The error covariance matrix of the SBWIV estimate, R, is set to m BWIVcorresponds to the precision of the SBWIV estimate. The representative time of the m-th time window may be, for example, the average value of the observation times associated with the orientation measurement values ​​included in the m-th time window, or may be a weighted average value of the observation times if there is variation in the error of the orientation measurement values.

[0045] TIFF0007415090000007.tif16166

[0046] The smoothing value calculation unit 16 obtains the SBWIV estimated value, which is the state estimated value, from the state estimated value calculation unit 14, and calculates the error covariance matrix R hat , which is the accuracy of the state estimated value, from the accuracy calculation unit 15. m BWIV Get the. The smoothed value calculation unit 16 calculates the error covariance matrix R m BWIV The target state is predicted by performing Kalman filtering on the SBWIV estimate using Then, the smoothed value calculation unit 16 calculates a smoothed value indicating the state of the target by using the state predicted value indicating the prediction result of the state of the target (step ST7 in FIG. 4).

[0047] The calculation process of the smoothed value by the smoothed value calculation unit 16 will be specifically described below. The motion model used in the Kalman filter processing is expressed as shown in the following equation (11).

[0048] TIFF0007415090000008.tif28166In equation (11), x m is the state vector that lists the 3D position and 3D velocity of the target, and w m is the driving noise.

[0049] The target observation model is expressed as follows:

[0050] TIFF0007415090000009.tif9166

[0051] In the target state estimation device shown in FIG. 1, unlike a normal Kalman filter, the input of the observation model is not a sensor observation value but an SBWIV estimated value. Therefore, the dimensions of the observation model and the motion model match in six dimensions of position and velocity. Also, v m is the error in the SBWIV estimate, not the observation error.

[0052] The smoothed value calculation unit 16 performs Kalman filter processing on the SBWIV estimated value based on the above model. When m=1, the SBWIV estimate x hat 1 is calculated as shown in the following equation (14). BWIV is the smoothed value x 1|1 As shown in the following equation (15), the error covariance matrix R of the state estimate is BWIV is the error covariance matrix P 1|1 It becomes.

[0053] TIFF0007415090000010.tif28166

[0054] When m>1, the prediction process of the smoothed value calculation unit 16 is expressed by the following equations (16) to (17).

[0055] TIFF0007415090000011.tif22166In equation (17), Q m-1 is the drive error covariance matrix at m-1.

[0056] The smoothed value calculation unit 16 calculates the state predicted value x m|m-1 and the prediction error covariance matrix P m|m-1 By performing smoothing processing using m|m Calculate. In addition, the smoothed value calculation unit 16 calculates the prediction error covariance matrix P m|m-1 By smoothing the error covariance matrix P m|m Calculate. The smoothing value calculation unit 16 calculates the target state estimate x m|m is output to the result output unit 17.

[0057] TIFF0007415090000012.tif37166

[0058] As described above, the smoothed value calculation unit 16 predicts the state of the target by performing Kalman filter processing on the SBWIV estimated value calculated by the state estimation value calculation unit 14 for each time corresponding to the time window, and calculates a smoothed value indicating the state of the target using a state prediction value indicating the predicted result of the state. Therefore, the target state estimation unit 5 can smooth the variation in the SBWIV estimated value caused by a decrease in the number of observations of the sensor signal, and can perform high-precision state estimation even when the target performs non-uniform linear motion. In addition, since the state of the target can be estimated for each time corresponding to the time window, the responsiveness of the state estimation can be improved.

[0059] The result output unit 17 receives the target state estimate value x m|m Get the. The result output unit 17 outputs the target state estimate x m|m is output to, for example, a radar device (not shown) or a display device (not shown). Target state estimate x m|m By obtaining the above, the trajectory of the target state is obtained.

[0060] When continuing the target state estimation process (step ST8 in FIG. 4: YES), the target state estimation unit 5 performs the processes of steps ST1 to ST8 in FIG. 4 again. When the target state estimation unit 5 does not continue the target state estimation process (step ST8 in FIG. 4: NO), the target state estimation unit 5 ends the series of processes.

[0061] In the above-described first embodiment, the target state estimation device is configured to include an orientation calculation value acquisition unit 11 that acquires an orientation calculation value indicating the orientation of the target every time a time corresponding to a time window elapses, a state estimation value calculation unit 14 that estimates the state of the target using the orientation calculation value acquired by the orientation calculation value acquisition unit 11 and outputs a state estimation value indicating the estimation result of the state, an accuracy calculation unit 15 that calculates the accuracy of the state estimation value output from the state estimation value calculation unit 14, and a smoothing value calculation unit 16 that predicts the state of the target by performing Kalman filter processing on the state estimation value output from the state estimation value calculation unit 14 using the accuracy calculated by the accuracy calculation unit 15, and calculates a smoothing value indicating the state of the target using a state prediction value indicating the prediction result of the state. Therefore, the target state estimation device can estimate the state of the target regardless of whether the motion of the target is a uniform linear motion or not.

[0062] Embodiment 2 In embodiment 2, a target state estimation device is described that includes an abnormal value determination unit 18 that uses a state prediction value indicating the state prediction result by a smoothed value calculation unit 16 to determine whether or not the azimuth calculation value acquired by the azimuth calculation value acquisition unit 11 is an abnormal value.

[0063] Fig. 5 is a configuration diagram showing a target state estimating device according to embodiment 2. In Fig. 5, the same reference numerals as in Fig. 1 indicate the same or corresponding parts, and detailed description thereof will be omitted. Fig. 6 is a hardware configuration diagram showing hardware of a target state estimating device according to embodiment 2. In Fig. 6, the same reference numerals as in Fig. 2 indicate the same or corresponding parts, and detailed description thereof will be omitted. The target state estimation unit 5 shown in FIG. 5 includes an orientation calculation value acquisition unit 11, an error setting unit 12, a prior information setting unit 13, a state estimation value calculation unit 14, an accuracy calculation unit 15, a smoothed value calculation unit 16, a result output unit 17, and an abnormal value determination unit 18.

[0064] Abnormal value determination section 18 is realized by, for example, abnormal value determination circuit 31 shown in FIG. The abnormal value determination unit 18 includes a state predicted value acquisition unit 18a, a prediction gate determination unit 18b, and a reliability setting unit 18c. The abnormal value determination unit 18 acquires a state prediction value indicating a prediction result of the state from the smoothed value calculation unit 16 , and acquires a direction calculation value from the direction calculation value acquisition unit 11 . The abnormal value determination unit 18 converts the state predicted value into a target azimuth and calculates the residual between the azimuth calculation value and the azimuth after conversion. The abnormal value determination unit 18 calculates the variance from the residual between the calculated orientation value and the transformed orientation, and the residual covariance matrix obtained from the prediction error covariance matrix of the target. Abnormal value determination unit 18 determines whether or not the azimuth calculation value acquired by azimuth calculation value acquisition unit 11 is an abnormal value, based on the variance.

[0065] The state predicted value acquisition unit 18a acquires, from the smoothed value calculation unit 16, a state predicted value indicating a prediction result of the state and a prediction error covariance matrix. The state predicted value acquisition unit 18a outputs the state predicted value and the prediction error covariance matrix to the prediction gate determination unit 18b.

[0066] The prediction gate determination unit 18b acquires the state predicted value and the prediction error covariance matrix from the state predicted value acquisition unit 18a, and acquires the azimuth calculation value from the azimuth calculation value acquisition unit 11. The predicted gate determination unit 18b converts the state predicted value into a target direction. The predicted gate determination unit 18b calculates the residual between the direction calculation value and the converted direction. The prediction gate determination unit 18b calculates a residual covariance matrix from the prediction error covariance matrix. The prediction gate determination unit 18b calculates the variance from the residual and the residual covariance matrix. Prediction gate determination unit 18b determines whether or not the azimuth calculation value acquired by azimuth calculation value acquisition unit 11 is an abnormal value, based on the variance.

[0067] When the prediction gate determination unit 18b determines that the azimuth calculation value is an abnormal value, the reliability setting unit 18c replaces the value of the measurement error variance corresponding to the azimuth calculation value with a larger value to reduce the influence of the abnormal azimuth measurement value on the state estimation. The measurement error variance corresponding to the azimuth calculation value is the measurement error variance set by the error setting unit 12.

[0068] In Fig. 5, it is assumed that each of the components of the target state estimation device, that is, the sensor signal acquisition unit 2, the direction calculation unit 3, the direction calculation value storage unit 4, the direction calculation value acquisition unit 11, the error setting unit 12, the prior information setting unit 13, the state estimation value calculation unit 14, the accuracy calculation unit 15, the smoothed value calculation unit 16, the result output unit 17, and the abnormal value determination unit 18, is realized by dedicated hardware as shown in Fig. 6. That is, it is assumed that the target state estimation device is realized by the sensor signal acquisition circuit 21, the direction calculation circuit 22, the direction calculation value storage circuit 23, the direction calculation value acquisition circuit 24, the error setting circuit 25, the prior information setting circuit 26, the state estimation value calculation circuit 27, the accuracy calculation circuit 28, the smoothed value calculation circuit 29, the result output circuit 30, and the abnormal value determination circuit 31.

[0069] Each of the sensor signal acquisition circuit 21, the orientation calculation circuit 22, the orientation calculation value acquisition circuit 24, the error setting circuit 25, the prior information setting circuit 26, the state estimation value calculation circuit 27, the accuracy calculation circuit 28, the smoothed value calculation circuit 29, the result output circuit 30 and the abnormal value determination circuit 31 corresponds to, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC, an FPGA, or a combination of these.

[0070] The components of the target state estimation device are not limited to those realized by dedicated hardware, and the target state estimation device may be realized by software, firmware, or a combination of software and firmware. When the target state estimation device is realized by software, firmware, or the like, the orientation calculation value storage unit 4 is configured on a memory 41 shown in Fig. 3. Programs for causing a computer to execute the respective processing procedures in the sensor signal acquisition unit 2, the orientation calculation unit 3, the orientation calculation value acquisition unit 11, the error setting unit 12, the prior information setting unit 13, the state estimation value calculation unit 14, the accuracy calculation unit 15, the smoothed value calculation unit 16, the result output unit 17, and the abnormal value determination unit 18 are stored in the memory 41 shown in Fig. 3. Then, a processor 42 shown in Fig. 3 executes the programs stored in the memory 41.

[0071] Also, Fig. 6 shows an example in which each of the components of the target state estimation device is realized by dedicated hardware, and Fig. 3 shows an example in which the target state estimation device is realized by software, firmware, etc. However, this is merely an example, and some of the components in the target state estimation device may be realized by dedicated hardware, and the remaining components may be realized by software, firmware, etc.

[0072] Next, the operation of the target state estimating device shown in Fig. 5 will be described. However, since the target state estimating device is similar to that shown in Fig. 1 except for abnormal value determining unit 18, only the operation of abnormal value determining unit 18 will be described here.

[0073] The state predicted value acquisition unit 18a receives the state predicted value x m|m-1 and the prediction error covariance matrix P m|m-1 and get. State prediction value x m|m-1 x k|m-1 This can be expressed as shown in the following equation (21). Prediction error covariance matrix P m|m-1 , P k|m-1 This can be expressed as shown in the following equation (22). The state prediction value acquisition unit 18a obtains the state prediction value x k|m-1 and the prediction error covariance matrix P k|m-1 and is output to the prediction gate determination unit 18b.

[0074] TIFF0007415090000013.tif40166In equation (23), t k is the observation time associated with the orientation measurement value of the target for anomaly judgment.

[0075] The prediction gate determination unit 18b receives the state prediction value x from the state prediction value acquisition unit 18a. k|m-1 and the prediction error covariance matrix P k|m-1 and obtains a calculated azimuth value from the calculated azimuth value acquisition unit 11. The calculated azimuth value is a calculated azimuth value θ tilde in the azimuth angle direction. k and the calculated azimuth value in the elevation angle direction φ tilde k It includes: The prediction gate determination unit 18b determines the state prediction value x k|m-1 Let the target direction be the azimuth angle θ(x k|m-1 ) and elevation angle φ(x k|m-1 ) and convert it to the predicted state value x k|m-1 The process of converting the above into the target's azimuth and the like is a well-known technique, so a detailed description thereof will be omitted.

[0076] The prediction gate determination unit 18b calculates the residual e between the calculated azimuth value and the converted azimuth as shown in the following equation (24). k Calculate. In addition, the prediction gate determination unit 18b calculates the prediction error covariance matrix P k|m-1 from the residual covariance matrix R s Calculate.

[0077] TIFF0007415090000014.tif26166In equation (25), H k corresponds to the first to third columns of the matrix expressed by equation (4).

[0078] The prediction gate determination unit 18b calculates the residual e k and the residual covariance matrix R s and the variance L 2 Calculate.

[0079] TIFF0007415090000015.tif10166

[0080] The prediction gate determination unit 18b determines the variance L 2 Compare with the threshold and variance L 2 Based on the comparison result between the calculated azimuth value and the threshold value, it is determined whether the calculated azimuth value is abnormal or not. The threshold value may be stored in an internal memory of the prediction gate determination unit 18b, or may be provided from outside the target state estimation device shown in FIG. Specifically, the prediction gate determination unit 18b calculates the variance L 2 If is greater than the threshold, the calculated direction is determined to be an abnormal value, and the variance L 2 If is equal to or smaller than the threshold, it is determined that the calculated azimuth value is not an abnormal value. The prediction gate determination unit 18b outputs a determination result indicating whether or not the azimuth calculation value is an abnormal value to the reliability setting unit 18c.

[0081] FIG. 7 is an explanatory diagram showing the concept of determining whether or not there is an abnormality in the azimuth measurement value. In FIG. 7, the decision gate is the state prediction value x m|m-1 For example, the azimuth direction θ tilde is set to surround the predicted position of the target. k If is included in the judgment gate, the calculated azimuth value θ tilde k is determined to be not an abnormal value. k If is outside the judgment gate, the calculated azimuth value θ tilde k is determined to be an abnormal value. The size of the determination gate is set, for example, in accordance with the assumed environment for measuring the orientation.

[0082] The reliability setting unit 18c acquires a determination result indicating whether or not the azimuth calculation value is an abnormal value from the prediction gate determination unit 18b. When the azimuth calculation value is determined to be an abnormal value, the reliability setting unit 18c replaces the value of the measurement error variance corresponding to the abnormal azimuth calculation value with a larger value. By replacing the value of the measurement error variance corresponding to the azimuth calculation value with a larger value, the influence of the abnormal azimuth measurement value on the state estimation when the state estimation value calculation unit 14 estimates the state of the target is reduced.

[0083] In the target state estimation device shown in FIG. 5, the prediction gate determination unit 18b calculates the variance L 2 Based on the result of the comparison between the variance L and the threshold value, it is determined whether the azimuth calculation value is an abnormal value. However, this is only one example. 2 The degree of abnormality may be classified into two or more levels depending on the value of, and the reliability setting unit 18c may change the value of the measurement error variance corresponding to the azimuth calculation value depending on the degree of abnormality.

[0084] In the above-mentioned second embodiment, the target state estimation device shown in Fig. 5 is configured such that the abnormal value determination unit 18 converts the state prediction value indicating the state prediction result by the smoothed value calculation unit 16 into the target's azimuth, calculates the variance from the residual between the azimuth calculation value acquired by the azimuth calculation value acquisition unit 11 and the converted azimuth, and the residual covariance matrix obtained from the prediction error covariance matrix of the target, and determines whether the azimuth calculation value acquired by the azimuth calculation value acquisition unit 11 is an abnormal value based on the variance. Therefore, like the target state estimation device shown in Fig. 1, the target state estimation device shown in Fig. 5 can estimate the state of the target regardless of whether the target's motion is a uniform linear motion or not, and can suppress deterioration of the state estimation accuracy due to an abnormal azimuth calculation value.

[0085] Embodiment 3 In the third embodiment, a target state estimating device including a smoothing processing unit 19 that further smoothes the smoothed value calculated by the smoothed value calculation unit 16 will be described.

[0086] Fig. 8 is a configuration diagram showing a target state estimating device according to embodiment 3. In Fig. 8, the same reference numerals as in Fig. 1 and Fig. 5 indicate the same or corresponding parts, and detailed description thereof will be omitted. Fig. 9 is a hardware configuration diagram showing hardware of a target state estimating device according to embodiment 3. In Fig. 9, the same reference numerals as in Fig. 2 and Fig. 6 indicate the same or corresponding parts, and detailed description thereof will be omitted. The target state estimation unit 5 shown in FIG. 8 includes an orientation calculation value acquisition unit 11, an error setting unit 12, a prior information setting unit 13, a state estimation value calculation unit 14, an accuracy calculation unit 15, a smoothed value calculation unit 16, a smoothing processing unit 19, and a result output unit 17.

[0087] The smoothing processing unit 19 is realized by, for example, a smoothing processing circuit 32 shown in FIG. The smoothing processor 19 obtains the smoothed value from the smoothed value calculator 16 . The smoothing processing unit 19 performs a smoothing process to further smooth the smoothed value, and outputs the smoothed value after the smoothing process to the result output unit 17.

[0088] The target state estimation unit 5 shown in Fig. 8 is obtained by applying a smoothing processing unit 19 to the target state estimation unit 5 shown in Fig. 1. However, this is merely an example, and the target state estimation unit 5 shown in Fig. 5 may also be obtained by applying the smoothing processing unit 19.

[0089] In Fig. 8, it is assumed that each of the components of the target state estimation device, that is, the sensor signal acquisition unit 2, the direction calculation unit 3, the direction calculation value storage unit 4, the direction calculation value acquisition unit 11, the error setting unit 12, the prior information setting unit 13, the state estimation value calculation unit 14, the accuracy calculation unit 15, the smoothing value calculation unit 16, the smoothing processing unit 19, and the result output unit 17, is realized by dedicated hardware as shown in Fig. 9. That is, it is assumed that the target state estimation device is realized by the sensor signal acquisition circuit 21, the direction calculation circuit 22, the direction calculation value storage circuit 23, the direction calculation value acquisition circuit 24, the error setting circuit 25, the prior information setting circuit 26, the state estimation value calculation circuit 27, the accuracy calculation circuit 28, the smoothing value calculation circuit 29, the smoothing processing circuit 32, and the result output circuit 30.

[0090] Each of the sensor signal acquisition circuit 21, the orientation calculation circuit 22, the orientation calculation value acquisition circuit 24, the error setting circuit 25, the prior information setting circuit 26, the state estimation value calculation circuit 27, the accuracy calculation circuit 28, the smoothing value calculation circuit 29, the smoothing processing circuit 32 and the result output circuit 30 corresponds to, for example, a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC, an FPGA, or a combination of these.

[0091] The components of the target state estimation device are not limited to those realized by dedicated hardware, and the target state estimation device may be realized by software, firmware, or a combination of software and firmware. When the target state estimation device is realized by software, firmware, or the like, the orientation calculation value storage unit 4 is configured on a memory 41 shown in Fig. 3. Programs for causing a computer to execute the respective processing procedures in the sensor signal acquisition unit 2, the orientation calculation unit 3, the orientation calculation value acquisition unit 11, the error setting unit 12, the prior information setting unit 13, the state estimation value calculation unit 14, the accuracy calculation unit 15, the smoothed value calculation unit 16, the smoothing processing unit 19, and the result output unit 17 are stored in the memory 41 shown in Fig. 3. Then, a processor 42 shown in Fig. 3 executes the programs stored in the memory 41.

[0092] Also, Fig. 9 shows an example in which each of the components of the target state estimation device is realized by dedicated hardware, and Fig. 3 shows an example in which the target state estimation device is realized by software, firmware, etc. However, this is merely an example, and some of the components in the target state estimation device may be realized by dedicated hardware, and the remaining components may be realized by software, firmware, etc.

[0093] Next, the operation of the target state estimating device shown in Fig. 8 will be described. However, since the target state estimating device is similar to that shown in Fig. 1 except for the smoothing processing unit 19, only the operation of the smoothing processing unit 19 will be described here.

[0094] After the target state estimation process in the target state estimation unit 5 is completed (step ST8 in FIG. 4: NO), the smoothing processing unit 19 receives the state predicted value x m|m-1 and the prediction error covariance matrix P m|m-1 and get. The smoothing processing unit 19 also receives the smoothed value, the target state estimate value x m|m and the error covariance matrix P m|m and get.

[0095] The smoothing processor 19 calculates a smoother gain S used in the smoothing process as shown in the following equation (27): m-1 Set. The smoothing processor 19 calculates a smoother gain S m-1 Using this, the target state estimate x m|m The smoothing process is performed to obtain the target state estimate x RTS,m-1 is output to the result output unit 17. In the formula (28), x m-1|m-1 x m|m This is the target state estimate one sampling earlier than The smoothing processor 19 calculates a smoother gain S m-1 Using the smooth error covariance matrix P m|m The smoothing process is performed, and the smoothed error covariance matrix P RTS,m-1 is output to the result output unit 17. In the formula (28), P m-1|m-1 , P m|m is the smoothed error covariance matrix for the time immediately preceding

[0096] TIFF0007415090000016.tif37166

[0097] In the above-described third embodiment, the target state estimation device shown in Fig. 8 is configured to include a smoothing processing unit 19 that further smooths the smoothed value calculated by the smoothed value calculation unit 16. Therefore, similar to the target state estimation device shown in Fig. 1, the target state estimation device shown in Fig. 8 can estimate the state of the target regardless of whether the motion of the target is a uniform linear motion or not. The target state estimation device shown in Fig. 8 can improve the accuracy of state estimation compared to the target state estimation device shown in Fig. 1, although it loses responsiveness of state estimation.

[0098] In addition, the present disclosure allows free combination of the respective embodiments, modification of any of the components of each embodiment, or omission of any of the components of each embodiment. [Industrial Applicability]

[0099] The present disclosure is suitable for a target state estimation device and a target state estimation method. [Explanation of symbols]

[0100] 1 sensor signal processing unit, 2 sensor signal acquisition unit, 3 heading calculation unit, 4 heading calculation value storage unit, 5 target state estimation unit, 11 heading calculation value acquisition unit, 12 error setting unit, 13 prior information setting unit, 14 state estimation value calculation unit, 15 accuracy calculation unit, 16 smoothing value calculation unit, 17 result output unit, 18 abnormal value determination unit, 18a state prediction value acquisition unit, 18b prediction gate determination unit, 18c reliability setting unit, 19 smoothing processing unit, 21 sensor signal acquisition circuit, 22 heading calculation circuit, 23 heading calculation value storage circuit, 24 heading calculation value acquisition circuit, 25 error setting circuit, 26 prior information setting circuit, 27 state estimation value calculation circuit, 28 accuracy calculation circuit, 29 smoothing value calculation circuit, 30 result output circuit, 31 abnormal value determination circuit, 32 smoothing processing circuit, 41 memory, 42 processor.

Claims

1. an azimuth calculation value acquisition unit that acquires an azimuth calculation value indicating an azimuth angle and an elevation angle of a target every time a time corresponding to a time window has elapsed; a state estimation value calculation unit that uses the azimuth calculation value acquired by the azimuth calculation value acquisition unit to construct a positioning equation, and solves the positioning equation by SBWIV to output a six-dimensional state estimation value including a position and a velocity of the target; an accuracy calculation unit that calculates accuracy of the six-dimensional state estimate value output from the state estimate calculation unit; a smoothing value calculation unit that performs a Kalman filter process on the six-dimensional state estimate value by using the accuracy calculated by the accuracy calculation unit to smooth the six-dimensional state estimate value; A target state estimation device comprising:

2. a sensor signal processing unit that acquires a sensor signal from a sensor that observes the target and calculates an orientation of the target based on the sensor signal; The direction calculation value acquisition unit is 2. The target state estimation device according to claim 1, wherein the direction calculation value is obtained from the sensor signal processing unit every time a time corresponding to the time window elapses.

3. an error setting unit that sets a measurement error variance of the azimuth calculation value acquired by the azimuth calculation value acquisition unit; a prior information setting unit that sets prior information regarding the state of the goal, The state estimation value calculation unit 2. The target state estimation device according to claim 1, further comprising: a target state estimation unit that estimates a state of the target using the azimuth calculation value acquired by the azimuth calculation value acquisition unit, the measurement error variance set by the error setting unit, and prior information set by the prior information setting unit.

4. 2. The target state estimation device according to claim 1, further comprising an abnormal value determination unit that determines whether the azimuth calculation value acquired by the azimuth calculation value acquisition unit is an abnormal value by using a state prediction value indicating a state prediction result by the smoothed value calculation unit.

5. The abnormal value determination unit The target state estimation device according to claim 4, characterized in that the state prediction value indicating the state prediction result by the smoothing value calculation unit is converted into the orientation of the target, a variance is calculated from a residual between the orientation calculation value acquired by the orientation calculation value acquisition unit and the converted orientation, and a residual covariance matrix obtained from a prediction error covariance matrix of the target, and based on the variance, it is determined whether the orientation calculation value acquired by the orientation calculation value acquisition unit is an abnormal value.

6. 2. The target state estimating device according to claim 1, further comprising a smoothing processing section for further smoothing the smoothed value calculated by said smoothed value calculation section.

7. A target state estimation method performed by an apparatus including an orientation calculation value acquisition unit, a state estimation value calculation unit, an accuracy calculation unit, and a smoothing value calculation unit, the azimuth calculation value acquisition unit acquires an azimuth calculation value indicating an azimuth angle and an elevation angle of a target every time a time corresponding to a time window elapses; the state estimate calculation unit constructs a positioning equation using the direction calculation value acquired by the direction calculation value acquisition unit, and outputs a six-dimensional state estimate including a position and a velocity of the target by solving the positioning equation by SBWIV; the accuracy calculation unit calculates accuracy of the six-dimensional state estimate value output from the state estimate calculation unit; The smoothed value calculation unit performs a Kalman filter process on the six-dimensional state estimate value by using the accuracy calculated by the accuracy calculation unit, thereby smoothing the six-dimensional state estimate value. Target state estimation method.

Images