JP2018116511A - State estimator, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a state estimator and a program which can estimate, when a state is estimated, with smaller calculation amount, short delay and high accuracy upon a state estimation using a system model regulating a time change of a predetermined state vector and which can remove influence of observation error.SOLUTION: A state estimator 1 according to the present invention a likelihood calculating unit 11 for calculating likelihood respectively corresponding to state vectors of a plurality of particles following a predetermined system model from input observation values, a filter distribution updating unit 12 for calculating a prediction distribution by subjecting a previously calculated filter distribution to diffusion process in accordance with the predetermined system model and for calculating, as a current filter distribution, a filter distribution obtained by subjecting weight of a state vector of each particle to a range adjusting processing for setting it within a predetermined range by allocating the likelihood, a weight register 13 for updating and holding weight of a state vector of each particle upon every calculation, and an estimated value calculating unit 14 for deriving a predetermined estimated value from the current filter distribution.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a state estimator and a program that perform state estimation using a system model that defines a temporal change of a predetermined state vector.

  State estimators that perform state estimation related to predetermined state vectors such as time series data can be broadly classified into those that handle linear or Gaussian time series data and those that handle nonlinear or non-Gaussian time series data. .

  In particular, a state estimator that handles nonlinear or non-Gaussian time series data can be used for time offset measurement in a system that synchronizes time via an IP (Internet Protocol) network.

  First, as a technique for synchronizing this time, generally, IEEE 1588 PTP (precision time protocol) is known (for example, refer to Patent Document 1), and a time control apparatus for establishing time synchronization based on this technique is known. The example which comprises is also known (for example, refer patent document 2).

  In Patent Document 2, as the time control device, a subtraction unit and a division unit that calculate a time difference from time information of packets exchanged between a master device and a slave device, an average value calculation unit that calculates an average value from the time difference, An example is shown in which a PID (Proportional-Integral-Differential) control unit and a time adjustment unit are provided.

  In PTP, the oscillation frequency is synchronized by adjusting the oscillation frequency of the clock of the slave device so that the difference between the transmission interval and the reception interval of the Sync message periodically transmitted from the master device to the slave device becomes equal. Then, after the oscillation frequency synchronization is established, it transmits Delay_req from the slave device, receives Delay_res as a reply, and uses the transmission time and reception time of those packets, the time of the master device and the time of the slave device The difference is measured, and the time of the slave device is adjusted so that the time difference becomes zero. In the measurement of the time difference, it is assumed that the Delay_req transmission delay and the Delay_req transmission delay are equal.

  An IP network is a network in which packets can be sent to any device connected to the IP network by relaying the packet to the next device using a router or switch. The time until the packet is relayed becomes longer. For this reason, a phenomenon called packet jitter in which the transmission delay time fluctuates occurs.

  As a result, the difference between the transmission interval and the reception interval of the Sync message, and the measured time difference between the master device and the slave device are influenced by packet jitter and contain many observation errors.

  The PID control unit controls the input value, the integrated value of the input, and the differential value of the input by multiplying each of the coefficients to a target value. A signal including a large amount of components that change to a large amount includes a high frequency component, and an excessive differential value may adversely affect PID control.

(Use of low-pass filter)
Therefore, in a system that synchronizes the time via the IP network, the observed time difference is not used for time control as it is, but the average value is used or the influence on the control of observation error and noise is reduced by using a low-pass filter. Mitigation has been done.

  However, in the low-pass filter, when pulse-like noise having a large amplitude is added, the low-frequency component is added as a signal, which may slightly affect the output.

  For this reason, instead of using the difference between the frequency distribution of time series data and noise like a low-pass filter to improve the signal observation accuracy, a state space model of the system that handles time series data is created and the state space is Predict model states and extract observed signals from predicted state-space model states to improve signal observation accuracy, shorten observation delays, and use the predicted states themselves for direct control Has been done.

  Kalman filters and particle filters are known as state estimators that estimate the state of a time-series model by using the statistical properties of states and observation values.

(Use of Kalman filter)
The Kalman filter is a technique that can be applied to a linear system in which noise included in a state or an observation value follows a normal distribution, and is widely used. However, the Kalman filter does not give good results in a system that does not follow a normal distribution, a nonlinear system, or a case where a sudden structural change is observed in time series.

(Use of particle filter)
Therefore, a particle filter is known as a state estimator applicable to a system that handles nonlinear or non-Gaussian time series data (for example, Non-Patent Document 1), and the particle filter is also used for tracking a subject in an image. (See, for example, Patent Document 3).

The particle filter is a state estimator that performs state estimation using a predetermined system model that defines a time change of a state vector expressed by a one-dimensional or multi-dimensional vector, and has a weight corresponding to the state vector and the state vector. The probability distribution of the state is expressed by the set. When the random variable a probability distribution over states of the prediction target and probability density function p (x) to x, which a certain state vector x _{i (hereinafter,} the state x _i) and the weight w _i of the state x _i It is characterized by being expressed by a set of particles. The weight w _i indicates the likelihood of each state x _i .

Probability density function of state x using N state vectors x _i (i = 1, 2... N) and weights w _i (i = 1, 2... N) of the state vectors. p (x) can be expressed as in equation (1).

