Fully working Kalman Filter