Odometry Accuracy

I mentor a middle school robotics team and my son is the programmer. He has been trying to get odometry working on their robot. After many hours and two failed attempts - it seems to work. He drove the robot randomly around the field back and forth a few times and checked the coordinate and orientation and it was within 3 inches on it’s location and 2 degrees on orientation. Should he keep trying to tune it or is this within the accuracy he can expect? He is using 2 parallel and 1 perpendicular tracking wheels. They are small Omni wheels.

His main goal is to score high on programming skills for spin up. The teams highest score so far is 80. He would love to double that.

He also started writing code to drive the robot to a point and turn to the goal but that isn’t working yet. (It turns and drive toward a point but is always off by around 5 inches or so. ). He is trying to figure out PID but it’s not working well so far.

I know nothing on code so I can’t really guide him much on this so will ask him to read the replies here. He has worked on it for around 15 hours this week and is hoping to get the odometry and driving working so has has time to code programming for their next tourney in about 3 weeks.

The task your son is trying to do is difficult for many experienced high school teams. And unless we get more specifics on the coding questions he has or the problems he’s encountering, we can’t help all that much. A big part of engineering is communication, and so I hope you’ll let your son follow up with his own questions and explanations. Typically questions related to code should include some of the relevant code to reference and build off of.

Anyway, to answer the main question, 2 degrees off on orientation is problematic. To remedy it, I would recommend using an IMU rather than 2 parallel trackers to measure rotation.


Need to have a better sensing of this - exactly how many times has he drove the robot around?

Why not get him to drive the robot according to the skills route that he has in mind? and at the end of the run, check the coordinates and see if the offset is within the requirements?

1 Like

Having him drive skills and check the accuracy at the end is a good idea. I know he has drove across the field a couple of times and spun the rollers and the parked the robot where he started to check the accuracy.

I don’t think he has a specific question yet. But I will tell him to post here when he does. He reads a lot on the forum and watches you tube videos posted by teams to figure things out. He works on coding for vex or for video games or random computer scripts pretty much every day. It’s his favorite thing.

He had been using the interior sensor for turns but he is is using the 2 parallel tracking wheels instead for the odometry to track the orientation. Is one better than the other? I think one of his odometry attempts used the inertia sensor but he didn’t get it work.

In my experience the inertial sensor is better for odometry than parallel tracking wheels.


