Support for 'lqr' and 'lqe' added

10 files changed, 712 insertions, 42 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/syslin.h b/2.3-1/src/c/CACSD/includes/syslin.h
index 7d361c97..21b23294 100644
--- a/2.3-1/src/c/CACSD/includes/syslin.h
+++ b/2.3-1/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/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_syslin.h b/2.3-1/src/c/CACSD/interfaces/int_syslin.h
index 2327c383..3f74ea6e 100644
--- a/2.3-1/src/c/CACSD/interfaces/int_syslin.h
+++ b/2.3-1/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/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..bc729fb9
--- /dev/null
+++ b/2.3-1/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/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..26a5e6c4
--- /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 = 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/2.3-1/src/c/CACSD/syslin/dsyslina.c b/2.3-1/src/c/CACSD/syslin/dsyslina.c
index cd7fa6bc..3c20c65c 100644
--- a/2.3-1/src/c/CACSD/syslin/dsyslina.c
+++ b/2.3-1/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