Filters, Cost Functions, and Controller Structures

Filters, Cost Functions, and

Controller Structures Robert Stengel Optimal Control and Estimation MAE 546 Princeton University, 2018 ! Dynamic systems as low-pass ﬁlters ! Frequency response of dynamic systems ! Shaping system response ! LQ regulators with output vector cost functions ! Implicit model-following ! Cost functions with augmented state vector

1 ∞ min J = min ⎡ΔxT (t)QΔx(t) + ΔuT (t)RΔu(t)⎤dt Δu Δu ∫ ⎣ ⎦ 2 0 subject to Δx!(t) = FΔx(t) + GΔu(t)

First-Order Low-Pass Filter

2 Low-Pass Filter Low-pass ﬁlter passes low-frequency signals and attenuates high-frequency signals 1st-order example

Δx!(t) = −aΔx(t) + aΔu(t) a = 0.1, 1, or 10

Laplace transform, with x(0) = 0 a Δx(s) = Δu(s) (s + a) Frequency response, s = jω a Δx( jω ) = Δu( jω ) ( jω + a) 3

Response of 1st-Order Low-Pass Filters to Step Input and Initial Condition Δx!(t) = −aΔx(t) + aΔu(t) a = 0.1, 1, or 10

Smoothing effect on sharp changes 4 Frequency Response of Dynamic Systems

Response of 1st-Order Low-Pass Filters to Sine-Wave Inputs

⎧ sin(t) Input ⎪ Δu(t) = ⎨ sin(2t) ⎪ sin 4t ⎩⎪ ( )

Output

x t = −10x t + 10u t ( ) ( ) ( )

x! t = −x t + u t ( ) ( ) ( )

6 x t = −0.1x t + 0.1u t ( ) ( ) ( ) Response of 1st-Order Low- Pass Filters to White Noise

Δx!(t) = −aΔx(t) + aΔu(t) a = 0.1, 1, or 10

Relationship of Input Frequencies to Filter Bandwidth

⎧ sin(ωt) ⎪ Δu(t) = ⎨ sin(2ωt) ⎪ sin 4ωt ⎩⎪ ( )

BANDWIDTH Input frequency at which output magnitude = –3dB

How do we calculate frequency response?

8 Bode Plot Asymptotes, Departures, and Phase Angles for 1st-Order Lags 10 100 100 H red ( jω ) = H ( jω ) = H green ( jω ) = jω +10 blue jω +10 jω +100 ( ) ( ) ( ) • General shape of amplitude ratio governed by asymptotes • Slope of asymptotes changes by multiples of ±20 dB/dec at poles or zeros of H(jω) • Actual AR departs from asymptotes • AR asymptotes of a real pole – When ω = 0, slope = 0 dB/ dec – When ω ≥ λ, slope = –20 dB/ dec

• Phase angle of a real, negative pole – When ω = 0, ϕ = 0° – When ω = λ, ϕ =–45° 9 – When ω -> ∞, ϕ -> –90°

2nd-Order Low-Pass Filter

Δ!x! t = −2ζω Δx! t −ω 2Δx t +ω 2Δu t ( ) n ( ) n ( ) n ( ) Laplace transform, with I.C. = 0

2 ω n Δx(s) = 2 2 Δu(s) s + 2ζω ns +ω n Frequency response, s = jω

ω 2 x j n u j Δ ( ω ) = 2 2 Δ ( ω ) ( jω ) + 2ζω n ( jω ) +ω n ( jω )Δu jω ! H ( ) 10 2nd-Order Step Response Increased damping decreases oscillatory over/undershoot Further increase eliminates oscillation

Amplitude Ratio Asymptotes and Departures of Second-Order Bode Plots (No Zeros)

• AR asymptotes of a pair of complex poles – When ω = 0, slope = 0 dB/dec

– When ω ≥ ωn, slope = –40 dB/ dec • Height of resonant peak depends on damping ratio

12 Phase Angles of Second-Order Bode Plots (No Zeros)

• Phase angle of a pair of complex negative poles – When ω = 0, ϕ = 0°

– When ω = ωn, ϕ =– 90° – When ω -> ∞, ϕ -> – 180° • Abruptness of phase shift depends on damping ratio