Here, δ is a Dirac delta function, and the state vector and the weight of the state vector are called particles. In the particulate filter, the values of w _i by the filter distribution calculation takes a value other than 1, the value of w _i by performing a resampling processing immediately becomes one.

  For example, an example of estimating the position and speed of a moving object (vehicle in the illustrated example) will be described with reference to FIG. 9 based on the processing procedure of the particle filter shown in FIG. FIG. 8 is a flowchart showing processing by a conventional particle filter. Moreover, FIG. 9 is explanatory drawing which shows the example of the whole operation | movement in the conventional particle filter.

  Here, x (t), y (t), v (t), and u (t) are respectively a state vector at time t (hereinafter referred to as state x (t)) and an observation value vector (hereinafter referred to as observation). Value y (t)), system noise vector (hereinafter referred to as noise vector v (t)), and observation noise vector (hereinafter referred to as observation noise u (t)). Then, the state space model of the time series data includes the system model of the state x (t) = F (x (t−1), v (t)) and the observed value y (t) = H (x (t), Suppose that it follows the observation model of u (t)).

  In the example of the state estimation system of the time series data of the moving object shown in FIG. 9, the position Xa ([km]) and the velocity Xb ([km / h]) at the time t when the system noise is added to the state x (t). The observed value y (t) is obtained by adding the observation noise u (t) to the position Xa (t) observed at time t.

  First, as shown in FIG. 8, the particle filter process first sets particles corresponding to the distribution of the initial state x (t = 0) using particles having a weight of 1 (step S110). The processing of the particle filter is sampled for each unit time step as time t = 1, 2,... After the initial time t = 0, and processed in time series, as shown in FIG. The state at the current time t is predicted from the distribution (hereinafter referred to as x (t−1 | t−1)) that approximates the estimation result with particles and the system model (hereinafter, the observed value y (t) is used). The state predicted at the current time t using the predicted distribution x (t | t−1) and the observed value y (t). Will be repeated. Hereinafter, the distribution estimated using the predicted distribution x (t | t−1) and the observed value y (t) is referred to as a filter distribution x (t | t).

Subsequently, as shown in FIGS. 8 and 9, the particle filter processing is based on the particles constituting the filter distribution x (t−1 | t−1) of the estimation result of the state at the previous time t−1. For each particle, a pseudo-random number is calculated, the system model x (t) = F (x (t−1), v (t)), the state vector x _i of each particle is x (t−1), By substituting the system noise generated from the pseudo random number into v (t) and updating the new state of each particle, the predicted distribution x (t | t−1) of the state at time t is calculated (step S120). ).

  With reference to FIG. 10, the calculation of the predicted distribution x (t | t−1) in the state at time t in this example will be described more specifically. FIG. 10 is an explanatory diagram showing an operation example of predictive distribution calculation in a conventional particle filter.

  In the calculation of the predicted distribution, the particle filter process calculates the system noise v (t) generated from the state vector of each particle and the pseudo random number for each particle at time t−1 as the system model x (t) = F ( x (t-1), v (t)).

  The particle filter can handle a non-linear system model, but for simplicity, a linear model that can express the system model as x (t) = Fx (x (t−1)) + Fv (v (t)) will be described as an example. . If there is no system noise, the state vector of each particle at time t is the state vector of each particle at time t−1 (refer to the value of each point of x (t−1) in FIG. 10A) and v ( The state Fx (x (t−1)) obtained by giving t) = 0 to the system model is obtained (see FIG. 10B). Since the influence of noise is added as a term of Fv (v (t)) in the system model, Fv (t) calculated by generating v (t) using a pseudorandom number as the value of Fx (x (t−1)). v (t)) is added. This is the predicted value of the state vector of each particle at time t (see FIG. 10C). A distribution expressed by these predicted particles is a predicted distribution t (t | t−1).

  As shown in FIG. 10C, it can be seen that by calculating with the presence of system noise, the predicted value of the state vector of the particle at time t is intentionally expanded. If the effects of system model modeling errors and state estimation errors accumulate, the error in the predicted value of the particle state vector increases. If the particle state vector no longer spreads, there will be no particles with a state vector that matches the actual system state vector. By adding such system noise, there is an effect of preventing an estimation error accompanying a large prediction error due to a state estimation error or a modeling error.

Subsequently, as illustrated in FIGS. 8 and 9, the particle filter process performs the likelihood p (y |) of the observed value y (t) for each particle in the predicted distribution x (t | t−1). x _i ) is calculated, and a filter distribution x (t | t) with the weights of the particles as w _i is calculated (step S130).

  Here, the state vector represented by each particle represents the predicted position and predicted speed of the moving object (vehicle in this example). Although the predicted value of the state vector of each particle varies from particle to particle, assuming that the observed value y (t) of the position actually observed is 10.2 km (see FIG. 9), FIG. The calculation of the state filter distribution x (t | t) in this example will be described more specifically with reference to FIG. FIG. 11 is an explanatory diagram showing an operation example of filter distribution calculation in a conventional particle filter.

Even when the observed value y (t) at time t is 10.2 km, since the observed noise is included in the observed result, the observed value does not necessarily match the actual position. Therefore, in calculating the filter distribution, the particle filter processing is performed for each particle in the predicted distribution x (t | t−1) (see FIG. 11A), and the observed value y (t) = 10 at time t. The highest likelihood is assigned to the particle corresponding to the coincidence with 2 km, and the lower likelihood is assigned to each particle as it moves away from the particle corresponding to the observed value y (t) (see FIG. 11B). There are various techniques for likelihood calculation. Here, an example of assigning by approximating a simple normal distribution curve is given. In this way, for each state represented by particles, the likelihood of obtaining an observed value is calculated, and the value is set as the weight w _{i of} each particle (see FIG. 11C). In FIG. 9 and FIG. 11 (c), the particle weight w _i is represented by the size of a black circle.

Subsequently, as shown in FIGS. 8 and 9, the particle filter process performs a resample process for reassigning particles to each particle of the filter distribution in proportion to the size of the particle weight (step S140). ). The resampling process is a process of assigning or deleting a new particle having a weight of 1 so that the number of particles having the state x _i is proportional to the weight w _i calculated in step S130. Particles are duplicated with new particles with weight 1 and particles with lower weight are deleted. After this resampling process, the process proceeds to step S120 so that 1 is added to t and prediction at the next time is performed.

  As a result of the weighting based on the likelihood, the particles after the resampling process (step S140) are concentrated and higher than the particles in the predicted distribution obtained in the calculation of the predicted distribution (step S120) as illustrated in FIG. A density particle distribution is obtained. Since the set of these particles represents the estimation result of the state distribution at time t, by calculating the expected value, median, and confidence interval of the particle state vector, the expected value of the state vector at time t and the center are calculated. Value and confidence interval can be derived (for example, 10.3 km as an expected value of position and 39 km / h as an expected value of velocity).

  In this way, the particle filter estimates the state distribution of the next time step using the immediately preceding state distribution, system model, and observation value, and uses the resulting state distribution for prediction of the next time. By repeating this process sequentially and performing statistical processing such as expected value calculation on a set of particles, state estimation can be performed.

