bug 4319 fix (ccontrg)
[scilab.git] / scilab / modules / cacsd / macros / ccontrg.sci
index 841a3f1..ac1dae7 100644 (file)
@@ -19,121 +19,123 @@ function [K]=ccontrg(PP,r,Gamma);
 //  b = ( b1 , b2 ) ,  c = ( c1 ) ,    d = ( d11  d12)
 //                         ( c2 )          ( d21  d22)
 //  r(1) and r(2) are the dimensions of d22 (rows x columns)
+  
+  
 //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=maxi(size(find(diag(s) > 1.0e-10)));
-[w,p,z]=svd(d21); r21=maxi(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)/..
+  [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=maxi(size(find(diag(s) > 1.0e-10)));
+  [w,p,z]=svd(d21); r21=maxi(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')/..
+  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=maxi(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);
+  ttt=py'*ttt;
+  
+  nmin=maxi(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 problem:
+// [go,xo]=par(a,b,c,rx,cx) solves the minimization (Parrot) problem:
 //
 //         || a   b ||
 //     min  ||       ||
@@ -149,51 +151,64 @@ function [go,xo]=parrt(a,b,c,rx,cx);
 //
 //!
 //constant
-TOLA=1.0e-8; // threshold used to discard near singularities in
-//             gs I - A'*A
-go=maxi(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=mini(ra,ca);
-d=diag(s);
-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
-   cross=diag(dd)\(s(nd+1:ra,nd+1:ca))';
-//cross  now contains the nonsingular part of s / (gs I - s' * s)
-   xo=-c*vnz*s'*unz'*b;
-end
+  TOLA=1.0e-8; // threshold used to discard near singularities in gs I - A'*A
+  
+  go=maxi(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=mini(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
+    
+    cross=diag(dd)\(s(nd+1:ra,nd+1:ca))';
+    
+    //cross  now contains the nonsingular part of s / (gs I - s' * s)
+    
+    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
+
+