diff options
Diffstat (limited to '2.3-1/src/c/CACSD/lqr/as_per_sci_code.c')
| -rw-r--r-- | 2.3-1/src/c/CACSD/lqr/as_per_sci_code.c | 144 |
1 files changed, 144 insertions, 0 deletions
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 |