Question about position tracking math

So we were thinking of doing a odom algorithm that’s pretty basic. It’s pretty much rcos(theta) =x and rsin(theta) =y where r is the average displacement. It’s nothing close to the complexity of the 5225A algorithm. I know that this will work ok with two encoders but we are worried about the side to side movement. Is there anyway we can account for this with a third encoder or should we just put traction wheels? More specifically, if we can account for it with a third encoder how excatly would we go about doing so?

Quick note: I’m assuming from you post that your 2 tracking wheels are parallel on the left and right.

It really depends how advanced you want to go and how accurate you want your posiiton data to be. If you’re only looking to do position tracking for the 15 second auton, you might be able to get away with just 2 tracking wheels doing distances and angles. For a full 1 minute auton skills run though, you’ll want to go for the full 3 wheel setup.

That would be where you take a deeper dive into the 5225a document to see how they implemented it. I recently did it myself and found that it wasn’t as scary as it seemed. If you’ve taken a geometry class you should be able to figure it out (drawing a picture really helped me).

I personally wouldn’t recommend this because omnis or basically any other drive type turn and maneuver so much better.


Their implementation wasn’t much more complex. They run the same chord length calculation as one of the parallel wheels and just switch the X and Y components of the displacement.

@trexing123 if you are going to just make calculations under the assumption that you traveled in a straight line, then you can do the same thing for a 3rd wheel and just switch the X and Y results. I would highly recommend you use arc based odometry like 5225A outlined in their document, as it is much more accurate. Here is a good video explaining the math: