  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.
