Chapter 8

State estimation with Kalman Filter

8.1 Introduction

This chapter describes how to estimate the values of state variables of a dynamic system. Why can such state estimators be useful? Supervision: State estimates can provide valuable information about important variables in a physical process, for example feed composition to a reactor, ow variations to a separator, etc. Control: In general, the more information the controller has about the process it controls, the better (mor accurate) it can control it. State estimators can be a practical or economical alternative to real measurements. For example, the environmental forces acting on a vessel can be used by the controller to better control the position of the vessel, as in Dynamic Positioning systems. Because the state estimators are kind of sensors implemented in software (computer programs) they are some times denoted soft sensors. This chapter describes the Kalman Filter which is the most important algorithm for state estimation. The Kalman Filter was developed by Rudolf E. Kalman around 1960 [7]. There is a continuous-time version of the Kalman Filter and several discrete-time versions. (The discrete-time versions are immediately ready for implementation in a computer program.) Here the predictor-corrector version of the discrete-time Kalman 101

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

102

Filter will be described. This version seems to be the most commonly used version.

8.2

Observability

A necessary condition for the Kalman Filter to work correctly is that the system for which the states are to be estimated, is observable. Therefore, you should check for observability before applying the Kalman Filter. (There may still be other problems that prevent the Kalman Filter from producing accurate state estimates, as a faulty or inaccurate mathematical model.) Observability for the discrete-time systems can be dened as follows [13]: The discrete-time system x(k + 1) = Ax(k) + Bu(k) y (k) = Cx(k) + Du(k) (8.1) (8.2)

is observable if there is a nite number of time steps k so that knowledge about the input sequence u(0), . . . , u(k 1) and the output sequence y(0), . . . , y (k 1) is sucient to determine the initial state state of the system, x(0). Let us derive a criterion for the system to be observable. Since the inuende of input u on state x is known from the model, let us for simplicity assume that u(k) = 0. From the model (8.1) (8.2) we get y (0) = Cx(0) y (1) = Cx(1) = CAx(0) . . . y(n 1) = CAn1 x(0) which can be expressed compactly as C CA x(0) = . . . | CAn1 {z }
Mob s

(8.3) (8.4)

(8.5)

y (0) y (1) . . .
Y

(8.6)

y (n 1) {z }

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo Let us make a denition: Observability matrix: C CA Mobs = . . . CAn1

103

(8.7)

(8.6) has a unique solution only if the rank of Mobs is n. Therefore: Observability Criterion: The system (8.1) (8.2) is observable if and only if the observability matrix has rank equal to n where n is the order of the system model (the number state variables). The rank can be checked by calculating the determinant of Mobs . If the determinant is non-zero, the rank is full, and hence, the system is observable. If the determinant is zero, system is non-observable. Non-observability has several concequences: The transfer function from the input variable y to the output variable y has an order that is less than the number of state variables (n). There are state variables or linear combinations of state variables that do not show any response. The steady-state value of the Kalman Filter gain can not be computed. This gain is used to update the state estimates from measurements of the (real) system. Example 18 Observability Given the following state space model: x1 (k) 1 a 0 x1 (k + 1) = + u(k) 0 1 1 x2 (k + 1) x2 (k) | {z } | {z }
A B

(8.8)

y(k) =

c1 0 | {z }
C

x1 (k ) x2 (k )

+ [0] u(k) |{z}

D

(8.9)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo The observability matrix is (n = 2) c 0 1 C Mobs = = 2 1 1 a = CA CA c1 0 0 1 The determinant of Mobs is det (Mobs ) = c1 ac1 c1 0 = a c1 2 The system is observable only if a c1 2 6= 0.

104

c1 0 c1 ac1

(8.10)

(8.11)

Assume that a 6= 0 which means that the rst state variable, x1 , contains some non-zero information about the second state variable, x2 . Then the system is observable if c1 6= 0, i.e. if x1 is measured. Assume that a = 0 which means that x1 contains no information about x2 . In this case the system is non-observable despite that x1 is measured.

[End of Example 18]

8.3

The Kalman Filter algorithm

The Kalman Filter is a state estimator which produces an optimal estimate in the sense that the mean value of the sum (actually of any linear combination) of the estimation errors gets a minimal value. In other words, The Kalman Filter gives the following sum of squared errors: (8.12) E [ ex T (k)ex (k)] = E ex1 2 (k ) + + exn 2 (k) a minimal value. Here, ex (k) = xest (x) x(k ) (8.13)

