JP4820132B2 - State estimation device and program thereof, and integrated state estimation system and integrated state estimation device - Google Patents

Description

  The present invention relates to a state estimation device and a program for estimating a state amount of an observation target based on an observation amount of the observation target, and an integrated state estimation system and an integrated state estimation device.

  Conventionally, an estimation method using a Kalman filter is known as a method for estimating a state quantity from an observation quantity to be observed. This Kalman filter combines multiple observations into a single observation vector and performs filtering processing along with the covariance matrix of the observation noise that is mixed during observation. is there.

As another method, an estimation method using a particle filter such as a condensation method is known (for example, see Non-Patent Document 1). This particle filter expresses a probability density function by finite particles (a set of a plurality of vectors), and estimates a state quantity from an observation amount input in time series using the probability density function. According to this particle filter, even when non-Gaussian noise that cannot be estimated by the Kalman filter is mixed, the state quantity can be estimated by an arbitrary non-Gaussian probability density function.
Furthermore, a method of adaptively switching the particle filter by variance of the estimated amount has been proposed (see Patent Document 1).
M. Isard and A. Blake, CONDENSATION-Conditional Density Propagation for Visual Tracking, International Journal of Computer Vision, vol.29, no.1, pp.5-28 (1998) JP 2005-44352 A

However, according to the conventional state estimation method described above, since a plurality of observation amounts are processed together, the processing is concentrated in the apparatus. For this reason, in the state estimation apparatus to which the conventional method is applied, the processing by this method is a factor that hinders the speeding up.
Moreover, in the state estimation device to which the conventional state estimation method is applied, the processing is concentrated. Therefore, when the state estimation device fails, the state estimation operation stops. For this reason, in order to perform state estimation stably, a plurality of the same state estimation apparatuses must be operated, and there is a problem that the entire system becomes expensive.
In addition, in the state estimation device to which the conventional state estimation method is applied, the state is estimated based on the observation amount determined in advance, so it is not easy to change the observation target or the target to be estimated. There is a problem of lacking.

  The present invention has been made in view of the above-described problems, and a state estimation device and a program thereof that improve the high-speed, reliability, and versatility of estimation processing by operating a plurality of state estimation operations in parallel. An object of the present invention is to provide an integrated state estimation system and an integrated state estimation device.

The present invention was created to achieve the above object, and first, the state estimation device according to claim 1 is configured to estimate a plurality of states connected to a communication path based on an observation amount of an observation target. A state estimation device for use in an integrated state estimation system that integrates and estimates state quantities indicating a state of an observation target by an apparatus , the state quantity storage means, state update means, state reception means, and state integration Means and a state transmission means.

  In such a configuration, the state estimation device updates the state quantity (internal state quantity) stored in the state quantity storage unit by the state update unit with the observation amount input from the outside. As a result, the internal state quantity is sequentially updated based on the observed quantity. The state quantity may be updated by an observation update equation of a general Kalman filter or an extended Kalman filter, or may be updated as a set of state vectors by a sequential Monte Carlo method.

In addition, the state estimation device receives, as an external state amount, the internal state quantity that is distributed and stored in the other state estimation apparatus transmitted from the other state estimation apparatus connected to the communication path by the state reception unit. Then, the state integration unit integrates the external state quantity and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity. Thereby, the state estimation device can take in and use the state quantity estimated in another state estimation device. It should be noted that the state quantity integration is to process state quantities distributed and stored in a plurality of state estimation devices into one state quantity, for example, taking an average value of a plurality of state quantities. It is good also as a new state quantity by this, and it is good also as a new state quantity by selecting each state quantity so that it may become a predetermined probability distribution.
In addition, the state estimation device uses the state transmission unit to set the internal state quantity stored in the state quantity storage unit to the other state estimation apparatus connected to the communication path as the external state quantity with respect to the other state estimation device. Send. As a result, the state estimation device can cause other state estimation devices to use the state quantity estimated by itself.

  According to a second aspect of the present invention, in the state estimation device according to the first aspect, the state quantity storage unit stores a plurality of the internal state quantities in association with time.

  In such a configuration, the state estimation device stores the internal state quantity in association with the time in the state quantity storage unit, so that the state quantity transmitted from the other state estimation apparatus and the internally stored state quantity are determined according to the time. Can be associated.

  Furthermore, the state estimation device according to claim 3 is the state estimation device according to claim 2, and is configured to include state prediction means.

  In this configuration, the state estimation device predicts the state quantity at a certain time based on the plurality of internal state quantities stored in the state quantity storage unit by the state prediction unit. The state quantity can be predicted by a general state transition equation or a transition probability between a current state and a future state.

  In addition, the integrated state estimation system according to claim 4 estimates, based on the observation amount of the observation target, a plurality of state estimation devices connected to the communication path to integrate the state amount indicating the state of the observation target. In this integrated state estimation system, the state estimation device includes a state quantity storage unit, a state reception unit, a state integration unit, and a state transmission unit, and at least one of the plurality of state estimation devices is a state update unit Further, at least one of the plurality of state estimation devices includes an estimated amount output unit.

In such a configuration, in the integrated state estimation system, the state estimation device configuring the system stores the state quantity to be observed as the internal state quantity in the state quantity storage unit. Then, the state estimation device receives, as the external state amount, the internal state quantity that is distributed and stored in the other state estimation apparatus transmitted from the other state estimation apparatus connected to the communication path by the state reception unit. Then, the state integration unit integrates the external state quantity and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity. In addition, the state estimation device uses the state transmission unit to set the internal state quantity stored in the state quantity storage unit to the other state estimation apparatus connected to the communication path as the external state quantity with respect to the other state estimation device. Send. In this way, the integrated state estimation device system can be integrated by transmitting and receiving state quantities in each state estimation device.

In addition, in the update of the state quantity, at least one of the plurality of state estimation devices sequentially updates the state quantity (internal state quantity) stored in the state quantity storage unit with the observation amount input from the outside by the state update unit. Do that. The number of state update means is determined based on the number of observations, and if at least one observation amount is provided, if it is provided in one of a plurality of state estimation devices Good.
Then, at least one of the plurality of state estimation devices outputs the internal state quantity stored in the state quantity storage means as the estimated quantity by the estimated quantity output means.

Furthermore, the integrated state estimation device according to claim 5 is an integrated state estimation device that estimates by integrating the state quantities indicating the state of the observation target based on the observation amount of the observation target, the integrated state estimation A plurality of state estimation means for estimating the state quantity , connected to the inside of the apparatus via a communication path , wherein the state estimation means comprises a state quantity storage means, a state reception means, a state integration means, and a state transmission means; And at least one of the plurality of state estimation units includes a state update unit, and at least one of the plurality of state estimation units includes an estimation amount output unit.

In such a configuration, in the integrated state estimation device, the state estimation unit stores the state quantity to be observed as the internal state quantity in the state quantity storage unit. Then, the state estimation means receives, as the external state quantity, the internal state quantity distributed and stored in the other state estimation means transmitted from the other state estimation means connected to the communication path. Then, the state integration unit integrates the external state quantity and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity. In addition, the state estimation unit causes the state transmission unit to use the internal state quantity stored in the state quantity storage unit as another external state quantity with respect to the other state estimation unit. Send. In this way, the integrated state estimation device can be integrated by transmitting and receiving state quantities in each state estimation unit.

