Chapter 7 Kalman Filter

7/29/2019 Chapter 7 Kalman Filter

1/8

Chapter 7

Kalman Filter

In this chapter, we study Kalman Filtering, which is a celebrated optimal observer/filter/predictiondesign method. In many ways, Kalman filter is the dual of LQ control. We shall develop Kalmanfilter in this way.

Recall that for time invariant systems, the design method for state feedback can be used todesign a state estimator (observer).

Here, the State Estimator design problem is to choose L in:

x = Ax + Bu + L(y(t) Cx) x = (A LC)x

so that the observer error dynamics is stable. This is ensured if A LC is stable.

The related State feedback Problem is to choose K in

x = ATx + CTu; u = Kx

x = (AT CTK)x.

AT CTK is stable. By choosing L = KT for the observer, the observer is ensured to be stable.

Sine the K obtained by LQ optimal (infinite horizon) control design is stabilizing as long assome stabilizability and detectability conditions are satisfied, L = KT can be used as a stabilizingobserver gain as well.

The questions we need to ask are:

How is it done - in the steady state case, and in the time varying (or finite horizon) case?

In what way is the resulting observer optimal?

7.1 Steady State Kalman Filter

Consider first the Infinite Horizon LQ Control:

x = Ax + Bu

where R is positive definite, Q = QT2 Q

1

2 with (A, B) stabilizable, (A, Q1

2 ) detectable.

We can solve for the positive (semi-) definite P in the ARE:

ATP + PA PBR1BTP + Q = 0. (7.1)

123


2/8

124 cPerry Y.Li

e.g. via the Hamiltonian matrix approach (using its eigenvectors) or by simply integrating theRiccati equation backwards in time.

We can get the optimal feedback gain: u = Kx where

K = R1BTP

Because (A, B) and (A, Q1

2 ) are stabilizable and detectable, A BK has eigenvalues on the openleft half plane.

The stable observer problem is to find L in:

x = Ax + Bu + L(y Cx)

so that A LC is stable.Let us solve LQ control problem for the dual problem:

x = ATx + CTu (7.2)

Transform the Algebraic Riccati Equation: CT B, AT A.

AP + PAT PC

TR1CP + Q = 0.

Stabilizing feedback gain K for (7.2) is:

K = LT = R1CP L = PCTR1

This generates the observer:

x = Ax + Bu + PCTR1(y Cx)

with observer error dynamics:x = (A PC

TR1C)x

which will be stable, as long as

1. (AT, CT) is stabilizable. This is the same as (A, C) being detectable.

2. (AT, Q1

2 ) is detectable. This is the same as (A, Q1

2 ) being stabilizable.

This design can be easily achieved by merely solving the Algebraic Riccati Equation (ARE). Mat-labs ARE.m command can be used. Questions we need to ask are:

In what way is this observer optimal?

How does finite time LQ extend to observer design?

7.2 Deterministic Kalman Filter

Problem statement:

Suppose we are given the output y() of a process over the interval [t0, tf]. It is anticipated thaty() is generated from a process of the form:

x = A(t)x + B(t)u

y = C(t)x


3/8

ME 8281:Advanced Control Systems, F01, F02, S06 125

To account for model uncertainty and measurement noise, the process model used is modified to:

x = A(t)x + B(t)u + Bw(t)w

y = C(t)x + v (7.3)

The state x(t|tf) is the state estimate the actual process at time t given all the measurements up to

time tf. x(t|tf) denotes derivative w.r.t. t. The final time tf is assumed to be fixed. In (7.3), wand v are the process and measurement noises that are assumed to be unknown. The initial state

x(t0|tf) is also assumed to be unknown.Choose the initial state x(t0|tf) and the measurement and process noises w(|tf), v(|tf) for

[t0, tf], so that:

1. The measured output is explained: i.e.

y() = C()x(|tf) + v(|tf)

for all [t0, tf].

2. Minimize the following objective function:

J(y(), x(t0|tf), w(|tf)) =12

xT(t0|tf)S1x(t0|tf)+

+1

2

tft0

wT(|tf)Q1()w(|tf) + v

T(|tf)R1()v(|tf) d

where v(|tf) = y() C()x(|tf).

Thus, the goal is to use as little noise or initial condition as possible so as to explainthe measuredoutput y().Remarks:

Set R1 large when measurement is accurate, i.e. assume little measurement noise v.

Set Q1 large when process model is accurate, i.e. assume little process noise w.

Set S1 large when estimate of x(t0|tf) = 0 is a confident guess of the initial state.

If a non-zero estimate (say x0) for the initial state is desired, it is possible to modify the costfunction so that:

1

2xT(t0|tf)S

1x(t0|tf) 1

2(x(t0|tf) x0)

TS1(x(t0|tf) x0).

It is clear that the optimal estimate at t0 before any measurement is made is: x(t0|t0) = x0.There are three types of Kalman filtering problem:

Filtering problem We are interested in solving for x(t|tf) where t < tf.

Prediction problem We are interested in solving x(t|tf) where t > tf.

