1 /* 2 3 Copyright (C) 2009-2016 Lukas F. Reichlin 4 5 This file is part of LTI Syncope. 6 7 LTI Syncope is free software: you can redistribute it and/or modify 8 it under the terms of the GNU General Public License as published by 9 the Free Software Foundation, either version 3 of the License, or 10 (at your option) any later version. 11 12 LTI Syncope is distributed in the hope that it will be useful, 13 but WITHOUT ANY WARRANTY; without even the implied warranty of 14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 GNU General Public License for more details. 16 17 You should have received a copy of the GNU General Public License 18 along with LTI Syncope. If not, see <http://www.gnu.org/licenses/>. 19 20 Solve algebraic Riccati equation. 21 Uses SLICOT SB02RD and SB02MT by courtesy of NICONET e.V. 22 <http://www.slicot.org> 23 24 Author: Lukas Reichlin <lukas.reichlin@gmail.com> 25 Created: December 2012 26 Version: 0.2 27 28 */ 29 30 #include <octave/oct.h> 31 #include "common.h" 32 #include <complex> 33 34 extern "C" 35 { 36 int F77_FUNC (sb02mt, SB02MT) 37 (char& JOBG, char& JOBL, 38 char& FACT, char& UPLO, 39 F77_INT& N, F77_INT& M, 40 double* A, F77_INT& LDA, 41 double* B, F77_INT& LDB, 42 double* Q, F77_INT& LDQ, 43 double* R, F77_INT& LDR, 44 double* L, F77_INT& LDL, 45 F77_INT* IPIV, F77_INT& OUFACT, 46 double* G, F77_INT& LDG, 47 F77_INT* IWORK, 48 double* DWORK, F77_INT& LDWORK, 49 F77_INT& INFO); 50 51 52 int F77_FUNC (sb02rd, SB02RD) 53 (char& JOB, char& DICO, 54 char& HINV, char& TRANA, 55 char& UPLO, char& SCAL, 56 char& SORT, char& FACT, 57 char& LYAPUN, 58 F77_INT& N, 59 double* A, F77_INT& LDA, 60 double* T, F77_INT& LDT, 61 double* V, F77_INT& LDV, 62 double* G, F77_INT& LDG, 63 double* Q, F77_INT& LDQ, 64 double* X, F77_INT& LDX, 65 double& SEP, 66 double& RCOND, double& FERR, 67 double* WR, double* WI, 68 double* S, F77_INT& LDS, 69 F77_INT* IWORK, 70 double* DWORK, F77_INT& LDWORK, 71 F77_LOGICAL* BWORK, 72 F77_INT& INFO); 73 } 74 75 // PKG_ADD: autoload ("__sl_are__", "__control_slicot_functions__.oct"); 76 DEFUN_DLD (__sl_are__, args, nargout, 77 "-*- texinfo -*-\n\ 78 Slicot SB02RD Release 5.0\n\ 79 No argument checking.\n\ 80 For internal use only.") 81 { 82 octave_idx_type nargin = args.length (); 83 octave_value_list retval; 84 85 if (nargin != 7) 86 { 87 print_usage (); 88 } 89 else 90 { 91 // SB02MT 92 93 // arguments in 94 char dico; 95 char jobg = 'G'; 96 char jobl; 97 char fact = 'N'; 98 char uplo = 'U'; 99 100 Matrix a = args(0).matrix_value (); 101 Matrix b = args(1).matrix_value (); 102 Matrix q = args(2).matrix_value (); 103 Matrix r = args(3).matrix_value (); 104 Matrix l = args(4).matrix_value (); 105 F77_INT discrete = args(5).int_value (); 106 F77_INT ijobl = args(6).int_value (); 107 108 if (discrete == 0) 109 dico = 'C'; 110 else 111 dico = 'D'; 112 113 if (ijobl == 0) 114 jobl = 'Z'; 115 else 116 jobl = 'N'; 117 118 F77_INT n = TO_F77_INT (a.rows ()); // n: number of states 119 F77_INT m = TO_F77_INT (b.columns ()); // m: number of inputs 120 121 F77_INT lda = max (1, n); 122 F77_INT ldb = max (1, n); 123 F77_INT ldq = max (1, n); 124 F77_INT ldr = max (1, m); 125 F77_INT ldl = max (1, n); 126 127 // arguments out 128 F77_INT ldg = max (1, n); 129 Matrix g (ldg, n); 130 131 // unused output arguments 132 OCTAVE_LOCAL_BUFFER (F77_INT, ipiv, m); 133 F77_INT oufact; 134 135 // workspace 136 OCTAVE_LOCAL_BUFFER (F77_INT, iwork_a, m); 137 138 F77_INT ldwork_a = max (2, 3*m, n*m); 139 OCTAVE_LOCAL_BUFFER (double, dwork_a, ldwork_a); 140 141 142 // error indicator 143 F77_INT info; 144 145 146 // SLICOT routine SB02MT 147 F77_XFCN (sb02mt, SB02MT, 148 (jobg, jobl, 149 fact, uplo, 150 n, m, 151 a.fortran_vec (), lda, 152 b.fortran_vec (), ldb, 153 q.fortran_vec (), ldq, 154 r.fortran_vec (), ldr, 155 l.fortran_vec (), ldl, 156 ipiv, oufact, 157 g.fortran_vec (), ldg, 158 iwork_a, 159 dwork_a, ldwork_a, 160 info)); 161 162 163 if (f77_exception_encountered) 164 error ("are: __sl_are__: exception in SLICOT subroutine SB02MT"); 165 166 if (info != 0) 167 { 168 if (info < 0) 169 error ("are: sb02mt: the %d-th argument had an illegal value", info); 170 else if (info == m+1) 171 error ("are: sb02mt: the matrix R is numerically singular"); 172 else 173 error ("are: sb02mt: the %d-th element (1 <= %d <= %d) of the d factor is " 174 "exactly zero; the UdU' (or LdL') factorization has " 175 "been completed, but the block diagonal matrix d is " 176 "exactly singular", info, info, m); 177 } 178 179 180 // SB02RD 181 182 // arguments in 183 char job = 'A'; 184 char hinv = 'D'; 185 char trana = 'N'; 186 char scal = 'G'; 187 char sort = 'S'; 188 char lyapun = 'O'; 189 190 F77_INT ldt = max (1, n); 191 F77_INT ldv = max (1, n); 192 F77_INT ldx = max (1, n); 193 F77_INT lds = max (1, 2*n); 194 195 // arguments out 196 Matrix x (ldx, n); 197 198 double sep; 199 double rcond; 200 double ferr; 201 202 ColumnVector wr (2*n); 203 ColumnVector wi (2*n); 204 205 // unused output arguments 206 Matrix t (ldt, n); 207 Matrix v (ldv, n); 208 Matrix s (lds, 2*n); 209 210 // workspace 211 F77_INT liwork_b = max (2*n, n*n); 212 OCTAVE_LOCAL_BUFFER (F77_INT, iwork_b, liwork_b); 213 214 F77_INT ldwork_b = 5 + max (1, 4*n*n + 8*n); 215 OCTAVE_LOCAL_BUFFER (double, dwork_b, ldwork_b); 216 217 OCTAVE_LOCAL_BUFFER (F77_LOGICAL, bwork_b, 2*n); 218 219 220 // SLICOT routine SB02RD 221 F77_XFCN (sb02rd, SB02RD, 222 (job, dico, 223 hinv, trana, 224 uplo, scal, 225 sort, fact, 226 lyapun, 227 n, 228 a.fortran_vec (), lda, 229 t.fortran_vec (), ldt, 230 v.fortran_vec (), ldv, 231 g.fortran_vec (), ldg, 232 q.fortran_vec (), ldq, 233 x.fortran_vec (), ldx, 234 sep, 235 rcond, ferr, 236 wr.fortran_vec (), wi.fortran_vec (), 237 s.fortran_vec (), lds, 238 iwork_b, 239 dwork_b, ldwork_b, 240 bwork_b, 241 info)); 242 243 244 static const char* err_msg[] = { 245 "0: OK", 246 "1: matrix A is (numerically) singular in discrete-" 247 "time case", 248 "2: the Hamiltonian or symplectic matrix H cannot be " 249 "reduced to real Schur form", 250 "3: the real Schur form of the Hamiltonian or " 251 "symplectic matrix H cannot be appropriately ordered", 252 "4: the Hamiltonian or symplectic matrix H has less " 253 "than N stable eigenvalues", 254 "5: if the N-th order system of linear algebraic " 255 "equations, from which the solution matrix X would " 256 "be obtained, is singular to working precision", 257 "6: the QR algorithm failed to complete the reduction " 258 "of the matrix Ac to Schur canonical form, T", 259 "7: if T and -T' have some almost equal eigenvalues, if " 260 "DICO = 'C', or T has almost reciprocal eigenvalues, " 261 "if DICO = 'D'; perturbed values were used to solve " 262 "Lyapunov equations, but the matrix T, if given (for " 263 "FACT = 'F'), is unchanged. (This is a warning " 264 "indicator.)"}; 265 266 error_msg ("are", info, 7, err_msg); 267 268 // resize 269 x.resize (n, n); 270 wr.resize (n); 271 wi.resize (n); 272 273 // assemble complex vector - adapted from DEFUN complex in data.cc 274 ComplexColumnVector pole (n, Complex ()); 275 276 for (F77_INT i = 0; i < n; i++) 277 pole.xelem (i) = Complex (wr(i), wi(i)); 278 279 // return value 280 retval(0) = x; 281 retval(1) = pole; 282 retval(2) = octave_value (ferr); 283 retval(3) = octave_value (rcond); 284 retval(4) = octave_value (sep); 285 } 286 287 return retval; 288 } 289