The state quantity is updated by at least one of the plurality of state estimation means sequentially updating the state quantity (internal state quantity) stored in the state quantity storage means by the observation quantity input from the outside by the state update means. Do that. Then, at least one of the plurality of state estimation means outputs the internal state quantity stored in the state quantity storage means as the estimated quantity by the estimated quantity output means.

Further, the state estimation program according to claim 6 estimates the state quantity indicating the state of the observation target by using a plurality of state estimation devices connected to the communication path based on the observation quantity of the observation target. The computer in the state estimation device used in the integrated state estimation system is configured to function as a state update unit, a state reception unit, a state integration unit, a state transmission unit, and an estimated amount output unit.

In such a configuration, the state estimation program sequentially updates the state quantity (internal state quantity) stored in the state quantity storage unit with the observation amount input from the outside by the state update unit. Then, the state estimation program receives the internal state quantity distributed and stored in the other state estimation apparatus transmitted from the other state estimation means connected to the communication path as the external state quantity by the state reception means. Then, the state integration unit integrates the external state quantity and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity.
In addition, the state estimation program causes the state transmission unit to use the internal state quantity stored in the state quantity storage unit as the external state quantity for the other state estimation apparatus in another state estimation apparatus connected to the communication path. Send. In addition, the state estimation program outputs the internal state quantity stored in the state quantity storage means as the estimated quantity by the estimated quantity output means.

The present invention has the following excellent effects.
According to the first, fourth, or sixth aspect of the invention, it is possible to estimate the state of the observation target by integrating the state quantities estimated by the plurality of state estimation devices. Thus, by distributing the estimation process to a plurality of state estimation devices, a parallel operation is possible, and the estimation operation can be speeded up. Further, according to the present invention, even when a failure occurs in any of the plurality of state estimation devices, the state estimation service can be continued by integrating the state quantities in the other state estimation devices. it can. In addition, according to the present invention, exchange and the like can be performed in units of state estimation devices. Therefore, even when a state is estimated based on a new observation amount, addition and change can be easily performed. Can increase the sex.

  According to the second aspect of the present invention, since the state quantity transmitted from another state estimation device and the state quantity stored therein are associated with each other by time, it is possible to synchronize the time of the state quantity. it can. Thereby, since the time of the state estimated between state estimation apparatuses can be made to correspond, a time lag is eliminated and the accuracy of state estimation can be improved.

According to the third aspect of the present invention, even when no observation amount is input, it is possible to estimate the state and output the estimated amount, so that the state estimation can be performed stably.
According to the fifth aspect of the present invention, it is possible to estimate the state of the observation target by integrating the state quantities estimated by the plurality of state estimating means. As a result, parallel operation becomes possible, and the estimation operation can be speeded up.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
[Outline of integrated state estimation system]
First, the configuration of the integrated state estimation system will be described with reference to FIG. FIG. 1 is an overall configuration diagram showing an overall configuration of an integrated state estimation system according to the present invention.
As shown in FIG. 1, the integrated state estimation system S connects a plurality of state estimation devices 10 via a communication channel (network N), and states (state quantities) of observation targets estimated by the state estimation devices 10. ) Are integrated, a desired state quantity is estimated and output as an estimated value.

The state estimation device 10 estimates the state to be observed (state amount X) based on the observation amount Y input from the outside, and integrates it with the state amount estimated by another state estimation device. Any one of the state estimation devices 10 may input the observation amount Y, and any one may output the estimation amount E. For example, does not output the outside estimate E as state estimating device 10 ₃ form, a form that does not enter the observables Y from the outside, such as in a state estimating apparatus _104, the observation quantity Y as state estimating device 10 ₅ A form in which no input or output of the estimated amount E is performed may be possible.

  Here, the observation target is an object that accompanies a change in state such as movement or fluctuation, or an event that is a person, vehicle, etc. that accompanies movement, or a stock price that accompanies fluctuation. For example, the state estimation apparatus 10 can estimate a person's three-dimensional position, speed, and the like by using a person as an observation target and using the position of the person in an image captured by a camera from a plurality of positions as an observation amount. .

(About state quantity)
Here, the state quantity used in the integrated state estimation system S will be described.
The state quantity is an amount of data indicating the state of movement, fluctuation, etc. of the observation target, and is, for example, the speed of the observation target. This state quantity can be expressed by, for example, one vector (hereinafter referred to as a state vector). For example, a 6-dimensional vector quantity composed of the 3-dimensional position and 3-dimensional velocity of an object, or a vector quantity such as a celestial orbit 6 element can be used as the state quantity.

  Further, for example, the state quantity can be expressed by a set of one vector and one matrix. In this case, the vector is the same as the state vector described above. On the other hand, the matrix can be expressed by a covariance matrix (hereinafter referred to as a state covariance matrix) representing an error included in the state vector.

  Further, for example, the state quantity can be expressed as a set of a plurality of vectors (hereinafter referred to as particles). Note that the individual vectors constituting the particles are the same as the state vectors described above. For example, as each vector, a 6-dimensional vector composed of a 3-dimensional position and a 3-dimensional velocity can be used.

(Observed quantity)
Next, the observation amount input to the integrated state estimation system S will be described.
The observation amount is a data amount as a result of observing the observation target, and is, for example, a two-dimensional position of the observation target. This observation amount can be expressed by, for example, one vector (hereinafter referred to as an observation vector). In addition, when a multidimensional observation result is obtained by a plurality of sensors or the like, it can be expressed as one vector by multiplexing data obtained from each sensor or the like as a row vector.

Further, for example, as the observation amount, in addition to the observation vector, an amount representing an error or reliability of the observation vector can be added. For example, the observation vector error can be expressed by a covariance matrix (hereinafter referred to as an observation covariance matrix).
Furthermore, for example, the observation amount may be defined as a probability density for the state vector described above.

  In the case of handling a scalar quantity in a state vector or an observation vector, this is regarded as a one-dimensional vector quantity. Further, when dealing with a matrix or a tensor quantity, a vector quantity is assumed to be a one-dimensional arrangement of each element according to a predetermined rule.

(System example)
Here, a car navigation system, which is a specific example of the integrated state estimation system, will be described with reference to FIG. FIG. 2 is an overall configuration diagram showing a configuration of a car navigation system which is a specific example of the integrated state estimation system.
The car navigation system SN has a configuration in which the state estimation devices 10 ₁ to 10 ₃ are connected via a communication path (network N). The car navigation system SN uses a three-dimensional position (for example, latitude, longitude, and altitude), a velocity vector, an attitude angle (for example, a roll angle, a pitch angle, and a yaw angle) of an automobile, and an angular velocity vector as a state quantity X. .

Here, the state estimating apparatus 10 ₁ inputs time data GPS (Global Positioning System) receiver 2 is observed as observables _{Y 1.} The state estimating device 10 ₂ inputs the time interval data of the vehicle speed pulse of a motor vehicle wheel vehicle speed pulse measuring device 3 was observed as the observation quantity Y _2. Furthermore, the state estimation unit ₁₀₃ inputs the angular acceleration data gyro sensor 4 is observed as an observation amount Y _3.
Then, for example, the state estimating apparatus 10 ₁ extracts only the position information of the car from the state quantity X, and outputs as the estimated amount of E.

