Odometry covariance computation bug?

[Martin Pecka]

Thanks for the covariance computations at https://github.com/Sollimann/CleanIt/blob/main/autonomy/src/slam/README.md !

However, in the definition of $F_p = \frac{\delta f}{\delta x, \delta y, \delta\theta}$, it seems to me the equation for $\frac{\delta f}{\delta\theta}$ should have $\Delta s$ in absolute values (on both first and second row). I can't figure out mathematically why it should be so, but practically, if I use the signed $\Delta s$, the covariance decreases when the robot reverses, which is probably not what you want.

Do you also observe this behavior?

[Kristoffer Solberg Rakstad]

Sorry, I didn't see this before now. The covariance should continue to grow regardless if your driving forward or in reverse because all motion (forward, backward, sideways etc..) introduces uncertainties and motion errors that is then integrated. So the covariance matrix of the odometric position estimate should be a strictly positive value

See section 5.2.4 here: https://github.com/Sollimann/CleanIt/blob/main/autonomy/docs/autonomous_mobile_robots.pdf

ref:

[Martin Pecka]

I agree with everything you said. However, no matter what I do (except adding the abs in the derivative $\frac{\delta f}{\delta \theta}$), the position covariance decreases when reversing. I wanted to figure out why. I triple-checked my implementation and it follows these equations exactly.

The $\Delta s$ values have to be signed (otherwise yaw rate calculation would not work). They go the the $\Sigma_\Delta$ covariance matrix in abs values, so the covariance is positive.