From e7dba33a909e158dffc6ddb3361df10d427db631 Mon Sep 17 00:00:00 2001 From: siddhu8990 Date: Tue, 7 Feb 2017 16:16:31 +0530 Subject: Support for 'lqr' and 'lqe' added --- src/c/CACSD/includes/lqe.h | 26 +++ src/c/CACSD/includes/lqr.h | 26 +++ src/c/CACSD/includes/syslin.h | 2 +- src/c/CACSD/interfaces/int_lqe.h | 25 +++ src/c/CACSD/interfaces/int_lqr.h | 25 +++ src/c/CACSD/interfaces/int_syslin.h | 36 ++-- src/c/CACSD/lqe/dlqea.c | 59 ++++++ src/c/CACSD/lqr/as_per_sci_code.c | 144 +++++++++++++++ src/c/CACSD/lqr/dlqra.c | 352 ++++++++++++++++++++++++++++++++++++ src/c/CACSD/syslin/dsyslina.c | 59 +++--- 10 files changed, 712 insertions(+), 42 deletions(-) create mode 100644 src/c/CACSD/includes/lqe.h create mode 100644 src/c/CACSD/includes/lqr.h create mode 100644 src/c/CACSD/interfaces/int_lqe.h create mode 100644 src/c/CACSD/interfaces/int_lqr.h create mode 100644 src/c/CACSD/lqe/dlqea.c create mode 100644 src/c/CACSD/lqr/as_per_sci_code.c create mode 100644 src/c/CACSD/lqr/dlqra.c (limited to 'src/c/CACSD') 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 +#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 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 -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 -- cgit