fixing Kalman, looks that it is working somehow.