% example 4.5, Aström_Adaptive Control, pag. 154
% "Direct minimum-variance self-tuning regulator"
% based on the system y(t+1)=r0*u(t)+s0*y(t)+e(t+1) 
% using extended least squares (els) estimation and 
% minimum variance controller
% elaboro E.Quintero.M 9sep'98
%----------------------------------------------------
clear
% parametros
time=0; tf=500;
lambda=1; % forgetting factor
h=1;    % sampling period
th1=0;  % initial estimates: th1=s0
p11=1;   % initial covariances
A=0.9; B=3; r0=1;
f1=0; u=0; V=0;
% initial 
randn('seed',2); 
i=1; e=randn(tf/h+1,1);
while(time<tf)
%--------------------------------------------------------     
      y=B*u+A*f1+e(i);
      i=i+1;
      if(time==0) xx=[time y u th1 V]; end
%--------------------------------------------------------

%     sort
%     1.0 parameter estimation
%     1.1 computation of p*f and estimator gain k
      pf1=p11*f1;
      denom=lambda+f1*pf1;
      k1=pf1/denom;

%     1.2 update estimates and covariances
      eps=y-f1*th1-u*r0;
      th1=th1+k1*eps;
      p11=(p11-pf1*k1)/lambda;

%     1.3 update and filter regression vector
      f1=y;

%     3.0 control law with anti-windup
      u=y*(-th1);

%     4.0 update sampling time
      time=time+h;
      V=V+y^2;
      xx=[xx; time y u th1 V];
end
format short e
time,s0=th1,V
subplot(2,1,1),plot(xx(:,1),xx(:,2))
subplot(2,1,2),plot(xx(:,1),xx(:,3))
figure
subplot(2,1,1),plot(xx(:,1),xx(:,4))
subplot(2,1,2),plot(xx(:,1),xx(:,5))
