private static void myOwn_f0b(
int in_cnt_measures, int in_iMx,
double[] in_Matrix_H, double[] in_Measure, double[] in_Noize_Z,
double[] in_CovarianceMatrixS_p, double[] in_CovarianceMatrixS_m,
double[] in_ErrorConditionVector_m, double[] in_ErrorConditionVector_p
)
{
double d_k = 0.0, b_k = 0.0, c_k = 0.0;
double[] f, e, S_k_m, S_k_p;
Matrix CovarianceMatrixS_p = new Matrix(in_iMx, in_iMx),
CovarianceMatrixS_m = new Matrix(in_iMx, in_iMx);
double[] StringOfMeasure = new double[in_iMx];
for (int t = 0; t < in_cnt_measures; t++)
{
d_k = in_Noize_Z[t] * in_Noize_Z[t];
e = new double[in_iMx];
S_k_m = new double[in_iMx];
f = new double[in_iMx];
S_k_p = new double[in_iMx];
SimpleOperations.MakeMatrixFromVector(CovarianceMatrixS_p, in_CovarianceMatrixS_p, in_iMx);
SimpleOperations.MakeMatrixFromVector(CovarianceMatrixS_m, in_CovarianceMatrixS_m, in_iMx);
for (int i = 0; i < in_iMx; i++)
{
StringOfMeasure[i] = in_Matrix_H[t * in_iMx + i];
}
SimpleOperations.CopyArray(f, CovarianceMatrixS_m.Transpose() * StringOfMeasure);
for (int i = 0; i < in_iMx; i++)
{
b_k = Math.Sqrt(d_k / (d_k + f[i] * f[i]));
c_k = f[i] / (Math.Sqrt(d_k * (d_k + f[i] * f[i])));
d_k = d_k + f[i] * f[i];
for (int j = 0; j < in_iMx; j++)
{
S_k_m[j] = in_CovarianceMatrixS_m[j * in_iMx + i];
}
for (int j = 0; j < in_iMx; j++)
{
S_k_p[j] = S_k_m[j] * b_k - e[j] * c_k;
in_CovarianceMatrixS_p[j * in_iMx + i] = S_k_p[j];
}
for (int j = 0; j < in_iMx; j++)
{
e[j] = e[j] + S_k_m[j] * f[i];
}
}
double h_x_ = 0.0;
for (int j = 0; j < in_iMx; j++)
{
h_x_ += StringOfMeasure[j] * in_ErrorConditionVector_m[j];
}
for (int j = 0; j < in_iMx; j++)
{
in_ErrorConditionVector_p[j] = in_ErrorConditionVector_m[j] + (in_Measure[t] - h_x_) * e[j] / d_k;
}
if (t < in_cnt_measures - 1)
{
for (int j = 0; j < in_iMx; j++)
{
in_ErrorConditionVector_m[j] = in_ErrorConditionVector_p[j];
}
for (int j = 0; j < in_iMx * in_iMx; j++)
{
in_CovarianceMatrixS_m[j] = in_CovarianceMatrixS_p[j];
}
}
}
}