% example 11.5, Aström_Adaptive Control, pag. 469
% "Tracking parameters of a time-varying system"
% based on the model Am(q)=q+am=q-exp(-h/Tm)
% Ao(q)=q+ao=q-exp(-h/To)
% using standard rls estimation and 
% pole placement design
% elaboro E.Quintero.M 12jul'99
%-------------------------------------------------
clear
% parametros
time=0; tf=1000; sigma=0.02;
lambda=0.95; % forgetting factor
To=1;   % observer time constant
Tm=1;
h=0.5;      % sampling period
ulim=20;   % limit of control signal
th=1;   % initial estimates
p=100;   % initial covariances
A=-exp(-1*h); a=A;
f=1;
y1=1; u1=0; v1=0; ysp1=0; ysp=1; u=0; twave=25;
% initial
% compute sampled Am and Ao
am=-exp(-h/Tm);
ao=-exp(-h/To);
% initial 
randn('seed',1); 
i=1; e=sigma*randn(tf/h+1,1);
while(time<tf)
%--------------------------------------------------------
      if(abs(time-twave)<h/2)
         if(ysp==1) ysp=-1; else ysp=1; end
         twave=twave+25;
      end
      B=0.9*sin(time*2*pi/400)+1;
      y=A*(-y1)+B*f+e(i);
      i=i+1;
      if(time==0) xx=[time ysp y u th B B-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-A*(-y1)-f'*th;
      th=th+k*eps;
      p=(p-pf*k')/lambda;

%     2.0 control design
%     2.1 rename parameters
      b=th;

%     2.2 solve the polynomial identity AR+BS=AoAm
      s0=(1+ao+am-a)/b;
      s1=(ao*am+a)/b;

%     2.3 compute polynomial T=Ao*Am(1)/B(1)
      t0=(1+am)/b; 
      t1=ao*t0;

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

%     1.3 update and filter regression vector
      f=u;

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

%     4.0 update sampling time
      time=time+h;
      xx=[xx; time ysp y u th B B-th];
end
format short e
figure
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))