Fixed issues with odometry calculation:
-Changed calculation of dth to (arc_length*2PI)/circumference
-Changed delta_x calculation to dx*cos(dth) and delta_y to dx*sin(dth) - dx is the distance traveled by the robot
-Changed new th value calculation to be in the range -pi,pi because that is how the tf::Quaternion class returns the yaw values. I added the utility class so that the displaceAngle could be used. So the new th value is: th = utility.displaceAngle(th, dth)
-Changed new x,y value calculation to after th being set, rather than before new th was set