Motion parameter estimation device, motion parameter estimation method and program

0001The present invention relates to a motion parameter estimation device, a motion parameter estimation method and a program.

0002Several techniques have been proposed for measuring the position of an object using a sensor such as sonar. For example, in the technique described in Patent Document 1, when a passive sonar is used, an active sonar is used at a different frequency.

<p num="0003"><patcit num="1"><text>Special Fair 03-060394 Gazette</text></patcit></p>

<p num="0004"> It is preferable that the combination of the active sensor and the passive sensor can improve the estimation accuracy of the motion of the object as compared with the case where each sensor is used alone. On the other hand, Patent Document 1 does not show a specific method for improving the estimation accuracy of the motion of an object by combining the measurement result of the active method and the measurement result of the passive method.</p><p num="0005"> The present invention provides a motion parameter estimation device, a motion parameter estimation method, and a program capable of improving the estimation accuracy of the motion of an object by combining an active sensor and a passive sensor.</p>

<p num="0006"> According to the first aspect of the present invention, the motion parameter estimator measures the position of an object using an active sensor, and uses a passive sensor to measure the object in a period shorter than the measurement cycle of the active sensor. A measurement unit that measures the direction of the object, a drive noise sample setting unit that sets a plurality of drive noise samples in a state equation indicating the motion of the object based on a predetermined probability distribution, and each of the drive noise samples. The particle calculation unit that calculates a plurality of particles in the particle filter by substituting into the state equation and each of the particles calculated by the particle calculation unit are substituted into the observation equation to calculate the estimated value of the measurement value of the measurement unit. A selection unit that selects a part of the plurality of the particles based on the error between the first estimation unit and the measurement value of the measurement unit and the estimation value of the first estimation unit, and the selection unit selects the selection unit. It is provided with a second estimation unit that calculates an estimated value of a motion parameter indicating the motion of the object based on the particles.</p><p num="0007"> The state equation shows the motion of the motion parameter estimator in addition to the motion of the object, and the observation equation is the measured value of the motion of the motion parameter estimator in addition to the equation of the measured value of the motion of the object. The second estimation unit may calculate an estimated value of the motion parameter indicating the motion of the object and the motion of the motion parameter estimator.</p><p num="0008"> The observation equation may include an equation for the measured value of the velocity of the motion parameter estimator.</p><p num="0009"> The observation equation may include an equation of a velocity measurement of the object based on the Doppler effect in the active sensor.</p><p num="0010"> The first estimation unit may calculate an estimated value of the measured value of the measurement unit using the observation equations of the active sensor and the passive sensor.</p><p num="0011"> According to the second aspect of the present invention, in the motion parameter estimation method, the position of the object is measured by using the active sensor, and the object is measured by using the passive sensor in a period shorter than the measurement cycle by the active sensor. A measurement step for measuring the direction of the object, a drive noise sample setting step for setting a plurality of drive noise samples in a state equation indicating the motion of the object based on a predetermined probability distribution, and each of the drive noise samples are described above. Substituting into the state equation, the particle calculation step of calculating a plurality of particles in the particle filter, and substituting each of the particles calculated in the particle calculation step into the observation equation, the estimated value of the measured value in the measurement step is calculated. A selection step of selecting a part of the plurality of the particles based on the error between the first estimation step to be performed, the measured value in the measurement step, and the estimated value in the first estimation step, and the selection. It includes a second estimation step of calculating an estimated value of a motion parameter indicating the motion of the object based on the particles selected in the step.</p><p num="0012"> According to a third aspect of the present invention, the program measures the position of an object using an active sensor on a computer, and uses a passive sensor to measure the object in a period shorter than the measurement period of the active sensor. A measurement step for measuring the direction of the object, a drive noise sample setting step for setting a plurality of drive noise samples in a state equation indicating the motion of the object based on a predetermined probability distribution, and each of the drive noise samples are described above. Substituting into the state equation to calculate a plurality of particles in the particle filter, and substituting each of the particles calculated in the particle calculation step into the observation equation to calculate the estimated value of the measured value in the measuring step. A selection step of selecting a part of the plurality of the particles based on the error between the first estimation step to be performed, the measured value in the measurement step, and the estimated value in the first estimation step, and the selection. It is a program for executing a second estimation step of calculating an estimated value of a motion parameter indicating the motion of the object based on the particles selected in the step.</p>

<p num="0013"> It can be done according to the above-mentioned motion parameter estimation device, motion parameter estimation method and program. The combination of the active sensor and the passive sensor can improve the estimation accuracy of the motion of the object.</p>

0014<figref num="1">It is a schematic block diagram which shows the functional structure of the motion parameter estimation apparatus which concerns on 1st Embodiment of this invention.</figref><figref num="2">It is a figure which shows the structural example of the measurement system which the motion parameter estimation apparatus which concerns on 1st Embodiment of this invention performs measurement.</figref><figref num="3">It is a figure which shows the measurement example of the direction of an object by the passive sensor which concerns on 1st Embodiment of this invention.</figref><figref num="4">It is a figure which shows the measurement example of the position of the object by the active sensor which concerns on 1st Embodiment of this invention.</figref><figref num="5">It is a flowchart which shows the example of the procedure of the process performed by the motion parameter estimation apparatus which concerns on 1st Embodiment of this invention.</figref><figref num="6">It is a figure which shows the example of convergence to the vicinity of the true value of the estimated value of the motion parameter by the motion parameter estimation device which concerns on 1st Embodiment of this invention.</figref>

