Kalman Filter (somewhat) working