7d6ada4e39db29baafcf04eb7552a8e46c601953
[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-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 discretisation 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   flag=h(1);
26   if flag(1)=='lss' then
27     h=ss2tf(h)
28   end
29   [m,n]=size(h.num)
30   if  n<>1 then  
31     error(msprintf(gettext("%s: Wrong size for input argument #%d: Single input system expected.\n"),"calfrq",1))
32   end
33   dom=h('dt')
34   select dom
35   case 'd' then 
36     dom=1
37   case [] then 
38     error(96,1)
39   case 0 then
40     error(96,1)
41   end;
42
43   if type(dom)==1 then 
44     nyq_frq=1/2/dom;
45     if fmax>nyq_frq then 
46       warning(msprintf(gettext("%s: Frequencies beyond Nyquist frequency are ignored.\n"),"calfrq"));
47       fmax=min(fmax,nyq_frq)
48     end
49     if fmin<-nyq_frq then 
50       warning(msprintf(gettext("%s: Negative frequencies below Nyquist frequency are ignored.\n"),"calfrq"));
51       fmin=max(fmin,-nyq_frq)
52     end
53   end
54   // Use symmetry to reduce the range
55   // --------------------------------
56   if fmin<0&fmax>=0 then
57     [frq,bnds,splitf]=calfrq(h,eps,-fmin)
58     ns1=size(splitf,'*')-1;
59     nsp=size(frq,'*');
60     bnds=[bnds(1),bnds(2),-bnds(4),-bnds(3)];
61
62     if fmax>eps then
63       if fmax==-fmin then
64         splitf=[1, (nsp+2)*ones(1,ns1)-splitf($:-1:2),nsp*ones(ns1)+splitf(2:$)];
65         bnds=[bnds(1),bnds(2),min(bnds(3),-bnds(3)),max(bnds(4),-bnds(4))];
66         frq=[-frq($:-1:1),frq]
67       else
68         [frq2,bnds2,splitf2]=calfrq(h,eps,fmax);
69         ns2=size(splitf2,'*')-1
70         splitf=[1, (nsp+2)*ones(1,ns1)-splitf($:-1:2),nsp*ones(ns2)+splitf2(2:$)];
71         bnds=[min(bnds(1),bnds2(1)),max(bnds(2),bnds2(2)),...
72               min(bnds(3),bnds2(3)),max(bnds(4),bnds2(4))];
73         frq=[-frq($:-1:1),frq2]
74       end
75       return
76     else
77       frq=-frq($:-1:1);
78       nsp=size(frq,'*');
79       
80       splitf=[1, (nsp+2)*ones(1,ns1)-splitf($:-1:2)]
81       bnds=bnds;
82       return;
83     end
84   elseif fmin<0&fmax<=0 then
85     [frq,bnds,splitf]=calfrq(h,-fmax,-fmin)
86     ns1=size(splitf,'*')-1;
87     frq=-frq($:-1:1);
88     nsp=size(frq,'*');
89     splitf=[1, (nsp+2)*ones(1,ns1)-splitf($:-1:2)]
90     bnds=[bnds(1),bnds(2),-bnds(4),-bnds(3)];
91     return;
92   elseif fmin >= fmax then
93     error(msprintf(gettext("%s: Wrong value for input arguments #%d and #%d: %s < %s expected.\n"),..
94                    "calfrq",2,3,"fmin","fmax"));
95   end
96
97   // Compute dicretisation over a given range
98   // ----------------------------------------
99
100
101   splitf=[]
102   if fmin==0 then fmin=min(1d-14,fmax/10);end
103   //
104   denh=h('den');numh=h('num')
105   l10=log(10)
106
107   // Locate singularities to avoid them
108   // ----------------------------------
109   if dom=='c' then
110     c=2*%pi;
111     //selection function for singularities in the frequency range
112     deff('f=%sel(r,fmin,fmax,tol)',['f=[],';
113                     'if prod(size(r))==0 then return,end';
114                     'f=imag(r(find((abs(real(r))<=tol*abs(r))&(imag(r)>=0))))';
115                     'if f<>[] then  f=f(find((f>fmin-tol)&(f<fmax+tol)));end']);
116   else
117     c=2*%pi*dom
118     //selection function for singularities in the frequency range
119     deff('[f]=%sel(r,fmin,fmax,dom,tol)',['f=[],';
120                     'if prod(size(r))==0 then return,end';
121                     'f=r(find( ((abs(abs(r)-ones(r)))<=tol)&(imag(r)>=0)))';
122                     'if f<>[] then ';
123                     '  f=atan(imag(f),real(f));nf=prod(size(f))';
124                     '  for k=1:nf ,';
125                     '    kk=int((fmax-f(k))/(2*%pi))+1;';
126                     '    f=[f;f(1:nf)+2*%pi*kk*ones(nf,1)];';
127                     '  end;'
128                     '  f=f(find((f>fmin-tol)&(f<fmax+tol)))';
129                     'end']);
130   end
131
132   sing=[];zers=[];
133   fmin=c*fmin,fmax=c*fmax;
134
135   for i=1:m
136     sing=[sing;%sel(roots(denh(i)),fmin,fmax,tol)];
137   end
138
139   pp=sort(sing');npp=size(pp,'*');//'
140
141   // singularities just on the left of the range
142   kinf=find(pp<fmin)
143   if kinf<>[] then
144     fmin=fmin+tol
145     pp(kinf)=[]
146   end
147
148   // singularities just on the right of the range
149   ksup=find(pp>=fmax)
150   if ksup<>[] then
151     fmax=fmax-tol
152     pp(ksup)=[]
153   end
154
155   //check for nearly multiple singularities
156   if pp<>[] then
157     dpp=pp(2:$)-pp(1:$-1)
158     keq=find(abs(dpp)<2*epss)
159     if keq<>[] then pp(keq)=[],end
160   end
161
162   if pp<>[] then
163     frqs=[fmin real(matrix([(1-epss)*pp;(1+epss)*pp],2*size(pp,'*'),1)') fmax]
164     //'
165   else
166     frqs=[fmin fmax]
167   end
168   nfrq=size(frqs,'*');
169
170   // Evaluate bounds of nyquist plot
171   //-------------------------------
172
173   xt=[];Pas=[]
174   for i=1:2:nfrq-1
175     w=logspace(log(frqs(i))/log(10),log(frqs(i+1))/log(10),100);
176     xt=[xt,w]
177     Pas=[Pas w(2)-w(1)]
178   end
179   if dom=='c' then 
180     rf=freq(h('num'),h('den'),%i*xt);
181   else
182     rf=freq(h('num'),h('den'),exp(%i*xt));
183   end
184   //
185   xmin=mini(real(rf));xmax=maxi(real(rf));
186   ymin=mini(imag(rf));ymax=maxi(imag(rf));
187   bnds=[xmin xmax ymin ymax];
188   dx=max([xmax-xmin,1]);dy=max([ymax-ymin,1]);
189
190   // Compute discretization with a step adaptation method
191   // ----------------------------------------------------
192   frq=[];
193   i=1;
194   nptr=nptmax; // number of unused discretization points
195   l10last=log10(frqs($));
196   while i<nfrq
197     f0=frqs(i);fmax=frqs(i+1);
198     while f0==fmax do
199       i=i+2;
200       f=frqs(i);fmax=frqs(i+1);
201     end
202     frq=[frq,f0];
203     pas=Pas(floor(i/2)+1)
204     splitf=[splitf size(frq,'*')];
205
206     f=mini(f0+pas,fmax);
207
208     if dom=='c' then //cas continu
209       while f0<fmax
210         rf0=freq(h('num'),h('den'),(%i*f0));
211         rfc=freq(h('num'),h('den'),%i*f);
212         // compute prediction error
213         epsd=pas/100;//epsd=1.d-8
214         
215         rfd=(freq(h('num'),h('den'),%i*(f0+epsd))-rf0)/(epsd);
216         rfp=rf0+pas*rfd;
217
218         e=maxi([abs(imag(rfp-rfc))/dy;abs(real(rfp-rfc))/dx])
219         if (e>k) then rf0=freq(h('num'),h('den'),(%i*f0));
220           rfc=freq(h('num'),h('den'),%i*f);
221           // compute prediction error
222           epsd=pas/100;//epsd=1.d-8
223
224           rfd=(freq(h('num'),h('den'),%i*(f0+epsd))-rf0)/(epsd);
225           rfp=rf0+pas*rfd;
226
227           e=maxi([abs(imag(rfp-rfc))/dy;abs(real(rfp-rfc))/dx])
228           // compute minimum frequency logarithmic step to ensure a maximum 
229           //of nptmax points to discretize
230           pasmin=f0*(10^((l10last-log10(f0))/(nptr+1))-1)
231           pas=pas/2
232           if pas<pasmin then
233             pas=pasmin
234             frq=[frq,f];nptr=max([1,nptr-1])
235             f0=f;f=mini(f0+pas,fmax)
236           else
237             f=mini(f0+pas,fmax)
238           end
239         elseif e<k/2 then
240           pas=2*pas
241           frq=[frq,f];nptr=max([1,nptr-1])
242           f0=f;f=mini(f0+pas,fmax),
243         else
244           frq=[frq,f];nptr=max([1,nptr-1])
245           f0=f;f=mini(f0+pas,fmax),
246         end
247       end
248     else  //cas discret
249       pas=pas/dom
250       while f0<fmax
251         rf0=freq(h('num'),h('den'),exp(%i*f0))
252         rfd=dom*(freq(h('num'),h('den'),exp(%i*(f0+pas/100)))-rf0)/(pas/100);
253         rfp=rf0+pas*rfd
254         rfc=freq(h('num'),h('den'),exp(%i*f));
255         e=maxi([abs(imag(rfp-rfc))/dy;abs(real(rfp-rfc))/dx])
256         if (e>k) then
257           pasmin=f0*(10^((l10last-log10(f0))/(nptr+1))-1)
258           pas=pas/2
259           if pas<pasmin then
260             pas=pasmin
261             frq=[frq,f];nptr=max([1,nptr-1])
262             f0=f;f=mini(f0+pas,fmax)
263           else
264             f=mini(f0+pas,fmax)
265           end
266         elseif e<k/2 then
267           pas=2*pas
268           frq=[frq,f];nptr=max([1,nptr-1])
269           f0=f;f=mini(f0+pas,fmax),
270         else
271           frq=[frq,f];nptr=max([1,nptr-1])
272           f0=f;f=mini(f0+pas,fmax),
273         end
274       end
275     end
276     i=i+2
277   end
278   frq( size(frq,'*') )=fmax
279   frq=frq/c;
280 endfunction