0015Hereinafter, embodiments of the present invention will be described, but the following embodiments do not limit the inventions claimed in the claims. Also, not all combinations of features described in the embodiments are essential to the means of solving the invention.

0016&lt;First Embodiment&gt; FIG. 1 is a schematic block diagram showing a functional configuration of the motion parameter estimation device according to the first embodiment of the present invention. As shown in the figure, the motion parameter estimation device 100 includes a measurement unit 110, a storage unit 180, and a control unit 190. The measuring unit 110 includes a passive sensor 111 and an active sensor 112. The control unit 190 includes a drive noise sample setting unit 191, a particle calculation unit 192, a first estimation unit 193, a selection unit 194, a correction unit 195, and a second estimation unit 196. Further, the motion parameter estimation device 100 is mounted on the moving body 10.

0017The motion parameter estimation device 100 estimates the value of the motion parameter indicating the motion of the object. The motion parameter estimation device 100 is configured by using, for example, a computer equipped with a sensor. FIG. 2 is a diagram showing a configuration example of a measurement system in which the motion parameter estimation device 100 measures. As shown in FIG. 2, the measurement system 1 includes a moving body 10 equipped with the motion parameter estimation device 100 and an object 20 to which the motion parameter estimation device 100 measures motion. Line L11 shows an example of the locus of the moving body 10. Line L12 shows an example of the trajectory of the object 20. Arrow B11 shows an example of the velocity vector of the moving body 10. Arrow B12 shows an example of the velocity vector of the object 20.

0018The motion parameter estimation device 100 can set various moving objects as measurement targets (motion parameter estimation targets). For example, the measurement target area of the motion parameter estimation device 100 may be underwater such as in the sea, and the object 20 may be an underwater vehicle. Alternatively, the measurement target area of the motion parameter estimation device 100 may be on the water surface such as the sea, and the object 20 may be an object moving on the water surface such as a ship. Alternatively, the measurement target area of the motion parameter estimation device 100 may be the land (ground), and the object 20 may be an object traveling on land such as a vehicle. Alternatively, the measurement target area of the motion parameter estimation device 100 may be in the air, and the object 20 may be an object moving in the air such as an airplane. Alternatively, the measurement target of the motion parameter estimation device 100 may be the universe, and the object 20 may be an object moving in the universe such as a spaceship.

0019Like the object 20, the moving body 10 can be various. For example, the moving body 10 may be an underwater navigating body. Alternatively, the moving body 10 may be an object that moves on the water surface such as a ship. Alternatively, the moving body 10 may be a vehicle or the like traveling on land. Alternatively, the moving body 10 may be an object that moves in the air, such as an airplane. Alternatively, the moving body 10 may be an object that moves in space, such as a spaceship. In the following, a case where the motion parameter estimation device 100 is mounted on the moving body 10 and moves will be described as an example, but the motion parameter estimation device 100 may be fixedly installed. For example, the motion parameter estimation device 100 may be provided in a non-moving structure such as in a building instead of the moving body 10.

0020Further, in the following description, a case where the measurement target region of the motion parameter estimation device 100 is a two-dimensional region and the motion parameter estimation device 100 uses xy coordinates (two-dimensional Cartesian coordinates) will be described as an example. The measurement target area of 100 may be three-dimensional. When the measurement target area of the motion parameter estimation device 100 is a three-dimensional region, the motion parameter estimation device 100 may use xyz coordinates (three-dimensional Cartesian coordinates). When the measurement target area of the motion parameter estimation device 100 is a three-dimensional region, the motion parameter estimation device 100 also performs the processing related to the x-coordinate and the y-coordinate described below for the z-coordinate. Further, the motion parameter estimation device 100 performs processing on a three-dimensional angle or direction in the same manner as the processing on a two-dimensional angle or direction described below.

0021The measuring unit 110 measures the motion parameters. The passive sensor 111 measures the direction of the object 20 as seen from the position of the measuring unit 110 (the position of the moving body 10) by measuring the physical energy that has reached the passive sensor 111 from the object 20. The direction of the object 20 is one of the motion parameters.

0022FIG. 3 is a diagram showing an example of measurement in the direction of the object 20 by the passive sensor 111. In the example of FIG. 3, the physical energy reached from the object 20 to the passive sensor 111 is indicated by the arrow B21. For example, when the passive sensor 111 is a passive sonar, the passive sensor 111 measures the sound that reaches the passive sensor 111 from the object 20. This sound may be a sound emitted by the object 20, or a sound reflected by the object 20 from a sound emitted by an object other than the object 20. Alternatively, if the passive sensor 111 is a passive radar, the passive sensor 111 measures the electromagnetic waves that reach the passive sensor 111 from the object 20. This electromagnetic wave may be an electromagnetic wave emitted by the object 20, or may be an electromagnetic wave reflected by the object 20 from an electromagnetic wave emitted by an object other than the object 20.

