home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21fb.zip / octave / SCRIPTS.ZIP / scripts / control / dlqe.m < prev    next >
Encoding:
Text File  |  1999-12-15  |  3.0 KB  |  124 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{l}, @var{m}, @var{p}, @var{e}] =} dlqe (@var{a}, @var{g}, @var{c}, @var{sigw}, @var{sigv}, @var{z})
  21. ## Construct the linear quadratic estimator (Kalman filter) for the
  22. ## discrete time system
  23. ## @iftex
  24. ## @tex
  25. ## $$
  26. ##  x_{k+1} = A x_k + B u_k + G w_k
  27. ## $$
  28. ## $$
  29. ##  y_k = C x_k + D u_k + w_k
  30. ## $$
  31. ## @end tex
  32. ## @end iftex
  33. ## @ifinfo
  34. ## 
  35. ## @example
  36. ## x[k+1] = A x[k] + B u[k] + G w[k]
  37. ##   y[k] = C x[k] + D u[k] + w[k]
  38. ## @end example
  39. ## 
  40. ## @end ifinfo
  41. ## where @var{w}, @var{v} are zero-mean gaussian noise processes with
  42. ## respective intensities @code{@var{sigw} = cov (@var{w}, @var{w})} and
  43. ## @code{@var{sigv} = cov (@var{v}, @var{v})}.
  44. ## 
  45. ## If specified, @var{z} is @code{cov (@var{w}, @var{v})}.  Otherwise
  46. ## @code{cov (@var{w}, @var{v}) = 0}.
  47. ## 
  48. ## The observer structure is
  49. ## @iftex
  50. ## @tex
  51. ## $$
  52. ##  z_{k+1} = A z_k + B u_k + k (y_k - C z_k - D u_k)
  53. ## $$
  54. ## @end tex
  55. ## @end iftex
  56. ## @ifinfo
  57. ## 
  58. ## @example
  59. ## z[k+1] = A z[k] + B u[k] + k (y[k] - C z[k] - D u[k])
  60. ## @end example
  61. ## @end ifinfo
  62. ## 
  63. ## @noindent
  64. ## The following values are returned:
  65. ## 
  66. ## @table @var
  67. ## @item l
  68. ## The observer gain, 
  69. ## @iftex
  70. ## @tex
  71. ## $(A - ALC)$.
  72. ## @end tex
  73. ## @end iftex
  74. ## @ifinfo
  75. ## (@var{a} - @var{a}@var{l}@var{c}).
  76. ## @end ifinfo
  77. ## is stable.
  78. ## 
  79. ## @item m
  80. ## The Riccati equation solution.
  81. ## 
  82. ## @item p
  83. ## The estimate error covariance after the measurement update.
  84. ## 
  85. ## @item e
  86. ## The closed loop poles of
  87. ## @iftex
  88. ## @tex
  89. ## $(A - ALC)$.
  90. ## @end tex
  91. ## @end iftex
  92. ## @ifinfo
  93. ## (@var{a} - @var{a}@var{l}@var{c}).
  94. ## @end ifinfo
  95. ## @end table
  96. ## @end deftypefn
  97.  
  98. function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, s)
  99. ## Written by A. S. Hodel (scotte@eng.auburn.edu) August, 1993.
  100. ## Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu)
  101. ## October, 1993
  102.  
  103.   if (nargin != 5 && nargin != 6)
  104.     error ("dlqe: invalid number of arguments");
  105.   endif
  106.  
  107. ## The problem is dual to the regulator design, so transform to dlqr call.
  108.  
  109.   if (nargin == 5)
  110.     [k, p, e] = dlqr (a', c', g*sigw*g', sigv);
  111.     m = p;
  112.     l = k';
  113.   else
  114.     [k, p, e] = dlqr (a', c', g*sigw*g', sigv, g*s);
  115.     m = p;
  116.     l = k';
  117.     a = a-g*t/sigv*c;
  118.     sigw = sigw-t/sigv;
  119.   endif
  120.  
  121.   p = a\(m-g*sigw*g')/a';
  122.  
  123. endfunction
  124.