% example 3.4, Aström_Adaptive Control, pag. 104
% indirect sel-tuning regulator
% based on the model H(q)=(b0*q+b1)/(q^2+a1*q+a2)
% using standard rls estimation and 
% pole placement design    
% polynomial B is canceled 
% elaboro E.Quintero.M 7jun'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 0 0.01 0.2]';   % initial estimates
p=[100 0 0 0;0 100 0 0;0 0 1 0;0 0 0 1];   % initial covariances
A1=-1.6065; A2=0.6065; B0=0.1065; B1=0.0902;
f=[0 0 0 0]';
y1=0; u1=0; ysp=1; u=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;
while(time<tf)
%--------------------------------------------------------
      if(abs(time-twave)<h/2)
         if(ysp==1) ysp=-1; else ysp=1; end
         twave=twave+25;
      end
      y=A1*f(1)+A2*f(2)+B0*f(3)+B1*f(4);
      if(time==0) xx=[time ysp y u th']; end
%--------------------------------------------------------

%     sort
%     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;

%     1.3 update and filter regression vector
      f=[-y f(1) u f(3)]';

%     2.0 control design
%     2.1 rename parameters
      a1=th(1);
      a2=th(2);
      b0=th(3);
      b1=th(4);

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

%     2.2 solve the polynomial identity AR+BS=AoAm
      r1=b1/b0; 
      s0=(am1-a1)/b0;
      s1=(am2-a2)/b0; 
      t0=bm0/b0;

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

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

%     4.0 update sampling time
      time=time+h;
      xx=[xx; time ysp y u th'];
end
format short e
time,a1,a2,b0,b1,r1,t0,s0,s1,bm0
figure
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(2,1,1),plot(xx1,yy1,xx1,yy2);
subplot(2,1,2),plot(xx1,yy3,xx1,yy4);

