% ejemplo 5.14, Aström_Adaptive Control, pag. 241
% "Second-order MRAS"
% continuous system gmras
% to simulate a general MRAS where 
%   G(s)=K/[s(s+a)]
%   Gm(s)=om*om/(s*s+2*z*om*s+om*om)
%   Ao(s)=s+ao
%   P1(s)=Am=s*s+p11*s+p12
%   P2(s)=Ao=s+p2
% elaboro E.Quintero.M 11sep'98
%---------------------------------------------------
clear
% parametros
time=0; tf=50;
a=1; K=2; b0=2; om=2; z=0.7; ao=3; gamma=1;
dt=0.05;    % integration step size
alfa=0.1; per=20; step=1;
twave=15; u=0; ef=0; uc=1;
ulim=5;   % limit of control signal
x1=0; x2=0; xm1=0; xm2=0; xc=0; xu=0; xy=0;
th1=0; th2=0; th3=0; th4=0; th5=0;
zc1=0; zc2=0; zu1=0; zu2=0; zy1=0; zy2=0;
y=0; ym=0;  
while(time<(tf-dt))
%---------------------------------------------------
      if(abs(time-twave)<dt/2)
         if(uc==1) uc=-1; else uc=1; end
         twave=twave+15;
      end
%      dy=-a*y+b*u;
      if(time==0) xx=[time y ym u ef th1 th2 th3 th4 th5]; end
%---------------------------------------------------
%     generation of fi=[fi1 fi2 fi3 fi4 fi5],
%     P1*fi=[fd1 fd2 fd3 fd4 fd5], and uf=(1/P1)*u
      p11=2*z*om;
      p12=om*om;
      p2=ao;
      dxc=-p2*xc+uc;
      dxu=-p2*xu+u;
      dxy=-p2*xy+y;
      fd1=uc-p2*xc;
      fd2=xc;
      fd3=-xu;
      fd4=-y+p2*xy;
      fd5=-xy;
      dzc1=-p11*zc1-p12*zc2+xc;
      dzc2=zc1;
      dzu1=-p11*zu1-p12*zu2+xu;
      dzu2=zu1;
      dzy1=-p11*zy1-p12*zy2+xy;
      dzy2=zy1;
      fi1=zc1;
      fi2=zc2;
      fi3=-zu2;
      fi4=-zy1;
      fi5=-zy2;
      uf=zu1+p2*zu2;
%     the system
      dx1=-a*x1+K*u;
      dx2=x1;
      y=x2;
%     the model
      dxm1=-2*z*om*xm1-om*om*xm2+om*om*uc;
      dxm2=xm1;
      ym=xm2;
%     if(mod(t,per)<per/2) uc=step; else uc=-step; end % ref signal
%     the residual
      ef=y-ym;
      eta=-(uf-th1*fi1-th2*fi2-th3*fi3-th4*fi4-th5*fi5);
      eps=ef+b0*eta;
%     the control signal
      ut=th1*fd1+th2*fd2+th3*fd3+th4*fd4+th5*fd5;
      if(ut<-ulim) u=-ulim; elseif(ut<ulim) u=ut; else u=ulim; end
%     parameter update
      den=fi1*fi1+fi2*fi2+fi3*fi3+fi4*fi4+fi5*fi5+alfa;
      dth1=-gamma*fi1*eps/den;
      dth2=-gamma*fi2*eps/den;
      dth3=-gamma*fi3*eps/den;
      dth4=-gamma*fi4*eps/den;
      dth5=-gamma*fi5*eps/den;

      x1=x1+dx1*dt;
      x2=x2+dx2*dt;
      xm1=xm1+dxm1*dt;
      xm2=xm2+dxm2*dt;
      xc=xc+dxc*dt;
      xu=xu+dxu*dt;
      xy=xy+dxy*dt;

      th1=th1+dth1*dt;
      th2=th2+dth2*dt;
      th3=th3+dth3*dt;
      th4=th4+dth4*dt;
      th5=th5+dth5*dt;

      zc1=zc1+dzc1*dt;
      zc2=zc2+dzc2*dt;
      zu1=zu1+dzu1*dt;
      zu2=zu2+dzu2*dt;
      zy1=zy1+dzy1*dt;
      zy2=zy2+dzy2*dt;

      time=time+dt;
      xx=[xx; time y ym u ef th1 th2 th3 th4 th5];
end
format short e
time,gamma,th1,th2
subplot(2,1,1),plot(xx(:,1),xx(:,2),xx(:,1),xx(:,3))
subplot(2,1,2),plot(xx(:,1),xx(:,5))
figure
subplot(1,1,1),plot(xx(:,1),xx(:,6),xx(:,1),xx(:,7),xx(:,1),xx(:,8), ...
                    xx(:,1),xx(:,9),xx(:,1),xx(:,10))