%
%  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 adaptive_least_squares(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);
%
%  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 = [];
%
  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;
%
     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);
%
%  Restart if the covariance matrix is nearly singular
%
    DD = svd(P);
    if (DD(4) < 1e-8) DD(4),K,phi,  end;
%
%  now update everything
%
    P_last = P;
    theta_last = theta;
    out(:,i) = theta;
%
%===================================================
%
%  Do not change the following line until you're told to
%
%    if (rem(i,1000) == 0 ) P_last=100.0*eye(4); end;
%
%===================================================
%
  end;
%
%  now make the output plots
%
  figure;
  orient landscape
  subplot(4,1,1); plot(out(1,:)); grid; ylabel('\theta_1');
  title(['Part k,  \lambda = ' num2str(lambda)]);
  subplot(4,1,2); plot(out(2,:)); grid; ylabel('\theta_2');
  subplot(4,1,3); plot(out(3,:)); grid; ylabel('\theta_3');
  subplot(4,1,4); plot(out(4,:)); grid; ylabel('\theta_4'); xlabel('Sample');
%
%=======================================================================
%  you will need to uncomment the following for part (l)
%
%   figure;
%   orient landscape
%   for i = 2:Nsteps
%     coef1(i) = 0.4+0.5*cos(3*pi*i/200);
%     coef2(i) = -0.5+0.3*sin(2*pi*i/200);
%     coef3(i) = 0.3;
%     coef4(i) = 0.4;
%   end;
%   s = [1:length(coef1)];
%   subplot(4,1,1); plot(s,out(1,:),'-',s,coef1,'--'); grid; ylabel('\theta_1');
%   title(['Part l \lambda = ' num2str(lambda)]);
%   subplot(4,1,2); plot(s,out(2,:),'-',s,coef2,'--'); grid; ylabel('\theta_2');
%   subplot(4,1,3); plot(s,out(3,:),'-',s,coef3,'--'); grid; ylabel('\theta_3');
%   subplot(4,1,4); plot(s,out(4,:),'-',s,coef4,'--'); grid; ylabel('\theta_4'); xlabel('Sample');
%
%=========================================================
%
  figure; 
  plot(out_y'); grid; xlabel('Sample'); ylabel('Output y(k)');
  return;
%
%
  
