# Wheel odometry simulation using ground truth

I'm tracking the state of a robot $X=(x,y,\theta)$ where $x$ and $y$ are the ground coordinates and $\theta$ the heading angle. I have the ground truth states $X_{gt}=(x_{gt},y_{gt},\theta_{gt})$.

I want to simulate the wheel odometry using the available gound truth data. How to do it? Is it possible to only add random gaussian noise the $X_{gt}$?

In this case, how much should the variances be?

You can't just take the ground truth states and get wheel encoder positions, or vice-versa, because the robot is nonholonomic. Nonholonomic is a fancy word that means "path dependent," essentially, and generally happens in robotics when you have fewer axes of control than degrees of freedom.

Consider a typical car (that uses Ackermann steering). In the car, you can go forward or backward (one axis of control), and you can turn the wheels left and right (a second axis of control). However, you can move sideways too, with a specific combination of moving forward and backward AND turning the wheels left and right. This is called parallel parking and is used to preserve the heading of the vehicle (parallel to the road) after moving the car to the outboard edge of the road.

So, in your car, you can change the forward/backward position, and the orientation, and the side-side position. The same goes with a differential steering robot - you can control the left and right sides independently, so only two axes of control.

With a differential steering robot (like a tank), you could turn right 90 degrees, move forward 10 meters, then turn left 90 degrees. The end effect is as though you translated sideways 10 meters. You could also turn right then left, then move forward 10 meters. The two turns "cancel" and the end result is that you've moved forward 10 meters.

So you can't look at rotations independent of linear translations. Going 100 miles and turning 180 degrees is a lot different than turning 180 degrees and going 100 miles. Similarly, just because the robot is at, say, a local origin, doesn't mean you can determine the wheel encoder counts. There may have been no motion (no encoder counts), or a forward/backward motion, or a circular motion.

Driving a circle would have the outboard wheels have higher counts than the inboard wheels, but both would be some net positive number of counts. Forward/backward would give net zero counts and both encoders would be even.

All this is to point out why it's not possible to go straight from position to absolute encoder counts or vice-versa. As with any path-dependent problem, the solution is to take an integral.

It's not clear how your robot is setup or how you're getting and changing the ground coordinates, but you need to convert the vehicle speed at the center of the vehicle to the linear speed at each wheel. You can then convert that to angular speed of the wheel (by dividing by the wheel circumference). Then you integrate. Multiply the angular speed of the wheel by however long the time step is in your simulation and add it to an existing "position" variable.

If you're going for high fidelity simulations, then I would personally opt to "bin" the results based on your simulated wheel encoder resolution than to just add bulk noise to the signal. If you had an 8 bit wheel encoder, then one rotation gets divided into 2^8 = 256 segments. Each segment is then (2*pi/256) = 24.5 mradians. You can keep an internal state that tracks the true wheel rotations, but only update the simulated wheel encoder if it rolls over into the next bin.

Consider the following code:

wheelLinearSpeed = <your wheel speed>;
angularSpeed = wheelLinearSpeed/wheelCircumference;
angularPosition = angularPosition + angularSpeed*dT;
encoderResolution = (2*pi) / (2^8);
encoderCounts = floor(angularPosition/encoderResolution);


Another way to consider the last two lines is, instead of:

encoderResolution = (2*pi) / (2^8);
encoderCounts = floor(angularPosition/encoderResolution);


you instead inverted encoderResolution and multiplied:

encoderCounts = floor(angularPosition * ((2^8) / 2*pi)) );


This may be cleverly regrouped as:

encoderCounts = floor( (angularPosition/(2*pi)) * (2^8));


Hopefully that expression makes it a little more clear - you're making a ratio of the current angular position to a complete revolution (angularPosition/(2*pi)), and using that to find the percentage of the total number of counts for one revolution.