Update kalman (still not good)