Odometry works!
Merge fixes