is the estimation error vector. (The Kaman Filter estimate is sometimes denoted the least mean-square estimate.) This assumes actually that the model is linear, so it is not fully correct for nonlinear models. It is assumed the that the system for which the states are to be estimated is excited by random (white) disturbances ( or process noise ) and that the

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

105

measurements (there must be at least one real measurement in a Kalman Filter) contain random (white) measurement noise. The Kalman Filter has many applications, e.g. in dynamic positioning of ships where the Kalman Filter estimates the position and the speed of the vessel and also environmental forces. These estimates are used in the positional control system of the ship. The Kalman Filter is also used in soft-sensor systems used for supervision, in fault-detection systems, and in Model-based Predictive Controllers (MPCs) which is an important type of model-based controllers. The Kalman Filter algorithm was originally developed for systems assumed to be represented with a linear state-space model. However, in many applications the system model is nonlinear. Furthermore the linear model is just a special case of a nonlinear model. Therefore, I have decided to present the Kalman Filter for nonlinear models, but comments are given about the linear case. The Kalman Filter for nonlinear models is denoted the Extended Kalman Filter because it is an extended use of the original Kalman Filter. However, for simplicity we can just denote it the Kalman Filter, dropping extended in the name. The Kalman Filter will be presented without derivation. The Kalman Filter presented below assumes that the system model consists of this discrete-time (possibly nonlinear) state space model: x(k + 1) = f [x(k), u(k)] + Gw(k ) and this (possibly nonlinear) measurement model: y (k) = g [x(k), u(k)] + Hw(k) + v (k) A linear model is just a special case: x(k + 1) = Ax(k) + Bu(k) + Gw(k) {z } |
=f

(8.14)

(8.15)

(8.16)

and

The models above contains the following variables and functions:

y(k) = Cx(k) + Du(k) + Hw(k) + v (k ) | {z }

=g

(8.17)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo x is the state vector of n state variables: x1 x2 x= . . . xn u is the input vector of m input variables: u1 u2 u= . . . um

106

(8.18)

(8.19)

It is assumed that the value of u is known. u includes control variables and known disturbances.

f is the system vector function:

where fi () is any nonlinear or linear function. w is random (white) disturbance (or process noise) vector: w1 w2 w= . . . wq with auto-covariance Rw (L) = Q (L)

f =

f1 () f2 () . . . fn ()

(8.20)

(8.21)

(8.22)

where Q (a q q matrix of constants) is the auto-covariance of w at lag L = 0. (L) is the unit pulse function, cf. (6.25). A standard assumption is that Q11 0 0 0 0 Q22 0 0 Q= = diag(Q11 , Q22 , , Qnn ) (8.23) . . 0 . 0 0 0 0 0 Qnn Hence, the number q of process disturbances is assumed to be equal to the number n of state variables. Qii is the variance of wi .

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

107

G is the process noise gain matrix relating the process noise to the state variables. It is common to assume that q = n, making G square: G11 0 0 0 0 G22 0 0 (8.24) G= . . 0 . 0 0 0 0 0 Qnn In addition it is common to set the elements of G equal to one: Gii = 1 making G an identity matrix: 1 0 G= 0 0 0 0 0 0 = In .. . 0 0 0 0 1 0 1 (8.25)

(8.26)

g is the measurement vector function: g1 () g2 () g= . . . gr ()

y is the measurement vector of r measurement variables: y1 y2 y= . . . yr

(8.27)

(8.28)

where gi () is any nonlinear or linear function. Typically, g is a linear function on the form g (x) = Cx (8.29)

where C is the measurement gain matrix. H is a gain matrix relating the disturbances directly to the measurements (there will in addition be an indirect relation because the disturbances acts on the states, and some of the states are measured). It is however common to assume that H is a zero matrix of dimension (r q ): 0 0 0 0 . . (8.30) H = 0 0 ... . 0 0 Hrq

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo v is a random (white) measurement noise vector: v1 v2 v= . . . vr with auto-covariance Rv (L) = R (L)

108

(8.31)

(8.32)

Hence, Rii is the variance of vi .

