Mercurial > hg > octave-terminal
changeset 201:a5d54ad5cf9a
[project @ 1993-11-08 21:39:23 by jwe]
Initial revision
author | jwe |
---|---|
date | Mon, 08 Nov 1993 21:39:23 +0000 |
parents | 2e4d2596f2c3 |
children | cfedacc895e7 |
files | scripts/control/dgram.m scripts/control/dlqe.m scripts/control/dlqr.m |
diffstat | 3 files changed, 149 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
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
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
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