Robotics Research Laboratory 1 Chapter 7 Multivariable and Optimal Control.

Robotics Research Labo-ratory

1

Chapter 7

Multivariable and Optimal Control


2

Time-Varying Optimal Control- deterministic systems

0 1 12

2

( ) ( ) ( ) time-varying gain

Notes : ( ) ( ) constant control gain in pole

So

placement

0, 0, 0 (non-negative definite matrix)

0 (posit

lution

iv

u k K k x k

u k Kx k

Q Q Q

Q

e definite matrix)

1

1 12 2 00

- a quadratic form

1 1 minimize ( ) ( ) 2 ( ) ( )

Cost function (performance ind

( ) ( ) ( ) ( )2 2

ex)

Constra

NT T T T

k

J x k Q x k x k Q u k u k Q u k x N Q x N

subject to given system ( 1) ( ) (

int

)x k Φx k Γu k


3

1 20

1 20

1 12 2

Cost functions

1 minimize ( ) ( ) ( ) ( )

2

1 minimize ( ) ( ) ( ) ( )

2

Stochastic systems

1 minimize ( ) ( ) 2 ( ) ( ) ( ) ( )

2

NT T

k

T T

k

T T T

k

J x k Q x k u k Q u k


J E x k Q x k x k Q u k u k Q u k

0

1 12 2

1

or minimize ( ) ( ) 2 ( ) ( ) ( ) ( )

subject to given system ( 1) ( ) ( ) ( )

where is white noise with ( )

N

T T T

T

J E x k Q x k x k Q u k u k Q u k

x k Φx k Γu k v k

v E vv R


4

1 20

1Minimize

2

subject to ( 1) ( ) ( ) 0; 1,2,3, .... ,

NT T

k

J x Q x u Q u

x k Φx k Γu k k N

LQ problem (Linear Quadratic)–Finite time problem

Using Lagrange multipliers

'1 2

0

'

1 1( ) ( ) ( ) ( ) ( 1) ( 1) ( ) ( )

2 2

Find the minimum of with respect to ( ), ( ), ( )

NT T T

k

J x k Q x k u k Q u k λ k x k Φx k Γu k

J x k u k λ k

'

2

'

'

1

0 ( ) ( 1) 0 ; control eq.( )

0 ( 1) ( ) ( ) 0 ; state eq.( 1)

0 ( ) ( ) ( 1) 0 ; adjoint(costate) eq.( )

T T

T T T

Ju k Q λ k Γ

u k

Jx k Φx k Γu k

λ k

Jx k Q λ k λ k Φ

x k


5

2

1

1

( 1) ( ) ( ) (0) is given

(

( ) ( 1) (1)

(2) .

1) ( ) ( ) (0) is not kn(3) . ownT T

T

x k Φx k Γu k x

λ k

u k Q Γ λ k

Φ λ k Φ Q λx k

( ) ( ), ( )

.

Because has no effect on should be zero in order

to minimize

u N x N u N

J

2

1

( ) ( 1) 0

( 1) 0

(

( ) ( 1)

( ) ( 1

( ) ( )

)

4)

Two Points Boundary Value Problem (TPBVP)

T T

x k x k

λ

λ N

k λ k

u N Q λ

N

Q x

N

λ

N

Γ


6

2

1

2

1

2

Assume that (5)

( ) ( 1) ( 1)

( 1) ( ) ( )

( ) ( 1) ( 1) ( )

( 1) ( ) (6)

where (7)

(5

( ) ( ) ( )

) (3)

( 1)

( )

T

T

T T

T

T

Q u k Γ S k x k

Γ S k Φx k Γu k

u k Q Γ S k Γ Γ S k Φx k

R Γ S k Φx k

λ k

λ k S k x k

R Q Γ S k Γ

Φ

1

1

1

11

( 1) ( )

( ) ( ) ( 1) ( 1) ( )

( 1) ( ) ( ) ( )

( 1) ( ) ( 1) ( ) ( )

T

T

T

T T

λ k Q x k

S k x k Φ S k x k Q x k

Φ S k Φx k Γu k Q x k

Φ S k Φx k ΓR Γ S k Φx k Q x k


7

11

11

11

1

( ) ( 1) ( 1)

( ) ( 1) ( 1

( ) ( ) ( ) ( )

( )

discrete

) ( 1) 0

The above equation i

(

s c

)

Riccati

( 1) ( 1) ( 1)

( )

ealled t quahe

T T

T T

T T

T

T

S k Φ S k Φ ΓR Γ S k Φ Q

S k Φ S k Φ Φ S k ΓR Γ S k Φ Q

S k Φ S k Φ Φ S k ΓR Γ S k Φ Q

S

x k x k x k x

Q

k

N

x k

1

-1

1 2 3

1

1

2

( ) ( 1)

( 1) ( 1) - ( 1) ( 1),

.

or

where

( ) ( )

( )

Note: Jacopo Francesco Riccat

tion

( 1) ( 1

i (1676 - 1754)

( )

( )

)

( )

)

