Updated encoder_and_imu_odom package, but it is not yet tested!