Got Kalman filter somewhat working.