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 continuous-time system.
21 Uses SLICOT SB10ID by courtesy of NICONET e.V.
22 <http://www.slicot.org>
23 
24 Author: Lukas Reichlin <lukas.reichlin@gmail.com>
25 Created: July 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 (sb10id, SB10ID)
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, F77_INT& NK,
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                   F77_INT* IWORK,
48                   double* DWORK, F77_INT& LDWORK,
49                   F77_LOGICAL* BWORK,
50                   F77_INT& INFO);
51 }
52 
53 // PKG_ADD: autoload ("__sl_sb10id__", "__control_slicot_functions__.oct");
54 DEFUN_DLD (__sl_sb10id__, args, nargout,
55    "-*- texinfo -*-\n\
56 Slicot SB10ID Release 5.0\n\
57 No argument checking.\n\
58 For internal use only.")
59 {
60     octave_idx_type nargin = args.length ();
61     octave_value_list retval;
62 
63     if (nargin != 5)
64     {
65         print_usage ();
66     }
67     else
68     {
69         // arguments in
70         Matrix a = args(0).matrix_value ();
71         Matrix b = args(1).matrix_value ();
72         Matrix c = args(2).matrix_value ();
73         Matrix d = args(3).matrix_value ();
74 
75         double factor = args(4).double_value ();
76 
77         F77_INT n = TO_F77_INT (a.rows ());      // n: number of states
78         F77_INT m = TO_F77_INT (b.columns ());   // m: number of inputs
79         F77_INT np = TO_F77_INT (c.rows ());     // np: number of outputs
80 
81         F77_INT lda = max (1, n);
82         F77_INT ldb = max (1, n);
83         F77_INT ldc = max (1, np);
84         F77_INT ldd = max (1, np);
85 
86         F77_INT ldak = max (1, n);
87         F77_INT ldbk = max (1, n);
88         F77_INT ldck = max (1, m);
89         F77_INT lddk = max (1, m);
90 
91         // arguments out
92         F77_INT nk;
93         Matrix ak (ldak, n);
94         Matrix bk (ldbk, np);
95         Matrix ck (ldck, n);
96         Matrix dk (lddk, np);
97         ColumnVector rcond (2);
98 
99         // workspace
100         F77_INT liwork = max (2*n, n*n, m, np);
101         F77_INT ldwork = 10*n*n + m*m + np*np + 2*m*n + 2*n*np + 4*n +
102                      5 + max (1, 4*n*n + 8*n);
103 
104         OCTAVE_LOCAL_BUFFER (F77_INT, iwork, liwork);
105         OCTAVE_LOCAL_BUFFER (double, dwork, ldwork);
106         OCTAVE_LOCAL_BUFFER (F77_LOGICAL, bwork, 2*n);
107 
108         // error indicator
109         F77_INT info;
110 
111 
112         // SLICOT routine SB10ID
113         F77_XFCN (sb10id, SB10ID,
114                  (n, m, np,
115                   a.fortran_vec (), lda,
116                   b.fortran_vec (), ldb,
117                   c.fortran_vec (), ldc,
118                   d.fortran_vec (), ldd,
119                   factor, nk,
120                   ak.fortran_vec (), ldak,
121                   bk.fortran_vec (), ldbk,
122                   ck.fortran_vec (), ldck,
123                   dk.fortran_vec (), lddk,
124                   rcond.fortran_vec (),
125                   iwork,
126                   dwork, ldwork,
127                   bwork,
128                   info));
129 
130         if (f77_exception_encountered)
131             error ("ncfsyn: __sl_sb10id__: exception in SLICOT subroutine SB10ID");
132 
133         static const char* err_msg[] = {
134             "0: OK",
135             "1: the X-Riccati equation is not solved successfully",
136             "2: the Z-Riccati equation is not solved successfully",
137             "3: the iteration to compute eigenvalues or singular "
138                 "values failed to converge",
139             "4: the matrix Ip - D*Dk is singular",
140             "5: the matrix Im - Dk*D is singular",
141             "6: the closed-loop system is unstable"};
142 
143         error_msg ("ncfsyn", info, 6, err_msg);
144 
145 
146         // resizing
147         ak.resize (nk, nk);
148         bk.resize (nk, np);
149         ck.resize (m, nk);
150         dk.resize (m, np);
151 
152         // return values
153         retval(0) = ak;
154         retval(1) = bk;
155         retval(2) = ck;
156         retval(3) = dk;
157         retval(4) = rcond;
158     }
159 
160     return retval;
161 }
162