%%% Programa: exemplo_4_2_lqstr.m
%%% Autor: Geovany A. Borges
%%% Objectivo: Exemplo 4.2 do livro de controle adaptativo refeito usando um controlador auto-ajustável linear quadrático.
%%% Notas:
%%%

close all;
echo off;

FlagImprimir = 0;

% Modelo:
a = -0.9; b = 3; c = -0.3;

% Controlador:
rho = 2;

% laço principal:
N = 3000;

y  = zeros(1,N);
ym = zeros(1,N);
u  = zeros(1,N);
e  = zeros(1,N);

lambda = 0.99;
epslon = zeros(1,N);
theta = zeros(3,N);
P = 100*eye(3);
theta(1,1) = -0.5;
theta(2,1) = 2;
theta(3,1) = -0.5;
for n=2:N,
   % Processo:
   e(n) = 0.01*(rand(1)-0.5);
   if n==1,
      y(n) = e(n);
   end
   if n>=2
      y(n) = -a*y(n-1)+b*u(n-1)+e(n)+c*e(n-1);
   end
   % Referência:
   if rem(n,200)==0
      if n == 200,
         ym(n) = 1;
      else
         ym(n) = -ym(n-1);
      end
   else
      ym(n) = ym(n-1);
   end
   % Controlador:
   if n >= 2
      % Step 1: Estimador.
      phi = [-y(n-1) u(n-1) epslon(n-1)]';
      epslon(n) = y(n) - phi'*theta(:,n-1);
      K = P*phi/(lambda+phi'*P*phi);
      theta(:,n) = theta(:,n-1) + K*epslon(n);
      P = (eye(3)-K*phi')*P/lambda;
      ae = theta(1,n);
      be = theta(2,n);
      ce = theta(3,n);
      % Step 2: Fatoração espectral.
      delta = rho^2*ae^2 - 4*(rho+ae^2+be^2)*(rho*a);
      p1_1 = (-(rho+ae^2*rho+be^2)+sqrt(delta))/(-2*rho*ae);
      p1_2 = (-(rho+ae^2*rho+be^2)-sqrt(delta))/(-2*rho*ae);
%      if abs(p1_1) < abs(p1_2)
%         if abs(p1_1)<1
%            p1 = p1_1;
%         end
%      else
%         if abs(p1_2)<1
%            p1 = p1_2;
%         end
%      end 
		p1 = p1_1;
      % Step 3: Diophantine.
      r1til = p1 + ce - ae - be;
      s1til = (ce*p1 - ae*r1til)/be;
      % Step 4: Lei de controle:
      r1 = r1til; s1 = s1til; t0 = (1+p1)/be; t1 = t0*c;
      u(n) = -r1*u(n-1)-y(n)-s1*y(n-1)+t0*ym(n)+t1*ym(n-1);
   end   
end

figure(1);
subplot(211); plot(u); hold on; ylabel('u(t)');
subplot(212); plot(y,'b'); hold on; plot(ym,'r'); ylabel('y(t), ym(t)'); xlabel('tempo');

figure(2);
subplot(311); plot(theta(1,:),'b'); hold on; plot(a*ones(1,N),'r'); ylabel('ae,a');
d = max(abs(a-theta(1,:))); axis([1 N a-1.1*d a+1.1*d]);
subplot(312); plot(theta(2,:),'b'); hold on; plot(b*ones(1,N),'r'); ylabel('be,b');
d = max(abs(b-theta(2,:))); axis([1 N b-1.1*d b+1.1*d]);
subplot(313); plot(theta(3,:),'b'); hold on; plot(c*ones(1,N),'r'); ylabel('ce,c');
d = max(abs(c-theta(3,:))); axis([1 N c-1.1*d c+1.1*d]);


% Controlador
%set(gca,'DataAspectRatio',[1 1 1]);
%axis([-200.0 3000 -200.0 2500.0]); hold on; 
%xlabel('X [m]','FontSize',FontSize);
%ylabel('Y [m]','FontSize',FontSize);
%set(gca,'Box','on','FontSize',FontSize);

if FlagImprimir
	imprimirtudo('figexemplo_4_2_lqstr',7,[1 2 2 2 2 2 2],5,6);
end
