Exactly Sparse Gaussian Variational Inference with Application to Derivative-Free Batch Nonlinear State Estimation

We present a Gaussian Variational Inference (GVI) technique that can be\napplied to large-scale nonlinear batch state estimation problems. The main\ncontribution is to show how to fit both the mean and (inverse) covariance of a\nGaussian to the posterior efficiently, by exploiting factorization of the joint\nlikelihood of the state and data, as is common in practical problems. This is\ndifferent than Maximum A Posteriori (MAP) estimation, which seeks the point\nestimate for the state that maximizes the posterior (i.e., the mode). The\nproposed Exactly Sparse Gaussian Variational Inference (ESGVI) technique stores\nthe inverse covariance matrix, which is typically very sparse (e.g.,\nblock-tridiagonal for classic state estimation). We show that the only blocks\nof the (dense) covariance matrix that are required during the calculations\ncorrespond to the non-zero blocks of the inverse covariance matrix, and further\nshow how to calculate these blocks efficiently in the general GVI problem.\nESGVI operates iteratively, and while we can use analytical derivatives at each\niteration, Gaussian cubature can be substituted, thereby producing an efficient\nderivative-free batch formulation. ESGVI simplifies to precisely the\nRauch-Tung-Striebel (RTS) smoother in the batch linear estimation case, but\ngoes beyond the 'extended' RTS smoother in the nonlinear case since it finds\nthe best-fit Gaussian (mean and covariance), not the MAP point estimate. We\ndemonstrate the technique on controlled simulation problems and a batch\nnonlinear Simultaneous Localization and Mapping (SLAM) problem with an\nexperimental dataset.\n