(

T

T

T TQ Γ S k Γ Γ

S k Φ M k Φ Q

M k S k S k ΓR Γ S k S N

S k Φu k x k

x k

dyq x q x y q

dx

k

Q

K

2( )x y


8

1

1

2

1

2

1

Procedure ( . 367)

1. ( ) and ( ) 0 since ( 1) 0

2. Let

3. Let ( ) ( ) ( ) ( ) ( )

4. Let ( 1) ( ) ( )

5. Store ( 1)

6. Let ( 1) ( )

7.

T T

T T

T

p

S N Q K N S N

k N

M k S k S k Γ Q Γ S k Γ Γ S k

K k Q Γ S k Γ Γ S k Φ

K k

S k Φ M k Φ Q

Let 1

8. Go to 3

k k


9

0 5 10 15 20 25 30 35 40 45 500

2

4

6

8

10

K t

heta

k

Control gains vs. time

Q2 = 1.0

Q2 = 0.1

Q2 = 0.01

0 5 10 15 20 25 30 35 40 45 500

1

2

3

4

5

K t

heta

dot

k

Q2 = 1.0

Q2 = 0.1

Q2 = 0.01


10

'1 2

0

1 2 10

2

Remark:

1( ) ( ) ( ) ( ) ( 1)( ( 1) ( ) ( ))

2

1 ( ) ( ) ( ) ( ) ( 1) ( 1) ( ( ) ( ) ) ( )

2

( ( ) ) ( )

1 ( ) ( ) ( 1) (

2

NT T T

k

NT T T T T

k

T

T T

J x k Q x k u k Q u k λ k x k Φx k Γu k

x k Q x k u k Q u k λ k x k λ k x k Q x k

u k Q u k

λ k x k λ k x

0

'

1)

1 (0) (0) ( 1) ( 1)

2

Since ( 1) 0,

1

1(0) (0) (0(0) (0)

2)

2

N

k

T T

T T

k

λ x λ

x

N

S x

x N

λ N

J J λ x


11

LQR (Linear Quadratic Regulator)-Infinite time problem

ARE(Algebraic Riccati Equation)– analytic solution is impossible in most cases.– numerical solution is required.

12

12 1

1 12 1 2

1

( 1) ( ) ( ) ( ) ( 1)

( ) ( ) ( )

( ) ( )

( 1) ( ) ( )

T

T T T

T T T T

T T

x k Φx k Γu k Φx k Γ Q Γ λ k

Φx k Γ Q Γ Φ λ k Φ Q x k

Φ ΓQ Γ Φ Q x k ΓQ Γ Φ λ k

λ k Φ Q x k Φ λ k

11 1

1 11 1

( ) ( 1) ( 1) ( 1) , ( )

IT T T T

T T

S Φ S S ΓR Γ S Φ Q Φ S Γ

S k Φ S k S k ΓR Γ S k Φ Q

R Γ S Φ

S N

Q

Q


12

1 2 20

Consider ( 1) ( ) ( ) and the performance index is

1given by ( ) ( ) ( ) ( ) , Q >0.

2

Assume that a positive-definite

T T

k

x k Φx k Γu k


Stability of the Closed - loop System (LQ controller)

steady-state solution of ARE exists.

Then the steady-state optimal solution law ( ) ( ) gives

an (closed -loop system 1) ( ) ( ).

Note:

In LQ controller, the poles

asymptotically sta

are

ble

u k Kx k

x k Φ ΓK x k

obtained from det( ) 0.

And the poles are the stable eigenvalues of the generalized

eigenvalue problem. Euler equation of LQ problem

λI Φ ΓK

n


13

1 12 1 2

1

How to obtain ( ) ( ) from ARE?

From the state equation and costate equation, we have

( 1) ( )

( 1) ( )

These equations are called Hamilton

T T T T

T T

u k K x k

x k x kΦ ΓQ Γ Φ Q ΓQ Γ Φ

λ k λ kΦ Q Φ

1 12 1 2

1 2 2

's equations

or the Euler equations.

is called a Hamiltonian matrix (constant matrix).

How to obtain ( ) ( ) from ?

T T T T

c T T

n n

c

c

Φ ΓQ Γ Φ Q ΓQ Γ ΦH

Φ Q Φ

H

u k K x k H


14

12

1

12

11

12

11

12

1 1 11 2

( ) ( ) ( )

( ) ( )

( ) ( ) ( )

( ) 0

( ) 0

det 0

det 00 ( )

T

T

T

T

T

T

T

T T

zX z ΦX z ΓU z

ΦX z Γ zQ Γ Λ z

Λ z Q X z zΦ Λ z

X zzI Φ ΓQ Γ

zΛ zQ z I Φ

zI Φ ΓQ Γ

Q z I Φ

zI Φ ΓQ Γ

z I Φ Q zI Φ ΓQ Γ

Remark : Using reciprocal root properties in p372


15

1 1 11 2

1 1 1 1 11 2

1

2

1

1

11 1

1

det( )det ( ) 0

det( )det ( ) ( ) ( ) 0

Let det( ) ( ), det( ) ( )

( ) ( )det ( ) 0

where a d

( )

n

T T

T T

T T T

T

T

T

zI Φ z I Φ Q zI Φ ΓQ Γ

zI Φ z I Φ I z I Φ Q zI Φ ΓQ Γ

zI Φ a z z I Φ a z

a z a z I ρ H zI Φ ΓΓ

Q

z I Φ H

ρH H ΓQ

1

1

1 1 1

Using the property of det

( )

( ) det( )

( ) ( )det 1 ( ) 0T T

T T

n m

T

Γ ΓΓ

I BA I AB

a z a z ρH zI Φ ΓΓ z I Φ H


16

-1.5 -1 -0.5 0 0.5 1 1.5-1.5

-1

-0.5

0

0.5

1

1.5

Real Axis

Imag

Axi

s

Symmetric root locus


17

1 12 1 2

1

How to obtain ( ) ( ) from ARE?

From the state equation and costate equation, we have

( 1) ( )

( 1) ( )

These equations are called Hamilton

T T T T

T T

u k K x k

x k x kΦ ΓQ Γ Φ Q ΓQ Γ Φ

λ k λ kΦ Q Φ

1 12 1 2

1 2 2

's equations

or the Euler equations.

is called a Hamiltonian matrix (constant matrix).

How to obtain ( ) ( ) from ?

T T T T

c T T

n n

c

c

Φ ΓQ Γ Φ Q ΓQ Γ ΦH

Φ Q Φ

H

u k K x k H


18

Eigenvector Decomposition

1

Recall: Coordinate change by similarity transformation

For ( 1) ( ),

( ) ( ), ( 1) ( 1)

The above state equation becomes

( 1) ( )

( 1) ( )

Let

x k Ax k

x k Mξ k x k Mξ k

Mξ k AMξ k

ξ k M AMξ k

Λ M

1 . ( 1) ( ).

In order to make a diagonalized matrix,

should consist of eigenvectors of .

AM ξ k Λξ k

Λ

M A


19

1*

* 1

0

0

where is a similarity transformation matrix of eigenvectors of .

c c

c c

c

EH H

E

H W H W

W H

0

0

I

I

X XW

Λ Λ

inside the unit circle

outside the unit circle

*1

*

* *0

* *0

* *

* *0

i.e.,

0 = from Hamilton's equations.

0

I

I

N

NN

xxW

λλ

X Xx x xW

Λ Λλ λ λ

x E x

λ E λ


20

* *

* *

* *

* 1

*

1

*

As goes to infinity, ( ) (0) 0 and ( ) (0)

Only sensible solution (0) 0, therefore ( ) 0

( ) ( ) (0)

(0) ( )

( ) ( ) (0)

( ) ( ) ( )

is

N N

k

I

I I

kI

kI I

I

N x N E x λ N E λ

λ λ k k

x k X x k X E x

x

Λ X S

E X x k

λ k Λ x k Λ E x

λ k x k x k

S

@

12( ) ( ), whe

re ( )

1Remark: (

the steady-state solution of ARE

0) (

.

0)2

T

T

u k K x k K Q Γ S Γ S Φ

J x S x


21

1 2

Procedure ( . 377)

1. Compute eigenvalues of .

2. Compute eingenvectors associated with the stable eigenvalues of .

3. Compute control gain with .

Riccati equation

( ) ( ) (

c

c

p

H

H

K S

y t q t q

23

2 2

22

) ( )

)

