public ConstantVelocityProcess(Vector position)
{
currentState = new ConstantVelocity2DModel
{
Position = new Vector(position.x / 2, 1),
Velocity = new Vector(0.3f * position.x / 100, 0.3f * position.x / 100)
};
initialState = currentState;
}
/// <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,
});
}
public ConstantVelocity2DModel GetNoisyState(double accelerationNoise)
{
var processNoiseMat = ConstantVelocity2DModel.GetProcessNoise(accelerationNoise);
var noise = normalDistribution.Generate(ConstantVelocity2DModel.Dimension).Multiply(processNoiseMat);
return(new ConstantVelocity2DModel
{
Position = new Vector
{
x = currentState.Position.x + (float)noise[0],
y = currentState.Position.y + (float)noise[2]
},
Velocity = new Vector
{
x = currentState.Velocity.x + (float)noise[1],
y = currentState.Velocity.y + (float)noise[3]
}
});
}
public void GoToNextState(out bool doneFullCycle)
{
Func <Vector, 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 Vector
{
x = prevPos.x + speed.x * TimeInterval,
y = prevPos.y + speed.y * TimeInterval
},
Velocity = speed
};
currentState = nextState;
}
/// <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.Dot(ToArray(this));
return(ConstantVelocity2DModel.FromArray(stateVector));
}