* Bug #14582: gettext or _ were applied to broken literal strings
[scilab.git] / scilab / modules / cacsd / macros / ui_observer.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA -
3 //
4 // Copyright (C) 2012 - 2016 - Scilab Enterprises
5 //
6 // This file is hereby licensed under the terms of the GNU GPL v2.0,
7 // pursuant to article 5.3.4 of the CeCILL v.2.1.
8 // This file was originally licensed under the terms of the CeCILL v2.1,
9 // and continues to be available under such terms.
10 // For more information, see the COPYING file which you should have received
11 // along with this program.
12
13 function [UIobs,J,N]=ui_observer(Sys,reject,C1,D1,flag,Alfa,Beta)
14     // ------------Unknown input observer------------------
15     // Sys: (w,u) --> y  = (A,B,C2,D2) syslin linear system with two inputs
16     // w and u, w being the unknown input.
17     // The  matrices B and D2 of Sys are (implicitly) partitioned as:
18     // B=[B1,B2] and D2=[D21,D22]  with B1=B(:,reject)  and D21=D2(:,reject)
19     // where reject = indices of unknown inputs.
20     // The matrices C1 and D1 define
21     // z = C1 x + D1 (w,u) = the to-be-estimated output
22     // the matrix D1 is (implicitly) partitioned as
23     // D1=[D11,D12] with D11=D(:,reject)
24     //
25     // The data (Sys, reject,C1, D1)   define a 2-input 2-output system:
26     //
27     //  xdot =  A x + B1  w + B2  u
28     //     z = C1 x + D11 w + D12 u
29     //     y = C2 x + D21 w + D22 u
30     //
31     //  An observer (u,y) --> zhat is looked for the output z.
32     // flag='ge' no stability constraints
33     //     ='st' stable observer    (default)
34     //     ='pp' observer with pole placement
35     //     Alfa,Beta = desired location of closed loop poles  (default -1, -2)
36     // UIobs = linear system (u,y) --> zhat such that:
37     // The transfer function: (w,u) --> z  equals the composed transfer function:
38     //            [0,I;        UIobs
39     //             Sys]
40     //      (w,u) -----> (u,y) -----> zhat
41     // i.e. transfer function of system {A,B,C1,D1} equals
42     // transfer function UIobs*[0,I; Sys]
43     // J=y-output to x-state injection
44     // N=y-output to z-estimated output injection
45     // Example:
46     //A=diag([3,-3,7,4,-4,8]);
47     //B=[eye(3,3);zeros(3,3)];
48     //C=[0,0,1,2,3,4;0,0,0,0,0,1];
49     //D=[1,2,3;0,0,0];
50     //rand('seed',0);w=ss2ss(syslin('c',A,B,C,D),rand(6,6));
51     //[A,B,C,D]=abcd(w);
52     //B=[B,matrix(1:18,6,3)];D=[D,matrix(-(1:6),2,3)];
53     //reject=1:3;
54     //Sys=syslin('c',A,B,C,D);
55     //N1=[-2,-3];C1=-N1*C;D1=-N1*D;
56     //nw=length(reject);nu=size(Sys('B'),2)-nw;
57     //ny=size(Sys('C'),1);nz=size(C1,1);
58     //[UIobs,J,N]=ui_observer(Sys,reject,C1,D1);
59     //
60     //W=[zeros(nu,nw),eye(nu,nu);Sys];UIobsW=UIobs*W;
61     //(w,u) --> z=UIobs*[0,I;Sys](w,u)
62     //clean(ss2tf(UIobsW));
63     //wu_to_z=syslin('c',A,B,C1,D1);clean(ss2tf(wu_to_z));
64     //clean(ss2tf(wu_to_z)-ss2tf(UIobsW),1.d-7)
65     //2nd ex: nx=2;ny=3;nwu=2;Sys=ssrand(ny,nwu,nx);
66     //        C1=rand(1,nx);D1=[0,1];
67     //        UIobs=ui_observer(Sys,1,C1,D1);
68     [LHS,RHS]=argn(0);
69     if RHS==6 then Beta=-1;end
70     if RHS==5 then Beta=-1;Alfa=-1;end
71     if RHS==4 then Beta=-1;Alfa=-1;flag="st";end
72     if RHS==3 then Beta=-1;Alfa=-1;flag="st";D1=[];end
73     if size(C1,2) ~= size(Sys("A"),1) then
74         msg = _("%s: Incompatible input arguments #%d and #%d: state dimension of #%d must be equal to the column dimension of #%d.\n")
75         error(msprintf(msg, "ui_observer", 1, 3, 1, 3))
76     end
77     if size(D1,2) ~= size(Sys("B"),2) then
78         msg = _("%s: Incompatible input arguments #%d and #%d: input dimension of #%d must be equal to the column dimension of #%d.\n")
79         error(msprintf(msg, "ui_observer", 4, 1, 4, 1))
80     end
81     not_reject=1:size(Sys,"c");not_reject(reject)=[];
82     Sys1=Sys(:,reject);      //A,B1,C2,D21
83     [X,dims,J,Y,k,Z]=cainv(Sys1,Alfa,Beta,flag);
84     Sys_new=ss2ss(Sys,X);
85     ns=dims(3);Jnew=X'*J;J2=Jnew(ns+1:$,:);
86     [Anew,Bnew,Cnew,Dnew]=abcd(Sys_new);
87     A22=Anew(ns+1:$,ns+1:$);C22=Cnew(:,ns+1:$);
88     B22=Bnew(ns+1:$,not_reject);D22=Dnew(:,not_reject);
89     Sys22=syslin(Sys("dt"),A22,B22,C22,D22);
90     UIobs=observer(Sys22,J2);
91     //
92     C1new=C1*X;
93     D11=D1(:,reject);D21=Dnew(:,reject);
94     C11=C1new(:,1:ns);C21=Cnew(:,1:ns);
95     //N s.t. [I,N]*[C11 D11;C21 D21]=[0,0]
96     N=lowlevel();
97     mprintf(gettext("%s: Residual norm = %g\n"),"ui_observer",norm([eye(size(N,1),size(N,1)),N]*[C11,D11;C21,D21]));
98     D12=D1(:,not_reject);C12=C1new(:,ns+1:$);
99     UIobs("C")=[C12+N*C22];UIobs("D")=[D12+N*D22,-N];
100
101 endfunction
102
103 function N=lowlevel()
104     ww=[C11 D11;C21 D21];
105     [xx,dd]=rowcomp(ww);
106     K=xx(dd+1:$,:);
107     colN=size(C21,1);rowN=size(C11,1);
108     if size(K,1) > rowN then K=K(1:rowN,:);end
109     Kleft=K(:,1:size(K,1))
110     if size(Kleft,"*")==1 & abs(Kleft) <1.d-8 then
111         N=[];
112         error(msprintf(gettext("%s: Bad conditionning.\n"),"ui_observer"));
113     end
114     if rcond(Kleft) <= 1.d-10 then
115         warning(msprintf(gettext("%s: Bad conditionning.\n"),"ui_observer"));
116         K1=pinv(Kleft)*K;N=K1(:,size(K,1)+1:$);return
117     end
118     K1=inv(Kleft)*K;   //test conditioning here!
119     N=K1(:,size(K,1)+1:$)
120 endfunction