diff options
Diffstat (limited to 'modules/cacsd/macros/ddp.sci')
| -rwxr-xr-x | modules/cacsd/macros/ddp.sci | 96 |
1 files changed, 96 insertions, 0 deletions
diff --git a/modules/cacsd/macros/ddp.sci b/modules/cacsd/macros/ddp.sci new file mode 100755 index 000000000..3e8f7de4b --- /dev/null +++ b/modules/cacsd/macros/ddp.sci @@ -0,0 +1,96 @@ +// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab +// Copyright (C) INRIA - F. Delebecque +// +// 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 [Closed,F,G]=ddp(Sys,zeroed,B1,D1,flag,Alfa,Beta) + //--------------Exact disturbance decoupling---------- + // Given a linear system, and a subset of outputs, z, which are to + // be zeroed, characterize the inputs w of Sys such that the + // transfer function from w to z is zero. + // + // Sys = linear system {A,B2,C,D2} with one input and two outputs + // i.e Sys: u-->(z,y) in the following + // + // xdot = A x + B1 w + B2 u + // z = C1 x + D11 w + D12 u + // y = C2 x + D21 w + D22 u + // + // outputs of Sys are partitioned into (z,y) where z is to be zeroed, + // i.e. the matrices C and D2 are: + // + // C=[C1;C2] D2=[D12;D22] + // C1=C(zeroed,:) D12=D1(zeroed,:) + // + // The control is u=Fx+Gw and one + // looks for F,G such that the closed loop system: w-->z given by + // + // xdot= (A+B2*F) x + (B1 + B2*G) w + // z = (C1+D12F) x + (D11+D12*G) w + // + // has zero transfer transfer function. + // + // flag='ge' : no stability constraints + // ='st' : look for stable closed loop system (A+B2*F stable) + // ='pp' : eigenvalues of A+B2*F are assigned to Alfa and Beta + // + // Closed = w-->y closed loop system + // + // xdot= (A+B2*F) x + (B1 + B2*G) w + // y = (C2+D22*F) x + (D21+D22*G) w + // + // Stability (resp. pole placement) requires stabilizability + // (resp. controllability) of (A,B2). + // + [LHS,RHS]=argn(0); + if RHS==5 then Beta=-1;end + if RHS==4 then Beta=-1;Alfa=-1;end + if RHS==3 then Beta=-1;Alfa=-1;flag="st";end + if RHS==2 then Beta=-1;Alfa=-1;flag="st";D1=zeros(size(Sys("C"),1),size(B1,2)); + end + if size(B1,1) ~= size(Sys("A"),1) then + error(msprintf(gettext("%s: Incompatible input arguments #%d and #%d: Same row dimensions expected.\n"),"ddp",1,3)) + end + if size(D1,2) ~= size(B1,2) then + error(msprintf(gettext("%s: Incompatible input arguments #%d and #%d: Same row dimensions expected.\n"),"ddp",3,4)) + end + Sys1=Sys(zeroed,:); + not_zeroed=1:size(Sys,1);not_zeroed(zeroed)=[]; + [X,dims,F,U,k,Z]=abinv(Sys1,Alfa,Beta,flag);nv=dims(3); + Sys_new=ss2ss(Sys,X);Fnew=F*X; + B1new=X'*B1;B2new=Sys_new("B"); + D11=D1(zeroed,:);D12=Sys1("D"); + B21=B1new(nv+1:$,:);B22=B2new(nv+1:$,:); + // G s.t. B21+B22*G=0 D11+D12*G=0 + G=lowlev(); + + [Anew,Bnew,Cnew,Dnew]=abcd(Sys_new); + Anew=Anew+B2new*Fnew;Cnew=Cnew+Dnew*Fnew; + B1new=B1new+B2new*G; + A11=Anew(1:nv,1:nv);C21=Cnew(not_zeroed,1:nv); + B11=B1new(1:nv,:);D21=D1(not_zeroed,:); + D22=Sys("D");D22=D22(not_zeroed,:);D21=D21+D22*G; + Closed=syslin(Sys("dt"),A11,B11,C21,D21); + + +endfunction + +function G=lowlev() + ww=[B21 B22;D11 D12]; + [xx,dd]=colcomp(ww); + K=kernel(ww); + rowG=size(B22,2);colG=size(B1,2); + if size(K,2) > colG then K=K(:,1:colG);end + Kup=K(1:size(K,2),:); + if rcond(Kup) <= 1.d-10 then + warning(msprintf(gettext("%s: Bad conditioning.\n"),"ddp")); + K1=K*pinv(Kup);G=K1(size(K,2)+1:$,:);return + end + K1=K*inv(Kup); //test conditioning here! + G=K1(size(K,2)+1:$,:); +endfunction |