Position Tracking/Odometry Tutorial

I will be allocating this thread to odometry, as requested by many. Because I do not have a robot with me, I have utilized Roblox Studio to help me figure out and test the code (then translating it from lua to C++). I am pretty new to odometry, but I am able to figure out the gist by teaching myself and trial/error (as what @Cody said, doing is the best teacher :wink:). So if/when I have the time, I will be posting tutorial videos on position tracking and odometry. If you need any help let me know, and for the meantime you can play the roblox game or edit it to see how the calculations work:

The Position tracking code:
OdomTutorial-2020-07-06T04-22-57.zip (8.9 KB)

If I have the time, I may upload more odometry tutorials, but there probably will be a point where I may need assistance from those who do know odometry to help me out with figuring out advanced algorithms that can be utilized.

Note: I would suggest watching the PID tutorial if you want an explanation on derivatives and integrals in code. What happens with position tracking is we do the following:

  1. Grab the local position
  2. Convert local position -> Local velocity by getting its derivative over time
  3. Use Sine, Cosine, and Trig knawledge to convert Local Velocity to World-Space Velocity utilizing the inertial sensor(to get rotations)
  4. Convert World-Space Velocity -> World-Space Position by getting its integral times time

Just as a refresher:
Position / dt = Velocity
Velocity * dt= Position

Translated in code:
Velocity = Derivative of Position

lastPos = 0;//Just for initialization, can be changed if necessary
Pos = SensorValue;

Velocity = Pos - lastPos;

lastPos = Pos;

Position = Integral of Velocity

Position = 0;//Assuming starting position is 0 before code ran
Position += Velocity;

Just as a note, your psuedocode really should include multiply/divide by dt.