( ) 1 2 , ( )

1 1 ( ) , ( )

p

p

t y q t y

ex

y t t ty y y t t

yy t y y t

t tt


22

12 1

12 1

1

continuous-time ( ) ( ) ( ) ( ) ( )

discrete-time ( 1) ( ) ( 1)

Matrix Riccati equation

( 1

T

T

T T T

A S t S t A S t ΓQ ΓS t S t Q

A S S A S ΓQ ΓS Q

Φ S k Φ S k Φ S k ΓR Γ S k

1

T 11

T

Lyapunov equatio

) ,

continuous-time

discrete-time

n

T T

T

Φ Q

Φ S Φ S Φ S ΓR Γ S Φ Q

A P PA Q

Φ PΦ P Q


23

1 20

1 1

2 2

1 2

T 11

1ex) Minimize ( ) ( ) ( ) ( )

2

( 1) ( )1 1 1 subject to ( )

( 1) ( )1 0 0

where I and 1

o

T

k

T


x k x ku k

x k x k

Q Q

S Φ S S ΓR Γ S Φ Q

T1

1T 11

1T 12 1

2

1r I

By matrix inversion lemma

I ( )

i.e., I

since

Note:

T T

T

T

T

S Φ S Φ Q

S Φ S Γ R Γ S Γ Γ S Φ Q

S Φ S ΓQ Γ S Φ Q

R Q Γ S Γ

Γ SRΓ

-1 1 1 1 1 1 1 ( ) ( )

A BCD A A B C DA B DA


24

1 11 2

11 12 11 12 11 12

12 22 12 22 12 22

12 11 11 12

1

( )

1 0 0 1 1 0 1 1 11 1 0

0 1 1 1 0 1 0 1 0

(1 )

T TS Q Φ I ΓQ Γ S Φ S

s s s s s s

s s s s s s

s s s s

s

22 11 12 11 22

22 11 11

22 12 12 22 12

1

( 1)(1 )

( 1) 1

3.7913 1.0000

1.0000 1.7913

s s s s

s s s

s s s s s

S


25

1 102 1 2

01 2 2

,

1 2 0 1

1 0 0 0

0 1 0 1

1 1 1 1

Eigenvectors

0.3548

T T T TI

c T TIn n

c

X XΦ ΓQ Γ Φ Q ΓQ Γ ΦH W

Λ ΛΦ Q Φ

H

W

Same problem but different approach

0.8463 0.2433 0.1233

0.1621 0.3866 0.5326 0.2699

0.4429 0.2830 0.3899 0.7374

0.8073 0.2329 0.7107 0.6068


26

*

1

1

Corresponding eigenvalues

2.1889 0 0 0

0 2.1889 0 0

0 0 0.4569 0

0 0 0 0.4569

0.3899 0.7374 0.2433 0.1233

0.7107 0.6068 0.5326 0.2699

3.7913 1.

c

I I

H

S Λ X

0000

1.0000 1.7913


27

Cost Equivalents

1 20

1 ( 1)

1 20

Analog cost function (Refer . 580 in Ogata's)

1 ( ) ( ) ( ) ( )

21

( ) ( ) ( ) ( )2

Because ( ) ( ) ( ) ( ) ( )

where ( )= , (

NT T Tc c c

N k T T Tc ckT

k

Fτ

p

J x t Q x t u t Q u t dt

x τ Q x τ u τ Q u τ dτ

x kT τ Φ τ x kT Γ τ u kT

Φ τ e Γ τ

0

111 12

0 21 22

111 12

0221 22

)

( )1 ( ) ( )

( )2

0 ( ) ( )( ) 0where

0 0( )

τ Fη

NT T

ck

TT c

Tc

e dηG

Q Q x kJ x k u k

Q Q u k

QQ Q Φ τ Γ τΦ τdτ

QQ Q IΓ τ I


28

1 111 12 22 21 22 21

1

220

Remarks :

i) Cross terms that weight the product of and

îi) Define and ( ) ( ) ( )

1 ˆ ( ) ( ) ( ) ( )2

subject to ( 1) ( )

NT T

k

x u

Q Q Q Q Q v k Q Q x k u k

J x k Qx k v k Q v k

x k Φx k Γ v

122 21

122 21

( ) ( )

( ) ( )

ˆ ( ) ( )

k Q Q x k

Φ ΓQ Q x k Γv k

Φx k Γv k


29

Least Squares Estimation

y Hx v

( ) ( )1 1

2 2T TJ v v y Hx y Hx

p1 measurement vector

p1 measurement error vector

pn matrixn1 unknown

