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