* Bug #12794 fixed - Cacsd: standardize calfrq.sci
[scilab.git] / scilab / modules / cacsd / macros / calfrq.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA -
3 //
4 // This file must be used under the terms of the CeCILL.
5 // This source file is licensed as described in the file COPYING, which
6 // you should have received as part of this distribution.  The terms
7 // are also available at
8 // http://www.cecill.info/licences/Licence_CeCILL_V2.1-en.txt
9
10
11 function [frq, bnds, splitf] = calfrq(h, fmin, fmax)
12     //!
13
14     eps = 1.d-14   //minimum absolute lower frequency
15     k = 0.001      // Minimum relative prediction error in the nyquist plan
16     epss = 0.002   // minimum frequency distance with a singularity
17     nptmax = 5000  //maximum number of discretization points
18     tol = 0.01     // Tolerance for testing pure imaginary numbers
19
20     // Check inputs
21     // ------------
22     if and(typeof(h) <> ["state-space" "rational"])
23         error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space or a transfer function expected.\n"), "calfrq", 1))
24     end
25     if typeof(h) == "state-space" then
26         h = ss2tf(h)
27     end
28
29     [m, n] = size(h.num)
30     dom = h("dt")
31     select dom
32     case "d" then
33         dom = 1
34     case [] then
35         error(96, 1)
36     case 0 then
37         error(96, 1)
38     end;
39
40     if type(dom) == 1 then
41         nyq_frq = 1/2/dom;
42         if fmax > nyq_frq then
43             warning(msprintf(gettext("%s: Frequencies beyond Nyquist frequency are ignored.\n"), "calfrq"));
44             fmax = min(fmax, nyq_frq)
45         end
46         if fmin < -nyq_frq then
47             warning(msprintf(gettext("%s: Negative frequencies below Nyquist frequency are ignored.\n"), "calfrq"));
48             fmin = max(fmin, -nyq_frq)
49         end
50     end
51     // Use symmetry to reduce the range
52     // --------------------------------
53     if fmin < 0 & fmax >= 0 then
54         [frq, bnds, splitf] = calfrq(h, eps, -fmin)
55         ns1 = size(splitf, "*")-1;
56         nsp = size(frq, "*");
57         bnds = [bnds(1), bnds(2), -bnds(4), -bnds(3)];
58
59         if fmax > eps then
60             if fmax == -fmin then
61                 splitf = [1, (nsp+2)*ones(1,ns1)-splitf($:-1:2), nsp*ones(ns1)+splitf(2:$)];
62                 bnds = [bnds(1), bnds(2), min(bnds(3), -bnds(3)), max(bnds(4), -bnds(4))];
63                 frq = [-frq($:-1:1), frq]
64             else
65                 [frq2, bnds2, splitf2] = calfrq(h, eps, fmax);
66                 ns2 = size(splitf2,"*")-1
67                 splitf = [1, (nsp+2)*ones(1,ns1)-splitf($:-1:2), nsp*ones(ns2)+splitf2(2:$)];
68                 bnds = [min(bnds(1), bnds2(1)), max(bnds(2), bnds2(2)),...
69                 min(bnds(3), bnds2(3)), max(bnds(4), bnds2(4))];
70                 frq = [-frq($:-1:1), frq2]
71             end
72             return
73         else
74             frq = -frq($:-1:1);
75             nsp = size(frq, "*");
76
77             splitf = [1, (nsp+2)*ones(1, ns1)-splitf($:-1:2)]
78             bnds = bnds;
79             return;
80         end
81     elseif fmin < 0 & fmax <= 0 then
82         [frq, bnds, splitf] = calfrq(h, -fmax, -fmin)
83         ns1 = size(splitf, "*")-1;
84         frq = -frq($:-1:1);
85         nsp = size(frq, "*");
86         splitf = [1, (nsp+2)*ones(1, ns1)-splitf($:-1:2)]
87         bnds = [bnds(1), bnds(2), -bnds(4), -bnds(3)];
88         return;
89     elseif fmin >= fmax then
90         error(msprintf(gettext("%s: Wrong value for input arguments #%d and #%d: %s < %s expected.\n"),..
91         "calfrq", 2, 3, "fmin", "fmax"));
92     end
93
94     // Compute dicretisation over a given range
95     // ----------------------------------------
96
97
98     splitf = []
99     if fmin == 0 then fmin = min(1d-14, fmax/10);end
100     //
101     denh = h("den"); numh = h("num")
102     l10 = log(10)
103
104     // Locate singularities to avoid them
105     // ----------------------------------
106     if dom == "c" then
107         c = 2*%pi;
108         // selection function for singularities in the frequency range
109         deff("f=%sel(r, fmin, fmax, tol)",["f = [],";
110         "if prod(size(r)) == 0 then return, end";
111         "f = imag(r(find((abs(real(r))<=tol*abs(r))&(imag(r)>=0))))";
112         "if f <> [] then f = f(find((f>fmin-tol)&(f<fmax+tol))); end"]);
113     else
114         c = 2*%pi*dom
115         // selection function for singularities in the frequency range
116         deff("[f] = %sel(r, fmin, fmax, dom, tol)",["f = [],";
117         "if prod(size(r)) == 0 then return, end";
118         "f = r(find( ((abs(abs(r)-ones(r)))<=tol)&(imag(r)>=0)))";
119         "if f <> [] then ";
120         "  f = atan(imag(f), real(f)); nf = prod(size(f))";
121         "  for k=1:nf,";
122         "    kk = int((fmax-f(k))/(2*%pi))+1;";
123         "    f = [f; f(1:nf)+2*%pi*kk*ones(nf, 1)];";
124         "  end;"
125         "  f = f(find((f>fmin-tol)&(f<fmax+tol)))";
126         "end"]);
127     end
128
129     sing = [];zers = [];
130     fmin = c*fmin, fmax = c*fmax;
131
132     for i=1:m
133         sing = [sing; %sel(roots(denh(i), "e"), fmin, fmax, tol)];
134     end
135
136     pp = gsort(sing', "g", "i"); npp = size(pp, "*");//'
137
138     // singularities just on the left of the range
139     kinf = find(pp<fmin)
140     if kinf <> [] then
141         fmin = fmin+tol
142         pp(kinf) = []
143     end
144
145     // singularities just on the right of the range
146     ksup = find(pp>=fmax)
147     if ksup <> [] then
148         fmax = fmax-tol
149         pp(ksup) = []
150     end
151
152     // check for nearly multiple singularities
153     if pp <> [] then
154         dpp = pp(2:$)-pp(1:$-1)
155         keq = find(abs(dpp)<2*epss)
156         if keq <> [] then pp(keq) = [], end
157     end
158
159     if pp <> [] then
160         frqs = [fmin real(matrix([(1-epss)*pp; (1+epss)*pp], 2*size(pp, "*"), 1)') fmax]
161         //'
162     else
163         frqs = [fmin fmax]
164     end
165     nfrq = size(frqs, "*");
166
167     // Evaluate bounds of nyquist plot
168     //-------------------------------
169     xt = []; Pas = []
170     for i=1:2:nfrq-1
171         w = logspace(log(frqs(i))/log(10), log(frqs(i+1))/log(10), 100);
172         xt = [xt, w]
173         Pas = [Pas w(2)-w(1)]
174     end
175     if dom == "c" then
176         rf = freq(h("num"), h("den"), %i*xt);
177     else
178         rf = freq(h("num"), h("den"), exp(%i*xt));
179     end
180     //
181     xmin = min(real(rf)); xmax = max(real(rf));
182     ymin = min(imag(rf)); ymax = max(imag(rf));
183     bnds = [xmin xmax ymin ymax];
184     dx = max([xmax-xmin, 1]); dy = max([ymax-ymin, 1]);
185
186     // Compute discretization with a step adaptation method
187     // ----------------------------------------------------
188     frq = [];
189     i = 1;
190     nptr = nptmax; // number of unused discretization points
191     l10last = log10(frqs($));
192     while i<nfrq
193         f0 = frqs(i); fmax = frqs(i+1);
194         while f0==fmax do
195             i = i+2;
196             f = frqs(i); fmax = frqs(i+1);
197         end
198         frq = [frq, f0];
199         pas = Pas(floor(i/2)+1)
200         splitf = [splitf size(frq, "*")];
201
202         f = min(f0+pas, fmax);
203
204         if dom == "c" then // Continuous case
205             while f0<fmax
206                 rf0 = freq(h("num"), h("den"), (%i*f0));
207                 rfc = freq(h("num"), h("den"), %i*f);
208                 // compute prediction error
209                 epsd = pas/100; // epsd = 1.d-8
210
211                 rfd = (freq(h("num"), h("den"), %i*(f0+epsd))-rf0)/(epsd);
212                 rfp = rf0+pas*rfd;
213
214                 e = max([abs(imag(rfp-rfc))/dy; abs(real(rfp-rfc))/dx])
215                 if e > k then rf0 = freq(h("num"), h("den"), (%i*f0));
216                     rfc = freq(h("num"), h("den"), %i*f);
217                     // compute prediction error
218                     epsd = pas/100; // epsd = 1.d-8
219
220                     rfd = (freq(h("num"), h("den"), %i*(f0+epsd))-rf0)/(epsd);
221                     rfp = rf0+pas*rfd;
222
223                     e = max([abs(imag(rfp-rfc))/dy; abs(real(rfp-rfc))/dx])
224                     // compute minimum frequency logarithmic step to ensure a maximum
225                     //of nptmax points to discretize
226                     pasmin = f0*(10^((l10last-log10(f0))/(nptr+1))-1)
227                     pas = pas/2
228                     if pas < pasmin then
229                         pas = pasmin
230                         frq = [frq, f]; nptr = max([1, nptr-1])
231                         f0 = f; f = min(f0+pas, fmax)
232                     else
233                         f = min(f0+pas, fmax)
234                     end
235                 elseif e < k/2 then
236                     pas = 2*pas
237                     frq = [frq, f]; nptr = max([1, nptr-1])
238                     f0 = f; f = min(f0+pas, fmax),
239                 else
240                     frq = [frq, f];nptr = max([1, nptr-1])
241                     f0 = f; f = min(f0+pas, fmax),
242                 end
243             end
244         else  // Discrete case
245             pas = pas/dom
246             while f0<fmax
247                 rf0 = freq(h("num"), h("den"), exp(%i*f0))
248                 rfd = dom*(freq(h("num"), h("den"), exp(%i*(f0+pas/100)))-rf0)/(pas/100);
249                 rfp = rf0+pas*rfd
250                 rfc = freq(h("num"), h("den"), exp(%i*f));
251                 e = max([abs(imag(rfp-rfc))/dy; abs(real(rfp-rfc))/dx])
252                 if e > k then
253                     pasmin = f0*(10^((l10last-log10(f0))/(nptr+1))-1)
254                     pas = pas/2
255                     if pas < pasmin then
256                         pas = pasmin
257                         frq = [frq, f]; nptr = max([1, nptr-1])
258                         f0 = f; f = min(f0+pas, fmax)
259                     else
260                         f = min(f0+pas, fmax)
261                     end
262                 elseif e < k/2 then
263                     pas = 2*pas
264                     frq = [frq, f]; nptr = max([1, nptr-1])
265                     f0 = f; f = min(f0+pas, fmax),
266                 else
267                     frq = [frq, f]; nptr = max([1, nptr-1])
268                     f0 = f; f = min(f0+pas, fmax),
269                 end
270             end
271         end
272         i = i+2
273     end
274     frq(size(frq, "*")) = fmax
275     frq = frq/c;
276 endfunction