1 files changed, 105 insertions, 0 deletions
diff --git a/modules/cacsd/macros/leqr.sci b/modules/cacsd/macros/leqr.sci
new file mode 100755
index 000000000..e0fe2ce90
--- /dev/null
+++ b/modules/cacsd/macros/leqr.sci
@@ -0,0 +1,105 @@
+// 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 [k,x,err]=leqr(p12,vx)
+    //h-infinity lqr gain for full-state lq problem
+    //(discrete or continuous)
+    //
+    //                discrete                        continuous
+    //   |i  -vx  0|   | a    0     b|       |i   0   0|   | a    vx    b  |
+    //  z|0   a'  0| - |-c'c  i    -s|      s|0   i   0| - |-c'c  -a'  -s  |
+    //   |0   b'  0|   | s'   0   d'd|       |0   0   0|   | s'   -b'   d'd|
+    //
+
+    [lhs,rhs]=argn(0);
+
+    if typeof(p12)<>"state-space" then
+        error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"leqr",1))
+    end
+    [a,b2,c1,d12]=p12(2:5);
+    [n,nu]=size(b2);
+    [ny,n]=size(c1);
+    dom=p12(7);
+    if dom==[] then
+        dom="c";
+        warning(msprintf(gettext("%s: Input argument %d is assumed continuous time.\n"),"leqr",1));
+    end
+    select dom
+        //      continuous
+    case "c" then
+        z=0*a;i=eye(a);
+        q=c1'*c1;r=d12'*d12;s=c1'*d12;
+        bige=[i,z,zeros(b2);
+        z,i,zeros(b2);
+        zeros(nu,2*n+nu)];
+        biga=[a,vx,b2;
+        -q,-a',-s;
+        s',b2',r];
+
+        [bige,biga,dummy,z]=balanc(bige,biga);
+        [w,k]=schur(biga,bige,"c");
+        if k<>n then
+            warning(msprintf(gettext("%s: Stable subspace is too small.\n"),"leqr"));
+            k=[];w=[];err=[];
+            return;
+        end
+
+        ws=z*w(:,1:n);
+        x12=ws(1:n,:);
+        if rcond(x12) < 1.d-6 then
+            warning(msprintf(gettext("%s: Bad conditionning.\n"),"leqr"));
+        end
+        k=ws(2*n+1:2*n+nu,:)/x12;
+        x=ws(n+1:2*n,:)/x12;
+        if lhs~=3 then return;end
+        ri=pinv(r);
+        err=norm((a-b2*ri*s')'*x+x*(a-b2*ri*s')-x*(b2*ri*b2'-vx)*x+q-s*ri*s',1)
+        //k=-ri*(b2'*x+s')
+        //      discrete  time
+    case "d" then
+        i=eye(a);z=0*i;
+        q=c1'*c1;r=d12'*d12;s=c1'*d12;
+        bige=[i,-vx,zeros(b2);
+        z,a',zeros(b2);
+        zeros(b2'),-b2',zeros(b2'*b2)];
+        biga=[a,z,b2;
+        -q,i, -s;
+        s', 0*b2', r];
+        [bige,biga,dummy,z]=balanc(bige,biga);
+        [w,k]=schur(biga,bige,"d");
+        if k<>n then
+            warning(msprintf(gettext("%s: Stable subspace is too small.\n"),"leqr"));
+            k=[];w=[];err=[];
+            return;
+        end
+        ws=z*w(:,1:n);
+        x12=ws(1:n,:);
+        if rcond(x12) <1.d-6 then
+            warning(msprintf(gettext("%s: Bad conditionning.\n"),"leqr"));
+            k=[];w=[];
+            return;
+        end
+
+        k=ws(2*n+1:2*n+nu,:)/x12;
+        x=ws(n+1:2*n,:)/x12;
+        if norm(x-x',1)>0.0001 then
+            warning(msprintf(gettext("%s: %s non symmetric.\n"),"leqr","x"));
+            k=[];w=[];
+            return;
+        end
+
+        //a'*x*a-(a'*x*b2+c1'*d12)*pinv(b2'*x*b2+d12'*d12)*(b2'*x*a+d12'*c1)+c1'*c1
+        if lhs~=3 then return;end
+        ri=pinv(r);
+        abar=a-b2*ri*s';
+        qbar=q-s*ri*s';
+        err=norm(x-(abar'*inv((inv(x)+b2*ri*b2'-vx))*abar+qbar),1);
+        //k=-ri*(b2'*inv(inv(x)+b2*ri*b2'-vx)*abar+s')
+    end
+endfunction