% example 3.7, Aström_Adaptive Control, pag. 115
% "direct self-tuner with do=1"
% based on the model H(q)=(b0*q+b1)/(q^2+a1*q+a2)
% using standard rls estimation    
% polynomial B is canceled 
% elaboro E.Quintero.M 8jun'99
%-------------------------------------------------
clear
% parametros
time=0; tf=100;
lambda=1; % forgetting factor
z=0.7;    % desired closed loop damping
w=1;      % desired closed loop natural frecuency
h=0.5;    % sampling period
ulim=4;   % limit of control signal
th=[0.1 0 0 0]';   % initial estimates
p=[100 0 0 0;0 100 0 0;0 0 1 0;0 0 0 1];   % initial covariances
R0=0.1065; R1=0.8467*R0; S0=2.6852*R0; S1=-1.0321*R0;
f=[0 0 0 0]';
ysp=1; u=0; u1=0; y1=0; twave=25;
% initial
% compute sampled Am and Ao
a=exp(-z*w*h);
am1=-2*a*cos(w*h*sqrt(1-z*z))
am2=a*a
t0=1+am1+am2
while(time<tf)
%--------------------------------------------------------
      if(abs(time-twave)<h/2)
         if(ysp==1) ysp=-1; else ysp=1; end
         twave=twave+25;
      end
      y=R0*f(1)+R1*f(2)+S0*f(3)+S1*f(4);
      if(time==0) xx=[time ysp y u t0/th(1) th(2:4)'/th(1)]; end
%--------------------------------------------------------

%     1.0 parameter estimation
%     1.1 computation of p*f and estimator gain k
      pf=p*f;
      denom=lambda+f'*pf;
      k=pf/denom;

%     1.2 update estimates and covariances
      eps=y-f'*th;
      th=th+k*eps;
      p=(p-pf*k')/lambda;

%     2.0 control design
%     2.1 rename parameters
      r0=th(1);
      r1=th(2);
      s0=th(3);
      s1=th(4);

%     2.3 compute polynomial T=Ao*Am(1)/B(1)
      t0=1+am1+am2;

%     3.0 control law with anti-windup
      v=(t0*ysp-s0*y-s1*y1-r1*u1)/r0;
      if(v<-ulim) u=-ulim; elseif(v<ulim) u=v; else u=ulim; end

%     filter input and output
      uf=-am1*f(1)-am2*f(2)+u;
      yf=-am1*f(3)-am2*f(4)+y;

%     1.3 update and filter regression vector
      f=[uf f(1) yf f(3)]';

%     3.1 update controller state
      y1=y;
      u1=u;

%     4.0 update sampling time
      time=time+h;
      xx=[xx; time ysp y u  t0/th(1) th(2:4)'/th(1)];
end
format short e
time,r1r0=r1/r0,t0r0=t0/r0,s0r0=s0/r0,s1r0=s1/r0
subplot(2,1,1),plot(xx(:,1),xx(:,2),xx(:,1),xx(:,3))
subplot(2,1,2),stairs(xx(:,1),xx(:,4))
figure
[xx1,yy1]=stairs(xx(1:20/h,1),xx(1:20/h,5));
[xx1,yy2]=stairs(xx(1:20/h,1),xx(1:20/h,6));
[xx1,yy3]=stairs(xx(1:20/h,1),xx(1:20/h,7));
[xx1,yy4]=stairs(xx(1:20/h,1),xx(1:20/h,8));
subplot(1,1,1),plot(xx1,yy1,xx1,yy2,xx1,yy3,xx1,yy4);
