% IDENT_NL  Nonlinear Identification
%   Identifies LTI-System G(z) and Feedback-Nonlinearity f(u,y), where
%
%             b_d*z^(-d) + ... + b_m*z^(-m)
%   G(z) = ----------------------------------; n >= m, 1 <= d <= m
%           1 + a_1*z^(-1) + ... + a_n*z^(-n)
% 
%   and
%             r       q
%   f(u,y) = Sum u^l Sum alpha_lm y^m; alpa_00 = 0 .
%            l=0     m=0
%
%   Call: [b, a, Alpha] = ident_nl(data, nn, nnl) where
%
%           data = [Output, Input];
%           nn   = [n, m, d];
%           nnl  = [q, r];
%
%           b     = [0_d, b_d,..., b_m, 0_(n-m)]^T;
%           a     = [1, a_1, ..., a_n]^T
%                   [a_00, a_01, a_02, ..., a_0q]
%                   [a_10, a_11,   ...,     a_1q]
%           Alpha = [:                       :  ]        
%                   [:                       :  ]
%                   [a_r0, a_r1,   ...,     a_rq]
%
%   The normalization b_d = 1 and alpha_01 = 0 is made.
%
%   Heiko Wolfram, TUC 2005.

% === Emacs Mode: -*- matlab -*- =============================================
% $Project: Publication at "7. Chemnitzer Fachtagung Mikrosystemtechnik" - 26-27.10.2005 $
% $Coordinator: TU Chemnitz <http://www.tu-chemnitz.de/etit/sse/mst> $ $
% $Source: /home/hwol/EigeneDateien/Projekte/All/Matlab/ident_nl.m,v $
% $Date: 2005/10/19 09:59:46 $
% $Revision: 0.3 $
% $State: Exp $
% $Author: hwol $
% $Copyright: Heiko Wolfram, TU Chemnitz, Dep. of Microsystems and Devices $
% $Email: heiko.wolfram@e-technik.tu-chemnitz.de, h_wol@gmx.de $
% $Software: Matlab $
% $SoftwareVersion: Version 5.3 $
% $Function: Identification of Feedback Model $
% $Note$
% $Parents$
% $Children$
%
% $Id: ident_nl.m,v 0.3 2005/10/19 09:59:46 hwol Exp $
% ============================================================================

function [b, a, Alpha, error, theta] = ident_nl(data, nn, nnl);
  
  % Test Inputs
  if nargin < 3,
    disp('USAGE: [b, a, Alpha, error, theta] = ident_nl(data, nn, nnl)');
    return;
  end;
  
  % Some Order Tests
  if (nn(3) < 1) | (nn(3) > nn(2)),
    disp('System must be proper (1 <= d <= m) !');
    return;
  end;
  
  if nn(2) > nn(1),
    disp('Order n must be greater or equal to Order m !');
    return;
  end;

  if nargout < 1,
    debug = 1;
  else;
    debug = 0;
  end;
  
  % Data has Column Form ?
  if max(size(data)) ~= size(data,1),
    data = data';
  end;
  
  % Call LSM 
  [theta, error] = LocalGetTheta(data, nn, nnl, debug);
  
  % Do Decompostion with SVD
  [alpha, b] = LocalParamDecomposition(theta(nn(1)+1:end), nn, nnl);

  % Add trailing Zeros and One
  a = [1; theta(1:nn(1) )];
  b = [ zeros(nn(3),1); b; zeros(nn(1) - nn(2) ,1) ];
  
  % Make Matrix from Vector
  Alpha = zeros(nnl(1)+1, nnl(2)+1);
  Alpha(:) = [0;0; alpha];
  
  Alpha = Alpha';
  
  if debug,
    
    yout = LocalSystemSolve(data, b, a, Alpha);
    LocalPlotData(data, yout);
    
    LocalFunctionPlot(Alpha, data);
    
    disp('Values:');
    
    filt(b(:)', a(:)'),
    Alpha,
    
  end;
  
%% End: function ident_nl

%
%=============================================================================
% LocalBuiltMu
% Built up Matrix Mu. Entries Mu(1,1:2) are unused for Vector mu.
%=============================================================================
%
function Mu = LocalBuiltMU(y, u, n_q, n_r),
  
  y_vec = zeros(n_q + 1, 1);
  u_vec = zeros(n_r + 1, 1);
  
  % Output Powers
  for h = 0:n_q,
    y_vec(h+1) = y^h;
  end;
  
  % Input Powers
  for h = 0:n_r,
    u_vec(h+1) = u^h;
  end;

  Mu = u_vec(:) * y_vec(:)';
  
