Mercurial > octave
comparison scripts/control/lqr.m @ 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 | |
| children | 16a24e76d6e0 |
comparison
equal
deleted
inserted
replaced
| 75:505c8b681f66 | 76:c69be6819009 |
|---|---|
| 1 function [k, p, e] = lqr (a, b, q, r, zz) | |
| 2 | |
| 3 # Usage: [k, p, e] = lqr (A, B, Q, R {,Z}) | |
| 4 # | |
| 5 # Linear quadratic regulator design for the continuous time system | |
| 6 # | |
| 7 # dx/dt = A x + B u | |
| 8 # | |
| 9 # to minimize the cost functional | |
| 10 # | |
| 11 # J = int_0^\infty{ x' Q x + u' R u } Z omitted | |
| 12 # | |
| 13 # or | |
| 14 # | |
| 15 # J = int_0^\infty{ x' Q x + u' R u +2 x' Z u} Z included | |
| 16 # | |
| 17 # Returns: | |
| 18 # | |
| 19 # k = state feedback gain, (A - B K) is stable | |
| 20 # p = solution of algebraic Riccati equation | |
| 21 # e = closed loop poles of (A - B K) | |
| 22 | |
| 23 # Written by A. S. Hodel (scotte@eng.auburn.edu) August 1993. | |
| 24 | |
| 25 if (nargin != 4 && nargin != 5) | |
| 26 error ("lqr: illegal number of arguments"); | |
| 27 endif | |
| 28 | |
| 29 # Check a. | |
| 30 if ((n = is_square (a)) == 0) | |
| 31 error ("lqr: requires 1st parameter(a) to be square"); | |
| 32 endif | |
| 33 | |
| 34 # Check b. | |
| 35 [n1, m] = size (b); | |
| 36 if (n1 != n) | |
| 37 error ("lqr: a,b not conformal"); | |
| 38 endif | |
| 39 | |
| 40 # Check q. | |
| 41 | |
| 42 if ((n1 = is_square (q)) == 0 || n1 != n) | |
| 43 error ("lqr: q must be square and conformal with a"); | |
| 44 endif | |
| 45 | |
| 46 # Check r. | |
| 47 if((m1 = is_square(r)) == 0 || m1 != m) | |
| 48 error ("lqr: r must be square and conformal with column dimension of b"); | |
| 49 endif | |
| 50 | |
| 51 # Check if n is there. | |
| 52 if (nargin == 5) | |
| 53 [n1, m1] = size (zz); | |
| 54 if (n1 != n || m1 != m) | |
| 55 error ("lqr: z must be identically dimensioned with b"); | |
| 56 endif | |
| 57 | |
| 58 # Incorporate cross term into a and q. | |
| 59 | |
| 60 ao = a - (b/r)*zz'; | |
| 61 qo = q - (zz/r)*zz'; | |
| 62 else | |
| 63 zz = zeros (n, m); | |
| 64 ao = a; | |
| 65 qo = q; | |
| 66 endif | |
| 67 | |
| 68 # Check that q, (r) are symmetric, positive (semi)definite | |
| 69 | |
| 70 if (is_symmetric (q) && is_symmetric (r) ... | |
| 71 && all (eig (q) >= 0) && all (eig (r) > 0)) | |
| 72 p = are (ao, (b/r)*b', qo); | |
| 73 k = r\(b'*p + zz'); | |
| 74 e = eig (a - b*k); | |
| 75 else | |
| 76 error ("lqr: q (r) must be symmetric positive (semi) definite"); | |
| 77 endif | |
| 78 | |
| 79 endfunction |