%%% Programa: exemplo_3_9_istr.m
%%% Autor: Geovany A. Borges
%%% Objectivo: Exemplo 3.9 do livro de controle adaptativo.
%%% Notas:
%%%

close all;
echo off;

FlagImprimir = 0;

% Modelo do processo:
b0 = 0.1065;
b1 = 0.0902;
a1 = -1.6065;
a2 = 0.6065;

% Modelo de referência:
bm0 = 0.1761;
am1 = -1.3205;
am2 = 0.4966;

% Observador:
ao = 0;

% laço principal:
N = 200;
h = 0.5;

y  = zeros(1,N);
v  = zeros(1,N);
u  = zeros(1,N);
uc = zeros(1,N);

lambda = 0.98;
epslon = zeros(1,N);
theta = zeros(4,N);
P = diag([100 100 100 100]);
theta(1,1) = 0;
theta(2,1) = 0;
theta(3,1) = 0.01;
theta(4,1) = 0.2;
for n=1:N,
   % Perturbação:
   if n>=40/h
      v(n) = 1;
   else
      v(n) = 0;
   end
   % Processo:
   if n==1,
      y(n) = 0;
   end
   if n==2
      y(n) = b0*u(n-1) - a1*y(n-1);
   end
   if n>=3
      y(n) = b0*(u(n-1)+v(n-1)) + b1*(u(n-2)+v(n-2)) - a1*y(n-1) - a2*y(n-2);
   end
   % Referência de controle:
   if rem(n,50)==1
      if n == 1,
         uc(n) = 1;
      else
         uc(n) = -uc(n-1);
      end
   else
      uc(n) = uc(n-1);
   end
   % Estimador.
   if n==2,
      theta(:,n) = theta(:,n-1);
   end
   if n>=3,
      phi = [-y(n-1) -y(n-2) u(n-1) u(n-2)]';
      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(4)-K*phi')*P/lambda;
   end
   a1e = theta(1,n);
   a2e = theta(2,n);
   b0e = theta(3,n);
   b1e = theta(4,n);
   % a1e = a1; a2e = a2; b0e = b0; b1e = b1;
   % Controlador:
   r1 = (ao*am2*b0e*b0e + (a2e-am2-ao*am1)*b0e*b1e+(ao+am1-a1e)*b1e*b1e)/(b1e*b1e-a1e*b0e*b1e+a2e*b0e*b0e);
   s0 = (b1e*(ao*am1-a2e-am1*a1e+a1e*a1e+am2-a1e*ao)+b0e*(am1*a2e-a1e*a2e-ao*am2+ao*a2e))/(b1e*b1e-a1e*b0e*b1e+a2e*b0e*b0e);
   s1 = (b1e*(a1e*a2e-am1*a2e+ao*am2-ao*a2e)+b0e*(a2e*am2-a2e*a2e-ao*am2*a1e+ao*a2e*am1))/(b1e*b1e-a1e*b0e*b1e+a2e*b0e*b0e);
   beta = (1+am1+am2)/(b0e+b1e);
   t0 = beta;
   t1 = beta*ao;
   if n == 1
      u(n) = -s0*y(n)+t0*uc(n);
   end   
   if n >= 2
      u(n) = -r1*u(n-1)-s0*y(n)-s1*y(n-1)+t0*uc(n)+t1*uc(n-1);
   end   
end

t = [1:N]'*h;

figure(1);
[T,U] = stairs(t,u);
subplot(211); plot(T,U); hold on; ylabel('u(t)');
subplot(212); plot(t,y,'b'); hold on; plot(t,uc,'r'); ylabel('y(t), uc(t)'); xlabel('tempo');

figure(2);
subplot(211); plot(t,theta(1:2,:),'b'); hold on; plot(t,[a1*ones(1,N);a2*ones(1,N)],'r--'); ylabel('(a1e,a1) e (a2e,a2)');
subplot(212); plot(t,theta(3:4,:),'b'); hold on; plot(t,[b0*ones(1,N);b1*ones(1,N)],'r--'); ylabel('(b1e,b1) e (b2e,b2)');


% 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_3_9_isrt',7,[1 2 2 2 2 2 2],5,6);
end