%% End: function LocalBuiltMu

%
%=============================================================================
% LocalBuiltPhi
% Built up Vector Phi.
%=============================================================================
%
function phi = LocalBuiltPhi(k, data, nn, nnl),
  
  % Output Vector
  phi_y = flipud( data(k-nn(1):k-1, 1) );
  
  % Input-Output Vector
  Phi_uy = zeros(nn(2) - nn(3) + 1 ,(nnl(1) + 1)*(nnl(2) + 1) - 2);
  
  for h = nn(3):nn(2),
    Mu = LocalBuiltMU(data(k-h, 1), data(k-h, 2), nnl(1), nnl(2) )';
    Mu = Mu(:); % Vector Form

    Phi_uy(h-nn(3)+1, :) =  Mu(3:end)';
  end;

  phi = [-phi_y; Phi_uy(:)];


%% End: function LocalBuiltPhi

%
%=============================================================================
% LocalGetTheta
% Get Parameter Vector theta from Input-Output Measurements
%=============================================================================
%
function [phi, error] = LocalGetTheta(data, nn, nnl, debug),
  
  N = size(data,1);
  Phi_N = zeros(N - nn(1),...
		(nn(2) - nn(3) + 1)*((nnl(1) + 1)*(nnl(2) + 1) - 2) + nn(1));
  
  for h = nn(1)+1:N,
    LocalProgress(h,N,1);
    Phi_N(h-nn(1),:) = LocalBuiltPhi(h, data, nn, nnl)';
  end;
  
  Y_N = data(nn(1)+1:N,1);
  
  % Get Parameter Vector
  phi = pinv(Phi_N)*Y_N;
  
  % Calculate Error
  error = sum( (Phi_N*phi - Y_N).^2);
  
  if debug,
    figure; plot(1:(N-nn(1)), Y_N, 'r', 1:(N-nn(1)), Phi_N*phi,'b');
    set(gca, 'FontSize', 16);
    grid on; xlabel('No. of Samples -->', 'FontSize', 18);
    ylabel('Amplitude -->', 'FontSize', 18);
    title('Real and Estimated Output', 'FontSize', 20);
    legend('Real', 'Estimated', 1);
  
    ax = axis;
    text(ax(1) + 0.1*(ax(2) - ax(1)),...
	 ax(3) + 0.1*(ax(4) - ax(3)),...
	 ['Error: ', num2str(error,5) ],...
	 'FontSize', 16);
    
  end;

%% End: function LocalGetTheta 

%
%=============================================================================
% LocalParamDecomposition
% Decompose Parameters from Vector theta_ba with SVD.
%=============================================================================
%
function [alpha, beta] = LocalParamDecomposition(theta, nn, nnl),

Theta = zeros(nn(2) - nn(3) + 1, (nnl(1)+1)*(nnl(2)+1)-2);

Theta(:) = theta;

