home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / are2 < prev    next >
Encoding:
Text File  |  1995-11-15  |  2.3 KB  |  83 lines
  1. //----------------------------------------------------------------------------
  2. //
  3. // are2
  4. //
  5. // Syntax: X=are2(A,B,C)
  6. //
  7. // This routine solves the Algebraic Riccati Equation. It returns a stabilizing
  8. // solution (if one can be computed) to the continuous time Riccati equation
  9. // given by,
  10. //
  11. //      A'*X + X*A - X*B*X + C = 0
  12. //
  13. //                 -1
  14. //   A'P + PA - PBR  B'P + Q = 0
  15. //
  16. //
  17. // The routine assumes that B is symmetric and nonnegative definite and
  18. // that C is symmetric.
  19. //
  20. // Calling the routine as X=are2(A,B,C) returns the solution to the ARE
  21. // in X.
  22. //
  23. // This routine is taken from the lqr solver so it uses Potter's algorithm
  24. // to the solve the ARE.
  25. //
  26. // Ref: (1) Kailath, "Linear Systems," Prentice-Hall, 1980
  27. //      (2) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
  28. //          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
  29. //      (3) Junkins, J., and Kim, Y., "Introduction to Dynamics and Control
  30. //          of Flexible Structures," AIAA Inc, Washington D.C., 1993.
  31. //
  32. // Copyright (C), by Jeffrey B. Layton, 1994
  33. // Version JBL 940918
  34. //----------------------------------------------------------------------------
  35.  
  36. are2 = function(A,B,C)
  37. {
  38.    local(nargs,Phi12,Phi22,d,e,index,X,v)
  39.  
  40. // Count number of input arguments
  41.    nargs=0;
  42.    if (exist(A)) {nargs=nargs+1;}
  43.    if (exist(B)) {nargs=nargs+1;}
  44.    if (exist(C)) {nargs=nargs+1;}
  45.  
  46.    if (nargs != 3) {
  47.        error("ARE2: Wrong number of input arguments.");
  48.    }
  49.  
  50. // Error Traps for input.
  51.    if (A.nr != A.nc) {
  52.        error("ARE2: Nonsquare A matrix");
  53.    }
  54.    if ( (B.nr != A.nr) || (B.nc != A.nr) ) {
  55.        error("ARE2: Incorrectly dimensioned B matrix");
  56.    }
  57.    if ( (C.nr != A.nr) || (C.nc != A.nr) ) {
  58.        error("ARE2: Incorrectly dimensioned C matrix");
  59.    }
  60.  
  61. // Start solution by finding the spectral decomposition and eigenvectors
  62. // of Hamiltonian.
  63.     
  64.    e=eig( [ A, B; C, -A' ] );
  65.    v=e.vec;
  66.    d=e.val;
  67.    
  68. // Sort eigenvectors by sorting eigenvalues (and storing the index).
  69.    index=sort( real( d ) ).ind;
  70.    d=real( d[ index ] );
  71.    
  72.    if ( !( d[A.nc] < 0 && d[A.nc+1] > 0 ) ) {
  73.        error("ARE2: Can't order eigenvalues");
  74.    }
  75.    
  76. // Form the Partitions of the PHI matrix and solve for P
  77.    Phi12=v[ 1:A.nc; index[1:A.nc] ];
  78.    Phi22=v[ (A.nc+1):(2*A.nc); index[1:A.nc] ];
  79.    X=-real(solve(Phi12',Phi22')');
  80.    
  81.    return X
  82. };
  83.