|
2313
|
1 ## Copyright (C) 1996 John W. Eaton |
|
|
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] = dlqr (A, B, Q, R {,Z}) |
|
|
21 ## |
|
|
22 ## Linear quadratic regulator design for the discrete time system |
|
|
23 ## |
|
|
24 ## x[k+1] = A x[k] + B u[k] |
|
|
25 ## |
|
|
26 ## to minimize the cost functional |
|
|
27 ## |
|
|
28 ## J = Sum { x' Q x + u' R u } Z omitted |
|
|
29 ## |
|
|
30 ## or |
|
|
31 ## |
|
|
32 ## J = Sum { 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) |
|
201
|
39 |
|
2312
|
40 ## Author: A. S. Hodel <scotte@eng.auburn.edu> |
|
|
41 ## R. B. Tenison <btenison@eng.auburn.edu> |
|
|
42 ## Created: August 1993 |
|
|
43 ## Adapted-By: jwe |
|
201
|
44 |
|
2312
|
45 function [k, p, e] = dlqr (a, b, q, r, zz) |
|
201
|
46 |
|
|
47 if (nargin != 4 && nargin != 5) |
|
904
|
48 error ("dlqr: invalid number of arguments"); |
|
201
|
49 endif |
|
|
50 |
|
2303
|
51 ## Check a. |
|
201
|
52 if ((n = is_square (a)) == 0) |
|
|
53 error ("dlqr: requires 1st parameter(a) to be square"); |
|
|
54 endif |
|
|
55 |
|
2303
|
56 ## Check b. |
|
201
|
57 [n1, m] = size (b); |
|
|
58 if (n1 != n) |
|
|
59 error ("dlqr: a,b not conformal"); |
|
|
60 endif |
|
|
61 |
|
2303
|
62 ## Check q. |
|
2325
|
63 |
|
201
|
64 if ((n1 = is_square (q)) == 0 || n1 != n) |
|
|
65 error ("dlqr: q must be square and conformal with a"); |
|
|
66 endif |
|
|
67 |
|
2303
|
68 ## Check r. |
|
201
|
69 if((m1 = is_square(r)) == 0 || m1 != m) |
|
|
70 error ("dlqr: r must be square and conformal with column dimension of b"); |
|
|
71 endif |
|
|
72 |
|
2303
|
73 ## Check if n is there. |
|
201
|
74 if (nargin == 5) |
|
|
75 [n1, m1] = size (zz); |
|
|
76 if (n1 != n || m1 != m) |
|
|
77 error ("dlqr: z must be identically dimensioned with b"); |
|
|
78 endif |
|
|
79 |
|
2303
|
80 ## Incorporate cross term into a and q. |
|
201
|
81 |
|
|
82 ao = a - (b/r)*zz'; |
|
|
83 qo = q - (zz/r)*zz'; |
|
|
84 else |
|
|
85 zz = zeros (n, m); |
|
|
86 ao = a; |
|
|
87 qo = q; |
|
|
88 endif |
|
|
89 |
|
2303
|
90 ## Check that q, (r) are symmetric, positive (semi)definite |
|
201
|
91 |
|
|
92 if (is_symmetric (q) && is_symmetric (r) ... |
|
|
93 && all (eig (q) >= 0) && all (eig (r) > 0)) |
|
|
94 p = dare (ao, b, qo, r); |
|
|
95 k = (r+b'*p*b)\b'*p*a + r\zz'; |
|
|
96 e = eig (a - b*k); |
|
|
97 else |
|
|
98 error ("dlqr: q (r) must be symmetric positive (semi) definite"); |
|
|
99 endif |
|
|
100 |
|
|
101 endfunction |
