clear variables
%
%  Program to model RLS for plant identification
%
%  determin the time vector
%
  Tf = 6;       % determine the final time
  Ts = 0.05;        % determine the sample interval
  t = [0:Ts:Tf];   % get a time vector
  N = length(t);
%
% enter the forgetting factor
%
  lambda = 0.5;
%
%  determine the input (reference) signal
%
  u = randn(1,N);
%  u = ones(1,N);
%  u = sin(2*pi*t);
%  uu = ones(1,N);
%  u = uu.*(t<1)+ uu.*((t>2)&(t<3))+uu.*(t>4);
% 
% prepare the reference signal and the corresponding time for Simulink
%
  ut = [t' u']; 
%
%  determine the (initial) transfer function for the plant
%
  Gp_z = tf([0.5 1.0],[1 0.5 0.1],Ts) 
  [Bp, Ap] = tfdata(Gp_z,'v');
  Nbp = length(Bp);
%
%  Now simulate the open loop plant and system identification 
%
%  initialize parameters
%
  y = zeros(1,N);
%
  phi = [0; 0; 0; 0; 0 ];
  P_last = eye(2*Nbp-1);
  theta_last = zeros(2*Nbp-1,1);
  out = zeros(2*Nbp-1,N); 
%
%  compute for the first time step
%
  y(1) = Bp(1)*u(1);
%  
  temp=[0; phi(1); u(1); 0; 0];
  phi = temp;
  K=P_last*phi/(lambda+phi'*P_last*phi);
  P=(eye(2*Nbp-1)-K*phi')*P_last/lambda;
  theta = theta_last + K*(y(1)-phi'*theta_last);
  P_last = P;
  theta_last = theta;
  out(:,1) = theta;
%
%  Now compute for the remaining times
%
  for n=2:N
    %
    % get the current time
    %
      tc = n*Ts;
    %
    %  modify the plant
    %
     if ((tc > 2.0) & (tc < 4.0))
       Bp = [0 2 1];
       Ap = [1 -0.2 0.3];
     elseif (tc >= 4.0)
       Bp = [0 1 -1];
       Ap = [1 0 -0.3];
     end;
    % Bp = [0 0.5 cos(2*pi*tc)];
    % Ap = [1 0.5*cos(pi*tc) 0.1]
    %
    %  determine the plant output
    %
    y(n) = Bp(1)*u(n);
    for k=2:min(n,Nbp)
      y(n) = y(n)-Ap(k)*y(n-k+1)+Bp(k)*u(n-k+1);
    end;
    %
    %  do the recursive least squares
    %
    if (n == 2) 
      temp=[y(1); phi(1); u(2); u(1); 0];
    else
      temp=[y(n-1); phi(1); u(n); u(n-1); u(n-2)];
    end;
%    
    phi = temp;
%
%  Now update the estimate of the parameters
%
    K=P_last*phi/(lambda+phi'*P_last*phi);
    P=(eye(2*Nbp-1)-K*phi')*P_last/lambda;
    theta = theta_last + K*(y(n)-phi'*theta_last);
%
%  now update everything
%
    P_last = P;
    theta_last = theta;
    out(:,n) = theta;
%
%  rest the covariance matrix if necessary
%
%  if (mod(n,10)==1) P_last = eye(size(P)); end;
%
  end;  
%
  sim('plant_identification');
%
% plot the results
%
  figure;
  plot(t,y); grid;
  plot(ts,ys,'o',t,y,'x'); grid; 
  legend('Simulink ','Matlab');
%  
  figure;
 
  subplot(4,1,1); plot(t,-out(1,:),'b-'); grid; ylabel('a_1');
   title(['\lambda = ',num2str(lambda)]);
  subplot(4,1,2); plot(t,-out(2,:),'b-'); grid; ylabel('a_2');
  subplot(4,1,3); plot(t,out(4,:),'b-'); grid; ylabel('b_1');
  subplot(4,1,4); plot(t,out(5,:),'b-'); grid; ylabel('b_2');

%
% extract the parameters and plot them
%
%   for i = 1:N
%       theta1(i) = theta_est(1,1,i);
%       theta2(i) = theta_est(2,1,i);
%       theta4(i) = theta_est(4,1,i);
%       theta5(i) = theta_est(5,1,i);
%   end;
%   figure;
%   subplot(4,1,1); plot(ts,-theta1,'r-',t,-out(1,:),'b:'); grid; ylabel('a_1');
%   subplot(4,1,2); plot(ts,-theta2,'r-',t,-out(2,:),'b:'); grid; ylabel('a_2');
%   subplot(4,1,3); plot(ts,theta4,'r-',t,out(4,:),'b:'); grid; ylabel('b_1');
%   subplot(4,1,4); plot(ts,theta5,'r-',t,out(5,:),'b:'); grid; ylabel('b_2');