vector

1

minimize to determine the best estimate of

given the measurements .

( ) ( ) 0

ˆ

ˆ is a best estimate of

T

T T

T T

J x

y

Jy Hx H

x

H y H Hx

x H H H y

x x


30

1 1 1

1

1

ˆ ( )

ˆ

Îf is zero mean, is an unbiased estimate (zero mean).

The covariance of the estimate error

ˆ ˆ

( ) (

T T T T T T

T T

T

T T T T

x H H H Hx v H H H H x H H H v

x x H H H v

v x x

P E x x x x

H H H E vv H H

1

2

1 2

)

If ,

( )

T

T

H

E vv Iσ

P H H σ


31

)

. . . . . . . . . . . .

20 1 2

21 11 1

202 22 2

213 33 3

2

ex Least- sqaures estimation

0 2 0 5 1 1 1 2 1 1 1 3 1 1 1 2 2 0 1 2 2 2 4 0

1

1

1

T

i i i i

y

y a a t a t v

y vt t

ay vt t

ay vt t

a

ˆ . . .

21 1

202 2

213 3

2

1

1

where and 1

0 7432 0 0943 0 0239T

t t

at t

H x at t

a

x


32

0 5 10 15 20 250

2

4

6

8

10

12

14Sales fit and prediction

Sal

es (

$100

0)

Months

. . . 20 7432 0 0943 0 0239y t t


33

Weighted Least Squares

ˆ1

1

2

1

2

From the previous result,

Note:

1. The covariance is used in weighting matrix, i.e.,( ) .

2. Covariance indicates the degree of uncertainty of measurem

T

T T

i

J v Wv

x H WH H Wy

W R

R σ I

ˆ

ˆ

11 1

11

ent error.

3.

is a best linear unbiased estimate.

4. The covariance of the estimate error is

T T

T

x H R H H R y

x

P H R H


34

Recursive Least Square

1 1

1 1

1 1

1 1

: old data, : new data

0 0ˆ

0 0

ˆ (old data only)

( )

o o o

n n n

T T

o o o oo o

n n n nn n

T To o o o o o o

T To o o n n n

y H Vx

y H V

o n

H H H yR Rx

H H H yR R

H R H x H R y

H R H H R H

1 1

1 1 1 1

1

ˆ + (old data and new data)

ˆ ˆ ˆLet

ˆ ˆ

where terms are cancelled out.

T To o o n n n

o

T T T Tn n n o o o o n n n n n n

To o o

x H R y H R y

x x δx

H R H x H R H H R H δx H R y

H R y


35

ˆ ˆ

ˆ

11 1 1

11 1 1

T T To o o n n n n n n n o

T To n n n n n n n o

δx H R H H R H H R y H x

P H R H H R y H x

11 1Define

Tn o n n nP P H R H

ˆ ˆ ˆ1To n n n n n ox x P H R y H x

old esti-mate

old estimatenew estimate

covariance of old esti-mate


36

?

:

11 1 1 1 1 1

1 11 1

1

1

1

Remarks :

i) How to calculte

- Matrix Inversion Lemma in Appendi

N

C

o

x

te

n

T T To n n

T

n o o n n n

Tn o o n n n o

o n n o

To

n n o

n nn

P

P H R H P P H R H P H H P

P H R

A BCD A A B C DA B

P P P

DA

P H H P

P

H R H

( ) ( ) (

( )

) ( )

)

)

(

(

1

1 11 1

11

1

1

ii) Comput

Note:

ation complexity

1

1

1 1 1

n

T To n n n n n o n

T T

Tn n n

P k P k

H

P H R H R H P H

PP

P k H HP k H R HP

k H R Hk

k

Sometime, it is a scalar. That is if we use just one new information.


37

Stochastic Models of DisturbanceWe have dealt with well-known well-defined, ideal systems.

- disturbance (process, load variation)- measurement noise

0real line

new (range) sample space

sample point in sample space(event)

s

iX

( )iX s

Some sample space and a probability distribution defined on

events in that sample space are give A single-valued real function

is then defined on the sample space so that to each

n.

point of

X

S s

S P

( ).

therefore corresponds a single real number

The function is called a random variable.

S

x X s

X


38

) : head-indicator function

- at least one head occurs in the two independent flips

of an unbiased coin.

- unity value for sample points in the set of particular i

Hex I

nterest

and zero elsewhere.


39

, ,

, ,

single point 1 range sample space

its inverse image in sample space (event)

single point 0 range sample sapce


1

H

H

I

I

H

S

HH HT TH S

S

TT S

P I P HH HT TH

1 1 1 3 + +

4 4 4 41

0 4H

P HH P HT P TH

P I P TT


40

( ) ( ( ), ( ) ( , )

( , ) : ( ), ( ),

( , ).( , ),( , )

2D vector ) for any

Range and

0 1 1 0 11

H T

I H T

I s I s I s x y s S

S x y x I s y I s s S

) : set-indicator function (2D random vector) ex I

1

2

3

random vecto

Note:

is called a

whose components are random variables , 's.

r

i

X

X X

X

X


41

,

(0,1) range sample space


(1,0) range sample sapce


(1,1) range sample sapce

its invers

I

I

I

S

TT S

S

HH S

S

HT TH

( , )

( , )

( , ) ,

e image in sample space (event)

10 1

41

1 0 4

1 1 111 + =

4 4 2

S

P I P TT

P I P HH

P I P HT TH P HT P TH


42

Suppose that ( ) has a probability density ( ).

( ) ( ) ( ) or ( ) 0

Probability distribution of ( )

( ) ( ) where is a real number.

( ) ( ) , : ( ) ,

X

xx

x X X

X

X s f x

dF xF x f ξ dξ f x

dx

X s

F x P X s x x

P X s x P X s x P s X s x

( ) 1 and ( ) 0, ( ) ( ) if

[ ( ) ] , 0 1

X X X X

i i i

s S

F F F b F a b a

P X s x p p


43

Ex)

3[ 1]

41

[ 0]4

