Target state estimation device and target state estimation method
The target state estimation device addresses the complexity and instability of BO-TMA by using Kalman filter processing and smoothed value calculation to accurately estimate target states for varying motion types, enhancing estimation stability and responsiveness.
Patent Information
- Application Number
- PCT/JP2023/017349
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Filing Date
- 2023-05-09
- Publication Date
- 2024-11-14
AI Technical Summary
BO-TMA techniques face challenges in estimating target state due to nonlinear relationships between target orientation and position, leading to increased calculation complexity and instability in state estimation, especially when target motion is non-uniform linear.
A target state estimation device that acquires direction calculation values over a time window, performs Kalman filter processing, and calculates smoothed values to estimate target state accurately regardless of motion type, using a combination of direction calculation, error setting, prior information, and accuracy calculation units.
Enables accurate target state estimation for both uniform and non-uniform linear motions by reducing calculation complexity and improving estimation stability through Kalman filter processing and smoothed value calculation.
Smart Images

Figure JP2023017349_14112024_PF_FP_ABST
Abstract
Description
Target state estimation device and target state estimation method
[0001] The present disclosure relates to a target state estimation device and a target state estimation method.
[0002] BO-TMA (Bearing Only Target Motion Analysis) is a technology for estimating the state of a moving target. Examples of target states include the target's position and velocity. BO-TMA estimates the target's state using only azimuth measurements from a direction sensor that measures the target's direction at each of multiple locations. Because the target's direction and position are nonlinearly related, BO-TMA deals with a nonlinear problem. Therefore, BO-TMA requires iterative calculations when estimating the target's state. Consequently, BO-TMA faces the challenge of increasing the amount of calculation required for state estimation. Another challenge is that the stability of the solution indicating the estimated state value may be compromised. To solve the challenges arising from dealing with nonlinear problems, for example, Non-Patent Document 1 discloses a technology that uses the geometric relationship between the target's direction, target position, and the position of the direction sensor to replace the nonlinear problem with a pseudo-linear problem and then estimate the target's state.
[0003] 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.
[0004] The technology disclosed in Non-Patent Document 1 is based on the premise that the target moves in a uniform linear motion. Therefore, when the target moves in a non-uniform linear motion, the technology has a problem in that it may not be able to estimate the state of the target.
[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 uniform linear motion or not.
[0006] The target state estimation device according to the present disclosure includes an orientation calculation value acquisition unit 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 that estimates the state of the target using the orientation calculation value acquired by the orientation calculation value acquisition unit and outputs a state estimation value indicating the state estimation result; an accuracy calculation unit that calculates the accuracy of the state estimation value output from the state estimation value calculation unit; and a smoothed value calculation unit 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 using the accuracy calculated by the accuracy calculation unit and calculates a smoothed value indicating the state of the target using a state prediction value indicating the state prediction result.
[0007] According to the present disclosure, the state of a target can be estimated regardless of whether the target's motion is uniform linear motion or not.
[0008] FIG. 1 is a configuration diagram showing a target state estimation device according to embodiment 1. FIG. 2 is a hardware configuration diagram showing the hardware of the target state estimation device according to embodiment 1. 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. FIG. 4 is a flowchart showing the processing procedure of a target state estimation unit 5. FIG. 5 is a configuration diagram showing a target state estimation device according to embodiment 2. FIG. 6 is a hardware configuration diagram showing the hardware of the target state estimation device according to embodiment 2. FIG. 7 is an explanatory diagram showing the concept of determining an abnormality in a direction measurement value. FIG. 8 is a configuration diagram showing a target state estimation device according to embodiment 3. FIG. 9 is a hardware configuration diagram showing the hardware of the target state estimation device according to embodiment 3.
[0009] In order to explain 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 estimation device according to embodiment 1. Fig. 2 is a hardware configuration diagram showing hardware of the target state estimation device according to embodiment 1. 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 acquisition unit 2 and an orientation calculation unit 3. The sensor signal processing unit 1 acquires a sensor signal from a sensor observing a target, and calculates the orientation of the target based on the sensor signal. The sensor signal processing unit 1 outputs an orientation calculation value indicating the calculation result of the orientation of the target to an orientation calculation value storage unit 4.
[0012] The sensor signal acquisition unit 2 is realized by, for example, a sensor signal acquisition circuit 21 shown in Fig. 2. The sensor signal acquisition unit 2 acquires a sensor signal from a sensor that observes a target, and outputs the sensor signal to the orientation calculation unit 3.
[0013] The direction calculation unit 3 is realized by, for example, the direction calculation circuit 22 shown in Fig. 2. 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, the azimuth calculation value storage circuit 23 shown in FIG.
[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 orientation calculation value acquisition unit 11 is realized, for example, by an orientation calculation value acquisition circuit 24 shown in FIG. 2. The orientation calculation value acquisition unit 11 acquires an orientation calculation value indicating the orientation calculated by the sensor signal processing unit 1 every time a time corresponding to a time window has elapsed. That is, the orientation calculation value acquisition unit 11 acquires an orientation calculation value stored in the orientation calculation value storage unit 4 every time a time corresponding to a time window has elapsed. The orientation calculation value acquisition unit 11 outputs the orientation calculation value to the state estimation value calculation unit 14.
[0016] The error setting unit 12 is realized by, for example, the error setting circuit 25 shown in Fig. 2. The error setting unit 12 sets the measurement error variance of the calculated bearing value acquired by the calculated bearing value acquisition unit 11. The error setting unit 12 outputs the measurement error variance of the calculated bearing value to the state estimated value calculation unit 14.
[0017] The prior information setting unit 13 is realized by, for example, the prior information setting circuit 26 shown in Fig. 2. The prior information setting unit 13 sets prior information regarding the state of the target. The prior information setting unit 13 outputs the prior information to the state estimation value calculation unit 14.
[0018] The state estimate value calculation unit 14 is realized by, for example, the state estimate value calculation circuit 27 shown in Fig. 2. The state estimate value calculation unit 14 acquires the calculated bearing value from the calculated bearing value acquisition unit 11, acquires the measurement error variance of the calculated bearing value from the error setting unit 12, and acquires prior information from the prior information setting unit 13. The state estimate value calculation unit 14 estimates the state of the target using the calculated bearing value, the measurement error variance, and the prior information. The state estimate value calculation unit 14 outputs a state estimate indicating the estimation result of the state of the target 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, the accuracy calculation circuit 28 shown in Fig. 2. The accuracy calculation unit 15 acquires the state estimation value from the state estimation value calculation unit 14. The accuracy calculation unit 15 calculates the accuracy of the state estimation value and outputs the accuracy of the state estimation value to the smoothed value calculation unit 16.
[0020] The smoothed value calculation unit 16 is realized by, for example, the smoothed value calculation circuit 29 shown in FIG. 2 . The smoothed value calculation unit 16 acquires the state estimation value from the state estimation value calculation unit 14 and acquires the accuracy of the state estimation 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 estimation value using the accuracy of the state estimation value. The smoothed value calculation unit 16 calculates a smoothed value indicating the state of the target using a state prediction value indicating the prediction result of the state. The smoothed value calculation unit 16 outputs the smoothed value indicating the state of the target to the result output unit 17.
[0021] The result output unit 17 is realized by, for example, the result output circuit 30 shown in Fig. 2. The result output unit 17 acquires a smoothed value indicating the state of the target 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, namely, a sensor signal acquisition unit 2, a direction calculation unit 3, a direction calculation value storage unit 4, a direction 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, is realized by dedicated hardware as shown in Fig. 2. In other words, it is assumed that the target state estimation device is realized by a sensor signal acquisition circuit 21, a direction calculation circuit 22, a direction calculation value storage circuit 23, a direction calculation value acquisition circuit 24, an error setting circuit 25, a prior information setting circuit 26, a state estimation value calculation circuit 27, an accuracy calculation circuit 28, a smoothed value calculation circuit 29, and a result output circuit 30.
[0023] Here, the direction calculation value storage 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), or 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). Furthermore, each of the sensor signal acquisition circuit 21, the direction calculation circuit 22, 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 is, 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.
[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 a 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] 3 is a hardware configuration diagram of a computer when the target state estimation device is realized by software, firmware, etc. When the target state estimation device is realized by software, firmware, etc., the orientation calculation value storage unit 4 is configured on a memory 41 of the computer. Programs for causing the computer to execute the respective processing procedures of 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 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.
[0027] Next, the operation of the target state estimation device shown in Fig. 1 will be described. Fig. 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 that observes the target. The number of sensors that observe the target may be one or more. The sensor signal output from the sensor includes the target observation time t and sensor coordinates that indicate the position of the sensor. The sensor coordinates are, for example, a position vector r of the sensor 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 orientation 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 calculation process of the target direction based on the sensor signal is a known technique, so a detailed description will be omitted. The direction calculation unit 3 calculates the direction of the target based on the calculated direction value, the target observation time t included in the sensor signal, and the sensor position vector r included in the sensor signal. k and is output to the azimuth calculation value storage unit 4. The azimuth calculation value is the azimuth calculation value θ tilde in the azimuth angle direction. k and the calculated azimuth value in the elevation direction φ tilde k In the text of the specification, because of the electronic filing system, it is not possible to put the symbol "~" above the letters, so the θ tilde k , φ tilde k The calculated azimuth value is expressed as follows: k and the target position vector p k As a result, the calculated azimuth value, the target observation time t, and the sensor position vector r are stored in the azimuth calculation value storage unit 4. k The following is a set of the calculated azimuth value, the target observation time t, and the sensor position vector r k The set of these is called the orientation data set DS.
[0029] The bearing calculation value acquisition unit 11 waits until the time corresponding to the time window has elapsed (if NO in step ST1 of FIG. 4). Every time the time corresponding to the time window has elapsed (if YES in step ST1 of FIG. 4), the bearing calculation value acquisition unit 11 acquires the bearing data set DS from the bearing calculation value storage unit 4 (if YES in step ST1 of FIG. 4). Specifically, the time corresponding to the time window is tw, and the observation time t included in the bearing data set DS previously acquired by the bearing calculation value acquisition unit 11 from the bearing calculation value storage unit 4 is t 1 In this case, the direction calculation value acquisition unit 11 selects the direction data set stored in the direction calculation value storage unit 4 whose observation time t is t 1Among the observation times after +tw, the bearing data set DS with the first observation time t is acquired. The time tw corresponding to the time window is set according to the motion of the target. That is, even if the target motion is non-uniform linear motion, the target motion can be considered to be uniform motion if the time tw corresponding to the time window is sufficiently short compared to the time required for the target's velocity to change. Also, even if the target motion is non-uniform linear motion, the target motion can be considered to be linear motion if the time tw corresponding to the time window is sufficiently short compared to the time required for the target's direction of travel to change. Therefore, the time tw corresponding to the time window is set to a time that allows the target motion to be considered to be uniform linear motion even if the target motion is non-uniform linear motion. Note that the time tw corresponding to the time window may change over time. Also, multiple time windows adjacent in time may partially overlap each other. The bearing calculation value acquisition unit 11 outputs the bearing data set DS acquired from the bearing calculation value storage unit 4 to the state estimation value calculation unit 14.
[0030] The error setting unit 12 sets the measurement error variance of the bearing calculation value included in the bearing data set DS acquired by the bearing calculation value acquisition unit 11 (step ST3 in FIG. 4 ). The method for setting the measurement error variance by the error setting unit 12 may be any method, but if some additional accuracy information is assigned to the bearing 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 bearing calculation value, a larger measurement error variance may be set as the accuracy of the bearing calculation value decreases. The error setting unit 12 outputs the measurement error variance of the bearing calculation value to the state estimation value calculation unit 14.
[0031] The prior information setting unit 13 sets prior information regarding the state of the target (step ST4 in FIG. 4 ). Specifically, the prior information setting unit 13 sets, as the prior information regarding the state of the target, for example, an expected value of the prior probability regarding the state of the target and a covariance matrix of the prior probability regarding the state of the target. The method for setting the prior information set by the prior information setting unit 13 may be any method. For example, the prior information may be set by using the predictive distribution of the Kalman filter used by the smoothing value calculation unit 16 as the prior distribution. If it is difficult for the prior information setting unit 13 to set prior information regarding the state of the target, the influence of the prior information may be made negligible by setting a sufficiently large value for the covariance matrix of the prior probability. The prior information setting unit 13 outputs the prior information to the state estimation value calculation unit 14.
[0032] The state estimation value calculation unit 14 acquires the orientation data set DS from the orientation calculation value acquisition unit 11, acquires the measurement error variance from the error setting unit 12, and acquires the 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, an SBWIV (Short-time Bayesian Weighted Instrumental Variable) estimated value. The state estimation value calculation unit 14 outputs the state estimation 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] The target state estimation process performed by the state estimation value calculation unit 14 will be specifically described below. Details of the target state estimation process are disclosed in Non-Patent Document 1. First, the state estimation value calculation unit 14 formulates a positioning equation such as that shown in the following equation (1), which is a positioning equation that pseudo-linearly expresses the relationship between the target orientation and target position.
[0034] In equation (1), T is a mathematical symbol indicating transposition.
[0035] The state estimate calculation unit 14 calculates an estimator called BWPLE (Bayesian Weighted Pseudo-Linear Estimator) by performing MAP (Maximum A Posteriori) 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 pseudo-linearization included in BWPLE. Finally, the state estimate calculation unit 14 calculates an SBWIV estimate, which is a bias-corrected state estimate, by applying an instrumental variable method using a bearing estimate calculated back from BBCPLE. When the target's motion 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 considered to be uniform linear motion, the error in the SBWIV estimated value can be made small enough to be practically unavoidable. 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 the above, the target state estimate value x 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] The accuracy calculation process of the SBWIV estimated value by the accuracy calculation unit 15 will be specifically described below. The ideal accuracy of the SBWIV estimated value asymptotically approaches the BCRB (Bayesian Cramer-Rao Bound). Therefore, the accuracy calculation unit 15 calculates the BCRB at the start time of the time window as shown in the following equation (2).
[0038]
[0039] In 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] In formula (5), M k is the time difference Δt from the start time of the time window to the kth observation time k is a transformation matrix expressed by the following equation (8).
[0041]
[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 the electronic filing system. BCRM,m,0 It is written as follows.
[0043]
[0044] The BCRB corresponding to the representative time of the m-th time window is expressed as in the following equation (10). BCRB is the theoretical lower limit of accuracy. Therefore, the accuracy calculation unit 15 calculates R hat by using any real number that is one or more adjustment parameters. BCRM,m The error covariance matrix R of the SBWIV estimate is m BWIV The error covariance matrix R of the SBWIV estimate is set to m BWIVcorresponds to the accuracy of the SBWIV estimated value. Note that the representative time of the m-th time window may be, for example, the average value of the observation times associated with the direction measurement values included in the m-th time window, or may be a weighted average value of the observation times when there is variation in the errors of the direction measurement values.
[0045]
[0046] The smoothed 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, which is the accuracy of the state estimated value, from the accuracy calculation unit 15. m BWIV The smoothed value calculation unit 16 obtains the error covariance matrix R m BWIV The smoothed value calculation unit 16 then calculates a smoothed value indicating the state of the target using the state prediction value indicating the prediction result of the state of the target (step ST7 in FIG. 4).
[0047] The following is a specific description of the calculation process of the smoothed value by the smoothed value calculation unit 16. The motion model used in the Kalman filter process is expressed as in the following equation (11).
[0048] In formula (11), x m is a state vector that lists the three-dimensional position and three-dimensional velocity of the target, and w m is the driving noise.
[0049] The target observation model is expressed as in the following equation (13).
[0050]
[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 with six dimensions of position and velocity. m is the error in the SBWIV estimate, not the observation error.
[0052] The smoothed value calculation unit 16 performs Kalman filtering on the SBWIV estimated value based on the above model. When m=1, the SBWIV estimated value x hat is calculated as shown in the following equation (14): 1 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 1 BWIV is the smoothing error covariance matrix P 1|1 This becomes:
[0053]
[0054] When m>1, the prediction process of the smoothed value calculation unit 16 is expressed by the following equations (16) and (17).
[0055] In formula (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 as shown in the following equations (18) and (19). m|m-1 and the prediction error covariance matrix P m|m-1 By performing smoothing processing using the above, the target state estimate value x m|m Furthermore, the smoothed value calculation unit 16 calculates the prediction error covariance matrix P m|m-1 By performing a smoothing process using m|m The smoothed value calculation unit 16 calculates the target state estimate x m|m is output to the result output unit 17.
[0057]
[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 that indicates the predicted state. Therefore, the target state estimation unit 5 can smooth the variation in the SBWIV estimated value that occurs due to a decrease in the number of observed sensor signals, and can also perform highly accurate state estimation even when the target performs non-uniform linear motion. Furthermore, since the target state 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 The result output unit 17 obtains the target state estimate x m|m is output to, for example, a radar device (not shown) or a display device (not shown). m|m By obtaining the above, the trajectory of the target state can be obtained.
[0060] If the target state estimation unit 5 continues the target state estimation process (step ST8 in FIG. 4: YES), it again performs the processes of steps ST1 to ST8 in FIG. 4. If the target state estimation unit 5 does not continue the target state estimation process (step ST8 in FIG. 4: NO), it ends the series of processes.
[0061] In the first embodiment described above, 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 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 state estimation result, 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 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 the state prediction value indicating the state prediction result. Therefore, the target state estimation device can estimate the state of the target regardless of whether the motion of the target is uniform linear motion or not.
[0062] Second Embodiment In a second embodiment, a target state estimation device will be described that includes an abnormal value determination unit 18 that determines whether or not the azimuth calculation value acquired by the azimuth calculation value acquisition unit 11 is an abnormal value, using a state prediction value that indicates the state prediction result by the smoothed value calculation unit 16.
[0063] Fig. 5 is a configuration diagram showing a target state estimation 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 estimation 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] The abnormal value determination unit 18 is realized, for example, by the abnormal value determination circuit 31 shown in FIG. 6 . The abnormal value determination unit 18 includes a state prediction 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 the state prediction result from the smoothed value calculation unit 16, and acquires a calculated heading value from the heading calculation value acquisition unit 11. The abnormal value determination unit 18 converts the state prediction value into a target heading and calculates a residual between the calculated heading value and the converted heading. The abnormal value determination unit 18 calculates a variance from the residual between the calculated heading value and the converted heading and a residual covariance matrix calculated from the prediction error covariance matrix of the target. The abnormal value determination unit 18 determines whether the calculated heading value acquired by the heading calculation value acquisition unit 11 is an abnormal value based on the variance.
[0065] The state predicted value acquisition unit 18a acquires a state predicted value indicating a state prediction result and a prediction error covariance matrix from the smoothed value calculation unit 16. 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 obtains the state predicted value and the prediction error covariance matrix from the state predicted value acquisition unit 18a, and obtains the calculated heading value from the heading calculation value acquisition unit 11. The prediction gate determination unit 18b converts the state predicted value into a target heading. The prediction gate determination unit 18b calculates the residual between the calculated heading value and the converted heading. The prediction gate determination unit 18b calculates a residual covariance matrix from the prediction error covariance matrix. The prediction gate determination unit 18b calculates a variance from the residual and the residual covariance matrix. The prediction gate determination unit 18b determines whether the calculated heading value acquired by the heading calculation value acquisition unit 11 is an abnormal value based on the variance.
[0067] When the prediction gate determination unit 18b determines that the calculated bearing value is abnormal, the reliability setting unit 18c replaces the value of the measurement error variance corresponding to the calculated bearing value with a larger value to reduce the influence of the abnormal measured bearing value on the state estimation. The measurement error variance corresponding to the calculated bearing value is the measurement error variance set by the error setting unit 12.
[0068] 5, it is assumed that the components of the target state estimation device, namely, 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, are each realized by dedicated hardware as shown in Fig. 6. In other words, it is assumed that the target state estimation device is realized by a sensor signal acquisition circuit 21, a direction calculation circuit 22, a direction calculation value storage circuit 23, a direction calculation value acquisition circuit 24, an error setting circuit 25, a prior information setting circuit 26, a state estimation value calculation circuit 27, an accuracy calculation circuit 28, a smoothed value calculation circuit 29, a result output circuit 30, and an abnormal value determination circuit 31.
[0069] Each of the sensor signal acquisition circuit 21, the direction calculation circuit 22, 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 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 of 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] 6 shows an example in which each of the components of the target state estimation device is realized by dedicated hardware, while 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 the same as that shown in Fig. 1 except for the abnormal value determining unit 18, only the operation of the 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 The predicted state value x is obtained. m|m-1 is x k|m-1 The prediction error covariance matrix P can be expressed as shown in the following equation (21). m|m-1 Is, P k|m-1 The state predicted value acquisition unit 18a can be expressed as shown in the following equation (22). k|m-1 and the prediction error covariance matrix P k|m-1 and is output to the prediction gate determination unit 18b.
[0074] In equation (23), t k is the observation time associated with the azimuth measurement value of the abnormality judgment target.
[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 the calculated azimuth value from the calculated azimuth value acquisition unit 11. The calculated azimuth value is the calculated azimuth value θ tilde in the azimuth angle direction. k and the calculated azimuth value in the elevation direction φ tilde k The prediction gate determination unit 18b calculates the state prediction value x k|m-1 is the target's azimuth, and the azimuth angle θ (x k|m-1 ) and elevation angle φ(x k|m-1) and convert it into the predicted state value x k|m-1 The process of converting the above into the target's azimuth and the like is well known, and therefore 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 Furthermore, 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] In formula (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 as shown in the following equation (26). k and the residual covariance matrix R s Dispersion L 2 Calculate.
[0079]
[0080] The prediction gate determination unit 18b determines the variance L 2 Compare with the threshold value, and the variance L 2 Based on the comparison result between the variance L and the threshold value, it is determined whether the calculated azimuth value is an abnormal value. 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. 5. Specifically, the prediction gate determination unit 18b determines whether the variance L 2 If is greater than the threshold, the calculated direction value is determined to be an abnormal value, and the variance L 2 If the calculated azimuth value is equal to or less than the threshold value, the prediction gate determining unit 18b determines that the calculated azimuth value is not an abnormal value. The prediction gate determining unit 18b outputs a determination result indicating whether the calculated azimuth value is an abnormal value to the reliability setting unit 18c.
[0081] 7 is an explanatory diagram showing the concept of determining an abnormality in the azimuth measurement value. In FIG. 7, the determination gate is m|m-1 For example, the calculated azimuth angle θ tilde is set to surround the predicted position of the target included in kIf 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 direction value θ tilde k is determined to be an abnormal value. The size of the determination gate is set, for example, in accordance with the expected azimuth measurement environment.
[0082] The reliability setting unit 18c acquires a determination result indicating whether the calculated heading value is an abnormal value from the prediction gate determination unit 18b. When the reliability setting unit 18c determines that the calculated heading value is an abnormal value, the reliability setting unit 18c replaces the value of the measurement error variance corresponding to the abnormal calculated heading value with a larger value. By replacing the value of the measurement error variance corresponding to the calculated heading value with a larger value, the influence of the abnormal measured heading 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 comparison result between the variance L and the threshold value, it is determined whether the calculated azimuth value is an abnormal value. 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 calculated direction value depending on the degree of abnormality.
[0084] In the second embodiment described above, the target state estimation device shown in Fig. 5 is configured so 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 orientation, calculates a variance from the residual between the orientation calculation value acquired by the orientation calculation value acquisition unit 11 and the converted orientation, and a residual covariance matrix obtained from the target's prediction error covariance matrix, and determines whether the orientation calculation value acquired by the orientation 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 is moving in a uniform linear motion or not, and can suppress deterioration of the state estimation accuracy due to an abnormal orientation calculation value.
[0085] Third Embodiment In a third embodiment, a target state estimating device including a smoothing processing unit 19 that further smooths 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 estimation device according to embodiment 3. In Fig. 8, the same reference numerals as those in Figs. 1 and 5 indicate the same or corresponding parts, and therefore detailed description thereof will be omitted. Fig. 9 is a hardware configuration diagram showing hardware of a target state estimation device according to embodiment 3. In Fig. 9, the same reference numerals as those in Figs. 2 and 6 indicate the same or corresponding parts, and therefore 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. 9. The smoothing processing unit 19 acquires the smoothed value from the smoothed value calculation unit 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, namely, 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 smoothing processing unit 19, and the result output unit 17, is realized by dedicated hardware as shown in Fig. 9. In other words, it is assumed that the target state estimation device is realized by a sensor signal acquisition circuit 21, a direction calculation circuit 22, a direction calculation value storage circuit 23, a direction calculation value acquisition circuit 24, an error setting circuit 25, a prior information setting circuit 26, a state estimation value calculation circuit 27, an accuracy calculation circuit 28, a smoothed value calculation circuit 29, a smoothing processing circuit 32, and a result output circuit 30.
[0090] Each of the sensor signal acquisition circuit 21, the direction calculation circuit 22, 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 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 of 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] 9 shows an example in which each of the components of the target state estimation device is realized by dedicated hardware, while 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 the same as 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 The smoothing processing unit 19 also obtains the target state estimation value x m|m and the smoothing error covariance matrix P m|m and get.
[0095] The smoothing processing unit 19 calculates a smoother gain S used in the smoothing process as shown in the following equation (27): m-1 The smoothing processor 19 sets the 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 value x RTS,m-1 is output to the result output unit 17. In equation (28), x m-1|m-1 is x m|m The smoothing processor 19 calculates the smoother gain S m-1 Using the smoothed error covariance matrix P m|m The smoothing process is performed to obtain the smoothed error covariance matrix P RTS,m-1 is output to the result output unit 17. In equation (28), P m-1|m-1 Is, P m|mis the smoothed error covariance matrix at the time immediately before
[0096]
[0097] In the third embodiment described above, the target state estimation device shown in Fig. 8 is configured to include smoothing processing unit 19 that further smooths the smoothed value calculated by 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 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 in state estimation.
[0098] In addition, the present disclosure allows for free combination of the respective embodiments, modification of any of the components of the respective embodiments, or omission of any of the components of the respective embodiments.
[0099] The present disclosure is suitable for a target state estimation device and a target state estimation method.
[0100] 1 Sensor signal processing unit, 2 Sensor signal acquisition unit, 3 Orientation calculation unit, 4 Orientation calculation value storage unit, 5 Target state estimation unit, 11 Orientation 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 Orientation calculation circuit, 23 Orientation calculation value storage circuit, 24 Orientation 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. A target state estimation device comprising: an orientation calculation value acquisition unit that acquires an orientation calculation value indicating the orientation of a target each time a time corresponding to a time window elapses; a state estimation value calculation unit that estimates a state of the target using the orientation calculation value acquired by the orientation calculation value acquisition unit, and outputs a state estimation value indicating a result of the estimation of the state; an accuracy calculation unit that calculates the accuracy of the state estimation value output from the state estimation value calculation unit; and a smoothing value calculation unit 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 using the accuracy calculated by the accuracy calculation unit, 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.
2. A target state estimation device as described in claim 1, further comprising a sensor signal processing unit that acquires a sensor signal from a sensor observing the target and calculates the azimuth of the target based on the sensor signal, wherein the azimuth calculation value acquisition unit acquires the azimuth calculation value from the sensor signal processing unit each time a time corresponding to the time window has elapsed.
3. A target state estimation device as described in claim 1, further comprising an error setting unit which sets a measurement error variance of the azimuth calculation value acquired by the azimuth calculation value acquisition unit, and a prior information setting unit which sets prior information regarding the state of the target, wherein the state estimation value calculation unit estimates the 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 the prior information set by the prior information setting unit.
4. The target state estimation device as described in claim 1, further comprising an abnormal value determination unit which determines whether the azimuth calculation value acquired by the azimuth calculation value acquisition unit is an abnormal value or not, using a state prediction value indicating the state prediction result by the smoothed value calculation unit.
5. The target state estimation device described in claim 4, characterized in that the abnormal value determination unit converts a state prediction value indicating the state prediction result by the smoothing value calculation unit into the orientation of the target, calculates a variance from the 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 the prediction error covariance matrix of the target, and determines whether the orientation calculation value acquired by the orientation calculation value acquisition unit is an abnormal value based on the variance.
6. 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, in which an orientation calculation value acquisition unit 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 estimates a state of the target using the orientation calculation value acquired by the orientation calculation value acquisition unit and outputs a state estimation value indicating a result of the estimation of the state; an accuracy calculation unit calculates the accuracy of the state estimation value output from the state estimation value calculation unit; and a smoothed value calculation unit predicts the state of the target by performing Kalman filter processing on the state estimation value output from the state estimation value calculation unit using the accuracy calculated by the accuracy calculation unit, 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.
Citation Information
Patent Citations
Single-station passive positioning method and device based on extended Kalman filtering
CN110471029A
Antenna controller
JP1992350582A
Method and system for tracking multiple targets
JP1998142325A
Electronic device, arrival angle estimation system, and signal processing method
JP2020071123A
Position orientation system and position orientation method
JP2021101193A
