add computation model based on kalman filter. It filters out invalid points. If finds too many invalid points, reset kalman filter to change speed. Still not very good because it does not use the condition that speed is constant. Will try nonlinear kalman filter