--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/classes/+utils/@math/pfallpsymz.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,216 @@
+% PFALLPSYMZ all pass filtering in order to stabilize TF poles and zeros.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% DESCRIPTION:
+% 
+%     All pass filtering in order to stabilize transfer functions poles. 
+%     It inputs a partial fraction expanded discrete model and outpu
+%     residues, poles direct terms and frequency response of the stabilized
+%     model. Function can handle multiple models with common poles.
+% 
+% CALL:
+% 
+%     [nr,np,nd,resp] = pfallpsymz(r,p,d,f,fs)
+% 
+% INPUTS:
+% 
+%     r: are residues. (Npx1) or (NpxM) vector
+%     p: are poles. (Npx1) vector
+%     d: is direct term (1x1) or (1xM) vector
+%     mresp: input model response. (Nx1) or (NxM) vector
+%     f: is the frequancies vector in (Hz). (Nx1) vector
+%     fs: is the sampling frequency in (Hz). (1x1)
+%     
+% OUTPUTS:
+% 
+%     nr: new residues. (Npx1) or (NpxM) vector
+%     np: new stable poles. (Npx1) vector
+%     nd: new direct term. (1x1) or (1xM) vector
+%     nmresp: new model response. (Nx1) or (NxM) vector
+% 
+% NOTE:
+% 
+%     This function make use of symbolic math toolbox functions
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% VERSION: $Id: pfallpsymz.m,v 1.5 2008/11/10 15:40:11 luigi Exp $
+%
+% HISTORY:     12-09-2008 L Ferraioli
+%                 Creation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function [nr,np,nd,nmresp] = pfallpsymz(r,p,d,mresp,f,fs)
+
+  % Reshaping
+  [a,b] = size(r);
+  if a<b
+    r = r.'; % reshape as a column vector
+  end
+
+  [a,b] = size(p);
+  if a<b
+    p = p.'; % reshape as a column vector
+  end
+
+  [a,b] = size(f);
+  if a<b
+    f = f.'; % reshape as a column vector
+  end
+
+  [a,b] = size(f);
+  if a<b
+    f = f.'; % reshape as a column vector
+  end
+
+  [a,b] = size(d);
+  if a > b
+    d = d.'; % reshape as a row
+    d = d(1,:); % taking the first row (the function can handle only simple constant direct terms)
+  end
+
+  if isempty(fs)
+    fs = 1;
+  end
+  [a,b] = size(fs);
+  if a ~= b
+    disp(' Fs has to be a number. Only first term will be considered! ')
+    fs = fs(1);
+  end
+
+  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  f = sym(f,'f');
+  fs = sym(fs,'f');
+  syms z
+  p = sym(p,'f');
+
+  % stabilize poles
+  sp = p;
+  unst = abs(double(sp)) > 1;
+  sp(unst) = 1./conj(sp(unst));
+  skp = prod(1./conj(p(unst)));
+
+  [Na,Nb] = size(r);
+
+  for nn = 1:Nb
+    
+%     pp = p(unst);
+%     psp = sp(unst);
+%     tterm = 1;
+%     for ii = 1:length(pp)
+%       tterm = tterm.*(z-pp(ii))./(z-psp(ii));
+%     end
+%     
+%     s = cos((2*pi/fs).*f) + 1i.*sin((2*pi/fs).*f);
+%     allresp = subs(tterm,z,s);
+%     phs = angle(allresp);
+%     nmresp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
+%     np = double(sp);
+
+    % digits(100)
+    % Defining inputs as symbolic variable
+    rt = sym(r(:,nn),'f');
+  %   p = sym(p,'f');
+    dt = sym(d(1,nn),'f');
+  %   f = sym(f,'f');
+  %   fs = sym(fs,'f');
+  %   syms z
+
+    % Function gain coefficient
+    k = sum(rt)+dt;
+
+    Np = length(p);
+
+    % Defining the symbolic transfer function
+    vter = rt.*z./(z-p);
+    vter = [vter; dt];
+    Mter = diag(vter);
+    H = trace(Mter);
+
+    % Factorizing the transfer function
+    HH = factor(H);
+
+    % Extracting numerator and denominator
+    [N,D] = numden(HH);
+
+    % Symbolci calculation of function zeros
+    cN = coeffs(expand(N),z);
+    n = length(cN);
+    cN2 = -1.*cN(2:end)./cN(1);
+    A = sym(diag(ones(1,n-2),-1));
+    A(1,:) = cN2;
+    zrs = eig(A);
+    if double(d) == 0
+      zrs = [zrs; sym(0,'f')];
+    end
+
+    % % stabilize zeros
+    % szrs = zrs;
+    % unst = abs(double(szrs)) > 1;
+    % szrs(unst) = 1./conj(szrs(unst));
+    % skz = prod(conj(zrs(unst)));
+
+  %   % stabilize poles
+  %   sp = p;
+  %   unst = abs(double(sp)) > 1;
+  %   sp(unst) = 1./conj(sp(unst));
+  %   skp = prod(1./conj(p(unst)));
+
+    % Correcting for some special cases
+    % if isempty(skz)
+    %   skz = sym(1,'f');
+    % end
+    if isempty(skp)
+      skp = sym(1,'f');
+    end
+    if isempty(k)
+      k = sym(1,'f');
+    end
+
+    % Calculating new gain
+    % sk = real(k*skz*skp);
+    sk = real(k*skp);
+
+    HHHn = sym(1,'f');
+
+    for jj = 1:Np
+      HHHn = HHHn.*(z-zrs(jj));
+      tsp = sp;
+      tsp(jj) = [];
+      tHHHd = sym(1,'f');
+      for kk = 1:Np-1
+        tHHHd = tHHHd.*(z-tsp(kk));
+      end
+      HHHd(jj,1) = tHHHd;
+
+    end
+
+    for jj = 1:Np
+      sr(jj,1) = subs(sk*HHHn/(z*HHHd(jj,1)),z,sp(jj));
+    end
+
+    np = double(sp);
+    for kk = 1:Np
+      if imag(np(kk)) == 0
+        sr(kk) = real(sr(kk));
+      end
+    end
+
+    nr(:,nn) = double(sr);
+    nd(:,nn) = double(dt);
+
+    % Model evaluation
+    pfparams.type = 'disc';
+    pfparams.freq = f;
+    pfparams.fs = fs;
+    pfparams.res = nr(:,nn);
+    pfparams.pol = np;
+    pfparams.dterm = nd(:,nn);
+    pfr = utils.math.pfresp(pfparams);
+    resp = pfr.resp;
+
+    ratio = mean(abs(mresp(:,nn))./abs(resp));
+    resp = resp.*ratio;
+    nr(:,nn) = nr(:,nn).*ratio;
+    nd(:,nn) = nd(:,nn).*ratio;
+    nmresp(:,nn) = resp;
+
+  end