--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/classes/+utils/@math/csd2tf2.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,480 @@
+% CSD2TF Input cross spectral density matrix and output stable transfer function 
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% DESCRIPTION:
+%
+%     Input cross spectral density (csd) and output corresponding
+%     stable functions. Identification can be performed for a simple system
+%     (one psd) or for a N dimensional system. Discrete transfer functions are
+%     output in partial fraction expansion:
+%
+%                r1                  rN
+%     f(z) = ----------- + ... + ----------- + d
+%            1-p1*z^{-1}         1-pN*z^{-1}
+%
+%     System identification is performed in frequency domain, the order of
+%     the model function is automatically chosen by the algorithm on the
+%     base of the input tolerance condition.
+%     In the case of simple systems the square root of the psd is fitted
+%     and then the model is stabilized by the application of an all-pass
+%     function.
+%     In the case of two dimensional systems, transfer functions frequency
+%     response is calculated by the eigendecomposition of the
+%     cross-spectral matrix. Then models are identified by fitting
+%     in frequency domain.
+%
+% CALL:
+%           out = csd2tf(csd,f,params)
+%
+% INPUT:
+%
+%     - csd is the cross spectral density matrix. It is in general a
+%     [n,n,m] dimensional matrix. Where n is the dimension of the system
+%     and m is the number of frequencies
+%     - f: is the corresponding frequencies vector in Hz (of length m)
+%     - params: is a struct of identification options, the possible values
+%     are:
+%
+%       params.TargetDomain = 'z' --> Perform z domain identification.
+%       Function output are residues and poles of a discrete system.
+%       params.TargetDomain = 's' --> Perform s domain identification.
+%       Function output are residues and poles of a continuous system.
+% 
+%       params.fullauto = 0 --> Perform a fitting loop as far as the number
+%       of iteration reach Nmaxiter. The order of the fitting function will
+%       be that specified in params.minorder. If params.dterm is setted to
+%       1 the function will fit only with direct term.
+%       params.fullauto = 1 --> Parform a full automatic search for the
+%       transfer function order. The fitting procedure will stop when the
+%       stopping condition defined in params.ctp is satisfied. Default
+%       value.
+%
+%       - params.Nmaxiter = # set the maximum number of fitting steps
+%       performed for each trial function order. Default is 50
+%
+%       - params.minorder = # set the minimum possible function order.
+%       Default is 2
+%
+%       - params.maxorder = # set the maximum possible function order.
+%       Default is 25
+%
+%       params.spolesopt = 1 --> use real starting poles
+%       params.spolesopt = 2 --> generates complex conjugates poles of the
+%       type \alfa e^{j\pi\theta} with \theta = linspace(0,pi,N/2+1).
+%       params.spolesopt = 3 --> generates complex conjugates poles of the
+%       type \alfa e^{j\pi\theta} with \theta = linspace(0,pi,N/2+2).
+%       Default option.
+%
+%       - params.weightparam = 0 --> use external weights
+%       - params.weightparam = 1 equal weights (one) for each point
+%       - params.weightparam = 2 weight with the inverse of absolute value
+%       of fitting data
+%       - params.weightparam = 3 weight with square root of the inverse of
+%       absolute value of fitting data
+%       - params.weightparam = 4 weight with the inverse of the square mean
+%       spread
+%
+%       params.extweights = [] --> A vector of externally provided weights.
+%       It has to be of the same size of input data.
+%
+%       - params.plot = 0 --> no plot during fit iteration
+%       - params.plot = 1 --> plot results at each fitting steps. default
+%       value.
+%
+%       - params.ctp = 'chival' --> check if the value of the Mean Squared
+%       Error is lower than 10^(-1*lsrcond).
+%       - params.ctp = 'chivar' --> check if the value of the Mean Squared
+%       Error is lower than 10^(-1*lsrcond) and if the relative variation of mean
+%       squared error is lower than 10^(-1*msevar).
+%       - params.ctp = 'lrs' --> check if the log difference between data and
+%       residuals is point by point larger than the value indicated in
+%       lsrcond. This mean that residuals are lsrcond order of magnitudes
+%       lower than data.
+%       - params.ctp = 'lrsmse' --> check if the log difference between data
+%       and residuals is larger than the value indicated in lsrcond and if
+%       the relative variation of mean squared error is lower than
+%       10^(-1*msevar).
+%
+%       - params.lrscond = # --> set conditioning value for point to point
+%       log residuals difference (params.ctp = 'lsr') and mean log residual
+%       difference (params.ctp = 'mlsrvar'). Default is 2. See help for
+%       stopfit.m for further remarks.
+%
+%       - params.msevar = # --> set conditioning value for root mean squared
+%       error variation. This allow to check that the relative variation of
+%       mean squared error is lower than 10^(-1*msevar).Default is 7. See
+%       help for stopfit.m for further remarks.
+%
+%       - params.fs set the sampling frequency (Hz). Default is 1 Hz
+%
+%       params.dterm = 0 --> Try to fit without direct term
+%       params.dterm = 1 --> Try to fit with and without direct term
+%
+%       params.spy = 0 --> Do not display the iteration progression
+%       params.spy = 1 --> Display the iteration progression
+% 
+%       params.usesym = 0 --> perform double-precision calculation for
+%       poles stabilization
+%       params.usesym = 1 --> perform symbolic math toolbox calculation for
+%       poles stabilization
+%
+%
+% OUTPUT:
+% 
+%     - ostruct is a struct with the fields:
+%       - ostruct(n).res --> is the vector of residues.
+%       - ostruct(n).poles --> is the vector of poles.
+%       - ostruct(n).dterm --> is the vector of direct terms.
+%       - ostruct(n).mresp --> is the vector of tfs models responses.
+%       - ostruct(n).rdl --> is the vector of fit residuals.
+%       - ostruct(n).mse --> is the vector of mean squared errors.
+%
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+% VERSION: $Id: csd2tf2.m,v 1.1 2009/10/16 17:16:54 luigi Exp $
+%
+% HISTORY:     22-04-2009 L Ferraioli
+%                 Creation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function ostruct = csd2tf2(csd,f,params)
+  
+  utils.helper.msg(utils.const.msg.MNAME, 'running %s/%s', mfilename('class'), mfilename);
+  
+  % Collect inputs
+  
+  % Default input struct
+  defaultparams = struct('TargetDomain','z','Nmaxiter',50, 'minorder',2,...
+    'maxorder',25, 'spolesopt',2, 'weightparam',1, 'plot',0,...
+    'ctp','chival','lrscond',2,'msevar',2,...
+    'fs',1,'dterm',0, 'spy',0, 'fullauto',1,...
+    'extweights', [],'usesym',1);
+  
+  names = {'TargetDomain','Nmaxiter','minorder','maxorder','spolesopt',...
+    'weightparam','plot','stopfitcond',...
+    'ctp','lrscond','msevar',...
+    'fs','dterm','spy','fullauto','extweights',...
+    'usesym'};
+  
+  % collecting input and default params
+  if ~isempty(params)
+    for jj=1:length(names)
+      if isfield(params, names(jj)) && ~isempty(params.(names{1,jj}))
+        defaultparams.(names{1,jj}) = params.(names{1,jj});
+      end
+    end
+  end
+  
+  % default values for input variables
+  target = defaultparams.TargetDomain; % target domain for system identification, can be 'z' or 's'
+  Nmaxiter = defaultparams.Nmaxiter; % Number of max iteration in the fitting loop
+  minorder = defaultparams.minorder; % Minimum model order
+  maxorder = defaultparams.maxorder; % Maximum model order
+  spolesopt = defaultparams.spolesopt; % 0, Fit with no complex starting poles (complex poles can be found as fit output). 1 fit with comples starting poles
+  weightparam = defaultparams.weightparam; % Weight 1./abs(y). Admitted values are 0, 1, 2, 3
+  checking = defaultparams.plot; % Never polt. Admitted values are 0 (No polt ever), 1 (plot at the end), 2 (plot at each step)
+  ctp = defaultparams.ctp;
+  lrscond = defaultparams.lrscond;
+  msevar = defaultparams.msevar;
+  fs = defaultparams.fs; % sampling frequency
+  idt = defaultparams.dterm;
+  spy = defaultparams.spy;
+  autosearch = defaultparams.fullauto;
+  extweights = defaultparams.extweights;
+  usesym = defaultparams.usesym; % method of calculation for the all pass filter
+  
+  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  % Checking inputs
+  
+  [a,b] = size(f);
+  if a < b % shifting to column
+    f = f.';
+  end
+  
+  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  % switching between inputs
+  
+  clear dim
+  % cecking for dimensionality
+  [nn,mm,kk] = size(csd);
+  if kk == 1
+    dim = '1dim';
+    utils.helper.msg(utils.const.msg.PROC1, ' Performing one dimesional identification on psd ')
+    if nn < mm % shift to column
+      csd = csd.';
+    end
+  else
+    dim ='ndim';
+    utils.helper.msg(utils.const.msg.PROC1, ' Performing N dimesional identification on csd ')
+  end
+  
+  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  % system identification
+  
+  switch dim
+    case '1dim'
+      
+      utils.helper.msg(utils.const.msg.PROC1, ' Performing z-domain identification ')
+      itf = abs(sqrt(csd)); % input data
+      
+      switch target % switch between z-domain and s-domain
+        case 'z'
+          % Fitting params
+          params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+            'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+            'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+            'stabfit',0,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+          % Fitting
+          utils.helper.msg(utils.const.msg.PROC1, ' Fitting absolute TF value with unstable model ')
+          [res,poles,dterm,mresp,rdl,msei] = utils.math.autodfit(itf,f,fs,params);
+
+          if usesym
+            % all pass filtering for poles stabilization
+            allpoles.poles = poles;
+            ntf = utils.math.pfallpsymz2(allpoles,mresp,f,fs);
+          else
+            % all pass filtering for poles stabilization
+            ntf = utils.math.pfallpz2(poles,mresp,f,fs);
+          end
+
+          % Fitting params
+          params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+            'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+            'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+            'stabfit',1,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+          utils.helper.msg(utils.const.msg.PROC1, ' Fitting TF with stable model ')
+          [res,poles,dterm,mresp,rdl,msef] = utils.math.autodfit(ntf,f,fs,params);
+
+          % Output data switching between output type
+          utils.helper.msg(utils.const.msg.PROC1, ' Output z-domain model ')
+          
+        case 's'
+           % Fitting params
+          params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+            'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+            'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+            'stabfit',0,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+          % Fitting
+          utils.helper.msg(utils.const.msg.PROC1, ' Fitting absolute TF value with unstable model ')
+          [res,poles,dterm,mresp,rdl,msei] = utils.math.autocfit(itf,f,params);
+
+          if usesym
+            % all pass filtering for poles stabilization
+            allpoles.poles = poles;
+            ntf = utils.math.pfallpsyms2(allpoles,mresp,f);
+          else
+            % all pass filtering for poles stabilization
+            ntf = utils.math.pfallps2(poles,mresp,f);
+          end
+
+          % Fitting params
+          params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+            'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+            'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+            'stabfit',1,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+          utils.helper.msg(utils.const.msg.PROC1, ' Fitting TF with stable model ')
+          [res,poles,dterm,mresp,rdl,msef] = utils.math.autocfit(ntf,f,params);
+
+          % Output data switching between output type
+          utils.helper.msg(utils.const.msg.PROC1, ' Output s-domain model ')
+
+      end
+      
+      ostruct = struct();
+      
+      ostruct.res = res;
+      ostruct.poles = poles;
+      ostruct.dterm = dterm;
+      ostruct.mresp = mresp;
+      ostruct.rdl = rdl;
+      ostruct.mse = msei;
+      
+    case 'ndim'
+      % switching between continuous and discrete type identification
+      
+%       utils.helper.msg(utils.const.msg.PROC1, ' Performing z-domain identification on 2dim system, z-domain output ')
+      tf = utils.math.ndeigcsd(csd,'OTP','TF','MTD','PAP'); % input data
+      
+      [nn,mm,kk] = size(tf);
+      
+%       % Shifting to columns
+%       [a,b] = size(tf11);
+%       if a<b
+%         tf11 = tf11.';
+%       end
+%       [a,b] = size(tf12);
+%       if a<b
+%         tf12 = tf12.';
+%       end
+%       [a,b] = size(tf21);
+%       if a<b
+%         tf21 = tf21.';
+%       end
+%       [a,b] = size(tf22);
+%       if a<b
+%         tf22 = tf22.';
+%       end
+%       
+%       % Collecting tfs
+%       f1 = [tf11 tf21];
+%       f2 = [tf12 tf22];
+      
+      %%% System identification
+      
+      % init output
+      ostruct = struct();
+      idx = 1;
+      
+      for dd = 1:mm
+        fun = squeeze(tf(1,dd,:));
+        % willing to work with columns
+        [a,b] = size(fun);
+        if a<b
+          fun = fun.';
+        end
+        for ff = 2:nn
+          tfun = squeeze(tf(ff,dd,:));
+          % willing to work with columns
+          [a,b] = size(tfun);
+          if a<b
+            tfun = tfun.';
+          end
+          fun = [fun tfun];
+        end
+        
+        switch target % switch between z-domain and s-domain
+          case 'z'
+            for pp = 1:size(fun,2)
+
+              % Fitting with unstable poles %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+              params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+                'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+                'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+                'stabfit',0,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+              % Fitting
+              utils.helper.msg(utils.const.msg.PROC1, ' Fitting with unstable common poles ')
+              [res,poles,dterm,tmresp,trdl,tmsei] = utils.math.autodfit(fun(:,pp),f,fs,params);
+
+              allpoles(pp).poles = poles;
+              mresp(:,pp) = tmresp;
+              rdl(:,pp) = trdl;
+              msei(:,pp) = tmsei;
+
+            end
+
+            if usesym
+              % Poles stabilization %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+              utils.helper.msg(utils.const.msg.PROC1, ' All pass filtering')
+              nfun = utils.math.pfallpsymz2(allpoles,mresp,f,fs);
+            else
+              % Poles stabilization %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+              utils.helper.msg(utils.const.msg.PROC1, ' All pass filtering')
+              nfun = utils.math.pfallpz2(allpoles,mresp,f,fs);
+            end
+
+            clear poles
+
+            % Fitting with stable poles %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+            for zz = 1:size(fun,2)
+
+              % Fitting params
+              params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+                'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+                'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+                'stabfit',1,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+              % Fitting
+              utils.helper.msg(utils.const.msg.PROC1, ' Fitting with stable common poles ')
+              [res,poles,dterm,tmresp,trdl,tmsef] = utils.math.autodfit(nfun(:,zz),f,fs,params);
+
+
+              % Output stable model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+              ostruct(idx).res = res;
+              ostruct(idx).poles = poles;
+              ostruct(idx).dterm = dterm;
+              ostruct(idx).mresp = mresp(:,zz);
+              ostruct(idx).rdl = rdl(:,zz);
+              ostruct(idx).mse = msei(:,zz);
+
+              idx = idx + 1;
+
+            end
+            
+          case 's'
+            for pp = 1:size(fun,2)
+
+              % Fitting with unstable poles %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+              params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+                'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+                'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+                'stabfit',0,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+              % Fitting
+              utils.helper.msg(utils.const.msg.PROC1, ' Fitting with unstable common poles ')
+              [res,poles,dterm,tmresp,trdl,tmsei] = utils.math.autocfit(fun(:,pp),f,params);
+
+              allpoles(pp).poles = poles;
+              mresp(:,pp) = tmresp;
+              rdl(:,pp) = trdl;
+              msei(:,pp) = tmsei;
+
+            end
+
+            if usesym
+              % Poles stabilization %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+              utils.helper.msg(utils.const.msg.PROC1, ' All pass filtering')
+              nfun = utils.math.pfallpsyms2(allpoles,mresp,f);
+            else
+              % Poles stabilization %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+              utils.helper.msg(utils.const.msg.PROC1, ' All pass filtering')
+              nfun = utils.math.pfallps2(allpoles,mresp,f);
+            end
+
+            clear poles
+
+            % Fitting with stable poles %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+            for zz = 1:size(fun,2)
+
+              % Fitting params
+              params = struct('spolesopt',spolesopt, 'Nmaxiter',Nmaxiter, 'minorder',minorder,...
+                'maxorder',maxorder, 'weightparam',weightparam, 'plot',checking,...
+                'ctp',ctp,'lrscond',lrscond,'msevar',msevar,...
+                'stabfit',1,'dterm',idt,'spy',spy,'fullauto',autosearch,'extweights',extweights);
+
+              % Fitting
+              utils.helper.msg(utils.const.msg.PROC1, ' Fitting with stable common poles ')
+              [res,poles,dterm,tmresp,trdl,tmsef] = utils.math.autocfit(nfun(:,zz),f,params);
+
+
+              % Output stable model %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+              ostruct(idx).res = res;
+              ostruct(idx).poles = poles;
+              ostruct(idx).dterm = dterm;
+              ostruct(idx).mresp = mresp(:,zz);
+              ostruct(idx).rdl = rdl(:,zz);
+              ostruct(idx).mse = msei(:,zz);
+
+              idx = idx + 1;
+
+            end
+        
+        end
+        
+      
+        
+        
+      end
+      
+  end
+end
+
+% END %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%