%
%  This funtion implements the recursive
%  least squares algorithm (with a forgetting
%  factor) for determining the coefficients of
%  a transfer function which is assumed to change
%  slowly over time.
%
function adapt(Nsteps,lambda);
%
%  determine the input signal. In a real
%  system the input would change as the 
%  system model changed.
%
  t = [1:Nsteps];
  u = randn(1,Nsteps);
%  u = 2*cos(2*pi*t/100);
%  sigma = 0.1;
%  u = 2*cos(2*pi*t/100)+sigma*randn(1,Nsteps);
  coef = zeros(4,1);
%
%  loop through the time steps and estimate the
%  plant dynamics at each time step.
%  We will assume the plant is given by
%
%  y(k) = a1y(k-1)+a2y(k-2)+b0u(k)+b1u(k-1)
%
%  to initialize, we will assume all previous y 
%    values are 0.0 and the error matrix is diagonal
%
  phi = [0; 0; u(1); 0 ];
  y = 0;
  P_last = eye(4);
  theta_last = zeros(4,1);
  out = zeros(4,Nsteps);
  out_y = [];
  values = [0;0;0;0];
%
  for i=2:Nsteps
%
%  update the phi vector
%
    temp=[y; phi(1); u(i); phi(3)];
    phi = temp;
%
%  get the new output at this time
%
  if ( i < 200) coef = [0.25; 0.125; 2; 1]; end;
  if ( i >= 200) coef = [1.5; -0.5626; 3; -1]; end;
%
%    if ( i < 200) coef = [0.25; 0.125; 2; 1]; end;
%    if ((i >= 200) & (i < 400)) coef = [1.5; -0.5625; 3; -1]; end;
%    if (i >= 400) coef = [0; 0.25; 0; 3]; end;
%
%        coef(1) = 0.4+0.5*cos(3*pi*i/200);
%        coef(2) = -0.5+0.3*sin(2*pi*i/200);
%        coef(3) = 0.3;
%        coef(4) = 0.4;
%        values = [values coef];
% 
     y = phi'*coef; 
     out_y = [out_y y];
%
%  Now update the estimate of the parameters
%
    K=P_last*phi*inv(lambda+phi'*P_last*phi);
    P=(eye(4)-K*phi')*P_last/lambda;
    theta = theta_last + K*(y-phi'*theta_last);
%
%  now update everything
%
    P_last = P;
    theta_last = theta;
    out(:,i) = theta;
%
%  Restart to be sure the covariance matrix does not become too close to
%  singular
%
%   if (rem(i,20) == 0 ) P_last=100.0*eye(4); end;
%
  end;
%
%  now make the output plots
%
  [m,n] = size(out);
  k = [0:n-1];
  figure
  hold on
  title(['Part l, \lambda = ', num2str(lambda)]);
  plot(k,out(1,:),'-','Linewidth',3);
  plot(k,out(2,:),'-','Linewidth',0.5);
  plot(k,out(3,:),':','Linewidth',3)
  plot(k,out(4,:),':','Linewidth',0.5); grid;
  legend('a_1','a_2','b_0','b_1');
%    plot(k,values(1,:),'-','Linewidth',1);
%    plot(k,values(2,:),'-','Linewidth',1);
%    plot(k,values(3,:),':','Linewidth',1)
%    plot(k,values(4,:),':','Linewidth',1);  
  hold off;
  
  return;
%
%
  
