% Solution for exercise in week 45, SCEV3006 Advanced Control
%

A=[1.5 1.0 0.10;
  -0.7 0.0 0.10
   0.0 0.0 0.85];
B=[0 0;
   0 1
   1 0];
D=[3 0 -0.6
   0 1    1];

Q=0.03*eye(2); P=eye(2);

At=[A zeros(3,2); D eye(2)]; 
Bt=[B; zeros(2,2)];    
Dt=[D eye(2)];
Qt=Dt'*Q*Dt;

G=dlqr(At,Bt,Qt,P); G=-G;
G1=G(:,1:3); G2=G(:,4:5);

r=[1;0.5];
N=250;
Ns=250;                                     % Simulation horizon.
rand('seed',0), randn('seed',0)
ref=[0.9*ones(Ns,1)+0.1*prbs1(Ns,30,70) ...
     0.4*ones(Ns,1)+0.1*prbs1(Ns,30,70)];   % Reference r_k.


Hd=D*inv(eye(3)-A)*B;
us=inv(Hd)*r
xs=inv(eye(3)-A)*B*us;

u_old=us;
x_old=xs;
y_old=r;
x=xs;


for k=1:N
    y=D*x;
        
    u=u_old + G1*(x-x_old) + G2*(y_old-ref(k,:)');
    
    u_old=u; x_old=x; y_old=y;  % Storing, u_(k-1), x_(k-1) and y_(k-1) for use in next time instant.
    
    Y(k,:)=y'; U(k,:)=u';
    
    x=A*x+B*u;
end

t=1:N; 
figure(1)
subplot(211), plot(t,[Y(:,1) ref(:,1)]), grid, ylabel('y1_k and r1_k')
title('Output y_k and reference r_k')
subplot(212), plot(t,[Y(:,2) ref(:,2)]), grid, ylabel('y2_k and r2_k')
xlabel('Discrete time')

figure(2)
subplot(211), plot(t,U(:,1)), grid, ylabel('u1_k')
title('Actual control input, u_k')
subplot(212), plot(t,U(:,2)), grid, ylabel('u2_k')
xlabel('Discrete time')
