Support for 'lqr' and 'lqe' added

1 files changed, 352 insertions, 0 deletions

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