Optimal Control, Guidance and Estimation...Optimal Missile Guidance Through LQR System dynamics Cost Function v a y v = = ɺ ɺ 1 1( )2 2 2 2 tf f t J c y a dt= + ∫ σ V LOS a v

1

Lecture Lecture Lecture Lecture –––– 13131313

Linear Quadratic Regulator (LQR) Linear Quadratic Regulator (LQR) Linear Quadratic Regulator (LQR) Linear Quadratic Regulator (LQR) –––– IIIIIIIIIIII

Prof. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant Padhi

Dept. of Aerospace Engineering

Indian Institute of Science - Bangalore

Optimal Control, Guidance and Estimation

OPTIMAL CONTROL, GUIDANCE AND ESTIMATION

Prof. Radhakant Padhi, AE Dept., IISc-Bangalore2

Outline

� Review of LQR solution using State

Transition Matrix (STM) Approach

� Application of STM for tactical missile

guidance

� Frequency-domain interpretation of

LQR and Robustness property

2

Solution of LQR Problems using State Solution of LQR Problems using State Solution of LQR Problems using State Solution of LQR Problems using State

Transition Matrix (STM) ApproachTransition Matrix (STM) ApproachTransition Matrix (STM) ApproachTransition Matrix (STM) Approach






STM Solution of LQR Problems(1) Soft constraint problems

� Performance Index (to minimize):

� Path Constraint:

� Boundary Conditions:

( )( )

( )( )

0

,

1 1

2 2

f

f

t

T T T

f f f

t

L X UX

J X S X X Q X U RU dt

ϕ

= + +∫��

X A X BU= +ɺ

( )

( )00 :Specified

: Fixed, : Freef f

X X

t X t

=

3




� Terminal penalty:

� Hamiltonian:

� State Equation:

� Costate Equation:

� Optimal Control Eq.:

� Boundary Condition:

( ) ( )1

2

T T TH X Q X U RU AX BUλ= + + +

( ) ( )1

2

T

f f f fX X S Xϕ =

X AX BU= +ɺ

( ) ( )/T

H X QX Aλ λ= − ∂ ∂ = − +ɺ

( ) 1/ 0 TH U U R B λ−∂ ∂ = ⇒ = −

( )/f f f fX S Xλ ϕ= ∂ ∂ =




( )

1

1

11 12

21 22

Substituting in the state equation

we can write:

The solution dictates that:

,

a

f

T

T

aT

A

f

t t

U R B

X XA BR BXA

Q A

X X Xt t

λ

λ λλ

ϕ ϕϕ

ϕ ϕλ λ λ

−

−

= −

− = =

− −

= =

ɺ

ɺ��

ft

4




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

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

11 12

11 12

11 12

21 22

21 22

However, we know that

So we can write:

, ,

, ,

, , ,

Similarly,

, ,

, , ,

f f f

f f f f

f f f f f

f f f f f f

f f f f

f f f f f f

S X

X t t t X t t

t t X t t S X

t t t t S X t t X

t t t X t t

t t t t S X t t X

λ

ϕ ϕ λ

ϕ ϕ

ϕ ϕ

λ ϕ ϕ λ

ϕ ϕ

=

= +

= +

= + =

= +

= + = Λ

X




( ) ( )( ) ( )

( )( )

In summary, we can write:

,

,

At , we must satisfy the B.C.

,This dictates that:

,

f f

f f

f

f f

f f f

f f

f f f

X t t t X

t t t X

t t

X X

S X

t t I

t t S

λ

λ

=

= Λ

=

=

=

=

Λ =

X

X

5




( ) ( )

[ ]2 2

22

However, we know

Substituting the solution froms of and ,

we get

This leads to

: We can find the closed f

a

f f

a

ff

a n nn nn n

XXA

X t t

X XA

XX

A

λλ

λ

×××

=

=

ΛΛ

= ΛΛ

X X

X X

Note

ɺ

ɺ

ɺ

ɺ

ɺ

ɺ

0

orm solution now.

Alternatively (less preferable), we can integrate

this system backwards from to ;ft t




( ) ( ) ( )

( ) ( ) ( )

( ) ( ) ( )

0

1

0 0 0 0

1

0 0

1

0 0

is not known.

However, at , we have:

, , . . ,

Substituting for we get:

, ,

t , ,

f

f f f f

f

f f

f f

X

t t

X t t t X i e X t t X

X

X t t t t t X

t t t t Xλ

−

−

−

=

= =

=

= Λ

Problem :

X X

X X

X

6




( ) ( ) ( ) ( )

