Optimal Control, Guidance and Estimation -...

Lecture Lecture Lecture Lecture –––– 33333333

LQG Design; LQG Design; LQG Design; LQG Design; NeighbouringNeighbouringNeighbouringNeighbouring Optimal ControlOptimal ControlOptimal ControlOptimal Control

& Sufficiency Condition& Sufficiency Condition& Sufficiency Condition& Sufficiency Condition

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

References

• A. E. Bryson and Y-C Ho: Applied Optimal

Control, Taylor and Francis, 1975.

� Brian D.O. Anderson and J. B. Moore: Optimal Control – Linear Quadratic methods, Prentice Hall , 1989.

� R. F. Stengel: Optimal Control and

Estimation, Dover Publications, 1994.

Robust Control Design Through Linear Robust Control Design Through Linear Robust Control Design Through Linear Robust Control Design Through Linear

Quadratic Gaussian Quadratic Gaussian Quadratic Gaussian Quadratic Gaussian ((((LQG) DesignLQG) DesignLQG) DesignLQG) Design






A Practical Control System

Plant

State

Estimation

Controlleroutput



Philosophy of LQG Design

� Controller: Linear Quadratic Regulator (LQR)

� State Estimation: Kalman Filter

LQG Design

LQ Controller

(LQR)LQ Estimator(Kalman Filter)



LQR Design: Summary

� Performance Index (to minimize):

� System dynamics:

� Boundary conditions:

( )0

1

2

T TJ X QX U RU dt

∞

= +∫

X AX BU= +ɺ

0(0) : SpecifiedX X=

where 0 (psdf), 0 (pdf)Q R≥ >



LQR Design: Summary

� Optimal control

where P is the solution of

(Algebraic Riccati Equation)

( )1 TU R B P X KX−= − = −

1 0T TPA A P PBR B P Q−+ − + =



Kalman Filter Design: Summary

� Goal: To obtain an estimate of the state vector

using the state dynamics as well as a ‘sequence of measurements’ as accurate as possible.

� System Dynamics:

where ‘W ’ is the process noise vector.

� Measured Output:

� Assumption: W and V are “white noise”

� Definitions:

X AX BU GW= + +ɺ

Y C X V= +

,T TQ E WW R E VV = =



Kalman Filter Design: Summary

� Initialize:

� Solve for Riccati matrix P from the Filter ARE:

� Compute Kalman Gain:

� Propagate the Filter Dynamics:

ˆ (0)X

1 0T T TAP PA PC R CP GQG

−+ − + =

1T

eK PC R

−=

ˆ ˆ ˆ( )eX AX BU K Y CX= + + −ɺ



LQG Design

� Design a deterministic LQR control , assuming perfect knowledge of the states and assuming that the plant is not affected by process and sensor noises.

� Design a Kalman Filter to estimate the states and compute the control using this estimated states . This design philosophy is called Linear Quadratic Gaussian (LQG) design.

� Justification for the LQG design comes from the “Separation Principle”.

U K X= −

ˆU K X= −



Separation Theorem in LQG Design

( )( )

( ) ( )

System dynamics:

ˆ

Error dynamics in Kalman filter:

Combined dynamics:

0

e e

e e

X AX BU GW AX BKX GW

AX BK X X GW

A BK X BK X GW

X A K C X GW K V

X A BK BK GWX

A K C GW K VXX

= + + = − +

= − − +

= − + +

= − + −

− = + − −

ɺ

ɶ

ɶ

ɺɶ ɶ

ɺ

ɶɺɶ



Separation Theorem in LQG Design

( )

( )( )

( )

Combined expected dynamics:

0

0

Poles of the combined

e e

e

X A BK BK GWXE E E

A K C GW K VXX

E X E XA BK BKd

A K Cdt E X E X

− = + − −

− = −

ɺ

ɶɺɶ

ɶ ɶ

( )( )

( ) ( )

expected dynamics are dictated by the following

characteristic equation:

0 0

e e

e

A BK BK sI A BK BKsI

A K C sI A K C

sI A BK sI A K C

− − − − − = − − −

= − − − − = 0

Hence, the poles of this system are poles of the controller and the poles of the filter.

Hence, the controller and the filter can be designed separately!

Short Period Control Using LQG Design Short Period Control Using LQG Design Short Period Control Using LQG Design Short Period Control Using LQG Design






