fixed kalman filter wandering