JP 2010-190635 A JP2013-83450A Japanese Patent Laid-Open No. 2006-58836

Tomoyuki Higuchi, “Particle Filter”, Journal of IEICE, Vol.88, No.12, pp.989-994, published December 01, 2005

  In a system that synchronizes time via an IP network, in order to synchronize the time of a clock or a transmitter with high accuracy, it is necessary to detect and correct a slight phase change due to a very low frequency change. For this reason, it is necessary to perform a filtering process that sufficiently removes observation errors and noises having low frequency components.

  Therefore, in order to sufficiently reduce errors and noise of low frequency components using a low-pass filter, it is necessary to increase the number of filter taps. When the number of taps of the low-pass filter is increased, the delay increases, and it takes a long time for observation and the time required for synchronizing the time. In addition, packet jitter occurs due to temporary packet concentration, and the size of the packet jitter changes depending on the concentration of the packet. Therefore, the delay varies greatly during the observation period, and the observation accuracy deteriorates and the control becomes unstable. There is a problem.

  In addition, the low-pass filter has a problem in that, when pulse-like noise having a large amplitude is added, the low-frequency component is added as a signal and affects the output.

  The Kalman filter is premised on the state and observation error following a normal distribution. However, most of the noise included in the time difference calculated from the packet time information is due to a change in the transmission delay of the IP network, which changes in a complicated manner depending on the congestion status of the IP network, and an error. The distribution of will change. The Kalman filter has a problem that good results cannot be obtained when a sudden structural change is observed in a time series, a system that does not follow a normal distribution, or a nonlinear system.

  In addition, the particle filter has an advantage that it can cope with a complicated distribution and can flexibly cope with a change in likelihood distribution. However, it is necessary to create a number of pseudo-random numbers proportional to the number of particles in the prediction distribution calculation, and a large calculation load is generated in the resampling process. For this reason, the particle filter has a large calculation load, and it has been difficult to use the particle filter for applications that operate at high speed in real time.

  In other words, not only a system that synchronizes time via an IP network, but also a control device that estimates the state of a moving object, when it is desired to operate at high speed in real time, the amount of calculation is small and the delay is short. Therefore, it is desirable to use a state estimator that can accurately estimate the state and eliminate the effects of observation errors.

  In view of the above-described problems, the object of the present invention is to accurately estimate a state with a small amount of calculation and a short delay when performing state estimation using a system model that defines a time change of a predetermined state vector. It is an object to provide a state estimator and a program that can remove the influence of observation errors.

  The state estimator of the present invention is a state estimator that performs state estimation using a predetermined system model that defines a temporal change of a state vector represented by one-dimensional or multi-dimensional vector particles. A likelihood calculation unit that calculates likelihoods corresponding to the state vectors of a plurality of particles according to the predetermined system model based on the input observation values according to the state probability distribution expressed by a set of state vector weights. And for each observation time of the observation value, the state vector of each particle and the state of each particle according to the predetermined system model, assuming that there is no system noise with respect to the state vector of each particle constituting the previously calculated filter distribution Update the weight of the vector, and apply a diffusion process to the weight to add particles constituting the prediction distribution or update the weight of each particle, In addition, a value obtained by multiplying each state vector constituting the prediction distribution by the likelihood calculated by the likelihood calculating unit is updated as a new weight, and the weight in the state vector of each particle is kept within a certain range. A filter distribution update unit that performs a range adjustment process, calculates a filter distribution composed of a state vector of each particle obtained by performing the range adjustment process and a weight of the state vector of each particle as a current filter distribution, and Each time the filter distribution is calculated by the filter distribution updating unit, a weight register that updates and holds the weight of the state vector of each particle, the state vector that constitutes the current filter distribution, and the weight of the state vector are predetermined. And an estimated value calculation unit for deriving an estimated value.

  In the state estimator according to the present invention, the filter distribution update unit may update the state vector of each particle updated according to the predetermined system model with respect to each state vector constituting the filter distribution calculated last time as the diffusion process. Including a weighted average or low-pass filter process based on the weight of the state vector of each particle, or a process of calculating a new weight by a process of uniformly adding a constant value to the weights of all or a certain range of state vectors. It is characterized by that.

  Further, in the state estimator of the present invention, the filter distribution updating unit increases or decreases all weights of a plurality of state vectors in the filter distribution at a constant ratio as the range adjustment process, and sets the largest weight value. It is characterized by including a process within a certain range.

  In the state estimator of the present invention, when the maximum value of the weight updated to the weight register exceeds the first threshold as the range adjustment process, the filter distribution update unit sets all the weights to be constant. It includes a process of increasing all weights at a constant ratio when the ratio is decreased and the maximum weight updated to the weight register is equal to or less than a second threshold value.

  In the state estimator according to the present invention, the filter distribution updating unit may calculate all the weights when the maximum weight updated to the weight register exceeds the first threshold as the range adjustment processing. Including a process of increasing all the weights by a ratio of 2 when the weight value is reduced by a ratio of 1/2 and the maximum weight value to be updated to the weight register is less than or equal to the second threshold value. And

  Further, in the state estimator of the present invention, the likelihood calculation unit calculates the likelihood by omitting a normalization process in which the sum of the likelihoods for the state vector of each particle is 1, and the value of the likelihood, Alternatively, a value that is a constant multiple of the likelihood is output to the filter distribution updating unit.

  Furthermore, the program of the present invention is a program for causing a computer to function as the state estimator of the present invention.

  According to the present invention, when estimating a state using a system model that defines a time change of a predetermined state vector, it is possible to accurately estimate a state with a small amount of calculation and a short delay, and to eliminate the influence of an observation error. A state estimator can be configured.

