|
76
|
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 |
