home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / gric < prev    next >
Encoding:
Text File  |  1995-11-15  |  4.0 KB  |  159 lines
  1. //----------------------------------------------------------------------------
  2. //
  3. // gric
  4. //
  5. // Syntax: DP=gric(Q,DA,B,DB,Q,DQ,R,DR)
  6. //
  7. // This routine computes the gradient of the solution of the Algebraic
  8. // Riccatti Equation (ARE) with respect to a scalar parameter 'p'
  9. // for Continuous systems. The ARE is,
  10. //
  11. //                 -1
  12. //   A'P + PA - PBR  B'P + Q = 0
  13. //
  14. // The gradient of the input matrices (A,B,Q, and R) with respect to 'p'
  15. // are required.
  16. //
  17. // Ref: (1) Kailath, "Linear Systems," Prentice-Hall, 1980
  18. //      (2) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
  19. //          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
  20. //      (3) Junkins, J., and Kim, Y., "Introduction to Dynamics and Control
  21. //          of Flexible Structures," AIAA Inc, Washington D.C., 1993.
  22. //
  23. // Copyright (C), by Jeffrey B. Layton, 1994
  24. // Version JBL 940918
  25. //----------------------------------------------------------------------------
  26.  
  27. rfile abcdchk
  28. rfile ric
  29.  
  30. static (gric_compute)    // Hide this function
  31.  
  32. gric = function(a,da,b,db,q,dq,r,dr)
  33. {
  34.    local (nargs,msg,estr)
  35.  
  36. // Count number of input arguments
  37.    nargs=0;
  38.    if (exist(a)) {nargs=nargs+1;}
  39.    if (exist(da)) {nargs=nargs+1;}
  40.    if (exist(b)) {nargs=nargs+1;}
  41.    if (exist(db)) {nargs=nargs+1;}
  42.    if (exist(q)) {nargs=nargs+1;}
  43.    if (exist(dq)) {nargs=nargs+1;}
  44.    if (exist(r)) {nargs=nargs+1;}
  45.    if (exist(dr)) {nargs=nargs+1;}
  46.  
  47.    if (nargs != 8) {
  48.       error("GRIC: Wrong number of input arguments.");
  49.    }
  50.  
  51. // Check Dimensions
  52. // ----------------
  53.  
  54. // Check if Q and R are consistent.
  55.    if ( a.nr != q.nr || a.nc != q.nc ) {
  56.        error("GRIC: A and Q must be the same size");
  57.    }
  58.    
  59.    if ( r.nr != r.nc || b.nc != r.nr ) {
  60.        error("GRIC: B and R must be consistent");
  61.    }
  62.    
  63. // Check if A and B are empty
  64.    if ( (!length(a)) || (!length(b)) ) {
  65.        error("GRIC: A and B matrices cannot be empty.");
  66.    }
  67.  
  68. // A has to be square
  69.    if (a.nr != a.nc) {
  70.        error("GRIC: A has to be square.");
  71.    }
  72.  
  73. // Check gradient matrices
  74. // -----------------------
  75.  
  76. // Check if DQ and DR are consistent.
  77.    if ( da.nr != dq.nr || da.nc != dq.nc ) {
  78.        error("GRIC: DA and DQ must be the same size");
  79.    }
  80.    
  81.    if ( dr.nr != dr.nc || db.nc != dr.nr ) {
  82.        error("GRIC: DB and DR must be consistent");
  83.    }
  84.    
  85. // Check if DA and DB are empty
  86.    if ( (!length(da)) || (!length(db)) ) {
  87.        error("GRIC: DA and DB matrices cannot be empty.");
  88.    }
  89.  
  90. // DA has to be square
  91.    if (da.nr != da.nc) {
  92.        error("GRIC: DA has to be square.");
  93.    }
  94.  
  95. // See if A and B are compatible.
  96.    msg="";
  97.    msg=abcdchk(a,b);
  98.    if (msg != "") {
  99.        estr="GRIC: "+msg;
  100.        error(estr);
  101.    }
  102.  
  103. // Check if Q is positive semi-definite and symmetgric
  104.    if (!issymm(q)) {
  105.        printf("%s","GRIC: Warning: Q is not symmetric.\n");
  106.    else
  107.        if (any(eig(q).val < -epsilon()*norm(q,"1")) ) {    
  108.            printf("%s","GRIC: Warning: Q is not positive semi-definite.\n");
  109.        }
  110.    }
  111.  
  112. // Check if R is positive definite and symmetgric
  113.    if (!issymm(r)) {
  114.        printf("%s","GRIC: Warning: R is not symmetric.\n");
  115.    else
  116.        if (any(eig(r).val < -epsilon()*norm(r,"1")) ) {
  117.            printf("%s","GRIC: Warning: R is not positive semi-definite.\n");
  118.        }
  119.    }
  120.  
  121. //
  122. // Call gric_compute to solve Riccatti.
  123. //
  124.     return gric_compute (a,da,b,db,q,dq,r,dr);
  125. };
  126.  
  127. //----------------------------------------------------------------------------
  128. //
  129. // This is where the computation is performed. Note that gric_compute is a
  130. // static variable and is never seen from the global workspace.
  131. //
  132.  
  133. gric_compute = function (a,da,b,db,q,dq,r,dr)
  134. {
  135.    local (p, abar, bbar, cbar, term1, term2, term3, dp)
  136.  
  137. // Solve Riccatti be calling ric
  138.    p=ric(a,b,q,r);
  139.  
  140. // Create matrices for solving sylvester equation.
  141.  
  142. // A bar:
  143.    abar = a' - p*(b/r)*b';
  144.  
  145. // B bar:
  146.    bbar = a - (b/r)*b'*p;
  147.  
  148. // C bar:
  149.    term1 = p*(b/r)*db'*p;
  150.    term2 = p*(b/r)*(dr/r)*b'*p;
  151.    term3 = p*(db/r)*b'*p;
  152.    cbar = da'*p + p*da - term3 + term2 - term1 + dq;
  153.  
  154. // Solve Sylvester's Equation (using lyap.r)
  155.    dp=lyap(abar,bbar,cbar);
  156.    
  157.    return dp;
  158. };
  159.