LINEAR QUADRATIC ESTIMATOR - University of …mocha-java.uccs.edu/ECE5530/ECE5530-CH05.pdf ·...

ECE5530: Multivariable Control Systems II. 5–1

LINEAR QUADRATIC ESTIMATOR

5.1: Setting up the optimal state estimator

■ We now start to put the pieces together

! Least-squares estimation and dynamic systems ➠ Observer.

■ Discrete case first, then convert to continuous case.

■ The filtering aspect will become apparent as we progress.

MODEL: Same as before

xkC1 D Adxk C wk

yk D Cdxk C vk:

■ Periodic measurements of system behavior available, but corrupted

by noise vk ➠ sensor noise.

■ Process noise wk and sensor noise vk are white. (If not, need to use

shaping stuff from before and augment system vector x).

ASSUME:

EŒwk! D 0I EŒwkwTj ! D „w".k " j /

EŒvk! D 0I EŒvkvTj ! D „v".k " j /

EŒwkvTj ! D 0 8 k; j

EŒx0! D Ox0:

Lecture notes prepared by Dr. Gregory L. Plett. Copyright © 2016, 2010, 2008, 2006, 2004, 2002, 2001, 1999, Gregory L. Plett

ECE5530, LINEAR QUADRATIC ESTIMATOR 5–2

GOAL: Use these periodic measurements of the system output to

develop an optimal estimate of the state xk ! Oxk, and develop a

measure of confidence in this estimate.

TERMINOLOGY:

replacements

k " 1 k k C 1 k C 2

."/."/."/."/ .C/.C/.C/ .C/

■ State estimate is “ Oxk”. Two estimates possible and used:

! One that uses all PRIOR information

# # # yk"2; yk"1 ➠ Ox"k :

! One that uses all POSSIBLE information

# # # yk"2; yk"1; yk

➠ OxCk :

■ Let us analyze one segment

k k C 1

Ox"k OxC

k Ox"kC1 OxC

kC1

STEP I: Measurement update: Use yk to go from Ox"k to OxC

k .

STEP II: Time update: (propagation) Use the (assumed known) plant

model to propagate statistics for OxCk to Ox"

kC1.

■ At each step, we have an estimation error:

QxCk

„ƒ‚…

error

D xk„ƒ‚…

truth

" OxCk

„ƒ‚…

estimate

;

with associated error covariance matrix (assuming/enforcing

EŒ QxCk ! D 0).

„Cx;k D E

!

QxCk . QxC

k /T"

D E!

.xk " OxCk /.xk " OxC

k /T"

:



■ Want „Cx;k to be “small”.

STEP I: Assume we have the initial values . Ox"k ; „"

x;k/.

■ Now, use new measurement to find optimal estimates of . OxCk ; „C

x;k/ ➠

Key part of recursive solution.

■ Enforce the constraint that the solution to the update problem

Ox"k ! OxC

k is a linear equation of the form

OxCk D L0

k Ox"k C Lkyk:

■ Updated estimate is a linear combination of previous estimate and

most recent measurement.

■ Lk, L0k time varying matrices; different; yet to be determined.

■ We are looking for the best LINEAR filter.

■ If wk, vk are also Gaussian, it will also turn out this is the BEST filter

possible. No nonlinear filter can do better!



5.2: Deriving the optimal state estimator

STEP I: Solution

■ Assume initial estimate unbiased ➠ EŒ Qx"k ! D 0.

■ Constrain solution so that EŒ QxCk ! D 0 also.

EŒxk " OxCk ! D EŒxk " L0

k Ox"k " Lkyk!

D EŒxk " L0k Ox"

k " Lkyk! C

0‚ …„ ƒ

EŒL0kxk! " EŒL0

kxk!

EŒ QxCk ! D EŒ.I " L0

k " LkCd/xk C L0kxk " L0

k Ox"k

„ ƒ‚ …

L0k Qx"

k

!

D EŒ.I " L0k " LkCd/xk!:

■ To ensure that this estimate is unbiased for all xk, must have

L0k C LkCd D I . Therefore

OxCk D L0

k Ox"k C Lkyk

D .I " LkCd/ Ox"k C Lkyk

D Ox"k C Lk

!

yk " Cd Ox"k

"

„ ƒ‚ …

Innovation

where the innovation is the error in our estimate, or “what is new” in

this measurement.

■ Exactly the same form as we have seen before in the updates we

have looked at! Need a good way to choose “blending factor” Lk.

Estimate covariance

■ Since the estimation error has zero mean, the covariance matrix is

„x;k D EŒ Qxk QxTk !.



■ Need to look at „"x;k and „C

x;k to see how covariance is changed by a

measurement.

■ Then, pick Lk for “optimal blending”.

Optimization

■ We will choose Lk to minimize the sum of the squares of the

estimation error vector:

Jk D EŒ#

QxCk

$T QxC

k !:

■ Equivalent to minimizing the “length” squared of the error vector,

which seems like an appealing objective.

■ Note that we can rewrite the cost in terms of the error covariance

matrix

Jk D traceŒ„Cx;k!

