Mercurial > hg > ltpda

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