Target trajectory estimation device and target trajectory estimation method

Abstract

The target trajectory estimation device includes an orientation information reading unit (41) that reads orientation information indicating orientation measurement values ​​obtained by each of a plurality of orientation sensors (10) that receive an incoming radio wave from an object target (100), and orientation measurement error information indicating orientation measurement errors that accompany the orientation measurement values ​​indicated by the orientation information or that are uniformly set; a time domain division unit (43) that divides the time during which each of the plurality of orientation sensors (10) observed the object target (100) into a plurality of time domains in chronological order, and divides the orientation information read by the orientation information reading unit (41) into each of the divided time domains; and a time domain division unit (43) that divides the orientation information into a plurality of time domains based on whether the positions indicated by the orientation measurement values ​​indicated by the orientation information present in each time domain are concentrated at one point or concentrated around a state vector consisting of position and velocity. the direction measurement unit (44) for determining whether a direction measurement value indicates an abnormal value, resetting the direction measurement error associated with the direction measurement value determined to be an abnormal value to a setting that reduces the reliability of the direction measurement value for the trajectory estimation, and resetting the direction measurement error associated with the direction measurement value determined to be not an abnormal value while leaving the reliability of the direction measurement value for the trajectory estimation unchanged; a direction measurement processing unit (45) for estimating a direction measurement value in each time domain for the object target (100) using the direction measurement value indicated by the direction information present in each time domain and the reset direction measurement error associated with the direction measurement value, for each time domain divided by the time domain dividing unit (43); and an output unit (46) for connecting the direction measurement values ​​for each time domain estimated by the direction measurement processing unit (45) in a time series and outputting object trajectory information indicating the trajectory of the object target (100).

Description

[Technical field]

[0001] The present disclosure relates to a target trajectory estimation device and a target trajectory estimation method that measure measured positions of a target. [Background technology]

[0002] In a target trajectory estimation device that estimates the trajectory of a target such as an airplane based on orientation measurement values ​​obtained from multiple active sensors such as radar sensors or passive sensors such as radio wave sensors, there is a technique for calculating a weighted positioning value by setting an orientation measurement error based on the reliability of each of the multiple orientation measurement values. For example, there is a method for estimating the position and velocity of a target, and their accuracy, while reflecting the difference in the time of orientation measurement by MAP (Maximum A Posteriori) estimation, based on different orientation measurement errors of multiple asynchronous sensors.

[0003] Furthermore, for example, Patent Document 1 discloses a method for determining an abnormal value during a positioning process for positioning a target in a target trajectory estimation device. The target trajectory estimation device disclosed in Patent Document 1 calculates a dilution of precision (DOP), which is an index indicating the positioning accuracy for each sensor group selected from a plurality of sensors, and sets sensors to be preferentially used for positioning processing. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2016-61705 A Summary of the Invention [Problem to be solved by the invention]

[0005] The target tracking device disclosed in Patent Document 1 has the property that the DOP decreases with deterioration of positioning accuracy when multiple sensors are positioned to indicate the same orientation relative to the target, and therefore can perform good abnormal value detection during positioning processing. However, if a particular sensor in a sensor group is in a multipath environment, it may contain many abnormal values, which may result in erroneous positioning processing.

[0006] The present disclosure has been made in consideration of the above-mentioned points, and aims to provide a target trajectory estimation device that can estimate a target positioning value in a multipath environment with high accuracy, even if a specific sensor is in a multipath environment, and has improved object trajectory estimation. [Means for solving the problem]

[0007] The target trajectory estimation device according to the present disclosure includes: direction information indicating direction measurement values ​​obtained by a plurality of direction sensors that receive radio waves arriving from a target; and information associated with the direction measurement values ​​indicated by the direction information. Orientation A direction information reading unit that reads measurement error information, a time domain division unit that divides the time when each of the plurality of direction sensors observed the target into a plurality of time domains in chronological order and divides the direction information read by the direction information reading unit into each of the divided time domains, and a direction measurement value indicated by the direction information present in each time domain for each of the time domains divided by the time domain division unit. Different The apparatus includes an abnormal value determination unit that determines whether an orientation measurement value indicates a normal value, resets the orientation measurement error associated with an orientation measurement value determined to be an abnormal value to a setting that reduces the reliability of the orientation measurement value for the trajectory estimation, and resets the orientation measurement error associated with an orientation measurement value determined to be not an abnormal value while keeping the reliability of the orientation measurement value for the trajectory estimation unchanged; a positioning processing unit that estimates a positioning value in each time domain for the target using the orientation measurement value indicated by the orientation information present in each time domain and the reset orientation measurement error associated with the orientation measurement value, for each time domain divided by the time domain division unit; and an output unit that connects the positioning values ​​for each time domain estimated by the positioning processing unit in a time series and outputs object trajectory information indicating the trajectory of the target. Effect of the Invention

[0008] According to the present disclosure, even if a specific orientation sensor is in a multipath environment, it is possible to estimate the target's position measurement value with high accuracy, and to perform trajectory estimation that is closer to the target's actual trajectory (true value). [Brief description of the drawings]

[0009] [Figure 1] 1 is a block diagram showing a schematic configuration of a target trajectory estimation system including a target trajectory estimation device according to a first embodiment. [Diagram 2] 4 is a diagram showing azimuth measurement values ​​of a plurality of azimuth sensors divided in the time domain in the target trajectory estimation device according to the embodiment 1. FIG. [Diagram 3] 2 is a diagram spatially illustrating azimuth measurement values ​​of a plurality of azimuth sensors in the target trajectory estimation device according to the embodiment 1. FIG. [Figure 4] 4 is a diagram showing normal values ​​and abnormal values ​​in azimuth measurement values ​​that are targets of positioning gate determination in the target trajectory estimation device according to the first embodiment. FIG. [Diagram 5] 4 is a diagram showing an abnormal value determination method for determining a positioning gate and a gate to be used in the target trajectory estimation device according to the first embodiment. FIG. [Figure 6] 5 is a flowchart showing an example of an abnormal value determination procedure in the target trajectory estimation device according to the first embodiment. [Figure 7] 1 is a functional block diagram illustrating an outline of an example of a hardware configuration of a target trajectory estimation device according to a first embodiment. [Figure 8] FIG. 11 is a block diagram showing a schematic configuration of a target trajectory estimation device according to a second embodiment. [Figure 9] 13 is a flowchart showing an example of a procedure from abnormal value determination using a time-forward Kalman filter to a tracking process in the target trajectory estimation device according to the second embodiment. [Figure 10] 11 is a flowchart showing an example of a procedure from abnormal value determination using a time-backward Kalman filter to tracking processing in the target trajectory estimation device in the embodiment 2. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0010] Embodiment 1 A target trajectory estimation device according to a first embodiment will be described with reference to FIGS. FIG. 1 is a block diagram showing a schematic configuration of a target trajectory estimation system including a target trajectory estimation device according to the first embodiment. As shown in FIG. 1, the target trajectory estimation system includes a plurality of orientation sensors 10 1 ~10 N and a plurality of azimuth measuring units 20 1 ~20 N The system further comprises a storage device 30, a target trajectory estimation device 40, and an output device 50.

[0011] The target trajectory estimation device 40 according to the first embodiment includes a plurality of orientation sensors 10. 1 ~10 N using only the orientation measurements, for example to measure the position and estimate the trajectory of a target, such as an airplane, without acquiring range data. When there is no need to explain the suffix N given to the direction sensor 10 and the direction measurement unit 20 individually, the following explanation will be given without the suffix in order to avoid complication of explanation.

[0012] A plurality of (N) orientation sensors 10 are arranged in series to receive signals from a target 100 such as an airplane, and obtain orientation information therefrom. Each of the orientation sensors 10 is a passive sensor such as a radio wave sensor that receives radio waves transmitted from the target 100 . Each of the multiple orientation sensors 10 has the same configuration.

[0013] Each azimuth sensor 10 may be an active sensor such as a radar sensor that outputs a signal and receives the signal that hits the target 100 and is reflected back. Furthermore, each orientation sensor 10 may be a sensor capable of sensing and receiving a physical quantity such as sound waves or heat emitted from the target 100 and estimating the direction of the target 100 . The measurement error of each orientation sensor 10 may differ from one orientation sensor 10 to another, and may also differ depending on the measurement time.

[0014] Each of the multiple (N) direction measurement units 20 measures a direction based on a signal received by the corresponding direction sensor 10, and outputs the measurement result as direction information indicating the direction measurement value. Each of the plurality of azimuth measuring units 20 analyzes the corresponding received signal from each of the N connected azimuth sensors 10, and calculates the arrival azimuth (θ k , φ k ) is measured as a directional measurement. Each of the plurality of azimuth measurement units 20 also stores the measurement time t k and the corresponding position information of the orientation sensor 10 (x r k , y r k , z r k ) into the azimuth measurement (θ k , φ k ) and output as azimuth information indicating the azimuth measurement value. The azimuth information is digital information.

[0015] Note that k is the azimuth measurement number and the measurement time t k It is an integer between 1 and K (>1) that specifies the The azimuth measurement value number k is a number given in chronological order to all asynchronous azimuth measurement values ​​while a plurality (N pieces) of azimuth sensors 10 observe the target 100, and there are K such numbers. In the position information, x, y, and z indicate the x-coordinate, the y-coordinate, and the z-coordinate, and r is an integer from 1 to N for identifying the direction sensor 10.

[0016] Each of the plurality of direction measurement units 20 calculates a direction measurement error (σ) in a non-multipath environment analyzed from the antenna configuration and SNR (signal-to-noise ratio) of the corresponding direction sensor 10. θ,k , σ φ,k ) is output as the azimuth measurement error information. θ,k , σ φ,k ) is the corresponding orientation measurement (θ k , φ k) The heading measurement error information is digital information.

[0017] That is, while each of the multiple orientation sensors 10 is observing the target 100, each of the multiple orientation sensors 10 receives K incoming radio waves from the target 100 in chronological order, and the multiple orientation measurement units 20 obtain K orientation measurement values ​​(θ k , φ k ) and orientation measurement error (σ θ,k , σ φ,k ) is obtained.

[0018] The storage device 30 stores the direction information from each of the multiple (N) direction measurement units 20 in a storage unit 31 . The storage device 30 stores the direction measurement values ​​(θ k , φ k ) and the azimuth measurement error (σ θ,k , σ φ,k ) is stored in the storage unit 310, and the direction information and direction measurement error information stored at the time of estimating the target trajectory are output.

[0019] The target trajectory estimation device 40 calculates the azimuth measurement value (θ k , φ k ) and the reset azimuth measurement error (σ θ,k , σ φ,k ) and the position information of the orientation sensor 10 (x r k , y r k , z r k ) and representative time t m Based on this, the trajectories of 100 targets, such as airplanes, are estimated. The target trajectory estimation device 40 is a device that has a high probability of being able to estimate the trajectory of the target 100 with high accuracy, even in an environment in which some of the multiple orientation sensors 10 do not indicate the orientation of the target 100, i.e., a so-called multipath environment.

[0020] The target trajectory estimation device 40 calculates the azimuth measurement value (θ k , φ k ) and orientation measurement error (σ θ,k , σ φ,k ) and the position information of the orientation sensor 10 (x r k , y r k , z r k ) and measurement time t k The time during which each of the multiple orientation sensors 10 observed the target 100 is divided into multiple time domains m in chronological order, and a representative time t m Position measurement value x m Calculate the representative time t m Position measurement value x m The trajectory of the target 100 is estimated by connecting the above, and output as object trajectory information.

