%Example 9.1 LQG design (see Table 9.1)

%Plant (2.26)
G=nd2sys([-2,1],[50 15 1],3);  % original plant 
int = nd2sys(1,[1 0.0001]);    % integrator (moved slightly into LHP for numerical reasons)
Gs = mmult(G,int);             % augmented plant (with integrator)
[A,B,C,D] = unpck(Gs);         % Augmented Plant is [A,B,C,D]
Q = 1.0*C'*C;                  % weight on outputs  
R = 1;                         % weight on inputs, R small gives faster response 
Kx = lqr(A,B,Q,R);             % optimal regulator
Bnoise = eye(size(A));         % process noise directly on states 
W = eye(size(A));              % process noise  
V = 1;                         % measurement noise, V small gives faster response 
Ke = lqe(A,Bnoise,C,W,V);      % optimal estimator
[Ac,Bc,Cc,Dc] = reg(A,B,C,D,Kx,Ke);          % combine regulator and estimator
Ks = pck(Ac,Bc,Cc,Dc); Klqg = mmult(Ks,int); % include integrator in final controller

c=Klqg; g=G;

gc = mmult(g,c);
s = minv(madd(1,gc));
t = msub(1,s);
u=1;
y1 = trsp(t,u,50);
%input signal
gs = mmult(c,s); u1=trsp(gs,u,50);
%Figure 9.6
subplot(111),vplot(y1,'-',u1,'--',1,':')
axis([0 50 -0.5 2.5]);
text(40,1.15,'yt');text(40,0.2,'ut');
xlabel('Time');%ylabel('y');