System Dynamics

[ ]

-16 longitudinal dynamics:

Angle of attack

Pitch rate

= Elevator deflection

= Vertical wind gust disturbance

1.01887 0.90506 0.00215; ;

0.82225 1.07741 0.17555

e g

T

e

g

F X AX B Gw

X q

q

w

A B G

δ

α

α

δ

= + +

=

=

=

− − = = = − −

ɺ

0.00203

0.00164

−



Augmented System with Shaping

Filter

The shaping filter dynamic: Z where, white noise

where, Gust noise

0 1 0; ; 2.

0.0823 0.5737 1

w w

g w g

w w w

A Z B w w

w C Z w

A B C

= =

= =

= = = − −

+

ɺ

[ ]

( )

1728 13.1192

The augmented system dynamics :

0 Z 0Z

20.2The elevator actuator with transfer function:

s+20.2

w w

e

w w

A GC CDX BXw

A Bδ

= + +

ɺ

ɺ



Augmented System with Shaping

Filter

1 1

2 2

-1.01887 0.90506 0.00441 0.02663 -0.00215

0.82225 -1.07741 -0.00365 -0.02152 -0.17555

= 0 0 0 1 0

0 0 0.0823 0.5737 0

0 0 0 0 20.2

aug aug aug aug aug

e e

X A X B u G w

q q

z z

z z

α α

δ δ

= + +

− − −

ɺ

ɺ

ɺ

ɺ

ɺɺ

0 0

0 0

0 0

0 1

20.2 0

Elevator actuator input

15.87875 1.48113 0 0 0The measured output: Y

0 1 0 0 0

Normal acceleration 15.87875 1.48113

Mea

z

aug aug

z

u w

u

nCX V X V

q

n q

V

α

+ +

=

= = + = +

= = +

= surement noise vector



Covariance Matrices and

Controller design

2

1

0.64 0The measurement noise covariance:

0 0.49

The process noise covariance: 25

The controller design is based on LQR control design.

will fi

T

aug lqr lqr lqr aug

lqr

R

Q

u KX R B P X

P

σ

−

=

= =

= − = −

( )0

1

nd out from the Algebric Riccati Equation(ARE):

0

1where the cost function

2

T T

lqr aug aug lqr lqr aug lqr lqr lqr

T T

aug lqr aug lqrt

P A A P P B R B P

J X Q X u R u dt

−

∞

+ − =

= = +∫



Results

( )Angle of attach Vs. Timeα ( )Pitch rate Vs. Timeq



Results

Control Vs. Time



Problem of LQG design &

Solution

� Problem

• Loss of Robustness

� Solution

• LTR Design

• Often implemented as LQG/LTR design

� Extension Ideas: 2 / DesignsH H∞

Neighbouring Optimal Control DesignNeighbouring Optimal Control DesignNeighbouring Optimal Control DesignNeighbouring Optimal Control Design






Optimal Control Problem

� Performance Index (PI):

� Path Constraint:

� Boundary Conditions:

� Augmented PI:

( ) ( )0

, ,

ft

f

t

J X L t X U dtϕ= + ∫

( ), ,X f t X U=ɺ

( ) 00 :SpecifiedX X=

( ) ( ): Fixed, 0 equationsf f

t X qψ =

( ) ( ) ( )0

ft

T T

f f

t

J X X L f X dtϕ ν ψ λ = + + + − ∫ ɺ



Necessary Conditions of

Optimality

� State Equation

� Costate Equation

� Optimal Control Equation

� Boundary Condition

( ), ,H

X f t X Uλ

∂= =

∂ɺ

H

Xλ

∂ = −

∂ ɺ

0H

U

∂=

∂

,T

f

f fX X

ϕ ψλ ν

∂ ∂= +

∂ ∂( )0 0

:FixedX t X=

( )Hamiltonian: TH L fλ+≜



Neighbouring Optimal Control:

Problem Formulation

( )

Assumption:

We have determined a control solution , satisfying all necessary conditions.

Let us consider "small perturbations" in the extremal path, produced by small

perturbations in the initial state

U t

( )

0 and terminal condition .

Questions:

1) Under what conditions is to be a local optimum?

2) Can we find the neighbouring optimal solution (using the available

optimal solution) in a

X

U t

δ δψ

guaranteed

n "efficient manner"?

3) Under what condition(s), such a neighbouring solution exists?