where R (a r r matrix of constants) is the auto-covariance of v at lag L = 0. A standard assumption is that R11 0 0 0 0 R22 0 0 R= (8.33) = diag(R11 , R22 , , Rrr ) .. 0 . 0 0 0 0 0 Rrr

Note: If you need to adjust the strength or power of the process noise w or the measurement noise v , you can do it by increasing the variances, Q and R, respectively. Here you have the Kalman Filter: (The formulas (8.35) (8.37) below are represented by the block diagram shown in Figure 8.1.)

Kalman Filter state estimation:

1. This step is the initial step, and the operations here are executed only once. Assume that the initial guess of the state is xinit . The initial value xp (0) of the predicted state estimate xp (which is calculated continuously as described below) is set equal to this initial value: Initial state estimate xp (0) = xinit (8.34)

2. Calculate the predicted measurement estimate from the predicted state estimate: Predicted measurement estimate: yp (k) = g [xp (k )] (8.35) (It is assumed that the noise terms Hv (k) and w(k) are not known or are unpredictable (since they are white noise), so they can not be used in the calculation of the predicted measurement estimate.)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

109

3. Calculate the so-called innovation process or variable it is actually the measurement estimate error as the dierence between the measurement y(k) and the predicted measurement yp (k): Innovation variable: e(k) = y (k) yp (k ) (8.36)

4. Calculate the corrected state estimate xc (k) by adding the corrective term Ke(k) to the predicted state estimate xp (k ): Corrected state estimate: xc (k) = xp (k) + Ke(k ) (8.37)

Here, K is the Kalman Filter gain. The calculation of K is described below. Note : It is xc (k) that is used as the state estimate in applications1 . About terminology : The corrected estimate is also denoted the posteriori estimate because it is calculated after the present measurement is taken. It is also denoted the measurement-updated estimate. Due to the measurement-based correction term of the Kalman Filter, you can except the errors of the state estimates to be smaller than if there were no such correction term. This correction can be regarded as a feedback correction of the estimates, and it is well known from dynamic system theory, and in particular control systems theory, that feedback from measurements reduces errors. This feedback is indicated in Figure 8.1. 5. Calculate the predicted state estimate for the next time step, xp (k + 1), using the present state estimate xc (k ) and the known input u(k) in process model: Predicted state estimate: xp (k + 1) = f [xc (k ), u(k )] (It is assumed that the noise term Gv (k ) is not known or is unpredictable, since it is random, so it can not be used in the calculation of the state estimate.) About terminology : The predicted estimate is also denoted the priori estimate because it is calculated before the present measurement is taken. It is also denoted the time-updated estimate. (8.38)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

Process noise (disturbances) Measurement noise

110

Real system (process)

w( k)

v(k)

(Commonly no connection ) Known inputs (control variables and disturbances) State variable (unknown value ) Measurement variable

u(k)

Process

x(k)

Sensor

y( k)

x(k+1 ) = f[ x(k), u( k)] + Gw( k) (Commonly no connection ) (Commonly no connection )

y(k) = g[ x(k),u (k)] + Hw(k) + v(k)

f()
System function

xp(k+1)

Predicted Unit estimate delay Corrected estimate

1/z

xp (k)

g()

yp(k) e(k )
Innovation variable or process Feedback correction of estimate

Measurement function

xc( k)

Ke(k)

Kalman gain

Kalman Filter

x c( k)

Applied state estimate

Figure 8.1: The Kalman Filter algorithm (8.35) (8.38) represented by a block diagram (8.35) (8.38) can be represented by the block diagram shown in Figure 8.1. The Kalman Filter gain is a time-varying gain matrix. It is given by the algorithm presented below. In the expressions below the following matrices are used: Auto-covariance matrix (for lag zero) of the estimation error of the corrected estimate: o n (8.39) Pc = Rexc (0) = E (x mxc ) (x mxc )T
Therefore, I have underlined the formula.

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

111

The transition matrix A of a linearized model of the original nonlinear model (8.14) calculated with the most recent state estimate, which is assumed to be the corrected estimate xc (k ): f () A= (8.41) x xc (k), u(k) The measurement gain matrix C of a linearized model of the original nonlinear model (8.15) calculated with the most recent state estimate: g () (8.42) C= x
xc (k), u(k)

