Particle initialization device, particle initialization method, and particle initialization program

Abstract

To provide a particle initialization device, particle initialization method, and particle initialization program that suppress decline in estimation accuracy in target motion analysis.SOLUTION: A particle initialization device that initializes particles when performing target motion analysis using a particle filter in modified polar coordinates that include distance change rates, is equipped with: an observation azimuth acquisition section that acquires observation azimuth information about the observed target's direction; a confidence region calculation section that calculates a confidence region in which particles are scattered using the observation azimuth information; and a distance change rate determination section that determines whether or not to acquire additional observation azimuth information based on validity of the distance change rate.SELECTED DRAWING: Figure 11

Description

[Technical field]

[0001] The present invention particularly relates to a particle initialization device, a particle initialization method, and a particle initialization program for initializing particles when performing motion analysis of a target using a particle filter. [Background technology]

[0002] Conventionally, in the field of sonar, the motion of a target is predicted from an observation value. In order to predict the motion of a target, it is necessary to estimate the position and velocity of the target, and a method using a particle filter has been proposed as a method for analyzing the motion of such a target. Target motion analysis using a particle filter is a method of expressing the state including the position and velocity of a target with particles. In this target motion analysis, a large number of particles having various target states are generated first. Each particle has a velocity and position state similar to the state of the target to be estimated, and processing is performed to increase particles that match the observation value and reduce particles that do not match the observation value. A point with a high density of particles indicates that the target is likely to exist, so the position and velocity of the target can be estimated by collecting particles in a place where the target is likely to exist.

[0003] Incidentally, in order to perform target motion analysis using a particle filter with a limited number of particles, it is necessary to scatter particles (initialize particles) within a reasonable range around the true state quantity of the target at the time of starting the target motion analysis. For example, Non-Patent Documents 1 to 4 disclose various initialization methods. [Prior art documents] [Non-patent literature]

[0004] [Non-Patent Document 1] Thomas Brehard, Jean-Pierre Le Cadre, Initialization of Particle Filter and Posterior Cramer-Rao Bound for Bearings-Only Tracking in Modified Polar Coordinate System, [Research Report] RR-5074, INRIA, 2004 [Non-Patent Document 2] Jean Dezert and Christian Musso, An Efficient Method for Generating Points Uniformly Distributed in Hyperellipsoids, Proceedings of the Workshop on Estimation, Tracking and Fusion: A Tribute to Yaakov Bar-Shalom. 2001. [Non-Patent Document 3] Y. Bar-Shalom et al., “Estimation with Applications to Tracking and Navigation,” John Wiley and Sons, inc. 2001, PP269-270 [Non-Patent Document 4] Thomas Brehard, Jean-Pierre Le Cadre, Initialization of Particle Filter and Posterior Cramer-Rao Bound for Bearings-Only Tracking in Modified Polar Coordinate System, [Research Report] RR-5074, INRIA, 2004 Summary of the Invention [Problem to be solved by the invention]

[0005] In conventional particle initialization methods, there is a possibility that particles may not exist around the true state of the target due to the influence of observation errors caused by noise. In estimation using a particle filter, if particles do not exist around the true state of the target during initialization, there is a problem that this has a fatal effect on the estimation accuracy.

[0006] The present invention has been made to solve the above-mentioned problems, and aims to provide a particle initialization device, a particle initialization method, and a particle initialization program that suppress the deterioration of estimation accuracy in target motion analysis. [Means for solving the problem]

[0007] The particle initialization device according to the present invention is a particle initialization device that initializes particles when performing target motion analysis using a particle filter in modified polar coordinates including a distance change rate, and includes an observation direction acquisition unit that acquires observation direction information about the observed direction of the target, a trust region calculation unit that calculates a trust region for scattering particles using the observation direction information, and a distance change rate determination unit that determines whether or not to acquire additional observation direction information based on the validity of the distance change rate.

[0008] The particle initialization method according to the present invention is a particle initialization method for initializing particles in modified polar coordinates including a distance change rate when performing a motion analysis of a target using a particle filter, and includes an observation direction acquisition step for acquiring observation direction information about the observed direction of the target, a trust region calculation step for calculating a trust region for scattering particles using the observation direction information, and a distance change rate determination step for determining whether or not to acquire additional observation direction information based on the validity of the distance change rate.

[0009] The particle initialization program according to the present invention is a particle initialization program that causes a processor of a particle initialization device to execute a particle initialization method for initializing particles when performing target motion analysis using a particle filter in modified polar coordinates including a distance change rate, and the particle initialization method includes an observation orientation acquisition step of acquiring observation orientation information about the observed target orientation, a trust region calculation step of calculating a trust region for scattering particles using the observation orientation information, and a distance change rate determination step of determining whether or not to acquire additional observation orientation information based on the validity of the distance change rate. Effect of the Invention

[0010] As described above, according to the present invention, the distance change rate determination unit determines whether or not to obtain additional observation direction information based on the validity of the distance change rate. Therefore, even if an observation error occurs due to noise, the number of observation values ​​can be increased and the particle filter can be initialized only when the total volume of the particles truly exceeds the volume of the confidence region. Therefore, according to the particle initialization device of the present invention, it is possible to suppress a decrease in estimation accuracy in target motion analysis by scattering particles around the true state quantity of the target. [Brief description of the drawings]

[0011] [Figure 1] FIG. 1 is a diagram for illustrating a schematic diagram of a conventional method for initializing particles. [Diagram 2] FIG. 1 is a block diagram showing a conventional particle initialization device. [Diagram 3] 1 is a flowchart showing a conventional particle initialization method. [Figure 4] 1 is a flowchart showing a method for calculating a confidence region in a conventional particle initialization method. [Diagram 5] 1 is a flowchart showing a method for calculating the total volume of particles in a conventional particle initialization method. [Figure 6] FIG. 1 is a diagram for explaining a volume occupied by a particle. [Figure 7] FIG. 13 is a diagram for explaining a method for calculating the volume carried by a particle. [Figure 8]11 is a diagram for explaining a process for determining the start of a desired motion analysis. FIG. [Figure 9] FIG. 1 is a diagram for explaining a problem caused by an observation error. [Figure 10] FIG. 13 is a diagram for explaining a case where the volume of the confidence region is less than the total volume of the particles. [Figure 11] 1 is a block diagram showing a particle initialization device according to a first embodiment. [Figure 12] 3 is a flowchart showing a particle initialization method according to the first embodiment. [Figure 13] 5 is a flowchart showing a method for determining the validity of a distance change rate according to the first embodiment. [Figure 14] 5 is a flowchart showing a method for determining the validity of a distance change rate according to the first embodiment. [Figure 15] FIG. 11 is a block diagram showing a particle initialization device according to a second embodiment. [Figure 16] 10 is a flowchart showing a particle initialization method according to a second embodiment. [Figure 17] 10 is a flowchart showing a method for determining the validity of a distance change rate according to the second embodiment. [Figure 18] FIG. 11 is a block diagram showing a particle initialization device according to a third embodiment. [Figure 19] 11 is a flowchart showing a particle initialization method according to the third embodiment. [Figure 20] 13 is a flowchart showing a method for assigning a possibility evaluation value according to the third embodiment. [Figure 21] FIG. 11 is a diagram showing a speed evaluation value table according to the third embodiment. [Figure 22] 13 is a flowchart showing a method for assigning a possibility evaluation value according to the third embodiment. [Diagram 23] FIG. 13 is a diagram showing a particle position evaluation value table according to the third embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0012] Hereinafter, the embodiments of the present invention will be described with reference to the drawings. The present invention is not limited to the following embodiments, and various modifications can be made without departing from the spirit of the present invention. The present invention also includes all combinations of possible configurations among the configurations shown in the following embodiments. In addition, in each drawing, the same reference numerals are used to denote the same or equivalent parts, and this is common throughout the entire specification.

[0013] Embodiment 1 A particle initialization device 10 according to the embodiment 1 will be described. The particle initialization device 10 according to the embodiment 1 outputs state quantities of particles as a result of initializing the particles to a target motion analysis device that analyzes the motion of a target in order to predict the state of the target from an observation value including information indicating the direction of the target observed by a sonar, a radar, or the like.

[0014] [About target motion analysis] Before describing the particle initialization device 10 according to the first embodiment, target motion analysis using a general particle filter and a conventional particle initialization device 100 will be described. First, target motion analysis using a particle filter will be described. Target motion analysis using a particle filter is a method of expressing the state (position and velocity) of a target with particles. In target motion analysis using a particle filter, a large number of particles having various target states are generated, and by increasing the number of particles that match the observed values ​​and decreasing the number of particles that do not match the observed values, particles are gathered at positions where the target is likely to be in a state, and the state quantity of the target is estimated. A position with a high density of particles indicates the correct target state. Particle X having the target state is generated. r,k (i) can be defined in Cartesian coordinates as equation (1).

[0015]

number

