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