Transformation of the System Equations Time-Domain System Equations Δx!(t) = F Δx(t) + G Δu(t) Δy(t) = H Δx(t) + H Δu(t) x u Laplace Transforms of System Equations sΔx(s) − Δx(0) = F Δx(s) + G Δu(s) 1 Δx(s) = [sI − F]− [Δx(0) + GΔ u(s)]

Δy(s) = HxΔx(s) + HuΔu(s) 14 Transfer Function Matrix Laplace Transform of Output Vector

−1 Δy(s) = HxΔx(s) + HuΔu(s) = Hx [sI − F] [Δx(0) + G Δu(s)]+ HuΔu(s) ⎡H sI F −1 G H ⎤ u(s) H sI F −1 x(0) = ⎣ x ( − ) + u ⎦Δ + x [ − ] Δ = Control Effect + Initial Condition Effect

Transfer Function Matrix relates control input to system output

with Hu = 0 and neglecting initial condition

1 (s) = H sI − F − G (r x m) H x [ ] 15

Scalar Frequency Response from Transfer Function Matrix Transfer function matrix with s = jω

−1 H (jω ) = H jωI − F G (r x m) x [ ]

Δy s −1 i ( ) (j ) H j I F G (r x m) = H ij ω = xi [ ω − ] j Δu j (s) H ith row of H xi = x G = jth column of G j 16 Second-Order Transfer Function Second-order dynamic system

⎡ ⎤ ⎡ ⎤ ⎡ ⎤ Δx!1 (t) ⎡ f f ⎤ Δx1 (t) ⎡ g g ⎤ Δu1 (t) Δx! (t) = ⎢ ⎥ = ⎢ 11 12 ⎥⎢ ⎥ + ⎢ 11 12 ⎥⎢ ⎥ ⎢ Δx! t ⎥ f f ⎢ Δx t ⎥ g f ⎢ Δu t ⎥ ⎣ 2 ( ) ⎦ ⎣⎢ 21 22 ⎦⎥⎣ 2 ( ) ⎦ ⎣⎢ 21 22 ⎦⎥⎣ 2 ( ) ⎦ ⎡ ⎤ ⎡ ⎤ Δy1 (t) ⎡ h h ⎤ Δx1 (t) Δy(t) = ⎢ ⎥ = ⎢ 11 12 ⎥⎢ ⎥ ⎢ Δy2 (t) ⎥ ⎢ h21 h22 ⎥⎢ Δx2 (t) ⎥ ⎣ ⎦ ⎣ ⎦⎣ ⎦ Second-order transfer function matrix ⎡ ⎤ (s − f11 ) − f12 adj⎢ ⎥ ⎡ ⎤ ⎢ − f21 (s − f22 ) ⎥ ⎡ ⎤ h11 h12 ⎣ ⎦ g11 g12 H (s) = HxA(s)G = ⎢ ⎥ ⎢ ⎥ h21 h22 ⎛ ⎞ g21 f22 ⎣⎢ ⎦⎥ (s − f11 ) − f12 ⎣⎢ ⎦⎥ det⎜ ⎟ f s f (r × n)(n × n)(n × m) ⎝⎜ − 21 ( − 22 ) ⎠⎟ = (r × m) = (2 × 2) 17 (n = m = r = 2)

Scalar Transfer Function

from Δuj to Δyi k sq b sq−1 ... b s b kijnij (s) ij ( + q−1 + + 1 + 0 ) H (s) = = ij Δ(s) sn + a sn−1 + ...+ a s + a ( n−1 1 0 )

Just one element of the matrix, H(s) Denominator polynomial contains n roots Each numerator term is a polynomial with q zeros, where q varies from term to term and ≤ n – 1

# zeros = q k s z s z ... s z # poles = n ij ( − 1)ij ( − 2 )ij ( − q ) = ij (s − λ1)(s − λ2 )...(s − λn ) 18 Scalar Frequency Response Function for Single Input/Output Pair Substitute: s = jω k j z j z ... j z ij ( ω − 1 )ij ( ω − 2 )ij ( ω − q ) H (jω ) = ij ij jω − λ jω − λ ... jω − λ ( 1 )( 2 ) ( n ) = a(ω)+ jb(ω) → AR(ω) e jφ (ω ) Frequency response is a complex function of input frequency, ω Real and imaginary parts, or ** Amplitude ratio and phase angle ** 19

