home *** CD-ROM | disk | FTP | other *** search
/ Geek Gadgets 1 / ADE-1.bin / ade-dist / octave-1.1.1p1-src.tgz / tar.out / fsf / octave / scripts / control / dlqe.m < prev    next >
Text File  |  1996-09-28  |  2KB  |  70 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 [l, m, p, e] = dlqe (a, g, c, sigw, sigv, zz)
  20.  
  21. # Usage: [l, m, p, e] = dlqe (A, G, C, SigW, SigV {,Z})
  22. #
  23. # Linear quadratic estimator (Kalman filter) design for the 
  24. # discrete time system
  25. #
  26. #  x[k+1] = A x[k] + B u[k] + G w[k]
  27. #    y[k] = C x[k] + D u[k] + w[k]
  28. #
  29. # where w, v are zero-mean gaussian noise processes with respective
  30. # intensities SigW = cov (w, w) and SigV = cov (v, v).
  31. #
  32. # Z (if specified) is cov(w,v); otherwise cov(w,v) = 0.
  33. #
  34. # Observer structure is 
  35. #     z[k+1] = A z[k] + B u[k] + k(y[k] - C z[k] - D u[k]).
  36. #
  37. # Returns:
  38. #
  39. #   l = observer gain, (A - A L C) is stable
  40. #   m = Ricatti equation solution
  41. #   p = the estimate error covariance after the measurement update
  42. #   e = closed loop poles of (A - A L C)
  43.  
  44. # Written by A. S. Hodel (scotte@eng.auburn.edu) August, 1993.
  45. # Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu)
  46. # October, 1993
  47.  
  48.   if (nargin != 5 && nargin != 6)
  49.     error ("dlqe: invalid number of arguments");
  50.   endif
  51.  
  52. # The problem is dual to the regulator design, so transform to lqr
  53. # call.
  54.  
  55.   if (nargin == 5)
  56.     [k, p, e] = dlqr (a', c', g*sigw*g', sigv);
  57.     m = p';
  58.     l = (m*c')/(c*m*c'+sigv);
  59.   else
  60.     [k, p, e] = dlqr (a', c', g*sigw*g', sigv, g*zz);
  61.     m = p';
  62.     l = (m*c'+a\g)/(c*m*c'+sigv);
  63.     a = a-g*t/sigv*c;
  64.     sigw = sigw-t/sigv;
  65.   endif
  66.  
  67.   p = a\(m-g*sigw*g')/a';
  68.  
  69. endfunction
  70.