% Uses the control tool box
% Plant model

num = 3*[-2 1]; den = conv([5 1],[10 1]);  % original plant
[a,b,c,d]=tf2ss(num,den);

[n,m]=size(b);   		% finds state-vector and control sizes
p    =size(c,1); 		% finds output vector size
% First design state feedback regulator
A = [a zeros(n,m) ; -c zeros(m,m)]; B = [b ; -d]; % augment plant with integrators
Q=[zeros(n,n) zeros(n,m); zeros(m,n) eye(m,m)];   % weight on integrated error 
R=1*eye(m);                   % Input weight - R small gives faster response
Kr=lqr(A,B,Q,R);            % Design optimal state-feedback regulator ;
Krp = Kr(1:m,1:n); Kri = Kr(1:m,n+1:n+m); % Extract integrator and state feedbacks:
% Now design Kalman filter for plant
Bnoise = eye(n);         % Process noise (disturbance) enters directly on states
W      = eye(n);         % Process noise weight
V      = 1*eye(m);       % Measurement noise weight - decreasing makes response faster
Ke = lqe(a,Bnoise,c,W,V);% no need to estimate integrator states
% Form 2DOF controller from  [r y]' to u with integrators included
Ac = [ zeros(m,m) zeros(m,n); -b*Kri   a-b*Krp-Ke*c];  % integrators included
Bc = [eye(m) -eye(m); zeros(n,m) +Ke];
Cc = [-Kri -Krp]; Dc = [zeros(m,m) zeros(m,m)];
K = ss(Ac,Bc,Cc,Dc);

Gsim = ss(a,b,[c;c],[d;d]);
Ksim = ss(Ac,Bc,[Cc;Cc],[Dc;Dc]);
sys = lft(Gsim,-Ksim,1,1);

        [out,T]=step(sys,0:50);
        plot(T,out(:,1),'-',T,out(:,2),'--'); 

    % Now form closed loop transfer function from [r] to [y u]'
	% (bearing in mind that Dc=0)
    %partition controller B-matrix to get Bc1 (r) and Bc2 (y)

    Bc1=Bc(:,1:m); Bc2=Bc(:,m+1:m+m);

    Acl = [a b*Cc ; Bc2*c Ac+Bc2*d*Cc ];
    Bcl = [zeros(n,m) ; Bc1 ];
    Ccl = [c d*Cc ; zeros(m,n) Cc]; % Augment to include control signal u
    Dcl = [zeros(m,m); zeros(m,m)];

    %
    % Setup simulation
    %

    t=linspace(0,50,1000);

    hold;
	for j=1:m
        y=step(Acl,Bcl,Ccl,Dcl,j,t);
        subplot(2,1,1), plot(t,y(:,j),'-',t,y(:,m+j),'--',[1 50],[1 1],':'); 
        xlabel('Time');
    end;
	hold;
	axis([0 50 -0.5 2.5])
    subplot(2,1,1), text(41,1.2,'y')
    subplot(2,1,1), text(41,0.5,'u')
