// ukf trial from matlab and c++ examples
// in progress
public MyUnscentedKalman(int states, int measurements)
{
this.L = states;
this.m = measurements;
state = new Matrix <double>(this.L, 1);
state[0, 0] = 1;
state[1, 0] = 1;
state[2, 0] = 1;
state[3, 0] = 0.5;
state[4, 0] = 0.5;
state[5, 0] = 0.5;
state[5, 0] = 0.1;
sigmaPoints = new Matrix <double>(this.L, 2 * this.L + 1);
stateCovariance = new Matrix <double>(L, L);
stateCovariance.SetIdentity(new MCvScalar(1.0));
meansWeights = new Matrix <double>(1, 2 * this.L + 1);
covarianceWeights = new Matrix <double>(1, 2 * this.L + 1);
covarianceWeightsDiagonal = new Matrix <double>(2 * this.L + 1, 2 * this.L + 1);
calculateVariables();
syntheticData = new VectorData();
}
public VectorData()
{
// is linear and probably a non linear transition cannot be translated to a transition matrix
transitionMatrix = new Emgu.CV.Matrix <double>(new double[, ] // n * n matrix A, that relates the state at k-1 step to state k step. In practice, it can change at each time step
{
// {1, 0, 0, 1f, 0, 0, 0.5f}, // x-pos, y-pos, z-pos, velocities acceleration expected combination
// {0, 1, 0, 0, 1f, 0, 0.5f},
// {0, 0, 1, 0, 0, 1f, 0.5f},
// {0, 0, 0, 1, 0, 0, 1f},
// {0, 0, 0, 0, 1, 0, 1f},
// {0, 0, 0, 0, 0, 1, 1f},
// {0, 0, 0, 0, 0, 0, 1},
// {1, 0, 0, 1f, 0, 0, 0.5f}, // x-pos, y-pos, z-pos, velocities acceleration expected combination
// {0, 1, 0, 0, 1f, 0, 0.5f},
// {0, 0, 1, 0, 0, 1f, 0.5f},
// {0, 0, 0, 1, 0, 0, 1},
// {0, 0, 0, 0, 1, 0, 1},
// {0, 0, 0, 0, 0, 1, 1},
// {0, 0, 0, 0, 0, 0, 1},
{ 1, 0, 0, 1f, 0, 0 }, // x-pos, y-pos, z-pos, velocities (no accel)
{ 0, 1, 0, 0, 1f, 0 },
{ 0, 0, 1, 0, 0, 1f },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 1, 0 },
{ 0, 0, 0, 0, 0, 1 },
});
measurementMatrix = new Emgu.CV.Matrix <double>(new double[, ] // m * n matrix H. follows the same rules as transition matrix A
{
// { 1, 0, 0, 0, 0, 0, 0},
// { 0, 1, 0, 0, 0, 0, 0},
// { 0, 0, 1, 0, 0, 0, 0},
{ 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 },
});
measurementMatrix.SetIdentity();
processNoise = new Emgu.CV.Matrix <double>(6, 6); //Linked to the size of the transition matrix
/* Q matrix */ processNoise.SetIdentity(new MCvScalar(1.0e-2)); //The smaller the value the more resistance to noise (default e-4)
measurementNoise = new Emgu.CV.Matrix <double>(3, 3); //Fixed according to input data
/* R matrix */ measurementNoise.SetIdentity(new MCvScalar(1.0e-2));
errorCovariancePost = new Emgu.CV.Matrix <double>(6, 6); //Linked to the size of the transition matrix
errorCovariancePost.SetIdentity();
invMeasurementNoise = new Emgu.CV.Matrix <double>(3, 3);
}
