1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
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);
}
|
