home *** CD-ROM | disk | FTP | other *** search
/ The Datafile PD-CD 5 / DATAFILE_PDCD5.iso / utilities / r / rlab / CTB / dlqr2 < prev    next >
Encoding:
Text File  |  1995-11-15  |  4.5 KB  |  157 lines
  1. //-------------------------------------------------------------------------------
  2. //
  3. // dlqr2
  4. //
  5. // Syntax: A=dlqr2(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. // Note: Three matrices are returned in a list.
  32. //
  33. //      A.k Optimal Feedback Gain Matrix.
  34. //      A.p Steady-State Solution to the Algebraic Riccatti Eqn.
  35. //      A.e Closed-Loop Eigenvalues
  36. //
  37. // Ref: (1) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
  38. //          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
  39. //
  40. // Copyright (C), by Jeffrey B. Layton, 1994
  41. // Version JBL 940918
  42. //-------------------------------------------------------------------------------
  43.  
  44. rfile abcdchk
  45.  
  46. static (dlqr2_compute)    // Hide this function
  47.  
  48. dlqr2 = function(a,b,q,r,ct)
  49. {
  50.    local(nargs,msg,estr)
  51.  
  52. // Count number of inputs arguments
  53.    nargs=0;
  54.    if (exist(a)) {nargs=nargs+1;}
  55.    if (exist(b)) {nargs=nargs+1;}
  56.    if (exist(q)) {nargs=nargs+1;}
  57.    if (exist(r)) {nargs=nargs+1;}
  58.    if (exist(ct)) {nargs=nargs+1;}
  59.  
  60.    if (nargs < 4) {
  61.        error("DLQR2: Wrong number of arguments.");
  62.    }
  63.  
  64. // Check compatibility of A and B matrices
  65.    msg="";
  66.    msg=abcdchk(a,b);
  67.    if (msg != "") {
  68.        estr="DLQR2: "+msg;
  69.        error(estr);
  70.    }
  71.  
  72. // Check if A and B are empty
  73.    if ( (!length(a)) || (!length(b)) ) {
  74.        error("DLQR2: A and B matrices cannot be empty.");
  75.    }
  76.  
  77. // Check of Q is if proper dimensions
  78.    if ( (a.nr != q.nr) || (a.nc != q.nc) ) {
  79.     error("DLQR2: A and Q must be the same size");
  80.    }
  81.  
  82. // Check if R is of proper dimensions
  83.    if ( (r.nr != r.nc) || (b.nc != r.nr) ) {
  84.     error("DLQR2: B and R must be consistent");
  85.    }
  86.  
  87. // Check if Q is positive semi-definite and symmetric
  88.    if (!issymm(q)) {
  89.        printf("%s","DLQR2: Warning: Q is not symmetric.\n");
  90.    else
  91.        if (any(eig(q).val < -epsilon()*norm(q,1)) ) {
  92.            printf("%s","DLQR2: Warning: Q is not positive semi-definite.\n");
  93.        }
  94.    }
  95.  
  96. // Check if R is positive definite and symmetric
  97.    if (!issymm(r)) {
  98.        printf("%s","DLQR2: Warning: R is not symmetric.\n");
  99.    else
  100.        if (any(eig(r).val < -epsilon()*norm(r,1)) ) {
  101.            printf("%s","DLQR2: Warning: R is not positive semi-definite.\n");
  102.        }
  103.    }
  104.  
  105. //
  106. // Call dlqr2_compute with revised arguments. This prevents us from needlessly
  107. // making a copy if ct does not exist, and also prevents us from overwriting
  108. // a and q if ct exists.
  109. //
  110.  
  111.    if (exist(ct)) {
  112.        if ( ct.nr != a.nr || ct.nc != r.nc) {
  113.             error("DLQR2: CT must be consistent with Q and R");
  114.        }
  115.        return dlqr2_compute (a-solve(r',b')'*ct', b, ...
  116.                            q-solve(r',ct')'*ct', r, ct);
  117.    else
  118.        return dlqr2_compute (a, b, q, r, zeros (a.nr, b.nc));
  119.    }
  120. };
  121.  
  122. //----------------------------------------------------------------------------
  123. //
  124. // This is where the computation is performed. Note that dlqr_compute is a
  125. // static variable and is never seen from the global workspace.
  126. //
  127.  
  128. dlqr2_compute = function (a, b, q, r, ct)
  129. {
  130.    local(G,A,v,d,index,e,Phi12,Phi22,p,k)
  131.  
  132. // Form G matrix (See the Ref.)
  133.    G=solve(r',b')'*b';
  134.  
  135. // Form the Hamiltonian and compute the eigenvectors
  136.    A=eig([a+G/a'*q,-G/a';-a'\q, inv(a)']);
  137.    v=A.vec;
  138.    d=A.val;
  139.  
  140. // Sort Eigenvectors by sorting eigenvalues and storing the index.
  141.    d=diag(d);
  142.    index=sort(abs(d));   // sort on magnitude of eigenvalues
  143.    e=real(d[ index ] );
  144.    if ( !((e[a.nc] < 1) && (e[a.nc+1] > 1) ) ) {
  145.     error("DLQR2: Can't order eigenvalues, (A,B) may be uncontrollable.");
  146.    else
  147.         e=d[index[1:a.nc]];
  148.    }
  149. // select vectors with eigenvalues inside unit circle
  150.    Phi12=v[1:a.nc,index[1:a.nc]];
  151.    Phi22=v([a.nc+1]:[2*a.nc],index[1:a.nc]);
  152.    p=real(Phi22/Phi12);
  153.    k=(r+b'*s*b)\b'*s*a + r\ct';
  154.  
  155.    return << k=k; p=p; e=e >>
  156. };
  157.