Handling of odometry data

[SeaTechRC]

Hello,

Upon further inspection I suspect there might be an incorrect implementation of calculating a new prediction in the odometer from the linear and angular velocity given by wheels. (At least in the case of differential wheeled robots.)

To explain: Currently the new pose calculated from the linear and angular velocity is calculated such that: $$x{t + \Delta t} = x{t} + \cos(\phi{t}) \cdot \Delta t$$ $$y{t + \Delta t} = y{t} + \sin(\phi{t}) \cdot \Delta t$$ $$\phi_{t + \Delta t} = \phi + \omega \cdot \Delta t$$

When looking at the kinematics of a differential wheeled robot we find that this equates to the derivative of the robots x, y and yaw times the time elapsed since the last update. This is okay as an approximation, however, it diverges especially quick if updates are infrequent.

The correct method would be to calculate the integral of the derivative. For this, it must be taken into account that the yaw (which is $$\phi$$) changes during driving. If we substitute the yaw with its respective function, we get that the integral for the relative movement is (from $$0$$ to $$\Delta t$$) is: $\Delta x = \int\limits_0^{\Delta t} V \cos(\phi_0 + \omega \cdot t) dt$

$\Delta y = \int\limits_0^{\Delta t} V \sin(\phi_0 + \omega \cdot t) dt$

$\Delta \phi = \int\limits_0^{\Delta t} \omega$

$x{t + \Delta t} = x{t} + \Delta x$ $y{t + \Delta t} = y{t} + \Delta y$ $\phi_{t + \Delta t} = \phi + \Delta \phi$

Calculating the integrals we get:

$\Delta x = \frac{V \cdot \sin(\phi_0 + \omega \cdot \Delta t)}{\omega}$

$\Delta y = -\frac{V \cdot \cos(\phi_0 + \omega \cdot \Delta t)}{\omega}$

$\Delta \phi = \omega \cdot \Delta t$

A good way to visualize it is, that the old method, at any given point, would simply move the robot in a straight line for $\Delta t$ seconds. Which does not match the curve the robot is actually driving (a circular path).

[SeaTechRC]

Correction to the integral: I made an error and didn't include the bounds.

The definite integrals would be: $\Delta x = V \cdot \frac{\sin(\omega \cdot \Delta t + \phi_0) - \sin(\phi_0)}{\omega}$

$\Delta y = -V \cdot \frac{\cos(\omega \cdot \Delta t + \phi_0) - \cos(\phi_0)}{\omega}$

Do note, that there is a seperate case where $\omega$ is 0 and therefore we simply drive in a straight line.

Adding acceleration and deceleration would also change the form of the integral, as the $V$ would be replaced with a function $V(t)$. This might be less important, but not taking acceleration into account is another small source of error.

[falkoschindler]

Thanks for bringing this up, @SeaTechRC! The current implementation is indeed rather simplistic and assumes frequent odometry measurements and a relatively slow and smooth motion. To be honest, I never really thought about using more complex formulas and expected them to be even more complicated than the ones you're proposing.

Some thoughts:

1. Assuming a circular motion is also just an approximation. Ultimately we don't know the true motion and need to make assumptions. But assuming to drive in circles is probably better than computing piecewise linear paths.
2. We usually work with robots not faster than 1 m/s and odometry measurements at least 20 times per second, resulting in 5cm line segments (at full speed!). When turning, the robots are usually much slower. So I doubt it makes a significant difference whether to assume a straight 5cm segment or an arc.
3. If your measurement intervals are longer, it's still vague to assume the robot is driving in a perfect circle. It could do all kinds of crazy things while the odometer isn't looking.
4. And if you're concerned about maximum precision, I doubt that wheel odometry will ever be that accurate, especially when turning.
5. If I'm correct you're referring to the implementation in Pose.__add__ where a PoseStep is added to a Pose. This piece of code is rather unrelated to the application of odometers and differential wheeled robots. Changing it would introduce a nasty breaking change, which wouldn't make much sense within the rosys.geometry module. So we would better add a new implementation within Odometer.handle_velocities.
6. Regarding the case where $\omega$ is 0: Even though we can easily handle this case with an if condition, I'm a bit worried about numerical instabilities when $\omega$ is close to 0. Actually I'm wondering how $\omega$ in the denominator works at all: For tiny turn rates the coordinate increments become huge. 🤔

Long story short, I don't think changing the formulas is strictly necessary, but I would probably accept a pull request. Apart from that I'd love to find a second source for these formulas - just to be sure. It's rather easy to introduce a subtle bug here that's hard to notice but would make things even more incorrect.