0023The active sensor 112 measures the position of the object 20 by outputting physical energy toward the object 20 and measuring the physical energy reflected by the object 20 and reaching the passive sensor 111. The position of the object 20 is one of the motion parameters.

0024FIG. 4 is a diagram showing an example of measuring the position of the object 20 by the active sensor 112. In the example of FIG. 4, the physical energy output by the active sensor 112 toward the object 20 is indicated by the arrow B31. In addition, the physical energy reflected by the object 20 and reaching the passive sensor 111 is indicated by the arrow B32. For example, when the active sensor 112 is an active sonar, the active sensor 112 outputs a sound (sound wave) toward the object 20, and measures the sound reflected by the object 20 and reaches the active sensor 112. Alternatively, when the active sensor 112 is an active radar, the active sensor 112 outputs an electromagnetic wave toward the object 20 and measures the electromagnetic wave reflected by the object 20 and reaches the active sensor 112. The active sensor 112 measures the relative position of the object 20 with respect to the position of the active sensor 112 (the position of the moving body 10). The active sensor 112 may determine the position of the object 20 with reference to a position other than the position of the active sensor 112 based on the relative position and the position of the active sensor 112 itself.

0025The storage unit 180 is configured by using a storage device included in the motion parameter estimation device 100, and stores various information. The control unit 190 controls each unit of the motion parameter estimation device 100 to execute various processes. The control unit 190 is configured by, for example, a CPU (Central Processing Unit) included in the motion parameter estimation device 100 reading a program from the storage unit 180 and executing the program.

0026The control unit 190 estimates the motion parameter of the object 20 using the measurement result by the passive sensor 111 and the measurement result by the active sensor 112. The control unit 190 estimates, for example, the position of the object 20. Here, the passive sensor 111 can measure the direction of the object 20 as seen from the position of the measuring unit 110, but cannot measure the relative distance between the measuring unit 110 and the object 20. Therefore, it is difficult to estimate the position of the object 20 with high accuracy by using only the measurement result of the passive sensor 111. On the other hand, the control unit 190 measures the position of the object 20 with higher accuracy than the case where only the measurement result of the passive sensor 111 is used by using the measurement result by the passive sensor 111 and the measurement result by the active sensor 112. can do.

0027Further, the active sensor 112 outputs physical energy to the object 20 and then waits for the arrival of the physical energy reflected by the object 20. Therefore, if only the measurement result of the active sensor 112 is used, it is necessary to increase the sampling period to some extent. When the sampling period becomes large to some extent, it may take time for the estimated value of the motion parameter to converge to a value close to the true value, for example, when the initial value of the motion parameter is unknown. On the other hand, by using the measurement result by the passive sensor 111 and the measurement result by the active sensor 112, the control unit 190 sets the estimated value of the motion parameter to true more quickly than when using only the measurement result of the active sensor 112. It can be converged to a value close to the value.

0028The drive noise sample setting unit 191 sets a drive noise sample. The drive noise referred to here is a parameter for matching the motion of the object 20 estimated using the motion model of the object 20 with the actual motion of the object 20. The control unit 190 uses the equation of state of the object 20 as a motion model of the object 20, and the equation of state includes driving noise. The equation of state of the object 20 used by the control unit 190 is defined as the equation (1), for example.

0029<maths num="1"><img id="000003" he="14" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0030Here, k indicates the sampling timing index of the passive sensor 111. Specifically, with time 0 as the reference time, k indicates the number of samplings performed by the passive sensor 111 after the reference time. Set the sampling period of the passive sensor 111 to T_SExpressed in, the kth sampling time is kT_SIt is expressed as. In the following, the timing of the kth sampling after the reference time by the passive sensor (that is, the sampling timing indicated by the index k) is referred to as timing k.

0031The motion parameter vector q (k) indicates the motion parameter at the timing k. In particular, the motion parameter vector q (k) indicates the position, velocity and acceleration of the object at timing k. The motion parameter vector q (k) is expressed by Eq. (2).

0032<maths num="2"><img id="000004" he="14" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0033Here, x (k) and y (k) indicate the x-coordinate value and the y-coordinate value of the target position at the timing k, respectively. x'(k) and y'(k) indicate the x-coordinate value and the y-coordinate value of the target velocity at the timing k, respectively. The first derivative is indicated by'. x'' (k) and y'' (k) indicate the x-coordinate value and the y-coordinate value of the target acceleration at the timing k, respectively. The second derivative is indicated by''. The superscript T to the right of the matrix or vector indicates the transpose of the matrix or vector.

0034The drive noise vector u (k) indicates the drive noise at the timing k. The drive noise vector u (k) is the two scalar variables u₁(k) and u₂It is expressed by Eq. (3) using (k).

0035<maths num="3"><img id="000005" he="14" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0036The matrix A in Eq. (1) is a matrix showing the coefficients. The matrix A is shown by Eq. (4).

0037<maths num="4"><img id="000006" he="47" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0038T as above_SIndicates the sampling period of the passive sensor 111. The matrix B is a matrix showing the coefficients. The matrix B is shown by Eq. (5).

