Added Kalman Filter, needs debugging