1 files changed, 76 insertions, 0 deletions
diff --git a/modules/cacsd/macros/gfrancis.sci b/modules/cacsd/macros/gfrancis.sci
new file mode 100755
index 000000000..d8b916306
--- /dev/null
+++ b/modules/cacsd/macros/gfrancis.sci
@@ -0,0 +1,76 @@
+// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
+// Copyright (C) INRIA -
+//
+// This file must be used under the terms of the CeCILL.
+// This source file is licensed as described in the file COPYING, which
+// you should have received as part of this distribution.  The terms
+// are also available at
+// http://www.cecill.info/licences/Licence_CeCILL_V2.1-en.txt
+
+function [L,M,T]= gfrancis(Plant,Model);
+    // This function computes an optimal model matching
+    // controller for the linear plant
+    //    x'= F*x + G*u
+    //    y = H*x + J*u
+    // and the linear model
+    //    xm'= A*xm + B*um
+    //    ym = C*xm + D*um
+    // The dimensions of x,u,y are n,m,p and those of xm,um,ym are
+    // nn,mm,pp and pp=p.
+    // The goal is for the plant to track the model
+    //    e = y - ym ---> 0
+    // while keeping stable the state x(t) of the plant. To accomplish
+    // this, we use feedforward and feedback
+    //    u = L*xm + M*um + K*(x-T*xm) = [K , L-K*T] *(x,xm) + M*um
+    // to drive the combined system to the closed loop invariant subspace
+    //    x = T*xm
+    // where e = 0.
+    // The matrices T,L,M satisfy generalized Francis equations
+    //    F*T + G*L = T*A
+    //    H*T + J*L = C
+    //          G*M = T*B
+    //          J*M = D
+    // The matrix K is chosen as stabilizing the pair (F,G) i.e
+    // F+G*K is stable.
+    // For more information on this approach, see
+    // Krener, A. J., Optimal model matching controllers for linear
+    // and nonlinear systems, Proceedings of NOLCOS, Bordeaux, 1992.
+    if typeof(Plant)<>"state-space" then
+        error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"gfrancis",1))
+    end
+    if Plant.dt<>"c" then
+        error(msprintf(gettext("%s: Wrong value for input argument #%d: Continuous time system expected.\n"),"gfrancis",1))
+    end
+    if typeof(Model)<>"state-space" then
+        error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"gfrancis",2))
+    end
+    if Model.dt<>"c" then
+        error(msprintf(gettext("%s: Wrong value for input argument #%d: Continuous time system expected.\n"),"gfrancis",2))
+    end
+
+    [F,G,H,J]=abcd(Plant);
+    [A,B,C,D]=abcd(Model);
+    [nf,nf]=size(F);[ny,nu]=size(J);
+    [na,na]=size(A);[lc,num]=size(D);
+    Ia=eye(na,na);Inf=eye(nf,nf);Iu=eye(num,num);
+    Mat=[Ia.*.F-A'.*.Inf, Ia.*.G, zeros(nf*na,nu*num);
+    Ia.*.H , Ia.*.J, zeros(ny*na,nu*num);
+    -B'.*.Inf, zeros(nf*num,nu*na), Iu.*.G;
+    zeros(ny*num,nf*na),zeros(ny*num,nu*na),Iu.*.J];
+
+    rhs=[zeros(nf*na,1);
+    matrix(C,size(C,"*"),1);
+    zeros(nf*num,1);
+    matrix(D,size(D,"*"),1)];
+    TLM=pinv(Mat)*rhs;
+    T=TLM(1:nf*na);T=matrix(T,nf,na);
+    L=TLM(nf*na+1:nf*na+nu*na);L=matrix(L,nu,na);
+    M=TLM(nf*na+nu*na+1:nf*na+nu*na+nu*num);M=matrix(M,nu,num);
+    Wplant=[F,G;H,J];
+    Wmodel=[A,B;C,D];
+    //check
+    err=norm(Wplant*[T,zeros(nf,num);
+    L,M]-[T,zeros(nf,lc);
+    zeros(lc,na),eye(lc,lc)]*Wmodel,1);
+    if err > 1.d-5 then warning(msprintf(gettext("%s: Francis equations not satisfied.\n"),"gfrancis"));end
+endfunction