[U,S,V] = svd(Theta');
alpha = U(:,1)*S(1,1)*V(1,1);
beta  = 1/V(1,1)*V(:,1);

% end ParamDecomposition

%
%=============================================================================
% LocalSolveFunction
% Solve Polynom Function.
%=============================================================================
%
function [yout] = LocalSolveFunction(Alpha, y, u, n_q, n_r),

  Mu = LocalBuiltMU(y, u, n_q, n_r);

  yout = sum(sum( Alpha .* Mu ));

% end  LocalSolveFunction

%
%=============================================================================
% LocalSystemSolve
% Solve System.
%=============================================================================
%
function [yout] = LocalSystemSolve(data, b, a, Alpha),
  
  fprintf('\rSimulate...');
  
  % Make length equal
  b = [zeros(length(a) - length(b), 1); b(:)];
  
  % Is System proper?
  if b(1) ~= 0,
    error('System must be proper!');
  end;
  
  % Is System monic ?
  if a(1) ~= 1,
    b = b/a(1);
    a = a/a(a);
  end;

  % Remove trailing zeros
  while (a(end) == 0) & (b(end) == 0),
    a(end) = []; b(end) = [];
  end;

  % Get Order
  [alph_r, alph_c] = size(Alpha);
  n_a = length(a) - 1;
  
  N = size(data, 1);
  
  % In- and Output Vectors
  yout = zeros(N,1);
  u_vec = zeros(n_a,1);
  y_vec = zeros(n_a,1);
  
  % Run Simulation
  for k = 1:N,
    
    LocalProgress(k,N,1);  
    
    % Filter new Output
    yout(k) = sum( -a(2:end) .* y_vec + b(2:end) .* u_vec );
        
    % Shift Vectors
    u_vec(2:end) = u_vec(1:end-1);
    y_vec(2:end) = y_vec(1:end-1);
    
    % Compute new In- and Output
    u_vec(1) = LocalSolveFunction(Alpha, yout(k), data(k,2),...
				  alph_c - 1, alph_r - 1);
    y_vec(1) = yout(k);

  end;
  
% end LocalSystemSolve

%
%=============================================================================
% LocalFunctionPlot
% Plots Data.
%=============================================================================
%
function LocalFunctionPlot(Alpha, data);
    
  [alph_r, alph_c] = size(Alpha);
  
  % Get max. Vaules
  max_y = max(abs(data(:,1) ));
  max_u = max(abs(data(:,2) ));
  
  % Round Values
  max_y = ceil(max_y * 10^(-floor(log10(max_y)) + 1)) *10^(floor(log10(max_y)) - 1);
  max_u = ceil(max_u * 10^(-floor(log10(max_u)) + 1)) *10^(floor(log10(max_u)) - 1);
  
  y_vec = linspace(-max_y, max_y, 100);
  u_vec = linspace(-max_u, max_u, 3);
  
  yout = zeros(length(u_vec), length(y_vec));
  
  for h = 1:length(u_vec),
    for k = 1:length(y_vec),
      
      yout(h,k) = LocalSolveFunction(Alpha, y_vec(k), u_vec(h),...
				     alph_c - 1, alph_r - 1);
    end;
  end;
    
  figure; plot(y_vec, yout, 'b');
  set(gca, 'FontSize', 16);
  
  grid on; xlabel('Input -->', 'FontSize', 18);
  ylabel('Output -->', 'FontSize', 18);
  title('Approximated Function Plot', 'FontSize', 20);
  
% end LocalFunctionPlot

%
%=============================================================================
% LocalPlotData
% Plots Data.
%=============================================================================
%
function LocalPlotData(data, yout);
    
  figure; plot(1:size(data,1), data(:,1), 'r',...
	       1:size(yout,1), yout, 'b');
  set(gca, 'FontSize', 16);
  
  grid on; xlabel('No. of Samples -->', 'FontSize', 18);
  ylabel('Amplitude -->', 'FontSize', 18);
  title('Real and Simulated NL-Output', 'FontSize', 20);
  
  % Min-Max Values
  y_min = min(data(:,1));
  y_max = max(data(:,1));
  
  % Round Values
  y_max = ceil( y_max * 10^(-floor(log10(y_max)) + 1)) *...
	  10^(floor(log10(y_max)) - 1);
  y_min = floor( y_min * 10^(-floor(log10(y_min)) + 1)) *...
	  10^(floor(log10(y_min)) - 1);
  
  % Change Axis
  axis( [0, size(data,1), y_min, y_max] );
  
  legend('Real', 'Simulated', 1);

  ax = axis; text(ax(1) + 0.1*(ax(2) - ax(1)),...
		  ax(3) + 0.1*(ax(4) - ax(3)),...
		  ['Error: ', num2str(sum( (yout - data(:,1)).^2 ),5) ],...
		  'FontSize', 16);
  
% end LocalPlotData

%
%=============================================================================
% LocalProgress
% Prints Progress Status
%=============================================================================
%
function LocalProgress(count, cend, timeupdate),
  
  persistent TVEC
  
  lvec = clock; % Local time
  
  if count >= cend,
    fprintf('\r           \r');
    return;

  elseif isempty(TVEC),

    TVEC = clock;
    fprintf('\r%3i %%      ', floor(count/cend * 100) );
    return;

  elseif abs( TVEC(end) - lvec(end) ) > timeupdate,

    TVEC = clock;
    fprintf('\r%3i %%      ', floor(count/cend * 100) );
    return;

  end;
  
%% End: function LocalProgress


%% EOF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%