home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21eb.zip / octave / SCRIPTS.ZIP / scripts.fat / control / dre.m < prev    next >
Text File  |  1999-04-29  |  4KB  |  127 lines
  1. # Copyright (C) 1998 A. Scottedward Hodel
  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 [tvals,Plist] = dre(sys,Q,R,Qf,t0,tf,Ptol,maxits)
  20. # [tvals,Plist] = dre(sys,Q,R,Qf,t0,tf{,Ptol,maxits});
  21. # Solve the differential Riccati equation
  22. #   -d P/dt = A'P + P A - P B inv(R) B' P + Q
  23. #   P(tf) = Qf
  24. # for the LTI system sys.  Solution of standard LTI
  25. # state feedback optimization
  26. #   min \int_{t_0}^{t_f} x' Q x + u' R u dt + x(t_f)' Qf x(t_f)
  27. # optimal input is
  28. #   u = - inv(R) B' P(t) x
  29. # inputs:
  30. #   sys: continuous time system data structure
  31. #   Q: state integral penalty
  32. #   R: input integral penalty
  33. #   Qf: state terminal penalty
  34. #   t0,tf: limits on the integral
  35. #   Ptol: tolerance (used to select time samples; see below); default = 0.1
  36. #   max number of refinement iterations (default=10)
  37. # outputs:
  38. #   tvals: time values at which P(t) is computed
  39. #   Plist: list values of P(t); nth(Plist,ii) is P(tvals(ii)).
  40. #
  41. #   tvals is selected so that || nth(Plist,ii) - nth(Plist,ii-1) || < Ptol
  42. #     for ii=2:length(tvals)
  43. #
  44. # Reference:
  45.  
  46. if(nargin < 6 | nargin > 8 | nargout != 2)
  47.   usage("[tvals,Plist] = dre(sys,Q,R,Qf,t0,tf{,Ptol})");
  48. elseif(!is_struct(sys))
  49.   error("sys must be a system data structure")
  50. elseif(is_digit(sys))
  51.   error("sys must be a continuous time system")
  52. elseif(!is_mat(Q) | !is_mat(R) | !is_mat(Qf))
  53.   error("Q, R, and Qf must be matrices.");
  54. elseif(!is_scal(t0) | !is_scal(tf))
  55.   error("t0 and tf must be scalars")
  56. elseif(t0 >= tf)        error("t0=%e >= tf=%e",t0,tf);
  57. elseif(nargin == 6)        Ptol = 0.1;
  58. elseif(!is_scal(Ptol))    error("Ptol must be a scalar");
  59. elseif(Ptol <= 0)        error("Ptol must be positive");
  60. endif
  61.  
  62. if(nargin < 8) maxits = 10;
  63. elseif(!is_scal(maxits))    error("maxits must be a scalar");
  64. elseif(maxits <= 0)        error("maxits must be positive");
  65. endif
  66. maxits = ceil(maxits);
  67.  
  68. [aa,bb] = sys2ss(sys);
  69. nn = sysdimen(sys,"cst");
  70. mm = sysdimen(sys,"in");
  71. pp = sysdimen(sys,"out");
  72.  
  73. if(size(Q) != [nn, nn])
  74.   error("Q(%dx%d); sys has %d states",rows(Q),columns(Q),nn);
  75. elseif(size(Qf) != [nn, nn])
  76.   error("Qf(%dx%d); sys has %d states",rows(Qf),columns(Qf),nn);
  77. elseif(size(R) != [mm, mm])
  78.   error("R(%dx%d); sys has %d inputs",rows(R),columns(R),mm);
  79. endif
  80.  
  81. # construct Hamiltonian matrix
  82. H = [aa , -(bb/R)*bb' ; -Q, -aa'];
  83.  
  84. # select time step to avoid numerical overflow
  85. fast_eig = max(abs(eig(H)));
  86. tc = log(10)/fast_eig;
  87. nst = ceil((tf-t0)/tc);
  88. tvals = -linspace(-tf,-t0,nst);
  89. Plist = list(Qf);
  90. In = eye(nn);
  91. n1 = nn+1;
  92. n2 = nn+nn;
  93. done = 0;
  94. while(!done)
  95.   done = 1;      # assume this pass will do the job
  96.   # sort time values in reverse order
  97.   tvals = -sort(-tvals);
  98.   tvlen = length(tvals);
  99.   maxerr = 0;
  100.   # compute new values of P(t); recompute old values just in case
  101.   for ii=2:tvlen
  102.     uv_i_minus_1 = [ In ; nth(Plist,ii-1) ];
  103.     delta_t = tvals(ii-1) - tvals(ii);
  104.     uv = expm(-H*delta_t)*uv_i_minus_1;
  105.     Qi = uv(n1:n2,1:nn)/uv(1:nn,1:nn);
  106.     Plist(ii) = (Qi+Qi')/2;
  107.     # check error
  108.     Perr = norm(nth(Plist,ii) - nth(Plist,ii-1))/norm(nth(Plist,ii));
  109.     maxerr = max(maxerr,Perr);
  110.     if(Perr > Ptol)
  111.       new_t = mean(tvals([ii,ii-1]));
  112.       tvals = [tvals, new_t];
  113.       done = 0;
  114.     endif
  115.   endfor
  116.  
  117.   # check number of iterations
  118.   maxits = maxits - 1;
  119.   done = done+(maxits==0);
  120. endwhile
  121. if(maxerr > Ptol)
  122.   warning("dre: \n\texiting with%4d points, max rel chg. =%e, Ptol=%e\n", ...
  123.     tvlen,maxerr,Ptol);
  124.   tvals = tvals(1:length(Plist));
  125. endif
  126. endfunction
  127.