bug 6744 fix
[scilab.git] / scilab / modules / cacsd / macros / p_margin.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) 2010 - INRIA - Serge Steer
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 function [phm,fr]=p_margin(h)
11   //compute the phase margin of a SISO transfer function
12   select typeof(h)
13   case 'rational' then
14   case 'state-space' then
15     h=ss2tf(h);
16   else
17     error(97,1)
18   end
19   if or(size(h)<>[1 1]) then
20    error(msprintf(_("%s: Wrong size for input argument #%d: Single input, single output system expected.\n"),"p_margin",1))
21   end
22   eps=1.e-7;// threshold used for testing if complex numbers are real or pure imaginary
23
24   if h.dt=='c' then  //continuous time case
25     w=poly(0,'w');
26     niw=horner(h.num,%i*w);
27     diw=horner(h.den,%i*w);
28     // |n(iw)/d(iw)|=1 <-- (n(iw)*n(-iw))/(d(iw)*d(-iw))=1 <--  (n(iw)*n(-iw)) - (d(iw)*d(-iw))=0
29     w=roots(real(niw*conj(niw)-diw*conj(diw)));
30     //select positive real roots
31     ws=w(find((abs(imag(w))<eps)&(real(w)>0))); //frequency points with unitary modulus
32     if ws==[] then 
33       phm=[];
34       fr=[];
35       return
36     end
37     ws=min(real(ws)); //keep only the first point where the open-loop phase first crosses 180°
38     f=horner(h,%i*ws);
39   else  //discrete time case
40     if h.dt=='d' then 
41       dt=1;
42     else 
43       dt=h.dt;
44     end
45     // |h(e^(i*w*dt))|=1 <-- h(e^(i*w*dt))*h(e^(-i*w*dt))
46     z=poly(0,varn(h.den));
47     sm=simp_mode();
48     simp_mode(%f);
49     hh=h*horner(h,1/z)-1;
50     simp_mode(sm);
51     //find the numerator roots
52     z=roots(hh.num);
53     z(abs(abs(z)-1)>eps)=[];// retain only roots with modulus equal to 1
54     w=log(z)/(%i*dt);
55     ws=real(w(abs(imag(w))<eps&real(w)>0)); //frequency points with unitary modulus
56     if ws==[] then 
57       phm=%inf;
58       fr=[];
59       return
60     end
61     ws=min(real(ws)); //keep only the first point where the open-loop phase first crosses 180°
62     f=horner(h,exp(%i*ws*dt));
63   end
64   phm=atand(imag(f),real(f));// phase of the frequency response in [-180 180]
65   phm=pmodulo(phm,360)-180;
66   fr=real(ws)/(2*%pi);
67 endfunction