fd9f25588c56ae1a586bfe322130dbb4d5d96c9d
[scilab.git] / scilab / modules / cacsd / macros / lqr.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA - 
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 [K,X]=lqr(P12)
11   //lqr gain for full-state LQ problem
12   //(discrete or continuous)
13   //          discrete                        continuous
14   //      |I   0   0|   | A    0   B  |      |I   0   0|   | A    0    B  |
15   //     z|0   A'  0| - |-C'C  I   -S'|    s |0   I   0| - |-C'C -A'  -S' |
16   //      |0   B'  0|   | S    0   D'D|      |0   0   0|   | S   -B'   D'D|
17   if typeof(P12)<>'state-space' then
18      error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"lqr",1))
19   end
20
21   [A,B2,C1,D12]=P12(2:5);
22   Q=C1'*C1;R=D12'*D12;S=D12'*C1;
23   [n,nu]=size(B2);
24   [ny,n]=size(C1);
25   select P12(7)
26   case [] then
27     error(msprintf(gettext("%s: Wrong value for input argument #%d: Time domain must be ''c'' or ''d''.\n"),"lqr",1))
28   case 'c' then
29     Z=0*A;I=eye(A);O=zeros(n,nu);
30     bigE=[I,Z,O; ...
31           Z,I,O; ...
32           zeros(nu,2*n+nu)];
33
34     bigA=[A ,Z,   B2; ...
35          -Q ,-A',-S'; ...
36           S ,B2', R];
37     Ri=inv(R);
38     Left=[I,  Z,  -B2*Ri;
39           Z,  I,  S'*Ri;
40           zeros(nu,2*n), Ri];
41     LA=Left*bigA;LE=Left*bigE;N=1:2*n;
42     //[wsmall,ks1]=schur(LA(N,N),LE(N,N),'c');
43     [wsmall,ks1]=schur(LA(N,N),'c');
44     if ks1<>n then 
45       error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr"));
46     end
47     X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12;
48     if rcond(X12)< 1.d-5 then 
49       warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr"));
50     end
51     K=-Ri*(B2'*X+S);
52     return;
53     //////////////////////////////
54     // Other implementation ... //
55     //////////////////////////////
56     //[Q,Z,Qd,Zd,numbeps,numbeta]=kroneck(bigE,bigA);
57     //[w,ks]=schur(bigA,bigE,'c');
58     //if ks<>n then error('lqr: stable subspace too small!');end
59     //ws=w(:,1:n);
60     //X12=ws(1:n,:);
61     //phi12=ws(n+1:2*n,:);
62     //u12=ws(2*n+1:2*n+nu,:);
63     //if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end
64     //K=u12/X12;
65     //X=phi12/X12;
66     //return
67   case 'd' then
68     I=eye(A);Z=0*I;
69     Q=C1'*C1;R=D12'*D12;S=D12'*C1;
70     bigE=[I,Z,0*B2; ...
71           Z,A',0*B2; ...
72           0*B2',-B2',0*B2'*B2];
73
74     bigA=[A,Z, B2; ...
75          -Q ,I, -S'; ...
76           S, 0*B2', R];
77
78     Ri=inv(R);
79
80     Left=[I,  Z,  -B2*Ri; ...
81           Z,  I,  S'*Ri; ...
82           zeros(nu,2*n), Ri];
83     LA=Left*bigA;LE=Left*bigE;N=1:2*n;
84     [wsmall,ks1]=schur(LA(N,N),LE(N,N),'d');
85     if ks1<>n then 
86       error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr"));
87     end
88     X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12;
89     if rcond(X12)< 1.d-5 then 
90       warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr"));
91     end
92     K=-pinv(B2'*X*B2+R)*(B2'*X*A+S);
93     return
94     ////////////////////
95     // Other form ... //
96     ////////////////////
97     //[w,ks]=schur(bigA,bigE,'d');
98     //if ks<>n then error('lqr: stable subspace too small!');end
99     //ws=w(:,1:n);
100     //X12=ws(1:n,:);
101     //phi12=ws(n+1:2*n,:);
102     //u12=ws(2*n+1:2*n+nu,:);
103     //if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end
104     //K=u12/X12;
105     //X=phi12/X12;
106     //return
107   end
108 endfunction