|
3431
|
1 ## Copyright (C) 1993, 1994, 1995 Auburn University. All rights reserved. |
|
|
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 the |
|
|
7 ## Free Software Foundation; either version 2, or (at your option) any |
|
|
8 ## later version. |
|
|
9 ## |
|
|
10 ## Octave is distributed in the hope that it will be useful, but WITHOUT |
|
|
11 ## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
|
|
12 ## FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License |
|
|
13 ## 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 02111 USA. |
|
|
18 |
|
|
19 ## -*- texinfo -*- |
|
|
20 ## @deftypefn {Function File} {[@var{k}, @var{p}, @var{e}] =} dlqr (@var{a}, @var{b}, @var{q}, @var{r}, @var{z}) |
|
|
21 ## Construct the linear quadratic regulator for the discrete time system |
|
|
22 ## @iftex |
|
|
23 ## @tex |
|
|
24 ## $$ |
|
|
25 ## x_{k+1} = A x_k + B u_k |
|
|
26 ## $$ |
|
|
27 ## @end tex |
|
|
28 ## @end iftex |
|
|
29 ## @ifinfo |
|
|
30 ## |
|
|
31 ## @example |
|
|
32 ## x[k+1] = A x[k] + B u[k] |
|
|
33 ## @end example |
|
|
34 ## |
|
|
35 ## @end ifinfo |
|
|
36 ## to minimize the cost functional |
|
|
37 ## @iftex |
|
|
38 ## @tex |
|
|
39 ## $$ |
|
|
40 ## J = \sum x^T Q x + u^T R u |
|
|
41 ## $$ |
|
|
42 ## @end tex |
|
|
43 ## @end iftex |
|
|
44 ## @ifinfo |
|
|
45 ## |
|
|
46 ## @example |
|
|
47 ## J = Sum (x' Q x + u' R u) |
|
|
48 ## @end example |
|
|
49 ## @end ifinfo |
|
|
50 ## |
|
|
51 ## @noindent |
|
|
52 ## @var{z} omitted or |
|
|
53 ## @iftex |
|
|
54 ## @tex |
|
|
55 ## $$ |
|
|
56 ## J = \sum x^T Q x + u^T R u + 2 x^T Z u |
|
|
57 ## $$ |
|
|
58 ## @end tex |
|
|
59 ## @end iftex |
|
|
60 ## @ifinfo |
|
|
61 ## |
|
|
62 ## @example |
|
|
63 ## J = Sum (x' Q x + u' R u + 2 x' Z u) |
|
|
64 ## @end example |
|
|
65 ## |
|
|
66 ## @end ifinfo |
|
|
67 ## @var{z} included. |
|
|
68 ## |
|
|
69 ## The following values are returned: |
|
|
70 ## |
|
|
71 ## @table @var |
|
|
72 ## @item k |
|
|
73 ## The state feedback gain, |
|
|
74 ## @iftex |
|
|
75 ## @tex |
|
|
76 ## $(A - B K)$ |
|
|
77 ## @end tex |
|
|
78 ## @end iftex |
|
|
79 ## @ifinfo |
|
|
80 ## (@var{a} - @var{b}@var{k}) |
|
|
81 ## @end ifinfo |
|
|
82 ## is stable. |
|
|
83 ## |
|
|
84 ## @item p |
|
|
85 ## The solution of algebraic Riccati equation. |
|
|
86 ## |
|
|
87 ## @item e |
|
|
88 ## The closed loop poles of |
|
|
89 ## @iftex |
|
|
90 ## @tex |
|
|
91 ## $(A - B K)$. |
|
|
92 ## @end tex |
|
|
93 ## @end iftex |
|
|
94 ## @ifinfo |
|
|
95 ## (@var{a} - @var{b}@var{k}). |
|
|
96 ## @end ifinfo |
|
|
97 ## @end table |
|
|
98 ## @strong{References} |
|
|
99 ## @enumerate |
|
|
100 ## @item Anderson and Moore, Optimal Control: Linear Quadratic Methods, |
|
|
101 ## Prentice-Hall, 1990, pp. 56-58 |
|
|
102 ## @item Kuo, Digital Control Systems, Harcourt Brace Jovanovich, 1992, |
|
|
103 ## section 11-5-2. |
|
|
104 ## @end enumerate |
|
|
105 ## @end deftypefn |
|
|
106 |
|
|
107 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
108 ## Created: August 1993 |
|
|
109 ## Converted to discrete time by R. B. Tenison |
|
|
110 ## (btenison@eng.auburn.edu) October 1993 |
|
|
111 |
|
|
112 function [k, p, e] = dlqr (a, b, q, r, s) |
|
|
113 |
|
|
114 if (nargin != 4 && nargin != 5) |
|
|
115 error ("dlqr: invalid number of arguments"); |
|
|
116 endif |
|
|
117 |
|
|
118 ## Check a. |
|
|
119 if ((n = is_square (a)) == 0) |
|
|
120 error ("dlqr: requires 1st parameter(a) to be square"); |
|
|
121 endif |
|
|
122 |
|
|
123 ## Check b. |
|
|
124 [n1, m] = size (b); |
|
|
125 if (n1 != n) |
|
|
126 error ("dlqr: a,b not conformal"); |
|
|
127 endif |
|
|
128 |
|
|
129 ## Check q. |
|
|
130 if ((n1 = is_square (q)) == 0 || n1 != n) |
|
|
131 error ("dlqr: q must be square and conformal with a"); |
|
|
132 endif |
|
|
133 |
|
|
134 ## Check r. |
|
|
135 if((m1 = is_square(r)) == 0 || m1 != m) |
|
|
136 error ("dlqr: r must be square and conformal with column dimension of b"); |
|
|
137 endif |
|
|
138 |
|
|
139 ## Check if n is there. |
|
|
140 if (nargin == 5) |
|
|
141 [n1, m1] = size (s); |
|
|
142 if (n1 != n || m1 != m) |
|
|
143 error ("dlqr: z must be identically dimensioned with b"); |
|
|
144 endif |
|
|
145 |
|
|
146 ## Incorporate cross term into a and q. |
|
|
147 |
|
|
148 ao = a - (b/r)*s'; |
|
|
149 qo = q - (s/r)*s'; |
|
|
150 else |
|
|
151 s = zeros (n, m); |
|
|
152 ao = a; |
|
|
153 qo = q; |
|
|
154 endif |
|
|
155 |
|
|
156 ## Check that q, (r) are symmetric, positive (semi)definite |
|
|
157 if (is_symmetric (q) && is_symmetric (r) ... |
|
|
158 && all (eig (q) >= 0) && all (eig (r) > 0)) |
|
|
159 p = dare (ao, b, qo, r); |
|
|
160 k = (r+b'*p*b)\b'*p*a + r\s'; |
|
|
161 e = eig (a - b*k); |
|
|
162 else |
|
|
163 error ("dlqr: q (r) must be symmetric positive (semi) definite"); |
|
|
164 endif |
|
|
165 |
|
|
166 endfunction |
