--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/m-toolbox/classes/+utils/@math/pfallpz.m	Wed Nov 23 19:22:13 2011 +0100
@@ -0,0 +1,221 @@
+% PFALLPZ all pass filtering to stabilize TF poles and zeros.
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% DESCRIPTION:
+% 
+%     All pass filtering in order to stabilize transfer function poles and
+%     zeros. It inputs a partial fraction expanded discrete model and
+%     outputs a pole-zero minimum phase system
+% 
+% CALL:
+% 
+%     [resp,np] = pfallpz(ir,ip,id,mresp,f,fs)
+%     [resp,np] = pfallpz(ir,ip,id,mresp,f,fs,minphase)
+%     [resp,np,nz] = pfallpz(ir,ip,id,mresp,f,fs,minphase)
+% 
+% INPUTS:
+% 
+%     ir: are residues
+%     ip: are poles
+%     id: is direct term
+%     f: is the frequancies vector in (Hz)
+%     fs: is the sampling frequency in (Hz)
+%     minphase: is a flag assuming true (output a minimum phase system) or
+%     false (output a stable non minimum phase system) values. Default,
+%     false
+%     
+% OUTPUTS:
+% 
+%     resp: is the functions phase frequency response
+%     np: are the new stable poles
+%
+% NOTE:
+% 
+%     This function make use of signal analysis toolbox functions
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% VERSION: $Id: pfallpz.m,v 1.6 2009/06/10 15:47:00 luigi Exp $
+% 
+% HISTORY:     12-09-2008 L Ferraioli
+%                 Creation
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function varargout = pfallpz(ir,ip,id,mresp,f,fs,varargin)
+
+  % Reshaping
+  [a,b] = size(ir);
+  if a<b
+    ir = ir.'; % reshape as a column vector
+  end
+
+  [a,b] = size(ip);
+  if a<b
+    ip = ip.'; % reshape as a column vector
+  end
+
+  [a,b] = size(f);
+  if a<b
+    f = f.'; % reshape as a column vector
+  end
+
+  [a,b] = size(id);
+  if a > b
+    id = id.'; % reshape as a row
+    id = id(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
+  
+  if nargin == 7
+    minphase = varargin{1};
+  else
+    minphase = false;
+  end
+
+  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  [Na,Nb] = size(ir);
+  np = zeros(Na,Nb);
+  nz = zeros(Na,Nb);
+  for nn = 1:Nb
+
+    r = ir(:,nn);
+    d = id(1,nn);
+    p = ip;
+%     iresp = mresp(:,nn);
+
+%     k = sum(r)+d;
+
+    % stabilizing poles
+    sp = p;
+    unst = abs(p) > 1;
+    sp(unst) = 1./conj(sp(unst));
+    
+    s = cos((2*pi/fs).*f) + 1i.*sin((2*pi/fs).*f);
+    
+    pp = p(unst);
+    psp = sp(unst);
+    for ii = 1:length(s)
+      nterm = 1;
+      for jj = 1:length(sp(unst))
+        nterm = nterm.*(s(ii)-pp(jj))./(s(ii)-psp(jj));
+      end
+      phs(ii,1) = angle(nterm);
+    end
+      
+    resp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
+    
+    % output stable poles
+    np(:,nn) = sp;
+    
+    if minphase
+      if d~=0
+        error('!!!Minimum phase filers can be obtained only when direct term is zero')
+      end
+      % finding zeros
+%       N = Na+1;
+%       [mults, idx] = mpoles(p,1e-15,0);  % checking for poles multiplicity
+%       p = p(idx);     % re-arrange poles & residues
+%       r = r(idx);
+%       den = poly(p);
+%       num = conv(den,d);
+%       for i=1:Na
+%          temp = poly( p([1:(i-mults(i)), (i+1):Na]) );
+%          num = num + [r(i)*temp zeros(1,N-length(temp))];
+%       end
+%       zrs = roots(num);
+      D = 0; % direct term of sigma
+  
+      A = diag(p);
+      B = ones(Na,1);
+      C = zeros(1,Na);
+      % Real poles have real residues, complex poles have comples residues
+
+      cindex=zeros(1,Na);
+      for m=1:Na 
+        if imag(p(m))~=0  
+          if m==1 
+            cindex(m)=1;
+          else
+            if cindex(m-1)==0 || cindex(m-1)==2
+              cindex(m)=1; cindex(m+1)=2; 
+            else
+              cindex(m)=2;
+            end
+          end 
+        end
+      end
+
+
+      for kk = 1:Na
+        if cindex(kk) == 1
+          A(kk,kk)=real(p(kk));
+          A(kk,kk+1)=imag(p(kk));
+          A(kk+1,kk)=-1*imag(p(kk));
+          A(kk+1,kk+1)=real(p(kk));
+          B(kk,1) = 2;
+          B(kk+1,1) = 0;
+          C(1,kk) = real(r(kk,1));
+          C(1,kk+1) = imag(r(kk,1));
+        elseif cindex(kk) == 0 % real pole
+          C(1,kk) = r(kk,1);
+        end
+      end
+      
+      [zrs,p2,k] = ss2zp(A,B,C,D,1);
+      
+      % stabilizing zeros
+      szrs = zrs;
+      % willing to work with columns
+      [a,b] = size(szrs);
+      if a<b
+        szrs = szrs.'; % reshape as a column vector
+        zrs = zrs.';
+      end
+      % adding the zero at the origin
+      zrs = [0;zrs];
+      szrs = [0;szrs];
+      % do stabilization
+      zunst = abs(zrs) > 1;
+      szrs(zunst) = 1./conj(zrs(zunst));
+      
+      zzrs = zrs(zunst);
+      zszrs = szrs(zunst);
+      for ii = 1:length(s)
+        nterm = 1;
+        for jj = 1:length(szrs(zunst))
+          nterm = nterm.*(s(ii)-zszrs(jj))./(s(ii)-zzrs(jj));
+        end
+        zphs(ii,1) = angle(nterm);
+      end
+
+      resp(:,nn) = resp(:,nn).*(cos(zphs)+1i.*sin(zphs));
+      
+      % output stable zeros
+      nz(:,nn) = szrs;
+    end
+    
+  end
+  
+  
+  % output
+  if nargout == 1
+    varargout{1} = resp;
+  elseif nargout == 2
+    varargout{1} = resp;
+    varargout{2} = np;
+  elseif (nargout == 3) && minphase
+    varargout{1} = resp;
+    varargout{2} = np;
+    varargout{3} = nz;
+  else
+    error('Too many output arguments!')
+  end
+end
+
+