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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
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);
|
