clear all clear functions figure(1) clf set(1,'papersize',[5,5],'paperposition',[0,0,5,5]) figure(2) clf set(2,'papersize',[5,3],'paperposition',[0,0,5,3]) % LQR figure(1) gamma=.1; rho=.01; A = [0,1,0;0,-.875,-20;0,0,-50]; B = [0;0;50]; % process dynamics C = [1,0,0]; % measured output G = [1,0,0;0,gamma*1,0]; H = [0;0]; % controlled output z Q = G'*G; R = H'*H+rho; N = G'*H; % weight matrices K=lqr(A,B,Q,R,N); % compute LQR gain G0=ss(A,B,K,0); % open-loop gain h1=mybode(G0,{10^-2,10^3},'-'); % step figure(2) xueq=[-A,B;-G(1,:),H(1,:)]\[0;0;0;1]; F=-xueq(1:3); N=xueq(end); T=ss(A-B*K,B*(K*F+N),G(1,:)-H(1,:)*K,H(1,:)*(K*F+N)); l1=mystep(T,1.9,'-'); %% LQG figure(1) sigma = 1e8; R=1; Q=sigma; % weight matrices Pkal=ss(A,[B B],C,0); % process for the kalman() command [est,L]=kalman(Pkal,inv(R),inv(Q));% compute LQG gain P=ss(A,B,C,0); % process for the reg() command Cs=-reg(P,K,L); % LQG/LQR controller (negative feedback) h2=mybode(Cs*P,{10^-2,10^3},'o-'); % bode plot of the open-loop gain % step figure(2) T=ss([A,B*K;-L*C,A-L*C-B*K],[B*N;L*C*F],[G(1,:),H(1,:)*K],[H(1,:)*N]); l2=mystep(T,1.9,'--'); %% legends figure(1) legend([h1,h2],'LQR open-loop gain','LQG/LQR open-loop gain','location','southwest') figure(2) legend([l1,l2],'LQR closed-loop response','LQG/LQR closed-loop response','location','southeast')