A high-dimensional Simultaneous Localization and Mapping (SLAM) algorithm is presented that replaces the particles in FastSLAM with individual Gaussians. In addition, the high-dimensional vehicle state is partitioned into linear and nonlinear parts and the nonlinear part is approximated by a mixture of Gaussians of which the means and covariances are propagated and updated using sparse grid quadrature. Preliminary simulation results of three-dimensional SLAM show that the Gaussian mixture approach is more accurate than the particle based approach.
展开▼