In this way, by connecting the state estimation devices 10 ₁ to 10 ₃ to the communication path (network N), the observation amount Y observed by each observation means of the GPS receiver 2, the vehicle speed pulse measuring device 3, and the gyro sensor 4. _1, Y _2, based on Y ₃ state quantity X are integrated, the state estimating device 10 _1, it is possible to estimate the position of the vehicle, and outputs as the estimated amount of E.

As described above, here, the integrated state estimation system S (see FIG. 1) and the car navigation system SN (see FIG. 2) are configured by a plurality of state estimation devices 10 connected to the network N. The estimation apparatus 10 is incorporated in the internal configuration as one means, and as shown in FIG. 3, each state estimation means 10 is connected via a communication path such as a bus B to constitute the integrated state estimation apparatus 1. It is good as well.
That is, the communication path connecting the state estimation device (state estimation means) is a metal line such as a LAN cable, a telephone line, a serial cable, an optical fiber, an infrared ray, a wireless line, a wiring in a board, a wiring between boards, an IC Any transmission medium such as wiring in the chip can be used. Further, the communication path may be routed through a relay device such as an exchange, a router, a hub, or a repeater.

  Hereinafter, although the internal configuration of the state estimation device 10 constituting the integrated state estimation system S will be described in detail, the internal configuration is also realized with the same configuration in the state estimation means 10 in the integrated state estimation device 1 shown in FIG. can do.

[First Embodiment: Configuration of State Estimation Device (State Estimation Unit)]
First, the configuration of the state estimation device 10 will be described with reference to FIG. FIG. 4 is a block configuration diagram showing an internal configuration of the state estimation apparatus according to the first embodiment of the present invention.
The state estimation device (state estimation means) 10 cooperates with other state estimation devices connected to the network N based on the observation amount of the observation target input from the outside, and obtains a state amount indicating the state of the observation target. To be estimated. Here, the state estimation device 10 includes a state amount storage unit 11, a state update unit 12, a state reception unit 13, a state integration unit 14, a state transmission unit 15, and an estimated amount output unit 16. .

The state quantity storage means 11 stores a state quantity (internal state quantity) held in the state estimation apparatus 10 among the state quantities indicating the state of the observation target. For example, a RAM (Random Access Memory) Etc.
The state quantity storage unit 11 reads or writes a state quantity by a state update unit 12 and a state integration unit 14 to be described later, and reads them by a state transmission unit 15 and an estimated amount output unit 16.

The state update unit 12 updates the state quantity stored in the state quantity storage unit 11 with the observation quantity Y input from the outside. Here, the state update unit 12 reads the state quantity (internal state quantity X _I ) from the state quantity storage unit 11, updates the state quantity with the observation quantity Y, and the updated state quantity (post-observation state quantity X _Y ) as an internal state quantity _{X I,} written in the state quantity storage unit 11.
The state updating unit 12 may write the observation amount Y as it is into the state amount storage unit 11 as the post-observation state amount _XY. However, when noise, error, etc. in the observation amount Y are modeled in advance. In order to improve the accuracy of state estimation, it is desirable to calculate and update the post-observation state quantity _XY based on the model.
Hereinafter, the state quantity update method in the state update unit 12 will be described using mathematical expressions.

(When state quantity and observation quantity are vectors)
First, the state in the case where the state vector x _I (ie, X _I = {x _I }) is adopted as the state quantity (internal state quantity X _I ) and the observation vector y (ie, Y = {y}) is adopted as the observation quantity Y. The amount update will be described. Incidentally, the function h and noise (noise) n of the state vector x _I to observation vector y, the state of the update is assumed to be modeled by the following equation (1).

Here, when the inverse function h ⁻¹ exists in the function h, an appropriate real value α (preferably 0 ≦
using real values) of alpha ≦ 1, can be obtained state vector x _Y after observation by the following equation (2).

That is, the post-observation state quantity _XY is X _Y = {x _Y }. Incidentally, the state quantity storing means 11, in a case where the state vector x _I is not be read (if not present), for example, to "1" the value of α in the formula (2), x _Y = It is good also as calculating _| requiring the state vector xY after observation by h <^-1> (y).

(When state quantity and observation quantity are vector and covariance matrix)
Next, the state vector x _I and the state covariance matrix P _I (that is, X _I = {x _I , P _I }) as the state quantity (internal state quantity X _I ), and the observation vector y and the observation covariance as the observation quantity Y The update of the state quantity when the matrix R (that is, Y = {y, R}) is employed will be described. Incidentally, the function h and noise (noise) n of the state vector x _I to observation vector y, the state of the update, and it can be modeled by the following equation (3).

Here, E [] represents an expected value, and the superscript T represents transposition.
At this time, for example, the post-observation state quantity X _Y = {x _Y , P _Y } can be determined by the observation update equation of the extended Kalman filter shown in the following formula (4).

Here, K represents filter sensitivity, H represents an observation matrix, and superscript T represents transposition.
If the state vector x _I and the state covariance matrix P _I cannot be read from the state quantity storage unit 11 (when they do not exist), for example, x _Y = h ⁻¹ (y), P _Y = diag {λ ₁ , λ ₂ ,..., Λ _D } (where diag is a diagonal matrix, λ ₁ , λ ₂ ,..., Λ _D is a non-negative real number, and D is a dimension of _XY ) Good.

(When state quantity is particle and observation quantity is probability density function)
Next, update of the state quantity when a particle is used as the state quantity (internal state quantity X _I ) and a probability density function is adopted as the observation quantity Y will be described.
First, N number the number of state vectors contained in the internal state quantity _{X I} read from the state quantity storing means 11, each of the state vector ^{x (i)} (provided that, i = 1,2, ..., N ) to . That is, X _I = {x ⁽¹⁾ , x ⁽²⁾ ,..., X ^(N) }.

On the other hand, the probability density function, which is the observation amount Y, is described as Y (x) with an argument as the state vector x.
First, the state update means 12 generates the i-th state vector according to the following equation (5) based on the observation amount Y (x ⁽ⁱ⁾ ) for all i satisfying 1 ≦ i ≦ N. The probability p (i) to be calculated is calculated.

Subsequently, the state updating unit 12 includes many state vectors x ⁽ⁱ⁾ having a large probability p (i) in the post-observation state quantity _XY , and few state vectors x ⁽ⁱ⁾ having a small probability p (i). The state quantity is reorganized (resampled) so as to be included (or not included). Note that the number of samples taking the state vector x ⁽ⁱ⁾ is preferably a frequency (number) proportional to the probability p (i) in order to improve accuracy. For example, the probability p (i) is p (1) = When 1/10, p (2) = 6/10, p (3) = 3/10, and the total number of state quantities is 10, one state vector x ⁽¹⁾ and one state vector x ⁽²⁾ The state quantities (particles) are regenerated as six state vectors x ⁽³⁾ .
For example, the state update unit 12 updates the state quantity according to the following procedure.
First, as shown in the following equation (6), a cumulative probability a (i) obtained by accumulating the probability p (i) shown in equation (5) is calculated, and the post-observation state quantity _XY is converted to an empty set (X _Y = φ).

  Subsequently, the state update unit 12 generates a uniform random number r satisfying 0 <r ≦ 1, as shown in the following formula (7).

  Furthermore, as shown in the following equation (8), the state update unit 12 searches for an array argument i that satisfies the cumulative probability a (i) ≧ r and has the minimum cumulative probability a (i). Let j be j.