[0021] The target trajectory estimation device 40 performs recursive processing to estimate a trajectory based on the azimuth measurement values ​​in a certain time domain while each of the multiple orientation sensors 10 observes the target 100, thereby obtaining object trajectory information. The output device 50 displays the estimated trajectory of the target 100 obtained from the object trajectory information from the target trajectory estimation device 40 .

[0022] The target trajectory estimation device 40 includes a direction information reading unit 41, an error setting unit 42, a time domain dividing unit 43, an abnormal value determining unit 44, a positioning processing unit 45, and an output unit 46. The direction information reading unit 41 reads the direction measurement values ​​(θ k , φ k ) and the orientation measurement error (σ θ,k , σ φ,k ) indicating the direction measurement error information (σ θ,k , σ φ,k ) to load the The direction information reading unit 41 also reads the direction measurement value (θ k , φ k ) and the position information (x rk , y r k , z r k ) and measurement time t k Load.

[0023] Direction measurement error (σ θ,k , σ φ,k When the plurality of azimuth measurement units 20 do not obtain azimuth measurement error information indicating azimuth measurement error (σ θ,k , σ φ,k ) is set uniformly. The azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is read into the direction information reading unit 41. Each of the multiple direction measurement units 20 has a direction measurement error (σ θ,k , σ φ,k ), each of the plurality of azimuth measurement units 20 functions as an error setting unit 42.

[0024] The direction information reading unit 41 reads the direction measurement values ​​(θ k , φ k ) and the orientation measurement value (θ k , φ k ) and the associated measurement time t k and the position information of each orientation sensor 10 (x r k , y r k , z r k ) is read. At this time, the azimuth measurement error (σ θ,k , σ φ,k), or the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) will also be loaded.

[0025] The time domain division unit 43 divides the azimuth measurement values ​​(θ k , φ k The direction information indicating the time period from the start of a flight to the end of the flight is divided into a set time domain. The set time domain is a short-term time domain. The time domain division unit 43 divides the time during which each of the multiple orientation sensors 10 observes the target 100 into multiple time domains in chronological order, and divides the orientation information read by the orientation information reading unit 41 into each of the divided time domains.

[0026] The time domain division unit 43 divides the measurement time t k Based on the radio waves received by the multiple direction sensors 10 from the target 100, the direction information reading unit 41 reads in the direction measurement value (θ k , φ k ) and the orientation measurement (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of each orientation sensor 10 (x r k , y r k , z r k ) is divided into M pieces.

[0027] Each of the M divided time domains is called m, and the representative time of each time domain m is called t m Let us assume that. In addition, in the time domain m, among the direction measurement value numbers k that are assigned in time series to the direction information and direction measurement error information for the radio wave from the target 100 received by each direction sensor 10, the first direction measurement value number is k m,s , the last orientation measurement number is k m,eLet us assume that.

[0028] For example, the orientation sensor 10 1 While observing target 100, K azimuth measurements (θ k , φ k ) into M pieces of direction information at equal time intervals in chronological order, and k is applied to the first of the m-th pieces of divided direction information. m,s The direction measurement value number is assigned to the last direction information, and k m,e This means that the azimuth measurement value is numbered as follows: Orientation Sensor 10 1 Location information (x 1 k , y 1 k , z 1 k ) and orientation measurement error (σ θ,k , σ φ,k The azimuth measurement error information indicating the azimuth measurement value (θ k , φ k ) is divided into M equal intervals in chronological order, the corresponding orientation measurements (θ k , φ k )

[0029] Now, an example in which the time domain dividing unit 43 divides the orientation information indicating the orientation measurement values ​​into M time domains m at equal intervals by the time interval W1 in chronological order in the case where there are four orientation sensors 10 will be described with reference to FIG. In FIG. 2, θ k (s1) is the first orientation sensor 10 1 The measured direction θ of the direction information received by the radio wave k , θ k (s4) is the fourth orientation sensor 10 4 The measured direction θ of the direction information received by the radio wave k , and 1 to M indicate the first time domain to the Mth time domain.

[0030] Also, typically, k 1,s is the first orientation sensor 10 in the time domain 1 1 The initial azimuth measurement value θ of the azimuth information received byk , k 1,e is the fourth orientation sensor 10 in the time domain 1 4 The measured direction θ of the last direction information received by the radio wave k , k M,s is the fourth orientation sensor 10 in the time domain M 4 The initial azimuth information azimuth measurement value θ k , k M,e is the fourth orientation sensor 10 in the time domain M 4 The measured direction θ of the last direction information received by the radio wave k This shows that.

[0031] In this way, the time domain division unit 43 calculates the azimuth measurement value (θ k , φ k ) is divided into M time domains m at equal intervals by the time interval W1 in chronological order, and K orientation measurement values ​​(θ k , φ k ) is divided into time series for each time domain m. In addition, in one time domain m for one orientation sensor 10, multiple orientation measurement values ​​(θ k , φ k ) may also be present, and an orientation measurement (θ k , φ k ) is not present or only one orientation measurement (θ k , φ k ) may exist in a time domain m.

[0032] The abnormal value determination unit 44 calculates the azimuth measurement value (θ k , φ k ) for each time domain m, k , φ k ) abnormality is judged, a reliability is set based on the degree of the judged abnormality, and reliability information is obtained. The abnormal value determination unit 44 detects the azimuth measurement values ​​(θ k , φ k) is judged to be an abnormal value, and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is reset, and the positioning process calculates the positioning value. k , φ k ) and reduce the reliability of the azimuth measurement value (θ k , φ k ) is processed to reduce the reliability of the trajectory estimation.

[0033] On the other hand, the abnormal value determination unit 44 determines the azimuth measurement value (θ k , φ k ) is not an abnormal value. k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is left as it is, and the azimuth measurement value (θ k , φ k The reliability of the direction measurement value (θ k , φ k ) is left as it is. The positioning process for calculating the position value may be a commonly known process.

[0034] The abnormal value determination unit 44 detects the azimuth measurement value (θ k , φ k ) indicates the azimuth of the target 100, and the azimuth measurement value (θ k , φ k ) does not indicate the direction of the target 100, the direction measurement (θ k , φ k ) is judged as an abnormal value, and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) was reset, and the result was that the azimuth measurement value (θ k , φ k) is subjected to a process of reducing the reliability of the trajectory estimation. The abnormal value determination unit 44 determines the azimuth measurement value (θ k , φ k ) is not an abnormal value. k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is left as it is, and the azimuth measurement value (θ k , φ k ) the reliability of the trajectory estimation remains the same.

[0035] In other words, the abnormal value determination unit 44 determines the azimuth measurement value (θ k , φ k ) is judged to be an abnormal value, and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k ) to lower the reliability of the trajectory estimation, and the orientation measurement value (θ k , φ k The azimuth measurement error associated with the azimuth measurement value (θ k , φ k ) is reset to the same reliability for the trajectory estimation.

[0036] The positioning processing unit 45 calculates the azimuth measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) and the azimuth measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of the orientation sensor 10 (x r k , y r k , z r k) to obtain the position measurement value for the target 100 in the time domain m, and obtain target position information indicating the position measurement value of the target 100 in the time domain m.

[0037] The positioning processing unit 45 calculates k which indicates the first azimuth measurement value number existing in the time domain m for each time domain m divided by the time domain dividing unit 43. m,s k indicates the last orientation measurement number m,e A number of azimuth measurement values ​​(θ k , φ k ), and multiple orientation measurement errors (σ θ,k , σ φ,k ), measurement time t k , the position information of the orientation sensor 10 (x r k , y r k , z r k ) to get the position value x m A positioning process is performed to calculate the representative time t m Position measurement value x as a state in m get. In addition, the positioning value x m The positioning process for calculating the above may be a commonly known process.

[0038] Now, the position measurement value x of the target 100 from N orientation sensors 10 is M In the case where four orientation sensors 10 are used to obtain the position measurement value x M The concept of obtaining this will be described with reference to Fig. 3. The number of orientation sensors 10 is not limited to four. In FIG. 3, S1 to S4 are the first direction sensors 10 1 to the fourth orientation sensor 10 4 In the time domain M, the first orientation sensor S1 to the fourth orientation sensor S4 are located at the time domain M, and the line segments d1 to d4 extending from the first orientation sensor S1 to the fourth orientation sensor S4 are located at the time domain M. 4 Each azimuth measurement value θ k Indicates x M,true denotes the position of the target 100 in the time domain M.

[0039] That is, the azimuth measurement value θ k is the true value x M,true Shows the true value x M,true Values ​​close to this are normal. In FIG. 3, the line segment d4 is the azimuth (true value x M,true ), whereas line segment d1 is an abnormal value that does not indicate the orientation (position) of target 100 due to a multipath environment.

[0040] In the state shown in FIG. 3, the abnormal value determination unit 44 detects the first direction sensor 10 1 The direction measurement value (θ k , φ k ) is judged to be an abnormal value, and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) and the azimuth measurement value (θ k , φ k ) reduces the confidence in the trajectory estimate. On the other hand, in the state shown in FIG. 3, the abnormal value determination unit 44 detects the fourth direction sensor 10 4 The direction measurement value (θ k , φ k ) is judged to be a normal value, and the orientation measurement error (σ θ,k , σ φ,k ) is left as it is and the azimuth measurement value (θ k , φ k ) is left as it is.

[0041] That is, the abnormal value determination unit 44 determines that the azimuth measurement value (θ k , φ k ) to reduce confidence in As a result, the positioning processing unit 45 detects the position of the first direction sensor 10 1 to the fourth orientation sensor 10 4 The direction measurement value (θ k , φ k ) and the measured direction (θk , φ k ) and the azimuth measurement error (σ θ,k , σ φ,k ), and the positioning process is performed to calculate the positioning value x m Then, the time domain m, that is, the representative time t m Position value of target 100 at x m The target position information indicating the position is obtained.

[0042] The positioning process for calculating the positioning value at this time is performed by the first direction sensor 10. 1 The direction measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is lowered, the reliability of the first orientation sensor 10 in the positioning process is 1 The direction measurement value (θ k , φ k ) can be suppressed, and the positioning value x indicating the estimated position of the target 100 obtained by the positioning processing unit 45 can be reduced. m The true value of the target 100 x M,true is obtained as a value close to In this way, as shown in FIG. 2, the representative time t m Position measurement value x 1 From position value x M is obtained by the positioning processor 45.

[0043] The output unit 46 outputs the representative time t from the time domain 1 to the time domain M, which indicates the estimated position of the target 100 obtained by the positioning processing unit 45. m Position measurement value x 1 From position value x M are all connected in chronological order as shown in FIG. 3 and output to the output device 50 as object trajectory information indicating the estimated trajectory of the target 100. The output device 50 is a device that uses the object trajectory information from the output unit 46, and is, for example, a display that displays the trajectory of an object based on the object trajectory information.

[0044] As shown in FIG. 1, abnormal value determination unit 44 includes a direction information reading unit 44a, a positioning gate determination unit 44b, and a reliability setting unit 44c. The direction information reading unit 44a reads direction measurement values ​​(θ k , φ k ) and the orientation measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) and the measurement time t k , the position information of the orientation sensor 10 (x r k , y r k , z r k ) in chronological order.

[0045] The direction information reading unit 44a is described as a functional unit for convenience of explanation. However, the direction information reading unit 44a reads the direction information and divides it into time domains m in chronological order by the time domain dividing unit 43. The direction information reading unit 44a reads the direction information and divides it into time domains m in chronological order by the time domain dividing unit 43. The direction information reading unit 44a reads the direction information (θ k , φ k ) and the orientation measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) and the measurement time t k , the position information of the orientation sensor 10 (x r k , y r k , z r k ) is directly used by the positioning gate determination unit 44b.

