Coverity: cacsd module memory errors fixed
[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         default :
216             Scierror(999, _("%s: Wrong value for input argument #%d: '%s' or '%s' expected.\n"), fname, 1, "1", "2");
217             return 0;
218     }
219
220     // job
221     CHECK_PARAM(pvApiCtx, 2);
222     iIJOB = getIntegerValue(pvApiCtx, 2);
223
224     switch (iIJOB)
225     {
226         case 1 :
227             cJOB = 'A';
228             break;
229         case 2 :
230             cJOB = 'C';
231             break;
232         case 3 :
233             cJOB = 'B';
234             break;
235         default:
236             Scierror(999, _("%s: Wrong value for input argument #%d: '%s', '%s' or '%s' expected.\n"), fname, 2, "1", "2", "3");
237             return 0;
238     }
239
240
241     // s
242     CHECK_PARAM(pvApiCtx, 3);
243     iNOBR = getIntegerValue(pvApiCtx, 3);
244     if (iNOBR < 1)
245     {
246         Scierror(999, _("%s: Wrong value for input argument #%d: Scalar positive integer expected.\n"), fname, 3);
247         return 0;
248     }
249
250     // n
251     CHECK_PARAM(pvApiCtx, 4);
252     iN = getIntegerValue(pvApiCtx, 4);
253     if (iN < 1)
254     {
255         Scierror(999, _("%s: Wrong value for input argument #%d: Scalar positive integer expected.\n"), fname, 4);
256         return 0;
257     }
258
259     if (iN >= iNOBR)
260     {
261         Scierror(999, _("%s: The order should be at most %d.\n"), fname, 4, iNOBR - 1);
262         return 0;
263     }
264
265
266     // l
267     CHECK_PARAM(pvApiCtx, 5);
268     iL = getIntegerValue(pvApiCtx, 5);
269     if (iL < 1)
270     {
271         Scierror(999, _("%s: The system has no outputs\n"), fname);
272         return 0;
273     }
274
275     // R(nr,nr)
276     sciErr = getVarAddressFromPosition(pvApiCtx, 6, &piAddrR);
277     if (sciErr.iErr)
278     {
279         printError(&sciErr, 0);
280         Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 6);
281         return 0;
282     }
283
284     sciErr = getMatrixOfDouble(pvApiCtx, piAddrR, &iNR, &iNCOL, &pdblR);
285     if (sciErr.iErr)
286     {
287         printError(&sciErr, 0);
288         Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 6);
289         return 0;
290     }
291
292
293     if (iNR < 2 * iL)
294     {
295         Scierror(999, _("%s: Wrong size for input argument #%d: Must have at least %d rows expected.\n"), fname, 6, 2 * iL);
296         return 0;
297     }
298
299
300     if (iNCOL < iNR)
301     {
302         Scierror(999, _("%s: Wrong size for input argument #%d: Must have at least %d columns expected.\n"), fname, 6, iNCOL);
303         return 0;
304     }
305
306     // m
307     iM = iNR / (2 * iNOBR) - iL;
308
309     // tol
310     if (iRhs > 6)
311     {
312         int* piAddr = NULL;
313
314         sciErr = getVarAddressFromPosition(pvApiCtx, 7, &piAddr);
315         if (sciErr.iErr)
316         {
317             printError(&sciErr, 0);
318             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 7);
319             return 0;
320         }
321
322         if (getScalarDouble(pvApiCtx, piAddr, &dblTOL))
323         {
324             printError(&sciErr, 0);
325             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 7);
326             return 0;
327         }
328     }
329
330
331     // t
332     if (iRhs > 7)
333     {
334         cJOBCK = 'K';
335         CHECK_PARAM(pvApiCtx, 8);
336         iNSMPL = getInputArgumentType(pvApiCtx, 8);
337
338         if (iNSMPL != 0 && iNSMPL < iNR)
339         {
340             Scierror(999, _("%s: The number of samples should be at least %d\n"), fname, iNR);
341             return 0;
342         }
343         else if (iNSMPL == 0)
344         {
345             cJOBCK = 'N';
346         }
347     }
348
349     // A(n,n)
350
351     if (iTASK >= 2 && iIJOB >= 3)
352     {
353         int* piAddr = NULL;
354         sciErr = getVarAddressFromPosition(pvApiCtx, 9, &piAddr);
355         if (sciErr.iErr)
356         {
357             printError(&sciErr, 0);
358             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 9);
359             return 0;
360         }
361
362         getMatrixOfDouble(pvApiCtx, piAddr, &iMA, &iNA, &pdblA);
363         iSizeA = iMA * iNA;
364
365         if (iMA != iN || iNA != iN)
366         {
367             Scierror(999, _("%s: Wrong size for input argument #%d: A matrix of size %dx%d expected.\n"), fname, 9, iN, iN);
368             return 0;
369         }
370
371         // C(1,n)
372         sciErr = getVarAddressFromPosition(pvApiCtx, 10, &piAddr);
373         if (sciErr.iErr)
374         {
375             printError(&sciErr, 0);
376             Scierror(999, _("%s: Can not read input argument #%d.\n"), fname, 10);
377             return 0;
378         }
379
380         getMatrixOfDouble(pvApiCtx, piAddr, &iMA, &iNA, &pdblC);
381         iSizeA = iMA * iNA;
382
383         if (iMA != iL)
384         {
385             Scierror(999, _("%s: Wrong size for input argument #%d: Must have %d rows expected.\n"), fname, 10, iL);
386             return 0;
387         }
388
389         if (iNA != iN)
390         {
391             Scierror(999, _("%s: Wrong size for input argument #%d: Must have %d columns expected.\n"), fname, 10, iN);
392             return 0;
393         }
394     }
395
396     // printw
397     if (iRhs >= 11)
398     {
399         int iPrintw = 0;
400         CHECK_PARAM(pvApiCtx, 11);
401         iPRINTW = getIntegerValue(pvApiCtx, 11);
402
403         switch (iPRINTW)
404         {
405             case 1 :
406                 bPRINTW = TRUE;
407                 break;
408             case 0 :
409                 bPRINTW = FALSE;
410                 break;
411             default :
412                 Scierror(999, _("%s: Wrong value for input argument #%d: '%s' or '%s' expected.\n"), fname, 11, "0", "1");
413                 return 0;
414         }
415     }
416
417     // Determine the lengths of working arrays.
418     // The value for LDWORK is the minimum value needed by IB01BD for each
419     // method and algorithm implemented.  Using a larger value could
420     // increase the efficiency.
421     iMNOBR = iM * iNOBR;
422     iLNOBR = iL * iNOBR;
423     iMNOBRN = iMNOBR + iN;
424     iLDUNN = (iLNOBR - iL) * iN;
425     iNPL = iN + iL;
426     iN2 = iN + iN;
427     iNN = iN * iN;
428     iNL = iN * iL;
429     iLL = iL * iL;
430
431     iLDA = Max(1, iN);
432     iLDB = iLDA;
433     iLDC = Max(1, iL);
434     iLDD = iLDC;
435     iLDO = iLNOBR;
436     iLDR = iNR;
437     if (iNSMPL != 0)
438     {
439         iLDK = iLDA;
440         iLDQ = iLDA;
441         iLDS = iLDA;
442         iLDRY = iLDC;
443         iLBWORK = iN2;
444     }
445     else
446     {
447         iLDK = 1;
448         iLDQ = 1;
449         iLDS = 1;
450         iLDRY = 1;
451         iLBWORK = 1;
452     }
453
454     iLIWORK = iMNOBR + iN;
455     if (iTASK == 1)
456     {
457         iLIWORK = Max(iLIWORK, iLNOBR);
458     }
459     else if (iTASK == 2)
460     {
461         iLIWORK = Max(iLIWORK, iM * iNPL);
462     }
463     else // not handled yet
464     {
465         iLIWORK = Max(iLIWORK, Max(iLNOBR, iM * iNPL));
466     }
467     if (iNSMPL >= 0)
468     {
469         iLIWORK = Max(iLIWORK, iNN);
470     }
471
472     iIAW = 0;
473     iLDWORK = iLDUNN + 4 * iN;
474     if (iTASK == 1)
475     {
476         iID = 0;
477     }
478     else
479     {
480         iID = iN;
481     }
482
483     if (iTASK != 2 && iIJOB <= 2)
484     {
485         iLDWORK = Max(iLDWORK, Max(2 * iLDUNN + iN2, iLDUNN + iNN + 7 * iN));
486     }
487
488     if ((iM > 0 && iIJOB != 2) || iTASK >= 2)
489     {
490         iLDWORK = Max(iLDWORK, 2 * iLDUNN + iNN + iID + 7 * iN);
491         if (iTASK == 1)
492         {
493             iLDWORK = Max(iLDWORK, Max(iLDUNN + iN + 6 * iMNOBR, iLDUNN + iN + Max(iL + iMNOBR, iLNOBR + Max (3 * iLNOBR, iM))));
494         }
495     }
496     else
497     {
498         if (iTASK != 2)
499         {
500             iIAW = iN + iNN;
501         }
502     }
503
504     if (iTASK != 1 || iNSMPL > 0)
505     {
506         iLDWORK = Max(iLDWORK,
507                       Max(iLDUNN + iIAW + iN2 + Max(5 * iN, iLNOBR + 2 * iMNOBR + iL),
508                           Max(iID + 4 * iMNOBRN, iID + iMNOBRN + iNPL)));
509         if (iTASK != 1 && iM > 0 && iIJOB != 2)
510         {
511             iLDWORK = Max(iLDWORK, iMNOBR * iNPL * (iM * iNPL + 1 ) + Max(iNPL * iNPL, 4 * iM * iNPL + 1));
512         }
513
514         iLDWORK = iLNOBR * iN + iLDWORK;
515     }
516
517     if (iNSMPL > 0 )
518     {
519         iLDWORK = Max(iLDWORK, Max(4 * iNN + 2 * iNL + iLL + Max (3 * iL, iNL ), 14 * iNN + 12 * iN + 5));
520     }
521
522     //Allocate variable dimension local arrays.
523     pA = (double*)MALLOC(sizeof(double) * iLDA * iN);
524     pB = (double*)MALLOC(sizeof(double) * iLDB * iM);
525     pC = (double*)MALLOC(sizeof(double) * iLDC * iN);
526     pD = (double*)MALLOC(sizeof(double) * iLDD * iM);
527     pDWORK = (double*)MALLOC(sizeof(double) * iLDWORK);
528     pIWORK = (int*)MALLOC(sizeof(int) * iLIWORK);
529     pQ = (double*)MALLOC(sizeof(double) * iLDQ * iN);
530     pR = (double*)MALLOC(sizeof(double) * iLDR * iNCOL);
531     pRY = (double*)MALLOC(sizeof(double) * iLDRY * iL);
532     pS = (double*)MALLOC(sizeof(double) * iLDS * iL);
533     pBWORK = (int*)MALLOC(sizeof(int) * iLBWORK);
534     pK = (double*)MALLOC(sizeof(double) * iLDK * iL);
535
536     //Copy inputs from scilab workspace to locally allocated arrays.
537     iSize = iLDR * iNCOL;
538     C2F(dcopy)(&iSize, pdblR, &iOne, pR, &iOne);
539     if (iTASK >= 2 && iIJOB >= 3)
540     {
541         iSize = iLDA * iN;
542         C2F(dcopy)(&iSize, pdblA, &iOne, pA, &iOne);
543         iSize = iLDC * iN;
544         C2F(dcopy)(&iSize, pdblC, &iOne, pC, &iOne);
545     }
546
547     //Do the actual computations.
548     C2F(ib01bd)(&cMETH, &cJOB, &cJOBCK, &iNOBR, &iN, &iM, &iL, &iNSMPL, pR, &iLDR,
549                 pA, &iLDA, pC, &iLDC, pB, &iLDB, pD, &iLDD, pQ, &iLDQ,
550                 pRY, &iLDRY, pS, &iLDS, pK, &iLDK, &dblTOL, pIWORK, pDWORK,
551                 &iLDWORK, pBWORK, &iWARN, &iINFO);
552
553     if (iWARN != 0 && iPRINTW)
554     {
555         sciprint("IWARN = %d ON EXIT FROM IB01BD\n", iWARN);
556     }
557
558     if (iINFO != 0)
559     {
560         Scierror(999, _("%s: INFO = %d ON EXIT FROM IB01BD\n"), fname, iINFO);
561     }
562     else //Copy output to scilab workspace.
563     {
564         if (iIJOB <= 2)
565         {
566             iIP = 1;
567             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iN, pA);
568             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
569
570             if (iLhs > 1)
571             {
572                 iIP = 2;
573                 createMatrixOfDouble(pvApiCtx, iRhs + iIP, iLDC, iN, pC);
574                 AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
575             }
576         }
577         else
578         {
579             iIP = 0;
580         }
581
582         if (iLhs > iIP)
583         {
584             if (iIJOB == 1 || iIJOB >= 3)
585             {
586                 iIP++;
587                 createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iM, pB);
588                 AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
589             }
590
591             if (iLhs > iIP)
592             {
593                 if (iIJOB == 1 || iIJOB == 4)
594                 {
595                     iIP++;
596                     createMatrixOfDouble(pvApiCtx, iRhs + iIP, iL, iM, pD);
597                     AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
598                 }
599             }
600         }
601
602         if (iNSMPL > 0 && iLhs > iIP)
603         {
604             iIP++;
605             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iN, pQ);
606             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
607             iIP++;
608             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iL, iL, pRY);
609             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
610             iIP++;
611             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iN, iL, pS);
612             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
613         }
614
615         if (iLhs > iIP)
616         {
617             iIP++;
618             if (iNSMPL == 0)
619             {
620                 iNRC = 4;
621             }
622             else
623             {
624                 iNRC = 12;
625             }
626
627             createMatrixOfDouble(pvApiCtx, iRhs + iIP, iNRC, 1, pDWORK + 2 - 1);
628             AssignOutputVariable(pvApiCtx, iIP) = iRhs + iIP;
629         }
630     }
631
632     //Deallocate local arrays.
633     FREE(pA);
634     FREE(pB);
635     FREE(pC);
636     FREE(pD);
637     FREE(pDWORK);
638     FREE(pIWORK);
639     FREE(pQ);
640     FREE(pR);
641     FREE(pRY);
642     FREE(pS);
643     FREE(pBWORK);
644     FREE(pK);
645
646     ReturnArguments(pvApiCtx);
647     return 0;
648 }