Mercurial > hg > octave-terminal
changeset 76:c69be6819009
[project @ 1993-08-30 15:29:38 by jwe]
Initial revision
author | jwe |
---|---|
date | Mon, 30 Aug 1993 15:29:38 +0000 |
parents | 505c8b681f66 |
children | 1cc0af4496c6 |
files | scripts/control/lqe.m scripts/control/lqr.m |
diffstat | 2 files changed, 118 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
new file mode 100644 --- /dev/null +++ b/scripts/control/lqe.m @@ -0,0 +1,39 @@ +function [k, p, e] = lqe (a, g, c, sigw, sigv, zz) + +# Usage: [k, p, e] = lqe (A, G, C, SigW, SigV {,Z}) +# +# Linear quadratic estimator (Kalman filter) design for the +# continuous time system +# +# dx/dt = A x + B u + G w +# y = C x + D u + w +# +# 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 dz/dt = A z + B u + k( y - C z - D u). +# +# Returns: +# +# k = observer gain, (A - K C) is stable +# p = solution of algebraic Riccati equation +# e = closed loop poles of (A - K C) + +# Written by A. S. Hodel (scotte@eng.auburn.edu) August, 1993. + + if (nargin != 5 && nargin != 6) + error ("lqe: illegal number of arguments"); + endif + +# The problem is dual to the regulator design, so transform to lqr +# call. + + if (nargin == 5) + [k, p, e] = lqr (a', c', g*sigw*g', sigv); + else + [k, p, e] = lqr (a', c', g*sigw*g', sigv, g*zz); + endif + +endfunction
new file mode 100644 --- /dev/null +++ b/scripts/control/lqr.m @@ -0,0 +1,79 @@ +function [k, p, e] = lqr (a, b, q, r, zz) + +# Usage: [k, p, e] = lqr (A, B, Q, R {,Z}) +# +# Linear quadratic regulator design for the continuous time system +# +# dx/dt = A x + B u +# +# to minimize the cost functional +# +# J = int_0^\infty{ x' Q x + u' R u } Z omitted +# +# or +# +# J = int_0^\infty{ 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. + + if (nargin != 4 && nargin != 5) + error ("lqr: illegal number of arguments"); + endif + +# Check a. + if ((n = is_square (a)) == 0) + error ("lqr: requires 1st parameter(a) to be square"); + endif + +# Check b. + [n1, m] = size (b); + if (n1 != n) + error ("lqr: a,b not conformal"); + endif + +# Check q. + + if ((n1 = is_square (q)) == 0 || n1 != n) + error ("lqr: q must be square and conformal with a"); + endif + +# Check r. + if((m1 = is_square(r)) == 0 || m1 != m) + error ("lqr: 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 ("lqr: 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 = are (ao, (b/r)*b', qo); + k = r\(b'*p + zz'); + e = eig (a - b*k); + else + error ("lqr: q (r) must be symmetric positive (semi) definite"); + endif + +endfunction