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 Compute initial state vector x0 21 Uses IB01CD by courtesy of NICONET e.V. 22 <http://www.slicot.org> 23 24 Author: Lukas Reichlin <lukas.reichlin@gmail.com> 25 Created: May 2012 26 Version: 0.2 27 28 */ 29 30 #include <octave/oct.h> 31 #include <octave/Cell.h> 32 #include "common.h" 33 34 extern "C" 35 { 36 int F77_FUNC (ib01cd, IB01CD) 37 (char& JOBX0, char& COMUSE, char& JOB, 38 F77_INT& N, F77_INT& M, F77_INT& L, 39 F77_INT& NSMP, 40 double* A, F77_INT& LDA, 41 double* B, F77_INT& LDB, 42 double* C, F77_INT& LDC, 43 double* D, F77_INT& LDD, 44 double* U, F77_INT& LDU, 45 double* Y, F77_INT& LDY, 46 double* X0, 47 double* V, F77_INT& LDV, 48 double& TOL, 49 F77_INT* IWORK, 50 double* DWORK, F77_INT& LDWORK, 51 F77_INT& IWARN, F77_INT& INFO); 52 } 53 54 // PKG_ADD: autoload ("__sl_ib01cd__", "__control_slicot_functions__.oct"); 55 DEFUN_DLD (__sl_ib01cd__, args, nargout, 56 "-*- texinfo -*-\n\ 57 Slicot IB01CD Release 5.0\n\ 58 No argument checking.\n\ 59 For internal use only.") 60 { 61 octave_idx_type nargin = args.length (); 62 octave_value_list retval; 63 64 if (nargin != 7) 65 { 66 print_usage (); 67 } 68 else 69 { 70 // arguments in 71 char jobx0 = 'X'; 72 char comuse = 'U'; 73 char jobbd = 'D'; 74 75 const Cell y_cell = args(0).cell_value (); 76 const Cell u_cell = args(1).cell_value (); 77 78 Matrix a = args(2).matrix_value (); 79 Matrix b = args(3).matrix_value (); 80 Matrix c = args(4).matrix_value (); 81 Matrix d = args(5).matrix_value (); 82 83 double rcond = args(6).double_value (); 84 double tol_c = rcond; 85 86 F77_INT n = TO_F77_INT (a.rows ()); // n: number of states 87 F77_INT m = TO_F77_INT (b.columns ()); // m: number of inputs 88 F77_INT l = TO_F77_INT (c.rows ()); // l: number of outputs 89 90 F77_INT lda = max (1, n); 91 F77_INT ldb = max (1, n); 92 F77_INT ldc = max (1, l); 93 F77_INT ldd = max (1, l); 94 95 // m and l are equal for all experiments, checked by iddata class 96 F77_INT n_exp = TO_F77_INT (y_cell.numel ()); // number of experiments 97 98 99 // arguments out 100 Cell x0_cell (n_exp, 1); // cell of initial state vectors x0 101 102 // repeat for every experiment in the dataset 103 // compute individual initial state vector x0 for every experiment 104 for (F77_INT i = 0; i < n_exp; i++) 105 { 106 Matrix y = y_cell.elem(i).matrix_value (); 107 Matrix u = u_cell.elem(i).matrix_value (); 108 109 F77_INT nsmp = TO_F77_INT (y.rows ()); // nsmp: number of samples 110 F77_INT ldv = max (1, n); 111 112 F77_INT ldu; 113 114 if (m == 0) 115 ldu = 1; 116 else // m > 0 117 ldu = nsmp; 118 119 F77_INT ldy = nsmp; 120 121 // arguments out 122 ColumnVector x0 (n); 123 Matrix v (ldv, n); 124 125 // workspace 126 F77_INT liwork_c = n; // if JOBX0 = 'X' and COMUSE <> 'C' 127 F77_INT ldwork_c; 128 F77_INT t = nsmp; 129 130 F77_INT ldw1_c = 2; 131 F77_INT ldw2_c = t*l*(n + 1) + 2*n + max (2*n*n, 4*n); 132 F77_INT ldw3_c = n*(n + 1) + 2*n + max (n*l*(n + 1) + 2*n*n + l*n, 4*n); 133 134 ldwork_c = ldw1_c + n*( n + m + l ) + max (5*n, ldw1_c, min (ldw2_c, ldw3_c)); 135 136 OCTAVE_LOCAL_BUFFER (F77_INT, iwork_c, liwork_c); 137 OCTAVE_LOCAL_BUFFER (double, dwork_c, ldwork_c); 138 139 // error indicators 140 F77_INT iwarn_c = 0; 141 F77_INT info_c = 0; 142 143 // SLICOT routine IB01CD 144 F77_XFCN (ib01cd, IB01CD, 145 (jobx0, comuse, jobbd, 146 n, m, l, 147 nsmp, 148 a.fortran_vec (), lda, 149 b.fortran_vec (), ldb, 150 c.fortran_vec (), ldc, 151 d.fortran_vec (), ldd, 152 u.fortran_vec (), ldu, 153 y.fortran_vec (), ldy, 154 x0.fortran_vec (), 155 v.fortran_vec (), ldv, 156 tol_c, 157 iwork_c, 158 dwork_c, ldwork_c, 159 iwarn_c, info_c)); 160 161 162 if (f77_exception_encountered) 163 error ("__sl_ib01cd__: exception in SLICOT subroutine IB01CD"); 164 165 static const char* err_msg_c[] = { 166 "0: OK", 167 "1: the QR algorithm failed to compute all the " 168 "eigenvalues of the matrix A (see LAPACK Library " 169 "routine DGEES); the locations DWORK(i), for " 170 "i = g+1:g+N*N, contain the partially converged " 171 "Schur form", 172 "2: the singular value decomposition (SVD) algorithm did " 173 "not converge"}; 174 175 static const char* warn_msg_c[] = { 176 "0: OK", 177 "1: warning message not specified", 178 "2: warning message not specified", 179 "3: warning message not specified", 180 "4: the least squares problem to be solved has a " 181 "rank-deficient coefficient matrix", 182 "5: warning message not specified", 183 "6: the matrix A is unstable; the estimated x(0) " 184 "and/or B and D could be inaccurate"}; 185 186 187 error_msg ("__sl_ib01cd__", info_c, 2, err_msg_c); 188 warning_msg ("__sl_ib01cd__", iwarn_c, 6, warn_msg_c); 189 190 x0_cell.elem(i) = x0; // add x0 from the current experiment to cell of initial state vectors 191 } 192 193 194 // return values 195 retval(0) = x0_cell; 196 } 197 198 return retval; 199 } 200