I don't know how to fully answer your question, but I'll do it the best I can. I'll assume your robot is like ours (I know it probably isn't) and that these equations will work just as well with your robot as it did with ours.
Assuming that all wheels on a single side of your robot travels at the same rate, let's look at my horrible paint job picture:
Let
R equal the speed of the right wheels (using an encoder on your right wheel),
et
L equal the speed of the left wheels (using an encoder on your left wheel),
and let
Base equal the width of your robot, or the distance between two wheels.
As you may know, to get angular speed, you can use the equation:
Angular Speed (rad/sec) = velocity (m/sec) / radius (m)
Since we're not recording speed, but angle and position, ignore velocity and use distance for now.
Since
R is larger than
L, relative to
L, the right side of the robot has moved
R - L. Let velocity (or distance) equal
R - L and make
Base the radius.
From that you get:
theta = (R - L) / Base, or the amount your robot has turned.
To get the heading in the x and y direction, you will need to know the point your robot is driving around. The distance traveled is greater on the right side, so your robot will arc (or drive) to the left. That means the point your robot is driving around will be somewhere to the left of your robot:
The radius of the arc can be calculated by finding the distance between the center of the circle and the center of your robot, or the length of the triangle - half the base.
To calculate the length of the triangle take the ratio between
Base and your robot's relative speed
(R - L). Multiply that ratio by the height of your triangle (or
R) and you get your length:
R * Base / (R - L).
Subtract half the base from that and you get
R * Base / (R - L) - Base / 2
which equals:
Base ( R / ( R - L ) - 1 / 2)
which when
R / (R - L) and 1 / 2 put into one fraction simplfies to:
Base ( (
2R - R + L) / 2( R - L))
which equals:
Base (
(R + L) / 2(R - L))
Since theta equals
(R - L) / Base the equation can then be simplified to:
(R + L) / (2 * theta)
Radius = (R + L) / (2 * theta)
Using your
Radius and
theta, you can get
x and
y.
y =
Radius * sin(
theta)
x =
Radius * (1 - cos(
theta))
1 - cos(theta) because your actual x is radius minus x
Of course these are all relative values, so you will need to recalculate based on the facing of your robot.
Hope that helps!