got kalman filter to converge