[SOLVED] How to calculates changes in position with perpendicular tracking wheels and a orientation sensor by assuming movement in arcs?

I want to use a system of odometry to find where my robot is at all times by finding the displacement of the robot every 10 ms. To do this I am using a vertical tracking wheel that tracks when the robot is moving forward or backward in relation to the robot, a horizontal tracking wheel that tracks when the robot is moving side to side in relation to the robot and a orientation sensor that gives the orientation of the robot at all times (the orientation sensor has no bounds). I use vertical_rotation_sensor.get_position() / 5729.57795131 to get how much the vertical tracking wheel has moved (converted to inches), horizontal_rotation_sensor.get_position() / 5729.57795131 to get how much the horizontal tracking wheel has moved (converted to inches), and imu.get_rotation() * M_PI / 180 to get the current orientation in radians. The vertical tracking wheel is offset from the center by -10 inches and the horizontal by 4.21875 inches. I use C++.

I tried calculating the movements by calculating the wheels movements with arcs. I expected that the numbers would be +- .1 - .5 inches off. When I move straight in any direction my code works very well but when I turn my robot any every if off by multiple inches. For example I drove 24 inches forward and my robot said I was a at x = 24.03 and y = .05 but when I turned one rotation making sure to stay on the circle's path the entire time and then moved back to the starting point I got x = .49 and y = -4.45. This was my code.

I start by finding the changes in the movement of each wheel and the orientation sensor. **This part works. **

double total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
double total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
double theta = imu.get_rotation() * M_PI / 180;

//finds changes in the tracking wheel's movement and orientation of the robot
double delta_wheel_movement_vertical = total_wheel_movement_vertical - previous_total_wheel_movement_vertical;
double delta_wheel_movement_horizontal = total_wheel_movement_horizontal - previous_total_wheel_movement_horizontal;
double delta_theta = theta - previous_theta;

//updates previous values for the next calculations
previous_total_wheel_movement_vertical = total_wheel_movement_vertical;
previous_total_wheel_movement_horizontal = total_wheel_movement_horizontal;
previous_theta = theta;

Then, I calculate the x and y movements relative to the robot by assuming the robot has moved in a arc and use each wheel's arc's chord length or if the robot hasn't turned I calculate the movement as lines.

        //calculates the chord length of the arc / the length of the movement of the tracking center
        if (fabs(delta_theta) != 0) {
            delta_local_y = 2 * sin(delta_theta / 2) * (delta_wheel_movement_vertical / delta_theta + tracking_wheel_offset_vertical);
            delta_local_x = 2 * sin(delta_theta / 2) * (delta_wheel_movement_horizontal / delta_theta + tracking_wheel_offset_horizontal);
        } else {
            delta_local_y = delta_wheel_movement_vertical;
            delta_local_x = delta_wheel_movement_horizontal;
        }

Then, I calculate the average angle of the arcs. This will be what I use as the angle for the final calculation.

        double average_theta = previous_theta - delta_theta / 2;

Finally, I put the average angle of the arc's and each arc's length into a formula that finds the endpoint of two perpendicular lines and add it to the starting global position to find the new global position.

        x += delta_local_y * cos(average_theta) - delta_local_x * sin(average_theta);
        y += delta_local_y * sin(average_theta) + delta_local_x * cos(average_theta);

This is all of my code for completeness.

//sets tracking wheel constants
const double tracking_wheel_offset_vertical = -10; //offset of tracking wheels from the tracking center of the robot in inches
const double tracking_wheel_offset_horizontal = 4.21875; //offset of tracking wheels from the tracking center of the robot in inches

//sets autonomous starting values
double x = 0; //global x position
double y = 0; //global y position
double delta_local_y = 0; //vertical chord length of the arc / movement of the tracking center
double delta_local_x = 0; //horizontal chord length of the arc / movement of the tracking center

//function to update the robot's estimated position
void update_pose() {
    //sets previous values to current values for intial calculations
    double previous_total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
    double previous_total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
    double previous_theta = imu.get_rotation() * M_PI / 180;

    while (true) {
        //delays the start of each cycle to save computing resources
        pros::delay(10);

        //gets current values of the tracking wheel in inches and the orientation of the robot in radians
        double total_wheel_movement_vertical = vertical_rotation_sensor.get_position() / 5729.57795131;
        double total_wheel_movement_horizontal = horizontal_rotation_sensor.get_position() / 5729.57795131;
        double theta = imu.get_rotation() * M_PI / 180;

        //finds changes in the tracking wheel's movement and orientation of the robot
        double delta_wheel_movement_vertical = total_wheel_movement_vertical - previous_total_wheel_movement_vertical;
        double delta_wheel_movement_horizontal = total_wheel_movement_horizontal - previous_total_wheel_movement_horizontal;
        double delta_theta = theta - previous_theta;

        //updates previous values for the next calculations
        previous_total_wheel_movement_vertical = total_wheel_movement_vertical;
        previous_total_wheel_movement_horizontal = total_wheel_movement_horizontal;
        previous_theta = theta;

        //calculates the chord length of the arc / the length of the movement of the tracking center
        if (fabs(delta_theta) != 0) {
            delta_local_y = 2 * sin(delta_theta / 2) * (delta_wheel_movement_vertical / delta_theta + tracking_wheel_offset_vertical);
            delta_local_x = 2 * sin(delta_theta / 2) * (delta_wheel_movement_horizontal / delta_theta + tracking_wheel_offset_horizontal);
        } else {
            delta_local_y = delta_wheel_movement_vertical;
            delta_local_x = delta_wheel_movement_horizontal;
        }

        //calculates the angle of the chord of the arc / which direction the tracking center moved
        double average_theta = previous_theta - delta_theta / 2;

        //changes in the global position based on the chord's length and angle / the direction and distance the tracking center moved
        x += delta_local_y * cos(average_theta) - delta_local_x * sin(average_theta);
        y += delta_local_y * sin(average_theta) + delta_local_x * cos(average_theta);

        //prints tracking and debuging values to screen
        pros::lcd::print(0, "X: %f", x);
        pros::lcd::print(1, "Y: %f", y);
        pros::lcd::print(2, "Theta: %f", theta * 180 / M_PI);
        pros::lcd::print(3, "Total Local X Movement: %f", total_wheel_movement_horizontal);
        pros::lcd::print(4, "Total Local Y Movement: %f", total_wheel_movement_vertical);
    }
}```


  [1]: http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf