Exact state and covariance sub-matrix recovery for submap based sparse EIF SLAM algorithm

This paper provides a novel state vector and covariance sub-matrix recovery algorithm for a recently developed submap based exactly sparse extended information filter (EIF) SLAM algorithm - sparse local submap joining filter (SLSJF). The algorithm achieves exact recovery instead of approximate recovery. The recovery algorithm is very efficient because of an incremental Cholesky factorization approach and a natural reordering of the global state vector. Simulation results show that the computation cost of the SLSJF is much lower as compared with the sequential map joining algorithm using extended Kalman filter (EKF). The SLSJF with the proposed recovery algorithm is also successfully applied to the Victoria Park data set.