1 /* 2 3 Copyright (C) 2014-2015 Thomas Vasileiou 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 H-infinity optimal controller using modified Glover's and Doyle's 21 formulas (continuous-time). 22 Uses SLICOT SB10AD by courtesy of NICONET e.V. 23 <http://www.slicot.org> 24 25 Author: Thomas Vasileiou <thomas-v@wildmail.com> 26 Created: January 2014 27 Version: 0.2 28 29 */ 30 31 #include <octave/oct.h> 32 #include "common.h" 33 34 extern "C" 35 { 36 int F77_FUNC (sb10ad, SB10AD) 37 (F77_INT& JOB, 38 F77_INT& N, F77_INT& M, F77_INT& NP, 39 F77_INT& NCON, F77_INT& NMEAS, 40 double& GAMMA, 41 double* A, F77_INT& LDA, 42 double* B, F77_INT& LDB, 43 double* C, F77_INT& LDC, 44 double* D, F77_INT& LDD, 45 double* AK, F77_INT& LDAK, 46 double* BK, F77_INT& LDBK, 47 double* CK, F77_INT& LDCK, 48 double* DK, F77_INT& LDDK, 49 double* AC, F77_INT& LDAC, 50 double* BC, F77_INT& LDBC, 51 double* CC, F77_INT& LDCC, 52 double* DC, F77_INT& LDDC, 53 double* RCOND, 54 double& GTOL, double& ACTOL, 55 F77_INT* IWORK, F77_INT& LIWORK, 56 double* DWORK, F77_INT& LDWORK, 57 F77_LOGICAL* BWORK, F77_INT& LBWORK, 58 F77_INT& INFO); 59 } 60 61 // PKG_ADD: autoload ("__sl_sb10ad__", "__control_slicot_functions__.oct"); 62 DEFUN_DLD (__sl_sb10ad__, args, nargout, 63 "-*- texinfo -*-\n\ 64 Slicot SB10AD Release 5.0\n\ 65 No argument checking.\n\ 66 For internal use only.") 67 { 68 octave_idx_type nargin = args.length (); 69 octave_value_list retval; 70 71 if (nargin != 9) 72 { 73 print_usage (); 74 } 75 else 76 { 77 // arguments in 78 Matrix a = args(0).matrix_value (); 79 Matrix b = args(1).matrix_value (); 80 Matrix c = args(2).matrix_value (); 81 Matrix d = args(3).matrix_value (); 82 83 F77_INT ncon = args(4).int_value (); 84 F77_INT nmeas = args(5).int_value (); 85 double gamma = args(6).double_value (); 86 double gtol = args(7).double_value (); 87 double actol = args(8).double_value (); 88 89 F77_INT n = TO_F77_INT (a.rows ()); // n: number of states 90 F77_INT m = TO_F77_INT (b.columns ()); // m: number of inputs 91 F77_INT np = TO_F77_INT (c.rows ()); // np: number of outputs 92 93 F77_INT lda = max (1, TO_F77_INT (a.rows ())); 94 F77_INT ldb = max (1, TO_F77_INT (b.rows ())); 95 F77_INT ldc = max (1, TO_F77_INT (c.rows ())); 96 F77_INT ldd = max (1, TO_F77_INT (d.rows ())); 97 98 F77_INT ldak = max (1, n); 99 F77_INT ldbk = max (1, n); 100 F77_INT ldck = max (1, ncon); 101 F77_INT lddk = max (1, ncon); 102 103 F77_INT ldac = max (1, 2*n); 104 F77_INT ldbc = max (1, 2*n); 105 F77_INT ldcc = max (1, np-nmeas); 106 F77_INT lddc = max (1, np-nmeas); 107 108 F77_INT job = 1; 109 110 // arguments out 111 Matrix ak (ldak, n); 112 Matrix bk (ldbk, nmeas); 113 Matrix ck (ldck, n); 114 Matrix dk (lddk, nmeas); 115 Matrix ac (ldac, 2*n); 116 Matrix bc (ldbc, m-ncon); 117 Matrix cc (ldcc, 2*n); 118 Matrix dc (lddc, m-ncon); 119 ColumnVector rcond (4); 120 121 // workspace 122 123 F77_INT m2 = ncon; 124 F77_INT m1 = m - m2; 125 F77_INT np2 = nmeas; 126 F77_INT np1 = np - np2; 127 F77_INT nd1 = np1 - m2; 128 F77_INT nd2 = m1 - np2; 129 130 F77_INT liwork = max (2*max (n, m-ncon, np-nmeas, ncon, nmeas), n*n); 131 F77_INT lw1 = n*m + np*n + np*m + m2*m2 + np2*np2; 132 F77_INT lw2 = max ((n + np1 + 1)*(n + m2) + 133 max (3*(n + m2) + n + np1, 5*(n + m2)), 134 (n + np2)*(n + m1 + 1) + 135 max (3*(n + np2) + n + m1, 5*(n + np2)), 136 m2 + np1*np1 + 137 max (np1*max (n, m1), 3*m2 + np1, 5*m2), 138 np2 + m1*m1 + 139 max (max (n, np1)*m1, 3*np2 + m1, 5*np2)); 140 F77_INT lw3 = max (nd1*m1 + max (4*min (nd1, m1) + max (nd1,m1), 141 6*min (nd1, m1)), np1*nd2 + 142 max (4*min (np1, nd2) + max (np1, nd2), 143 6*min (np1, nd2))); 144 F77_INT lw4 = 2*m*m + np*np + 2*m*n + m*np + 2*n*np; 145 F77_INT lw5 = 2*n*n + m*n + n*np; 146 F77_INT lw6 = max (m*m + max (2*m1, 3*n*n + 147 max (n*m, 10*n*n + 12*n + 5)), 148 np*np + max (2*np1, 3*n*n + 149 max (n*np, 10*n*n + 12*n + 5))); 150 F77_INT lw7 = m2*np2 + np2*np2 + m2*m2 + 151 max (nd1*nd1 + max (2*nd1, (nd1 + nd2)*np2), 152 nd2*nd2 + max (2*nd2, nd2*m2), 3*n, 153 n*(2*np2 + m2) + 154 max (2*n*m2, m2*np2 + 155 max (m2*m2 + 3*m2, np2*(2*np2 + m2 + max (np2, n))))); 156 F77_INT ldwork = lw1 + max (1, lw2, lw3, lw4, lw5 + max (lw6,lw7)); 157 F77_INT lbwork = 2*n; 158 159 OCTAVE_LOCAL_BUFFER (F77_INT, iwork, liwork); 160 OCTAVE_LOCAL_BUFFER (double, dwork, ldwork); 161 OCTAVE_LOCAL_BUFFER (F77_LOGICAL, bwork, lbwork); 162 163 // error indicator 164 F77_INT info; 165 166 167 // SLICOT routine SB10AD 168 F77_XFCN (sb10ad, SB10AD, 169 (job, 170 n, m, np, 171 ncon, nmeas, 172 gamma, 173 a.fortran_vec (), lda, 174 b.fortran_vec (), ldb, 175 c.fortran_vec (), ldc, 176 d.fortran_vec (), ldd, 177 ak.fortran_vec (), ldak, 178 bk.fortran_vec (), ldbk, 179 ck.fortran_vec (), ldck, 180 dk.fortran_vec (), lddk, 181 ac.fortran_vec (), ldac, 182 bc.fortran_vec (), ldbc, 183 cc.fortran_vec (), ldcc, 184 dc.fortran_vec (), lddc, 185 rcond.fortran_vec (), 186 gtol, actol, 187 iwork, liwork, 188 dwork, ldwork, 189 bwork, lbwork, 190 info)); 191 192 if (f77_exception_encountered) 193 error ("hinfsyn: __sl_sb10ad__: exception in SLICOT subroutine SB10AD"); 194 195 static const char* err_msg[] = { 196 "0: successful exit", 197 "1: the matrix [A-j*omega*I, B2; C1, D12] had " 198 "not full column rank in respect to the tolerance EPS", 199 "2: the matrix [A-j*omega*I, B1; C2, D21] " 200 "had not full row rank in respect to the tolerance EPS", 201 "3: the matrix D12 had not full column rank in " 202 "respect to the tolerance SQRT(EPS)", 203 "4: the matrix D21 had not full row rank in respect " 204 "to the tolerance SQRT(EPS)", 205 "5: the singular value decomposition (SVD) algorithm " 206 "did not converge (when computing the SVD of one of the matrices " 207 "[A, B2; C1, D12], [A, B1; C2, D21], D12 or D21)", 208 "6: the controller is not admissible (too small value " 209 "of gamma)", 210 "7: the X-Riccati equation was not solved " 211 "successfully (the controller is not admissible or " 212 "there are numerical difficulties)", 213 "8: the Y-Riccati equation was not solved " 214 "successfully (the controller is not admissible or " 215 "there are numerical difficulties)", 216 "9: the determinant of Im2 + Tu*D11HAT*Ty*D22 is " 217 "zero [3]", 218 "10: there was numerical problems when estimating" 219 "the singular values of D1111, D1112, D1111', D1121'", 220 "11: the matrices Inp2 - D22*DK or Im2 - DK*D22" 221 "are singular to working precision", 222 "12: a stabilizing controller cannot be found"}; 223 224 error_msg ("hinfsyn", info, 12, err_msg); 225 226 227 // return values 228 retval(0) = ak; 229 retval(1) = bk; 230 retval(2) = ck; 231 retval(3) = dk; 232 retval(4) = ac; 233 retval(5) = bc; 234 retval(6) = cc; 235 retval(7) = dc; 236 retval(8) = gamma; 237 retval(9) = rcond; 238 } 239 240 return retval; 241 } 242