( ) ( ) ( ) ( )( )

( )

1

11

0 0 0

0

Finally,

, ,

This gives a "sample-data feedback law" (where the most

recent sample time is ). If a continous determi

T

T

f f

K t

U t R t B t t

R t B t t t t t X K t X

t

λ−

−−

= −

= − Λ = − X��

( ) ( ) ( )

nation of

the state is made, the most recent sample time is the

current time. In that case:

U t K t X t= −



STM Solution of LQR Problems(2) Hard constraint problems: Zero terminal error

( )

( )

( )( )

( )( )

( ) ( ) ( )

0 0

1

1

, : Given

1

2

, 0, 1, ,

1

2

f

o

f

o

t

T T

t

f

f i f

n f

tqT T T

i i f

i t

X AX BU X t X

J X QX U RU dt

x t

X t x t i q n

x t

J x t X QX U RU AX BU X dtυ λ=

= + =

= +

= = = ≤

= + + + + −

∫

∑ ∫

ɺ

⋮ ⋯

ɺ

7




( )

( )( )

1

0 0

System dynamics:

Boundary conditions:

: Given

0, 1, ,

0, ( 1), ,

T

aT

i f

i f

X XA BR BXA

Q A

X t X

x t i q

t i q n

λ λλ

λ

− − = =

− −

=

= =

= = +

TPBVP Formulation

ɺ

ɺ

⋯

⋯

( )

STM Solution:

,

f

f

t

X Xt tϕ

λ λ

=




( )( )

( )( )

( ) ( )

( ) ( )

1 q 1

Collecting the appopriate entries of the matrix,

the general solution can be written as:

,

,

where

, , | ( ), , ( )

, : STM for

, : STM for

f

f

T

q f n f

f

f

t tX t

t t t

x t x t

t t X t

t t t

ϕ

µ

λ µ

µ υ υ

λ

+

=

Λ

Λ

X

X

≜ ⋯ ⋯

8




[ ]

( )[ ]

[ ]( ) ( ) ( )

( )[ ] ( )

[ ]( )

2 22 12 1

0

In summary, we have:

With:

0,

0 |

| 0,

0

These equations can be integrated backwards from to .

Preferab

a n nnn

q n

f f

n q n qn q q

q n qq q

f f

n q n

f

A

t tI

It t

t t

×××

×

− × −− ×

× −×

− ×

= ΛΛ

=

Λ =

X X

X

ɺ

ɺ

ly, one should find the closed form solution.




( )

( ) ( )

( ) ( ) ( ) ( )

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

0 0

1

0 0

1

0 0

11

0 0

Clearly, at , if , is non-singular, then

,

In that case,

, ,

, ,

Solution form is same. However, the B.C. is

differe

f

f

f f

T

f f

t t t t

X t t X t

t t t t t X t

U t R t B t t t t t X t

µ

λ

−

−

−−

=

=

= Λ

= − Λ

X

X

X

Note :

nt and hence solution is different.

9




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

( )

( ) ( )

( ) ( )( )

( )

0

11

0

For continuous data (i.e )

, ,

Problem: As , , ,

However, , is singular.

Hence, as .

This makes sense as

T

f f

K t

f f f f

f f

f

t t

U t R t B t t t t t X t

K t X t

t t t t t t

t t

K t t t

−−

→

= − Λ

= −

→ →

→ ∞ →

X

X X

X

��

we are insisting on zero terminal error.

Example: Optimal Missile Guidance Example: Optimal Missile Guidance Example: Optimal Missile Guidance Example: Optimal Missile Guidance

Through STM Solution of LQRThrough STM Solution of LQRThrough STM Solution of LQRThrough STM Solution of LQR




10



Fundamental Problem of

Tactical Missile Guidance

PN Guidance: M Ma NV λ= ɺ

Ma

LOS

MV

TV

λM

θ

Tθ

M

T



Optimal Missile Guidance

Through LQR

� System dynamics

� Cost Function

v a

y v

=

=

ɺ

ɺ

( )2 21 1

2 2

ft

f

t

J c y a dt= + ∫

Vσ

LOS

a

v

11




Through LQR

� System matrices in the LQR formulation

� Time-to-go definition

[ ] ( ), Lateral acceleration

0 0 1,

1 0 0

0 0 0 0, 1,

0 0 0

T

f

X v y U a

A B

Q R Sc

= =

= =

= = =

( )go ft t t= −




Through LQR

( ) ( )

( )

1

0 0 1 0

1 0 0 0

0 0 0 1

0 0 0 0

