diff options
Diffstat (limited to 'modules/cacsd/macros/calfrq.sci')
| -rwxr-xr-x | modules/cacsd/macros/calfrq.sci | 276 |
1 files changed, 276 insertions, 0 deletions
diff --git a/modules/cacsd/macros/calfrq.sci b/modules/cacsd/macros/calfrq.sci new file mode 100755 index 000000000..524bf49c3 --- /dev/null +++ b/modules/cacsd/macros/calfrq.sci @@ -0,0 +1,276 @@ +// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab +// Copyright (C) INRIA - +// +// This file must be used under the terms of the CeCILL. +// This source file is licensed as described in the file COPYING, which +// you should have received as part of this distribution. The terms +// are also available at +// http://www.cecill.info/licences/Licence_CeCILL_V2.1-en.txt + + +function [frq, bnds, splitf] = calfrq(h, fmin, fmax) + //! + + eps = 1.d-14 //minimum absolute lower frequency + k = 0.001 // Minimum relative prediction error in the nyquist plan + epss = 0.002 // minimum frequency distance with a singularity + nptmax = 5000 //maximum number of discretization points + tol = 0.01 // Tolerance for testing pure imaginary numbers + + // Check inputs + // ------------ + if and(typeof(h) <> ["state-space" "rational"]) + error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space or a transfer function expected.\n"), "calfrq", 1)) + end + if typeof(h) == "state-space" then + h = ss2tf(h) + end + + [m, n] = size(h.num) + dom = h("dt") + select dom + case "d" then + dom = 1 + case [] then + error(96, 1) + case 0 then + error(96, 1) + end; + + if type(dom) == 1 then + nyq_frq = 1/2/dom; + if fmax > nyq_frq then + warning(msprintf(gettext("%s: Frequencies beyond Nyquist frequency are ignored.\n"), "calfrq")); + fmax = min(fmax, nyq_frq) + end + if fmin < -nyq_frq then + warning(msprintf(gettext("%s: Negative frequencies below Nyquist frequency are ignored.\n"), "calfrq")); + fmin = max(fmin, -nyq_frq) + end + end + // Use symmetry to reduce the range + // -------------------------------- + if fmin < 0 & fmax >= 0 then + [frq, bnds, splitf] = calfrq(h, eps, -fmin) + ns1 = size(splitf, "*")-1; + nsp = size(frq, "*"); + bnds = [bnds(1), bnds(2), -bnds(4), -bnds(3)]; + + if fmax > eps then + if fmax == -fmin then + splitf = [1, (nsp+2)*ones(1,ns1)-splitf($:-1:2), nsp*ones(ns1)+splitf(2:$)]; + bnds = [bnds(1), bnds(2), min(bnds(3), -bnds(3)), max(bnds(4), -bnds(4))]; + frq = [-frq($:-1:1), frq] + else + [frq2, bnds2, splitf2] = calfrq(h, eps, fmax); + ns2 = size(splitf2,"*")-1 + splitf = [1, (nsp+2)*ones(1,ns1)-splitf($:-1:2), nsp*ones(ns2)+splitf2(2:$)]; + bnds = [min(bnds(1), bnds2(1)), max(bnds(2), bnds2(2)),... + min(bnds(3), bnds2(3)), max(bnds(4), bnds2(4))]; + frq = [-frq($:-1:1), frq2] + end + return + else + frq = -frq($:-1:1); + nsp = size(frq, "*"); + + splitf = [1, (nsp+2)*ones(1, ns1)-splitf($:-1:2)] + bnds = bnds; + return; + end + elseif fmin < 0 & fmax <= 0 then + [frq, bnds, splitf] = calfrq(h, -fmax, -fmin) + ns1 = size(splitf, "*")-1; + frq = -frq($:-1:1); + nsp = size(frq, "*"); + splitf = [1, (nsp+2)*ones(1, ns1)-splitf($:-1:2)] + bnds = [bnds(1), bnds(2), -bnds(4), -bnds(3)]; + return; + elseif fmin >= fmax then + error(msprintf(gettext("%s: Wrong value for input arguments #%d and #%d: %s < %s expected.\n"),.. + "calfrq", 2, 3, "fmin", "fmax")); + end + + // Compute dicretisation over a given range + // ---------------------------------------- + + + splitf = [] + if fmin == 0 then fmin = min(1d-14, fmax/10);end + // + denh = h("den"); numh = h("num") + l10 = log(10) + + // Locate singularities to avoid them + // ---------------------------------- + if dom == "c" then + c = 2*%pi; + // selection function for singularities in the frequency range + deff("f=%sel(r, fmin, fmax, tol)",["f = [],"; + "if prod(size(r)) == 0 then return, end"; + "f = imag(r(find((abs(real(r))<=tol*abs(r))&(imag(r)>=0))))"; + "if f <> [] then f = f(find((f>fmin-tol)&(f<fmax+tol))); end"]); + else + c = 2*%pi*dom + // selection function for singularities in the frequency range + deff("[f] = %sel(r, fmin, fmax, dom, tol)",["f = [],"; + "if prod(size(r)) == 0 then return, end"; + "f = r(find( ((abs(abs(r)-ones(r)))<=tol)&(imag(r)>=0)))"; + "if f <> [] then "; + " f = atan(imag(f), real(f)); nf = prod(size(f))"; + " for k=1:nf,"; + " kk = int((fmax-f(k))/(2*%pi))+1;"; + " f = [f; f(1:nf)+2*%pi*kk*ones(nf, 1)];"; + " end;" + " f = f(find((f>fmin-tol)&(f<fmax+tol)))"; + "end"]); + end + + sing = [];zers = []; + fmin = c*fmin, fmax = c*fmax; + + for i=1:m + sing = [sing; %sel(roots(denh(i), "e"), fmin, fmax, tol)]; + end + + pp = gsort(sing', "g", "i"); npp = size(pp, "*");//' + + // singularities just on the left of the range + kinf = find(pp<fmin) + if kinf <> [] then + fmin = fmin+tol + pp(kinf) = [] + end + + // singularities just on the right of the range + ksup = find(pp>=fmax) + if ksup <> [] then + fmax = fmax-tol + pp(ksup) = [] + end + + // check for nearly multiple singularities + if pp <> [] then + dpp = pp(2:$)-pp(1:$-1) + keq = find(abs(dpp)<2*epss) + if keq <> [] then pp(keq) = [], end + end + + if pp <> [] then + frqs = [fmin real(matrix([(1-epss)*pp; (1+epss)*pp], 2*size(pp, "*"), 1)') fmax] + //' + else + frqs = [fmin fmax] + end + nfrq = size(frqs, "*"); + + // Evaluate bounds of nyquist plot + //------------------------------- + xt = []; Pas = [] + for i=1:2:nfrq-1 + w = logspace(log(frqs(i))/log(10), log(frqs(i+1))/log(10), 100); + xt = [xt, w] + Pas = [Pas w(2)-w(1)] + end + if dom == "c" then + rf = freq(h("num"), h("den"), %i*xt); + else + rf = freq(h("num"), h("den"), exp(%i*xt)); + end + // + xmin = min(real(rf)); xmax = max(real(rf)); + ymin = min(imag(rf)); ymax = max(imag(rf)); + bnds = [xmin xmax ymin ymax]; + dx = max([xmax-xmin, 1]); dy = max([ymax-ymin, 1]); + + // Compute discretization with a step adaptation method + // ---------------------------------------------------- + frq = []; + i = 1; + nptr = nptmax; // number of unused discretization points + l10last = log10(frqs($)); + while i<nfrq + f0 = frqs(i); fmax = frqs(i+1); + while f0==fmax do + i = i+2; + f = frqs(i); fmax = frqs(i+1); + end + frq = [frq, f0]; + pas = Pas(floor(i/2)+1) + splitf = [splitf size(frq, "*")]; + + f = min(f0+pas, fmax); + + if dom == "c" then // Continuous case + while f0<fmax + rf0 = freq(h("num"), h("den"), (%i*f0)); + rfc = freq(h("num"), h("den"), %i*f); + // compute prediction error + epsd = pas/100; // epsd = 1.d-8 + + rfd = (freq(h("num"), h("den"), %i*(f0+epsd))-rf0)/(epsd); + rfp = rf0+pas*rfd; + + e = max([abs(imag(rfp-rfc))/dy; abs(real(rfp-rfc))/dx]) + if e > k then rf0 = freq(h("num"), h("den"), (%i*f0)); + rfc = freq(h("num"), h("den"), %i*f); + // compute prediction error + epsd = pas/100; // epsd = 1.d-8 + + rfd = (freq(h("num"), h("den"), %i*(f0+epsd))-rf0)/(epsd); + rfp = rf0+pas*rfd; + + e = max([abs(imag(rfp-rfc))/dy; abs(real(rfp-rfc))/dx]) + // compute minimum frequency logarithmic step to ensure a maximum + //of nptmax points to discretize + pasmin = f0*(10^((l10last-log10(f0))/(nptr+1))-1) + pas = pas/2 + if pas < pasmin then + pas = pasmin + frq = [frq, f]; nptr = max([1, nptr-1]) + f0 = f; f = min(f0+pas, fmax) + else + f = min(f0+pas, fmax) + end + elseif e < k/2 then + pas = 2*pas + frq = [frq, f]; nptr = max([1, nptr-1]) + f0 = f; f = min(f0+pas, fmax), + else + frq = [frq, f];nptr = max([1, nptr-1]) + f0 = f; f = min(f0+pas, fmax), + end + end + else // Discrete case + pas = pas/dom + while f0<fmax + rf0 = freq(h("num"), h("den"), exp(%i*f0)) + rfd = dom*(freq(h("num"), h("den"), exp(%i*(f0+pas/100)))-rf0)/(pas/100); + rfp = rf0+pas*rfd + rfc = freq(h("num"), h("den"), exp(%i*f)); + e = max([abs(imag(rfp-rfc))/dy; abs(real(rfp-rfc))/dx]) + if e > k then + pasmin = f0*(10^((l10last-log10(f0))/(nptr+1))-1) + pas = pas/2 + if pas < pasmin then + pas = pasmin + frq = [frq, f]; nptr = max([1, nptr-1]) + f0 = f; f = min(f0+pas, fmax) + else + f = min(f0+pas, fmax) + end + elseif e < k/2 then + pas = 2*pas + frq = [frq, f]; nptr = max([1, nptr-1]) + f0 = f; f = min(f0+pas, fmax), + else + frq = [frq, f]; nptr = max([1, nptr-1]) + f0 = f; f = min(f0+pas, fmax), + end + end + end + i = i+2 + end + frq(size(frq, "*")) = fmax + frq = frq/c; +endfunction |