It is a block diagram which shows schematic structure of the state estimator of one Embodiment by this invention. It is a flowchart which shows the filter distribution update process in the state estimator of one Embodiment by this invention. It is explanatory drawing which shows the example of a whole operation | movement of the filter distribution update part in the state estimator of one Embodiment by this invention. It is explanatory drawing which shows the operation example regarding the filter distribution calculation process of the filter distribution update part in the state estimator of one Embodiment by this invention. It is explanatory drawing which shows the operation example regarding the weight register which concerns on the process of the filter distribution update part in the state estimator of one Embodiment by this invention. It is explanatory drawing which shows the operation example regarding the likelihood calculation which concerns on the process of the filter distribution update part in the state estimator of one Embodiment by this invention. It is explanatory drawing which shows the operation example regarding the range adjustment process of the filter distribution update part in the state estimator of one Embodiment by this invention. It is a flowchart which shows the process by the conventional particle filter. It is explanatory drawing which shows the example of the whole operation | movement in the conventional particle filter. It is explanatory drawing which shows the operation example of the prediction distribution calculation in the conventional particle filter. It is explanatory drawing which shows the operation example of filter distribution calculation in the conventional particle filter.

  Hereinafter, a state estimator 1 according to an embodiment of the present invention will be described with reference to FIGS. 1 to 7.

(Device configuration)
First, FIG. 1 shows a schematic configuration of a state estimator 1 according to an embodiment of the present invention. The state estimator 1 is configured as a device that performs state estimation based on an input observation value using a predetermined system model that defines a time change of a state vector expressed by one-dimensional or multi-dimensional vector particles. The state probability distribution is expressed by a set of state vectors and weights of the state vectors, and includes a likelihood calculating unit 11, a filter distribution updating unit 12, a weight register 13, and an estimated value calculating unit 14.

In the state estimator 1 according to the present invention, the state x probability distribution is divided into N state vectors x _i (i = 1, 2... N) and weights w _i (i = 1, 2... N) of the state vectors. .N), the probability density function p (x) can be expressed as the above-described equation (1).

  The likelihood calculation unit 11 calculates the likelihood corresponding to the state vector of each particle constituting the prediction distribution x (t | t−1) from the input observation value, and calculates the value or a value of a constant multiple thereof. This is a functional unit that outputs to the filter distribution updating unit 12. The likelihood calculation unit 11 calculates the likelihood that an observation value is obtained when the state estimation target system is in a state represented by a state vector. The likelihood calculation method used in the particle filter can be used as it is.

  However, the likelihood calculated by the likelihood calculating unit 11 according to the present invention does not necessarily have to be normalized so that the total likelihood for each state vector is 1. In other words, a division circuit (division process) can be provided to omit the process of normalizing the likelihood, thereby contributing to speeding up of the process. Even if this normalization process is omitted, since a range adjustment process, which will be described later, is provided, there is no problem in operation.

  The filter distribution updating unit 12 determines that there is no system noise with respect to the state vector of each particle constituting the filter distribution x (t−1 | t−1) calculated at the previous time for each observation time. In accordance with a predetermined system model that defines the time change of the state vector expressed by the above, the state vector of each particle and the weight of each particle held in the weight register 13 are updated, and the updated state vector of each particle and each of these The prediction distribution x (t | t−1) is calculated by performing diffusion processing on the weights for the state vectors of the particles and updated, and for each particle constituting the prediction distribution x (t | t−1), the input Updating the likelihood of the particle state vector calculated by the likelihood calculation unit 11 based on the observed value by multiplying the weight of the particle by a new weight, Filter distribution before Nji adjustment x '(t | t) is calculated. Then, the filter distribution updating unit 12 performs range adjustment processing on the weight of each particle constituting the filter distribution x ′ (t | t) before the range adjustment, and the weight held in the weight register 13 is applied to the range. The weight is updated to the weight after the adjustment process, and the filter distribution x (t | t) expressed by each particle to which the weight subjected to the range adjustment process is given is output to the estimated value calculation unit 14.

  More specifically, the filter distribution updating unit 12 first calculates the state vector of each particle constituting the filter distribution x (t−1 | t−1) calculated at the previous time for each time when the observation value was obtained. Based on the weight held in the weight register 13, assuming that there is no system noise, the state vector at the time when the observation value is obtained is calculated to update the state vector of each particle, and the weight held in the weight register 13 Is updated in association with the updated state vector of each particle. Note that if the state vectors after the update of the state vectors of two or more particles match, the filter distribution update unit 12 collects the particles into one, and adds a value obtained by adding the weights of the particles before the update. The process of setting the weight of the collected particles is performed. Subsequently, the filter distribution updating unit 12 adds a constant value evenly to the weight corresponding to each state vector calculated as having no system noise, to the weighted average or low-pass filter processing, or the weight of the state vector in a certain range. Through the process, a diffusion process for expanding the variance of the predicted distribution is performed to obtain a predicted distribution x (t | t−1).

  For example, when the range of the state vector is limited within the range of a constant multiple of the standard deviation of the previously estimated state vector centered on the expected value of the previously estimated state vector, All within range.

  As a result, the state prediction distribution at the observation time calculated from the previous filter distribution x (t−1 | t−1) is diffused without calculating the system noise v (t) using pseudorandom numbers, and the state The prediction range can be intentionally expanded. Thus, the filter distribution updating unit 12 uses the state vector and weight of each particle constituting the previously calculated filter distribution x (t−1 | t−1), the system model, and the state without system noise at the observation time. A predicted distribution x (t | t−1) is calculated without calculating a pseudo-random number by calculating a prediction distribution and performing a diffusion process on the prediction distribution.

  Subsequently, the filter distribution updating unit 12 calculates the likelihood calculation unit 11 based on the input observation value for each state vector constituting the prediction distribution x (t | t−1) subjected to the diffusion process. The filter distribution x (t | t) is calculated by using a value obtained by multiplying the weight of the relevant particle by the weight of the particle as a new weight.

  Subsequently, the filter distribution updating unit 12 performs range adjustment processing for keeping the weight of each state vector constituting the filter distribution x (t | t) within a certain range, and is held in the weight register 13. The weight is updated with the weight after the range adjustment process. Accordingly, the filter distribution update unit 12 causes the weight register 13 to update and hold the weight of each state vector of each particle constituting the filter distribution every time the filter distribution is calculated.

  The range adjustment process is a process for increasing / decreasing all the weights of a plurality of state vectors in the filter distribution at a certain ratio and keeping the largest weight value within a certain range. In particular, in the range adjustment process, when the maximum value of the weight updated to the weight register 13 exceeds the first threshold value Th1, all the weights are reduced by a certain ratio, and the weight updated to the weight register 13 is updated. When the maximum value of becomes less than or equal to the second threshold Th2, all weights are increased at a constant ratio. Since there is this range adjustment process, the likelihood normalization process used to calculate the filter distribution can be omitted, and the overall operation can be speeded up.

  Then, the filter distribution update unit 12 outputs the filter distribution expressed by the state vector and the particles to which the weight subjected to the range adjustment process is given to the estimated value calculation unit 14.

  The weight register 13 stores weights respectively corresponding to a plurality of predetermined state vectors of particles according to a predetermined system model as an initial value, but every time the filter distribution update unit 12 calculates the filter distribution, It is a functional unit that updates and holds the weights of the state vectors of a plurality of particles constituting the filter distribution. That is, the weight register 13 reads the weight of the previous value in order to associate with the state vector of the particles constituting the filter distribution calculated this time, and associates the current value with the plurality of state vectors in the filter distribution after the current calculation. Each time the filter distribution is calculated by writing the weight, the function unit updates and holds the weights of the plurality of state vectors in the filter distribution. The weight register 13 is also used when the filter distribution updating unit 12 calculates the predicted distribution x (t | t−1).

  From the current filter distribution x (t | t) expressed by the state vector and the weight of each particle to which the weight subjected to the range adjustment process obtained from the filter distribution updating unit 12 is given the weight, the estimated value calculation unit 14 This is a functional unit that derives the expected value, median value, or confidence interval of the state estimated by calculating the expected value, median value, or confidence interval of the particle state vector, and outputs the result to the outside as an estimated value.