(recall xT x D traceŒxxT !).

Covariance update

■ First, we must write „Cx;k in terms of „"

x;k and the measurement noise.

■ Know OxCk D .I " LkCd/ Ox"

k C Lkyk. Therefore, estimation error

dynamics given by

QxCk D xk " OxC

k

D xk " .I " LkCd/ Ox"k " Lk.Cdxk C vk/

D .I " LkCd/ Qx"k " Lkvk:



■ Now, form

„Cx;k D EŒ QxC

k

#

QxCk

$T !

D EŒf.I " LkCd/ Qx"k " Lkvkgf.I " LkCd/ Qx"

k " LkvkgT !:

■ As before, three terms,

1. EŒ.I " LkCd/ Qx"k

#

Qx"k

$T .I " LkCd/T ! D .I " LkCd/„"

x;k.I " LkCd/T .

2. EŒLkvkvTk LT

k ! D Lk„vLTk .

3. EŒ Qx"k vT

k ! D 0 since prior estimate error is uncorrelated with the

measurement noise at this time step.

■ Therefore, for any Lk,

„Cx;k D .I " LkCd/„"

x;k.I " LkCd/T C Lk„vLTk :

■ These two equations tell us how to update Qxk and „x;k, given any Lk.

■ Now, optimize to find best Lk.

■ The “trace” operator makes life quite easy since we have

d trace.AB/

dAD BT ; AB square

d trace.ACAT /

dAD 2AC; C symmetric:

■ FormdJk

dLkD 0 for optimal gain. Recall

Jk D EŒ QxTk

C QxCk ! D trace.„C

x;k/

D trace#

.I " LkCd/„"x;k.I " LkCd/T

$

C trace.Lk„vLTk /

■ Then,dJk

dLk

D "2„"x;kC T

d C 2Lk.Cd„"x;kC T

d C „v/ D 0



which gives

L$k D „"

x;kC Td

!

Cd„"x;kC T

d C „v

""1

which is the Kalman gain matrix—minimizing gain.

■ Can show that with this gain

„Cx;k D .I " L$

kCd/„"x;k

D

%

The optimized value of the updated

error covariance matrix.

&

■ Thus, in terms of the mean-square estimation error,

Lk D „"x;kC T

d

!

Cd„"x;kC T

d C „v

""1

is the optimal blending ratio ➠ Very similar form to the gain matrices

studied earlier.

(End of STEP I, measurement update).

STEP II: Time update (propagation).

■ Have developed update estimates at time “kC”.

■ Need to propagate to develop predictions of the best estimate at time

“.k C 1/"”.

■ Have already studied this case. Using known model and process

noise covariance,

Ox"kC1 D Ad OxC

k .C deterministic input/

„"x;kC1 D Ad„C

x;kATd C „w:



5.3: Kalman filter loop

■ We can combine the parts that make up this one segment

.#/"k ) .#/"

kC1 to form a recursion:

Prior estimates Ox"k ; „"

x;kUsually initialize with Ox0; „x;0

Compute Kalman gain

Lk D „"x;kC T

d

!

Cd „"x;kC T

d C „v

""1

Update estimate with yk

OxCk D Ox"

k C Lk

!

yk " Cd Ox"k

"

Update covariance„C

x;k D .I " LkCd /„"x;k

PropagateOx"kC1 D Ad OxC

k

„"x;kC1 D Ad „C

x;kATd C „w

ykk ! k C 1

■ “Simple” to perform on a digital computer.

EXAMPLE: Px.t/ D w.t/, "t D 1, Sw D 1.

■ Independent noisy measurements y.t/ D x.t/ C v.t/.

■ Standard deviation of measurement noise #v D 1=2.

■ Convert continuous to discrete

xk D Adxk"1 C wk"1; EŒwkwl ! D „w".k " l/:

! Find Ad from zero-order-hold equivalent: Ad D 1.

! Find „w using „w D

Z 1

0

e0Swe0 dt D 1.

! Measurement equation, yk D Cdxk C vk.

EŒvkvk! D „v D #2v D 1=4; Cd D 1:

! Assume that there is no initial uncertainty. Ox"0 D 0 and „"

x;0 D 0.



Time step 0:

■ L0 D „"x;0C

Td .Cd„"

x;0CTd C „v/"1 D 0.

■ OxC0 D Ox"

0 C L0.y0 " Cd Ox"0 / D Ox"

0 D 0.

■ „Cx;0 D .I " L0Cd/„"

x;0 D „"x;0 D 0.

■ Ox"1 D Ad OxC

0 D 0 and „"x;1 D Ad„C

x;0ATd C „w D 0 C 1 D 1.

Time step 1:

■ L1 D „"x;1C

Td .Cd„"

x;1CTd C „v/"1 D 1 # 1.1 # 1 # 1 C 1=4/"1 D 4=5.

■ OxC1 D Ox"

1 C L1.y1 " Cd Ox"1 / D 0 C .4=5/y1 D .4=5/y1.

■ „Cx;1 D .I " L1Cd/„"

x;1 D 1=5.

