home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / xc12 < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.1 KB  |  91 lines
  1. //---------------------------------------------------------------------
  2. //
  3. // xc12
  4. //
  5. // Syntax: R=xc12(X,Ap,Bp,Dp,Mp,W,X0,nc)
  6. //
  7. // This routine computes the two matrix covariance assignability
  8. // constraints for a given plant.
  9. //
  10. // The input is:
  11. // X = covariance to be assigned
  12. // [Ap,Bp,Dp,Mp] = plant dynamic matrices
  13. // W = applied initial disturbances
  14. // X0 = applied initial conditions
  15. // nc = order of desired compensator
  16. //
  17. // The routine returns a list:
  18. //
  19. //    R.C1 = First matrix assignability condition
  20. //    R.C2 = Second matrix assignability condition
  21. //
  22. // Originally written by Lee D. Peterson, modified by Jeff Layton and
  23. // ported to RLaB by Jeff Layton.
  24. // Version 931026
  25. //
  26. // Note: current version does not check compatibility of dimensions
  27. //---------------------------------------------------------------------
  28.  
  29. rfile jpinv
  30.  
  31. xc12 = function(X,Ap,Bp,Dp,Mp,W,X0,nc)
  32. {
  33.    local(nx,nu,nw,nz,A,B,M,D,Xp,Xpc,C1,C2,...
  34.          Xc,X0p,X0c,T,u,s,D0,Dp0,Qp,Xpb,Xpbi,Qpb,L);
  35.  
  36. // Get plant dimensions
  37.  
  38.    nx=Ap.nr;
  39.    nu=Bp.nc;
  40.    nw=Dp.nc;
  41.    nz=Mp.nr;
  42.  
  43. // Form up output feedback matrices
  44.  
  45.    A=[Ap,zeros(nx,nc);zeros(nc,nx),zeros(nc,nc)];
  46.  
  47.    B=[Bp,zeros(nx,nc);zeros(nc,nu),eye(nc,nc)];
  48.  
  49.    M=[Mp,zeros(nz,nc);zeros(nc,nx),eye(nc,nc)];
  50.  
  51.    D=[Dp;zeros(nc,nw)];
  52.  
  53. // Extract partitions of X
  54.  
  55.    Xp = X[1:nx;1:nx];
  56.    Xpc = X[1:nx;nx+1:nx+nc];
  57.    Xc = X[nx+1:nx+nc;nx+1:nx+nc];
  58.  
  59. // Extract partitions of X0
  60.  
  61.    X0p = X0[1:nx;1:nx];
  62.    X0c = X0[nx+1:nx+nc;nx+1:nx+nc];
  63.  
  64. // Form D0
  65.    T=svd(D*W*D'+X0);
  66.    u=T.u;
  67.    s=diag(T.sigma);
  68.    D0 = u*sqrt(s);
  69.    clear(u,s);
  70.    Dp0=D0[1:nx;];
  71.  
  72. // Form defined matrices
  73.  
  74. //Qp = Xp*Ap' + Ap*Xp + Dp0*Dp0';
  75.    Qp=Xp*Ap'+Ap*Xp+Dp*W*Dp'+X0p;
  76.    Xpb=Xp-Xpc*inv(Xc)*Xpc';
  77.    Xpbi=inv(Xpb);
  78.    Qpb=Xpbi*(Xpb*Ap'+Ap*Xpb+Dp0*Dp0'+...
  79.        Xpc*inv(Xc)*X0c*inv(Xc)*Xpc')*inv(Xpb);
  80.    clear(Xpbi,Dp0,Xpb);
  81.    L=(eye(nx+nc,nx+nc)-jpinv(M)*M)*inv(X)*B*jpinv(B);
  82.  
  83. // Form the three assignability conditions and return
  84.  
  85.    C1=(eye(nx,nx)-Bp*jpinv(Bp))*Qp*(eye(nx,nx)-Bp*jpinv(Bp));
  86.  
  87.    C2=(eye(nx,nx)-jpinv(Mp)*Mp)*Qpb*(eye(nx,nx)-jpinv(Mp)*Mp);
  88.  
  89.    return << C1=C1; C2=C2>>
  90. };
  91.