How to calculate heading more accurately using dead wheels?

Question

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.

Answer 1

The FTC team I coach (Moderately Dangerous) uses the encoders on the motors rather than drag wheels, so I don't know if their solution will work as well for you. They flip there problem.

Drive the the robot in a spin for 500 counts on the left encoder. Record the number of counts on the right and number of degrees the robot turns 5-10 times. Do the same for several other counts on the left. You can now do a linear regression between the difference between the left and right counts and observed turn. If you get a good line fit the slope should give you the mapping.

Answer 2

You need to develop the differential kinematics of this mobile platform for all of the wheels then you can combine it in your model. Other way to do it is to implement an observer based technique to estimate more accurate the angular speed rather than having a time sample based differentiation.