■ Ox"2 D Ad OxC

1 D .4=5/y1 and „"x;2 D Ad„C

x;1ATd C „w D .1=5/ C 1 D 6=5.

Time step 2:

■ L2 D „"x;2C

Td .Cd„"

x;2CTd C „v/"1 D .6=5/.6=5 C 1=4/"1 D 24=29.

■ OxC2 D Ox"

2 C L2.y2 " Cd Ox"2 / D .4=29/y1 C .24=29/y2.

■ „Cx;2 D 6=29 and so forth, hopefully to steady state.

„"x;0 D 0 „"

x;1 D 1 „"x;2 D 1:2 „"

x;3 D 1:207

# % # % # % # %

„Cx;0 D 0 „C

x;1 D 0:2 „Cx;2 D 0:207 „C

x;3 D 0:2071

■ Covariance increases during propagation and is then reduced by

each measurement.

■ Covariance still oscillates at steady state between „"x;ss and „C

x;ss.



0 5 10 15 20 25 30 35 40−2

0

2

4

6

8true estimateerror

Iteration

Sta

teKalman filter in action

0 5 10 15 20 25 30 35 400

0.2

0.4

0.6

0.8

1

1.2

1.4

Iteration

Cova

riance

Error covariance „"x;k and „C

x;k

„!x

„Cx

% Kalman Filter Example, modified (slightly) from Claire Tomlin's code

a=1; c=1; % System dynamics

Xi_v=1/4; Xi_w=1;% White noise covariance

randn('seed',sum(40*clock)); % Init rand # generator

w = sqrt(Xi_w)*randn(40,1); % Generate disturbance

v = sqrt(Xi_v)*randn(40,1);% Generate noise

x(1) = 0;% init state for simulation

xhatminus(1) = 0;% init prior state info

Xi_x_minus(1) = 0;% init prior error covariance

for k = 1:40,

% Compute Kalman gain

L(k) = Xi_x_minus(k)*c'*inv(c*Xi_x_minus(k)*c'+Xi_v);

x(k+1) = a*x(k)+w(k); % compute noisy state dynamic

y(k) = c*x(k)+v(k); % simulate noisy measurements

% Measurement update with y(k)

xhatplus(k) = xhatminus(k)+L(k)*(y(k)-c*xhatminus(k));

% Update covariance

Xi_x_plus(k) = (1-L(k)*c)*Xi_x_minus(k);

% Propagate

xhatminus(k+1) = a*xhatplus(k);

Xi_x_minus(k+1) = a*Xi_x_plus(k)*a'+Xi_w;

end;

figure(1); clf; k=0:39;

plot(k,x(1:40),'-',k,xhatplus(1:40),'--',0:39,x(1:40)-xhatplus,'-.');

legend('true','estimate','error'); title('Kalman filter in action');



xlabel('Iteration'); ylabel('State'); grid;

figure(2); clf;

plot(k,Xi_x_minus(1:40),'o',k,Xi_x_plus(1:40),'+');

grid; legend('Xi_x^{-}','Xi_x^{+}',0);

title('Error covariance before (o) and after ^{+} meas. update');

xlabel('Iteration'); ylabel('Covariance');

Discrete Kalman filter versus RLS

KALMAN: First the covariance matrix:

„Cx;k D .I " LkCd/„"

x;k

D „"x;k " „"

x;kC Td

!

Cd„"x;kC T

d C „v

""1Cd„"

x;k:

■ Now, use matrix inversion lemma (proof in book appendix)!

NA"1 C NBT NC "1 NB""1

D NA " NA NBT!

NB NA NBT C NC""1 NB NA

where we substitute

NA D „"x;k

NB D CdNC D „v:

■ Thenh

„Cx;k

i

"1 D!

„"x;k

""1 C C T

d „"1v Cd :

■ For the gain, use the following form of the matrix inversion lemma:!

NA"1 C NBT NC "1 NB""1 NBT NC "1 D NA NBT

!NB NA NBT C NC

""1

with the same substitutions (proof in notes appendix).

■ So:

Lk D „"x;kC T

d

!

Cd„"x;kC T

d C „v

""1

D!#

„"x;k

$"1 C C T

d „"1v Cd

""1C T

d „"1v

D „Cx;kC T

d „"1v :



RLS: Covariance update was

Q"1kC1 D Q"1

k C H TkC1R

"1kC1HkC1:

■ The gain was

gain D QkC1HTkC1R

"1kC1:

■ Therefore, measurement update step of Kalman filter is the same as

the recursive form of the maximum-likelihood estimate (when v in

y D Hx C v is Gaussian, OxML is linear).

! Therefore, if wk is Gaussian, a linear filter is the best filter possible!

No nonlinear filter can do better!

■ Time update is additional step necessary when estimating a moving

(dynamic) state vector.



5.4: Continuous-time filters

GOAL: Develop an estimator Ox.t/ that is a linear function of

measurements y.$/, 0 % $ % t , and minimizes the function

E!

.x.t/ " Ox.t//T .x.t/ " Ox.t//"

:

ASSUMED: Dynamics of linear system are

