function definition and help file made coherent + unit test added
[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)),fmin,fmax,tol)];
134   end
135
136   pp=sort(sing');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
170   xt=[];Pas=[]
171   for i=1:2:nfrq-1
172     w=logspace(log(frqs(i))/log(10),log(frqs(i+1))/log(10),100);
173     xt=[xt,w]
174     Pas=[Pas w(2)-w(1)]
175   end
176   if dom=='c' then 
177     rf=freq(h('num'),h('den'),%i*xt);
178   else
179     rf=freq(h('num'),h('den'),exp(%i*xt));
180   end
181   //
182   xmin=mini(real(rf));xmax=maxi(real(rf));
183   ymin=mini(imag(rf));ymax=maxi(imag(rf));
184   bnds=[xmin xmax ymin ymax];
185   dx=max([xmax-xmin,1]);dy=max([ymax-ymin,1]);
186
187   // Compute discretization with a step adaptation method
188   // ----------------------------------------------------
189   frq=[];
190   i=1;
191   nptr=nptmax; // number of unused discretization points
192   l10last=log10(frqs($));
193   while i<nfrq
194     f0=frqs(i);fmax=frqs(i+1);
195     while f0==fmax do
196       i=i+2;
197       f=frqs(i);fmax=frqs(i+1);
198     end
199     frq=[frq,f0];
200     pas=Pas(floor(i/2)+1)
201     splitf=[splitf size(frq,'*')];
202
203     f=mini(f0+pas,fmax);
204
205     if dom=='c' then //cas continu
206       while f0<fmax
207         rf0=freq(h('num'),h('den'),(%i*f0));
208         rfc=freq(h('num'),h('den'),%i*f);
209         // compute prediction error
210         epsd=pas/100;//epsd=1.d-8
211         
212         rfd=(freq(h('num'),h('den'),%i*(f0+epsd))-rf0)/(epsd);
213         rfp=rf0+pas*rfd;
214
215         e=maxi([abs(imag(rfp-rfc))/dy;abs(real(rfp-rfc))/dx])
216         if (e>k) then rf0=freq(h('num'),h('den'),(%i*f0));
217           rfc=freq(h('num'),h('den'),%i*f);
218           // compute prediction error
219           epsd=pas/100;//epsd=1.d-8
220
221           rfd=(freq(h('num'),h('den'),%i*(f0+epsd))-rf0)/(epsd);
222           rfp=rf0+pas*rfd;
223
224           e=maxi([abs(imag(rfp-rfc))/dy;abs(real(rfp-rfc))/dx])
225           // compute minimum frequency logarithmic step to ensure a maximum 
226           //of nptmax points to discretize
227           pasmin=f0*(10^((l10last-log10(f0))/(nptr+1))-1)
228           pas=pas/2
229           if pas<pasmin then
230             pas=pasmin
231             frq=[frq,f];nptr=max([1,nptr-1])
232             f0=f;f=mini(f0+pas,fmax)
233           else
234             f=mini(f0+pas,fmax)
235           end
236         elseif e<k/2 then
237           pas=2*pas
238           frq=[frq,f];nptr=max([1,nptr-1])
239           f0=f;f=mini(f0+pas,fmax),
240         else
241           frq=[frq,f];nptr=max([1,nptr-1])
242           f0=f;f=mini(f0+pas,fmax),
243         end
244       end
245     else  //cas discret
246       pas=pas/dom
247       while f0<fmax
248         rf0=freq(h('num'),h('den'),exp(%i*f0))
249         rfd=dom*(freq(h('num'),h('den'),exp(%i*(f0+pas/100)))-rf0)/(pas/100);
250         rfp=rf0+pas*rfd
251         rfc=freq(h('num'),h('den'),exp(%i*f));
252         e=maxi([abs(imag(rfp-rfc))/dy;abs(real(rfp-rfc))/dx])
253         if (e>k) then
254           pasmin=f0*(10^((l10last-log10(f0))/(nptr+1))-1)
255           pas=pas/2
256           if pas<pasmin then
257             pas=pasmin
258             frq=[frq,f];nptr=max([1,nptr-1])
259             f0=f;f=mini(f0+pas,fmax)
260           else
261             f=mini(f0+pas,fmax)
262           end
263         elseif e<k/2 then
264           pas=2*pas
265           frq=[frq,f];nptr=max([1,nptr-1])
266           f0=f;f=mini(f0+pas,fmax),
267         else
268           frq=[frq,f];nptr=max([1,nptr-1])
269           f0=f;f=mini(f0+pas,fmax),
270         end
271       end
272     end
273     i=i+2
274   end
275   frq( size(frq,'*') )=fmax
276   frq=frq/c;
277 endfunction