kalman almost working, still buggy