Computationally Efficient Belief Space Planning via Augmented Matrix Determinant Lemma and Reuse of Calculations

We develop a computationally efficient approach for evaluating the information theoretic term within belief space planning (BSP) considering both unfocused and focused problem settings, where uncertainty reduction of the entire system or only of chosen variables is of interest, respectively. State-of-the-art approaches typically calculate, for each candidate action, the posterior information (or covariance) matrix and its determinant (required for entropy). In contrast, our approach reduces run-time complexity by avoiding these calculations, requiring instead a one-time calculation that depends on (the increasing with time) state dimensionality, and per-candidate calculations that are independent of the latter. To that end, we develop an augmented version of the matrix determinant lemma, and show computations can be reused when evaluating impact of different candidate actions. These two key ingredients result in a computationally efficient BSP approach that accounts for different sources of uncertainty and can be used with various sensing modalities. We examine the unfocused and focused instances of our approach, and compare it to the state of the art, in simulation and using real-world data, considering the problem of autonomous navigation in unknown environments.