|
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 |
|
|
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 -*- |
|
3502
|
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 $$ |
|
|
31 ## $$ P(t_f) = Qf $$ |
|
3431
|
32 ## @end tex |
|
|
33 ## @end iftex |
|
|
34 ## for the LTI system sys. Solution of standard LTI |
|
|
35 ## state feedback optimization |
|
3439
|
36 ## @ifinfo |
|
|
37 ## @example |
|
|
38 ## min \int_@{t_0@}^@{t_f@} x' Q x + u' R u dt + x(t_f)' Qf x(t_f) |
|
|
39 ## @end example |
|
|
40 ## @end ifinfo |
|
|
41 ## @iftex |
|
|
42 ## @tex |
|
|
43 ## $$ \min \int_{t_0}^{t_f} x^T Q x + u^T R u dt + x(t_f)^T Qf x(t_f) $$ |
|
|
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 |
|
4787
|
80 ## list values of @var{p}(@var{t}); @var{plist} @{ @var{ii} @} |
|
3502
|
81 ## is @var{p}(@var{tvals}(@var{ii})). |
|
3431
|
82 ## |
|
|
83 ## @item tvals |
|
|
84 ## @example |
|
4787
|
85 ## is selected so that || Plist@{ii@} - Plist@{ii-1@} || < Ptol |
|
3431
|
86 ## for ii=2:length(tvals) |
|
|
87 ## @end example |
|
|
88 ## @end table |
|
|
89 ## @end deftypefn |
|
|
90 |
|
|
91 function [tvals, Plist] = dre (sys, Q, R, Qf, t0, tf, Ptol, maxits) |
|
|
92 |
|
|
93 if(nargin < 6 | nargin > 8 | nargout != 2) |
|
|
94 usage("[tvals,Plist] = dre(sys,Q,R,Qf,t0,tf{,Ptol})"); |
|
4030
|
95 elseif(!isstruct(sys)) |
|
3431
|
96 error("sys must be a system data structure") |
|
|
97 elseif(is_digital(sys)) |
|
|
98 error("sys must be a continuous time system") |
|
4030
|
99 elseif(!ismatrix(Q) | !ismatrix(R) | !ismatrix(Qf)) |
|
3431
|
100 error("Q, R, and Qf must be matrices."); |
|
4030
|
101 elseif(!isscalar(t0) | !isscalar(tf)) |
|
3431
|
102 error("t0 and tf must be scalars") |
|
|
103 elseif(t0 >= tf) error("t0=%e >= tf=%e",t0,tf); |
|
|
104 elseif(nargin == 6) Ptol = 0.1; |
|
4030
|
105 elseif(!isscalar(Ptol)) error("Ptol must be a scalar"); |
|
3431
|
106 elseif(Ptol <= 0) error("Ptol must be positive"); |
|
|
107 endif |
|
|
108 |
|
|
109 if(nargin < 8) maxits = 10; |
|
4030
|
110 elseif(!isscalar(maxits)) error("maxits must be a scalar"); |
|
3431
|
111 elseif(maxits <= 0) error("maxits must be positive"); |
|
|
112 endif |
|
|
113 maxits = ceil(maxits); |
|
|
114 |
|
|
115 [aa,bb] = sys2ss(sys); |
|
|
116 nn = sysdimensions(sys,"cst"); |
|
|
117 mm = sysdimensions(sys,"in"); |
|
|
118 pp = sysdimensions(sys,"out"); |
|
|
119 |
|
|
120 if(size(Q) != [nn, nn]) |
|
|
121 error("Q(%dx%d); sys has %d states",rows(Q),columns(Q),nn); |
|
|
122 elseif(size(Qf) != [nn, nn]) |
|
|
123 error("Qf(%dx%d); sys has %d states",rows(Qf),columns(Qf),nn); |
|
|
124 elseif(size(R) != [mm, mm]) |
|
|
125 error("R(%dx%d); sys has %d inputs",rows(R),columns(R),mm); |
|
|
126 endif |
|
|
127 |
|
|
128 ## construct Hamiltonian matrix |
|
|
129 H = [aa , -(bb/R)*bb' ; -Q, -aa']; |
|
|
130 |
|
|
131 ## select time step to avoid numerical overflow |
|
|
132 fast_eig = max(abs(eig(H))); |
|
|
133 tc = log(10)/fast_eig; |
|
|
134 nst = ceil((tf-t0)/tc); |
|
|
135 tvals = -linspace(-tf,-t0,nst); |
|
|
136 Plist = list(Qf); |
|
|
137 In = eye(nn); |
|
|
138 n1 = nn+1; |
|
|
139 n2 = nn+nn; |
|
|
140 done = 0; |
|
|
141 while(!done) |
|
|
142 done = 1; # assume this pass will do the job |
|
|
143 ## sort time values in reverse order |
|
|
144 tvals = -sort(-tvals); |
|
|
145 tvlen = length(tvals); |
|
|
146 maxerr = 0; |
|
|
147 ## compute new values of P(t); recompute old values just in case |
|
|
148 for ii=2:tvlen |
|
4771
|
149 uv_i_minus_1 = [ In ; Plist{ii-1} ]; |
|
3431
|
150 delta_t = tvals(ii-1) - tvals(ii); |
|
|
151 uv = expm(-H*delta_t)*uv_i_minus_1; |
|
|
152 Qi = uv(n1:n2,1:nn)/uv(1:nn,1:nn); |
|
|
153 Plist(ii) = (Qi+Qi')/2; |
|
|
154 ## check error |
|
4771
|
155 Perr = norm(Plist{ii} - Plist{ii-1})/norm(Plist{ii}); |
|
3431
|
156 maxerr = max(maxerr,Perr); |
|
|
157 if(Perr > Ptol) |
|
|
158 new_t = mean(tvals([ii,ii-1])); |
|
|
159 tvals = [tvals, new_t]; |
|
|
160 done = 0; |
|
|
161 endif |
|
|
162 endfor |
|
|
163 |
|
|
164 ## check number of iterations |
|
|
165 maxits = maxits - 1; |
|
|
166 done = done+(maxits==0); |
|
|
167 endwhile |
|
|
168 if(maxerr > Ptol) |
|
|
169 warning("dre: \n\texiting with%4d points, max rel chg. =%e, Ptol=%e\n", ... |
|
|
170 tvlen,maxerr,Ptol); |
|
|
171 tvals = tvals(1:length(Plist)); |
|
|
172 endif |
|
|
173 |
|
|
174 endfunction |
