Filters, Cost Functions, and Controller Structures Robert Stengel Optimal Control and Estimation MAE 546 Princeton University, 2018 ! Dynamic systems as low-pass filters ! 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) Copyright 2018 by Robert Stengel. All rights reserved. For educational use only. http://www.princeton.edu/~stengel/MAE546.html http://www.princeton.edu/~stengel/OptConEst.html 1 First-Order Low-Pass Filter 2 Low-Pass Filter Low-pass filter 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 5 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 = 7 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 ! H ( jω )Δu( jω ) 10 2nd-Order Step Response Increased damping decreases oscillatory over/undershoot Further increase eliminates oscillation 11 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 13 Transformation of the System Equations Time-Domain System Equations Δx!(t) = F Δx(t) + G Δu(t) Δy(t) = HxΔx(t) + HuΔu(t) 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 H (s) = Hx [sI − F] G (r x m) 15 Scalar Frequency Response from Transfer Function Matrix Transfer function matrix with s = jω −1 H (jω ) = Hx [ jωI − F] G (r x m) Δ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 th G j = j column of G 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) n n−1 Δ (s + an−1s + ...+ a1s + a0 ) 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/fileexchange/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-specific 21 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 23 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 25 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 27 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) = HxΔx(t) + HuΔu(t) " FΔx(t) + GΔu(t) ∞ ∞ 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, ΔxM (t) ≈ Δx(t) and Δx! M (t) ≈ FM Δx(t) 33 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 ∫{[ M ] M [ M ]} 2 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,