Mercurial > hg > ltpda

annotate m-toolbox/classes/+utils/@math/pfallps.m @ 44:409a22968d5e default

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add unit tests
author Daniele Nicolodi <nicolodi@science.unitn.it>
date Tue, 06 Dec 2011 18:42:11 +0100
parents f0afece42f48
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 % PFALLPS all pass filtering in order 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] = pfallps(ir,ip,id,mresp,f)
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
13 % [resp,np] = pfallps(ir,ip,id,mresp,f,minphase)
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
14 % [resp,np,nz] = pfallps(ir,ip,id,mresp,f,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 % minphase: is a flag assuming true (output a minimum phase system) or
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
23 % false (output a stable non minimum phase system) values. Default,
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
24 % true
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
25 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
26 % OUTPUTS:
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
27 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
28 % resp: is the minimum phase frequency response
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
29 % np: are new stable poles
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
30 % nz: are new stable zeros, this will be set only if minphase is set to
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
31 % false
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
32 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
33 % NOTE:
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
34 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
35 % This function make use of signal analysis toolbox functions
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
36 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
37 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
38 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
39 % VERSION: $Id: pfallps.m,v 1.7 2008/12/22 18:44:42 luigi Exp $
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
40 %
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
41 % HISTORY: 12-09-2008 L Ferraioli
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
42 % Creation
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
43 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
44 function varargout = pfallps(ir,ip,id,mresp,f,varargin)
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
45
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
46 % Reshaping
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
47 [a,b] = size(ir);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
48 if a<b
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
49 ir = ir.'; % reshape as a column vector
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
50 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
51
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
52 [a,b] = size(ip);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
53 if a<b
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
54 ip = ip.'; % reshape as a column vector
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
55 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
56
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
57 [a,b] = size(f);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
58 if a<b
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
59 f = f.'; % reshape as a column vector
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
60 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
61
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
62 [a,b] = size(id);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
63 if a > b
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
64 id = id.'; % reshape as a row
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
65 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
66 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
67
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
68 if nargin == 6
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
69 minphase = varargin{1};
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
70 else
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
71 minphase = false;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
72 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
73
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
74 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
75
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
76 % stabilizing poles
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
77 sp = p;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
78 unst = real(sp) > 0;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
79 sp(unst) = -1*conj(sp(unst));
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
80
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
81 [Na,Nb] = size(r);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
82 for nn = 1:Nb
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
83
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
84 s = 1i.*2.*pi.*f;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
85 pp = p(unst);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
86 psp = sp(unst);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
87 for ii = 1:length(s)
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
88 nterm = 1;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
89 for jj = 1:length(sp(unst))
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
90 nterm = nterm.*(s(ii)-pp(jj))./(s(ii)-psp(jj));
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
91 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
92 phs(ii,1) = angle(nterm);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
93 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
94
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
95 resp(:,nn) = mresp(:,nn).*(cos(phs)+1i.*sin(phs));
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
96
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
97 % output stable poles
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
98 np(:,nn) = sp;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
99
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
100 if minphase
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
101 % finding zeros
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
102 [num,den] = residue(r,p,d);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
103 zrs = roots(num);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
104
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
105 % stabilizing zeros
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
106 szrs = zrs;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
107 zunst = abs(zrs) > 1;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
108 szrs(zunst) = 1./conj(zrs(zunst));
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
109
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
110 zzrs = zrs(zunst);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
111 zszrs = szrs(zunst);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
112 for ii = 1:length(s)
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
113 nterm = 1;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
114 for jj = 1:length(szrs(zunst))
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
115 nterm = nterm.*(s(ii)-zszrs(jj))./(s(ii)-zzrs(jj));
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
116 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
117 zphs(ii,1) = angle(nterm);
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
118 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
119
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
120 resp(:,nn) = resp(:,nn).*(cos(zphs)+1i.*sin(zphs));
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
121
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
122 % output stable zeros
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
123 nz(:,nn) = szrs;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
124 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
125 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
126 % output
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
127 if nargout == 1
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
128 varargout{1} = resp;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
129 elseif nargout == 2
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
130 varargout{1} = resp;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
131 varargout{2} = np;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
132 elseif (nargout == 3) && minphase
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
133 varargout{1} = resp;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
134 varargout{2} = np;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
135 varargout{3} = nz;
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
136 else
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
137 error('Too many output arguments!')
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
138 end
f0afece42f48 Import.
Daniele Nicolodi <nicolodi@science.unitn.it>
parents:
diff changeset
139 end