0039<maths num="5"><img id="000007" he="47" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0040The state equation shown in Eq. (1) is the value of the motion parameter (target position, velocity, acceleration) and drive noise at timing k, and the value of motion parameter (target position, velocity, and acceleration) at timing k + 1. The relationship with acceleration) is shown.

0041The drive noise sample setting unit 191 sets a plurality of drive noise samples based on a predetermined probability distribution. Here, the control unit 190 calculates an estimated value of the motion parameter using the particle filter. The drive noise sample setting unit 191 sets the drive noise by the number of particles in the particle filter. The probability distribution used by the drive noise sample setting unit 191 to set the drive noise sample is not limited to a specific probability distribution. For example, the drive noise sample setting unit 191 may set the drive noise based on the Gaussian distribution. Alternatively, the drive noise sample setting unit 191 may set the drive noise based on a distribution other than the Gaussian distribution.

0042The particle calculation unit 192 calculates a plurality of particles in the particle filter by substituting each of the drive noise samples set by the drive noise sample setting unit 191 into the equation of state. The particle calculation unit 192 substitutes the value of the motion parameter at the timing k-1 and the value of the driving noise into the state equation, and calculates the value of the motion parameter at the timing k as a particle in the particle filter.

0043The first estimation unit 193 substitutes each of the particles calculated by the particle calculation unit 192 into the observation equation to calculate the estimated value of the measured value of the motion parameter. The observation equation referred to here is an equation for estimating the measured value of the measuring unit 110 from the motion parameter, and an observation error (measurement noise) is added to the value of the motion parameter. The observation equation in the active sensor 112 is defined as, for example, Eq. (6).

0044<maths num="6"><img id="000008" he="15" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0045Here, the vector m (k) indicates the measured value of the motion parameter at the timing k. For the active sensor 112, the vector m (k) is given by Eq. (7).

0046<maths num="7"><img id="000009" he="15" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0047Where x_m(k) indicates the measured value of the x-coordinate value of the target position at the timing k. y_m(k) indicates the measured value of the y-coordinate value of the target position at the timing k.

0048The matrix C in Eq. (6) is a matrix showing the coefficients. The matrix C is shown by Eq. (8).

0049<maths num="8"><img id="000010" he="23" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0050Also, w in equation (6)_A(k) shows the observation error of the active sensor 112 at the timing k. On the other hand, the observation equation in the passive sensor 111 is defined as, for example, Eq. (9).

0051<maths num="9"><img id="000011" he="23" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0052Here, atan indicates an arctangent. x_O(k) indicates the x-coordinate value of the position of the measuring unit 110 at the timing k. y_O(k) indicates the y-coordinate value of the position of the measuring unit 110 at the timing k. w_P(k) shows the observation error of the passive sensor 111 at the timing k. Further, in the case of the passive sensor 111, m (k) is expressed by Eq. (10).

0053<maths num="10"><img id="000012" he="18" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0054Where θ_m(k) indicates the measured value (angle measurement value) of the target orientation with respect to the position of the measurement unit 110 at the timing k.

0055The selection unit 194 selects a part of the plurality of particles based on the error between the measured value of the motion parameter by the measuring unit 110 and the estimated value of the measured value of the motion parameter calculated by the first estimation unit 193. To do. Specifically, the selection unit 194 calculates the likelihood of particles from the reciprocal of the error, and selects a predetermined number of particles in descending order of likelihood.

0056The correction unit 195 applies an extended Kalman filter to each of the particles selected by the selection unit 194 to correct each of the particles. The correction unit 195 makes corrections so as to increase the likelihood of each particle. The second estimation unit 196 obtains an estimated value of the motion parameter based on the particles corrected by the correction unit 195. Further, the second estimation unit 196 is based on the position and velocity of the object 20 indicated by the motion parameters, the distance between the measurement unit 110 and the object 20, and the object 20 as seen from the position of the measurement unit 110. Calculate the direction and the magnitude of the velocity of the object 20.

0057Next, the operation of the motion parameter estimation device 100 will be described with reference to FIG. FIG. 5 is a flowchart showing an example of a processing procedure performed by the motion parameter estimation device 100. The motion parameter estimation device 100 repeats the processes of steps S101 to S110 of FIG. 5 for each sampling timing of the passive sensor 111. Here, in the active sensor 112, it is necessary to wait until the physical energy output toward the object 20 is reflected and returned to the active sensor 112. On the other hand, the passive sensor 111 does not have the limitation of waiting time as in the case of the active sensor 112. Therefore, the sampling period of the passive sensor 111 can be set shorter than the sampling period of the active sensor 112. Therefore, the motion parameter estimation device 100 measures the motion parameters by setting the sampling cycle of the passive sensor 111 to be shorter than the sampling cycle of the active sensor 112. Specifically, every time the passive sensor 111 measures the motion parameter n times (n is a positive integer), the active sensor 112 measures the motion parameter once. As a result, there is a sampling timing in which the passive sensor 111 and the active sensor 112 measure the motion parameter, and a sampling timing in which only the passive sensor 111 measures the motion parameter.

