more kalman work, still buggy