bug 4799 fix
[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   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=mini(real(rf));xmax=maxi(real(rf));
182   ymin=mini(imag(rf));ymax=maxi(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=mini(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=maxi([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=maxi([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=mini(f0+pas,fmax)
232           else
233             f=mini(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=mini(f0+pas,fmax),
239         else
240           frq=[frq,f];nptr=max([1,nptr-1])
241           f0=f;f=mini(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=maxi([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=mini(f0+pas,fmax)
259           else
260             f=mini(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=mini(f0+pas,fmax),
266         else
267           frq=[frq,f];nptr=max([1,nptr-1])
268           f0=f;f=mini(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