MATLAB Bode Plot with asymp.m

http://www.mathworks.com/matlabcentral/ http://www.mathworks.com/matlabcentral/ﬁleexchange/10183-bode-plot-with-asymptotes

2nd-Order Pitch Rate Frequency Response, with zero

bode.m asymp.m

20 Desirable Open-Loop Frequency Response Characteristics (Bode)

• High gain (amplitude) at low frequency – Desired response is slowly varying • Low gain at high frequency – Random errors vary rapidly • Crossover region is problem-speciﬁc

Examples of Proportional LQ Regulator Response

22 Example: Open-Loop Stable and Unstable 2nd-Order LTI System Response to Initial Condition

⎡ ⎤ Stable Eigenvalues = 0 1 -0.5000 + 3.9686i FS = ⎢ ⎥ ⎣ −16 −1 ⎦ -0.5000 - 3.9686i

⎡ 0 1 ⎤ Unstable Eigenvalues = FU = ⎢ ⎥ ⎣ −16 +0.5 ⎦ 0.2500 + 3.9922i 0.2500 - 3.9922i

Example: Stabilizing Effect of Linear- Quadratic Regulators for Unstable 2nd-Order System

∞ ⎡1 2 2 2 ⎤ min J = min Δx1 + Δx2 + rΔu dt Δu Δu ⎢ ∫( ) ⎥ ⎣2 0 ⎦

⎡ Δx (t) ⎤ Δu(t) = − ⎡ c c ⎤⎢ 1 ⎥ = −c Δx (t) − c Δx (t) ⎣ 1 2 ⎦ Δx (t) 1 1 2 2 ⎣⎢ 2 ⎦⎥

For the r = 1 r = 100 unstable Control Gain (C) = Control Gain (C) = system 0.2620 1.0857 0.0028 0.1726 Riccati Matrix (S) = Riccati Matrix (S) = 2.2001 0.0291 30.7261 0.0312 0.0291 0.1206 0.0312 1.9183

Closed-Loop Eigenvalues = Closed-Loop Eigenvalues = -6.4061 -0.5269 + 3.9683i -2.8656 -0.5269 - 3.9683i 24 Example: Stabilizing/Filtering Effect of LQ Regulators for the Unstable 2nd- Order System

r = 1 Control Gain (C) = 0.2620 1.0857

Riccati Matrix (S) = 2.2001 0.0291 0.0291 0.1206

Closed-Loop Eigenvalues = -6.4061 -2.8656

r = 100 Control Gain (C) = 0.0028 0.1726

Riccati Matrix (S) = 30.7261 0.0312 0.0312 1.9183

Closed-Loop Eigenvalues = -0.5269 + 3.9683i -0.5269 - 3.9683i

Example: Open-Loop Response of a Stable 2nd-Order System to Random Disturbance

Eigenvalues = -1.1459, -7.8541

26 Example: Disturbance Response of Unstable System with Optimal Feedback

LQ Regulators with Output Vector Cost Functions

28 Quadratic Weighting of the Output

Δy(t) = HxΔx(t) + HuΔu(t)

1 ∞ J = ⎡ΔyT (t)Q Δy(t)⎤dt ∫ ⎣ y ⎦ 2 0 ∞ 1 T = H Δx(t) + H Δu(t) Q H Δx(t) + H Δu(t) dt ∫{[ x u ] y [ x u ]} 2 0

Note: State, not output, is fed back 29

Quadratic Weighting of the Output

⎧ ⎡ T T ⎤ ⎫ 1 ∞ ⎪ Hx QyHx Hx QyHu ⎡ Δx(t) ⎤⎪ ⎡ T T ⎤⎢ ⎥ dt min J = min ∫ ⎨ Δx (t) Δu (t) T T ⎢ ⎥⎬ Δu Δu 2 0 ⎣ ⎦⎢ ⎥ Δu(t) ⎪ Hu QyHx Hu QyHu + Ro ⎢ ⎥⎪ ⎩ ⎣ ⎦⎣ ⎦⎭

∞ ⎧ ⎡ ⎤ ⎫ 1 ⎪ QO MO ⎡ Δx(t) ⎤⎪ ⎡ T T ⎤⎢ ⎥ ! min ∫ ⎨ Δx (t) Δu (t) T ⎢ ⎥⎬dt Δu 2 0 ⎣ ⎦ M R R Δu(t) ⎪ ⎢ O O + o ⎥⎣⎢ ⎦⎥⎪ ⎩ ⎣ ⎦ ⎭

−1 T T Δu(t) = −(RO + Ro ) ⎣⎡MO + GO PSS ⎦⎤Δx(t) = −CΔx(t)

Output vector may not contain control (i.e., Hu = 0) Ad hoc Ro added to assure invertability 30 State Rate Can Be Expressed as an Output to be Minimized

Δx!(t) = FΔx(t) + GΔu(t) y(t) H x(t) H u(t) F x(t) G u(t) Δ = xΔ + uΔ " Δ + Δ ∞ ∞ 1 T 1 T min J = ⎡Δy (t)QyΔy(t)⎤dt = ⎡Δx! (t)QyΔx!(t)⎤dt Δu 2 ∫ ⎣ ⎦ 2 ∫ ⎣ ⎦ 0 0

∞ ⎧ ⎡ T T ⎤ ⎫ 1 ⎪ F QyF F QyG ⎡ Δx(t) ⎤⎪ min J = min ⎨⎡ ΔxT (t) ΔuT (t) ⎤⎢ ⎥⎢ ⎥⎬dt Δu Δu 2 ∫ ⎣ ⎦⎢ T T ⎥ Δu(t) 0 ⎪ G QyF G QyG + Ro ⎢ ⎥⎪ ⎩ ⎣ ⎦⎣ ⎦⎭ ∞ ⎧ ⎡ ⎤ ⎫ 1 ⎪ QSR MSR ⎡ Δx(t) ⎤⎪ ! min ⎨⎡ ΔxT (t) ΔuT (t) ⎤⎢ ⎥⎢ ⎥⎬dt Δu 2 ∫ ⎣ ⎦ MT R R Δu(t) 0 ⎪ ⎢ SR SR + o ⎥⎣⎢ ⎦⎥⎪ ⎩ ⎣ ⎦ ⎭ −1 T T Δu(t) = −(RSR + Ro ) ⎣⎡MSR + GSR PSS ⎦⎤Δx(t) = −CΔx(t) Special case of output weighting 31

Implicit Model-Following LQ Regulator

Simulator aircraft dynamics Princeton Variable-Response Research Aircraft Δx(t) = FΔx(t) + GΔu(t) Ideal aircraft dynamics

Δx M (t) = FM ΔxM (t) F-16 Feedback control law to provide dynamic similarity

Δu(t) = −CIMF Δx(t)

Another special case of output weighting

32 Implicit Model-Following LQ Regulator Assume state dimensions are the same Δx!(t) = FΔx(t) + GΔu(t) x (t) F x (t) Δ! M = M Δ M If simulation is successful, Δx (t) ≈ Δx(t) and Δx! (t) ≈ F Δx(t) M M M

Implicit Model-Following LQ Regulator Cost function penalizes difference between actual and ideal model dynamics

∞ 1 T J = Δx(t) − Δx (t) Q Δx(t) − Δx (t) dt 2 ∫{[ M ] M [ M ]} 0

∞ ⎧ T T ⎫ T ⎡ ⎤ 1 ⎪ (F − FM ) QM (F − FM ) (F − FM ) QM G ⎡ Δx(t) ⎤⎪ min J = min ⎨⎡ Δx(t) Δu(t) ⎤ ⎢ ⎥⎢ ⎥⎬dt Δu Δu 2 ∫ ⎣ ⎦ ⎢ T T ⎥ Δu(t) 0 ⎪ G QM (F − FM ) G QM G + Ro ⎢ ⎥⎪ ⎩ ⎣ ⎦⎣ ⎦⎭ ∞ ⎧ ⎡ ⎤ ⎫ 1 ⎪ T QIMF MIMF ⎡ Δx(t) ⎤⎪ ! min ⎨⎡ Δx(t) Δu(t) ⎤ ⎢ ⎥⎢ ⎥⎬dt Δu 2 ∫ ⎣ ⎦ MT R R Δu(t) 0 ⎪ ⎢ IMF IMF + o ⎥⎣⎢ ⎦⎥⎪ ⎩ ⎣ ⎦ ⎭ Therefore, ideal model is implicit in the optimizing feedback control law

