/// <summary>
///
/// </summary>
/// <param name="distance"></param>
/// <param name="angle"></param>
public void Run(int distance, double angle)
{
State previous = states[currentState++];
//Compute next state
State actual = new State();
actual.xpos = previous.xpos + (int)MaCRo.Tools.exMath.Cos(angle) * distance;
actual.ypos = previous.ypos + (int)MaCRo.Tools.exMath.Sin(angle) * distance;
actual.angle = angle;
}
public KalmanFilter()
{
numMaxStates = 150;
currentState = 0;
states = new State[5];
states[0] = new State();
states[0].angle = 0;
states[0].velocity = 0;
states[0].xpos = 0;
states[0].ypos = 0;
}