diff options
Diffstat (limited to 'modules/cacsd/macros/lqr.sci')
| -rwxr-xr-x | modules/cacsd/macros/lqr.sci | 108 |
1 files changed, 108 insertions, 0 deletions
diff --git a/modules/cacsd/macros/lqr.sci b/modules/cacsd/macros/lqr.sci new file mode 100755 index 000000000..2d48ca8e4 --- /dev/null +++ b/modules/cacsd/macros/lqr.sci @@ -0,0 +1,108 @@ +// 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]=lqr(P12) + //lqr gain for full-state LQ problem + //(discrete or continuous) + // discrete continuous + // |I 0 0| | A 0 B | |I 0 0| | A 0 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| + if typeof(P12)<>"state-space" then + error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"lqr",1)) + end + + [A,B2,C1,D12]=P12(2:5); + Q=C1'*C1;R=D12'*D12;S=D12'*C1; + [n,nu]=size(B2); + [ny,n]=size(C1); + if P12(7) == [] then + error(msprintf(gettext("%s: Wrong value for input argument #%d: Time domain must be ''c'' or ''d''.\n"),"lqr",1)) + elseif P12(7) == "c" + Z=0*A;I=eye(A);O=zeros(n,nu); + bigE=[I,Z,O; ... + Z,I,O; ... + zeros(nu,2*n+nu)]; + + bigA=[A ,Z, B2; ... + -Q ,-A',-S'; ... + S ,B2', R]; + Ri=inv(R); + Left=[I, Z, -B2*Ri; + Z, I, S'*Ri; + zeros(nu,2*n), Ri]; + LA=Left*bigA;LE=Left*bigE;N=1:2*n; + //[wsmall,ks1]=schur(LA(N,N),LE(N,N),'c'); + [wsmall,ks1]=schur(LA(N,N),"c"); + if ks1<>n then + error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr")); + end + X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12; + if rcond(X12)< 1.d-5 then + warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr")); + end + K=-Ri*(B2'*X+S); + return; + ////////////////////////////// + // Other implementation ... // + ////////////////////////////// + //[Q,Z,Qd,Zd,numbeps,numbeta]=kroneck(bigE,bigA); + //[w,ks]=schur(bigA,bigE,'c'); + //if ks<>n then error('lqr: stable subspace too small!');end + //ws=w(:,1:n); + //X12=ws(1:n,:); + //phi12=ws(n+1:2*n,:); + //u12=ws(2*n+1:2*n+nu,:); + //if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end + //K=u12/X12; + //X=phi12/X12; + //return + elseif P12(7) == "d" | type(P12(7))==1 + I=eye(A);Z=0*I; + Q=C1'*C1;R=D12'*D12;S=D12'*C1; + bigE=[I,Z,0*B2; ... + Z,A',0*B2; ... + 0*B2',-B2',0*B2'*B2]; + + bigA=[A,Z, B2; ... + -Q ,I, -S'; ... + S, 0*B2', R]; + + Ri=inv(R); + + Left=[I, Z, -B2*Ri; ... + Z, I, S'*Ri; ... + zeros(nu,2*n), Ri]; + LA=Left*bigA;LE=Left*bigE;N=1:2*n; + [wsmall,ks1]=schur(LA(N,N),LE(N,N),"d"); + if ks1<>n then + error(msprintf(gettext("%s: Stable subspace is too small.\n"),"lqr")); + end + X12=wsmall(1:n,1:n);phi12=wsmall(n+1:$,1:n);X=phi12/X12; + if rcond(X12)< 1.d-5 then + warning(msprintf(gettext("%s: Bad conditionning.\n"),"lqr")); + end + K=-pinv(B2'*X*B2+R)*(B2'*X*A+S); + return + //////////////////// + // Other form ... // + //////////////////// + //[w,ks]=schur(bigA,bigE,'d'); + //if ks<>n then error('lqr: stable subspace too small!');end + //ws=w(:,1:n); + //X12=ws(1:n,:); + //phi12=ws(n+1:2*n,:); + //u12=ws(2*n+1:2*n+nu,:); + //if rcond(X12)< 1.d-5 then warning('lqr: bad conditionning!');end + //K=u12/X12; + //X=phi12/X12; + //return + end + +endfunction |