fix bug in odometry calculation

Before, the overflow handling code was also triggered on zero crossings.
This lead to huge values in the x and y positions (and also a wrong
theta angle) of the odometry.