Some fixes on the Kalman filter. Works without Odometry.