Added odometry measurement
Tried to tune some constants but it just didnt work too well yet.