% Example 1.3 in lecture notes
%

clear
a =[-1.0000    0.1000
    0.2000   -2.0000]
%b=eye(2);
b = [1;2]; 
[n,r]=size(b);

x0=[0;0]; x=x0; 
x1=[1;1]; t1=4;
Wct1=gram2(a,b,t1);

N=1000;
t=0; dt=(4.5-0)/N;

for it=1:N
  f = inv(Wct1)*(expm(a*t1)*x0-x1);
  u = -b'*expm(a'*(t1-t))*inv(Wct1)*(expm(a*t1)*x0-x1);  % the control input
  eu(it,1:r)=u'; ex(it,1:n)=x'; et(it,1)=t;
  x = x + dt*(a*x+b*u);                                  % integrate with Euler
  t=t+dt;
end

subplot(211), plot(et,eu)
grid
title('Control input u(t)')
subplot(212), plot(et,ex)
grid
title('States x1(t) and x2(t)')
xlabel('Time [sec]')