bug 14192 fix + 0 divide bug fix
[scilab.git] / scilab / modules / cacsd / macros / g_margin.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) ???? - 2016 - INRIA -  Serge Steer
3 //
4 // This file is hereby licensed under the terms of the GNU GPL v2.0,
5 // pursuant to article 5.3.4 of the CeCILL v.2.1.
6 // This file was originally licensed under the terms of the CeCILL v2.1,
7 // and continues to be available under such terms.
8 // For more information, see the COPYING file which you should have received
9 // along with this program.
10
11 function [gm,fr] = g_margin(h)
12 // Compute the gain margin of a SISO transfer function
13
14   if argn(2) < 1 then
15     error(msprintf(_("%s: Wrong number of input argument(s): %d expected.\n"),"g_margin",1));
16   end
17   if and(typeof(h)<>["state-space","rational"]) then
18     error(msprintf(gettext("%s: Wrong type for input argument: Linear dynamical system expected.\n"),"g_margin",1))
19   end
20   if size(h,"*")<>1 then
21     error(msprintf(gettext("%s: Wrong size for input argument #%d: Single input, single output system expected.\n"),"g_margin",1))
22   end
23
24   if typeof(h)=="state-space" then
25     h=ss2tf(h)
26   end
27
28
29     ieee_save=ieee();ieee(2);
30     //
31     epsr=1.e-7;//used for testing if complex numbers are real
32     eps1=1.e-7;//used for testing if complex numbers have a modulus near 1
33     epssing=1e-10; //used for testing if arguments are not singular points of h
34     if h.dt=="c" then  //continuous time case
35         // get s such as h(s)=h(-s) and s=iw
36         s=%i*poly(0,"w");
37         //compute h(s)-h(-s)=num/den
38         num=imag(horner(h.num,s)*conj(horner(h.den,s)))
39         den=real(horner(h.den,s)*conj(horner(h.den,s)))
40         //necessary condition
41         w=roots(num,"e");
42         ws=[];
43         if w<>[] then
44           ws=real(w(abs(imag(w))<epsr&real(w)<=0)) //points where phase is -180°
45         end
46         if ws<>[] then
47           //remove nearly singular points
48           ws(abs(horner(num,ws))>=epssing*abs(horner(den,ws)))=[]
49         end
50         if ws==[] then gm=%inf,fr=[],return,end
51         mingain=real(freq(h.num,h.den,%i*ws))
52     else  //discrete time case
53         if h.dt=="d" then dt=1,else dt=h.dt,end
54         //get z such as h(z)=h(1/z) and z=e^(%i*w*dt)
55         //form hh=h(z)-h(1/z)
56         z=poly(0,varn(h.den));
57         sm=simp_mode();simp_mode(%f);hh=h-horner(h,1/z);simp_mode(sm)
58         //find the numerator roots
59         z=roots(hh.num,"e");
60         if z<>[] then
61           z(abs(abs(z)-1)>eps1)=[]// retain only roots with modulus equal to 1
62         end
63         if z<>[] then
64           //remove nearly singular points
65           z(abs(horner(hh.num,z))>=epssing*abs(horner(hh.den,z)))=[];
66         end
67         ws=[];
68         if z<>[] then
69           w=log(z)/(%i*dt)
70           ws=real(w(abs(imag(w))<epsr)) //points where phase is -180°
71         end
72         if ws==[] then gm=%inf,fr=[],return,end
73         mingain=real(horner(h,exp(%i*ws*dt)))
74         ieee(ieee_save)
75     end
76
77     k=find(mingain<0)
78     if k==[] then gm=%inf,fr=[],return,end
79     mingain=abs(mingain(k));
80     ws=abs(ws(k))// select positive frequency
81
82     gm=-20*log(mingain)/log(10) //tranform into Db
83     [gm,k]=min(gm);ws=ws(k);//select the minimum
84
85     fr=ws/(2*%pi) //transform in Hz
86     ieee(ieee_save)
87 endfunction