|
3381
|
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 ## [tvals,Plist] = dre(sys,Q,R,Qf,t0,tf{,Ptol,maxits}); |
|
|
20 ## Solve the differential Riccati equation |
|
|
21 ## -d P/dt = A'P + P A - P B inv(R) B' P + Q |
|
|
22 ## P(tf) = Qf |
|
|
23 ## for the LTI system sys. Solution of standard LTI |
|
|
24 ## state feedback optimization |
|
|
25 ## min \int_{t_0}^{t_f} x' Q x + u' R u dt + x(t_f)' Qf x(t_f) |
|
|
26 ## optimal input is |
|
|
27 ## u = - inv(R) B' P(t) x |
|
|
28 ## inputs: |
|
|
29 ## sys: continuous time system data structure |
|
|
30 ## Q: state integral penalty |
|
|
31 ## R: input integral penalty |
|
|
32 ## Qf: state terminal penalty |
|
|
33 ## t0,tf: limits on the integral |
|
|
34 ## Ptol: tolerance (used to select time samples; see below); default = 0.1 |
|
|
35 ## max number of refinement iterations (default=10) |
|
|
36 ## outputs: |
|
|
37 ## tvals: time values at which P(t) is computed |
|
|
38 ## Plist: list values of P(t); nth(Plist,ii) is P(tvals(ii)). |
|
|
39 ## |
|
|
40 ## tvals is selected so that || nth(Plist,ii) - nth(Plist,ii-1) || < Ptol |
|
|
41 ## for ii=2:length(tvals) |
|
3236
|
42 |
|
3385
|
43 function [tvals, Plist] = dre (sys, Q, R, Qf, t0, tf, Ptol, maxits) |
|
3381
|
44 |
|
|
45 if(nargin < 6 | nargin > 8 | nargout != 2) |
|
|
46 usage("[tvals,Plist] = dre(sys,Q,R,Qf,t0,tf{,Ptol})"); |
|
|
47 elseif(!is_struct(sys)) |
|
|
48 error("sys must be a system data structure") |
|
|
49 elseif(is_digital(sys)) |
|
|
50 error("sys must be a continuous time system") |
|
|
51 elseif(!is_matrix(Q) | !is_matrix(R) | !is_matrix(Qf)) |
|
|
52 error("Q, R, and Qf must be matrices."); |
|
|
53 elseif(!is_scalar(t0) | !is_scalar(tf)) |
|
|
54 error("t0 and tf must be scalars") |
|
|
55 elseif(t0 >= tf) error("t0=%e >= tf=%e",t0,tf); |
|
|
56 elseif(nargin == 6) Ptol = 0.1; |
|
|
57 elseif(!is_scalar(Ptol)) error("Ptol must be a scalar"); |
|
|
58 elseif(Ptol <= 0) error("Ptol must be positive"); |
|
|
59 endif |
|
3236
|
60 |
|
3381
|
61 if(nargin < 8) maxits = 10; |
|
|
62 elseif(!is_scalar(maxits)) error("maxits must be a scalar"); |
|
|
63 elseif(maxits <= 0) error("maxits must be positive"); |
|
|
64 endif |
|
|
65 maxits = ceil(maxits); |
|
|
66 |
|
|
67 [aa,bb] = sys2ss(sys); |
|
|
68 nn = sysdimensions(sys,"cst"); |
|
|
69 mm = sysdimensions(sys,"in"); |
|
|
70 pp = sysdimensions(sys,"out"); |
|
3236
|
71 |
|
3381
|
72 if(size(Q) != [nn, nn]) |
|
|
73 error("Q(%dx%d); sys has %d states",rows(Q),columns(Q),nn); |
|
|
74 elseif(size(Qf) != [nn, nn]) |
|
|
75 error("Qf(%dx%d); sys has %d states",rows(Qf),columns(Qf),nn); |
|
|
76 elseif(size(R) != [mm, mm]) |
|
|
77 error("R(%dx%d); sys has %d inputs",rows(R),columns(R),mm); |
|
|
78 endif |
|
3236
|
79 |
|
3381
|
80 ## construct Hamiltonian matrix |
|
|
81 H = [aa , -(bb/R)*bb' ; -Q, -aa']; |
|
3236
|
82 |
|
3381
|
83 ## select time step to avoid numerical overflow |
|
|
84 fast_eig = max(abs(eig(H))); |
|
|
85 tc = log(10)/fast_eig; |
|
|
86 nst = ceil((tf-t0)/tc); |
|
|
87 tvals = -linspace(-tf,-t0,nst); |
|
|
88 Plist = list(Qf); |
|
|
89 In = eye(nn); |
|
|
90 n1 = nn+1; |
|
|
91 n2 = nn+nn; |
|
|
92 done = 0; |
|
|
93 while(!done) |
|
|
94 done = 1; # assume this pass will do the job |
|
|
95 ## sort time values in reverse order |
|
|
96 tvals = -sort(-tvals); |
|
|
97 tvlen = length(tvals); |
|
|
98 maxerr = 0; |
|
|
99 ## compute new values of P(t); recompute old values just in case |
|
|
100 for ii=2:tvlen |
|
|
101 uv_i_minus_1 = [ In ; nth(Plist,ii-1) ]; |
|
|
102 delta_t = tvals(ii-1) - tvals(ii); |
|
|
103 uv = expm(-H*delta_t)*uv_i_minus_1; |
|
|
104 Qi = uv(n1:n2,1:nn)/uv(1:nn,1:nn); |
|
|
105 Plist(ii) = (Qi+Qi')/2; |
|
|
106 ## check error |
|
|
107 Perr = norm(nth(Plist,ii) - nth(Plist,ii-1))/norm(nth(Plist,ii)); |
|
|
108 maxerr = max(maxerr,Perr); |
|
|
109 if(Perr > Ptol) |
|
|
110 new_t = mean(tvals([ii,ii-1])); |
|
|
111 tvals = [tvals, new_t]; |
|
|
112 done = 0; |
|
|
113 endif |
|
|
114 endfor |
|
3236
|
115 |
|
3381
|
116 ## check number of iterations |
|
|
117 maxits = maxits - 1; |
|
|
118 done = done+(maxits==0); |
|
|
119 endwhile |
|
|
120 if(maxerr > Ptol) |
|
|
121 warning("dre: \n\texiting with%4d points, max rel chg. =%e, Ptol=%e\n", ... |
|
|
122 tvlen,maxerr,Ptol); |
|
|
123 tvals = tvals(1:length(Plist)); |
|
|
124 endif |
|
3236
|
125 endfunction |