Solution:

( ) ( )( ),

( ) ( )( )

To compute , lets compute

a

a

T

T

A

A

f f

f f

f f

X XX A BR B

Q A

X t X tX tt t t t

t tt

t A

λ λλ

ϕ ϕλ λλ

ϕ

−

− − = = −− −

= = −

ɺ

ɺ��

��

2 3 4 5, , , .a a a aA A A etc…

12




Through LQR

2

3

4

0 0 1 0 0 0 1 0 0 0 0 1

1 0 0 0 1 0 0 0 0 0 1 0

0 0 0 1 0 0 0 1 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 1 0 0 1 0 0 0 0 0

0 0 1 0 1 0 0 0 0 0 0 1

0 0 0 0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0

0 0 0 1

0 0

a

a

a

A

A

A

− − − = = − −

− − = = −

=

0 0 1 0 0 0 0 0

1 0 0 0 0 0 0 0

0 0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

− = −




Through LQR

( )

2

2 32 32 3

1 0 / 2

1 / 2 / 6

2! 3! 0 0 1

0 0 0 1

aA t

a a a

t t

t t tt tt e I A t A A

tϕ

−

− = = + + + = −

Hence,

( ) ( )

2

2 3

1 0 ( ) ( ) / 2

( ) 1 ( ) / 2 ( ) / 6,

0 0 1 ( )

0 0 0 1

f f

f f f

f f

f

t t t t

t t t t t tt t t t

t tϕ ϕ

− − −

− − − − = − = −

13




Through LQR

( )Define . Thengo f

t t t= −

( )

2

2 3

1 0 / 2

1 / 2 / 6,

0 0 1

0 0 0 1

go go

go go go

f

go

t t

t t tt t

tϕ

− − − = −

11 12 11 12

( , )

( ) ( , ) ( , ) ( , ) ( , )

f

f f f f f f f f

t t

X t t t X t t t t t t S Xϕ ϕ λ ϕ ϕ = + = + X

��




Through LQR

11 12

2

2 3

2

3

21 22

( , ) ( , ) ( , )

1 0 / 2 0 0

1 / 2 / 6 0

1 / 2

1 / 6

( , ) ( , ) ( , )

0 0 1 0 0 0

0 0 0 1 0 0

f f f f

go go

go go go

go

go go

f f f f

go go

t t t t t t S

t t

t t t c

ct

t ct

t t t t t t S

t ct

c c

ϕ ϕ

ϕ ϕ

= +

= + − − −

=

− −

Λ = +

− − = + =

X

14




Through LQR

( ) ( ) ( )

( ) ( ) ( )

[ ]

( )

12

1

3

1

12

3

2

3 3

Hence

1 / 20( , ) ( , )

1 / 60

Finally

1 / 20 ( )1 0

1 / 60 ( )

1 1 1

3 3

gogo

f f

go go

T

gogo

go go

go go

go go

ctctt t t t t X t X t

t ctc

U t a t R B t

ctct v t

t ctc y t

t tv t

t tc

λ

λ

−

−

−

−

− = Λ = − −

= = −

− = − − −

= − −

+ +

X

( )1

y t

c



Final Latax Expression

� Latax solution:

� Special case ( ):

( ) ( ) ( )2

3 31 1 1 1

3 3

go go

go go

t ta t v t y t

t tc c

= − −

+ +

c → ∞

( )( ) ( )

23

go go

v t y ta t

t t

= − +

15



Correlation Between Linear Optimal

Guidance and PN Guidance

( ) ( )

( )( ) ( ) ( )

( ) ( )

2 2

2

( ) 1 ( )

Changing sign and differentiating both sides,

( 1)1 1

Hence

3 3

f f

f

ff f

f f

y t y t

VV t t t t

t t y y v y

V V t tt t t t

v yV

t t t t

σ

σ

σ

− = =

− −

− − − = − = − + −− −

= − + − −

ɺɺ

ɺ

( )Lets assume: 0, : is constantVσ → Note

Sign convention for :

anti-clockwise positive

clockwise negative

σ ⇒ ⇒

Note :

Vσ

LOS

a

v



Correlation Between Linear Optimal

Guidance and PN Guidance

� Comparing the Expressions:

� PN is an Optimal Guidance Provided:

• Linearized engagement dynamics is considered

• Non-maneuvering and stationary (slow-moving) targets

• LOS angle is not high

• Induced drag minimization (through Latax minimization) issue is ignored

• N = 3 is used as the navigation constant

3a V σ= ɺ

(This is the optimal guidance law, which is same as PN guidance law with N = 3 )

