home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21fb.zip / octave / SCRIPTS.ZIP / scripts.fat / control / dlqr.m < prev    next >
Text File  |  1999-12-24  |  4KB  |  167 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}] =} dlqr (@var{a}, @var{b}, @var{q}, @var{r}, @var{z})
  21. ## Construct the linear quadratic regulator for the discrete time system
  22. ## @iftex
  23. ## @tex
  24. ## $$
  25. ##  x_{k+1} = A x_k + B u_k
  26. ## $$
  27. ## @end tex
  28. ## @end iftex
  29. ## @ifinfo
  30. ## 
  31. ## @example
  32. ## x[k+1] = A x[k] + B u[k]
  33. ## @end example
  34. ## 
  35. ## @end ifinfo
  36. ## to minimize the cost functional
  37. ## @iftex
  38. ## @tex
  39. ## $$
  40. ##  J = \sum x^T Q x + u^T R u
  41. ## $$
  42. ## @end tex
  43. ## @end iftex
  44. ## @ifinfo
  45. ## 
  46. ## @example
  47. ## J = Sum (x' Q x + u' R u)
  48. ## @end example
  49. ## @end ifinfo
  50. ## 
  51. ## @noindent
  52. ## @var{z} omitted or
  53. ## @iftex
  54. ## @tex
  55. ## $$
  56. ##  J = \sum x^T Q x + u^T R u + 2 x^T Z u
  57. ## $$
  58. ## @end tex
  59. ## @end iftex
  60. ## @ifinfo
  61. ## 
  62. ## @example
  63. ## J = Sum (x' Q x + u' R u + 2 x' Z u)
  64. ## @end example
  65. ## 
  66. ## @end ifinfo
  67. ## @var{z} included.
  68. ## 
  69. ## The following values are returned:
  70. ## 
  71. ## @table @var
  72. ## @item k
  73. ## The state feedback gain,
  74. ## @iftex
  75. ## @tex
  76. ## $(A - B K)$
  77. ## @end tex
  78. ## @end iftex
  79. ## @ifinfo
  80. ## (@var{a} - @var{b}@var{k})
  81. ## @end ifinfo
  82. ## is stable.
  83. ## 
  84. ## @item p
  85. ## The solution of algebraic Riccati equation.
  86. ## 
  87. ## @item e
  88. ## The closed loop poles of
  89. ## @iftex
  90. ## @tex
  91. ## $(A - B K)$.
  92. ## @end tex
  93. ## @end iftex
  94. ## @ifinfo
  95. ## (@var{a} - @var{b}@var{k}).
  96. ## @end ifinfo
  97. ## @end table
  98. ## @strong{References}
  99. ## @enumerate
  100. ## @item Anderson and Moore, Optimal Control: Linear Quadratic Methods,
  101. ##      Prentice-Hall, 1990, pp. 56-58
  102. ## @item  Kuo, Digital Control Systems, Harcourt Brace Jovanovich, 1992, 
  103. ##      section 11-5-2.
  104. ## @end enumerate
  105. ## @end deftypefn
  106.  
  107. function [k, p, e] = dlqr (a, b, q, r, s)
  108. ## Written by A. S. Hodel (scotte@eng.auburn.edu) August 1993.
  109. ## Converted to discrete time by R. B. Tenison
  110. ## (btenison@eng.auburn.edu) October 1993
  111.  
  112.   if (nargin != 4 && nargin != 5)
  113.     error ("dlqr: invalid number of arguments");
  114.   endif
  115.  
  116. ## Check a.
  117.   if ((n = is_sqr (a)) == 0)
  118.     error ("dlqr: requires 1st parameter(a) to be square");
  119.   endif
  120.  
  121. ## Check b.
  122.   [n1, m] = size (b);
  123.   if (n1 != n)
  124.     error ("dlqr: a,b not conformal");
  125.   endif
  126.  
  127. ## Check q.
  128.   
  129.   if ((n1 = is_sqr (q)) == 0 || n1 != n)
  130.     error ("dlqr: q must be square and conformal with a");
  131.   endif
  132.  
  133. ## Check r.
  134.   if((m1 = is_sqr(r)) == 0 || m1 != m)
  135.     error ("dlqr: r must be square and conformal with column dimension of b");
  136.   endif
  137.  
  138. ## Check if n is there.
  139.   if (nargin == 5)
  140.     [n1, m1] = size (s);
  141.     if (n1 != n || m1 != m)
  142.       error ("dlqr: z must be identically dimensioned with b");
  143.     endif
  144.  
  145. ## Incorporate cross term into a and q.
  146.  
  147.     ao = a - (b/r)*s';
  148.     qo = q - (s/r)*s';
  149.   else
  150.     s = zeros (n, m);
  151.     ao = a;
  152.     qo = q;
  153.   endif
  154.  
  155. ## Check that q, (r) are symmetric, positive (semi)definite
  156.  
  157.   if (is_symm (q) && is_symm (r) ...
  158.       && all (eig (q) >= 0) && all (eig (r) > 0))
  159.     p = dare (ao, b, qo, r);
  160.     k = (r+b'*p*b)\b'*p*a + r\s';
  161.     e = eig (a - b*k);
  162.   else
  163.     error ("dlqr: q (r) must be symmetric positive (semi) definite");
  164.   endif
  165.  
  166. endfunction
  167.