I did a test to see how big the error was i told it to goto coordinate 0,300. The Y error was 303/281. Should i multiply that in ? Here is my code:`
void positionTrackingInit() {
LeftEncoderY.setPosition(0, deg);
RightEncoderY.setPosition(0, deg);
EncoderX.setPosition(0, deg);
xPosGlobal = X_START;
yPosGlobal = Y_START;
while(1) {
//Get encoder values (DEGREES)

LPos = LeftEncoderY.position(rotationUnits::deg);
RPos = RightEncoderY.position(rotationUnits::deg);
SPos = EncoderX.position(rotationUnits::deg);

//Calculate distance traveled by tracking each wheel (INCHES)
  //Converts degrees to radians
deltaDistL = ((LPos - LPrevPos) * M_PI / 180) * WHEEL_RADIUS;
deltaDistR = ((RPos - RPrevPos) * M_PI / 180) * WHEEL_RADIUS;
deltaDistS = ((SPos - SPrevPos) * M_PI / 180) * WHEEL_RADIUS;

//Update previous values to be used next loop (DEGREES)
LPrevPos = LPos;
RPrevPos = RPos;
SPrevPos = SPos;

//Total change in each of the L and R encoders since last reset (INCHES)
//These are used to calculate the absolute orientation of the bot
totalDeltaDistL += deltaDistL;
totalDeltaDistR += deltaDistR;

//Calculate the current absolute orientation (RADIANS)
//currentAbsoluteOrientation = THETA_START - ( (totalDeltaDistL - totalDeltaDistR) / (LTrackRadius + RTrackRadius) );
currentAbsoluteOrientation = CalulatedHeadingRad;

//Calculate the change in the angle of the bot (RADIANS)
deltaTheta = currentAbsoluteOrientation - previousTheta;

//Update the previous Theta value (RADIANS)  
previousTheta = currentAbsoluteOrientation;

//If we didn't turn, then we only translated
if(deltaTheta == 0) {
  deltaXLocal = deltaDistS;
  // could be either L or R, since if deltaTheta == 0 we assume they're =
  deltaYLocal = deltaDistL;
//Else, caluclate the new local position
else {
  //Calculate the changes in the X and Y values (INCHES)
  //General equation is:
    //Distance = 2 * Radius * sin(deltaTheta / 2)
  deltaXLocal = 2 * sin(deltaTheta / 2.0) * ((deltaDistS / deltaTheta) + STrackRadius);
  deltaYLocal = 2 * sin(deltaTheta / 2.0) * ((deltaDistR / deltaTheta) - RTrackRadius);


//The average angle of the robot during it's arc (RADIANS)
avgThetaForArc = currentAbsoluteOrientation - (deltaTheta / 2);

deltaXGlobal = (deltaYLocal * cos(avgThetaForArc)) - (deltaXLocal * sin(avgThetaForArc));
deltaYGlobal = (deltaYLocal * sin(avgThetaForArc)) + (deltaXLocal * cos(avgThetaForArc));

//Wraps angles back around if they ever go under 0 or over 2 pi
while(currentAbsoluteOrientation >= 2 * M_PI) {
  currentAbsoluteOrientation -= 2 * M_PI;

while(currentAbsoluteOrientation < 0) {
  currentAbsoluteOrientation += 2 * M_PI;

//Update global positions
yPosGlobal += deltaXGlobal;
xPosGlobal += deltaYGlobal;

// Brain.Screen.setCursor(4,8);
// Brain.Screen.print(deltaDistS/deltaTheta);
// Brain.Screen.setCursor(5,8);
// Brain.Screen.print((deltaDistL+deltaDistR)/2);
// Brain.Screen.setCursor(6,8);
// Brain.Screen.print(deltaDistS);
// Brain.Screen.setCursor(7,8);
// Brain.Screen.print(deltaTheta);

//loop every 20 milliseconds


I had to flip X and Y because Y it would have messed up my calculations

Welcome to the vex forum!

First of all, I still strongly recommend using an ineryial sensor to track rotation. Using parallel trackers was more effective in 2018, when the best gyro available was still pretty bad, but the new imu will get the job fine. All you’d need to change is to determine your absolute orientation by inputting the gyro heading (and converting to radians) instead of the difference in the left and right trackers.

This is the procedure I go through for tuning odom with an IMU. (The tuning with imu is quite a bit easier than with three wheels.)

  1. Spin the robot 4 or 5 times. Check the rotation the IMU outputs. If it’s too low or too high, just multiply the output by desiredoutput/realoutput in your code.

  2. Line up the robot with a wall or some other long, straight surface. Roll the robot forward along the wall to see if the sideways tracker spins at all, and the other way to see if the forward tracker spins. If they spin when they shouldn’t, fix the physical angle they’re mounted at to be as perpendicular as possible to each other and the chassis.

  3. THEN you should tune the multipliers for X and Y. If you try to do it before steps 1 and 2 you’ll have kind of a hard time because the rest of the robot will still drag you down. Just roll the robot against the wall for a few feet, 12 feet if you can, and see what the trackers output. Multiply each one by a scale factor individually. The easiest way to do this is just to adjust the wheel radius in code.

Try that out. It should be pretty simple and get you good mileage if your dead set on doing odom.


Thank you @2775Josh.I tunned the inertial sensor like you said and compared it to the Wheel heading after driving to the end of a 25 ft hallway with random turns and back, the errors where 1 deg to13 deg the inertial sensor did better by a lot. I drove it around and got the same. I had to convert inertial rotation to custom heading because the heading that the sensor generated was wrong Testing Code:` double CalulatedIMUHeading = fmod(Inertial1.rotation(deg)1.017731141.011366640.988717633,360);
void IMUHeadingInit(){
CalulatedIMUHeading = fmod(Inertial1.rotation(deg)1.017731141.01136664
if(CalulatedIMUHeading < 0 ){
while(CalulatedIMUHeading < 0){
CalulatedIMUHeading = 360 + CalulatedIMUHeading;
double CalulatedHeadingRad = 0;
double CalulatedHeadingDeg = 0;
double OrientationStart = 0;
void OrientationInit(){

if (LeftEncoderY.position(deg)>RightEncoderY.position(deg)){
int difference = LeftEncoderY.position(deg)-RightEncoderY.position(deg);
difference = difference % 2076;
CalulatedHeadingDeg = difference * 0.17230968573* 1.010271089411.012931761.0756543564*0.99538803543

CalulatedHeadingDeg = fmod(CalulatedHeadingDeg,360);
int difference = RightEncoderY.position(deg)-LeftEncoderY.position(deg);
difference = difference % 2076;
CalulatedHeadingDeg = 360 -( difference * 0.17230968573* 1.010271089411.012931761.0756543564*0.99538803543

CalulatedHeadingDeg = fmod(CalulatedHeadingDeg,360);

if (CalulatedHeadingDeg ==360){
CalulatedHeadingDeg= 0;
CalulatedHeadingRad= CalulatedHeadingDeg* M_PI / 180;

void usercontrol(){
double LeftV = 0;
double RightV = 0;
double PreviousLeftV = 0;
double PreviousRightV = 0;
double moveamount = Controller1.Axis3.position(percentUnits::pct); // Delcares the Left Vertical axis on the joystick
double turnamount = Controller1.Axis4.position(percentUnits::pct); // Delcares the Left Horizational axis on the joystick
LeftV = ((moveamount/2) + (turnamount/2))/1.25; // Delares the LeftV by calulating it using the joystick
RightV = ((moveamount/2) - (turnamount/2))/1.25;// Delares the RightV by calulating it using the joystick
if (PreviousLeftV == 0 && PreviousRightV == 0){ // Checks if it is stopped
LF.spin(directionType::fwd,LeftV,percentUnits::pct); // Spins the Left Front Motor with the calulated Velocity
LR.spin(directionType::fwd,LeftV,percentUnits::pct); // Spins the Left Rear Motor with the calulated Velocity
RF.spin(directionType::fwd,RightV,percentUnits::pct);// Spins the Right Front Motor with the calulated Velocity
RR.spin(directionType::fwd,RightV,percentUnits::pct);// Spins the Right Rear Motor with the calulated Velocity
}else{ // Else
LF.spin(directionType::fwd,(LeftV + PreviousLeftV) * 0.75 ,percentUnits::pct); // Caluates The Final Velocity by Adding the left velocity with the previous and multipling it by 3 fourths
LR.spin(directionType::fwd,(LeftV + PreviousLeftV) * 0.75,percentUnits::pct);// Caluates The Final Velocity by Adding the left velocity with the previous and multipling it by 3 fourths
RF.spin(directionType::fwd,(RightV + PreviousRightV)* 0.75,percentUnits::pct);// Caluates The Final Velocity by Adding the right velocity with the previous and multipling it by 3 fourths
RR.spin(directionType::fwd,(RightV + PreviousRightV)* 0.75,percentUnits::pct);}// Caluates The Final Velocity by Adding the right velocity with the previous and multipling it by 3 fourths

if (PreviousLeftV >= 0.0000000000000001 || PreviousLeftV <= -0.00000000000001){ // If the previous state was stopped
LeftV = (LeftV + PreviousLeftV) * 0.75; // Delcares The Velocity
 if (PreviousRightV >= 0.0000000000000001 || PreviousRightV <= -0.00000000000001){// If the previous state was stopped
RightV =(RightV +  PreviousRightV) * 0.75;// Delcares The Velocity

PreviousLeftV = LeftV; // Sets the LeftV to the previous LeftV
PreviousRightV = RightV; // Sets the RightV the previous RightV
if (PreviousLeftV >= 0.000000000000000000000000000001){ // Checks If The Left V is greater than 0
PreviousLeftV = PreviousLeftV - 0.09; // Decreases the Velocity
if (PreviousLeftV <= -0.000000000000000000000000000001){ // Checks If The Left V is less than 0
PreviousLeftV = PreviousLeftV +0.09; // Decreases the Velocity
if (PreviousRightV >= 0.000000000000000000000000000001){ // Checks If The Right V is greater than 0
PreviousRightV = PreviousRightV - 0.09;// Decreases the Velocity
if (PreviousRightV <= -0.000000000000000000000000000001){// Checks If The Right V is less than 0
PreviousRightV = PreviousRightV+0.09;// Decreases the Velocity
wait(25,msec); // A Small wait to save resources

competition VRC;
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
thread InitializeT1(OrientationInit);
thread InitializeT2(IMUHeadingInit);
while (true){
Brain.Screen.setCursor(1, 1);
Brain.Screen.print(“IMU Heading:”);


Brain.Screen.setCursor(2, 1);
Brain.Screen.print("IMU Rotation:");
Brain.Screen.print(Inertial1.rotation(deg) * 1.01773114*1.01136664*0.988717633);
Brain.Screen.setCursor(3, 1);
Brain.Screen.print("Wheel Heading:");



We tested the if the wheels where placed correctly we got 2 1/2 out of 20 ft and 3 in on one wheel and 6 inches on the other. This could be because of the boards on our floor not being straight. Should i just use the encoder with the least error?

Before you start picking encoders, be absolutely sure that your trackers are as straight as possible using something like the method I described in step 2. Then go ahead and use whichever one works best. It’ll probably be easiest to use the right encoder since that’s the one you already have programmed.


Thank you so much,@2775Josh The error in my odometry is has decreased from 2 in to 0.5 over 1 min of turning and driving. The Solution was to bias the encoders to make them more parrell. Thank you so much Josh!

1 Like