0
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
1 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
2 % DESCRIPTION:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
3 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
4 % All pass filtering in order to stabilize transfer function poles and
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
5 % zeros. It inputs a partial fraction expanded continuous model and
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
6 % outputs a pole-zero minimum phase system
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
7 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
8 % CALL:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
9 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
10 % [nr,np,nd,resp] = pfallpsyms(r,p,d,f)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
11 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
12 % INPUTS:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
13 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
14 % r: are residues
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
15 % p: are poles
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
16 % d: is direct term
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
17 % f: is the frequancies vector in (Hz)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
18 % fs: is the sampling frequency in (Hz)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
19 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
20 % OUTPUTS:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
21 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
22 % resp: is the minimum phase frequency response
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
23 % np: are the new stable poles
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
24 % nz: are the new stable zeros
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
25 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
26 % NOTE:
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
27 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
28 % This function make use of symbolic math toolbox functions
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
29 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
30 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
31 % VERSION: '$Id: pfallpsyms.m,v 1.3 2008/11/10 15:40:11 luigi Exp $';
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
32 %
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
33 % HISTORY: 12-09-2008 L Ferraioli
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
34 % Creation
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
35 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
36 function [nr,np,nd,nmresp] = pfallpsyms(r,p,d,f)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
37
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
38 % Reshaping
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
39 [a,b] = size(r);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
40 if a<b
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
41 r = r.'; % reshape as a column vector
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
42 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
43
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
44 [a,b] = size(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
45 if a<b
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
46 p = p.'; % reshape as a column vector
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
47 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
48
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
49 [a,b] = size(f);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
50 if a<b
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
51 f = f.'; % reshape as a column vector
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
52 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
53
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
54 [a,b] = size(d);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
55 if a ~= b
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
56 disp(' Function can handle only single constant direct terms. Only first term will be considered! ')
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
57 d = d(1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
58 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
59
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
60 if isempty(d)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
61 d = 0;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
62 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
63
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
64 % digits(100)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
65 % Defining inputs as symbolic variable
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
66 r = sym(r,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
67 p = sym(p,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
68 d = sym(d,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
69 f = sym(f,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
70 syms z
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
71
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
72 % Function gain coefficient
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
73 if double(d) == 0
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
74 k = sum(r);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
75 else
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
76 k = d;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
77 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
78
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
80 f = sym(f,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
81 syms z
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
82 p = sym(p,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
83
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
84 % stabilize poles
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
85 sp = p;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
86 unst = abs(double(sp)) > 1;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
87 sp(unst) = 1./conj(sp(unst));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
88 skp = prod(1./conj(p(unst)));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
89
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
90 [Na,Nb] = size(r);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
91
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
92 for nn = 1:Nb
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
93
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
94 % digits(100)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
95 % Defining inputs as symbolic variable
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
96 rt = sym(r(:,nn),'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
97 % p = sym(p,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
98 dt = sym(d(1,nn),'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
99 % f = sym(f,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
100 % fs = sym(fs,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
101 % syms z
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
102
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
103 % Function gain coefficient
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
104 if double(d) == 0
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
105 k = sum(r);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
106 else
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
107 k = d;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
108 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
109
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
110 Np = length(p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
111
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
112 % Defining the symbolic transfer function
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
113 vter = rt./(z-p);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
114 vter = [vter; dt];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
115 Mter = diag(vter);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
116 H = trace(Mter);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
117
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
118 % Factorizing the transfer function
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
119 HH = factor(H);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
120
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
121 % Extracting numerator and denominator
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
122 [N,D] = numden(HH);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
123
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
124 % Symbolci calculation of function zeros
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
125 cN = coeffs(expand(N),z);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
126 n = length(cN);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
127 cN2 = -1.*cN(2:end)./cN(1);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
128 A = sym(diag(ones(1,n-2),-1));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
129 A(1,:) = cN2;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
130 zrs = eig(A);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
131 % if double(d) == 0
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
132 % zrs = [zrs; sym(0,'f')];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
133 % end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
134
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
135 if isempty(skp)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
136 skp = sym(1,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
137 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
138 if isempty(k)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
139 k = sym(1,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
140 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
141
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
142 % Calculating new gain
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
143 % sk = real(k*skz*skp);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
144 sk = real(k*skp);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
145
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
146 HHHn = sym(1,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
147
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
148 for ii = 1:length(zrs)
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
149 HHHn = HHHn.*(z-zrs(ii));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
150 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
151 for jj = 1:Np
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
152 tsp = sp;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
153 tsp(jj) = [];
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
154 tHHHd = sym(1,'f');
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
155 for kk = 1:Np-1
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
156 tHHHd = tHHHd.*(z-tsp(kk));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
157 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
158 HHHd(jj,1) = tHHHd;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
159
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
160 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
161
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
162 for jj = 1:Np
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
163 sr(jj,1) = subs(sk*HHHn/(z*HHHd(jj,1)),z,sp(jj));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
164 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
165
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
166 np = double(sp);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
167 for kk = 1:Np
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
168 if imag(np(kk)) == 0
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
169 sr(kk) = real(sr(kk));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
170 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
171 end
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
172
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
173 nr(:,nn) = double(sr);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
174 nd(:,nn) = double(dt);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
175
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
176 % Model evaluation
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
177 pfparams.type = 'cont';
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
178 pfparams.freq = f;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
179 pfparams.res = nr(:,nn);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
180 pfparams.pol = np;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
181 pfparams.dterm = nd(:,nn);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
182 pfr = utils.math.pfresp(pfparams);
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
183 resp = pfr.resp;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
184
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
185 ratio = mean(abs(mresp(:,nn))./abs(resp));
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
186 resp = resp.*ratio;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
187 nr(:,nn) = nr(:,nn).*ratio;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
188 nd(:,nn) = nd(:,nn).*ratio;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
189 nmresp(:,nn) = resp;
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
190
|
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff
changeset
|
191 end
|