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