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 Positive feedback controller for a discrete-time system (D != 0). 21 Uses SLICOT SB10ZD by courtesy of NICONET e.V. 22 <http://www.slicot.org> 23 24 Author: Lukas Reichlin <lukas.reichlin@gmail.com> 25 Created: August 2011 26 Version: 0.4 27 28 */ 29 30 #include <octave/oct.h> 31 #include "common.h" 32 33 extern "C" 34 { 35 int F77_FUNC (sb10zd, SB10ZD) 36 (F77_INT& N, F77_INT& M, F77_INT& NP, 37 double* A, F77_INT& LDA, 38 double* B, F77_INT& LDB, 39 double* C, F77_INT& LDC, 40 double* D, F77_INT& LDD, 41 double& FACTOR, 42 double* AK, F77_INT& LDAK, 43 double* BK, F77_INT& LDBK, 44 double* CK, F77_INT& LDCK, 45 double* DK, F77_INT& LDDK, 46 double* RCOND, 47 double& TOL, 48 F77_INT* IWORK, 49 double* DWORK, F77_INT& LDWORK, 50 F77_LOGICAL* BWORK, 51 F77_INT& INFO); 52 } 53 54 // PKG_ADD: autoload ("__sl_sb10zd__", "__control_slicot_functions__.oct"); 55 DEFUN_DLD (__sl_sb10zd__, args, nargout, 56 "-*- texinfo -*-\n\ 57 Slicot SB10ZD 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 != 6) 65 { 66 print_usage (); 67 } 68 else 69 { 70 // arguments in 71 Matrix a = args(0).matrix_value (); 72 Matrix b = args(1).matrix_value (); 73 Matrix c = args(2).matrix_value (); 74 Matrix d = args(3).matrix_value (); 75 76 double factor = args(4).double_value (); 77 double tol = args(5).double_value (); 78 79 F77_INT n = TO_F77_INT (a.rows ()); // n: number of states 80 F77_INT m = TO_F77_INT (b.columns ()); // m: number of inputs 81 F77_INT np = TO_F77_INT (c.rows ()); // np: number of outputs 82 83 F77_INT lda = max (1, n); 84 F77_INT ldb = max (1, n); 85 F77_INT ldc = max (1, np); 86 F77_INT ldd = max (1, np); 87 88 F77_INT ldak = max (1, n); 89 F77_INT ldbk = max (1, n); 90 F77_INT ldck = max (1, m); 91 F77_INT lddk = max (1, m); 92 93 // arguments out 94 Matrix ak (ldak, n); 95 Matrix bk (ldbk, np); 96 Matrix ck (ldck, n); 97 Matrix dk (lddk, np); 98 ColumnVector rcond (6); 99 100 // workspace 101 F77_INT liwork = 2 * max (n, m+np); 102 F77_INT ldwork = 16*n*n + 5*m*m + 7*np*np + 6*m*n + 7*m*np + 103 7*n*np + 6*n + 2*(m + np) + 104 max (14*n+23, 16*n, 2*m-1, 2*np-1); 105 106 OCTAVE_LOCAL_BUFFER (F77_INT, iwork, liwork); 107 OCTAVE_LOCAL_BUFFER (double, dwork, ldwork); 108 OCTAVE_LOCAL_BUFFER (F77_LOGICAL, bwork, 2*n); 109 110 // error indicator 111 F77_INT info; 112 113 114 // SLICOT routine SB10ZD 115 F77_XFCN (sb10zd, SB10ZD, 116 (n, m, np, 117 a.fortran_vec (), lda, 118 b.fortran_vec (), ldb, 119 c.fortran_vec (), ldc, 120 d.fortran_vec (), ldd, 121 factor, 122 ak.fortran_vec (), ldak, 123 bk.fortran_vec (), ldbk, 124 ck.fortran_vec (), ldck, 125 dk.fortran_vec (), lddk, 126 rcond.fortran_vec (), 127 tol, 128 iwork, 129 dwork, ldwork, 130 bwork, 131 info)); 132 133 if (f77_exception_encountered) 134 error ("ncfsyn: __sl_sb10zd__: exception in SLICOT subroutine SB10ZD"); 135 136 static const char* err_msg[] = { 137 "0: OK", 138 "1: the P-Riccati equation is not solved successfully", 139 "2: the Q-Riccati equation is not solved successfully", 140 "3: the iteration to compute eigenvalues or singular " 141 "values failed to converge", 142 "4: the matrix (gamma^2-1)*In - P*Q is singular", 143 "5: the matrix Rx + Bx'*X*Bx is singular", 144 "6: the matrix Ip + D*Dk is singular", 145 "7: the matrix Im + Dk*D is singular", 146 "8: the matrix Ip - D*Dk is singular", 147 "9: the matrix Im - Dk*D is singular", 148 "10: the closed-loop system is unstable"}; 149 150 error_msg ("ncfsyn", info, 10, err_msg); 151 152 153 // resizing 154 ak.resize (n, n); 155 bk.resize (n, np); 156 ck.resize (m, n); 157 dk.resize (m, np); 158 159 // return values 160 retval(0) = ak; 161 retval(1) = bk; 162 retval(2) = ck; 163 retval(3) = dk; 164 retval(4) = rcond; 165 } 166 167 return retval; 168 } 169