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