home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21eb.zip / octave / SCRIPTS.ZIP / scripts.fat / control / lqr.m < prev    next >
Text File  |  1999-04-29  |  3KB  |  110 lines
  1. # Copyright (C) 1993, 1994, 1995 John W. Eaton
  2. # 
  3. # This file is part of Octave.
  4. # 
  5. # Octave is free software; you can redistribute it and/or modify it
  6. # under the terms of the GNU General Public License as published by the
  7. # Free Software Foundation; either version 2, or (at your option) any
  8. # later version.
  9. # 
  10. # Octave is distributed in the hope that it will be useful, but WITHOUT
  11. # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  12. # FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  13. # for more details.
  14. # 
  15. # You should have received a copy of the GNU General Public License
  16. # along with Octave; see the file COPYING.  If not, write to the Free
  17. # Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  
  19. function [k, p, e] = lqr (a, b, q, r, s)
  20.  
  21. # Usage: [k, p, e] = lqr (A, B, Q, R {,S})
  22. #
  23. # Linear quadratic regulator design for the continuous time system
  24. #   dx/dt = A x + B u
  25. # to minimize the cost functional
  26. #
  27. #  J = int_0^\infty{ [x' u'] [Q  S'; S R] [x ; u] dt}
  28. #
  29. # inputs:
  30. #   A, B: coefficient matrices for continuous time system
  31. #   Q, R, S: cost functional coefficient matrices.
  32. #      Q: must be nonnegative definite.
  33. #      R: must be positive definite
  34. #      S: defaults to 0
  35. # 
  36. # if S is omitted, the optimization simplifies to the usual
  37. #
  38. #  J = int_0^\infty{ x' Q x + u' R u }
  39. #
  40. # Returns:
  41. #
  42. #   k = state feedback gain, (A - B K) is stable and minimizes the
  43. #       cost functional
  44. #   p = solution of algebraic Riccati equation
  45. #   e = closed loop poles of (A - B K)
  46. #
  47. # reference: Anderson and Moore, OPTIMAL CONTROL: LINEAR QUADRATIC METHODS,
  48. # Prentice-Hall, 1990, pp. 56-58
  49.  
  50.  
  51. # Written by A. S. Hodel (scotte@eng.auburn.edu) August 1993.
  52.  
  53.   # disp("lqr: entry");
  54.  
  55.   if ((nargin != 4) && (nargin != 5))
  56.     error ("lqr: invalid number of arguments");
  57.   endif
  58.  
  59. # Check a.
  60.   if ((n = is_sqr (a)) == 0)
  61.     error ("lqr: requires 1st parameter(a) to be square");
  62.   endif
  63.  
  64. # Check b.
  65.   [n1, m] = size (b);
  66.   if (n1 != n)
  67.     error ("lqr: a,b not conformal");
  68.   endif
  69.  
  70. # Check q.
  71.   
  72.   if ( ((n1 = is_sqr (q)) == 0) || (n1 != n))
  73.     error ("lqr: q must be square and conformal with a");
  74.   endif
  75.  
  76. # Check r.
  77.   if ( ((m1 = is_sqr(r)) == 0) || (m1 != m))
  78.     error ("lqr: r must be square and conformal with column dimension of b");
  79.   endif
  80.  
  81. # Check if n is there.
  82.   if (nargin == 5)
  83.     [n1, m1] = size (s);
  84.     if ( (n1 != n) || (m1 != m))
  85.       error ("lqr: z must be identically dimensioned with b");
  86.     endif
  87.  
  88. # Incorporate cross term into a and q.
  89.     ao = a - (b/r)*s';
  90.     qo = q - (s/r)*s';
  91.   else
  92.     s = zeros (n, m);
  93.     ao = a;
  94.     qo = q;
  95.   endif
  96.  
  97. # Check that q, (r) are symmetric, positive (semi)definite
  98.  
  99.   if (is_symm (q) && is_symm (r) ...
  100.       && all (eig (q) >= 0) && all (eig (r) > 0))
  101.     p = are (ao, (b/r)*b', qo);
  102.     k = r\(b'*p + s');
  103.     e = eig (a - b*k);
  104.   else
  105.     error ("lqr: q (r) must be symmetric positive (semi) definite");
  106.   endif
  107.  
  108.   # disp("lqr: exit");
  109. endfunction
  110.