STATE ESTIMATION DEVICE

0Associated
Cases 
0Associated
Defendants 
0Accused
Products 
0Forward
Citations 
0
Petitions 
1
Assignment
First Claim
1. A state estimation device to receive, as inputs, an output from a target system and a nonlinear model for estimating a state of the target system by using an extended Kalman filter, the nonlinear model being a modeled target system, wherein the nonlinear model includes a nonlinear continuoustime state equation, the state estimation device comprising:
 stateanderror estimation circuitry to, on a basis of an observation equation of the nonlinear model, obtain an estimated state value and an estimated value of an error covariance matrix of the estimated state value, the estimated state value estimating a state of the target system at a certain time point;
stateequation discretization circuitry to obtain a discretetime state equation by discretizing the nonlinear continuoustime state equation on the basis of the estimated state value at the time point, by using a quadratic or higherorder integration technique;
stateequation linearization circuitry to obtain an approximate value of a Jacobian at the time point by using the discretetime state equation at the time point and a difference method; and
stateanderror prediction circuitry to predict, from the approximate value of the Jacobian, the error covariance matrix provided after a lapse of an infinitesimal time period since the time point, and to predict, from the discretetime state equation, the state provided after a lapse of the infinitesimal time period since the time point.
1 Assignment
0 Petitions
Accused Products
Abstract
A state estimation device receives, as inputs, an output from a target system and a nonlinear model that is a modeled target system, and estimates a state of the target system by using an extended Kalman filter. The nonlinear model includes a nonlinear continuoustime state equation. The device includes a stateanderror estimation unit that, on the basis of an observation equation of the nonlinear model, obtains an estimated state value and an estimated value of an error covariance matrix of the estimated state value, the estimated state value being indicative of an estimated state of the target system at a certain time point. The state estimation device also includes stateequation discretization circuitry that obtains a discretetime state equation by discretizing the nonlinear continuoustime state equation on the basis of the estimated state value at this time point, by using a quadratic or higherorder integration technique.
0 Citations
No References
No References
6 Claims
 1. A state estimation device to receive, as inputs, an output from a target system and a nonlinear model for estimating a state of the target system by using an extended Kalman filter, the nonlinear model being a modeled target system, wherein the nonlinear model includes a nonlinear continuoustime state equation, the state estimation device comprising:
stateanderror estimation circuitry to, on a basis of an observation equation of the nonlinear model, obtain an estimated state value and an estimated value of an error covariance matrix of the estimated state value, the estimated state value estimating a state of the target system at a certain time point; stateequation discretization circuitry to obtain a discretetime state equation by discretizing the nonlinear continuoustime state equation on the basis of the estimated state value at the time point, by using a quadratic or higherorder integration technique; stateequation linearization circuitry to obtain an approximate value of a Jacobian at the time point by using the discretetime state equation at the time point and a difference method; and stateanderror prediction circuitry to predict, from the approximate value of the Jacobian, the error covariance matrix provided after a lapse of an infinitesimal time period since the time point, and to predict, from the discretetime state equation, the state provided after a lapse of the infinitesimal time period since the time point.  View Dependent Claims (2, 3, 4, 5, 6)
1 Specification
The present invention relates to a state estimation device that estimates a state or parameter of a target system.
As one example of a conventional state estimation device for a nonlinear system, NonPatent Literature 1 discloses a state estimation device that uses an extended Kalman filter (EKF). Extended Kalman filter is used for estimation of an internal state of a system on the basis of both an output from a target system and a nonlinear discretetime system that is the modeled target system. In addition, when that system has an unknown parameter, the system is converted into an expanded system containing the unknown parameter as a state, and an extended Kalman filter is then applied thereto to estimate the state and the parameter. For such extended Kalman filter to be applied to a nonlinear system, the system is linearized using firstorder Taylor approximation, which requires the Jacobian of a state equation or observation equation. Moreover, as the target system is a nonlinear discretetime system, the state equation should be discretized beforehand if the state equation is given in continuous time.
For example, Patent Literature 1 discloses a technology that, where the target system is represented as a nonlinear continuoustime system, discretizes the target system beforehand by using an Euler method, and then applies an extended Kalman filter to thereby estimate the state and an unknown parameter.
In other words, where the target system is the nonlinear discretetime system and the state equation is given in continuous time, the use of an extended Kalman filter requires the discretization. The process for estimating the state by using an extended Kalman filter needs calculation of the Jacobian of the state equation.
Patent Literature 1: Japanese Patent Application Laidopen No. 2005248946
NonPatent Literature 1: Takashi YAHAGI, “Kalman Filter and Adaptive Signal Processing”, Corona publishing, December 2005, pp.4851
Unfortunately, the conventional technology described above has difficulties in calculating a Jacobian in a case in which a quadratic or higherorder discretization technique is used for discretization of a continuoustime state equation. To address this issue, the discretization needs to be performed using an Euler method that can analytically derive a Jacobian. However, such an Euler method, which provides accuracy of firstorder approximation, poses a problem of only conducting the estimation with low accuracy due to a large discretization error in a case in which the target is formulated by a continuoustime state equation.
The present invention has been made in view of the foregoing, and it is an object of the present invention to provide a state estimation device capable of estimating a state with high accuracy by using an extended Kalman filter in a case in which the state equation of a target system is a nonlinear continuoustime function.
To solve the above problem and achieve the object, the present invention provides a state estimation device to receive, as inputs, an output from a target system and a nonlinear model for estimating a state of the target system by using an extended Kalman filter, the nonlinear model being a modeled target system, wherein the nonlinear model includes a nonlinear continuoustime state equation, the state estimation device comprising: a stateanderror estimation unit to, on a basis of an observation equation of the nonlinear model, obtain an estimated state value and an estimated value of an error covariance matrix of the estimated state value, the estimated state value estimating a state of the target system at a certain time point; a stateequation discretization unit to obtain a discretetime state equation by discretizing the nonlinear continuoustime state equation on the basis of the estimated state value at the time point, by using a quadratic or higherorder integration technique; a stateequation linearization unit to obtain an approximate value of a Jacobian at the time point by using the discretetime state equation at the time point and a difference method; and a stateanderror prediction unit to predict, from the approximate value of the Jacobian, the error covariance matrix provided after a lapse of an infinitesimal time period since the time point, and to predict, from the discretetime state equation, the state provided after a lapse of the infinitesimal time period since the time point.
The state estimation device according to the present invention is capable of estimating the state with the high accuracy by using an extended Kalman filter in a case in which the state equation of the target system is the nonlinear continuoustime function.
A state estimation device according to embodiments of the present invention will be described in detail below with reference to the drawings. Note that these embodiments are not intended to limit the scope of this invention.
Note that, in the description below, the parameters are vector values except for the time t and the scalar value ε. However, no particular distinction is made therebetween in the specification, but a vector value is indicated in bold in a mathematical formula. In addition, temporal differentiation is expressed as (d/dt)x in the specification, but is indicated by a dot above the applicable letter in a mathematical formula. Furthermore, a predicted value and an estimated value are indicated by a notation of “(hat)” in the specification, but by a hat symbol above the applicable letter in a mathematical formula. A state value or an equation of an expanded system model is indicated by a notation of “(tilde)”, but by a tilde above the applicable letter in a mathematical formula.
The stateanderror estimation unit 1 provides an estimated value x_{kk }(hat) of a current state and an estimated value Σ_{kk }(hat) of a current error covariance matrix, on the basis of: a coefficient C of the linear observation equation; the system output y(t_{k}) at a current time t_{k}; the observation noise covariance matrix R; and a predicted current state value x_{kk−1 }(hat) and a predicted current error covariance matrix value Σ_{kk−1 }(hat) both of which were calculated at a previous time point.
The stateequation discretization unit 2 outputs a discretetime state equation f_{d}(t_{k}, x_{kk }(hat)), which has been discretized using a fourthorder RungeKutta method, on the basis of the nonlinear continuoustime state equation f(t, x) and the current estimated state value x_{kk }(hat).
The stateequation linearization unit 3 calculates, from the discretetime state equation f_{d}(t_{k}, x_{kk }(hat)), an approximate value of the Jacobian αf_{d}(t, x)/αx^{T}t=t_{k}, x=x_{kk }(hat) of the state equation by using a difference method. Note that the suffix T indicates a transposed matrix.
The state prediction unit 5 outputs the discretetime state equation f_{d}(t_{k}, x_{kk }(hat)) as the predicted state value x_{k+1k }(hat) for a next time point. From αf_{d}(t, x)/αx^{T}t=t_{k}, x=x_{kk }(hat) (i.e., the Jacobian F_{k }of the state equation), the estimated current error covariance matrix value Σ_{kk }(hat), and the system noise covariance matrix Q, the error prediction unit 6 calculates a predicted value of the error covariance matrix Σ_{k+1k }(hat) for the next time point.
[Formula 1]
State equation: {dot over (x)}(t)=f(t, x(t))
Observation equation: y(t)=Cx
where State vector: x∈^{n×1 }
System output: y∈^{q×1 }
Observation matrix: C∈^{q×n} (1)
In addition, the state value and the error covariance matrix value at the time t_{k }estimated on the basis of the output value y(t_{k}) at the time t_{k }are represented as x_{kk }(hat) and Σ_{kk }(hat), respectively. The state value and the error covariance matrix value at a next time point t_{k+1 }predicted on the basis of the output value y(t_{k}) at the time t_{k }are represented as x_{k+1k }(hat) and Σ_{k+1k }(hat), respectively.
First, the stateanderror estimation unit 1 performs initialization (S1). Specifically, the stateanderror estimation unit 1 provides initial values that are a predicted state value x_{10 }(hat) and a predicted error covariance matrix value Σ_{10 }(hat) at a state estimation start time t_{1}.
Next, the stateanderror estimation unit 1 calculates a Kalman gain K_{k }(S2). Specifically, the stateanderror estimation unit 1 calculates a Kalman gain K_{k }by using below Formula (2) at the time t_{k}, on the basis of: the predicted current error covariance matrix value Σ_{kk−1 }(hat) calculated at a previous time point t_{k−1}; the coefficient C of the linear observation equation; and the observation noise covariance matrix R. Note here that Σ_{kk−1 }(hat) is an n×n matrix, R is a q×q matrix, and K_{k }is an n×q matrix. Note that the suffix “−1” indicates an inverse matrix.
[Formula 2]
K_{k}={circumflex over (Σ)}_{kk−1}C^{T}(C{circumflex over (Σ)}_{kk−1}C^{T}+R)^{−1} (2)
Next, the stateanderror estimation unit 1 calculates the estimated error covariance matrix value Σ_{kk }(hat) (S3). Specifically, the stateanderror estimation unit 1 calculates the current estimated error covariance matrix value Σ_{kk }(hat) by using below Formula (3). Formula (3) represents a difference between the predicted error covariance matrix value Σ_{kk−1 }(hat) and a matrix that is a product obtained by multiplying this predicted value Σ_{kk−1 }(hat) by both the Kalman gain K_{k }and the coefficient C of the linear observation equation. Note that Σ_{kk }(hat) is an n×n matrix.
[Formula 3]
{circumflex over (Σ)}_{kk}={circumflex over (Σ)}_{kk−1}−K_{k}C{circumflex over (Σ)}_{kk−1} (3)
Then, the stateanderror estimation unit 1 calculates the estimated state value x_{kk }(hat) (S4). Specifically, the stateanderror estimation unit 1 calculates the current estimated state value x_{kk }(hat) by using below Formula (4). Formula (4) represents a sum of the predicted state value x_{kk−1 }(hat) and a value that is obtained by multiplying a difference by the Kalman gain K_{k}, the difference being obtained by subtracting, from the current system output y(t_{k}), the system output y_{kk−1 }(hat) predicted using the linear observation equation. Note that x_{kk }(hat) is an n×1 matrix.
[Formula 4]
{circumflex over (x)}_{kk}={circumflex over (x)}_{kk−1}+K_{k}(y(t_{k})−{circumflex over (y)}_{kk−1})={circumflex over (x)}_{kk−1}+K_{k}(y(t_{k})=C{circumflex over (x)}_{kk−1}) (4)
In the stateequation discretization unit 2, the state x(t_{k+1}) for the next time point is expressed by below Formula (5) representing a sum of the current state x(t_{k}) and an integral value of the state equation from the time t_{k }to the time t_{k+1}.
[Formula 5]
x(t_{k+1})=x(t_{k})+∫_{t}_{k}^{t}^{k+1}f(t, x(t))dt (5)
Therefore, the state equation discretization unit 2 calculates an approximate value of the nonlinear discretetime state equation x(t_{k+1})=f_{d}(t_{k}, x(t_{k})) by using a quadratic or higherorder integration technique. The quadratic or higherorder numerical integration technique is, for example, a calculation technique using a fourthorder RungeKutta method as discussed below.
First, α=f(t_{k}, x(t_{k})) is calculated from the current state x(t_{k}).
Next, β=x(t_{k})+(ΔT/2)α is calculated using the result of calculation of this α and a time interval ΔT=t_{k+1}−t_{k}.
Then, γ=f(t_{k})+(ΔT/2), β) is calculated using the result of calculation of this β.
Then, λ=x(t_{k})+(ΔT/2)γ is calculated using the result of calculation of this γ.
Then, ζ=f(t_{k}+(ΔT/2), λ) is calculated using the result of calculation of this λ.
Then, η=x(t_{k})+ΔTζ is calculated using the result of calculation of this ζ.
Then, ζ=f(t_{k+1}, η) is calculated using the result of calculation of this η.
Then, the discretetime state equation f_{d}(t_{k}, x(t_{k}))=x(t_{k})+(ΔT/6) (α+2(γ+ζ)+ζ is calculated.
The stateequation discretization unit 2 then substitutes x_{kk }(hat) for x(t_{k}) (S5), and performs a calculation using a quadratic or higherorder numerical integration technique (S6). Specifically, the stateequation equation discretization unit 2 substitutes the current estimated state value x_{kk }(hat) for the current state x(t_{k}), and calculates the discretetime state equation f_{d}(t_{k}, x_{kk }(hat)) by using the above RungeKutta method.
The state prediction unit 5 calculates the predicted state value x_{k+1k }(hat) (S7). Specifically, as shown by below Formula (6), the state prediction unit 5 outputs the value of the discretetime state equation f_{d}(t_{k}, X_{kk }(hat)) as the predicted state value x_{k+1k }(hat) for the next time point t_{k+1}.
[Formula 6]
{circumflex over (x)}_{k+1k}=f_{d}(t_{k}, {circumflex over (x)}_{kk}) (6)
The stateequation linearization unit 3 calculates the approximate Jacobian value F_{k }on the basis of the discretetime state equation f_{d }and a predetermined infinitesimal scalar value ε by using a difference method. Note that F_{k }is an n×n matrix. The stateequation linearization unit 3 first calculates a value x_{i }(hat) (S8). Specifically, as shown by below Formula (7), the stateequation linearization unit 3 multiplies only the ith component of the state by (1+ε) with respect to the current estimated state value x_{kk }(hat), thereby providing the resulting value x_{i }(hat).
[Formula 7]
{circumflex over (x)}
_{i}
={circumflex over (x)}
_{kk}
+εP
_{i}
{circumflex over (x)}
_{kk }
where P_{i}∈^{n×n}:
n×n matrix having (i,i) elements being 1, and the other elements being 0 (7)
The stateequation linearization unit 3 then performs a calculation using a quadratic or higherorder numerical integration (S9), and calculates the ith column of the Jacobian approximate value by using a difference method (S10). Specifically, the stateequation linearization unit 3 calculates the value of the discretetime state equation on the basis of x_{i }(hat) by using the above described numerical integration technique. Then, as shown by below Formula (8), the stateequation linearization unit 3 approximates the ith column (i=1, 2, . . . , n) of the Jacobian by providing a value obtained by dividing, by a value εx_{kk}(i) (hat), the difference from the discretetime state equation f_{d}(t_{k}, x_{kk }(hat)) for the current estimated state value x_{kk }(hat), the value εx_{kk}(i) (hat) being obtained by multiplying the ith component of the current estimated state value by ε. Note that this operation is consecutively repeated from i=1 to i=n.
Then, the error prediction unit 6 calculates the predicted error covariance matrix value Σ_{k+1k }(hat) (S11). Specifically, the error prediction unit 6 calculates the error covariance matrix Σ_{k+1k }(hat) for the next time point on the basis of: the approximate Jacobian value F_{k}; the current estimated error covariance matrix value Σ_{kk }(hat); and the system noise covariance matrix Q, by using Formula (9) below. Note that Q is an n×n matrix.
[Formula 9]
{circumflex over (Σ)}_{k+1k}=F_{k}{circumflex over (Σ)}_{kk}F_{k}^{T}+Q (9)
The state estimation device 10 then advances the time from t_{k }to t_{k+1}, and thus repeats the above steps S2 to S11 to output the estimated state value at each time point. The calculation operations at the above steps are performed in the processor 21. The calculated values obtained at the corresponding steps, such as the Kalman gain K_{k}, the estimated values x_{kk }(hat) and Σ_{kk }(hat), the predicted values x_{kk−1 }(hat) and Σ_{kk−1 }(hat) , the discretetime state equation f_{d}, and the approximate Jacobian value F_{k }of the state equation are stored in the memory 22 and are read from the memory 22 and used at the corresponding steps.
As described above, even when the state equation of a target system is given by a nonlinear continuoustime function, the state estimation device according to this embodiment enables a highorder, i.e., quadratic or higherorder, integration technique to be applied to discretization of the state equation because an approximate Jacobian value is calculated on the basis of the estimated state value at each time point. This can reduce the discretization error in the process of state estimation that uses an extended Kalman filter, and can thus improve state estimation accuracy.
One known state estimation device for a nonlinear system other than one that uses an extended Kalman filter is a state estimation device that uses an unscented Kalman filter. Use of an extended Kalman filter enables the state estimation device according to the present invention to reduce the number of design parameters requiring adjustment, and also reduce the amount of calculation as compared to when an unscented Kalman filter is used. Note that the state estimation device of this embodiment uses a fourthorder RungeKutta method for the approximation relating to Formula (5) described above, but the present invention is not limited thereto. An integration technique having accuracy of quadratic or higherorder approximation, such as Heun'"'"'s method, can be satisfactorily used.
The infinitesimal scalar value ε which the stateequation linearization unit 3 uses in a difference method is preferably as low a value as possible depending on the target system, and may be externally changeable via the input device 24 depending on an estimation result displayed on the display device 25. Externally changing the infinitesimal scalar value ε to a minimum value without causing an anomaly such as divergence of an estimation result can reduce the error caused by approximation of the Jacobian. In addition, instead of using an infinitesimal scalar value ε, a matrix of vector n×1 may be used as the parameter ε, such that the calculation is performed using an infinitesimal value ε(i) that is partially or wholly different across the states, as given by Formula (10) below.
Moreover, if a relationship of x_{kk}(i) (hat)≈0 applies, Formula (11) below may be used instead, which uses an infinitesimal value ε′ temporarily or for the entire period.
where E_{i}∈^{n×1 }
ndimensional vector having an ith component being 1, and the other elements being 0 (11)
The observationequation linearization unit 7 analytically calculates a Jacobian H from the observation equation h(t, x). From the Jacobian H_{k }of the observation equation, the system output y(t_{k}), the predicted current state value x_{kk−1 }(hat), and Σ_{kk−1 }(hat) , the stateanderror estimation unit 1 provides the current estimated value x_{kk }(hat) of the state and the current estimated value Σ_{kk }(hat) of the error covariance matrix. The stateequation discretization unit 2, the stateequation linearization unit 3, and the stateanderror prediction unit 4 operate similarly to the first embodiment.
[Formula 12]
State equation: {dot over (x)}(t)=f(t, x(t))
Observation equation: y(t)=h(t, x(t))
where State vector: x∈^{n×1 }
System output: y∈^{q×1} (12)
The operation of the state estimation device 10A illustrated in
Next, the observationequation linearization unit 7 substitutes, at each time point, the predicted state value x_{kk−1 }(hat) for the current time t_{k }in the Jacobian of the observation equation as shown by below Formula (14), and thus obtains the current Jacobian H_{k }of the observation equation (S22). Note that x_{kk−1 }(hat) is an n×1 matrix, and H_{k }is a q×n matrix.
From Formula (14), the observation equation can be approximated by a product obtained by multiplying the state by the Jacobian in a vicinity of the predicted current state value k_{kk−1 }(hat), as shown by below Formula (15).
[Formula 15]
y(t_{k})≈H_{k}x(t_{k}) (15)
Thus, state estimation is performed by replacing the coefficient C of the observation equation in the first embodiment with the Jacobian H_{k}, and by using the original nonlinear function h(t_{k}, x_{kk−1 }(hat)) for the purpose of calculation of the predicted output value y_{kk−1 }(hat). The calculation operations at the above steps are performed in the processor 21. The calculated values obtained at the corresponding steps, such as the Kalman gain K_{k}, the estimated values x_{kk }(hat) and Σ_{kk }(hat), the predicted values x_{kk−1 }(hat) and Σ_{kk−1 }(hat), the discretetime state equation f_{d}, and the approximate Jacobian value F_{k }of the state equation are stored in the memory 22, and are read from the memory 22 and used at the corresponding steps.
As described above, even when the state equation of a target system is a nonlinear continuoustime function, and the observation equation is given by a nonlinear function, the state estimation device according to this embodiment can reduce the amount of calculation by analytically calculating the Jacobian of the observation equation that needs no discretization. Further, the state estimation device according to this embodiment enables a highorder, i.e., quadratic or higherorder, integration technique to be applied to discretization of the state equation because an approximate value of the Jacobian of the state equation is calculated on the basis of the estimated state value at each time point. As a result, it becomes possible to reduce the discretization error in the process of state estimation that uses an extended Kalman filter, and thus improve state estimation accuracy.
The state estimation device according to this embodiment receives, as inputs, a targetsystem nonlinear continuoustime state equation (d/dt)x=f(t, x, u), an observation equation y=h(t, x, u), and system input and output u(t_{k}) and y(t_{k}) at each time point. The state estimation device according to this embodiment uses design parameters: the system noise covariance matrix Q; the observation noise covariance matrix R; and the infinitesimal scalar value s to be used in a difference method. The nonlinear observation equation y=h(t, x, u) is used in place of the linear observation equation y=Cx.
The observationequation linearization unit 7 analytically calculates the Jacobian H from the observation equation h(t, x, u). From the Jacobian H_{k }of the observation equation, system output y(t_{k}), the predicted current state value x_{kk−1 }(hat), and the predicted current error covariance matrix value Σ_{kk−1 }(hat), the stateanderror estimation unit 1 provides the current estimated value x_{kk }(hat) of the state and the current estimated value Σ_{kk }(hat) of the error covariance matrix.
The stateequation discretization unit 2 outputs a discretetime state equation f_{d}(t_{k}, x_{kk }(hat), u(t_{k})), which has been discretized using a fourthorder RungeKutta method, on the basis of: the nonlinear continuoustime state equation (d/dt)x=f(t, x, u); the current state and error covariance matrix x_{kk }(hat) and Σ_{kk }(hat); and the system input u(t_{k}).
From the discretizationtime state equation f_{d}(t_{k}, x_{kk }(hat), u(t_{k})) and the infinitesimal scalar value ε, the stateequation linearization unit 3 calculates the Jacobian αf_{d}(t, x, u)/αx^{T}t=t_{k}, x=x_{kk }(hat), u=u(t_{k}) of the state equation by using a difference method. The state prediction unit 5 outputs the discretizationtime state equation f_{d}(t, x, u)/αx^{T}t=t_{k}, x=x_{kk }(hat), u(t_{k})) as the predicted state value x_{k+1k }(hat) for the next time point. From the Jacobian αf_{d}(t, x, u)/αx^{T}t=t_{k}, x=x_{kk }(hat), u=u(t_{k}) of the state equation, the current estimated error covariance matrix value Σ_{kk }(hat), and the system noise covariance matrix Q, the error prediction unit 6 calculates the predicted error covariance matrix value Σ_{k+1k }(hat) for the next time point.
The state estimation device according to this embodiment will next be described in detail. First, assume that the nonlinear model of the target system can be expressed by the state equation and the observation equation of below Formula (16).
[Formula 16]
State equation: {dot over (x)}(t)=f(t, x(t), u(t))
Observation equation: y(t)=h(t, x(t), u(t))
where State vector: x∈^{n×1 }
System output: y∈^{q×1 }
System input: u∈^{m×1} (16)
The initialization operation at step S1 is similar to the initialization operation in the first embodiment. The observationequation linearization unit 7 analytically calculates the Jacobian H of the observation equation, as shown by below Formula (17) (S21). Note that H is a q×n matrix.
Next, the observationequation linearization unit 7 substitutes, at each time point, the predicted state value x_{kk−1 }(hat) and the system input u(t_{k}) for the current time t_{k }into the Jacobian of the observation equation, and thus obtains the current Jacobian H_{k }as shown by below Formula (18) (S22). Note that x_{kk−1 }(hat) is an n×1 matrix, and H_{k }is a q×n matrix.
Then, as shown by below Formula (19), the stateanderror estimation unit 1 calculates the Kalman gain K_{k }from: the predicted current error covariance matrix value Σ_{kk−1 }(hat); the Jacobian H_{k}; and the observation noise covariance matrix R (S2).
[Formula 19]
K_{k}={circumflex over (Σ)}_{kk−1}H_{k}^{T}(H_{k}{circumflex over (Σ)}_{kk−1}H_{k}^{T}+R)^{−1} (19)
Next, the stateanderror estimation unit 1 calculates the current estimated error covariance matrix value Σ_{kk }(hat) as shown by below Formula (20) (S3). The the current estimated error covariance matrix value Σ_{kk }(hat) is the difference between the predicted error covariance matrix value Σ_{kk−1 }(hat) and a matrix that is a product obtained by multiplying the predicted value Σ_{kk−1 }(hat) by both the Kalman gain K_{k }and the coefficient H_{k }of the observation equation. Note that Σ_{kk }(hat) is an n×n matrix.
[Formula 20]
{circumflex over (Σ)}_{kk}={circumflex over (Σ)}_{k−1}−K_{k}H_{k}{circumflex over (Σ)}_{kk−1} (20)
Then, the stateanderror estimation unit 1 calculates the current estimated state value x_{kk }(hat) as shown by below Formula (21) (S4). The current estimated state value x_{kk }(hat) is a sum of the predicted current state value x_{kk−1 }(hat) and a product of the Kalman gain and a difference between the current system output y(t_{k}) and the system output y_{kk−1 }(hat) that is predicted using the observation equation on the basis of the predicted state value x_{kk−1 }(hat) and the system input u(t_{k}). Note that x_{kk }(hat) is an n×1 matrix.
In the stateequation discretization unit 2, the state x(t_{k+1}) for the next time point is expressed by a sum of the current state x(t_{k}) and an integral value of the state equation from the time t_{k }to the time t_{k+1}, as shown by below Formula (22). Thus, the stateequation discretization unit 2 calculates an approximate value of the nonlinear discretetime state equation x(t_{k+1})=f_{d}(t_{k}, x(t_{k}), u(t_{k})) by using a quadratic or higherorder integration technique. The quadratic or higherorder numerical integration technique is, for example, a calculation technique using a fourthorder RungeKutta method that also takes into account the system input.
[Formula 22]
x(t_{k+1})=x(t_{k})+∫_{t}_{k}^{t}^{k+1}f(t, x(t), u(t))dt (22)
First, α=f(t_{k}, x(t_{k}), u(t_{k})) is calculated from the current state x(t_{k}) and the system input u(t_{k}).
Next, β=x(t_{k})+(ΔT/2)α is calculated using the result of calculation of this α and a time interval ΔT=t_{k+1}−t_{k}.
Then, γ=f(t_{k}+(ΔT/2), β, u(t_{k})) is calculated using the result of calculation of this β.
Then, λ=x(t_{k})+(ΔT/2)γ is calculated using the result of calculation of this γ.
Then, ζ=f(t_{k}+(ΔT/2), λ, u(t_{k})) is calculated using the result of calculation of this λ.
Then, η=x(t_{k})+ΔTζ is calculated using the result of calculation of this ζ.
Then, ζ=f(t_{k−1}, η, u(t_{k})) is calculated using the result of calculation of this η.
Then, the discretetime state equation f_{d}(t_{k}, x(t_{k}), u(t_{k}))=x(t_{k})+(ΔT/6) (α+2(γ+ζ)+ζ) is calculated.
The stateequation discretization unit 2 then substitutes the current estimated state value x_{kk }(hat) for the current state x(t_{k}) (S5), and calculates the discretetime state equation f_{d}(t_{k}, x_{kk }(hat), u(t_{k})) by using the above RungeKutta method (S6).
As shown by below Formula (23), the state prediction unit 5 outputs the discretetime state equation f_{d}(t_{k}, x_{kk }(hat), u(t_{k})) as the predicted state value x_{k+1k }(hat) for the next time point t_{k+1 }(S7).
[Formula 23]
{circumflex over (x)}_{k+1k}=f_{d}(t_{k}, {circumflex over (x)}_{kk}, u(t_{k})) (23)
The stateequation linearization unit 3 calculates the approximate Jacobian value F_{k }on the basis of the discretetime state equation f_{d}(t_{k}, x_{kk }(hat), u(t_{k})) and the predetermined infinitesimal scalar value ε, by using a difference method. Note that F_{k }is an n×n matrix. The stateequation linearization unit 3 first calculates x_{i }(hat) from the current estimated state value x_{kk }(hat), as shown by above Formula (7) (S8).
Next, the stateequation linearization unit 3 calculates the value of the discretetime state equation on the basis of x_{i }(hat) by using the numerical integration technique described above (S9). Then, as shown by below Formula (24), the stateequation linearization unit 3 approximates the ith column (i=1, 2, . . . , n) of the Jacobian (S10) by providing a value obtained by dividing, by a value εx_{kk }(hat), the difference from the discretetime state equation f_{d}(t_{k}, x_{kk }(hat), u(t_{k})) for the current estimated state value x_{kk }(hat), the value εx_{kk}(i) (hat) being obtained by multiplying the ith component of the current estimated state value by ε.
where P_{i}∈^{n×n}.
n×n matrix having (i,i) elements being 1, and the other elements being 0 (24)
The error prediction unit 6 then calculates the error covariance matrix Σ_{k+1k }(hat) for the next time point, on the basis of: the approximate Jacobian value F_{k}; the current estimated error covariance matrix value Σ_{kk }(hat); and the system noise covariance matrix Q, by using below Formula (25) (S11). Note that Q is an n×n matrix.
[Formula 25]
{circumflex over (Σ)}_{k+1k}=F_{k}{circumflex over (Σ)}_{kk}F_{k}^{T}+Q (25)
The calculation operations at the above steps are performed in the processor 21. The calculated values obtained at the corresponding steps, such as the Kalman gain K_{k}, the estimated values x_{kk }(hat) and Σ_{kk }(hat), the predicted values x_{kk−1 }(hat) and Σ_{kk−1 }(hat) , the discretetime state equation f_{d}, and the approximate Jacobian value F_{k }of the state equation are stored in the memory 22, and are read from the memory 22 and used at the corresponding steps.
As described above, even when a target system receives an input, and the state equation of the target system is given by a nonlinear continuoustime function, the state estimation device according to this embodiment enables a highorder, i.e., quadratic or higherorder, integration technique to be applied to discretization of the state equation because an approximate Jacobian value is calculated on the basis of the estimated state value at each time point. This can reduce the discretization error in the process of state estimation that uses an extended Kalman filter, and can thus improve state estimation accuracy.
Note that the state estimation device according to this embodiment uses a fourthorder RungeKutta method for the approximation relating to Formula (22) described above, but the present invention is not limited thereto. An integration technique having accuracy of quadratic or higherorder approximation, such as Heun'"'"'s method, can be satisfactorily used.
Although this embodiment is based on the assumption that the observation equation of the target system is nonlinear, the present invention is also applicable to a case in which the observation equation is a linear function. The case in which the observation equation is a linear function can be addressed by omitting the observationequation linearization unit 7, and using H_{k }that is the coefficient matrix for the state in the observation equation.
The infinitesimal scalar value ε used in a difference method is preferably as low a value as possible depending on the target system, and may be externally changeable via the input device 24 depending on an estimation result displayed on the display device 25. Externally changing the infinitesimal scalar value ε to a minimum value without causing an anomaly such as divergence of the estimation result can reduce the error caused by approximation of the Jacobian. In addition, instead of using an infinitesimal scalar value ε, a matrix of vector n×1 may be used as the parameter ε, such that the calculation is performed using an infinitesimal value ε(i) that is partially or wholly different across the states, as given by Formula (26) below.
Moreover, if a relationship of x_{kk}(i) (hat)≈0 applies, Formula (27) below may be used instead, which uses an infinitesimal value ε′ temporarily or for the entire period.
The state estimation device according to this embodiment receives, as inputs, a continuoustime model (d/dt)x=f(t, x, θ) and y=h(t, x, θ) containing an unknown parameter θ of the target system, and the system output y(t_{k}) at each time point. The state estimation device 10C uses design parameters: the system noise covariance matrix Q; the observation noise covariance matrix R; and the infinitesimal scalar value ε to be used in a difference method. The state estimation device 10C estimates the unknown parameter θ and the state x at the same time. The nonlinear observation equation y=h(t, x, θ) is used in place of the linear observation equation y=Cx.
The model conversion unit 8 converts the continuoustime models into respective nonlinear continuoustime models using a new state vector having therein the state x and the unknown parameter 8 before conversion.
The state estimation device 10B uses, as inputs, the nonlinear continuoustime model generated by the conversion, and calculates an estimated value of the new state vector. The observationequation linearization unit 7, the stateanderror estimation unit 1, the stateequation discretization unit 2, the stateequation linearization unit 3, the state prediction unit 5, and the error prediction unit 6 of the state estimation device 10B are identical to those of the first to third embodiments.
A process for estimating the state and the parameter by the state estimation device according to this embodiment will next be described in detail. Assume that the model of the target system can be expressed by the following continuoustime state equation and observation equation each containing an unknown parameter θ, as shown by Formula (28) below.
[Formula 28]
{dot over (x)}=f(t, x, θ)
y=h(t, x, θ)
where State vector: x∈^{n×1 }
System output: y∈^{q×1 }
Unknown parameter: θ∈^{p×1} (28)
The process includes an additional operation to be performed by the model conversion unit 8 before the initialization step (S1) in
The model conversion unit 8 converts the continuoustime model of the system containing an unknown parameter θ into new nonlinear continuoustime models shown by below Formula (29) using a new state vector x (tilde)=[x^{T}θ^{T}]^{T }having therein the state x and the unknown parameter θ before conversion. Note that x (tilde) is an (n+p)×1 matrix.
[Formula 29]
{tilde over ({dot over (x)})}={tilde over (f)}(t, {tilde over (x)})
y=h(t, {tilde over (x)}) (29)
In this process, in a case in which the unknown parameter θ varies in time, the unknown parameter θ is modeled, and is set as n+1:n+p components of the function f (tilde). Alternatively, in a case in which the unknown parameter θ is constant or varies in time at a rate 10 or more times slower than the sampling rate, the n+1:n+p components of the function f (tilde) are set to zero. The subsequent operations are similar to those shown in
The calculation operations at the above steps are performed in the processor 21. The calculated values obtained at the corresponding steps, such as the Kalman gain K_{k}, the estimated values x_{kk }(tilde) (hat) and Σ_{kk }(hat), the predicted values x_{kk−1 }(tilde) (hat) and Σ_{kk−1 }(hat), the discretetime state equation f_{d }(tilde), and the approximate Jacobian value F_{k }(tilde) of the state equation are stored in the memory 22, and are read from the memory 22 and used at the corresponding steps.
As described above, even when the state equation of a target system is a continuoustime function, and contains an unknown parameter, the state estimation device according to this embodiment can estimate the state by conversion of the continuoustime model into a model having a new state having therein the state and the unknown parameter before conversion. This can reduce the discretization error, and can thus perform state estimation and parameter estimation with high accuracy.
Note that the continuoustime model containing an unknown parameter, used as inputs in this embodiment may be nonlinear or linear, and even linear continuoustime model is output as a nonlinear model from the model conversion unit. In addition, as long as the continuoustime model causes the model conversion unit to provide an output that includes a nonlinear continuoustime state equation, the unknown parameter may be contained in one or both of the state equation and the observation equation. Moreover, the continuoustime models may or may not contain a system input.
The configurations described in the foregoing embodiments are merely examples of various aspects of the present invention. These configurations may be combined with a known other technology, and moreover, a part of such configurations may be omitted and/or modified without departing from the spirit of the present invention.
1 stateanderror estimation unit; 2 stateequation discretization unit; 3 stateequation linearization unit; 4 stateanderror prediction unit; 5 state prediction unit; 6 error prediction unit; 7 observationequation linearization unit; 8 model conversion unit; 10, 10A, 10B, 10C state estimation device; 21 processor; 22 memory; 23 system interface; 24 input device; 25 display device.