0 for - < < 0

1( ) for 0 < 1

41 for 1 <

This distribution function is a step function,which has two possible values.

It is called a Bernoull

H

H

H

I

P I

P I

x

F x x

x

1 2 1 1 2 2

i randon variable.

Note:

( , , ... , ) , , ... ,X n n nF x x x P X x X x X x


44

Let be an -random vector with probability density ( ).

[ ] ( ) or

Remarks :

i) [ ] is a linear operator

ii) [ ] is called a mean (statistical av

[

erage) o

]

f

kk

X

X k

X n f x

E X E X x P Xxf x dx

E

E X

x

m X

2

iii) [ ] is a first moment

[ ] ( ) or the moment of

the central moment of

( ) ( )( )

[

]k kj j

k kX

k

T

j

E X

E X x f x dx kth X

E X E X kth X

Var X E X E X E X m X m

E X x P X x

2 2 2 2

the variation (matrix) of

( ) [ 2 ] [ ] [ ]

X

Var X E X XE X E X E X E X


45

Let :

and ( ) is a function of random variables and a random -vector.

( ) ( ) ( )

Let be a random -vector with mean and

be a random -vector with mean .

Let ( , ) be their

n m

X

X

Y

XY

g R R

g X m

E g X g x f x dx

X n m

Y m m

f x y

joint probability density.

( )( )

( )( ) ( , )

is covariance matrix between and .

TX Y

TX Y XY

E X m Y m

x m y m f x y dx dy

X Y


46

The random vectors and are independent

if ( , ) ( ) ( )

or ( , ) ( ) ( ).

The random vectors and are uncorrelated

if [ ] [ ] [ ]

Let be an n-random vector with normal (or Gauss

XY X Y

XY X Y

T T

X Y

F x y F x F y

f x y f x f y

X Y

E XY E X E Y

X

1/ 2 1/ 2

ian) density

1 1( ) exp ( ) ( )

(2 ) (det ) 2

where is a constant -vector, is an symmetric

and positive definite matrix.

[ ] (mean)

( )( ) (varianc

only first and s

)

e

e

TX n

T

f x x m P x mπ P

m n P n n

E X m

E X m X m P

cond moments are required.


47

1

2

1 1

A vector random process is a family of vector time function

denoted by

( )

( ) ( ) , 0

( )

A random process is characterized by specifying its distribution

function

( , ... , ,

n

m

X t

X tX t t

X t

F x x t

1 1 2 2

1 2

1 2

, )

( ) , ( ) , ... , ( )

for all vector , , ... ,

for all , ,

for all

... ,

...

m

m m

m

m

P X t x X t x X t x

x x x

t t

m

t

t


48

F(X

, t2) F

(X, t

1)

F(X

, t3)

01

X

X

0 t1 t2 t3

t

X(•, 1)

X(•, 2)

X(•, 3)


49

Remark:

1

1

1 111 1 1

( , ) : random vector

( , ) : time function vector

The density function for a random process ( ) is

, , , , ,

covariance

nm

n mn n nm

T

T

X t

X ω

X t

Ff x x t t

x x x x

E X t m t

E X t m t X τ m τ

E X t m t X t m t

,

v

autocorrela

ari

tion

ance

TE X t X τ R t τ


50

Let ( ) and ( ) be random processes.

( ) ( )

is cross covariance between ( ) and ( ).

( ) and ( ) are uncorrelated

if

. ., ( ) ( ) 0

A

T

X Y

T T

T

X Y

X t Y t

E X t m t Y τ m τ

X t Y τ

X t Y τ

E X t Y τ E X t E Y τ

i e E X t m t Y τ m τ

1 1

1 1

1 1

random process ( ) is stationary in the strict sense

if , ,

, ,

for all , , , , , and all for .

. ., independent of .

m m

m m

m m

X t

P X t x X t x

P X t τ x X t τ x

x x t t m τ

i e t


51

1

If ( ) is stationary, then ( , ) ( ( ) ) is indepent of

( , )and ( , ) is also

If the following two things hold,

is constant.

,

independent of

e

n

.

d peT

x

nx

xn

E

X t F x t P

X t

X t x t

F x tf x t t

x

m

E R t τ

x

X t X τ

ds on only .

. ., ,

,then ( ) is wide sense stationary (or weakly stationary)

t τ

i e R t τ R t τ

X t


52

A random process X(t) is Gaussian process

if for any t1, …, tm, and any m, the random vector

X(t1) … X(tm) have the Gaussian distribution.

A Gaussian process is completely characterized

by its mean

and its autocorrelation

If Gaussian process X(t) is w.s.s, then it is strictly stationary.

Assume that X(t) is wide sense stationary

Let

then is Fourier

transform of

. It is called a Spectral Density Matrix.

1

2jωτS ω R τ e dτ

π

( ) ( )E X t m t

( ) ( ) ( , )TE X t X τ R t τ

( ) ( ) ( ) ,TR τ E X t X t τ τ where - < <

( )R τ


53

Remark:

A random process X(t) is a Markov process

if

for all t1<t2<···<tm, all m, all x1,…, xm

A random processes X(t) is independent

if the random vectors X(t1) ··· X(tm)

are mutually independent for all t1<t2<···<tm and all m.

, ,1 1 1

1

1

1m

m

m m

m m m

m

P X t x X t x X t x

X tP X t xx

Note: Andrei Andreevich Markov (1856 – 1922)

*( ) ( ) ( ) ( )TS ω S ω S ω S ω


54

ex) Consider a scalar random process X(t), t 0 defined from

where X(0) is zero mean Gaussian random variable with .

0 0 t

X(t)

2σ

( )( )

1

1

dX tX t

dt t

1( ) (0) (solution)

11

( ,0) state transition funtion1

X t Xt

Φ tt


55

1 2

11

2

2 22

( ), 0 is a Gaussian process.

Let

1( ) ( )

1

( ) is a Markov process.

1 1( ) (0) (0) 0

1 1

1 1 1( ) ( ) (0) (0)

