Belief space planning (BSP) is a fundamental problem in robotics. Determining an optimal action quickly grows intractable as it involves calculating the expected accumulated cost (reward), where the expectation accounts for all future measurement realizations. State of the art approaches therefore resort to simplifying assumptions and approximations to reduce computational complexity. Importantly, while in robotics re-planning is essential, these approaches calculate each planning session from scratch. In this work we contribute a novel approach, iX-BSP, that is based on the key insight that calculations in consecutive planning sessions are similar in nature and can be thus re-used. Our approach performs incremental calculation of the expectation by appropriately re-using computations already performed in a precursory planing session while accounting for the information obtained in inference between the two planning sessions. The formulation of our approach considers general distributions and accounts for data association aspects. We evaluate iX-BSP in statistical simulation and show that incremental expectation calculations significantly reduce runtime without impacting performance.

Inference and decision making under uncertainty are essential in numerous robotics problems. In recent years, the similarities between inference and control triggered much work, from developing unified computational frameworks to pondering about the duality between the two. In spite of the aforementioned efforts, inference and control, as well as inference and belief space planning (BSP) are still treated as two separate processes. In this paper we propose a novel approach that utilizes the similarities between inference and BSP and make the key observation that inference can be efficiently updated using the precursory planning stage, thus paving the way towards a joint inference and BSP paradigm. We develop four different methods that implement our novel approach under simplifying assumptions and validate them in the context of autonomous navigation in unknown environment. Results indicate that not only our methods improve running time by at least two orders of magnitude, compared to iSAM2 paradigm, they also found to be less sensitive to state dimensionality and loop closures.