private void initializeKalman()
{
float accelNoise = (float)numProcessNoise.Value;
float measurementNoise = (float)numMeasurementNoise.Value;
var measurementDimension = 2; //just coordinates
var initialState = process.GetNoisyState(accelNoise); //assuming we measured process params (noise)
var initialStateError = ModelState.GetProcessNoise(accelNoise);
kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(accelNoise);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, measurementNoise).ElementwisePower(2); //assuming we measured process params (noise) - ^2 => variance
kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{ 1, 0, 0, 0 }, //picks X
{ 0, 0, 1, 0 } //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix(ConstantVelocityProcess.TimeInterval);
}
public void GoToNextState(out bool doneFullCycle)
{
Func<PointF, bool> isBorder = (point) =>
{
return point.X <= 0 || point.X >= WorkingArea.Width ||
point.Y <= 0 || point.Y >= WorkingArea.Height;
};
doneFullCycle = false;
var prevPos = currentState.Position;
var speed = currentState.Velocity;
if (isBorder(currentState.Position))
{
var temp = speed.X;
speed.X = -speed.Y;
speed.Y = temp;
if (speed.Equals(initialState.Velocity)) doneFullCycle = true;
}
var nextState = new ConstantVelocity2DModel
{
Position = new PointF
{
X = prevPos.X + speed.X * TimeInterval,
Y = prevPos.Y + speed.Y * TimeInterval
},
Velocity = speed
};
currentState = nextState;
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState {
Position = startPoint, Velocity = new PointF(0.5f, -2f)
};
var initialStateError = ModelState.GetProcessNoise(0);
kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(2);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, 1);
kalman.MeasurementMatrix = new double[, ] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{ 1, 0, 0, 0 }, //picks X
{ 0, 0, 1, 0 } //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
public ConstantVelocityProcess()
{
currentState = new ConstantVelocity2DModel
{
Position = new PointF(50, 1),
Velocity = new PointF(0.3f, 0.3f)
};
initialState = currentState;
}
public static double[] ToArray(ConstantVelocity2DModel modelState)
{
return new double[]
{
modelState.Position.X,
modelState.Velocity.X,
modelState.Position.Y,
modelState.Velocity.Y,
};
}
/// <summary>
/// Converts the model to the array.
/// </summary>
/// <param name="modelState">Model to convert.</param>
/// <returns>Array.</returns>
public static double[] ToArray(ConstantVelocity2DModel modelState)
{
return(new double[]
{
modelState.Position.X,
modelState.Velocity.X,
modelState.Position.Y,
modelState.Velocity.Y,
});
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.2f, -1.5f)};
var initialStateError = ModelState.GetProcessNoise(10);
kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(1);
kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, 10000.0);
kalman.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix();
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState {
Position = startPoint, Velocity = new PointF(0.2f, -1.5f)
};
var initialStateError = ModelState.GetProcessNoise(10);
kalman = new DiscreteKalmanFilter <ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(1);
kalman.MeasurementNoise = Matrix.Diagonal <double>(kalman.MeasurementVectorDimension, 10000.0);
kalman.MeasurementMatrix = ModelState.GetPositionMeasurementMatrix();
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
private void initializeKalman(PointF startPoint)
{
var measurementDimension = 2; //just coordinates
var initialState = new ModelState { Position = startPoint, Velocity = new PointF(0.5f, -2f)};
var initialStateError = ModelState.GetProcessNoise(0);
kalman = new DiscreteKalmanFilter<ModelState, PointF>(initialState, initialStateError,
measurementDimension /*(position)*/, 0 /*no control*/,
x => ModelState.ToArray(x), x => ModelState.FromArray(x), x => new double[] { x.X, x.Y });
kalman.ProcessNoise = ModelState.GetProcessNoise(2);
kalman.MeasurementNoise = Matrix.Diagonal<double>(kalman.MeasurementVectorDimension, 1);
kalman.MeasurementMatrix = new double[,] //just pick point coordinates for an observation [2 x 4] (look at ConstantVelocity2DModel)
{
//X, vX, Y, vY (look at ConstantVelocity2DModel)
{1, 0, 0, 0}, //picks X
{0, 0, 1, 0} //picks Y
};
kalman.TransitionMatrix = ModelState.GetTransitionMatrix();
}
public static ConstantVelocity2DModel Evaluate(ConstantVelocity2DModel state, double[,] transitionMat)
{
var stateVector = state.ToArray().Multiply(transitionMat);
return ConstantVelocity2DModel.FromArray(stateVector);
}
/// <summary>
/// Evaluates the model by using the provided transition matrix.
/// </summary>
/// <param name="transitionMat">Transition matrix.</param>
/// <returns>New model state.</returns>
public ConstantVelocity2DModel Evaluate(double[,] transitionMat)
{
var stateVector = transitionMat.Multiply(ToArray(this));
return(ConstantVelocity2DModel.FromArray(stateVector));
}
public static ConstantVelocity2DModel Evaluate(ConstantVelocity2DModel state, double[,] transitionMat)
{
var stateVector = state.ToArray().Multiply(transitionMat);
return(ConstantVelocity2DModel.FromArray(stateVector));
}
