diff scripts/control/base/lqg.m @ 7126:4a375de63f66

[project @ 2007-11-08 03:44:14 by jwe]
author jwe
date Thu, 08 Nov 2007 03:44:15 +0000
parents 120f3135952f
children
line wrap: on
line diff
--- a/scripts/control/base/lqg.m
+++ b/scripts/control/base/lqg.m
@@ -70,89 +70,98 @@
 
 function [K, Q1, P1, Ee, Er] = lqg (sys, Sigw, Sigv, Q, R, input_list)
 
-  if ( (nargin < 5) | (nargin > 6))
+  if (nargin < 5 || nargin > 6)
     print_usage ();
-
-  elseif(!isstruct(sys) )
-    error("sys must be in system data structure");
+  elseif (! isstruct (sys))
+    error ("sys must be in system data structure");
   endif
 
-  DIG = is_digital(sys);
-  [A,B,C,D,tsam,n,nz,stname,inname,outname] = sys2ss(sys);
-  [n,nz,nin,nout] = sysdimensions(sys);
-  if(nargin == 5)
+  DIG = is_digital (sys);
+  [A, B, C, D, tsam, n, nz, stname, inname, outname] = sys2ss (sys);
+  [n, nz, nin, nout] = sysdimensions (sys);
+  if (nargin == 5)
     ## construct default input_list
     input_list = (columns(Sigw)+1):nin;
   endif
 
-  if( !(n+nz) )
-      error(["lqg: 0 states in system"]);
+  if (! (n+nz))
+    error("lqg: 0 states in system");
 
-  elseif(nin != columns(Sigw)+ columns(R))
-    error(["lqg: sys has ",num2str(nin)," inputs, dim(Sigw)=", ...
-          num2str(columns(Sigw)),", dim(u)=",num2str(columns(R))])
+  elseif (nin != columns (Sigw) + columns (R))
+    error ("lqg: sys has %d inputs, dim(Sigw)=%d, dim(u)=%d",
+	   nin, columns (Sigw), columns (R));
 
-  elseif(nout != columns(Sigv))
-    error(["lqg: sys has ",num2str(nout)," outputs, dim(Sigv)=", ...
-          num2str(columns(Sigv)),")"])
+  elseif (nout != columns (Sigv))
+    error ("lqg: sys has %d outputs, dim(Sigv)=%d", nout, columns (Sigv));
   endif
 
   ## check for names of signals
-  if(is_signal_list(input_list) | ischar(input_list))
-    input_list = sysidx(sys,"in",input_list);
+  if (is_signal_list (input_list) || ischar (input_list))
+    input_list = sysidx (sys, "in", input_list);
   endif
 
-  if(length(input_list) != columns(R))
-    error(["lqg: length(input_list)=",num2str(length(input_list)), ...
-          ", columns(R)=", num2str(columns(R))]);
+  if (length (input_list) != columns (R))
+    error ("lqg: length(input_list)=%d, columns(R)=%d",
+	   length (input_list), columns (R));
+  endif
+
+  if (! issquare (Sigw))
+    error ("lqg: Sigw is not square");
   endif
 
-  varname = {"Sigw","Sigv","Q","R"};
-  for kk=1:length(varname);
-    eval(sprintf("chk = issquare(%s);",varname{kk}));
-    if(! chk ) error("lqg: %s is not square",varname{kk}); endif
-  endfor
+  if (! issquare (Sigv))
+    error ("lqg: Sigv is not square");
+  endif
+
+  if (! issquare (Q))
+    error ("lqg: Q is not square");
+  endif
+
+  if (! issquare (R))
+    error ("lqg: Q is not square");
+  endif
 
   ## permute (if need be)
-  if(nargin == 6)
-    all_inputs = sysreorder(nin,input_list);
+  if (nargin == 6)
+    all_inputs = sysreorder (nin, input_list);
     B = B(:,all_inputs);
-    inname = inname(all_inputs);
+    inname = inname (all_inputs);
   endif
 
   ## put parameters into correct variables
-  m1 = columns(Sigw);
+  m1 = columns (Sigw);
   m2 = m1+1;
   G = B(:,1:m1);
   B = B(:,m2:nin);
 
   ## now we can just do the design; call dlqr and dlqe, since all matrices
   ## are not given in Cholesky factor form (as in h2syn case)
-  if(DIG)
-    [Ks, P1, Er] = dlqr(A,B,Q,R);
-    [Ke, Q1, jnk, Ee] = dlqe(A,G,C,Sigw,Sigv);
+  if (DIG)
+    [Ks, P1, Er] = dlqr (A, B, Q, R);
+    [Ke, Q1, jnk, Ee] = dlqe (A, G, C, Sigw, Sigv);
   else
-    [Ks, P1, Er] = lqr(A,B,Q,R);
-    [Ke, Q1, Ee] = lqe(A,G,C,Sigw,Sigv);
+    [Ks, P1, Er] = lqr (A, B, Q, R);
+    [Ke, Q1, Ee] = lqe (A, G, C, Sigw, Sigv);
   endif
   Ac = A - Ke*C - B*Ks;
   Bc = Ke;
   Cc = -Ks;
-  Dc = zeros(rows(Cc),columns(Bc));
+  Dc = zeros (rows (Cc), columns (Bc));
 
   ## fix state names
-  stname1 = strappend(stname,"_e");
+  stname1 = strappend (stname, "_e");
 
   ## fix controller output names
-  outname1 = strappend(inname(m2:nin),"_K");
+  outname1 = strappend (inname(m2:nin), "_K");
 
   ## fix controller input names
-  inname1 = strappend(outname,"_K");
+  inname1 = strappend (outname, "_K");
 
-  if(DIG)
-    K = ss(Ac,Bc,Cc,Dc,tsam,n,nz,stname1,inname1,outname1,1:rows(Cc));
+  if (DIG)
+    K = ss (Ac, Bc, Cc, Dc, tsam, n, nz, stname1, inname1, outname1,
+	    1:rows(Cc));
   else
-    K = ss(Ac,Bc,Cc,Dc,tsam,n,nz,stname,inname1,outname1);
+    K = ss (Ac, Bc, Cc, Dc, tsam, n, nz, stname, inname1, outname1);
   endif
 
 endfunction