Observer problem We are interested in x(t | t) where t is the current time. Thus, we need tosolve x(tf|tf) and then determine how this varies with tf. Conceptually, we need to resolvethis problem when tf changes. But, in practice, this is not necessary.

In the sequel, we shall drop the B(t)u term in

x = A(t)x + B(t)u

for convenience. It can easily be carried through.


4/8

126 cPerry Y.Li

7.3 Optimal filtering / observer Problem

The free variables in the optimal filtering / observer problem are:

x(t0|tf), the estimate of the initial state and

process noise w().

Notice that once these have been chosen, v(|tf) = y() C()x( |tf) is fixed.Let the final time tf be fixed. To avoid clutter, we shall omit the final time argument tf. Thus,

x() means x( | xf) etc.The constraint of the system is:

x() = Ax() + Bw()w(), [t0, tf].

We can convert this constrained optimal control problem into unconstrained optimization problemusing the Lagrange multiplier () n.

The cost function for the unconstrained optimization problem is:

J =1

2xT(t0)S

1x(t0) +1

2

tft0

wTQ1()w + vTR1()v + 2T()[ x (A()x + Bw()w)] d

Using integration by partsx = x x

and considering the variation of J with respect to variations in x(), w(), x(tf) and x(t0):

J =

tft0

wT()Q1() T()Bw()

w() d

+ tf

t0vT()R1()CT() T() T()A() x() d

+ T(tf)x(tf) +

x(t0)S1 T(t0)

x(t0)

and the variation with respect to () is simply

+

tft0

[ x (A()x + Bw()w)]() d

Since x(), w(), x(t0), x(tf) as well as () can be arbitrary, the necessary condition foroptimality is:

w() w() = Q()BTw()()

x() () = AT()() CT()R1()[y() C()x()]

() x = A()x + Bww

giving the differential equations:

x

=

A BwQB

Tw

CTR1C AT

x

0

CTR1y()

(7.4)


5/8


and the boundary conditions:

x(tf) (tf) = 0

x(t0) (t0) = S1x(t0)

This is a two point boundary value problem.

Notice that there is some similarity to the Hamilton-Jacobi-Bellman equation we saw for LQcontrol problem.

We solve this using a similar technique as for LQ to resolve the two point boundary valueproblem. Assume that

x(t) = P(t)(t) + g(t) (7.5)

Here g(t) is used to account for the exogenous term CT()R1()y().For (7.5) to be consistent with the HJB equations and boundary conditions, we need:

P = A()P + P AT() + Bw()Q()BTw() P C

T()R1()C()P (7.6)

g = A()g P()CT()R1()[C()g y()] (7.7)

with initial condition g(t0) = 0 and P(t0) = S.The first differential equation is the Filtering Riccati Differential Equation, which reduces to

the CTRDE for LQ control if A AT, C BT and (tf ).Notice that all boundary conditions are assigned at = t0. Thus, (7.6)-(7.7) can be integrated

forward in time.Substituting (7.5) into (7.4),

= (AT CTR1CP) + CTR1[Cg(t) y]; (tf) = 0. (7.8)

(t) can be integrated backwards in time, using P(t) and g(t) obtained from (7.6) and (7.7).Once (t) is obtained, the optimal estimate of the state at = t is then given by:

x(t | tf) = P(t|tf)(t|tf) + g(t|tf).

Here we re-inserted the argument tf to emphasize that the measurements y(t) is available on theinterval [t0, tf].Summary

Differential equations:

To be integrated forward in time

P = A()P + P AT() + Q() P CT()R1()C()P

g = A()g P()CT()R1()[C()g y()]

To be integrated backward in time

= (AT CTR1CP) CTR1y; (tf) = 0.

Boundary conditions:

g(t0) = 0 and P(t0) = S, (tf) = 0.

State estimate:x(t|tf) = P(t|tf)(t|tf) + g(t|tf).


6/8

128 cPerry Y.Li

7.4 Deterministic Optimal Observer

Let us denote P(t | tf) and g(t |tf) as the solutions to (7.6)-(7.7) with the available measurementup to tf. To design an observer, we are interested in using available information up to current timeto estimate the state at the same current time, i.e. we wish to find the solution x(t|t). This is doneby taking tf = t,

x(t | t) = P(t | t)(t|t) + g(t|t) = g(t|t)

since (t | t) = (tf) = 0.Now, since the boundary conditions for (7.6)-(7.7) are given at t0 (so they are integrated forward

in time), so for any > 0,

P(t|tf) = P(t|tf + ); g(t |tf) = g(t|tf + )

Hence, as tf increases, the solution for P(t|tf) and g(t|tf) can be reused. In fact, the terminal timetf does not enter into the solution of P() or g().

This means that the optimal observer is given by:

d

dt x(t |t) = g(t) = A(t)g(t) P(t)CT

(t)R1

(t)[C(t)g(t) y(t)]= Ax(t|t) P(t)CT(t)R1(t)

L(t)

[C(t)x(t|t) y()]; (7.9)

with initial condition x(0 | 0) = x0 = 0, and

P = A(t)P + P AT(t) + Bw(t)Q(t)BTw(t) P C

