Mercurial > hg > ltpda

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