home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / LQRD.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  2.7 KB  |  82 lines
  1. function [k,s,e] = lqrd(a,b,q,r,nn,Ts)
  2. %LQRD   Discrete linear quadratic regulator design from continuous 
  3. %       cost function.
  4. %    [K,S,E] = LQRD(A,B,Q,R,Ts) calculates the optimal feedback gain 
  5. %    matrix K such that the discrete feedback law  u[n] = -K x[n] 
  6. %    minimizes a discrete cost function equivalent to the continuous 
  7. %    cost function
  8. %        J = Integral {x'Qx + u'Ru} dt
  9. %                                                       .
  10. %    subject to the continuous constraint equation:  x = Ax + Bu
  11. %
  12. %    Also returned is S, the discrete Riccati equation solution, and 
  13. %    the closed loop eigenvalues E = EIG(Ad-Bd*K).
  14. %
  15. %    The gain matrix is determined by discretizing the continuous plant
  16. %    (A,B,C,D) and continuous weighting matrices (Q,R) using the sample
  17. %    time Ts and the zero order hold approximation. The gain matrix is
  18. %    then calculated using DLQR.
  19. %
  20. %    [K,S,E] = LQRD(A,B,Q,R,N,Ts) includes the cross-term N that 
  21. %    relates u to x in the cost function.
  22. %        J = Integral {x'Qx + u'Ru + 2*x'Nu}
  23. %
  24. %    See also: C2D, LQED, DLQR, and LQR.
  25.  
  26. %    Clay M. Thompson 7-16-90
  27. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  28.  
  29. % Reference: This routine is based on the routine JDEQUIV.M by Franklin, 
  30. % Powell and Workman and is described on pp. 439-441 of "Digital Control
  31. % of Dynamic Systems".
  32.  
  33. error(nargchk(5,6,nargin));
  34. error(abcdchk(a,b));
  35. [nx,na] = size(a); 
  36. [nb,nu] = size(b);
  37.  
  38. [nq,mq] = size(q);
  39. if (nx ~= nq) | (nx ~= mq), error('A and Q must be the same size.'); end
  40. [nr,mr] = size(r);
  41. if (mr ~= nr) | (nu ~= mr), error('B and R must be consistent.'); end
  42.  
  43. if nargin==5,
  44.   Ts = nn;
  45.   nn = zeros(nb,nu);
  46. else
  47.   [nnn,mn] = size(nn);
  48.   if (nnn ~= nx) | (mn ~= nu), error('N must be consistent with Q and R.'); end
  49. end
  50.  
  51. % Check if q is positive semi-definite and symmetric
  52. if any(eig(q) < -eps) | (norm(q'-q,1)/norm(q,1) > eps)
  53.   disp('Warning: Q is not symmetric and positive semi-definite');
  54. end
  55. % Check if r is positive definite and symmetric
  56. if any(eig(r) <= -eps) | (norm(r'-r,1)/norm(r,1) > eps)
  57.   disp('Warning: R is not symmetric and positive definite');
  58. end
  59.  
  60. % Discretize the state-space system.
  61. [ad,bd] = c2d(a,b,Ts);
  62.  
  63. % --- Determine discrete equivalent of continuous cost function ---
  64. n = nx+nu;
  65. Za = zeros(nx); Zb = zeros(nx,nu); Zu = zeros(nu);
  66. M = [ -a' Zb   q  nn
  67.       -b' Zu  nn'  r
  68.       Za  Zb   a   b
  69.       Zb' Zu  Zb' Zu];
  70. phi = expm(M*Ts);
  71. phi12 = phi(1:n,n+1:2*n);
  72. phi22 = phi(n+1:2*n,n+1:2*n);
  73. QQ = phi22'*phi12;
  74. QQ = (QQ+QQ')/2;        % Make sure QQ is symmetric
  75. Qd = QQ(1:nx,1:nx);
  76. Rd = QQ(nx+1:n,nx+1:n);
  77. Nd = QQ(1:nx,nx+1:n);
  78.  
  79. % Design the gain matrix using the discrete plant and discrete cost function
  80. [k,s,e] = dlqr(ad,bd,Qd,Rd,Nd);
  81.  
  82.