Exactly Sparse Gaussian Variational Inference with Application to Derivative-Free Batch Nonlinear State Estimation
This provides an efficient derivative-free solution for large-scale nonlinear state estimation problems like SLAM, though it is incremental as it builds on existing variational inference and sparsity exploitation methods.
The paper tackles large-scale nonlinear batch state estimation by proposing an Exactly Sparse Gaussian Variational Inference (ESGVI) technique that efficiently fits both the mean and inverse covariance of a Gaussian to the posterior, exploiting sparsity for scalability. It demonstrates the method on simulation and experimental batch nonlinear SLAM problems, showing it generalizes the Rauch-Tung-Striebel smoother to nonlinear cases.
We present a Gaussian Variational Inference (GVI) technique that can be applied to large-scale nonlinear batch state estimation problems. The main contribution is to show how to fit both the mean and (inverse) covariance of a Gaussian to the posterior efficiently, by exploiting factorization of the joint likelihood of the state and data, as is common in practical problems. This is different than Maximum A Posteriori (MAP) estimation, which seeks the point estimate for the state that maximizes the posterior (i.e., the mode). The proposed Exactly Sparse Gaussian Variational Inference (ESGVI) technique stores the inverse covariance matrix, which is typically very sparse (e.g., block-tridiagonal for classic state estimation). We show that the only blocks of the (dense) covariance matrix that are required during the calculations correspond to the non-zero blocks of the inverse covariance matrix, and further show how to calculate these blocks efficiently in the general GVI problem. ESGVI operates iteratively, and while we can use analytical derivatives at each iteration, Gaussian cubature can be substituted, thereby producing an efficient derivative-free batch formulation. ESGVI simplifies to precisely the Rauch-Tung-Striebel (RTS) smoother in the batch linear estimation case, but goes beyond the 'extended' RTS smoother in the nonlinear case since it finds the best-fit Gaussian (mean and covariance), not the MAP point estimate. We demonstrate the technique on controlled simulation problems and a batch nonlinear Simultaneous Localization and Mapping (SLAM) problem with an experimental dataset.