Mercurial > hg > ltpda

comparison m-toolbox/classes/+utils/@math/pfallpz.m @ 0:f0afece42f48

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Import.
author Daniele Nicolodi <nicolodi@science.unitn.it>
date Wed, 23 Nov 2011 19:22:13 +0100
parents
children
comparison
equal deleted inserted replaced
-1:000000000000 0:f0afece42f48
1 % PFALLPZ all pass filtering to stabilize TF poles and zeros.
2 %
3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 % DESCRIPTION:
5 %
6 % All pass filtering in order to stabilize transfer function poles and
7 % zeros. It inputs a partial fraction expanded discrete model and
8 % outputs a pole-zero minimum phase system
9 %
10 % CALL:
11 %
12 % [resp,np] = pfallpz(ir,ip,id,mresp,f,fs)
13 % [resp,np] = pfallpz(ir,ip,id,mresp,f,fs,minphase)
14 % [resp,np,nz] = pfallpz(ir,ip,id,mresp,f,fs,minphase)
15 %
16 % INPUTS:
17 %
18 % ir: are residues
19 % ip: are poles
20 % id: is direct term
21 % f: is the frequancies vector in (Hz)
22 % fs: is the sampling frequency in (Hz)
23 % minphase: is a flag assuming true (output a minimum phase system) or
24 % false (output a stable non minimum phase system) values. Default,
25 % false
26 %
27 % OUTPUTS:
28 %
29 % resp: is the functions phase frequency response
30 % np: are the new stable poles
31 %
32 % NOTE:
33 %
34 % This function make use of signal analysis toolbox functions
35 %
36 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
37 % VERSION: $Id: pfallpz.m,v 1.6 2009/06/10 15:47:00 luigi Exp $
38 %
39 % HISTORY: 12-09-2008 L Ferraioli
40 % Creation
41 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
42 function varargout = pfallpz(ir,ip,id,mresp,f,fs,varargin)
43
44 % Reshaping
45 [a,b] = size(ir);
46 if a<b
47 ir = ir.'; % reshape as a column vector
48 end
49
50 [a,b] = size(ip);
51 if a<b
52 ip = ip.'; % reshape as a column vector
53 end
54
55 [a,b] = size(f);
56 if a<b
57 f = f.'; % reshape as a column vector
58 end
59
60 [a,b] = size(id);
61 if a > b
62 id = id.'; % reshape as a row
63 id = id(1,:); % taking the first row (the function can handle only simple constant direct terms)
64 end
65
66 if isempty(fs)
67 fs = 1;
68 end
69 [a,b] = size(fs);
70 if a ~= b
71 disp(' Fs has to be a number. Only first term will be considered! ')
72 fs = fs(1);
73 end
74
75 if nargin == 7
76 minphase = varargin{1};
77 else
78 minphase = false;
79 end
80
81 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
82 [Na,Nb] = size(ir);
83 np = zeros(Na,Nb);
84 nz = zeros(Na,Nb);
85 for nn = 1:Nb
86
87 r = ir(:,nn);
88 d = id(1,nn);
89 p = ip;
90 % iresp = mresp(:,nn);
91
92 % k = sum(r)+d;
93
94 % stabilizing poles
95 sp = p;
96 unst = abs(p) > 1;
97 sp(unst) = 1./conj(sp(unst));
98
99 s = cos((2*pi/fs).*f) + 1i.*sin((2*pi/fs).*f);
100
101 pp = p(unst);
102 psp = sp(unst);
103 for ii = 1:length(s)
104 nterm = 1;
105 for jj = 1:length(sp(unst))
106 nterm = nterm.*(s(ii)-pp(jj))./(s(ii)-psp(jj));
107 end
108 phs(ii,1) = angle(nterm);
109 end
110
111 resp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
112
113 % output stable poles
114 np(:,nn) = sp;
115
116 if minphase
117 if d~=0
118 error('!!!Minimum phase filers can be obtained only when direct term is zero')
119 end
120 % finding zeros
121 % N = Na+1;
122 % [mults, idx] = mpoles(p,1e-15,0); % checking for poles multiplicity
123 % p = p(idx); % re-arrange poles & residues
124 % r = r(idx);
125 % den = poly(p);
126 % num = conv(den,d);
127 % for i=1:Na
128 % temp = poly( p([1:(i-mults(i)), (i+1):Na]) );
129 % num = num + [r(i)*temp zeros(1,N-length(temp))];
130 % end
131 % zrs = roots(num);
132 D = 0; % direct term of sigma
133
134 A = diag(p);
135 B = ones(Na,1);
136 C = zeros(1,Na);
137 % Real poles have real residues, complex poles have comples residues
138
139 cindex=zeros(1,Na);
140 for m=1:Na
141 if imag(p(m))~=0
142 if m==1
143 cindex(m)=1;
144 else
145 if cindex(m-1)==0 || cindex(m-1)==2
146 cindex(m)=1; cindex(m+1)=2;
147 else
148 cindex(m)=2;
149 end
150 end
151 end
152 end
153
154
155 for kk = 1:Na
156 if cindex(kk) == 1
157 A(kk,kk)=real(p(kk));
158 A(kk,kk+1)=imag(p(kk));
159 A(kk+1,kk)=-1*imag(p(kk));
160 A(kk+1,kk+1)=real(p(kk));
161 B(kk,1) = 2;
162 B(kk+1,1) = 0;
163 C(1,kk) = real(r(kk,1));
164 C(1,kk+1) = imag(r(kk,1));
165 elseif cindex(kk) == 0 % real pole
166 C(1,kk) = r(kk,1);
167 end
168 end
169
170 [zrs,p2,k] = ss2zp(A,B,C,D,1);
171
172 % stabilizing zeros
173 szrs = zrs;
174 % willing to work with columns
175 [a,b] = size(szrs);
176 if a<b
177 szrs = szrs.'; % reshape as a column vector
178 zrs = zrs.';
179 end
180 % adding the zero at the origin
181 zrs = [0;zrs];
182 szrs = [0;szrs];
183 % do stabilization
184 zunst = abs(zrs) > 1;
185 szrs(zunst) = 1./conj(zrs(zunst));
186
187 zzrs = zrs(zunst);
188 zszrs = szrs(zunst);
189 for ii = 1:length(s)
190 nterm = 1;
191 for jj = 1:length(szrs(zunst))
192 nterm = nterm.*(s(ii)-zszrs(jj))./(s(ii)-zzrs(jj));
193 end
194 zphs(ii,1) = angle(nterm);
195 end
196
197 resp(:,nn) = resp(:,nn).*(cos(zphs)+1i.*sin(zphs));
198
199 % output stable zeros
200 nz(:,nn) = szrs;
201 end
202
203 end
204
205
206 % output
207 if nargout == 1
208 varargout{1} = resp;
209 elseif nargout == 2
210 varargout{1} = resp;
211 varargout{2} = np;
212 elseif (nargout == 3) && minphase
213 varargout{1} = resp;
214 varargout{2} = np;
215 varargout{3} = nz;
216 else
217 error('Too many output arguments!')
218 end
219 end
220
221