16

Frequency Domain Interpretation of Frequency Domain Interpretation of Frequency Domain Interpretation of Frequency Domain Interpretation of

LQR and Robustness MarginsLQR and Robustness MarginsLQR and Robustness MarginsLQR and Robustness Margins






Frequency Domain Interpretation

( ) ( )

[ ]

1

o

Optimal Trajectory

Assumtions: (i) , is stabilizable

(ii) , is observable

Open-Loop Characteristic Polynomial

TX A BR B P X A BK X

A B

A Q

s sI A

−= − = −

∆ = −

ɺ

17




( )

[ ] [ ] [ ]

[ ] [ ]

[ ]

1

1

1

Closed-Loop Characteristic Polynomial

=

c

o

s sI A BK sI A BK

sI A BK sI A sI A

I K sI A B sI A

I K sI A B s

−

−

−

∆ = − − = − +

= − + − −

+ − −

= + − ∆

[ ]1

Loop Gain Matrix:

K sI A B−

− −[ ]1

Return Difference Matrix:

I K sI A B−

+ −



Kalman Equation

in Frequency Domain

[ ]

( )

1

1

Algebraic Riccati Equation:

Add and subtract :

Define

T T

T T

T T

PA A P PBR B P Q

sP

sP PA sP A P PBR B P Q

P sI A sI A P K RK Q

s s

−

−

− − + =

− − − + =

− + − − + =

Φ ≜ [ ]

( ) [ ]

( ) [ ]( ) [ ]( )( ) ( )

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

1

1

1 11

Then

Pre-multiply by and Post-multiply by

TTT T

T T

T T T T T T T T

I A

s sI A

s sI A sI A sI A

B s s B

B s PB B P s B B s K RK s B B s Q s B

−

−

− −−

−

Φ − − −

Φ − − − = − − = − −

Φ − Φ

Φ − + Φ + Φ − Φ = Φ − Φ

≜

≜

18




( ) ( ) ( ) ( )

[ ]

1

1 1

However,

. .

Adding on both sides, after some algebra it can be shown that

. .

T

T

T

TT T

T T

K R B P

i e RK B P

K R PB

R

B s Q s B R I K s B R I K s B

i e

B sI A Q sI A B

−

− −

=

=

=

Φ − Φ + = + Φ − + Φ

− − −

[ ] [ ]1 1

This is called as the "Kalman Equation" in the frequency domain.

T

R

I K sI A B R I K sI A B− −

+

= + − − + −



Example: Double Integrator

( )

1 2

2

2 2 2

1 20

1

2

Identitfy the Various Matrices

0 1 0 1 0 ; ;

0 0 1 0 1

x x

x u

J x x u dt

A B b Q

∞

=

=

= + +

= = = =

∫

System Dynamics :

Performance Index :

Solution :

ɺ

ɺ

; 1R r

= =

19




[ ] [ ]

[ ]

[ ]

2 21 1

1

2

11 12

With , , and

The Kalman equation

10

and ; 1 1

T T

Q R I C D I B I j s

I K sI A B I C sI A B

ssI A

s s

K k k

ω

− −

−

= = = = = =

+ − = + −

− =

=




[ ] ( ) [ ] ( )

[ ] ( ) ( )

2 2

11 12 11 12

2 2

1 1 1 1

0 0So 1 1

1 11 10 0

1 1 1 1

1 0 0 1 0 1

0 1 11 10 0

A ge

s ss sk k k k

s s

s ss s

s s

− − + + =

−

− − +

−

neral a set of Algebraic Equations are obtained by equating

the powers of .s

20




( )2 2

11 12 112 4 2 4

11 12

*

* *

A single Scalar Equation:

1 1 1 1 1 2 1

gives 1, 3

Optimal feedback control:

1 3

:

This

k k ks s s s

k k

u

u KX

+ − + = − +

= =

= − = −

Note

can be verified with Algebraic Riccati Equation formulation.



LQR Design:

Robustness of Closed Loop System

� Gain Margin:

� Phase Margin:

Reference: D. S. Naidu, Optimal Control Systems, CRC Press, 2003 (Chp. 4, pp.184-187)

1Minimum , Maximum

2= = ∞

060≥

Note: Margins are valid only with exact state feedback.

21



Thanks for the Attention….!!

Optimal Control, Guidance and Estimation...Optimal Missile Guidance Through LQR System dynamics Cost Function v a y v = = ɺ ɺ 1 1( )2 2 2 2 tf f t J c y a dt= + ∫ σ V LOS a v

Documents