예제 #1
0
        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];
                    }
                }
            }
        }