[0046] The positioning gate determination unit 44b determines, for each of the plurality of orientation sensors 10, the orientation measurement value (θ k , φ k ) is a process for determining abnormal values. The azimuth measurement value (θ k , φ k The process of determining whether the direction measurement values ​​(θ k , φ k ) are concentrated at one point, i.e., they indicate a unique position, or if the result of the positioning process includes a velocity component, the orientation measurement value (θ k , φ k Based on where the azimuth measurements (θ k , φ k ) is an abnormal value or a normal value.

[0047] The azimuth measurement value (θ k , φ k The determination of whether the azimuth measurement value (θ) is an abnormal value or a normal value is performed by selecting a plurality of azimuth sensors from the N azimuth sensors 10, and comparing the azimuth measurement values ​​(θ) of the plurality of azimuth sensors for each group of sensors. k , φ k The reliability is evaluated by determining whether or not a set of sensors is present within the positioning gate, and the magnitude of variance between the provisional positioning values ​​for each sensor pair with the highest reliability is regarded as the degree of abnormality of the positioning in the time domain. A time domain in which the degree of abnormality is persistently lower than a set value is regarded as a normal time domain, and a time domain in which the degree of abnormality is persistently higher than the set value is regarded as an abnormal time domain. The time region with the lowest degree of abnormality may be determined as the normal time region.

[0048] Now, in the case where four of the N orientation sensors 10 are selected as orientation sensors 10 for calculating an estimated position of the target 100, the concept of the process by the positioning gate determination unit 44b to determine whether a value is abnormal or normal will be described with reference to Fig. 4. The number of orientation sensors 10 is not limited to four. In FIG. 4, S1 to S4 are the first direction sensors 10 1 to the fourth orientation sensor 10 4In the figure, dashed lines d1 to d4 extending from the first orientation sensor S1 to the fourth orientation sensor S4 are respectively included in the time domain m. 4 Each azimuth measurement value θ k Indicates x m,true denotes the position of the target 100 in the time domain m.

[0049] In addition, the azimuth measurement values ​​(θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) shall be set equal. The azimuth measurement values ​​(θ k , φ k ) and the associated heading measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of each orientation sensor 10 (x r k , y r k , z r k ) to perform positioning processing, and the position value x of target 100 is m get.

[0050] Position value x m is the true value x M,true The distance from the line segment d2 is, for example, the azimuth measurement value θ k and the azimuth measurement value θ shown by the line segment d3 k When is close to linear dependence, the azimuth measurement value θ shown by the line segment d1 k and the azimuth measurement value θ shown by line segment d4 k Both are equally reliable and the positioning value x m To calculate the true value x M,true It deviates greatly from. That is, the position value x of target 100 m is the azimuth measurement value θ indicated by the line segment d1 k Due to the effect of the target of 100, the true value xM,true is far away.

[0051] On the other hand, the azimuth measurement (θ k , φ k ) is concentrated, or if you trust that the target 100 is in the state vector, the position where you can trust that the target 100 is in the state vector is the azimuth measurement value θ k and the azimuth measurement value θ shown by the line segment d3 k and the azimuth measurement value θ shown by line segment d4 k The position where is concentrated is the true value x M,true The area around this area is:

[0052] Therefore, the azimuth measurement value θ indicated by the line segment d1 k can be judged to be an abnormal value. The positioning gate determination unit 44b determines whether the azimuth measurement value θ k The azimuth measurement error (σ θ,k , σ φ,k ) is reset so that its contribution to the positioning process for calculating the positioning value is reduced.

[0053] Next, the abnormal value determination process performed by the positioning gate determination unit 44b will be described with reference to FIG. Now, a case will be described in which an estimated position of the target 100 is obtained from N direction sensors 10, that is, four direction sensors 10 are selected as the direction sensors 10 that perform the process of determining abnormal values. The number of direction sensors 10 is not limited to four. In FIG. 5, S1 to S4 are the first direction sensors 10 1 to the fourth orientation sensor 10 4 In the figure, dashed lines d1 to d4 extending from the first orientation sensor S1 to the fourth orientation sensor S4 are respectively included in the time domain m. 4 Each azimuth measurement value θ k Shows.

[0054] The positioning gate determination unit 44b performs the following process. First, a certain number of sensors, three in this example, are selected from the first to fourth direction sensors S1 to S4 and grouped. For example, the sensors are grouped into a group of direction sensors S1, S2, and S3 (first group), a group of direction sensors S1, S2, and S4 (second sensor group), a group of direction sensors S1, S3, and S4 (third sensor group), and a group of direction sensors S2, S3, and S4 (fourth sensor group).

[0055] Secondly, for each of the first to fourth sensor groups, the direction measurement value (θ k , φ k ) is calculated by the commonly known positioning process, and the provisional positioning value x m,{1,2,3} , provisional position value x m,{1,2,4} , provisional position value x m,{1,3,4} , provisional position value x m,{2,3,4} and provisional positioning accuracy R m,{1,2,3} , provisional positioning accuracy R m,{1,2,4} , provisional positioning accuracy R m,{1,3,4} , provisional positioning accuracy R m,{2,3,4} Estimate.

[0056] Third, for each sensor set, the corresponding tentative position x m,{1,2,3} , provisional position value x m,{1,2,4} , provisional position value x m,{1,3,4} , provisional position value x m,{2,3,4} Then, positioning gates Ga to Gd are set according to the above. For each time domain m, the azimuth measurement values ​​(θ k , φ k ) is present within the positioning gates Ga to Gd.

[0057] Fourth, for each sensor set, the orientation measurement (θ k , φ k ) is present within the corresponding positioning gate Ga to Gd, the azimuth measurement value (θ k , φ k) is evaluated, and the provisional position value x m,Ai If the confidence level of is high, that is, the orientation measurement value (θ k , φ k ) is within each of the positioning gates Ga to Gd, the orientation measurement is considered to be consistent, and the orientation measurement value (θ k , φ k ) is judged as a normal value, and the direction measurement value (θ k , φ k ) is judged as an abnormal value, and a positioning gate judgment is performed on the direction measurement value.

[0058] In short, the positioning gate determination unit 44b selects a certain number of orientation sensors, for example three orientation sensors, from the plurality of orientation sensors S1 to S4, and divides them into a plurality of sensor groups. In each of the plurality of sensor groups, the unit 44b estimates a provisional positioning value for each time domain m divided by the time domain division unit 43, sets positioning gates G1 to G4 determined by the estimated provisional positioning values, and calculates the orientation measurement value (θ k , φ k ) is determined to be a normal value if it is within the set positioning gates G1 to G4, and the azimuth measurement value (θ k , φ k ) is not present within any of the set positioning gates G1 to G4, it is determined to be an abnormal value.

[0059] The positioning gate determination by the positioning gate determining unit 44b will be described below using mathematical expressions. Let us assume that the number of orientation sensors 10 capable of observing the target 100 is N, and the set of numbered orientation sensors 10 used for positioning in the time domain m is U={1, 2, . . . , N}. Let the measurement time t k Let S be the set of orientation measurement numbers k for identifying m ={k m,s ,···,k m,e}. The direction measurement number k is the measurement time t k Orientation measurement (θ k , φ k ) corresponds to km,s is the azimuth measurement of the first azimuth information in the time domain m (θ k , φ k ), and k m,e is the azimuth measurement of the first azimuth information in the time domain m (θ k , φ k )

[0060] When selecting three orientation sensors 10 from set U, the combination is as follows: N C 3 There is a street. The sensor set selected at this time is A i (i=1,2,...,I)(I= N C 3 ), the sensor combination is expressed by the following equation (1). A 1 ={1,2,3}, A 2 ={1,2,4}, , A I ={N-2,N-1,N} (1)

[0061] Sensor group A i Let S be the set of azimuth measurement values ​​k that exist in the m-th time domain of m,Ai Let S be the set m,Ai The direction measurement value number k in the is expressed in chronological order by the following equation (2). k=k (m,Ai),s , , k (m,Ai),e (2) That is, the above formula (2) expresses the azimuth measurement number k as k m,Ai The azimuth measurements are expressed as individual orientation measurements.

[0062] When the azimuth measurement value number k is expressed by the above formula (2), in the sets shown in the following formula (3), there is overlap in the azimuth measurement value numbers between the sets as shown in the following formula (4). TIFF0007433560000001.tif13166 TIFF0007433560000002.tif24166

[0063] At the positioning gate determined by the positioning gate determination unit 44b, first, the sensor group Ai The azimuth measurement value (θ k , φ k ) to obtain the representative time t m,Ai The provisional position value x used for m,Ai and provisional positioning accuracy R m,Ai is obtained by performing a positioning process that calculates a commonly known positioning value. TIFF0007433560000003.tif19166

[0064] Sensor group A i In the tentative position value x m,Ai is the orientation measurement (θ k , φ k ) and the orientation measurement (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is used to perform positioning processing. In addition, sensor group A i In this case, the provisional positioning accuracy R m,Ai is the provisional position value x m,Ai and provisional position value x m,Ai The position and orientation measurement error (σ θ,k , σ φ,k ) is estimated using

[0065] As a positioning method for performing the positioning process, for example, a method based on weighted least squares estimation or a method based on MAP (Maximum a Posteriori) estimation is used. Also, state (position value) x m If the state x contains not only position components but also velocity components, m is a state vector having six components, which are the three dimensions of position and the three dimensions of velocity in a three-dimensional space, and a method for estimating the position and velocity of a target by MAP estimation based on different orientation measurement times and orientation measurement errors of multiple sensors, as shown in the following non-patent document, may be used. Provisional positioning accuracy R m,AiFor example, a method of setting the observation error covariance with the Bayesian Cramer-Rao Boundary (BCRB) described in the following non-patent document as the lower limit is used as the estimation method for .

[0066] Non-patent literature: 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,Feb.15,2018.

[0067] The kth orientation measurement (θ k , φ k ) residual L k,Ai is the measurement time t m,Ai The state transition matrix Φ k|(m,Ai) Measurement time t k The provisional position value x shown in the following equation (6) is converted to k,Ai and the provisional positioning accuracy R k,Ai It is calculated using the statistical distance defined by:

[0068] TIFF0007433560000004.tif12166 TIFF0007433560000005.tif18166

[0069] TIFF0007433560000006.tif14166 TIFF0007433560000007.tif13166

[0070] TIFF0007433560000008.tif12166 TIFF0007433560000009.tif12166 TIFF0007433560000010.tif14166

[0071] In addition, the second term Qk on the right side of the above equation (8) is the driving noise covariance. The above equation (9) expresses the relationship between the predicted value and the azimuth measurement value (θ k , φ k ) residual e k,Ai The above equation (10) represents the sensor group A i The residual error covariance of DOA in the above equation (10) is shown. k is the partial differential matrix of the position component with respect to the angle component. The above equation (11) is the error covariance of the DOA Σ θφ,k The above equation (12) expresses the k-th azimuth measurement value (θ k , φ k ) residual L k,Ai This indicates the square of .

[0072] Provisional position value x k,Ai Sensor set A used in the calculation of i The azimuth measurement value (θ k , φ k ) set S m,Ai Among them, the number of orientation measurements k=k that is equal to or greater than a certain ratio parameter β (m,Ai),S , , k (m,Ai),e Residual L k,Ai is the threshold α 1 If so, then all heading measurements within that threshold are flagged.

[0073] Sensor group A i The flagged orientation measurement (θ k , φ k ) is the set of T m,Ai´ Then, sensor group A that satisfies this flag condition i´ Provisional position value x k,Ai´ is highly likely to be the target 100 because the orientation measurements are concentrated in the set T m,Ai´ The orientation measurement value (θ k , φ k ) is likely to be the target normal value. In the following equations (13) and (14), IF is a decision formula that returns 1 if the condition in the parentheses is met, and returns 0 if it is not met.

[0074] TIFF0007433560000011.tif23166 TIFF0007433560000012.tif17166

[0075] This process is repeated for all sensor sets A i The measured azimuth (θ k , φ k ) set T m,Ai´ At this time, the union set T m The orientation measurement value (θ k , φ k ) is judged as a normal value in this positioning gate judgment, and the azimuth measurement value (θ k , φ k ), that is, the azimuth measurement value (θ k , φ k ) is judged to be an abnormal value.

[0076] TIFF0007433560000013.tif14166 TIFF0007433560000014.tif11166

[0077] At this time, all sensor groups A i The direction measurement value (θ k , φ k ) into a set T m Instead of including them in the set of sensors A, we perform spatial clustering of the provisional positioning values ​​to determine which sensor group A i indicates the same target 100, and the set T m You can also decide.

[0078] The reliability setting unit 44c is configured to determine whether the azimuth measurement value (θk , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is the accuracy in a multipath environment, and the azimuth measurement error is degraded. In other words, the azimuth measurement value (θ k , φ k ) parameter value with lowered confidence (σ θ,err´ , σ φ,err´ ) and set the parameter value (σ θ,err´ , σ φ,err´ ) is obtained as the reset orientation measurement error and is used by the positioning processor 45.

[0079] Orientation measurement (θ k , φ k ) parameter value with lowered confidence (σ θ,err´ , σ φ,err´ ) should be intentionally large. Intentionally increasing the measured azimuth value (θ k , φ k ) in the azimuth measurement error associated with θ,k Let the value of θ,k σ at parameter values ​​greater than θ,err´ This means that the value is set to In short, the azimuth measurement value (θ k , φ k ) is reset to a parameter value greater than the value indicated by the orientation measurement error associated with the orientation measurement determined to be normal. That is, the parameter value is a parameter element that is determined by a reliability level set for a direction measurement value that is determined to be an abnormal value relative to a direction measurement value that is determined to be a normal value.

[0080] The reliability setting unit 44c is configured to set the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is left as it is, and the positioning processing unit 45 calculates the azimuth measurement value (θ k , φ k) is used as the reset heading measurement error that is taken as is and does not reduce the confidence level.

[0081] Next, the operation of the target trajectory estimation device, mainly the operation of the abnormal value determination unit 44, will be described with reference to FIG. The direction information reading unit 41 reads the direction measurement values ​​(θ k , φ k ) and the azimuth measurement value (θ k , φ k ) and the associated measurement time t k and the position information of each orientation sensor 10 (x r k , y r k , z r k ) is read.

[0082] In addition, the azimuth measurement error (σ θ,k , σ φ,k ) or the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to load the

[0083] Next, the time domain division unit 43 divides the azimuth measurement value (θ k , φ k ) and the orientation measurement (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of each orientation sensor 10 (x r k , y r k , z r k ) is measured at time t k Divide it into M pieces based on.

[0084] The time domain division unit 43 divides the time domain m into N sensors, and calculates the azimuth measurement values ​​(θ k , φ k ), the determination as to whether or not it is abnormal value determination unit 44 is performed in accordance with steps ST1 to ST14 shown in FIG. In step ST1, the direction information reading unit 44a reads direction measurement values ​​(θ k , φ k ) and the orientation measurement (θ k ,φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of the orientation sensor 10 (x r k , y r k , z r k ) to load the Step ST1 is a step of reading in azimuth information for each time domain m. The time domain division unit 43 divides the azimuth measurement values ​​(θ k , φ k ) etc., is obtained by the positioning gate determination unit 44b.

[0085] Steps ST2 to ST14 are steps performed by the positioning gate determination unit 44b. In step ST2, a certain number of orientation sensors 10 are selected from the plurality of orientation sensors 10 and grouped into a plurality of sensor groups. For example, four orientation sensors are grouped into sensor groups each consisting of three orientation sensors 10. Step ST2 is a step for selecting a sensor group.

[0086] In step ST3, for each of the grouped sensor sets, the azimuth measurement value (θ k , φ k) is subjected to a generally known positioning process to calculate a tentative position value. Step ST3 is a step of calculating a tentative position value in each time domain m for each set.

[0087] For example, when four direction sensors are grouped into a first group, a second group, and a third group, a third group, and a fourth group, a provisional position value x m,{1,2,3} , and the provisional position value x m,{1,2,4} , and the provisional position value x m,{1,3,4} , the fourth set contains the provisional position value x m,{2,3,4} Calculate. Such calculation of tentative position values ​​is performed for m time domains from time domain 1 to time domain M, and the number of tentative position values ​​calculated is equal to the product of the number of time domains and the number of sensor pairs.

[0088] In step ST4, the tentative positioning accuracy associated with all the tentative position values ​​calculated in step ST3 is estimated. Step ST4 is a step of estimating the tentative positioning accuracy associated with the tentative position values. The estimation of the provisional positioning accuracy is performed by calculating the provisional positioning value and the position information (x r k , y r k , z r k ) and orientation measurement error (σ θ,k , σ φ,k ) is estimated using For example, provisional position value x m,{1,2,3} For the provisional positioning accuracy R m,{1,2,3} , provisional position value x m,{1,2,4} For the provisional positioning accuracy R m,{1,2,4} , provisional position value x m,{1,3,4} For the provisional positioning accuracy R m,{1,3,4} , provisional position value x m,{2,3,4 Provisional positioning accuracy R m,{2,3,4} Estimate.

[0089] In step ST5, a positioning gate is set for all the tentative position values ​​calculated in step ST3. Step ST5 is a step for creating a positioning gate. The positioning gate setting is created from the provisional position value and provisional positioning accuracy. For example, provisional position value x m,{1,2,3} For the positioning gate Ga, the provisional position value x m,{1,2,4} For positioning gate Gb, provisional position value x m,{1,3,4} For positioning gate Gc, provisional position value x m,{2,3,4 Set the positioning gate Gd to

[0090] In step ST6, the azimuth measurement value (θ k , φ k ) is selected, and in step ST7, the selected azimuth measurement value (θ k , φ k ) enters the corresponding gate. Step ST7 is a step for determining the azimuth measurement value. That is, the selected orientation measurement (θ k , φ k In the time domain where θ exists, the selected orientation measurement value (θ k , φ k If it is determined that the value is included, the process proceeds to step ST8, and if it is determined that the value is not included, the process proceeds to step ST9.

[0091] In step ST8, the azimuth measurement value (θ k , φ k ) is considered a candidate for normality. In step ST9, the azimuth measurement value (θ k , φ k ) have all been selected, and if not, the process returns to step ST6, and steps ST7 and ST8 are repeated. In step ST9, the azimuth measurement value (θ k , φ k ) are all selected, the process proceeds to step ST10. In this manner, the azimuth measurement value (θ k , φ k) is judged as to whether it will enter positioning gate G or not.

[0092] In step ST10, for each time domain m, the azimuth measurement value (θ k , φ k ) entered the positioning gate G at a high rate, that is, the azimuth measurement value (θ k , φ k If it is determined that a large proportion of the data has entered the positioning gate G, the process proceeds to step ST11, and if it is determined that the data has not entered the positioning gate G, the process proceeds to step ST12.

[0093] In step ST11, the azimuth measurement values ​​(θ k , φ k ) enters the positioning gate G at a high rate, the positioning measurement value (θ k , φ k ) is selected as the normal value from the normal value candidates. In step ST12, the positioning measurement value (θ k , φ k ) has been determined as a normal value of the sensor set at least once, and if it has been determined to have been determined to have been a normal value at least once, the process proceeds to step ST13, and if it has not been determined to have been determined to have been a normal value at least once, the process proceeds to step ST14.

[0094] In step ST13, the positioning measurement value (θ k , φ k ) is determined as a normal value in abnormal value determination section 44. In step ST14, the positioning measurement value (θ k , φ k ) is regarded as an abnormal value in abnormal value determination section 44. In this manner, the azimuth measurement value (θ k , φ k ) are judged to be normal or abnormal.

[0095] Steps ST10 to ST14 are, in short, a process of calculating the selected orientation measurement (θ k , φ k ) entered the positioning gate G, that is, the selected orientation measurement value (θ k , φ k ) residual L k,Ai is the threshold α 1 It is determined whether the number of entries is equal to or greater than a ratio parameter β. In other words, the selected orientation measurement (θ k , φ k ) are concentrated at a single point, i.e., pointing to a unique position, or are concentrated around a state vector consisting of position and velocity. Based on the result of the determination, a determination is made as to whether the value is normal or abnormal.

[0096] In step 15, the reliability setting unit 44c determines whether the azimuth measurement value (θ k , φ k ) versus the orientation measurement (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to increase the azimuth measurement value (θ k , φ k ) and reduces confidence in For example, the orientation measurement error (σ θ,k , σ φ,k ) into the azimuth measurement (θ k , φ k ) parameter value with lowered confidence (σ θ,err´ , σ φ,err´ )

[0097] Next, the positioning processing unit 45 calculates the direction measurement value (θ k , φ k ) and the orientation measurement (θ k , φ k ) and the azimuth measurement error (σ θ,k , σ φ,k) and measurement time t k and the position information of the orientation sensor 10 (x r k , y r k , z r k ) to calculate the positioning value, and the positioning value x m and obtain the position measurement value x of target 100 in time domain m. m The target position information indicating the position is obtained.

[0098] The output unit 46 outputs the representative time t from the time domain 1 to the time domain M, which indicates the estimated position of the target 100 obtained by the positioning processing unit 45. m Position measurement value x 1 From position value x M are all connected in chronological order and output to the output device 50 as object trajectory information indicating the estimated trajectory of the target 100.

[0099] The target trajectory estimation device is realized by a hardware configuration of a computer, and as shown in FIG. 7, includes a processor 401, a memory 402, an input interface 403, and an output interface 404, which are connected to each other via a bus 405. The memory 402 includes a large-capacity semiconductor memory (RAM: Random Access Memory) and a storage device (ROM: Read only memory) such as a non-volatile recording device, such as a hard disk device or an SSD device.

[0100] The processor 401 controls and manages the memory 402 , the input interface 403 , and the output interface 404 . The processor 401 temporarily stores the program stored in the ROM in the memory 402 in the RAM, and executes the process of generating an estimated trajectory of the target 100 according to the software program stored in the RAM.

[0101] The input interface 403 corresponds to the direction information reading unit 41 shown in FIG. 1, and the output interface 404 corresponds to the output unit 46 shown in FIG. The functions of the time domain division unit 43, the abnormal value determination unit 44, the positioning processing unit 45, and part of the output unit 46 shown in FIG. 1 are realized by the processor 401 loading a program stored in the memory 402 and operating in accordance with the loaded program.

[0102] The target trajectory estimation method of the target trajectory estimation device, including steps ST1 to ST14, is performed by the processor 401 executing processes in accordance with a program stored in the memory 402. That is, the program stored in the memory 402 divides the time during which each of the multiple orientation sensors 10 that received the incoming radio wave from the target 100 observed the target 100 into multiple time domains m in chronological order, and calculates the orientation measurement values ​​(θ k , φ k ) for each divided time domain m; selecting a certain number of orientation sensors 10 from the plurality of orientation sensors 10 and grouping them into a plurality of sensor groups; and, in each of the plurality of sensor groups, calculating a provisional positioning value x for each divided time domain m. m,Ai The estimated provisional position value x m,Ai A positioning gate G is set, and the orientation measurement value (θ k , φ k ) is determined to be a normal value if it is within the set positioning gate G, and the azimuth measurement value (θ k , φ k ) is determined to be an abnormal value if it does not exist within any of the set positioning gates G; k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k ) to a setting that reduces the reliability of the trajectory estimation, and for each divided time domain m, the direction measurement value (θ k , φ k ) and the measured direction (θ k , φ k) associated with the reset orientation measurement error (σ θ,k , σ φ,k ) to estimate positioning values ​​for the target 100 in each time domain m, and a step of outputting object trajectory information indicating the trajectory of the target 100 by connecting the estimated positioning values ​​for each time domain m in a time series.

[0103] As described above, the target trajectory estimation device 40 according to the first embodiment divides the time during which each of the multiple direction sensors 10 that received the incoming radio waves from the target 100 observed the target 100 into multiple time domains m in chronological order, and calculates the direction measurement values ​​(θ k , φ k A time domain division unit 43 divides the direction information indicating the direction of the time domain m into divided time domains m, and for each divided time domain m, a direction measurement value (θ k , φ k ) is judged to be an abnormal value, and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k ) to a setting that lowers the reliability of the trajectory estimation, and the orientation measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k ) for the trajectory estimation without changing it, and for each time domain m, the direction measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) associated with the reset orientation measurement error (σ θ,k , σ φ,k), and an output unit 46 that outputs object trajectory information indicating the trajectory of the target 100 by connecting the positioning values ​​for each time domain m estimated by the positioning processing unit 45 in a time series, so that even if the orientation sensor 10 is in a multipath environment, the positioning value of the target 100 can be estimated with high accuracy, and a trajectory estimation that is closer to the actual trajectory of the target 100 can be performed.

[0104] The target trajectory estimation device 40 according to the first embodiment detects the azimuth measurement value (θ k , φ k The determination of whether the azimuth measurement value (θ) indicates an abnormal value is performed by selecting a certain number of azimuth sensors 10 from the plurality of azimuth sensors 10, grouping them into a plurality of sensor groups, and for each of the plurality of sensor groups, calculating the azimuth measurement value (θ) indicated by the azimuth information present in each time domain m for each divided time domain m. k , φ k ) and the measured direction (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is used to perform positioning processing, and the position measurement value x m and obtain the position measurement value x m and the position measurement value x m The position and orientation measurement error (σ θ,k , σ φ,k ) to obtain the provisional positioning accuracy R m The measured direction (θ k , φ k ) is the provisional position value x m,Ai and provisional positioning accuracy R m,Ai The azimuth measurement value (θ k , φ k ) is evaluated, and the azimuth measurement value (θ k , φ k ) is the normal value, and the orientation measurement value with low reliability (θ k , φ k ) is judged to be an abnormal value, so the azimuth measurement value (θ k , φk ) is determined by using a positioning gate G that determines the consistency of the azimuth measurement value (θ k , φ k To set the confidence level of the anomalous orientation measurements (θ k , φ k It is possible to calculate positioning values ​​with reduced contribution of

[0105] Also, the normal orientation measurement (θ k , φ k ) can be identified. Furthermore, the positioning gate G determines the positioning value x for each sensor group. m In order to determine whether anomalies occur, the provisional positioning accuracy R m Based on the positioning gate G defined by k , φ k ) can be judged to be abnormal. k , φ k ) to establish robustness against abnormal values ​​that occur in multipath environments, and to obtain normal azimuth measurements (θ k , φ k ) can be used to perform highly accurate positioning processing.

[0106] In the target trajectory estimation device 40 according to the first embodiment, an abnormal value determination unit 44 selects a certain number of orientation sensors 10 from the plurality of orientation sensors 10, classifies them into a plurality of sensor groups, and calculates a provisional position value x for each divided time region m in each of the plurality of sensor groups. m,Ai The estimated provisional position value x m,Ai A positioning gate G is set, and the orientation measurement value (θ k , φ k ) is determined to be a normal value if it is within the set positioning gate G, and the azimuth measurement value (θ k , φ k ) is determined to be an abnormal value if it does not exist within any of the set positioning gates G; k, φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k The positioning processing unit 45 is provided with a reliability setting unit 44c for resetting the reliability of the trajectory estimation of the time domain m to a lower setting, and the positioning processing unit 45 is provided with a reliability setting unit 44c for resetting the reliability of the trajectory estimation of the time domain m to a lower setting, k , φ k ) and the measured direction (θ k , φ k ) associated with the reset orientation measurement error (σ θ,k , σ φ,k ) is used to estimate the positioning value for the target 100 in each time domain m, and the output unit 46 connects the estimated positioning values ​​for each time domain m in a time series to output object trajectory information showing the trajectory of the target 100. Therefore, the positioning processing unit 45 outputs the azimuth measurement value (θ k , φ k ) is the reduced confidence level of the heading measurement error (σ θ,k , σ φ,k ), the positioning process can be performed using the above-mentioned method, so that even if the orientation sensor 10 is in a multipath environment, the positioning value of the target 100 can be estimated with high accuracy, and a trajectory estimation that is closer to the actual trajectory of the target 100 can be performed. Also, the provisional position value x m,Ai The azimuth measurement value (θ k , φ k ) abnormalities can be detected.

[0107] Embodiment 2 A target trajectory estimation device according to a second embodiment will be described with reference to FIGS. The target trajectory estimation device according to the second embodiment is configured to estimate the azimuth measurement values ​​(θ k , φ k ) is measured by the multiple orientation sensors 10 while each of the multiple orientation sensors 10 observes the target 100 (hereinafter referred to as the target observation time). k , φk ) in a batch process to estimate the trajectory, but otherwise it is the same. In addition, in Figs. 8 to 10, the same reference numerals as those in Figs. 1 to 7 designate the same or corresponding parts.

[0108] FIG. 8 is a block diagram showing a schematic configuration of a target trajectory estimation system including a target trajectory estimation device according to the second embodiment. As shown in FIG. 8, the target trajectory estimation system includes a plurality of orientation sensors 10 1 ~10 N and a plurality of azimuth measuring units 20 1 ~20 N The system includes a storage device 30, a target trajectory estimation device 40A, and an output device 50. Multiple orientation sensors 10 1 ~10 N and a plurality of azimuth measuring units 20 1 ~20 N The storage device 30 and the output device 50 are the same as those described in the first embodiment. 1 ~10 N and a plurality of azimuth measuring units 20 1 ~20 N Since these are the same as the storage device 30 and the output device 50, their explanation will be omitted.

[0109] The target trajectory estimation device 40A includes a direction information reading unit 41, an error setting unit 42, a time domain dividing unit 43, an abnormal value determining unit 44A, a positioning processing unit 45, an accuracy estimating unit 47, a tracking processing unit 48, and an output unit 46. The direction information reading unit 41 and the time domain dividing unit 43 are the same as the direction information reading unit 41 and the time domain dividing unit 43 described in the first embodiment, and therefore a description thereof will be omitted.

[0110] Abnormal value determination unit 44A includes a direction information reading unit 44a, a positioning gate determination unit 44b, a reliability setting unit 44c, a time domain selection unit 44d, a prediction information reading unit 44e, and a prediction gate determination unit 44f. The direction information reading unit 44a, the positioning gate determination unit 44b, and the reliability setting unit 44c are substantially the same as those in the target trajectory estimation device according to the first embodiment.

[0111] The positioning gate determination unit 44b will be briefly described as an example that has been described in the first embodiment. The positioning gate determination unit 44b selects a certain number of orientation sensors 10 from the plurality of orientation sensors 10, classifies them into a plurality of sensor groups, and calculates a provisional position value x for each of the plurality of sensor groups for each time domain m divided by the time domain division unit 43. m,Ai The estimated provisional position value x m,Ai A positioning gate G is set, and the orientation measurement value (θ k , φ k ) is determined to be a normal value if it is within the set positioning gate G, and the orientation measurement value (θ k , φ k ) is not present within any of the set positioning gates G, it is determined to be an abnormal value.

[0112] The reliability setting unit 44c will be briefly described as an example described in the first embodiment. The reliability setting unit 44c is configured to determine whether the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) into the orientation measurement (θ k , φ k ) parameter value with lowered confidence (σ θ,err´ , σ φ,err´ ), and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) into the orientation measurement (θ k , φ k ) will not be lowered, but will be reset to a high level of reliability.

[0113] In short, the positioning gate determination unit 44b and the reliability setting unit 44c determine all the azimuth measurement values ​​(θ k , φ k) is subjected to a positioning gate judgment using positioning gate G to determine whether the value is normal or abnormal, and the direction measurement value (θ k , φ k ) is considered to have high reliability, and the direction measurement value (θ k , φ k ) is considered to be less reliable.

[0114] The time domain selection unit 44d selects a sensor group A that is determined to have high reliability in the positioning gate determination using the positioning gate G in the entire time domain of the target observation time. i´ Provisional position value x m,Ai´ Calculate the variance between the normal time regions m and m based on the variance. s Select as. Selected time domain m s is the tracking origin time domain. time domain m s The method of selecting is to select a time region in which the variance is small for a long period of time or a time region in which the variance is minimal.

[0115] The positioning processing unit 45 selects the tracking start time region m s Starting from the starting point, the azimuth measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) the azimuth measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of the orientation sensor 10 (x r k , y r k , z r k ) to get the position value x m The positioning process calculates the position value x m and obtain the position measurement value x of target 100 in time domain m. m The target position information indicating the position is obtained.

[0116] The accuracy estimation unit 47 estimates the azimuth measurement error (σ θ,k , σ φ,k ) to obtain the positioning value x estimated by the positioning processing unit 45. m Positioning accuracy R m Estimate. The accuracy estimation unit 47 estimates the positioning value x m and the position information of the orientation sensor 10 (x r k , y r k , z r k ) and the azimuth measurement error (σ θ,k , σ φ,k ) to position value x m Positioning accuracy R m Estimate.

[0117] The tracking processing unit 48 calculates the positioning value x estimated by the positioning processing unit 45. m and the position measurement value x m The positioning accuracy R estimated by the accuracy estimation unit 47 corresponding to m Using the position measurement value x m Filter to get smoothed and predicted values The tracking processing unit 48 calculates the tracking start time region m s Starting from the starting point, for each adjacent time domain m, the positioning value x m and positioning accuracy R m Using the Kalman filter, the position measurement value x m Calculate the filtering of

[0118] The tracking processing unit 48 calculates the tracking start time region m s Starting from the point, the adjacent position value x m and the predicted value based on the continuity of the target 100's movement, and the smoothed value x m|m This smoothed value x m|m is the output of the target position at the representative time. The tracking process can be performed, for example, at any measurement time t k Predicted value of x k|m There is a Kalman filter that outputs the following in parallel.

[0119] In the second embodiment, the tracking start time region m s The smoothed value x m|m Calculate the tracking start time domain m s The smoothed value x is obtained by applying a time-reverse Kalman filter to the positioning process of the adjacent time domain in the reverse direction, that is, in the direction going back from the starting point. m|m Request. The Kalman filter in the tracking process is a process that estimates a smoothed value (weighted average) for the current time and a predicted value for the next time (calculated using a motion model from the smoothed value for the current time) from the predicted value for the previous time and the observed value for the current time.

[0120] Here, the tracking start time domain m s In this case, it is necessary to select a time region with a low degree of abnormality so that the positioning value can be determined to be close to the true value by the positioning gate judgment functioning normally. In the positioning gate judgment, the provisional positioning value x is used to judge whether the orientation measurement values ​​of the orientation sensor are concentrated at one point or one state vector. m,Ai´ had been calculated. Therefore, to check whether the positioning gate worked properly, we need to check the provisional positioning values ​​x m,Ai´ The magnitude of this variance can be said to be the degree of anomaly in the positioning in the time domain. When evaluating the degree of anomaly in the entire time domain, the tracking start time domain m s In order to facilitate tracking processing, a time region in which the degree of abnormality is continuously low or a time region in which the degree of abnormality is the lowest is selected.

[0121] First, the tracking origin time domain m s The time-reverse Kalman filter applied to the tracking process of the time domain m will be explained below using mathematical expressions. The time-backward Kalman filter motion model is x m|m+1 is the predicted value, x m+1|m+1 is used as the smoothed value, and the representative time t m+1 From m The time-reverse state transition matrix Γm|m+1 It is described using: Predicted value x m|m+1 is expressed by the following equation (17), and the state transition matrix Γ m|m+1 is expressed by the following equation (18).

[0122] TIFF0007433560000015.tif12166 TIFF0007433560000016.tif17166

[0123] W m+1 ~N(0,Q´ m|m+1 ) is the drive noise. The time-reverse drive error covariance Q´ m|m+1 Since the position component changes in the negative direction when positive driving noise is added to the velocity component, it is necessary to give a negative sign to the off-diagonal block, and Σ q is set as the covariance matrix of the three-dimensional position components as shown in the following equation (19).

[0124] TIFF0007433560000017.tif27166

[0125] The observation model is x m =x m,true +v m It becomes. v m ~N(0,R m ) is the observation noise, which corresponds to the estimation error of the positioning value. The initial step of the time-backward Kalman filter (m=m s ) in the tracking origin time domain m s Position measurement value x ms and positioning accuracy R ms is the smoothed value x ms|ms (=x ms ) and the smoothing error covariance P ms|ms (=R ms )

[0126] m≦m s In the case of −1, the prediction process in the time backward direction is expressed by the following equations (20) and (21). x m|m+1= Γ m+1 x m+1|m+1 (20) TIFF0007433560000018.tif15166

[0127] The smoothing process is expressed by the following equations (22) to (24). K m =P m|m+1 +(P m|m+1 +R m ) -1 (twenty two) x m|m =x m|m+1 +K m (x m -x m|m+1 ) (twenty three) P m|m =P m|m+1 -K m P m|m+1 (twenty four) P m|m+1 is the prediction error covariance, K m is the Kalman gain.

[0128] In this way, the tracking processing unit 48 uses a time-reverse Kalman filter to calculate the position measurement value x in the time domain m. m and the predicted value x of time domain m predicted by the above equation (20) at time domain m+1. m|m+1 The smoothed value x obtained by m|m is a representative time t m The output of the tracking processor 48 is the tracking start time domain m s Previous position value x m Connect them in chronological order to obtain the tracking origin time domain m s The object trajectory information indicating the estimated trajectory of the target 100 in the past is obtained.

[0129] On the other hand, the prediction information obtained by the tracking processing unit 48 is read by a prediction information reading unit 44e, and a prediction gate determination unit 44f determines the azimuth measurement value (θ k , φ k ) is subjected to a prediction gate judgment, which is an abnormal value judgment for the For convenience of explanation, the prediction information reading unit 44e is explained as a functional unit, but the prediction information obtained by the tracking processing unit 48 is directly used by the prediction gate determination unit 44f.

[0130] The prediction gate determination unit 44f predicts the predicted value x k|m+1 Predictive gate judgment is made based on the above. The prediction gate determination unit 44f uses the predicted value x k|m+1 and the positioning accuracy R estimated by the accuracy estimation unit 47. m The prediction accuracy P estimated from k|m+1 A prediction gate is set based on the direction information in the time domain m, and the direction measurement value (θ k , φ k ) is determined to be a normal value if it is within the set prediction gate, and the orientation measurement value (θ k , φ k ) is not present within any of the prediction gates, it is determined to be an abnormal value.

[0131] The method by which the predicted gate determination unit 44f performs predicted gate determination using tracking processing by a time-reverse Kalman filter is shown below by an equation. The kth orientation measurement (θ k , φ k ) residual L k´ is the state transition matrix Φ k|(m,Ai) Measurement time t k Predicted value x transformed to k|m+1 is the prediction error covariance P k|m+1 It is calculated using the statistical distance defined by the following equation (25): Prediction error covariance P k|m+1 is expressed by the following equation (27). The following equation (26) is the state transition matrix Φ k|m+1 Represents.

[0132] TIFF0007433560000019.tif14166 TIFF0007433560000020.tif18166

[0133] TIFF0007433560000021.tif13166 TIFF0007433560000022.tif15166

[0134] TIFF0007433560000023.tif12166 TIFF0007433560000024.tif14166

[0135] The above equation (28) expresses the predicted value and the measured azimuth value (θ k , φ k ) residual e k Shows. Equation (29) above shows the residual error covariance of the predicted DOA. Equation (30) above expresses the kth azimuth measurement value (θ k , φ k ) residual L k´ This indicates the square of .

[0136] The predicted gate center predicted value x used by the predicted gate determination unit 44f in predictive gate determination m|m+1 is the provisional position value x of the center of the positioning gate used by the positioning gate determination unit 44b in determining the positioning gate. k,Ai Unlike this, the positioning is highly reliable because it is obtained by processing the positioning based on the direction measurement values ​​that have already been checked for abnormal values ​​by the positioning gate. Therefore, the reliability setting unit 44c determines the residual L k´ is the threshold α 2 The azimuth measurement error (σ θ,k , σ φ,k ) again.

[0137] That is, as shown in the following equations (31) and (32), the residual L k´ is the threshold α 2 When the following condition is satisfied, the azimuth measurement error σ θk is the normal value σ θk´ , the azimuth measurement error σ φk is the normal value σ φk´ , the residual L in if k´ is the threshold α 2When the condition that exceeds σ is satisfied, the azimuth measurement error θk is an outlier σ θ,err´ , the azimuth measurement error σ φk is an outlier σ φ,err´ Reset it to. The reset azimuth measurement error (σ θ,k , σ φ,k ) is the tracking origin time domain m s The time domain m is used for the positioning process in the positioning processor 45.

[0138] TIFF0007433560000025.tif22166 TIFF0007433560000026.tif19166

[0139] On the other hand, the tracking processing unit 48 detects the tracking start time region m s The tracking process for the time domain adjacent to the starting point in the direction of time lapse is performed by applying a time-forward Kalman filter to obtain the positioning value x m Request. That is, the tracking processing unit 48 calculates the positioning value x m and the predicted value x of time domain m predicted at time domain m-1 m|m-1 The weighted average of x and m|m get. Predicted value x m|m-1 and the smoothed value x m|m The tracking process for obtaining is substantially the same as the tracking process using a time-reverse Kalman filter, and is a normal time-forward Kalman filter, so a description thereof will be omitted.

[0140] In this way, the tracking processing unit 48 uses a time-forward Kalman filter to calculate the position measurement value x in the adjacent time domain m. m and the predicted value x of time domain m predicted at time domain m-1 m|m-1 The smoothed value x obtained by tracking m|m is a representative time t m Position measurement value x m Then, the tracking start time domain m s Subsequent position measurement xm Connect them in chronological order to obtain the tracking origin time domain m s The object trajectory information indicating the estimated trajectory of the target 100 thereafter is obtained.

[0141] The output unit 46 outputs the tracking start time domain m obtained by the tracking processing unit 48 using a time-forward Kalman filter. s The estimated trajectory of the target 100 thereafter and the tracking origin time domain m obtained by the tracking processing unit 48 using a time-reverse Kalman filter s The estimated trajectory of the target 100 from the previous time period is connected to obtain object trajectory information indicating the estimated trajectory of the target 100 in the entire time domain of the target observation time.

[0142] In addition, the predicted gate determination performed by the predicted gate determination unit 44f is also performed using tracking processing by a Kalman filter in the time-forward direction, and the predicted gate determination unit 44f uses a predicted value x k|m-1 Predictive gate judgment is made based on the above. Predicted value x k|m-1 The prediction gate decision centered on is essentially the time-backward predicted value x k|m+1 Since this is the same as the prediction gate determination centered on the gate number 1, the explanation will be omitted.

[0143] The reliability setting unit 44c resets the residual L obtained by the prediction gate determination unit 44f in the same manner as resetting the reliability using the Kalman filter in the time-reverse direction. k´ is the threshold α 2 The azimuth measurement error (σ θ,k , σ φ,k ) again. The reset azimuth measurement error (σ θ,k , σ φ,k ) is the tracking origin time domain m s This is used for the subsequent positioning process in the positioning processor 45 from time domain m.

[0144] Next, the operation of the target trajectory estimation device will be described with reference to FIGS. 9 and 10, focusing mainly on the tracking process that differs from the first embodiment in the operation of abnormal value determination section 44A. Similar to the operation explained in the first embodiment, steps up to step ST15 shown in FIG. 6 are carried out.

[0145] Steps ST12 to ST15 shown in FIG. 6 correspond to steps ST101 and ST102 shown in FIG. That is, the positioning gate determination unit 44b performs positioning gate determination for each time domain m, and calculates the azimuth measurement value (θ k , φ k ) is judged to be normal or abnormal, and the reliability setting unit 44c determines whether the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) again.

[0146] In steps ST101 and ST102, the positioning gate determination unit 44b and the reliability setting unit 44c calculate all the azimuth measurement values ​​(θ k , φ k ) is subjected to a positioning gate judgment using positioning gate G to determine whether the value is normal or abnormal, and the direction measurement value (θ k , φ k ) is considered to have high reliability, and the direction measurement value (θ k , φ k ) is a step for resetting the reliability, which is deemed to be low.

[0147] In step ST103, the time domain selection unit 44d selects a sensor group A that is determined to have high reliability in the positioning gate determination using the positioning gate G in the entire time domain. i´ Provisional position value x m,Ai´ Calculate the variance between them. In step ST104, the normal time region m is selected based on the variance transition calculated in the time region selection unit 44d as the tracking origin time region m sSelect as.

[0148] Here, the tracking start time domain m s Therefore, it is necessary to select a time region with a low degree of abnormality so that the positioning value can be determined to be close to the true value by the positioning gate judgment functioning normally. In the positioning gate judgment, the provisional positioning value x is used to judge whether the orientation measurement values ​​of the orientation sensor are concentrated at one point or one state vector. m,Ai´ had been calculated. Therefore, to check whether the positioning gate worked properly, we need to check the provisional positioning values ​​x m,Ai´ The magnitude of this variance can be said to be the degree of anomaly in the positioning in the time domain. When evaluating the degree of anomaly in the entire time domain, the tracking start time domain m s In order to facilitate tracking processing, a time region in which the degree of abnormality is continuously low or a time region in which the degree of abnormality is the lowest is selected.

[0149] Selected tracking origin time region m s In the case where positioning processing is performed in the direction in which time elapses from the starting point, that is, in the forward direction, the process proceeds to step ST105 in FIG. 9, and the tracking starting point time region m s 10. In the case where positioning processing is to be performed in the direction going back in time from the starting point, that is, in the case where positioning processing is to be performed in the adjacent time domain in the reverse direction, the process proceeds to step ST205 in FIG.

[0150] Proceed to step ST105, and the tracking start time region m s After the process from the start point to the time domain M is completed, the process may proceed to step ST205. Conversely, the process may proceed to step ST205 and the tracking start point time domain m s After the process is completed up to time region 1 starting from step ST102, the process may proceed to step ST105.

[0151] Steps ST105 to ST113 shown in FIG. 9 are steps in which the Kalman filter in the time-forward direction is applied. In step ST105, first, the tracking start time region m is set as the time region m in which the positioning process is performed as the start point.s Select .

[0152] In step ST106, the positioning processing unit 45 calculates the azimuth measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) the azimuth measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of the orientation sensor 10 (x r k , y r k , z r k ) is read, and in step ST107, the read information is used to perform positioning processing, and the position measurement value x m Calculate.

[0153] In step ST108, the accuracy estimation unit 47 estimates the positioning value x m and the position information of the orientation sensor 10 (x r k , y r k , z r k ) and the azimuth measurement error (σ θ,k , σ φ,k ) to position value x m Positioning accuracy R m Estimate.

[0154] In step ST109, the tracking processing unit 48 performs a filtering process in the time forward direction on the time domain m to obtain a smoothed value x m|m and the predicted value x m+1|m Calculate. The calculated smoothed value x m|m is the representative time t m This is used as the output for tracking processing in

[0155] In step ST110, if the time domain m is the time domain M, the process ends, and if the time domain m is not the time domain M, the process proceeds to step ST111, where the time domain m is set to the next time domain m+1, and the process proceeds to step ST112. In step ST112, the prediction gate determination unit 44f calculates the predicted value x k|m-1 Predictive gate judgment is made based on the above. The prediction gate determination by the prediction gate determination unit 44f in step ST112 is performed based on the predicted value x k|m-1 The orientation measurement (θ k , φ k ) is subjected to a prediction gate judgment, which is an abnormal value judgment for the

[0156] In step ST113, the reliability setting unit 44c determines whether the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) with a lowered confidence level for the direction measurement error (σ θ,k , σ φ,k ) and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is reset to have a high reliability, and the process proceeds to step ST105.

[0157] In step ST105, the time domain m updated in step ST111 is selected, and steps ST106 to ST113 are repeated until the time domain m becomes the time domain M in step ST110. In this way, the tracking origin time domain m s Representative time t from to time domain M m Position measurement value x m is obtained.

[0158] The positioning process in the positioning processor 45 is carried out by calculating the azimuth measurement value (θ k , φk ) is predicted, and the re-established azimuth measurement error (σ θ,k , σ φ,k ) is tracked from the starting time domain m s It is used for all time domains m through time domain M.

[0159] Steps ST205 to ST213 shown in FIG. 10 are steps in which a time-backward Kalman filter is applied. In step ST205, first, the tracking start time region m is set as the time region m in which the positioning process is performed as the start point. s Select .

[0160] In step ST206, the positioning processing unit 45 calculates the azimuth measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) the azimuth measurement error (σ θ,k , σ φ,k ) and measurement time t k and the position information of the orientation sensor 10 (x r k , y r k , z r k ) is read, and in step ST207, the read information is used to perform positioning processing, and the position measurement value x m Calculate.

[0161] In step ST208, the accuracy estimation unit 47 estimates the positioning value x m and the position information of the orientation sensor 10 (x r k , y r k , z r k ) and the azimuth measurement error (σ θ,k , σ φ,k ) to position value x m Positioning accuracy R m Estimate.

[0162] In step ST209, the tracking processing unit 48 performs a filtering process in the time reverse direction on the time domain m to obtain a smoothed value x m|m and the predicted value x m-1|m Calculate the predicted value x m-1|m is calculated by the above formula (17), and the smoothed value x m|m is calculated using equation (23) above. The calculated smoothed value x m|m is the representative time t m This is used as the output for tracking processing in

[0163] In step ST210, if the time domain m is time domain 1, the process ends, and if the time domain m is not time domain 1, the process proceeds to step ST211, where the time domain m is set to the next time domain m-1, and the process proceeds to step ST212. In step ST212, the prediction gate determination unit 44f reads the prediction value x k|m+1 Predictive gate judgment is made based on the above. The prediction gate determination by the prediction gate determination unit 44f in step ST212 is performed based on the prediction value x k|m+1 The orientation measurement (θ k , φ k ) is subjected to a prediction gate judgment, which is an abnormal value judgment for the

[0164] In step ST213, the reliability setting unit 44c determines whether the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) with a lowered confidence level for the direction measurement error (σ θ,k , σ φ,k ) and the azimuth measurement value (θ k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) is reset to have a high reliability, and the process proceeds to step ST205.

[0165] In step ST205, the time domain m updated in step ST211 is selected, and steps ST206 to ST213 are repeated until time domain m becomes time domain 1 in step ST210. In this way, the time domain 1 to the tracking start time domain m s Representative time t m Position measurement value x m is obtained.

[0166] The positioning process in the positioning processor 45 is carried out by calculating the azimuth measurement value (θ k , φ k ) is predicted, and the re-established azimuth measurement error (σ θ,k , σ φ,k ) is tracked from the starting time domain m s It is used for all time domains m through time domain 1.

[0167] The output unit 46 outputs the representative time t from the time domain 1 to the time domain M, which indicates the estimated position of the target 100 obtained by the tracking processing unit 48 in steps ST109 and ST209. m Position measurement value x 1 From position value x M are all connected in chronological order and output to the output device 50 as object trajectory information indicating the estimated trajectory of the target 100.

[0168] The target trajectory estimation device is realized by a hardware configuration of a computer shown in FIG. 7, similar to the target trajectory estimation device according to the first embodiment. The input interface 403 corresponds to the direction information reading unit 41 shown in FIG. 7, and the output interface 404 corresponds to the output unit 46 shown in FIG. The functions of the time domain division unit 43, the abnormal value determination unit 44A, the positioning processing unit 45, the accuracy estimation unit 47, the tracking processing unit 48, and a part of the output unit 46 shown in FIG. 7 are realized by the processor 401 loading a program stored in the memory 402 and operating in accordance with the loaded program.

[0169] The target trajectory estimation method of the target trajectory estimation device, which includes steps ST1 to ST14, steps ST101 to ST113, and steps ST201 to ST213, is performed by the processor 401 executing processes in accordance with the programs stored in the memory 402. That is, the program stored in the memory 402 "divides the time during which each of the multiple direction sensors 10 that received the incoming radio wave from the target 100 observed the target 100 into multiple time domains m in chronological order, and calculates the direction measurement values ​​(θ k , φ k ) for each divided time domain m; selecting a certain number of orientation sensors 10 from the plurality of orientation sensors 10 and grouping them into a plurality of sensor groups; and, in each of the plurality of sensor groups, calculating a provisional positioning value x for each divided time domain m. m,Ai The estimated provisional position value x m,Ai A positioning gate G is set, and the orientation measurement value (θ k , φ k ) is determined to be a normal value if it is within the set positioning gate G, and the azimuth measurement value (θ k , φ k ) is determined to be an abnormal value if it does not exist within any of the set positioning gates G; k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k ) to a setting that reduces the reliability of the trajectory estimation, and for each divided time domain m, the direction measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) associated with the reset orientation measurement error (σ θ,k , σ φ,k) to estimate a position measurement value for the target 100 in each time domain m; a step of outputting object trajectory information showing the trajectory of the target 100 by connecting the estimated position measurement values ​​for each time domain m in a time series; and a step of determining whether the position measurement value is normal or abnormal, the provisional position measurement value x of the sensor set determined to have high reliability is m,Ai Calculate the variance between the two, and based on the calculated variance transition, calculate the tracking origin time domain m s The procedure for selecting the tracking origin time domain m s The estimated positioning value in the time domain m divided starting from and the reset azimuth measurement error (σ θ,k , σ φ,k ) to estimate the positioning accuracy for the estimated positioning value, and the tracking origin time domain m s A procedure for obtaining a positioning value consisting of a predicted value and a smoothed value using an estimated positioning value in a time domain m divided from a starting point and a positioning accuracy corresponding to the positioning value, and s A prediction gate is set based on the positioning value consisting of the smoothed value in the time domain m divided from the starting point and the prediction accuracy estimated from the positioning accuracy corresponding to the positioning value, and the azimuth measurement value (θ k , φ k ) is determined to be a normal value if it is within the set prediction gate, and the azimuth measurement value (θ k , φ k ) is not present within any of the prediction gates, it is determined to be an abnormal value, and the tracking origin time domain m s The reset azimuth measurement error (σ θ,k , σ φ,k ) that are judged to be abnormal because they do not exist within any of the prediction gates. k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k) to a lower reliability setting for the trajectory estimation, and the tracking origin time domain m s The estimated positioning value and the reset azimuth measurement error (σ θ,k , σ φ,k ) to estimate the positioning accuracy for the estimated positioning value, and the tracking origin time domain m s A procedure for obtaining a positioning value consisting of a predicted value and a smoothed value using an estimated positioning value in a time domain m divided from a starting point and a positioning accuracy corresponding to the positioning value, and s A prediction gate is set based on the positioning value consisting of the smoothed value in the time domain m divided from the starting point and the prediction accuracy estimated from the positioning accuracy corresponding to the positioning value, and the azimuth measurement value (θ k , φ k ) is determined to be a normal value if it is within the set prediction gate, and the azimuth measurement value (θ k , φ k ) is not present within any of the prediction gates, it is determined to be an abnormal value, and the tracking origin time domain m s The reset azimuth measurement error (σ θ,k , σ φ,k ) that are judged to be abnormal because they do not exist within any of the prediction gates. k , φ k ) associated with the heading measurement error (σ θ,k , σ φ,k ) to the corresponding azimuth measurement value (θ k , φ k ) to a setting that reduces the reliability of the trajectory estimation, and for each divided time domain m, the direction measurement value (θ k , φ k ) and the measured direction (θ k , φ k ) associated with the reset orientation measurement error (σ θ,k , σ φ,k) is set again as the azimuth measurement error (σ θ,k , σ φ,k ) to estimate a position measurement value for the target in each time domain m, and a step of outputting object trajectory information indicating the trajectory of the target 100 by connecting the position measurement values ​​consisting of smoothed values ​​for each time domain m in a time series.

[0170] As described above, the target trajectory estimation device 40A according to the second embodiment has the same effects as the target trajectory estimation device 40 according to the first embodiment. In addition, the target trajectory estimation device 40A according to the second embodiment has the same effects as the target trajectory estimation device 40 according to the first embodiment. θ,k , σ φ,k ) to obtain the positioning value x estimated by the positioning processing unit 45. m Positioning accuracy R m and a positioning accuracy estimation unit 47 that estimates the positioning value x m and the position measurement value x m The positioning accuracy R estimated by the accuracy estimation unit 47 corresponding to m Using the position measurement value x m The tracking processing unit 48 filters the signal to obtain a smoothed value and a predicted value. The abnormal value determination unit 44A determines the reset azimuth measurement error (σ θ,k , σ φ,k ) is set again, so that two different complementary abnormal value judgments are performed, resulting in more accurate and reliable positioning values ​​being used to estimate the trajectory of the target 100. In particular, even if a multipath environment in which trajectory estimation is difficult exists around the target, trajectory estimation can be performed.