0058(Step S101) The drive noise sample setting unit 191 generates a sample of drive noise (drive noise vector u (k)) by the number of particles P. Below, u is a sample of drive noise at timing k.^pNotated as (k). After step S101, the process proceeds to step S102.

0059(Step S102) The particle calculation unit 192 uses P samples (vector u) generated in step S101.^pSubstitute each of (k)) into the equation of state (Equation (1)) to calculate P particles. In the following, the particle at timing k is q^pNotated as (k). After step S102, the process proceeds to step S103.

0060(Step S103) The first estimation unit 193 substitutes each of the particles obtained in step S102 into the observation equation, and calculates the estimated value of the measured value by the measuring unit 110. As described above, there are an observation equation (Equation (6)) in the case of the active sensor 112 and an observation equation (Equation (9)) in the case of the passive sensor 111. Here, the first estimation unit 193 obtains an estimated value of the measured value of the direction of the target with respect to the sensor at the timing k by using the observation equation (Equation (9)) in the case of the passive sensor 111. In the following, the estimated value of the measured value of the target orientation with respect to the sensor at timing k is θ.^pNotated as (k). Observation error W_ARegarding (k), for example, the probability distribution to which the observation error follows and its parameters are set in advance based on the characteristics of the sensor (passive sensor 111 or active sensor 112) and the actual environment (environment for measurement). .. Then, by generating a random number based on the set probability distribution and parameters, it is possible to generate an observation error corresponding to each particle. After step S103, the process proceeds to step S104.

0061(Step S104) The selection unit 194 calculates an error between the estimated value of the measured value obtained in step S103 and the measured value. Here, the selection unit 194 is the estimated value θ of the measured value of the target orientation with respect to the sensor at the timing k.^pFrom (k), the measured value θ of the target orientation with respect to the sensor at timing k_mThe difference obtained by subtracting (k) is calculated as an error. In the following, the error between the estimated value of the measured value obtained in step S103 and the measured value by the measuring unit 110 is e.^pNotated as (k). After step S104, the process proceeds to step S105.

0062(Step S105) The selection unit 194 has the error e calculated in step S104.^pReciprocal of (k) e^p(k)^-1From the likelihood π at timing k^pCalculate (k). The selection unit 194 is the reciprocal of the error e^p(k)^-1Is normalized so that the sum is 1. Likelihood π at timing k^p(k) is expressed as Eq. (11).

0063<maths num="11"><img id="000013" he="23" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0064After step S105, the process proceeds to step S106.

0065(Step S106) The selection unit 194 is a particle having a high likelihood (likelihood π).^pSelect (resampling) P particles (P is a positive integer) in order from the particle with the larger value of (k). Here, P is a predetermined constant. The selection unit 194 rejects particles other than the selected P particles. After step S106, the process proceeds to step S107.

0066(Step S107) The correction unit 195 applies an extended Kalman filter to each of the P particles selected in step S106 to correct the value of each particle. The correction unit 195 makes corrections so as to increase the likelihood of each particle. Therefore, the correction unit 195 recalculates the estimated value of the measured value by the sonar using each of the particles selected by the selection unit 194. Here, unlike the case of step S103, the correction unit 195 uses the observation equation (Equation (6)) by the active sensor 112 at the time of active sensing. On the other hand, the correction unit 195 uses the observation equation (Equation (9)) by the passive sensor 111 at the time of passive sensing. As a result, the information of the distance measurement by the active sensor 112 can be reflected in the likelihood correction of the particles.

0067Here, for passive sensing, the observation equation by passive sonar is a non-linear equation including arctangent (atan). Therefore, based on the framework of the extended Kalman filter, the observation equation is Taylor-expanded and the approximate value truncated by the first-order term is adopted. Specifically, Eq. (9) is modified and used as in Eq. (12).

0068<maths num="12"><img id="000014" he="14" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0069Here, the vector C (k) is a vector indicating a coefficient, and is expressed by Eq. (13).

0070<maths num="13"><img id="000015" he="24" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0071Partial differential h_k/ x (k) is expressed by Eq. (14).

0072<maths num="14"><img id="000016" he="23" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0073α (k) indicates the ratio of the y-coordinate value of the relative position of the object to the sensor divided by the x-coordinate value. α (k) is expressed by Eq. (15).

0074<maths num="15"><img id="000017" he="21" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0075Partial differential h_k/ y (k) is expressed as in equation (16).

0076<maths num="16"><img id="000018" he="21" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0077By using the observation equation shown in Eq. (12), the correction unit 195 can calculate the Kalman gain during passive sensing as well as during active sensing. The correction unit 195 makes corrections using Kalman gain for each particle. The Kalman gain L (k) here is expressed by Eq. (17).

0078<maths num="17"><img id="000019" he="17" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0079P (k) indicates the estimation error covariance matrix at timing k. W (k) shows the variance matrix of the observation error at timing k. The particle correction using the Kalman gain L (k) here is shown by Eq. (18).

0080<maths num="18"><img id="000020" he="18" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0081Q on the left side of equation (18)^p(k) indicates the corrected particle at the timing k. Q on the right side^p(k) indicates the particle before correction at the timing k. Also, q_e^p(k) shows the estimated value of the motion parameter at the timing k. After step S107, the process proceeds to step S108.

