Fast covariance recovery in incremental nonlinear least square solvers

Many estimation problems in robotics rely on efficiently solving nonlinear least squares (NLS). For example, it is well known that the simultaneous localisation and mapping (SLAM) problem can be formulated as a maximum likelihood estimation (MLE) and solved using NLS, yielding a mean state vector. However, for many applications recovering only the mean vector is not enough. Data association, active decisions, next best view, are only few of the applications that require fast state covariance recovery. The problem is not simple since, in general, the covariance is obtained by inverting the system matrix and the result is dense. The main contribution of this paper is a novel algorithm for fast incremental covariance update, complemented by a highly efficient implementation of the covariance recovery. This combination yields to two orders of magnitude reduction in computation time, compared to the other state of the art solutions. The proposed algorithm is applicable to any NLS solver implementation, and does not depend on incremental strategies described in our previous papers, which are not a subject of this paper.