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')
  