0082(Step S108) The second estimation unit 196 calculates the arithmetic mean of the particles after correction using Kalman gain. The second estimation unit 196 uses the obtained average value as the motion parameter estimation value of the target at the timing k. In the following, the motion parameter estimate of the target at timing k is q_eNotated as (k). After step S108, the process proceeds to step S109.

0083(Step S109) The second estimation unit 196 is the motion parameter estimation value q of the target at the timing k._eBased on (k) and the position of the sensor, the distance between the sensor and the target, the direction of the target with respect to the sensor, and the relative velocity are calculated. The distance R (k) between the sensor and the target at the timing k is expressed by Eq. (19).

0084<maths num="19"><img id="000021" he="20" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0085As the x-coordinate value x (k) and y-coordinate value y (k) of the target position here, both are q._eUse the value in (k). The direction θ (k) of the object with respect to the sensor at the timing k is expressed by Eq. (20).

0086<maths num="20"><img id="000022" he="20" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0087As the x-coordinate value x (k) and y-coordinate value y (k) of the target position here, both are q._eUse the value in (k). The relative velocity v (k) of the object with respect to the sensor at the timing k is expressed by Eq. (21).

0088<maths num="21"><img id="000023" he="20" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0089As the x-coordinate value x'(k) and y-coordinate value y'(k) of the target velocity here, both are q_eUse the value in (k). After step S109, the process proceeds to step S110.

0090(Step S110) The control unit 190 sets k: = k + 1. That is, the index value indicating the sampling timing is advanced (increased) by 1. After step S110, the process from step S101 is repeated at the next sampling timing.

0091FIG. 6 is a diagram showing an example of convergence of the estimated value of the motion parameter by the motion parameter estimation device 100 to the vicinity of the true value. The horizontal axis of the graph shown in FIG. 6 indicates the time, and the vertical axis indicates the value of the motor parameter. The line L21 shows the true value of the motor parameter. Line L22 shows the estimated value of the motion parameter by the motion parameter estimation device 100. Line L23 shows the estimated values of the motion parameters when only the active sensor is used. The time t11 indicates the sampling period when only the active sensor is used. In the case of using only the active sensor, as described above, the active sensor has a waiting time from outputting the physical energy to the object to receiving the physical energy reflected by the object, so it is indicated by the time t11. As such, a certain sampling interval occurs. On the other hand, in the motion parameter estimation device 100, the passive sensor 111 can measure the motion of the object at a sampling interval shorter than the sampling interval of the active sensor 112. In this respect, the motion parameter estimation device 100 can measure the motion of the object in a shorter cycle than the case of using only the active sensor, and is expected to estimate the motion of the object with higher accuracy. In addition, it is expected that it will converge to near the true value more quickly.

0092As described above, the measuring unit 110 measures the position of the object using the active sensor 112, and measures the direction of the object 20 using the passive sensor 111 in a cycle shorter than the measurement cycle of the active sensor 112. .. The drive noise sample setting unit 191 sets a plurality of drive noise samples in the equation of state based on a predetermined probability distribution. The particle calculation unit 192 calculates a plurality of particles in the particle filter by substituting each of the driving noise samples into the equation of state. The first estimation unit 193 substitutes each of the particles calculated by the particle calculation unit 192 into the observation equation, and calculates the estimated value of the measured value of the measurement unit 110. The selection unit 194 selects a part of the plurality of particles based on the error between the measured value of the measuring unit 110 and the estimated value of the first estimation unit 193. The second estimation unit 196 calculates an estimated value of the motion parameter based on the particles selected by the selection unit 194. In this way, in the motion parameter estimation device 100, by applying the particle filter to the model including the state equation and the observation equation, the measurement result of the active sensor 112 and the measurement result of the passive sensor 111 can be applied to the same model. it can. As a result, the motion parameter estimation device 100 can use the measurement result of the position of the object 20 by the active sensor 112, and measures the motion of the object 20 in a shorter sampling cycle than the case where the active sensor 112 alone is used. be able to. In this respect, according to the motion parameter estimation device 100, the estimation accuracy of the motion of the object 20 can be improved by combining the active sensor 112 and the passive sensor 111.

0093&lt;Second embodiment&gt; In the first embodiment, the position of the sensor ([x_O(k) y_O(k)]) error is not considered. On the other hand, in any positioning such as positioning using GNSS (Global Navigation Satellite System) or dead reckoning, an error may occur in the positioning result of the sensor position. Therefore, in the second embodiment, the motion parameter of the sensor itself is estimated in addition to the motion parameter of the target.

0094The configuration of the motion parameter estimation device 100 in the second embodiment is the same as that in the first embodiment described with reference to FIG. Further, the processing procedure in the second embodiment is the same as the processing procedure in the case of the first embodiment described with reference to FIG. However, in the second embodiment, the mathematical model is extended as follows. The motion parameter vector q (k) includes the motion parameters of the sensor in addition to the motion parameters of interest. The motion parameter state vector q (k) at the timing k in the second embodiment is expressed by Eq. (22).

