home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / dare2 < prev    next >
Encoding:
Text File  |  1995-11-15  |  3.9 KB  |  137 lines
  1. //-------------------------------------------------------------------------------
  2. //
  3. // dare2
  4. //
  5. // Syntax: A=dare2(a,b,q,r,ct)
  6. //
  7. // This routine solve the steady-state Discrete Algebraic Riccati Equation.
  8. //
  9. //                              -1
  10. //       P - A'PA + A'PB(R+B'PB)  BP'A - Q = 0
  11. //
  12. // The routine returns the steady-state Discrete ARE solution.
  13. //
  14. // Ref: (1) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
  15. //          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
  16. //      (2) Kailath, "Linear Systems," Prentice-Hall, 1980.
  17. //      (3) Pappas, T., Laub, A. J., and Sandell, N. R., "On the Numerical
  18. //          Solution of the Discrete-Time Algebraic Riccati Equation," IEEE
  19. //          Trans. AC-25, 1980, pp. 631-641.
  20. //
  21. // Copyright (C), by Jeffrey B. Layton, 1994
  22. // Version JBL 940918
  23. //-------------------------------------------------------------------------------
  24.  
  25. rfile abcdchk
  26.  
  27. static (dare2_compute)    // Hide this function
  28.  
  29. dare2 = function(a,b,q,r,ct)
  30. {
  31.    local(nargs,msg,estr)
  32.  
  33. // Count number of input arguments
  34.    nargs=0;
  35.    if (exist(a)) {nargs=nargs+1;}
  36.    if (exist(b)) {nargs=nargs+1;}
  37.    if (exist(q)) {nargs=nargs+1;}
  38.    if (exist(r)) {nargs=nargs+1;}
  39.    if (exist(ct)) {nargs=nargs+1;}
  40.  
  41.    if (nargs < 4 ) {
  42.        error("DARE2: Wrong number of inputs");
  43.    }
  44.  
  45. // Check compatibility of A and B matrices
  46.    msg="";
  47.    msg=abcdchk(a,b);
  48.    if (msg != "") {
  49.        estr="DARE2: "+msg;
  50.        error(estr);
  51.    }
  52.  
  53. // Check if A and B are empty
  54.    if ( (!length(a)) || (!length(b)) ) {
  55.        error("DARE2: A and B matrices cannot be empty.");
  56.    }
  57.  
  58. // Check of Q is if proper dimensions
  59.    if ( (a.nr != q.nr) || (a.nc != q.nc) ) {
  60.     error("DARE2: A and Q must be the same size");
  61.    }
  62.  
  63. // Check if R is of proper dimensions
  64.    if ( (r.nr != r.nc) || (b.nc != r.nr) ) {
  65.     error("DARE2: B and R must be consistent");
  66.    }
  67.  
  68. // Check if Q is positive semi-definite and symmetric
  69.    if (!issymm(q)) {
  70.        printf("%s","dare2: Warning: Q is not symmetric.\n");
  71.    else
  72.        if (any(eig(q).val < -epsilon()*norm(q,"1")) ) {
  73.            printf("%s","dare2: Warning: Q is not positive semi-definite.\n");
  74.        }
  75.    }
  76.  
  77. // Check if R is positive definite and symmetric
  78.    if (!issymm(r)) {
  79.        printf("%s","dare2: Warning: R is not symmetric.\n");
  80.    else
  81.        if (any(eig(r).val < -epsilon()*norm(r,"1")) ) {
  82.            printf("%s","dare2: Warning: R is not positive semi-definite.\n");
  83.        }
  84.    }
  85.  
  86. //
  87. // Call dare2_compute with revised arguments. This prevents us from needlessly
  88. // making a copy if ct does not exist, and also prevents us from overwriting
  89. // a and q if ct exists.
  90. //
  91.  
  92.    if (exist(ct)) {
  93.        if ( ct.nr != a.nr || ct.nc != r.nc) {
  94.             error("dare2: CT must be consistent with Q and R");
  95.        }
  96.        return dare2_compute (a-solve(r',b')'*ct', b, ...
  97.                              q-solve(r',ct')'*ct', r, ct, a.nc);
  98.    else
  99.        return dare2_compute (a, b, q, r, zeros (a.nr, b.nc), a.nc );
  100.    }
  101. };
  102.  
  103. //----------------------------------------------------------------------------
  104. //
  105. // This is where the computation is performed. Note that dare2_compute is a
  106. // static variable and is never seen from the global workspace.
  107. //
  108.  
  109. dare2_compute = function (a, b, q, r, ct, n)
  110. {
  111.    local(G,A,v,d,index,e,Phi12,Phi22,p,Adum)
  112.  
  113. // Form G matrix (See the Ref.)
  114.    G=solve(r',b')'*b';
  115.  
  116. // Form the Hamiltonian and compute the eigenvectors
  117.    A=eig([a+G/a'*q,-G/a';-a'\q, inv(a)']);
  118.    v=A.vec;
  119.    d=A.val;
  120.  
  121. // Sort Eigenvectors by sorting eigenvalues and storing the index.
  122.    Adum=sort(abs(d));   // sort on magnitude of eigenvalues
  123.    index=Adum.ind;
  124.    e=Adum.val;
  125.    if ( !((e[n] < 1) && (e[n+1] > 1) ) ) {
  126.     error("dare2: Can't order eigenvalues, (A,B) may be uncontrollable.");
  127.    else
  128.         e=d[index[1:n]];
  129.    }
  130. // select vectors with eigenvalues inside unit circle
  131.    Phi12=v[1:n,index[1:n]];
  132.    Phi22=v[[n+1]:[2*n],index[1:n]];
  133.    p=real(Phi22/Phi12);
  134.  
  135.    return p
  136. };
  137.