% Example based on Exercise 5, Task 1.

% Model and parameters in LQ objective
a=-0.5; b=1; d=1; c=0.6;
Q_x=2; Q_z=1; Q_v=1; P=1;

% Matrices in aygmented problem
Qt=diag([Q_x,Q_z,Q_v]);
At=[a 0 c;-d 0 0;0 0 0]; Bt=[b;0;0];

% Solve Riccati equation (this version works, matlab LQR does not)
R=are_schur(At,Bt,Qt,P)
G=-inv(P)*Bt'*R;
G1=G(1); G2=G(:,2); G3=G(:,3);

% Simulate system
v=1;
r=1;
x=0; z=0;
dt=0.01;
t=0:dt:20; t=t';
N=length(t)

for k=1:N
    y=d*x;              % Measurement
    
    if k>N/2; r=0.5; end
    
% LQ optimal PI controller with feedforward from disturbance    
    u=G1*x+G2*z+G3*v;
    z=z+dt*(r-y);
% Store some variables for plotting purposes
    Y(k,1)=y; Rf(k,1)=r;    
  
    x=x+dt*(a*x+b*u+c*v); % Update, feed control to the process
end

figure(1)
plot(t,Rf,'-',t,Y,'-.')
legend('Reference','Process output')

