I am trying to figure out a more accurate way to calculate the change in the heading of a robot. I have two small omni wheels on either side of the robot facing forward and one on the right side facing sideways. Currently the heading is calculated by subtracting the difference in position of the left encoder by the difference in position of the right encoder and dividing that by the distance between them every iteration of the loop (roughly every 5ms currently).

double angleChange = (leftDifference - rightDifference) / wheelBase;


$$\Delta \theta = \frac{\Delta L - \Delta R}b$$

The dead wheels are 60mm in diameter and the encoders are 4000 counts per rotation. Using the calculation described it is fairly accurate but the heading drifts relatively quickly leading to the accumulation of positioning errors as well. Is there a way to do this better? I know of another group that changed their calculations to improve it but haven't figured out exactly how.

Interview with the group that improved their solution (their odometry is described near the end): https://www.youtube.com/watch?v=zun--sNljks

Their robot driving autonomously (first 30 seconds): https://www.youtube.com/watch?v=NQvhvYJXVMA

The encoders are used rather than an IMU for the lower response time.