% example 2.14, Aström_Adaptive Control, pag. 74
% "closed-loop estimation"
% based on the system y(t)+ay(t-1)=bu(t-1)+e(t)    
% elaboro E.Quintero.M 9jun'99
%------------------------------------------------------
clear
time=0; tf=1000; sigma=0.5;
lambda=1; % forgetting factor
h=1;    % sampling period
th=[0 0]';   % initial estimates
p=[100 0;0 100];   % initial covariances
A=-0.8; B=0.5; C=0;
f=[0 0]'; u=1; y1=0;
% initial 
randn('state',0); 
i=1; e=sigma*randn(tf/h+1,1);
while(time<tf)
%--------------------------------------------------------     
      y=A*f(1)+B*f(2)+e(i);
      i=i+1;
      if(time==0) xx=[time y u 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;

%     3.0 control law
      u=-0.2*y;
%      u=-0.32*y1;
      y1=y;

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

%     4.0 update sampling time
      time=time+h;
      xx=[xx; time y u th'];  
end
format short e
time,a=th(1),b=th(2),p
figure
subplot(2,1,1),plot(xx(:,1),xx(:,2))
subplot(2,1,2),plot(xx(:,1),xx(:,3))
figure
subplot(1,1,1),plot(xx(:,1),xx(:,4),xx(:,1),xx(:,5),...
[0 tf],[A A],'--',[0 tf],[B B],'--')

