bc845d58028f091e77fe033d9865bfd2f1345dab
[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 //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