State your position: Accurate position estimation and propagation in autonomous vehicles

Abstract

This article is centered on how we can accurately estimate the robot pose uncertainty and how this uncertainty is propagated to future states (the robot state, in this context, is its pose). Some attention has been paid to this issue in the past. The first approach considering the uncertainty of position estimation. A min/max error bound approach is proposed resulting in bigger and bigger circles in the x-y plane representing the possible positions for the robot. Those circles are computed as projections of cylinders in the configuration space. Basically, the same approach was independently derived using a scalar as an uncertainty measure in the plane position but without reference to the orientation error. The main contribution of this article is the proposal of a novel solution for the calculation of such cross-covariance terms. In this way, we are able to catch the highly nonlinear behavior of the pose uncertainty while accurately estimating it. The basic idea is to fit the covariance matrix for the previous pose using a set of equations obtained by eigen decomposition. The cross-covariance terms are then derived using these set of equations together with the already-known expressions for the vehicle pose increments