Auto-covariance matrix (for lag zero) of the estimation error of the predicted estimate: n o Pd = Rexd (0) = E (x mxd ) (x mxd )T (8.40)

However, it is very common that C = g (), and in these cases no linearization is necessary.

The Kalman Filter gain is calculated as follows (these calculations are repeated each program cycle):

Kalman Filter gain:

1. This step is the initial step, and the operations here are executed only once. The initial value Pp (0) can be set to some guessed value (matrix), e.g. to the identity matrix (of proper dimension). 2. Calculation of the Kalman Gain : Kalman Filter gain: K (k ) = Pp (k )C T [CPp (k)C T + R]1 (8.43)

3. Calculation of auto-covariance of corrected state estimate error : Auto-covariance of corrected state estimate error: Pc (k) = [I K (k )C ] Pp (k) (8.44)

4. Calculation of auto-covariance of the next time step of predicted state estimate error : Auto-covariance of predicted state estimate error: Pp (k + 1) = APc (k )AT + GQGT (8.45)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo Here are comments to the Kalman Filter algorithm presented above: 1. Order of formulas in the program cycle: The Kalman Filter formulas can be executed in the following order: (8.43), Kalman Filter gain

112

(8.36), innovation process (variable)

(8.37), corrected state estimate, which is the state estimate to be used in applications (8.38), predicted state estimate of next time step (8.41), transition matrix in linear model (8.44), auto-covarience of error of corrected estimate (8.42), measurement matrix in linear model

(8.45), auto-covarience of error of predicted estimate of next time step 2. Steady-state Kalman Filter gain. If the model is linear and time invariant (i.e. system matrices are not varying with time) the auto-covariances Pc and Pp will converge towards steady-state values. Consequently, the Kalman Filter gain will converge towards a steady-state Kalman Filter gain value, Ks 2 , which can be pre-calculated. It is quite common to use only the steady-state gain in applications. For a nonlinear system Ks may vary with the operating point (if the system matrix A of the linearized model varies with the operating point). In practical applications Ks may be re-calculated as the operating point changes. Figure 8.2 illustrates the information needed to compute the steady-state Kalman Filter gain, Ks . 3. Model errors. There are always model errors since no model can give a perfect description of a real (practical system). It is possible to analyze the implications of the model errors on the state estimates calculated by the Kalman Filter, but this will not be done in this book. However, in general, the estimation errors are smaller with the Kalman Filter than with a so-called ballistic state estimator , which is the state estimator that you get if the Kalman Filter gain K is set to zero. In the latter case there is no correction of the estimates. It is only the predictions (8.38) that make up the estimates. The Kalman Filter then just runs as a simulator.
2

MATLAB and LabVIEW have functions for calculating the steady-state Kalman Gain.

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

113

Transition matrix Process noise gain matrix Measurement gain matrix Process noise auto -covariance Measurement noise auto -covariance

A G C Q R

Steady state Kalman Filter gain Ks

Ks

Figure 8.2: Illustration of what information is needed to compute the steadystate Kalman Filter gain, Ks . Note that you can try to estimate model errors by augmenting the states with states representing the model errors. Augmented Kalman Filter is described in Section 8.4. 4. How to tune the Kalman Filter: Usually it is necessary to ne-tune the Kalman Filter when it is connected to the real system. The process disturbance (noise) auto-covariance Q and/or the measurement noise auto-covariance R are commonly used for the tuning. However, since R is relatively easy to calculate from a time series of measurements (using some variance function in for example LabVIEW or MATLAB), we only consider adjusting Q here. What is good behaviour of the Kalman Filter? How can good behaviour be observed? It is when the estimates seems to have reasonable values as you judge from your physical knowledge about the physical process. In addition the estimates must not be too noisy! What is the cause of the noise? In real systems it is mainly the measurement noise that introduces noise into the estimates. How do you tune Q to avoid too noisy estimates? The larger Q the stronger measurement-based updating of the state estimates because a large Q tells the Kalman Filter that the variations in the real state variables are assumed to be large (remember that the process noise inuences on the state variables, cf. (8.15)). Hence, the larger Q the larger Kalman Gain K and the stronger updating of the estimates. But this causes more measurement noise to be added to the estimates because the measurement noise is a term in the innovation process e which is calculated by K : xc (k) = xp (k ) + Ke(k ) = xp (k ) + K {g [x(k)] + v(k ) g [xp (k )]} (8.46) (8.47)

