Using odom data may be a mistake, seems to move and then rewind as the odom slows.
Motion model appears to be working.