[0171] It should be noted that the embodiments may be freely combined, or any of the components in each embodiment may be modified, or any of the components in each embodiment may be omitted. [Industrial Applicability]

[0172] A target trajectory estimation device according to the present disclosure is suitable for use in a target trajectory estimation system that estimates the trajectory of a target, such as an airplane, or other moving object. [Explanation of symbols]

[0173] 10, 10 1 ~10 N and multiple orientation sensors, 20 1 ~20 N A plurality of direction measurement units, 30 storage device, 40 target trajectory estimation device, 41 direction information reading unit, 42 error setting unit, 43 time domain division unit, 44 abnormal value determination unit, 44b positioning gate determination unit, 44c reliability setting unit, 44f prediction gate determination unit, 45 positioning processing unit, 46 output unit, 47 accuracy estimation unit, 48 tracking processing unit, 50 output device, 100 target.

Claims

1. a direction information reading unit for reading direction information indicating direction measurement values ​​obtained by each of a plurality of direction sensors receiving an incoming radio wave from a target, and direction measurement error information indicating direction measurement errors associated with the direction measurement values ​​indicated by the direction information; a time domain division unit that divides the time during which each of the plurality of orientation sensors observes the target into a plurality of time domains in chronological order, and divides the orientation information read by the orientation information reading unit into each of the divided time domains; an abnormal value determination unit that determines whether or not an orientation measurement value indicated by the orientation information present in each time domain indicates an abnormal value for each time domain divided by the time domain division unit, resets the orientation measurement error associated with the orientation measurement value determined to be an abnormal value to a setting that reduces the reliability of the orientation measurement value with respect to the trajectory estimation, and resets the orientation measurement error associated with the orientation measurement value determined not to be an abnormal value to a setting that reduces the reliability of the orientation measurement value with respect to the trajectory estimation, while leaving the reliability of the orientation measurement value unchanged; a positioning processing unit that estimates a positioning value for the target in each time domain for each time domain divided by the time domain dividing unit, using a direction measurement value indicated by direction information present in each time domain and a reset direction measurement error associated with the direction measurement value; an output unit that outputs object trajectory information indicating a trajectory of the target by connecting the positioning values ​​for each time domain estimated by the positioning processing unit in a time series; A target trajectory estimation device comprising:

