diff options
35 files changed, 842 insertions, 60 deletions
diff --git a/includes/sci2clib.h b/includes/sci2clib.h index 14b8b4c..1bc3371 100644 --- a/includes/sci2clib.h +++ b/includes/sci2clib.h @@ -377,6 +377,12 @@ extern "C" { #include "syslin.h" #include "int_syslin.h" +#include "lqr.h" +#include "int_lqr.h" + +#include "lqe.h" +#include "int_lqe.h" + #include "schur.h" #include "int_schur.h" /*Functions related to opencv*/ diff --git a/macros/ASTManagement/AST_HandleEndGenFun.bin b/macros/ASTManagement/AST_HandleEndGenFun.bin Binary files differindex a9e28ef..5c2833e 100644 --- a/macros/ASTManagement/AST_HandleEndGenFun.bin +++ b/macros/ASTManagement/AST_HandleEndGenFun.bin diff --git a/macros/ASTManagement/AST_HandleEndGenFun.sci b/macros/ASTManagement/AST_HandleEndGenFun.sci index fa7b33a..5ff7181 100644 --- a/macros/ASTManagement/AST_HandleEndGenFun.sci +++ b/macros/ASTManagement/AST_HandleEndGenFun.sci @@ -316,7 +316,7 @@ NOutArg_mod = NOutArg OutArg(1).FindLike = InArg(1).FindLike; OutArg(1).Scope = 'Number_d'; else - OutArg = FA_GetOutArgInfo(InArg,NInArg,OutArg,NOutArg,SharedInfo,FunPrecSpecifier,FunTypeAnnot,FunSizeAnnot,ReportFileName); + OutArg = FA_GetOutArgInfo(InArg,NInArg,OutArg,NOutArg,SharedInfo,FunPrecSpecifier,FunTypeAnnot,FunSizeAnnot,ReportFileName,ASTFunName); end // #RNU_RES_B diff --git a/macros/ASTManagement/AST_HandleHeader.bin b/macros/ASTManagement/AST_HandleHeader.bin Binary files differindex 268846f..87ef763 100644 --- a/macros/ASTManagement/AST_HandleHeader.bin +++ b/macros/ASTManagement/AST_HandleHeader.bin diff --git a/macros/ASTManagement/AST_HandleHeader.sci b/macros/ASTManagement/AST_HandleHeader.sci index 75b2f97..0237bd5 100644 --- a/macros/ASTManagement/AST_HandleHeader.sci +++ b/macros/ASTManagement/AST_HandleHeader.sci @@ -165,7 +165,7 @@ SharedInfo.CurrentFunInfo.OutArg = ... SharedInfo.CurrentFunInfo.OutArg,NOutArg,... SharedInfo,... SharedInfo.CurrentFunInfo.FunPrecSpecifier,... - SharedInfo.CurrentFunInfo.FunTypeAnnot,SharedInfo.CurrentFunInfo.FunSizeAnnot,ReportFileName); + SharedInfo.CurrentFunInfo.FunTypeAnnot,SharedInfo.CurrentFunInfo.FunSizeAnnot,ReportFileName,''); //#RNU_RES_B // ------------------------------------------------------------------------- // --- Stores InArg structure into the temporary variables symbol table. --- diff --git a/macros/FunctionAnnotation/FA_GetOutArgInfo.bin b/macros/FunctionAnnotation/FA_GetOutArgInfo.bin Binary files differindex 2b75b7a..e8048b7 100644 --- a/macros/FunctionAnnotation/FA_GetOutArgInfo.bin +++ b/macros/FunctionAnnotation/FA_GetOutArgInfo.bin diff --git a/macros/FunctionAnnotation/FA_GetOutArgInfo.sci b/macros/FunctionAnnotation/FA_GetOutArgInfo.sci index d9696c8..a35281a 100644 --- a/macros/FunctionAnnotation/FA_GetOutArgInfo.sci +++ b/macros/FunctionAnnotation/FA_GetOutArgInfo.sci @@ -1,5 +1,6 @@ function UpdatedOutArg = ...
- FA_GetOutArgInfo(InArg,NInArg,OutArg,NOutArg,SharedInfo,FunPrecSpecifier,FunTypeAnnot,FunSizeAnnot,ReportFileName)
+ FA_GetOutArgInfo(InArg,NInArg,OutArg,NOutArg,SharedInfo,FunPrecSpecifier, ...
+ FunTypeAnnot,FunSizeAnnot,ReportFileName,ASTFunName)
// function UpdatedOutArg = ...
// FA_GetOutArgInfo(InArg,NInArg,OutArg,NOutArg,SharedInfo,FunPrecSpecifier,FunTypeAnnot,FunSizeAnnot,ReportFileName)
// -----------------------------------------------------------------
@@ -23,7 +24,7 @@ function UpdatedOutArg = ... // ------------------------------
// --- Check input arguments. ---
// ------------------------------
-SCI2CNInArgCheck(argn(2),9,9);
+SCI2CNInArgCheck(argn(2),10,10);
// -----------------------
// --- Initialization. ---
// -----------------------
@@ -121,8 +122,13 @@ for counterout = 1:NOutArg UpdatedOutArg(counterout).Size(iterOutputPosition) = string(tmpeval);
end
end
-
- UpdatedOutArg(counterout).Value = %nan;
+ if(ASTFunName == 'syslin')
+ no_of_st = eval(InArg(2).Size(1))
+ no_of_ip = eval(InArg(3).Size(2))
+ UpdatedOutArg(counterout).Value = no_of_st+no_of_ip*0.1;
+ else
+ UpdatedOutArg(counterout).Value = %nan;
+ end
UpdatedOutArg(counterout).Dimension = GetSymbolDimension(UpdatedOutArg(counterout).Size);
UpdatedOutArg(counterout).Scope = 'Temp';//NUT anche su questo si puo' ragionare verifica anche la handleoperation.
end
diff --git a/macros/FunctionAnnotation/FA_SCHUR_SZ.bin b/macros/FunctionAnnotation/FA_SCHUR_SZ.bin Binary files differindex 9d77eae..d16f029 100644 --- a/macros/FunctionAnnotation/FA_SCHUR_SZ.bin +++ b/macros/FunctionAnnotation/FA_SCHUR_SZ.bin diff --git a/macros/FunctionAnnotation/FA_SZ_LQE.bin b/macros/FunctionAnnotation/FA_SZ_LQE.bin Binary files differnew file mode 100644 index 0000000..2fb9772 --- /dev/null +++ b/macros/FunctionAnnotation/FA_SZ_LQE.bin diff --git a/macros/FunctionAnnotation/FA_SZ_LQE.sci b/macros/FunctionAnnotation/FA_SZ_LQE.sci new file mode 100644 index 0000000..e7aecf1 --- /dev/null +++ b/macros/FunctionAnnotation/FA_SZ_LQE.sci @@ -0,0 +1,8 @@ +function outsize = FA_SZ_LQE(inval,insz) + + inval = eval(inval) + insz = eval(insz) + outsize(1) = string(int(inval)); + outsize(2) = string(insz-int(inval)); + +endfunction diff --git a/macros/FunctionAnnotation/FA_SZ_LQR.bin b/macros/FunctionAnnotation/FA_SZ_LQR.bin Binary files differnew file mode 100644 index 0000000..2de9d56 --- /dev/null +++ b/macros/FunctionAnnotation/FA_SZ_LQR.bin diff --git a/macros/FunctionAnnotation/FA_SZ_LQR.sci b/macros/FunctionAnnotation/FA_SZ_LQR.sci new file mode 100644 index 0000000..2f199d0 --- /dev/null +++ b/macros/FunctionAnnotation/FA_SZ_LQR.sci @@ -0,0 +1,6 @@ +function outsize = FA_SZ_LQR(inval) + + inval = eval(inval) + outsize(1) = string(int(inval)); + outsize(2) = string(modulo(inval*10,10)); + endfunction diff --git a/macros/FunctionAnnotation/FA_SZ_LQR_K.bin b/macros/FunctionAnnotation/FA_SZ_LQR_K.bin Binary files differnew file mode 100644 index 0000000..a936e3b --- /dev/null +++ b/macros/FunctionAnnotation/FA_SZ_LQR_K.bin diff --git a/macros/FunctionAnnotation/lib b/macros/FunctionAnnotation/lib Binary files differindex a1a11a9..4053992 100644 --- a/macros/FunctionAnnotation/lib +++ b/macros/FunctionAnnotation/lib diff --git a/macros/FunctionAnnotation/names b/macros/FunctionAnnotation/names index 64cb257..e072098 100644 --- a/macros/FunctionAnnotation/names +++ b/macros/FunctionAnnotation/names @@ -19,6 +19,8 @@ FA_SZ_COL_DIAG_IN_EX FA_SZ_DIFF FA_SZ_FROM_VAL FA_SZ_LINSPACE_ROW +FA_SZ_LQE +FA_SZ_LQR FA_SZ_OPAPEX FA_SZ_OPBACKSLASH FA_SZ_OPCC diff --git a/macros/ToolInitialization/INIT_FillSCI2LibCDirs.bin b/macros/ToolInitialization/INIT_FillSCI2LibCDirs.bin Binary files differindex 3e07d5a..ae688b2 100644 --- a/macros/ToolInitialization/INIT_FillSCI2LibCDirs.bin +++ b/macros/ToolInitialization/INIT_FillSCI2LibCDirs.bin diff --git a/macros/ToolInitialization/INIT_FillSCI2LibCDirs.sci b/macros/ToolInitialization/INIT_FillSCI2LibCDirs.sci index 93ca508..bcf2670 100644 --- a/macros/ToolInitialization/INIT_FillSCI2LibCDirs.sci +++ b/macros/ToolInitialization/INIT_FillSCI2LibCDirs.sci @@ -5926,19 +5926,19 @@ PrintStringInfo('NIN= 4',ClassFileName,'file','y'); PrintStringInfo('NOUT= 1',ClassFileName,'file','y');
PrintStringInfo('OUT(1).TP= ''ss''',ClassFileName,'file','y');
PrintStringInfo('OUT(1).SZ(1)= FA_ADD(IN(2).SZ(1),IN(4).SZ(1))',ClassFileName,'file','y');
-PrintStringInfo('OUT(1).SZ(2)= FA_ADD(FA_ADD(IN(2).SZ(2),IN(3).SZ(2)), ''1'')',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(2)= FA_ADD(FA_ADD(IN(2).SZ(2),IN(3).SZ(2)), ''2'')',ClassFileName,'file','y');
PrintStringInfo('NIN= 5',ClassFileName,'file','y');
PrintStringInfo('NOUT= 1',ClassFileName,'file','y');
PrintStringInfo('OUT(1).TP= ''ss''',ClassFileName,'file','y');
PrintStringInfo('OUT(1).SZ(1)= FA_ADD(IN(2).SZ(1),IN(4).SZ(1))',ClassFileName,'file','y');
-PrintStringInfo('OUT(1).SZ(2)= FA_ADD(FA_ADD(IN(2).SZ(2),IN(3).SZ(2)), ''1'')',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(2)= FA_ADD(FA_ADD(IN(2).SZ(2),IN(3).SZ(2)), ''2'')',ClassFileName,'file','y');
PrintStringInfo('NIN= 6',ClassFileName,'file','y');
PrintStringInfo('NOUT= 1',ClassFileName,'file','y');
PrintStringInfo('OUT(1).TP= ''ss''',ClassFileName,'file','y');
PrintStringInfo('OUT(1).SZ(1)= FA_ADD(IN(2).SZ(1),IN(4).SZ(1))',ClassFileName,'file','y');
-PrintStringInfo('OUT(1).SZ(2)= FA_ADD(FA_ADD(IN(2).SZ(2),IN(3).SZ(2)), ''1'')',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(2)= FA_ADD(FA_ADD(IN(2).SZ(2),IN(3).SZ(2)), ''2'')',ClassFileName,'file','y');
ClassFileName = fullfile(SCI2CLibCFLClsDir,ClassName+ExtensionCFuncListCls);
PrintStringInfo('g2d2d2d2'+ArgSeparator+'ss2',ClassFileName,'file','y');
@@ -6114,6 +6114,60 @@ INIT_GenAnnFLFunctions(FunctionName,SCI2CLibCAnnFunDir,ClassName,GeneralReport,E INIT_GenAnnFLFunctions(FunctionName,SCI2CLibCFLFunDir,ClassName,GeneralReport,ExtensionCFuncListFun);
+//------------------------------------
+//---- Class lqr ---------------------
+//------------------------------------
+ClassName = 'LQR';
+
+// --- Class Annotation. ---
+PrintStringInfo(' Adding Class: '+ClassName+'.',GeneralReport,'file','y');
+ClassFileName = fullfile(SCI2CLibCAnnClsDir,ClassName+ExtensionCAnnCls);
+
+PrintStringInfo('NIN= 1',ClassFileName,'file','y');
+PrintStringInfo('NOUT= 2',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).TP= ''d''',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(1)= FA_SZ_1(FA_SZ_LQR(IN(1).VAL))',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(2)= FA_SZ_1(FA_SZ_LQR(IN(1).VAL))',ClassFileName,'file','y');
+PrintStringInfo('OUT(2).TP= ''d''',ClassFileName,'file','y');
+PrintStringInfo('OUT(2).SZ(1)= FA_SZ_2(FA_SZ_LQR(IN(1).VAL))',ClassFileName,'file','y');
+PrintStringInfo('OUT(2).SZ(2)= FA_SZ_1(FA_SZ_LQR(IN(1).VAL))',ClassFileName,'file','y');
+
+ClassFileName = fullfile(SCI2CLibCFLClsDir,ClassName+ExtensionCFuncListCls);
+PrintStringInfo('ss2'+ArgSeparator+'d2d2',ClassFileName,'file','y');
+
+// --- Annotation Function And Function List Function. ---
+FunctionName = 'lqr';
+PrintStringInfo(' Adding Function: '+FunctionName+'.',GeneralReport,'file','y');
+INIT_GenAnnFLFunctions(FunctionName,SCI2CLibCAnnFunDir,ClassName,GeneralReport,ExtensionCAnnFun);
+INIT_GenAnnFLFunctions(FunctionName,SCI2CLibCFLFunDir,ClassName,GeneralReport,ExtensionCFuncListFun);
+
+//------------------------------------
+//---- Class lqe ---------------------
+//------------------------------------
+ClassName = 'LQE';
+
+// --- Class Annotation. ---
+PrintStringInfo(' Adding Class: '+ClassName+'.',GeneralReport,'file','y');
+ClassFileName = fullfile(SCI2CLibCAnnClsDir,ClassName+ExtensionCAnnCls);
+
+PrintStringInfo('NIN= 1',ClassFileName,'file','y');
+PrintStringInfo('NOUT= 2',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).TP= ''d''',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(1)= FA_SZ_1(FA_SZ_LQE(IN(1).VAL,IN(1).SZ(1)))',ClassFileName,'file','y');
+PrintStringInfo('OUT(1).SZ(2)= FA_SZ_1(FA_SZ_LQE(IN(1).VAL,IN(1).SZ(1)))',ClassFileName,'file','y');
+PrintStringInfo('OUT(2).TP= ''d''',ClassFileName,'file','y');
+PrintStringInfo('OUT(2).SZ(1)= FA_SZ_1(FA_SZ_LQE(IN(1).VAL,IN(1).SZ(1)))',ClassFileName,'file','y');
+PrintStringInfo('OUT(2).SZ(2)= FA_SZ_2(FA_SZ_LQE(IN(1).VAL,IN(1).SZ(1)))',ClassFileName,'file','y');
+
+ClassFileName = fullfile(SCI2CLibCFLClsDir,ClassName+ExtensionCFuncListCls);
+PrintStringInfo('ss2'+ArgSeparator+'d2d2',ClassFileName,'file','y');
+
+// --- Annotation Function And Function List Function. ---
+FunctionName = 'lqe';
+PrintStringInfo(' Adding Function: '+FunctionName+'.',GeneralReport,'file','y');
+INIT_GenAnnFLFunctions(FunctionName,SCI2CLibCAnnFunDir,ClassName,GeneralReport,ExtensionCAnnFun);
+INIT_GenAnnFLFunctions(FunctionName,SCI2CLibCFLFunDir,ClassName,GeneralReport,ExtensionCFuncListFun);
+
// ////////////////////////////////////////////
// /////PARTE INTRODOTTA DA ALBERTO MOREA
// /////////////////////////////////////////////
diff --git a/macros/findDeps/getAllHeaders.bin b/macros/findDeps/getAllHeaders.bin Binary files differindex 394987b..f5bb98e 100644 --- a/macros/findDeps/getAllHeaders.bin +++ b/macros/findDeps/getAllHeaders.bin diff --git a/macros/findDeps/getAllHeaders.sci b/macros/findDeps/getAllHeaders.sci index cbc2d54..b37f3e5 100644 --- a/macros/findDeps/getAllHeaders.sci +++ b/macros/findDeps/getAllHeaders.sci @@ -149,6 +149,8 @@ function allHeaders = getAllHeaders(SharedInfo) "src/c/Files/includes/files.h" "src/c/string/includes/convstr.h" "src/c/CACSD/includes/syslin.h" + "src/c/CACSD/includes/lqr.h" + "src/c/CACSD/includes/lqe.h" "src/c/linearAlgebra/includes/schur.h"]; //Header files required for "Arduino" output diff --git a/macros/findDeps/getAllInterfaces.bin b/macros/findDeps/getAllInterfaces.bin Binary files differindex 002cb38..da506e8 100644 --- a/macros/findDeps/getAllInterfaces.bin +++ b/macros/findDeps/getAllInterfaces.bin diff --git a/macros/findDeps/getAllInterfaces.sci b/macros/findDeps/getAllInterfaces.sci index 54c32e5..32a0c6b 100644 --- a/macros/findDeps/getAllInterfaces.sci +++ b/macros/findDeps/getAllInterfaces.sci @@ -144,6 +144,8 @@ function allInterfaces = getAllInterfaces(SharedInfo) "src/c/Files/interfaces/int_files.h" "src/c/string/interfaces/int_convstr.h" "src/c/CACSD/interfaces/int_syslin.h" + "src/c/CACSD/interfaces/int_lqr.h" + "src/c/CACSD/interfaces/int_lqe.h" "src/c/linearAlgebra/interfaces/int_schur.h"]; //Interface files required for "Arduino" output diff --git a/macros/findDeps/getAllSources.bin b/macros/findDeps/getAllSources.bin Binary files differindex 28567f6..aa5d474 100644 --- a/macros/findDeps/getAllSources.bin +++ b/macros/findDeps/getAllSources.bin diff --git a/macros/findDeps/getAllSources.sci b/macros/findDeps/getAllSources.sci index 58c23e9..3f789b4 100644 --- a/macros/findDeps/getAllSources.sci +++ b/macros/findDeps/getAllSources.sci @@ -979,6 +979,8 @@ function allSources = getAllSources(SharedInfo) "src/c/Files/mput/i16mputa.c" "src/c/string/convstr/gconvstrs.c" "src/c/CACSD/syslin/dsyslina.c" + "src/c/CACSD/lqr/dlqra.c" + "src/c/CACSD/lqe/dlqea.c" "src/c/linearAlgebra/schur/dschura.c" "src/c/linearAlgebra/schur/dgschura.c"]; diff --git a/src/c/CACSD/includes/lqe.h b/src/c/CACSD/includes/lqe.h new file mode 100644 index 0000000..1cc902f --- /dev/null +++ b/src/c/CACSD/includes/lqe.h @@ -0,0 +1,26 @@ + /* Copyright (C) 2017 - IIT Bombay - FOSSEE + + This file must be used under the terms of the CeCILL. + This source file is licensed as described in the file COPYING, which + you should have received as part of this distribution. The terms + are also available at + http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt + Author: Siddhesh Wani + Organization: FOSSEE, IIT Bombay + Email: toolbox@scilab.in + */ +#ifndef __LQE_H__ +#define __LQE_H__ + + +#ifdef __cplusplus +extern "C" { +#endif + +void dlqea(double* sys, int sys_rows, int sys_cols, double* X, double* K); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /*__LQE_H__*/ diff --git a/src/c/CACSD/includes/lqr.h b/src/c/CACSD/includes/lqr.h new file mode 100644 index 0000000..062d6e4 --- /dev/null +++ b/src/c/CACSD/includes/lqr.h @@ -0,0 +1,26 @@ + /* Copyright (C) 2017 - IIT Bombay - FOSSEE + + This file must be used under the terms of the CeCILL. + This source file is licensed as described in the file COPYING, which + you should have received as part of this distribution. The terms + are also available at + http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt + Author: Siddhesh Wani + Organization: FOSSEE, IIT Bombay + Email: toolbox@scilab.in + */ +#ifndef __LQR_H__ +#define __LQR_H__ + + +#ifdef __cplusplus +extern "C" { +#endif + +void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /*__LQR_H__*/ diff --git a/src/c/CACSD/includes/syslin.h b/src/c/CACSD/includes/syslin.h index 7d361c9..21b2329 100644 --- a/src/c/CACSD/includes/syslin.h +++ b/src/c/CACSD/includes/syslin.h @@ -20,7 +20,7 @@ extern "C" { #endif -void dsyslina(double* A, int no_of_states, double* B, int no_of_inputs, \ +void dsyslina(char* dom, double* A, int no_of_states, double* B, int no_of_inputs, \ double* C, int no_of_outputs, double* D, double* X0, double* out); diff --git a/src/c/CACSD/interfaces/int_lqe.h b/src/c/CACSD/interfaces/int_lqe.h new file mode 100644 index 0000000..d801bf3 --- /dev/null +++ b/src/c/CACSD/interfaces/int_lqe.h @@ -0,0 +1,25 @@ + /* Copyright (C) 2017 - IIT Bombay - FOSSEE + + This file must be used under the terms of the CeCILL. + This source file is licensed as described in the file COPYING, which + you should have received as part of this distribution. The terms + are also available at + http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt + Author: Siddhesh Wani + Organization: FOSSEE, IIT Bombay + Email: toolbox@scilab.in + */ +#ifndef __INT_LQE_H__ +#define __INT_LQE_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define ss2lqed2d2(in1,size1,out1,out2) dlqea(in1,size1[0],size1[1],out1,out2); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /*__INT_LQE_H__*/ diff --git a/src/c/CACSD/interfaces/int_lqr.h b/src/c/CACSD/interfaces/int_lqr.h new file mode 100644 index 0000000..e6f534f --- /dev/null +++ b/src/c/CACSD/interfaces/int_lqr.h @@ -0,0 +1,25 @@ + /* Copyright (C) 2017 - IIT Bombay - FOSSEE + + This file must be used under the terms of the CeCILL. + This source file is licensed as described in the file COPYING, which + you should have received as part of this distribution. The terms + are also available at + http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt + Author: Siddhesh Wani + Organization: FOSSEE, IIT Bombay + Email: toolbox@scilab.in + */ +#ifndef __INT_LQR_H__ +#define __INT_LQR_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define ss2lqrd2d2(in1,size1,out1,out2) dlqra(in1,size1[0],size1[1],out1,out2); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /*__INT_LQR_H__*/ diff --git a/src/c/CACSD/interfaces/int_syslin.h b/src/c/CACSD/interfaces/int_syslin.h index 2327c38..3f74ea6 100644 --- a/src/c/CACSD/interfaces/int_syslin.h +++ b/src/c/CACSD/interfaces/int_syslin.h @@ -18,57 +18,57 @@ extern "C" { #endif #define g2d2d2d2syslinss2(in1,size1,in2,size2,in3,size3,in4,size4,out) \ - dsyslina(in2,size2[0],in3,size3[1],in4,size4[0],NULL,NULL,out) + dsyslina(in1,in2,size2[0],in3,size3[1],in4,size4[0],NULL,NULL,out) #define g2d2d2d2d2syslinss2(in1,size1,in2,size2,in3,size3,in4,size4,in5, \ - size5,out) dsyslina(in2,size2[0],in3,size3[1],in4,size4[0],in5,NULL,out) + size5,out) dsyslina(in1,in2,size2[0],in3,size3[1],in4,size4[0],in5,NULL,out) #define g2d2d2d2d2d2syslinss2(in1,size1,in2,size2,in3,size3,in4,size4, \ - in5,size5,in6,size6,out) dsyslina(in2,size2[0],in3,size3[1],in4, \ + in5,size5,in6,size6,out) dsyslina(in1,in2,size2[0],in3,size3[1],in4, \ size4[0],in5,in6,out) // #define g2d0d0d0syslinss2(in1,size1,in2,in3,in4,out) \ - dsyslina(in2,1,in3,1,in4,1,NULL,NULL,out) + dsyslina(in1,&in2,1,&in3,1,&in4,1,NULL,NULL,out) #define g2d0d2d0syslinss2(in1,size1,in2,in3,size3,in4,out) \ - dsyslina(in2,1,in3,size3[1],in4,1,NULL,NULL,out) + dsyslina(in1,&in2,1,in3,size3[1],&in4,1,NULL,NULL,out) #define g2d0d0d2syslinss2(in1,size1,in2,in3,in4,size4,out) \ - dsyslina(in2,1,in3,1,in4,size4[0],NULL,NULL,out) + dsyslina(in1,&in2,1,&in3,1,in4,size4[0],NULL,NULL,out) #define g2d0d2d2syslinss2(in1,size1,in2,in3,size3,in4,size4,out) \ - dsyslina(in2,1,in3,size3[1],in4,size4[0],NULL,NULL,out) + dsyslina(in1,&in2,1,in3,size3[1],in4,size4[0],NULL,NULL,out) // #define g2d0d0d0d0syslinss2(in1,size1,in2,in3,in4,in5,out) \ - dsyslina(in2,1,in3,1,in4,1,in5,NULL,out) + dsyslina(in1,&in2,1,&in3,1,&in4,1,&in5,NULL,out) #define g2d0d2d0d2syslinss2(in1,size1,in2,in3,size3,in4,in5,size5, \ - out) dsyslina(in2,1,in3,size3[1],in4,1,in5,NULL,out) + out) dsyslina(in1,&in2,1,in3,size3[1],&in4,1,&in5,NULL,out) #define g2d0d0d2d2syslinss2(in1,size1,in2,in3,in4,size4,in5,size5, \ - out) dsyslina(in2,1,in3,1,in4,size4[0],in5,NULL,out) + out) dsyslina(in1,&in2,1,&in3,1,in4,size4[0],in5,NULL,out) #define g2d0d2d2d2syslinss2(in1,size1,in2,in3,size3,in4,size4,in5, \ - size5, out) dsyslina(in2,1,in3,size3[1],in4,size4[0],in5,NULL,out) + size5, out) dsyslina(in1,&in2,1,in3,size3[1],in4,size4[0],in5,NULL,out) #define g2d2d2d2d0syslinss2(in1,size1,in2,size2,in3,size3,in4,size4, \ - in5,out) dsyslina(in2,size2[0],in3,size3[1],in4,size4[0],in5,NULL,out) + in5,out) dsyslina(in1,in2,size2[0],in3,size3[1],in4,size4[0],&in5,NULL,out) // #define g2d0d0d0d0d0syslinss2(in1,size1,in2,in3,in4,in5,in6,out) \ - dsyslina(in2,1,in3,1,in4,1,in5,in6,out) + dsyslina(in1,&in2,1,&in3,1,&in4,1,&in5,&in6,out) #define g2d0d2d0d2d0syslinss2(in1,size1,in2,in3,size3,in4,in5,size5, \ - in6,out) dsyslina(in2,1,in3,size3[1],in4,1,in5,in6,out) + in6,out) dsyslina(in1,&in2,1,in3,size3[1],in4,1,in5,&in6,out) #define g2d0d0d2d2d0syslinss2(in1,size1,in2,in3,in4,size4,in5,size5, \ - in6,out) dsyslina(in2,1,in3,1,in4,size4[0],in5,in6,out) + in6,out) dsyslina(in1,&in2,1,in3,1,in4,size4[0],in5,&in6,out) #define g2d0d2d2d2d0syslinss2(in1,size1,in2,in3,size3,in4,size4,in5, \ - size5,in6,out) dsyslina(in2,1,in3,size3[1],in4,size4[0],in5,in6,out) + size5,in6,out) dsyslina(in1,&in2,1,in3,size3[1],in4,size4[0],in5,&in6,out) #define g2d2d2d2d0d2syslinss2(in1,size1,in2,size2,in3,size3,in4,size4, \ - in5,in6,size6,out) dsyslina(in2,size2[0],in3,size3[1],in4,size4[0], \ - in5,in6,out) + in5,in6,size6,out) dsyslina(in1,in2,size2[0],in3,size3[1],in4,size4[0], \ + &in5,in6,out) #ifdef __cplusplus } /* extern "C" */ diff --git a/src/c/CACSD/lqe/dlqea.c b/src/c/CACSD/lqe/dlqea.c new file mode 100644 index 0000000..bc729fb --- /dev/null +++ b/src/c/CACSD/lqe/dlqea.c @@ -0,0 +1,59 @@ +/* Copyright (C) 2017 - IIT Bombay - FOSSEE + + This file must be used under the terms of the CeCILL. + This source file is licensed as described in the file COPYING, which + you should have received as part of this distribution. The terms + are also available at + http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt + Author: Siddhesh Wani + Organization: FOSSEE, IIT Bombay + Email: toolbox@scilab.in +*/ + +/*Function for calculating lqe gain. Refer 'lqe.sci' in scilab source.*/ + +#include "lqr.h" +#include "matrixTranspose.h" + +void dlqea(double* sys, int sys_rows, int sys_cols, double* X, double* K) +{ + + double *sys_lqr, *K_lqr; + int row, col; + int sys_lqr_rows = sys_cols-2; + int sys_lqr_cols = sys_rows + 2; + int no_of_ip, no_of_st; + + no_of_ip = sys[sys_rows*(sys_cols-1)+1]; + no_of_st = sys[sys_rows*(sys_cols-1)]; + + /*Transpose given system and calculate LQR with new system. Transpose result + got from LQR*/ + + sys_lqr = (double*) malloc(sys_lqr_rows*sys_lqr_cols*sizeof(double)); + + for(col = 0; col<sys_lqr_cols; col++) + { + for(row = 0; row<sys_lqr_rows; row++) + { + sys_lqr[col*sys_lqr_rows+row] = sys[row*sys_rows+col]; + + } + } + + /*Copy initial states and type of system*/ + for(row = 0; row<sys_lqr_rows; row++) + { + sys_lqr[sys_lqr_rows*(sys_lqr_cols-2) + row] = sys[sys_rows*(sys_cols-2)+row]; + } + + /*Copy no of states and no of inputs*/ + sys_lqr[sys_lqr_rows*(sys_lqr_cols-1)] = no_of_st; + sys_lqr[sys_lqr_rows*(sys_lqr_cols-1)+1] = sys_rows - no_of_st; + + /*Calculate LQR gain*/ + K_lqr = (double*) malloc(no_of_ip*no_of_st*sizeof(double)); + dlqra(sys_lqr,sys_lqr_rows,sys_lqr_cols,X,K_lqr); + + dtransposea(K_lqr,sys_rows - no_of_st,no_of_st,K); +}
\ No newline at end of file diff --git a/src/c/CACSD/lqr/as_per_sci_code.c b/src/c/CACSD/lqr/as_per_sci_code.c new file mode 100644 index 0000000..01f002c --- /dev/null +++ b/src/c/CACSD/lqr/as_per_sci_code.c @@ -0,0 +1,144 @@ + + sizeBA = 2*no_of_states + no_of_inputs; + BigE = (double*) malloc (sizeBA*sizeBA*sizeof(double)); + BigA = (double*) malloc (sizeBA*sizeBA*sizeof(double)); + + /*Setup BigE*/ + deyea(BigE,sizeBA,sizeBA); + + for(row = no_of_states*2; row<sizeBA; row++) + { + BigE[row*sizeBA+row] = 0; + } + + /*Setup BigA*/ + for(col=0; col < no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + BigA[col*sizeBA+row] = A[col*no_of_states + row]; + } + } + + for(col=no_of_states; col < no_of_states*2; col++) + { + for(row = 0; row < no_of_states; row++) + { + BigA[col*sizeBA+row] = 0; + } + } + + for(col=2*no_of_states; col < sizeBA; col++) + { + for(row = 0; row < no_of_states; row++) + { + BigA[col*sizeBA+row] = B[col*no_of_states + row]; + } + } + + for(col=0; col < no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + BigA[col*sizeBA+row] = -1.0*Q[col*no_of_states + row]; + } + } + + for(col=no_of_states; col < 2*no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + BigA[col*sizeBA+row] = -1.0*A[row*no_of_states + col]; + } + } + + for(col=2*no_of_states; col < sizeBA; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + BigA[col*sizeBA+row] = -1.0*S[row*no_of_inputs + col]; + } + } + + for(col=0; col < no_of_states; col++) + { + for(row = 2*no_of_states; row < sizeBA; row++) + { + BigA[col*sizeBA+row] = S[col*no_of_inputs + row]; + } + } + + for(col=no_of_states; col < 2*no_of_states; col++) + { + for(row = 2*no_of_states; row < sizeBA; row++) + { + BigA[col*sizeBA+row] = B[row*no_of_inputs + col]; + } + } + + + for(col=2*no_of_states; col < sizeBA; col++) + { + for(row = 2*no_of_states; row < sizeBA; row++) + { + BigA[col*sizeBA+row] = R[col*no_of_inputs + row]; + } + } + + /*Free up unwanted variables*/ + free(A); + free(C); + free(C_t); + free(D); + free(D_t); + free(Q); + + /*Inverse of R*/ + Ri = (double*) malloc(no_of_inputs*no_of_inputs*sizeof(double)); + dinverma(R,Ri,no_of_inputs); + + /*Setup Left*/ + Left = (double*) malloc(sizeBA*sizeBA*sizeof(double)); + deyea(Left,sizeBA,sizeBA); + + BRi = (double*) malloc(no_of_states*no_of_inputs*sizeof(double)); + S_t = (double*) malloc(no_of_states*no_of_inputs*sizeof(double)); + StRi = (double*) malloc(no_of_states*no_of_inputs*sizeof(double)); + + dtransposea(S,no_of_inputs,no_of_states,S_t); + dmula(B,no_of_states,no_of_inputs,Ri,no_of_inputs,no_of_inputs,BRi); + dmula(S_t,no_of_states,no_of_inputs,Ri,no_of_inputs,no_of_inputs,StRi); + + for(col=2*no_of_states; col < sizeBA; col++) + { + for(row = 0; row < no_of_states; row++) + { + Left[col*sizeBA+row] = -1.0*BRi[col*no_of_states + row]; + } + } + + for(col=2*no_of_states; col < sizeBA; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + Left[col*sizeBA+row] = StRi[col*no_of_states + row]; + } + } + + for(col=2*no_of_states; col < sizeBA; col++) + { + for(row = 2*no_of_states; row < sizeBA; row++) + { + Left[col*sizeBA+row] = Ri[col*no_of_states + row]; + } + } + + /*Freeup umwanted variables*/ + free(R); + free(BRi); + free(S_t); + free(StRi); + free(B); + + LA = (double*) malloc(sizeBA*sizeBA*sizeof(double)); + dmula(Left,sizeBA,sizeBA,BigA,sizeBA,sizeBA,LA);
\ No newline at end of file diff --git a/src/c/CACSD/lqr/dlqra.c b/src/c/CACSD/lqr/dlqra.c new file mode 100644 index 0000000..26a5e6c --- /dev/null +++ b/src/c/CACSD/lqr/dlqra.c @@ -0,0 +1,352 @@ +/* Copyright (C) 2017 - IIT Bombay - FOSSEE + + This file must be used under the terms of the CeCILL. + This source file is licensed as described in the file COPYING, which + you should have received as part of this distribution. The terms + are also available at + http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt + Author: Siddhesh Wani + Organization: FOSSEE, IIT Bombay + Email: toolbox@scilab.in +*/ + +/*Function for calculating lqr gain. Refer 'lqr.sci' in scilab source.*/ + +#include <stdlib.h> +#include "matrixTranspose.h" +#include "matrixMultiplication.h" +#include "eye.h" +#include "matrixInversion.h" +#include "subtraction.h" +#include "addition.h" +#include "schur.h" +#include "matrixDivision.h" + +void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K) +{ + int no_of_states, no_of_inputs, no_of_outputs, dom = 1; + int row,col; + no_of_states = sys[sys_rows*(sys_cols-1)]; + no_of_inputs = sys[sys_rows*(sys_cols-1) + 1]; + no_of_outputs = sys_rows - no_of_states; + + double *A, *B, *C, *D; + double *B_t, *C_t, *D_t; + double *Q, *R, *S; + double *Ri, *LA, *LE; + double *BRi, *StRi, *S_t; + double *buf1, *buf2, *buf3, *buf4, *buf5, *buf6; + + int ks; + double *wsmall, *X12, *phi12; + + A = (double*) malloc (no_of_states*no_of_states*sizeof(double)); + B = (double*) malloc (no_of_states*no_of_inputs*sizeof(double)); + C = (double*) malloc (no_of_states*no_of_outputs*sizeof(double)); + D = (double*) malloc (no_of_inputs*no_of_outputs*sizeof(double)); + + B_t = (double*) malloc (no_of_states*no_of_inputs*sizeof(double)); + C_t = (double*) malloc (no_of_states*no_of_outputs*sizeof(double)); + D_t = (double*) malloc (no_of_inputs*no_of_outputs*sizeof(double)); + + /*Get A from system matrix*/ + for(col = 0; col < no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + A[col*no_of_states + row] = sys[col*sys_rows + row]; + } + } + + /*Get matrix B from system matrix*/ + for(col=0; col < no_of_inputs; col++) + { + for(row = 0; row < no_of_states; row++) + { + B[col * no_of_states + row] = \ + sys[col * sys_rows + no_of_states*sys_rows + row]; + } + } + + /*Get matrix C from system matrix*/ + for(col=0; col < no_of_states; col++) + { + for(row = 0; row < no_of_outputs; row++) + { + C[col * no_of_outputs + row] = \ + sys[no_of_states + (col*sys_rows) + row]; + } + } + + /*Get matrix D from system matrix*/ + for(col=0; col < no_of_inputs; col++) + { + for(row = 0; row < no_of_outputs; row++) + { + D[col * no_of_outputs + row] = \ + sys[(no_of_states+col)*sys_rows + no_of_states + row]; + } + } + + dom = sys[(sys_rows*(sys_cols-2)) + no_of_states]; + + Q = (double*) malloc (no_of_states*no_of_states*sizeof(double)); + R = (double*) malloc (no_of_inputs*no_of_inputs*sizeof(double)); + S = (double*) malloc (no_of_inputs*no_of_states*sizeof(double)); + + dtransposea(B,no_of_states,no_of_inputs,B_t); + dtransposea(C,no_of_outputs,no_of_states,C_t); + dtransposea(D,no_of_outputs,no_of_inputs,D_t); + + dmulma(C_t,no_of_states,no_of_outputs,C,no_of_outputs,no_of_states,Q); + dmulma(D_t,no_of_inputs,no_of_outputs,D,no_of_outputs,no_of_inputs,R); + dmulma(D_t,no_of_inputs,no_of_outputs,C,no_of_outputs,no_of_states,S); + + /*Free up unwanted variables*/ + + free(C); + free(C_t); + free(D); + free(D_t); + + + /*Inverse of R*/ + Ri = (double*) malloc(no_of_inputs*no_of_inputs*sizeof(double)); + dinverma(R,Ri,no_of_inputs); + + BRi = (double*) malloc(no_of_states*no_of_inputs*sizeof(double)); + S_t = (double*) malloc(no_of_states*no_of_inputs*sizeof(double)); + StRi = (double*) malloc(no_of_states*no_of_inputs*sizeof(double)); + + dtransposea(S,no_of_inputs,no_of_states,S_t); + dmulma(B,no_of_states,no_of_inputs,Ri,no_of_inputs,no_of_inputs,BRi); + dmulma(S_t,no_of_states,no_of_inputs,Ri,no_of_inputs,no_of_inputs,StRi); + + buf1 = (double*) malloc(no_of_states*no_of_states*sizeof(double)); + buf2 = (double*) malloc(no_of_states*no_of_states*sizeof(double)); + + if(dom == 1) + { + /*Setup LA*/ + LA = (double*) malloc(4*no_of_states*no_of_states*sizeof(double)); + /*Block 11 --> A - B*Ri*S*/ + dmulma(BRi,no_of_states,no_of_inputs,S,no_of_inputs,no_of_states,buf1); + ddiffa(A,no_of_states*no_of_states,buf1,no_of_states*no_of_states,buf2); + for(col=0; col < no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + LA[col*2*no_of_states+row] = buf2[col*no_of_states + row]; + } + } + + /*Block 22= Block 11' --> -A' + S'*Ri*B'*/ + dtransposea(buf2,no_of_states,no_of_states,buf1); + for(col=no_of_states; col < 2*no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + LA[col*2*no_of_states+row] = \ + -1.0*buf1[(col-no_of_states)*no_of_states + (row-no_of_states)]; + } + } + + /*Block 12 --> -B*Ri*B'*/ + dmulma(BRi,no_of_states,no_of_inputs,B_t,no_of_inputs,no_of_states,buf1); + for(col=no_of_states; col < 2*no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + LA[col*2*no_of_states+row] = \ + -1.0*buf1[(col-no_of_states)*no_of_states + row]; + } + } + + /*Block 21 --> -Q + S'*Ri*S*/ + dmulma(StRi,no_of_states,no_of_inputs,S,no_of_inputs,no_of_states,buf1); + ddiffa(buf1,no_of_states*no_of_states,Q,no_of_states*no_of_states,buf2); + for(col=0; col < no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + LA[col*2*no_of_states+row] = \ + buf2[col*no_of_states + (row-no_of_states)]; + } + } + + + /*Freeup umwanted variables*/ + free(A); + free(Q); + free(R); + free(BRi); + free(S_t); + free(StRi); + free(B); + + /*Find schur decomposition of LA*/ + wsmall = (double*) malloc(4*no_of_states*no_of_states*sizeof(double)); + ks = dschura(LA,2*no_of_states,1,2,wsmall,NULL); + + X12 = (double*) malloc(no_of_states*no_of_states*sizeof(double)); + phi12 = (double*) malloc(no_of_states*no_of_states*sizeof(double)); + + for(col=0; col < no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + X12[col*no_of_states + row] = wsmall[col*2*no_of_states+row]; + } + } + + for(col=0; col < no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + phi12[col*no_of_states + (row-no_of_states)] = \ + wsmall[col*2*no_of_states+row]; + } + } + + drdivma(phi12,no_of_states,no_of_states,X12,no_of_states,no_of_states,X); + + buf3 = (double*) malloc(no_of_inputs*no_of_states*sizeof(double)); + buf4 = (double*) malloc(no_of_inputs*no_of_states*sizeof(double)); + + dmulma(B_t,no_of_inputs,no_of_states,X,no_of_states,no_of_states,buf3); + dadda(buf3,no_of_inputs*no_of_states,S,no_of_inputs*no_of_states,buf4); + dmulma(Ri,no_of_inputs,no_of_inputs,buf4,no_of_inputs,no_of_states,buf3); + + for(row = 0;row<no_of_inputs*no_of_states;row++) + { + K[row] = -buf3[row]; + } + + } + else if(dom == 2) + { + /*Setup LA and LE*/ + LA = (double*) malloc(4*no_of_states*no_of_states*sizeof(double)); + deyea(LA,2*no_of_states,2*no_of_states); + LE = (double*) malloc(4*no_of_states*no_of_states*sizeof(double)); + deyea(LE,2*no_of_states,2*no_of_states); + + /*Block 11 --> A - B*Ri*S*/ + dmulma(BRi,no_of_states,no_of_inputs,S,no_of_inputs,no_of_states,buf1); + ddiffa(A,no_of_states*no_of_states,buf1,no_of_states*no_of_states,buf2); + for(col=0; col < no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + LA[col*2*no_of_states+row] = buf2[col*no_of_states + row]; + } + } + + /*Block 22= Block 11' --> A' - S'*Ri*B'*/ + dtransposea(buf2,no_of_states,no_of_states,buf1); + for(col=no_of_states; col < 2*no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + LE[col*2*no_of_states+row] = \ + buf1[(col-no_of_states)*no_of_states + (row-no_of_states)]; + } + } + + /*Block 21 --> -Q + S'*Ri*S*/ + dmulma(StRi,no_of_states,no_of_inputs,S,no_of_inputs,no_of_states,buf1); + ddiffa(buf1,no_of_states*no_of_states,Q,no_of_states*no_of_states,buf2); + for(col=0; col < no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + LA[col*2*no_of_states+row] = \ + buf2[col*no_of_states + (row-no_of_states)]; + } + } + + + /*Block 12 --> B*Ri*B'*/ + dmulma(BRi,no_of_states,no_of_inputs,B_t,no_of_inputs,no_of_states,buf1); + for(col=no_of_states; col < 2*no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + LE[col*2*no_of_states+row] = \ + buf1[(col-no_of_states)*no_of_states + row]; + } + } + + + free(Q); + free(BRi); + free(S_t); + free(StRi); + + /*Find schur decomposition of LA*/ + wsmall = (double*) malloc(4*no_of_states*no_of_states*sizeof(double)); + ks = dgschura(LA,2*no_of_states,LE,2,2,wsmall,NULL,NULL,NULL); + + X12 = (double*) malloc(no_of_states*no_of_states*sizeof(double)); + phi12 = (double*) malloc(no_of_states*no_of_states*sizeof(double)); + + for(col=0; col < no_of_states; col++) + { + for(row = 0; row < no_of_states; row++) + { + X12[col*no_of_states + row] = wsmall[col*2*no_of_states+row]; + } + } + + for(col=0; col < no_of_states; col++) + { + for(row = no_of_states; row < 2*no_of_states; row++) + { + phi12[col*no_of_states + (row-no_of_states)] = \ + wsmall[col*2*no_of_states+row]; + } + } + + drdivma(phi12,no_of_states,no_of_states,X12,no_of_states,no_of_states,X); + + buf5 = (double*) malloc(no_of_inputs*no_of_inputs*sizeof(double)); + buf6 = (double*) malloc(no_of_inputs*no_of_inputs*sizeof(double)); + buf3 = (double*) malloc(no_of_inputs*no_of_states*sizeof(double)); + buf4 = (double*) malloc(no_of_inputs*no_of_states*sizeof(double)); + + /*inv(B'XB+R)*/ + dmulma(B_t,no_of_inputs,no_of_states,X,no_of_states,no_of_states,buf3); + dmulma(buf3,no_of_inputs,no_of_states,B_t,no_of_states,no_of_inputs,buf6); + dadda(buf6,no_of_inputs*no_of_inputs,R,no_of_inputs*no_of_inputs,buf5); + dinverma(buf5,buf6,no_of_inputs); + /*B'XA+S*/ + dmulma(B_t,no_of_inputs,no_of_states,X,no_of_states,no_of_states,buf3); + dmulma(buf3,no_of_inputs,no_of_states,A,no_of_states,no_of_states,buf4); + dadda(buf4,no_of_inputs*no_of_states,S,no_of_inputs*no_of_states,buf3); + + dmulma(buf6,no_of_inputs,no_of_inputs,buf3,no_of_inputs,no_of_states,buf4); + + for(row = 0;row<no_of_inputs*no_of_states;row++) + { + K[row] = -buf4[row]; + } + + free(A); + free(B); + free(R); + free(buf5); + free(buf6); + + } + + free(B_t); + free(S); + free(wsmall); + free(X12); + free(phi12); + free(buf1); + free(buf2); + free(buf3); + free(buf4); + +}
\ No newline at end of file diff --git a/src/c/CACSD/syslin/dsyslina.c b/src/c/CACSD/syslin/dsyslina.c index cd7fa6b..3c20c65 100644 --- a/src/c/CACSD/syslin/dsyslina.c +++ b/src/c/CACSD/syslin/dsyslina.c @@ -11,62 +11,70 @@ */ /*Function for declaring state space system*/ +/*output structure is as follows : + | A B X0 | + | C D dom| + where dom is 1 for 'c' and 2 for 'd' + Another column is appended with no_of_states and no_inputs as its elements */ + + #include <stdlib.h> -void dsyslina(double* A, int no_of_states, double* B, int no_of_inputs, \ +void dsyslina(char* dom, double* A, int no_of_states, double* B, int no_of_inputs, \ double* C, int no_of_outputs, double* D, double* X0, double* out) { int row = 0,col = 0; - int no_of_cols = no_of_states + no_of_inputs + 1; - + int no_of_cols = no_of_states + no_of_inputs + 2; + int no_of_rows = no_of_states + no_of_outputs; /*Copy matrix A into out matrix*/ - for(row = 0; row < no_of_states; row++) + for(col = 0; col < no_of_states; col++) { - for(col = 0; col < no_of_states; col++) + for(row = 0; row < no_of_states; row++) { - out[row*no_of_cols + col] = A[row*no_of_states + col]; + out[col*no_of_rows + row] = A[col*no_of_states + row]; } } /*Copy matrix B in out matrix*/ - for(row = 0; row < no_of_states; row++) + for(col=0; col < no_of_inputs; col++) { - for(col=0; col < no_of_inputs; col++) + for(row = 0; row < no_of_states; row++) { - out[row * no_of_cols + no_of_states + col] = B[row * no_of_inputs + col]; + out[col * no_of_rows + no_of_states*no_of_rows + row] \ + = B[col * no_of_states + row]; } } /*Copy matrix C in out matrix*/ - for(row = 0; row < no_of_outputs; row++) + for(col=0; col < no_of_states; col++) { - for(col=0; col < no_of_states; col++) + for(row = 0; row < no_of_outputs; row++) { - out[(no_of_states + row)*no_of_cols + col] = C[row * no_of_states + col]; + out[no_of_states + (col*no_of_rows) + row] = C[col * no_of_outputs + row]; } } /*Copy matrix D in out matrix*/ if( D != NULL) { - for(row = 0; row < no_of_outputs; row++) + for(col=0; col < no_of_inputs; col++) { - for(col=0; col < no_of_inputs; col++) + for(row = 0; row < no_of_outputs; row++) { - out[(no_of_states+row)*no_of_cols + no_of_states+col] = \ - D[row * no_of_inputs + col]; + out[(no_of_states+col)*no_of_rows + no_of_states + row] = \ + D[col * no_of_outputs + row]; } } } else { - for(row = 0; row < no_of_outputs; row++) + for(col=0; col < no_of_inputs; col++) { - for(col=0; col < no_of_inputs; col++) + for(row = 0; row < no_of_outputs; row++) { - out[(no_of_states+row)*no_of_cols + no_of_states+col] = 0; + out[(no_of_states+col)*no_of_rows + no_of_states + row] = 0; } } } @@ -76,19 +84,24 @@ void dsyslina(double* A, int no_of_states, double* B, int no_of_inputs, \ { for(row = 0; row < no_of_states; row++) { - out[(row+1)*(no_of_cols) - 1] = X0[row]; - + out[(no_of_rows*(no_of_cols-1))+row] = X0[row]; } } else { for(row = 0; row < no_of_states; row++) { - out[(row+1)*(no_of_cols) - 1] = 0; - + out[(no_of_rows*(no_of_cols-1))+row] = 0; } } + if(*dom == 'c') + out[(no_of_rows*(no_of_cols-2)) + no_of_states] = 1; + else if(*dom == 'd') + out[(no_of_rows*(no_of_cols-2)) + no_of_states] = 2; + /*Insert no of states and inputs in last column*/ + out[(no_of_rows*(no_of_cols-1))] = no_of_states; + out[(no_of_rows*(no_of_cols-1))+1] = no_of_inputs; }
\ No newline at end of file diff --git a/src/c/linearAlgebra/schur/dgschura.c b/src/c/linearAlgebra/schur/dgschura.c index 56d4845..f17da8c 100644 --- a/src/c/linearAlgebra/schur/dgschura.c +++ b/src/c/linearAlgebra/schur/dgschura.c @@ -15,14 +15,15 @@ #include "lapack.h" #include "stdlib.h" #include "string.h" - +#include <math.h> /*flag --> 0: nothing --> 1: continuous --> 2: discrete */ -lapack_logical selctg2( double* in1, double* in2, double* in3); +lapack_logical selctg21( double* in1, double* in2, double* in3); +lapack_logical selctg22( double* in1, double* in2, double* in3); double dgschura(double* in1, int size, double* in2, int flag, int nout, \ double* out1, double* out2, double* out3, double* out4) @@ -40,7 +41,7 @@ double dgschura(double* in1, int size, double* in2, int flag, int nout, \ modifies the input variable*/ /*Used incase of flag > 0*/ - LAPACK_D_SELECT3 selctg = &selctg2; + LAPACK_D_SELECT3 selctg = &selctg21; if(nout >= 2){ JOBVSL = 'V'; @@ -143,7 +144,15 @@ double dgschura(double* in1, int size, double* in2, int flag, int nout, \ return ret; } -lapack_logical selctg2(double* in1, double* in2, double* in3) +lapack_logical selctg21(double* in1, double* in2, double* in3) +{ + if((sqrt(*in1**in1+*in2**in2)/ *in3) < 1) + return 1; + else + return 0; +} + +lapack_logical selctg22(double* in1, double* in2, double* in3) { if((sqrt(*in1**in1+*in2**in2)/ *in3) < 1) return 1; diff --git a/src/c/linearAlgebra/schur/dschura.c b/src/c/linearAlgebra/schur/dschura.c index c31ddca..79c4197 100644 --- a/src/c/linearAlgebra/schur/dschura.c +++ b/src/c/linearAlgebra/schur/dschura.c @@ -15,14 +15,15 @@ #include "lapack.h" #include "stdlib.h" #include "string.h" - +#include <math.h> /*flag --> 0: nothing --> 1: continuous --> 2: discrete */ -lapack_logical selctg1( double* in1, double* in2); +lapack_logical selctg11( double* in1, double* in2); +lapack_logical selctg12( double* in1, double* in2); double dschura(double* in1, int size, int flag, int nout, double* out1, \ double* out2) @@ -38,9 +39,15 @@ double dschura(double* in1, int size, int flag, int nout, double* out1, \ double *buf; /*input is copied to buf, since lapack function direclty modifies the input variable*/ - /*Used incase of flag > 0*/ - LAPACK_D_SELECT2 selctg = &selctg1; + /*Used incase of flag > 0*/ + LAPACK_D_SELECT2 selctg; + + if(flag == 1 || flag == 0) + selctg = &selctg11; + else if(flag == 2) + selctg = &selctg12; + if(nout >= 2) JOBVS = 'V'; if(flag > 0) SORT = 'S'; @@ -101,7 +108,15 @@ double dschura(double* in1, int size, int flag, int nout, double* out1, \ return ret; } -lapack_logical selctg1(double* in1, double* in2) +lapack_logical selctg11(double* in1, double* in2) +{ + if(*in1 <= 0) + return 1; + else + return 0; +} + +lapack_logical selctg12(double* in1, double* in2) { if(sqrt(*in1**in1+*in2**in2) < 1) |