|
3431
|
1 ## Copyright (C) 1998 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 |
|
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. |
|
3431
|
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. |
|
3431
|
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/>. |
|
3431
|
18 |
|
|
19 ## -*- texinfo -*- |
|
5016
|
20 ## @deftypefn {Function File} {[@var{tvals}, @var{plist}] =} dre (@var{sys}, @var{q}, @var{r}, @var{qf}, @var{t0}, @var{tf}, @var{ptol}, @var{maxits}) |
|
3431
|
21 ## Solve the differential Riccati equation |
|
|
22 ## @ifinfo |
|
|
23 ## @example |
|
|
24 ## -d P/dt = A'P + P A - P B inv(R) B' P + Q |
|
|
25 ## P(tf) = Qf |
|
3439
|
26 ## @end example |
|
3431
|
27 ## @end ifinfo |
|
|
28 ## @iftex |
|
|
29 ## @tex |
|
3439
|
30 ## $$ -{dP \over dt} = A^T P+PA-PBR^{-1}B^T P+Q $$ |
|
5016
|
31 ## $$ P(t_f) = Q_f $$ |
|
3431
|
32 ## @end tex |
|
|
33 ## @end iftex |
|
5016
|
34 ## for the @acronym{LTI} system sys. Solution of |
|
|
35 ## standard @acronym{LTI} state feedback optimization |
|
3439
|
36 ## @ifinfo |
|
|
37 ## @example |
|
5016
|
38 ## min int(t0, tf) ( x' Q x + u' R u ) dt + x(tf)' Qf x(tf) |
|
3439
|
39 ## @end example |
|
|
40 ## @end ifinfo |
|
|
41 ## @iftex |
|
|
42 ## @tex |
|
5016
|
43 ## $$ \min \int_{t_0}^{t_f} x^T Q x + u^T R u dt + x(t_f)^T Q_f x(t_f) $$ |
|
3439
|
44 ## @end tex |
|
|
45 ## @end iftex |
|
3431
|
46 ## optimal input is |
|
3439
|
47 ## @ifinfo |
|
|
48 ## @example |
|
3431
|
49 ## u = - inv(R) B' P(t) x |
|
3439
|
50 ## @end example |
|
|
51 ## @end ifinfo |
|
|
52 ## @iftex |
|
|
53 ## @tex |
|
|
54 ## $$ u = - R^{-1} B^T P(t) x $$ |
|
|
55 ## @end tex |
|
|
56 ## @end iftex |
|
3431
|
57 ## @strong{Inputs} |
|
3439
|
58 ## @table @var |
|
3431
|
59 ## @item sys |
|
|
60 ## continuous time system data structure |
|
3502
|
61 ## @item q |
|
3431
|
62 ## state integral penalty |
|
3502
|
63 ## @item r |
|
3431
|
64 ## input integral penalty |
|
3502
|
65 ## @item qf |
|
3431
|
66 ## state terminal penalty |
|
|
67 ## @item t0 |
|
|
68 ## @itemx tf |
|
|
69 ## limits on the integral |
|
3502
|
70 ## @item ptol |
|
3431
|
71 ## tolerance (used to select time samples; see below); default = 0.1 |
|
|
72 ## @item maxits |
|
|
73 ## number of refinement iterations (default=10) |
|
|
74 ## @end table |
|
|
75 ## @strong{Outputs} |
|
3439
|
76 ## @table @var |
|
3431
|
77 ## @item tvals |
|
3502
|
78 ## time values at which @var{p}(@var{t}) is computed |
|
|
79 ## @item plist |
|
5016
|
80 ## list values of @var{p}(@var{t}); @var{plist} @{ @var{i} @} |
|
|
81 ## is @var{p}(@var{tvals}(@var{i})) |
|
|
82 ## @end table |
|
|
83 ## @var{tvals} is selected so that: |
|
|
84 ## @iftex |
|
|
85 ## @tex |
|
|
86 ## $$ \Vert plist_{i} - plist_{i-1} \Vert < ptol $$ |
|
|
87 ## @end tex |
|
|
88 ## @end iftex |
|
|
89 ## @ifinfo |
|
3431
|
90 ## @example |
|
5016
|
91 ## || Plist@{i@} - Plist@{i-1@} || < Ptol |
|
3431
|
92 ## @end example |
|
5016
|
93 ## @end ifinfo |
|
|
94 ## for every @var{i} between 2 and length(@var{tvals}). |
|
3431
|
95 ## @end deftypefn |
|
|
96 |
|
|
97 function [tvals, Plist] = dre (sys, Q, R, Qf, t0, tf, Ptol, maxits) |
|
|
98 |
|
|
99 if(nargin < 6 | nargin > 8 | nargout != 2) |
|
6046
|
100 print_usage (); |
|
4030
|
101 elseif(!isstruct(sys)) |
|
3431
|
102 error("sys must be a system data structure") |
|
|
103 elseif(is_digital(sys)) |
|
|
104 error("sys must be a continuous time system") |
|
4030
|
105 elseif(!ismatrix(Q) | !ismatrix(R) | !ismatrix(Qf)) |
|
3431
|
106 error("Q, R, and Qf must be matrices."); |
|
4030
|
107 elseif(!isscalar(t0) | !isscalar(tf)) |
|
3431
|
108 error("t0 and tf must be scalars") |
|
|
109 elseif(t0 >= tf) error("t0=%e >= tf=%e",t0,tf); |
|
|
110 elseif(nargin == 6) Ptol = 0.1; |
|
4030
|
111 elseif(!isscalar(Ptol)) error("Ptol must be a scalar"); |
|
3431
|
112 elseif(Ptol <= 0) error("Ptol must be positive"); |
|
|
113 endif |
|
|
114 |
|
|
115 if(nargin < 8) maxits = 10; |
|
4030
|
116 elseif(!isscalar(maxits)) error("maxits must be a scalar"); |
|
3431
|
117 elseif(maxits <= 0) error("maxits must be positive"); |
|
|
118 endif |
|
|
119 maxits = ceil(maxits); |
|
|
120 |
|
|
121 [aa,bb] = sys2ss(sys); |
|
|
122 nn = sysdimensions(sys,"cst"); |
|
|
123 mm = sysdimensions(sys,"in"); |
|
|
124 pp = sysdimensions(sys,"out"); |
|
|
125 |
|
|
126 if(size(Q) != [nn, nn]) |
|
|
127 error("Q(%dx%d); sys has %d states",rows(Q),columns(Q),nn); |
|
|
128 elseif(size(Qf) != [nn, nn]) |
|
|
129 error("Qf(%dx%d); sys has %d states",rows(Qf),columns(Qf),nn); |
|
|
130 elseif(size(R) != [mm, mm]) |
|
|
131 error("R(%dx%d); sys has %d inputs",rows(R),columns(R),mm); |
|
|
132 endif |
|
|
133 |
|
|
134 ## construct Hamiltonian matrix |
|
|
135 H = [aa , -(bb/R)*bb' ; -Q, -aa']; |
|
|
136 |
|
|
137 ## select time step to avoid numerical overflow |
|
|
138 fast_eig = max(abs(eig(H))); |
|
|
139 tc = log(10)/fast_eig; |
|
|
140 nst = ceil((tf-t0)/tc); |
|
|
141 tvals = -linspace(-tf,-t0,nst); |
|
|
142 Plist = list(Qf); |
|
|
143 In = eye(nn); |
|
|
144 n1 = nn+1; |
|
|
145 n2 = nn+nn; |
|
|
146 done = 0; |
|
|
147 while(!done) |
|
|
148 done = 1; # assume this pass will do the job |
|
|
149 ## sort time values in reverse order |
|
|
150 tvals = -sort(-tvals); |
|
|
151 tvlen = length(tvals); |
|
|
152 maxerr = 0; |
|
|
153 ## compute new values of P(t); recompute old values just in case |
|
|
154 for ii=2:tvlen |
|
4771
|
155 uv_i_minus_1 = [ In ; Plist{ii-1} ]; |
|
3431
|
156 delta_t = tvals(ii-1) - tvals(ii); |
|
|
157 uv = expm(-H*delta_t)*uv_i_minus_1; |
|
|
158 Qi = uv(n1:n2,1:nn)/uv(1:nn,1:nn); |
|
|
159 Plist(ii) = (Qi+Qi')/2; |
|
|
160 ## check error |
|
4771
|
161 Perr = norm(Plist{ii} - Plist{ii-1})/norm(Plist{ii}); |
|
3431
|
162 maxerr = max(maxerr,Perr); |
|
|
163 if(Perr > Ptol) |
|
|
164 new_t = mean(tvals([ii,ii-1])); |
|
|
165 tvals = [tvals, new_t]; |
|
|
166 done = 0; |
|
|
167 endif |
|
|
168 endfor |
|
|
169 |
|
|
170 ## check number of iterations |
|
|
171 maxits = maxits - 1; |
|
|
172 done = done+(maxits==0); |
|
|
173 endwhile |
|
|
174 if(maxerr > Ptol) |
|
|
175 warning("dre: \n\texiting with%4d points, max rel chg. =%e, Ptol=%e\n", ... |
|
|
176 tvlen,maxerr,Ptol); |
|
|
177 tvals = tvals(1:length(Plist)); |
|
|
178 endif |
|
|
179 |
|
|
180 endfunction |
