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