blob: cb81b532829435959ef3024130587345b607107b (
plain) (
blame)
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
145
146
147
148
149
150
151
152
153
154
155
156
157
158
|
/*
* mrdivide.c
*
* Code generation for function 'mrdivide'
*
* C source code generated on: Wed Jul 11 08:38:35 2012
*
*/
/* Include files */
#include "rt_nonfinite.h"
#include "attitudeKalmanfilter.h"
#include "mrdivide.h"
/* Type Definitions */
/* Named Constants */
/* Variable Declarations */
/* Variable Definitions */
/* Function Declarations */
/* Function Definitions */
/*
*
*/
void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
{
int32_T jy;
int32_T iy;
real32_T b_A[81];
int8_T ipiv[9];
int32_T j;
int32_T mmj;
int32_T jj;
int32_T jp1j;
int32_T c;
int32_T ix;
real32_T temp;
int32_T k;
real32_T s;
int32_T loop_ub;
real32_T Y[108];
for (jy = 0; jy < 9; jy++) {
for (iy = 0; iy < 9; iy++) {
b_A[iy + 9 * jy] = B[jy + 9 * iy];
}
ipiv[jy] = (int8_T)(1 + jy);
}
for (j = 0; j < 8; j++) {
mmj = -j;
jj = j * 10;
jp1j = jj + 1;
c = mmj + 9;
jy = 0;
ix = jj;
temp = fabsf(b_A[jj]);
for (k = 2; k <= c; k++) {
ix++;
s = fabsf(b_A[ix]);
if (s > temp) {
jy = k - 1;
temp = s;
}
}
if ((real_T)b_A[jj + jy] != 0.0) {
if (jy != 0) {
ipiv[j] = (int8_T)((j + jy) + 1);
ix = j;
iy = j + jy;
for (k = 0; k < 9; k++) {
temp = b_A[ix];
b_A[ix] = b_A[iy];
b_A[iy] = temp;
ix += 9;
iy += 9;
}
}
loop_ub = (jp1j + mmj) + 8;
for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
b_A[iy] /= b_A[jj];
}
}
c = 8 - j;
jy = jj + 9;
for (iy = 1; iy <= c; iy++) {
if ((real_T)b_A[jy] != 0.0) {
temp = b_A[jy] * -1.0F;
ix = jp1j;
loop_ub = (mmj + jj) + 18;
for (k = 10 + jj; k + 1 <= loop_ub; k++) {
b_A[k] += b_A[ix] * temp;
ix++;
}
}
jy += 9;
jj += 9;
}
}
for (jy = 0; jy < 12; jy++) {
for (iy = 0; iy < 9; iy++) {
Y[iy + 9 * jy] = A[jy + 12 * iy];
}
}
for (iy = 0; iy < 9; iy++) {
if (ipiv[iy] != iy + 1) {
for (j = 0; j < 12; j++) {
temp = Y[iy + 9 * j];
Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
Y[(ipiv[iy] + 9 * j) - 1] = temp;
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 0; k < 9; k++) {
jy = 9 * k;
if ((real_T)Y[k + c] != 0.0) {
for (iy = k + 2; iy < 10; iy++) {
Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
}
}
}
}
for (j = 0; j < 12; j++) {
c = 9 * j;
for (k = 8; k > -1; k += -1) {
jy = 9 * k;
if ((real_T)Y[k + c] != 0.0) {
Y[k + c] /= b_A[k + jy];
for (iy = 0; iy + 1 <= k; iy++) {
Y[iy + c] -= Y[k + c] * b_A[iy + jy];
}
}
}
}
for (jy = 0; jy < 9; jy++) {
for (iy = 0; iy < 12; iy++) {
y[iy + 12 * jy] = Y[jy + 9 * iy];
}
}
}
/* End of code generation (mrdivide.c) */
|
