Belief space planning (BSP) is a fundamental problem in robotics and artificial intelligence, with applications including autonomous driving, sensor deployment and active SLAM. The goal is to autonomously determine best actions according to a specified objective function, given the current belief about random variables of interest (e.g. robot poses, tracked target or mapped environment), while accounting for different sources of uncertainty. The objective function typically contains multiple terms, such as distance to goal, control cost and a final belief uncertainty. The latter measures the amount of information contributed by candidate actions and is the most computationally expensive term to evaluate.
I present an efficient approach for evaluating the information theoretic term within BSP, where during belief propagation the state vector can stay fixed in size or be augmented by additional variables (e.g. new robot poses). Both unfocused and focused problem settings are considered, whereas uncertainty reduction of the entire system or only of chosen variables is of interest, respectively. State of the art approaches typically propagate the belief state, for each candidate action, through calculation of the posterior information (or covariance) matrix and subsequently compute its determinant (required for entropy). In contrast, our approach reduces run-time complexity by avoiding these calculations. We formulate the problem in terms of factor graphs and show that belief propagation is not needed, requiring instead a one-time calculation that depends on 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 re-used when evaluating impact of different candidate actions. These two key ingredients result in a computationally efficient (augmented) BSP approach that accounts for different sources of uncertainty and can be used with various sensing modalities.
