Kalman Filter implemented and alpha tested.