1 1 ( 1)( 1)

1( )

( 1)

m

mm m

m

X t t

t t t

tX t X t

t

X t

E X t E X E Xt t

E X t X τ E X X σt τ t τ

E X t σt

not w.s.s.


56

The density function X(t) is

A random process w(t), t 0 is a white process

if it is zero mean with the property that w(t1) and w(t2)

are independent for all t1 t2 and

where Q(t1) is intensity

( )

( , ) .

2 2

21

21

2

t xσt

f x t eπσ

1 2 1 2 1 1 2( , ) ( ) ( ) ( ) ( )wR t t E w t w t Q t δ t t


57

Remarks:

i) If Q(t) is constant, i.e. Q(t) = Q then w(t) is w.s.s.

and the spectral density is

ii) A white process is not a mathematical rigorous

random process.

iii) A sample function for a white noise process can be

thought as composed of superposition of large number of independent pulse of brief duration with random amplitude.

iv) If the amplitude of the pulse is Gaussian, the w(t) is

a Gaussian white noise.

v) A white noise is a ‘derivative’ of a Wiener process

(Brown motion)

( ) wS ω Q ω


58

ex)

Similarly,

Since {v(k)} is a white process, {X(k)} is a random process.

X(0) should be specified.

It is assumed that

white process

Wiener process

( )( ) ( ) ( )

dX tA t X t w t

dt

0 00( ) ( ) ( ) ( ) ( )

t t

t tX t X t A τ X τ dτ w τ dτ

( 1) ( ) ( )

( ) ( )

X k X k v k

y k CX k

Φ

( )

( ( ) )( ( ) )

( ) ( )

0

0 0 0

1

0

0 0 T

T

E X m

E X m X m R

E v k v k R


59

0

1 0

Mean: ( 1) Φ ( ), (0)

Define ( ) ( ( ) ( ))( ( ) ( ))

Variance: ( 1) Φ ( )Φ , (0)

Autocorrelation:

( , ) ( ( ) ( )

x x x

Tx x x

Tx x x

Tx

m k m k m m

P k E X k m k X k m k

P k P k R P R

R k τ k E X k τ X k

0

Φ ( ( ) ( )

If (0) 0, ( , ) Φ ( )

For the output,

( ) ( ) ( )

( , ) ( ) ( ) ( , )

( , ) ( ) ( )

τ T

Tx x

y x

T T Ty x

Tyx

E X k X k

E X m R k τ k P k

m k E CX k Cm k

R k τ k E CX k τ X k C CR k τ k C

R k τ k E CX k τ X k

( , ) xCR k τ k


60

k

j j

yj

uj

Ty

u

y k h k j u j h j u k j

m k E y k E h j u k j

h j m k j

R k τ k E y k τ y k

h i R k

0

0

0

Consider I/O model.

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( )

( , ) ( ) ( )

( ) ( T

i j

yu uj

τ j i k h j

R k τ k h j R k τ j k

0 0

0

, ) ( )

Similarly,

( , ) ( ) ( , )


61

-

-

-

By the definition of a spectral density function

1 ( ) ( )

2

In a discrete-time system

1 ( ) ( )

2

From the above definition

1 ( ) ( )

2

jωτ

jnω

n

jnωy y

n

S ω R τ e dτπ

S ω R n eπ

S ω R n eπ

- 0 0

0 0 0

1 ( ) ( ) ( )

2

1 ( ) ( ) ( )

2

jnω Tu

n k l

jkω jmω jlω Tu

k m l

e h k R n l k h lπ

e h k e R m e h lπ


62

Remarks:

A stable linear time-invariant discrete-time system

has a pulse transfer function H(z).

Suppose that the input u(k) is w.s.s. with a spectral

density matrix .

Then the output y(k) is w.s.s. and the spectral density

of the output y(k) is

In a scalar case,

jω T jωy uS ω H e S ω H e

h(k-j)u(j) y(k)

jω T jωy uS ω H e S ω H e

jωy uS ω H e S ω

2

uS ω


63

T

T

u

jω jωx

x k ax k u k

y k x k e k

E u k E u k u k r

E e k E e k e k r

H zz a

rS ω

πr r

S ω H e H eπ π

1

2

1

1 1

ex) ( 1) ( ) ( )

( ) ( ) ( )

( ) 0, ( ) ( )

( ) 0, ( ) ( )

1 ( )

( )2

( ) ( ) ( ) =2 2 (

y

y

a a ω

rS ω r

π a a ω

y t

S ω

2

12 2

1 2 cos )

1 ( )

2 1 2 cos

Suppose ( ) is a wide sense stationary random process

with spectral density ( ).


64

jω T jωYS ω G e G e


65

ex)

12 1

2 11 2 2

1

2 1

1

2 2 21 2 1 2 1 2

2

21 2

12

11

2

2

(1 ) ( (1 ) )( (1 ) )where

2

1(1

2

jω

jω

Y

z e

z e

rS ω r

π z a z a

r r a r a z z

π z a z a

λ z b z bπ z a z a

r r a r r a r r ab

ar

λ r r

2 2 21 2 1 2) ( (1 ) )( (1 ) )a r r a r r a

( )( )λ z b z b r r a r a z z 2 1 2 11 2 2 Note: 1


66

white noisewith intensity I

Note: Norbert Wiener (1894 – 1964) Wiener filter for stationary I/O case in 1949

“Everything” can be generated by filtering white noise .

L.T.Iw(k) y(k)

white processwith intensity I

colored noise

z bG z λ

z az b

Y z λ W zz a

( )

( ) ( )


67

z zF z

π z z z z

1

1 2 2

ex)

We want to generate a stochastic signal with the spectral density

1 0.3125 0.125( )

2 2.25 1.5( ) 0.5( )

Then the desired noise properties a

zH z

z z2

re obtained by filtering white noise

through the filter

0.5 0.25

0.5

Reference: "Probabilty and Random Processes", W.B. Davenport,Jr., McGraw-Hill

"Probability, Random Variables, and Stochastic Processes", A. Papoulis,