[0016] In formula (1), "v x " indicates the velocity in the x-axis direction, and "v y" indicates the velocity in the y-axis direction, "x" indicates the x-axis coordinate of the particle's position, and "y" indicates the y-axis coordinate of the particle's position. "k" is an index indicating time, "i" is an index indicating the particle number, and "N" is the number of particles.

[0017] However, when the observation value indicates only the direction, the position and velocity of the target are not directly observed. In particular, before the observer changes course, information about the position and velocity of the target is not available. Therefore, before the observer changes course, the particles of the particle filter do not gather at a position with a high probability of being possible, and all state quantities including the position and velocity of the target become indefinite and cannot be estimated.

[0018] Only when the observer changes course will the particles gather at a position with the highest possible state, and all state quantities can be estimated from that collection of particles. MAP estimation and EAP estimation are known methods for estimating the target's position and velocity from particles. However, these methods have the disadvantage that the solution is difficult to stabilize, since the particles do not converge unless the observer changes course.

[0019] In such a case, as disclosed in, for example, Non-Patent Document 1, modified polar coordinates are used, which is a coordinate system that can separate variables that can be estimated from the observed value of only the azimuth before the observer changes course (observable state variables) from variables that cannot be estimated (unobservable state variables). r,k (i) is expressed in modified polar coordinates as equation (2).

[0020]

number

[0021] In equation (2), "β" indicates the direction and "r" indicates the distance. "β'" indicates the direction change and "r' / r" indicates the rate of change in distance. In the following explanation, letters such as "β" with a dot above them in the formula will be written as "β'". "k" is an index indicating the time, and "i" is an index indicating the particle number. Furthermore, "N" is the number of particles. Here, we will explain the case where the reciprocal of the distance is used as the modified polar coordinate system, but logarithmic distance may be used instead of the reciprocal of the distance.

[0022] Using modified polar coordinates, particle Y 1,k (i) ~Y 3,k (i) Since the state variables of can be converged to some extent before changing course, particles can be gathered within a highly probable range even before changing course. 4,k (i) Therefore, the use of modified polar coordinates has a higher chance of obtaining a stable solution than the Cartesian coordinate system.

[0023] Here, in order to perform target motion analysis using a particle filter with a limited number of particles, it is necessary to scatter particles in a reasonable range around the true state quantity of the target at the start of target motion analysis (initialize the particles). In a Cartesian coordinate system, it is sufficient to scatter particles within the region of the expected position and within the expected speed range. On the other hand, in a modified polar coordinate system, it is difficult to predict the possible ranges of the azimuth change and distance change rate in advance, and it has been a challenge to predict the possible ranges of all state variables.

[0024] To solve this problem, the conventional non-patent document 1 discloses the following method that utilizes the fact that the observable state variables can be converged to a certain extent by acquiring multiple observation directions. First, a range (trust region) in which the observable state variables are likely to be from the observation direction is calculated. Then, when N particles are uniformly scattered within the calculated trust region, it is determined whether the amount of movement of each particle (the volume carried by the particle) can satisfy the range of the trust region. This makes it possible to determine whether it is necessary to collect more observation values ​​to narrow the trust region, or whether it is possible to perform target motion analysis by scattering particles within the current trust region.

[0025] FIG. 1 is a diagram for explaining a conventional method for initializing particles. The coordinates in FIG. 1 are originally four-dimensional space in a modified polar coordinate system, but for the sake of simplicity, they are shown in two-dimensional space of Y1 and Y2. First, the number of particles to be scattered is set. Next, the observation directions for 2k+1 particles from time 0 to time 2k are used to calculate the observable state variable Y 1,k ~Y 3,k The confidence region of the target is calculated. The confidence region is calculated by first finding the estimated value of the target's state quantity at time k estimated from 2k+1 observation directions (the center value of the confidence region) as shown in Figure 1(a), and then finding the spread of the distribution (the range of the confidence region). The spread of the distribution can be found from the Fisher information matrix (FIM). The confidence region can be narrowed by increasing the number of observation directions.

[0026] Unobservable state variable Y 4,k Regarding is uncertain before the course change, and the confidence region does not change even if observations are collected, so it is calculated by 1 / R max ~1 / R min This is a fixed interval. min " is the minimum distance, and "R max " is the maximum distance. The range for detecting a target is determined by the performance of the observation equipment.