Then, the state update unit 12 includes the state vector x ^(j) in the post-observation state quantity _XY as shown in the following equation (9).

Then, the state updating means 12, the equation (7) processing the predetermined number of times to Formula (9) (e.g., the same N times the number of state vectors contained in the internal state quantity X _I) is repeated. As a result, the probability density function is approximated by the frequency of particles in the post-observation state quantity _XY .
Incidentally, the state quantity storing means 11, when it can not be read the internal state quantity X _I (if not present), for example, the state update unit 12 of N generate state vector by a random number, these The summary is defined as a post-observation state quantity _XY . Further, the random number at this time may follow the probability density function Y (x).
Returning to FIG. 4, the description of the configuration of the state estimation device 10 will be continued.

The state receiving means 13 receives a state quantity (reception state quantity X _R ; external state quantity) held by another state estimation apparatus connected to the network N and transmitted from the other state estimation apparatus. . The reception reception state quantity X _R that is output to the state integration unit 14.
Here, the other state estimation device transmits a message including the state quantity (reception state quantity X _R ) via the network N. Note that this message may include information on the transmission source (for example, an IP address).

The state integration unit 14 integrates the state quantity (reception state quantity X _R ) received by the state reception unit 13 and the state quantity (internal state quantity X _I ) stored in the state quantity storage unit 11. _XJ is generated. The integration state quantity X _J is stored in the state quantity storage unit 11 as the internal state quantity X _I, the internal state quantity X _I is updated.
The state integration unit 14, in a case where the state quantity storing means 11 can not be read the internal state quantity X _I (if not present), the received state amount (reception state quantity X _R) as it is integrated state the amount _{X J.}
Hereinafter, the state quantity integration method in the state integration unit 14 will be described using mathematical expressions.

(When the state quantity is a vector)
First, the integration of state quantities when one state vector is adopted as the state quantities (internal state quantity _XI and reception state quantity X _R ) will be described.
When the state vector is adopted as the state quantity, the state integration unit 14 sets the state vector of the internal state quantity X _I as x _I , the state vector of the reception state quantity X _R as x _R, and after integration by the following equation (10) to calculate the state vector _{x J.}

Here, β is an appropriate real value, preferably 0 ≦ β ≦ 1.
The integrated state quantity X _J is X _J = {x _J }.

(When the state quantity is a vector and covariance matrix)
Next, integration of state quantities when a state vector and a state covariance matrix are employed as state quantities (internal state quantity X _I and reception state quantity X _R ) will be described.
As the quantity of state, in the case of employing the state vector and state covariance matrix, condition integration unit 14, a state vector x _I and the state covariance matrix P _I of the internal state quantity X _I, the reception state quantity X _R of the state vector x based on the _R and state covariance matrix P _R, the state vector x _J and the state covariance matrix P _J integration state quantity X _J is calculated by the following procedures.
First, the state integration unit 14, as shown in equation (11) below, extracts the i-th row i column component of the state covariance matrix _{P I} and the state covariance matrix _{P R,} respectively _v I (i) and v _R (i) (where i = 1, 2,..., D; D is the number of dimensions of the state vector).

Subsequently, the state integration unit 14, as shown in the following equation (12), to extract the i-line component of the state vector _{x I} and the state vector _{x R,} respectively _x I (i) and _x R (i) To do.

Then, the state integration means 14 determines the scalar quantity x _T (i) according to the following expression (13) according to the magnitude of v _I (i) and v _R (i) extracted in the expression (11). decide.

Then, the state integration unit 14 arranges the scalar quantity x _T (i) determined by the equation (13) with respect to the array argument i as shown in the following equation (14), thereby obtaining the state vector x _J Configure.

On the other hand, the state integration unit 14 calculates the smaller one of v _I (i) and v _R (i) extracted from the equation (11) from the state covariance matrix P _J as shown in the following equation (15). A diagonal matrix arranged for the array argument i.

The integrated state quantity X _J is X _J = {x _J , P _J }.

(When the state quantity is a particle)
Next, the integration of state quantities in the case where particles are employed as the state quantities (internal state quantities X _I ) will be described.
In the case of adopting particles as the state quantities, the state integration unit 14 selects the state vectors “2N” included in the union (X _I ∪X _R ) of the internal state quantity X _I and the received state quantity X _R from N ′ state vectors are selected probabilistically, and the selected N ′ state vectors are set as an integrated state quantity X _J. The number N'state vectors included in the integrated state quantity X _J can be the same as the number N of the internal state quantity X _I.

Here, the number of state vectors included in the internal state quantity X _I and the reception state quantity X _R is N, and the state vectors are x _I ⁽ⁱ⁾ and x _R ⁽ⁱ⁾ (where i = 1, 2). , ..., N). ^{_{That, X I = {x I (}} 1), x I (2), ..., x I (N)}, X R = {x R (1), x R (2), ..., x R (N) }.
First, the state integration unit 14 sets the integrated state quantity X _J as an empty set (X _J = φ).
Subsequently, the state integration unit 14 generates a uniform random number r satisfying 0 <r ≦ 1, as shown in the following equation (16).

Then, the state integration unit 14 determines whether the uniform random number r is larger or smaller than a predetermined constant (threshold γ [0 <γ ≦ 1 real value]) as shown in the following equation (17): The state vector x _I ⁽ⁱ⁾ or x _R ⁽ⁱ⁾ is included in the integrated state quantity X _J.

Then, the state integration unit 14 repeats the processing of the above equations (16) and (17) for the number of times of state vectors (N). Thereby, it is possible to configure the integration state quantity X _J by stochastically selected state vector group from the state vectors included in the internal state quantity X _I and receiving the state quantity X _R. In this case, for example, when the threshold value gamma "0.5", so that the state vector with equal probability from internal state X _I and receiving the state quantity X _R is selected. Also, as the value of γ is large, as compared with the internal state quantity X _I, it emphasizes the reception state quantity X _R.
Returning to FIG. 4, the description of the configuration of the state estimation device 10 will be continued.

The state transmission unit 15 reads out the state quantity (internal state quantity X _I ) stored in the state quantity storage unit 11 and transmits it to another state estimation device connected to the network N.
The state transmitting unit 15 includes address information (such as an IP address) indicating a transmission destination and a transmission source when transmitting a message including the state quantity (internal state quantity X _I ) to another state estimation device. It is good.
In addition, when the state quantity (internal state quantity X _I ) cannot be read from the state quantity storage unit 11 because the state quantity is not yet stored, the state transmission unit 15 may not perform transmission. Then, a message indicating that transmission is impossible may be transmitted.
Also, status transmission unit 15 may be periodically send internal state quantity X _I, it may be transmitted based on a request from another state estimating device.

