line source
+ − % 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