diff options
Diffstat (limited to '2.3-1/src/c/CACSD/lqr/dlqra.c')
| -rw-r--r-- | 2.3-1/src/c/CACSD/lqr/dlqra.c | 55 |
1 files changed, 55 insertions, 0 deletions
diff --git a/2.3-1/src/c/CACSD/lqr/dlqra.c b/2.3-1/src/c/CACSD/lqr/dlqra.c index 02ac1504..15120f7d 100644 --- a/2.3-1/src/c/CACSD/lqr/dlqra.c +++ b/2.3-1/src/c/CACSD/lqr/dlqra.c @@ -22,7 +22,12 @@ #include "schur.h" #include "matrixDivision.h" +<<<<<<< HEAD +void dlqra(double* sys, int sys_rows, int sys_cols, double* Q, double* R, \ + double* S, double* K, double* X) +======= void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K) +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 { int no_of_states, no_of_inputs, no_of_outputs, dom = 1; int row,col; @@ -32,7 +37,11 @@ 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; +<<<<<<< HEAD + //double *Q, *R, *S; +======= double *Q, *R, *S; +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 double *Ri, *LA, *LE; double *BRi, *StRi, *S_t; double *buf1, *buf2, *buf3, *buf4, *buf5, *buf6; @@ -90,20 +99,45 @@ 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]; +<<<<<<< HEAD +======= 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)); +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 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); +<<<<<<< HEAD + 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); + } + + 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*/ +======= 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*/ +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 free(C); free(C_t); free(D); @@ -177,8 +211,13 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K) /*Freeup umwanted variables*/ free(A); +<<<<<<< HEAD + //free(Q); + //free(R); +======= free(Q); free(R); +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 free(BRi); free(S_t); free(StRi); @@ -278,7 +317,11 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K) } +<<<<<<< HEAD + //free(Q); +======= free(Q); +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 free(BRi); free(S_t); free(StRi); @@ -316,7 +359,11 @@ 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); +<<<<<<< HEAD + dmulma(buf3,no_of_inputs,no_of_states,B,no_of_states,no_of_inputs,buf6); +======= dmulma(buf3,no_of_inputs,no_of_states,B_t,no_of_states,no_of_inputs,buf6); +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 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 +380,22 @@ void dlqra(double* sys, int sys_rows, int sys_cols, double* X, double* K) free(A); free(B); +<<<<<<< HEAD + //free(R); +======= free(R); +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 free(buf5); free(buf6); } free(B_t); +<<<<<<< HEAD + //free(S); +======= free(S); +>>>>>>> 9e5793a7b05b23e6044a6d7a9ddd5db39ba375f0 free(wsmall); free(X12); free(phi12); |