summaryrefslogtreecommitdiff
path: root/src/c/CACSD/lqr
diff options
context:
space:
mode:
Diffstat (limited to 'src/c/CACSD/lqr')
-rw-r--r--src/c/CACSD/lqr/as_per_sci_code.c144
-rw-r--r--src/c/CACSD/lqr/dlqra.c352
2 files changed, 496 insertions, 0 deletions
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
generated by cgit (git 2.34.1) at 2024-12-20 17:53:45 +0000