home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / xg123 < prev    next >
Encoding:
Text File  |  1995-11-15  |  1.4 KB  |  55 lines
  1. //-----------------------------------------------------------------------------
  2. //
  3. // xg123
  4. //
  5. // Syntax: R=xg123(X,A,B,D,M,W,X0)
  6. //
  7. // This routine computes the 3 matrices that define the feedback
  8. // gains for the Covariance Control Formulation.
  9. //
  10. // The input is:
  11. // X = covariance to be assigned
  12. // [A,B,D,M] = augmented dynamic matrices
  13. // W = applied initial disturbances
  14. // X0 = applied initial conditions
  15. //
  16. // Originally written by Lee D. Peterson, modified by Jeff Layton and
  17. // Ported to RLaB by Jeff Layton
  18. // Version 931026
  19. //
  20. // Note: current version does not check compatibility of dimensions
  21. //-----------------------------------------------------------------------------
  22.  
  23. rfile jpinv
  24.  
  25. xg123 = function(X,A,B,D,M,W,X0)
  26. {
  27.    local(nx,ny,L,Q,Term1,Lplus,Xi,P,Gamma,G1,G2,G3)
  28.  
  29. // Get plant dimensions
  30.    nx=X.nr;
  31.    ny=X.nc;
  32.  
  33. // Form defined matrices
  34.  
  35.    L=(eye(nx,nx)-jpinv(M)*M)*inv(X)*B*jpinv(B);
  36.    Q=X*A'+A*X+D*W*D'+X0;
  37.    Term1=(eye(nx,nx)-jpinv(M)*M);
  38.    Lplus=jpinv(L);
  39.    Xi=inv(X);
  40.    P=2.0*Lplus*Term1*Xi*Q+(eye(nx,nx)-Lplus*Term1*Xi)*Q*Lplus*L;
  41.    Gamma=jpinv(M)*M*X*(eye(nx,nx)-B*jpinv(B));
  42.  
  43. // Form the three gain coefficient matrices and return
  44.  
  45.    G1=-0.5*jpinv(B)*Q*(2*eye(nx,nx)-B*jpinv(B))*...
  46.       inv(X)*jpinv(M)+...
  47.       0.5*jpinv(B)*(P'-P)*B*jpinv(B)*inv(X)*jpinv(M);
  48.  
  49.    G2=jpinv(B)*X*jpinv(M)*M*(eye(nx,nx)-Gamma*jpinv(Gamma));
  50.  
  51.    G3=(eye(nx,nx)-Gamma*jpinv(Gamma))*jpinv(M);
  52.  
  53.    return << G1=G1; G2=G2; G3=G3 >>
  54. };
  55.