home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21fb.zip / octave / SCRIPTS.ZIP / scripts / control / lqr.m < prev    next >
Encoding:
Text File  |  1999-12-15  |  3.8 KB  |  174 lines
  1. ## Copyright (C) 1993, 1994, 1995 Auburn University.  All Rights Reserved
  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, 59 Temple Place, Suite 330, Boston, MA 02111 USA.
  18.  
  19. ## -*- texinfo -*-
  20. ## @deftypefn {Function File} {[@var{k}, @var{p}, @var{e}] =} lqr (@var{a}, @var{b}, @var{q}, @var{r}, @var{z})
  21. ## construct the linear quadratic regulator for the continuous time system
  22. ## @iftex
  23. ## @tex
  24. ## $$
  25. ##  {dx\over dt} = A x + B u
  26. ## $$
  27. ## @end tex
  28. ## @end iftex
  29. ## @ifinfo
  30. ## 
  31. ## @example
  32. ## dx
  33. ## -- = A x + B u
  34. ## dt
  35. ## @end example
  36. ## 
  37. ## @end ifinfo
  38. ## to minimize the cost functional
  39. ## @iftex
  40. ## @tex
  41. ## $$
  42. ##  J = \int_0^\infty x^T Q x + u^T R u
  43. ## $$
  44. ## @end tex
  45. ## @end iftex
  46. ## @ifinfo
  47. ## 
  48. ## @example
  49. ##       infinity
  50. ##       /
  51. ##   J = |  x' Q x + u' R u
  52. ##      /
  53. ##     t=0
  54. ## @end example
  55. ## @end ifinfo
  56. ## 
  57. ## @noindent
  58. ## @var{z} omitted or
  59. ## @iftex
  60. ## @tex
  61. ## $$
  62. ##  J = \int_0^\infty x^T Q x + u^T R u + 2 x^T Z u
  63. ## $$
  64. ## @end tex
  65. ## @end iftex
  66. ## @ifinfo
  67. ## 
  68. ## @example
  69. ##       infinity
  70. ##       /
  71. ##   J = |  x' Q x + u' R u + 2 x' Z u
  72. ##      /
  73. ##     t=0
  74. ## @end example
  75. ## 
  76. ## @end ifinfo
  77. ## @var{z} included.
  78. ## 
  79. ## The following values are returned:
  80. ## 
  81. ## @table @var
  82. ## @item k
  83. ## The state feedback gain,
  84. ## @iftex
  85. ## @tex
  86. ## $(A - B K)$
  87. ## @end tex
  88. ## @end iftex
  89. ## @ifinfo
  90. ## (@var{a} - @var{b}@var{k})
  91. ## @end ifinfo
  92. ## is stable and minimizes the cost functional
  93. ## 
  94. ## @item p
  95. ## The stabilizing solution of appropriate algebraic Riccati equation.
  96. ## 
  97. ## @item e
  98. ## The vector of the closed loop poles of
  99. ## @iftex
  100. ## @tex
  101. ## $(A - B K)$.
  102. ## @end tex
  103. ## @end iftex
  104. ## @ifinfo
  105. ## (@var{a} - @var{b}@var{k}).
  106. ## @end ifinfo
  107. ## @end table
  108. ##
  109. ## @strong{Reference} 
  110. ## Anderson and Moore, OPTIMAL CONTROL: LINEAR QUADRATIC METHODS,
  111. ## Prentice-Hall, 1990, pp. 56-58
  112. ## @end deftypefn
  113.  
  114. function [k, p, e] = lqr (a, b, q, r, s)
  115.  
  116.   ## Written by A. S. Hodel (scotte@eng.auburn.edu) August 1993.
  117.  
  118.   ## disp("lqr: entry");
  119.  
  120.   if ((nargin != 4) && (nargin != 5))
  121.     error ("lqr: invalid number of arguments");
  122.   endif
  123.  
  124.   ## Check a.
  125.   if ((n = is_square (a)) == 0)
  126.     error ("lqr: requires 1st parameter(a) to be square");
  127.   endif
  128.  
  129.   ## Check b.
  130.   [n1, m] = size (b);
  131.   if (n1 != n)
  132.     error ("lqr: a,b not conformal");
  133.   endif
  134.  
  135.   ## Check q.
  136.   if ( ((n1 = is_square (q)) == 0) || (n1 != n))
  137.     error ("lqr: q must be square and conformal with a");
  138.   endif
  139.  
  140.   ## Check r.
  141.   if ( ((m1 = is_square(r)) == 0) || (m1 != m))
  142.     error ("lqr: r must be square and conformal with column dimension of b");
  143.   endif
  144.  
  145.   ## Check if n is there.
  146.   if (nargin == 5)
  147.     [n1, m1] = size (s);
  148.     if ( (n1 != n) || (m1 != m))
  149.       error ("lqr: z must be identically dimensioned with b");
  150.     endif
  151.  
  152.     ## Incorporate cross term into a and q.
  153.     ao = a - (b/r)*s';
  154.     qo = q - (s/r)*s';
  155.   else
  156.     s = zeros (n, m);
  157.     ao = a;
  158.     qo = q;
  159.   endif
  160.  
  161.   ## Check that q, (r) are symmetric, positive (semi)definite
  162.  
  163.   if (is_symmetric (q) && is_symmetric (r) ...
  164.       && all (eig (q) >= 0) && all (eig (r) > 0))
  165.     p = are (ao, (b/r)*b', qo);
  166.     k = r\(b'*p + s');
  167.     e = eig (a - b*k);
  168.   else
  169.     error ("lqr: q (r) must be symmetric positive (semi) definite");
  170.   endif
  171.  
  172.   ## disp("lqr: exit");
  173. endfunction
  174.