c57dc7aa97a0cb688e54e5c59ad1c3da9dbccea3
[scilab.git] / scilab / modules / cacsd / macros / csim.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) ???? - 2016 - INRIA - Serge Steer
3 //
4 // Copyright (C) 2012 - 2016 - Scilab Enterprises
5 //
6 // This file is hereby licensed under the terms of the GNU GPL v2.0,
7 // pursuant to article 5.3.4 of the CeCILL v.2.1.
8 // This file was originally licensed under the terms of the CeCILL v2.1,
9 // and continues to be available under such terms.
10 // For more information, see the COPYING file which you should have received
11 // along with this program.
12 function [y,x]=csim(u,dt,sl,x0,tol)
13     //Syntax:
14     //  [y [,x]]=csim(u,dt,sl,[x0])
15     // simulation of the controlled linear system sl.
16     // sl is assumed to be a continuous-time system.
17     // u is the control and x0 the initial state.
18     //
19     //u can be:
20     // - a function
21     //    [inputs]=u(t)
22     // - a list
23     //    list(ut,parameter1,....,parametern) such that
24     //    inputs=ut(t,parameter1,....,parametern)
25     // - the character string 'impuls' for impulse response calculation
26     //    (here sl is assumed SISO without direct feedthrough and x0=0)
27     // - the character string 'step' for step response calculation
28     //    (here sl is assumed SISO without direct feedthrough and x0=0)
29     //dt is a vector of instants with dt(1) = initial time
30     //                   that is:           x0=x
31     //                                          dt(1)
32     //
33     //y matrix such that:
34     //  y=[y       y  ...  y     ]
35     //      dt(1)   dt(2)   dt(n)
36     //x matrix such that:
37     //  x=[x       x  ...  x     ]
38     //      dt(1)   dt(2)   dt(n)
39     //
40     //See also:
41     // dsimul flts ltitr rtitr ode impl
42     //!
43
44     [lhs,rhs]=argn(0)
45     //
46     if rhs<3 then error(39),end
47     sltyp=typeof(sl)
48     if and(sltyp<>["state-space" "rational" "zpk"]) then
49         args=["u","dt","sl","x0","tol"];
50         ierr=execstr("[y,x]=%"+overloadname(sl)+"_csim("+strcat(args(1:rhs),",")+")","errcatch")
51         if ierr<>0 then
52             error(msprintf(_("%s: Wrong type for input argument #%d: Linear dynamical system expected.\n"),"csim",3))
53         end
54         return
55     end
56     if sltyp=="rational" then
57         sl=tf2ss(sl)
58     elseif sltyp=="zpk" then
59         sl=zpk2ss(sl),
60     end
61     if sl.dt<>"c" then
62         warning(msprintf(gettext("%s: Input argument #%d is assumed continuous time.\n"),"csim",1));
63     end
64     //
65     [a,b,c,d]=abcd(sl);
66     if degree(d)>0 then
67         error(msprintf(gettext("%s: Wrong type for input argument #%d: A proper system expected\n"),"csim",1));
68     end
69     //[ma,mb]=size(b) replaced by  ma=size(a,1);mb=size(d,2);in case of no-state system
70     ma=size(a,1);
71     mb=size(d,2);
72     //
73     imp=0;step=0
74     text="if t==0 then y=0, else y=1,end"
75     //
76     select type(u)
77     case 10 then //input given by its type (step or impuls)
78         if mb<>1 then
79             error(msprintf(gettext("%s: Wrong type for input argument #%d: A SIMO expected.\n"),"csim",1));
80         end;
81         if part(u,1)=="i" then
82             //impulse response
83             imp=1;
84             dt(dt==0)=%eps^2;
85         elseif part(u,1)=="s" then
86             step=1
87             if norm(d,1)<>0 then
88                 dt(dt==0)=%eps^2;
89             end;
90         else
91             error(msprintf(gettext("%s: Wrong value for input argument #%d: Must be in the set {%s}.\n"),"csim",1,"""step"",""impuls"""))
92         end;
93         deff("[y]=u(t)",text);
94     case 11 then //input given by a function of time
95         comp(u)
96     case 13 then //input given by a function of time
97     case 1 then //input given by a vector of data
98         [mbu,ntu]=size(u);
99         if mbu<>mb | ntu<>size(dt,"*") then
100             error(msprintf(gettext("%s: Incompatible input arguments #%d and #%d: Same column dimensions expected.\n"),"csim",1,2))
101         end
102     case 15 then  //input given by a list: function of time with parameters
103         uu=u(1),
104         if type(uu)==11 then
105             comp(uu),
106             u(1)=uu,
107         end
108     else error(msprintf(gettext("%s: Wrong type for input argument #%d: Function expected"), "csim", 2));
109     end;
110     //
111     if isempty(dt) then
112         y = [];
113         x = [];
114         return
115     end
116     if rhs==3 then x0=sl(6),end
117     if imp==1|step==1 then x0=0*x0,end
118     nt=size(dt,"*");x=0*ones(ma,nt);
119     [a,v]=balanc(a);
120     v1=v;//save for backward transformation
121
122     //apply transformation u without matrix inversion
123     [k,l]=find(v<>0) //get the permutation
124
125     //apply right transformation
126     v=v(k,l);//diagonal matrix
127     c=c(:,k)*v;
128     //apply left transformation
129     v=diag(1 ./diag(v));b=v*b(k,:);x0=v*x0(k)
130
131     [a,v2,bs]=bdiag(a,1);b=v2\b;c=c*v2;x0=v2\x0;
132     //form the differential equation function
133     if type(u)==1 then
134         //form a continuous time interpolation of the given data
135         ut=u;
136         if min(size(ut))==1 then ut=matrix(ut,1,-1),end
137         deff("[y]=u(t)",["ind=find(dt<=t);nn=ind($)"
138         "if isempty(nn) then"
139         "y=[]"
140         "elseif (t==dt(nn)|nn==nt) then "
141         "   y=ut(:,nn)"
142         "else "
143         "   y=ut(:,nn)+(t-dt(nn))/(dt(nn+1)-dt(nn))*(ut(:,nn+1)-ut(:,nn))"
144         "end"]);
145         deff("[ydot]=%sim2(%tt,%y)","ydot = [];if ~isempty(u(%tt)) then ydot = ak*%y+bk*u(%tt);end");
146     elseif type(u)<>15 then
147         deff("[ydot]=%sim2(%tt,%y)","ydot=ak*%y+bk*u(%tt)");
148         ut=ones(mb,nt);for k=1:nt, ut(:,k)=u(dt(k)),end
149     else
150         %sim2=u
151         tx=" ";for l=2:size(u), tx=tx+",%"+string(l-1);end;
152         deff("[ydot]=sk(%tt,%y,u"+tx+")","ydot=ak*%y+bk*u(%tt"+tx+")");
153         %sim2(0)=sk;u=u(1)
154         deff("[ut]=uu(t)",...
155         ["["+part(tx,3:length(tx))+"]=%sim2(3:"+string(size(%sim2))+")";
156         "ut=ones(mb,nt);for k=1:nt, ut(:,k)=u(t(k)"+tx+"),end"])
157         ut=uu(dt);
158     end;
159
160     //simulation
161     k=1;
162     for n=bs',
163         kk=k:k+n-1;
164         ak=a(kk,kk);
165         bk=b(kk,:);
166         nrmu=max([norm(bk*ut,1),norm(x0(kk))]);
167         if nrmu > 0 then
168             if rhs<5 then
169                 atol=1.d-12*nrmu;rtol=atol/100;
170             else
171                 atol=tol(1);rtol=tol(2);
172             end
173             xkk=ode("adams",x0(kk),0,dt,rtol,atol,%sim2);
174             if size(xkk,2)<>size(x,2) then
175                 error(msprintf(_("%s: Simulation failed before final time is reached.\n"),"csim"))
176             end
177             x(kk,:)=xkk;
178             if imp==1 then x(kk,:)=ak*x(kk,:)+bk*ut,end
179         end;
180         k=k+n
181     end;
182     y = c*x + d*ut
183     if lhs==2 then x=v1*v2*x,end
184 endfunction