(Device operation)
Hereinafter, the operation of the state estimator 1 shown in FIG. 1 will be described in detail focusing on the filter distribution update processing shown in FIGS. 2 and 3 in order to be able to compare with the particle filter of the conventional technique. FIG. 2 is a flowchart showing a process (filter distribution update process) of the filter distribution update unit 12 in the state estimator 1 according to the embodiment of the present invention. FIG. 3 is an explanatory diagram illustrating an overall operation example of the filter distribution updating unit 12 in the state estimator 1 according to the embodiment of the present invention.

Before describing with reference to FIG. 2 and FIG. 3, first, the state estimator 1 according to the present embodiment uses a probability density function p (X) of a state to be predicted (a random variable) X as a state x _i . It is the same as the particle filter in that the weight w _i of the state is expressed by a set of particles, but the main difference is that the prediction distribution corresponding to the addition of system noise is performed not by pseudorandom numbers but by diffusion processing. The difference is that, in obtaining the state estimation result based on the filter distribution, the range adjustment is performed instead of the resampling process.

  In addition, x (t), y (t), v (t), and u (t) represent a state vector, an observation value vector, a system noise vector, and an observation noise vector at time t, respectively. . Then, the state space model of the time series data includes the system model of the state x (t) = F (x (t−1), v (t)) and the observed value y (t) = H (x (t), Suppose that it follows the observation model of u (t)).

In the example of the state estimation system of the time series data of the moving object shown in FIG. 3, the position Xa ([km]) and the speed Xb ([km / h]) at the time t when the system noise is added to the state x (t). The observed value y (t) is obtained by adding the observation noise u (t) to the position Xa (t) observed at time t. The possible states x _i of the state x (t) at time t are assumed to be Xa = {10.1, 10.2, 10.3, 10.4, 10.5, 10.6}, Xb = {39, 40 , 41}, the operation will be described. In this case, the state (Xa, Xb) can take the state of 18 in which Xa and Xb are combined. In the processing of this example, an example in which any one of these 18 states is taken at time t will be described. However, the states that can be taken as time elapses should be updated, and interpolation interpolation is performed. It can also be configured.

  Here, it is preferable that each state that the state x (t) can take corresponds to a bit string or a numerical value. For example, {“001”, “010”, “011”, “100”, “101”, “110”, “111”} for Xa and {“00”, “01”, By making "10"} correspond, the state of (Xa, Xb) can be associated with a 5-bit binary number obtained by combining these binary numbers, and the processing configuration can be simplified and contributed to high-speed processing. . In addition, an integer from 1 to 18 may correspond to each of the 18 types of states in order.

  The filter distribution updating unit 12 in the state estimator 1 shown in FIG. 1 is expressed in two dimensions in this example based on the likelihood obtained from the likelihood calculating unit 11 and the weight held in the weight register 13. The current filter distribution x (t, from which the estimated value can be derived from the filter distribution x (t−1 | t−1) previously calculated every time the observation value y (t) is obtained according to the system model that defines the change in state. | T) is obtained, and the operation is performed to obtain the current filter distribution.

Likelihood calculating unit 11 receives the observation value y (t) = y, the state x (t) is the likelihood p to observe the observed value y when _{x i} | calculates the (y _{x i),} the value Or a value of a constant multiple thereof is output to the filter distribution updating unit 12.

The likelihood calculation unit 11 and the filter distribution update unit 12 are connected, for example, in the process in which the filter distribution update unit 12 calculates the filter distribution, the likelihood of the state x _i required for the calculation is calculated by the likelihood calculation unit 11. The likelihood calculating unit 11 can calculate the likelihood based on the observed value y for the requested state x _i and output the likelihood to the filter distribution updating unit 12. Request of the likelihood of the state x _i may be specified by using the bit string or a number that each state x _i is made to correspond that can be taken of the state x (t). In addition, the likelihood calculating unit 11 calculates the likelihood of the possible state x _{i at} a time, and collectively applies the bit string and the numerical value corresponding to the state x _i in the order corresponding to the filter distribution updating unit 12. It can also be set as the structure output to.

