* Bug #4731 fixed - lqr() failed when time domain of input was a number.
[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     if P12(7) == [] then
26         error(msprintf(gettext("%s: Wrong value for input argument #%d: Time domain must be ''c'' or ''d''.\n"),"lqr",1))
27     elseif P12(7) == 'c'
28         Z=0*A;I=eye(A);O=zeros(n,nu);
29         bigE=[I,Z,O; ...
30         Z,I,O; ...
31         zeros(nu,2*n+nu)];
32
33         bigA=[A ,Z,   B2; ...
34         -Q ,-A',-S'; ...
35         S ,B2', R];
36         Ri=inv(R);
37         Left=[I,  Z,  -B2*Ri;
38         Z,  I,  S'*Ri;
39         zeros(nu,2*n), Ri];
40         LA=Left*bigA;LE=Left*bigE;N=1:2*n;
41         //[wsmall,ks1]=schur(LA(N,N),LE(N,N),'c');
42         [wsmall,ks1]=schur(LA(N,N),'c');
43         if ks1<>n then 
44             error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr"));
45         end
46         X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12;
47         if rcond(X12)< 1.d-5 then 
48             warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr"));
49         end
50         K=-Ri*(B2'*X+S);
51         return;
52         //////////////////////////////
53         // Other implementation ... //
54         //////////////////////////////
55         //[Q,Z,Qd,Zd,numbeps,numbeta]=kroneck(bigE,bigA);
56         //[w,ks]=schur(bigA,bigE,'c');
57         //if ks<>n then error('lqr: stable subspace too small!');end
58         //ws=w(:,1:n);
59         //X12=ws(1:n,:);
60         //phi12=ws(n+1:2*n,:);
61         //u12=ws(2*n+1:2*n+nu,:);
62         //if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end
63         //K=u12/X12;
64         //X=phi12/X12;
65         //return
66     elseif P12(7) == 'd' | type(P12(7))==1 
67         I=eye(A);Z=0*I;
68         Q=C1'*C1;R=D12'*D12;S=D12'*C1;
69         bigE=[I,Z,0*B2; ...
70         Z,A',0*B2; ...
71         0*B2',-B2',0*B2'*B2];
72
73         bigA=[A,Z, B2; ...
74         -Q ,I, -S'; ...
75         S, 0*B2', R];
76
77         Ri=inv(R);
78
79         Left=[I,  Z,  -B2*Ri; ...
80         Z,  I,  S'*Ri; ...
81         zeros(nu,2*n), Ri];
82         LA=Left*bigA;LE=Left*bigE;N=1:2*n;
83         [wsmall,ks1]=schur(LA(N,N),LE(N,N),'d');
84         if ks1<>n then 
85             error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr"));
86         end
87         X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12;
88         if rcond(X12)< 1.d-5 then 
89             warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr"));
90         end
91         K=-pinv(B2'*X*B2+R)*(B2'*X*A+S);
92         return
93         ////////////////////
94         // Other form ... //
95         ////////////////////
96         //[w,ks]=schur(bigA,bigE,'d');
97         //if ks<>n then error('lqr: stable subspace too small!');end
98         //ws=w(:,1:n);
99         //X12=ws(1:n,:);
100         //phi12=ws(n+1:2*n,:);
101         //u12=ws(2*n+1:2*n+nu,:);
102         //if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end
103         //K=u12/X12;
104         //X=phi12/X12;
105         //return
106     end
107
108 endfunction