Odometry Turning X & Y Gain

Cross Post on VF
While making an odometry system based on 5225 Pilons’ document we came across a problem. While turning the robot the x and y coordinates start to gain and then after ~180 they start to recede, as if the robot is traveling in a circle. In our video we turn the robot clockwise but the x and y values drift up to 7 inches off. We are using 3 tracking wheels and have read other topics such as Odometry Turning Help and Odometry Turning Issue but they didn’t solve our issue.

Our code (PROS):

list<float> OdomPosTracking(float leftWheelOffset, float leftWheelCir, 
                            float rightWheelOffset, float rightWheelCir, 
                            float centerWheelOffset, float centerWheelCir, 
                            float xInit, float yInit, float tInit)
{
    float currentLeftPositionCentidegrees = leftTracking.get_position();
	float currentRightPositionCentidegrees = rightTracking.get_position();
	float currentCenterPositionCentidegrees = centerTracking.get_position();

	float currentLeftPositionInches = currentLeftPositionCentidegrees / 100.0 / 360.0 * leftWheelCir;
	float currentRightPositionInches = currentRightPositionCentidegrees / 100.0 / 360.0 * rightWheelCir;
	float currentCenterPositionInches = currentCenterPositionCentidegrees / 100.0 / 360.0 * centerWheelCir;

	float leftChangeInches = currentLeftPositionInches - previousLeftPositionInches;
	float rightChangeInches = currentRightPositionInches - previousRightPositionInches;
	float centerChangeInches = currentCenterPositionInches - previousCenterPositionInches;

	previousLeftPositionInches = currentLeftPositionInches;
	previousRightPositionInches = currentRightPositionInches;
	previousCenterPositionInches = currentCenterPositionInches;

	float currentGlobalThetaRadians = startingThetaRadians + (currentLeftPositionInches - currentRightPositionInches) / (leftWheelOffset + rightWheelOffset);
	currentGlobalThetaDegrees = currentGlobalThetaRadians * 180.0 / pi;

	float deltaThetaRadians = currentGlobalThetaRadians - previousGlobalThetaRadians;

	if (deltaThetaRadians == 0.0){
		localChangeX = centerChangeInches;
		localChangeY = rightChangeInches;
	}
	else{
		localChangeX = 2.0 * sin(deltaThetaRadians / 2.0) * (centerChangeInches / deltaThetaRadians + centerWheelOffset);
		localChangeY = 2.0 * sin(deltaThetaRadians / 2.0) * (rightChangeInches / deltaThetaRadians + rightWheelOffset);
	}

	float avgTheta = previousGlobalThetaRadians + deltaThetaRadians / 2.0;

	float localChangeTheta = atan2(localChangeX, localChangeY);
	float localChangeR = sqrt(pow(localChangeX, 2.0) + pow(localChangeY, 2.0));

	float modLocalTheta = localChangeTheta - avgTheta;

	float globalChangeX = localChangeR * cos(modLocalTheta);
	float globalChangeY = localChangeR * sin(modLocalTheta);

	currentGlobalX += globalChangeX;
	currentGlobalY += globalChangeY;

	previousGlobalThetaRadians = currentGlobalThetaRadians;

    list<float> pos {currentGlobalX, currentGlobalY, currentGlobalThetaDegrees};
    return pos;
}

This all runs continuously in a loop inside its own task.