The Datafile PD-CD 5/DATAFILE_PDCD5.iso/utilities/r/rlab/CTB/lqr

Text File | 1995-11-15 | 4.7 KB | 174 lines

//----------------------------------------------------------------------------
//
// lqr
//
// Syntax: G=lqr(A,B,Q,R,CT)
//
// This routine computes the Linear Quadratic Deisgn for Continuous Systems.
// It calculates the optimal feedback gain matrix K such that the feedback
// law u = -Kx  minimizes the following cost function:
//
//           Inf
//     J = Integral (x'Qx + u'Ru) dt
//            0
//
// subject to the constraint
//   .
//   x = Ax + Bu
//
// The solution P solves the following equation:
//
//                 -1
//   A'P + PA - PBR  B'P + Q = 0
//
// The routine returns the gains (G.k), the steady-solution to the Algebraic
// Riccatti Eqn. (G.p) and the eigenvalues of the closed loop system
// eig(A-B*K) (G.e).
//
// When the term CT is included, a Cross-Term is added that relates u to x
// in the following cost function:
//
//           Inf
//     J = Integral (x'Qx + u'Ru + 2*x'CTu) dt
//            0
//
// Note: Three matrices are returned in a list.
//
//       G.k = Optimal Feedback Gain matrix
//       G.p = Steady-State Solution to the Algebraic Riccatti Eqn.
//       G.e = Closed Loop eigenvalues.
//
// Ref: (1) Kailath, "Linear Systems," Prentice-Hall, 1980
//      (2) Laub, A. J., "A Schur Method for Solving Algebraic Riccati
//          Equations," IEEE Trans. AC-24, 1979, pp. 913-921.
//      (3) Junkins, J., and Kim, Y., "Introduction to Dynamics and Control
//          of Flexible Structures," AIAA Inc, Washington D.C., 1993.
//
// Copyright (C), by Jeffrey B. Layton, 1994
// Version JBL 940925
//----------------------------------------------------------------------------

rfile abcdchk
rfile ric

static (lqr_compute)    // Hide this function

lqr = function(a,b,q,r,ct)
{
   local (msg,estr)

// Count number of input arguments
   nargs=0;
   if (exist(a)) {nargs=nargs+1;}
   if (exist(b)) {nargs=nargs+1;}
   if (exist(q)) {nargs=nargs+1;}
   if (exist(r)) {nargs=nargs+1;}
   if (exist(ct)) {nargs=nargs+1;}

   if (nargs < 4 ) {
       error("LQR: Wrong number of input arguments.");
   }

// Check if Q and R are consistent.
   if ( a.nr != q.nr || a.nc != q.nc ) {
       error("LQR: A and Q must be the same size");
   }

   if ( r.nr != r.nc || b.nc != r.nr ) {
       error("LQR: B and R must be consistent");
   }

// Check if A and B are empty
   if ( (!length(a)) || (!length(b)) ) {
       error("LQR: A and B matrices cannot be empty.");
   }

// A has to be square
   if (a.nr != a.nc) {
       error("LQR: A has to be square.");
   }

// See if A and B are compatible.
   msg="";
   msg=abcdchk(a,b);
   if (msg != "") {
       estr="LQR: "+msg;
       error(estr);
   }

// Check if Q is positive semi-definite and symmetric
   if (!issymm(q)) {
       printf("%s","LQR: Warning: Q is not symmetric.\n");
   else
       if (any(eig(q).val < -epsilon()*norm(q,"1")) ) {
           printf("%s","LQR: Warning: Q is not positive semi-definite.\n");
       }
   }

// Check if R is positive definite and symmetric
   if (!issymm(r)) {
       printf("%s","LQR: Warning: R is not symmetric.\n");
   else
       if (any(eig(r).val < -epsilon()*norm(r,"1")) ) {
           printf("%s","LQR: Warning: R is not positive semi-definite.\n");
       }
   }

//
// Call lqr_compute with revised arguments. This prevents us from needlessly
// making a copy if ct does not exist, and also prevents us from overwriting
// a and q if ct exists.
//

// If Cross-Term exists, modify Q and A.
   if (exist(ct)) {
       if ( ct.nr != a.nr || ct.nc != r.nc) {
            error("LQR: CT must be consistent with Q and R");
       }
       return lqr_compute (a-solve(r',b')'*ct', b, ...
                           q-solve(r',ct')'*ct', r, ct);
   else
       return lqr_compute (a, b, q, r, zeros (a.nr, b.nc));
   }
};

//----------------------------------------------------------------------------
//
// This is where the computation is performed. Note that lqr_compute is a
// static variable and is never seen from the global workspace.
//

lqr_compute = function (a, b, q, r, ct)
{
   local (Phi12, Phi22, d, e, index, k, p, v)

// Start solution by finding the spectral decomposition and eigenvectors
// of Hamiltonian.

   e=eig( [ a, solve(r',b')'*b'; q, -a' ] );
   v=e.vec;
   d=e.val;

// Sort eigenvectors by sorting eigenvalues (and storing the index).
   index=sort( real( d ) ).ind;
   d=real( d[ index ] );

   if ( !( d[a.nc] < 0 && d[a.nc+1] > 0 ) ) {
       error("lqr: Can't order eigenvalues");
   }

// Form the Partitions of the PHI matrix and solve for P and then the
// Control Gains.
   Phi12=v[ 1:a.nc; index[1:a.nc] ];
   Phi22=v[ (a.nc+1):(2*a.nc); index[1:a.nc] ];
   p=-real(solve(Phi12',Phi22')');

// Compute Control Gains
   k=solve( r, ct'+b'*p );

// Compute eigenvalues
   e=d;

   return << k=k; p=p; e=e >>;
};