home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / octa21fb.zip / octave / SCRIPTS.ZIP / scripts.fat / control / lqg.m < prev    next >
Text File  |  1999-12-24  |  5KB  |  148 lines
  1. ## Copyright (C) 1996, 1997 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{Q}, @var{P}, @var{Ee}, @var{Er}] =} lqg(@var{sys}, @var{Sigw}, @var{Sigv}, @var{Q}, @var{R}, @var{in_idx})
  21. ## Design a linear-quadratic-gaussian optimal controller for the system
  22. ## @example
  23. ## dx/dt = A x + B u + G w       [w]=N(0,[Sigw 0    ])
  24. ##     y = C x + v               [v]  (    0   Sigv ])
  25. ## @end example
  26. ## or
  27. ## @example 
  28. ## x(k+1) = A x(k) + B u(k) + G w(k)       [w]=N(0,[Sigw 0    ])
  29. ##   y(k) = C x(k) + v(k)                  [v]  (    0   Sigv ])
  30. ## @end example
  31. ## 
  32. ## @strong{Inputs}
  33. ## @table @var
  34. ## @item  sys
  35. ## system data structure
  36. ## @item  Sigw, Sigv
  37. ## intensities of independent Gaussian noise processes (as above)
  38. ## @item  Q, R
  39. ## state, control weighting respectively.  Control ARE is
  40. ## @item  in_idx
  41. ## indices of controlled inputs
  42. ## 
  43. ##      default: last dim(R) inputs are assumed to be controlled inputs, all
  44. ##               others are assumed to be noise inputs.
  45. ## @end table
  46. ## @strong{Outputs}
  47. ## @table @var
  48. ## @item    K
  49. ## system data structure format LQG optimal controller
  50. ## (Obtain A,B,C matrices with @code{sys2ss}, @code{sys2tf}, or @code{sys2zp} as appropriate)
  51. ## @item    P
  52. ## Solution of control (state feedback) algebraic Riccati equation
  53. ## @item    Q
  54. ## Solution of estimation algebraic Riccati equation
  55. ## @item    Ee
  56. ## estimator poles
  57. ## @item    Es
  58. ## controller poles
  59. ## @end table
  60. ## @end deftypefn
  61.  
  62. ## See also:  h2syn, lqe, lqr
  63.  
  64. function [K, Q1, P1, Ee, Er] = lqg (sys, Sigw, Sigv, Q, R, input_list)
  65.  
  66.   ## Written by A. S. Hodel August 1995; revised for new system format
  67.   ## August 1996
  68.  
  69.   if ( (nargin < 5) | (nargin > 6))
  70.     usage("[K,Q1,P1,Ee,Er] = lqg(sys,Sigw, Sigv,Q,R{,input_list})");
  71.  
  72.   elseif(!is_struct(sys) )
  73.     error("sys must be in system data structure");
  74.   endif
  75.  
  76.   DIG = is_digit(sys);
  77.   [A,B,C,D,tsam,n,nz,stname,inname,outname] = sys2ss(sys);
  78.   [n,nz,nin,nout] = sysdimen(sys);
  79.   if(nargin == 5)
  80.     ## construct default input_list
  81.     input_list = (columns(Sigw)+1):nin;
  82.   endif
  83.  
  84.   if( !(n+nz) )
  85.       error(["lqg: 0 states in system"]);
  86.  
  87.   elseif(nin != columns(Sigw)+ columns(R))
  88.     error(["lqg: sys has ",num2str(nin)," inputs, dim(Sigw)=", ...
  89.       num2str(columns(Sigw)),", dim(u)=",num2str(columns(R))])
  90.  
  91.   elseif(nout != columns(Sigv))
  92.     error(["lqg: sys has ",num2str(nout)," outputs, dim(Sigv)=", ...
  93.       num2str(columns(Sigv)),")"])
  94.   elseif(length(input_list) != columns(R))
  95.     error(["lqg: length(input_list)=",num2str(length(input_list)), ...
  96.       ", columns(R)=", num2str(columns(R))]);
  97.   endif
  98.  
  99.   varname = list("Sigw","Sigv","Q","R");
  100.   for kk=1:length(varname);
  101.     eval(sprintf("chk = is_sqr(%s);",nth(varname,kk)));
  102.     if(! chk ) error("lqg: %s is not square",nth(varname,kk)); endif
  103.   endfor
  104.  
  105.   ## permute (if need be)
  106.   if(nargin == 6)
  107.     all_inputs = sysreord(nin,input_list);
  108.     B = B(:,all_inputs);
  109.     inname = inname(all_inputs);
  110.   endif
  111.  
  112.   ## put parameters into correct variables
  113.   m1 = columns(Sigw);
  114.   m2 = m1+1;
  115.   G = B(:,1:m1);
  116.   B = B(:,m2:nin);
  117.  
  118.   ## now we can just do the design; call dlqr and dlqe, since all matrices
  119.   ## are not given in Cholesky factor form (as in h2syn case)
  120.   if(DIG)
  121.     [Ks, P1, Er] = dlqr(A,B,Q,R);
  122.     [Ke, Q1, jnk, Ee] = dlqe(A,G,C,Sigw,Sigv);
  123.   else
  124.     [Ks, P1, Er] = lqr(A,B,Q,R);
  125.     [Ke, Q1, Ee] = lqe(A,G,C,Sigw,Sigv);
  126.   endif
  127.   Ac = A - Ke*C - B*Ks;
  128.   Bc = Ke;
  129.   Cc = -Ks;
  130.   Dc = zeros(rows(Cc),columns(Bc));
  131.  
  132.   ## fix state names
  133.   stname1 = strappen(stname,"_e");
  134.  
  135.   ## fix controller output names
  136.   outname1 = strappen(inname(m2:nin),"_K");
  137.  
  138.   ## fix controller input names
  139.   inname1 = strappen(outname,"_K");
  140.  
  141.   if(DIG)
  142.     K = ss2sys(Ac,Bc,Cc,Dc,tsam,n,nz,stname1,inname1,outname1,1:rows(Cc));
  143.   else
  144.     K = ss2sys(Ac,Bc,Cc,Dc,tsam,n,nz,stname,inname1,outname1);
  145.   endif
  146.  
  147. endfunction
  148.