|
7017
|
1 ## Copyright (C) 1996, 1997, 2000, 2002, 2004, 2005, 2006, 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 -*- |
|
3502
|
21 ## @deftypefn {Function File} {[@var{k}, @var{q1}, @var{p1}, @var{ee}, @var{er}] =} lqg (@var{sys}, @var{sigw}, @var{sigv}, @var{q}, @var{r}, @var{in_idx}) |
|
3431
|
22 ## Design a linear-quadratic-gaussian optimal controller for the system |
|
|
23 ## @example |
|
|
24 ## dx/dt = A x + B u + G w [w]=N(0,[Sigw 0 ]) |
|
|
25 ## y = C x + v [v] ( 0 Sigv ]) |
|
|
26 ## @end example |
|
|
27 ## or |
|
|
28 ## @example |
|
7031
|
29 ## x(k+1) = A x(k) + B u(k) + G w(k) [w]=N(0,[Sigw 0 ]) |
|
|
30 ## y(k) = C x(k) + v(k) [v] ( 0 Sigv ]) |
|
3431
|
31 ## @end example |
|
|
32 ## |
|
|
33 ## @strong{Inputs} |
|
|
34 ## @table @var |
|
|
35 ## @item sys |
|
|
36 ## system data structure |
|
3502
|
37 ## @item sigw |
|
|
38 ## @itemx sigv |
|
3431
|
39 ## intensities of independent Gaussian noise processes (as above) |
|
3502
|
40 ## @item q |
|
|
41 ## @itemx r |
|
5016
|
42 ## state, control weighting respectively. Control @acronym{ARE} is |
|
3431
|
43 ## @item in_idx |
|
5016
|
44 ## names or indices of controlled inputs (see @command{sysidx}, @command{cellidx}) |
|
3431
|
45 ## |
|
|
46 ## default: last dim(R) inputs are assumed to be controlled inputs, all |
|
|
47 ## others are assumed to be noise inputs. |
|
|
48 ## @end table |
|
|
49 ## @strong{Outputs} |
|
|
50 ## @table @var |
|
3502
|
51 ## @item k |
|
5016
|
52 ## system data structure format @acronym{LQG} optimal controller (Obtain A, B, C |
|
|
53 ## matrices with @command{sys2ss}, @command{sys2tf}, or @command{sys2zp} as |
|
|
54 ## appropriate). |
|
3502
|
55 ## @item p1 |
|
5016
|
56 ## Solution of control (state feedback) algebraic Riccati equation. |
|
3502
|
57 ## @item q1 |
|
5016
|
58 ## Solution of estimation algebraic Riccati equation. |
|
3502
|
59 ## @item ee |
|
5016
|
60 ## Estimator poles. |
|
3502
|
61 ## @item es |
|
5016
|
62 ## Controller poles. |
|
3431
|
63 ## @end table |
|
5642
|
64 ## @seealso{h2syn, lqe, lqr} |
|
3431
|
65 ## @end deftypefn |
|
|
66 |
|
|
67 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
68 ## Created: August 1995 |
|
|
69 ## revised for new system format August 1996 |
|
|
70 |
|
|
71 function [K, Q1, P1, Ee, Er] = lqg (sys, Sigw, Sigv, Q, R, input_list) |
|
|
72 |
|
7126
|
73 if (nargin < 5 || nargin > 6) |
|
6046
|
74 print_usage (); |
|
7126
|
75 elseif (! isstruct (sys)) |
|
|
76 error ("sys must be in system data structure"); |
|
3431
|
77 endif |
|
|
78 |
|
7126
|
79 DIG = is_digital (sys); |
|
|
80 [A, B, C, D, tsam, n, nz, stname, inname, outname] = sys2ss (sys); |
|
|
81 [n, nz, nin, nout] = sysdimensions (sys); |
|
|
82 if (nargin == 5) |
|
3431
|
83 ## construct default input_list |
|
|
84 input_list = (columns(Sigw)+1):nin; |
|
|
85 endif |
|
|
86 |
|
7126
|
87 if (! (n+nz)) |
|
|
88 error("lqg: 0 states in system"); |
|
3431
|
89 |
|
7126
|
90 elseif (nin != columns (Sigw) + columns (R)) |
|
|
91 error ("lqg: sys has %d inputs, dim(Sigw)=%d, dim(u)=%d", |
|
|
92 nin, columns (Sigw), columns (R)); |
|
3431
|
93 |
|
7126
|
94 elseif (nout != columns (Sigv)) |
|
|
95 error ("lqg: sys has %d outputs, dim(Sigv)=%d", nout, columns (Sigv)); |
|
3462
|
96 endif |
|
|
97 |
|
|
98 ## check for names of signals |
|
7126
|
99 if (is_signal_list (input_list) || ischar (input_list)) |
|
|
100 input_list = sysidx (sys, "in", input_list); |
|
3462
|
101 endif |
|
|
102 |
|
7126
|
103 if (length (input_list) != columns (R)) |
|
|
104 error ("lqg: length(input_list)=%d, columns(R)=%d", |
|
|
105 length (input_list), columns (R)); |
|
|
106 endif |
|
|
107 |
|
|
108 if (! issquare (Sigw)) |
|
|
109 error ("lqg: Sigw is not square"); |
|
3431
|
110 endif |
|
|
111 |
|
7126
|
112 if (! issquare (Sigv)) |
|
|
113 error ("lqg: Sigv is not square"); |
|
|
114 endif |
|
|
115 |
|
|
116 if (! issquare (Q)) |
|
|
117 error ("lqg: Q is not square"); |
|
|
118 endif |
|
|
119 |
|
|
120 if (! issquare (R)) |
|
|
121 error ("lqg: Q is not square"); |
|
|
122 endif |
|
3431
|
123 |
|
|
124 ## permute (if need be) |
|
7126
|
125 if (nargin == 6) |
|
|
126 all_inputs = sysreorder (nin, input_list); |
|
3431
|
127 B = B(:,all_inputs); |
|
7126
|
128 inname = inname (all_inputs); |
|
3431
|
129 endif |
|
|
130 |
|
|
131 ## put parameters into correct variables |
|
7126
|
132 m1 = columns (Sigw); |
|
3431
|
133 m2 = m1+1; |
|
|
134 G = B(:,1:m1); |
|
|
135 B = B(:,m2:nin); |
|
|
136 |
|
|
137 ## now we can just do the design; call dlqr and dlqe, since all matrices |
|
|
138 ## are not given in Cholesky factor form (as in h2syn case) |
|
7126
|
139 if (DIG) |
|
|
140 [Ks, P1, Er] = dlqr (A, B, Q, R); |
|
|
141 [Ke, Q1, jnk, Ee] = dlqe (A, G, C, Sigw, Sigv); |
|
3431
|
142 else |
|
7126
|
143 [Ks, P1, Er] = lqr (A, B, Q, R); |
|
|
144 [Ke, Q1, Ee] = lqe (A, G, C, Sigw, Sigv); |
|
3431
|
145 endif |
|
|
146 Ac = A - Ke*C - B*Ks; |
|
|
147 Bc = Ke; |
|
|
148 Cc = -Ks; |
|
7126
|
149 Dc = zeros (rows (Cc), columns (Bc)); |
|
3431
|
150 |
|
|
151 ## fix state names |
|
7126
|
152 stname1 = strappend (stname, "_e"); |
|
3431
|
153 |
|
|
154 ## fix controller output names |
|
7126
|
155 outname1 = strappend (inname(m2:nin), "_K"); |
|
3431
|
156 |
|
|
157 ## fix controller input names |
|
7126
|
158 inname1 = strappend (outname, "_K"); |
|
3431
|
159 |
|
7126
|
160 if (DIG) |
|
|
161 K = ss (Ac, Bc, Cc, Dc, tsam, n, nz, stname1, inname1, outname1, |
|
|
162 1:rows(Cc)); |
|
3431
|
163 else |
|
7126
|
164 K = ss (Ac, Bc, Cc, Dc, tsam, n, nz, stname, inname1, outname1); |
|
3431
|
165 endif |
|
|
166 |
|
|
167 endfunction |