2. a direction information reading unit for reading direction information indicating direction measurement values ​​obtained by each of a plurality of direction sensors receiving an incoming radio wave from a target, and direction measurement error information indicating direction measurement errors that are associated with the direction measurement values ​​indicated by the direction information or that are uniformly set; a time domain division unit that divides the time during which each of the plurality of orientation sensors observes the target into a plurality of time domains in chronological order, and divides the orientation information read by the orientation information reading unit into each of the divided time domains; an abnormal value determination unit which determines whether or not the orientation measurement values ​​indicate abnormal values, for each time domain divided by the time domain division unit, based on whether the positions indicated by the orientation measurement values ​​indicated by the orientation information present in each time domain are concentrated at one point or concentrated around a state vector consisting of position and velocity, and resets the orientation measurement error associated with the orientation measurement value determined to be an abnormal value to a setting that reduces the reliability of the orientation measurement value with respect to the trajectory estimation, and resets the orientation measurement error associated with the orientation measurement value determined not to be an abnormal value, while leaving the reliability of the orientation measurement value with respect to the trajectory estimation unchanged; a positioning processing unit that estimates a positioning value for the target in each time domain for each time domain divided by the time domain dividing unit, using a direction measurement value indicated by direction information present in each time domain and a reset direction measurement error associated with the direction measurement value; an output unit that outputs object trajectory information indicating a trajectory of the target by connecting the positioning values ​​for each time domain estimated by the positioning processing unit in a time series; A target trajectory estimation device comprising:

3. 3. The target trajectory estimation device according to claim 2, wherein the determination as to whether or not the orientation measurement value indicates an abnormal value by the abnormal value determination unit comprises: selecting a certain number of orientation sensors from the plurality of orientation sensors and grouping them into a plurality of sensor groups; performing positioning processing in each of the plurality of sensor groups, for each time domain divided by the time domain division unit, using the orientation measurement value indicated by the orientation information present in each time domain and the orientation measurement error associated with the orientation measurement value, to obtain a tentative positioning value in the sensor group for the target; estimating a tentative positioning accuracy in the sensor group using the tentative positioning value, the position of the orientation sensor corresponding to the tentative positioning value, and the orientation measurement error; evaluating the reliability of the orientation measurement value by determining whether the orientation measurement value indicated by the orientation information present in the time domain is within a positioning gate determined by the tentative positioning value and the tentative positioning accuracy; and determining whether or not the orientation measurement value indicated by the orientation information present in the time domain is within a positioning gate determined by the tentative positioning value and the tentative positioning accuracy,

4. 3. The target trajectory estimation device according to claim 2, wherein the setting of the orientation measurement error associated with an orientation measurement value determined to be an abnormal value by the abnormal value determination unit to reduce the reliability of the orientation measurement value in the trajectory estimation is performed by resetting the value indicated by the orientation measurement error associated with the orientation measurement value determined to be an abnormal value to a parameter value greater than the value indicated by the orientation measurement error associated with an orientation measurement value determined not to be an abnormal value.

