clear variables
clear all
%
%  Program to model RLS for plant identification
%
%  determin the time vector
%
  Ts = 0.01;        % determine the sample interval
  Tf = 600*Ts;       % determine the final time
  t = [0:Ts:Tf];   % get a time vector
  N = length(t);
%
% enter the forgetting factor
%
  lambda = 0.95;
%
%  determine the input (reference) signal
%
  r = randn(1,N);
%   r = ones(1,N);
%  r = sin(2*pi*t);
  uu = ones(1,N);
  r = uu.*(t<1)+ uu.*((t>2)&(t<3))+uu.*(t>4);
% 
% prepare the reference signal and the corresponding time for Simulink
%
  rt = [t' r'];
  dp = [0.02 0.015];
%
% determine the (initial) state model
%
  b0 = 01.0; b1 = 0.5; b2 = 1.0;
  a1 = 0.5; a2 = 0.1;
 %
  G = [0 1; -a2 -a1];
  H = [0; 1];
  C = [(b2-a2*b0) (b1-a1*b0)];
  D = b0;
  [n,m] = size(H);
%
%  initialize parameters
%
  y = zeros(1,N);
  x = zeros(n,N);  
  e = zeros(1,N);
  e = r;
%
%  initial state
%
  x(:,2) = G*x(:,1) + H*e(1);
  y(1) = C*x(:,1)+D*e(1);  
%
%  Now simulate the open loop plant and system identification 
%
  phi = [0; 0; 0; 0; 0 ];
  P_last = eye(2*n+1);
  theta_last = zeros(2*n+1,1);
  out = zeros(2*n+1,N); 
%  
  temp=[0; phi(1); e(1); 0; 0];
  phi = temp;
  K=P_last*phi/(lambda+phi'*P_last*phi);
  P=(eye(2*n+1)-K*phi')*P_last/lambda;
  theta = theta_last + K*(y(1)-phi'*theta_last);
  P_last = P;
  theta_last = theta;
  out(:,1) = theta;
%
%  get the initial estimate
%
  a1 = -theta(1);
  a2 = -theta(2);
  b0 = theta(3);
  b1 = theta(4);
  b2 = theta(5);
  G_est = [0 1; -a2 -a1];
  H_est = [0;1]; 
  C_est = [(b2-a2*b0) (b1-a1*b0)];
  D_est = b0;
  Kf = acker(G_est,H_est,dp);
  DD = (C_est-D_est*Kf)*pinv(eye(2,2)-G_est+H_est*Kf,0.001)*H_est+D_est;
  Gpf = 1/DD
  if (isnan(Gpf) ) Gpf = 1.0; end; 
  if (abs(Gpf) > 1.0 ) Gpf = sign(Gpf)*1.0; end;
%
%  Now compute for the remaining times
%
  for k=2:N
    %
    % get the current time
    %
      tc = (k-1)*Ts;
    %
     b0 = 1.0; b1 = 0.5; b2 = cos(2*pi*tc);
     a1 = 0.5*cos(pi*tc); a2 = 0.1;
    %
    %  modify the plant
    %
     if ((tc > 2.0) & (tc < 4.0))
        b1 = 2; b2 = 1;
        a1 = -0.2; a2=0.3;
     elseif (tc >= 4.0)
         b1 = 1; b2 = -1;
         a1 = 0; a2 = -0.3;
     end;
    %
    %  determine the new state model
    %
     G = [0 1; -a2 -a1];
     H = [0; 1];
     C = [(b2-a2*b0) (b1-a1*b0)];
     D = b0;
    %
    %  determine the system output;
    %
     e(k) = Gpf*e(k)-Kf*x(:,k-1);
     x(:,k+1) = G*x(:,k)+H*e(:,k);
     y(k) = C*x(:,k)+D*e(:,k);  
    %
    %  do the recursive least squares
    %
     if (k == 2) 
      temp=[y(1); phi(1); e(2); e(1); 0];
     else
      temp=[y(k-1); phi(1); e(k); e(k-1); e(k-2)];
     end;
    %    
     phi = temp;
    %
    %  Now update the estimate of the parameters
    %
     K=P_last*phi/(lambda+phi'*P_last*phi);
     P=(eye(2*n+1)-K*phi')*P_last/lambda;
     theta = theta_last + K*(y(k)-phi'*theta_last);
    %
    % now update everything
    %
     P_last = P;
     theta_last = theta;
     out(:,k) = theta;
    %
    % rest the covariance matrix if necessary
    %
    % if (svds(P_last,1,0.0) < 0.01) P_last = eye(5,5); end;
   %  if (mod(n,10)==1) P_last = eye(size(P)); end;
   % 
   %  update the feedback gain
   %
    a1 = -theta(1);
    a2 = -theta(2);
    b0 = theta(3);
    b1 = theta(4);
    b2 = theta(5);
    G_est = [0 1; -a2 -a1];
    H_est = [0;1]; 
    C_est = [(b2-a2*b0) (b1-a1*b0)];
    D_est = b0;
    Kf = acker(G_est,H_est,dp);
    DD = (C_est-D_est*Kf)*pinv(eye(2,2)-G_est+H_est*Kf,0.001)*H_est+D_est;
    Gpf = 1/DD;
    if (isnan(Gpf) ) Gpf = 1.0; end; 
    if (abs(Gpf) > 1.0 ) Gpf = sign(Gpf)*1.0; end;
    Gpf
  end;  
%
disp('+++++++++++++++++++++++++++++');
sim('projectE_state');
%
% plot the results
%
  figure;
%  plot(t,y); grid;
%  plot(ts,ys,'o'); grid; axis([0 max(ts) -5 5]);
   plot(ts,ys,'o',t,y,'x'); grid; 
   xlabel('Time (sec)');
   ylabel('Output y(t)');
   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'); legend('Simulink','Matlab');
  subplot(4,1,2); plot(ts,-theta2,'r-',t,-out(2,:),'b:'); grid; ylabel('a_2');legend('Simulink','Matlab');
  subplot(4,1,3); plot(ts,theta4,'r-',t,out(4,:),'b:'); grid; ylabel('b_1');legend('Simulink','Matlab');
  subplot(4,1,4); plot(ts,theta5,'r-',t,out(5,:),'b:'); grid; ylabel('b_2');legend('Simulink','Matlab');
