|
2847
|
1 ## Copyright (C) 1996, 1997 John W. Eaton |
|
2313
|
2 ## |
|
|
3 ## This file is part of Octave. |
|
|
4 ## |
|
|
5 ## Octave is free software; you can redistribute it and/or modify it |
|
|
6 ## under the terms of the GNU General Public License as published by |
|
|
7 ## the Free Software Foundation; either version 2, or (at your option) |
|
|
8 ## any later version. |
|
|
9 ## |
|
|
10 ## Octave is distributed in the hope that it will be useful, but |
|
|
11 ## WITHOUT ANY WARRANTY; without even the implied warranty of |
|
|
12 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
|
13 ## General Public License for more details. |
|
|
14 ## |
|
|
15 ## You should have received a copy of the GNU General Public License |
|
|
16 ## along with Octave; see the file COPYING. If not, write to the Free |
|
|
17 ## Software Foundation, 59 Temple Place - Suite 330, Boston, MA |
|
|
18 ## 02111-1307, USA. |
|
245
|
19 |
|
2311
|
20 ## Usage: [k, p, e] = lqr (A, B, Q, R {,Z}) |
|
|
21 ## |
|
|
22 ## Linear quadratic regulator design for the continuous time system |
|
|
23 ## |
|
|
24 ## dx/dt = A x + B u |
|
|
25 ## |
|
|
26 ## to minimize the cost functional |
|
|
27 ## |
|
|
28 ## J = int_0^\infty{ x' Q x + u' R u } Z omitted |
|
|
29 ## |
|
|
30 ## or |
|
|
31 ## |
|
|
32 ## J = int_0^\infty{ x' Q x + u' R u +2 x' Z u} Z included |
|
|
33 ## |
|
|
34 ## Returns: |
|
|
35 ## |
|
|
36 ## k = state feedback gain, (A - B K) is stable |
|
|
37 ## p = solution of algebraic Riccati equation |
|
|
38 ## e = closed loop poles of (A - B K) |
|
76
|
39 |
|
2312
|
40 ## Author: A. S. Hodel <scotte@eng.auburn.edu> |
|
|
41 ## Created: August 1993 |
|
|
42 ## Adapted-By: jwe |
|
76
|
43 |
|
2312
|
44 function [k, p, e] = lqr (a, b, q, r, zz) |
|
76
|
45 |
|
|
46 if (nargin != 4 && nargin != 5) |
|
904
|
47 error ("lqr: invalid number of arguments"); |
|
76
|
48 endif |
|
|
49 |
|
2303
|
50 ## Check a. |
|
76
|
51 if ((n = is_square (a)) == 0) |
|
|
52 error ("lqr: requires 1st parameter(a) to be square"); |
|
|
53 endif |
|
|
54 |
|
2303
|
55 ## Check b. |
|
76
|
56 [n1, m] = size (b); |
|
|
57 if (n1 != n) |
|
|
58 error ("lqr: a,b not conformal"); |
|
|
59 endif |
|
|
60 |
|
2303
|
61 ## Check q. |
|
2325
|
62 |
|
76
|
63 if ((n1 = is_square (q)) == 0 || n1 != n) |
|
|
64 error ("lqr: q must be square and conformal with a"); |
|
|
65 endif |
|
|
66 |
|
2303
|
67 ## Check r. |
|
76
|
68 if((m1 = is_square(r)) == 0 || m1 != m) |
|
|
69 error ("lqr: r must be square and conformal with column dimension of b"); |
|
|
70 endif |
|
|
71 |
|
2303
|
72 ## Check if n is there. |
|
76
|
73 if (nargin == 5) |
|
|
74 [n1, m1] = size (zz); |
|
|
75 if (n1 != n || m1 != m) |
|
|
76 error ("lqr: z must be identically dimensioned with b"); |
|
|
77 endif |
|
|
78 |
|
2303
|
79 ## Incorporate cross term into a and q. |
|
76
|
80 |
|
|
81 ao = a - (b/r)*zz'; |
|
|
82 qo = q - (zz/r)*zz'; |
|
|
83 else |
|
|
84 zz = zeros (n, m); |
|
|
85 ao = a; |
|
|
86 qo = q; |
|
|
87 endif |
|
|
88 |
|
2303
|
89 ## Check that q, (r) are symmetric, positive (semi)definite |
|
76
|
90 |
|
|
91 if (is_symmetric (q) && is_symmetric (r) ... |
|
|
92 && all (eig (q) >= 0) && all (eig (r) > 0)) |
|
|
93 p = are (ao, (b/r)*b', qo); |
|
|
94 k = r\(b'*p + zz'); |
|
|
95 e = eig (a - b*k); |
|
|
96 else |
|
|
97 error ("lqr: q (r) must be symmetric positive (semi) definite"); |
|
|
98 endif |
|
|
99 |
|
|
100 endfunction |
