diff options
Diffstat (limited to '2.3-1/src/c/CACSD')
| -rw-r--r-- | 2.3-1/src/c/CACSD/includes/lqr.h | 2 | ||||
| -rw-r--r-- | 2.3-1/src/c/CACSD/interfaces/int_lqr.h | 9 | ||||
| -rw-r--r-- | 2.3-1/src/c/CACSD/lqe/dlqea.c | 2 | ||||
| -rw-r--r-- | 2.3-1/src/c/CACSD/lqr/dlqra.c | 42 |
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); |