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; }