home *** CD-ROM | disk | FTP | other *** search
/ Geek Gadgets 1 / ADE-1.bin / ade-bin / lib / octave / 1.1.1 / m / control / dlqr.m < prev    next >
Encoding:
Text File  |  1996-10-12  |  2.6 KB  |  100 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] = dlqr (a, b, q, r, zz)
  20.  
  21. # Usage: [k, p, e] = dlqr (A, B, Q, R {,Z})
  22. #
  23. # Linear quadratic regulator design for the discrete time system
  24. #
  25. #   x[k+1] = A x[k] + B u[k]
  26. #
  27. # to minimize the cost functional
  28. #
  29. #  J = Sum { x' Q x + u' R u }             Z omitted
  30. #
  31. # or
  32. #
  33. #  J = Sum { x' Q x + u' R u +2 x' Z u}        Z included
  34. #
  35. # Returns:
  36. #
  37. #   k = state feedback gain, (A - B K) is stable
  38. #   p = solution of algebraic Riccati equation
  39. #   e = closed loop poles of (A - B K)
  40.  
  41. # Written by A. S. Hodel (scotte@eng.auburn.edu) August 1993.
  42. # Converted to discrete time by R. B. Tenison
  43. # (btenison@eng.auburn.edu) October 1993
  44.  
  45.   if (nargin != 4 && nargin != 5)
  46.     error ("dlqr: invalid number of arguments");
  47.   endif
  48.  
  49. # Check a.
  50.   if ((n = is_square (a)) == 0)
  51.     error ("dlqr: requires 1st parameter(a) to be square");
  52.   endif
  53.  
  54. # Check b.
  55.   [n1, m] = size (b);
  56.   if (n1 != n)
  57.     error ("dlqr: a,b not conformal");
  58.   endif
  59.  
  60. # Check q.
  61.   
  62.   if ((n1 = is_square (q)) == 0 || n1 != n)
  63.     error ("dlqr: q must be square and conformal with a");
  64.   endif
  65.  
  66. # Check r.
  67.   if((m1 = is_square(r)) == 0 || m1 != m)
  68.     error ("dlqr: r must be square and conformal with column dimension of b");
  69.   endif
  70.  
  71. # Check if n is there.
  72.   if (nargin == 5)
  73.     [n1, m1] = size (zz);
  74.     if (n1 != n || m1 != m)
  75.       error ("dlqr: z must be identically dimensioned with b");
  76.     endif
  77.  
  78. # Incorporate cross term into a and q.
  79.  
  80.     ao = a - (b/r)*zz';
  81.     qo = q - (zz/r)*zz';
  82.   else
  83.     zz = zeros (n, m);
  84.     ao = a;
  85.     qo = q;
  86.   endif
  87.  
  88. # Check that q, (r) are symmetric, positive (semi)definite
  89.  
  90.   if (is_symmetric (q) && is_symmetric (r) ...
  91.       && all (eig (q) >= 0) && all (eig (r) > 0))
  92.     p = dare (ao, b, qo, r);
  93.     k = (r+b'*p*b)\b'*p*a + r\zz';
  94.     e = eig (a - b*k);
  95.   else
  96.     error ("dlqr: q (r) must be symmetric positive (semi) definite");
  97.   endif
  98.  
  99. endfunction
  100.