0095<maths num="22"><img id="000024" he="22" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0096In the second embodiment, the drive noise is also extended. The drive noise vector u (k) at the timing k in the second embodiment is expressed by Eq. (23).

0097<maths num="23"><img id="000025" he="15" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0098u₁(k) and u₂Like (k), u₃(k), u₄All of (k) are scalar variables. In the second embodiment, the matrices A and B of the equation of state shown in Eq. (1) are also extended. In the second embodiment, the matrix A is replaced with the matrix A in the equation (1).₁Is used. Matrix A₁Is expressed as in equation (24).

0099<maths num="24"><img id="000026" he="21" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0100Where R^12×12Indicates a 12-by-12 real matrix. In the second embodiment, the matrix B is replaced with the matrix B in the equation (1).₁Is used. Matrix B₁Is expressed as in equation (25).

0101<maths num="25"><img id="000027" he="21" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0102Where 0_6×2Indicates a 6-by-2 zero matrix. R^12×4Indicates a 12-by-4 real matrix. In the second embodiment, the matrix C of the observation equation shown in Eq. (6) is also extended. In the second embodiment, the matrix C is replaced with the matrix C in Eq. (6).₁Is used. Matrix C₁Is expressed as in equation (26).

0103<maths num="26"><img id="000028" he="21" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0104Where 0_2×6Indicates a 2-by-6 zero matrix. In the second embodiment, an extension is performed in which the measured value of the self-position is also added to the observed value vector. In the second embodiment, Eq. (27) is used instead of Eq. (7) as the observed value vector in the case of active sonar.

0105<maths num="27"><img id="000029" he="19" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0106Where x_OmIndicates the x-coordinate value of the measured value (positioning result) of the position of the sensor itself. y_OmIndicates the y-coordinate value of the measured value of the position of the sensor itself. Since the position of the sensor can be measured separately from the active sonar and the passive sonar, equation (28) is used as the observed value vector in the case of the passive sonar.

0107<maths num="28"><img id="000030" he="16" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0108θ_m(k) is as described with reference to Eqs. (9) and (12). Eq. (29) is used instead of Eq. (13) as an approximation of the non-linear function at the time of passive in step S107 of FIG.

0109<maths num="29"><img id="000031" he="32" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0110In the case of a passive sensor, the matrix C shown in Eq. (30) is replaced with the matrix C of the observation equation shown in Eq. (6).₁Is used.

0111<maths num="30"><img id="000032" he="20" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0112C₂Is expressed as in equation (31).

0113<maths num="31"><img id="000033" he="15" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0114As mentioned above, 0_2×6Indicates a 2-by-6 zero matrix. Also, R^2×12Shows a 2-by-12 real matrix.

0115As described above, in the second embodiment, the equation of state shows the motion of the motion parameter estimation device 100 in addition to the motion of the object 20. Further, the observation equation includes the equation of the measured value of the motion of the motion parameter estimator 100 in addition to the equation of the measured value of the motion of the object 20. The second estimation unit 196 calculates an estimated value of the motion parameter indicating the motion of the object 20 and the motion of the motion parameter estimation device 100. As a result, the motion parameter estimation device 100 of the second embodiment can correct the measurement error of the motion of the motion parameter estimation device 100 itself. In this respect, the estimation accuracy of the motion of the object 20 can be further improved.

0116&lt;Third embodiment&gt; If a measured value of the sensor's speed can be obtained in addition to the sensor's own position, the estimated speed of the state, the focusing speed near the true value, or both can be further improved by using the measured value of the sensor's measurement. there is a possibility. Therefore, in the third embodiment, in addition to the expansion in the second embodiment, the speed of the sensor itself is further expanded to be included in the observed value vector. The configuration of the motion parameter estimation device 100 in the third embodiment is the same as that in the first embodiment described with reference to FIG. Further, the processing procedure in the third embodiment is the same as the processing procedure in the case of the first embodiment described with reference to FIG. However, in the third embodiment, the mathematical model is further extended from the case of the second embodiment. The observed value vector at the time of active sensing in the third embodiment is shown by Eq. (32).

0117<maths num="32"><img id="000034" he="26" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0118x_Om(k)'indicates the measured value of the x-coordinate value of the sensor velocity at the timing k. y_Om(k)'indicates the measured value of the y-coordinate value of the velocity of the sensor at the timing k. Further, in the third embodiment, the matrix C is replaced with the matrix C in the equation (6).₂Is used. Matrix C₂Is expressed as in equation (33).

0119<maths num="33"><img id="000035" he="20" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0120Where 0_2×6Indicates a 2-by-6 zero matrix. 0_4×6Indicates a 4-by-6 zero matrix. Also, matrix C₃Is expressed as in equation (34).

0121<maths num="34"><img id="000036" he="34" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0122On the other hand, the measured value vector at the time of passive sensing is shown by Eq. (35).

0123<maths num="35"><img id="000037" he="26" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0124In this case, in Eq. (6), C instead of the matrix C₄Use (k). C₄(k) is expressed as Eq. (36).

0125<maths num="36"><img id="000038" he="19" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0126Where R^5×12Indicates a real matrix with 5 rows and 12 columns. Matrix C₅Is expressed as in equation (37).

