|
7017
|
1 ## Copyright (C) 1993, 1994, 1995, 2000, 2002, 2004, 2005, 2007 |
|
|
2 ## Auburn University. All rights reserved. |
|
3431
|
3 ## |
|
|
4 ## This file is part of Octave. |
|
|
5 ## |
|
|
6 ## Octave is free software; you can redistribute it and/or modify it |
|
7016
|
7 ## under the terms of the GNU General Public License as published by |
|
|
8 ## the Free Software Foundation; either version 3 of the License, or (at |
|
|
9 ## your option) any later version. |
|
3431
|
10 ## |
|
7016
|
11 ## Octave is distributed in the hope that it will be useful, but |
|
|
12 ## WITHOUT ANY WARRANTY; without even the implied warranty of |
|
|
13 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
|
14 ## General Public License for more details. |
|
3431
|
15 ## |
|
|
16 ## You should have received a copy of the GNU General Public License |
|
7016
|
17 ## along with Octave; see the file COPYING. If not, see |
|
|
18 ## <http://www.gnu.org/licenses/>. |
|
3431
|
19 |
|
|
20 ## -*- texinfo -*- |
|
|
21 ## @deftypefn {Function File} {[@var{k}, @var{p}, @var{e}] =} lqr (@var{a}, @var{b}, @var{q}, @var{r}, @var{z}) |
|
|
22 ## construct the linear quadratic regulator for the continuous time system |
|
|
23 ## @iftex |
|
|
24 ## @tex |
|
|
25 ## $$ |
|
|
26 ## {dx\over dt} = A x + B u |
|
|
27 ## $$ |
|
|
28 ## @end tex |
|
|
29 ## @end iftex |
|
|
30 ## @ifinfo |
|
|
31 ## |
|
|
32 ## @example |
|
|
33 ## dx |
|
|
34 ## -- = A x + B u |
|
|
35 ## dt |
|
|
36 ## @end example |
|
|
37 ## |
|
|
38 ## @end ifinfo |
|
|
39 ## to minimize the cost functional |
|
|
40 ## @iftex |
|
|
41 ## @tex |
|
|
42 ## $$ |
|
|
43 ## J = \int_0^\infty x^T Q x + u^T R u |
|
|
44 ## $$ |
|
|
45 ## @end tex |
|
|
46 ## @end iftex |
|
|
47 ## @ifinfo |
|
|
48 ## |
|
|
49 ## @example |
|
|
50 ## infinity |
|
|
51 ## / |
|
|
52 ## J = | x' Q x + u' R u |
|
|
53 ## / |
|
|
54 ## t=0 |
|
|
55 ## @end example |
|
|
56 ## @end ifinfo |
|
|
57 ## |
|
|
58 ## @noindent |
|
|
59 ## @var{z} omitted or |
|
|
60 ## @iftex |
|
|
61 ## @tex |
|
|
62 ## $$ |
|
|
63 ## J = \int_0^\infty x^T Q x + u^T R u + 2 x^T Z u |
|
|
64 ## $$ |
|
|
65 ## @end tex |
|
|
66 ## @end iftex |
|
|
67 ## @ifinfo |
|
|
68 ## |
|
|
69 ## @example |
|
|
70 ## infinity |
|
|
71 ## / |
|
|
72 ## J = | x' Q x + u' R u + 2 x' Z u |
|
|
73 ## / |
|
|
74 ## t=0 |
|
|
75 ## @end example |
|
|
76 ## |
|
|
77 ## @end ifinfo |
|
|
78 ## @var{z} included. |
|
|
79 ## |
|
|
80 ## The following values are returned: |
|
|
81 ## |
|
|
82 ## @table @var |
|
|
83 ## @item k |
|
|
84 ## The state feedback gain, |
|
|
85 ## @iftex |
|
|
86 ## @tex |
|
|
87 ## $(A - B K)$ |
|
|
88 ## @end tex |
|
|
89 ## @end iftex |
|
|
90 ## @ifinfo |
|
|
91 ## (@var{a} - @var{b}@var{k}) |
|
|
92 ## @end ifinfo |
|
|
93 ## is stable and minimizes the cost functional |
|
|
94 ## |
|
|
95 ## @item p |
|
|
96 ## The stabilizing solution of appropriate algebraic Riccati equation. |
|
|
97 ## |
|
|
98 ## @item e |
|
|
99 ## The vector of the closed loop poles of |
|
|
100 ## @iftex |
|
|
101 ## @tex |
|
|
102 ## $(A - B K)$. |
|
|
103 ## @end tex |
|
|
104 ## @end iftex |
|
|
105 ## @ifinfo |
|
|
106 ## (@var{a} - @var{b}@var{k}). |
|
|
107 ## @end ifinfo |
|
|
108 ## @end table |
|
|
109 ## |
|
|
110 ## @strong{Reference} |
|
5016
|
111 ## Anderson and Moore, @cite{Optimal control: linear quadratic methods}, |
|
|
112 ## Prentice-Hall, 1990, pp. 56--58. |
|
3431
|
113 ## @end deftypefn |
|
|
114 |
|
|
115 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
116 ## Created: August 1993. |
|
|
117 |
|
|
118 function [k, p, e] = lqr (a, b, q, r, s) |
|
|
119 |
|
|
120 ## disp("lqr: entry"); |
|
|
121 |
|
|
122 if ((nargin != 4) && (nargin != 5)) |
|
|
123 error ("lqr: invalid number of arguments"); |
|
|
124 endif |
|
|
125 |
|
|
126 ## Check a. |
|
4030
|
127 if ((n = issquare (a)) == 0) |
|
3431
|
128 error ("lqr: requires 1st parameter(a) to be square"); |
|
|
129 endif |
|
|
130 |
|
|
131 ## Check b. |
|
|
132 [n1, m] = size (b); |
|
|
133 if (n1 != n) |
|
|
134 error ("lqr: a,b not conformal"); |
|
|
135 endif |
|
|
136 |
|
|
137 ## Check q. |
|
4030
|
138 if ( ((n1 = issquare (q)) == 0) || (n1 != n)) |
|
3431
|
139 error ("lqr: q must be square and conformal with a"); |
|
|
140 endif |
|
|
141 |
|
|
142 ## Check r. |
|
4030
|
143 if ( ((m1 = issquare(r)) == 0) || (m1 != m)) |
|
3431
|
144 error ("lqr: r must be square and conformal with column dimension of b"); |
|
|
145 endif |
|
|
146 |
|
|
147 ## Check if n is there. |
|
|
148 if (nargin == 5) |
|
|
149 [n1, m1] = size (s); |
|
|
150 if ( (n1 != n) || (m1 != m)) |
|
|
151 error ("lqr: z must be identically dimensioned with b"); |
|
|
152 endif |
|
|
153 |
|
|
154 ## Incorporate cross term into a and q. |
|
|
155 ao = a - (b/r)*s'; |
|
|
156 qo = q - (s/r)*s'; |
|
|
157 else |
|
|
158 s = zeros (n, m); |
|
|
159 ao = a; |
|
|
160 qo = q; |
|
|
161 endif |
|
|
162 |
|
|
163 ## Check that q, (r) are symmetric, positive (semi)definite |
|
|
164 |
|
4030
|
165 if (issymmetric (q) && issymmetric (r) ... |
|
3431
|
166 && all (eig (q) >= 0) && all (eig (r) > 0)) |
|
|
167 p = are (ao, (b/r)*b', qo); |
|
|
168 k = r\(b'*p + s'); |
|
|
169 e = eig (a - b*k); |
|
|
170 else |
|
|
171 error ("lqr: q (r) must be symmetric positive (semi) definite"); |
|
|
172 endif |
|
|
173 |
|
|
174 ## disp("lqr: exit"); |
|
|
175 endfunction |