where v is real measurement noise. So, the main tuning rule is as follows: Select as large Q as possible without the state estimates becoming too noisy.

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

114

But Q is a matrix! How to select it large or small. Since each of the process disturbances typically are assumed to act on their respective state independently, Q can be set as a diagonal matrix: Q11 0 0 0 0 Q22 0 0 Q= = diag(Q11 , Q22 , , Qnn ) (8.48) .. 0 . 0 0 0 0 0 Qnn

where Q0 is the only tuning parameter. If you do not have any idea about a proper value of Q0 you may initially try Q0 = 0.01

where each of the diagonal elements can be adjusted independently. If you do not have any idea about numerical values, you can start by setting all the diagonal elements to one, and hence Q is 1 0 0 0 0 1 0 0 (8.49) Q = Q0 0 0 ... 0 0 0 0 1

(8.50)

Then you may adjust Q0 or try to ne tune each of the diagonal elements individually. 5. The error-model: Assuming that the system model is linear and that the model is correct (giving a correct representation of the real system), it can be shown that the behaviour of the error of the corrected state estimation, exc (k ), cf. (8.13), is given by the following error-model :3 Error-model of Kalman Filter: exc (k + 1) = (I KC ) Aexc (k ) + (I KC ) Gv (k ) Kw(k + 1) (8.51) This model can be used to analyze the Kalman Filter. Note: (8.13) is not identical to the auto-covariance of the estimation error which is Pc (k ) = E {[exc (k) mxc (k)][exc (k) mxc (k )]T } (8.52)

But (8.13) is the trace of Pc (the trace is the sum of the diagonal elements): (8.53) exc = trace [ Pc (k)]
You can derive this model by subtracting the model describing the corrected state estimate from the model that describes the real state (the latter is simply the process model).
3

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

115

6. The dynamics of the Kalman Filter. The error-model (8.51) of the Kalman Filter represents a dynamic system. The dynamics of the Kalman Filter is given can be analyzed by calculating the eigenvalues of the system matrix of (8.51). These eigenvalues are {1 , 2 , . . . , n } = eig [(I KC )A] (8.54)

7. The stability of the Kalman Filter. It can be shown that the Kalman Filter always is an asymptotically stable dynamic system (otherwise it could not give an optimal estimate). In other words, the eigenvalues dened by (8.54) are always inside the unity circle. 8. Testing the Kalman Filter before practical use: As with every model-based algorithm (as controllers) you should test your Kalman Filter with a simulated process before applying it to the real system. You can implement a simulator in e.g. LabVIEW or MATLAB/Simulink since you already have a model (the Kalman Filter is model-based). Firstly, you should test the Kalman Filter with the nominal model in the simulator, including process and measurement noise. This is the model that you are basing the Kalman Filter on. Secondly, you should introduce some reasonable model errors by making the simulator model somewhat dierent from the Kalman Filter model, and observe if the Kalman Filter still produces usable estimates. 9. Predictor type Kalman Filter. In the predictor type of the Kalman Filter there is only one formula for the calculation of the state estimate: xest (k + 1) = Axest (k) + K [y (k) Cxest (k )] (8.55)

Thus, there is no distinction between the predicted state estimate and the corrected estimate (it the same variable). (Actually, this is the original version of the Kalman Filter[7].) The predictor type Kalman Filter has the drawback that there is a time delay of one time step between the measurement y (k) and the calculated state estimate xest (k + 1).

8.4

Estimating parameters and disturbances with Kalman Filter

In many applications the Kalman Filter is used to estimate parameters and/or disturbances in addition to the ordinary state variables. One

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

116

example is dynamic positioning systems for ship position control where the Kalman Filter is used to estimate environmental forces acting on the ship (these estimates are used in the controller as a feedforward control signal). These parameters and/or disturbances must be represented as state variables. They represent additional state variables. The original state vector is augmented with these new state variables which we may denote the augmentative states. The Kalman Filter is used to estimate the augmented state vector which consists of both the original state variables and the augmentative state variables. But how can you model these augmentative state variables? The augmentative model must be in the form of a dierence equation because that is the model form of state variables are dened. To set up an augmentative model you must make an assumption about the behaviour of the augmentative state. Let us look at some augmentative models. Augmentative state is (almost) constant: The most common augmentative model is based on the assumption that the augmentative state variable xa is slowly varying, almost constant. The corresponding dierential equation is x a (t) = 0 (8.56)

