Rewrote KalmanFilter
Edited KalmanFilter, removed comment of old code.