Found and fixed error in odometry: matrices were multiplied in wrong order.