Discretizing this dierential equation with the Euler Forward method gives (8.57) xa (k + 1) = xa (k) which is a dierence equation ready for Kalman Filter algorithm. It is however common to assume that the state is driven by some noise, hence the augmentative model become: xa (k + 1) = xa (k) + wa (k ) (8.58)

where wa is white process noise with assumed auto-covariance on the form Rwa (L) = Qa (L). As pointed out in Section 8.3, the variance Qa can be used as a tuning parameter of the Kalman Filter. Augmentative state has (almost) constant rate: The corresponding dierential equation is x a = 0 or, in state space form, with xa1 xa , x a1 = xa2 x a2 = 0 (8.60) (8.61) (8.59)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

117

where xa2 is another augmentative state variable. Applying Euler Forward discretization with sampling interval h [sec] to (8.60) (8.61) and including white process noise to the resulting dierence equations gives xa1 (k + 1) = xa1 (k) + hxa2 (k) + wa1 (k) xa2 (k + 1) = xa2 (k ) + wa2 (k) (8.62) (8.63)

Once you have dened the augmented model, you can design and implement the Kalman Filter in the usual way. The Kalman Filter then estimates both the original states and the augmentative states. The following example shows how the state augementation can be done in a practical application. The example also shows how to use functions in LabVIEW and in MATLAB to calculate the steady state Kalman Filter gain. Example 19 Kalman Filter Figure 8.3 shows a liquid tank. We will design a steady state Kalman Filter to estimate the outow Fout . The level h is measured. Mass balance of the liquid in the tank is (mass is Ah) (t) = Kp u Fout (t) Atank h = Kp u Fout (t) After cancelling the density the model is (t) = h 1 [Kp u Fout (t)] Atank (8.66) (8.64) (8.65)

We assume that the unknown outow is slowly changing, almost constant. We dene the following augmentative model: out (t) = 0 F (8.67)

The model of the system is given by (8.66) (8.67). Although it is not necessary, it is convenient to rename the state variables using standard names. So we dene (8.68) x1 = h x2 = Fout (8.69)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

u [V]

118

Kp [m3 /V]

Fin [m3/s] Measured Level h [m] sensor m [kg] [kg/m3]

h [m]

0 A [m2]

Fout [m 3/s]

Kalman Filter

Estimated Fout

Figure 8.3: Example 19: Liquid tank The model (8.66) (8.67) is now x 1 (t) = 1 Atank [Kp u(t) x2 (t)] (8.70) (8.71)

x 2 (t) = 0

Applying Euler Forward discretization with time step T and including white disturbance noise in the resulting dierence equations yields T x1 (k + 1) = x1 (k) + [Kp u(k ) x2 (k)] + w1 (k) Atank {z } |
f1 ()

(8.72)

or

x(k + 1) = f [x(k), u(k )] + w(k)

x2 (k + 1) = x2 (k ) + w2 (k ) | {z }
f2 ()

(8.73)

(8.74)

w1 and w2 are independent (uncorrelated) white process noises with assumed variances Rw1 (L) = Q1 (L) and Rw2 (L) = Q2 (L) respectively. Here, Q1 and Q2 are variances. The multivariable noise model is then w1 w= (8.75) w2

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo with auto-covariance Rw (L) = Q (L) where Q= Q11 0 0 Q22

119

(8.76) (8.77)

