d0e2088b94879386498b56e5ce7cca323a5c6b23
[scilab.git] / scilab / modules / cacsd / sci_gateway / c / sci_sident.c
1 /*
2 * Scilab (http://www.scilab.org/) - This file is part of Scilab
3 * Copyright (C) 2013 - Scilab Enterprises - Antoine ELIAS
4 *
5  * Copyright (C) 2012 - 2016 - Scilab Enterprises
6  *
7  * This file is hereby licensed under the terms of the GNU GPL v2.0,
8  * pursuant to article 5.3.4 of the CeCILL v.2.1.
9  * This file was originally licensed under the terms of the CeCILL v2.1,
10  * and continues to be available under such terms.
11  * For more information, see the COPYING file which you should have received
12  * along with this program.
13 *
14 */
15
16 //SIDENT.F - Gateway function for computation of a discrete-time
17 //           state-space realization (A,B,C,D) and Kalman gain
18 //           using SLICOT routine IB01BD.
19 //
20 //RELEASE 4.0, WGS COPYRIGHT 2000.
21 //
22 //scilab call:
23 //  [(A,C)(,B(,D))(,K,Q,Ry,S,rcnd)] = sident(meth,job,s,n,l,R(,tol,t,A,
24 //                                           C,printw))
25 //
26 //Purpose:
27 //  To compute a state-space realization (A,B,C,D) and the Kalman
28 //  predictor gain K of a discrete-time system, given the system
29 //  order and the relevant part of the R factor of the concatenated
30 //  block-Hankel matrices, using subspace identification techniques
31 //  (MOESP and N4SID).
32 //
33 //Input parameters:
34 //  meth  - integer option to determine the method to use:
35 //          = 1 : MOESP method with past inputs and outputs;
36 //          = 2 : N4SID method;
37 //          = 3 : combined method: A and C via MOESP, B and D via N4SID.
38 //  job   - integer option to determine the calculation to be performed:
39 //          = 1 : compute all system matrices, A, B, C, D;
40 //          = 2 : compute the matrices A and C only;
41 //          = 3 : compute the matrix B only;
42 //          = 4 : compute the matrices B and D only.
43 //  s     - the number of block rows in the processed input and output
44 //          block Hankel matrices.  s > 0.
45 //  n     - the order of the system.
46 //  l     - the number of system outputs.
47 //  R     - the 2*(m+l)*s-by-2*(m+l)*s part of  R  contains the
48 //          processed upper triangular factor  R  from the
49 //          QR factorization of the concatenated block-Hankel matrices,
50 //          and further details needed for computing system matrices.
51 //          (Above, m denotes the number of system inputs, determined
52 //          by s, l, and the size of R.)
53 //  tol   - (optional) tolerance used for estimating the rank of
54 //          matrices. if  tol > 0,  the given value of  tol  is
55 //          used as a lower bound for the reciprocal condition number;
56 //          an m-by-n matrix whose estimated condition number is less
57 //          than  1/tol  is considered to be of full rank.
58 //          Default:    m*n*epsilon_machine where epsilon_machine is
59 //          the relative machine precision.
60 //  t     - (optional) the total number of samples used for calculating
61 //          the covariance matrices.  Either  t = 0, or  t >= 2*(m+l)*s.
62 //          This parameter is not needed if the covariance matrices
63 //          and/or the Kalman predictor gain matrix are not desired.
64 //          if t = 0, K, Q, Ry, and S are not computed.
65 //          Default:    t = 0.
66 //  A     - (optional) the n-by-n system state matrix A.
67 //          This parameter is needed if meth >= 2 and job >= 3.
68 //  C     - (optional) the l-by-n system output matrix C.
69 //          This parameter is needed if meth >= 2 and job >= 3.
70 //  printw- (optional) switch for printing the warning messages.
71 //          = 1:  print warning messages;
72 //          = 0:  do not print warning messages.
73 //          Default:    printw = 0.
74 //
75 //Output parameters:
76 //  A     - if job <= 2, the n-by-n system state matrix A.
77 //  C     - if job <= 2, the l-by-n system output matrix C.
78 //  B     - if job <> 2, the n-by-m system input matrix B.
79 //  D     - if job = 1 or 4, the l-by-m system matrix D.
80 //  K     - (optional) the n-by-l Kalman predictor gain matrix K.
81 //  Q     - (optional) the n-by-n positive semidefinite state covariance
82 //          matrix used as state weighting matrix when computing the
83 //          Kalman gain.
84 //  Ry    - (optional) the l-by-l positive (semi)definite output
85 //          covariance matrix used as output weighting matrix when
86 //          computing the Kalman gain.
87 //  S     - (optional) the n-by-l state-output cross-covariance matrix
88 //          used as cross-weighting matrix when computing the Kalman
89 //          gain.
90 //  rcnd  - (optional) vector of length lr, containing estimates of the
91 //          reciprocal condition numbers of the matrices involved in
92 //          rank decisions, least squares or Riccati equation solutions,
93 //          where lr = 4,  if Kalman gain matrix K is not required, and
94 //                lr = 12, if Kalman gain matrix K is required.
95 //
96 //Contributor:
97 //  V. Sima, Research Institute for Informatics, Bucharest, Oct. 1999.
98 //
99 //Revisions:
100 //  V. Sima, May 2000, July 2000.
101 /*--------------------------------------------------------------------------*/
102 #include <string.h>
103 #include "api_scilab.h"
104 #include "Scierror.h"
105 #include "sciprint.h"
106 #include "localization.h"
107 #include "BOOL.h"
108 #include "sci_malloc.h"
109 #include "gw_cacsd.h"
110 #include "elem_common.h"
111 /*--------------------------------------------------------------------------*/
112 extern int C2F(ib01bd)( char*, char*, char*, int*, int*, int*, int*, int*, double*, int*,
113                         double*, int*, double*, int*, double*, int*, double*, int*, double*, int*,
114                         double*, int*, double*, int*, double*, int*, double*, int*, double*,
115                         int*, int*, int*, int*);
116
117 /*--------------------------------------------------------------------------*/
118 int sci_sident(char *fname, void* pvApiCtx)
119 {
120     SciErr sciErr;
121     int iOne = 1;
122
123     int iTASK = 0;
124     char cMETH = 'C';
125     int iIJOB = 0;
126     char cJOB = 'D';
127     int iNOBR = 0;
128     int iN = 0;
129     int iL = 0;
130     int* piAddrR = NULL;
131     int iNR = 0;
132     int iNCOL = 0;
133     double* pdblR = NULL;
134     int iM = 0;
135     double dblTOL = 0;
136     int iNSMPL = 0;
137     char cJOBCK = 'N';
138     int iMA = 0;
139     int iNA = 0;
140     int iSizeA = 0;
141     double* pdblA = NULL;
142     double* pdblC = NULL;
143     int iPRINTW = 0;
144     BOOL bPRINTW = FALSE;
145
146     int iMNOBR = 0;
147     int iLNOBR = 0;
148     int iMNOBRN = 0;
149     int iLDUNN = 0;
150     int iNPL = 0;
151     int iN2 = 0;
152     int iNN = 0;
153     int iNL = 0;
154     int iLL = 0;
155
156     int iLDA = 0;
157     int iLDB = 0;
158     int iLDC = 0;
159     int iLDD = 0;
160     int iLDO = 0;
161     int iLDR = 0;
162
163     int iLDK = 0;
164     int iLDQ = 0;
165     int iLDS = 0;
166     int iLDRY = 0;
167     int iLBWORK = 0;
168     int iLIWORK = 0;
169     int iLDWORK = 0;
170     int iID = 0;
171     int iIAW = 0;
172     int iWARN = 0;
173     int iINFO = 0;
174     int iIP = 0;
175     int iNRC = 0;
176
177     //working array
178     double* pA = NULL;
179     double* pB = NULL;
180     double* pC = NULL;
181     double* pD = NULL;
182     double* pDWORK = NULL;
183
184     int* pIWORK = NULL;
185
186     double* pQ = NULL;
187     double* pR = NULL;
188     double* pRY = NULL;
189     double* pS = NULL;
190
191     int* pBWORK = NULL;
192
193     double* pK = NULL;
194
195     int iSize = 0;
196
197     int iRhs = nbInputArgument(pvApiCtx);
198     int iLhs = nbOutputArgument(pvApiCtx);
199
200     CheckInputArgumentAtLeast(pvApiCtx, 6);
201     CheckOutputArgumentAtLeast(pvApiCtx, 1);
202
203     // meth
204     CHECK_PARAM(pvApiCtx, 1);
205     iTASK = getIntegerValue(pvApiCtx, 1);
206
207     switch (iTASK)
208     {
209         case 1 :
210             cMETH = 'M';
211             break;
212         case 2 :
213             cMETH = 'N';
214             break;
215         case 3 :
216             cMETH = 'C';
217             break;
218         default :
219             Scierror(999, _("%s: Wrong value for input argument #%d: '%s', '%s' or '%s' expected.\n"), fname, 1, "1", "2", "3");
220             return 0;
221     }
222
223     // job
224     CHECK_PARAM(pvApiCtx, 2);
225     iIJOB = getIntegerValue(pvApiCtx, 2);
226
227     switch (iIJOB)
228     {
229         case 1 :
230             cJOB = 'A';
231             break;
232         case 2 :
233             cJOB = 'C';
234             break;
235         case 3 :
236             cJOB = 'B';
237             break;
238         case 4 :
239             cJOB = 'D';
240             break;
241         default:
242             Scierror(999, _("%s: Wrong value for input argument #%d: '%s', '%s', '%s' or '%s' expected.\n"), fname, 2, "1", "2", "3", "4");
243             return 0;
244     }
245
246
247     // s
248     CHECK_PARAM(pvApiCtx, 3);
249     iNOBR = getIntegerValue(pvApiCtx, 3);
250     if (iNOBR < 1)
251     {
252         Scierror(999, _("%s: Wrong value for input argument #%d: Scalar positive integer expected.\n"), fname, 3);
253         return 0;
254     }
255
256     // n
257     CHECK_PARAM(pvApiCtx, 4);
258     iN = getIntegerValue(pvApiCtx, 4);
259     if (iN < 1)
260     {
261         Scierror(999, _("%s: Wrong value for input argument #%d: Scalar positive integer expected.\n"), fname, 4);
262         return 0;
263     }
264
265     if (iN >= iNOBR)
266     {
267         Scierror(999, _("%s: The order should be at most %d.\n"), fname, 4, iNOBR - 1);
268         return 0;
269     }
270
271
272     // l
273     CHECK_PARAM(pvApiCtx, 5);
274     iL = getIntegerValue(pvApiCtx, 5);
275     if (iL < 1)
276     {
277         Scierror(999, _("%s: The system has no outputs\n"), fname);
278         return 0;
279     }
280
281     // R(nr,nr)
282     sciErr = getVarAddressFromPosition(pvApiCtx, 6, &piAddrR);
283     if (sciErr.iErr)
284     {
285         printError(&sciErr, 0);
286         Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 6);
287         return 0;
288     }
289
290     sciErr = getMatrixOfDouble(pvApiCtx, piAddrR, &iNR, &iNCOL, &pdblR);
291     if (sciErr.iErr)
292     {
293         printError(&sciErr, 0);
294         Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 6);
295         return 0;
296     }
297
298
299     if (iNR < 2 * iL)
300     {
301         Scierror(999, _("%s: Wrong size for input argument #%d: Must have at least %d rows expected.\n"), fname, 6, 2 * iL);
302         return 0;
303     }
304
305
306     if (iNCOL < iNR)
307     {
308         Scierror(999, _("%s: Wrong size for input argument #%d: Must have at least %d columns expected.\n"), fname, 6, iNCOL);
309         return 0;
310     }
311
312     // m
313     iM = iNR / (2 * iNOBR) - iL;
314
315     // tol
316     if (iRhs > 6)
317     {
318         int* piAddr = NULL;
319
320         sciErr = getVarAddressFromPosition(pvApiCtx, 7, &piAddr);
321         if (sciErr.iErr)
322         {
323             printError(&sciErr, 0);
324             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 7);
325             return 0;
326         }
327
328         if (getScalarDouble(pvApiCtx, piAddr, &dblTOL))
329         {
330             printError(&sciErr, 0);
331             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 7);
332             return 0;
333         }
334     }
335
336
337     // t
338     if (iRhs > 7)
339     {
340         cJOBCK = 'K';
341         CHECK_PARAM(pvApiCtx, 8);
342         iNSMPL = getIntegerValue(pvApiCtx, 8);
343
344         if (iNSMPL != 0 && iNSMPL < iNR)
345         {
346             Scierror(999, _("%s: The number of samples should be at least %d\n"), fname, iNR);
347             return 0;
348         }
349         else if (iNSMPL == 0)
350         {
351             cJOBCK = 'N';
352         }
353     }
354
355     // A(n,n)
356
357     if (iTASK >= 2 && iIJOB >= 3)
358     {
359         int* piAddr = NULL;
360         sciErr = getVarAddressFromPosition(pvApiCtx, 9, &piAddr);
361         if (sciErr.iErr)
362         {
363             printError(&sciErr, 0);
364             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 9);
365             return 0;
366         }
367
368         getMatrixOfDouble(pvApiCtx, piAddr, &iMA, &iNA, &pdblA);
369         iSizeA = iMA * iNA;
370
371         if (iMA != iN || iNA != iN)
372         {
373             Scierror(999, _("%s: Wrong size for input argument #%d: A matrix of size %dx%d expected.\n"), fname, 9, iN, iN);
374             return 0;
375         }
376
377         // C(1,n)
378         sciErr = getVarAddressFromPosition(pvApiCtx, 10, &piAddr);
379         if (sciErr.iErr)
380         {
381             printError(&sciErr, 0);
382             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 10);
383             return 0;
384         }
385
386         getMatrixOfDouble(pvApiCtx, piAddr, &iMA, &iNA, &pdblC);
387         iSizeA = iMA * iNA;
388
389         if (iMA != iL)
390         {
391             Scierror(999, _("%s: Wrong size for input argument #%d: Must have %d rows expected.\n"), fname, 10, iL);
392             return 0;
393         }
394
395         if (iNA != iN)
396         {
397             Scierror(999, _("%s: Wrong size for input argument #%d: Must have %d columns expected.\n"), fname, 10, iN);
398             return 0;
399         }
400     }
401
402     // printw
403     if (iRhs >= 11)
404     {
405         int iPrintw = 0;
406         CHECK_PARAM(pvApiCtx, 11);
407         iPRINTW = getIntegerValue(pvApiCtx, 11);
408
409         switch (iPRINTW)
410         {
411             case 1 :
412                 bPRINTW = TRUE;
413                 break;
414             case 0 :
415                 bPRINTW = FALSE;
416                 break;
417             default :
418                 Scierror(999, _("%s: Wrong value for input argument #%d: '%s' or '%s' expected.\n"), fname, 11, "0", "1");
419                 return 0;
420         }
421     }
422
423     // Determine the lengths of working arrays.
424     // The value for LDWORK is the minimum value needed by IB01BD for each
425     // method and algorithm implemented.  Using a larger value could
426     // increase the efficiency.
427     iMNOBR = iM * iNOBR;
428     iLNOBR = iL * iNOBR;
429     iMNOBRN = iMNOBR + iN;
430     iLDUNN = (iLNOBR - iL) * iN;
431     iNPL = iN + iL;
432     iN2 = iN + iN;
433     iNN = iN * iN;
434     iNL = iN * iL;
435     iLL = iL * iL;
436
437     iLDA = Max(1, iN);
438     iLDB = iLDA;
439     iLDC = Max(1, iL);
440     iLDD = iLDC;
441     iLDO = iLNOBR;
442     iLDR = iNR;
443     if (iNSMPL != 0)
444     {
445         iLDK = iLDA;
446         iLDQ = iLDA;
447         iLDS = iLDA;
448         iLDRY = iLDC;
449         iLBWORK = iN2;
450     }
451     else
452     {
453         iLDK = 1;
454         iLDQ = 1;
455         iLDS = 1;
456         iLDRY = 1;
457         iLBWORK = 1;
458     }
459
460     iLIWORK = iMNOBR + iN;
461     if (iTASK == 1)
462     {
463         iLIWORK = Max(iLIWORK, iLNOBR);
464     }
465     else if (iTASK == 2)
466     {
467         iLIWORK = Max(iLIWORK, iM * iNPL);
468     }
469     else // not handled yet
470     {
471         iLIWORK = Max(iLIWORK, Max(iLNOBR, iM * iNPL));
472     }
473     if (iNSMPL >= 0)
474     {
475         iLIWORK = Max(iLIWORK, iNN);
476     }
477
478     iIAW = 0;
479     iLDWORK = iLDUNN + 4 * iN;
480     if (iTASK == 1)
481     {
482         iID = 0;
483     }
484     else
485     {
486         iID = iN;
487     }
488
489     if (iTASK != 2 && iIJOB <= 2)
490     {
491         iLDWORK = Max(iLDWORK, Max(2 * iLDUNN + iN2, iLDUNN + iNN + 7 * iN));
492     }
493
494     if ((iM > 0 && iIJOB != 2) || iTASK >= 2)
495     {
496         iLDWORK = Max(iLDWORK, 2 * iLDUNN + iNN + iID + 7 * iN);
497         if (iTASK == 1)
498         {
499             iLDWORK = Max(iLDWORK, Max(iLDUNN + iN + 6 * iMNOBR, iLDUNN + iN + Max(iL + iMNOBR, iLNOBR + Max (3 * iLNOBR, iM))));
500         }
501     }
502     else
503     {
504         if (iTASK != 2)
505         {
506             iIAW = iN + iNN;
507         }
508     }
509
510     if (iTASK != 1 || iNSMPL > 0)
511     {
512         iLDWORK = Max(iLDWORK,
513                       Max(iLDUNN + iIAW + iN2 + Max(5 * iN, iLNOBR + 2 * iMNOBR + iL),
514                           Max(iID + 4 * iMNOBRN, iID + iMNOBRN + iNPL)));
515         if (iTASK != 1 && iM > 0 && iIJOB != 2)
516         {
517             iLDWORK = Max(iLDWORK, iMNOBR * iNPL * (iM * iNPL + 1 ) + Max(iNPL * iNPL, 4 * iM * iNPL + 1));
518         }
519
520         iLDWORK = iLNOBR * iN + iLDWORK;
521     }
522
523     if (iNSMPL > 0 )
524     {
525         iLDWORK = Max(iLDWORK, Max(4 * iNN + 2 * iNL + iLL + Max (3 * iL, iNL ), 14 * iNN + 12 * iN + 5));
526     }
527
528     //Allocate variable dimension local arrays.
529     pA = (double*)MALLOC(sizeof(double) * iLDA * iN);
530     pB = (double*)MALLOC(sizeof(double) * iLDB * iM);
531     pC = (double*)MALLOC(sizeof(double) * iLDC * iN);
532     pD = (double*)MALLOC(sizeof(double) * iLDD * iM);
533     pDWORK = (double*)MALLOC(sizeof(double) * iLDWORK);
534     pIWORK = (int*)MALLOC(sizeof(int) * iLIWORK);
535     pQ = (double*)MALLOC(sizeof(double) * iLDQ * iN);
536     pR = (double*)MALLOC(sizeof(double) * iLDR * iNCOL);
537     pRY = (double*)MALLOC(sizeof(double) * iLDRY * iL);
538     pS = (double*)MALLOC(sizeof(double) * iLDS * iL);
539     pBWORK = (int*)MALLOC(sizeof(int) * iLBWORK);
540     pK = (double*)MALLOC(sizeof(double) * iLDK * iL);
541
542     //Copy inputs from scilab workspace to locally allocated arrays.
543     iSize = iLDR * iNCOL;
544     C2F(dcopy)(&iSize, pdblR, &iOne, pR, &iOne);
545     if (iTASK >= 2 && iIJOB >= 3)
546     {
547         iSize = iLDA * iN;
548         C2F(dcopy)(&iSize, pdblA, &iOne, pA, &iOne);
549         iSize = iLDC * iN;
550         C2F(dcopy)(&iSize, pdblC, &iOne, pC, &iOne);
551     }
552
553     //Do the actual computations.
554     C2F(ib01bd)(&cMETH, &cJOB, &cJOBCK, &iNOBR, &iN, &iM, &iL, &iNSMPL, pR, &iLDR,
555                 pA, &iLDA, pC, &iLDC, pB, &iLDB, pD, &iLDD, pQ, &iLDQ,
556                 pRY, &iLDRY, pS, &iLDS, pK, &iLDK, &dblTOL, pIWORK, pDWORK,
557                 &iLDWORK, pBWORK, &iWARN, &iINFO);
558
559     if (iWARN != 0 && iPRINTW)
560     {
561         sciprint("IWARN = %d ON EXIT FROM IB01BD\n", iWARN);
562     }
563
564     if (iINFO != 0)
565     {
566         Scierror(999, _("%s: INFO = %d ON EXIT FROM IB01BD\n"), fname, iINFO);
567     }
568     else //Copy output to scilab workspace.
569     {
570         if (iIJOB <= 2)
571         {
572             iIP = 1;
573             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iN, pA);
574             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
575
576             if (iLhs > 1)
577             {
578                 iIP = 2;
579                 createMatrixOfDouble(pvApiCtx, iRhs + iIP, iLDC, iN, pC);
580                 AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
581             }
582         }
583         else
584         {
585             iIP = 0;
586         }
587
588         if (iLhs > iIP)
589         {
590             if (iIJOB == 1 || iIJOB >= 3)
591             {
592                 iIP++;
593                 createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iM, pB);
594                 AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
595             }
596
597             if (iLhs > iIP)
598             {
599                 if (iIJOB == 1 || iIJOB == 4)
600                 {
601                     iIP++;
602                     createMatrixOfDouble(pvApiCtx, iRhs + iIP, iL, iM, pD);
603                     AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
604                 }
605             }
606         }
607
608         if (iNSMPL > 0 && iLhs > iIP)
609         {
610             iIP++;
611             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iN, pQ);
612             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
613             iIP++;
614             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iL, iL, pRY);
615             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
616             iIP++;
617             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iL, pS);
618             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
619         }
620
621         if (iLhs > iIP)
622         {
623             iIP++;
624             if (iNSMPL == 0)
625             {
626                 iNRC = 4;
627             }
628             else
629             {
630                 iNRC = 12;
631             }
632
633             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iNRC, 1, pDWORK + 2 - 1);
634             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
635         }
636     }
637
638     //Deallocate local arrays.
639     FREE(pA);
640     FREE(pB);
641     FREE(pC);
642     FREE(pD);
643     FREE(pDWORK);
644     FREE(pIWORK);
645     FREE(pQ);
646     FREE(pR);
647     FREE(pRY);
648     FREE(pS);
649     FREE(pBWORK);
650     FREE(pK);
651
652     ReturnArguments(pvApiCtx);
653     return 0;
654 }