The weight register 13 stores the weight w _i of particles having a specific state x (t) = x _i , and the function of reading and writing the weight w _{i in association} with the state x (t) = x _i It is a memory part with. Hereinafter, the weight register corresponding to the state x _i at time t is denoted by w _i . Further, the weight of the state (Xa, Xb) in the system for predicting the position of the moving object illustrated in FIG. 3 is assumed to be W (Xa, Xb).

  As described above, the states x (t) can be in 18 possible combinations of Xa and Xb in this example. Since there is a weight corresponding to each of these states, the weight register 13 stores 18 kinds of weights W, specifies the weight W corresponding to each state under the control of the filter distribution update unit 12, and reads and writes the weights. Operates to do. As described above, each state can be associated with an address of a storage device (not shown) in the weight register 13 by associating each state with a bit string or a numerical value. Can be achieved.

(Filter distribution update process)
First, as shown in FIG. 2, the filter distribution updating unit 12 in the state estimator 1 sets an initial weight in the weight register 13 prior to the processing (step S1). Here, when the initial distribution of each state can be predicted, the value is set, but when it cannot be predicted, an equal weight may be set. Thereafter, the weight held in the weight register 13 is a predicted distribution with no system noise based on the previous filter distribution x (t−1 | t−1) described below by the processing of the filter distribution updating unit 12. When calculating x ′ (t | t−1), subsequently calculating the prediction distribution x (t | t−1) obtained by performing the diffusion process, and subsequently, the filter distribution x (t | t taking into account the likelihood. ) At the time of calculation, and at the time of calculation of the filter distribution x (t | t) that is finally subjected to the range adjustment processing, it is updated in association with the update of the state vector of each particle.

  Then, the filter distribution update unit 12 based on the weight held in the weight register 13 for each particle (state vector and its weight) in the “previous filter distribution x (t−1 | t−1)”. Prediction distribution x (t | t−1) is calculated by performing diffusion processing (step S2), and likelihood calculation unit 11 is based on the input observation value y (t) for each particle in the prediction distribution. The filter distribution x (t | t) is calculated by using a value obtained by multiplying the likelihood calculated by the weight of the particle as a new weight (step S3).

  Then, the filter distribution updating unit 12 performs a range adjustment process on the weight of each particle in the filter distribution x (t | t) (step S4), and the weight held in the weight register 13 is applied to the weight after the range adjustment process. The current filter distribution x (t | t) expressed by each particle to which the weight subjected to the range adjustment process is applied is output to the estimated value calculation unit 14. The filter distribution is updated by repeating steps S2 to S4.

  Hereinafter, each processing of steps S2 to S4 in the filter distribution update unit 12 will be described in order with reference to FIG.

  First, the filter distribution updating unit 12 calculates a predicted distribution x (t | t−1) based on the diffusion process from the previous filter distribution x (t−1 | t−1) (step S2).

  Here, in the processing of the particle filter described above, a pseudo-random number v (t) is generated and then applied to the system model x (t) = F (x (t−1), v (t)), thereby predicting the distribution x. Although (t | t−1) is calculated, a large amount of calculation resources are required for the calculation of the pseudo-random number, which hinders high-speed processing.

  Therefore, the filter distribution updating unit 12 according to the present invention is configured to diffuse to particles present in the vicinity in the state variable space based on the weight held in the weight register 13 without generating a pseudo-random number. ing.

  For example, the diffusion process can be a weighted average using the weights of particles existing in the vicinity, a low-pass filter, or a process of adding a constant value evenly to the weights of a certain range of state vectors. Thereafter, the new weight of each particle subjected to the diffusion process is used for the predicted distribution x (t | t−1).

  If a weighted average or low-pass filter is used for this diffusion process, the weight of the particles is diffused to the nearby particles in the state variable space, so it is similar to the process of diffusing each particle with pseudorandom numbers in the conventional particle filter. Can give an effect.

  More specifically, in the conventional particle filter, it is necessary to calculate a pseudo-random number of the number of particles × the number of dimensions of system noise in order to calculate a prediction distribution for each particle. The distribution updating unit 12 can significantly reduce the calculation amount by performing diffusion processing instead of using pseudorandom numbers. In the diffusion process, for example, for each particle predicted as having no system noise at time (t), a new particle having a weight of 0 is added if there is no particle around each particle of interest (for example, four points). Is diffused by giving, as a new weight, a weighted average of the weights around the particles.

  As another example, for particles having a state vector near the expected value of the previously estimated filter distribution x (t−1 | t−1) corresponding to all the lattice points in FIG. When spreading by assigning a new weight to a constant range of equally spaced state vectors including the predicted value of the state vector at time t, a particle with a weight of 0 is added if no particle already exists at time t After that, by giving a weighted average value of the surrounding weights, a value subjected to low-pass filter processing, or a value obtained by adding a constant value to the original weight as a new weight for all particles Spread.

More specifically, when calculating the predicted distribution, the filter distribution updating unit 12 first obtains a newly calculated filter distribution x (t−1 | t−1) (FIG. 4A). ))), Based on the weight held in the weight register 13, x ′ (t) is determined as no system noise in the system model x (t) = F (x (t−1), v (t)). = Predicted distribution x ′ (t | t−1) of F (x (t−1), 0) is calculated (see FIG. 4B). In FIG. 4A and the like, the particle weight w _i is represented by the size of a black circle.

