% example 4.4, Aström_Adaptive Control, pag. 146
% stochastic indirect self-tuning regulator
% based on the system y(t+1)+ay(t)=bu(t)+e(t+1)+ce(t) 
% using extended least squares (els) estimation and 
% minimum variance controller    
% elaboro E.Quintero.M 3jul'98
%----------------------------------------------------
clear
% parametros
time=0; tf=500;
lambda=1; % forgetting factor
h=1;    % sampling period
th=[0 1 0]';   % initial estimates
p=[100 0 0;0 100 0;0 0 100];   % initial covariances
A=-0.9; B=3; C=-0.3;
f=[0 0 0]'; u=0; s0=0; V=0;
% initial 
randn('seed',2); 
i=1; e=randn(tf/h+1,1);
while(time<tf)
%--------------------------------------------------------     
      y=A*f(1)+B*f(2)+C*f(3)+e(i);
      i=i+1;
      if(time==0) xx=[time y u th' s0 V]; 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 u eps]';

%     3.0 control law with anti-windup
      u=y*(th(1)-th(3))/th(2);

%     4.0 update sampling time
      time=time+h;
      s0=-(th(1)-th(3))/th(2);
      V=V+eps^2;
      xx=[xx; time y u th' s0 V];
end
format short e
time,a=th(1),b=th(2),c=th(3),s0,V
figure
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),xx(:,1),xx(:,5),xx(:,1),xx(:,6),...
[0 tf],[A A],'g--',[0 tf],[B B],'g--',[0 tf],[C C],'g--')
S0=-(A-C)/B;
subplot(2,1,2),plot(xx(:,1),xx(:,7),[0 tf],[S0 S0],'g--')
figure
subplot(1,1,1),plot(xx(:,1),xx(:,8))