% example 11.2, Aström_Adaptive Control, pag. 457
% "windup and how to avoid it"
% 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 not canceled
% elaboro E.Quintero.M 26jun'01
%-------------------------------------------------
clear
% parametros
time=0; tf=100;
lambda=1; % forgetting factor
To=200;   % observer time constant
z=0.7;    % desired closed loop damping
w=1;      % desired closed loop natural frecuency
h=0.5;      % sampling period
ulim=0.5;   % limit of contro 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; v1=0; ysp1=0; ysp=1; u=0; v=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;
if(w*To>100) aop=0; else aop=-exp(-h/To); end
ao=-aop; 
ao=0;
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*u+B1*u1;  %f(3) por u, f(4) por u1
      if(time==0) xx=[time ysp y v th']; 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;

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

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

%     2.2 solve the polynomial identity AR+BS=AoAm
      n=b1*b1-a1*b0*b1+a2*b0*b0;
      r1=(ao*am2*b0^2+(a2-am2-ao*am1)*b0*b1+(ao+am1-a1)*b1^2)/n;
      w1=(a2*am1+a2*ao-a1*a2-am2*ao)*b0;
      s0=(w1+(-a1*am1-a1*ao-a2+a1^2+am2+am1*ao)*b1)/n;
      w2=(-a1*am2*ao+a2*am2+a2*am1*ao-a2^2)*b0;
      s1=(w2+(-a2*am1-a2*ao+a1*a2+am2*ao)*b1)/n;

%     2.3 compute polynomial T=Ao*Am(1)/B(1)
      bs=b0+b1;
      as=1+am1+am2;
      bm0=as/bs;
      t0=bm0; 
      t1=bm0*ao; 
      u1=u;
      
%     3.0 control law without anti-windup
      v=-ao*v1+t0*ysp+t1*ysp1-s0*y-s1*y1+(ao-r1)*u1;
      if(v<-ulim) u=-ulim; elseif(v<ulim) u=v; else u=ulim; end

%     3.1 update controller state
      y1=y;
      
      v1=v;
      ysp1=ysp;

%     4.0 update sampling time
      time=time+h;
      xx=[xx; time ysp y v th'];
end
format short e
time,a1,a2,b0,b1,r1,t0,s0,s1
subplot(2,1,1),plot(xx(:,1),xx(:,2),xx(:,1),xx(:,3))
subplot(2,1,2),stairs(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),xx(:,7),xx(:,1),xx(:,8))