−1 Δu(t) = − R + R ⎡M T + G T P ⎤Δx t = −CΔx t ( IMF o ) ⎣ IMF IMF SS ⎦ ( ) ( ) 34 Cost Functions with Augmented State Vector

Integral Compensation Can Reduce Steady-State Errors ! Sources of Steady-State Error ! Constant disturbance ! Errors in system dynamic model

! Selector matrix, HI, can reduce or mix integrals in feedback

Δx!(t) = FΔx(t) + GΔu(t) Δξ! t = H Δx(t) ( ) I

⎡ ⎤ Δx!(t) ⎡ F G ⎤⎡ Δx(t) ⎤ ⎡ G ⎤ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ + ⎢ ⎥Δu(t) ! HI 0 Δξ t 0 ⎢ Δξ(t) ⎥ ⎣⎢ ⎦⎥⎢ ( ) ⎥ ⎣ ⎦ 36 ⎣ ⎦ ⎣ ⎦ LQ Proportional- Integral (PI) Control

t Δu(t) = −C Δx t − C H Δx τ dτ B ( ) I ∫ I ( ) 0 −C Δx t − C Δξ t ! B ( ) I ( ) where the integral state is t ⎡ Δx(t) ⎤ Δξ t H Δx τ dτ ⎢ ⎥ ( ) ! ∫ I ( ) define Δχ (t) ! 0 ⎢ Δξ t ⎥ ⎣ ( ) ⎦ dim H = m × n ( I ) 37

Integral State is Added to the Cost Function and the Dynamic Model

∞ 1 T T T min J = min ⎡Δx (t)QxΔx(t) + Δξ (t)Qξ Δξ(t) + Δu (t)RΔu(t)⎤dt Δu Δu ∫ ⎣ ⎦ 2 0

∞ ⎡ ⎡ ⎤ ⎤ 1 Qx 0 = min ⎢ΔχT (t)⎢ ⎥Δχ(t) + ΔuT (t)RΔu(t)⎥dt Δu ∫ 2 ⎢ ⎢ 0 Qξ ⎥ ⎥ 0 ⎣ ⎣ ⎦ ⎦ subject to Δχ! (t) = F Δχ(t) + G Δu(t) χ χ

−1 ⎡ T ⎤ Δu(t) = −R ⎣ G 0 ⎦PSS Δχ(t) = −Cχ Δχ(t)

= −CBΔx(t) − CI Δξ(t)

38 Integral State is Added to the Cost Function and the Dynamic Model

Δu(t) = −Cχ Δχ(t)

= −CBΔx(t) − CI Δξ(t)

Δu(s) = −Cχ Δχ(s)

= −CBΔx(s) − CI Δξ(s)

HxΔx(s) = −CBΔx(s) − CI s

Bode Plot of Control Law: [CBs + CI Hx ] Δu(s) = − Δx(s) Integrator plus numerator s matrix of zeros

Actuator Dynamics and Proportional-Filter LQ Regulators

40 Actuator Dynamics May Impact System Response

Hydraulic Actuator Electric Motor

Electro/ mechanical transduction via small piston

Force multiplication by large piston 41

Actuator Dynamics May Affect System Response Δx!(t) = FΔx(t) + GΔu(t) Augment state dynamics to include actuator dynamics

Δu!(t) = −KΔu(t) + Δv(t)

⎡ Δx!(t) ⎤ ⎡ F G ⎤⎡ Δx(t) ⎤ ⎡ 0 ⎤ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ + ⎢ ⎥Δv(t) Δu!(t) 0 −K Δu(t) ⎢ Iv ⎥ ⎣⎢ ⎦⎥ ⎣ ⎦⎣⎢ ⎦⎥ ⎣ ⎦ Control variable is actuator forcing function 42 LQ Regulator with Actuator Dynamics Re-deﬁne state and control vectors ⎡ Δx(t) ⎤ Δχ(t) ! ⎢ ⎥; Δv(t) = Control u(t) ⎣⎢ Δ ⎦⎥ ⎡ F G ⎤ ⎡ 0 ⎤ Fχ ! ⎢ ⎥; Gχ ! ⎢ ⎥ 0 −K ⎢ Iv ⎥ ⎣ ⎦ ⎣ ⎦

