home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / LQR.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  2.3 KB  |  84 lines
  1. function [k,s,e]=lqr(a,b,q,r,nn)
  2. %LQR    Linear quadratic regulator design for continuous systems.
  3. %    [K,S,E] = LQR(A,B,Q,R)  calculates the optimal feedback gain 
  4. %    matrix K such that the feedback law  u = -Kx  minimizes the cost
  5. %    function:
  6. %        J = Integral {x'Qx + u'Ru} dt
  7. %
  8. %    subject to the constraint equation: 
  9. %        .
  10. %        x = Ax + Bu 
  11. %                
  12. %    Also returned is S, the steady-state solution to the associated 
  13. %    algebraic Riccati equation and the closed loop eigenvalues E:
  14. %                  -1
  15. %        0 = SA + A'S - SBR  B'S + Q     E = EIG(A-B*K)
  16. %
  17. %    [K,S,E] = LQR(A,B,Q,R,N) includes the cross-term N that relates
  18. %    u to x in the cost function.
  19. %
  20. %        J = Integral {x'Qx + u'Ru + 2*x'Nu}
  21. %
  22. %    The controller can be formed with REG.
  23. %
  24. %    See also: LQRY, LQR2, and REG.
  25.  
  26. %    J.N. Little 4-21-85
  27. %    Revised 8-27-86 JNL
  28. %    Revised 7-18-90 Clay M. Thompson
  29. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  30.  
  31. error(nargchk(4,5,nargin));
  32. error(abcdchk(a,b));
  33. if ~length(a) | ~length(b)
  34.         error('A and B matrices cannot be empty.')
  35. end
  36.  
  37. [m,n] = size(a);
  38. [mb,nb] = size(b);
  39. [mq,nq] = size(q);
  40. if (m ~= mq) | (n ~= nq) 
  41.     error('A and Q must be the same size');
  42. end
  43. [mr,nr] = size(r);
  44. if (mr ~= nr) | (nb ~= mr)
  45.     error('B and R must be consistent');
  46. end
  47.  
  48. if nargin == 5
  49.     [mn,nnn] = size(nn);
  50.     if (mn ~= m) | (nnn ~= nr)
  51.         error('N must be consistent with Q and R');
  52.     end
  53.     % Add cross term
  54.     q = q - nn/r*nn';
  55.     a = a - b/r*nn';
  56. else
  57.     nn = zeros(m,nb);
  58. end
  59.  
  60. % Check if q is positive semi-definite and symmetric
  61. nq = norm(q,1);
  62. if any(eig(q) < -eps*nq) | (norm(q'-q,1)/nq > eps)
  63.     disp('Warning: Q is not symmetric and positive semi-definite');
  64. end
  65. % Check if r is positive definite and symmetric
  66. nr = norm(r,1);
  67. if any(eig(r) <= -eps*nr) | (norm(r'-r,1)/nr > eps)
  68.     disp('Warning: R is not symmetric and positive definite');
  69. end
  70.  
  71. % Start eigenvector decomposition by finding eigenvectors of Hamiltonian:
  72. [v,d] = eig([a b/r*b';q, -a']);
  73. d = diag(d);
  74. [e,index] = sort(real(d));     % sort on real part of eigenvalues
  75. if (~( (e(n)<0) & (e(n+1)>0) ))
  76.     error('Can''t order eigenvalues, (A,B) may be uncontrollable.');
  77. else
  78.   e = d(index(1:n));         % Return closed-loop eigenvalues
  79. end
  80. chi = v(1:n,index(1:n));     % select vectors with negative eigenvalues
  81. lambda = v((n+1):(2*n),index(1:n));
  82. s = -real(lambda/chi);
  83. k = r\(nn'+b'*s);
  84.