home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / dlqr < prev    next >
Encoding:
Text File  |  1995-11-15  |  4.7 KB  |  162 lines
  1. //-------------------------------------------------------------------------------
  2. //
  3. // dlqr
  4. //
  5. // Syntax: A=dlqr(a,b,q,r,ct)
  6. //
  7. // This routine computes the Linear Quadratic Regulator Design for Discrete
  8. // Systems. It computes the optimal feedback gain matrix K such that the
  9. // feedback law, u[n] = -Kx[n]  minimizes the following cost function:
  10. //
  11. //       J = Sum {x'Qx + u'Ru}
  12. //
  13. // subject to the constraint equation:   
  14. //
  15. //       x[n+1] = Ax[n] + Bu[n] 
  16. //                
  17. // The solution P solves the steady-state discrete Riccatti equation:
  18. //
  19. //                              -1
  20. //       P - A'PA + A'PB(R+B'PB)  BP'A - Q = 0
  21. //
  22. // The routine returns the gains (G.k), the steady-state solution to the
  23. // discrete Riccatti equation (G.p), and the eigenvalues ofthe closed loop
  24. // system eig(A-B*K) (G.e).
  25. //
  26. // When the term CT is included, a Cross-Term is added to the cost
  27. // function which relates u to x in the following cost function:
  28. //
  29. //       J = Sum {x'Qx + u'Ru + 2*x'Nu}
  30. //
  31. //
  32. // Note: Three matrices are returned in a list.
  33. //
  34. //      A.k Optimal Feedback Gain Matrix.
  35. //      A.p Steady-State Solution to the Algebraic Riccatti Eqn.
  36. //      A.e Closed-Loop Eigenvalues
  37. //
  38. // Ref: (1) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
  39. //          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
  40. //      (2) Kailath, "Linear Systems," Prentice-Hall, 1980.
  41. //      (3) Pappas, T., Laub, A. J., and Sandell, N. R., "On the Numerical
  42. //          Solution of he Discrete-Time Algebraic Riccati Equation," IEEE
  43. //          Trans. AC-25, 1980, pp. 631-641.
  44. //
  45. // Copyright (C), by Jeffrey B. Layton, 1994
  46. // Version JBL 931215
  47. //-------------------------------------------------------------------------------
  48.  
  49. rfile abcdchk
  50.  
  51. static (dlqr_compute)    // Hide this function
  52.  
  53. dlqr = function(a,b,q,r,ct)
  54. {
  55.    local(nargs,msg,estr)
  56.  
  57. // Count number of Input Arguments
  58.    nargs=0;
  59.    if (exist(a)) {nargs=nargs+1;}
  60.    if (exist(b)) {nargs=nargs+1;}
  61.    if (exist(q)) {nargs=nargs+1;}
  62.    if (exist(r)) {nargs=nargs+1;}
  63.    if (exist(ct)) {nargs=nargs+1;}
  64.  
  65.    if (nargs < 4) {
  66.        error("DLQR: Wrong number of input arguments.");
  67.    }
  68.  
  69. // Check compatibility of A and B matrices
  70.    msg="";
  71.    msg=abcdchk(a,b);
  72.    if (msg != "") {
  73.        estr="DLQR: "+msg;
  74.        error(estr);
  75.    }
  76.  
  77. // Check if A and B are empty
  78.    if ( (!length(a)) || (!length(b)) ) {
  79.        error("DLQR: A and B matrices cannot be empty.");
  80.    }
  81.  
  82. // Check if Q is if proper dimensions
  83.    if ( (a.nr != q.nr) || (a.nc != q.nc) ) {
  84.     error("DLQR: A and Q must be the same size");
  85.    }
  86.  
  87. // Check if R is of proper dimensions
  88.    if ( (r.nr != r.nc) || (b.nc != r.nr) ) {
  89.     error("DLQR: B and R must be consistent");
  90.    }
  91.  
  92. // Check if Q is positive semi-definite and symmetric
  93.    if (!issymm(q)) {
  94.        printf("%s","DLQR: Warning: Q is not symmetric.\n");
  95.    else
  96.        if (any(eig(q).val < -epsilon()*norm(q,"1")) ) {
  97.            printf("%s","DLQR: Warning: Q is not positive semi-definite.\n");
  98.        }
  99.    }
  100.  
  101. // Check if R is positive definite and symmetric
  102.    if (!issymm(r)) {
  103.        printf("%s","DLQR: Warning: R is not symmetric.\n");
  104.    else
  105.        if (any(eig(r).val < -epsilon()*norm(r,"1")) ) {
  106.            printf("%s","DLQR: Warning: R is not positive semi-definite.\n");
  107.        }
  108.    }
  109.  
  110. //
  111. // Call dlqr_compute with revised arguments. This prevents us from needlessly
  112. // making a copy if ct does not exist, and also prevents us from overwriting
  113. // a and q if ct exists.
  114. //
  115.  
  116.    if (exist(ct)) {
  117.        if ( ct.nr != a.nr || ct.nc != r.nc) {
  118.             error("DLQR: CT must be consistent with Q and R");
  119.        }
  120.        return dlqr_compute (a-solve(r',b')'*ct', b, ...
  121.                             q-solve(r',ct')'*ct', r, ct, a.nc);
  122.    else
  123.        return dlqr_compute (a, b, q, r, zeros (a.nr, b.nc), a.nc );
  124.    }
  125. };
  126.  
  127. //----------------------------------------------------------------------------
  128. //
  129. // This is where the computation is performed. Note that dlqr_compute is a
  130. // static variable and is never seen from the global workspace.
  131. //
  132.  
  133. dlqr_compute = function (a, b, q, r, ct, n)
  134. {
  135.    local(G,A,v,d,index,e,Phi12,Phi22,p,k,Adum)
  136.  
  137. // Form G matrix (See the Ref.)
  138.    G=solve(r',b')'*b';
  139.  
  140. // Form the Hamiltonian and compute the eigenvectors
  141.    A=eig([a+G/a'*q,-G/a';-a'\q, inv(a)']);
  142.    v=A.vec;
  143.    d=A.val;
  144.  
  145. // Sort Eigenvectors by sorting eigenvalues and storing the index.
  146.    Adum=sort(abs(d));   // sort on magnitude of eigenvalues
  147.    index=Adum.ind;
  148.    e=Adum.val;
  149.    if ( !((e[n] < 1) && (e[n+1] > 1) ) ) {
  150.     error("DLQR: Can't order eigenvalues, (A,B) may be uncontrollable.");
  151.    else
  152.         e=d[index[1:n]];
  153.    }
  154. // select vectors with eigenvalues inside unit circle
  155.    Phi12=v[1:n,index[1:n]];
  156.    Phi22=v[[n+1]:[2*n],index[1:n]];
  157.    p=real(Phi22/Phi12);
  158.    k=solve(r+b'*p*b,b')*p*a + r\ct';
  159.  
  160.    return << k=k; p=p; e=e >>
  161. };
  162.