T(t)R1(t)C(t)P;

P(t0) = S and L(t) is the observer gain.

The Filtering Riccati Equation becomes the control Riccati Equation if we make the substi-tution:

A

T

A, CT

B, (tf )to obtain:

P = AT()P + P A() + Q() P B()R1()BT()P

The asymptotic solution P(t ) for the Kalman Filtering problem is analogous to P(t0 ) for the LQ control problem. Asymptotic properties of the Kalman filter can be obtainedusing this analogy.

The ARE to be solved is obtained using the above substitutions:

AP + PAT + Q PC

T(t)R1CP = 0.

The positive (semi-definite) solution P can be obtained via the eigenvectors of the Hamiltonian

matrix in (7.4).

In the time invariant case, P(t ) converges if (A, C) is detectable.

If (AT, Q1

2 ) is detectable, i.e. (A, Q1

2T) is stabilizable, then

Ac = A PCTR1C

is stable. Notice that although Q1 is used in the performance function, the Riccati equationuses Q. So, it is possible to implement a Kalman filter with Q not invertible. In this case,this means that Bw does not have the maximum column rank and can be reduced.


7/8


7.5 Stochastic Kalman Filter

Consider a stochastic process:

x = Ax + w; x(t0) = x0

y = Cx + v (7.10)

Here w(t) and v(t) are random noises, and x0 is a random variable: such that

E[w(t)wT()] = Q(t)(t ) (7.11)

E[v(t)vT()] = R(t)(t ) (7.12)

E[vi(t)wj() = 0 (7.13)

E[x0xT0 ] = S (7.14)

where E(x) is the expected value (mean) of the random variable x. In other words,

w(t) is un-related to w() whenever = t.

v(t) is un-related to v() whenever = t.

v(t) is not related to w() at all.

Q(t) is the expected size of w(t)wT(t).

R(t) is the expected size of v(t)vT(t).

S is the expected size of x0xT0 .

Assume that y() is measured for [t0, t]. If we apply the deterministic Kalman Filter tothis stochastic process to obtain x(t|t), and let x(t) = x(t|t) x(t). Then, it turns out that thisestimate minimizes:

E[x(t)xT(t)]

Moreover, the solution to the Filtering Riccati Equation is

P(t) = E[x(t)xT(t)]

See Goodwin Chapter 22.10 for the derivation of this result.

7.5.1 Colored noise

If the expected noises are not white - i.e. n(t) and n(t + ) are related in some way (or E(n(t)n(t +)) = 0 for t = ), then one can augment the model with a noise model.

For example, if the noise w is low pass band limited to [0, n], then we can assume that w isthe output of an appropriate low pass filter with [0, n] as the passband and a white noise input:

z = Azz + Bzw1

w = Czz + Dzw1

Here w is the band limited noise, but w1 is white. In general, the noise shaping filter should bedesign so that it matches the frequency spectrum of the colored noise. This is so because thespectrum of a white noise is a constant so that the spectrum of the output of the shaping filter willbe proportional to the frequency response of the filter itself.

Once the shaping filter has been designed, it can be incorporated into the process model so thatthe input to the system is now white. From the augmented model, the standard Kalman filtersolution can be obtained.


8/8

130 cPerry Y.Li

7.6 Linear Quadratic Gaussian (LQG)

Consider the system driven by Gaussian noise:

x = Ax + Bu + w

y = Cx + v

where

E[w(t)wT()] = Qw(t)(t ) (7.15)

E[v(t)vT()] = Rw(t)(t ) (7.16)

E[vi(t)wj() = 0 (7.17)

E[x0xT0 ] = Sw (7.18)

Moreover, assume that the probability distributions of w and v are zero mean, and Gaussian.Suppose that one wants to minimize:

J = E 1Tt0+Tt0

[xTQx + uTRu] dtwhere T .

This is called the Linear Quadratic Gaussian (LQG) problem.It turns out that the optimal control in this sense is given by:

u = Kx

where

K is the gain obtained by considering the LQR problem.

x is the state estimate obtained from using the Kalman Filter.

From the separation theorem, we know that the observer estimated state feedback generates astable controller such that the closed loop system has eigenvalues given by the eigenvalues of theobserver error and the eigenvalues of the state feedback controller. The LQG result shows thatKalman Filter and LQ generates an Optimal Controller in the stochastic sense.

In fact, the LQR controller which assumes that the state x(t) is directly available is optimal inthe stochastic setting.

The LQG controller is optimal over ALL causal, linear and nonlinear controllers!

A drawback of LQG controller is that although it consists of a LQR and Kalman filter, bothhave good robustness properties, the LQG controller may not be robust. A design procedure called

Loop Transfer Recovery (LTR) can sometimes be applied. The main idea of LTR is: Make Kalman filter look like state feedback

This is achieved by making the Kalman filter rely on output injection rather than open loopestimation.

This in turn is achieved by using large process noise (large Q) or small measurement noise(small R) models.

For details, see (Anderson and Moore, 1990).

Documents

Chapter 7 Kalman Filter