McGraw-Hill

"Computer-Controlled Systems - Theory and Design", K.J. Astrom and

B. Wittenmark, Prentice-Hall


68

LQ + Kalman Filter

( ~ state feedback + observer by pole placement)

LQG(Linear Quadratic Gaussian) problem

- Partially informed states

1

0

0

12

( 1) ( ) ( ) ( )

( ) ( ) ( )

(0)

(0)

( ) ( ) positive semi-definte

( ) ( )

( ) ( ) postive definte

w

v

x k Φx k Γu k Γw k

y k Hx k v k

E x m

Var x R

E w k w k R

E w k v k R

E v k v k R


69

12

1

0

1

If 0, ( ) and ( ) are independent.

1 1

2 2

Assumptions:

, , , and may be time-invariant deterministic.

( ) 0

( ) 0

( ) ( ) 0

( ) ( ) 0 if

N

k

R w k v k

J E x N Sx N x k Qx k u k Ru k

Φ Γ Γ H

E w k

E v k

E w i v j

E w i w j i j

0

( ) ( ) 0 if

( (0) ) ( ) 0

( ) ( ) v

E v i v j i j

E x m y k

E v k v k R


70

Given y(0), y(1), … y(k), determine the optimal estimate

such that an n n positive definite matrix

i.e., minimum variance of error

Remarks:

i) P(k) is minimum

*P(k) is minimum where is an arbi-

trary vector

ii) P(k) is minimum

x̂ k x k x k1 1 1 (estimate = true - error)

Problem Formulation

J E x k x k trace P k

P k E x k x k1 1 1 is minimum


71

Let the prediction-type Kalman filter have the form.

- Predictor type, One-step-ahead estimator

-

where L(k) is time-varying

y(k) is a measured output

is an output from the model.

Define as a reconstruction error.

ˆ ˆ( ) (ˆ( ) ( )) ( ) ( )1 Φx k Γux k y kk L k Hx k

ˆ( )Hx k

ˆx x x

ˆ( ) ( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )

1

1

1

x k Φx k Γu k Γw k Φ L k H x k

Γu k L k Hx k v k

Φ L k H x k L k v k Γw k


72

0

1

1

0

( 1) ( ) ( )

(0)

( 1) ( 1) ( 1)

(( - ( ) ) ( ) ( ) ( ) ( )))

( ( )( - ( ) ) ( )

Îf (0) , ( ) 0 0

( ) ( ))

Defi

E x k Φ L k H E x k

E x m

P k E x k x k

E Φ L k H x k Γw k L k v k

x k Φ L k H

E x m E x k

w Γ

k

k v k L k

1

ne ( ) ( )

( ) ( ) ( ) ( )

Φ L k H N k

L k v k Γw k e k


73

where

*

( ) ( )

P k E N k x k e k x k N k e k

E N k x k x k N k N k E x k e k

E e k x k N k E e k e k

1

0

0

E x k e k E x e k 0 0 0

1 1

1

v w

P k Φ L k H P k Φ L k H

L k R L k Γ R Γ

P E x x P 00 0 0


74

minimize P k 1 matrix

scalar α P k α 1

*( )

1 1

1

1

1

independent term of

w

v

v

v v

α P k α α ΦP k Φ Γ R Γ L k HP k Φ

ΦP k H L k L k R HP k H L k α

α L k α

α L k ΦP k H R HP k H

R HP k H L k ΦP k H R HP k H α


75

ˆ ˆ ˆ

ˆ( ) ( )

,

1

1

0

1

1 1

If 0

then

1

0 0

0

1

v

w v

v

x k Φx k Γu k L

L k ΦP k H R HP k H

P k ΦP k Φ Γ R Γ ΦP k H R

L k ΦP k H R HP

HP k H HP k Φ

k y k Hx k

x E x

k H

P P

Note: Kalman and Bucy filter for time-varying state space in 1960


76

Remarks:

i)

ii) a priori information are

iii) due to system dynamics

due to disturbance

w(k)

last term due to newly measured information

iv) P(k) does not depend on the observation. Thus the

gain can be precomputed in forward time and

stored.

v) steady-state Kalman filter – all constants

, , , .0 and 0w vR R P m

ΦP k Φ

1

1 1w vP ΦPΦ Γ R Γ ΦPH R HPH HPΦ

1

vL ΦPH R HPH

( ) is time-varying.L k

1 1wΓ R Γ


77

** *

ˆ( ) ( ) ( )( ( ) ( ))

( ) ( ) () ( ) )( 1 1

Another Form of Kalman Filter - Filter type (p. 389~391)

At the measurement time (measurement update)

~ Recursive Least Squar e

where

v v

x k x k L k y k Hx k

L k P k H R M k H HM k H R

*

* *( ) ( ) ( ) ( ( ) ) ( )

ˆ(

ˆ( ) ( ) ( ), ( )

( ) ( ) ( ) )

)

( ) (

0

1

1

1 0

1

Between measurement (time update)

and

Notes:

0

1 ,

is the ac

0 0 0w

v

x k Φx k Γu k E x m

M k ΦP k Φ Γ R Γ M E x x R

P k M k M k H HM k H R HM k

x k

ˆ. ~ ( | )

ˆ( ) . ~ ( | )

( ) ~ ( | ) ( ) ~ ( | )

tual state estimate at

is the predicted state estimate at the sampling instant 1

Also, and 1

k x k k

x k k x k k

P k P k k M k P k k


78

) ( ) ( )

( ) ( )

( )

( ) .

, ,

ˆ ˆ ˆ( ) ( ) ( ) ( )

2 2

1

where

0 2

0 0 5

1 0 1

1

w

ex x k x k

y k x k v k

E v k R k σ

E x

P

Φ R H

x k x k L k y k

( )

( )( )

( )

( )( ) , ( ) .

( )

2

2

2

1 0 0 5 decrease with time

x k

P kL k

σ P k

σ P kP k P

σ P k


79

0 100 200 300 400 500 6000

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

0.45

