Bug fixed for GSL library

4 files changed, 36 insertions, 19 deletions

diff --git a/2.3-1/src/c/CACSD/includes/lqr.h b/2.3-1/src/c/CACSD/includes/lqr.h
index 062d6e41..5b20ee83 100644
--- a/2.3-1/src/c/CACSD/includes/lqr.h
+++ b/2.3-1/src/c/CACSD/includes/lqr.h
@@ -17,7 +17,7 @@
 extern "C" {
 #endif
 
-void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K);
+void dlqra(double* sys, int sys_rows, int sys_cols, double* Q, double* R, double* S, double* X, double* K);
 
 #ifdef  __cplusplus 
 } /* extern "C" */
diff --git a/2.3-1/src/c/CACSD/interfaces/int_lqr.h b/2.3-1/src/c/CACSD/interfaces/int_lqr.h
index e6f534fc..8deba90b 100644
--- a/2.3-1/src/c/CACSD/interfaces/int_lqr.h
+++ b/2.3-1/src/c/CACSD/interfaces/int_lqr.h
@@ -16,7 +16,14 @@
 extern "C" {
 #endif
 
-#define ss2lqrd2d2(in1,size1,out1,out2) dlqra(in1,size1[0],size1[1],out1,out2);
+#define ss2lqrd2d2(in1,size1,out1,out2) dlqra(in1,size1[0],size1[1],NULL,\
+				NULL,NULL,out1,out2);
+
+#define ss2d2d2lqrd2d2(in1,size1,in2,size2,in3,size3,out1,out2) dlqra(in1,\
+				size1[0],size1[1],in2,in3,NULL,out1,out2);
+
+#define ss2d2d2d2lqrd2d2(in1,size1,in2,size2,in3,size3,in4,size4,out1,out2) \
+				dlqra(in1,size1[0],size1[1],in2,in3,in4,out1,out2);
 
 #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
index 63f8d62d..e0be5b34 100644
--- a/2.3-1/src/c/CACSD/lqe/dlqea.c
+++ b/2.3-1/src/c/CACSD/lqe/dlqea.c
@@ -54,7 +54,7 @@ void dlqea(double* sys, int sys_rows, int sys_cols, double* X, double* K)
 
 	/*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);
+	dlqra(sys_lqr,sys_lqr_rows,sys_lqr_cols,NULL,NULL,NULL,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/dlqra.c b/2.3-1/src/c/CACSD/lqr/dlqra.c
index 02ac1504..1440c3a8 100644
--- a/2.3-1/src/c/CACSD/lqr/dlqra.c
+++ b/2.3-1/src/c/CACSD/lqr/dlqra.c
@@ -22,7 +22,8 @@
 #include "schur.h"
 #include "matrixDivision.h" 
 
-void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
+void dlqra(double* sys, int sys_rows, int sys_cols, double* Q, double* R, \
+            double* S, double* K, double* X)
 {
     int no_of_states, no_of_inputs, no_of_outputs, dom = 1;
     int row,col;
@@ -32,7 +33,7 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
 
     double *A, *B, *C, *D;
     double *B_t, *C_t, *D_t;
-    double *Q, *R, *S;
+    //double *Q, *R, *S;
     double *Ri, *LA, *LE;
     double *BRi, *StRi, *S_t;
     double *buf1, *buf2, *buf3, *buf4, *buf5, *buf6;
@@ -90,20 +91,29 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
 
     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);
+    if(Q == NULL)
+    {/*If Q is not provided*/
+        Q = (double*) malloc (no_of_states*no_of_states*sizeof(double));
+        dmulma(C_t,no_of_states,no_of_outputs,C,no_of_outputs,no_of_states,Q);
+    }
+    
+    if(R == NULL)
+    {/*If R is not provided*/
+        R = (double*) malloc (no_of_inputs*no_of_inputs*sizeof(double));
+        dmulma(D_t,no_of_inputs,no_of_outputs,D,no_of_outputs,no_of_inputs,R);
+    }
 
-    /*Free up unwanted variables*/
+    if(S == NULL)
+    {/*If Q is not provided*/    
+        S = (double*) malloc (no_of_inputs*no_of_states*sizeof(double));
+        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);
@@ -177,8 +187,8 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
 
         /*Freeup umwanted variables*/
         free(A);    
-        free(Q);
-        free(R);
+        //free(Q);
+        //free(R);
         free(BRi);
         free(S_t);
         free(StRi);
@@ -278,7 +288,7 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
         }
 
 
-        free(Q);
+        //free(Q);
         free(BRi);
         free(S_t);
         free(StRi);
@@ -316,7 +326,7 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
 
          /*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);
+        dmulma(buf3,no_of_inputs,no_of_states,B,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*/
@@ -333,14 +343,14 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K)
 
         free(A);
         free(B);
-        free(R);
+        //free(R);
         free(buf5);
         free(buf6);
         
     }
 
     free(B_t);
-    free(S);
+    //free(S);
     free(wsmall);
     free(X12);
     free(phi12);