sci2c arduino updated

13 files changed, 1215 insertions, 0 deletions

diff --git a/2.3-1/src/c/CACSD/includes/lqe.h b/2.3-1/src/c/CACSD/includes/lqe.h
new file mode 100644
index 00000000..1cc902fd
--- /dev/null
+++ b/2.3-1/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/2.3-1/src/c/CACSD/includes/lqr.h b/2.3-1/src/c/CACSD/includes/lqr.h
new file mode 100644
index 00000000..062d6e41
--- /dev/null
+++ b/2.3-1/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/2.3-1/src/c/CACSD/includes/obscont.h b/2.3-1/src/c/CACSD/includes/obscont.h
new file mode 100644
index 00000000..f316adb5
--- /dev/null
+++ b/2.3-1/src/c/CACSD/includes/obscont.h
@@ -0,0 +1,28 @@
+ /* 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 __OBSCONT_H__
+#define __OBSCONT_H__
+
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+void dobsconta(double* sys, int sys_rows, int sys_cols, double* Kc, double* Kf, \
+			double *K, double *r);
+
+
+#ifdef  __cplusplus 
+} /* extern "C" */
+#endif
+
+#endif /*__OBSCONT_H__*/
diff --git a/2.3-1/src/c/CACSD/includes/syslin.h b/2.3-1/src/c/CACSD/includes/syslin.h
new file mode 100644
index 00000000..21b23294
--- /dev/null
+++ b/2.3-1/src/c/CACSD/includes/syslin.h
@@ -0,0 +1,31 @@
+/* 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 __SYSLIN_H__
+#define __SYSLIN_H__
+
+#include <stdlib.h>
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+
+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);
+
+
+#ifdef  __cplusplus
+} /* extern "C" */
+#endif
+
+#endif /*__SYSLIN_H__*/
\ No newline at end of file
diff --git a/2.3-1/src/c/CACSD/interfaces/int_lqe.h b/2.3-1/src/c/CACSD/interfaces/int_lqe.h
new file mode 100644
index 00000000..d801bf3e
--- /dev/null
+++ b/2.3-1/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/2.3-1/src/c/CACSD/interfaces/int_lqr.h b/2.3-1/src/c/CACSD/interfaces/int_lqr.h
new file mode 100644
index 00000000..e6f534fc
--- /dev/null
+++ b/2.3-1/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/2.3-1/src/c/CACSD/interfaces/int_obscont.h b/2.3-1/src/c/CACSD/interfaces/int_obscont.h
new file mode 100644
index 00000000..fd3a823c
--- /dev/null
+++ b/2.3-1/src/c/CACSD/interfaces/int_obscont.h
@@ -0,0 +1,29 @@
+ /* 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_OBSCONT_H__
+#define __INT_OBSCONT_H__
+
+ #ifdef  __cplusplus
+ extern "C" {
+ #endif
+
+#define ss2d2d2obscontd2(in1,size1,in2,size2,in3,size3,out) \
+ 	dobsconta(in1,size1[0],size1[1],in2,in3,out,NULL)
+
+#define ss2d2d2obscontd2d2(in1,size1,in2,size2,in3,size3,out1,out2) \
+ 	dobsconta(in1,size1[0],size1[1],in2,in3,out1,out2)
+
+ #ifdef  __cplusplus
+ } /* extern "C" */
+ #endif
+
+#endif /*__INT_OBSCONT_H__*/
diff --git a/2.3-1/src/c/CACSD/interfaces/int_syslin.h b/2.3-1/src/c/CACSD/interfaces/int_syslin.h
new file mode 100644
index 00000000..3f74ea6e
--- /dev/null
+++ b/2.3-1/src/c/CACSD/interfaces/int_syslin.h
@@ -0,0 +1,78 @@
+/* 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_SYSLIN_H__
+#define __INT_SYSLIN_H__
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+#define g2d2d2d2syslinss2(in1,size1,in2,size2,in3,size3,in4,size4,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(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(in1,in2,size2[0],in3,size3[1],in4, \
+ 	size4[0],in5,in6,out)
+//
+#define g2d0d0d0syslinss2(in1,size1,in2,in3,in4,out) \
+	dsyslina(in1,&in2,1,&in3,1,&in4,1,NULL,NULL,out)
+
+#define g2d0d2d0syslinss2(in1,size1,in2,in3,size3,in4,out) \
+	dsyslina(in1,&in2,1,in3,size3[1],&in4,1,NULL,NULL,out)
+
+#define g2d0d0d2syslinss2(in1,size1,in2,in3,in4,size4,out) \
+	dsyslina(in1,&in2,1,&in3,1,in4,size4[0],NULL,NULL,out)
+
+#define g2d0d2d2syslinss2(in1,size1,in2,in3,size3,in4,size4,out) \
+	dsyslina(in1,&in2,1,in3,size3[1],in4,size4[0],NULL,NULL,out)
+//
+#define g2d0d0d0d0syslinss2(in1,size1,in2,in3,in4,in5,out) \
+	dsyslina(in1,&in2,1,&in3,1,&in4,1,&in5,NULL,out)
+
+#define g2d0d2d0d2syslinss2(in1,size1,in2,in3,size3,in4,in5,size5, \
+	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(in1,&in2,1,&in3,1,in4,size4[0],in5,NULL,out)
+
+#define g2d0d2d2d2syslinss2(in1,size1,in2,in3,size3,in4,size4,in5, \
+	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(in1,in2,size2[0],in3,size3[1],in4,size4[0],&in5,NULL,out)
+//
+#define g2d0d0d0d0d0syslinss2(in1,size1,in2,in3,in4,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(in1,&in2,1,in3,size3[1],in4,1,in5,&in6,out)
+
+#define g2d0d0d2d2d0syslinss2(in1,size1,in2,in3,in4,size4,in5,size5, \
+	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(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(in1,in2,size2[0],in3,size3[1],in4,size4[0], \
+	&in5,in6,out)
+
+#ifdef  __cplusplus
+} /* extern "C" */
+#endif
+
+
+#endif /*__INT_SYSLIN_H__*/
\ No newline at end of file
diff --git a/2.3-1/src/c/CACSD/lqe/dlqea.c b/2.3-1/src/c/CACSD/lqe/dlqea.c
new file mode 100644
index 00000000..63f8d62d
--- /dev/null
+++ b/2.3-1/src/c/CACSD/lqe/dlqea.c
@@ -0,0 +1,60 @@
+/* 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 <stdlib.h>
+#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 = (int)sys[sys_rows*(sys_cols-1)+1];
+	no_of_st = (int)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/2.3-1/src/c/CACSD/lqr/as_per_sci_code.c b/2.3-1/src/c/CACSD/lqr/as_per_sci_code.c
new file mode 100644
index 00000000..01f002c3
--- /dev/null
+++ b/2.3-1/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/2.3-1/src/c/CACSD/lqr/dlqra.c b/2.3-1/src/c/CACSD/lqr/dlqra.c
new file mode 100644
index 00000000..02ac1504
--- /dev/null
+++ b/2.3-1/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 = (int)sys[sys_rows*(sys_cols-1)];
+    no_of_inputs = (int)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 = (int)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 = (int)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 = (int)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/2.3-1/src/c/CACSD/obscont/dobsconta.c b/2.3-1/src/c/CACSD/obscont/dobsconta.c
new file mode 100644
index 00000000..c67b70e2
--- /dev/null
+++ b/2.3-1/src/c/CACSD/obscont/dobsconta.c
@@ -0,0 +1,284 @@
+/* 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 observer controller. 
+  Refer 'obscont.sci' in scilab source.*/
+
+#include <stdlib.h>
+#include "matrixMultiplication.h"
+#include "addition.h"
+
+void dobsconta(double* sys, int sys_rows, int sys_cols, double* Kc, double* Kf, \
+			double *K, double *r)
+{
+	int no_of_states, no_of_inputs, no_of_outputs, dom = 1;
+    int row,col;
+    no_of_states = (int)sys[sys_rows*(sys_cols-1)];
+    no_of_inputs = (int)sys[sys_rows*(sys_cols-1) + 1];
+    no_of_outputs = sys_rows - no_of_states;
+
+ 	int no_of_cols;
+    int no_of_rows;
+   
+    double *A, *B, *C, *D;
+    double *BKc, *KfC, *KfD,*DKc;
+    double *buf, *buf1, *buf2;
+
+    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));
+
+	BKc = (double*) malloc (no_of_states*no_of_states*sizeof(double));
+	KfC = (double*) malloc (no_of_states*no_of_states*sizeof(double));
+	KfD = (double*) malloc (no_of_states*no_of_inputs*sizeof(double));
+	DKc = (double*) malloc (no_of_outputs*no_of_states*sizeof(double));
+	buf = (double*) malloc (no_of_states*no_of_states*sizeof(double));
+	buf1 = (double*) malloc (no_of_states*no_of_inputs*sizeof(double));
+	buf2 = (double*) malloc (no_of_outputs*no_of_states*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 = (int)sys[(sys_rows*(sys_cols-2)) + no_of_states];
+
+    dmulma(B,no_of_states,no_of_inputs,Kc,no_of_inputs,no_of_states,BKc);
+    dmulma(Kf,no_of_states,no_of_outputs,C,no_of_outputs,no_of_states,KfC);
+    dmulma(Kf,no_of_states,no_of_outputs,D,no_of_outputs,no_of_inputs,KfD);
+
+    /*Variables are reused to save memory*/
+    /*A+BKc*/
+    dadda(A,no_of_states*no_of_states,BKc,no_of_states*no_of_states,buf);
+    /*A+BKc+KfC*/
+    dadda(KfC,no_of_states*no_of_states,buf,no_of_states*no_of_states,A);
+    /*KfDKc*/
+    dmulma(KfD,no_of_states,no_of_inputs,Kc,no_of_inputs,no_of_states,BKc);
+    /*A+BKc+KfC+KfDKc*/
+    dadda(A,no_of_states*no_of_states,BKc,no_of_states*no_of_states,buf);
+   
+    /*Dimensions of K*/
+    no_of_cols = no_of_states + no_of_inputs + 2;
+    no_of_rows = no_of_states + no_of_outputs;
+
+    if(r == NULL)
+    {
+  	    /*Dimensions of K*/
+	    no_of_cols = no_of_states + no_of_outputs + 2;
+	    no_of_rows = no_of_states + no_of_inputs;
+
+    	for(col = 0; col < no_of_states; col++)
+	    {
+	    	for(row = 0; row < no_of_states; row++)
+	    	{
+	    		K[col*no_of_rows + row] = buf[col*no_of_states + row];
+	    	}
+	    	
+	    }
+
+	    /*Copy matrix -Kf in out matrix*/
+	    for(col=0; col < no_of_outputs; col++)
+	    {
+	    	for(row = 0; row < no_of_states; row++)
+	    	{
+	    		K[col * no_of_rows + no_of_states*no_of_rows + row]  \
+	    				= -1.0*Kf[col * no_of_states + row];
+	    	}
+	    }
+
+	    /*Copy matrix Kc in out matrix*/
+	    for(col=0; col < no_of_states; col++)
+	    {
+	    	for(row = 0; row < no_of_inputs; row++)
+	    	{
+	    		K[no_of_states + (col*no_of_rows) + row] = Kc[col * no_of_inputs + row];
+	    	}
+	    }
+
+	    K[(no_of_rows*(no_of_cols-2))	+ no_of_states] = dom; 
+    	/*Insert no of states and inputs in last column*/
+		K[(no_of_rows*(no_of_cols-1))] = no_of_states;
+		K[(no_of_rows*(no_of_cols-1))+1] = no_of_outputs;
+		/*For K, no_of_outputs is no of inputs*/ 
+
+    }
+    else
+    {
+    	/*Dimensions of K*/
+	    no_of_cols = no_of_states + no_of_inputs + no_of_outputs + 2;
+    	no_of_rows = no_of_states + no_of_outputs + no_of_inputs;
+
+    	r[0] = no_of_outputs;
+    	r[1] = no_of_inputs;
+
+    	for(col = 0; col < no_of_states; col++)
+	    {
+	    	for(row = 0; row < no_of_states; row++)
+	    	{
+	    		K[col*no_of_rows + row] = buf[col*no_of_states + row];
+	    	}
+	    	
+	    }
+
+	    /*Copy matrix -Kf in out matrix*/
+	    for(col=0; col < no_of_outputs; col++)
+	    {
+	    	for(row = 0; row < no_of_states; row++)
+	    	{
+	    		K[col * no_of_rows + no_of_states*no_of_rows + row]  \
+	    				= -1.0*Kf[col * no_of_states + row];
+	    	}
+	    }
+
+	    /*B+Kf*D*/
+	    dadda(B,no_of_states*no_of_inputs,KfD,no_of_states*no_of_inputs,buf1);
+
+	    /*Copy matrix (B+KfD) in out matrix*/
+	    for(col=0; col < no_of_inputs; col++)
+	    {
+	    	for(row = 0; row < no_of_states; row++)
+	    	{
+	    		K[col * no_of_rows + (no_of_states+no_of_outputs)*no_of_rows \
+	    			 + row] = buf1[col * no_of_states + row];
+	    	}
+	    }
+
+	    /*DKc*/
+	    dmulma(D,no_of_outputs,no_of_inputs,Kc,no_of_inputs,no_of_states,DKc);
+	    /*C+DKc*/
+	    dadda(C,no_of_outputs*no_of_states,DKc,no_of_outputs*no_of_states,buf2);
+
+	    /*Copy matrix Kc in out matrix*/
+	    for(col=0; col < no_of_states; col++)
+	    {
+	    	for(row = 0; row < no_of_inputs; row++)
+	    	{
+	    		K[no_of_states + (col*no_of_rows) + row] \
+	    				= Kc[col * no_of_inputs + row];
+	    	}
+	    }
+
+	    /*Copy -(C+DKc) in out matrix*/
+		for(col=0; col < no_of_states; col++)
+	    {
+	    	for(row = 0; row < no_of_outputs; row++)
+	    	{
+	    		K[(no_of_states+no_of_inputs) + (col*no_of_rows) + row] \
+	    				= -1.0*buf2[col * no_of_outputs + row];
+	    	}
+	    }
+
+	    /*Block 22 for K*/
+		for(col=0; col < no_of_outputs; col++)
+	    {
+	    	for(row = 0; row < no_of_inputs; row++)
+	    	{
+	    		K[no_of_states + (col*no_of_rows) + no_of_states*no_of_rows \
+	    			 + row] = 0;
+	    	}
+	    }	    
+
+	    for(col=0; col < no_of_inputs; col++)
+	    {
+	    	for(row = 0; row < no_of_inputs; row++)
+	    	{
+	    		if(row == col)
+	    			K[no_of_states + (col*no_of_rows) + \
+	    				(no_of_states+no_of_outputs)*no_of_rows + row] = 1;
+	    		else
+	    			K[no_of_states + (col*no_of_rows) + \
+	    				(no_of_states+no_of_outputs)*no_of_rows + row] = 0;
+	    	}
+	    }	    
+
+		for(col=0; col < no_of_outputs; col++)
+	    {
+	    	for(row = 0; row < no_of_outputs; row++)
+	    	{
+	    		if(row == col)
+	    			K[no_of_states + no_of_inputs + (col*no_of_rows) + \
+	    				no_of_states*no_of_rows + row] = 1;
+	    		else
+	    			K[no_of_states + no_of_inputs + (col*no_of_rows) + \
+	    				no_of_states*no_of_rows + row] = 0;
+	    	}
+	    }	    
+
+
+	    for(col=0; col < no_of_inputs; col++)
+	    {
+	    	for(row = 0; row < no_of_outputs; row++)
+	    	{
+	    		K[no_of_states + no_of_inputs + (col*no_of_rows) + \
+	    				(no_of_states+no_of_outputs)*no_of_rows + row] \
+	    				= -1.0*D[col*no_of_inputs + row];
+	    	}
+	    }	    
+
+
+    	K[(no_of_rows*(no_of_cols-2))+ no_of_states] = dom; 
+    	/*Insert no of states and inputs in last column*/
+		K[(no_of_rows*(no_of_cols-1))] = no_of_states;
+		K[(no_of_rows*(no_of_cols-1))+1] = no_of_outputs + no_of_inputs;
+		/*For K, (no_of_outputs+no_of_inputs) is no of inputs*/ 
+
+
+    }
+
+
+    free(A);
+    free(B);
+    free(C);
+    free(D);
+    free(BKc);
+    free(KfC);
+    free(KfD);
+    free(DKc);
+    free(buf);
+    free(buf1);
+    free(buf2);
+}
diff --git a/2.3-1/src/c/CACSD/syslin/dsyslina.c b/2.3-1/src/c/CACSD/syslin/dsyslina.c
new file mode 100644
index 00000000..3c20c65c
--- /dev/null
+++ b/2.3-1/src/c/CACSD/syslin/dsyslina.c
@@ -0,0 +1,107 @@
+/* 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 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(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 + 2;
+    int no_of_rows = no_of_states + no_of_outputs;
+    /*Copy matrix A into out matrix*/
+    for(col = 0; col < no_of_states; col++)
+    {
+    	for(row = 0; row < no_of_states; row++)
+    	{
+    		out[col*no_of_rows + row] = A[col*no_of_states + row];
+    	}
+    	
+    }
+
+    /*Copy matrix B in out matrix*/
+    for(col=0; col < no_of_inputs; col++)
+    {
+    	for(row = 0; row < no_of_states; row++)
+    	{
+    		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(col=0; col < no_of_states; col++)
+    {
+    	for(row = 0; row < no_of_outputs; row++)
+    	{
+    		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(col=0; col < no_of_inputs; col++)
+		{
+	    	for(row = 0; row < no_of_outputs; row++)
+	    	{
+	    		out[(no_of_states+col)*no_of_rows + no_of_states +  row] = \
+	    			D[col * no_of_outputs + row];
+	    	}
+	    }
+	}
+	else
+	{
+	    for(col=0; col < no_of_inputs; col++)
+		{
+	    	for(row = 0; row < no_of_outputs; row++)
+	    	{
+	    		out[(no_of_states+col)*no_of_rows + no_of_states +  row] = 0;
+	    	}
+	    }
+	}	
+
+	/*Copy matrix X0 in out matrix*/
+	if( X0 != NULL)
+	{
+	    for(row = 0; row < no_of_states; row++)
+		{
+	    	out[(no_of_rows*(no_of_cols-1))+row] = X0[row];
+	    }
+	}
+	else
+	{
+	    for(row = 0; row < no_of_states; row++)
+		{
+	    	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