5. 4. The target trajectory estimation device according to claim 3, wherein the estimation of the provisional positioning accuracy in the sensor set in the abnormal value determination unit is performed by calculating a lower limit of variance that the provisional positioning value can theoretically take (BCRB: Bayesian Cramer-Rao Boundary) using the position of the orientation sensor corresponding to the provisional positioning value in the sensor set and the orientation measurement error set by an error setting unit.

6. The abnormal value determination unit a positioning gate determination unit which selects a certain number of orientation sensors from the plurality of orientation sensors and groups them into a plurality of sensor groups, estimates a tentative positioning value for each of the plurality of sensor groups for each of the plurality of sensor groups, sets a positioning gate determined by the obtained tentative positioning value, and determines that an orientation measurement value indicated by orientation information present in the time domain is a normal value if it is present within the set positioning gate, and determines that an orientation measurement value indicated by orientation information present in the time domain is an abnormal value if it is not present within any of the set positioning gates; a reliability setting unit that resets the reliability of the trajectory estimation of the azimuth measurement value determined as an abnormal value by the positioning date determination unit to a setting that lowers the reliability of the azimuth measurement value with respect to the trajectory estimation; The target trajectory estimation device according to claim 2 .

7. 7. The target trajectory estimation device according to claim 6, wherein the tentative position value estimation performed by the position determination unit is performed by either a weighted least squares estimation method or a MAP (Maximum a Posteriori) estimation method.

