home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / ric < prev    next >
Encoding:
Text File  |  1995-11-15  |  3.3 KB  |  127 lines
  1. //----------------------------------------------------------------------------
  2. //
  3. // ric
  4. //
  5. // Syntax: P=ric(A,B,Q,R)
  6. //
  7. // This routine solves the Algebriac Riccati Equation (ARE) for
  8. // Continuous Systems. The ARE is,
  9. //
  10. //                 -1
  11. //   A'P + PA - PBR  B'P + Q = 0
  12. //
  13. //
  14. // Ref: (1) Kailath, "Linear Systems," Prentice-Hall, 1980
  15. //      (2) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
  16. //          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
  17. //      (3) Junkins, J., and Kim, Y., "Introduction to Dynamics and Control
  18. //          of Flexible Structures," AIAA Inc, Washington D.C., 1993.
  19. //
  20. // Copyright (C), by Jeffrey B. Layton, 1994
  21. // Version JBL 940903
  22. //----------------------------------------------------------------------------
  23.  
  24. rfile abcdchk
  25.  
  26. static (ric_compute)    // Hide this function
  27.  
  28. ric = function(a,b,q,r)
  29. {
  30.    local (nargs,msg,estr)
  31.  
  32. // Count number of input arguments
  33.    nargs=0;
  34.    if (exist(a)) {nargs=nargs+1;}
  35.    if (exist(b)) {nargs=nargs+1;}
  36.    if (exist(q)) {nargs=nargs+1;}
  37.    if (exist(r)) {nargs=nargs+1;}
  38.  
  39.    if (nargs != 4) {
  40.        error("RIC: Wrong number of input arguments.");
  41.    }
  42.  
  43. // Check if Q and R are consistent.
  44.    if ( a.nr != q.nr || a.nc != q.nc ) {
  45.        error("RIC: A and Q must be the same size");
  46.    }
  47.    
  48.    if ( r.nr != r.nc || b.nc != r.nr ) {
  49.        error("RIC: B and R must be consistent");
  50.    }
  51.    
  52. // Check if A and B are empty
  53.    if ( (!length(a)) || (!length(b)) ) {
  54.        error("RIC: A and B matrices cannot be empty.");
  55.    }
  56.  
  57. // A has to be square
  58.    if (a.nr != a.nc) {
  59.        error("RIC: A has to be square.");
  60.    }
  61.  
  62. // See if A and B are compatible.
  63.    msg="";
  64.    msg=abcdchk(a,b);
  65.    if (msg != "") {
  66.        estr="RIC: "+msg;
  67.        error(estr);
  68.    }
  69.  
  70. // Check if Q is positive semi-definite and symmetric
  71.    if (!issymm(q)) {
  72.        printf("%s","RIC: Warning: Q is not symmetric.\n");
  73.    else
  74.        if (any(eig(q).val < -epsilon()*norm(q,"1")) ) {    
  75.            printf("%s","RIC: Warning: Q is not positive semi-definite.\n");
  76.        }
  77.    }
  78.  
  79. // Check if R is positive definite and symmetric
  80.    if (!issymm(r)) {
  81.        printf("%s","RIC: Warning: R is not symmetric.\n");
  82.    else
  83.        if (any(eig(r).val < -epsilon()*norm(r,"1")) ) {
  84.            printf("%s","RIC: Warning: R is not positive semi-definite.\n");
  85.        }
  86.    }
  87.  
  88. //
  89. // Call ric_compute to solve Riccatti.
  90. //
  91.     return ric_compute (a, b, q, r);
  92. };
  93.  
  94. //----------------------------------------------------------------------------
  95. //
  96. // This is where the computation is performed. Note that ric_compute is a
  97. // static variable and is never seen from the global workspace.
  98. //
  99.  
  100. ric_compute = function (a, b, q, r)
  101. {
  102.    local (Phi12, Phi22, d, e, index, p, v)
  103.  
  104. // Start solution by finding the spectral decomposition and eigenvectors
  105. // of Hamiltonian.
  106.     
  107.    e=eig( [ a, solve(r',b')'*b'; q, -a' ] );
  108.    v=e.vec;
  109.    d=e.val;
  110.    
  111. // Sort eigenvectors by sorting eigenvalues (and storing the index).
  112.    index=sort( real( d ) ).ind;
  113.    d=real( d[ index ] );
  114.    
  115.    if ( !( d[a.nc] < 0 && d[a.nc+1] > 0 ) ) {
  116.        error("ric: Can't order eigenvalues");
  117.    }
  118.    
  119. // Form the Partitions of the PHI matrix and solve for P and then the
  120. // Control Gains.
  121.    Phi12=v[ 1:a.nc; index[1:a.nc] ];
  122.    Phi22=v[ (a.nc+1):(2*a.nc); index[1:a.nc] ];
  123.    p=-real(solve(Phi12',Phi22')');
  124.    
  125.    return p;
  126. };
  127.