0.5

k

p(k)

L(k)

0 100 200 300 400 500 600-2.5

-2.4

-2.3

-2.2

-2.1

-2

-1.9

-1.8

-1.7

k

x__hat

E[v(k)2] = 1

E[v(k)2] = 0.5


80

time

transient

1

2

0

L = 0.01

L = 0.05

L(k):optimal gain

Steady-state

x

where 1σ


81

colored noise

( ) ( ) ( )1

1

1

where is a white noise

is a colored noise.

x k Φ x k w k

y k H x k n k

w k

n k

Frequency Domain Properties of Kalman Filter

( ) ( ) ( )

.

2

2

1

where and are white noises

z k Φ z k v k

n k H z k e k

v k e k


82

( )

( ) ( ) ( )

( ) ( ) ( )

( )

( )

( ), ( ) ( ) .

1

2

1 2

1

2

1 2

1

1

01

01

where and are uncorrelated white noises

x k Φ x k w k

z k Φ z k v k

y k H x k H z k e k

Φx k x k w k

Φz k z k v k

x ky k H H e k

z k

w k v k e k


83

ˆ ˆ( ) ( )

ˆ ˆ( ) ( ) ( )ˆ ˆ( ) ( )

1 11 2

2 2

The steady-state Kalman filter for is given by

01

01

x

Φ Lx k x ky k H x k H z k

Φ Lz k z k

ˆ ˆ( ) ( )( )

ˆ ˆ( ) ( )

ˆ ˆ

1 1 1 1 2 1

2 1 2 2 2 2

1

1 1 1 2 1

2 1 2 2 2 2

1= +

1

Pulse-transfer function from to and

0

Φ L H L H Lx k x ky k

L H Φ L H Lz k z k

y x z

zI Φ L H L H LH z I

L H zI Φ L H L


84

Remarks:

i) It gives an idea how the Kalman filter attenuates dif-

ferent frequency.

ii) Kalman filter has zeros at the poles of the noise

model. (notch filter)


85

Smoothing: To estimate the Wednesday temperature based on temperature measurements from Monday, Tuesday and Thursday.

Filtering: To estimate the Wednesday temperature based on temperature measurements from Monday, Tuesday and Wednesday.

Prediction: To estimate the Wednesday temperature based on temperature based on temperature measurements from Sunday, Monday and Tuesday.


86

-1

1 12 2 00

1

0

Minimize ( ) ( ) 2 ( ) ( ) ( ) ( ) ( ) ( )

subject to ( 1) ( ) ( ) ( )

where (0) 0, (0) (0) ,

NT T T

k

T Tw

E x k Q x k x k Q u k u k Q u k x N Q x N

x k Φx k Γu k Γ w k

E x E x x R E ww R

The control law ( ) ( ) ( ) gives the minimum.u k K k x k

Stochastic LQ Control Problem


87

1

0

Given the linear stochastic difference equatiom

( 1) ( ) ( ) ( )

( ) ( ) ( )

where (0) 0, (0) (0) ,

( ) ( )

( ) (

T


y k Cx k v k

E x E x x R

w k w kE

v k v k

12

12

-1

1 2 00

,)

ˆ find a linear control law ( ) ( ) ( ) that minimizes

( ) ( ) ( ) ( ) ( ) ( )

Tw

v

NT T

k

R R

R R

u k K k x k

E x k Q x k u k Q u k x N Q x N

LQG Control Problem


88

For ( 1) ( ) ( ) ( )

ˆThe state feedback control law ( ) ( ) ( ) is independent of ( ).

It is a unique admissible control stategy that minimizes

x k Φx k Γu k w k

u k K k x k w k

The Ideas of Separation (Separation Theorem)

the cost function.

The Kalman filter minimizes ( ) ( ) ( ) .

Âs a result, ( ) is reconstructed.

ˆThis makes it possible to use the control law ( ) ( ) ( )

with the dynamics ( 1) ( )

TP k E x k x k

x k

u k K k x k

x k Φx k

(

( ) ( ) and

ˆ ( 1) ( ) ( ( ) ( )) ( ) ( ) ( ) ( )

The term ( ) ( ) is viewed as a part of the

) ( ) ( )

noise.

ΓK k

Γu k w k

x k Φx k Γ K k x k w k Φx k ΓK k x k x k w

ΓK k x k

k


89

( ) ( ) ( ) ( )

( ) ( ) ( )

( ) ( )

( ) ( )

1

12

12

Given the linear stochastic difference equation

1

where

find a l

Tw

v


y k Cx k v k

R Rw k w kE

R Rv k v k

ˆ( ) ( ) ( )

( ) ( ) ( ) ( )

ˆ ˆ ˆ ˆ( ) ( ) ( ) ( ) ( ) ( )

1 20

inear control law that minimizes

where x(k+1)=

T T

k

u k K k x k

E x k Q x k u k Q u k

u k x k Φx k Γu k y k HxK L k

Stationary LQG Control Problem


90

2

1

1 1

1

1

1

2

LQ:

Kalman Filter:

wher

e

T T

T T T T Tw

T T

T

v

T

P ΦPΦ Γ R Γ

S Φ SΦ Φ SΓ Q Γ SΓ Γ S

ΦPH

Φ Q

K Q Γ SΓ Γ S

R H

Φ

HPH PΦ

1

1 2

1

1

1 1

or : LQ

Kalman Filt

er:

T Tv

T T T T

T

v

T T

w

S Φ SΦ Q K Q Γ

L ΦPH R HPH

P ΦPΦ Γ R Γ L R H

S

L

K

PH

Γ


91

LQ Kalman filter

1

2

11

T

T

Tw

v

k

Φ

H

Γ

N k

Φ

Γ

Q

Q

S

R Γ

R

T

P

K L

Control and Estimation Duality

Date post:	13-Dec-2015
Category:	Documents
Upload:	randell-julian-anderson
View:	230 times
Download:	3 times

Robotics Research Laboratory 1 Chapter 7 Multivariable and Optimal Control.

Documents