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;
}
public Vector3 update(Vector3 point)
{
generateSigmaPoints();
unscentedTransformation(syntheticData.transitionMatrix, sigmaPoints, L, syntheticData.processNoise);
var x1 = trans_mean_mat;
var x_capital_1 = trans_sigmaPoints;
var P1 = trans_stateCovariance;
var x_capital_2 = trans_deviation;
unscentedTransformation(syntheticData.measurementMatrix, x_capital_1, m, syntheticData.measurementNoise);
//updating
trans_cross_covariance = x_capital_2 * covarianceWeightsDiagonal * trans_deviation.Transpose();
// inverse of P2 (trans_covariance)
Emgu.CV.Matrix <double> inv_trans_covariance = new Matrix <double>(trans_stateCovariance.Rows, trans_stateCovariance.Cols);
CvInvoke.cvInvert(trans_stateCovariance, inv_trans_covariance, SOLVE_METHOD.CV_SVD_SYM);
KalmanGain = trans_cross_covariance * inv_trans_covariance;
Emgu.CV.Matrix <double> thisMeasurement = new Matrix <double>(m, 1);
thisMeasurement[0, 0] = point.x;
thisMeasurement[1, 0] = point.y;
thisMeasurement[2, 0] = point.z;
//update state
state = x1 + KalmanGain * (thisMeasurement - trans_mean_mat);
//update covariance
stateCovariance = P1 - KalmanGain * trans_cross_covariance.Transpose();
return(new Vector3((float)state[0, 0], (float)state[1, 0], (float)state[2, 0]));
}