The estimated amount output means 16 reads the state quantity (internal state quantity X _I ) stored in the state quantity storage means 11 and outputs it as an estimated quantity E to the outside.
Incidentally, the estimated amount output unit 16 may be outputted to the internal state quantity X _I directly as the estimated amount E, and outputs only a part of the state quantity, and outputs by performing predetermined conversion of the state variable It is good also as doing.

  For example, when the state quantity is a state vector, the estimated quantity output means 16 can extract a partial component of the state vector and set the estimated quantity E as a new vector quantity or scalar quantity. Specifically, when the state vector includes a three-dimensional position and a three-dimensional velocity, only the three-dimensional position is set as the estimated amount E. Alternatively, the estimated amount output means 16 may output the result obtained by performing arithmetic processing, for example, linear transformation, on the state vector as the estimated amount E.

Further, for example, when the state quantity is a particle, the estimated quantity output means 16 can extract a part of the state vector constituting the particle and set it as the estimated quantity E. Alternatively, the estimated amount output means 16 may calculate an average vector for all the state vectors constituting the particle and output the average vector as the estimated amount E.
If the state quantity (internal state quantity X _I ) cannot be read from the state quantity storage unit 11 because the state quantity is not yet stored, the estimated quantity output unit 16 does not output the estimated quantity E. Alternatively, it may be output that it cannot be output.

  The configuration of the state estimation device 10 has been described above, but the present invention is not limited to this configuration. For example, the estimation amount E may be output by another state estimation device, and the state estimation device 10 may not output. In this case, the estimated amount output means 16 can be omitted from the configuration of the state estimation device 10. Further, it may be configured such that the observation amount Y from the outside is input by another state estimation device, and the state estimation device 10 does not input. In this case, the state update unit 12 can be omitted from the configuration of the state estimation device 10.

In addition, as shown in FIG. 5, the state update unit 12 is provided in the external observation device 20, the state update unit 12 is omitted from the state estimation device 10, and the input / output unit 17 that performs only data input / output is provided. It is good also as a structure of the estimation apparatus 10B. For example, the observation device 20 includes a position observation unit including a sensor or the like (not shown) and a state update unit 12. Then, the state update unit 12 of the observation device 20 calculates a state quantity (post-observation state quantity X _Y ) based on the position (observation quantity) of the observation target as a result of observation by the position observation means (not shown). , it is also possible to update the internal state quantity X _I stored in the state quantity storage unit 11.

In this case, the observation apparatus 20 may output an evaluation value for the post-observation state quantity _XY together with the post-observation state quantity _XY to the state estimation apparatus 10B. For example, the observation device 20 determines whether or not an observation target exists in the image input by the camera that is a sensor (whether it is captured), and if it is determined that it exists, the evaluation value is not high. If judged, a low value is set as the evaluation value. By referring to this evaluation value, the state estimation device 10B, in a state integration unit 14, when performing the integration of state quantities, to determine the degree of integration with the internal state quantity X _I and the receiving state quantity X _R It is good.

  Moreover, the state estimation apparatus 10 (10B) is realizable by making a general computer function as each above-mentioned means. This program (state estimation program) can be distributed via a communication line, or can be distributed on a recording medium such as a CD-ROM.

  As described above, the state estimation device 10 (10B) estimates the observation target state by integrating the state amount estimated by the other state estimation device and the state amount estimated by the state estimation device 10. Can do. Thus, the state estimation device 10 can perform state estimation in a distributed manner in cooperation with other state estimation devices. In addition, for example, by storing a state quantity common to the state estimation device 10 in another state estimation device, state estimation can be continued even when a failure occurs in the state estimation device 10. It becomes possible.

[First Embodiment: Operation of State Estimation Device (State Estimation Unit)]
Next, the operation of the state estimation device 10 will be described with reference to FIGS. FIG. 6 is a flowchart showing the overall operation of the state estimation apparatus. FIG. 7 is a flowchart showing the operation of the state update means in the state estimation device. FIG. 8 is a flowchart showing the operation of the state integration unit in the state estimation device.

(Overall operation)
First, the operation of the state estimation device 10 will be described with reference to FIG.
The state estimation device 10 determines whether or not the observation amount Y is input from the outside by the state update unit 12 (step S1). When the observed amount Y has been input (Yes in step S1), the state update unit 12, the observed quantity Y, and updates the internal state quantity X _I stored in the state quantity storage unit 11 (step S2) . The details of the internal state quantity update process in step S2 will be described later with reference to FIG. On the other hand, when the observation amount Y is not input (No in step S1), the state update unit 12 proceeds directly to the operation of step S3.

Further, the state estimation device 10 determines whether or not the state integration unit 14 has received a state quantity (reception state quantity X _R ) from the other state estimation apparatus connected to the network N in the state reception unit 13. (Step S3). Then, when receiving the reception state quantity X _R (Yes in step S3), and the state integration unit 14 integrates the internal state quantity X _I stored in the receiving state quantity X _R and the state quantity storing means 11 ( Step S4). The details of the internal state quantity integration process in step S4 will be described later with reference to FIG. On the other hand, when the reception state quantity _{X R} is not received (No in step S3), and the state integration unit 14 directly proceeds to the operation of step S5.

The state estimating device 10, the status transmission unit 15, part of the internal state quantity X _I stored in the state quantity storage unit 11 or reads all, a transmission state quantity X _S, is connected to the network N It transmits to another state estimation apparatus (step S5). The state transmitting unit 15, when the state quantity storing means 11 can not read the internal state quantity X _I, does not transmit, or send a message that it is impossible transmission.
The state estimating device 10, the estimator output means 16 reads the internal state quantity X _I stored in the state quantity storage unit 11, and output as the estimated amount E (step S6). Incidentally, the estimated amount output unit 16, and outputs only a part of the internal state quantity X _I, may be and outputs by performing predetermined conversion on the internal state quantity X _I.

The operation of the state estimation device 10 has been described above, but this operation is not limited to this procedure. Here, the operation has been described in time series for convenience of explanation, but the internal state quantity update process (step S2) and the internal state quantity integration process (step S4) may be operated in parallel, or the observed quantity If reception of the input and receiving the state quantity X _R of Y is irregularly input may be operated in accordance with the timing thereof.

(Internal state quantity update processing)
Next, referring to FIG. 7 (refer to FIG. 4 as appropriate), the operation of the state quantity update process in the state update unit 12 of the state estimation device 10 will be described. This operation corresponds to the operation in step S2 in FIG. Here, as an example of the state quantity update process, an operation when the state quantity is a particle and the observation quantity is a probability density function will be described.

First, the state update unit 12 determines whether or not the internal state quantity X _I (X _I = {x ⁽¹⁾ , x ⁽²⁾ ,..., X ^(N) }) is stored in the state quantity storage unit 11. Determination is made (step S20). When the internal state quantity _{X I} is stored (Yes at step S20), the state updating means 12, N pieces of the state vector ^x contained in the internal state quantity _{X I} ⁽ⁱ⁾ (provided that, i = 1, 2,..., N), the probability p (i) of occurrence of a state vector is calculated based on the observed quantity Y (x ⁽ⁱ⁾ ) (step S21: see the above equation (5)). Further, the state updating unit 12 calculates N cumulative probabilities a (i) obtained by accumulating the probabilities p (i) calculated in step S21 up to i-th for every i (step S22).

