|
3421
|
1 ## Copyright (C) 1996 Auburn University. All rights reserved. |
|
|
2 ## |
|
3426
|
3 ## This file is part of Octave. |
|
|
4 ## |
|
|
5 ## Octave is free software; you can redistribute it and/or modify it |
|
7016
|
6 ## under the terms of the GNU General Public License as published by |
|
|
7 ## the Free Software Foundation; either version 3 of the License, or (at |
|
|
8 ## your option) any later version. |
|
3421
|
9 ## |
|
7016
|
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. |
|
3426
|
14 ## |
|
|
15 ## You should have received a copy of the GNU General Public License |
|
7016
|
16 ## along with Octave; see the file COPYING. If not, see |
|
|
17 ## <http://www.gnu.org/licenses/>. |
|
3421
|
18 |
|
|
19 ## O B S O L E T E * * * D O N O T U S E! |
|
|
20 ## |
|
|
21 ## Use lqg instead. |
|
|
22 ## |
|
|
23 ## function [K,Q,P,Ee,Er] = dlqg(A,B,C,G,Sigw,Sigv,Q,R) |
|
|
24 ## function [K,Q,P,Ee,Er] = dlqg(Sys,Sigw,Sigv,Q,R) |
|
3426
|
25 ## |
|
3421
|
26 ## design a discrete-time linear quadratic gaussian optimal controller |
|
|
27 ## for the system |
|
|
28 ## |
|
|
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 ]) |
|
|
31 ## |
|
|
32 ## Outputs: |
|
|
33 ## K: system data structure format LQG optimal controller |
|
|
34 ## P: Solution of control (state feedback) algebraic Riccati equation |
|
|
35 ## Q: Solution of estimation algebraic Riccati equation |
|
|
36 ## Ee: estimator poles |
|
|
37 ## Es: controller poles |
|
|
38 ## inputs: |
|
3426
|
39 ## A,B,C,G, or Sys: state space representation of system. |
|
|
40 ## Sigw, Sigv: covariance matrices of independent Gaussian noise processes |
|
3421
|
41 ## (as above) |
|
3426
|
42 ## Q, R: state, control weighting matrices for dlqr call respectively. |
|
3421
|
43 ## |
|
|
44 ## See also: lqg, dlqe, dlqr |
|
|
45 |
|
|
46 ## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu> |
|
|
47 ## Created: August 1995 |
|
|
48 |
|
|
49 function [K, Q, P, Ee, Er] = dlqg (A, B, C, G, Sigw, Sigv, Q, R) |
|
|
50 |
|
|
51 warning("dlqg: obsolete. use lqg instead (system data structure format)"); |
|
|
52 |
|
|
53 if (nargin == 5) |
|
|
54 ## system data structure format |
|
|
55 |
|
|
56 ## check that it really is system data structure |
|
4030
|
57 if(! isstruct(A) ) |
|
3421
|
58 error("dlqg: 5 arguments, first argument is not a system data structure structure") |
|
|
59 endif |
|
|
60 |
|
|
61 sys = sysupdate(sys,"ss"); # make sure in proper form |
|
|
62 [ncstates,ndstates,nin,nout] = sysdimensions(sys); |
|
|
63 if(ndstates == -1) |
|
|
64 error("this message should never appear: bad system dimensions"); |
|
|
65 endif |
|
|
66 |
|
|
67 if(ncstates) |
|
|
68 error("dlqg: system has continuous-time states (try lqg?)") |
|
|
69 elseif(ndstates < 1) |
|
|
70 error("dlqg: system has no discrete time states") |
|
|
71 elseif(nin <= columns(Sigw)) |
|
|
72 error(["dlqg: ",num2str(nin)," inputs provided, noise dimension is ", ... |
|
3426
|
73 num2str(columns(Sigw))]) |
|
3421
|
74 elseif(nout != columns(Sigv)) |
|
|
75 error(["dlqg: number of outputs (",num2str(nout),") incompatible with ", ... |
|
3426
|
76 "dimension of Sigv (",num2str(columns(Sigv)),")"]) |
|
3421
|
77 endif |
|
|
78 |
|
|
79 ## put parameters into correct variables |
|
|
80 R = Sigw; |
|
|
81 Q = G; |
|
|
82 Sigv = C; |
|
|
83 Sigw = B; |
|
|
84 [A,B,C,D] = sys2ss(Sys) |
|
|
85 [n,m] = size(B) |
|
|
86 m1 = columns(Sigw); |
|
|
87 m2 = m1+1; |
|
|
88 G = B(:,1:m1); |
|
|
89 B = B(:,m2:m); |
|
|
90 |
|
|
91 elseif (nargin == 8) |
|
|
92 ## state-space format |
|
|
93 m = columns(B); |
|
|
94 m1 = columns(G); |
|
|
95 p = rows(C); |
|
|
96 n = abcddim(A,B,C,zeros(p,m)); |
|
|
97 n1 = abcddim(A,G,C,zeros(p,m1)); |
|
|
98 if( (n == -1) || (n1 == -1)) |
|
|
99 error("dlqg: A,B,C,G incompatibly dimensioned"); |
|
|
100 elseif(p != columns(Sigv)) |
|
|
101 error("dlqg: C, Sigv incompatibly dimensioned"); |
|
|
102 elseif(m1 != columns(Sigw)) |
|
|
103 error("dlqg: G, Sigw incompatibly dimensioned"); |
|
|
104 endif |
|
|
105 else |
|
3427
|
106 error ("dlqg: invalid number of arguments") |
|
3421
|
107 endif |
|
|
108 |
|
4030
|
109 if (! (issquare(Sigw) && issquare(Sigv) ) ) |
|
3421
|
110 error("dlqg: Sigw, Sigv must be square"); |
|
|
111 endif |
|
|
112 |
|
|
113 ## now we can just do the design; call dlqr and dlqe, since all matrices |
|
|
114 ## are not given in Cholesky factor form (as in h2syn case) |
|
|
115 [Ks, P, Er] = dlqr(A,B,Q,R); |
|
|
116 [Ke, Q, jnk, Ee] = dlqe(A,G,C,Sigw,Sigv); |
|
|
117 Ac = A - Ke*C - B*Ks; |
|
|
118 Bc = Ke; |
|
|
119 Cc = -Ks; |
|
|
120 Dc = zeros(rows(Cc),columns(Bc)); |
|
4771
|
121 K = ss(Ac,Bc,Cc,Dc,1); |
|
3421
|
122 disp("HODEL: need to add names to this guy!") |
|
|
123 |
|
|
124 endfunction |
