1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
|
// 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]=ccontrg(PP,r,Gamma);
//***********************************
// returns a realization of the central controller for the
// general problem using the formulas in Gahinet, 92
// Note that Gamma must be > gopt (output of gamitg)
// PP contains the parameters of plant realization (sylin list)
// b = ( b1 , b2 ) , c = ( c1 ) , d = ( d11 d12)
// ( c2 ) ( d21 d22)
// r(1) and r(2) are the dimensions of d22 (rows x columns)
if argn(2)<>3 then
error(msprintf(gettext("%s: Wrong number of input arguments: %d expected.\n"),"ccontrg",3))
end
if typeof(PP)<>"state-space" then
error(msprintf(gettext("%s: Wrong type for input argument #%d: Linear state space expected.\n"),"ccontrg",1))
end
if PP.dt<>"c" then
error(msprintf(gettext("%s: Wrong value for input argument #%d: Continuous time system expected.\n"),"ccontrg",1))
end
if typeof(r)<>"constant"|~isreal(r) then
error(msprintf(gettext("%s: Wrong type for argument #%d: Real vector expected.\n"),"ccontrg",2))
end
if size(r,"*")<>2 then
error(msprintf(gettext("%s: Wrong size for input argument #%d: %d expected.\n"),"ccontrg",2,2))
end
r=int(r);
if or(r<=0) then
error(msprintf(gettext("%s: Wrong values for input argument #%d: Elements must be positive.\n"),"ccontrg",2))
end
//parameter recovery
[a,b1,b2,c1,c2,d11,d12,d21,d22]=smga(PP,r);
//dimensions
[na,na]=size(a); nh=2*na;
[p1,m2]=size(d12),
[p2,m1]=size(d21),
gs=Gamma**2;
//HAMILTONIAN SETUP
//------------------
sh22=m1+m2;
h11=[a,0*eye(a);-c1'*c1,-a'];
h12=[-b1,-b2;c1'*d11,c1'*d12];
h21=[d11'*c1,b1';d12'*c1,b2'];
h22=gs*eye(m1,m1); h22(sh22,sh22)=0;
h22=h22-[d11,d12]'*[d11,d12];
sj22=p1+p2;
j11=[a',0*eye(a);-b1*b1',-a];
j12=[-c1',-c2';b1*d11',b1*d21']
j21=[d11*b1',c1;d21*b1',c2];
j22=gs*eye(p1,p1); j22(sj22,sj22)=0;
j22=j22-[d11;d21]*[d11;d21]';
//computation of Xinf and Yinf
//-----------------------------
//compute orthon. bases of the negative inv. subspaces.
[q,r]=qr([h12;h22]);
q12=q(1:nh,sh22+1:nh+sh22); q22=q(nh+1:nh+sh22,sh22+1:nh+sh22);
hr=q12'*h11+q22'*h21;
[uh,dh]=schur(hr,q12',"c");
px=uh(1:na,1:na); qx=uh(na+1:nh,1:na);
[q,r]=qr([j12;j22]);
q12=q(1:nh,sj22+1:nh+sj22); q22=q(nh+1:nh+sj22,sj22+1:nh+sj22);
jr=q12'*j11+q22'*j21;
[uj,dj]=schur(jr,q12',"c");
py=uj(1:na,1:na); qy=uj(na+1:nh,1:na);
//computation of M,N
[uz,sz,vz]=svd(qx'*qy/gs-px'*py);
sz=sqrt(sz);
//DETERMINATION OF DK
//-------------------
[u,s,v]=svd(d12); r12=max(size(find(diag(s) > 1.0e-10)));
[w,p,z]=svd(d21); r21=max(size(find(diag(p) > 1.0e-10)));
u1=u(:,1:r12); v1=v(:,1:r12); w1=w(:,1:r21); z1=z(:,1:r21);
s1=s(1:r12,1:r12); ph1=p(1:r21,1:r21);
d11tr=u'*d11*z;
[g0,d0]=parrt(d11tr(r12+1:p1,r21+1:m1),d11tr(r12+1:p1,1:r21),..
d11tr(1:r12,r21+1:m1),r12,r21);
dk=v1*s1\(d0-d11tr(1:r12,1:r21))/ph1*w1';
//DETERMINATION OF BK, CK
//-----------------------
hd11=(eye(p1,p1)-u1*u1')*d11;
hb1=b1-b2*v1*(s1\(u1'*d11));
that=dk*c2*px+v1*s1\(u1'*c1*px+s1\v1'*b2'*qx+..
(d0*z1'+u1'*d11*(eye(m1,m1)-z1*z1'))*..
((gs*eye(m1,m1)-hd11'*hd11)\(hb1'*qx+hd11'*c1*px)));
td11=d11*(eye(m1,m1)-z1*z1');
tc1=c1-(d11*z1/ph1)*w1'*c2;
ttil=py'*b2*dk+(py'*b1*z1+qy'*c2'*w1/ph1+..
((qy'*tc1'+py'*b1*td11')/(gs*eye(p1,p1)-td11*td11'))*..
((eye(p1,p1)-u1*u1')*d11*z1+u1*d0))/ph1*w1';
ck=-that*uz/sz; bk=-sz\vz'*ttil;
//just checking...
x=qx/px; y=qy/py;
d12p=pinv(d12); d21p=pinv(d21);
thh=d12p*(c1+d12p'*b2'*x)+dk*c2+(d12p*d11+dk*d21)/..
(gs*eye(m1,m1)-hd11'*hd11)*((b1-b2*d12p*d11)'*x+hd11'*c1);
thh=thh*px;
ttt=(b1+y*c2'*d21p')*d21p+b2*dk+(y*tc1'+b1*td11')/..
(gs*eye(p1,p1)-td11*td11')*(d11*d21p+d12*dk);
ttt=py'*ttt;
nmin=max(norm(hd11),norm(td11));
ncom=norm(d11+d12*dk*d21);
//DETERMINATION OF AK
//-------------------
ca=a+b2*dk*c2; cb=b1+b2*dk*d21; cc=c1+d12*dk*c2; Cd=d11+d12*dk*d21;
ak=py'*b2*that+ttil*c2*px-py'*ca*px-qy'*ca'*qx/gs+..
[-qy'*cc'/Gamma,py'*cb-ttil*d21]/..
[Gamma*eye(p1,p1),Cd;Cd',Gamma*eye(m1,m1)]*..
[cc*px-d12*that;-cb'*qx/Gamma];
ak=sz\(vz'*ak*uz)/sz;
K=syslin("c",ak,bk,ck,dk);
endfunction
function [go,xo]=parrt(a,b,c,rx,cx);
//
// [go,xo]=par(a,b,c,rx,cx) solves the minimization (Parrot) problem:
//
// || a b ||
// min || ||
// X || c x ||2
//
// an explicit solution is
// 2 T -1 T
// Xo = - c ( go I - a a) a b
// where T T
// go = max ( || (a , b) || , || (a , c) || )
//
// rx and cx are the dimensions of Xo (optional if a is nondegenerate)
//
//!
//constant
TOLA=1.0e-8; // threshold used to discard near singularities in gs I - A'*A
go=max(norm([a b]),norm([a;c]));
[ra,cb]=size(b); [rc,ca]=size(c); xo=0;
//MONITOR LIMIT CASES
//--------------------
if ra==0 | ca == 0 | go == 0 then xo(rx,cx)=0; return; end
//COMPUTE Xo IN NONTRIVIAL CASES
//------------------------------
gs=(go**2);
[u,s,v]=svd(a);
//form gs I - s' * s
ns=min(ra,ca);
if size(s,1)>1 then
d=diag(s);
else
d=s(1)
end
dd=gs*ones(d)-d**2;
//isolate the non (nearly) singular part of gs I - A'*A
nnz1=nthresh(dd/go,TOLA);
nd=ns-nnz1; //number of singular values thresholded out
//compute xo
if nnz1==0 then
xo(rc,cb)=0;
else
unz=u(:,nd+1:ra);
vnz=v(:,nd+1:ca);
s=s(nd+1:ra,nd+1:ca);
for i=1:nnz1
s(i,i)=s(i,i)/dd(i+nd);
end
xo=-c*vnz*s'*unz'*b;
end
endfunction
function n=nthresh(d,tol)
n=find(d<=tol,1)
if n==[] then
n=size(d,"*"),
else
n=n-1
end
endfunction
|