Here, the state update unit 12 initializes the state quantity after observation (post-observation state quantity X _Y ) to an empty set (X _Y = φ) and the variable m used for the number of loops to “1” (step S23). ).
Then, the state updating means 12 generates a real-valued uniform random number r satisfying 0 <r ≦ 1 (step S24), and a (i) ≧ r is set in the cumulative probability a (i) calculated in step S22. An array argument i that satisfies and satisfies the minimum a (i) is searched for j (step S25). Subsequently, the state update unit 12 adds the state vector x ^(j) corresponding to the array argument j searched in step S25 to the post-observation state quantity _XY (step S26).

Thereafter, the state updating unit 12 increments (+1) the variable m of the number of loops (step S27). If the value of the variable m is N or less (Yes in step S28), the state updating unit 12 returns to step S24 and continues the operation. On the other hand, when the value of the variable m exceeds N (No in step S28), the operation is terminated.
Incidentally, in step S20, if the state quantity storing means 11 internal state quantity _{X I} is not stored (No at step S20), the state updating means 12, N pieces of the state vector by a random number _{(r 1, r} 2, .., R _N ) to generate a post-observation state quantity _XY (step S29), and the operation ends.
By performing the above operation, the state updating means 12, the internal state quantity X _I stored in the state quantity storage unit 11, expressed by approximation of the probability density function has a high probability of particles more frequently Can do.

(Internal state quantity integration processing)
Next, referring to FIG. 8 (refer to FIG. 4 as appropriate), the operation of the internal state quantity integration process in the state integration unit 14 of the state estimation device 10 will be described. This operation corresponds to the operation in step S4 in FIG. Here, as an example of the internal state quantity integration process, an operation when the state quantity is a particle will be described.
Note that the number of state vectors included in the internal state quantity X _I and the reception state quantity X _R is N, and the state vectors are x _I ⁽ⁱ⁾ and x _R ⁽ⁱ⁾ (where i = 1, 2, ..., N). ^{_{That, X I = {x I (}} 1), x I (2), ..., x I (N)}, X R = {x R (1), x R (2), ..., x R (N) }.

First, the state integration unit 14 initializes a state quantity after integration (integrated state quantity X _Y ) to an empty set (X _Y = φ) and a variable m used for the number of loops to “1” (step S40).
Then, the state integration means 14 generates a real random number random number r satisfying 0 <r ≦ 1 (step S41), and the uniform random number r is a predetermined constant (a threshold value γ [0 <γ ≦ 1 real number). [Numerical value]) It is determined whether or not (step S42).

If the uniform random number r is equal to or smaller than the threshold γ (Yes in step S42), the state integration unit 14 includes the mth state vector x _R ^(m) included in the reception state amount X _R in the integrated state amount X _J. ⁾ Is added (step S43). On the other hand, when the uniform random number r is less than the threshold value γ (No in step S42), the state integration unit 14 includes the mth state vector x _I ^(m) included in the internal state quantity X _I in the integrated state quantity X _J. Is added (step S44).
Thereafter, the state integration unit 14 increments (+1) the variable m of the number of loops (step S45). If the value of the variable m is N or less (Yes in step S46), the state integration unit 14 returns to step S41 and continues the operation. On the other hand, when the value of the variable m exceeds N (No in step S46), the operation is terminated.

By performing the above operation, the state integration unit 14 uses the state vector group selected based on the predetermined threshold γ from the state vectors included in the internal state amount X _I and the reception state amount X _R as the integrated state amount. it is possible to configure the X _J. That is, it is possible to set which of the internal state quantity and the reception state quantity is to be emphasized by the threshold value γ.

[Second Embodiment: Configuration of State Estimation Device (State Estimation Unit)]
Next, another configuration of the state estimation device will be described with reference to FIG. FIG. 9 is a block diagram showing the internal configuration of the state estimation apparatus according to the second embodiment of the present invention.
The state estimation device 10C estimates a state amount indicating the state of the observation target in cooperation with another state estimation device connected to the network N based on the observation amount of the observation target input from the outside. . Note that the state estimation device 10C retains state quantities (internal state quantities) at a plurality of points in time, thereby reducing the effects of time differences due to processing differences among the plurality of state estimation apparatuses and differences in observation times. Absorb. Here, the state estimation device 10C includes a state amount storage unit 11C, a state update unit 12C, a state reception unit 13C, a state integration unit 14C, a state transmission unit 15C, an estimated amount output unit 16C, and a state prediction unit. 18.

The state quantity storage unit 11C stores a state quantity (internal state quantity) held in the state estimation apparatus 10C among the state quantities indicating the state of the observation target. The state quantity storage unit 11C is different from the state quantity storage unit 11 (FIG. 4) in that it stores internal state quantities at a plurality of points in time.
For example, in the state quantity storage unit 11C, an array having M elements corresponding to time “0” to time “M−1” is secured.

Here, each means (state update means 12C, state integration means 14C, state transmission means 15C, estimated amount output means 16C and state prediction means 18) that writes to or reads from the state quantity storage means 11C is 0 ≦ t <M. When referring to the internal state quantity X _I (t) at the time indicated by the integer value t, the t th element of the array having M elements may be referred to. Further, when the time is indicated by an integer value other than 0 ≦ t <M, it is assumed that no operation is performed at the time of writing to the state quantity storage unit 11C, and nothing is read at the time of reading (the empty set φ is read).
Further, the state quantity storage unit 11C may be configured to secure an array having M elements and store time information (time t) and internal state quantity X _I (t) in each element.

Here, with reference to FIG. 10, a method for accessing (reading / writing) the internal state quantity stored in the state quantity storage unit 11C will be described. FIG. 10 is a data structure diagram showing an example of the contents stored in the state quantity storage means. In FIG. 10, as an example, an array having 10 elements is secured in the state quantity storage unit 11C, and time information (time t) and internal state quantity X _I (t) are associated with addresses 0 to 9, respectively. This indicates the state memorized.
First, a method for writing the internal state quantity X _I (t) at time t will be described.
Here, the remainder when time t is divided by the number of elements T in the array is assumed to be address A (Ath address). That is, the address A is calculated by the following equation (18).

Here, the operator% indicates a remainder, and t% M indicates a remainder obtained by dividing t by M.
A pair of the time t and the internal state quantity X _I (t) is written to the address A calculated in this way.
For example, as shown in FIG. 10, when the internal state quantity X _I (1025) is written at time t = “1025”, the address A = “1025% 10” = “5” is obtained by the above equation (18). Therefore, a pair of the time information t = “1025” and the internal state quantity X _I (1025) at t = “1025” is written to the address A = “5”.

Next, a method of reading the internal state quantity X _I (t) at time t will be described. The address is calculated by the equation (18).
Here, it is assumed that time τ is written as time information at address A (Ath address). If τ = t, the state quantity written in the address A is the internal state quantity X _I (t) at the desired time t, so the internal state quantity X _I (t) may be read as it is. . If τ ≠ t, the state quantity written in the address A is not at the desired time t. For example, the read data is assumed to be an empty set φ.

For example, as shown in FIG. 10, when the internal state quantity X _I (1032) at time t = “1032” is read, the address A = “1032% 10” = “2” is obtained by the equation (18). Then, the time information (time τ) written in the address A = “2” is read. In this case, τ = “1032”. That is, since τ = t = “1032”, it is determined that the state quantity written at the address A = “2” is the internal state quantity X _I (1032) at the desired time t = “1032”. The internal state quantity X _I (1032) written at the address A = “2” may be read out.

