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