35 #define DEBUG_PRINT printf
36 #define DEBUG_MAT_PRINT MAT_PRINT
38 #define DEBUG_PRINT(...)
39 #define DEBUG_MAT_PRINT(...)
57 for (i = 0; i < n; i++) {
58 for (j = 0; j < (i + 1); j++) {
60 for (k = 0; k < j; k++) {
61 s += o[i][k] * o[j][k];
64 o[i][j] = sqrtf(in[i][i] -
s);
67 o[i][j] = 1.0 / o[j][j] * (in[i][j] -
s);
94 float _z[
m][n], _z1[
m][n], _z2[
m][
m];
98 for (i = 0; i <
m; i++)
for (k = 0; k <
m; k++) { q[i][k] = &_q[i][k][0]; }
100 for (k = 0; k < n && k <
m - 1; k++) {
101 float e[
m], x[
m], a,
b;
105 if (in[k][k] > 0) { a = -a; }
106 for (i = 0; i <
m; i++) {
107 e[i] = (i == k) ? 1 : 0;
108 e[i] = x[i] + a * e[i];
116 for (i = 1; i < n && i <
m - 1; i++) {
134 if (absa == 0) {
return 0.0; }
135 return (absa * sqrtf(1.0 + (absb / absa) * (absb / absa)));
136 }
else if (absb == 0.0) {
139 return (absb * sqrtf(1.0 + (absa / absb) * (absa / absb)));
144 static int imin(
int num1,
int num2) {
145 return (num1 > num2 ) ? num2 : num1;
170 int flag, i, its, j, jj, k, l, NM;
171 float C, F,
H, S,
X,
Y,
Z, tmp;
177 for (i = 0; i < n; ++i) {
184 for (k = i; k <
m; ++k) {
185 Scale = Scale + fabsf(a[k][i]);
188 for (k = i; k <
m; ++k) {
189 a[k][i] = a[k][i] / Scale;
190 S = S + a[k][i] * a[k][i];
200 for (j = l; j < n; ++j) {
202 for (k = i; k <
m; ++k) {
203 S = S + a[k][i] * a[k][j];
210 for (k = i; k <
m; ++k) {
211 a[k][j] = a[k][j] + F * a[k][i];
215 for (k = i; k <
m; ++k) {
216 a[k][i] = Scale * a[k][i];
225 if ((i <
m) && (i != (n - 1))) {
226 for (k = l; k < n; ++k) {
227 Scale = Scale + fabsf(a[i][k]);
230 for (k = l; k < n; ++k) {
231 a[i][k] = a[i][k] / Scale;
232 S = S + a[i][k] * a[i][k];
244 for (k = l; k < n; ++k) {
245 rv1[k] = a[i][k] /
H;
248 for (j = l; j <
m; ++j) {
250 for (k = l; k < n; ++k) {
251 S = S + a[j][k] * a[i][k];
253 for (k = l; k < n; ++k) {
254 a[j][k] = a[j][k] + S * rv1[k];
258 for (k = l; k < n; ++k) {
259 a[i][k] = Scale * a[i][k];
263 tmp = fabsf(w[i]) + fabsf(rv1[i]);
270 for (i = n - 1; i >= 0; --i) {
273 for (j = l; j < n; ++j) {
275 v[j][i] = (a[i][j] / a[i][l]) / G;
280 for (j = l; j < n; ++j) {
282 for (k = l; k < n; ++k) {
283 S = S + a[i][k] * v[k][j];
285 for (k = l; k < n; ++k) {
286 v[k][j] = v[k][j] + S * v[k][i];
290 for (j = l; j < n; ++j) {
301 for (i =
imin(
m,n) - 1; i >= 0; --i) {
305 for (j = l; j < n; ++j) {
312 for (j = l; j < n; ++j) {
314 for (k = l; k <
m; ++k) {
315 S = S + a[k][i] * a[k][j];
317 F = (S / a[i][i]) * G;
318 for (k = i; k <
m; ++k) {
319 a[k][j] = a[k][j] + F * a[k][i];
323 for (j = i; j <
m; ++j) {
324 a[j][i] = a[j][i] * G;
327 for (j = i; j <
m; ++j) {
331 a[i][i] = a[i][i] + 1.0;
336 for (k = (n - 1); k >= 0; --k) {
338 for (its = 1; its <= 30; ++its) {
342 for (l = k; l >= 0; --l) {
344 if ((fabsf(rv1[l]) + ANorm) == ANorm) {
347 }
else if ((fabsf(w[NM]) + ANorm) == ANorm) {
356 for (i = l; i <= k; ++i) {
358 if ((fabsf(F) + ANorm) != ANorm) {
370 for (j = 0; j <
m; ++j) {
373 a[j][NM] = (
Y * C) + (
Z * S);
374 a[j][i] = -(
Y * S) + (
Z * C);
385 for (j = 0; j < n; ++j) {
403 F = ((
Y -
Z) * (
Y +
Z) + (G -
H) * (G +
H)) / (2.0 *
H *
Y);
413 if ((F + tmp) != 0) {
414 F = ((
X -
Z) * (
X +
Z) +
H * ((
Y / (F + tmp)) -
H)) /
X;
422 for (j = l; j <= NM; ++j) {
438 F = (
X * C) + (G * S);
439 G = -(
X * S) + (G * C);
442 for (jj = 0; jj < n; ++jj) {
445 v[jj][j] = (
X * C) + (
Z * S);
446 v[jj][i] = -(
X * S) + (
Z * C);
458 F = (C * G) + (S *
Y);
459 X = -(S * G) + (C *
Y);
460 for (jj = 0; jj <
m; ++jj) {
463 a[jj][j] = (
Y * C) + (
Z * S);
464 a[jj][i] = -(
Y * S) + (
Z * C);
499 for (k = 0; k < l; k++) {
500 for (j = 0; j < n; j++) {
503 for (i = 0; i <
m; i++) {
s += u[i][j] *
b[i][k]; }
508 for (j = 0; j < n; j++) {
510 for (jj = 0; jj < n; jj++) {
s += v[j][jj] * tmp[jj]; }
549 float _AA[count][D_1];
551 float _targets_all[count][1];
554 for (sam = 0; sam < count; sam++) {
555 for (d = 0; d <
D; d++) {
556 AA[sam][d] = samples[sam][d];
563 targets_all[sam][0] = targets[sam];
568 float _parameters[D_1][1];
587 MAT_MUL(count, D_1, 1, bb, AA, parameters);
589 MAT_SUB(count, 1, C, bb, targets_all);
591 for (sam = 0; sam < count; sam++) {
592 *fit_error += fabsf(C[sam][0]);
598 for (d = 0; d < D_1; d++) {
599 params[d] = parameters[d][0];
618 float *params,
float *fit_error)
648 float _AA[count][D_1];
650 float _AAT[D_1][count];
652 float _AATAA[D_1][D_1];
654 float _PRIOR[D_1][D_1];
656 for(d = 0; d <
D; d++) {
657 PRIOR[d][d] = priors[d];
660 PRIOR[
D][
D] = priors[
D];
666 float _targets_all[count][1];
669 for (sam = 0; sam < count; sam++) {
670 for (d = 0; d <
D; d++) {
671 AA[sam][d] = samples[sam][d];
678 targets_all[sam][0] = targets[sam];
702 float _INV_AATAA[D_1][D_1];
730 float _PINV[D_1][count];
733 MAT_MUL(D_1, D_1, count, PINV, INV_AATAA, AAT);
738 float _parameters[D_1][1];
740 MAT_MUL(D_1, count, 1, parameters, PINV, targets_all);
757 MAT_MUL(count, D_1, 1, bb, AA, parameters);
760 MAT_SUB(count, 1, C, bb, targets_all);
763 for (sam = 0; sam < count; sam++) {
764 *fit_error += fabsf(C[sam][0]);
770 for (d = 0; d < D_1; d++) {
771 params[d] = parameters[d][0];