diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/c/CACSD/includes/lqe.h | 26 | ||||
| -rw-r--r-- | src/c/CACSD/includes/lqr.h | 26 | ||||
| -rw-r--r-- | src/c/CACSD/includes/syslin.h | 2 | ||||
| -rw-r--r-- | src/c/CACSD/interfaces/int_lqe.h | 25 | ||||
| -rw-r--r-- | src/c/CACSD/interfaces/int_lqr.h | 25 | ||||
| -rw-r--r-- | src/c/CACSD/interfaces/int_syslin.h | 36 | ||||
| -rw-r--r-- | src/c/CACSD/lqe/dlqea.c | 59 | ||||
| -rw-r--r-- | src/c/CACSD/lqr/as_per_sci_code.c | 144 | ||||
| -rw-r--r-- | src/c/CACSD/lqr/dlqra.c | 352 | ||||
| -rw-r--r-- | src/c/CACSD/syslin/dsyslina.c | 59 | ||||
| -rw-r--r-- | src/c/linearAlgebra/schur/dgschura.c | 17 | ||||
| -rw-r--r-- | src/c/linearAlgebra/schur/dschura.c | 25 |
12 files changed, 745 insertions, 51 deletions
diff --git a/src/c/CACSD/includes/lqe.h b/src/c/CACSD/includes/lqe.h new file mode 100644 index 0000000..1cc902f --- /dev/null +++ b/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/src/c/CACSD/includes/lqr.h b/src/c/CACSD/includes/lqr.h new file mode 100644 index 0000000..062d6e4 --- /dev/null +++ b/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/src/c/CACSD/includes/syslin.h b/src/c/CACSD/includes/syslin.h index 7d361c9..21b2329 100644 --- a/src/c/CACSD/includes/syslin.h +++ b/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/src/c/CACSD/interfaces/int_lqe.h b/src/c/CACSD/interfaces/int_lqe.h new file mode 100644 index 0000000..d801bf3 --- /dev/null +++ b/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/src/c/CACSD/interfaces/int_lqr.h b/src/c/CACSD/interfaces/int_lqr.h new file mode 100644 index 0000000..e6f534f --- /dev/null +++ b/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/src/c/CACSD/interfaces/int_syslin.h b/src/c/CACSD/interfaces/int_syslin.h index 2327c38..3f74ea6 100644 --- a/src/c/CACSD/interfaces/int_syslin.h +++ b/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/src/c/CACSD/lqe/dlqea.c b/src/c/CACSD/lqe/dlqea.c new file mode 100644 index 0000000..bc729fb --- /dev/null +++ b/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/src/c/CACSD/lqr/as_per_sci_code.c b/src/c/CACSD/lqr/as_per_sci_code.c new file mode 100644 index 0000000..01f002c --- /dev/null +++ b/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/src/c/CACSD/lqr/dlqra.c b/src/c/CACSD/lqr/dlqra.c new file mode 100644 index 0000000..26a5e6c --- /dev/null +++ b/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/src/c/CACSD/syslin/dsyslina.c b/src/c/CACSD/syslin/dsyslina.c index cd7fa6b..3c20c65 100644 --- a/src/c/CACSD/syslin/dsyslina.c +++ b/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 diff --git a/src/c/linearAlgebra/schur/dgschura.c b/src/c/linearAlgebra/schur/dgschura.c index 56d4845..f17da8c 100644 --- a/src/c/linearAlgebra/schur/dgschura.c +++ b/src/c/linearAlgebra/schur/dgschura.c @@ -15,14 +15,15 @@ #include "lapack.h" #include "stdlib.h" #include "string.h" - +#include <math.h> /*flag --> 0: nothing --> 1: continuous --> 2: discrete */ -lapack_logical selctg2( double* in1, double* in2, double* in3); +lapack_logical selctg21( double* in1, double* in2, double* in3); +lapack_logical selctg22( double* in1, double* in2, double* in3); double dgschura(double* in1, int size, double* in2, int flag, int nout, \ double* out1, double* out2, double* out3, double* out4) @@ -40,7 +41,7 @@ double dgschura(double* in1, int size, double* in2, int flag, int nout, \ modifies the input variable*/ /*Used incase of flag > 0*/ - LAPACK_D_SELECT3 selctg = &selctg2; + LAPACK_D_SELECT3 selctg = &selctg21; if(nout >= 2){ JOBVSL = 'V'; @@ -143,7 +144,15 @@ double dgschura(double* in1, int size, double* in2, int flag, int nout, \ return ret; } -lapack_logical selctg2(double* in1, double* in2, double* in3) +lapack_logical selctg21(double* in1, double* in2, double* in3) +{ + if((sqrt(*in1**in1+*in2**in2)/ *in3) < 1) + return 1; + else + return 0; +} + +lapack_logical selctg22(double* in1, double* in2, double* in3) { if((sqrt(*in1**in1+*in2**in2)/ *in3) < 1) return 1; diff --git a/src/c/linearAlgebra/schur/dschura.c b/src/c/linearAlgebra/schur/dschura.c index c31ddca..79c4197 100644 --- a/src/c/linearAlgebra/schur/dschura.c +++ b/src/c/linearAlgebra/schur/dschura.c @@ -15,14 +15,15 @@ #include "lapack.h" #include "stdlib.h" #include "string.h" - +#include <math.h> /*flag --> 0: nothing --> 1: continuous --> 2: discrete */ -lapack_logical selctg1( double* in1, double* in2); +lapack_logical selctg11( double* in1, double* in2); +lapack_logical selctg12( double* in1, double* in2); double dschura(double* in1, int size, int flag, int nout, double* out1, \ double* out2) @@ -38,9 +39,15 @@ double dschura(double* in1, int size, int flag, int nout, double* out1, \ double *buf; /*input is copied to buf, since lapack function direclty modifies the input variable*/ - /*Used incase of flag > 0*/ - LAPACK_D_SELECT2 selctg = &selctg1; + /*Used incase of flag > 0*/ + LAPACK_D_SELECT2 selctg; + + if(flag == 1 || flag == 0) + selctg = &selctg11; + else if(flag == 2) + selctg = &selctg12; + if(nout >= 2) JOBVS = 'V'; if(flag > 0) SORT = 'S'; @@ -101,7 +108,15 @@ double dschura(double* in1, int size, int flag, int nout, double* out1, \ return ret; } -lapack_logical selctg1(double* in1, double* in2) +lapack_logical selctg11(double* in1, double* in2) +{ + if(*in1 <= 0) + return 1; + else + return 0; +} + +lapack_logical selctg12(double* in1, double* in2) { if(sqrt(*in1**in1+*in2**in2) < 1) |