Mercurial > hg > ltpda

comparison m-toolbox/classes/+utils/@math/pfallpsymz.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 % PFALLPSYMZ all pass filtering in order to stabilize TF poles and zeros.
2 %
3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4 % DESCRIPTION:
5 %
6 % All pass filtering in order to stabilize transfer functions poles.
7 % It inputs a partial fraction expanded discrete model and outpu
8 % residues, poles direct terms and frequency response of the stabilized
9 % model. Function can handle multiple models with common poles.
10 %
11 % CALL:
12 %
13 % [nr,np,nd,resp] = pfallpsymz(r,p,d,f,fs)
14 %
15 % INPUTS:
16 %
17 % r: are residues. (Npx1) or (NpxM) vector
18 % p: are poles. (Npx1) vector
19 % d: is direct term (1x1) or (1xM) vector
20 % mresp: input model response. (Nx1) or (NxM) vector
21 % f: is the frequancies vector in (Hz). (Nx1) vector
22 % fs: is the sampling frequency in (Hz). (1x1)
23 %
24 % OUTPUTS:
25 %
26 % nr: new residues. (Npx1) or (NpxM) vector
27 % np: new stable poles. (Npx1) vector
28 % nd: new direct term. (1x1) or (1xM) vector
29 % nmresp: new model response. (Nx1) or (NxM) vector
30 %
31 % NOTE:
32 %
33 % This function make use of symbolic math toolbox functions
34 %
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
36 % VERSION: $Id: pfallpsymz.m,v 1.5 2008/11/10 15:40:11 luigi Exp $
37 %
38 % HISTORY: 12-09-2008 L Ferraioli
39 % Creation
40 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 function [nr,np,nd,nmresp] = pfallpsymz(r,p,d,mresp,f,fs)
42
43 % Reshaping
44 [a,b] = size(r);
45 if a<b
46 r = r.'; % reshape as a column vector
47 end
48
49 [a,b] = size(p);
50 if a<b
51 p = p.'; % reshape as a column vector
52 end
53
54 [a,b] = size(f);
55 if a<b
56 f = f.'; % reshape as a column vector
57 end
58
59 [a,b] = size(f);
60 if a<b
61 f = f.'; % reshape as a column vector
62 end
63
64 [a,b] = size(d);
65 if a > b
66 d = d.'; % reshape as a row
67 d = d(1,:); % taking the first row (the function can handle only simple constant direct terms)
68 end
69
70 if isempty(fs)
71 fs = 1;
72 end
73 [a,b] = size(fs);
74 if a ~= b
75 disp(' Fs has to be a number. Only first term will be considered! ')
76 fs = fs(1);
77 end
78
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80 f = sym(f,'f');
81 fs = sym(fs,'f');
82 syms z
83 p = sym(p,'f');
84
85 % stabilize poles
86 sp = p;
87 unst = abs(double(sp)) > 1;
88 sp(unst) = 1./conj(sp(unst));
89 skp = prod(1./conj(p(unst)));
90
91 [Na,Nb] = size(r);
92
93 for nn = 1:Nb
94
95 % pp = p(unst);
96 % psp = sp(unst);
97 % tterm = 1;
98 % for ii = 1:length(pp)
99 % tterm = tterm.*(z-pp(ii))./(z-psp(ii));
100 % end
101 %
102 % s = cos((2*pi/fs).*f) + 1i.*sin((2*pi/fs).*f);
103 % allresp = subs(tterm,z,s);
104 % phs = angle(allresp);
105 % nmresp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
106 % np = double(sp);
107
108 % digits(100)
109 % Defining inputs as symbolic variable
110 rt = sym(r(:,nn),'f');
111 % p = sym(p,'f');
112 dt = sym(d(1,nn),'f');
113 % f = sym(f,'f');
114 % fs = sym(fs,'f');
115 % syms z
116
117 % Function gain coefficient
118 k = sum(rt)+dt;
119
120 Np = length(p);
121
122 % Defining the symbolic transfer function
123 vter = rt.*z./(z-p);
124 vter = [vter; dt];
125 Mter = diag(vter);
126 H = trace(Mter);
127
128 % Factorizing the transfer function
129 HH = factor(H);
130
131 % Extracting numerator and denominator
132 [N,D] = numden(HH);
133
134 % Symbolci calculation of function zeros
135 cN = coeffs(expand(N),z);
136 n = length(cN);
137 cN2 = -1.*cN(2:end)./cN(1);
138 A = sym(diag(ones(1,n-2),-1));
139 A(1,:) = cN2;
140 zrs = eig(A);
141 if double(d) == 0
142 zrs = [zrs; sym(0,'f')];
143 end
144
145 % % stabilize zeros
146 % szrs = zrs;
147 % unst = abs(double(szrs)) > 1;
148 % szrs(unst) = 1./conj(szrs(unst));
149 % skz = prod(conj(zrs(unst)));
150
151 % % stabilize poles
152 % sp = p;
153 % unst = abs(double(sp)) > 1;
154 % sp(unst) = 1./conj(sp(unst));
155 % skp = prod(1./conj(p(unst)));
156
157 % Correcting for some special cases
158 % if isempty(skz)
159 % skz = sym(1,'f');
160 % end
161 if isempty(skp)
162 skp = sym(1,'f');
163 end
164 if isempty(k)
165 k = sym(1,'f');
166 end
167
168 % Calculating new gain
169 % sk = real(k*skz*skp);
170 sk = real(k*skp);
171
172 HHHn = sym(1,'f');
173
174 for jj = 1:Np
175 HHHn = HHHn.*(z-zrs(jj));
176 tsp = sp;
177 tsp(jj) = [];
178 tHHHd = sym(1,'f');
179 for kk = 1:Np-1
180 tHHHd = tHHHd.*(z-tsp(kk));
181 end
182 HHHd(jj,1) = tHHHd;
183
184 end
185
186 for jj = 1:Np
187 sr(jj,1) = subs(sk*HHHn/(z*HHHd(jj,1)),z,sp(jj));
188 end
189
190 np = double(sp);
191 for kk = 1:Np
192 if imag(np(kk)) == 0
193 sr(kk) = real(sr(kk));
194 end
195 end
196
197 nr(:,nn) = double(sr);
198 nd(:,nn) = double(dt);
199
200 % Model evaluation
201 pfparams.type = 'disc';
202 pfparams.freq = f;
203 pfparams.fs = fs;
204 pfparams.res = nr(:,nn);
205 pfparams.pol = np;
206 pfparams.dterm = nd(:,nn);
207 pfr = utils.math.pfresp(pfparams);
208 resp = pfr.resp;
209
210 ratio = mean(abs(mresp(:,nn))./abs(resp));
211 resp = resp.*ratio;
212 nr(:,nn) = nr(:,nn).*ratio;
213 nd(:,nn) = nd(:,nn).*ratio;
214 nmresp(:,nn) = resp;
215
216 end