Assuming that the level x1 is measured, we add the following measurement equation to the state space model: y (k) = g [xp (k ), u(k)] + v(k) = x1 (k) + v (k) where v is white measurement noise with assumed variance Rv (L) = R (L) where R is the measurement variance. The following numerical values are used: Sampling time: T = 0.1 s Atank = 0.1 m2 Kp = 0.001 (m3 /s)/V 0.01 0 Q= (initally, may be adjusted) 0 0.01 R = 0.0001 m2 (Gaussian white noise) We will set the initial estimates as follows: x1p (0) = x1 (0) = y (0) (from the sensor) x2p (0) = 0 (assuming no information about initial value The Kalman Filter algorithm is as follows: The predicted level measurement is calculated according to (8.35): yp (k) = g [xp (k ), u(k)] = x1p (k) (8.87) (8.85) (8.86) (8.80) (8.81) (8.82) (8.83) (8.84) (8.79) (8.78)

with initial value as given by (8.85). The innovation variable is calculated according to (8.36), where y is the level measurement: e(k) = y (k ) yp (k) The corrected state estimate is calculated according to (8.37): xc (k ) = xp (k) + Ke(k) (8.89) (8.88)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo or, in detail: K11 e(k) K21 | {z }
K

120

x1c (k) x2c (k)

x1p (k ) x2p (k )

(8.90)

This is the applied esimate!

The predicted state estimate for the next time step, xp (k + 1), is calculated according to (8.38): (8.91) xp (k + 1) = f [xc (k ), u(k )] or, in detail: x1c (k ) + f1 x1p (k + 1) = = f2 x2p (k + 1)

T Atank

[Kp u(k ) x2c (k)] x2c (k)

(8.92)

To calculate the steady state Kalman Filter gain Ks the following information is needed, cf. Figure 8.2: f () A = x xp (k), u(k) f1 f1 x2 x1 = f2 f2 x1 x2 xp (k), u(k) T 1 Atank = 0 1 G= 1 0 0 1 = I2 (identity matrix)

(8.93)

(8.94)

(8.95)

(8.96)

Q=

0.01 0 0 0.01

g () C = x xp (k), u(k) = 1 0 (initially, may be adjusted) R = 0.001 m2

(8.97) (8.98) (8.99) (8.100)

Figure 8.4 shows the front panel of a LabVIEW simulator of this example. The outow Fout = x2 was changed during the simulation, and the Kalman Filter estimates the correct steady state value (the estimate is however

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

121

Figure 8.4: Example 19: Front panel of a LabVIEW simulator of this example noisy). During the simulations, I found that (8.99) gave too noise estimate of x2 = Fout . I ended up with 0.01 0 (8.101) Q= 0 106 as a proper value. Figure 8.5 shows how the steady state Kalman Filter gain Ks is calculated using the Kalman Gain function. The gure also shows how to check for observability with the Observability Matrix function. Figure 8.6 shows the implementation of the Kalman Filter equations in a Formula Node. Below is MATLAB code that calculates the steady state Kalman Filter gain. It is calculated with the dlqe4 function (in Control System Toolbox). A=[1,-1;0,1] G=[1,0;0,1] C=[1,0]
4

dlqe = Discrete-time linear quadratic estimator.

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

122

Figure 8.5: Example 19: Calculation of the steady state Kalman Filter gain with the Kalman Gain function, and checking for observability with the Observability Matrix function. Q=[0.01,0;0,1e-6] R=[0.0001]; [K,Pp,Pc,E] = dlqe(A,G,C,Q,R) K is the steady state Kalman Filter gain. (Pp and Pc are steady state estimation error auto-covariances, cf. (8.45) and (8.44). E is a vector containing the eigenvalues of the Kalman Filter, cf. (8.54).) MATLAB answers K = 0.9903 -0.0099 Which is the same as calculated by the LabVIEW function Kalman Gain, cf. Figure 8.5.

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

123

Figure 8.6: Example 19: Implementation of the Kalman Filter equations in a Formula Node. (The Kalman Filter gain is fetched from the While loop in the Block diagram using local variables.)
[End of Example 19]

8.5

State estimators for deterministic systems: Observers

Assume that the system (process) for which we are to calculate the state estimate has no process disturbances : w(k ) 0 and no measurement noise signals : v(k) 0 (8.103) (8.102)

Then the system is not a stochastic system any more. In stead we can say is is non-stochastic or deterministic. An observer is a state estimator for a deterministic system. It is common to use the same estimator formulas in

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo an observer as in the Kalman Filter, namely (8.35), (8.36), (8.37) and (8.38). For convenience these formulas are repeated here: Predicted measurement estimate: yp (k) = g [xp (k )] Innovation variable: e(k) = y (k ) yp (k) Corrected state estimate: xc (k ) = xp (k) + Ke(k) Predicted state estimate: xp (k + 1) = f [xc (k ), u(k )]

124

(8.104)

(8.105)

(8.106)

(8.107)

The estimator gain K (corresponding to the Kalman Filter gain in the stochastic case) is calculated from a specication of the eigenvalues of the error-model of the observer. The error-model is given by (8.51) with w(k) = 0 and v (k ) = 0, hence: Error-model of observers: exc (k + 1) = (I KC ) Aexc (k) = Ae exc (k ) | {z }
Ae

(8.108)

where Ae is the transition matrix of the error-model. The error-model shows how the state estimation error develops as a function of time from an initial error exc (0). If the error-model is asymptotically stable, the steady-state estimation errors will be zero (because (8.108) is an autonomous system.) The dynamic behaviour of the estimation errors is given by the eigenvalues of the transition matrix Ae (8.108). But how to calculate a proper value of the estimator gain K ? We can no longer use the formulas (8.43) (8.45). In stead, K can be calculated from the specied eigenvalues {z1 , z2 , . . . , zn } of Ae : eig [Ae ] = eig [(I KC ) A] = {z1 , z2 , . . . , zn } (8.109)

As is known from mathematics, the eigenvalues are the -roots of the characteristic equation: det [zI (I KC ) A] = (z z1 )(z z2 ) (z zn ) = 0 (8.110)

In (8.110) all terms except K are known or specied. To actually calculate K from (8.109) you should use computer tools as Matlab or LabVIEW. In

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

125

Matlab you can use the function place (in Control System Toobox). In LabVIEW you can use the function CD Place.vi (in Control Design Toolkit), and in MathScript (LabVIEW) you can use place. But how to specify the eigenvalues {zi }? One possibility is to specify discrete-time Butterworth eigenvalues {zi }. Alternatively you can specify continuous-time eigenvalues {si } which you transform to discrete-time eigenvalues using the transformation zi = esi h where h [s] is the time step. Example 20 Calculating the observer gain K The following MathScript-script or Matlab-script calculates the estimator gain K using the place function. Note that place calculates the gain K1 so that the eigenvalues of the matrix (A1 B1 K1 ) are as specied. place is used as follows: K=place(A,B,z) But we need to calculate K so that the eigenvalues of (I KC ) A = A KCA are as specied. The eigenvalues of A KCA are the same as the eigenvalues of (A KCA)T = AT (AC )T K T Therefore we must use place as follows: K1=place(A,(C*A),z); K=K1 Here is the script: h=0.05; %Time step s=[-2+2j, -2-2j]; %Specified s-eigenvalues z=exp(s*h); %Corresponding z-eigenvalues A = [1,0.05;0,0.95]; %Transition matrix B = [0;0.05]; %Input matrix C = [1,0]; %Output matrix D = [0]; %Direct-output matrix K1=place(A,(C*A),z); %Calculating gain K1 using place K=K1 %Transposing K The result is (8.112) (8.111)

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo K = 0.13818 0.22376

126

The function CD Place.vi on the Control Design Toolkit palette in LabVIEW can also be used to calculate the estimator gain K . Figure 8.7 shows the front panel and Figure 8.8 shows the block diagram of a LabVIEW program that calculates K for the same system as above. The same K is obtained.

Figure 8.7: Example 20: Front panel of the LabVIEW program to calculate the estimator gain K

Figure 8.8: Example 20: Block diagram of the LabVIEW program to calculate the estimator gain K

[End of Example 20]

F. Haugen: Kompendium for Kyb. 2 ved Hgskolen i Oslo

127

8.6

Kalman Filter or Observer?

Now you have seen two ways to calculate the estimator gain K of a state estimator: K is the Kalman Gain in a Kalman Filter K is the estimator gain in an observer The formulas for calculating the state estimate are the same in both cases: Kalman Filter: (8.35), (8.36), (8.37), (8.38). Observer: (8.104), (8.105), (8.106), (8.107). The observer may seem simpler than the Kalman Filter, but there is a potential danger in using the observer: The observer does not calculate any optimal state estimate in the least mean square sense, and hence the state estimate may become too noisy due to the inevitable measurement noise if you specify too fast error models dynamics (i.e. too large absolute value of the error-model eigenvalues). This is because fast dynamics requires a large estimator gain K , causing the measurement noise to give large inuence on the state estimates. In my opinion, the Kalman Filter gives a better approach to state estimation because it is easier and more intuitive to tune the lter in terms of process and measurement noise variances than in terms of eigenvalues of the error-model.