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