Px.t/ D Ax.t/ C Bww.t/

y.t/ D Cyx.t/ C v.t/

and EŒw.t/! D EŒv.t/! D 0; EŒw.t1/w.t2/T ! D Swı.t1 " t2/,

EŒv.t1/v.t2/T ! D Svı.t1 " t2/, EŒw.t1/v.t2/! D 0, Sw > 0, Sv > 0, Ox.0/ and

„x.0/ given.

APPROACH: As before, analyze discrete case as "t ! 0.

■ To proceed, we must relate the discrete and continuous sensor

noises. (We already saw how to relate process noises).

■ One approach: Use discrete noise ("t small)

vk D1

"t

Z "t

0

v.t/ dt .average value/:

■ Then,

„v D EŒvkvTk ! D

1

"t 2

Z "t

0

Z "t

0

EŒv.%/v.$/T ! d% d$

D1

"t 2# "t # Sv D

Sv

"t:

■ As "t ! 0, the discrete noise tends to infinite pulse covariance with

weight Sv, similar to Svı.t/.



Conversion

■ Recall

! „"x;kC1 D Ad„C

x;kATd C „w.

! „Cx;k D .I " LkCd/„"

x;k.

■ With malice aforethought, consider

Lk

"tD

„"x;kC T

d

"t

!

Cd„"x;kC T

d C „v

""1

D „"x;kC T

d

!

Cd„"x;kC T

d "t C „v"t""1

:

■ As "t ! 0, if „"x;k finite,

Cd„"x;kC T

d "t ! 0

„v"t ! Sv

„"x;k ! „x.t/

Cd ! Cy

9

>>>>=

>>>>;

as "t ! 0Lk

"t! L.t/ D „x.t/C T

y S"1v :

■ Already looked at propagation case

„"x;kC1 D Ad„C

x;kATd C „w

& .I C A"t/„Cx;k.I C A"t/T C BwSwBT

w "t

D „Cx;k C "t

h

A„Cx;k C „C

x;kAT C BwSwBTw

i

C o."t 2/;

but „Cx;k D „"

x;k " LkCd„"x;k . . . combine. . .

„"x;kC1 D „"

x;k " LkCd„"x;k

C"th

A„"x;k " ALkCd„"

x;k C „"x;kAT

"LkCd„"x;kAT C BwSwBT

w

i

Co."t 2/:



■ Rearrange. . .

„"x;kC1 " „"

x;k

"tD

"Lk

"tCd„"

x;k C A„"x;k C „"

x;kAT C BwSwBTw

"ALkCd„"x;k " LkCd„"

x;kAT C o."t /:

■ As "t ! 0, LHS ! P„x.t/, Cd ! Cy,Lk

"t! L.t/, „"

x;k ! „x.t/ and

ALkCd„"x;k ! 0.

Resulting expression

P„x.t/ D

Riccati for measurement update‚ …„ ƒ

A„x.t/ C „x.t/AT C BwSwBTw

„ ƒ‚ …

Lyapunov for propagation

" „x.t/C Ty S"1

v Cy„x.t/„ ƒ‚ …

'0

:

■ Matrix RICCATI equation for error covariance.

■ Equation includes the following impact on P„x.t/:

! A„x.t/ C „x.t/AT➠ Homogeneous part.

! BwSwBTw ➠ Increase due to process noise.

! „x.t/C Ty S"1

v Cy„x.t/ ➠ Decrease due to measurements.

Culture

Count Jacopo Francesco Riccati (1676–1754) was an Italian savant who wrote on mathematics,

physics, and philosophy. He was chiefly responsible for introducing the ideas of Newton to Italy.

At one point he was offered the presidency of the St. Petersburg Academy of Sciences but

understandably he preferred the leisure and comfort of his aristocratic life in Italy to

administrative responsibilities in Russia. Though widely known in scientific circles of his time,

he now survives only through the differential equation bearing his name. Even this was an

accident of history, for Riccati merely discussed special cases of this equation without offering

any solutions, and most of these special cases were successfully treated by various members

of the Bernoulli family. The details of this complex story can be found in G. N. Watson “A

treatise on the Theory of Bessel Functions.” 2d ed., pp. 1–3, Cambridge, London, 1944. The

special Riccati equation Py C by2 D cxm is known to be solvable in finite terms if and only if the

exponent m is "2 or of the form "4k=.2k C 1/ for some integer k.



(Taken from “Differential Equations with Applications and Historical Notes” by George F.

Simmons).

State-Estimate Update

■ Recall

1. Ox"k D Ad OxC

k"1.

2. OxCk D Ox"

k C Lk.yk " Cd Ox"k /.

■ Substitute (1) into (2)

OxCk D Ad OxC

k"1 C Lk.yk " CdAd OxCk"1/:

■ Substitute:

OxCk D .I C A"t/ OxC

k"1 C Lk

'

yk " Cd.I C A"t/ Oxk"1

(

:

■ So,

OxCk " OxC

k"1

"tD A OxC

k"1 CLk

"t

'

yk " Cd OxCk"1 " CdA"t OxC

k"1

(

:

■ Then,

POx.t/ D A Ox.t/ C L.t/!

y.t/ " Cy Ox.t/"

L.t/ D „x.t/C Ty S"1

v

P„x.t/ D A„x.t/ C „x.t/AT C BwSwBTw " „x.t/C T

y S"1v Cy„x.t/:

■ This is called a “Kalman–Bucy” filter, or a continuous-time Kalman

filter/estimator.

EXAMPLE: Estimate the value of a constant given continuous

measurements of x corrupted by Gaussian white noise.

Px D 0 .trivial dynamics of constant/



y D x C v;

where v ( N .0; Sv/. Then A D Bw D Sw D 0.

■ The Riccati equation becomes: P„x.t/ D "„2x.t/=Sv.

■ Integrate:Z „x.t/

„x;0

d„x.$/

„2x.$/

D "1

Sv

Z t

0

d$

"„"1x .$/

ˇˇ„x.t/

„x;0D "t=Sv

1

„x;0"

1

„x.t/D "t=Sv

1

„x;0C

t

SvD

1

„x.t/

„x.t/ D„x;0Sv

Sv C „x;0tI

L.t/ D „x.t/=Sv D„x;0

Sv C „x;0t:

■ As t ! 1, L.t/ ! 0, so that Ox.t/ ! constant.

POx.t/ D„x;0

Sv C „x;0t„ ƒ‚ …

L.t/

Œy.t/ " Ox.t/! ! 0:



5.5: Steady-state Kalman filters

■ Several examples have begun to demonstrate that the filter gains can

have a transient and then settle to constant values.

Steady stateL.t/

■ Since the estimator will typically be running for a long time, most of

the operations will occur with the filter in the steady-state region.

■ Therefore, recursive/differential equations do not need to be solved!

Steady-state solution

■ The Kalman gain depends on state covariance, which for a discrete-

time system has two steady-state solutions: „"x;ss and „C

x;ss. Then,

Lss D „Cx;ssC

Td „"1

v :

■ Consider the filter loop as k ! 1

„"x;ss D Ad„C

x;ssATd C „w

„Cx;ss D „"

x;ss " „"x;ssC

Td

!

Cd„"x;ssC

Td C „v

""1Cd„"

x;ss:

■ Combine these two to get

„"x;ss D „w C Ad„"

x;ssATd "

Ad„"x;ssC

Td

!

Cd„"x;ssC

Td C „v

""1Cd„"

x;ssATd :

■ One matrix equation, one

matrix unknown ➠ dlqe.m

k k C 1 k C 2 k C 3

„x



EXAMPLE: xkC1 D xk C wk. yk D xk C vk. Steady-state covariance?

■ Let EŒwk! D EŒvk! D 0, EŒwkwl ! D 1".k " l/, EŒvkvl ! D 2".k " l/

■ EŒx0! D Ox0 and EŒ.x0 " Ox0/.x0 " Ox0/! D „x;0.

■ Notice Ad D 1, Cd D 1, „w D 1 and „v D 2.'

„Cx;k

(

"1 D#

„"x;k

$"1 C C T

d „"1v Cd

„Cx;k D

11

„"x;k

C 12

D2„"

x;k

2 C „"x;k

:

„"x;kC1 D Ad„C

x;kATd C „w D „C

x;k C 1 D2„"

x;k

2 C „"x;k

C 1

„"x;ss D

2„"x;ss

2 C „"x;ss

C 1

which gives us „"x;ss D 2 and „C

x;ss D 1.

„"x;0 D 1 „"

x;1 D 3 „"x;2 D 11=5 „"

x;3 D 43=21

# % # % # % # %

„Cx;0 D 2 „C

x;1 D 6=5 „Cx;2 D 22=21 „C

x;3 D 86=85

Steady-state in continuous time

■ The same kind of result often holds for LTI continuous-time systems.

■ The steady-state Kalman gain is associated with P„x.t/ ! 0, giving

the algebraic Riccati equation (ARE)

A„x C „xAT C BwSwBTw " „xC T

y S"1v Cy„x D 0

and

L D „xC Ty S"1

v :



■ Same as time-varying case, but with constant „x and L. care.m

■ Tradeoff between sensor and process noise now explicit:

L D „xC Ty S"1

v .

! If uncertain about estimate („x large), then innovation .y " Cy Ox/

weighted heavily (big L).

! If Sv small (measurements accurate) then new measurements

highly weighted (big L).

■ We can think of „xC Ty S"1

v as analogous to SNR.

NOTE: Continuous case requires Sv > 0 (S"1v explicit) but discrete does

not („"1v can be avoided).

Stability of steady-state Kalman filter

■ Steady-state gain is much easier to compute/use than time-varying

solution.

■ Small performance reduction if we use steady-state value for ALL

time since transient region is quite short (relatively). But, what can we

say about stability?

CTS SYSTEM:

Px.t/ D Ax.t/ C Bww.t/

y.t/ D Cyx.t/ C v.t/

where w ( N .0; Sw/ and v ( N .0; Sv/; w; v white and independent.

ASSUME:

1. Sv > 0, Sw > 0.



2. All plant dynamics constant

3. ŒA; Cy! detectable (unstable modes observable)

4. ŒA; Bw! stabilizable (unstable modes controllable)

■ Time-varying Kalman gains for this time-invariant system satisfy

P„x D A„x C „xAT C BwSwBTw " „xC T

y S"1v Cy„x

L D „xC Ty S"1

v ;

„x;0 given.

■ With the above assumptions, „x.t/ ! „x;ss regardless of „x;0 as

t ! 1, where „x;ss solves

0 D A„x;ss C „x;ssAT C BwSwBT

w " „x;ssCTy S"1

v Cy„x;ss:

■ Stabilizable/detectable implies „x;ss ' 0 (unique)

■ „x;ss > 0 iff ŒA; Bw! controllable.

■ If „x;ss exists, the steady-state optimal estimator

POx D A Ox C Lss.y.t/ " Cy Ox.t//

Lss D „x;ssCTy S"1

v

is asymptotically stable if (1)–(4) hold.

Big conclusion

1. Time-varying Kalman filter gains provide PERFORMANCE OPTIMAL

filter design, but,

2. Under MILD conditions, we can show that the closed-loop estimation

error dynamics are stable when we use the steady-state gains for all

time ➠ BIG computational savings.



5.6: Frequency-domain interpretation

■ We are interested in finding the estimation-error dynamics. We start

with the plant dynamics:

Px D Ax C Buu C Bww

y D Cyx C v

where w ( N .0; Sw/, BwSwBTw > 0, v ( N .0; Sv/, Sv > 0; w; v white

and independent, ŒA; Cy! observable.

■ Strong form of assumptions.

■ Steady-state Kalman filter:

POx D A Ox C Buu C L.y " Cy Ox/:

■ This gives estimation error dynamics: Qx D x " Ox

PQx D Px " POx D Ax C Bww "!

.A " LCy/ Ox C L.Cyx C v/"

D .A " LCy/ Qx C Bww " Lv:

■ Filter stability governed by eigenvalues of .A " LCy/.

■ Equation makes explicit the tradeoff between:

! Speed of estimator error decay (L big, eig.A " LCy/ far in LHP)

! Susceptibility of estimator error to being corrupted by sensor

noise. (L small so Lv small).

■ Kalman filter selects the optimal balance between these two goals.

■ Have POx D A Ox C L.y " Cy Ox/ . . . (steady-state filter)



■ Laplace transform both sides. . . (assume scalar state for now)

OX.s/

Y.s/D

L

s " A C LCyD

LLCy"A

sLCy"A

C 1:

■ Pole at s D ".LCy " A/ and dc-gain of L=.LCy " A/.

■ This is the transfer function of the filter applied to the measurements

to form the estimate Ox (low-pass).

■ Increasing L pushes the filter transfer function up and out.

Increasing L

f

ˇˇˇ OX=Y

ˇˇˇ

■ Eventually, estimate would be too corrupted by the noise in the

measurements.

■ Note that balancing the sensor-noise impact done with respect to the

process noise .Bww/.

■ It turns out that the ratio Sw=Sv plays a key role in the selection of L.

EXAMPLE: System driven by noise

Px D w

y D x C v

where EŒw! D EŒv! D 0 and w and v independent;

EŒw.t/w.t C $/! D Swı.$/ and EŒv.t/v.t C $/! D Svı.$/.

■ EŒx.0/! D 0, EŒx.0/2! D „x.0/.



■ The optimal filter is

POx D„x.t/

Sv

.y " Ox/

„x.t/ Dp

SwSv1 C be"2˛t

1 " be"2˛t;

where ˛ Dp

Sw=Sv and b D .„x.0/ "p

SwSv/=.„x.0/ Cp

SwSv/.

■ Convergence speed of „x.t/ determined by Sw=Sv.

■ Steady-state:

„x.t/ ! „x;ss Dp

SwSv

Lss D„x;ss

SvD

p

Sw=Sv:

Therefore, steady-state filter:

POx D "

s

Sw

SvOx C

s

Sw

Svy;

and the closed-loop pole location determined by Sw=Sv.

■ If Sw=Sv small, sensors are relatively noisy.

■ If Sw=Sv large, sensors relatively clean.

More “culture”

■ There are three main types of Kalman filter:

1. We have concentrated on the “filtering problem”.

■ Use data up to and

including the current time

to provide an estimate for

the current time.

DATA

Current time

Ox.t/



2. The “smoothing problem” (offline, post-analysis)




a past time.

DATA

Current timePrior time

Ox.to/

3. The “prediction problem”




a future time.

DATA

Current time Futuretime

Ox.tf /

■ All three filters have application to control theory, but #2 and #3 are

used more often by signal-processing people.



5.7: Symmetric root locus

■ Can extend this analysis to more complex cases and determine

where all the closed-loop poles end up.

■ Map pole-loci of A " LssCy for valid Lss.

■ Model of plant

Px D Ax C Buu C Bww; x 2 Rn

y D Cyx C v:

■ For now, .u; w; y; v/ scalars. v; w white. w ( N .0; Sw/, v ( N .0; Sv/.

ŒA; Bw! stabilizable; ŒA; Cy! detectable.

■ Define scalar transfer function

Gyw.s/ D Cy.sI " A/"1Bw DN.s/

D.s/: : : .process noise to clean sensor/:

■ Steady-state optimal observer poles are the LHP zeros of

."1/nD.s/D."s/

%

1 CSw

Sv

Gyw.s/Gyw."s/

&

D 0

or

D.s/D."s/ ˙Sw

Sv

N.s/N."s/ D 0

where ˙ is chosen so no poles on imaginary axis.

■ Symmetric root locus in the ratio Sw=Sv.

■ Changing Sw=Sv uniquely determines the location of the estimator

poles.

1. As Sw=Sv ! 0 the n poles approach (where pi D &i .A/)

Opi D

(

pi; if R.pi/ % 0 : : : stableI

"pi; if R.pi/ > 0 : : : unstable:



■ As sensors become relatively noisy, filter gains very low.

Essentially open-loop, but must be stable.

2. As Sw=Sv ! 1, —low sensor noise, high bandwidth—assume N.s/

has m roots í

(a) m of the n poles approach

Opi D

(

í ; if R.í/ % 0I

"í ; if R.í/ > 0;

the finite zeros of N.s/=D.s/.

(b) Remaining n " m poles asymptotically approach straight lines that

intersect at origin with angles

˙l'

n " ml D 0; 1; : : : .n " m " 1/=2 n " m odd

˙.l C 1=2/'

n " ml D 0; 1; : : : .n " m/=2 " 1 n " m even;

a BUTTERWORTH configuration.

Butterworth examples

n " m D 1 n " m D 2 n " m D 3

R.s/ R.s/R.s/

I.s/ I.s/I.s/

45ı 72ı

■ Remember, these are only asymptotic trajectories for the poles. We

can use standard root-locus arguments to find out where the rest of

the poles end up at intermediate values of Sw=Sv.

■ Note the key role played by the ratio Sw=Sv.



EXAMPLE: Undamped harmonic oscillator."

Px1

Px2

#

D

"

0 1

"!2o 0

# "

x1

x2

#

C

"

0

1

#

w

and w ( N .0; 1/, v ( N .0; Sv/.

y D x2 C v:

■ Location of optimal poles?

Gyw.s/ Dh

0 1i

( "

s 0

0 s

#

"

"

0 1

"!2o 0

#) "1 "

0

1

#

Ds

s2 C !2o

DN.s/

D.s/:

■ Need to find stable solutions of

.s2 C !2o/2 "

1

Sv.s2/ D 0

or

1 "1

Sv

s2

.s2 C !2o/2

D 0:

Sv ! 1

Sv ! 0 Sv ! 0

■ As Sv ! 0 (low sensor noise), one observer pole tends to the origin

(shielded from process noise by system zero at origin). The other

pole ! "1, which essentially results in differentiation of the output to

determine the state.

■ As Sv ! 1 (high sensor noise), poles revert to the open-loop poles

(open-loop observer).



5.8: Relationship between LQE and LQR

■ For LQR, we solve the Riccati equation

" PP .t/ D P.t/A C AT P.t/ C Q " P.t/BuR"1BTu P.t/:

P.t/ is the linear function relating the costate to the state

(&.t/ D P.t/x.t/), Q is the state-weighting penalty and R is the

control-weighting penalty.

■ For LQE, we found that we need to solve the Riccati equation

P„x.t/ D „x.t/AT C A„x.t/ C BwSwBTw " „x.t/C T

y S"1v Cy„x.t/:

„x.t/ is the error covariance matrix, Sw is the process noise spectral

density and Sv is the measurement noise spectral density.

■ The LQR and LQE Riccati equations are duals. First note that LQR is

solved backward in time. If it were to be solved forward in time we

would have

PP .t/ D P.t/A C AT P.t/ C Q " P.t/BuR"1BTu P.t/:

■ Then, replace A , AT , Bu , C Ty , K , L, Q , BwSwBT

w , R , Sv

and P , „x to notice the duality.

■ Duality is explored in further detail in the text to show that LQE is H2

optimal estimation when cast in a certain framework just as LQR is

H2 optimal control in a similar specific framework.

Solving for the gain matrix L.t/

■ Here, we use duality to formulate an estimator Hamiltonian matrix,

which may be used to solve the estimator Riccati equation.



■ Recall that the LQR Riccati equation could be solved by dividing P.t/

into two parts. We do the same thing for „x.t/

„x.t/ D T2.t/T"11 .t/:

■ Substitute this result into the LQE Riccati equation

dT2.t/T"11 .t/

dtD T2.t/T

"11 .t/AT C AT2.t/T

"11 .t/ C BwSwBT

w

"T2.t/T"11 .t/C T

y S"1v CyT2.t/T

"11 .t/:

■ Note that (the first from the product rule; the second from

dŒT1.t/T"11 .t/!=dt D dI=dt D 0.)

dT2.t/T"11 .t/

dtD

dT2.t/

dtT "1

1 .t/ C T2.t/dŒT "1

1 .t/!

dt;

anddŒT "1

1 .t/!

dtD "T "1

1 .t/ PT1.t/T"11 .t/:

■ Then

PT2.t/T"11 .t/ " T2.t/T

"11 .t/ PT1.t/T

"11 .t/

D T2.t/T"11 .t/AT C AT2.t/T

"11 .t/ C BwSwBT

w

" T2.t/T"11 .t/C T

y SvCyT2.t/T"11 .t/:

■ Right-multiply by T1.t/ and regroup

PT2.t/ " T2.t/T"11 .t/Œ PT1.t/! D BwSwBT

w T1.t/ C AT2.t/

"T2.t/T"11 .t/

h

"AT T1.t/ C C Ty S"1

v CyT2.t/i

:

■ Comparing the two sides of the equation, a solution is obtained when

T1.t/ and T2.t/ are solutions to the linear differential equations:

PT1.t/ D "AT T1.t/ C C Ty S"1

v CyT2.t/

PT2.t/ D BwSwBTw T1.t/ C AT2.t/:



■ Combining yields the Hamiltonian system for the Kalman filter"

PT1.t/

PT2.t/

#

D

"

"AT C Ty S"1

v Cy

BwSwBTw A

# "

T1.t/

T2.t/

#

D Y

"

T1.t/

T2.t/

#

■ The matrix Y is termed the Hamiltonian of the Kalman filter.

■ The initial conditions of the Hamiltonian system must satisfy

„x.0/ D T2.0/T "11 .0/;

so T1.0/ and T2.0/ are not uniquely specified. One choice is"

T1.0/

T2.0/

#

D

"

I

„x.0/

#

:

■ The Hamiltonian system is then solved as"

T1.t/

T2.t/

#

D eYt

"

I

„x.0/

#

D

"

ˆ11.t/ ˆ12.t/

ˆ21.t/ ˆ22.t/

# "

I

„x.0/

#

or

„x.t/ D fˆ21.t/ C ˆ22.t/„x.0/g fˆ11.t/ C ˆ12.t/„x.0/g"1

and the Kalman gain is

L.t/ D fˆ21.t/ C ˆ22.t/„x.0/g fˆ11.t/ C ˆ12.t/„x.0/g"1 C Ty S"1

v :

The steady-state Kalman gain

■ We can also use the Kalman Hamiltonian matrix to find the

steady-state gain matrix Lss much the same way we used the LQR

Hamiltonian matrix to find the steady-state matrix Kss.

■ The eigenequation of the Kalman Hamiltonian is"

"AT C Ty S"1

v Cy

BwSwBTw A

#

‰ D ‰ƒ:



■ Where ƒ is a diagonal matrix containing the eigenvalues of the

Hamiltonian (the first nx of which are in the left-half plane) and ‰ is

the matrix of eigenvectors. We can write ‰ as

‰ D

"

‰11 ‰12

‰21 ‰22

#

:

■ Using properties of the matrix exponential,"

T1.t/

T2.t/

#

D eYt

"

I

„x.0/

#

D e‰ƒ‰"1t

"

I

„x.0/

#

D ‰eƒt‰"1

"

I

„x.0/

#

D

"

‰11 ‰12

‰21 ‰22

# "

e"&t 0

0 e&t

#

‰"1

"

I

„x.0/

#

D

"

‰11e"&t ‰12e

&t

‰21e"&t ‰22e

&t

# "

.matrix top/

.matrix bottom/

#

:

■ As t ! 1, e"&t ! 0 and"

T1.t/

T2.t/

#

!

"

‰12

‰22

#

e&t .matrix bottom/:

■ The steady-state Riccati solution is then

„x;ss D T2.1/T "11 .1/

D ‰22.‰12/"1:

and the steady-state gain matrix is

Lss D ‰22.‰12/"1C T

y S"1v :

■ Note that the formula in the book is incorrect.



Appendix: Proof of matrix inversion lemma, form 2

■ Want to show!

NA"1 C NBT NC "1 NB""1 NBT NC "1 D NA NBT

!NB NA NBT C NC

""1:

■ Multiply on left by!

NA"1 C NBT NC "1 NB"

NBT NC "1 D!

NA"1 C NBT NC "1 NB"

NA NBT!

NB NA NBT C NC""1

D NBT!

NB NA NBT C NC""1

C NBT NC "1 NB NA NBT!

NB NA NBT C NC""1

D NBT˚

I C NC "1 NB NA NBT) !

NB NA NBT C NC""1

D NBT NC "1˚

NC C NB NA NBT) !

NB NA NBT C NC""1

D NBT NC "1:


LINEAR QUADRATIC ESTIMATOR - University of …mocha-java.uccs.edu/ECE5530/ECE5530-CH05.pdf ·...

Documents

Transcript of LINEAR QUADRATIC ESTIMATOR - University of …mocha-java.uccs.edu/ECE5530/ECE5530-CH05.pdf ·...