Further, when reading the internal state quantity X _I (1033) at time t = “1033”, the address A = “1033% 10” = “3” is obtained by the equation (18). Then, the time information (time τ) written at the address A = “3” is read. In this case, τ = “1023”. That is, since τ ≠ t = “1033”, it is determined that the state quantity written at the address A = “3” is not the internal state quantity X _I (1033) at the desired time t = “1033”. Can do. Therefore, the read data is an empty set φ.
Thereby, it is possible to prevent a time lag in reading / writing of the internal state quantity X _I (t) at time t.
Returning to FIG. 9, the description of the configuration of the state estimation device 10C will be continued.

The state updating means 12C updates the internal state quantity X _I (t _Y ) stored in the state quantity storage means 11C with the observation quantity Y (t _Y ) at time t _Y input from the outside.
The state receiving unit 13C holds the other state estimation device connected to the network N, receives the reception state amount X _R (t _R ) at the time t _R transmitted from the other state estimation device, and integrates the states. This is output to the means 14C.

The state integration unit 14C receives the reception state quantity X _R (t _R ) at the time t _R received by the state reception unit 13C and the internal state quantity X _I (t _{R at the} time t _R stored in the state quantity storage unit 11C. ) To generate an integrated state quantity X _J (t _R ) at time t _R.
The state transmission unit 15C reads the internal state quantity X _I (t _S ) at a certain time t _S stored in the state quantity storage unit 11C, and transmits it to another state estimation device connected to the network N It is.

The estimated amount output means 16C reads out the internal state quantity X _I (t _E ) at a certain time t _E stored in the state quantity storage means 11C and outputs it as an estimated quantity E (t _E ) to the outside. is there.
The state update unit 12C, the state reception unit 13C, the state integration unit 14C, the state transmission unit 15C, and the estimated amount output unit 16C are based on time (time t _Y , time t _R , time t _S, and time t _E ). The state updating unit 12 and the state receiving unit described with reference to FIG. 4 except that it operates, updates the internal state quantity X _I (t), and outputs a control signal synchronized with the time to the state prediction unit 18. 13, the state integration unit 14, the state transmission unit 15, and the estimated amount output unit 16 have the same functions, and thus description of each function is omitted.

The state prediction means 18 predicts a state quantity (predicted state quantity) X _P (t _P ) at a certain time t _P based on the timing notified by the control signal.
Here, the state predicting means 18 includes an internal state quantity _X I _{(t O)} at a certain time _{t O,} read from the internal state quantity _X I _{(t P)} and the state quantity storing means 11C at a certain time _{t P,} predicted By performing the process, the predicted state quantity X _P (t _P ) at time t _P is predicted and written to the state quantity storage unit 11C as the internal state quantity X _I (t _P ).
This state prediction means 18 first uses the predetermined transformation rule F (t _O , t _P ; X) representing the state transition from time t _O to time t _{P according} to the following equation (19). A state quantity after the transition (post-transition state quantity X _Q (t _P )) is calculated.

Further, the state predicting means 18 uses the predicted state quantity X _P (at time t _P ) based on the calculated post-transition state quantity X _Q (t _P ) and the internal state quantity X _I (t _O ) at time t _O. t _P ) is calculated. The method for calculating the predicted state quantity X _P (t _P ) from the post-transition state quantity X _Q (t _P ) and the internal state quantity X _I (t _O ) is the same as the state integration unit 14 described with reference to FIG. Can be used. In this case, the internal state quantity X _I , the received state quantity X _R, and the integrated state quantity X _J in the state integration unit 14 are respectively converted into the internal state quantity X _I (t _O ), the post-transition state quantity X _Q (t _P ), and the prediction state quantity _{X P (t} P) and read as well be omitted.

The state prediction unit 18 may use the post-transition state quantity X _Q (t _P ) as it is as the predicted state quantity X _P (t _P ). In this case, the state prediction unit 18 uses the state quantity storage unit. It is not necessary to read the internal state quantity X _I (t _O ) at time t _O from 11C.

Further, for example, when the time t _R is notified as a control signal from the state integration unit 14C, the state prediction unit 18 operates as time t _O = “t _R ” and time t _P = “t _R +1”. Further, the state predicting means 18 determines the time t _O and the time t _P as (t _O , t _P ) = (t _R , t _R ± 1), (t _R , t _R ± 2) _,. , T _R ± T _R ), it may be applied over time T _R before and after time t _R.
Alternatively, the state predicting unit 18 may be configured such as immediately after the operation of the state updating unit 12C, before or after the operation of the state receiving unit 13C, immediately before the operation of the state transmitting unit 15C, immediately before the operation of the estimated amount output unit 16C, Similarly to the above, the operation may be performed based on the control signal.
Further, the state prediction means 18 may set the time t _O and the time t _P based on a timer (not shown) or the like without depending on the control signal.
Hereinafter, the calculation of the state quantity after the transition in the equation (19) will be further described using mathematical expressions.

(When the state quantity is a vector)
First, calculation of a post-transition state quantity when one state vector is employed as the state quantity (internal state quantity X _I ) will be described.
In this case, the state prediction means 18 can use the state transition matrix Φ per unit time as the conversion rule F of the equation (19).
That is, when the state vector representing the internal state quantity X _I (t _O ) is x _I (t _O ), the state vector x _Q representing the post-transition state quantity X _Q (t _P ) that is the state quantity after the state transition. (T _P ) can be calculated by the following equation (20) using the state transition matrix Φ.

At this time, the post-transition state quantity X _Q (t _P ) becomes X _Q (t _P ) = {x _Q (t _P )}.

(When the state quantity is a vector and covariance matrix)
Next, the calculation of the post-transition state quantity when a state vector and a state covariance matrix are employed as the state quantity (internal state quantity X _I ) will be described.
In this case, the state prediction means 18 can use the state transition matrix Φ per unit time and the process noise covariance matrix Q per unit time as the conversion rule F of the equation (19).
That is, when the state vector representing the internal state quantity X _I (t _O ) is x _I (t _O ) and the state covariance matrix is P _I (t _O ), the post-transition state quantity X _Q (t _P ) is A state vector x _Q (t _P ) to be expressed and its state covariance matrix P _Q (t _P ) are expressed by the following equation using a state transition matrix Φ per unit time and a covariance matrix Q of process noise per unit time. It can be calculated by (21). The superscript T represents transposition.

At this time, the post-transition state quantity X _Q (t _P ) becomes X _Q (t _P ) = {x _Q (t _P ), P _Q (t _P )}.

(When the state quantity is a particle)
Next, the post-transition state quantity calculation in the case where particles are used as the state quantity (internal state quantity X _I ) will be described.
In this case, the state prediction means 18 can use the state transition matrix Φ per unit time and the process noise covariance matrix Q per unit time as the conversion rule F of the equation (19).
That is, the number of state vectors included in the internal state quantity X _I (t _O ) is N, and each state vector is x _I ⁽ⁱ⁾ (t _O ) (where i = 1, 2,..., N). Then, the state vector x _Q ⁽ⁱ⁾ (t _P ) included in the post-transition state quantity X _Q (t _P ) represents the state transition matrix Φ per unit time and the process noise covariance matrix Q per unit time. And can be calculated by the following equation (22). Note that n is noise (noise), E [] is an expected value, and the superscript T represents transposition.