As an example, a description will be given assuming that the system that is the target of state estimation is a linear system model having a two-dimensional state vector that can be expressed by the following equation.
Xa (t) = Xa (t−1) + Xb (t−1) * Δt + v1 (t)
Xb (t) = Xb (t-1) + v2 (t)
Here, the two-dimensional state vector has elements Xa and Xb, the state vector at time t is composed of Xa (t) and Xb (t), and Δt is from the previous observation time to the current observation time. Elapsed time, and v1 (t) and v2 (t) are system noises at time t.

  In the conventional particle filter, a pseudo random number is calculated every time a predicted value of each particle is calculated, and v1 (t) and v2 (t) calculated from the state vector of the particle at time t-1 and the pseudo random number are The predicted value of the state vector was calculated by assigning it to the system model. In the conventional particle filter, since the particles are duplicated by the resampling process, there are a large number of particles having the same state vector. However, since there is a pseudo-random term, the particles having the same value at the time t−1 in the state vector. Even so, the predicted value of the state vector is not the same value. Thus, the system noise has the effect of increasing the variance of the prediction distribution.

  On the other hand, the state estimator 1 according to the present invention does not calculate pseudo random numbers. For this reason, the state estimator 1 according to the present invention assigns the state of each particle t-1 to the system model with no system noise, that is, v1 (t) and v2 (t) as 0, After calculating the predicted value of the state vector of the particle, the state vector is diffused. In this way, the state estimator 1 according to the present invention can obtain the same effect as the application of the system noise by increasing the variance of the prediction distribution by the diffusion process.

  In the state estimator 1 according to the present invention, as shown in FIG. 5, there is no system noise, that is, v1 (t) and v2 (t) are set to 0, and the state (Xa, Xb) of each particle is this system model. The state prediction value with no system noise is calculated, and the weight of each particle before prediction is set in association with it as it is. When two or more particles have the same predicted state, the particles are combined into one, and a value obtained by adding the weights of the particles before the update is used as the combined particle weight. In this way, the predicted distribution x ′ (t | t−1) shown in FIG. 4B is calculated from the filter distribution x (t−1 | t−1) shown in FIG.

  In the example shown in FIG. 5, W ′ (Xa + Xb * Δt, Xb) = W (Xa, Xb). Here, W ′ (Xa, Xb) is a weight when there is no system noise.

  Subsequently, the filter distribution updating unit 12 performs a diffusion process for adding new particles to the vicinity of each particle in the filter distribution based on the weight held in the weight register 13, so that the system noise v <b> 1 ( t) and v2 (t) are given, and a predicted distribution x (t | t−1) indicating the state x (t) of each particle is calculated (see FIG. 4C).

For example, as an example of performing diffusion processing with a weighted average,
W ″ (Xa, Xb) = (W ′ (Xa−0.1, Xb)
+ W ′ (Xa, Xb) * 12
+ W '(Xa + 0.1, Xb)
+ W ′ (Xa, Xb−1)
+ W ′ (Xa, Xb + 1)) / 16
As described above, the weighted average of the weights W ′ (Xa, Xb) of each particle when the system noise obtained by adding four points around the target particle is eliminated, and the weight W ″ (Xa) of each particle after diffusion. , Xb) can be calculated (see FIG. 5).

Subsequently, filter the distribution updating unit 12, the prediction was subjected to diffusion treatment distribution x | against state x _i of each particle in the (t t-1) (see FIG. 6 (a)), the input observations The likelihood p (y | x _i ) calculated by the likelihood calculating unit 11 is assigned based on y (t) = y (see FIG. 6B), and the filter distribution shown in FIG. 6C is calculated. (Step S3). At this time, the filter distribution updating unit 12 uses a value obtained by multiplying the weight w _i of the state x (t) = x _{i by} the likelihood p (y | x _i ) as a new weight of the state x (t) = x _i. As w _i (W ′ ″ shown in FIG. 5), the weight held in the weight register 13 is updated (see FIG. 4D and FIG. 5).

  Subsequently, the filter distribution updating unit 12 performs a range adjustment process for keeping the weight of each particle in the filter distribution within a certain range, and associates the weight held in the weight register 13 with each particle in the filter distribution. The weight is updated to the weight after the range adjustment process (step S4). Accordingly, the filter distribution updating unit 12 causes the weight register 13 to update and hold the weight of each of the plurality of particles in the filter distribution every time the filter distribution is calculated.

  As described above, in the conventional particle filter, as described above, the resampling process is performed after the filter distribution is calculated. However, the filter distribution updating unit 12 according to the present invention performs the range adjustment without performing the resampling process. Operate.

