16.323 Lecture 4: HJB Equation

16.323 Lecture 4 HJB Equation • DP in continuous time • HJB Equation • Continuous LQR Factoids: for symmetric R ∂uT Ru = 2uT R ∂u ∂Ru =R ∂u Spr 2008 DP in Continuous Time 16.323 4–1 • Have analyzed a couple of approximate solutions to the classic control problem of minimizing: � tf min J = h(x(tf ), tf ) + g(x(t), u(t), t) dt t0 subject to ẋ x(t0) m(x(tf ), tf ) u(t) = = = ∈ a(x, u, t) given 0 set of terminal conditions U set of possible constraints • Previous approaches discretized in time, state, and control actions – Useful for implementation on a computer, but now want to consider the exact solution in continuous time – Result will be a nonlinear partial differential equation called the Hamilton-Jacobi-Bellman equation (HJB) – a key result. • First step: consider cost over the interval [t, tf ], where t ≤ tf of any control sequence u(τ ), t ≤ τ ≤ tf � tf J(x(t), t, u(τ )) = h(x(tf ), tf ) + g(x(τ ), u(τ ), τ ) dτ t – Clearly the goal is to pick u(τ ), t ≤ τ ≤ tf to minimize this cost. J �(x(t), t) = min J(x(t), t, u(τ )) u(τ )∈U t≤τ ≤tf June 18, 2008 Spr 2008 16.323 4–2 • Approach: – Split time interval [t, tf ] into [t, t + Δt] and [t + Δt, tf ], and are specifically interested in the case where Δt → 0 – Identify the optimal cost-to-go J �(x(t + Δt), t + Δt) – Determine the “stage cost” in time [t, t + Δt] – Combine above to find best strategy from time t. – Manipulate result into HJB equation. • Split: J �(x(t), t) = min � u(τ )∈U t≤τ ≤tf � h(x(tf ), tf ) + � g(x(τ ), u(τ ), τ )) dτ t � � = min h(x(tf ), tf ) + u(τ )∈U t≤τ ≤tf tf t+Δt � tf g(x, u, τ ) dτ + t � g(x, u, τ ) dτ t+Δt • Implicit here that at time t+Δt, the system will be at state x(t+Δt). – But from the principle of optimality, we can write that the optimal cost-to-go from this state is: J �(x(t + Δt), t + Δt) • Thus can rewrite the cost calculation as: �� t+Δt � J �(x(t), t) = min g(x, u, τ ) dτ + J �(x(t + Δt), t + Δt) u(τ )∈U t≤τ ≤t+Δt June 18, 2008 t Spr 2008 16.323 4–3 • Assuming J �(x(t + Δt), t + Δt) has bounded second derivatives in both arguments, can expand this cost as a Taylor series about x(t), t � � � ∂J J �(x(t + Δt), t + Δt) ≈ J �(x(t), t) + (x(t), t) Δt ∂t � � � ∂J + (x(t), t) (x(t + Δt) − x(t)) ∂x – Which for small Δt can be compactly written as: J �(x(t + Δt), t + Δt) ≈ J �(x(t), t) + Jt�(x(t), t)Δt +Jx�(x(t), t)a(x(t), u(t), t)Δt • Substitute this into the cost calculation with a small Δt to get J �(x(t), t) = min {g(x(t), u(t), t)Δt + J �(x(t), t) u(t)∈U +Jt�(x(t), t)Δt + Jx�(x(t), t)a(x(t), u(t), t)Δt} • Extract the terms that are independent of u(t) and cancel 0 = Jt�(x(t), t)+ min {g(x(t), u(t), t) + Jx�(x(t), t)a(x(t), u(t), t)} u(t)∈U – This is a partial differential equation in J �(x(t), t) that is solved backwards in time with an initial condition that J �(x(tf ), tf ) = h(x(tf )) for x(tf ) and tf combinations that satisfy m(x(tf ), tf ) = 0 June 18, 2008 Spr 2008 HJB Equation 16.323 4–4 • For simplicity, define the Hamiltonian H(x, u, Jx�, t) = g(x(t), u(t), t) + Jx�(x(t), t)a(x(t), u(t), t) then the HJB equation is −Jt�(x(t), t) = min H(x(t), u(t), Jx�(x(t), t), t) u(t)∈U – A very powerful result, that is both a necessary and sufficient condition for optimality – But one that is hard to solve/use in general. • Some references on numerical solution methods: – M. G. Crandall, L. C. Evans, and P.-L. Lions, ”Some properties of viscosity solutions of Hamilton-Jacobi equations,” Transactions of the American Mathematical Society, vol. 282, no. 2, pp. 487–502, 1984. – M. Bardi and I. Capuzzo-Dolcetta (1997), “Optimal Control and Viscosity Solutions of Hamilton-Jacobi-Bellman Equations,” Sys­ tems & Control: Foundations & Applications, Birkhauser, Boston. • Can use it to directly solve the continuous LQR problem June 18, 2008 Spr 2008 HJB Simple Example 16.323 4–5 • Consider the system with dynamics ẋ = Ax + u for which A + AT = 0 and �u� ≤ 1, and the cost function � tf J= dt = tf 0 • Then the Hamiltonian is H = 1 + Jx�(Ax + u) and the constrained minimization of H with respect to u gives u� = −(Jx�)T /�Jx�� • Thus the HJB equation is: −Jt� = 1 + Jx�(Ax) − �Jx�� • As a candidate solution, take J �(x) = xT x/�x� = �x�, which is not an explicit function of t, so Jx� xT = �x� and Jt� = 0 which gives: xT �x� 0 = 1+ (Ax) − �x� �x� 1 = (xT Ax) �x� 1 1 T = x (A + AT )x = 0 �x� 2 so that the HJB is satisfied and the optimal control is: x u� = − �x� June 18, 2008 Spr 2008 Continuous LQR 16.323 4–6 • Specialize to a linear system model and a quadratic cost function ẋ(t) = A(t)x(t) + B(t)u(t) � tf � � 1 1 T T T J = x(tf ) Hx(tf )+ x(t) Rxx(t)x(t) + u(t) Ruu(t)u(t) dt 2 2 t0 – Assume that tf fixed and there are no bounds on u, – Assume H, Rxx(t) ≥ 0 and Ruu(t) > 0, then � 1� H(x, u, Jx�, t) = x(t)T Rxx(t)x(t) + u(t)T Ruu(t)u(t) 2 +Jx�(x(t), t) [A(t)x(t) + B(t)u(t)] • Now need to find the minimum of H with respect to u, which will occur at a stationary point that we can find using (no constraints) ∂H = u(t)T Ruu(t) + Jx�(x(t), t)B(t) = 0 ∂u – Which gives the optimal control law: −1 u�(t) = −Ruu (t)B(t)T Jx�(x(t), t)T – Since ∂ 2H = Ruu(t) > 0 ∂u2 then this defines a global minimum. June 18, 2008 Spr 2008 16.323 4–7 • Given this control law, can rewrite the Hamiltonian as: H(x, u�, Jx�, t) = � 1� T � −1 −1 T � T x(t) Rxx(t)x(t) + Jx (x(t), t)B(t)Ruu (t)Ruu(t)Ruu (t)B(t) Jx (x(t), t) 2 � � � −1 T � T +Jx (x(t), t) A(t)x(t) − B(t)Ruu (t)B(t) Jx (x(t), t) 1 = x(t)T Rxx(t)x(t) + Jx�(x(t), t)A(t)x(t) 2 1 − Jx�(x(t), t)B(t)Ru−u1(t)B(t)T Jx�(x(t), t)T 2 • Might be difficult to see where this is heading, but note that the boundary condition for this PDE is: 1 J �(x(tf ), tf ) = xT (tf )Hx(tf ) 2 – So a candidate solution to investigate is to maintain a quadratic form for this cost for all time t. So could assume that 1 P (t) = P T (t) J �(x(t), t) = xT (t)P (t)x(t), 2 and see what conditions we must impose on P (t). 6 – Note that in this case, J � is a function of the variables x and t7 ∂J � = xT (t)P (t) ∂x ∂J � 1 = xT (t)Ṗ (t)x(t) ∂t 2 • To use HJB equation need to evaluate: −Jt�(x(t), t) = min H(x(t), u(t), Jx�, t) u(t)∈U 6 See AM, pg. 21 on how to avoid having to make this assumption. 7 Partial derivatives taken wrt one variable assuming the other is fixed. Note that there are 2 independent variables in this problem x and t. x is time-varying, but it is not a function of t. June 18, 2008 Spr 2008 16.323 4–8 • Substitute candidate solution into HJB: 1 1 − x(t)T Ṗ (t)x(t) = x(t)T Rxx(t)x(t) + xT P (t)A(t)x(t) 2 2 1 −1 − xT (t)P (t)B(t)Ruu (t)B(t)T P (t)x(t) 2 1 1 x(t)T Rxx(t)x(t) + xT (t){P (t)A(t) + A(t)T P (t)}x(t) 2 2 1 −1 − xT (t)P (t)B(t)Ruu (t)B(t)T P (t)x(t) 2 which must be true for all x(t), so we require that P (t) solve = −1 −Ṗ (t) = P (t)A(t) + A(t)T P (t) + Rxx(t) − P (t)B(t)Ruu (t)B(t)T P (t) P (tf ) = H • If P (t) solves this Differential Riccati Equation, then the HJB equation is satisfied by the candidate J �(x(t), t) and the resulting control is optimal. • Key thing about this J � solution is that, since Jx� = xT (t)P (t), then −1 u�(t) = −Ruu (t)B(t)T Jx�(x(t), t)T −1 = −Ruu (t)B(t)T P (t)x(t) – Thus optimal feedback control is a linear state feedback with gain −1 F (t) = Ruu (t)B(t)T P (t) ⇒ u(t) = −F (t)x(t) � Can be solved for ahead of time. June 18, 2008 Spr 2008 16.323 4–9 • As before, can evaluate the performance of some arbitrary timevarying feedback gain u = −G(t)x(t), and the result is that 1 JG = xT S(t)x 2 where S(t) solves −Ṡ(t) = {A(t) − B(t)G(t)}T S(t) + S(t){A(t) − B(t)G(t)} + Rxx(t) + G(t)T Ruu(t)G(t) S(tf ) = H – Since this must be true for arbitrary G, then would expect that this −1 reduces to Riccati Equation if G(t) ≡ Ruu (t)B T (t)S(t) • If we assume LTI dynamics and let tf → ∞, then at any finite time t, would expect the Differential Riccati Equation to settle down to a steady state value (if it exists) which is the solution of −1 T P A + AT P + Rxx − P BRuu B P =0 – Called the (Control) Algebraic Riccati Equation (CARE) – Typically assume that Rxx = CzT RzzCz , Rzz > 0 associated with performance output variable z(t) = Cz x(t) June 18, 2008 Spr 2008 LQR Observations 16.323 4–10 • With terminal penalty, H = 0, the solution to the differential Riccati Equation (DRE) approaches a constant iff the system has no poles that are unstable, uncontrollable8, and observable9 by z(t) – If a constant steady state solution to the DRE exists, then it is a positive semi-definite, symmetric solution of the CARE. • If [A, B, Cz ] is both stabilizable and detectable (i.e. all modes are stable or seen in the cost function), then: – Independent of H ≥ 0, the steady state solution Pss of the DRE approaches the unique PSD symmetric solution of the CARE. • If a steady state solution exists Pss to the DRE, then the closed-loop system using the static form of the feedback −1 T u(t) = −Ruu B Pssx(t) = −Fssx(t) is asymptotically stable if and only if the system [A, B, Cz ] is stabilizable and detectable. – This steady state control minimizes the infinite horizon cost func­ tion lim J for all H ≥ 0 tf →∞ • The solution Pss is positive definite if and only if the system [A, B, Cz ] is stabilizable and completely observable. • See Kwakernaak and Sivan, page 237, Section 3.4.3. 8 16.31 Notes on Controllability 9 16.31 Notes on Observability June 18, 2008 Spr 2008 Scalar LQR Example 16.323 4–11 • A scalar system with dynamics ẋ = ax + bu and with cost (Rxx > 0 and Ruu > 0) � ∞ J= (Rxxx2(t) + Ruuu2(t)) dt 0 • This simple system represents one of the few cases for which the differential Riccati equation can be solved analytically: P (τ ) = (aPtf + Rxx) sinh(βτ ) + βPtf cosh(βτ ) (b2Ptf /Ruu − a) sinh(βτ ) + β cosh(βτ ) � where τ = tf − t, β = a2 + b2(Rxx/Ruu). – Note that for given a and b, ratio Rxx/Ruu determines the time constant of the transient in P (t) (determined by β). • The steady-state P solves the CARE: 2 2 2aPss + Rxx − Pss b /Ruu = 0 which gives (take positive one) that � � � a + a2 + b2Rxx/Ruu a+β a + β −a + β Pss = = 2 = >0 b /Ruu b2/Ruu −a + β b2/Ruu • With Ptf = 0, the solution of the differential equation reduces to: P (τ ) = Rxx sinh(βτ ) (−a) sinh(βτ ) + β cosh(βτ ) where as τ → tf (→ ∞) then sinh(βτ ) → cosh(βτ ) → eβτ /2, so P (τ ) = June 18, 2008 Rxx Rxx sinh(βτ ) → = Pss (−a) sinh(βτ ) + β cosh(βτ ) (−a) + β Spr 2008 16.323 4–12 • Then the steady state feedback controller is u(t) = −Kx(t) where � a + a2 + b2Rxx/Ruu −1 Kss = Ruu bPss = b • The closed-loop dynamics are ẋ = (a − bKss)x = Acl x(t) � � � b = a − (a + a2 + b2Rxx/Ruu) x b � = − a2 + b2Rxx/Ruu x which are clearly stable. � • As Rxx/Ruu → ∞, Acl ≈ −|b| Rxx/Ruu – Cheap control problem – Note that smaller Ruu leads to much faster response. • As Rxx/Ruu → 0, K ≈ (a + |a|)/b – Expensive control problem – If a < 0 (open-loop stable), K ≈ 0 and Acl = a − bK ≈ a – If a > 0 (OL unstable), K ≈ 2a/b and Acl = a − bK ≈ −a • Note that in the expensive control case, the controller tries to do as little as possible, but it must stabilize the unstable open-loop system. – Observation: optimal definition of “as little as possible” is to put the closed-loop pole at the reflection of the open-loop pole about the imaginary axis. June 18, 2008 Spr 2008 Numerical P Integration 16.323 4–13 • To numerically integrate solution of P , note that we can use standard Matlab integration tools if we can rewrite the DRE in vector form. – Define a vec operator so that ⎡ P11 ⎢P ⎢ 12 ⎢ ... ⎢ ⎢ ⎢P vec(P ) = ⎢ 1n ⎢ P22 ⎢ ⎢ P23 ⎢ . ⎣ .. Pnn ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥≡y ⎥ ⎥ ⎥ ⎥ ⎦ – The unvec(y) operation is the straightforward – Can now write the DRE as differential equation in the variable y • Note that with τ = tf − t, then dτ = −dt, – t = tf corresponds to τ = 0, t = 0 corresponds to τ = tf – Can do the integration forward in time variable τ : 0 → tf • Then define a Matlab function as doty = function(y); global A B Rxx Ruu % P=unvec(y); % % assumes that P derivative wrt to tau (so no negative) dot P = (P*A + A^T*P+Rxx-P*B*Ruu^{-1}*B^T*P);% doty = vec(dotP); % return – Which is integrated from τ = 0 with initial condition H – Code uses a more crude form of integration June 18, 2008 Spr 2008 16.323 4–14 Figure 4.1: Comparison of numerical and analytical P Figure 4.2: Comparison showing response with much larger Rxx /Ruu June 18, 2008 Spr 2008 16.323 4–15 Figure 4.3: State response with high and low Ruu . State response with timevarying gain almost indistinguishable – highly dynamic part of x response ends before significant variation in P . June 18, 2008 Spr 2008 16.323 4–16 Figure 4.4: Comparison of numerical and analytical P using a better integration scheme June 18, 2008 Spr 2008 Numerical Calculation of P 1 2 3 4 5 6 7 8 % Simple LQR example showing time varying P and gains % 16.323 Spring 2008 % Jonathan How % reg2.m clear all;close all; set(0, ’DefaultAxesFontSize’, 14, ’DefaultAxesFontWeight’,’demi’) set(0, ’DefaultTextFontSize’, 14, ’DefaultTextFontWeight’,’demi’) global A B Rxx Ruu 9 10 11 12 A=3;B=11;Rxx=7;Ptf=13;tf=2;dt=.0001; Ruu=20^2; Ruu=2^2; 13 14 15 16 17 18 19 20 21 22 % integrate the P backwards (crude form) time=[0:dt:tf]; P=zeros(1,length(time));K=zeros(1,length(time));Pcurr=Ptf; for kk=0:length(time)-1 P(length(time)-kk)=Pcurr; K(length(time)-kk)=inv(Ruu)*B’*Pcurr; Pdot=-Pcurr*A-A’*Pcurr-Rxx+Pcurr*B*inv(Ruu)*B’*Pcurr; Pcurr=Pcurr-dt*Pdot; end 23 24 25 26 27 28 29 30 31 32 options=odeset(’RelTol’,1e-6,’AbsTol’,1e-6) [tau,y]=ode45(@doty,[0 tf],vec(Ptf)); Tnum=[];Pnum=[];Fnum=[]; for i=1:length(tau) Tnum(length(tau)-i+1)=tf-tau(i); temp=unvec(y(i,:)); Pnum(length(tau)-i+1,:,:)=temp; Fnum(length(tau)-i+1,:)=-inv(Ruu)*B’*temp; end 33 34 35 % get the SS result from LQR [klqr,Plqr]=lqr(A,B,Rxx,Ruu); 36 37 38 39 40 41 42 43 % Analytical pred beta=sqrt(A^2+Rxx/Ruu*B^2); t=tf-time; Pan=((A*Ptf+Rxx)*sinh(beta*t)+beta*Ptf*cosh(beta*t))./... ((B^2*Ptf/Ruu-A)*sinh(beta*t)+beta*cosh(beta*t)); Pan2=((A*Ptf+Rxx)*sinh(beta*(tf-Tnum))+beta*Ptf*cosh(beta*(tf-Tnum)))./... ((B^2*Ptf/Ruu-A)*sinh(beta*(tf-Tnum))+beta*cosh(beta*(tf-Tnum))); 44 45 46 47 48 49 50 51 52 53 54 55 figure(1);clf plot(time,P,’bs’,time,Pan,’r.’,[0 tf],[1 1]*Plqr,’m--’) title([’A = ’,num2str(A),’ B = ’,num2str(B),’ R_{xx} = ’,num2str(Rxx),... ’ R_{uu} = ’,num2str(Ruu),’ P_{tf} = ’,num2str(Ptf)]) legend(’Numerical’,’Analytic’,’Pss’,’Location’,’West’) xlabel(’time’);ylabel(’P’) if Ruu > 10 print -r300 -dpng reg2_1.png; else print -r300 -dpng reg2_2.png; end 56 57 58 59 60 61 62 63 64 65 66 67 figure(3);clf plot(Tnum,Pnum,’bs’,Tnum,Pan2,’r.’,[0 tf],[1 1]*Plqr,’m--’) title([’A = ’,num2str(A),’ B = ’,num2str(B),’ R_{xx} = ’,num2str(Rxx),... ’ R_{uu} = ’,num2str(Ruu),’ P_{tf} = ’,num2str(Ptf)]) legend(’Numerical’,’Analytic’,’Pss’,’Location’,’West’) xlabel(’time’);ylabel(’P’) if Ruu > 10 print -r300 -dpng reg2_13.png; else print -r300 -dpng reg2_23.png; end June 18, 2008 16.323 4–17 Spr 2008 16.323 4–18 68 69 70 71 72 73 74 75 76 77 78 Pan2=inline(’((A*Ptf+Rxx)*sinh(beta*t)+beta*Ptf*cosh(beta*t))/((B^2*Ptf/Ruu-A)*sinh(beta*t)+beta*cosh(beta*t))’); x1=zeros(1,length(time));x2=zeros(1,length(time)); xcurr1=[1]’;xcurr2=[1]’; for kk=1:length(time)-1 x1(:,kk)=xcurr1; x2(:,kk)=xcurr2; xdot1=(A-B*Ruu^(-1)*B’*Pan2(A,B,Ptf,Ruu,Rxx,beta,tf-(kk-1)*dt))*x1(:,kk); xdot2=(A-B*klqr)*x2(:,kk); xcurr1=xcurr1+xdot1*dt; xcurr2=xcurr2+xdot2*dt; end 79 80 81 82 83 84 85 86 87 88 89 figure(2);clf plot(time,x2,’bs’,time,x1,’r.’);xlabel(’time’);ylabel(’x’) title([’A = ’,num2str(A),’ B = ’,num2str(B),’ R_{xx} = ’,num2str(Rxx),... ’ R_{uu} = ’,num2str(Ruu),’ P_{tf} = ’,num2str(Ptf)]) legend(’K_{ss}’,’K_{analytic}’,’Location’,’NorthEast’) if Ruu > 10 print -r300 -dpng reg2_11.png; else print -r300 -dpng reg2_22.png; end 6 function [doy]=doty(t,y); global A B Rxx Ruu; P=unvec(y); dotP=P*A+A’*P+Rxx-P*B*Ruu^(-1)*B’*P; doy=vec(dotP); return 1 function y=vec(P); 1 2 3 4 5 2 3 4 5 6 y=[]; for ii=1:length(P); y=[y;P(ii,ii:end)’]; end 7 8 return 1 function P=unvec(y); 2 3 4 5 6 7 8 9 10 11 N=max(roots([1 1 -2*length(y)])); P=[];kk=N;kk0=1; for ii=1:N; P(ii,ii:N)=[y(kk0+[0:kk-1])]’; kk0=kk0+kk; kk=kk-1; end P=(P+P’)-diag(diag(P)); return June 18, 2008 Spr 2008 Finite Time LQR Example 16.323 4–19 • Simple system with t0 = 0 and tf = 10sec. � � � � 0 1 0 ẋ = x+ u 0 1 1 � � � � � � 10 � 0 0 q 0 2J = xT (10) x(10) + xT (t) x(t) + ru2(t) dt 0 0 0 h 0 • Compute gains using both time-varying P (t) and steady-state value. Figure 4.5: Set q = 1, r = 3, h = 4 June 18, 2008 Spr 2008 16.323 4–20 • Find state solution x(0) = [1 1]T using both sets of gains Figure 4.6: Time-varying and constant gains - Klqr = [0.5774 2.4679] Figure 4.7: State response - Constant gain and time-varying gain almost indistin­ guishable because the transient dies out before the time at which the gains start to change – effectively a steady state problem. • For most applications, the static gains are more than adequate - it is only when the terminal conditions are important in a short-time horizon problem that the time-varying gains should be used. – Significant savings in implementation complexity & computa­ tion. June 18, 2008 Spr 2008 Finite Time LQR Example 1 2 3 4 5 6 7 8 9 10 % Simple LQR example showing time varying P and gains % 16.323 Spring 2008 % Jonathan How % reg1.m % clear all;%close all; set(0, ’DefaultAxesFontSize’, 14, ’DefaultAxesFontWeight’,’demi’) set(0, ’DefaultTextFontSize’, 14, ’DefaultTextFontWeight’,’demi’) global A B Rxx Ruu jprint = 0; 11 12 13 14 15 h=4;q=1;r=3; A=[0 1;0 1];B=[0 1]’;tf=10;dt=.01; Ptf=[0 0;0 h];Rxx=[q 0;0 0];Ruu=r; Ptf=[0 0;0 1];Rxx=[q 0;0 100];Ruu=r; 16 17 18 19 20 21 22 % alternative calc of Ricc solution H=[A -B*B’/r ; -Rxx -A’]; [V,D]=eig(H); % check order of eigenvalues Psi11=V(1:2,1:2); Psi21=V(3:4,1:2); Ptest=Psi21*inv(Psi11); 23 24 if 0 25 26 27 28 29 30 31 32 33 34 35 36 % integrate the P backwards (crude) time=[0:dt:tf]; P=zeros(2,2,length(time)); K=zeros(1,2,length(time)); Pcurr=Ptf; for kk=0:length(time)-1 P(:,:,length(time)-kk)=Pcurr; K(:,:,length(time)-kk)=inv(Ruu)*B’*Pcurr; Pdot=-Pcurr*A-A’*Pcurr-Rxx+Pcurr*B*inv(Ruu)*B’*Pcurr; Pcurr=Pcurr-dt*Pdot; end 37 38 39 40 41 42 43 44 45 46 47 48 else % integrate forwards (ODE) options=odeset(’RelTol’,1e-6,’AbsTol’,1e-6) [tau,y]=ode45(@doty,[0 tf],vec(Ptf),options); Tnum=[];Pnum=[];Fnum=[]; for i=1:length(tau) time(length(tau)-i+1)=tf-tau(i); temp=unvec(y(i,:)); P(:,:,length(tau)-i+1)=temp; K(:,:,length(tau)-i+1)=inv(Ruu)*B’*temp; end 49 50 end % if 0 51 52 53 % get the SS result from LQR [klqr,Plqr]=lqr(A,B,Rxx,Ruu); 54 55 56 57 58 59 60 61 62 63 64 65 66 67 x1=zeros(2,1,length(time)); x2=zeros(2,1,length(time)); xcurr1=[1 1]’; xcurr2=[1 1]’; for kk=1:length(time)-1 dt=time(kk+1)-time(kk); x1(:,:,kk)=xcurr1; x2(:,:,kk)=xcurr2; xdot1=(A-B*K(:,:,kk))*x1(:,:,kk); xdot2=(A-B*klqr)*x2(:,:,kk); xcurr1=xcurr1+xdot1*dt; xcurr2=xcurr2+xdot2*dt; end June 18, 2008 16.323 4–21 Spr 2008 68 69 x1(:,:,length(time))=xcurr1; x2(:,:,length(time))=xcurr2; 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 figure(5);clf subplot(221) plot(time,squeeze(K(1,1,:)),[0 10],[1 1]*klqr(1),’m--’,’LineWidth’,2) legend(’K_1(t)’,’K_1’) xlabel(’Time (sec)’);ylabel(’Gains’) title([’q = ’,num2str(1),’ r = ’,num2str(r),’ h = ’,num2str(h)]) subplot(222) plot(time,squeeze(K(1,2,:)),[0 10],[1 1]*klqr(2),’m--’,’LineWidth’,2) legend(’K_2(t)’,’K_2’) xlabel(’Time (sec)’);ylabel(’Gains’) subplot(223) plot(time,squeeze(x1(1,1,:)),time,squeeze(x1(2,1,:)),’m--’,’LineWidth’,2), legend(’x_1’,’x_2’) xlabel(’Time (sec)’);ylabel(’States’);title(’Dynamic Gains’) subplot(224) plot(time,squeeze(x2(1,1,:)),time,squeeze(x2(2,1,:)),’m--’,’LineWidth’,2), legend(’x_1’,’x_2’) xlabel(’Time (sec)’);ylabel(’States’);title(’Static Gains’) 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 figure(6);clf subplot(221) plot(time,squeeze(P(1,1,:)),[0 10],[1 1]*Plqr(1,1),’m--’,’LineWidth’,2) legend(’P(t)(1,1)’,’P_{lqr}(1,1)’,’Location’,’SouthWest’) xlabel(’Time (sec)’);ylabel(’P’) title([’q = ’,num2str(1),’ r = ’,num2str(r),’ h = ’,num2str(h)]) subplot(222) plot(time,squeeze(P(1,2,:)),[0 10],[1 1]*Plqr(1,2),’m--’,’LineWidth’,2) legend(’P(t)(1,2)’,’P_{lqr}(1,2)’,’Location’,’SouthWest’) xlabel(’Time (sec)’);ylabel(’P’) subplot(223) plot(time,squeeze(P(2,1,:)),[0 10],[1 1]*squeeze(Plqr(2,1)),’m--’,’LineWidth’,2), legend(’P(t)(2,1)’,’P_{lqr}(2,1)’,’Location’,’SouthWest’) xlabel(’Time (sec)’);ylabel(’P’) subplot(224) plot(time,squeeze(P(2,2,:)),[0 10],[1 1]*squeeze(Plqr(2,2)),’m--’,’LineWidth’,2), legend(’P(t)(2,2)’,’P_{lqr}(2,2)’,’Location’,’SouthWest’) xlabel(’Time (sec)’);ylabel(’P’) axis([0 10 0 8]) if jprint; print -dpng -r300 reg1_6.png end 111 112 113 114 115 116 117 118 119 120 121 122 123 figure(1);clf plot(time,squeeze(K(1,1,:)),[0 10],[1 1]*klqr(1),’r--’,’LineWidth’,3) legend(’K_1(t)(1,1)’,’K_1(1,1)’,’Location’,’SouthWest’) xlabel(’Time (sec)’);ylabel(’Gains’) title([’q = ’,num2str(1),’ r = ’,num2str(r),’ h = ’,num2str(h)]) print -dpng -r300 reg1_1.png figure(2);clf plot(time,squeeze(K(1,2,:)),[0 10],[1 1]*klqr(2),’r--’,’LineWidth’,3) legend(’K_2(t)(1,2)’,’K_2(1,2)’,’Location’,’SouthWest’) xlabel(’Time (sec)’);ylabel(’Gains’) if jprint; print -dpng -r300 reg1_2.png end 124 125 126 127 128 129 130 figure(3);clf plot(time,squeeze(x1(1,1,:)),time,squeeze(x1(2,1,:)),’r--’,’LineWidth’,3), legend(’x_1’,’x_2’) xlabel(’Time (sec)’);ylabel(’States’);title(’Dynamic Gains’) if jprint; print -dpng -r300 reg1_3.png end 131 132 133 134 135 136 137 figure(4);clf plot(time,squeeze(x2(1,1,:)),time,squeeze(x2(2,1,:)),’r--’,’LineWidth’,3), legend(’x_1’,’x_2’) xlabel(’Time (sec)’);ylabel(’States’);title(’Static Gains’); if jprint; print -dpng -r300 reg1_4.png end June 18, 2008 16.323 4–22 Spr 2008 Weighting Matrix Selection16.323 4–23 • A good rule of thumb when selecting the weighting matrices Rxx and Ruu is to normalize the signals: α12 ⎢ (x1)2 max ⎢ 2 ⎢ α 2 ⎢ = ⎢ (x2)2max ⎢ ... ⎢ ⎢ ⎣ ⎡ Rxx β12 ⎢ (u1)2 max ⎢ 2 ⎢ β 2 ⎢ = ρ⎢ (u2)2max ⎢ ... ⎢ ⎢ ⎣ ⎤ αn2 (xn)2max ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ⎡ Ruu ⎤ 2 βm (um)2max ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ • The (xi)max and (ui)max represent the largest desired response/control input for that component of the state/actuator signal. � � • The i αi2 = 1 and i βi2 = 1 are used to add an additional relative weighting on the various components of the state/control • ρ is used as the last relative weighting between the control and state penalties ⇒ gives us a relatively concrete way to discuss the relative size of Rxx and Ruu and their ratio Rxx/Ruu • Note: to directly compare the continuous and discrete LQR, you must modify the weighting matrices for the discrete case, as outlined here using lqrd. June 18, 2008