∞ 1 T T T min J = min ⎡Δx (t)QxΔx(t) + Δu (t)RuΔu(t) + Δv (t)Rv Δv(t)⎤dt Δv Δv ∫ ⎣ ⎦ 2 0 ∞ ⎡ ⎡ ⎤ ⎤ 1 T Qx 0 T = min ⎢Δχ (t)⎢ ⎥Δχ(t) + Δv (t)Rv Δv(t)⎥dt Δv 2 ∫ 0 R 0 ⎣⎢ ⎣⎢ u ⎦⎥ ⎦⎥

subject to Δχ! (t) = F Δχ(t) + G Δv(t) χ χ 43

LQ Regulator with Actuator Dynamics

T ⎡ Δx t ⎤ −1 ⎡ ⎤ ( ) Δv(t) = −C Δχ(t) = −R 0 Iv P ⎢ ⎥ χ v ⎣ ⎦ SS u(t) ⎣⎢ Δ ⎦⎥

= −CBΔx(t) − CAΔu(t)

Control Velocity

Δv(s) = −CBΔx(s) − CAΔu(s) 44 LQ Regulator with Actuator Dynamics

Δu!(t) = −KΔu(t) − CAΔu(t) − CBΔx(t) sΔu(s) = −KΔu(s) − C Δu(s) − C Δx s A B ( ) Control Displacement

⎣⎡sI + (K + CA )⎦⎤Δu(s) = −CBΔx(s) −1 Δu(s) = − ⎣⎡sI + (K + CA )⎦⎤ CBΔx(s) 45

LQ Regulator with Artiﬁcial Actuator Dynamics LQ control variable is derivative of actual system control

⎡ Δx(t) ⎤ ⎡ F G ⎤ ⎡ Δx(t) ⎤ ⎡ 0 ⎤ ⎢ ⎥ = ⎢ ⎥ ⎢ ⎥ + ⎢ ⎥ Δv(t) ⎢ Δu(t) ⎥ 0 0 ⎢ Δu(t) ⎥ I ⎣ ⎦ ⎣ ⎦ ⎣ ⎦ ⎣ ⎦

Δv(t) = Δu! (t) = −C Δx t − C Δu(t) Int B ( ) A

CA introduces artiﬁcial actuator dynamics, e.g., low-pass ﬁlter 46 Proportional-Filter (PF) LQ Regulator

⎡ Δx(t) ⎤ ⎡ F G ⎤ ⎡ 0 ⎤ Δχ(t) = ⎢ ⎥; F = ; G = u(t) χ ⎢ 0 0 ⎥ χ ⎢ I ⎥ ⎣⎢ Δ ⎦⎥ ⎣ ⎦ ⎣ ⎦

Optimal LQ Regulator Δv(t) = Δu! (t) = −C Δχ(t) Integrator χ

CA provides low-pass ﬁltering effect on the control input

−1 Δu(s) = −[sI + CA ] CBΔx(s)

Proportional LQ Regulator: High- Frequency Control in Response to High-Frequency Disturbances 2nd-Order System Response with Perfect Actuator

Good disturbance rejection, but high bandwidth may be unrealistic 48 Proportional-Filter LQ Regulator Reduces High-Frequency Control Signals 2nd-Order System Response

... at the expense of decreased disturbance rejection 49

Next Time: Linear-Quadratic Control System Design

50 Supplemental Material

Princeton Variable-Response Research Aircraft (VRA)

52 Aircraft That Simulate Other Aircraft

• Closed-loop control • Variable-stability research aircraft, e.g., TIFS, Learjet, NT-33A, and Princeton Variable-Response Research Aircraft (Navion)

USAF/Calspan TIFS Princeton VRA

USAF/Calspan NT-33A CalSpan Learjet