|
3696
|
1 ## Copyright (C) 1993, 1994, 1995 Auburn University |
|
3431
|
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 |
|
5307
|
17 ## Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA |
|
|
18 ## 02110-1301 USA. |
|
3431
|
19 |
|
|
20 ## -*- texinfo -*- |
|
|
21 ## @deftypefn {Function File} {[@var{k}, @var{p}, @var{e}] =} dlqr (@var{a}, @var{b}, @var{q}, @var{r}, @var{z}) |
|
|
22 ## Construct the linear quadratic regulator for the discrete time system |
|
|
23 ## @iftex |
|
|
24 ## @tex |
|
|
25 ## $$ |
|
|
26 ## x_{k+1} = A x_k + B u_k |
|
|
27 ## $$ |
|
|
28 ## @end tex |
|
|
29 ## @end iftex |
|
|
30 ## @ifinfo |
|
|
31 ## |
|
|
32 ## @example |
|
|
33 ## x[k+1] = A x[k] + B u[k] |
|
|
34 ## @end example |
|
|
35 ## |
|
|
36 ## @end ifinfo |
|
|
37 ## to minimize the cost functional |
|
|
38 ## @iftex |
|
|
39 ## @tex |
|
|
40 ## $$ |
|
|
41 ## J = \sum x^T Q x + u^T R u |
|
|
42 ## $$ |
|
|
43 ## @end tex |
|
|
44 ## @end iftex |
|
|
45 ## @ifinfo |
|
|
46 ## |
|
|
47 ## @example |
|
|
48 ## J = Sum (x' Q x + u' R u) |
|
|
49 ## @end example |
|
|
50 ## @end ifinfo |
|
|
51 ## |
|
|
52 ## @noindent |
|
|
53 ## @var{z} omitted or |
|
|
54 ## @iftex |
|
|
55 ## @tex |
|
|
56 ## $$ |
|
|
57 ## J = \sum x^T Q x + u^T R u + 2 x^T Z u |
|
|
58 ## $$ |
|
|
59 ## @end tex |
|
|
60 ## @end iftex |
|
|
61 ## @ifinfo |
|
|
62 ## |
|
|
63 ## @example |
|
|
64 ## J = Sum (x' Q x + u' R u + 2 x' Z u) |
|
|
65 ## @end example |
|
|
66 ## |
|
|
67 ## @end ifinfo |
|
|
68 ## @var{z} included. |
|
|
69 ## |
|
|
70 ## The following values are returned: |
|
|
71 ## |
|
|
72 ## @table @var |
|
|
73 ## @item k |
|
|
74 ## The state feedback gain, |
|
|
75 ## @iftex |
|
|
76 ## @tex |
|
|
77 ## $(A - B K)$ |
|
|
78 ## @end tex |
|
|
79 ## @end iftex |
|
|
80 ## @ifinfo |
|
|
81 ## (@var{a} - @var{b}@var{k}) |
|
|
82 ## @end ifinfo |
|
|
83 ## is stable. |
|
|
84 ## |
|
|
85 ## @item p |
|
|
86 ## The solution of algebraic Riccati equation. |
|
|
87 ## |
|
|
88 ## @item e |
|
|
89 ## The closed loop poles of |
|
|
90 ## @iftex |
|
|
91 ## @tex |
|
|
92 ## $(A - B K)$. |
|
|
93 ## @end tex |
|
|
94 ## @end iftex |
|
|
95 ## @ifinfo |
|
|
96 ## (@var{a} - @var{b}@var{k}). |
|
|
97 ## @end ifinfo |
|
|
98 ## @end table |
|
|
99 ## @end deftypefn |
|
|
100 |
|
|
101 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
102 ## Created: August 1993 |
|
|
103 ## Converted to discrete time by R. B. Tenison |
|
|
104 ## (btenison@eng.auburn.edu) October 1993 |
|
|
105 |
|
|
106 function [k, p, e] = dlqr (a, b, q, r, s) |
|
|
107 |
|
|
108 if (nargin != 4 && nargin != 5) |
|
|
109 error ("dlqr: invalid number of arguments"); |
|
|
110 endif |
|
|
111 |
|
4611
|
112 ## Dimension check is done inside dare.m |
|
|
113 [n,m] = size(b); |
|
3431
|
114 |
|
4611
|
115 ## Check if s is there. |
|
3431
|
116 if (nargin == 5) |
|
|
117 [n1, m1] = size (s); |
|
|
118 if (n1 != n || m1 != m) |
|
|
119 error ("dlqr: z must be identically dimensioned with b"); |
|
|
120 endif |
|
|
121 |
|
|
122 ## Incorporate cross term into a and q. |
|
|
123 ao = a - (b/r)*s'; |
|
|
124 qo = q - (s/r)*s'; |
|
|
125 else |
|
|
126 s = zeros (n, m); |
|
|
127 ao = a; |
|
|
128 qo = q; |
|
|
129 endif |
|
|
130 |
|
4611
|
131 ## Checking stabilizability and detectability (dimensions are checked inside these calls) |
|
|
132 tol = 200*eps; |
|
|
133 if (is_stabilizable (ao, b,tol,1) == 0) |
|
|
134 error ("dlqr: (a,b) not stabilizable"); |
|
3431
|
135 endif |
|
4611
|
136 dflag = is_detectable (ao, qo, tol,1); |
|
|
137 if ( dflag == 0) |
|
|
138 warning ("dlqr: (a,q) not detectable"); |
|
|
139 elseif ( dflag == -1) |
|
|
140 error("dlqr: (a,q) has non minimal modes near unit circle"); |
|
|
141 end |
|
|
142 |
|
|
143 ## Compute the Riccati solution |
|
|
144 p = dare (ao, b, qo, r); |
|
|
145 k = (r+b'*p*b)\(b'*p*a + s'); |
|
|
146 e = eig (a - b*k); |
|
|
147 |
|
3431
|
148 |
|
|
149 endfunction |
|
3696
|
150 |
