Optimal Control, Guidance and Estimation -...
Transcript of 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
Prof. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore4
A Practical Control System
Plant
State
Estimation
Controlleroutput
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore5
Philosophy of LQG Design
� Controller: Linear Quadratic Regulator (LQR)
� State Estimation: Kalman Filter
LQG Design
LQ Controller
(LQR)LQ Estimator(Kalman Filter)
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore6
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≥ >
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore7
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−+ − + =
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore8
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 = =
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore9
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= + + −ɺ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore10
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= −
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore11
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
= + + = − +
= − − +
= − + +
= − + −
− = + − −
ɺ
ɶ
ɶ
ɺɶ ɶ
ɺ
ɶɺɶ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore12
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
Prof. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore14
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
−
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore15
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δ
= + +
ɺ
ɺ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore16
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
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore17
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
−
∞
+ − =
= = +∫
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore18
Results
( )Angle of attach Vs. Timeα ( )Pitch rate Vs. Timeq
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore19
Results
Control Vs. Time
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore20
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
Prof. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore22
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ϕ ν ψ λ = + + + − ∫ ɺ
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore23
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λ+≜
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore24
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?
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore25
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δ ψ δ δψ=
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore26
Neighbouring Optimal Control
[ ] [ ]
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:
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore27
Neighbouring Optimal Control
( ) ( )( ) ( )
( )
( )
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−
=
−≜
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore28
Neighbouring Optimal Control
( ) ( )
( ) ( )
( )
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ϕ ν ψ ψ= + = =
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore29
Neighbouring Optimal Control
( ) ( )( ) ( )
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δ+ − + + + − + =ɺ ɺ ⋯⋯
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore30
Neighbouring Optimal Control
( )( )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ϕ ν ψ ψ= + = =
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore31
Neighbouring Optimal Control
( ) ( )
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 δψ−
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore32
Neighbouring Optimal Control
( ) ( )
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
δ δψ
δ δψ
− − − −
= − + − −
= − −
������������� �������
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore33
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
Prof. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant PadhiProf. Radhakant Padhi
Dept. of Aerospace Engineering
Indian Institute of Science - Bangalore
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore35
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”)
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore36
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)
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore37
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.
OPTIMAL CONTROL, GUIDANCE AND ESTIMATION
Prof. Radhakant Padhi, AE Dept., IISc-Bangalore38
Thanks for the Attention….!!