Initial implementation of Kalman Filter