8. 3. The target trajectory estimation device according to claim 2, wherein the determination as to whether or not the orientation measurement value indicates an abnormal value in the abnormal value determination unit comprises: selecting a fixed number of orientation sensors from the plurality of orientation sensors and grouping them into a plurality of sensor groups; estimating a tentative positioning value and a tentative positioning accuracy for each of the plurality of sensor groups for each of the plurality of sensor groups for each time domain divided by the time domain division unit; evaluating the reliability by determining whether the obtained tentative positioning value is within a positioning gate determined by the obtained tentative positioning value and the tentative positioning accuracy; and determining the magnitude of variance between the tentative positioning values ​​for each sensor group with a high evaluated reliability as the degree of abnormality in positioning in the time domain.

9. an accuracy estimation unit that estimates a positioning accuracy for a positioning value estimated by the positioning processing unit, using an orientation measurement error reset by the abnormal value determination unit, for each time domain divided by the time domain division unit; a tracking processing unit that obtains a smoothed value and a predicted value by filtering the positioning value using the positioning value estimated by the positioning processing unit and the positioning accuracy estimated by the accuracy estimation unit corresponding to the positioning value, The abnormal value determination unit resets the reset azimuth measurement error based on the predicted value by the tracking processing unit. The target trajectory estimation device according to any one of claims 1 to 8.

10. the tracking processing unit selects one time domain in which there exists an orientation measurement value determined by the abnormal value determination unit to be not an abnormal value, and obtains a predicted value and a smoothed value in a time domain one time after the tracking start point time domain using a time-forward Kalman filter starting from the tracking start point time domain, which is the selected time domain, and obtains a predicted value and a smoothed value in a time domain one time before the tracking start point time domain using a time-reverse Kalman filter starting from the tracking start point time domain; The smoothed value for each time domain estimated by the tracking processing unit and used by the output unit is set as an output of the target trajectory estimation device. The target trajectory estimation device according to claim 9 .

11. The target trajectory estimation device according to claim 10, wherein the selection of the tracking start time domain by the tracking processing unit selects a time domain in which a period of time during which the variance of the orientation measurement values ​​present in the time domain is small continues or a time domain in which the variance is minimum.

12. 11. The target trajectory estimation device according to claim 10, wherein the selection of the tracking start time region by the tracking processing unit comprises: selecting a fixed number of orientation sensors from the plurality of orientation sensors and grouping them into a plurality of sensor groups; estimating a tentative positioning value and a tentative positioning accuracy for each of the plurality of sensor groups for each of the time regions divided by the time region dividing unit; evaluating reliability by determining whether the obtained tentative positioning value is within a positioning gate determined by the obtained tentative positioning value and the tentative positioning accuracy; determining a degree of variance between the tentative positioning values ​​for each sensor group with high evaluated reliability as a degree of abnormality of positioning in the time region; and selecting a time region in which the degree of abnormality is consistently lower than a set value or a time region with the lowest degree of abnormality.

13. 11. The target trajectory estimation device according to claim 10, wherein the re-setting of the reset orientation measurement error by the abnormal value determination unit is performed by setting, for each time domain divided by the time domain division unit, a prediction gate determined by a prediction accuracy estimated from a prediction value by the tracking processing unit and the positioning accuracy estimated by the accuracy estimation unit, determining that an orientation measurement value indicated by orientation information present in the time domain is within the set prediction gate as a normal value, and determining that an orientation measurement value indicated by orientation information present in the time domain is not within any of the set prediction gates as an abnormal value.

14. an accuracy estimation unit that estimates a positioning accuracy for a positioning value estimated by the positioning processing unit, using an orientation measurement error that is reset by the reliability setting unit in the abnormal value determination unit, for each time domain divided by the time domain dividing unit; a tracking processing unit that obtains a predicted value and a smoothed value by filtering with a Kalman filter using the positioning value estimated by the positioning processing unit and the positioning accuracy estimated by the accuracy estimation unit associated with the positioning value in each of the adjacent time domains divided by the time domain division unit, the abnormal value determination unit includes a prediction gate determination unit that sets a prediction gate determined by a prediction accuracy estimated from the prediction value by the tracking processing unit and the positioning accuracy estimated by the accuracy estimation unit, determines that an orientation measurement value indicated by the orientation information present in the time domain is a normal value when it is present within the set prediction gate, and determines that an orientation measurement value indicated by the orientation information present in the time domain is an abnormal value when it is not present within any of the prediction gates; the reliability setting unit in the abnormal value determination unit resets, among the reset orientation measurement errors, an orientation measurement error associated with an orientation measurement value determined to be an abnormal value by the prediction gate determination unit to a setting that lowers the reliability of the orientation measurement value for the trajectory estimation; The smoothed value for each time domain estimated by the tracking processing unit and used by the output unit is set as an output of the target trajectory estimation device. The target trajectory estimation device according to claim 6.

15. A target trajectory estimation method for measuring a positioning value of a target by a target trajectory estimation device including a time domain division unit, an abnormal value determination unit having a positioning date determination unit and a reliability setting unit, a positioning processing unit, and an output unit, a step in which the time domain division unit divides a time during which each of a plurality of orientation sensors that receives an incoming radio wave from the target observes the target into a plurality of time domains in chronological order, and divides orientation information indicating orientation measurement values ​​obtained by each of the plurality of orientation sensors into each of the divided time domains; a positioning gate determination unit in the abnormal value determination unit selecting a certain number of orientation sensors from the plurality of orientation sensors and classifying them into a plurality of sensor groups, estimating a tentative positioning value for each of the plurality of sensor groups for each of the plurality of sensor groups, setting a positioning gate determined by the obtained tentative positioning value, determining that an orientation measurement value indicated by orientation information present in the time domain is within the set positioning gate as a normal value, and determining that an orientation measurement value indicated by orientation information present in the time domain is not within any of the set positioning gates as an abnormal value; a reliability setting unit in the abnormal value determination unit resetting a reliability of the azimuth measurement error associated with the azimuth measurement value determined to be an abnormal value to a setting that reduces the reliability of the azimuth measurement value with respect to the trajectory estimation; a step in which the positioning processing unit estimates a positioning value for the target in each time domain using a direction measurement value indicated by direction information present in each time domain and a reset direction measurement error associated with the direction measurement value, for each of the divided time domains; a step of outputting object trajectory information indicating a trajectory of the target by connecting the estimated positioning values ​​for each time domain in a time series by the output unit; A target trajectory estimation method comprising: