The problem i am facing is to try and calculate the x and y position of a robot with dead reckoning.

Reading from the encoders and getting proper rotations of the wheels of my robot works. The robot has 3 wheels, where 2 of the 3 can be controlled like in this picture:


Calculating the distance from these wheel rotations and from those distances the heading of the robot is tested and works properly. Here is the code for the heading:

void MotorHandler::UpdateHeading(Position& currentPosition, double leftMotorDistance, double rightMotorDistance)
   currentPosition.heading = (double)((int)(((rightMotorDistance - leftMotorDistance) / (2 * PI * AXLE_LENGTH)) * FULL_ROTATION_DEGREES) % 360);

Where axle_length is the length between both wheels and full_rotation_degree is 360.

But after this, when i try to calculate the x and y of the robot at any given moment i get weird results. Here is the code for calculating the x and y:

void MotorHandler::CalculateCurrentPosition(Position& currentPosition)
double leftMotorDistance = (motors[0]->GetRotation()/FULL_ROTATION_PULSES) * WHEEL_CIRCUMFERENCE;
double rightMotorDistance = (motors[1]->GetRotation()/FULL_ROTATION_PULSES) * WHEEL_CIRCUMFERENCE;

double headingToRadians = (currentPosition.heading * PI) / 180;

if (fabs(leftMotorDistance - rightMotorDistance) < 1.0e-6)
    currentPosition.X = leftMotorDistance * cos(headingToRadians);
    currentPosition.Y = rightMotorDistance * sin(headingToRadians);
    double wd = (rightMotorDistance - leftMotorDistance) / AXLE_LENGTH;
    double expr = (AXLE_LENGTH * (rightMotorDistance + leftMotorDistance)) / (2 * (rightMotorDistance - leftMotorDistance));

    currentPosition.X = expr * sin(wd + headingToRadians) - expr * sin(headingToRadians);
    currentPosition.Y = -expr * cos(wd + headingToRadians) + expr *  cos(headingToRadians);
    UpdateHeading(currentPosition, leftMotorDistance, rightMotorDistance);

Where the starting x, y and heading are 0. As said, calculating the heading of the robot works properly, which means that the distances of the wheels also are calculated correctly and works properly.

Links used:

https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf http://rossum.sourceforge.net/papers/DiffSteer/
Calculate position of differential drive robot


  • $\begingroup$ At first sight, I would say that for dead reckoning you need to accumulate values somewhere, otherwise you are just calculating relative positions to the previous timestep. $\endgroup$
    – 50k4
    Commented Dec 17, 2017 at 21:00
  • $\begingroup$ Instead of looking at the difference between the previous step and now I just look at everything from the start and assign that. The wheel rotations just keep getting added up and thus the distance the robot has traveled too. $\endgroup$
    – Mathijs F
    Commented Dec 17, 2017 at 21:16

1 Answer 1


This is another aspect of a longer answer I wrote here. Briefly restated, your problem is that a wheeled robot like this is nonholonomic, which means you can just use absolute encoder counts and get a valid result for position and heading.

You have two axes of control: a left wheel and a right wheel; and you have three degrees of freedom: left/right translation, forward/backward translation, and rotation.

I'll restate the example given at the other answer. Consider:

  1. Moving forward 10 meters and turning 180 degrees, =OR=
  2. Turning 180 degrees and moving forward 10 meters.

How do these scenarios work in your code? It looks like your heading is probably calculated correctly, but your position is based solely on the current heading. In both cases above, the robot ends after having turned 180 degrees. In the second case, your code should give correct answers, because there is no translation before you rotate.

The first case, however, would see your code correctly track the movement as it goes forward 10 meters but then, as the robot rotates, your code uses the current heading to index the distance traveled, and so it would appear as if your robot were quickly scribing a circle of radius 10 meters.

Again, as stated in the other answer, the solution is to do as @50k4 mentioned and accumuluate current position as a differential quantity for each sample, and not just look at overall counts.

  • 1
    $\begingroup$ Yes this was the problem ty. Using the delta distances in combination with the absolute heading did the trick. Instead of the absolute distances as shown in the original example code. $\endgroup$
    – Mathijs F
    Commented Dec 19, 2017 at 16:43
  • $\begingroup$ @MathijsF - Glad you got the problem fixed! $\endgroup$
    – Chuck
    Commented Dec 19, 2017 at 18:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.