Neighbouring Optimal Control

Note that the available control solution satisfies all the necessary conditions

of optimality; i.e. it makes 0. Hence, to address our problem,

we will have to consider the "second variation", which

Jδ =

( )( )

[ ] [ ]

0

2

is given by:

1 1

2 2

With respect to the perturbations, the deviation dynamics can be written as:

Similarly, the deviation

f

f

t

XX XUT T T T

XX XX t

UX UUt

X U

H H XJ X X X U dt

H H U

X f X f U

δδ δ ϕ ν ψ δ δ δ

δ

δ δ δ

= + +

= +

∫

ɺ

( )0

in boundary conditions can be written as:

:Specified, :Specifiedf

X tX Xδ ψ δ δψ=




[ ] [ ]

The problem appears as a "linear quadratic regulator

(LQR) problem with cross-product terms" between

the state and control.

1) State Equation:

2) Costate Equation:

X UX f X f Uδ δ δ= +ɺ

=

3) Optimal Control Equation: 0

XX XU X

T

XX XU X

UU UX U

H X H U H

H X H U f

H U H X H

λ

λ

δλ δ δ δλ

δ δ δλ

δ δ δλ

= − − −

− − −

= + +

ɺ

( )( )[ ]0

=

4) Boundary Conditions: =

: Specified, : Specified

f

f

T

UU UX U

T T

f XX X XX t

X t

H U H X f

X d

X X

δ δ δλ

δλ ϕ ν ψ δ ψ ν

δ ψ δ δψ

+ +

+ +

=

Observations:

Necessary Conditions:




( ) ( )( ) ( )

( )

( )

1

1

1

Optimal Control Equation:

State and Costate Equations:

where

T

UU UX U

T

X U UU UX

T

U UU U

U H H X f

A t B t XX

C t A t

A t f f H H

B t f H f

δ δ δλ

δδ

δλδλ

−

−

−

= − +

− = − −

−

ɺ

ɺ

≜

≜

( ) 1

(Note: )

T

XX XU UU UX

B B

C t H H H H−

=

−≜




( ) ( )

( ) ( )

( )

Seek the solution of and as

, 0 (pdf matrix)

Note: and are infinitesimal "constant vectors"

T

P t X R t d P

R t X Q t d

d

δλ δψ

δλ δ ν

δψ δ ν

δψ ν

= + >

= +

( ) ( ) ( )( )

( ) ( ) [ ]

Boundary Conditions:

=

This gives

f

f

T T

f f f f XX X XX t

T

f f f X t

P t X R t d X d

R t X Q t d X

δλ δ ν ϕ ν ψ δ ψ ν

δψ δ ν ψ δ

+ = + +

= + =

( ) ( )( ) ( ) ( ) ( ), , 0ff

T T

f XX X f X fX tt

P t R t Q tϕ ν ψ ψ= + = =




( ) ( )( ) ( )

Next, differentiating and , we obtain

0

However,

Hence

T T

T

P X P X R d

R X R X Q d

A t B t XX

C t A t

δλ δψ

δλ δ δ ν

δ δ ν

δδ

δλδλ

δλ

= + +

= + +

− = − −

=

ɺ ɺ ɺ ɺ

ɺɺ ɺ

ɺ

ɺ

ɺ

( )

( ) ( )( )

T

P X P X R d

P X P A X B R d

C X A P X R d P X P A X B P X R d R d

δ δ ν

δ δ δλ ν

δ δ ν δ δ δ ν ν

+ +

= + − +

− − + = + − + +

ɺ ɺ ɺ

ɺ ɺ

ɺ ɺ

( )Note: and are constant vectorsdδψ ν

( ) ( ) 0 (1)T TP PA PBP A P C X R PBR A Rδ+ − + + + − + =ɺ ɺ ⋯⋯




( )( )Similarly, 0

T T

T T

R X R X Q d

R X R A X B P X R d Q d

δ δ ν

δ δ δ ν ν

= + +

= + − + +

ɺɺ ɺ

ɺɺ

( )0 (2)T T TR R A BP X Q R BR dδ ν = + − + −

ɺɺ ⋯⋯

From equations (1) and (2), we obtain

0

0

0

T

T

T

P PA PBP A P C

R PBR A R

Q R BR

+ − + + =

− + =

− =

ɺ

ɺ

ɺ

Differential equations and boundary conditons are now available for solving

