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