After the calculation of the filter distribution, the value of the weight w _i may become smaller as a whole, in which case the calculation accuracy is reduced. In addition, the value of the weight w _i may become extremely large, and an overflow occurs and deviates greatly from the correct weight. In this case, the calculation accuracy also decreases. For example, when the observed noise in an observed value is extremely large in the positive direction with respect to the observed true value, the weight of the entire particle in the filter distribution is affected, and all the weights become smaller overall. Sometimes. In addition, since the output of the likelihood calculation unit 11 is a constant multiple of the likelihood, the value of the weight w _i may be extremely large depending on the value of the constant.

  Therefore, after the filter distribution is calculated, the filter distribution updating unit 12 increases / decreases all the weights of the plurality of particles in the filter distribution at a certain ratio, and performs a range adjustment process for keeping the largest weight value within a certain range. Do.

  In particular, in the filter distribution updating unit 12 of the present embodiment, as shown in FIG. 7, when the maximum value of the weight updated to the weight register 13 exceeds the first threshold value Th1 as the range adjustment process, When the weight is reduced by a certain ratio and the maximum value of the weight updated to the weight register 13 is equal to or less than the second threshold Th2, all the weights are increased by a certain ratio. Since there is this range adjustment process, the likelihood normalization process used to calculate the filter distribution can be omitted, and the overall operation can be speeded up.

  Furthermore, when the maximum value of the weight updated to the weight register 13 exceeds the first threshold Th1, all the weights are reduced by a ratio of “½ times”, and the maximum weight updated to the weight register 13 is set. When the value is equal to or less than the second threshold Th2, if all the weights are increased by a ratio of “2 times”, it is realized by a bit shift by one operation clock, which contributes to further speedup. It will be a thing.

  Then, the filter distribution updating unit 12 outputs the current filter distribution expressed by each particle to which the weight subjected to the range adjustment process is given to the estimated value calculation unit 14.

  The estimated value calculation unit 14 obtains the expected value, median value, or confidence interval of the particle from the current filter distribution expressed by each particle to which the weight subjected to the range adjustment processing obtained from the filter distribution update unit 12 is given. Is derived and output to the outside as a state estimation result.

  For example, in the example of the moving object state estimation system shown in FIG. 3, the estimated value calculation unit 14 determines that the expected value Ea of the position Xa is all particles (state x (t)) at time t held in the weight register 13. ) Weights W (Xa, Xb) can be read out and calculated by ΣXa * W (Xa, Xb) / ΣW (Xa, Xb). Note that Σ represents addition processing for all states.

  As described above, the state estimator 1 according to the present invention uses the filter distribution update unit 12 to calculate the filter distribution previously calculated every time an observation value is obtained in accordance with a predetermined system model that defines a change in state represented in a plurality of dimensions. Since the prediction distribution is calculated by performing a diffusion process on each state vector based on the weight held in the weight register 13, high-speed processing with high accuracy is possible.

  Further, the state estimator 1 according to the present invention is configured such that particles for one state are collected into one, a filter distribution is calculated from the predicted distribution, and the range adjustment process is performed without performing the resampling process. Therefore, higher speed processing is possible.

  On the other hand, in the conventional particle filter, the prediction distribution is calculated for each particle, so it is necessary to calculate the number of pseudo-random numbers proportional to the number of particles. It requires a huge amount of calculation and is difficult to use for applications that operate at high speed in real time.

  In the particle filter of the conventional technique, processing other than processing involving pseudo-random numbers can be grouped for each state, but a copy of the particles is created by the resampling processing, so there are a plurality of particles having the same state. As a result, it becomes impossible to associate the state with the particle and its weight on a one-to-one basis. For this reason, although the particle filter of the conventional technique increases versatility, the processing efficiency is very poor.

  The state estimator 1 according to the present invention can solve these particle filter problems, accurately estimate the state with a small amount of calculation and a short delay, and can remove the influence of the observation error.

  The state estimator 1 according to the present invention has a configuration that does not require calculation or resampling processing with a large calculation load while taking advantage of the particle filter, so that it is linear or Gaussian with a small calculation load. It is not limited to systems that handle time-series data, but is also a practical configuration for state-estimation systems for time-series data based on models that do not follow the normal distribution or nonlinear models, and for systems that have a large change in likelihood distribution. It has become.

  The state estimator 1 according to the present invention can function as a computer, and a program for causing the computer to realize each component is stored in a memory of the computer. Under the control of a central processing unit (CPU) provided in the computer, a program in which processing content for realizing the function of each component is described is appropriately read from the memory, and the function of each component is It can be realized on a computer.

  The state estimator 1 and the program according to the present invention are not limited to the above-described embodiments, but are limited only by the description of the scope of claims.

  According to the present invention, when estimating the state of time-series data, the state is accurately estimated with a small amount of calculation and a short delay, and the influence of the observation error can be removed. It is useful for the use of a signal processing device such as a time-series data control device, a time-series analysis device, or tracking of a moving object.

DESCRIPTION OF SYMBOLS 1 State estimator 11 Likelihood calculation part 12 Filter distribution update part 13 Weight register 14 Estimated value calculation part

Claims (7)

A state estimator that performs state estimation using a predetermined system model that defines a time change of a state vector represented by one-dimensional or multi-dimensional vector particles,
A likelihood corresponding to each of the plurality of particle state vectors according to the predetermined system model is calculated based on an input observation value according to a state probability distribution expressed by a set of state vectors and weights of the state vectors. A likelihood calculator,
At each observation time of the observation value, the state vector of each particle and the state vector of each particle are determined according to the predetermined system model, assuming that there is no system noise with respect to the state vector of each particle constituting the previously calculated filter distribution. The weight is updated, and the weight is subjected to diffusion processing to add particles constituting the prediction distribution or update the weight of each particle, and the likelihood calculating unit applies the weight to each state vector constituting the prediction distribution. A value obtained by multiplying the calculated likelihood is updated as a new weight, a range adjustment process is performed to keep the weight in the state vector of each particle within a certain range, and the state of each particle obtained by performing the range adjustment process A filter distribution updating unit that calculates a filter distribution composed of a vector and a weight of a state vector of each particle as the current filter distribution;
Each time the filter distribution is calculated by the filter distribution update unit, a weight register that updates and holds the weight of the state vector of each particle;
An estimated value calculation unit for deriving a predetermined estimated value from each state vector constituting the current filter distribution and the weight of the state vector;
A state estimator comprising:   The filter distribution updating unit performs a weighted average or low pass based on the state vector updated according to the predetermined system model and the weight of the state vector for each state vector expressing the filter distribution calculated last time as the diffusion process. The state estimator according to claim 1, further comprising: a filter process or a process of calculating a new weight by a process of adding a constant value evenly to all or a predetermined range of state vector weights.   The filter distribution update unit includes, as the range adjustment process, a process of increasing / decreasing all the weights of a plurality of state vectors in the filter distribution at a certain ratio and keeping the largest weight value within a certain range. The state estimator according to claim 1, wherein the state estimator is characterized.   When the maximum value of the weight updated to the weight register exceeds the first threshold as the range adjustment process, the filter distribution update unit reduces all the weights by a certain ratio, and transfers to the weight register. The state according to any one of claims 1 to 3, further comprising a process of increasing all the weights at a constant ratio when the maximum weight value to be updated is equal to or less than the second threshold value. Estimator.   The filter distribution update unit, as the range adjustment processing, when the maximum value of the weight to be updated to the weight register exceeds the first threshold, all the weights are reduced by a factor of 1/2, 5. The state according to claim 4, further comprising a process of increasing all weights by a ratio of 2 when a maximum value of weights to be updated to the weight register is equal to or less than the second threshold value. Estimator.   The likelihood calculation unit calculates a likelihood by omitting a normalization process in which the total likelihood for each state vector is 1, and calculates the likelihood value or a value of a constant multiple of the likelihood as the filter. The state estimator according to claim 1, wherein the state estimator is output to a distribution updating unit.   The program for functioning a computer as a state estimator as described in any one of Claim 1 to 6.