void leastEntryAlgo(float A[][M][maxDegree+1], float P[][N][maxDegree+1], float Pinv[][N][maxDegree+1], float Q[][M][maxDegree+1], float Qinv[][M][maxDegree+1]) {
int n, m; // used in for loops over rows, columns respectively
int p; // used in for loops over polynomial array
int tempN, tempM, tempMin; //used to store temporary locations in A
int finished = 0; // bool to decide if while loop is over
int tempRowEntry, tempColEntry, tempMult, tempEntry;
int diag = 0;
float q[maxDegree+1];
// for finishedRows and finishedColumns,
// set entry i to -1 if row/col i is not finished, or
// set entry i to i if row/col i is finished
int finishedRows[N];
int finishedColumns[M];
//initializes finishedRows and finishedColumns
for(n = 0; n < N; ++n) {
finishedRows[n] = -1;
}
for(m = 0; m < M; ++m) {
finishedColumns[m] = -1;
}
// begin the least entry algorithm
while (finished == 0) {
// resets tempM and tempN to -1
tempM = -1;
tempN = -1;
// finds least non-zero entry, stores it in tempN, tempM
findLeastEntry(A, finishedRows, finishedColumns, &tempN, &tempM, &finished);
/* the loop should never break here; this if statement
is more or less a "just in case" used when developing */
if(finished == 1) { break; }
// resets tempRowEntry and tempColEntry to -1
tempRowEntry = -1;
tempColEntry = -1;
/* checks if we can do row operations. by convention established
(arbitrarily) in this program, we do row oeprations when we can */
if(contains(finishedColumns, M, tempM) == 0) {
// finds an entry for the least entry to reduce
for(n = 0; n < N; ++n) {
if(equalsZero(A[n][tempM]) == 0 && n != tempN) {
tempRowEntry = n;
break;
}
}
/* if there is no good entry to operate on, break the loop
in practice this would never happen, but convienient for dev. */
if (tempRowEntry == -1) { break; }
/* sets the vector q (in the sense of a = b*q + r)
so we can use it to do row operations */
eucDiv(A[tempRowEntry][tempM], A[tempN][tempM], q);
rowOperations2(A, P, Pinv, tempRowEntry, tempN, q);
}
// if we cannot do row operations, we do column operations
else {
for(m = 0; m < M; ++m) {
if(equalsZero(A[tempN][m]) == 0 && m != tempM) {
tempColEntry = m;
break;
}
}
if (tempColEntry == -1) { break; }
eucDiv(A[tempN][tempColEntry], A[tempN][tempM], q);
columnOperations2(A, Q, Qinv, tempColEntry, tempM, q);
}
// updates rows and colums that are finished
updateFinishedRows(A, finishedRows);
updateFinishedColumns(A, finishedColumns);
finished = done(finishedRows, finishedColumns, N, M);
}
// now we can compute the rank
rank = getRank(A);
/* perform type 3 operations to order the
non-zero elements onto the diagonals
*/
orderDiagonals(A, P, Pinv, Q, Qinv);
// make all polynomials monic
makeAllMonic(A, P, Pinv);
// computation trick: transpose Q and Pinv so they are correct
transposeM(Qinv);
transposeN(Pinv);
}
/* reset != 0 --> reset matrices S and P */
static void anfisKalman(FIS *fis, int reset, DOUBLE alpha)
{
DOUBLE **S, **P, **a, **b, **a_t, **b_t;
DOUBLE **tmp1, **tmp2, **tmp3, **tmp4;
DOUBLE **tmp5, **tmp6, **tmp7;
/* ========== */
int in_n;
int out_n = fis->out_n;
int i, j;
DOUBLE denom;
if (fis->method==1)
in_n = ((fis->order)*(fis->in_n)+1)*fis->rule_n;
else
in_n = fis->para_n;
S = fis->S;
P = fis->P;
a = fis->a;
b = fis->b;
a_t = fis->a_t;
b_t = fis->b_t;
tmp1 = fis->tmp1;
tmp2 = fis->tmp2;
tmp3 = fis->tmp3;
tmp4 = fis->tmp4;
tmp5 = fis->tmp5;
tmp6 = fis->tmp6;
tmp7 = fis->tmp7;
/* reset S[][] and P[][] */
if (reset != 0) {
/*====== already defined in param passed in
double alpha;
if (fis->method==1)
alpha = 1e6;
else
alpha = 1e-1; =======*/
for (i = 0; i < in_n; i++)
for (j = 0; j < out_n; j++)
P[i][j] = 0;
for (i = 0; i < in_n; i++)
for (j = 0; j < in_n; j++)
if (i == j)
S[i][j] = alpha;
else
S[i][j] = 0;
return;
}
/* reset == 0, normal operation */
for (i = 0; i < in_n; i++)
a[i][0] = fis->kalman_io_pair[i];
for (i = 0; i < out_n; i++)
b[i][0] = fis->kalman_io_pair[i+in_n];
transposeM(a, in_n, 1, a_t);
transposeM(b, out_n, 1, b_t);
/* recursive formulas for S, covariance matrix */
anfisMtimesM(S, a, in_n, in_n, 1, tmp1);
anfisMtimesM(a_t, tmp1, 1, in_n, 1, tmp2);
denom = fis->lambda + tmp2[0][0];
anfisMtimesM(a_t, S, 1, in_n, in_n, tmp3);
anfisMtimesM(tmp1, tmp3, in_n, 1, in_n, tmp4);
anfisStimesM(1/denom, tmp4, in_n, in_n, tmp4);
anfisMminusM(S, tmp4, in_n, in_n, S);
anfisStimesM(1/fis->lambda, S, in_n, in_n, S);
/* recursive formulas for P, the estimated parameter matrix */
anfisMtimesM(a_t, P, 1, in_n, out_n, tmp5);
anfisMminusM(b_t, tmp5, 1, out_n, tmp5);
anfisMtimesM(a, tmp5, in_n, 1, out_n, tmp6);
anfisMtimesM(S, tmp6, in_n, in_n, out_n, tmp7);
anfisMplusM(P, tmp7, in_n, out_n, P);
for (i = 0; i < in_n; i++)
for (j = 0; j < out_n; j++)
fis->kalman_para[i][j] = P[i][j];
}
