# HG changeset patch
# User jwe
# Date 752794763 0
# Node ID a5d54ad5cf9a80009f9f0b44cb1d195076d3245a
# Parent  2e4d2596f2c3f29514861b7847fd40ed05636a8b
[project @ 1993-11-08 21:39:23 by jwe]
Initial revision

diff --git a/scripts/control/dgram.m b/scripts/control/dgram.m
new file mode 100644
--- /dev/null
+++ b/scripts/control/dgram.m
@@ -0,0 +1,17 @@
+function gramian = dgram (A, B)
+
+#  Usage: gramian = dgram (A, B)
+#
+#  Returns the discrete controllability and observability gramian.
+#
+#  dgram (A, B)   returns the discrete controllability gramian.
+#  dgram (A', C') returns the observability gramian.
+
+#  Written by R. Bruce Tenison (btenison@eng.auburn.edu)
+#  October 1993
+
+  [U, Sig, V] = svd (B);
+
+  gramian = U * dlyap (U'*A*U, Sig*Sig') * U';
+
+endfunction
diff --git a/scripts/control/dlqe.m b/scripts/control/dlqe.m
new file mode 100644
--- /dev/null
+++ b/scripts/control/dlqe.m
@@ -0,0 +1,51 @@
+function [l, m, p, e] = dlqe (a, g, c, sigw, sigv, zz)
+
+# Usage: [l, m, p, e] = dlqe (A, G, C, SigW, SigV {,Z})
+#
+# Linear quadratic estimator (Kalman filter) design for the 
+# discrete time system
+#
+#  x[k+1] = A x[k] + B u[k] + G w[k]
+#    y[k] = C x[k] + D u[k] + w[k]
+#
+# where w, v are zero-mean gaussian noise processes with respective
+# intensities SigW = cov (w, w) and SigV = cov (v, v).
+#
+# Z (if specified) is cov(w,v); otherwise cov(w,v) = 0.
+#
+# Observer structure is 
+#     z[k+1] = A z[k] + B u[k] + k(y[k] - C z[k] - D u[k]).
+#
+# Returns:
+#
+#   l = observer gain, (A - A L C) is stable
+#   m = Ricatti equation solution
+#   p = the estimate error covariance after the measurement update
+#   e = closed loop poles of (A - A L C)
+
+# Written by A. S. Hodel (scotte@eng.auburn.edu) August, 1993.
+# Modified for discrete time by R. Bruce Tenison (btenison@eng.auburn.edu)
+# October, 1993
+
+  if (nargin != 5 && nargin != 6)
+    error ("dlqe: illegal number of arguments");
+  endif
+
+# The problem is dual to the regulator design, so transform to lqr
+# call.
+
+  if (nargin == 5)
+    [k, p, e] = dlqr (a', c', g*sigw*g', sigv);
+    m = p';
+    l = (m*c')/(c*m*c'+sigv);
+  else
+    [k, p, e] = dlqr (a', c', g*sigw*g', sigv, g*zz);
+    m = p';
+    l = (m*c'+a\g*t)/(c*m*c'+sigv);
+    a = a-g*t/sigv*c;
+    sigw = sigw-t/sigv*t';
+  endif
+
+  p = a\(m-g*sigw*g')/a';
+
+endfunction
diff --git a/scripts/control/dlqr.m b/scripts/control/dlqr.m
new file mode 100644
--- /dev/null
+++ b/scripts/control/dlqr.m
@@ -0,0 +1,81 @@
+function [k, p, e] = dlqr (a, b, q, r, zz)
+
+# Usage: [k, p, e] = dlqr (A, B, Q, R {,Z})
+#
+# Linear quadratic regulator design for the continuous time system
+#
+#   x[k+1] = A x[k] + B u[k]
+#
+# to minimize the cost functional
+#
+#  J = Sum { x' Q x + u' R u } 			Z omitted
+#
+# or
+#
+#  J = Sum { x' Q x + u' R u +2 x' Z u}		Z included
+#
+# Returns:
+#
+#   k = state feedback gain, (A - B K) is stable
+#   p = solution of algebraic Riccati equation
+#   e = closed loop poles of (A - B K)
+
+# Written by A. S. Hodel (scotte@eng.auburn.edu) August 1993.
+# Converted to discrete time by R. B. Tenison
+# (btenison@eng.auburn.edu) October 1993
+
+  if (nargin != 4 && nargin != 5)
+    error ("dlqr: illegal number of arguments");
+  endif
+
+# Check a.
+  if ((n = is_square (a)) == 0)
+    error ("dlqr: requires 1st parameter(a) to be square");
+  endif
+
+# Check b.
+  [n1, m] = size (b);
+  if (n1 != n)
+    error ("dlqr: a,b not conformal");
+  endif
+
+# Check q.
+  
+  if ((n1 = is_square (q)) == 0 || n1 != n)
+    error ("dlqr: q must be square and conformal with a");
+  endif
+
+# Check r.
+  if((m1 = is_square(r)) == 0 || m1 != m)
+    error ("dlqr: r must be square and conformal with column dimension of b");
+  endif
+
+# Check if n is there.
+  if (nargin == 5)
+    [n1, m1] = size (zz);
+    if (n1 != n || m1 != m)
+      error ("dlqr: z must be identically dimensioned with b");
+    endif
+
+# Incorporate cross term into a and q.
+
+    ao = a - (b/r)*zz';
+    qo = q - (zz/r)*zz';
+  else
+    zz = zeros (n, m);
+    ao = a;
+    qo = q;
+  endif
+
+# Check that q, (r) are symmetric, positive (semi)definite
+
+  if (is_symmetric (q) && is_symmetric (r) ...
+      && all (eig (q) >= 0) && all (eig (r) > 0))
+    p = dare (ao, b, qo, r);
+    k = (r+b'*p*b)\b'*p*a + r\zz';
+    e = eig (a - b*k);
+  else
+    error ("dlqr: q (r) must be symmetric positive (semi) definite");
+  endif
+
+endfunction