% dlq_ex4_du.m

% Modellparametre
V=1; k=1; s=2;

% Stasjonaerverdier
u1s=10; u2s=1;
n1=-u1s+sqrt(u1s^2+4*k*V*s*u1s*u2s); d1=2*k*V*s; x1s=n1/d1 % Loeser f1(x,u)=0.
n2=k*x1s^2*V; d2=u1s; x2s=n2/d2                            % Loeser f2(x,u)=0.

r=[x1s;x2s];

% Linearisert kontinuerlig modell
Ac=[-u1s/V-2*s*k*x1s, 0; 2*k*x1s, -u1s/V];
Bc=[u2s/V, u1s/V; -x2s/V, 0];
Dc=eye(2);

h=0.01; t0=0; t1=4; N=(t1-t0)/h;
% Diskret modell
[A,B,D,E]=c2dm(Ac,Bc,Dc,zeros(2,2),h,'zoh');

% LQ-regulator design
P=[1,0;0,100]; Q=[50,0;0,100];
%P=eye(2);Q=eye(2)*50;
[G1,G2]=dlqdu_pi(A,B,D,Q,P);

% Simulering
f=zeros(2,1); U=zeros(N,2); Y=zeros(N,2); R=zeros(N,2); T=zeros(N,1);
x=[x1s;x2s]; x_old=x; y_old=D*x_old; 
u=[u1s;u2s]; u_old=u;

for i=1:N
   y=D*x;
   
   if i > 100
      r(2)=0.07;
   end
   if i > 250
      r(1)=0.85; r(2)=0.07;
   end

   u=u+G1*(x-x_old)+G2*(y_old-r);
   y_old=y; x_old=x;
   
   U(i,:)=u'; Y(i,:)=y'; R(i,:)=r'; T(i)=t0;
   
   f(1)=u(1)*(u(2)-x(1))/V-s*k*x(1)^2;
   f(2)=-u(1)*x(2)/V+k*x(1)^2;
   x=x+h*f;
   t0=t0+h;                                    % Update time.
end

subplot(221), plot(T,U(:,1)), grid, title('u_1: LQ-optimal')
subplot(222), plot(T,U(:,2)), grid, title('u_2: LQ-optimal')
subplot(223), plot(T,R(:,1),'r--',T,Y(:,1),'b-'), grid, title('y_1')
legend('r','y_1'), xlabel('Time [s]')
subplot(224), plot(T,R(:,2),'r--',T,Y(:,2),'b-'), grid, title('y_2')
legend('r','y_1'), xlabel('Time [s]')