0127<maths num="37"><img id="000039" he="15" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0128As mentioned above, 0_4×6Indicates a 4-by-6 zero matrix. R^4×12Shows a 4-by-12 real matrix.

0129As described above, in the third embodiment, the observation equation includes the equation of the measured value of the velocity of the motion parameter estimation device 100. As a result, in the motion parameter estimation device 100 of the third embodiment, the motion estimation accuracy of the motion parameter estimation device 100 itself can be further improved, and thus the motion estimation accuracy of the object 20 can be further improved.

0130&lt;Fourth Embodiment&gt; By measuring the Doppler effect by the active sensor 112, the velocity component in the sensor direction of the target velocity can be measured. Extensions that add this to one of the measurements may further improve the estimated rate of state, the rate of focusing near the true value, or both. Therefore, in the fourth embodiment, the measured value of the velocity component in the sensor direction of the target velocity is extended to be included in the observed value vector. The fourth embodiment can be applied to any of the first to third embodiments. The configuration of the motion parameter estimation device 100 in the fourth embodiment is the same as that in the first embodiment described with reference to FIG. Further, the processing procedure in the fourth embodiment is the same as the processing procedure in the case of the first embodiment described with reference to FIG. However, in the fourth embodiment, the mathematical model is extended as follows.

0131The Doppler effect of the active sensor 112 appears in the velocity component of the velocity of the object 20 in the direction of the measuring unit 110. Of the velocities of the object 20 at timing k, the velocity component v in the direction of the measuring unit 110_d(k) is expressed as Eq. (38).

0132<maths num="38"><img id="000040" he="15" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0133Here, v (k) indicates the velocity of the object at the timing k. φ (k) indicates the traveling direction of the object as an angle with respect to the x-axis. Using the state to be estimated (the component of the motion parameter vector q (k)), Eq. (38) is given as Eq. (39).

0134<maths num="39"><img id="000041" he="32" wi="170" file="JP2018063142A_D0001.tif" img-format="tif" img-content="drawing" /></maths>

0135Equation (39) is added as one of the observation equations during active sensing.

0136As described above, in the fourth embodiment, the observation equation includes the equation of the measured value of the velocity of the object 20 based on the Doppler effect in the active sensor 112. According to the motion parameter estimation device 100 in the fourth embodiment, the estimation accuracy of the motion of the object 20 can be further improved in that the measured value of the motion of the object 20 increases.

0137&lt;Fifth Embodiment&gt; In the process shown in FIG. 5, the measured value by passive sensing (direction of the target as seen from the sensor) is always used in step S103. Therefore, in step S104, the error is always calculated using the measured value by passive sensing. On the other hand, at the time of active sensing, by using not only the measured value of the target direction but also the measured value of the distance, the estimated speed of the state, the focusing speed near the true value, or both of them may be further improved. Therefore, in the fifth embodiment, the error measurement method is switched between active sensing and passive sensing. At the time of active sensing, not only the direction of the target but also the measured value of the distance is used. Specifically, in step S103 of FIG. 5, the observation equation shown in Eq. (6) is used during active sensing. On the other hand, during passive sensing, the observation equation shown in Eq. (9) is used. As the fifth embodiment, the fifth embodiment can be applied to any of the first to fourth embodiments.

0138As described above, the first estimation unit 193 calculates the estimated value of the measured value of the measurement unit 110 by using the observation equations of the active sensor 112 and the passive sensor 111, respectively. The accuracy of the estimated value calculated by the first estimation unit 193 is improved by the first estimation unit 193 calculating the estimated value of the measured value of the measuring unit 110 based not only on the direction of the object 20 but also on the position of the object 20. It is expected that this will be improved and the estimation accuracy of the motion of the object 20 will be further improved.

0139A program for realizing all or a part of the functions of the control unit 190 is recorded on a computer-readable recording medium, and the program recorded on the recording medium is read by the computer system and executed. May be processed. The term "computer system" as used herein includes hardware such as an OS and peripheral devices. In addition, the "computer system" shall include the homepage providing environment (or display environment) if the WWW system is used. Further, the "computer-readable recording medium" refers to a portable medium such as a flexible disk, a magneto-optical disk, a ROM, or a CD-ROM, or a storage device such as a hard disk built in a computer system. Further, a "computer-readable recording medium" is a communication line for transmitting a program via a network such as the Internet or a communication line such as a telephone line, and dynamically holds the program for a short period of time. In that case, it also includes the one that holds the program for a certain period of time, such as the volatile memory inside the computer system that is the server or client. Further, the above-mentioned program may be a program for realizing a part of the above-mentioned functions, and may be a program for realizing the above-mentioned functions in combination with a program already recorded in the computer system.

0140Although the embodiments of the present invention have been described in detail with reference to the drawings, the specific configuration is not limited to this embodiment, and design changes and the like within a range not deviating from the gist of the present invention are also included.

01411 Measurement system 10 mobile 20 objects 100 Motion parameter estimator 110 Measuring unit 111 Passive sensor 112 Active sensor 180 memory 190 Control 191 Drive noise sample setting unit 192 Particle calculation unit 193 First Estimator 194 Selection 195 Correction section 196 Second estimation department