H 2 Optimal State Feedback Control Synthesis Raktim Bhattacharya Aerospace Engineering, Texas A&M University
Motivation
Motivation w(t) u(t) G K y(t) z(t) w(t) are exogenous signals reference, process noise, sensor noise z(t) are signals we want to keep small e(t), u(t) Find controller K that minimizes G w z 2 Appropriate when spectral density function of w(t) is known For example w(t) can be stationary noise H2 optimal is then linear quadratic Gaussian (LQG) AERO 632, Instructor: Raktim Bhattacharya 3 / 38
Motivation Stochastic Input For stationary stochastic process w(t), autocorrelation matrix is R w (τ) := E [w(t + τ)w (t)] The Fourier transform of R w (τ) is the spectral density Ŝw(jω) Signal power is related to it by E [ w(t) ] 2 := tr [R w ()] = 1 ] tr [Ŝw (jω) dω 2π If z and w are related by z = P w, for a stable LTI system P, then Ŝ z (jω) = ˆP (jω)ŝw(jω) ˆP (jω) AERO 632, Instructor: Raktim Bhattacharya 4 / 38
Motivation Stochastic Input (contd.) Therefore, E [ z(t) ] 2 = 1 2π [ tr ˆP (jω) Ŝ w (jω) ˆP ] (jω) dω. Looks like weighted H 2 norm of P. If w(t) is white noise = Ŝw(jω) = I. H 2 norm is output variance with white noise input. For any other Ŝw(jω), Therefore, E [ z(t) ] 2 = 1 2π Ŝ w (jω) = Ŵ (jω)ŵ (jω). Think of ˆP (jω)ŵ (jω) as weighted system [( ) tr ˆP (jω) Ŵ (jω) I (Ŵ (jω) ˆP )] (jω) dω. AERO 632, Instructor: Raktim Bhattacharya 5 / 38
Motivation K Stochastic Input (contd.) P z(t) w(t) z(t) P w(t) z(t) P w(t) W white noise E [ z(t) ] 2 = 1 P z(t) 2π w(t) [( ) tr ˆP (jω) Ŵ (jω) W white noise Think of ˆP (jω)ŵ (jω) as weighted system I (Ŵ (jω) ˆP )] (jω) dω. AERO 632, Instructor: Raktim Bhattacharya 6 / 38
Motivation Impulse Response Input signal is known in advance Tracking a fixed signal e.g. step Consider a special case: Scalar w(t) = δ(t). Implies z 2 2 = = 1 2π = 1 2π = ˆP 2 2 z (t)z(t)dt ẑ (jω)ẑ(jω)dω ˆP (jω) ˆP (jω)dω AERO 632, Instructor: Raktim Bhattacharya 7 / 38
P W z(t) w(t) white noise Motivation Impulse Response (contd.) What about any other referencez(t) r(t)? P r(t) z(t) P r(t) z(t) P r(t) h(t) (t) r(t) can bepreplaced byh(t) the impulse response h(t) of a known z(t) r(t) (t) filter W (jω) z(t) becomes impulse response of a weighted plant 1 AERO 632, Instructor: Raktim Bhattacharya 8 / 38
Computation of H 2 Norm Best computed in state-space realization of system State Space Model: General MIMO LTI system modeled as ẋ = Ax + Bu, y = Cx + Du, where x R n, u R m, y R p. Transfer Function Ĝ(s) = D + C(sI A) 1 B strictly proper when D = Impulse Response G(t) = L 1 { C(sI A) 1 B } = Ce ta B. AERO 632, Instructor: Raktim Bhattacharya 9 / 38
H 2 Norm MIMO Systems Ĝ(jω) 2 2 = 1 [Ĝ ] tr 2π (jω)ĝ(jω) for matrix transfer function = G(t) 2 2 Parseval [ ] = tr Ce ta BB T e tat C T dt ( = tr C ) e ta BB T e tat dt } {{ } L c C T Lc = controllability Gramian = tr [ CL c C T ] AERO 632, Instructor: Raktim Bhattacharya 1 / 38
H 2 Norm (contd.) MIMO Systems For any matrix M tr [M M] = tr [MM ] = Ĝ(jω) 2 2 = tr BT ( ) e tat C T Ce ta dt B }{{} L o = tr [ B T L o B ] L o = observability Gramian H 2 Norm of Ĝ(jω) Ĝ(jω) 2 2 = tr [ CL c C T ] = tr [ B T L o B ]. AERO 632, Instructor: Raktim Bhattacharya 11 / 38
H 2 Norm How to determine L c and L o? They are solutions of the following equation AL c + L c A T + BB T =, A T L o + L o A + C T C =. AERO 632, Instructor: Raktim Bhattacharya 12 / 38
Proposition 1 Suppose P is a state-space system with realization (A, B, C). Then iff X = X T > such that A is Hurwitz and ˆP 2 1, tr [CXC ] < 1 and AX + XA + BB <. Proof Only If Recall ˆP 2 = tr [CL c C ]. Therefore for X = L c and A is Hurwitz ˆP 2 is finite tr [CL c C ] < 1 = ˆP 2 < 1, AERO 632, Instructor: Raktim Bhattacharya 13 / 38
contd. Consider X = e ta (BB + ϵi n )e ta dt, is continuous in ϵ equals to L c when ϵ =. It can be shown that this X satisfies Lyapunov equation follow the derivation AX + XA + BB + ϵi n =, or AX + XA + BB <. AERO 632, Instructor: Raktim Bhattacharya 14 / 38
contd. Proof If X satisfies and Implies, A is Hurwitz. AX + XA + BB < tr [CXC ] < 1. Proposition: It can be shown that if L c satisfies and X satisfies A L c + L c A + Q =, then X L c. A X + XA + Q, AERO 632, Instructor: Raktim Bhattacharya 15 / 38
contd. Inequality X L c implies ˆP 2 = tr [CL c C ] tr [CXC ] < 1. AERO 632, Instructor: Raktim Bhattacharya 16 / 38
System Dynamics w(t) u(t) G y(t) z(t) K Dynamics ẋ = Ax + B w w + B u u z = C z x + D u u y = x With u = Kx, F l (Ĝ, K) := Ĝw z(s) = = Ĝ(s) = [ A + Bu K B w C z + D u K A B w B u C z D u I ]. AERO 632, Instructor: Raktim Bhattacharya 17 / 38
Controller Synthesis Proposition There exists feedback gain K that internally stabilizes G and satisfies F l (Ĝ, K) 2 < 1 iff Z R m n such that K = ZX 1, where X > satisfies inequalities [ ] [ ] X A Bu + [ [ X Z Z ] A ] B u + B w B w <, tr [ (C z X + D u Z)X 1 (C z X + D u Z) ] < 1. Proof: Use closed-loop state-space data and earlier proposition. Not an LMI. AERO 632, Instructor: Raktim Bhattacharya 18 / 38
Controller Synthesis (contd.) Apply Schur complement to get following convex problem. There exists feedback gain K that internally stabilizes G satisfying F l (Ĝ, K) 2 < 1 iff X R n n, W R q q, and Z R m n, such that and K = ZX 1, min tr [W ] W [ ] [ ] X A Bu + [ [ X Z Z ] A ] Bu + B w Bw <, [ ] W (C z X + D u Z) (C z X + D u Z) > X AERO 632, Instructor: Raktim Bhattacharya 19 / 38
Very simple example System Dynamics 3 2 1 2 3 ẋ = 1 2 1 x + 2 u + w, 1 1 1 1 1 [ ] [ ] 1 1 1 1 z = x + u. 1 1 1 Compare with LQR AERO 632, Instructor: Raktim Bhattacharya 2 / 38
Very simple example 6 4 H 2 LQR Output z1 2 2 4 6 2 4 6 8 1 12 14 16 18 2 Time(s) Output z2 4 3 2 1 1 2 3 2 4 6 8 1 12 14 16 18 2 Time(s) tr [W ] = 3.3491e 4 Good disturbance attenuation. AERO 632, Instructor: Raktim Bhattacharya 21 / 38
MATLAB Code clear; clc; A = [-3-2 1; 1 2 1; 1-1 -1;]; Bu = [2 ; 2; 1]; Bw = [3;;1]; Cz = [1 1; 1 1]; Du = [1 1; 1]; nx = 3; nu = 2; nz = 2; nw = 1; cvx_begin sdp variable X(nx,nx) symmetric variable W(nz,nz) symmetric variable Z(nu,nx) [A Bu]*[X;Z] + [X Z ]*[A ;Bu ] + Bw*Bw < [W (Cz*X + Du*Z); (Cz*X + Du*Z) X] > minimize trace(w) cvx_end h2k = Z*inv(X); [lqrk,s,e] = lqr(a,bu,cz *Cz,Du *Du); h2g = ss(a+bu*h2k, Bw, Cz + Du*h2K, zeros(nw,1)) lqrg = ss(a-bu*lqrk, Bw, Cz + Du*lqrK,... zeros(nw,1)); T = [:.1:2]; w = 5*randn(length(T),1); [y1,t1,x1] = lsim(h2g,w,t); [y2,t2,x2] = lsim(lqrg,w,t); f1=figure(1); clf; set(f1, defaulttextinterpreter, latex ); for i=1:2 subplot(2,1,i); plot(t1,y1(:,i), r,t2,y2(:,i), b:,... linewidth,2); xlabel( Time(s) ); ylabel(sprintf( Output $z_%d$,i)); end subplot(2,1,1); legend( H_2, LQR ); print -depsc h2ex1.eps AERO 632, Instructor: Raktim Bhattacharya 22 / 38
Regulator with disturbance Active Suspension Equations of motion k1 m1 m2 u(t) x1(t) c1 x2(t) m 1 ẍ 1 = k 1 (x 1 x 2 ) c 1 (ẋ 1 ẋ 2 ) + u, m 2 ẍ 2 = k 1 (x 1 x 2 ) + c 1 (ẋ 1 ẋ 2 ) k 2 (x 2 d) u. State Variables k2 q 1 := x 1, q 2 := x 2, q 3 := ẋ 1, q 4 := ẋ 2. d(t) AERO 632, Instructor: Raktim Bhattacharya 23 / 38
Regulator with disturbance Active Suspension Linear System q 1 1 q 1 q 2 q 3 = 1 q 2 k 1 /m 1 k 1 /m 1 c 1 /m 1 c 1 /m 1 q 3 + q 4 k 1 /m 2 (k 1 + k 2 )/m 2 c 1 /m 2 c 1 /m 2 q 4 1/m 1 u + d. 1/m 2 k 2 /m 2 Output z(t) z = q 1 + u. AERO 632, Instructor: Raktim Bhattacharya 24 / 38
Regulator with disturbance Active Suspension.8 d(t) = white noise [ 1, 1] m H2 LQR.8 d(t) =.5 sin (5 T) m H2 LQR.6.6.4.4.2.2 output z(t) output z(t).2.2.4.4.6.6.8 2 4 6 8 1 12 14 16 18 2 Time.8.2.4.6.8 1 1.2 1.4 1.6 1.8 2 Time tr [W ] = 1.17367e 1 Very good disturbance rejection. AERO 632, Instructor: Raktim Bhattacharya 25 / 38
MATLAB Code clear; clc; % System Parameters m1 = 29; % kg -- Body mass m2 = 6; % kg -- suspension mass k1 = 162; % N/m k2 = 191; % N/m c1 = 1; % Ns/m A = [ 1 ; 1; -k1/m1 k1/m1 -c1/m1 c1/m1; k1/m1 -(k1+k2)/m2 c1/m2 -c1/m2]; Bu = [;;1/m1;-1/m2]; Bw = [;;;k2/m2]; nx = 4; nu = 1; nz = 1; nw = 1; Cz = [1,,,]; Du = 1*ones(nz,nu); cvx_begin sdp variable X(nx,nx) symmetric variable W(nz,nz) symmetric variables Z(nu,nx) gam [A Bu]*[X;Z] + [X Z ]*[A ;Bu ] + Bw*Bw < [W (Cz*X + Du*Z); (Cz*X + Du*Z) X] > minimize trace(w) cvx_end h2k = Z*inv(X); [lqrk,s,e] = lqr(a,bu,cz *Cz,Du *Du); % Simulation h2g = ss(a+bu*h2k, Bw, Cz + Du*h2K, zeros(nz,nw)); lqrg = ss(a-bu*lqrk, Bw, Cz + Du*lqrK, zeros(nz,nw)); T = [:.1:2]/1; w = 2*rand(length(T),1)-1; w =.5*sin(1*T); [y1,t1,x1] = lsim(h2g,w,t); [y2,t2,x2] = lsim(lqrg,w,t); f1 = figure(1); clf; set(f1, defaulttextinterpreter, latex ); plot(t1,y1, r,t2,y2, b:, Linewidth,2); xlabel( Time ); ylabel( output $z(t)$ ); title( $d(t) =.5*\sin\;\; (5*T)$ m ); title( $d(t)$ = white noise $\in [-1,\;1]$ m ); legend( H2, LQR ); print -depsc h2qcar2.eps AERO 632, Instructor: Raktim Bhattacharya 26 / 38
Tracking with disturbance Let dynamical system be ẋ = Ax + B u u + B d d, z = [ ] x, y = C u y x. Design a controller of the form u = Kx + Gr K is designed in H 2 optimal sense G is regular tracking gain AERO 632, Instructor: Raktim Bhattacharya 27 / 38
Tracking with disturbance Closed-loop system with K is therefore ẋ = (A + BK)x + B u Gr, y = C y x. Ignore disturbance when determining G Steady-state response to constant r is = (A + BK)x ss + BGr, y ss = C y x ss = r. Or x ss = (A + BK) 1 BGr, implies C y (A + BK) 1 BGr = r, or C y (A + BK) 1 BG = I. Solve for G AERO 632, Instructor: Raktim Bhattacharya 28 / 38
Tracking with disturbance Existence of solution of C y (A + BK) 1 BG = I is necessary and sufficient condition for existence of a tracking controller. In general, when y = C y x + D y u, the equation becomes [ Dy (C y + D y K)(A + BK) 1 B ] G = I. Can be rewritten in terms of Π Π = (A + BK) 1 BG = (C y + D y K)Π + D y G = I, AERO 632, Instructor: Raktim Bhattacharya 29 / 38
Tracking with disturbance Or Or rearranged to (A + BK)Π + BG =, (C y + D y K)Π + D y G = I. AΠ + B(KΠ + G) =, C y Π + D y (KΠ + G) = I. Therefore, with Γ := KΠ + G, we get [ ] [ ] [ ] A B Π nx n + y C y D y Γ I ny ny =. This is the so called regulator equation. Get G = Γ KΠ. AERO 632, Instructor: Raktim Bhattacharya 3 / 38
Tracking with disturbance Solve regulator equation [ A B C y D y ] [ Π Γ ] + [ nx n y I ny n y ] =. Get G = Γ KΠ. Control law is u = Kx + Gr. AERO 632, Instructor: Raktim Bhattacharya 31 / 38
Tracking with disturbance Longitudinal F16 Control Law Longitudinal Motion States [V (ft/s) α(rad) θ(rad) q(rad/s)] T Controls [T (lb) δ e (deg)] Constraints on u [ ] 1 u 25 Trimmed at steady-level flight at 932 ft/s x trim = Disturbance in α equation [ 932.2894 ], u trim = [ ] 5318.2 1.3935 [ ] 19 25 AERO 632, Instructor: Raktim Bhattacharya 32 / 38
Tracking with disturbance Longitudinal F16 Control Law d(t) System Interconnection r G d x K + F 16 Cy y Control Objective Design K to minimize G d x 2 [ ] V Design G to track r := reference 1γ AERO 632, Instructor: Raktim Bhattacharya 33 / 38
Tracking with disturbance Longitudinal F16 Control Law Disturbance Rejection d(t) U [ 1,1] rad, tr [W ] =.118159.2.15.1 V ft/s.5 -.5 -.1 2 4 6 8 1 12 14 16 18 2 Time AERO 632, Instructor: Raktim Bhattacharya 34 / 38
Tracking with disturbance Longitudinal F16 Control Law Tracking Performance V ref = 5 ft/s step, γ ref = V (ft/s) 6 5 4 3 2 1 5 1 15 2 Gamma (deg) 1 1 2 3 4 5 1 15 2 1 15 Thrust (lb) 9 8 7 6 Elevator (deg) 1 5 5 5 1 15 2 Time 5 5 1 15 2 Time AERO 632, Instructor: Raktim Bhattacharya 35 / 38
Tracking with disturbance Longitudinal F16 Control Law Tracking Performance V ref =, γ ref = 45 deg step V (ft/s) 2 15 1 5 5 5 1 15 2 Gamma (deg) 8 6 4 2 2 5 1 15 2 2 5 Thrust (lb) 15 1 5 Elevator (deg) 5 1 5 5 1 15 2 Time 15 5 1 15 2 Time AERO 632, Instructor: Raktim Bhattacharya 36 / 38
Tracking with disturbance Longitudinal F16 Control Law Tracking Performance V ref = 5 ft/s step, γ ref = 45 deg step V (ft/s) 7 6 5 4 3 2 1 5 1 15 2 Gamma (deg) 6 4 2-2 -4 5 1 15 2 AERO 632, Instructor: Raktim Bhattacharya 37 / 38
Tracking with disturbance Longitudinal F16 Control Law Simulink Cy* u F16 Output V, gamma ysim To Workspace Vref Br* u Gain1 Gref x' = Ax+Bu y = Cx+Du F16 Longi h2k* u Gain Uniform Random Number AERO 632, Instructor: Raktim Bhattacharya 38 / 38