[0027] From these, the volume of the trust region V(CA(Y^ k In the following explanation, a letter such as "Y" with a "^" above it in a formula will be written as "Y^".

[0028] As shown in Figure 1(c), when N particles are uniformly distributed within the trust region, the range in which each particle can move in four-dimensional space (the volume carried by the particle) is restricted by the disturbance given by the Markov process in the particle filter. The volume carried by each particle V(B(Y k (i) ) is found, the sum of the volumes carried by all particles (total volume of the particles)Σ i=1 N V(B(Y k (i) )) and the volume of the trust region V(CA(Y^ k )) to determine whether the particle filter works correctly with the N particles scattered within the current trust region.

[0029] For example, as shown in Fig. 1(c), the volume of the trust region V(CA(Y^ k ))>total volume of particles Σ i=1 N V(B(Y k (i) )), if the true state of the target is outside the range of the particle, the particle filter cannot capture the true state of the target. In contrast, as shown in Figure 1(d), the volume of the trust region V(CA(Y^ k )) ≦ total volume of particles Σ i=1 N V(B(Y k (i) )), the particle filter can capture the true state of the target. Therefore, the conventional method of initializing particles is to use the volume of the trust region V(CA(Y^ k )) ≦ total volume of particles Σ i=1 N V(B(Y k (i))) is satisfied and it is determined that the region is sufficiently reliable for the N particles to be scattered. Then, when the above condition is satisfied, particles are uniformly scattered within the region as an initialization process, and the calculation of the target motion analysis by the particle filter starts from time k.

[0030] [Regarding the conventional particle initialization device 100] Next, a conventional particle initialization device 100 employing the above conventional initialization method will be described. FIG. 2 is a block diagram showing the conventional particle initialization device 100. The conventional particle initialization device 100 initializes the state quantity of the particles based on the observation direction information of the target inputted as an observation value to the first input terminal 30 and the number N of particles inputted to the second input terminal 40, and outputs the initialization result from the output terminal 50. As shown in FIG. 2, the particle initialization device 100 includes, as functional units, an observation direction acquisition unit 11, a trust region calculation unit 12, a trust region volume calculation unit 13, a particle total volume calculation unit 14, and an analysis start determination unit 15. The particle initialization device 100 is composed of a calculation device such as a microcomputer that realizes each functional unit by a processor reading and executing a program stored in a memory, or hardware such as a circuit device corresponding to each functional unit.

[0031] The observation direction acquisition unit 11 receives 2k+1 pieces of observation direction information Z0 to Z1 from time 0 to time 2k input to the first input terminal 30. 2k The observation direction acquisition unit 11 outputs observation direction information, which is information about the direction of the observed target, to the confidence region calculation unit 12.

[0032] The trust region calculation unit 12 calculates the 2k+1 pieces of observation direction information Z0 to Z 2k Based on this, the target state estimate Y^, which is an observable variable at time k that is the center value of the trust region, is calculated. r,k , r = 1, 2, 3, and the Fisher information matrix J(Y^ r,k), r=1, 2, 3. The trust region calculation unit 12 outputs the Fisher information matrix to the trust region volume calculation unit 13. The trust region calculation unit 12 also outputs the target state quantity estimate, which is an observable variable at time k, and the Fisher information matrix to the total particle volume calculation unit 14.

[0033] The trust region volume calculation unit 13 calculates the Fisher information matrix J(Y^ r,k ),r=1,2,3, and the expected minimum distance R min and the maximum distance R max Based on this, the volume of the trust region V(CA(Y^ k The trust region volume calculation unit 13 outputs the calculated volume of the trust region to the analysis start determination unit 15.

[0034] The particle total volume calculation unit 14 calculates the number N of particles input to the second input terminal 40, the target state quantity estimate Y^, which is an observable variable at time k output from the trust region calculation unit 12, r,k , r=1,2,3, and Fisher's information matrix J(Y^ r,k ), r = 1, 2, 3, the state quantity Y of the particle at time k r,k (i) , r=1,2,3,4,i=1,2,…,N, and the total volume of the particles V(B(Y k ) is calculated. The total particle volume calculation unit 14 outputs the calculated state quantities of the particles at time k and the total volume of the particles to the analysis start determination unit 15. The number N of particles is input from the second input terminal 40 only when the particle initialization device 100 is started up for the first time.

[0035] The analysis start determination unit 15 determines whether the target motion analysis can be started based on the volume of the trust region output from the trust region volume calculation unit 13 and the total volume of the particles output from the total particle volume calculation unit 14. If the determination condition is satisfied, the state quantity Y r,k (i) , r=1,2,3,4,i=1,2,...,N are output to output terminal 50.

[0036] (Conventional particle initialization method) The details of the initialization process by the conventional particle initialization device 100 having the above-mentioned configuration will be described with reference to Fig. 3. Fig. 3 is a flowchart showing a conventional particle initialization method. First, the observation direction acquisition unit 11 acquires 2k+1 pieces of observation direction information Z0 to Z1 from time 0 to time 2k. 2k (Step S1).

[0037] Next, the trust region calculation unit 12 calculates the 2k+1 pieces of observation direction information Z0 to Z 2k Based on this, the target state estimate Y^, which is an observable variable at time k that is the center value of the trust region, is calculated. r,k , r = 1, 2, 3, and the Fisher information matrix J(Y^ r,k ), r=1, 2, 3 is calculated (step S2).

[0038] A method for calculating the trust region will be described in detail with reference to FIG. 4. FIG. 4 is a flowchart showing a method for calculating the trust region in a conventional particle initialization method. First, the trust region calculation unit 12 calculates 2k+1 pieces of observation direction information Z0 to Z1 output from the observation direction acquisition unit 11. 2k Based on this, the target state estimate Y^, which is an observable variable at time k that is the center value of the trust region, is calculated. r,k , r=1, 2, 3 are calculated (step S21). Specifically, the state quantity estimate of the target, which is an observable variable at time k, is calculated using the following nonlinear equation, Equation (3).

[0039]

number

[0040] In formula (3), "Z t " indicates the 2k+1 observation directions, and "δ t " indicates the time interval of the observation direction to be input. 1,k " indicates the target's heading change, and "Y 2,k " indicates the target range change rate, and "Y 3,k" indicates the target's direction. Furthermore, "σ v 2 " indicates the error variance of the observed direction, and "W" has a mean of 0 and a variance of σ v 2 The Gaussian noise distribution of t is shown. "t" is an index indicating the time from 0 to 2k, and "k" is an index indicating the center time of t.

[0041] Equation (3) is "observation direction Z at time t" t is the target's heading Y at time k 3,k and the change in orientation Y at time k 1,k and the rate of change of distance Y 2,k Using the time difference (tk) from -1 ((tk)δ t Y 1,k / 1+(tk)δ t Y 2,k ) and the noise distribution W. Here, the noise distribution W is assumed to be Gaussian, so the observation direction Z t The state variable Y given r,k The possible probability of is given by the following equation (4).

[0042]

number

[0043] In formula (4), "Y r,k ,r=1,2,3" indicates the observable state variables at time k, and "P(Y r,k │Z t )" is the 2k+1 observation direction Z t The observable state variable Y at time k under the condition r,k The probability density function of P(Y r,k │Z t ) is obtained as the maximum value of the target estimate Y^ r,k,r=1,2,3 can be calculated. Note that since equation (4) is a nonlinear equation, it is not easy to solve analytically. For this reason, a numerical solution such as the Gauss-Newton method (GN) is usually used. An explanation of the numerical solution method is omitted here, but any method that can find a solution may be used.

[0044] Next, the trust region calculation unit 12 calculates the Fisher information matrix J(Y^ r,k ), r=1, 2, 3 is calculated (step S22). The Fisher information matrix can be calculated as shown in equation (5).

[0045]

number

[0046] In equation (5), "Λ(Y r,k │Z t )" is the 2k+1 observation direction Z t The target estimate Y^ of the observable state variables at time k under the condition r,k ,r=1,2,3, and "ln(Λ(Y r,k │Z t ))" indicates the log-likelihood function. Also, "Δ Yr,k Yr,k " is Y r,k are the second derivatives with respect to Y 1,k ~Y 3,k This shows that the second partial differentiation with respect to σ v 2 " indicates the error variance of the observed direction, and "E{·}" means taking the expected value.

[0047] Returning to FIG. 3, the trust region volume calculation unit 13 calculates the Fisher information matrix J(Y^ r,k ),r=1,2,3, and the expected minimum distance R min and the maximum distance R max Using this, the volume of the trust region V(CA(Y^ k) is calculated (step S3). The volume of the trust region is calculated as shown in equation (6).

[0048]

number

[0049] In equation (6), "J(Y^ r,k )" indicates the Fisher information matrix output from the trust region calculation unit 12, and "det(·)" indicates a function for calculating the determinant. min " indicates the minimum distance, and "R max " indicates the maximum distance. In this way, the range for detecting a target is determined by the performance of the observation equipment, so the upper and lower limits are used. "Γ" is the gamma function. "χ4 2 "(1-α)" is the (1-α) quantile of the chi-square distribution with four degrees of freedom. In general, the value of α is often set to 0.05 (=5%), but a representative value can be chosen from the appropriate width of the confidence region in the target motion analysis.

[0050] "(πχ4 2 (1-α) (3 / 2) / Γ(5 / 2)√(det(J(Y^ r,k The term Y 1,k ~Y 3,k The confidence region for (1 / R min )-(1 / R max ) term is Y 4,k This is a term that indicates the possible range of distances. Since this is an unobservable state variable, even if observed directions are collected before a course change, the distribution of distances will not narrow. Therefore, the range is set by assuming that the target exists within this interval as a uniform distribution.

[0051] The particle total volume calculation unit 14 calculates the number N of particles input to the second input terminal 40, the target state quantity estimate Y^, which is an observable variable at time k output from the trust region calculation unit 12, r,k , r=1,2,3, and Fisher's information matrix J(Y^ r,k), r = 1, 2, 3, the state quantity Y of the particle at time k r,k (i) , r=1,2,3,4,i=1,2,…,N, and the total volume of the particles V(B(Y k ) is calculated (step S4). Note that steps S3 and S4 may be processed in parallel.

[0052] A method for calculating the total volume of particles will be described in detail with reference to FIG. 5. FIG. 5 is a flowchart showing a method for calculating the total volume of particles in a conventional particle initialization method. First, the total particle volume calculation unit 14 uniformly scatters N particles (step S41). In particular, Y 1,k ~Y 3,k Regarding the above, the estimated state value Y^ of the target, which is an observable variable at time k output from the trust region calculation unit 12, is r,k , r=1,2,3, and Fisher's information matrix J(Y^ r,k ),r=1,2,3, the target estimate Y^ of the observable state variables at time k is r,k ,r=1,2,3 is the center value, ±1 / √(J(Y^ r,k )) within the confidence region of Y 4,k Regarding (1 / R min )-(1 / R max ) in the interval. By this scattering, the state quantity Y r,k (i) , r=1,2,3,4,i=1,2,...,N are determined. As an example of the scattering method, the method described in Non-Patent Document 2 can be used, but any method can be used as long as it can scatter the particles uniformly.

[0053] Next, the particle total volume calculation unit 14 calculates the volume carried by each particle from the state quantity of the particles scattered within the trust region (step S42). The volume carried by the particle will be described with reference to FIG. 6. FIG. 6 is a diagram for explaining the volume carried by the particle. The processing flow shown on the left side of FIG. 6 shows the processing of a general particle filter. In "sampling", particles are generated according to a probability density function that is held in advance. In "weight update", weights are given to the particles generated by sampling according to the observed value. In "resampling", the weights of the particles are equalized by generating particles according to the weights of the particles. In the "Markov process", in order to estimate the state of the particle at the next time, the state quantity from time k-1 to time k is changed according to the model and a disturbance W is given, as shown on the right side of FIG. 6. The result is used as the particle at the next sampling time, and this is repeated.

[0054] In the initialization of particles, in order for the particles scattered within the trust region to converge to the true value by operating the particle filter, the range in which each particle can move due to this disturbance is required. Therefore, the volume carried by a particle is determined by the variance of the disturbance in this Markov process.

[0055] The process of step S42 will be described with reference to FIG. 7. FIG. 7 is a diagram for explaining a method for calculating the volume carried by a particle. In step S42, assuming that the particles dispersed at time k-1 have shifted to the dispersion position at time k, the volume that may move due to disturbance during the process of this shift is calculated. FIG. 7(a) shows the state transition of a particle that has changed over time from time k to time k-1. First, the total particle volume calculation unit 14 calculates the state quantity Y r,k (i) , r=1,2,3,4,i=1,2,...,N, the particle state quantity is reversed to that at time k-1 without any disturbance. FIG. 7(b) shows the state transition of a particle that changes over time from time k-1 to time k. Then, the total particle volume calculation unit 14 calculates the particle state quantity at time k-1 from the particle state quantity at time k, and then disturbs the particle state quantity at time k-1 to obtain the particle state quantity Y at time k that is affected by the disturbance. r,k (i),r=1,2,3,4,i=1,2,…,N. The disturbance of the state of each particle at time k can be defined as equation (7), and from this equation, the volume V(B(Y k (i) )) can be obtained.

[0056]

number

[0057] In equation (7), "B(Y r,k (i) )" indicates the disturbance of the state quantity of each particle at time k, "A" indicates the matrix that represents the change in one time step in the Cartesian coordinate system, and "f mp c " indicates a conversion function from a modified polar coordinate system to a Cartesian coordinate system. Furthermore, "Q" indicates the covariance given in the case of a continuous white noise model when an acceleration disturbance is added to the velocity, and "η" indicates the standard deviation of the process noise given to the acceleration. Details of this formula are described in Non-Patent Document 1. Furthermore, the "symbol combining a circle and a cross" indicates the Kronecker product, and "Id2" indicates a 2-row, 2-column unit matrix. In addition, Non-Patent Document 4 indicates that a specific calculation method for formula (7) is given by formula (8).

[0058]

number

[0059] In equation (8), "V(B(Y k (i) ))" indicates the volume carried by each particle. From equation (8), Y 4,k-1 (i) By calculating Y, the volume of each particle can be calculated. 4,k-1 (i) As described in Non-Patent Document 1, can be calculated as shown in Equation (9).

[0060]

number

[0061] In the formula (9), "δ t " indicates the time interval of the observation direction to be input.

[0062] 5, the particle total volume calculation unit 14 sums up the volumes of the individual particles (step S43). The total volume of the particles can be calculated as shown in Equation (10).

[0063]

number

[0064] In equation (10), "V(B(Y k ))" indicates the total volume of the particle. Also, "i" is the index indicating the particle number, and "N" is the number of particles.

[0065] The analysis start determination unit 15 determines whether or not the target motion analysis can be started based on the volume of the trust region output from the trust region volume calculation unit 13 and the total volume of particles output from the total particle volume calculation unit 14. Specifically, the determination is made based on the determination condition defined in equation (11) (step S5).

[0066]

number

[0067] "V(CA(Y^ k ))" indicates the volume of the confidence region, and "V(B(Y k ))" indicates the total volume of the particle. If the judgment condition is satisfied (step S5: YES), it is determined that the target motion analysis can be started. In this case, the analysis start judgment unit 15 judges whether the state quantity Y 1,k (i) ~Y 4,k (i)is output to the output terminal 50 as the initialization result, and the target motion analysis is started using the output value. If the judgment condition is not satisfied (step S5: NO), it is determined that the target motion analysis cannot be started. In this case, the particle initialization device 100 updates k by one time without outputting anything, and repeats the process from step S1. In other words, two new observation directions are acquired, and the state quantities of the particles are recalculated.

[0068] The process of step S5 will be described with reference to FIG. 8. FIG. 8 is a diagram for explaining the process of determining the start of target motion analysis. As shown in FIG. 8, particles are scattered within the trust region. Each particle has a volume due to the influence of disturbances in the Markov process. Furthermore, the process of step S5 is repeated, and as the number of observation directions increases, the trust region becomes smaller and the particles are scattered more densely. Then, when the determination condition defined in equation (11) is satisfied, it is determined that the trust region is narrowed sufficiently and that the particles can be scattered within the current trust region to perform target motion analysis.

[0069] (Problems with the conventional particle initialization device 100) In the conventional particle initialization method, the effect of noise-induced observation errors can cause the range rate to become an outlier, resulting in no particles existing around the true state of the target. In particular, the range rate, Y 2,k =r'k / rk, the distribution may not be Gaussian and outliers may easily occur. In estimation using a particle filter, if there are no particles around the target true state quantity at the time of initialization, there is a problem that this has a fatal effect on the estimation accuracy. This problem will be further explained from the following two perspectives.

[0070] First, when the noise characteristics are poor with a small number of observations, outliers occur in the range change rate that constitutes the trust region, and the possible range of movement of particles scattered in the trust region is overestimated. This overestimation may result in each scattered particle being determined to fill the trust region even though the trust region is not sufficiently smaller than the volume carried by the particle. In this case, an event may occur in which no particles exist around the true state of the target at the time of initialization, making it impossible to estimate the state of the target in the target motion analysis.

[0071] Secondly, when the noise quality is poor with a small number of observations, outliers occur in the range change rates that make up the trust region, and the outliers in the range change rates cause a large deviation in the estimate of the center value of the trust region, and the trust region found near the shifted center value is underestimated. This can result in the formation of a trust region that does not include the true state quantity of the target. Figure 9 is a diagram for explaining problems caused by observation errors. Figure 9 is a two-dimensional diagram of a cost function with the range change rate, which is the cause of the problem, on the horizontal axis, and the trust region is found by a quadratic surface approximation of the cost function. The cost function is expressed by the following equation (12).

[0072]

number

[0073] Equation (12) is a nonlinear function obtained by taking the logarithm of equation (4). The solid line in Fig. 9 shows the cross section of the cost function of the range change rate where the heading change takes the true value, and the dashed line shows the cross section of the cost function of the range change rate where the heading change cost obtained by the Gauss-Newton (GN) method takes the minimum value.

[0074] The left diagram in Figure 9 shows a cross section of the cost function when the nature of the observation error is not bad and no outliers occur in the range change rate, even with a small number of observations. As shown in the left diagram in Figure 9, the center value of the trust region obtained by the numerical solution has a characteristic close to a single peak. For this reason, particles are scattered around the true state quantity of the target.

[0075] On the other hand, the right diagram of Fig. 9 shows a cross section of the cost function when an outlier occurs in the range change rate due to the influence of bad observation errors in the case of a small number of observation values. As shown in the right diagram of Fig. 9, the shape of the cost function has a small pot-shaped characteristic, and the center value of the trust region is significantly shifted from the true state quantity of the target. The trust region obtained from the vicinity of the shifted center value is underestimated, and the trust region in the range change rate direction is narrowed. As a result, even though the trust region is not actually small enough, the volume of the trust region is underestimated, so the above formula (11) is established, and the initialization of the particles is completed in a situation where the true state quantity of the target is not included in the obtained trust region. This can cause an event in which particles do not exist around the true state quantity of the target, and a problem can occur in which the state of the target cannot be estimated in the target motion analysis.

[0076] Furthermore, a problem other than the above-mentioned observation error will also be explained. Figure 10 is a diagram for explaining the case where the volume of the trust region is less than the total volume of the particles. In Figure 10, the horizontal axis is the number of observations, and the vertical axis is the volume of the trust region and the total volume of the particles. The right diagram of Figure 10 is an enlarged view of the left diagram. As shown in Figure 10, even if the trust region is sufficiently narrow, there are cases where the volume of the trust region does not converge to the total volume of the particles, and the target motion analysis cannot be started. The cause of this is that when the target motion analysis is performed using a particle filter, depending on the scenario before the course change, there is a limit to the convergence of the trust region even if the observation values ​​are increased, and the number of particles set initially is insufficient to capture the trust region.

[0077] [Configuration of particle initialization device 10] FIG. 11 is a block diagram showing a particle initialization device 10 according to the first embodiment. As shown in FIG. 11, the particle initialization device 10 includes, as functional units, an observation direction acquisition unit 11, a trust region calculation unit 12, a trust region volume calculation unit 13, a total particle volume calculation unit 14, an analysis start determination unit 15, and a first distance change rate determination unit 21. Such a particle initialization device 10 is configured by a calculation device such as a microcomputer that realizes each functional unit by a processor reading and executing a program stored in a memory, or hardware such as a circuit device corresponding to each functional unit. In the particle initialization device 10 according to the first embodiment, the configurations of the observation direction acquisition unit 11, the trust region calculation unit 12, the trust region volume calculation unit 13, the total particle volume calculation unit 14, and the analysis start determination unit 15 are common to those of the conventional particle initialization device 100. Therefore, detailed description will be omitted.

[0078] In addition to the conventional functions of the observation direction acquisition unit 11, the observation direction acquisition unit 11 also acquires observation direction information when the first distance change rate determination unit 21 determines that it is necessary to additionally acquire observation direction information. The observation direction acquisition unit 11 outputs the observation direction information to the confidence region calculation unit 12.

[0079] The trust region calculation unit 12 uses the observation direction information output from the observation direction acquisition unit 11 to calculate the target state quantity estimate, which is an observable variable at time k that is the center value of the trust region, and the Fisher information matrix, and outputs them to the first distance change rate determination unit 21.

[0080] The first distance change rate determination unit 21 uses the Fisher information matrix J(Y^ r,k), r=1,2,3, the validity of the distance rate is judged, and based on the result of the judgment of the validity of the distance rate, whether or not to additionally acquire observation direction information is judged. Specifically, the first distance rate judgment unit 21 judges the validity of the section in which the distance rate can move. If the first distance rate judgment unit 21 judges that the section in which the distance rate can move is valid, it outputs the Fisher information matrix to the trust region volume calculation unit 13. In addition, the first distance rate judgment unit 21 outputs the Fisher information matrix and the state quantity estimate of the target, which is an observable variable at time k, to the total particle volume calculation unit 14. On the other hand, if the first distance rate judgment unit 21 judges that the section in which the distance rate can move is not valid, it instructs the observation direction acquisition unit 11 to acquire two new observation directions and update k by one time. In this case, the trust region calculation unit 12 executes the above-mentioned process again. The first distance rate judgment unit 21 corresponds to the "distance rate judgment unit" of the present invention.

[0081] The confidence region volume calculation unit 13 calculates the volume of the confidence region based on the Fisher information matrix output from the first distance change rate determination unit 21 , and outputs it to the analysis start determination unit 15 .

[0082] The total particle volume calculation unit 14 calculates the state quantity of the particle and the total volume of the particle at time k based on the Fisher information matrix output from the first distance change rate determination unit 21 and the state quantity estimate of the target, which is an observable variable at time k, and outputs the calculated state quantity and the total volume of the particle to the analysis start determination unit 15.

[0083] The analysis start determination unit 15 determines whether or not the target motion analysis can be started based on the volume of the trust region output from the trust region volume calculation unit 13 and the total volume of the particles output from the total particle volume calculation unit 14. When the analysis start determination unit 15 determines that the target motion analysis can be started, it outputs the state quantity of the particles at time k output from the total particle volume calculation unit 14 to the output terminal 50. When the analysis start determination unit 15 determines that the target motion analysis cannot be started, the particle initialization device 10 executes the processing by the observation orientation acquisition unit 11 again without outputting anything to the output terminal 50.

[0084] (Particle initialization method) A particle initialization method by the particle initialization device 10 of the embodiment 1 will be described. Note that, in the particle initialization method of the embodiment 1, the observation orientation acquisition unit 11, the trust region calculation unit 12, the trust region volume calculation unit 13, the total particle volume calculation unit 14, and the analysis start determination unit 15 perform the same processes as in the particle initialization method by the conventional particle initialization device 100, and therefore detailed descriptions thereof will be omitted.

[0085] 12 is a flowchart showing the particle initialization method according to the embodiment 1. Steps S1 to S5 of the particle initialization method according to the embodiment 1 are similar to steps S1 to S5 of the conventional particle initialization method. However, the observation direction acquisition unit 11 acquires observation direction information and outputs it to the confidence region calculation unit 12 even when the first distance change rate determination unit 21 determines that the distance change rate is not valid.

[0086] As shown in FIG. 12, the first distance change rate determination unit 21 calculates the Fisher information matrix J(Y^ r,k ), r=1, 2, 3, the validity of the range change rate is judged, and based on the result of the judgment of the validity of the range change rate, it is judged whether or not to additionally acquire observation direction information (step S11).

[0087] FIG. 13 is a flowchart showing a method for determining the validity of a distance change rate according to the first embodiment. The method for determining the validity of a distance change rate will be described with reference to FIG. 13. First, the first distance change rate determination unit 21 calculates the inverse matrix of the Fisher information matrix output from the trust region calculation unit 12 to obtain the Cramer-Rao lower bound J(Y^ r,k ) -1 ,r=1,2,3. Then, find the eigenvalues ​​from the Cramer-Rao lower bound, and find the maximum eigenvalue A max_eigvalue is calculated (step S111).

[0088] As mentioned above, if an outlier occurs in the distance rate that constitutes the confidence region, information about the outlier is reflected in the maximum eigenvalue of the Cramer-Rao lower bound. Therefore, the validity of the distance rate can be determined by calculating the range in which the distance rate can move from the maximum eigenvalue. The maximum eigenvalue A of the Cramer-Rao lower bound max_eigvalue can be calculated as shown in equation (13).

[0089]

number

[0090] In equation (13), "eig(J(Y^ r,k ) -1 )" indicates the Cramer-Rao lower bound eigenvalue. Also, "max(·)" is a function that outputs the maximum value in (·), and "eig(·)" is a function that calculates the eigenvalues ​​of a matrix.

[0091] Next, the first distance change rate determination unit 21 determines a range B within which the distance change rate can move based on the maximum eigenvalue of the Cramer-Rao lower limit. rangeate_area (Step S112). rangeate_area can be calculated as shown in equation (14).

[0092]

number

[0093] In equation (14), "χ4 2 "(α)" is the (1-α) quantile of the chi-square distribution with four degrees of freedom. Generally, the value of α is set to 0.05 (=5%), but a representative value can be taken from an appropriate width of the confidence region in the target motion analysis, as in the conventional particle initialization method.

[0094] The first distance change rate determination unit 21 then uses the range in which the distance change rate can move as a determination value and sets the threshold value γ thr_rangerateIt is determined whether the section in which the range change rate can move is valid by using the formula (15) (step S113).

[0095]

number

[0096] In equation (15), "γ thr_rangerate " is set from the viewpoint of judging whether the section in which the distance rate can move is due to an outlier. If the condition of formula (15) is satisfied (step S113: YES), the first distance rate judgment unit 21 judges that the section in which the distance rate can move is valid, and outputs the Fisher information matrix to the trust region volume calculation unit 13. In addition, the first distance rate judgment unit 21 outputs the target state quantity estimate, which is an observable variable at time k, and the Fisher information matrix to the total particle volume calculation unit 14. If the condition is not satisfied (step S113: NO), the first distance rate judgment unit 21 judges that the section in which the distance rate can move is not valid, and instructs the observation direction acquisition unit 11 to acquire two new observation directions and update k by one time. In this case, the state quantities of the particles are recalculated at the trust region calculation unit 12 and subsequent units.

[0097] In determining the validity of the distance change rate in first distance change rate determination unit 21, instead of the process described in FIG. 13, the maximum range of the distance change rate direction of the trust region can be determined as the determination value, as shown in FIG. 14. FIG. 14 is a flowchart showing a method of determining the validity of the distance change rate according to the first embodiment. First, first distance change rate determination unit 21 performs inverse matrix calculation of the Fisher information matrix output from trust region calculation unit 12, and determines the range CA(Y^ k ) is calculated (step S114). k ) can be calculated as shown in equation (16).

[0098]

number

[0099] Next, the first distance change rate determination unit 21 calculates the trust region CA(Y^ k ) to the maximum value β max , and the minimum value β min Calculate the maximum range in the range change rate direction β rangerate_max (Step S115) The maximum range in the range change rate direction can be calculated as shown in Equation (17).

[0100]

number

[0101] In formula (17), "max{}" is a function that outputs the maximum value among the variables in {}.

[0102] The first distance change rate determination unit 21 determines the maximum range β rangerate_max is used as the judgment value, and the threshold γ thr_rangerate2 The validity of the range change rate is determined using equation (18) (step S116).

[0103]

number

[0104] In equation (18), "threshold γ thr_rangerate2" is set from the viewpoint of judging whether the maximum range of the distance change rate direction is due to an outlier. If the condition of formula (18) is satisfied (step S116: YES), the first distance change rate judgment unit 21 judges that the maximum range of the distance change rate is valid, and outputs the Fisher information matrix to the trust region volume calculation unit 13. In addition, the first distance change rate judgment unit 21 outputs the target state quantity estimate, which is an observable variable at time k, and the Fisher information matrix to the total particle volume calculation unit 14. If the condition is not satisfied (step S116: NO), the first distance change rate judgment unit 21 judges that the maximum range of the distance change rate is not valid, and instructs the observation direction acquisition unit 11 to acquire two new observation directions and update k by one time. In this case, the state quantities of the particles are recalculated at the trust region calculation unit 12 and subsequent units.

[0105] As described above, according to the first embodiment, the first distance change rate determination unit 21 determines whether or not to perform additional acquisition of observation azimuth information based on the validity of the distance change rate. Therefore, even if an outlier caused by an observation error occurs in the distance change rate constituting the trust region, the number of observation values ​​can be increased to prevent an event in which the range in which particles scattered in the trust region can move is overestimated. Therefore, the initialization of the particle filter is performed only when the total volume of the particles truly exceeds the volume of the trust region, regardless of the outlier. Therefore, according to the particle initialization device 10 of the first embodiment, it is possible to suppress a decrease in the estimation accuracy in the target motion analysis by scattering particles around the true state quantity of the target.

[0106] Embodiment 2 FIG. 15 is a block diagram showing a particle initialization device 10A according to the second embodiment. As shown in FIG. 15, the particle initialization device 10A includes, as functional units, an observation direction acquisition unit 11, a trust region calculation unit 12, a trust region volume calculation unit 13, a total particle volume calculation unit 14, an analysis start determination unit 15, and a second distance change rate determination unit 22. Such a particle initialization device 10A is configured by a calculation device such as a microcomputer that realizes each functional unit by a processor reading and executing a program stored in a memory, or hardware such as a circuit device corresponding to each functional unit. In the particle initialization device 10A according to the second embodiment, the configurations of the observation direction acquisition unit 11, the trust region calculation unit 12, the trust region volume calculation unit 13, the total particle volume calculation unit 14, and the analysis start determination unit 15 are common to those of the conventional particle initialization device 10. Therefore, detailed description will be omitted.

[0107] In addition to the conventional functions of the observation direction acquisition unit 11, the observation direction acquisition unit 11 also acquires observation direction information when the second distance change rate determination unit 22 determines that it is necessary to additionally acquire observation direction information. The observation direction acquisition unit 11 outputs the observation direction information to the confidence region calculation unit 12.

[0108] The trust region calculation unit 12 uses the observation direction information output from the observation direction acquisition unit 11 to calculate the target state quantity estimate, which is an observable variable at time k, which is the center value of the trust region, and the Fisher information matrix, and outputs them to the trust region volume calculation unit 13 and the total particle volume calculation unit 14.

[0109] The trust region volume calculation unit 13 calculates the volume of the trust region based on the Fisher information matrix output from the trust region calculation unit 12 , and outputs it to the second distance change rate determination unit 22 .

[0110] The total particle volume calculation unit 14 calculates the state quantity and the total volume of the particle at time k based on the Fisher information matrix output from the trust region calculation unit 12 and the state quantity estimate of the target, which is an observable variable at time k, and outputs the calculated state quantity and the total volume of the particle to the second distance change rate determination unit 22.

[0111] The second distance change rate determination unit 22 calculates the volume V(CA(Y^ k )) to determine the validity of the distance change rate, and based on the result of the determination of the validity of the distance change rate, to determine whether or not to additionally obtain observation direction information. Specifically, the second distance change rate determination unit 22 determines the validity of the change rate with respect to the increase in the observed value of the distance change rate. When the second distance change rate determination unit 22 determines that the change rate with respect to the increase in the observed value of the distance change rate is valid, it outputs to the analysis start determination unit 15 the volume of the trust region at time k calculated by the trust region volume calculation unit 13, and the state quantity of the particle at time k and the total volume of the particle calculated by the total particle volume calculation unit 14. On the other hand, when the second distance change rate determination unit 22 determines that the change rate with respect to the increase in the observed value of the distance change rate is not valid, it instructs the observation direction acquisition unit 11 to newly obtain two observation directions and update k by one time. In this case, the trust region calculation unit 12 executes the above-mentioned process again. The second distance change rate determination unit 22 corresponds to the "distance change rate determination unit" of the present invention.

[0112] The analysis start determination unit 15 determines whether or not the target motion analysis can be started based on the volume of the confidence region, the state quantity of the particle at time k, and the total volume of the particle output from the second distance change rate determination unit 22. When the analysis start determination unit 15 determines that the target motion analysis can be started, it outputs the state quantity of the particle at time k output from the second distance change rate determination unit 22 to the output terminal 50. When the analysis start determination unit 15 determines that the target motion analysis cannot be started, the particle initialization device 10A executes the process by the observation orientation acquisition unit 11 again without outputting anything to the output terminal 50.

[0113] (Particle initialization method) A particle initialization method by the particle initialization device 10A of the embodiment 2 will be described. Note that, in the particle initialization method of the embodiment 2, the observation orientation acquisition unit 11, the trust region calculation unit 12, the trust region volume calculation unit 13, the total particle volume calculation unit 14, and the analysis start determination unit 15 perform the same processes as in the particle initialization method by the conventional particle initialization device 10, and therefore detailed descriptions thereof will be omitted.

[0114] 16 is a flowchart showing a particle initialization method according to embodiment 2. Steps S1 to S5 of the particle initialization method according to embodiment 2 are similar to steps S1 to S5 of the conventional particle initialization method. However, even when the second distance change rate determination unit 22 determines that the distance change rate is not valid, the observation direction acquisition unit 11 acquires observation direction information and outputs it to the confidence region calculation unit 12.

[0115] As shown in FIG. 16, the second distance change rate determination unit 22 calculates the volume V(CA(Y^ k )), the validity of the range change rate is judged, and based on the result of the judgment of the validity of the range change rate, it is judged whether or not to additionally acquire observation direction information (step S21).

[0116] FIG. 17 is a flowchart showing a method of determining the validity of a distance change rate according to the second embodiment. The method of determining the validity of a distance change rate will be described with reference to FIG. 17. First, the second distance change rate determination unit 22 calculates the volume V(CA(Y^ k-1 )) and calculate the volume V(CA(Y^ k )) and store it (step S211).

[0117] Next, the second distance change rate determination unit 22 calculates the volume V(CA(Y^ k )) and the volume V(CA(Y^ k-1 The volume change rate V(CA(Y^)) is calculated (step S212). k )) rate_change is calculated by equation (19).

[0118]

number

[0119] Equation (19) calculates the rate of change of volume normalized by the volume one time before, but it may be normalized by the current time, or it may not be necessary to normalize if a reasonable threshold can be set.

[0120] As described above, when an outlier occurs in the distance change rate constituting the trust region, a large deviation in the estimate of the center value of the trust region due to the outlier in the distance change rate and an underestimation of the trust region obtained from the vicinity of the shifted center value are reflected in the rate of change of the volume of the trust region. Therefore, by calculating the rate of change of the volume of the trust region, the validity of the rate of change in response to an increase in the observed value of the distance change rate can be determined.

[0121] Then, the second distance change rate determination unit 22 calculates the volume change rate V(CA(Y^ k )) rate_change is used as the judgment value, and the threshold γ thr_range It is determined whether the rate of change in distance change rate with respect to an increase in the observed value is valid using equation (20) (step S213).

[0122]

number

[0123] In equation (20), "γ thr_range" is set from the viewpoint of how much the rate of change of the volume of the trust region changes due to an outlier. If the condition of formula (20) is satisfied (step S213: YES), the second distance change rate determination unit 22 determines that the rate of change with respect to the increase in the observed value of the distance change rate is appropriate, and outputs the volume of the trust region at time k calculated by the trust region volume calculation unit 13, and the state quantity of the particle and the total volume of the particle at time k calculated by the total particle volume calculation unit 14 to the analysis start determination unit 15. In addition, the second distance change rate determination unit 22 outputs the state quantity estimate value of the target, which is an observable variable at time k, and the Fisher information matrix to the total particle volume calculation unit 14. If the condition is not satisfied (step S213: NO), the second distance change rate determination unit 22 determines that the rate of change with respect to the increase in the observed value of the distance change rate is not appropriate, and instructs the observation direction acquisition unit 11 to acquire two new observation directions and update k by one time. In this case, the state quantity of the particle is recalculated after the trust region calculation unit 12.

[0124] As described above, according to the second embodiment, the second distance change rate determination unit 22 determines whether or not to perform additional acquisition of observation azimuth information based on the validity of the distance change rate. Therefore, even if an outlier caused by an observation error occurs in the distance change rate constituting the trust region, the number of observation values ​​can be increased to prevent a large deviation in the estimation of the center value of the trust region due to the outlier of the distance change rate, or an underestimation of the trust region obtained from the vicinity of the shifted center value. Therefore, the initialization of the particle filter is performed only when the total volume of the particles truly exceeds the volume of the trust region, regardless of the outlier. Therefore, according to the particle initialization device 10A of the second embodiment, it is possible to scatter particles around the true state quantity of the target, and to suppress a decrease in the estimation accuracy in the target motion analysis.

[0125] Embodiment 3 FIG. 18 is a block diagram showing a particle initialization device 10B according to the third embodiment. As shown in FIG. 18, the particle initialization device 10B includes, as functional units, an observation direction acquisition unit 11, a trust region calculation unit 12, a trust region volume calculation unit 13, a total particle volume calculation unit 14, an analysis start determination unit 15, a convergence determination unit 23, a required particle number calculation unit 24, a particle scattering unit 25, an evaluation value assignment unit 26, and a particle removal unit 27. Such a particle initialization device 10B is configured by a calculation device such as a microcomputer that realizes each functional unit by a processor reading and executing a program stored in a memory, or hardware such as a circuit device corresponding to each functional unit. In the particle initialization device 10B according to the third embodiment, the configurations of the observation direction acquisition unit 11, the trust region calculation unit 12, the trust region volume calculation unit 13, the total particle volume calculation unit 14, and the analysis start determination unit 15 are common to those of the conventional particle initialization device 100. Therefore, detailed description will be omitted.

[0126] In addition to the conventional functions of the observation direction acquisition unit 11, the observation direction acquisition unit 11 also acquires observation direction information when the convergence determination unit 23 determines that it is necessary to additionally acquire observation direction information. The observation direction acquisition unit 11 outputs the observation direction information to the trust region calculation unit 12.

[0127] The trust region calculation unit 12 uses the observation direction information output from the observation direction acquisition unit 11 to obtain the target state quantity estimate, which is an observable variable at time k that is the center value of the trust region, and the Fisher information matrix, and outputs them to the trust region volume calculation unit 13 and the total particle volume calculation unit 14. In addition, the trust region calculation unit 12 outputs the target state quantity estimate, which is an observable variable at time k, and the Fisher information matrix to the particle scattering unit 25.

[0128] The trust region volume calculation unit 13 calculates the volume of the trust region based on the Fisher information matrix output from the trust region calculation unit 12 , and outputs it to the analysis start determination unit 15 .

[0129] The total particle volume calculation unit 14 calculates the state quantity and the total volume of the particles at time k based on the Fisher information matrix output from the trust region calculation unit 12 and the state quantity estimate of the target, which is an observable variable at time k, and outputs the calculated state quantity and the total volume of the particles to the analysis start determination unit 15.

[0130] The analysis start determination unit 15 determines whether or not the target motion analysis can be started based on the volume of the trust region output from the trust region volume calculation unit 13 and the total volume of the particles output from the total particle volume calculation unit 14. When the analysis start determination unit 15 determines that the target motion analysis can be started, it outputs the state quantity of the particles at time k output from the total particle volume calculation unit 14 to the output terminal 50. When the analysis start determination unit 15 determines that the target motion analysis cannot be started, it outputs the volume of the trust region, the state quantity of the particles at time k, and the total volume of the particles to the convergence determination unit 23.

[0131] The convergence determination unit 23 determines whether or not convergence of the volume of the trust region can be expected by collecting further observation directions. If the convergence determination unit 23 determines that convergence of the volume of the trust region cannot be expected, it outputs the volume of the trust region, the total volume of the particles, and the set number of particles to the required particle number calculation unit 24. If the convergence determination unit 23 determines that convergence of the volume of the trust region can be expected, it instructs the observation direction acquisition unit 11 to acquire two new observation directions and update k by one time.

[0132] The required particle number calculation unit 24 calculates the number of new particles (required number of particles) to be scattered within the trust region based on the volume of the trust region and the total volume of the particles output from the convergence judgment unit 23, and the set number of particles, and outputs the number of new particles to the particle scattering unit 25.

[0133] The particle scattering unit 25 calculates the state quantity of each particle for the required number of particles at time k based on the Fisher information matrix and the state quantity estimate of the target, which is an observable variable at time k, output from the trust region calculation unit 12, and the required number of particles to be scattered within the trust region output from the required particle number calculation unit 24, and outputs the state quantity to the evaluation value assignment unit 26.

[0134] The evaluation value assigning unit 26 assigns an evaluation value (possibility evaluation value) to each state quantity of particles for the necessary number of particles at time k output from the particle scattering unit 25 according to the possibility that the particle contributes to estimating the state of the target in the target motion analysis after initialization. The evaluation value assigning unit 26 outputs the state quantities of particles for the necessary number of particles at time k to which the possibility evaluation value has been assigned to the particle removing unit 27.

[0135] The particle removal unit 27 removes particles with low possibility evaluation values ​​based on the state quantities of particles for the necessary number of particles at time k to which the possibility evaluation values ​​are assigned output from the evaluation value assignment unit 26. As a result, target motion analysis is started based on the state quantities of the remaining particles at time k.

[0136] (Particle initialization method) A particle initialization method by the particle initialization device 10B of the embodiment 3 will be described. Note that, in the particle initialization method of the embodiment 3, the observation orientation acquisition unit 11, the trust region calculation unit 12, the trust region volume calculation unit 13, the total particle volume calculation unit 14, and the analysis start determination unit 15 perform the same processes as in the particle initialization method by the conventional particle initialization device 100, and therefore detailed descriptions thereof will be omitted.

[0137] FIG. 19 is a flowchart showing a particle initialization method according to the third embodiment. Steps S1 to S5 of the particle initialization method according to the third embodiment are the same as steps S1 to S5 of the conventional particle initialization method. However, in addition to the processing of the observation direction acquisition unit 11 of the conventional particle initialization device 10, the observation direction acquisition unit 11 also acquires observation direction information and outputs it to the trust region calculation unit 12 when the convergence determination unit 23 determines that convergence of the volume of the trust region is expected (step S1). Furthermore, in addition to the processing of the trust region calculation unit 12 of the conventional particle initialization device 100, the trust region calculation unit 12 acquires observation direction information and outputs it to the trust region calculation unit 12 when the convergence determination unit 23 determines that convergence of the volume of the trust region is expected. r,k , r=1,2,3, and Fisher's information matrix J(Y^ r,k ), r=1, 2, 3 are also output to the particle scattering unit 25 (step S31).

[0138] As shown in FIG. 19, when the analysis start determination unit 15 determines that the target motion analysis cannot be started, the convergence determination unit 23 compares the change rate of the volume of the trust region with a threshold value (step S32) in the same manner as the second distance change rate determination unit 22 of the second embodiment. Note that the second distance change rate determination unit 22 of the second embodiment uses the change rate of the volume of the trust region to determine the influence of the outlier of the distance change rate from the validity of the change rate with respect to the increase in the observed value of the distance change rate, but the convergence determination unit 23 uses the change rate of the volume of the trust region to determine whether or not the convergence of the volume of the trust region can be expected by collecting the observation orientation. Therefore, the convergence determination unit 23 performs the same process as the process flow shown in FIG. 17, but the threshold value is different from that of Equation (20). In the convergence determination unit 23, the threshold value is set from the viewpoint of the convergence of the volume of the trust region.

[0139] If the rate of change of the volume of the trust region does not exceed the threshold (step S32: YES), the convergence determination unit 23 determines that convergence of the volume of the trust region is not expected, and outputs the volume of the trust region, the total volume of the particles, and the set number of particles to the required particle number calculation unit 24. If the rate of change of the volume of the trust region exceeds the threshold (step S32: NO), the convergence determination unit 23 determines that convergence of the volume of the trust region is expected, and instructs the observation orientation acquisition unit 11 to acquire two new observation orientations and update k by one time.

[0140] The required particle number calculation unit 24 calculates the volume V(CA(Y^ k )), the total volume of the particles V(B(Y k )) and the initially set number of particles N, the number of particles required to be scattered within the trust region is calculated (step S33). add is calculated using equation (21).

[0141]

number

[0142] In equation (21), "V(CA(Y^ k))" indicates the volume of the confidence region, and "V(B(Y k ))" indicates the total volume of the particles, and "N" indicates the number of particles initially set.

[0143] The particle scattering unit 25 scatters the required number of particles N add That is, the particle scattering unit 25 scatters particles of the target state quantity estimate Y^, which is an observable variable at time k output from the trust region calculation unit 12. r,k , r=1,2,3, and Fisher's information matrix J(Y^ r,k ), r=1, 2, 3, and the required particle number N output from the required particle number calculation unit 24 add Based on this, the particle state quantity Y at time k is calculated based on the required number of particles. r,k (i) ,r=1,2,3,4,i=1,2,…,N add The calculation method is the same as the process in step S41 of the total particle volume calculation unit 14, except that the number of particles to be scattered is different.

[0144] The evaluation value assigning unit 26 calculates the particle state quantity Y at time k based on the required number of particles output from the particle scattering unit 25. r,k (i) ,r=1,2,3,4,i=1,2,…,N add is assigned a possibility evaluation value (step S35).

[0145] Fig. 20 is a flowchart showing a method for assigning a possibility evaluation value according to the third embodiment. The method for assigning a possibility evaluation value will be described with reference to Fig. 20. First, the evaluation value assigning unit 26 calculates the particle state quantity Y r,k (i) ,r=1,2,3,4,i=1,2,…,N add Using the velocity v in the Cartesian coordinate system (i) (Step S351). The velocity component X1 converted into Cartesian coordinates as shown in equation (22) is used for the velocity conversion. (i) and X2 (i) It can be calculated from:

[0146]

number

[0147] Next, the evaluation value assigning unit 26 calculates the particle state quantity Y r,k (i) ,r=1,2,3,4,i=1,2,…,N add The corresponding speeds v (i) In response to the request, a possibility evaluation value between 0 and 1 is assigned based on the speed evaluation value table (step S352).

[0148] The velocity evaluation value table is stored in advance in a storage area of ​​the particle initialization device 10B. FIG. 21 is a diagram showing a velocity evaluation value table according to the third embodiment. As shown in FIG. 21, based on the velocity evaluation value table, the maximum velocity that can be reliably taken is γ thr_velocity_low The speeds below this are given a likelihood score of 1, and the maximum speed that is unlikely but should be considered is γ thr_velocity_high Any speed above this is given a probability score of 0. Also, γ thr_velocity_low and γ thr_velocity_high A linearly interpolated possibility evaluation value is given between the two. Note that the possibility evaluation value may be given by a curve instead of linear interpolation. (i) is given by equation (23) using the speed evaluation value table.

[0149]

number

[0150] As another method, a possibility evaluation value may be assigned to a particle based on the statistical properties of the distribution of the trust region and the position of the particle in the trust region. In other words, a possibility evaluation value may be assigned to the state quantity of a particle based on the value of the quantile α of the chi-square distribution used in the calculation of the volume of the trust region in the trust region volume calculation unit 13. FIG. 22 is a flowchart showing a method of assigning a possibility evaluation value according to the third embodiment. A method of assigning a possibility evaluation value different from the method explained in FIG. 20 will be explained using FIG. 22. First, the state quantity Y of a particle at time k based on the required number of particles output from the particle scattering unit 25 is calculated as follows: r,k (i) ,r=1,2,3,4,i=1,2,…,N add is a chi-squared distribution with four degrees of freedom. Therefore, it is possible to determine to which position in the chi-squared distribution with four degrees of freedom the state quantity of each particle belongs. The evaluation value assigning unit 26 calculates an index index indicating to which position in the chi-squared distribution with four degrees of freedom the state quantity of each particle belongs. (i) (step S353).

[0151] Next, the evaluation value assigning unit 26 calculates the particle state quantity Y r,k (i) ,r=1,2,3,4,i=1,2,…,N add For each, the corresponding index (i) In response to the result, a possibility evaluation value is assigned based on the particle position evaluation value table (step S354).

[0152] The particle position evaluation value table is stored in advance in a storage area of ​​the particle initialization device 10B. FIG. 23 is a diagram showing a particle position evaluation value table according to the third embodiment. The particle position evaluation value table is configured to give different evaluation values ​​by changing the value of the (1-α) quantile α for a chi-square distribution with four degrees of freedom. As shown in FIG. 23, based on the particle position evaluation value table, when the state amount of a particle belongs to the interval up to (1-α4), a possibility evaluation value ξ4 is given. When the state amount of a particle belongs to the interval (1-α4) to (1-α3), a possibility evaluation value ξ3 is given. When the state amount of a particle belongs to the interval (1-α3) to (1-α2), a possibility evaluation value ξ2 is given. When the state amount of a particle belongs to the interval (1-α2) to (1-α1), a possibility evaluation value ξ1 is given. The possibility evaluation value l (i) is given by equation (24) using the particle position evaluation value table.

[0153]

number

[0154] In the example of FIG. 22, the possibility evaluation value changes stepwise. However, a particle position evaluation value table that changes continuously with respect to α may be used.

[0155] Then, the particle removal unit 27 calculates the possibility evaluation value l outputted from the evaluation value assignment unit 26. (i) Using the above, particles for which equation (25) holds are deleted as inefficient particles when estimating the state of the target (step S36).

[0156]

number

[0157] The particles that were not deleted and were kept are Y r,k (j) ,r=1,2,3,4,j=1,2,…,N redctionThe output is used to start target motion analysis. redction " is N add The number of particles is calculated by subtracting the total number of particles reduced from the number of particles reduced. The threshold value l of the possibility evaluation value in Eq. (25) thr The number of particles to be reduced can be increased or decreased by changing , which allows the balance to be adjusted between the reduction in the number of particles to be processed and the performance degradation caused by excluding inefficient particles when estimating the state of the target in the target motion analysis.

[0158] According to the third embodiment, the convergence of the trust region is judged, and when the convergence of the trust region is not expected, the number of particles scattered in the trust region can be efficiently increased to compensate for the shortage. Therefore, even if the number of particles initially set is insufficient to capture the trust region, the particle filter can be used to perform the target motion analysis.

[0159] Moreover, according to the third embodiment, by excluding inefficient particles when estimating the state of a target, an increase in the processing load due to an increase in particles is suppressed.

[0160] Although the first to third embodiments of the present invention have been described above, the present invention is not limited to the first to third embodiments described above, and various modifications or applications are possible within the scope of the present invention. For example, the particle initialization device 10 of the first embodiment may be combined with the particle initialization device 10A of the second embodiment. In addition, the particle initialization device 10 of the first embodiment, the particle initialization device 10A of the second embodiment, or a particle initialization device combining the particle initialization device 10 of the first embodiment and the particle initialization device 10A of the second embodiment may include the convergence determination unit 23, the required particle number calculation unit 24, the particle scattering unit 25, the evaluation value assignment unit 26, and the particle removal unit 27 described in the third embodiment.

[0161] In addition, in the first embodiment, the method for determining the validity of the distance change rate has been described as using the maximum eigenvalue of the inverse matrix of Fisher's information matrix and using the range of the trust region. However, any method for determining the validity of the distance change rate may be used.

[0162] In addition, in the third embodiment, as a means for evaluating the possibility of the true state quantity of each particle, a method for evaluating from the velocity components in Cartesian coordinates and a method for evaluating from the statistical properties of the distribution of the trust region and the position of the trust region of the particle have been described, but any method for evaluating the possibility of the true state quantity of each particle can be used.

[0163] Furthermore, the particle initialization method described in the present invention can be applied to a target motion analysis device that estimates target motion parameters from azimuth information using passive observation data. For example, the first to third embodiments are intended to be applied to sonar that estimates target motion parameters when passively observing sound, but they can also be applied to radar that passively receives radio waves. [Explanation of symbols]

[0164] 10, 10A, 10B, 100 particle initialization device, 11 observation direction acquisition unit, 12 trust region calculation unit, 13 trust region volume calculation unit, 14 total particle volume calculation unit, 15 analysis start determination unit, 21 first distance change rate determination unit, 22 second distance change rate determination unit, 23 convergence determination unit, 24 required particle number calculation unit, 25 particle dispersion unit, 26 evaluation value assignment unit, 27 particle removal unit, 30 first input terminal, 40 second input terminal, 50 output terminal.

Claims

1. A particle initialization device for initializing particles when performing a motion analysis of a target using a particle filter in a modified polar coordinate system including a range change rate, comprising: an observation direction acquisition unit that acquires observation direction information regarding the direction of an observed target; a confidence region calculation unit that calculates a confidence region for scattering the particles using the observation direction information; a distance change rate determination unit that determines whether or not to additionally obtain the observation direction information based on the validity of the distance change rate. Particle initialization device.

2. The distance change rate determination unit determines whether or not to additionally obtain the observation direction information based on the validity of a section in which the distance change rate can move. The particle initialization device according to claim 1 .

3. The distance change rate determination unit calculating a maximum eigenvalue of an inverse of a Fisher information matrix based on the estimated heading, heading change, and range rate; Calculating the range of motion of the range rate based on the maximum eigenvalue; The range of possible movement of the distance change rate is compared with a threshold value to determine the validity of the range of possible movement of the distance change rate. The particle initialization device according to claim 2 .

4. The distance change rate determination unit calculating the bounds of the confidence region from a Fisher information matrix for the heading, heading change, and range rate estimates; Calculating a maximum range of the trust region in the range rate direction based on the extent of the trust region; The maximum range of the distance rate direction of the trust region is compared with a threshold value to determine the validity of the range range. The particle initialization device according to claim 2 .

5. The distance change rate determination unit determines whether or not to additionally acquire the observation direction information based on the validity of a change rate of the distance change rate relative to an increase in the observation direction information. The particle initialization device according to claim 1 .

6. The distance change rate determination unit Calculate a rate of change in the volume of the confidence region before and after the increase in the observation orientation information; The change rate of the volume of the confidence region is compared with a threshold value to determine the validity of the change rate of the distance change rate relative to an increase in the observation direction information. The particle initialization device according to claim 5 .

7. a convergence determination unit that determines the convergence of the trust region from a change in the volume of the trust region with respect to an increase in the observation orientation information; a required particle number calculation unit that calculates a required number of particles based on the total volume of the particles and the trust region when convergence of the trust region is not expected; and a particle scattering unit that scatters the necessary number of particles to the trust region when convergence of the trust region is not expected. The particle initialization device according to any one of claims 1 to 6.

8. an evaluation value assigning unit that assigns a possibility evaluation value for a true state quantity to the particles scattered in the confidence region; and a particle removal unit that reduces the number of the particles based on the possibility evaluation value. The particle initialization device according to claim 7 .

9. The evaluation value assignment unit: Transforming the state quantities of the particles into Cartesian coordinates; assigning said likelihood assessment value to said particle based on the particle's velocity components in said Cartesian coordinates; The particle initialization device according to claim 8 .

10. The evaluation value assignment unit is assigning said likelihood score to said particle based on statistical properties of the distribution of said confidence regions and the position of said particle in said confidence regions; The particle initialization device according to claim 8 .

11. A particle initialization method for initializing particles when performing a motion analysis of a target using a particle filter in a modified polar coordinate system including a range change rate, comprising the steps of: an observation direction acquisition step of acquiring observation direction information regarding the direction of an observed target; a confidence region calculation step of calculating a confidence region for scattering the particles using the observation direction information; and a distance change rate determination step of determining whether or not to additionally obtain the observation direction information based on the validity of the distance change rate. Particle initialization method.

12. A particle initialization program for causing a processor of a particle initialization device to execute a particle initialization method for initializing particles when performing a motion analysis of a target using a particle filter in a modified polar coordinate system including a range change rate, the particle initialization program comprising: The particle initialization method includes: an observation direction acquisition step of acquiring observation direction information regarding the direction of an observed target; a confidence region calculation step of calculating a confidence region for scattering the particles using the observation direction information; and a distance change rate determination step of determining whether or not to additionally obtain the observation direction information based on the validity of the distance change rate. Particle initialization program.

Images