home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / xsopt < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.7 KB  |  128 lines
  1. // function S = xsopt(X,Mp,g1,g2,g3,R,nx,nu,nz,nc)
  2. //-------------------------------------------------------------------------
  3. //
  4. // xsopt
  5. //
  6. // Syntax: S=xsopt(X,Mp,g1,g2,g3,R,nx,nu,nz,nc)
  7. //
  8. // Computes the optimal S=-S^* matrix with respect to which the
  9. // control cost function
  10. //
  11. //   V = tr(UR)
  12. //
  13. // is stationary, all other variables being constant, while 
  14. // assigning the covariance X to the closed-loop system
  15. //
  16. // g1, g2, g3 are matrices defined so that the 
  17. // controller
  18. //
  19. //  xc_dot = Ac * xc + F * z
  20. //       u =  G * xc + H * z
  21. //
  22. // is given by
  23. //
  24. //  G = [ H G    = g1 + g2 * S * g3
  25. //        F Ac ]
  26. //
  27. // nx, nu, nz, nc == dimensions of plant, actuator, sensor 
  28. //                   and controller state vectors
  29. //
  30. // Originally written by L. D. Peterson
  31. // Modified by Jeff Layton 10/14/90
  32. // Ported to RLaB, 10/13/93
  33. //
  34. // Version JBL 931013
  35. // Note: current version does not check compatibility of dimensions
  36. //-------------------------------------------------------------------------
  37.  
  38. rfile jpinv
  39.  
  40. xsopt = function(X,Mp,g1,g2,g3,R,nx,nu,nz,nc)
  41. {
  42.    local(rowsg2,ns,m,Xp,Xpc,Xc,tol,i,j,k,prodi,G,H,MpXpMptr,...
  43.          MpXpc,G0Xc,G0,H0,b1,b2,b4,b,P1,P2,P4,P,alpha,b3,P3,S)
  44.  
  45. // Get dimension of S
  46.  
  47.    rowsg2=g2.nr;
  48.    ns=g2.nc;
  49.  
  50. // Degrees of freedom in the S matrix
  51.  
  52.    m=ns*(ns-1) / 2;
  53.  
  54. // Extract partitions of X
  55.  
  56.    Xp=X[1:nx,1:nx];
  57.    Xpc=X[1:nx,nx+1:nx+nc];
  58.    Xc=X[nx+1:nx+nc,nx+1:nx+nc];
  59.  
  60. // Form all the Gi and Hi matrices
  61.  
  62.    G0=xgpart(g1,nu,nz,nc);
  63.    H0=xhpart(g1,nu,nz,nc);
  64.  
  65.    tol=max([size(g2),size(g3)])*epsilon()*norm(g2)*norm(g3);
  66.  
  67.    for (i in 1:m) {
  68.         prodi=g2*xsi(ns,i)*g3;
  69.         for (j in 1:(nu+nc)) {
  70.              for (k in 1:(nz+nc)) {
  71.                   if ( abs(prod[j,k]) <= tol ) {
  72.                       prod[j,k]=0;
  73.                   }
  74.              }
  75.         }
  76.         G[i]=xgpart(prodi,nu,nz,nc);
  77.         H[i]=xhpart(prodi,nu,nz,nc);
  78.    }
  79.  
  80. // Form some constant matrices 
  81.  
  82.    MpXpMptr=Mp*Xp*Mp';
  83.    MpXpc=Mp*Xpc;
  84.    G0Xc=G0*Xc;
  85.  
  86. // Form the b vectors
  87.  
  88.    b1=zeros(m,1);
  89.    b2=zeros(m,1);
  90.    b4=zeros(m,1);
  91.    for (i in 1:m) {
  92.         b1[i] = trace(H0*MpXpMptr*(H[i]')*R);
  93.         b2[i] = trace(H0*MpXpc*(G[i]')*R) + trace(H[i]*MpXpc*G0'*R);
  94.         b4[i] = trace(G0Xc*(G[i]')*R);
  95.    }
  96.  
  97.    b3=b2;
  98.    b=b1+b2/2+b3/2+b4;
  99.  
  100. // Form the P matrices
  101.  
  102.    P1=zeros(m,m);
  103.    P2=zeros(m,m);
  104.    P4=zeros(m,m);
  105.    for (i in 1:m) {
  106.         for (j in 1:m) {
  107.              P1[i;j]=trace(H[i]*MpXpMptr*(H[j]')*R);
  108.              P2[i;j]=trace(H[i]*MpXpc*(G[j]')*R);
  109.              P4[i;j]=trace(G[i]*Xc*(G[j]')*R);
  110.         }
  111.    }
  112.    P3=P2';
  113.    P=P1+P2+P3+P4;
  114.  
  115. // Solve alpha equation
  116.  
  117.    alpha=-jpinv(P)*b;
  118.  
  119. // Formulate the optimal S matrix
  120.  
  121.    S=zeros(ns,ns);
  122.    for (i in 1:m) {
  123.         S=S+alpha[i]*xsi(ns,i);
  124.    }
  125.  
  126.    return S
  127. };
  128.