Ejemplo n.º 1
0
        private void unscentedTransformation(Emgu.CV.Matrix <double> map, Emgu.CV.Matrix <double> points, int outputs, Emgu.CV.Matrix <double> additiveCovariance)
        {
            int sigma_point_number = points.Cols;             // try points.cols better

            trans_mean_mat    = new Matrix <double>(outputs, 1);
            trans_sigmaPoints = new Matrix <double>(outputs, sigma_point_number);

            for (int i = 0; i < sigma_point_number; i++)
            {
                Emgu.CV.Matrix <double> transformed_point = map * points.GetCol(i);
                trans_mean_mat += meansWeights[0, i] * transformed_point;

                // store transformed point
                for (int j = 0; j < outputs; j++)
                {
                    trans_sigmaPoints[j, i] = transformed_point[j, 0];
                }
            }

            Emgu.CV.Matrix <double> intermediate_matrix_1 = new Matrix <double>(trans_mean_mat.Rows, sigma_point_number);
            for (int i = 0; i < sigma_point_number; i++)
            {
                for (int j = 0; j < trans_mean_mat.Rows; j++)
                {
                    intermediate_matrix_1[j, i] = trans_mean_mat[j, 0];
                }
            }

            trans_deviation = trans_sigmaPoints - intermediate_matrix_1;             // Y1=Y-y(:,ones(1,L));

            trans_stateCovariance = trans_deviation * covarianceWeightsDiagonal * trans_deviation.Transpose() + additiveCovariance;
        }