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 //cas continu
 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  //cas discret
 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