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