At this time, the post-transition state quantity X _Q (t _P ) is expressed as X _Q (t _P ) = {x _Q ⁽¹⁾ (t _P ), x _Q ⁽²⁾ (t _P ),..., X _Q ^(N) (T _P )}.
The configuration of the state estimation device 10C has been described above, but the present invention is not limited to this configuration. For example, as shown in FIG. 11, the state update unit 12C is provided in the external observation device 20C, the state update unit 12C is omitted from the state estimation device 10C, and the input / output unit 17C that performs only data input / output is provided. It is good also as a structure of 10D of estimation apparatuses. From this case, the position observation means such as a sensor position of the observed object is the result of observation (not shown) (observation amount) in association with the time t _Y, the observation apparatus 20 state updating unit 12C, state estimation Input to device 10D.

  The state estimation device 10C (10D) can be realized by causing a general computer to function as each of the above-described means. This program (state estimation program) can be distributed via a communication line, or can be distributed on a recording medium such as a CD-ROM.

As described above, the state estimation device 10C (10D) estimates the state of the observation target by integrating the state amount estimated by the other state estimation device and the state amount estimated by the state estimation device 10C. Can do. Accordingly, the state estimation device 10C can perform state estimation in a distributed manner in cooperation with other state estimation devices. Further, for example, by storing a state quantity common to the state estimation device 10C in another state estimation device, state estimation can be continued even when a failure occurs in the state estimation device 10C. It becomes possible. Furthermore, since the state estimation device 10C performs prediction with the passage of time, the accuracy of state estimation can be increased.
Note that the operation of the state estimation device 10C is the state estimation device 10 (FIG. 4) described with reference to FIGS. 6 to 8 except that state amounts (internal state amounts) at a plurality of time points are held and state amounts are predicted. Since the basic operation is the same, the description is omitted.

It is a whole block diagram which shows the whole structure of the integrated state estimation system which concerns on this invention. It is a whole block diagram which shows the structure of the car navigation system which is a specific example of an integrated state estimation system. It is a block diagram which shows the structure of the integrated state estimation apparatus which concerns on this invention. It is a block block diagram which shows the internal structure of the state estimation apparatus which concerns on 1st Embodiment of this invention. It is a block block diagram which shows the modification of the state estimation apparatus which concerns on 1st Embodiment of this invention. It is a flowchart which shows the whole operation | movement of the state estimation apparatus which concerns on this invention. It is a flowchart which shows operation | movement of the state update means in the state estimation apparatus which concerns on this invention. It is a flowchart which shows operation | movement of the state integration means in the state estimation apparatus which concerns on this invention. It is a block block diagram which shows the internal structure of the state estimation apparatus which concerns on 2nd Embodiment of this invention. It is a data structure figure which shows an example of the memory content of a state quantity memory | storage means. It is a block block diagram which shows the modification of the state estimation apparatus which concerns on 2nd Embodiment of this invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS S Integrated state estimation system 1 Integrated state estimation apparatus 10 State estimation apparatus 11 State quantity storage means 12 State update means 13 State reception means 14 State integration means 15 State transmission means 16 Estimated quantity output means 17 Input / output means 18 State prediction means

Claims (6)

Based on the observation of the observation target, by a plurality of connected state estimation device in the communication path, met the state estimation apparatus used in the integrated state estimation system for estimating by integrating the state quantity indicating a state of the observation target And
State quantity storage means for storing the state quantity of the observation target as an internal state quantity;
State update means for sequentially updating the internal state quantity stored in the state quantity storage means based on the observed quantity input from the outside;
A status receiving means for receiving the internal state quantity that is distributed and stored in the other state estimating device transmitted from another state estimation device connected to the communication channel as an external state variable,
A state integration unit that integrates the external state quantity received by the state reception unit and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity;
State transmitting means for transmitting the internal state quantity stored in the state quantity storage means to another state estimation apparatus connected to the communication path as an external state quantity for the other state estimation apparatus ;
A state estimation device comprising:   The state estimation apparatus according to claim 1, wherein the state quantity storage unit stores a plurality of the internal state quantities in association with time.   The state estimation device according to claim 2, further comprising a state prediction unit that predicts a state amount at a certain time based on a plurality of internal state amounts stored in the state amount storage unit. Based on the observation amount of the observation target, an integrated state estimation system that estimates the state amount indicating the state of the observation target by using a plurality of state estimation devices connected to the communication path,
The state estimation device includes:
State quantity storage means for storing the state quantity of the observation target as an internal state quantity;
State receiving means for receiving the internal state quantity distributed and stored in the other state estimation apparatus transmitted from the other state estimation apparatus connected to the communication path as an external state quantity;
A state integration unit that integrates the external state quantity received by the state reception unit and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity;
A state transmission unit that transmits the internal state quantity stored in the state quantity storage unit to the other state estimation apparatus connected to the communication path as an external state quantity with respect to the other state estimation unit ;
At least one of the plurality of state estimation devices includes a state update unit that sequentially updates the internal state quantity stored in the state quantity storage unit of the state estimation apparatus based on the observation amount input from the outside,
Integrated state estimation, wherein at least one of the plurality of state estimation devices includes an estimated amount output unit that outputs an internal state amount stored in a state amount storage unit of the state estimation device as an estimated amount system. An integrated state estimation device that integrates and estimates a state quantity indicating the state of the observation target based on the observation quantity of the observation target,
A plurality of state estimation means for estimating the state quantity , connected to the inside of the integrated state estimation device via a communication path ,
The state estimating means includes
State quantity storage means for storing the state quantity of the observation target as an internal state quantity;
The internal state quantity is distributed and stored in the other state estimating means to be transmitted from another state estimation means connected to said communication path, and the state receiving means for receiving an external state variable,
A state integration unit that integrates the external state quantity received by the state reception unit and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity;
The other state estimation means connected to the communication path comprises a state transmission means for transmitting the internal state quantity stored in the state quantity storage means as an external state quantity for the other state estimation means ,
At least one of the plurality of state estimation means includes a state update means for sequentially updating the internal state quantity stored in the state quantity storage means of the state estimation means based on the observation amount input from the outside,
Integrated state estimation, wherein at least one of the plurality of state estimation means includes an estimation amount output means for outputting an internal state quantity stored in a state quantity storage means of the state estimation means as an estimation quantity apparatus. A computer in the state estimation device used in an integrated state estimation system in which a plurality of state estimation devices connected to a communication path are used to integrate and estimate state amounts indicating the state of the observation target based on the observation amount of the observation target The
In a state quantity storage means for storing the state quantity as an internal state quantity based on the observed quantity input from the outside, a state update means for sequentially updating the internal state quantity,
A state receiving means for receiving, as an external state quantity, an internal state quantity distributed and stored in the other state estimation apparatus transmitted from the other state estimation apparatus connected to the communication path;
A state integration unit that integrates the external state quantity received by the state reception unit and the internal state quantity stored in the state quantity storage unit, and updates the internal state quantity;
State transmitting means for transmitting the internal state quantity stored in the state quantity storage means to the other state estimation apparatus connected to the communication path as an external state quantity for the other state estimation apparatus ;
Estimated amount output means for outputting the internal state quantity stored in the state quantity storage means as an estimated quantity;
A state estimation program characterized by functioning as

Images