, , matrices.P Q R ( ) ( )( ) ( ) ( ) ( ), , 0ff

T T

f XX X f X fX tt

P t R t Q tϕ ν ψ ψ= + = =




( ) ( )

0

1

0 0 0

After integrating the equations from to , the value can be computed as

Hence, the existence of for all depends on the non-sing

f

T

t t d

d Q t R t X

d

ν

ν δψ δ

ν δψ

− = −

( )

( )0

0

ularity of .

If " is singular", then the optimization problem is said to be "abnormal"

and in that case the neighbouring optimal solution doesnot exist.

However, assuming the problem to be normal

Q t

Q t

( )

( )

0

0 0 0 0

1

0 0 0 0 0 0

1

0 0 0 0 0 0

(i.e. to be non-singular),

T

T

Q t

d P X R d

P X R Q R X

P R Q R X R

λ δ ν

δ δψ δ

δ

−

−

= +

= + −

= − + 1

0Q δψ−




( ) ( )

0

1

Note that was evaluated at . In terms of a feedback law, however, can be

evaluated at the current time as

Finally, the control expres

T

d t d

t

d Q t R t X

ν ν

ν δψ δ− = −

( )

1

1

1 1 1

sion is

T

UU UX U

T

UU UX U

T T T

UU UX U UU U

U H H X f

H H X f P X R d

H H f P X H f RQ R

δ δ δλ

δ δ ν

δ δψ δ

−

−

− − −

= − +

= − + +

= − + − −

( ) ( )

( ) ( )1 2

1 1 1 1

1 2

: A linear feedback law

T T T T

UU UX U U UU U

K t K t

X

H H f P f RQ R X H f RQ

K t X K t

δ δψ

δ δψ

− − − −

= − + − −

= − −

��



Neighbouring Optimal Control:

Simplified Version

( )[ ]

( )2

In many problems, the constraint is not critical, and hence, is not imposed.

Assuming that 0 (a psdf matrix), the expression for becomes

1 1

2 2

f

f

XX ft

XX XUT T T

f f f

UX

X C

S J

H HJ X S X X U

H

ψ

ϕ δ

δ δ δ δ δ

=

= ≥

= + 0

This leads to a regular LQR problem with cross-product term....we know its solution!

Furthermore, as far as the neighboring optimal solution is concerned, to simplify

numerica

ft

UUt

Xdt

H U

δ

δ

∫

l computation, it is imposed that with artificial increase in weights

on and (to have a somewhat similar effect as a finite-time problem).

In that case, the problem boils down to the regula

ft

X Uδ δ

→ ∞

r time LQR problem, which is

solved by solving the Algebraic Riccati Equation (ARE) online (SDRE formulation).

∞ −

Sufficiency Condition for OptimalitySufficiency Condition for OptimalitySufficiency Condition for OptimalitySufficiency Condition for Optimality






Sufficiency Condition

� In weak sense:

� In strong sense:

when and are small.X Xδ δ ɺ

when is small.Xδ

Here the conditions in “weak sense” only are summarized.

(see References for conditions in “strong sense”)



Sufficiency Condition

( )

( )

0The neighbouring optimum paths exist in a weak sense, if , ,

the following conditions are satisfied:

(1) 0 (a pdf matrix) : Convexity condition

(2) 0 (a ndf matrix) : Normality c

f

UU

t t t

H t

Q t

∀ ∈

>

<

( ) ( ) ( ) ( )1

ondition

(3) is finite: Jacobi condition

Note that condition (3) is a substitute for the exact condition, which

requires that there is no "conjugate point" on the optimal path.

TP t R t Q t R t− −

Theorem-1 (existence for neighbouring optimum path)

The conditions in Theorem - 1, along with the necessary conditions,

form a set of sufficient conditions for a trajectory to be local minimum.

Theorem-2 (sufficiency condition for minimization)



References

• A. E. Bryson and Y-C Ho: Applied Optimal

Control, Taylor and Francis, 1975.

� Brian D.O. Anderson and J. B. Moore: Optimal Control – Linear Quadratic methods, Prentice Hall , 1989.

� R. F. Stengel: Optimal Control and

Estimation, Dover Publications, 1994.



Thanks for the Attention….!!

Optimal Control, Guidance and Estimation -...

Documents

Transcript of Optimal Control, Guidance and Estimation -...