% ejemplo 3.6, Aström_Adaptive Control, pag. 110
% continuous time STR for the system b/[s(s+a)]
% desired response given by am2/(s^2+am1*s+am2)
% observer polynomial s+ao 
% elaboro E.Quintero.M 7jun'99
%-------------------------------------------------
clear
% parametros
time=0; tf=100;
am1=1.4; am2=1; alpha=0; ao=2;
dt=0.05;    % integration step size
ulim=4;    % limit of control signal
th=[2 0.2]';    % initial estimate
p=[100 0;0 100];     % initial covariances
f=[0 0]'; ysp=1; twave=25;
A=[0 1;0 -1]; B=[0;1];
y=0; yf=0; yf1=0; xu=0; u=0; uf=0; uf1=0; x=[0 0]';
xu=0;
while(time<(tf-dt))
%--------------------------------------------------------
      if(abs(time-twave)<dt/2)
         if(ysp==1) ysp=-1; else ysp=1; end
         twave=twave+25;
      end
      dx=A*x+B*u;
      y=x(1);
      if(time==0) xx=[time ysp y u th']; end
%--------------------------------------------------------
%     filter input and output
      dyf=yf1;
      dyf1=-am1*yf1+am2*(y-yf);
      duf=uf1;
      duf1=-am1*uf1+am2*(u-uf);

%     update parameter estimate
      f=[-yf1 uf]';
      e=dyf1-f'*th;
      pf=p*f;
      dth=pf*e;

%     update covariance matrix
      dp=alpha*p-pf*pf';

%     control design
      a=th(1);
      b=th(2);
      r1=am1+ao-a;
      s0=(am2+am1*ao-a*r1)/b;
      s1=am2*ao/b;
      t0=am2/b;

%     control signal computation
      dxu=-ao*xu-(s1-ao*s0)*y+(ao-r1)*u;
      v=t0*ysp-s0*y+xu;
      if(v<-ulim) u=-ulim; elseif(v>ulim) u=ulim; else u=v; end

      x=x+dx*dt;

      yf=yf+dyf*dt;
      yf1=yf1+dyf1*dt;
      uf=uf+duf*dt;
      uf1=uf1+duf1*dt;
      xu=xu+dxu*dt;
      th=th+dth*dt;
      p=p+dp*dt;
      time=time+dt;
      xx=[xx; time ysp y u th'];
end
format short e
time,a,b,r1,s0,s1,t0
figure
subplot(2,1,1),plot(xx(:,1),xx(:,2),xx(:,1),xx(:,3))
subplot(2,1,2),plot(xx(:,1),xx(:,4))
figure
subplot(2,1,1),plot(xx(:,1),xx(:,5),xx(:,1),xx(:,6))
subplot(2,1,2),plot(xx(1:400,1),xx(1:400,5),xx(1:400,1),xx(1:400,6))
